recipes-wifi/linux-mt76/files/patches-3.x/0127-mtk-mt76-mt7996-add-critical-update-support.patch

From 8a5fc9a07ebf08ffdd31d05beaa2d399b67f6a10 Mon Sep 17 00:00:00 2001
From: StanleyYP Wang <StanleyYP.Wang@mediatek.com>
Date: Tue, 23 Apr 2024 15:22:24 +0800
Subject: [PATCH 127/199] mtk: mt76: mt7996: add critical update support

Add critical update support
modification: wmm configuration
inclusion: channel switch
(affiliated link's per-STA profile CSA/eCSA countdown is included)

Fix the CU flag countdown issue of the non-CU link as reported from cert.
1. Avoid setting the CSA beacon twice during channel switch.
2. Raise the bypass_seq_bitmap up for the non-CU link.

Reset the beacon when switching channels during CAC
Otherwise, the FW will not be able to perform the CSA countdown.
Also, modify the bpcc check since channel switch will add the bpcc twice
(before CSA and after CSA).

Signed-off-by: StanleyYP Wang <StanleyYP.Wang@mediatek.com>
Signed-off-by: Shayne Chen <shayne.chen@mediatek.com>
---
 mt76_connac_mcu.h |   2 +
 mt7996/main.c     | 107 +++++++++++++++++++++++++++++++++-------------
 mt7996/mcu.c      |  99 ++++++++++++++++++++++++++++++++++++++++++
 mt7996/mcu.h      |  26 ++++++++++-
 mt7996/mt7996.h   |   4 ++
 5 files changed, 208 insertions(+), 30 deletions(-)

diff --git a/mt76_connac_mcu.h b/mt76_connac_mcu.h
index 870417f3..57cae8ae 100644
--- a/mt76_connac_mcu.h
+++ b/mt76_connac_mcu.h
@@ -1380,6 +1380,8 @@ enum {
 	UNI_BSS_INFO_OFFLOAD = 25,
 	UNI_BSS_INFO_MLD = 26,
 	UNI_BSS_INFO_PM_DISABLE = 27,
+	UNI_BSS_INFO_BCN_CRIT_UPDATE = 32,
+	UNI_BSS_INFO_BCN_STA_PROF_CSA = 37,
 };
 
 enum {
diff --git a/mt7996/main.c b/mt7996/main.c
index f52bb2f9..0d25d763 100644
--- a/mt7996/main.c
+++ b/mt7996/main.c
@@ -370,6 +370,7 @@ static int mt7996_add_bss_conf(struct mt7996_phy *phy,
 	mconf->mt76.band_idx = band_idx;
 	mconf->mt76.wmm_idx = vif->type == NL80211_IFTYPE_AP ? 0 : 3;
 	mconf->link_id = link_id;
+	mconf->bpcc = 0;
 
 	ret = mt7996_mcu_add_dev_info(phy, conf, mconf, true);
 	if (ret)
@@ -979,7 +980,9 @@ static void mt7996_link_info_changed(struct ieee80211_hw *hw,
 		mconf->mt76.beacon_rates_idx =
 			mt7996_get_rates_table(hw, info, mconf, true, false);
 
-		mt7996_mcu_add_beacon(hw, info, mconf, info->enable_beacon);
+		/* The CSA beacon will be set in channel_switch_beacon */
+		if (!info->csa_active)
+			mt7996_mcu_add_beacon(hw, info, mconf, info->enable_beacon);
 	}
 
 	if (changed & (BSS_CHANGED_UNSOL_BCAST_PROBE_RESP |
@@ -1030,21 +1033,45 @@ mt7996_channel_switch_beacon(struct ieee80211_hw *hw,
 	struct mt7996_dev *dev = mt7996_hw_dev(hw);
 	struct mt7996_vif *mvif = (struct mt7996_vif *)vif->drv_priv;
 	struct mt7996_phy *phy = mt7996_band_phy(hw, chandef->chan->band);
-	unsigned long valid_links = vif->valid_links ?: BIT(0);
+	struct ieee80211_bss_conf *conf;
+	struct mt7996_bss_conf *mconf;
+	u16 valid_links = vif->valid_links ?: BIT(0);
 	unsigned int link_id;
 
 	mutex_lock(&dev->mt76.mutex);
-	for_each_set_bit(link_id, &valid_links, IEEE80211_MLD_MAX_NUM_LINKS) {
-		struct mt7996_bss_conf *mconf =
-			mconf_dereference_protected(mvif, link_id);
-		struct ieee80211_bss_conf *conf =
-			link_conf_dereference_protected(vif, link_id);
+	link_id = mvif->band_to_link[phy->mt76->band_idx];
 
-		if (!mconf || phy != mconf->phy)
-			continue;
+	if (!mvif->cs_ready_links)
+		mvif->cs_link_id = link_id;
+
+	mvif->cs_ready_links |= BIT(link_id);
+	if (mvif->cs_ready_links != valid_links)
+		goto out;
+
+	link_id = mvif->cs_link_id;
+	do {
+		valid_links &= ~BIT(link_id);
+		mconf = mconf_dereference_protected(mvif, link_id);
+		conf = link_conf_dereference_protected(vif, link_id);
+		if (!conf || !mconf)
+			goto fail;
+
+		/* Reset the beacon when switching channels during CAC */
+		if (link_id == mvif->cs_link_id &&
+		    !cfg80211_reg_can_beacon(hw->wiphy, &phy->mt76->chandef, vif->type))
+			mt7996_mcu_add_beacon(hw, conf, mconf, false);
 
 		mt7996_mcu_add_beacon(hw, conf, mconf, true);
-	}
+		link_id = ffs(valid_links) - 1;
+	} while (valid_links);
+
+out:
+	mutex_unlock(&dev->mt76.mutex);
+	return;
+fail:
+	mvif->cs_ready_links = 0;
+	mvif->cs_link_id = IEEE80211_LINK_UNSPECIFIED;
+	dev_err(dev->mt76.dev, "link %d: failed to switch beacon\n", link_id);
 	mutex_unlock(&dev->mt76.mutex);
 }
 
@@ -2534,32 +2561,54 @@ mt7996_switch_vif_chanctx(struct ieee80211_hw *hw,
 			  int n_vifs,
 			  enum ieee80211_chanctx_switch_mode mode)
 {
-	struct mt7996_chanctx *old_ctx = mt7996_chanctx_get(vifs->old_ctx);
-	struct mt7996_chanctx *new_ctx = mt7996_chanctx_get(vifs->new_ctx);
-	struct mt7996_phy *phy = old_ctx->phy;
+	struct mt7996_dev *dev = mt7996_hw_dev(hw);
+	struct mt7996_chanctx *old_ctx;
+	struct mt7996_chanctx *new_ctx;
+	struct mt7996_phy *phy;
+	int i, ret = 0;
 
-	wiphy_info(hw->wiphy, "%s: old=%d, new=%d\n", __func__, vifs->old_ctx->def.chan->hw_value, vifs->new_ctx->def.chan->hw_value);
+	for (i = 0; i < n_vifs; i++) {
+		if (vifs[i].old_ctx == vifs[i].new_ctx)
+			continue;
 
-	if (new_ctx->nbss_assigned && phy->chanctx == new_ctx) {
-		new_ctx->nbss_assigned += n_vifs;
-		return 0;
-	}
+		wiphy_info(hw->wiphy, "%s: old=%d, new=%d\n",
+			   __func__, vifs[i].old_ctx->def.chan->hw_value,
+			   vifs[i].new_ctx->def.chan->hw_value);
 
-	if (WARN_ON(old_ctx != phy->chanctx))
-		return -EINVAL;
+		mutex_lock(&dev->mt76.mutex);
 
-	mutex_lock(&phy->dev->mt76.mutex);
+		old_ctx = mt7996_chanctx_get(vifs[i].old_ctx);
+		new_ctx = mt7996_chanctx_get(vifs[i].new_ctx);
+		phy = old_ctx->phy;
 
-	phy->chanctx = new_ctx;
-	phy->mt76->radar_enabled = vifs->new_ctx->radar_enabled;
-	new_ctx->assigned = true;
-	new_ctx->chandef = vifs->new_ctx->def;
-	new_ctx->phy = phy;
-	new_ctx->nbss_assigned += n_vifs;
+		if (new_ctx->nbss_assigned && phy->chanctx == new_ctx) {
+			new_ctx->nbss_assigned++;
+			mutex_unlock(&dev->mt76.mutex);
+			continue;
+		}
 
-	mutex_unlock(&phy->dev->mt76.mutex);
+		if (WARN_ON(old_ctx != phy->chanctx)) {
+			ret = -EINVAL;
+			mutex_unlock(&dev->mt76.mutex);
+			goto out;
+		}
+
+		phy->chanctx = new_ctx;
+		phy->mt76->radar_enabled = vifs[i].new_ctx->radar_enabled;
+		new_ctx->assigned = true;
+		new_ctx->chandef = vifs[i].new_ctx->def;
+		new_ctx->phy = phy;
+		new_ctx->nbss_assigned++;
+
+		mutex_unlock(&dev->mt76.mutex);
+
+		ret = mt7996_set_channel(phy, &new_ctx->chandef);
+		if (ret)
+			goto out;
+	}
 
-	return mt7996_set_channel(phy, &new_ctx->chandef);
+out:
+	return ret;
 }
 
 static int
diff --git a/mt7996/mcu.c b/mt7996/mcu.c
index 28123fb7..ea69b66f 100644
--- a/mt7996/mcu.c
+++ b/mt7996/mcu.c
@@ -370,6 +370,7 @@ mt7996_mcu_csa_finish(void *priv, u8 *mac, struct ieee80211_vif *vif)
 	struct mt76_phy *mphy = (struct mt76_phy *)priv;
 	struct mt7996_vif *mvif = (struct mt7996_vif *)vif->drv_priv;
 	struct ieee80211_bss_conf *link_conf;
+	unsigned long valid_links = vif->valid_links ?: BIT(0);
 	int link_id, band_idx = mphy->band_idx;
 
 	link_id = mvif->band_to_link[band_idx];
@@ -378,7 +379,16 @@ mt7996_mcu_csa_finish(void *priv, u8 *mac, struct ieee80211_vif *vif)
 	if (!link_conf || !link_conf->csa_active || vif->type == NL80211_IFTYPE_STATION)
 		return;
 
+	pr_info("%s: link_id=%d\n", __func__, link_id);
+	mvif->cs_ready_links = 0;
+	mvif->cs_link_id = IEEE80211_LINK_UNSPECIFIED;
 	ieee80211_csa_finish(vif, link_id);
+	/* remove CSA for affiliated links */
+	for_each_set_bit(link_id, &valid_links, IEEE80211_MLD_MAX_NUM_LINKS) {
+		if (link_id == link_conf->link_id)
+			continue;
+		ieee80211_csa_finish(vif, link_id);
+	}
 }
 
 static void
@@ -3235,6 +3245,93 @@ mt7996_mcu_beacon_mbss(struct sk_buff *rskb, struct sk_buff *skb,
 	}
 }
 
+static bool
+mt7996_mcu_beacon_is_cu_link(struct sk_buff *skb, struct mt7996_bss_conf *mconf,
+			     u16 tail_offset)
+{
+	const struct element *elem;
+	u8 *beacon_tail = skb->data + tail_offset;
+	bool has_ml_ie = false;
+	int bpcc;
+
+	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_BASIC,
+					  elem->datalen - 1)) {
+			has_ml_ie = true;
+			break;
+		}
+
+	if (!has_ml_ie)
+		return false;
+
+	bpcc = ieee80211_mle_get_bss_param_ch_cnt(elem->data + 1);
+	if (bpcc < 0 || bpcc == mconf->bpcc)
+		return false;
+
+	mconf->bpcc = bpcc;
+
+	return true;
+}
+
+static void
+mt7996_mcu_beacon_crit_update(struct sk_buff *rskb, struct sk_buff *skb,
+			      struct ieee80211_bss_conf *conf,
+			      struct mt7996_bss_conf *mconf,
+			      struct ieee80211_mutable_offsets *offs)
+{
+	struct ieee80211_mgmt *mgmt = (void *)skb->data;
+	struct bss_bcn_crit_update_tlv *crit;
+	struct tlv *tlv;
+
+	if (!ieee80211_vif_is_mld(conf->vif) ||
+	    !(mgmt->u.beacon.capab_info & WLAN_CAPABILITY_PBCC))
+		return;
+
+	tlv = mt7996_mcu_add_uni_tlv(rskb, UNI_BSS_INFO_BCN_CRIT_UPDATE, sizeof(*crit));
+
+	/* TODO: Support 11vMBSS */
+	crit = (struct bss_bcn_crit_update_tlv *)tlv;
+	crit->bss_bitmap = cpu_to_le32(BIT(0));
+	/* The beacon of the CU link should be set in sequence
+	 * to ensure it appears in the air before the beacon of
+	 * the non-CU link.
+	 */
+	if (!mt7996_mcu_beacon_is_cu_link(skb, mconf, offs->tim_offset))
+		crit->bypass_seq_bitmap = cpu_to_le32(BIT(0));
+	else
+		crit->bypass_seq_bitmap = cpu_to_le32(0);
+	crit->tim_ie_pos[0] = cpu_to_le16(offs->tim_offset);
+	crit->cap_info_ie_pos[0] = cpu_to_le16(offsetof(struct ieee80211_mgmt,
+							u.beacon.capab_info));
+}
+
+static void
+mt7996_mcu_beacon_sta_prof_csa(struct sk_buff *rskb,
+			       struct ieee80211_bss_conf *conf,
+			       struct ieee80211_mutable_offsets *offs)
+{
+	struct ieee80211_vif *vif = conf->vif;
+	struct mt7996_vif *mvif = (struct mt7996_vif *)vif->drv_priv;
+	struct mt7996_bss_conf *cs_mconf;
+	struct bss_bcn_sta_prof_cntdwn_tlv *sta_prof;
+	struct tlv *tlv;
+
+	if (!ieee80211_vif_is_mld(vif) || !offs->sta_prof_cntdwn_offs[0])
+		return;
+
+	cs_mconf = mconf_dereference_protected(mvif, mvif->cs_link_id);
+	if (!cs_mconf)
+		return;
+
+	tlv = mt7996_mcu_add_uni_tlv(rskb, UNI_BSS_INFO_BCN_STA_PROF_CSA, sizeof(*sta_prof));
+
+	sta_prof = (struct bss_bcn_sta_prof_cntdwn_tlv *)tlv;
+	sta_prof->sta_prof_csa_offs = cpu_to_le16(offs->sta_prof_cntdwn_offs[0] - 4);
+	sta_prof->cs_bss_idx = cs_mconf->mt76.idx;
+}
+
 static void
 mt7996_mcu_beacon_cont(struct mt7996_dev *dev, struct ieee80211_bss_conf *conf,
 		       struct sk_buff *rskb, struct sk_buff *skb,
@@ -3315,6 +3412,8 @@ int mt7996_mcu_add_beacon(struct ieee80211_hw *hw,
 	mt7996_mcu_beacon_cont(dev, conf, rskb, skb, bcn, &offs);
 	mt7996_mcu_beacon_mbss(rskb, skb, conf, bcn, &offs);
 	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);
 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 5e217719..c39dcc3c 100644
--- a/mt7996/mcu.h
+++ b/mt7996/mcu.h
@@ -503,6 +503,27 @@ struct bss_bcn_mbss_tlv {
 	__le16 offset[MAX_BEACON_NUM];
 } __packed __aligned(4);
 
+struct bss_bcn_crit_update_tlv {
+	__le16 tag;
+	__le16 len;
+	__le32 bss_bitmap;
+	/* Bypass the beacon sequence handling in firmware for the
+	 * BSSes in the bitmap. If the flag is set for a BSS, then the
+	 * firmware will not set the beacon of the BSS in sequence.
+	 */
+	__le32 bypass_seq_bitmap;
+	__le16 tim_ie_pos[32];
+	__le16 cap_info_ie_pos[32];
+} __packed;
+
+struct bss_bcn_sta_prof_cntdwn_tlv {
+	__le16 tag;
+	__le16 len;
+	__le16 sta_prof_csa_offs;
+	u8 cs_bss_idx;
+	u8 pkt_content[9];
+} __packed;
+
 struct bss_txcmd_tlv {
 	__le16 tag;
 	__le16 len;
@@ -954,7 +975,10 @@ enum {
 					 sizeof(struct bss_bcn_content_tlv) +	\
 					 4 + MT_TXD_SIZE +			\
 					 sizeof(struct bss_bcn_cntdwn_tlv) +	\
-					 sizeof(struct bss_bcn_mbss_tlv))
+					 sizeof(struct bss_bcn_mbss_tlv) +	\
+					 sizeof(struct bss_bcn_crit_update_tlv) +	\
+					 sizeof(struct bss_bcn_sta_prof_cntdwn_tlv))	\
+
 #define MT7996_MAX_BSS_OFFLOAD_SIZE	(MT7996_MAX_BEACON_SIZE +		\
 					 MT7996_BEACON_UPDATE_SIZE)
 
diff --git a/mt7996/mt7996.h b/mt7996/mt7996.h
index 827c24bf..78226f7b 100644
--- a/mt7996/mt7996.h
+++ b/mt7996/mt7996.h
@@ -351,6 +351,7 @@ struct mt7996_bss_conf {
 
 	u8 link_id;
 	u8 own_mld_id;
+	u8 bpcc;
 };
 
 struct mt7996_vif {
@@ -365,6 +366,9 @@ struct mt7996_vif {
 	u8 group_mld_id;
 	u8 mld_remap_id;
 
+	u8 cs_link_id;
+	u16 cs_ready_links;
+
 	u8 band_to_link[__MT_MAX_BAND];
 
 	/* for beacon monitoring */
-- 
2.18.0