[PATCH 2/2] [PATCH 2/2] wifi: mt76: mt7925: add STA_REC_SUSWM TLV for SU beamforming capability for MT7928

[JB Tsai <jb.tsai@mediatek.com>]

From: Shengwei Lu <shengwei.lu@mediatek.com>

MT7928 firmware requires a STA_REC_SUSWM TLV to convey SU beamforming
and spatial stream capabilities when associating with a BSS. Add:

 - sta_rec_su_swm struct with a capability bitmap (tag 0x1A)
 - STA_REC_SUSWM enum entry in the STA record tag list
 - mt7928_mcu_sta_suswm_tlv() that resolves the peer link_sta and own
   sband, then calls mt7928_get_bss_suswm_cap() to obtain the SU BF
   bfer/bfee/nsts summary and fills the TLV
 - Call site in mt7925_mcu_sta_cmd() guarded by is_mt7928()

The capability bitmap bit layout passed to firmware:
  bit 0 - AP is SU beamformer capable
  bit 1 - local device is SU beamformee capable
  bit 2 - AP NSTS <= 2 (within supported spatial-stream limit)

Signed-off-by: Shengwei Lu <shengwei.lu@mediatek.com>
Co-developed-by: Sean Wang <sean.wang@mediatek.com>
Signed-off-by: Sean Wang <sean.wang@mediatek.com>
---
 .../wireless/mediatek/mt76/mt76_connac_mcu.h  |  1 +
 .../net/wireless/mediatek/mt76/mt7925/mcu.c   | 67 +++++++++++++++++++
 .../net/wireless/mediatek/mt76/mt7925/mcu.h   |  8 +++
 3 files changed, 76 insertions(+)

diff --git a/drivers/net/wireless/mediatek/mt76/mt76_connac_mcu.h b/drivers/net/wireless/mediatek/mt76/mt76_connac_mcu.h
index 51380849d24e..b2c79d65525a 100644
--- a/drivers/net/wireless/mediatek/mt76/mt76_connac_mcu.h
+++ b/drivers/net/wireless/mediatek/mt76/mt76_connac_mcu.h
@@ -857,6 +857,7 @@ enum {
 	STA_REC_PHY = 0x15,
 	STA_REC_HE_6G = 0x17,
 	STA_REC_HE_V2 = 0x19,
+	STA_REC_SUSWM = 0x1A,
 	STA_REC_MLD = 0x20,
 	STA_REC_EHT_MLD = 0x21,
 	STA_REC_EHT = 0x22,
diff --git a/drivers/net/wireless/mediatek/mt76/mt7925/mcu.c b/drivers/net/wireless/mediatek/mt76/mt7925/mcu.c
index 983a9eacd8c7..e94742c8c370 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7925/mcu.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7925/mcu.c
@@ -8,6 +8,7 @@
 #include "mcu.h"
 #include "mac.h"
 #include "nan.h"
+#include "ies.h"
 
 #define MT_STA_BFER			BIT(0)
 #define MT_STA_BFEE			BIT(1)
@@ -2110,6 +2111,70 @@ mt7925_mcu_sta_mld_tlv(struct sk_buff *skb,
 	mld->link_num = cnt;
 }
 
+static void
+mt7928_mcu_sta_suswm_tlv(struct sk_buff *skb,
+			 struct ieee80211_vif *vif, struct mt76_wcid *wcid)
+{
+	struct mt792x_vif *mvif = (struct mt792x_vif *)vif->drv_priv;
+	struct mt792x_phy *phy = mvif->phy;
+	struct ieee80211_link_sta *link_sta;
+	struct ieee80211_bss_conf *link_conf;
+	struct cfg80211_chan_def *chandef;
+	struct mt792x_bss_conf *mconf;
+	enum nl80211_band band;
+	struct ieee80211_sta *sta;
+	struct ieee80211_supported_band *sband;
+	const struct cfg80211_bss_ies *ies;
+	struct cfg80211_bss *bss;
+	struct mt7928_suswm_cap cap = {};
+	struct sta_rec_su_swm *su_swm;
+	struct tlv *tlv;
+
+	if (vif->type != NL80211_IFTYPE_STATION || !vif->cfg.assoc)
+		return;
+
+	rcu_read_lock();
+	sta = ieee80211_find_sta(vif, vif->bss_conf.bssid);
+
+	if (!sta)
+		goto exit;
+
+	link_sta = mt792x_sta_to_link_sta(vif, sta, wcid->link_id);
+
+	if (!link_sta)
+		goto exit;
+
+	link_conf = mt792x_vif_to_bss_conf(vif, link_sta->link_id);
+	mconf = mt792x_vif_to_link(mvif, link_sta->link_id);
+
+	chandef = mconf->mt76.ctx ? &mconf->mt76.ctx->def :
+				    &link_conf->chanreq.oper;
+	band = chandef->chan->band;
+
+	sband = phy->mt76->hw->wiphy->bands[band];
+
+	bss = link_conf->bss;
+	if (!bss || !bss->ies)
+		goto exit;
+
+	ies = rcu_dereference(bss->ies);
+
+	mt7928_get_bss_suswm_cap(ies, link_sta, sband, vif->type, &cap);
+
+	tlv = mt76_connac_mcu_add_tlv(skb, STA_REC_SUSWM, sizeof(*su_swm));
+	su_swm = (struct sta_rec_su_swm *)tlv;
+
+	if (cap.bfer_en)
+		su_swm->su_swm_cap_map |= MT_SU_SWM_BFER;
+	if (cap.bfee_en)
+		su_swm->su_swm_cap_map |= MT_SU_SWM_BFEE;
+	if (cap.nsts > 0 && cap.nsts <= 2)
+		su_swm->su_swm_cap_map |= MT_SU_SWM_NSTS_MEET;
+
+exit:
+	rcu_read_unlock();
+}
+
 static void
 mt7925_mcu_sta_remove_tlv(struct sk_buff *skb)
 {
@@ -2179,6 +2244,8 @@ mt7925_mcu_sta_cmd(struct mt76_phy *phy,
 			mlink = &mvif->sta.deflink;
 
 		mt7925_mcu_sta_hdr_trans_tlv(skb, info->vif, mlink);
+		if (is_mt7928(dev))
+			mt7928_mcu_sta_suswm_tlv(skb, info->vif, info->wcid);
 	}
 
 	return mt76_mcu_skb_send_msg(dev, skb, info->cmd, true);
diff --git a/drivers/net/wireless/mediatek/mt76/mt7925/mcu.h b/drivers/net/wireless/mediatek/mt76/mt7925/mcu.h
index 11f9eac13ffc..f8696ce36413 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7925/mcu.h
+++ b/drivers/net/wireless/mediatek/mt76/mt7925/mcu.h
@@ -493,6 +493,13 @@ struct sta_rec_eht_mld {
 	u8 __rsv2[3];
 } __packed;
 
+struct sta_rec_su_swm {
+	__le16 tag;
+	__le16 len;
+	u8   su_swm_cap_map;
+	u8 rsv[3];
+} __packed;
+
 struct bss_ifs_time_tlv {
 	__le16 tag;
 	__le16 len;
@@ -542,6 +549,7 @@ struct bss_rlm_tlv {
 					 sizeof(struct sta_rec_eht) +		\
 					 sizeof(struct sta_rec_hdr_trans) +	\
 					 sizeof(struct sta_rec_mld) +		\
+					 sizeof(struct sta_rec_su_swm) +		\
 					 sizeof(struct tlv) * 2 +		\
 					 sizeof(struct sta_rec_remove))
 
-- 
2.45.2