recipes-wifi/linux-mt76/files/patches-3.x/0095-wifi-mt76-mt7996-rework-TXS-for-multi-link-support.patch

From 1f35388fa6214a36e5f7a4770c248a187a543019 Mon Sep 17 00:00:00 2001
From: Shayne Chen <shayne.chen@mediatek.com>
Date: Mon, 4 Dec 2023 11:57:38 +0800
Subject: [PATCH 095/116] wifi: mt76: mt7996: rework TXS for multi-link support

This is a preliminary patch to add MLO support for mt7996 chipsets.

Co-developed-by: Bo Jiao <Bo.Jiao@mediatek.com>
Signed-off-by: Bo Jiao <Bo.Jiao@mediatek.com>
Signed-off-by: Shayne Chen <shayne.chen@mediatek.com>
---
 mt7996/mac.c    |  9 +++++----
 mt7996/main.c   |  1 +
 mt7996/mt7996.h | 28 ++++++++++++++++++++++++++++
 3 files changed, 34 insertions(+), 4 deletions(-)

diff --git a/mt7996/mac.c b/mt7996/mac.c
index 0fa3266cc..65431c7d7 100644
--- a/mt7996/mac.c
+++ b/mt7996/mac.c
@@ -1192,7 +1192,7 @@ mt7996_mac_add_txs_skb(struct mt7996_dev *dev, struct mt76_wcid *wcid,
 		struct ieee80211_sta *sta;
 		u8 tid;
 
-		sta = container_of((void *)wcid, struct ieee80211_sta, drv_priv);
+		sta = wcid_to_sta(wcid);
 		tid = FIELD_GET(MT_TXS0_TID, txs);
 		ieee80211_refresh_tx_agg_session_timer(sta, tid);
 	}
@@ -1310,9 +1310,10 @@ static void mt7996_mac_add_txs(struct mt7996_dev *dev, void *data)
 	struct mt76_wcid *wcid;
 	__le32 *txs_data = data;
 	u16 wcidx;
-	u8 pid;
+	u8 band, pid;
 
 	wcidx = le32_get_bits(txs_data[2], MT_TXS2_WCID);
+	band = le32_get_bits(txs_data[2], MT_TXS2_BAND);
 	pid = le32_get_bits(txs_data[3], MT_TXS3_PID);
 
 	if (pid < MT_PACKET_ID_NO_SKB)
@@ -1323,7 +1324,7 @@ static void mt7996_mac_add_txs(struct mt7996_dev *dev, void *data)
 
 	rcu_read_lock();
 
-	wcid = rcu_dereference(dev->mt76.wcid[wcidx]);
+	wcid = mt7996_get_link_wcid(dev, wcidx, band);
 	if (!wcid)
 		goto out;
 
@@ -1332,7 +1333,7 @@ static void mt7996_mac_add_txs(struct mt7996_dev *dev, void *data)
 	if (!wcid->sta)
 		goto out;
 
-	mlink = container_of(wcid, struct mt7996_link_sta, wcid);
+	mlink = wcid_to_mlink(wcid);
 	spin_lock_bh(&dev->mt76.sta_poll_lock);
 	if (list_empty(&mlink->wcid.poll_list))
 		list_add_tail(&mlink->wcid.poll_list, &dev->mt76.sta_poll_list);
diff --git a/mt7996/main.c b/mt7996/main.c
index 3bd97d4aa..aceb77ab2 100644
--- a/mt7996/main.c
+++ b/mt7996/main.c
@@ -2317,6 +2317,7 @@ mt7996_assign_vif_chanctx(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	mconf = mconf_dereference_protected(mvif, link_id);
 	mconf->chanctx = ctx;
 	ctx->nbss_assigned++;
+	mvif->band_to_link[phy->mt76->band_idx] = link_id;
 
 	if (mt7996_hw_phy(hw) == phy)
 		mvif->master_link_id = link_id;
diff --git a/mt7996/mt7996.h b/mt7996/mt7996.h
index 21a95c146..c6ca00f1a 100644
--- a/mt7996/mt7996.h
+++ b/mt7996/mt7996.h
@@ -357,6 +357,8 @@ struct mt7996_vif {
 	u8 master_link_id;
 	u8 group_mld_id;
 	u8 mld_remap_id;
+
+	u8 band_to_link[__MT_MAX_BAND];
 };
 
 /* crash-dump */
@@ -839,6 +841,32 @@ wcid_to_mlink(struct mt76_wcid *wcid)
 	return container_of(wcid, struct mt7996_link_sta, wcid);
 }
 
+static inline struct mt76_wcid *
+mt7996_get_link_wcid(struct mt7996_dev *dev, u16 idx, u8 band_idx)
+{
+	struct mt7996_link_sta *mlink;
+	struct mt76_wcid *wcid;
+	u8 link_id;
+
+	if (!idx || idx >= ARRAY_SIZE(dev->mt76.wcid))
+		return NULL;
+
+	wcid = rcu_dereference(dev->mt76.wcid[idx]);
+	if (!wcid)
+		return NULL;
+
+	if (wcid->phy_idx == band_idx)
+		return wcid;
+
+	mlink = wcid_to_mlink(wcid);
+	link_id = mlink->sta->vif->band_to_link[band_idx];
+	mlink = rcu_dereference(mlink->sta->link[link_id]);
+	if (!mlink)
+		return wcid;
+
+	return &mlink->wcid;
+}
+
 extern const struct ieee80211_ops mt7996_ops;
 extern struct pci_driver mt7996_pci_driver;
 extern struct pci_driver mt7996_hif_driver;
-- 
2.39.2