wifi: mt76: mt7925: update mt7925_mcu_sta_mld_tlv for MLO

Update mt7925_mcu_sta_mld_tlv for the MLO-enabled firmware.

Co-developed-by: Ming Yen Hsieh <mingyen.hsieh@mediatek.com>
Signed-off-by: Ming Yen Hsieh <mingyen.hsieh@mediatek.com>
Co-developed-by: Deren Wu <deren.wu@mediatek.com>
Signed-off-by: Deren Wu <deren.wu@mediatek.com>
Signed-off-by: Sean Wang <sean.wang@mediatek.com>
Link: https://patch.msgid.link/25ea3ffd3875083577ab440ec63f3dcd9dc233f4.1720248331.git.sean.wang@kernel.org
Signed-off-by: Felix Fietkau <nbd@nbd.name>

diff --git a/drivers/net/wireless/mediatek/mt76/mt7925/mcu.c b/drivers/net/wireless/mediatek/mt76/mt7925/mcu.c
index da3192bd82cdbaf2f13439efb882cc1ec354f58b..59ae1fbc238b0386376d276aa66c4cf4dbc37e15 100644 (file)
--- a/drivers/net/wireless/mediatek/mt76/mt7925/mcu.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7925/mcu.c
@@ -1742,19 +1742,34 @@ static void
 mt7925_mcu_sta_mld_tlv(struct sk_buff *skb,
                       struct ieee80211_vif *vif, struct ieee80211_sta *sta)
 {
-       struct mt76_wcid *wcid = (struct mt76_wcid *)sta->drv_priv;
+       struct mt792x_vif *mvif = (struct mt792x_vif *)vif->drv_priv;
+       struct mt792x_sta *msta = (struct mt792x_sta *)sta->drv_priv;
+       unsigned long valid = mvif->valid_links;
+       struct mt792x_bss_conf *mconf;
+       struct mt792x_link_sta *mlink;
        struct sta_rec_mld *mld;
        struct tlv *tlv;
+       int i, cnt = 0;
 
        tlv = mt76_connac_mcu_add_tlv(skb, STA_REC_MLD, sizeof(*mld));
        mld = (struct sta_rec_mld *)tlv;
-       memcpy(mld->mac_addr, vif->addr, ETH_ALEN);
-       mld->primary_id = cpu_to_le16(wcid->idx);
-       mld->wlan_id = cpu_to_le16(wcid->idx);
+       memcpy(mld->mac_addr, sta->addr, ETH_ALEN);
+       mld->primary_id = cpu_to_le16(msta->deflink.wcid.idx);
+       mld->wlan_id = cpu_to_le16(msta->deflink.wcid.idx);
+       mld->link_num = min_t(u8, hweight16(mvif->valid_links), 2);
+
+       for_each_set_bit(i, &valid, IEEE80211_MLD_MAX_NUM_LINKS) {
+               if (cnt == mld->link_num)
+                       break;
 
-       /* TODO: 0 means deflink only, add secondary link(1) later */
-       mld->link_num = !!(hweight8(vif->active_links) > 1);
-       WARN_ON_ONCE(mld->link_num);
+               mconf = mt792x_vif_to_link(mvif, i);
+               mlink = mt792x_sta_to_link(msta, i);
+               mld->link[cnt].wlan_id = cpu_to_le16(mlink->wcid.idx);
+               mld->link[cnt++].bss_idx = mconf->mt76.idx;
+
+               if (mlink != &msta->deflink)
+                       mld->secondary_id = cpu_to_le16(mlink->wcid.idx);
+       }
 }
 
 static int