[PATCH wireless-next v2 3/4] wifi: mac80211: add per-link PROBE_PEER support

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

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

Extend ieee80211_probe_peer() to support STA/P2P-client mode. Reject the
request if a peer MAC address is provided or if the interface is not
associated.

For MLO STA/P2P-client, construct the nullfunc using MLD addresses and
set IEEE80211_LINK_UNSPECIFIED in the TX control flags so the driver can
choose the link. For non-MLO STA/P2P-client, use link 0 together with
the corresponding per-link BSSID and link address.

In the TX status path, report probe status for STA/P2P-client mode
without a peer address and include the transmitted link in
cfg80211_probe_status(). Add link_valid/link_id bitfields to
struct ieee80211_tx_info.status so drivers can report the link used
for the completed TX.

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

diff --git a/include/net/mac80211.h b/include/net/mac80211.h
index 40cb20d9309c..f93d15c61a52 100644
--- a/include/net/mac80211.h
+++ b/include/net/mac80211.h
@@ -1364,7 +1364,7 @@ struct ieee80211_tx_info {
 			u8 pad;
 			u16 tx_time;
 			u8 flags;
-			u8 pad2;
+			u8 link_valid:1, link_id:4, pad2:3;
 			void *status_driver_data[16 / sizeof(void *)];
 		} status;
 		struct {
diff --git a/net/mac80211/cfg.c b/net/mac80211/cfg.c
index abd0ac20f0da..f5b3c9bd67f8 100644
--- a/net/mac80211/cfg.c
+++ b/net/mac80211/cfg.c
@@ -4753,101 +4753,174 @@ 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;
+	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 = peer;
+	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;
+	/* STA/P2P: userspace must NOT provide peer MAC, AP is implied.	 */
+	if (sdata->vif.type == NL80211_IFTYPE_STATION ||
+	    sdata->vif.type == NL80211_IFTYPE_P2P_CLIENT) {
+		if (peer)
+			return -EINVAL;
+
+		if (!sdata->u.mgd.associated)
+			return -ENOTCONN;
+	} else if (!peer) {
+		/* AP/GO: must have a peer MAC. */
+		return -EINVAL;
 	}
 
-	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;
+	if (sdata->vif.type == NL80211_IFTYPE_AP ||
+	    sdata->vif.type == 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;
 
-			if (unlikely(!conf)) {
-				ret = -ENOLINK;
-				goto unlock;
+		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,
+				 * 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;
 			}
+			/* 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;
+		}
+
+		size = sizeof(*nullfunc);
+		fc = cpu_to_le16(IEEE80211_FTYPE_DATA |
+				 (qos ? IEEE80211_STYPE_QOS_NULLFUNC
+				      : IEEE80211_STYPE_NULLFUNC) |
+				 IEEE80211_FCTL_FROMDS);
+		if (!qos)
+			size -= 2;
+
+		skb = dev_alloc_skb(local->hw.extra_tx_headroom + size);
+		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;
+
+		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);
+		}
+
+		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);
+
+		skb_set_queue_mapping(skb, IEEE80211_AC_VO);
+		skb->priority = 7;
+		if (qos)
+			nullfunc->qos_ctrl = cpu_to_le16(7);
+
+		ret = ieee80211_attach_ack_skb(local, skb, cookie, GFP_ATOMIC);
+		if (ret) {
+			kfree_skb(skb);
+			return ret;
 		}
-		/* MLD transmissions must not rely on the band */
+
+		local_bh_disable();
+		ieee80211_xmit(sdata, sta, skb);
+		local_bh_enable();
+
+		return 0;
+	}
+
+	/*
+	 * STA/P2P: send a nullfunc to probe the AP/peer.
+	 * For MLO, let the driver/firmware decide which link to use.
+	 */
+	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 {
-		chanctx_conf = rcu_dereference(sdata->vif.bss_conf.chanctx_conf);
-		if (WARN_ON(!chanctx_conf)) {
-			ret = -EINVAL;
-			goto unlock;
-		}
-		band = chanctx_conf->def.chan->band;
 		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);
 	}
 
-	if (qos) {
-		fc = cpu_to_le16(IEEE80211_FTYPE_DATA |
-				 IEEE80211_STYPE_QOS_NULLFUNC |
-				 IEEE80211_FCTL_FROMDS);
-	} else {
+	qos = sta ? sta->sta.wme : false;
+
+	size = sizeof(*nullfunc);
+	fc = cpu_to_le16(IEEE80211_FTYPE_DATA |
+			 (qos ? IEEE80211_STYPE_QOS_NULLFUNC
+			      : IEEE80211_STYPE_NULLFUNC) |
+			 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, 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);
+
 	skb_set_queue_mapping(skb, IEEE80211_AC_VO);
 	skb->priority = 7;
 	if (qos)
@@ -4856,18 +4929,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 4a64ac6d1451..e049ab946e7b 100644
--- a/net/mac80211/status.c
+++ b/net/mac80211/status.c
@@ -622,7 +622,8 @@ static void ieee80211_report_ack_skb(struct ieee80211_local *local,
 		return;
 
 	if (info->flags & IEEE80211_TX_INTFL_NL80211_FRAME_TX) {
-		u64 cookie = IEEE80211_SKB_CB(skb)->ack.cookie;
+		struct ieee80211_tx_info *skb_info = IEEE80211_SKB_CB(skb);
+		u64 cookie = skb_info->ack.cookie;
 		struct ieee80211_sub_if_data *sdata;
 		struct ieee80211_hdr *hdr = (void *)skb->data;
 		bool is_valid_ack_signal =
@@ -652,12 +653,33 @@ static void ieee80211_report_ack_skb(struct ieee80211_local *local,
 								skb->len,
 								acked,
 								GFP_ATOMIC);
-			else if (ieee80211_is_any_nullfunc(hdr->frame_control))
-				cfg80211_probe_status(sdata->dev, hdr->addr1,
-						      cookie, -1, acked,
+			else if (ieee80211_is_any_nullfunc(hdr->frame_control)) {
+				const u8 *peer_addr;
+				int link_id = -1;
+
+				/*
+				 * STA/P2P client: peer_addr omitted;
+				 * AP/GO: report RA
+				 */
+				if (sdata->vif.type ==
+				    NL80211_IFTYPE_STATION ||
+				    sdata->vif.type ==
+				    NL80211_IFTYPE_P2P_CLIENT) {
+					peer_addr = NULL;
+
+					if (ieee80211_vif_is_mld(&sdata->vif) &&
+					    info->status.link_valid)
+						link_id = info->status.link_id;
+				} else {
+					peer_addr = hdr->addr1;
+				}
+
+				cfg80211_probe_status(sdata->dev, peer_addr,
+						      cookie, link_id, acked,
 						      info->status.ack_signal,
 						      is_valid_ack_signal,
 						      GFP_ATOMIC);
+			}
 			else if (ieee80211_is_mgmt(hdr->frame_control))
 				cfg80211_mgmt_tx_status_ext(&sdata->wdev,
 							    &status,
-- 
2.34.1