wifi: mt76: mt7996: use correct link_id when filling TXD and TXP

Obtain the correct link ID and, if needed, switch to the corresponding
wcid before populating the TX descriptor and TX payload.

Rules for link id:
- For QoS data of MLD peers (excluding EAPOL), select the primary or
  secondary wcid based on whether the TID is odd or even to meet FW/HW
  requirements
- For other packets, use IEEE80211_TX_CTRL_MLO_LINK if specified
  (such as multicast and broadcast packets)

Signed-off-by: Shayne Chen <shayne.chen@mediatek.com>
Acked-by: Lorenzo Bianconi <lorenzo@kernel.org>
Link: https://patch.msgid.link/20251106064203.1000505-8-shayne.chen@mediatek.com
Signed-off-by: Felix Fietkau <nbd@nbd.name>

diff --git a/drivers/net/wireless/mediatek/mt76/mt7996/mac.c b/drivers/net/wireless/mediatek/mt76/mt7996/mac.c
index 7d5f9d9b3b624908ee7d1ca838502063b4870b13..579084cbb30f3c1c2815bd57ff5df880d702a202 100644 (file)
--- a/drivers/net/wireless/mediatek/mt76/mt7996/mac.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7996/mac.c
@@ -1035,15 +1035,20 @@ int mt7996_tx_prepare_skb(struct mt76_dev *mdev, void *txwi_ptr,
                          struct ieee80211_sta *sta,
                          struct mt76_tx_info *tx_info)
 {
+       struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)tx_info->skb->data;
        struct mt7996_dev *dev = container_of(mdev, struct mt7996_dev, mt76);
        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 = vif ? (struct mt7996_vif *)vif->drv_priv : NULL;
+       struct mt7996_sta *msta = sta ? (struct mt7996_sta *)sta->drv_priv : NULL;
+       struct mt76_vif_link *mlink = NULL;
        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;
        __le32 *ptr = (__le32 *)txwi_ptr;
        u8 *txwi = (u8 *)txwi_ptr;
+       u8 link_id;
 
        if (unlikely(tx_info->skb->len <= ETH_HLEN))
                return -EINVAL;
