wifi: mt76: mt7996: Fix mt7996_mcu_bss_mld_tlv routine

Update mt7996_mcu_bss_mld_tlv routine to properly support MLO
configuring the BSS.

Fixes: 98686cd21624c ("wifi: mt76: mt7996: add driver for MediaTek Wi-Fi 7 (802.11be) devices")
Signed-off-by: Lorenzo Bianconi <lorenzo@kernel.org>
Link: https://patch.msgid.link/20250710-mt7996-mlo-fixes-v3-v1-1-e7595b089f2c@kernel.org
Signed-off-by: Felix Fietkau <nbd@nbd.name>

diff --git a/drivers/net/wireless/mediatek/mt76/mt7996/main.c b/drivers/net/wireless/mediatek/mt76/mt7996/main.c
index e4a1b05ddfee83d8ce3465af0eabb81ec18c9a43..9bf66e112fd190dc84171982e8c4ecd7f6919f56 100644 (file)
--- a/drivers/net/wireless/mediatek/mt76/mt7996/main.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7996/main.c
@@ -138,6 +138,28 @@ static int get_omac_idx(enum nl80211_iftype type, u64 mask)
        return -1;
 }
 
+static int get_own_mld_idx(u64 mask, bool group_mld)
+{
+       u8 start = group_mld ? 0 : 16;
+       u8 end = group_mld ? 15 : 63;
+       int idx;
+
+       idx = get_free_idx(mask, start, end);
+       if (idx)
+               return idx - 1;
+
+       /* If the 16-63 range is not available, perform another lookup in the
+        * range 0-15
+        */
+       if (!group_mld) {
+               idx = get_free_idx(mask, 0, 15);
+               if (idx)
+                       return idx - 1;
+       }
+
+       return -EINVAL;
+}
+
 static void
 mt7996_init_bitrate_mask(struct ieee80211_vif *vif, struct mt7996_vif_link *mlink)
 {
@@ -279,7 +301,7 @@ int mt7996_vif_link_add(struct mt76_phy *mphy, struct ieee80211_vif *vif,
        struct mt7996_dev *dev = phy->dev;
        u8 band_idx = phy->mt76->band_idx;
        struct mt76_txq *mtxq;
-       int idx, ret;
+       int mld_idx, idx, ret;
 
        mlink->idx = __ffs64(~dev->mt76.vif_mask);
        if (mlink->idx >= mt7996_max_interface_num(dev))
@@ -289,6 +311,17 @@ int mt7996_vif_link_add(struct mt76_phy *mphy, struct ieee80211_vif *vif,
        if (idx < 0)
                return -ENOSPC;
 
+       if (!dev->mld_idx_mask) { /* first link in the group */
+               mvif->mld_group_idx = get_own_mld_idx(dev->mld_idx_mask, true);
+               mvif->mld_remap_idx = get_free_idx(dev->mld_remap_idx_mask,
+                                                  0, 15);
+       }
+
+       mld_idx = get_own_mld_idx(dev->mld_idx_mask, false);
+       if (mld_idx < 0)
+               return -ENOSPC;
+
+       link->mld_idx = mld_idx;
        link->phy = phy;
        mlink->omac_idx = idx;
        mlink->band_idx = band_idx;
@@ -301,6 +334,11 @@ int mt7996_vif_link_add(struct mt76_phy *mphy, struct ieee80211_vif *vif,
                return ret;
 
        dev->mt76.vif_mask |= BIT_ULL(mlink->idx);
+       if (!dev->mld_idx_mask) {
+               dev->mld_idx_mask |= BIT_ULL(mvif->mld_group_idx);
+               dev->mld_remap_idx_mask |= BIT_ULL(mvif->mld_remap_idx);
+       }
+       dev->mld_idx_mask |= BIT_ULL(link->mld_idx);
        phy->omac_mask |= BIT_ULL(mlink->omac_idx);
 
        idx = MT7996_WTBL_RESERVED - mlink->idx;
@@ -380,7 +418,13 @@ void mt7996_vif_link_remove(struct mt76_phy *mphy, struct ieee80211_vif *vif,
        }
 
        dev->mt76.vif_mask &= ~BIT_ULL(mlink->idx);
+       dev->mld_idx_mask &= ~BIT_ULL(link->mld_idx);
        phy->omac_mask &= ~BIT_ULL(mlink->omac_idx);
+       if (!(dev->mld_idx_mask & ~BIT_ULL(mvif->mld_group_idx))) {
+               /* last link */
+               dev->mld_idx_mask &= ~BIT_ULL(mvif->mld_group_idx);
+               dev->mld_remap_idx_mask &= ~BIT_ULL(mvif->mld_remap_idx);
+       }
 
        spin_lock_bh(&dev->mt76.sta_poll_lock);
        if (!list_empty(&msta_link->wcid.poll_list))

diff --git a/drivers/net/wireless/mediatek/mt76/mt7996/mcu.c b/drivers/net/wireless/mediatek/mt76/mt7996/mcu.c
index e6db3e0b2ffdaf1633da1bafc77e27d923661e0b..aad58f7831c7b2c62f2185c0010640a03479b599 100644 (file)
--- a/drivers/net/wireless/mediatek/mt76/mt7996/mcu.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7996/mcu.c
@@ -899,17 +899,28 @@ mt7996_mcu_bss_txcmd_tlv(struct sk_buff *skb, bool en)
 }
 
 static void
-mt7996_mcu_bss_mld_tlv(struct sk_buff *skb, struct mt76_vif_link *mlink)
+mt7996_mcu_bss_mld_tlv(struct sk_buff *skb,
+                      struct ieee80211_bss_conf *link_conf,
+                      struct mt7996_vif_link *link)
 {
+       struct ieee80211_vif *vif = link_conf->vif;
+       struct mt7996_vif *mvif = (struct mt7996_vif *)vif->drv_priv;
        struct bss_mld_tlv *mld;
        struct tlv *tlv;
 
        tlv = mt7996_mcu_add_uni_tlv(skb, UNI_BSS_INFO_MLD, sizeof(*mld));
-
        mld = (struct bss_mld_tlv *)tlv;
-       mld->group_mld_id = 0xff;
-       mld->own_mld_id = mlink->idx;
-       mld->remap_idx = 0xff;
+       mld->own_mld_id = link->mld_idx;
+       mld->link_id = link_conf->link_id;
+
+       if (ieee80211_vif_is_mld(vif)) {
+               mld->group_mld_id = mvif->mld_group_idx;
+               mld->remap_idx = mvif->mld_remap_idx;
+               memcpy(mld->mac_addr, vif->addr, ETH_ALEN);
+       } else {
+               mld->group_mld_id = 0xff;
+               mld->remap_idx = 0xff;
+       }
 }
 
 static void
@@ -1108,6 +1119,8 @@ int mt7996_mcu_add_bss_info(struct mt7996_phy *phy, struct ieee80211_vif *vif,
                goto out;
 
        if (enable) {
+               struct mt7996_vif_link *link;
+
                mt7996_mcu_bss_rfch_tlv(skb, phy);
                mt7996_mcu_bss_bmc_tlv(skb, mlink, phy);
                mt7996_mcu_bss_ra_tlv(skb, phy);
@@ -1118,7 +1131,8 @@ int mt7996_mcu_add_bss_info(struct mt7996_phy *phy, struct ieee80211_vif *vif,
                        mt7996_mcu_bss_he_tlv(skb, vif, link_conf, phy);
 
                /* this tag is necessary no matter if the vif is MLD */
-               mt7996_mcu_bss_mld_tlv(skb, mlink);
+               link = container_of(mlink, struct mt7996_vif_link, mt76);
+               mt7996_mcu_bss_mld_tlv(skb, link_conf, link);
        }
 
        mt7996_mcu_bss_mbssid_tlv(skb, link_conf, enable);

diff --git a/drivers/net/wireless/mediatek/mt76/mt7996/mcu.h b/drivers/net/wireless/mediatek/mt76/mt7996/mcu.h
index 130ea95626d5b1aacf748d572c62307f19681851..7b21d6ae7e43505cb824e00f8b55b0add015a5e5 100644 (file)
--- a/drivers/net/wireless/mediatek/mt76/mt7996/mcu.h
+++ b/drivers/net/wireless/mediatek/mt76/mt7996/mcu.h
@@ -481,7 +481,8 @@ struct bss_mld_tlv {
        u8 own_mld_id;
        u8 mac_addr[ETH_ALEN];
        u8 remap_idx;
-       u8 __rsv[3];
+       u8 link_id;
+       u8 __rsv[2];
 } __packed;
 
 struct sta_rec_ht_uni {

diff --git a/drivers/net/wireless/mediatek/mt76/mt7996/mt7996.h b/drivers/net/wireless/mediatek/mt76/mt7996/mt7996.h
index bbd4679edc9d315de99e8a747f31b843b795b64a..b98cfe6e5be8c6b77357cf7d2a76135f10609ca4 100644 (file)
--- a/drivers/net/wireless/mediatek/mt76/mt7996/mt7996.h
+++ b/drivers/net/wireless/mediatek/mt76/mt7996/mt7996.h
@@ -248,11 +248,16 @@ struct mt7996_vif_link {
 
        struct ieee80211_tx_queue_params queue_params[IEEE80211_NUM_ACS];
        struct cfg80211_bitrate_mask bitrate_mask;
+
+       u8 mld_idx;
 };
 
 struct mt7996_vif {
        struct mt7996_vif_link deflink; /* must be first */
        struct mt76_vif_data mt76;
+
+       u8 mld_group_idx;
+       u8 mld_remap_idx;
 };
 
 /* crash-dump */
@@ -337,6 +342,9 @@ struct mt7996_dev {
        u32 q_int_mask[MT7996_MAX_QUEUE];
        u32 q_wfdma_mask;
 
+       u64 mld_idx_mask;
+       u64 mld_remap_idx_mask;
+
        const struct mt76_bus_ops *bus_ops;
        struct mt7996_phy phy;