[PATCH] mac80211: support FTM responder configuration/statistics

[PATCH] mac80211: support FTM responder configuration/statistics

[Pradeep Kumar Chitrapu]

New bss param ftm_responder is used to notify the driver to
enable fine timing request (FTM) responder role in AP mode.

Plumb the new cfg80211 API for FTM responder statistics through to
the driver API in mac80211.

Signed-off-by: David Spinadel <david.spinadel@intel.com>
Signed-off-by: Johannes Berg <johannes.berg@intel.com>
Signed-off-by: Pradeep Kumar Chitrapu <pradeepc@codeaurora.org>
---
 include/net/mac80211.h    | 28 ++++++++++++++++
 net/mac80211/cfg.c        | 85 +++++++++++++++++++++++++++++++++++++++++++++++
 net/mac80211/driver-ops.h | 16 +++++++++
 net/mac80211/trace.h      | 23 +++++++++++++
 net/mac80211/util.c       |  5 +++
 5 files changed, 157 insertions(+)

diff --git a/include/net/mac80211.h b/include/net/mac80211.h
index c4fadbafbf21..2ccd4d1bef89 100644
--- a/include/net/mac80211.h
+++ b/include/net/mac80211.h
@@ -309,6 +309,8 @@ struct ieee80211_vif_chanctx_switch {
  * @BSS_CHANGED_KEEP_ALIVE: keep alive options (idle period or protected
  *	keep alive) changed.
  * @BSS_CHANGED_MCAST_RATE: Multicast Rate setting changed for this interface
+ * @BSS_CHANGED_FTM_RESPONDER: fime timing reasurement request responder
+ *	functionality changed for this BSS (AP mode).
  *
  */
 enum ieee80211_bss_change {
@@ -338,6 +340,7 @@ enum ieee80211_bss_change {
 	BSS_CHANGED_MU_GROUPS		= 1<<23,
 	BSS_CHANGED_KEEP_ALIVE		= 1<<24,
 	BSS_CHANGED_MCAST_RATE		= 1<<25,
+	BSS_CHANGED_FTM_RESPONDER	= 1<<26,
 
 	/* when adding here, make sure to change ieee80211_reconfig */
 };
@@ -464,6 +467,21 @@ struct ieee80211_mu_group_data {
 };
 
 /**
+ * ieee80211_ftm_responder_params - FTM responder parameters
+ *
+ * @lci: LCI subelement content
+ * @civicloc: CIVIC location subelement content
+ * @lci_len: LCI data length
+ * @civicloc_len: Civic data length
+ */
+struct ieee80211_ftm_responder_params {
+	const u8 *lci;
+	const u8 *civicloc;
+	size_t lci_len;
+	size_t civicloc_len;
+};
+
+/**
  * struct ieee80211_bss_conf - holds the BSS's changing parameters
  *
  * This structure keeps information about a BSS (and an association
@@ -562,6 +580,9 @@ struct ieee80211_mu_group_data {
  * @protected_keep_alive: if set, indicates that the station should send an RSN
  *	protected frame to the AP to reset the idle timer at the AP for the
  *	station.
+ * @ftm_responder: whether to enable or disable fine timing measurement FTM
+ *	responder functionality.
+ * @ftmr_params: configurable lci/civic parameter when enabling FTM responder.
  */
 struct ieee80211_bss_conf {
 	const u8 *bssid;
@@ -612,6 +633,8 @@ struct ieee80211_bss_conf {
 	bool allow_p2p_go_ps;
 	u16 max_idle_period;
 	bool protected_keep_alive;
+	bool ftm_responder;
+	struct ieee80211_ftm_responder_params *ftmr_params;
 };
 
 /**
@@ -3598,6 +3621,8 @@ enum ieee80211_reconfig_type {
  *	aggregating two specific frames in the same A-MSDU. The relation
  *	between the skbs should be symmetric and transitive. Note that while
  *	skb is always a real frame, head may or may not be an A-MSDU.
+ * @get_ftm_responder_stats: Retrieve FTM responder statistics, if available.
+ *	Statistics should be cumulative, currently no way to reset is provided.
  */
 struct ieee80211_ops {
 	void (*tx)(struct ieee80211_hw *hw,
@@ -3883,6 +3908,9 @@ struct ieee80211_ops {
 	bool (*can_aggregate_in_amsdu)(struct ieee80211_hw *hw,
 				       struct sk_buff *head,
 				       struct sk_buff *skb);
+	int (*get_ftm_responder_stats)(struct ieee80211_hw *hw,
+				       struct ieee80211_vif *vif,
+				       struct cfg80211_ftm_responder_stats *ftm_stats);
 };
 
 /**
diff --git a/net/mac80211/cfg.c b/net/mac80211/cfg.c
index 504627e2117f..c3904d45e82a 100644
--- a/net/mac80211/cfg.c
+++ b/net/mac80211/cfg.c
@@ -790,6 +790,48 @@ static int ieee80211_set_probe_resp(struct ieee80211_sub_if_data *sdata,
 	return 0;
 }
 
+static int ieee80211_set_ftm_responder_params(
+				struct ieee80211_sub_if_data *sdata,
+				const u8 *lci, size_t lci_len,
+				const u8 *civicloc, size_t civicloc_len)
+{
+	struct ieee80211_ftm_responder_params *new, *old;
+	struct ieee80211_bss_conf *bss_conf;
+	u8 *pos;
+	int len;
+
+	if ((!lci || !lci_len) && (!civicloc || !civicloc_len))
+		return 1;
+
+	bss_conf = &sdata->vif.bss_conf;
+	old = bss_conf->ftmr_params;
+	len = lci_len + civicloc_len;
+
+	new = kzalloc(sizeof(*new) + len, GFP_KERNEL);
+	if (!new)
+		return -ENOMEM;
+
+	pos = (u8 *)(new + 1);
+	if (lci_len) {
+		new->lci_len = lci_len;
+		new->lci = pos;
+		memcpy(pos, lci, lci_len);
+		pos += lci_len;
+	}
+
+	if (civicloc_len) {
+		new->civicloc_len = civicloc_len;
+		new->civicloc = pos;
+		memcpy(pos, civicloc, civicloc_len);
+		pos += civicloc_len;
+	}
+
+	bss_conf->ftmr_params = new;
+	kfree(old);
+
+	return 0;
+}
+
 static int ieee80211_assign_beacon(struct ieee80211_sub_if_data *sdata,
 				   struct cfg80211_beacon_data *params,
 				   const struct ieee80211_csa_settings *csa)
@@ -863,6 +905,20 @@ static int ieee80211_assign_beacon(struct ieee80211_sub_if_data *sdata,
 	if (err == 0)
 		changed |= BSS_CHANGED_AP_PROBE_RESP;
 
+	if (params->ftm_responder != -1) {
+		sdata->vif.bss_conf.ftm_responder = params->ftm_responder;
+		err = ieee80211_set_ftm_responder_params(sdata,
+							 params->lci,
+							 params->lci_len,
+							 params->civicloc,
+							 params->civicloc_len);
+
+		if (err < 0)
+			return err;
+
+		changed |= BSS_CHANGED_FTM_RESPONDER;
+	}
+
 	rcu_assign_pointer(sdata->u.ap.beacon, new);
 
 	if (old)
@@ -1063,6 +1119,9 @@ static int ieee80211_stop_ap(struct wiphy *wiphy, struct net_device *dev)
 		kfree_rcu(old_probe_resp, rcu_head);
 	sdata->u.ap.driver_smps_mode = IEEE80211_SMPS_OFF;
 
+	kfree(sdata->vif.bss_conf.ftmr_params);
+	sdata->vif.bss_conf.ftmr_params = NULL;
+
 	__sta_info_flush(sdata, true);
 	ieee80211_free_keys(sdata, true);
 
@@ -2875,6 +2934,20 @@ static int ieee80211_start_radar_detection(struct wiphy *wiphy,
 		memcpy(pos, beacon->probe_resp, beacon->probe_resp_len);
 		pos += beacon->probe_resp_len;
 	}
+	if (beacon->ftm_responder)
+		new_beacon->ftm_responder = beacon->ftm_responder;
+	if (beacon->lci) {
+		new_beacon->lci_len = beacon->lci_len;
+		new_beacon->lci = pos;
+		memcpy(pos, beacon->lci, beacon->lci_len);
+		pos += beacon->lci_len;
+	}
+	if (beacon->civicloc) {
+		new_beacon->civicloc_len = beacon->civicloc_len;
+		new_beacon->civicloc = pos;
+		memcpy(pos, beacon->civicloc, beacon->civicloc_len);
+		pos += beacon->civicloc_len;
+	}
 
 	return new_beacon;
 }
@@ -3765,6 +3838,17 @@ static int ieee80211_get_txq_stats(struct wiphy *wiphy,
 	return ret;
 }
 
+static int
+ieee80211_get_ftm_responder_stats(struct wiphy *wiphy,
+				  struct net_device *dev,
+				  struct cfg80211_ftm_responder_stats *ftm_stats)
+{
+	struct ieee80211_local *local = wiphy_priv(wiphy);
+	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
+
+	return drv_get_ftm_responder_stats(local, sdata, ftm_stats);
+}
+
 const struct cfg80211_ops mac80211_config_ops = {
 	.add_virtual_intf = ieee80211_add_iface,
 	.del_virtual_intf = ieee80211_del_iface,
@@ -3859,4 +3943,5 @@ static int ieee80211_get_txq_stats(struct wiphy *wiphy,
 	.set_multicast_to_unicast = ieee80211_set_multicast_to_unicast,
 	.tx_control_port = ieee80211_tx_control_port,
 	.get_txq_stats = ieee80211_get_txq_stats,
+	.get_ftm_responder_stats = ieee80211_get_ftm_responder_stats,
 };
diff --git a/net/mac80211/driver-ops.h b/net/mac80211/driver-ops.h
index e42c641b6190..0b1747a2313d 100644
--- a/net/mac80211/driver-ops.h
+++ b/net/mac80211/driver-ops.h
@@ -1183,6 +1183,22 @@ static inline int drv_can_aggregate_in_amsdu(struct ieee80211_local *local,
 	return local->ops->can_aggregate_in_amsdu(&local->hw, head, skb);
 }
 
+static inline int
+drv_get_ftm_responder_stats(struct ieee80211_local *local,
+			    struct ieee80211_sub_if_data *sdata,
+			    struct cfg80211_ftm_responder_stats *ftm_stats)
+{
+	u32 ret = -EOPNOTSUPP;
+
+	if (local->ops->get_ftm_responder_stats)
+		ret = local->ops->get_ftm_responder_stats(&local->hw,
+							 &sdata->vif,
+							 ftm_stats);
+	trace_drv_get_ftm_responder_stats(local, sdata, ftm_stats);
+
+	return ret;
+}
+
 static inline int drv_start_nan(struct ieee80211_local *local,
 				struct ieee80211_sub_if_data *sdata,
 				struct cfg80211_nan_conf *conf)
diff --git a/net/mac80211/trace.h b/net/mac80211/trace.h
index 0ab69a1964f8..588c51a67c89 100644
--- a/net/mac80211/trace.h
+++ b/net/mac80211/trace.h
@@ -2600,6 +2600,29 @@ struct trace_switch_entry {
 	)
 );
 
+TRACE_EVENT(drv_get_ftm_responder_stats,
+	TP_PROTO(struct ieee80211_local *local,
+		 struct ieee80211_sub_if_data *sdata,
+		 struct cfg80211_ftm_responder_stats *ftm_stats),
+
+	TP_ARGS(local, sdata, ftm_stats),
+
+	TP_STRUCT__entry(
+		LOCAL_ENTRY
+		VIF_ENTRY
+	),
+
+	TP_fast_assign(
+		LOCAL_ASSIGN;
+		VIF_ASSIGN;
+	),
+
+	TP_printk(
+		LOCAL_PR_FMT VIF_PR_FMT,
+		LOCAL_PR_ARG, VIF_PR_ARG
+	)
+);
+
 #endif /* !__MAC80211_DRIVER_TRACE || TRACE_HEADER_MULTI_READ */
 
 #undef TRACE_INCLUDE_PATH
diff --git a/net/mac80211/util.c b/net/mac80211/util.c
index ef5d1f60a63b..bec424316ea4 100644
--- a/net/mac80211/util.c
+++ b/net/mac80211/util.c
@@ -2178,6 +2178,11 @@ int ieee80211_reconfig(struct ieee80211_local *local)
 		case NL80211_IFTYPE_AP:
 			changed |= BSS_CHANGED_SSID | BSS_CHANGED_P2P_PS;
 
+			if (sdata->vif.bss_conf.ftm_responder == 1 &&
+			    wiphy_ext_feature_isset(sdata->local->hw.wiphy,
+					NL80211_EXT_FEATURE_ENABLE_FTM_RESPONDER))
+				changed |= BSS_CHANGED_FTM_RESPONDER;
+
 			if (sdata->vif.type == NL80211_IFTYPE_AP) {
 				changed |= BSS_CHANGED_AP_PROBE_RESP;
 
-- 
1.9.1

Re: [PATCH] mac80211: support FTM responder configuration/statistics

[Johannes Berg]

Hi Pradeep,

On Wed, 2018-10-03 at 20:19 -0700, Pradeep Kumar Chitrapu wrote:
> New bss param ftm_responder is used to notify the driver to
> enable fine timing request (FTM) responder role in AP mode.

I just realized that this is broken in nl80211_channel_switch() and
ieee80211_set_csa_beacon(), doing a CSA will always disable FTM unless
it was actually included in the new configuration.

Doing the trivial thing:

 	memset(&params, 0, sizeof(params));
+	params.beacon_after.ftm_responder = -1;

in nl80211_channel_switch() will not help because then mac80211 will
lose all the extra configuration, and will actually store -1 into its
enabled value which is really strange.

I'd appreciate if you could take a look at this.

Thanks,
johannes

Re: [PATCH] mac80211: support FTM responder configuration/statistics

[Pradeep Kumar Chitrapu]

> 
> I just realized that this is broken in nl80211_channel_switch() and
> ieee80211_set_csa_beacon(), doing a CSA will always disable FTM unless
> it was actually included in the new configuration.
Hi Johannes

oops..Yes, there is a bug in the patch. The code below, I think, must 
fix this issue.
Please let me know your comments..

diff --git a/net/mac80211/cfg.c b/net/mac80211/cfg.c
index 51622333d460..70d6de29425b 100644
--- a/net/mac80211/cfg.c
+++ b/net/mac80211/cfg.c
@@ -2934,19 +2934,20 @@ static int 
ieee80211_start_radar_detection(struct wiphy *wiphy,
                 memcpy(pos, beacon->probe_resp, beacon->probe_resp_len);
                 pos += beacon->probe_resp_len;
         }
-       if (beacon->ftm_responder)
+       if (beacon->ftm_responder != -1) {
                 new_beacon->ftm_responder = beacon->ftm_responder;
-       if (beacon->lci) {
-               new_beacon->lci_len = beacon->lci_len;
-               new_beacon->lci = pos;
-               memcpy(pos, beacon->lci, beacon->lci_len);
-               pos += beacon->lci_len;
-       }
-       if (beacon->civicloc) {
-               new_beacon->civicloc_len = beacon->civicloc_len;
-               new_beacon->civicloc = pos;
-               memcpy(pos, beacon->civicloc, beacon->civicloc_len);
-               pos += beacon->civicloc_len;
+               if (beacon->lci) {
+                       new_beacon->lci_len = beacon->lci_len;
+                       new_beacon->lci = pos;
+                       memcpy(pos, beacon->lci, beacon->lci_len);
+                       pos += beacon->lci_len;
+               }
+               if (beacon->civicloc) {
+                       new_beacon->civicloc_len = beacon->civicloc_len;
+                       new_beacon->civicloc = pos;
+                       memcpy(pos, beacon->civicloc, 
beacon->civicloc_len);
+                       pos += beacon->civicloc_len;
+               }
         }

         return new_beacon;


> 
> Doing the trivial thing:
> 
>  	memset(&params, 0, sizeof(params));
> +	params.beacon_after.ftm_responder = -1;
This would not be needed then.
> 
> in nl80211_channel_switch() will not help because then mac80211 will
> lose all the extra configuration, and will actually store -1 into its
> enabled value which is really strange.
> 
> I'd appreciate if you could take a look at this.
> 
> Thanks,
> johannes

Re: [PATCH] mac80211: support FTM responder configuration/statistics

[Johannes Berg]

Hi,

Actually, I think my suggestion _would_ in fact fix the whole issue.

> diff --git a/net/mac80211/cfg.c b/net/mac80211/cfg.c
> index 51622333d460..70d6de29425b 100644
> --- a/net/mac80211/cfg.c
> +++ b/net/mac80211/cfg.c
> @@ -2934,19 +2934,20 @@ static int 
> ieee80211_start_radar_detection(struct wiphy *wiphy,
>                  memcpy(pos, beacon->probe_resp, beacon->probe_resp_len);
>                  pos += beacon->probe_resp_len;
>          }
> -       if (beacon->ftm_responder)
> +       if (beacon->ftm_responder != -1) {

This doesn't make sense without the change I suggested, since if we
don't do the change I suggested, beacon->ftm_responder will never
actually be -1.

I'd change it like this, fixing the memory overflow bug along the way:

diff --git a/net/mac80211/cfg.c b/net/mac80211/cfg.c
index 51622333d460..818aa0060349 100644
--- a/net/mac80211/cfg.c
+++ b/net/mac80211/cfg.c
@@ -2891,7 +2891,7 @@ cfg80211_beacon_dup(struct cfg80211_beacon_data *beacon)
 
 	len = beacon->head_len + beacon->tail_len + beacon->beacon_ies_len +
 	      beacon->proberesp_ies_len + beacon->assocresp_ies_len +
-	      beacon->probe_resp_len;
+	      beacon->probe_resp_len + beacon->lci_len + beacon->civicloc_len;
 
 	new_beacon = kzalloc(sizeof(*new_beacon) + len, GFP_KERNEL);
 	if (!new_beacon)
@@ -2934,8 +2934,9 @@ cfg80211_beacon_dup(struct cfg80211_beacon_data *beacon)
 		memcpy(pos, beacon->probe_resp, beacon->probe_resp_len);
 		pos += beacon->probe_resp_len;
 	}
-	if (beacon->ftm_responder)
-		new_beacon->ftm_responder = beacon->ftm_responder;
+
+	/* might copy -1, meaning no changes requested */
+	new_beacon->ftm_responder = beacon->ftm_responder;
 	if (beacon->lci) {
 		new_beacon->lci_len = beacon->lci_len;
 		new_beacon->lci = pos;
diff --git a/net/wireless/nl80211.c b/net/wireless/nl80211.c
index 744b5851bbf9..8d763725498c 100644
--- a/net/wireless/nl80211.c
+++ b/net/wireless/nl80211.c
@@ -7870,6 +7870,7 @@ static int nl80211_channel_switch(struct sk_buff *skb, struct genl_info *info)
 	}
 
 	memset(&params, 0, sizeof(params));
+	params.beacon_csa.ftm_responder = -1;
 
 	if (!info->attrs[NL80211_ATTR_WIPHY_FREQ] ||
 	    !info->attrs[NL80211_ATTR_CH_SWITCH_COUNT])


If that seems good to you I'll submit the patch.

johannes

Re: [PATCH] mac80211: support FTM responder configuration/statistics

[Pradeep Kumar Chitrapu]

>  	memset(&params, 0, sizeof(params));
> +	params.beacon_csa.ftm_responder = -1;
Hi Johannes,

Agree with the rest, however, I think this may not be needed because 
ftm_responder is already being set to -1,
if NL80211_ATTR_FTM_RESPONDER attribute is not included, in 
nl80211_parse_beacon, which will be called from
nl80211_channel_switch. Please let me know if I may have missed 
something..

         if (attrs[NL80211_ATTR_FTM_RESPONDER]) {

         } else {
                 bcn->ftm_responder = -1;
         }

> 
>  	if (!info->attrs[NL80211_ATTR_WIPHY_FREQ] ||
>  	    !info->attrs[NL80211_ATTR_CH_SWITCH_COUNT])
> 
> 
> If that seems good to you I'll submit the patch.
Sure..Thanks.
> 
> johannes

Re: [PATCH] mac80211: support FTM responder configuration/statistics

[Johannes Berg]

On Mon, 2018-10-29 at 11:59 -0700, Pradeep Kumar Chitrapu wrote:
> >  	memset(&params, 0, sizeof(params));
> > +	params.beacon_csa.ftm_responder = -1;
> 
> Hi Johannes,
> 
> Agree with the rest, however, I think this may not be needed because 
> ftm_responder is already being set to -1,
> if NL80211_ATTR_FTM_RESPONDER attribute is not included, in 
> nl80211_parse_beacon, which will be called from
> nl80211_channel_switch. Please let me know if I may have missed 
> something..
> 
>          if (attrs[NL80211_ATTR_FTM_RESPONDER]) {
> 
>          } else {
>                  bcn->ftm_responder = -1;
>          }
> 

Yes, but we may not get there?

        if (!need_new_beacon)
                goto skip_beacons;

        err = nl80211_parse_beacon(rdev, info->attrs, &params.beacon_after);
[...]
skip_beacons:
[...]
	err = rdev_channel_switch(rdev, dev, &params);


johannes

Re: [PATCH] mac80211: support FTM responder configuration/statistics

[Pradeep Kumar Chitrapu]

> Yes, but we may not get there?
> 
>         if (!need_new_beacon)
>                 goto skip_beacons;
ah!..yes...Thanks..