@@ -1051,6 +1056,30 @@ int mt7996_tx_prepare_skb(struct mt76_dev *mdev, void *txwi_ptr,
        if (!wcid)
                wcid = &dev->mt76.global_wcid;
 
+       if ((is_8023 || ieee80211_is_data_qos(hdr->frame_control)) && sta->mlo &&
+           likely(tx_info->skb->protocol != cpu_to_be16(ETH_P_PAE))) {
+               u8 tid = tx_info->skb->priority & IEEE80211_QOS_CTL_TID_MASK;
+
+               link_id = (tid % 2) ? msta->seclink_id : msta->deflink_id;
+       } else {
+               link_id = u32_get_bits(info->control.flags,
+                                      IEEE80211_TX_CTRL_MLO_LINK);
+       }
+
+       if (link_id != wcid->link_id && link_id != IEEE80211_LINK_UNSPECIFIED) {
+               if (msta) {
+                       struct mt7996_sta_link *msta_link =
+                               rcu_dereference(msta->link[link_id]);
+
+                       if (msta_link)
+                               wcid = &msta_link->wcid;
+               } else if (mvif) {
+                       mlink = rcu_dereference(mvif->mt76.link[link_id]);
+                       if (mlink && mlink->wcid)
+                               wcid = mlink->wcid;
+               }
+       }
+
        t = (struct mt76_txwi_cache *)(txwi + mdev->drv->txwi_size);
        t->skb = tx_info->skb;
 
@@ -1155,10 +1184,7 @@ int mt7996_tx_prepare_skb(struct mt76_dev *mdev, void *txwi_ptr,
                if (!is_8023 && mt7996_tx_use_mgmt(dev, tx_info->skb))
                        txp->fw.flags |= cpu_to_le16(MT_CT_INFO_MGMT_FRAME);
 
-               if (vif) {
-                       struct mt7996_vif *mvif = (struct mt7996_vif *)vif->drv_priv;
-                       struct mt76_vif_link *mlink = NULL;
-
+               if (mvif) {
                        if (wcid->offchannel)
                                mlink = rcu_dereference(mvif->mt76.offchannel_link);
                        if (!mlink)

diff --git a/drivers/net/wireless/mediatek/mt76/mt7996/main.c b/drivers/net/wireless/mediatek/mt76/mt7996/main.c
index c26f8f49ce44a4420d0c915ac7b4164987c7f2ca..ef605e81cba9aca90fb881022ceeb6bdabb7dc70 100644 (file)
--- a/drivers/net/wireless/mediatek/mt76/mt7996/main.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7996/main.c
@@ -963,6 +963,7 @@ mt7996_mac_sta_init_link(struct mt7996_dev *dev,
 
                msta_link = &msta->deflink;
                msta->deflink_id = link_id;
+               msta->seclink_id = msta->deflink_id;
 
                for (i = 0; i < ARRAY_SIZE(sta->txq); i++) {
                        struct mt76_txq *mtxq;
@@ -977,6 +978,11 @@ mt7996_mac_sta_init_link(struct mt7996_dev *dev,
                msta_link = kzalloc(sizeof(*msta_link), GFP_KERNEL);
                if (!msta_link)
                        return -ENOMEM;
+
+               if (msta->seclink_id == msta->deflink_id &&
+                   (sta->valid_links & ~BIT(msta->deflink_id)))
+                       msta->seclink_id = __ffs(sta->valid_links &
+                                                ~BIT(msta->deflink_id));
        }
 
        INIT_LIST_HEAD(&msta_link->rc_list);
@@ -1051,6 +1057,8 @@ mt7996_mac_sta_remove_links(struct mt7996_dev *dev, struct ieee80211_vif *vif,
                if (msta->deflink_id == link_id) {
                        msta->deflink_id = IEEE80211_LINK_UNSPECIFIED;
                        continue;
+               } else if (msta->seclink_id == link_id) {
+                       msta->seclink_id = IEEE80211_LINK_UNSPECIFIED;
                }
 
                kfree_rcu(msta_link, rcu_head);
@@ -1146,6 +1154,7 @@ mt7996_mac_sta_add(struct mt7996_dev *dev, struct ieee80211_vif *vif,
        mutex_lock(&dev->mt76.mutex);
 
        msta->deflink_id = IEEE80211_LINK_UNSPECIFIED;
+       msta->seclink_id = IEEE80211_LINK_UNSPECIFIED;
        msta->vif = mvif;
        err = mt7996_mac_sta_add_links(dev, vif, sta, links);
 

diff --git a/drivers/net/wireless/mediatek/mt76/mt7996/mcu.c b/drivers/net/wireless/mediatek/mt76/mt7996/mcu.c
index f1892aaf6a91d7c404d8d55538d8738f935047cc..2bb98df317a6186b74465b36fc8e1e36186793d5 100644 (file)
--- a/drivers/net/wireless/mediatek/mt76/mt7996/mcu.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7996/mcu.c
@@ -2394,8 +2394,8 @@ mt7996_mcu_sta_mld_setup_tlv(struct mt7996_dev *dev, struct sk_buff *skb,
        mld_setup->primary_id = cpu_to_le16(msta_link->wcid.idx);
 
        if (nlinks > 1) {
-               link_id = __ffs(sta->valid_links & ~BIT(msta->deflink_id));
-               msta_link = mt76_dereference(msta->link[link_id], &dev->mt76);
+               msta_link = mt76_dereference(msta->link[msta->seclink_id],
+                                            &dev->mt76);
                if (!msta_link)
                        return;
        }

diff --git a/drivers/net/wireless/mediatek/mt76/mt7996/mt7996.h b/drivers/net/wireless/mediatek/mt76/mt7996/mt7996.h
index 65eeb37ab8c7262afad737edb197018144cd65f6..c47820699ec8880687c9df35dddb38b235cb3154 100644 (file)
--- a/drivers/net/wireless/mediatek/mt76/mt7996/mt7996.h
+++ b/drivers/net/wireless/mediatek/mt76/mt7996/mt7996.h
@@ -243,6 +243,7 @@ struct mt7996_sta {
        struct mt7996_sta_link deflink; /* must be first */
        struct mt7996_sta_link __rcu *link[IEEE80211_MLD_MAX_NUM_LINKS];
        u8 deflink_id;
+       u8 seclink_id;
 
        struct mt7996_vif *vif;
 };