From f60e775a0f4ca3389d4777b1b2200a5be06cab92 Mon Sep 17 00:00:00 2001
From: Shayne Chen <shayne.chen@mediatek.com>
Date: Wed, 13 Dec 2023 16:58:31 +0800
Subject: [PATCH 102/116] wifi: mt76: mt7996: rework scanning parts for MLD STA
 support

During the first scanning, the STA VIF is still a legacy interface,
and at the moment it doesn't know if it's MLD or not. So do dome tricks
here to let the legacy interface be abled to scan all bands.
This is a preliminary patch to add MLO support for mt7996 chipsets.

Signed-off-by: Shayne Chen <shayne.chen@mediatek.com>
---
 mt7996/mac.c  | 34 +++++++++++++++++++++++++---------
 mt7996/main.c | 17 +++++++++++++++++
 2 files changed, 42 insertions(+), 9 deletions(-)

diff --git a/mt7996/mac.c b/mt7996/mac.c
index 6ca3e06e2..d6d1c0798 100644
--- a/mt7996/mac.c
+++ b/mt7996/mac.c
@@ -2770,23 +2770,40 @@ static void
 mt7996_scan_send_probe(struct mt7996_phy *phy, struct cfg80211_ssid *ssid,
 		       const u8 *dst)
 {
-	struct ieee80211_hw *hw = phy->mt76->hw;
 	struct cfg80211_scan_request *req = phy->scan_req;
 	struct ieee80211_vif *vif = phy->scan_vif;
+	struct ieee80211_bss_conf *conf;
 	struct mt7996_vif *mvif;
 	struct mt7996_link_sta *mlink;
 	struct ieee80211_tx_info *info;
+	struct ieee80211_hw *hw;
 	struct sk_buff *skb;
+	unsigned long valid_links;
+	unsigned int link_id;
 
 	if (!req || !vif)
 		return;
 
+	valid_links = vif->valid_links ?: BIT(0);
 	mvif = (struct mt7996_vif *)vif->drv_priv;
+	hw = mvif->hw;
+
+	rcu_read_lock();
+
+	for_each_set_bit(link_id, &valid_links, IEEE80211_MLD_MAX_NUM_LINKS) {
+		conf = rcu_dereference(vif->link_conf[link_id]);
+		mlink = rcu_dereference(mvif->sta.link[link_id]);
+		if (mlink->wcid.phy_idx != phy->mt76->band_idx)
+			continue;
+	}
 
-	skb = ieee80211_probereq_get(hw, vif->addr,
+	if (unlikely(!conf))
+		goto unlock;
+
+	skb = ieee80211_probereq_get(hw, conf->addr,
 				     ssid->ssid, ssid->ssid_len, req->ie_len);
 	if (!skb)
-		return;
+		goto unlock;
 
 	if (is_unicast_ether_addr(dst)) {
 		struct ieee80211_hdr_3addr *hdr =
@@ -2804,30 +2821,29 @@ mt7996_scan_send_probe(struct mt7996_phy *phy, struct cfg80211_ssid *ssid,
 
 	skb_set_queue_mapping(skb, IEEE80211_AC_VO);
 
-	rcu_read_lock();
-	if (!ieee80211_tx_prepare_skb(mt76_main_hw(phy->mt76), vif, skb,
-				      phy->scan_chan->band,
-				      NULL)) {
+	if (!ieee80211_tx_prepare_skb(hw, vif, skb,
+				      phy->scan_chan->band, NULL)) {
 		rcu_read_unlock();
 		ieee80211_free_txskb(hw, skb);
 		return;
 	}
 
 	local_bh_disable();
-	mlink = rcu_dereference(mvif->sta.link[0]);
 	mt76_tx(phy->mt76, NULL, &mlink->wcid, skb);
 	local_bh_enable();
 
+unlock:
 	rcu_read_unlock();
 }
 
 void mt7996_scan_complete(struct mt7996_phy *phy, bool aborted)
 {
+	struct mt7996_vif *mvif = (struct mt7996_vif *)phy->scan_vif->drv_priv;
 	struct cfg80211_scan_info info = {
 		.aborted = aborted,
 	};
 
-	ieee80211_scan_completed(phy->mt76->hw, &info);
+	ieee80211_scan_completed(mvif->hw, &info);
 	phy->scan_chan = NULL;
 	phy->scan_req = NULL;
 	phy->scan_vif = NULL;
diff --git a/mt7996/main.c b/mt7996/main.c
index 8bf520120..66972080f 100644
--- a/mt7996/main.c
+++ b/mt7996/main.c
@@ -2191,6 +2191,8 @@ mt7996_hw_scan(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 {
 	struct cfg80211_scan_request *req = &hw_req->req;
 	struct mt7996_phy *phy = mt7996_band_phy(hw, req->channels[0]->band);
+	struct mt7996_vif *mvif = (struct mt7996_vif *)vif->drv_priv;
+	int ret;
 
 	mutex_lock(&phy->dev->mt76.mutex);
 	if (WARN_ON(phy->scan_req || phy->scan_chan)) {
@@ -2202,6 +2204,17 @@ mt7996_hw_scan(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	phy->scan_req = req;
 	phy->scan_vif = vif;
 	phy->scan_chan_idx = 0;
+	if (vif->type == NL80211_IFTYPE_STATION && !ieee80211_vif_is_mld(vif) &&
+	    (phy->mt76 != mvif->deflink.phy->mt76)) {
+		phy->mt76->main_phy = hw->priv;
+		mt7996_remove_bss_conf(vif, &vif->bss_conf, &mvif->deflink);
+
+		ret = mt7996_add_bss_conf(phy, vif, &vif->bss_conf);
+		if (ret) {
+			mutex_unlock(&phy->dev->mt76.mutex);
+			return ret;
+		}
+	}
 	mutex_unlock(&phy->dev->mt76.mutex);
 
 	ieee80211_queue_delayed_work(mt76_main_hw(phy->mt76), &phy->scan_work, 0);
@@ -2212,6 +2225,7 @@ mt7996_hw_scan(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 static void
 mt7996_cancel_hw_scan(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 {
+	struct mt7996_vif *mvif = (struct mt7996_vif *)vif->drv_priv;
 	int band;
 
 	for (band = 0; band < NUM_NL80211_BANDS; band++) {
@@ -2229,6 +2243,9 @@ mt7996_cancel_hw_scan(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 
 		mutex_lock(&phy->dev->mt76.mutex);
 		mt7996_scan_complete(phy, true);
+		if (vif->type == NL80211_IFTYPE_STATION && !ieee80211_vif_is_mld(vif) &&
+		    (phy->mt76 != mvif->deflink.phy->mt76))
+			phy->mt76->main_phy = NULL;
 		mutex_unlock(&phy->dev->mt76.mutex);
 	}
 }
-- 
2.39.2