autobuild_mac80211_release/mt7988_mt7996_mac80211_mlo/package/kernel/mt76/patches/0109-wifi-mt76-mt7996-rework-TXD-for-multi-link-support.patch

From 78eed11814955fbb83bd1adb71a46aef5e9b98b7 Mon Sep 17 00:00:00 2001
From: Shayne Chen <shayne.chen@mediatek.com>
Date: Mon, 4 Dec 2023 11:25:54 +0800
Subject: [PATCH 109/120] wifi: mt76: mt7996: rework TXD 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    | 88 +++++++++++++++++++++++++++++++++++--------------
 mt7996/mt7996.h |  9 +++++
 2 files changed, 73 insertions(+), 24 deletions(-)

diff --git a/mt7996/mac.c b/mt7996/mac.c
index 2f5a57f53..f6f028824 100644
--- a/mt7996/mac.c
+++ b/mt7996/mac.c
@@ -621,9 +621,8 @@ mt7996_mac_write_txwi_8023(struct mt7996_dev *dev, __le32 *txwi,
 	u32 val;
 
 	if (wcid->sta) {
-		struct ieee80211_sta *sta;
+		struct ieee80211_sta *sta = wcid_to_sta(wcid);
 
-		sta = container_of((void *)wcid, struct ieee80211_sta, drv_priv);
 		wmm = sta->wme;
 	}
 
@@ -724,6 +723,10 @@ mt7996_mac_write_txwi_80211(struct mt7996_dev *dev, __le32 *txwi,
 		txwi[3] |= cpu_to_le32(val);
 		txwi[3] &= ~cpu_to_le32(MT_TXD3_HW_AMSDU);
 	}
+
+	if (ieee80211_vif_is_mld(info->control.vif) &&
+	    (multicast || unlikely(skb->protocol == cpu_to_be16(ETH_P_PAE))))
+		txwi[5] |= cpu_to_le32(MT_TXD5_FL);
 }
 
 void mt7996_mac_write_txwi(struct mt7996_dev *dev, __le32 *txwi,
@@ -733,10 +736,12 @@ void mt7996_mac_write_txwi(struct mt7996_dev *dev, __le32 *txwi,
 {
 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 	struct ieee80211_vif *vif = info->control.vif;
+	struct mt7996_vif *mvif = (struct mt7996_vif *)vif->drv_priv;
+	struct mt7996_bss_conf *mconf;
 	u8 band_idx = (info->hw_queue & MT_TX_HW_QUEUE_PHY) >> 2;
 	u8 p_fmt, q_idx, omac_idx = 0, wmm_idx = 0;
+	u8 link_id;
 	bool is_8023 = info->flags & IEEE80211_TX_CTL_HW_80211_ENCAP;
-	struct mt76_vif *mvif;
 	u16 tx_count = 15;
 	u32 val;
 	bool inband_disc = !!(changed & (BSS_CHANGED_UNSOL_BCAST_PROBE_RESP |
@@ -744,11 +749,16 @@ void mt7996_mac_write_txwi(struct mt7996_dev *dev, __le32 *txwi,
 	bool beacon = !!(changed & (BSS_CHANGED_BEACON |
 				    BSS_CHANGED_BEACON_ENABLED)) && (!inband_disc);
 
-	mvif = vif ? (struct mt76_vif *)vif->drv_priv : NULL;
-	if (mvif) {
-		omac_idx = mvif->omac_idx;
-		wmm_idx = mvif->wmm_idx;
-		band_idx = mvif->band_idx;
+	if (likely(wcid != &dev->mt76.global_wcid))
+		link_id = wcid->link_id;
+	else
+		link_id = u32_get_bits(info->control.flags, IEEE80211_TX_CTRL_MLO_LINK);
+
+	mconf = rcu_dereference(mvif->link[link_id]);
+	if (mconf) {
+		omac_idx = mconf->mt76.omac_idx;
+		wmm_idx = mconf->mt76.wmm_idx;
+		band_idx = mconf->mt76.band_idx;
 	}
 
 	if (inband_disc) {
@@ -795,7 +805,10 @@ void mt7996_mac_write_txwi(struct mt7996_dev *dev, __le32 *txwi,
 		val |= MT_TXD5_TX_STATUS_HOST;
 	txwi[5] = cpu_to_le32(val);
 
-	val = MT_TXD6_DIS_MAT | MT_TXD6_DAS;
+	val = MT_TXD6_DAS;
+	if ((q_idx >= MT_LMAC_ALTX0 && q_idx <= MT_LMAC_BCN0))
+		val |= MT_TXD6_DIS_MAT;
+
 	if (is_mt7996(&dev->mt76))
 		val |= FIELD_PREP(MT_TXD6_MSDU_CNT, 1);
 	else
@@ -814,16 +827,18 @@ void mt7996_mac_write_txwi(struct mt7996_dev *dev, __le32 *txwi,
 			     is_multicast_ether_addr(hdr->addr1);
 		u8 idx = MT7996_BASIC_RATES_TBL;
 
-		if (mvif) {
-			if (mcast && mvif->mcast_rates_idx)
-				idx = mvif->mcast_rates_idx;
-			else if (beacon && mvif->beacon_rates_idx)
-				idx = mvif->beacon_rates_idx;
+		if (mconf) {
+			if (mcast && mconf->mt76.mcast_rates_idx)
+				idx = mconf->mt76.mcast_rates_idx;
+			else if (beacon && mconf->mt76.beacon_rates_idx)
+				idx = mconf->mt76.beacon_rates_idx;
 			else
-				idx = mvif->basic_rates_idx;
+				idx = mconf->mt76.basic_rates_idx;
 		}
 
 		val = FIELD_PREP(MT_TXD6_TX_RATE, idx) | MT_TXD6_FIXED_BW;
+		if (mcast)
+			val |= MT_TXD6_DIS_MAT;
 		txwi[6] |= cpu_to_le32(val);
 		txwi[3] |= cpu_to_le32(MT_TXD3_BA_DISABLE);
 	}
@@ -839,17 +854,48 @@ int mt7996_tx_prepare_skb(struct mt76_dev *mdev, void *txwi_ptr,
 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx_info->skb);
 	struct ieee80211_key_conf *key = info->control.hw_key;
 	struct ieee80211_vif *vif = info->control.vif;
+	struct mt7996_vif *mvif = (struct mt7996_vif *)vif->drv_priv;
+	struct mt7996_sta *msta;
+	struct mt7996_bss_conf *mconf;
 	struct mt76_connac_txp_common *txp;
 	struct mt76_txwi_cache *t;
 	int id, i, pid, nbuf = tx_info->nbuf - 1;
 	bool is_8023 = info->flags & IEEE80211_TX_CTL_HW_80211_ENCAP;
 	u8 *txwi = (u8 *)txwi_ptr;
+	u8 link_id;
 
 	if (unlikely(tx_info->skb->len <= ETH_HLEN))
 		return -EINVAL;
 
-	if (!wcid)
-		wcid = &dev->mt76.global_wcid;
+	if (WARN_ON(!wcid))
+		return -EINVAL;
+
+	msta = sta ? (struct mt7996_sta *)sta->drv_priv : &mvif->sta;
+	if (ieee80211_is_data_qos(hdr->frame_control) && sta->mlo) {
+		if (unlikely(tx_info->skb->protocol == cpu_to_be16(ETH_P_PAE))) {
+			link_id = msta->pri_link;
+		} else {
+			u8 tid = tx_info->skb->priority & IEEE80211_QOS_CTL_TID_MASK;
+
+			link_id = (tid % 2) ? msta->sec_link : msta->pri_link;
+		}
+	} else {
+		link_id = u32_get_bits(info->control.flags, IEEE80211_TX_CTRL_MLO_LINK);
+
+		if (link_id == IEEE80211_LINK_UNSPECIFIED || (sta && !sta->mlo))
+			link_id = wcid->link_id;
+	}
+
+	if (link_id != wcid->link_id) {
+		struct mt7996_link_sta *mlink = rcu_dereference(msta->link[link_id]);
+
+		if (mlink)
+			wcid = &mlink->wcid;
+	}
+
+	mconf = rcu_dereference(mvif->link[wcid->link_id]);
+	if (!mconf)
+		return -EINVAL;
 
 	t = (struct mt76_txwi_cache *)(txwi + mdev->drv->txwi_size);
 	t->skb = tx_info->skb;
@@ -894,13 +940,7 @@ int mt7996_tx_prepare_skb(struct mt76_dev *mdev, void *txwi_ptr,
 	if (!is_8023 && ieee80211_is_mgmt(hdr->frame_control))
 		txp->fw.flags |= cpu_to_le16(MT_CT_INFO_MGMT_FRAME);
 
-	if (vif) {
-		struct mt7996_vif *mvif = (struct mt7996_vif *)vif->drv_priv;
-		struct mt7996_bss_conf *mconf = &mvif->deflink;
-
-		txp->fw.bss_idx = mconf->mt76.idx;
-	}
-
+	txp->fw.bss_idx = mconf->mt76.idx;
 	txp->fw.token = cpu_to_le16(id);
 	txp->fw.rept_wds_wcid = cpu_to_le16(sta ? wcid->idx : 0xfff);
 
diff --git a/mt7996/mt7996.h b/mt7996/mt7996.h
index 2932ab8d5..664d6d2ec 100644
--- a/mt7996/mt7996.h
+++ b/mt7996/mt7996.h
@@ -826,6 +826,15 @@ mlink_dereference_protected(struct mt7996_sta *msta, u8 link_id)
 					 lockdep_is_held(&msta->vif->dev->mt76.mutex));
 }
 
+static inline struct mt7996_link_sta *
+wcid_to_mlink(struct mt76_wcid *wcid)
+{
+	if (!wcid)
+		return NULL;
+
+	return container_of(wcid, struct mt7996_link_sta, 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