blob: 10de90198a334125cf21ef461e77bb5637effb04 [file] [log] [blame]
From 95ad534b954c4ed55417dd5e1fbfb80410c52835 Mon Sep 17 00:00:00 2001
From: Shayne Chen <shayne.chen@mediatek.com>
Date: Fri, 31 May 2024 18:14:59 +0800
Subject: [PATCH 170/193] mtk: mt76: mt7996: add AP affiliated link removal
support
Add support for ap link removal of MLD reconfiguration.
Change-Id: I27a611dcf6698f89f6a3a67f893a166d2afe6b1f
Signed-off-by: Shayne Chen <shayne.chen@mediatek.com>
---
mt76_connac_mcu.h | 3 +
mt7996/main.c | 68 ++++++++++++---
mt7996/mcu.c | 217 ++++++++++++++++++++++++++++++++++++++++++++++
mt7996/mcu.h | 89 ++++++++++++++++++-
mt7996/mt7996.h | 2 +
5 files changed, 367 insertions(+), 12 deletions(-)
diff --git a/mt76_connac_mcu.h b/mt76_connac_mcu.h
index 302567e..4976d94 100644
--- a/mt76_connac_mcu.h
+++ b/mt76_connac_mcu.h
@@ -1079,6 +1079,7 @@ enum {
MCU_UNI_EVENT_PP = 0x5a,
MCU_UNI_EVENT_PER_STA_INFO = 0x6d,
MCU_UNI_EVENT_ALL_STA_INFO = 0x6e,
+ MCU_UNI_EVENT_MLD = 0x81,
};
#define MCU_UNI_CMD_EVENT BIT(1)
@@ -1324,6 +1325,7 @@ enum {
MCU_UNI_CMD_ALL_STA_INFO = 0x6e,
MCU_UNI_CMD_ASSERT_DUMP = 0x6f,
MCU_UNI_CMD_PTA_3WIRE_CTRL = 0x78,
+ MCU_UNI_CMD_MLD = 0x82,
};
enum {
@@ -1398,6 +1400,7 @@ enum {
UNI_BSS_INFO_PM_DISABLE = 27,
UNI_BSS_INFO_BCN_CRIT_UPDATE = 32,
UNI_BSS_INFO_BCN_STA_PROF_CSA = 37,
+ UNI_BSS_INFO_BCN_ML_RECONF = 38,
};
enum {
diff --git a/mt7996/main.c b/mt7996/main.c
index 90320e5..a9ae083 100644
--- a/mt7996/main.c
+++ b/mt7996/main.c
@@ -319,6 +319,21 @@ static void mt7996_remove_bss_conf(struct ieee80211_vif *vif,
list_del_init(&mlink->wcid.poll_list);
spin_unlock_bh(&dev->mt76.sta_poll_lock);
+ /* reassign a new bss wcid if the previous one was removed */
+ if (vif->txq && ieee80211_vif_is_mld(vif) &&
+ hweight16(vif->valid_links) > 1) {
+ struct mt76_txq *mtxq = (struct mt76_txq *)vif->txq->drv_priv;
+
+ if (mtxq->wcid == mlink->wcid.idx) {
+ u8 new_link = __ffs(vif->valid_links & ~BIT(link_id));
+ struct mt7996_link_sta *new_mlink =
+ mlink_dereference_protected(&mvif->sta, new_link);
+
+ if (new_mlink)
+ mtxq->wcid = new_mlink->wcid.idx;
+ }
+ }
+
mt76_wcid_cleanup(&dev->mt76, &mlink->wcid);
if (mlink != &mvif->sta.deflink)
@@ -425,7 +440,7 @@ static int mt7996_add_bss_conf(struct mt7996_phy *phy,
mt7996_mac_wtbl_update(dev, idx,
MT_WTBL_UPDATE_ADM_COUNT_CLEAR);
- if (vif->txq) {
+ if (vif->txq && hweight16(vif->valid_links) <= 1) {
mtxq = (struct mt76_txq *)vif->txq->drv_priv;
mtxq->wcid = idx;
}
@@ -1127,6 +1142,8 @@ static void mt7996_remove_link_sta(struct mt7996_dev *dev,
{
struct ieee80211_sta *sta = link_sta->sta;
struct mt7996_sta *msta = (struct mt7996_sta *)sta->drv_priv;
+ u16 valid_links = sta->valid_links;
+ bool pri_changed;
int i;
if (!mlink)
@@ -1147,6 +1164,7 @@ static void mt7996_remove_link_sta(struct mt7996_dev *dev,
mt7996_mac_twt_teardown_flow(dev, mlink, i);
rcu_assign_pointer(mlink->sta->link[mlink->wcid.link_id], NULL);
+ rcu_assign_pointer(dev->mt76.wcid[mlink->wcid.idx], NULL);
spin_lock_bh(&dev->mt76.sta_poll_lock);
if (!list_empty(&mlink->wcid.poll_list))
@@ -1155,17 +1173,41 @@ static void mt7996_remove_link_sta(struct mt7996_dev *dev,
list_del_init(&mlink->rc_list);
spin_unlock_bh(&dev->mt76.sta_poll_lock);
- /* TODO: update primary link */
- if (sta->valid_links) {
- if (mlink->wcid.link_id == msta->pri_link)
- msta->pri_link = msta->sec_link;
+ valid_links &= ~BIT(mlink->wcid.link_id);
+ if (!valid_links)
+ goto done;
- if (sta->valid_links & ~(BIT(msta->pri_link)))
- msta->sec_link = __ffs(sta->valid_links & ~(BIT(msta->pri_link)));
- else
- msta->sec_link = msta->pri_link;
+ /* update primary and secondary link */
+ pri_changed = mlink->wcid.link_id == msta->pri_link;
+ if (pri_changed)
+ msta->pri_link = msta->sec_link;
+
+ if (valid_links & ~(BIT(msta->pri_link)))
+ msta->sec_link = __ffs(valid_links & ~(BIT(msta->pri_link)));
+ else
+ msta->sec_link = msta->pri_link;
+
+ if (pri_changed) {
+ struct mt7996_link_sta *mlink_new =
+ mlink_dereference_protected(msta, msta->pri_link);
+
+ if (!mlink_new)
+ goto done;
+
+ mlink_new->wcid.ampdu_state = mlink->wcid.ampdu_state;
+ for (i = 0; i < ARRAY_SIZE(mlink->wcid.aggr); i++)
+ rcu_assign_pointer(mlink_new->wcid.aggr[i], mlink->wcid.aggr[i]);
+ for (i = 0; i < ARRAY_SIZE(sta->txq); i++) {
+ struct mt76_txq *mtxq;
+
+ if (!sta->txq[i])
+ continue;
+ mtxq = (struct mt76_txq *)sta->txq[i]->drv_priv;
+ mtxq->wcid = mlink_new->wcid.idx;
+ }
}
+done:
mt76_wcid_cleanup(&dev->mt76, &mlink->wcid);
mt76_wcid_mask_clear(dev->mt76.wcid_mask, mlink->wcid.idx);
mt76_wcid_mask_clear(dev->mt76.wcid_phy_mask, mlink->wcid.idx);
@@ -3107,9 +3149,15 @@ mt7996_change_sta_links(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
mt76_vif_dbg(vif, "STA %pM old=0x%x, new=0x%x\n", sta->addr, old_links, new_links);
mutex_lock(&dev->mt76.mutex);
- if (rem)
+ if (rem) {
mt7996_mac_sta_remove_links(dev, vif, sta, rem);
+ /* Todo: update hw info of MLD STA */
+ /* ret = mt7996_mcu_add_mld_sta(dev, vif, sta, new_links); */
+ /* if (ret) */
+ /* goto remove; */
+ }
+
ret = mt7996_mac_sta_add_links(dev, vif, sta, add, false);
if (ret)
goto remove;
diff --git a/mt7996/mcu.c b/mt7996/mcu.c
index fb4f5f5..b6cc459 100644
--- a/mt7996/mcu.c
+++ b/mt7996/mcu.c
@@ -1355,6 +1355,48 @@ mt7996_mcu_pp_event(struct mt7996_dev *dev, struct sk_buff *skb)
}
}
+static void
+mt7996_mcu_mld_reconf_finish(void *priv, u8 *mac, struct ieee80211_vif *vif)
+{
+ struct mt7996_mld_event_data *data = priv;
+ struct mt7996_mcu_mld_ap_reconf_event *reconf = (void *)data->data;
+
+ if (!ether_addr_equal(vif->addr, data->mld_addr))
+ return;
+
+ ieee80211_links_removed(vif, le16_to_cpu(reconf->link_bitmap));
+}
+
+static void
+mt7996_mcu_mld_event(struct mt7996_dev *dev, struct sk_buff *skb)
+{
+ struct mt7996_mcu_mld_event *event = (void *)skb->data;
+ struct mt7996_mld_event_data data = {};
+ struct tlv *tlv;
+ int len;
+
+ memcpy(data.mld_addr, event->mld_addr, ETH_ALEN);
+ skb_pull(skb, sizeof(*event));
+ tlv = (struct tlv *)skb->data;
+ len = skb->len;
+
+ while (len > 0 && le16_to_cpu(tlv->len) <= len) {
+ switch (le16_to_cpu(tlv->tag)) {
+ case UNI_EVENT_MLD_RECONF_AP_REM_TIMER:
+ data.data = (u8 *)tlv;
+ ieee80211_iterate_active_interfaces_atomic(dev->mt76.hw,
+ IEEE80211_IFACE_ITER_RESUME_ALL,
+ mt7996_mcu_mld_reconf_finish, &data);
+ break;
+ default:
+ break;
+ }
+
+ len -= le16_to_cpu(tlv->len);
+ tlv = (struct tlv *)((u8 *)(tlv) + le16_to_cpu(tlv->len));
+ }
+}
+
static void
mt7996_mcu_uni_rx_unsolicited_event(struct mt7996_dev *dev, struct sk_buff *skb)
{
@@ -1383,6 +1425,9 @@ mt7996_mcu_uni_rx_unsolicited_event(struct mt7996_dev *dev, struct sk_buff *skb)
case MCU_UNI_EVENT_PP:
mt7996_mcu_pp_event(dev, skb);
break;
+ case MCU_UNI_EVENT_MLD:
+ mt7996_mcu_mld_event(dev, skb);
+ break;
#ifdef CONFIG_MTK_DEBUG
case MCU_UNI_EVENT_SR:
mt7996_mcu_rx_sr_event(dev, skb);
@@ -3176,6 +3221,44 @@ out:
MCU_WMWA_UNI_CMD(STA_REC_UPDATE), true);
}
+int mt7996_mcu_add_mld_sta(struct mt7996_dev *dev, struct ieee80211_vif *vif,
+ struct ieee80211_sta *sta, unsigned long add)
+{
+ struct mt7996_vif *mvif = (struct mt7996_vif *)vif->drv_priv;
+ struct mt7996_sta *msta = (struct mt7996_sta *)sta->drv_priv;
+ u8 link_id;
+
+ if (!sta->mlo)
+ return 0;
+
+ for_each_set_bit(link_id, &add, IEEE80211_MLD_MAX_NUM_LINKS) {
+ struct mt7996_bss_conf *mconf =
+ mconf_dereference_protected(mvif, link_id);
+ struct mt7996_link_sta *mlink =
+ mlink_dereference_protected(msta, link_id);
+ struct sk_buff *skb;
+ int ret;
+
+ if (!mconf || !mlink)
+ continue;
+
+ skb = __mt76_connac_mcu_alloc_sta_req(&dev->mt76, &mconf->mt76,
+ &mlink->wcid,
+ MT7996_STA_UPDATE_MAX_SIZE);
+ if (IS_ERR(skb))
+ return PTR_ERR(skb);
+ /* starec mld setup */
+ mt7996_mcu_sta_mld_setup_tlv(dev, skb, sta, add);
+ /* starec eht mld */
+ mt7996_mcu_sta_eht_mld_tlv(dev, skb, sta);
+ ret = mt76_mcu_skb_send_msg(&dev->mt76, skb,
+ MCU_WMWA_UNI_CMD(STA_REC_UPDATE), true);
+ if (ret)
+ return ret;
+ }
+ return 0;
+}
+
int mt7996_mcu_teardown_mld_sta(struct mt7996_dev *dev,
struct mt7996_bss_conf *mconf,
struct mt7996_link_sta *mlink)
@@ -3388,6 +3471,51 @@ int mt7996_mcu_add_dev_info(struct mt7996_phy *phy,
&data, sizeof(data), true);
}
+static int
+mt7996_mcu_mld_reconf(struct mt7996_dev *dev, struct ieee80211_vif *vif,
+ u16 removed_links, u16 *removal_count)
+{
+ struct mt7996_vif *mvif = (struct mt7996_vif *)vif->drv_priv;
+ struct mld_req_hdr hdr = { .mld_idx = 0xff };
+ struct mld_reconf_timer *rt;
+ struct sk_buff *skb;
+ struct tlv *tlv;
+ int len = sizeof(hdr) + sizeof(*rt);
+ unsigned long rem = removed_links;
+ u8 link_id;
+
+ memcpy(hdr.mld_addr, vif->addr, ETH_ALEN);
+
+ skb = mt76_mcu_msg_alloc(&dev->mt76, NULL, len);
+ if (!skb)
+ return -ENOMEM;
+
+ skb_put_data(skb, &hdr, sizeof(hdr));
+
+ tlv = mt7996_mcu_add_uni_tlv(skb, UNI_CMD_MLD_RECONF_AP_REM_TIMER, sizeof(*rt));
+ rt = (struct mld_reconf_timer *)tlv;
+ rt->link_bitmap = cpu_to_le16(removed_links);
+
+ for_each_set_bit(link_id, &rem, IEEE80211_MLD_MAX_NUM_LINKS) {
+ struct ieee80211_bss_conf *conf =
+ link_conf_dereference_protected(vif, link_id);
+ struct mt7996_bss_conf *mconf =
+ mconf_dereference_protected(mvif, link_id);
+ u8 band_idx;
+ u16 to_sec;
+
+ if (!conf || !mconf)
+ continue;
+
+ band_idx = mconf->phy->mt76->band_idx;
+ to_sec = conf->beacon_int * removal_count[link_id] / 1000;
+ rt->to_sec[band_idx] = cpu_to_le16(to_sec);
+ rt->bss_idx[band_idx] = mconf->mt76.idx;
+ }
+
+ return mt76_mcu_skb_send_msg(&dev->mt76, skb, MCU_WM_UNI_CMD(MLD), true);
+}
+
static void
mt7996_mcu_beacon_cntdwn(struct ieee80211_bss_conf *conf, struct sk_buff *rskb,
struct sk_buff *skb,
@@ -3582,6 +3710,94 @@ mt7996_mcu_beacon_cont(struct mt7996_dev *dev, struct ieee80211_bss_conf *conf,
mt7996_packet_log_to_host(dev, skb->data, skb->len, PKT_BIN_DEBUG_TX, 0);
}
+static void
+mt7996_mcu_beacon_ml_reconf(struct mt7996_dev *dev,
+ struct ieee80211_bss_conf *conf,
+ struct sk_buff *rskb, struct sk_buff *skb,
+ struct ieee80211_mutable_offsets *offs)
+{
+ struct mt7996_vif *mvif = (struct mt7996_vif *)conf->vif->drv_priv;
+ struct bss_bcn_ml_reconf_tlv *reconf;
+ struct bss_bcn_ml_reconf_offset *reconf_offs;
+ const struct element *elem, *sub;
+ struct tlv *tlv;
+ u16 removal_offs[IEEE80211_MLD_MAX_NUM_LINKS] = {};
+ u16 removal_count[IEEE80211_MLD_MAX_NUM_LINKS] = {};
+ u16 tail_offset = offs->tim_offset + offs->tim_length;
+ unsigned long removed_links = 0;
+ bool has_reconf = false;
+ u8 link_id, *beacon_tail = skb->data + tail_offset;
+
+ if (!ieee80211_vif_is_mld(conf->vif))
+ return;
+
+ /* TODO: currently manually parse reconf info directly from the IE, it
+ * is expected to be passed from upper layer in the future.
+ */
+ for_each_element_extid(elem, WLAN_EID_EXT_EHT_MULTI_LINK,
+ beacon_tail, skb->len - tail_offset) {
+ if (ieee80211_mle_type_ok(elem->data + 1,
+ IEEE80211_ML_CONTROL_TYPE_RECONF,
+ elem->datalen - 1)) {
+ has_reconf = true;
+ break;
+ }
+ }
+
+ if (!has_reconf)
+ return;
+
+ for_each_mle_subelement(sub, elem->data + 1, elem->datalen - 1) {
+ struct ieee80211_mle_per_sta_profile *prof = (void *)sub->data;
+ u8 *pos = prof->variable;
+ u16 control;
+
+ if (sub->id != IEEE80211_MLE_SUBELEM_PER_STA_PROFILE)
+ continue;
+
+ if (!ieee80211_mle_reconf_sta_prof_size_ok(sub->data,
+ sub->datalen))
+ return;
+
+ control = le16_to_cpu(prof->control);
+ link_id = control & IEEE80211_MLE_STA_RECONF_CONTROL_LINK_ID;
+
+ removed_links |= BIT(link_id);
+
+ if (control & IEEE80211_MLE_STA_RECONF_CONTROL_STA_MAC_ADDR_PRESENT)
+ pos += 6;
+
+ if (control & IEEE80211_MLE_STA_RECONF_CONTROL_AP_REM_TIMER_PRESENT) {
+ removal_offs[link_id] = pos - skb->data;
+ removal_count[link_id] = le16_to_cpu(*(__le16 *)pos);
+ }
+ }
+
+ if (!removed_links)
+ return;
+
+ /* the first link to be removed */
+ if (conf->link_id == ffs(removed_links) - 1)
+ mt7996_mcu_mld_reconf(dev, conf->vif, removed_links, removal_count);
+
+ tlv = mt7996_mcu_add_uni_tlv(rskb, UNI_BSS_INFO_BCN_ML_RECONF,
+ sizeof(*reconf) +
+ sizeof(*reconf_offs) * hweight16(removed_links));
+ reconf = (struct bss_bcn_ml_reconf_tlv *)tlv;
+ reconf->reconf_count = hweight16(removed_links);
+
+ reconf_offs = (struct bss_bcn_ml_reconf_offset *)reconf->offset;
+ for_each_set_bit(link_id, &removed_links, IEEE80211_MLD_MAX_NUM_LINKS) {
+ struct mt7996_bss_conf *mconf =
+ mconf_dereference_protected(mvif, link_id);
+
+ reconf_offs->ap_removal_timer_offs =
+ cpu_to_le16(removal_offs[link_id]);
+ reconf_offs->bss_idx = mconf->mt76.idx;
+ reconf_offs++;
+ }
+}
+
int mt7996_mcu_add_beacon(struct ieee80211_hw *hw,
struct ieee80211_bss_conf *conf,
struct mt7996_bss_conf *mconf, int en)
@@ -3635,6 +3851,7 @@ int mt7996_mcu_add_beacon(struct ieee80211_hw *hw,
mt7996_mcu_beacon_cntdwn(conf, rskb, skb, &offs);
mt7996_mcu_beacon_sta_prof_csa(rskb, conf, &offs);
mt7996_mcu_beacon_crit_update(rskb, skb, conf, mconf, &offs);
+ mt7996_mcu_beacon_ml_reconf(dev, conf, rskb, skb, &offs);
out:
dev_kfree_skb(skb);
return mt76_mcu_skb_send_msg(&phy->dev->mt76, rskb,
diff --git a/mt7996/mcu.h b/mt7996/mcu.h
index 739e357..7c559d2 100644
--- a/mt7996/mcu.h
+++ b/mt7996/mcu.h
@@ -546,6 +546,20 @@ struct bss_bcn_sta_prof_cntdwn_tlv {
u8 pkt_content[9];
} __packed;
+struct bss_bcn_ml_reconf_tlv {
+ __le16 tag;
+ __le16 len;
+ u8 reconf_count;
+ u8 rsv[3];
+ u8 offset[];
+} __packed;
+
+struct bss_bcn_ml_reconf_offset {
+ __le16 ap_removal_timer_offs;
+ u8 bss_idx;
+ u8 rsv;
+} __packed;
+
struct bss_txcmd_tlv {
__le16 tag;
__le16 len;
@@ -594,6 +608,17 @@ struct bss_mld_tlv {
u8 __rsv[3];
} __packed;
+struct bss_mld_link_op_tlv {
+ __le16 tag;
+ __le16 len;
+ u8 group_mld_id;
+ u8 own_mld_id;
+ u8 mac_addr[ETH_ALEN];
+ u8 remap_idx;
+ u8 link_operation;
+ u8 rsv[2];
+} __packed;
+
struct sta_rec_ht_uni {
__le16 tag;
__le16 len;
@@ -1003,8 +1028,9 @@ enum {
sizeof(struct bss_bcn_cntdwn_tlv) + \
sizeof(struct bss_bcn_mbss_tlv) + \
sizeof(struct bss_bcn_crit_update_tlv) + \
- sizeof(struct bss_bcn_sta_prof_cntdwn_tlv)) \
-
+ sizeof(struct bss_bcn_sta_prof_cntdwn_tlv) + \
+ sizeof(struct bss_bcn_ml_reconf_tlv) + \
+ 3 * sizeof(struct bss_bcn_ml_reconf_offset))
#define MT7996_MAX_BSS_OFFLOAD_SIZE (MT7996_MAX_BEACON_SIZE + \
MT7996_BEACON_UPDATE_SIZE)
@@ -1105,6 +1131,65 @@ enum {
UNI_CMD_THERMAL_PROTECT_DUTY_CONFIG,
};
+struct mld_req_hdr {
+ u8 ver;
+ u8 mld_addr[ETH_ALEN];
+ u8 mld_idx;
+ u8 flag;
+ u8 rsv[3];
+ u8 buf[];
+} __packed;
+
+struct mld_reconf_timer {
+ __le16 tag;
+ __le16 len;
+ __le16 link_bitmap;
+ __le16 to_sec[__MT_MAX_BAND]; /* timeout of reconf (second) */
+ u8 bss_idx[__MT_MAX_BAND];
+ u8 rsv;
+} __packed;
+
+struct mld_reconf_stop_link {
+ __le16 tag;
+ __le16 len;
+ __le16 link_bitmap;
+ u8 rsv[2];
+ u8 bss_idx[16];
+} __packed;
+
+enum {
+ UNI_CMD_MLD_RECONF_AP_REM_TIMER = 0x03,
+};
+
+struct mt7996_mcu_mld_event {
+ struct mt7996_mcu_rxd rxd;
+
+ /* fixed field */
+ u8 ver;
+ u8 mld_addr[ETH_ALEN];
+ u8 mld_idx;
+ u8 rsv[4];
+ /* tlv */
+ u8 buf[];
+} __packed;
+
+struct mt7996_mld_event_data {
+ u8 mld_addr[ETH_ALEN];
+ u8 *data;
+};
+
+struct mt7996_mcu_mld_ap_reconf_event {
+ __le16 tag;
+ __le16 len;
+ __le16 link_bitmap;
+ u8 bss_idx[3];
+ u8 rsv[3];
+} __packed;
+
+enum {
+ UNI_EVENT_MLD_RECONF_AP_REM_TIMER = 0x04,
+};
+
struct tx_power_ctrl {
u8 _rsv[4];
diff --git a/mt7996/mt7996.h b/mt7996/mt7996.h
index a02e976..a7655a6 100644
--- a/mt7996/mt7996.h
+++ b/mt7996/mt7996.h
@@ -1087,6 +1087,8 @@ int mt7996_mcu_add_rate_ctrl(struct mt7996_dev *dev,
struct mt7996_bss_conf *mconf,
struct ieee80211_link_sta *link_sta,
struct mt7996_link_sta *mlink, bool changed);
+int mt7996_mcu_add_mld_sta(struct mt7996_dev *dev, struct ieee80211_vif *vif,
+ struct ieee80211_sta *sta, unsigned long add);
int mt7996_set_channel(struct mt7996_phy *phy, struct cfg80211_chan_def *chandef);
int mt7996_mcu_set_chan_info(struct mt7996_phy *phy, u16 tag, bool sta);
int mt7996_mcu_set_tx(struct mt7996_dev *dev, struct mt7996_bss_conf *mconf);
--
2.45.2