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 from 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 --- .../net/wireless/mediatek/mt76/mt7996/mac.c | 34 ++++++++++++++++--- .../net/wireless/mediatek/mt76/mt7996/main.c | 6 ++++ .../net/wireless/mediatek/mt76/mt7996/mcu.c | 4 +-- .../wireless/mediatek/mt76/mt7996/mt7996.h | 1 + 4 files changed, 39 insertions(+), 6 deletions(-) diff --git a/drivers/net/wireless/mediatek/mt76/mt7996/mac.c b/drivers/net/wireless/mediatek/mt76/mt7996/mac.c index 7d5f9d9b3b62..579084cbb30f 100644 --- 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 c26f8f49ce44..3958688d622b 100644 --- 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); diff --git a/drivers/net/wireless/mediatek/mt76/mt7996/mcu.c b/drivers/net/wireless/mediatek/mt76/mt7996/mcu.c index f1892aaf6a91..2bb98df317a6 100644 --- 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 65eeb37ab8c7..c47820699ec8 100644 --- 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; }; -- 2.51.0