[PATCH wireless-next v3 3/4] wifi: mac80211: implement STA-mode peer probing

[Priyansha Tiwari <priyansha.tiwari@oss.qualcomm.com>]

From: Priyansha Tiwari <priyansha.tiwari@oss.qualcomm.com>

Add STA/P2P-client support to ieee80211_probe_peer(): when called
for a station interface, send a null-data frame (TODS) to the
associated AP and report the ACK via cfg80211_probe_status().

For MLO connections the driver/firmware selects the link
(IEEE80211_LINK_UNSPECIFIED); for non-MLO the single link is used.

Signed-off-by: Priyansha Tiwari <priyansha.tiwari@oss.qualcomm.com>
---
 include/net/mac80211.h |   2 +-
 net/mac80211/cfg.c     | 151 +++++++++++++++++++++++------------------
 net/mac80211/status.c  |   5 +-
 3 files changed, 89 insertions(+), 69 deletions(-)

diff --git a/include/net/mac80211.h b/include/net/mac80211.h
index 4fb579805e0f..6df439ef9807 100644
--- a/include/net/mac80211.h
+++ b/include/net/mac80211.h
@@ -1396,7 +1396,7 @@ struct ieee80211_tx_info {
 			u8 pad;
 			u16 tx_time;
 			u8 flags;
-			u8 pad2;
+			u8 link_valid:1, link_id:4;
 			void *status_driver_data[16 / sizeof(void *)];
 		} status;
 		struct {
diff --git a/net/mac80211/cfg.c b/net/mac80211/cfg.c
index f948b1331e3e..55fe50629a98 100644
--- a/net/mac80211/cfg.c
+++ b/net/mac80211/cfg.c
@@ -4922,101 +4922,122 @@ static int ieee80211_probe_peer(struct wiphy *wiphy, struct net_device *dev,
 	struct ieee80211_local *local = sdata->local;
 	struct ieee80211_qos_hdr *nullfunc;
 	struct sk_buff *skb;
-	int size = sizeof(*nullfunc);
 	__le16 fc;
-	bool qos;
+	bool qos, fromds;
+	struct ieee80211_bss_conf *conf;
 	struct ieee80211_tx_info *info;
 	struct sta_info *sta;
 	struct ieee80211_chanctx_conf *chanctx_conf;
-	struct ieee80211_bss_conf *conf;
 	enum nl80211_band band;
-	u8 link_id;
+	const u8 *peer_addr;
+	const u8 *src_addr;
+	int link_id;
+	int size;
 	int ret;
 
 	/* the lock is needed to assign the cookie later */
 	lockdep_assert_wiphy(local->hw.wiphy);
 
-	rcu_read_lock();
-	sta = sta_info_get_bss(sdata, peer);
-	if (!sta) {
-		ret = -ENOLINK;
-		goto unlock;
-	}
-
-	qos = sta->sta.wme;
+	guard(rcu)();
 
-	if (ieee80211_vif_is_mld(&sdata->vif)) {
-		if (sta->sta.mlo) {
-			link_id = IEEE80211_LINK_UNSPECIFIED;
-		} else {
-			/*
-			 * For non-MLO clients connected to an AP MLD, band
-			 * information is not used; instead, sta->deflink is
-			 * used to send packets.
-			 */
-			link_id = sta->deflink.link_id;
+	switch (sdata->vif.type) {
+	case NL80211_IFTYPE_AP:
+	case NL80211_IFTYPE_P2P_GO:
+		sta = sta_info_get_bss(sdata, peer);
+		if (!sta)
+			return -ENOLINK;
 
-			conf = rcu_dereference(sdata->vif.link_conf[link_id]);
+		qos = sta->sta.wme;
+		peer_addr = sta->sta.addr;
 
-			if (unlikely(!conf)) {
-				ret = -ENOLINK;
-				goto unlock;
+		if (ieee80211_vif_is_mld(&sdata->vif)) {
+			if (sta->sta.mlo) {
+				link_id = IEEE80211_LINK_UNSPECIFIED;
+				src_addr = sdata->vif.addr;
+			} else {
+				/*
+				 * For non-MLO clients connected to an AP MLD,
+				 * use the link address for the client's link.
+				 */
+				link_id = sta->deflink.link_id;
+				conf = rcu_dereference(sdata->vif.link_conf[link_id]);
+				if (unlikely(!conf))
+					return -ENOLINK;
+				src_addr = conf->addr;
 			}
+			/* MLD transmissions must not rely on the band */
+			band = 0;
+		} else {
+			chanctx_conf = rcu_dereference(sdata->vif.bss_conf.chanctx_conf);
+			if (WARN_ON(!chanctx_conf))
+				return -EINVAL;
+			band = chanctx_conf->def.chan->band;
+			link_id = 0;
+			src_addr = sdata->vif.addr;
 		}
-		/* MLD transmissions must not rely on the band */
-		band = 0;
-	} else {
-		chanctx_conf = rcu_dereference(sdata->vif.bss_conf.chanctx_conf);
-		if (WARN_ON(!chanctx_conf)) {
-			ret = -EINVAL;
-			goto unlock;
+		fromds = true;
+		break;
+
+	case NL80211_IFTYPE_STATION:
+	case NL80211_IFTYPE_P2P_CLIENT:
+		if (ieee80211_vif_is_mld(&sdata->vif)) {
+			link_id = IEEE80211_LINK_UNSPECIFIED;
+			peer_addr = sdata->vif.cfg.ap_addr;
+			src_addr = sdata->vif.addr;
+			band = 0;
+			sta = sta_info_get(sdata, sdata->vif.cfg.ap_addr);
+		} else {
+			link_id = 0;
+			conf = rcu_dereference(sdata->vif.link_conf[0]);
+			if (!conf)
+				return -ENOLINK;
+			band = conf->chanreq.oper.chan->band;
+			peer_addr = conf->bssid;
+			src_addr = conf->addr;
+			sta = sta_info_get_bss(sdata, peer_addr);
 		}
-		band = chanctx_conf->def.chan->band;
-		link_id = 0;
+		qos = sta ? sta->sta.wme : false;
+		fromds = false;
+		break;
+
+	default:
+		return -EOPNOTSUPP;
 	}
 
-	if (qos) {
-		fc = cpu_to_le16(IEEE80211_FTYPE_DATA |
-				 IEEE80211_STYPE_QOS_NULLFUNC |
-				 IEEE80211_FCTL_FROMDS);
-	} else {
+	size = sizeof(*nullfunc);
+	fc = cpu_to_le16(IEEE80211_FTYPE_DATA |
+			 (qos ? IEEE80211_STYPE_QOS_NULLFUNC
+			      : IEEE80211_STYPE_NULLFUNC) |
+			 (fromds ? IEEE80211_FCTL_FROMDS : IEEE80211_FCTL_TODS));
+	if (!qos)
 		size -= 2;
-		fc = cpu_to_le16(IEEE80211_FTYPE_DATA |
-				 IEEE80211_STYPE_NULLFUNC |
-				 IEEE80211_FCTL_FROMDS);
-	}
 
 	skb = dev_alloc_skb(local->hw.extra_tx_headroom + size);
-	if (!skb) {
-		ret = -ENOMEM;
-		goto unlock;
-	}
+	if (!skb)
+		return -ENOMEM;
 
 	skb->dev = dev;
-
 	skb_reserve(skb, local->hw.extra_tx_headroom);
 
 	nullfunc = skb_put(skb, size);
+	memset(nullfunc, 0, size);
 	nullfunc->frame_control = fc;
-	nullfunc->duration_id = 0;
-	memcpy(nullfunc->addr1, sta->sta.addr, ETH_ALEN);
-	if (ieee80211_vif_is_mld(&sdata->vif) && !sta->sta.mlo) {
-		memcpy(nullfunc->addr2, conf->addr, ETH_ALEN);
-		memcpy(nullfunc->addr3, conf->addr, ETH_ALEN);
-	} else {
-		memcpy(nullfunc->addr2, sdata->vif.addr, ETH_ALEN);
-		memcpy(nullfunc->addr3, sdata->vif.addr, ETH_ALEN);
-	}
-	nullfunc->seq_ctrl = 0;
 
-	info = IEEE80211_SKB_CB(skb);
+	memcpy(nullfunc->addr1, peer_addr, ETH_ALEN);
+	memcpy(nullfunc->addr2, src_addr, ETH_ALEN);
+	memcpy(nullfunc->addr3, fromds ? src_addr : peer_addr, ETH_ALEN);
 
+	info = IEEE80211_SKB_CB(skb);
 	info->flags |= IEEE80211_TX_CTL_REQ_TX_STATUS |
 		       IEEE80211_TX_INTFL_NL80211_FRAME_TX;
 	info->band = band;
-
 	info->control.flags |= u32_encode_bits(link_id,
 					       IEEE80211_TX_CTRL_MLO_LINK);
+	if (link_id != IEEE80211_LINK_UNSPECIFIED) {
+		info->status.link_valid = 1;
+		info->status.link_id = link_id;
+	}
+
 	skb_set_queue_mapping(skb, IEEE80211_AC_VO);
 	skb->priority = 7;
 	if (qos)
@@ -5025,18 +5046,14 @@ static int ieee80211_probe_peer(struct wiphy *wiphy, struct net_device *dev,
 	ret = ieee80211_attach_ack_skb(local, skb, cookie, GFP_ATOMIC);
 	if (ret) {
 		kfree_skb(skb);
-		goto unlock;
+		return ret;
 	}
 
 	local_bh_disable();
 	ieee80211_xmit(sdata, sta, skb);
 	local_bh_enable();
 
-	ret = 0;
-unlock:
-	rcu_read_unlock();
-
-	return ret;
+	return 0;
 }
 
 static int ieee80211_cfg_get_channel(struct wiphy *wiphy,
diff --git a/net/mac80211/status.c b/net/mac80211/status.c
index 1bb622d06acf..f80496445f94 100644
--- a/net/mac80211/status.c
+++ b/net/mac80211/status.c
@@ -655,7 +655,10 @@ static void ieee80211_report_ack_skb(struct ieee80211_local *local,
 								GFP_ATOMIC);
 			else if (ieee80211_is_any_nullfunc(hdr->frame_control))
 				cfg80211_probe_status(sdata->dev, hdr->addr1,
-						      cookie, -1, acked,
+						      cookie,
+						      info->status.link_valid ?
+							info->status.link_id : -1,
+						      acked,
 						      info->status.ack_signal,
 						      is_valid_ack_signal,
 						      GFP_ATOMIC);
-- 
2.34.1