[rdk-b][common][bsp][Refactor and sync kernel/wifi from Openwrt]
[Description]
Refactor and sync kernel/wifi from Openwrt
1. wifi release from mt76 openwrt 2022/12/19
[Release-log]
N/A
diff --git a/recipes-wifi/linux-mt76/files/patches/1116-mt76-testmode-add-iBF-eBF-cal-and-cert-commands-with.patch b/recipes-wifi/linux-mt76/files/patches/1116-mt76-testmode-add-iBF-eBF-cal-and-cert-commands-with.patch
new file mode 100644
index 0000000..0ada55c
--- /dev/null
+++ b/recipes-wifi/linux-mt76/files/patches/1116-mt76-testmode-add-iBF-eBF-cal-and-cert-commands-with.patch
@@ -0,0 +1,1909 @@
+From f6c9069fc14f2c0749a8a55fbb55ad020f16838c Mon Sep 17 00:00:00 2001
+From: StanleyYP Wang <StanleyYP.Wang@mediatek.com>
+Date: Thu, 15 Dec 2022 19:45:18 +0800
+Subject: [PATCH 1116/1131] mt76: testmode: add iBF/eBF cal and cert commands
+ with golden
+
+Signed-off-by: StanleyYP Wang <StanleyYP.Wang@mediatek.com>
+---
+ mt76.h | 4 +
+ mt76_connac_mcu.c | 3 +
+ mt7915/mac.c | 4 +-
+ mt7915/main.c | 54 ++---
+ mt7915/mcu.c | 29 ++-
+ mt7915/mcu.h | 172 ++++++++++++++++
+ mt7915/mmio.c | 2 +
+ mt7915/mt7915.h | 14 +-
+ mt7915/mtk_debugfs.c | 35 ++++
+ mt7915/mtk_mcu.c | 246 ++++++++++++++++++++++-
+ mt7915/regs.h | 4 +
+ mt7915/testmode.c | 458 ++++++++++++++++++++++++++++---------------
+ mt7915/testmode.h | 134 +------------
+ testmode.c | 1 +
+ testmode.h | 9 +
+ tools/fields.c | 9 +
+ 16 files changed, 855 insertions(+), 323 deletions(-)
+
+diff --git a/mt76.h b/mt76.h
+index 2bacc1b..c6aefbf 100644
+--- a/mt76.h
++++ b/mt76.h
+@@ -681,6 +681,7 @@ struct mt76_testmode_data {
+
+ struct list_head tm_entry_list;
+ struct mt76_wcid *cur_entry;
++ struct ieee80211_vif *second_vif;
+ u8 entry_num;
+ union {
+ struct mt76_testmode_entry_data ed;
+@@ -709,6 +710,9 @@ struct mt76_testmode_data {
+
+ u8 txbf_act;
+ u16 txbf_param[8];
++ bool is_txbf_dut;
++ bool bf_en;
++ bool ebf;
+
+ u32 tx_pending;
+ u32 tx_queued;
+diff --git a/mt76_connac_mcu.c b/mt76_connac_mcu.c
+index 291a7c1..14ce09e 100644
+--- a/mt76_connac_mcu.c
++++ b/mt76_connac_mcu.c
+@@ -2756,6 +2756,7 @@ int mt76_connac_mcu_bss_basic_tlv(struct sk_buff *skb,
+ u32 type = vif->p2p ? NETWORK_P2P : NETWORK_INFRA;
+ struct bss_info_basic *bss;
+ struct tlv *tlv;
++ struct mt76_testmode_data *td = &phy->test;
+
+ tlv = mt76_connac_mcu_add_tlv(skb, BSS_INFO_BASIC, sizeof(*bss));
+ bss = (struct bss_info_basic *)tlv;
+@@ -2815,6 +2816,8 @@ int mt76_connac_mcu_bss_basic_tlv(struct sk_buff *skb,
+ bss->dtim_period = vif->bss_conf.dtim_period;
+ bss->phy_mode = mt76_connac_get_phy_mode(phy, vif,
+ chandef->chan->band, NULL);
++ } else if (td->bf_en) {
++ memcpy(bss->bssid, vif->addr, ETH_ALEN);
+ } else {
+ memcpy(bss->bssid, phy->macaddr, ETH_ALEN);
+ }
+diff --git a/mt7915/mac.c b/mt7915/mac.c
+index 0746e4b..013dfbd 100644
+--- a/mt7915/mac.c
++++ b/mt7915/mac.c
+@@ -753,8 +753,10 @@ mt7915_mac_write_txwi_tm(struct mt7915_phy *phy, __le32 *txwi,
+ val |= MT_TXD6_LDPC;
+
+ txwi[3] &= ~cpu_to_le32(MT_TXD3_SN_VALID);
+- if (phy->test.bf_en)
++ if (td->bf_en && !td->ebf)
+ val |= MT_TXD6_TX_IBF | MT_TXD6_TX_EBF;
++ else if (td->bf_en && td->ebf)
++ val |= MT_TXD6_TX_EBF;
+
+ txwi[6] |= cpu_to_le32(val);
+ #endif
+diff --git a/mt7915/main.c b/mt7915/main.c
+index 4c3d822..a347e77 100644
+--- a/mt7915/main.c
++++ b/mt7915/main.c
+@@ -205,46 +205,37 @@ static void mt7915_init_bitrate_mask(struct ieee80211_vif *vif)
+ }
+ }
+
+-static int mt7915_add_interface(struct ieee80211_hw *hw,
+- struct ieee80211_vif *vif)
++int mt7915_init_vif(struct mt7915_phy *phy, struct ieee80211_vif *vif, bool bf_en)
+ {
+ struct mt7915_vif *mvif = (struct mt7915_vif *)vif->drv_priv;
+- struct mt7915_dev *dev = mt7915_hw_dev(hw);
+- struct mt7915_phy *phy = mt7915_hw_phy(hw);
++ struct mt7915_dev *dev = phy->dev;
+ struct mt76_txq *mtxq;
+ bool ext_phy = phy != &dev->phy;
+ int idx, ret = 0;
+
+- mutex_lock(&dev->mt76.mutex);
+-
+- mt76_testmode_reset(phy->mt76, true);
+-
+- if (vif->type == NL80211_IFTYPE_MONITOR &&
+- is_zero_ether_addr(vif->addr))
+- phy->monitor_vif = vif;
++ /* To differentiate the mac address of TXD and TXCMD interface */
++ vif->addr[0] |= bf_en;
+
+ mvif->mt76.idx = __ffs64(~dev->mt76.vif_mask);
+- if (mvif->mt76.idx >= (MT7915_MAX_INTERFACES << dev->dbdc_support)) {
+- ret = -ENOSPC;
+- goto out;
+- }
++ if (mvif->mt76.idx >= (MT7915_MAX_INTERFACES << dev->dbdc_support))
++ return -ENOSPC;
+
+ idx = get_omac_idx(vif->type, phy->omac_mask);
+- if (idx < 0) {
+- ret = -ENOSPC;
+- goto out;
+- }
++ if (idx < 0)
++ return -ENOSPC;
++
+ mvif->mt76.omac_idx = idx;
+ mvif->phy = phy;
+ mvif->mt76.band_idx = phy->mt76->band_idx;
+
+- mvif->mt76.wmm_idx = (vif->type != NL80211_IFTYPE_AP && vif->type != NL80211_IFTYPE_MONITOR);
++ mvif->mt76.wmm_idx = (vif->type != NL80211_IFTYPE_AP &&
++ vif->type != NL80211_IFTYPE_MONITOR) || bf_en;
+ if (ext_phy)
+ mvif->mt76.wmm_idx += 2;
+
+ ret = mt7915_mcu_add_dev_info(phy, vif, true);
+ if (ret)
+- goto out;
++ return ret;
+
+ dev->mt76.vif_mask |= BIT_ULL(mvif->mt76.idx);
+ phy->omac_mask |= BIT_ULL(mvif->mt76.omac_idx);
+@@ -279,7 +270,26 @@ static int mt7915_add_interface(struct ieee80211_hw *hw,
+ mt7915_mcu_add_sta(dev, vif, NULL, true);
+ rcu_assign_pointer(dev->mt76.wcid[idx], &mvif->sta.wcid);
+
+-out:
++ return ret;
++}
++
++static int mt7915_add_interface(struct ieee80211_hw *hw,
++ struct ieee80211_vif *vif)
++{
++ struct mt7915_dev *dev = mt7915_hw_dev(hw);
++ struct mt7915_phy *phy = mt7915_hw_phy(hw);
++ int ret = 0;
++
++ mutex_lock(&dev->mt76.mutex);
++
++ mt76_testmode_reset(phy->mt76, true);
++
++ if (vif->type == NL80211_IFTYPE_MONITOR &&
++ is_zero_ether_addr(vif->addr))
++ phy->monitor_vif = vif;
++
++ ret = mt7915_init_vif(phy, vif, false);
++
+ mutex_unlock(&dev->mt76.mutex);
+
+ return ret;
+diff --git a/mt7915/mcu.c b/mt7915/mcu.c
+index b8243e1..5d41ebf 100644
+--- a/mt7915/mcu.c
++++ b/mt7915/mcu.c
+@@ -193,6 +193,7 @@ mt7915_mcu_send_message(struct mt76_dev *mdev, struct sk_buff *skb,
+ int ret;
+
+ ret = mt76_connac2_mcu_fill_message(mdev, skb, cmd, wait_seq);
++
+ if (ret)
+ return ret;
+
+@@ -383,10 +384,12 @@ mt7915_mcu_rx_ext_event(struct mt7915_dev *dev, struct sk_buff *skb)
+ case MCU_EXT_EVENT_BCC_NOTIFY:
+ mt7915_mcu_rx_bcc_notify(dev, skb);
+ break;
+-#ifdef CONFIG_NL80211_TESTMODE
++#if defined CONFIG_NL80211_TESTMODE || defined MTK_DEBUG
+ case MCU_EXT_EVENT_BF_STATUS_READ:
+- mt7915_tm_txbf_status_read(dev, skb);
++ mt7915_mcu_txbf_status_read(dev, skb);
+ break;
++#endif
++#ifdef CONFIG_NL80211_TESTMODE
+ case MCU_EXT_EVENT_RF_TEST:
+ mt7915_tm_rf_test_event(dev, skb);
+ break;
+@@ -673,11 +676,22 @@ int mt7915_mcu_add_bss_info(struct mt7915_phy *phy,
+ if (enable)
+ mt76_connac_mcu_bss_omac_tlv(skb, vif);
+
+- mt76_connac_mcu_bss_basic_tlv(skb, vif, NULL, phy->mt76,
+- mvif->sta.wcid.idx, enable);
++ if (vif->type == NL80211_IFTYPE_MONITOR) {
++ struct mt76_testmode_data *td = &phy->mt76->test;
++ struct mt76_wcid *wcid;
++
++ if (!td->aid || list_empty(&td->tm_entry_list))
++ wcid = &mvif->sta.wcid;
++ else
++ wcid = list_first_entry(&td->tm_entry_list, struct mt76_wcid, list);
+
+- if (vif->type == NL80211_IFTYPE_MONITOR)
++ mt76_connac_mcu_bss_basic_tlv(skb, vif, NULL, phy->mt76,
++ wcid->idx, enable);
+ goto out;
++ }
++
++ mt76_connac_mcu_bss_basic_tlv(skb, vif, NULL, phy->mt76,
++ mvif->sta.wcid.idx, enable);
+
+ if (enable) {
+ mt7915_mcu_bss_rfch_tlv(skb, vif, phy);
+@@ -3403,6 +3417,7 @@ int mt7915_mcu_set_ser(struct mt7915_dev *dev, u8 action, u8 set, u8 band)
+
+ int mt7915_mcu_set_txbf(struct mt7915_dev *dev, u8 action)
+ {
++#define MT_BF_PROCESSING 4
+ struct {
+ u8 action;
+ union {
+@@ -3429,7 +3444,6 @@ int mt7915_mcu_set_txbf(struct mt7915_dev *dev, u8 action)
+ .action = action,
+ };
+
+-#define MT_BF_PROCESSING 4
+ switch (action) {
+ case MT_BF_SOUNDING_ON:
+ req.snd.snd_mode = MT_BF_PROCESSING;
+@@ -4347,6 +4361,9 @@ int mt7915_mcu_set_txbf_sound_info(struct mt7915_phy *phy, u8 action,
+ req.he_opt = v2;
+ req.glo_opt = v3;
+ break;
++ case BF_SND_CFG_INF:
++ req.inf = v1;
++ break;
+ default:
+ return -EINVAL;
+ }
+diff --git a/mt7915/mcu.h b/mt7915/mcu.h
+index 9a48524..a30f52d 100644
+--- a/mt7915/mcu.h
++++ b/mt7915/mcu.h
+@@ -475,10 +475,12 @@ enum {
+ };
+
+ enum {
++ MT_BF_SOUNDING_OFF = 0,
+ MT_BF_SOUNDING_ON = 1,
+ MT_BF_DATA_PACKET_APPLY = 2,
+ MT_BF_PFMU_TAG_READ = 5,
+ MT_BF_PFMU_TAG_WRITE = 6,
++ MT_BF_STA_REC_READ = 13,
+ MT_BF_PHASE_CAL = 14,
+ MT_BF_IBF_PHASE_COMP = 15,
+ MT_BF_PROFILE_WRITE_ALL = 17,
+@@ -486,6 +488,176 @@ enum {
+ MT_BF_MODULE_UPDATE = 25
+ };
+
++#if defined CONFIG_NL80211_TESTMODE || defined MTK_DEBUG
++struct mt7915_pfmu_tag1 {
++ __le32 pfmu_idx:10;
++ __le32 ebf:1;
++ __le32 data_bw:2;
++ __le32 lm:2;
++ __le32 is_mu:1;
++ __le32 nr:3, nc:3;
++ __le32 codebook:2;
++ __le32 ngroup:2;
++ __le32 _rsv:2;
++ __le32 invalid_prof:1;
++ __le32 rmsd:3;
++
++ __le32 col_id1:6, row_id1:10;
++ __le32 col_id2:6, row_id2:10;
++ __le32 col_id3:6, row_id3:10;
++ __le32 col_id4:6, row_id4:10;
++
++ __le32 ru_start_id:7;
++ __le32 _rsv1:1;
++ __le32 ru_end_id:7;
++ __le32 _rsv2:1;
++ __le32 mob_cal_en:1;
++ __le32 _rsv3:15;
++
++ __le32 snr_sts0:8, snr_sts1:8, snr_sts2:8, snr_sts3:8;
++ __le32 snr_sts4:8, snr_sts5:8, snr_sts6:8, snr_sts7:8;
++
++ __le32 _rsv4;
++} __packed;
++
++struct mt7915_pfmu_tag2 {
++ __le32 smart_ant:24;
++ __le32 se_idx:5;
++ __le32 _rsv:3;
++
++ __le32 _rsv1:8;
++ __le32 rmsd_thres:3;
++ __le32 _rsv2:5;
++ __le32 ibf_timeout:8;
++ __le32 _rsv3:8;
++
++ __le32 _rsv4:16;
++ __le32 ibf_data_bw:2;
++ __le32 ibf_nc:3;
++ __le32 ibf_nr:3;
++ __le32 ibf_ru:8;
++
++ __le32 mob_delta_t:8;
++ __le32 mob_lq_result:7;
++ __le32 _rsv5:1;
++ __le32 _rsv6:16;
++
++ __le32 _rsv7;
++} __packed;
++
++struct mt7915_pfmu_tag {
++ struct mt7915_pfmu_tag1 t1;
++ struct mt7915_pfmu_tag2 t2;
++};
++
++struct mt7915_bf_status_hdr {
++ u8 format_id;
++ u8 bw;
++ u16 subcarrier_idx;
++ bool bfer;
++ u8 rsv[3];
++} __packed;
++
++struct mt7915_bf_status {
++ struct mt7915_bf_status_hdr hdr;
++ u8 buf[1000];
++} __packed;
++
++struct mt7915_txbf_phase_out {
++ u8 c0_l;
++ u8 c1_l;
++ u8 c2_l;
++ u8 c3_l;
++ u8 c0_m;
++ u8 c1_m;
++ u8 c2_m;
++ u8 c3_m;
++ u8 c0_h;
++ u8 c1_h;
++ u8 c2_h;
++ u8 c3_h;
++ u8 c0_uh;
++ u8 c1_uh;
++ u8 c2_uh;
++ u8 c3_uh;
++};
++
++struct mt7915_txbf_phase {
++ u8 status;
++ struct {
++ u8 r0_uh;
++ u8 r0_h;
++ u8 r0_m;
++ u8 r0_l;
++ u8 r0_ul;
++ u8 r1_uh;
++ u8 r1_h;
++ u8 r1_m;
++ u8 r1_l;
++ u8 r1_ul;
++ u8 r2_uh;
++ u8 r2_h;
++ u8 r2_m;
++ u8 r2_l;
++ u8 r2_ul;
++ u8 r3_uh;
++ u8 r3_h;
++ u8 r3_m;
++ u8 r3_l;
++ u8 r3_ul;
++ u8 r2_uh_sx2;
++ u8 r2_h_sx2;
++ u8 r2_m_sx2;
++ u8 r2_l_sx2;
++ u8 r2_ul_sx2;
++ u8 r3_uh_sx2;
++ u8 r3_h_sx2;
++ u8 r3_m_sx2;
++ u8 r3_l_sx2;
++ u8 r3_ul_sx2;
++ u8 m_t0_h;
++ u8 m_t1_h;
++ u8 m_t2_h;
++ u8 m_t2_h_sx2;
++ u8 r0_reserved;
++ u8 r1_reserved;
++ u8 r2_reserved;
++ u8 r3_reserved;
++ u8 r2_sx2_reserved;
++ u8 r3_sx2_reserved;
++ } phase;
++};
++
++struct mt7915_pfmu_data {
++ __le16 subc_idx;
++ __le16 phi11;
++ __le16 phi21;
++ __le16 phi31;
++};
++
++struct mt7915_ibf_cal_info {
++ u8 format_id;
++ u8 group_l_m_n;
++ u8 group;
++ bool sx2;
++ u8 status;
++ u8 cal_type;
++ u8 _rsv[2];
++ u8 buf[1000];
++} __packed;
++
++enum {
++ IBF_PHASE_CAL_UNSPEC,
++ IBF_PHASE_CAL_NORMAL,
++ IBF_PHASE_CAL_VERIFY,
++ IBF_PHASE_CAL_NORMAL_INSTRUMENT,
++ IBF_PHASE_CAL_VERIFY_INSTRUMENT,
++};
++
++#define MT7915_TXBF_SUBCAR_NUM 64
++
++#endif
++
+ enum {
+ MURU_SET_ARB_OP_MODE = 14,
+ MURU_SET_PLATFORM_TYPE = 25,
+diff --git a/mt7915/mmio.c b/mt7915/mmio.c
+index 26881fd..c1b421d 100644
+--- a/mt7915/mmio.c
++++ b/mt7915/mmio.c
+@@ -131,6 +131,7 @@ static const u32 mt7915_offs[] = {
+ [MDP_BNRCFR1] = 0x074,
+ [ARB_DRNGR0] = 0x194,
+ [ARB_SCR] = 0x080,
++ [ARB_TQSAXM0] = 0x030,
+ [RMAC_MIB_AIRTIME14] = 0x3b8,
+ [AGG_AALCR0] = 0x048,
+ [AGG_AWSCR0] = 0x05c,
+@@ -207,6 +208,7 @@ static const u32 mt7916_offs[] = {
+ [MDP_BNRCFR1] = 0x094,
+ [ARB_DRNGR0] = 0x1e0,
+ [ARB_SCR] = 0x000,
++ [ARB_TQSAXM0] = 0x180,
+ [RMAC_MIB_AIRTIME14] = 0x0398,
+ [AGG_AALCR0] = 0x028,
+ [AGG_AWSCR0] = 0x030,
+diff --git a/mt7915/mt7915.h b/mt7915/mt7915.h
+index d845206..ab1efe6 100644
+--- a/mt7915/mt7915.h
++++ b/mt7915/mt7915.h
+@@ -322,7 +322,6 @@ struct mt7915_phy {
+
+ u8 spe_idx;
+
+- bool bf_en;
+ bool bf_ever_en;
+ } test;
+ #endif
+@@ -427,7 +426,7 @@ struct mt7915_dev {
+ void __iomem *dcm;
+ void __iomem *sku;
+
+-#ifdef CONFIG_NL80211_TESTMODE
++#if defined CONFIG_NL80211_TESTMODE || defined MTK_DEBUG
+ struct {
+ void *txbf_phase_cal;
+ void *txbf_pfmu_data;
+@@ -566,6 +565,7 @@ void mt7915_dma_prefetch(struct mt7915_dev *dev);
+ void mt7915_dma_cleanup(struct mt7915_dev *dev);
+ int mt7915_dma_reset(struct mt7915_dev *dev, bool force);
+ int mt7915_txbf_init(struct mt7915_dev *dev);
++int mt7915_init_vif(struct mt7915_phy *phy, struct ieee80211_vif *vif, bool bf_en);
+ void mt7915_init_txpower(struct mt7915_dev *dev,
+ struct ieee80211_supported_band *sband);
+ void mt7915_reset(struct mt7915_dev *dev);
+@@ -644,8 +644,10 @@ int mt7915_mcu_fw_log_2_host(struct mt7915_dev *dev, u8 type, u8 ctrl);
+ int mt7915_mcu_fw_dbg_ctrl(struct mt7915_dev *dev, u32 module, u8 level);
+ void mt7915_mcu_rx_event(struct mt7915_dev *dev, struct sk_buff *skb);
+ void mt7915_mcu_exit(struct mt7915_dev *dev);
+-int mt7915_tm_txbf_status_read(struct mt7915_dev *dev, struct sk_buff *skb);
++
++#ifdef CONFIG_NL80211_TESTMODE
+ void mt7915_tm_rf_test_event(struct mt7915_dev *dev, struct sk_buff *skb);
++#endif
+
+ static inline u16 mt7915_wtbl_size(struct mt7915_dev *dev)
+ {
+@@ -782,4 +784,10 @@ enum {
+
+ #endif
+
++#if defined CONFIG_NL80211_TESTMODE || defined MTK_DEBUG
++int mt7915_mcu_txbf_status_read(struct mt7915_dev *dev, struct sk_buff *skb);
++int mt7915_mcu_txbf_profile_tag_read(struct mt7915_phy *phy, u8 pfmu_idx, bool bfer);
++int mt7915_mcu_txbf_sta_rec_read(struct mt7915_dev *dev, u16 wlan_idx);
++#endif
++
+ #endif
+diff --git a/mt7915/mtk_debugfs.c b/mt7915/mtk_debugfs.c
+index 583bc19..a2ceb6f 100644
+--- a/mt7915/mtk_debugfs.c
++++ b/mt7915/mtk_debugfs.c
+@@ -2831,6 +2831,36 @@ mt7915_txpower_level_set(void *data, u64 val)
+ DEFINE_DEBUGFS_ATTRIBUTE(fops_txpower_level, NULL,
+ mt7915_txpower_level_set, "%lld\n");
+
++static int
++mt7915_txbf_pfmu_tag_read(void *data, u64 val)
++{
++ struct mt7915_phy *phy = data;
++ u8 pfmu_idx = (u8)val;
++
++ pr_info("%s: %d pfmu_tag cmd sent out ---\n", __func__, __LINE__);
++ mt7915_mcu_txbf_profile_tag_read(phy, pfmu_idx, true);
++
++ return 0;
++}
++
++DEFINE_DEBUGFS_ATTRIBUTE(fops_txbf_pfmu_tag_idx, NULL,
++ mt7915_txbf_pfmu_tag_read, "%llx\n");
++
++static int
++mt7915_txbf_sta_rec_read(void *data, u64 val)
++{
++ struct mt7915_dev *dev = data;
++ u16 wlan_idx = (u16)val;
++
++ pr_info("%s: %d sta_rec cmd sent out ---\n", __func__, __LINE__);
++ mt7915_mcu_txbf_sta_rec_read(dev, wlan_idx);
++
++ return 0;
++}
++
++DEFINE_DEBUGFS_ATTRIBUTE(fops_txbf_sta_rec, NULL,
++ mt7915_txbf_sta_rec_read, "%llx\n");
++
+ /* usage: echo 0x[arg3][arg2][arg1] > fw_wa_set */
+ static int
+ mt7915_wa_set(void *data, u64 val)
+@@ -2969,6 +2999,11 @@ int mt7915_mtk_init_debugfs(struct mt7915_phy *phy, struct dentry *dir)
+ debugfs_create_file("txpower_level", 0400, dir, dev,
+ &fops_txpower_level);
+
++ debugfs_create_file("txbf_pfmu_tag_idx", 0600, dir, phy,
++ &fops_txbf_pfmu_tag_idx);
++ debugfs_create_file("txbf_sta_rec", 0600, dir, dev,
++ &fops_txbf_sta_rec);
++
+ debugfs_create_u8("sku_disable", 0600, dir, &dev->dbg.sku_disable);
+
+ debugfs_create_devm_seqfile(dev->mt76.dev, "fw_version", dir,
+diff --git a/mt7915/mtk_mcu.c b/mt7915/mtk_mcu.c
+index 143dae2..906c872 100644
+--- a/mt7915/mtk_mcu.c
++++ b/mt7915/mtk_mcu.c
+@@ -1,9 +1,10 @@
+ #include <linux/firmware.h>
+ #include <linux/fs.h>
+-#include<linux/inet.h>
++#include <linux/inet.h>
+ #include "mt7915.h"
+ #include "mcu.h"
+ #include "mac.h"
++#include "testmode.h"
+
+ int mt7915_mcu_set_txpower_level(struct mt7915_phy *phy, u8 drop_level)
+ {
+@@ -49,3 +50,246 @@ int mt7915_mcu_set_txpower_level(struct mt7915_phy *phy, u8 drop_level)
+ MCU_EXT_CMD(TX_POWER_FEATURE_CTRL), &req,
+ sizeof(req), true);
+ }
++
++#if defined CONFIG_NL80211_TESTMODE || defined MTK_DEBUG
++static void mt7915_txbf_dump_pfmu_tag(struct mt7915_dev *dev, struct mt7915_pfmu_tag *tag)
++{
++ u32 *raw_t1 = (u32 *)&tag->t1;
++ u32 *raw_t2 = (u32 *)&tag->t2;
++
++ dev_info(dev->mt76.dev, "=================== TXBf Profile Tag1 Info ==================\n");
++ dev_info(dev->mt76.dev,
++ "DW0 = 0x%08x, DW1 = 0x%08x, DW2 = 0x%08x\n",
++ raw_t1[0], raw_t1[1], raw_t1[2]);
++ dev_info(dev->mt76.dev,
++ "DW4 = 0x%08x, DW5 = 0x%08x, DW6 = 0x%08x\n\n",
++ raw_t1[3], raw_t1[4], raw_t1[5]);
++ dev_info(dev->mt76.dev, "PFMU ID = %d Invalid status = %d\n",
++ tag->t1.pfmu_idx, tag->t1.invalid_prof);
++ dev_info(dev->mt76.dev, "iBf/eBf = %d\n\n", tag->t1.ebf);
++ dev_info(dev->mt76.dev, "DBW = %d\n", tag->t1.data_bw);
++ dev_info(dev->mt76.dev, "SU/MU = %d\n", tag->t1.is_mu);
++ dev_info(dev->mt76.dev, "RMSD = %d\n", tag->t1.rmsd);
++ dev_info(dev->mt76.dev,
++ "nrow = %d, ncol = %d, ng = %d, LM = %d, CodeBook = %d MobCalEn = %d\n",
++ tag->t1.nr, tag->t1.nc, tag->t1.ngroup, tag->t1.lm, tag->t1.codebook,
++ tag->t1.mob_cal_en);
++ dev_info(dev->mt76.dev, "RU start = %d, RU end = %d\n",
++ tag->t1.ru_start_id, tag->t1.ru_end_id);
++ dev_info(dev->mt76.dev, "Mem Col1 = %d, Mem Row1 = %d, Mem Col2 = %d, Mem Row2 = %d\n",
++ tag->t1.col_id1, tag->t1.row_id1, tag->t1.col_id2, tag->t1.row_id2);
++ dev_info(dev->mt76.dev, "Mem Col3 = %d, Mem Row3 = %d, Mem Col4 = %d, Mem Row4 = %d\n\n",
++ tag->t1.col_id3, tag->t1.row_id3, tag->t1.col_id4, tag->t1.row_id4);
++ dev_info(dev->mt76.dev,
++ "STS0_SNR = 0x%02x, STS1_SNR = 0x%02x, STS2_SNR = 0x%02x, STS3_SNR = 0x%02x\n",
++ tag->t1.snr_sts0, tag->t1.snr_sts1, tag->t1.snr_sts2, tag->t1.snr_sts3);
++ dev_info(dev->mt76.dev,
++ "STS4_SNR = 0x%02x, STS5_SNR = 0x%02x, STS6_SNR = 0x%02x, STS7_SNR = 0x%02x\n",
++ tag->t1.snr_sts4, tag->t1.snr_sts5, tag->t1.snr_sts6, tag->t1.snr_sts7);
++ dev_info(dev->mt76.dev, "=============================================================\n");
++
++ dev_info(dev->mt76.dev, "=================== TXBf Profile Tag2 Info ==================\n");
++ dev_info(dev->mt76.dev,
++ "DW0 = 0x%08x, DW1 = 0x%08x, DW2 = 0x%08x\n",
++ raw_t2[0], raw_t2[1], raw_t2[2]);
++ dev_info(dev->mt76.dev,
++ "DW3 = 0x%08x, DW4 = 0x%08x, DW5 = 0x%08x\n\n",
++ raw_t2[3], raw_t2[4], raw_t2[5]);
++ dev_info(dev->mt76.dev, "Smart antenna ID = 0x%x, SE index = %d\n",
++ tag->t2.smart_ant, tag->t2.se_idx);
++ dev_info(dev->mt76.dev, "RMSD threshold = %d\n", tag->t2.rmsd_thres);
++ dev_info(dev->mt76.dev, "Timeout = 0x%x\n", tag->t2.ibf_timeout);
++ dev_info(dev->mt76.dev, "Desired BW = %d, Desired Ncol = %d, Desired Nrow = %d\n",
++ tag->t2.ibf_data_bw, tag->t2.ibf_nc, tag->t2.ibf_nr);
++ dev_info(dev->mt76.dev, "Desired RU Allocation = %d\n", tag->t2.ibf_ru);
++ dev_info(dev->mt76.dev, "Mobility DeltaT = %d, Mobility LQ = %d\n",
++ tag->t2.mob_delta_t, tag->t2.mob_lq_result);
++ dev_info(dev->mt76.dev, "=============================================================\n");
++}
++
++static void mt7915_txbf_dump_sta_rec(struct mt7915_dev *dev, struct sta_rec_bf *sta_info)
++{
++ dev_info(dev->mt76.dev, "===================== BF Station Record =====================\n");
++ dev_info(dev->mt76.dev, "pfmu = %d\n", sta_info->pfmu);
++ dev_info(dev->mt76.dev, "su_mu = %d\n", sta_info->su_mu);
++ dev_info(dev->mt76.dev, "bf_cap = %d\n", sta_info->bf_cap);
++ dev_info(dev->mt76.dev, "sounding_phy = %d\n", sta_info->sounding_phy);
++ dev_info(dev->mt76.dev, "ndpa_rate = %d\n", sta_info->ndpa_rate);
++ dev_info(dev->mt76.dev, "ndp_rate = %d\n", sta_info->ndp_rate);
++ dev_info(dev->mt76.dev, "rept_poll_rate = %d\n", sta_info->rept_poll_rate);
++ dev_info(dev->mt76.dev, "tx_mode = %d\n", sta_info->tx_mode);
++ dev_info(dev->mt76.dev, "ncol = %d\n", sta_info->ncol);
++ dev_info(dev->mt76.dev, "nrow = %d\n", sta_info->nrow);
++ dev_info(dev->mt76.dev, "bw = %d\n", sta_info->bw);
++ dev_info(dev->mt76.dev, "mem_total = %d\n", sta_info->mem_total);
++ dev_info(dev->mt76.dev, "mem_20m = %d\n", sta_info->mem_20m);
++ dev_info(dev->mt76.dev, "mem_row0 = %d\n", sta_info->mem[0].row);
++ dev_info(dev->mt76.dev, "mem_col0 = %d\n", sta_info->mem[0].col);
++ dev_info(dev->mt76.dev, "mem_row1 = %d\n", sta_info->mem[1].row);
++ dev_info(dev->mt76.dev, "mem_col1 = %d\n", sta_info->mem[1].col);
++ dev_info(dev->mt76.dev, "mem_row2 = %d\n", sta_info->mem[2].row);
++ dev_info(dev->mt76.dev, "mem_col2 = %d\n", sta_info->mem[2].col);
++ dev_info(dev->mt76.dev, "mem_row3 = %d\n", sta_info->mem[3].row);
++ dev_info(dev->mt76.dev, "mem_col3 = %d\n", sta_info->mem[3].col);
++ dev_info(dev->mt76.dev, "smart_ant = 0x%x\n", sta_info->smart_ant);
++ dev_info(dev->mt76.dev, "se_idx = %d\n", sta_info->se_idx);
++ dev_info(dev->mt76.dev, "auto_sounding = %d\n", sta_info->auto_sounding);
++ dev_info(dev->mt76.dev, "ibf_timeout = 0x%x\n", sta_info->ibf_timeout);
++ dev_info(dev->mt76.dev, "ibf_dbw = %d\n", sta_info->ibf_dbw);
++ dev_info(dev->mt76.dev, "ibf_ncol = %d\n", sta_info->ibf_ncol);
++ dev_info(dev->mt76.dev, "ibf_nrow = %d\n", sta_info->ibf_nrow);
++ dev_info(dev->mt76.dev, "nrow_gt_bw80 = %d\n", sta_info->nrow_gt_bw80);
++ dev_info(dev->mt76.dev, "ncol_gt_bw80 = %d\n", sta_info->ncol_gt_bw80);
++ dev_info(dev->mt76.dev, "ru_start_idx = %d\n", sta_info->ru_start_idx);
++ dev_info(dev->mt76.dev, "trigger_su = %d\n", sta_info->trigger_su);
++ dev_info(dev->mt76.dev, "trigger_mu = %d\n", sta_info->trigger_mu);
++ dev_info(dev->mt76.dev, "ng16_su = %d\n", sta_info->ng16_su);
++ dev_info(dev->mt76.dev, "ng16_mu = %d\n", sta_info->ng16_mu);
++ dev_info(dev->mt76.dev, "codebook42_su = %d\n", sta_info->codebook42_su);
++ dev_info(dev->mt76.dev, "codebook75_mu = %d\n", sta_info->codebook75_mu);
++ dev_info(dev->mt76.dev, "he_ltf = %d\n", sta_info->he_ltf);
++ dev_info(dev->mt76.dev, "=============================================================\n");
++}
++
++static void mt7915_txbf_dump_cal_phase(struct mt7915_dev *dev,
++ struct mt7915_txbf_phase *phase, int group)
++{
++ dev_info(dev->mt76.dev, "Group %d and Group M\n", group);
++ dev_info(dev->mt76.dev, "m_t0_h = %d\n", phase->phase.m_t0_h);
++ dev_info(dev->mt76.dev, "m_t1_h = %d\n", phase->phase.m_t1_h);
++ dev_info(dev->mt76.dev, "m_t2_h = %d\n", phase->phase.m_t2_h);
++
++ dev_info(dev->mt76.dev, "r0_uh = %d\n", phase->phase.r0_uh);
++ dev_info(dev->mt76.dev, "r0_h = %d\n", phase->phase.r0_h);
++ dev_info(dev->mt76.dev, "r0_m = %d\n", phase->phase.r0_m);
++ dev_info(dev->mt76.dev, "r0_l = %d\n", phase->phase.r0_l);
++
++ dev_info(dev->mt76.dev, "r1_uh = %d\n", phase->phase.r1_uh);
++ dev_info(dev->mt76.dev, "r1_h = %d\n", phase->phase.r1_h);
++ dev_info(dev->mt76.dev, "r1_m = %d\n", phase->phase.r1_m);
++ dev_info(dev->mt76.dev, "r1_l = %d\n", phase->phase.r1_l);
++
++ dev_info(dev->mt76.dev, "r2_uh = %d\n", phase->phase.r2_uh);
++ dev_info(dev->mt76.dev, "r2_h = %d\n", phase->phase.r2_h);
++ dev_info(dev->mt76.dev, "r2_m = %d\n", phase->phase.r2_m);
++ dev_info(dev->mt76.dev, "r2_l = %d\n", phase->phase.r2_l);
++
++ dev_info(dev->mt76.dev, "r3_uh = %d\n", phase->phase.r3_uh);
++ dev_info(dev->mt76.dev, "r3_h = %d\n", phase->phase.r3_h);
++ dev_info(dev->mt76.dev, "r3_m = %d\n", phase->phase.r3_m);
++ dev_info(dev->mt76.dev, "r3_l = %d\n", phase->phase.r3_l);
++ dev_info(dev->mt76.dev, "r3_ul = %d\n", phase->phase.r3_ul);
++}
++
++int mt7915_mcu_txbf_status_read(struct mt7915_dev *dev, struct sk_buff *skb)
++{
++#define BF_PFMU_TAG 16
++#define BF_STA_REC 20
++#define BF_CAL_PHASE 21
++#define GROUP_M 1
++ u8 format_id;
++
++ skb_pull(skb, sizeof(struct mt76_connac2_mcu_rxd));
++ format_id = *(u8 *)skb->data;
++
++ if (format_id == BF_PFMU_TAG) {
++ struct mt7915_pfmu_tag *pfmu_tag;
++
++ skb_pull(skb, 8);
++ pfmu_tag = (struct mt7915_pfmu_tag *)skb->data;
++ mt7915_txbf_dump_pfmu_tag(dev, pfmu_tag);
++ if (dev->test.txbf_pfmu_tag)
++ memcpy(dev->test.txbf_pfmu_tag, pfmu_tag, sizeof(struct mt7915_pfmu_tag));
++ } else if (format_id == BF_STA_REC) {
++ struct sta_rec_bf *sta_rec;
++
++ skb_pull(skb, sizeof(struct mt7915_bf_status_hdr));
++ /* padding 4 byte since bf_status->buf does not contain tag & len */
++ skb_push(skb, 4);
++ sta_rec = (struct sta_rec_bf *)skb->data;
++
++ mt7915_txbf_dump_sta_rec(dev, sta_rec);
++ } else if (format_id == BF_CAL_PHASE) {
++ u8 phase_out_len = sizeof(struct mt7915_txbf_phase_out);
++ struct mt7915_ibf_cal_info *cal;
++ struct mt7915_txbf_phase_out phase_out;
++ struct mt7915_txbf_phase *phase =
++ (struct mt7915_txbf_phase *)dev->test.txbf_phase_cal;
++
++ cal = (struct mt7915_ibf_cal_info *)skb->data;
++ memcpy(&phase_out, cal->buf, phase_out_len);
++ switch (cal->cal_type) {
++ case IBF_PHASE_CAL_NORMAL:
++ case IBF_PHASE_CAL_NORMAL_INSTRUMENT:
++ /* Only calibrate group M */
++ if (cal->group_l_m_n != GROUP_M)
++ break;
++ phase = &phase[cal->group];
++ memcpy(&phase->phase, cal->buf + phase_out_len, sizeof(phase->phase));
++ phase->status = cal->status;
++
++ dev_info(dev->mt76.dev, "Calibrated result = %d\n", phase->status);
++ mt7915_txbf_dump_cal_phase(dev, phase, cal->group);
++ break;
++ case IBF_PHASE_CAL_VERIFY:
++ case IBF_PHASE_CAL_VERIFY_INSTRUMENT:
++ dev_info(dev->mt76.dev, "Verification result = %d\n", cal->status);
++ break;
++ default:
++ break;
++ }
++
++ dev_info(dev->mt76.dev, "c0_h = %d, c1_h = %d, c2_h = %d\n",
++ phase_out.c0_h, phase_out.c1_h, phase_out.c2_h);
++ dev_info(dev->mt76.dev, "c0_m = %d, c1_m = %d, c2_m = %d\n",
++ phase_out.c0_m, phase_out.c1_m, phase_out.c2_m);
++ dev_info(dev->mt76.dev, "c0_l = %d, c1_l = %d, c2_l = %d\n",
++ phase_out.c0_l, phase_out.c1_l, phase_out.c2_l);
++ dev_info(dev->mt76.dev, "c3_m = %d, c3_h = %d\n", phase_out.c3_m, phase_out.c3_h);
++ }
++
++ wake_up(&dev->mt76.tx_wait);
++
++ return 0;
++}
++
++int mt7915_mcu_txbf_profile_tag_read(struct mt7915_phy *phy, u8 pfmu_idx, bool bfer)
++{
++ struct mt7915_dev *dev = phy->dev;
++ struct {
++ u8 format_id;
++ u8 pfmu_idx;
++ bool bfer;
++ u8 dbdc_idx;
++ } __packed req = {
++ .format_id = MT_BF_PFMU_TAG_READ,
++ .pfmu_idx = pfmu_idx,
++ .bfer = bfer,
++ .dbdc_idx = phy->mt76->band_idx,
++ };
++ struct mt7915_pfmu_tag *tag = phy->dev->test.txbf_pfmu_tag;
++
++ /* Reset to 0 for mt7915_tm_txbf_profile_tag_write wait_event */
++ tag->t1.pfmu_idx = 0;
++
++ return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(TXBF_ACTION), &req,
++ sizeof(req), true);
++}
++
++int mt7915_mcu_txbf_sta_rec_read(struct mt7915_dev *dev, u16 wlan_idx)
++{
++ struct {
++ u8 action;
++ u8 wlan_idx_lo;
++ u8 wlan_idx_hi;
++ u8 rsv[5];
++ } __packed req = {
++ .action = MT_BF_STA_REC_READ,
++ .wlan_idx_lo = to_wcid_lo(wlan_idx),
++ .wlan_idx_hi = to_wcid_hi(wlan_idx),
++ };
++
++ return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(TXBF_ACTION), &req,
++ sizeof(req), true);
++}
++#endif
+diff --git a/mt7915/regs.h b/mt7915/regs.h
+index f4b5709..007fd4d 100644
+--- a/mt7915/regs.h
++++ b/mt7915/regs.h
+@@ -61,6 +61,7 @@ enum offs_rev {
+ MDP_BNRCFR1,
+ ARB_DRNGR0,
+ ARB_SCR,
++ ARB_TQSAXM0,
+ RMAC_MIB_AIRTIME14,
+ AGG_AALCR0,
+ AGG_AWSCR0,
+@@ -530,6 +531,9 @@ enum offs_rev {
+ #define MT_ARB_DRNGR0(_band, _n) MT_WF_ARB(_band, (__OFFS(ARB_DRNGR0) + \
+ (_n) * 4))
+
++#define MT_ARB_TQSAXM0(_band) MT_WF_ARB(_band, __OFFS(ARB_TQSAXM0))
++#define MT_ARB_TQSAXM_ALTX_START_MASK GENMASK(12, 8)
++
+ /* RMAC: band 0(0x820e5000), band 1(0x820f5000) */
+ #define MT_WF_RMAC_BASE(_band) ((_band) ? 0x820f5000 : 0x820e5000)
+ #define MT_WF_RMAC(_band, ofs) (MT_WF_RMAC_BASE(_band) + (ofs))
+diff --git a/mt7915/testmode.c b/mt7915/testmode.c
+index 2ae6c07..70382b9 100644
+--- a/mt7915/testmode.c
++++ b/mt7915/testmode.c
+@@ -53,6 +53,8 @@ struct reg_band {
+ static struct reg_band reg_backup_list[TM_REG_MAX_ID];
+
+ static void mt7915_tm_update_entry(struct mt7915_phy *phy);
++static int mt7915_tm_set_ipg_params(struct mt7915_phy *phy, u32 ipg, u8 mode, bool bf_sounding);
++static int mt7915_tm_txbf_set_rate(struct mt7915_phy *phy, struct mt76_wcid *wcid);
+
+ static u8 mt7915_tm_chan_bw(enum nl80211_chan_width width)
+ {
+@@ -73,6 +75,25 @@ static u8 mt7915_tm_chan_bw(enum nl80211_chan_width width)
+ return width_to_bw[width];
+ }
+
++static u8 mt7915_tm_rate_to_phy(u8 tx_rate_mode)
++{
++ static const u8 rate_to_phy[] = {
++ [MT76_TM_TX_MODE_CCK] = MT_PHY_TYPE_CCK,
++ [MT76_TM_TX_MODE_OFDM] = MT_PHY_TYPE_OFDM,
++ [MT76_TM_TX_MODE_HT] = MT_PHY_TYPE_HT,
++ [MT76_TM_TX_MODE_VHT] = MT_PHY_TYPE_VHT,
++ [MT76_TM_TX_MODE_HE_SU] = MT_PHY_TYPE_HE_SU,
++ [MT76_TM_TX_MODE_HE_EXT_SU] = MT_PHY_TYPE_HE_EXT_SU,
++ [MT76_TM_TX_MODE_HE_TB] = MT_PHY_TYPE_HE_TB,
++ [MT76_TM_TX_MODE_HE_MU] = MT_PHY_TYPE_HE_MU,
++ };
++
++ if (tx_rate_mode > MT76_TM_TX_MODE_MAX)
++ return -EINVAL;
++
++ return rate_to_phy[tx_rate_mode];
++}
++
+ static void
+ mt7915_tm_update_channel(struct mt7915_phy *phy)
+ {
+@@ -273,17 +294,33 @@ mt7915_tm_add_txbf(struct mt7915_phy *phy, struct ieee80211_vif *vif,
+ struct mt7915_vif *mvif = (struct mt7915_vif *)vif->drv_priv;
+ struct mt7915_sta *msta = (struct mt7915_sta *)sta->drv_priv;
+ struct mt7915_dev *dev = phy->dev;
++ struct mt76_testmode_data *td = &phy->mt76->test;
+ struct sk_buff *skb;
+ struct sta_rec_bf *bf;
+ struct tlv *tlv;
+- u8 ndp_rate;
++ u8 ndp_rate, ndpa_rate, rept_poll_rate, bf_bw;
++
++ if (td->tx_rate_mode == MT76_TM_TX_MODE_HE_SU) {
++ rept_poll_rate = 0x49;
++ ndpa_rate = 0x49;
++ ndp_rate = 0;
++ } else if (td->tx_rate_mode == MT76_TM_TX_MODE_VHT) {
++ rept_poll_rate = 0x9;
++ ndpa_rate = 0x9;
++ ndp_rate = 0;
++ } else {
++ rept_poll_rate = 0;
++ ndpa_rate = 0;
++ if (nr == 1)
++ ndp_rate = 8;
++ else if (nr == 2)
++ ndp_rate = 16;
++ else
++ ndp_rate = 24;
++ }
+
+- if (nr == 1)
+- ndp_rate = 8;
+- else if (nr == 2)
+- ndp_rate = 16;
+- else
+- ndp_rate = 24;
++ /* TODO: check 160Hz */
++ bf_bw = mt7915_tm_chan_bw(phy->mt76->chandef.width);
+
+ skb = mt76_connac_mcu_alloc_sta_req(&dev->mt76, &mvif->mt76,
+ &msta->wcid);
+@@ -299,8 +336,11 @@ mt7915_tm_add_txbf(struct mt7915_phy *phy, struct ieee80211_vif *vif,
+ bf->ncol = nc;
+ bf->nrow = nr;
+ bf->ndp_rate = ndp_rate;
++ bf->ndpa_rate = ndpa_rate;
++ bf->rept_poll_rate = rept_poll_rate;
++ bf->bw = bf_bw;
+ bf->ibf_timeout = 0xff;
+- bf->tx_mode = MT_PHY_TYPE_HT;
++ bf->tx_mode = mt7915_tm_rate_to_phy(td->tx_rate_mode);
+
+ if (ebf) {
+ bf->mem[0].row = 0;
+@@ -353,11 +393,8 @@ mt7915_tm_entry_add(struct mt7915_phy *phy, u8 aid)
+ }
+
+ memcpy(sta->addr, ed->addr[0], ETH_ALEN);
+- if (phy->test.bf_en) {
+- u8 addr[ETH_ALEN] = {0x00, 0x11, 0x11, 0x11, 0x11, 0x11};
+-
+- memcpy(sta->addr, addr, ETH_ALEN);
+- }
++ if (td->bf_en)
++ memcpy(sta->addr, td->addr[0], ETH_ALEN);
+
+ if (td->tx_rate_mode >= MT76_TM_TX_MODE_HT)
+ memcpy(&sta->deflink.ht_cap, &sband->ht_cap, sizeof(sta->deflink.ht_cap));
+@@ -382,6 +419,14 @@ mt7915_tm_entry_add(struct mt7915_phy *phy, u8 aid)
+ list_add_tail(&msta->wcid.list, &td->tm_entry_list);
+ td->entry_num++;
+
++ mt7915_mcu_add_bss_info(phy, phy->monitor_vif, true);
++
++ if (td->bf_en) {
++ mt7915_tm_set_ipg_params(phy, td->tx_ipg, td->tx_rate_mode, true);
++ mt7915_tm_set_tam_arb(phy, td->bf_en, 0);
++ mt7915_tm_txbf_set_rate(phy, &msta->wcid);
++ }
++
+ return 0;
+ }
+
+@@ -451,7 +496,7 @@ mt7915_tm_update_entry(struct mt7915_phy *phy)
+ struct mt76_testmode_entry_data *ed, tmp;
+ struct mt76_wcid *wcid, *last;
+
+- if (!td->aid || phy->test.bf_en)
++ if (!td->aid || td->bf_en)
+ return;
+
+ memcpy(&tmp, &td->ed, sizeof(tmp));
+@@ -472,20 +517,30 @@ mt7915_tm_update_entry(struct mt7915_phy *phy)
+ static int
+ mt7915_tm_txbf_init(struct mt7915_phy *phy, u16 *val)
+ {
++#define EBF_BBP_RX_OFFSET 0x10280
++#define EBF_BBP_RX_ENABLE (BIT(0) | BIT(15))
++#define WF1 BIT(1)
++#define WF2 BIT(2)
+ struct mt76_testmode_data *td = &phy->mt76->test;
+ struct mt7915_dev *dev = phy->dev;
++ struct mt76_phy *mphy = phy->mt76;
+ bool enable = val[0];
+ void *phase_cal, *pfmu_data, *pfmu_tag;
+- u8 addr[ETH_ALEN] = {0x00, 0x22, 0x22, 0x22, 0x22, 0x22};
++ u8 sub_addr = td->is_txbf_dut ? TXBF_DUT_MAC_SUBADDR : TXBF_GOLDEN_MAC_SUBADDR;
++ u8 peer_addr = td->is_txbf_dut ? TXBF_GOLDEN_MAC_SUBADDR : TXBF_DUT_MAC_SUBADDR;
++ u8 bss_addr = TXBF_DUT_MAC_SUBADDR;
++ u8 addr[ETH_ALEN] = {0x00, sub_addr, sub_addr, sub_addr, sub_addr, sub_addr};
++ u8 bssid[ETH_ALEN] = {0x00, bss_addr, bss_addr, bss_addr, bss_addr, bss_addr};
++ u8 peer_addrs[ETH_ALEN] = {0x00, peer_addr, peer_addr, peer_addr, peer_addr, peer_addr};
+
+ if (!enable) {
+- phy->test.bf_en = 0;
++ td->bf_en = 0;
+ return 0;
+ }
+
+ if (!dev->test.txbf_phase_cal) {
+ phase_cal = devm_kzalloc(dev->mt76.dev,
+- sizeof(struct mt7915_tm_txbf_phase) *
++ sizeof(struct mt7915_txbf_phase) *
+ MAX_PHASE_GROUP_NUM,
+ GFP_KERNEL);
+ if (!phase_cal)
+@@ -495,7 +550,10 @@ mt7915_tm_txbf_init(struct mt7915_phy *phy, u16 *val)
+ }
+
+ if (!dev->test.txbf_pfmu_data) {
+- pfmu_data = devm_kzalloc(dev->mt76.dev, 512, GFP_KERNEL);
++ pfmu_data = devm_kzalloc(dev->mt76.dev,
++ sizeof(struct mt7915_pfmu_data) *
++ MT7915_TXBF_SUBCAR_NUM,
++ GFP_KERNEL);
+ if (!pfmu_data)
+ return -ENOMEM;
+
+@@ -504,21 +562,77 @@ mt7915_tm_txbf_init(struct mt7915_phy *phy, u16 *val)
+
+ if (!dev->test.txbf_pfmu_tag) {
+ pfmu_tag = devm_kzalloc(dev->mt76.dev,
+- sizeof(struct mt7915_tm_pfmu_tag), GFP_KERNEL);
++ sizeof(struct mt7915_pfmu_tag), GFP_KERNEL);
+ if (!pfmu_tag)
+ return -ENOMEM;
+
+ dev->test.txbf_pfmu_tag = pfmu_tag;
+ }
+
++ td->bf_en = 1;
++ memcpy(td->addr[0], peer_addrs, ETH_ALEN);
++ memcpy(td->addr[1], addr, ETH_ALEN);
++ memcpy(td->addr[2], bssid, ETH_ALEN);
+ memcpy(phy->monitor_vif->addr, addr, ETH_ALEN);
+ mt7915_mcu_add_dev_info(phy, phy->monitor_vif, true);
+
+- td->tx_rate_mode = MT76_TM_TX_MODE_HT;
+- td->tx_mpdu_len = 1024;
+- td->tx_rate_sgi = 0;
+- td->tx_ipg = 100;
+- phy->test.bf_en = 1;
++ /* Add second interface in wtbl for using TXCMD to transmit sounding */
++ td->second_vif = kzalloc(sizeof(*td->second_vif) + sizeof(struct mt7915_vif), GFP_KERNEL);
++ memcpy(td->second_vif, phy->monitor_vif, sizeof(*td->second_vif));
++ mt7915_init_vif(phy, td->second_vif, td->bf_en);
++
++ if (td->ebf && !td->is_txbf_dut) {
++ bool is_160hz = val[1];
++
++ /* Turn On BBP CR for RX */
++ mt76_set(dev, EBF_BBP_RX_OFFSET, EBF_BBP_RX_ENABLE);
++ dev_info(dev->mt76.dev, "Set BBP RX CR = %x\n", mt76_rr(dev, EBF_BBP_RX_OFFSET));
++
++ /* Set TX antenna mask of golden: default use WF0 only */
++ td->tx_antenna_mask = 1;
++ if (is_mt7915(&dev->mt76)) {
++ /* Add WF1/WF2 for dbdc/single band in BW 160 */
++ td->tx_antenna_mask |= is_160hz & (dev->dbdc_support ? WF1 : WF2);
++ /* Shift to WF2/WF3 for dbdc band 1 */
++ td->tx_antenna_mask <<= 2 * phy->mt76->band_idx;
++ }
++ } else if (td->ebf && td->is_txbf_dut) {
++ /* Enable ETxBF Capability */
++ dev->ibf = false;
++ mt7915_mcu_set_txbf(dev, MT_BF_TYPE_UPDATE);
++ /* Set TX antenna mask of DUT */
++ td->tx_antenna_mask = mphy->chainmask >> (dev->chainshift * phy->mt76->band_idx);
++ td->tx_spe_idx = phy->mt76->band_idx ? 25 : 24;
++ /* Shift to WF2/WF3 for dbdc band 1, Nss = 2 */
++ if ((hweight8(td->tx_antenna_mask) == 2) && phy->mt76->band_idx)
++ td->tx_antenna_mask <<= 2;
++ } else {
++ if (td->is_txbf_dut) {
++ int nss;
++
++ /* Enable ITxBF Capability */
++ dev->ibf = true;
++ mt7915_mcu_set_txbf(dev, MT_BF_TYPE_UPDATE);
++ td->tx_antenna_mask = mphy->chainmask >> (dev->chainshift *
++ phy->mt76->band_idx);
++ nss = hweight8(td->tx_antenna_mask);
++ if (nss > 1 && nss <= 4)
++ td->tx_rate_idx = 15 + 8 * (nss - 2);
++ else
++ td->tx_rate_idx = 31;
++ } else {
++ td->tx_antenna_mask = 1;
++ mt76_set(dev, EBF_BBP_RX_OFFSET, EBF_BBP_RX_ENABLE);
++ dev_info(dev->mt76.dev, "Set BBP RX CR = %x\n",
++ mt76_rr(dev, EBF_BBP_RX_OFFSET));
++ }
++ td->tx_rate_mode = MT76_TM_TX_MODE_HT;
++ td->tx_mpdu_len = 1024;
++ td->tx_rate_sgi = 0;
++ td->tx_ipg = 100;
++ }
++
++ mt7915_mcu_add_bss_info(phy, phy->monitor_vif, true);
+
+ return mt7915_tm_set_trx(phy, TM_MAC_TX, true);
+ }
+@@ -545,8 +659,7 @@ mt7915_tm_txbf_phase_comp(struct mt7915_phy *phy, u16 *val)
+ .read_from_e2p = val[3],
+ .disable = val[4],
+ };
+- struct mt7915_tm_txbf_phase *phase =
+- (struct mt7915_tm_txbf_phase *)dev->test.txbf_phase_cal;
++ struct mt7915_txbf_phase *phase = (struct mt7915_txbf_phase *)dev->test.txbf_phase_cal;
+
+ wait_event_timeout(dev->mt76.tx_wait, phase[val[2]].status != 0, HZ);
+ memcpy(req.buf, &phase[val[2]].phase, sizeof(req.buf));
+@@ -559,32 +672,9 @@ mt7915_tm_txbf_phase_comp(struct mt7915_phy *phy, u16 *val)
+ sizeof(req), true);
+ }
+
+-static int
+-mt7915_tm_txbf_profile_tag_read(struct mt7915_phy *phy, u8 pfmu_idx)
+-{
+- struct mt7915_dev *dev = phy->dev;
+- struct {
+- u8 format_id;
+- u8 pfmu_idx;
+- bool bfer;
+- u8 dbdc_idx;
+- } __packed req = {
+- .format_id = MT_BF_PFMU_TAG_READ,
+- .pfmu_idx = pfmu_idx,
+- .bfer = 1,
+- .dbdc_idx = phy != &dev->phy,
+- };
+- struct mt7915_tm_pfmu_tag *tag = phy->dev->test.txbf_pfmu_tag;
+-
+- tag->t1.pfmu_idx = 0;
+-
+- return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(TXBF_ACTION), &req,
+- sizeof(req), true);
+-}
+-
+ static int
+ mt7915_tm_txbf_profile_tag_write(struct mt7915_phy *phy, u8 pfmu_idx,
+- struct mt7915_tm_pfmu_tag *tag)
++ struct mt7915_pfmu_tag *tag)
+ {
+ struct mt7915_dev *dev = phy->dev;
+ struct {
+@@ -611,8 +701,6 @@ static int
+ mt7915_tm_txbf_apply_tx(struct mt7915_phy *phy, u16 wlan_idx, bool ebf,
+ bool ibf, bool phase_cal)
+ {
+-#define to_wcid_lo(id) FIELD_GET(GENMASK(7, 0), (u16)id)
+-#define to_wcid_hi(id) FIELD_GET(GENMASK(9, 8), (u16)id)
+ struct mt7915_dev *dev = phy->dev;
+ struct {
+ u8 category;
+@@ -641,14 +729,15 @@ static int mt7915_tm_txbf_set_rate(struct mt7915_phy *phy,
+ {
+ struct mt7915_dev *dev = phy->dev;
+ struct mt76_testmode_entry_data *ed = mt76_testmode_entry_data(phy->mt76, wcid);
++ struct mt76_testmode_data *td = &phy->mt76->test;
+ struct ieee80211_sta *sta = wcid_to_sta(wcid);
+ struct sta_phy rate = {};
+
+ if (!sta)
+ return 0;
+
+- rate.type = MT_PHY_TYPE_HT;
+- rate.bw = mt7915_tm_chan_bw(phy->mt76->chandef.width);
++ rate.type = mt7915_tm_rate_to_phy(td->tx_rate_mode);
++ rate.bw = mt7915_tm_chan_bw(phy->mt76->chandef.width); /* TODO: check 160Hz */
+ rate.nss = ed->tx_rate_nss;
+ rate.mcs = ed->tx_rate_idx;
+ rate.ldpc = (rate.bw || ed->tx_rate_ldpc) * GENMASK(2, 0);
+@@ -662,13 +751,14 @@ mt7915_tm_txbf_set_tx(struct mt7915_phy *phy, u16 *val)
+ {
+ bool bf_on = val[0], update = val[3];
+ /* u16 wlan_idx = val[2]; */
+- struct mt7915_tm_pfmu_tag *tag = phy->dev->test.txbf_pfmu_tag;
++ struct mt7915_dev *dev = phy->dev;
++ struct mt7915_pfmu_tag *tag = dev->test.txbf_pfmu_tag;
+ struct mt76_testmode_data *td = &phy->mt76->test;
+ struct mt76_wcid *wcid;
+
+ if (bf_on) {
+ mt7915_tm_set_trx(phy, TM_MAC_RX_RXV, false);
+- mt7915_tm_txbf_profile_tag_read(phy, 2);
++ mt7915_mcu_txbf_profile_tag_read(phy, 2, true);
+ tag->t1.invalid_prof = false;
+ mt7915_tm_txbf_profile_tag_write(phy, 2, tag);
+
+@@ -683,7 +773,7 @@ mt7915_tm_txbf_set_tx(struct mt7915_phy *phy, u16 *val)
+ } else {
+ phy->test.bf_ever_en = false;
+
+- mt7915_tm_txbf_profile_tag_read(phy, 2);
++ mt7915_mcu_txbf_profile_tag_read(phy, 2, true);
+ tag->t1.invalid_prof = true;
+ mt7915_tm_txbf_profile_tag_write(phy, 2, tag);
+ }
+@@ -698,6 +788,7 @@ mt7915_tm_txbf_set_tx(struct mt7915_phy *phy, u16 *val)
+ static int
+ mt7915_tm_txbf_profile_update(struct mt7915_phy *phy, u16 *val, bool ebf)
+ {
++#define MT_ARB_IBF_ENABLE (BIT(0) | GENMASK(9, 8))
+ static const u8 mode_to_lm[] = {
+ [MT76_TM_TX_MODE_CCK] = 0,
+ [MT76_TM_TX_MODE_OFDM] = 0,
+@@ -711,7 +802,8 @@ mt7915_tm_txbf_profile_update(struct mt7915_phy *phy, u16 *val, bool ebf)
+ struct mt76_testmode_data *td = &phy->mt76->test;
+ struct mt76_wcid *wcid;
+ struct ieee80211_vif *vif = phy->monitor_vif;
+- struct mt7915_tm_pfmu_tag *tag = phy->dev->test.txbf_pfmu_tag;
++ struct mt7915_dev *dev = phy->dev;
++ struct mt7915_pfmu_tag *tag = dev->test.txbf_pfmu_tag;
+ u8 pfmu_idx = val[0], nc = val[2], nr;
+ bool is_atenl = val[6];
+ int ret;
+@@ -729,11 +821,15 @@ mt7915_tm_txbf_profile_update(struct mt7915_phy *phy, u16 *val, bool ebf)
+ tag->t1.nr = nr;
+ tag->t1.nc = nc;
+ tag->t1.invalid_prof = true;
+-
+- tag->t1.snr_sts4 = 0xc0;
+- tag->t1.snr_sts5 = 0xff;
+- tag->t1.snr_sts6 = 0xff;
+- tag->t1.snr_sts7 = 0xff;
++ tag->t1.data_bw = mt7915_tm_chan_bw(phy->mt76->chandef.width); /* TODO: check 160Hz */
++ tag->t2.se_idx = td->tx_spe_idx;
++
++ if (is_atenl) {
++ tag->t1.snr_sts4 = 0xc0;
++ tag->t1.snr_sts5 = 0xff;
++ tag->t1.snr_sts6 = 0xff;
++ tag->t1.snr_sts7 = 0xff;
++ }
+
+ if (ebf) {
+ tag->t1.row_id1 = 0;
+@@ -761,6 +857,19 @@ mt7915_tm_txbf_profile_update(struct mt7915_phy *phy, u16 *val, bool ebf)
+ if (ret)
+ return ret;
+
++ if (td->ebf) {
++ mt76_set(dev, MT_ARB_TQSAXM0(phy->mt76->band_idx), MT_ARB_TQSAXM_ALTX_START_MASK);
++ dev_info(dev->mt76.dev, "Set TX queue start CR for AX management (0x%x) = 0x%x\n",
++ MT_ARB_TQSAXM0(phy->mt76->band_idx),
++ mt76_rr(dev, MT_ARB_TQSAXM0(phy->mt76->band_idx)));
++ } else if (!td->ebf && ebf) {
++ /* iBF's ebf profile update */
++ mt76_set(dev, MT_ARB_TQSAXM0(phy->mt76->band_idx), MT_ARB_IBF_ENABLE);
++ dev_info(dev->mt76.dev, "Set TX queue start CR for AX management (0x%x) = 0x%x\n",
++ MT_ARB_TQSAXM0(phy->mt76->band_idx),
++ mt76_rr(dev, MT_ARB_TQSAXM0(phy->mt76->band_idx)));
++ }
++
+ if (!ebf && is_atenl)
+ return mt7915_tm_txbf_apply_tx(phy, 1, false, true, true);
+
+@@ -778,7 +887,7 @@ mt7915_tm_txbf_phase_cal(struct mt7915_phy *phy, u16 *val)
+ u8 category;
+ u8 group_l_m_n;
+ u8 group;
+- bool sx2;
++ bool dbdc_idx;
+ u8 cal_type;
+ u8 lna_gain_level;
+ u8 _rsv[2];
+@@ -786,12 +895,12 @@ mt7915_tm_txbf_phase_cal(struct mt7915_phy *phy, u16 *val)
+ .category = MT_BF_PHASE_CAL,
+ .group = val[0],
+ .group_l_m_n = val[1],
+- .sx2 = val[2],
++ .dbdc_idx = phy->mt76->band_idx,
+ .cal_type = val[3],
+ .lna_gain_level = val[4],
+ };
+- struct mt7915_tm_txbf_phase *phase =
+- (struct mt7915_tm_txbf_phase *)dev->test.txbf_phase_cal;
++ struct mt7915_txbf_phase *phase =
++ (struct mt7915_txbf_phase *)dev->test.txbf_phase_cal;
+
+ phase[req.group].status = 0;
+
+@@ -799,53 +908,10 @@ mt7915_tm_txbf_phase_cal(struct mt7915_phy *phy, u16 *val)
+ sizeof(req), true);
+ }
+
+-int mt7915_tm_txbf_status_read(struct mt7915_dev *dev, struct sk_buff *skb)
+-{
+-#define BF_PFMU_TAG 16
+-#define BF_CAL_PHASE 21
+- u8 format_id;
+-
+- skb_pull(skb, sizeof(struct mt76_connac2_mcu_rxd));
+- format_id = *(u8 *)skb->data;
+-
+- if (format_id == BF_PFMU_TAG) {
+- struct mt7915_tm_pfmu_tag *tag = dev->test.txbf_pfmu_tag;
+-
+- skb_pull(skb, 8);
+- memcpy(tag, skb->data, sizeof(struct mt7915_tm_pfmu_tag));
+- } else if (format_id == BF_CAL_PHASE) {
+- struct mt7915_tm_ibf_cal_info *cal;
+- struct mt7915_tm_txbf_phase *phase =
+- (struct mt7915_tm_txbf_phase *)dev->test.txbf_phase_cal;
+-
+- cal = (struct mt7915_tm_ibf_cal_info *)skb->data;
+- switch (cal->cal_type) {
+- case IBF_PHASE_CAL_NORMAL:
+- case IBF_PHASE_CAL_NORMAL_INSTRUMENT:
+- if (cal->group_l_m_n != GROUP_M)
+- break;
+- phase = &phase[cal->group];
+- memcpy(&phase->phase, cal->buf + 16, sizeof(phase->phase));
+- phase->status = cal->status;
+- /* for passing iTest script */
+- dev_info(dev->mt76.dev, "Calibrated result = %d\n", phase->status);
+- break;
+- case IBF_PHASE_CAL_VERIFY:
+- case IBF_PHASE_CAL_VERIFY_INSTRUMENT:
+- break;
+- default:
+- break;
+- }
+- }
+-
+- wake_up(&dev->mt76.tx_wait);
+-
+- return 0;
+-}
+-
+ static int
+ mt7915_tm_txbf_profile_update_all(struct mt7915_phy *phy, u16 *val)
+ {
++#define MT7915_TXBF_PFMU_DATA_LEN (MT7915_TXBF_SUBCAR_NUM * sizeof(struct mt7915_pfmu_data))
+ struct mt76_testmode_data *td = &phy->mt76->test;
+ u16 pfmu_idx = val[0];
+ u16 subc_id = val[1];
+@@ -854,9 +920,9 @@ mt7915_tm_txbf_profile_update_all(struct mt7915_phy *phy, u16 *val)
+ u16 angle31 = val[4];
+ u16 angle41 = val[5];
+ s16 phi11 = 0, phi21 = 0, phi31 = 0;
+- struct mt7915_tm_pfmu_data *pfmu_data;
++ struct mt7915_pfmu_data *pfmu_data;
+
+- if (subc_id > 63)
++ if (subc_id > MT7915_TXBF_SUBCAR_NUM - 1)
+ return -EINVAL;
+
+ if (td->tx_antenna_mask == 2) {
+@@ -870,7 +936,7 @@ mt7915_tm_txbf_profile_update_all(struct mt7915_phy *phy, u16 *val)
+ phi31 = (s16)(angle41 - angle31);
+ }
+
+- pfmu_data = (struct mt7915_tm_pfmu_data *)phy->dev->test.txbf_pfmu_data;
++ pfmu_data = (struct mt7915_pfmu_data *)phy->dev->test.txbf_pfmu_data;
+ pfmu_data = &pfmu_data[subc_id];
+
+ if (subc_id < 32)
+@@ -880,21 +946,21 @@ mt7915_tm_txbf_profile_update_all(struct mt7915_phy *phy, u16 *val)
+ pfmu_data->phi11 = cpu_to_le16(phi11);
+ pfmu_data->phi21 = cpu_to_le16(phi21);
+ pfmu_data->phi31 = cpu_to_le16(phi31);
+- if (subc_id == 63) {
++ if (subc_id == MT7915_TXBF_SUBCAR_NUM - 1) {
+ struct mt7915_dev *dev = phy->dev;
+ struct {
+ u8 format_id;
+ u8 pfmu_idx;
+ u8 dbdc_idx;
+ u8 _rsv;
+- u8 buf[512];
++ u8 buf[MT7915_TXBF_PFMU_DATA_LEN];
+ } __packed req = {
+ .format_id = MT_BF_PROFILE_WRITE_ALL,
+ .pfmu_idx = pfmu_idx,
+ .dbdc_idx = phy != &dev->phy,
+ };
+
+- memcpy(req.buf, dev->test.txbf_pfmu_data, 512);
++ memcpy(req.buf, dev->test.txbf_pfmu_data, MT7915_TXBF_PFMU_DATA_LEN);
+
+ return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(TXBF_ACTION),
+ &req, sizeof(req), true);
+@@ -906,7 +972,7 @@ mt7915_tm_txbf_profile_update_all(struct mt7915_phy *phy, u16 *val)
+ static int
+ mt7915_tm_txbf_e2p_update(struct mt7915_phy *phy)
+ {
+- struct mt7915_tm_txbf_phase *phase, *p;
++ struct mt7915_txbf_phase *phase, *p;
+ struct mt7915_dev *dev = phy->dev;
+ u8 *eeprom = dev->mt76.eeprom.data;
+ u16 offset;
+@@ -916,7 +982,7 @@ mt7915_tm_txbf_e2p_update(struct mt7915_phy *phy)
+ is_7976 = mt7915_check_adie(dev, false) || is_mt7916(&dev->mt76);
+ offset = is_7976 ? 0x60a : 0x651;
+
+- phase = (struct mt7915_tm_txbf_phase *)dev->test.txbf_phase_cal;
++ phase = (struct mt7915_txbf_phase *)dev->test.txbf_phase_cal;
+ for (i = 0; i < MAX_PHASE_GROUP_NUM; i++) {
+ p = &phase[i];
+
+@@ -931,17 +997,75 @@ mt7915_tm_txbf_e2p_update(struct mt7915_phy *phy)
+ return 0;
+ }
+
++static int
++mt7915_tm_trigger_sounding(struct mt7915_phy *phy, u16 *val, bool en)
++{
++ struct mt7915_dev *dev = phy->dev;
++ u8 sounding_mode = val[0];
++ u8 MU_num = val[1];
++ u32 sounding_interval = (u32)val[2] << 2; /* input unit: 4ms */
++ enum sounding_mode {
++ SU_SOUNDING,
++ MU_SOUNDING,
++ SU_PERIODIC_SOUNDING,
++ MU_PERIODIC_SOUNDING,
++ BF_PROCESSING,
++ TXCMD_NONTB_SU_SOUNDING,
++ TXCMD_VHT_MU_SOUNDING,
++ TXCMD_TB_PER_BRP_SOUNDING,
++ TXCMD_TB_SOUNDING,
++
++ /* keep last */
++ NUM_SOUNDING_MODE,
++ SOUNDING_MODE_MAX = NUM_SOUNDING_MODE - 1,
++ };
++ struct {
++ u8 cmd_category_id;
++ u8 sounding_mode;
++ u8 MU_num;
++ u8 rsv;
++ u8 wlan_idx[4];
++ u32 sounding_interval; /* unit: ms */
++ } __packed req = {
++ .cmd_category_id = en ? MT_BF_SOUNDING_ON : MT_BF_SOUNDING_OFF,
++ .sounding_mode = sounding_mode,
++ .MU_num = MU_num,
++ .sounding_interval = cpu_to_le32(sounding_interval),
++ .wlan_idx[0] = val[3],
++ .wlan_idx[1] = val[4],
++ .wlan_idx[2] = val[5],
++ .wlan_idx[3] = val[6],
++ };
++
++ if (sounding_mode > SOUNDING_MODE_MAX)
++ return -EINVAL;
++
++ /* Enable Tx MAC HW before trigger sounding */
++ if (en)
++ mt7915_tm_set_trx(phy, TM_MAC_TX, true);
++
++ return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(TXBF_ACTION),
++ &req, sizeof(req), true);
++}
++
+ static int
+ mt7915_tm_set_txbf(struct mt7915_phy *phy)
+ {
++#define TXBF_IS_DUT_MASK BIT(0)
++#define TXBF_EBF_MASK BIT(1)
+ struct mt76_testmode_data *td = &phy->mt76->test;
+ u16 *val = td->txbf_param;
+
+- pr_info("ibf cal process: act = %u, val = %u, %u, %u, %u, %u, %u\n",
+- td->txbf_act, val[0], val[1], val[2], val[3], val[4], val[5]);
++ dev_info(phy->dev->mt76.dev, "ibf cal process: act = %u, val = %u, %u, %u, %u, %u, %u\n",
++ td->txbf_act, val[0], val[1], val[2], val[3], val[4], val[5], val[6]);
+
+ switch (td->txbf_act) {
++ case MT76_TM_TXBF_ACT_GOLDEN_INIT:
+ case MT76_TM_TXBF_ACT_INIT:
++ case MT76_TM_TX_EBF_ACT_GOLDEN_INIT:
++ case MT76_TM_TX_EBF_ACT_INIT:
++ td->ebf = !!u32_get_bits(td->txbf_act, TXBF_EBF_MASK);
++ td->is_txbf_dut = !!u32_get_bits(td->txbf_act, TXBF_IS_DUT_MASK);
+ return mt7915_tm_txbf_init(phy, val);
+ case MT76_TM_TXBF_ACT_UPDATE_CH:
+ mt7915_tm_update_channel(phy);
+@@ -967,6 +1091,36 @@ mt7915_tm_set_txbf(struct mt7915_phy *phy)
+
+ return mt7915_tm_txbf_apply_tx(phy, wlan_idx, ebf, ibf, phase_cal);
+ }
++ case MT76_TM_TXBF_ACT_TRIGGER_SOUNDING:
++ return mt7915_tm_trigger_sounding(phy, val, true);
++ case MT76_TM_TXBF_ACT_STOP_SOUNDING:
++ memset(val, 0, sizeof(td->txbf_param));
++ return mt7915_tm_trigger_sounding(phy, val, false);
++ case MT76_TM_TXBF_ACT_PROFILE_TAG_READ:
++ case MT76_TM_TXBF_ACT_PROFILE_TAG_WRITE:
++ case MT76_TM_TXBF_ACT_PROFILE_TAG_INVALID: {
++ u8 pfmu_idx = val[0];
++ bool bfer = !!val[1];
++ struct mt7915_dev *dev = phy->dev;
++ struct mt7915_pfmu_tag *tag = dev->test.txbf_pfmu_tag;
++
++ if (!tag) {
++ dev_err(dev->mt76.dev,
++ "pfmu tag is not initialized!\n");
++ return 0;
++ }
++
++ if (td->txbf_act == MT76_TM_TXBF_ACT_PROFILE_TAG_WRITE)
++ return mt7915_tm_txbf_profile_tag_write(phy, pfmu_idx, tag);
++ else if (td->txbf_act == MT76_TM_TXBF_ACT_PROFILE_TAG_READ)
++ return mt7915_mcu_txbf_profile_tag_read(phy, pfmu_idx, bfer);
++
++ tag->t1.invalid_prof = !!val[0];
++
++ return 0;
++ }
++ case MT76_TM_TXBF_ACT_STA_REC_READ:
++ return mt7915_mcu_txbf_sta_rec_read(phy->dev, val[0]);
+ default:
+ break;
+ };
+@@ -1186,9 +1340,10 @@ mt7915_tm_set_ipi(struct mt7915_phy *phy)
+
+ static int
+ mt7915_tm_set_wmm_qid(struct mt7915_phy *phy, u8 qid, u8 aifs, u8 cw_min,
+- u16 cw_max, u16 txop, u8 tx_cmd)
++ u16 cw_max, u16 txop, u8 tx_cmd, bool bf_sounding)
+ {
+- struct mt7915_vif *mvif = (struct mt7915_vif *)phy->monitor_vif->drv_priv;
++ struct mt76_testmode_data *td = &phy->mt76->test;
++ struct mt7915_vif *mvif;
+ struct mt7915_mcu_tx req = {
+ .valid = true,
+ .mode = tx_cmd,
+@@ -1196,6 +1351,9 @@ mt7915_tm_set_wmm_qid(struct mt7915_phy *phy, u8 qid, u8 aifs, u8 cw_min,
+ };
+ struct edca *e = &req.edca[0];
+
++ mvif = bf_sounding ? (struct mt7915_vif *)td->second_vif->drv_priv :
++ (struct mt7915_vif *)phy->monitor_vif->drv_priv;
++
+ e->queue = qid + mvif->mt76.wmm_idx * MT76_CONNAC_MAX_WMM_SETS;
+ e->set = WMM_PARAM_SET;
+
+@@ -1208,17 +1366,19 @@ mt7915_tm_set_wmm_qid(struct mt7915_phy *phy, u8 qid, u8 aifs, u8 cw_min,
+ }
+
+ static int
+-mt7915_tm_set_ipg_params(struct mt7915_phy *phy, u32 ipg, u8 mode)
++mt7915_tm_set_ipg_params(struct mt7915_phy *phy, u32 ipg, u8 mode, bool bf_sounding)
+ {
+ #define TM_DEFAULT_SIFS 10
+ #define TM_MAX_SIFS 127
+ #define TM_MAX_AIFSN 0xf
+ #define TM_MIN_AIFSN 0x1
+ #define BBP_PROC_TIME 1500
++#define TM_DEFAULT_CW 1
+ struct mt7915_dev *dev = phy->dev;
+ u8 sig_ext = (mode == MT76_TM_TX_MODE_CCK) ? 0 : 6;
+ u8 slot_time = 9, sifs = TM_DEFAULT_SIFS;
+ u8 aifsn = TM_MIN_AIFSN;
++ bool tx_cmd;
+ u8 band = phy->mt76->band_idx;
+ u32 i2t_time, tr2t_time, txv_time;
+ u16 cw = 0;
+@@ -1232,6 +1392,7 @@ mt7915_tm_set_ipg_params(struct mt7915_phy *phy, u32 ipg, u8 mode)
+ ipg -= sig_ext;
+
+ if (ipg <= (TM_MAX_SIFS + slot_time)) {
++ cw = TM_DEFAULT_CW;
+ sifs = ipg - slot_time;
+ } else {
+ u32 val = (ipg + slot_time) / slot_time;
+@@ -1267,10 +1428,12 @@ done:
+
+ mt7915_tm_set_slot_time(phy, slot_time, sifs);
+
++ /* HE MU data and iBF/eBF sounding packet use TXCMD */
++ tx_cmd = (mode == MT76_TM_TX_MODE_HE_MU) || bf_sounding;
++
+ return mt7915_tm_set_wmm_qid(phy,
+ mt76_connac_lmac_mapping(IEEE80211_AC_BE),
+- aifsn, cw, cw, 0,
+- mode == MT76_TM_TX_MODE_HE_MU);
++ aifsn, cw, cw, 0, tx_cmd, bf_sounding);
+ }
+
+ static int
+@@ -1469,7 +1632,7 @@ mt7915_tm_init(struct mt7915_phy *phy, bool en)
+
+ phy->mt76->test.aid = 0;
+ phy->mt76->test.tx_mpdu_len = 0;
+- phy->test.bf_en = 0;
++ phy->mt76->test.bf_en = 0;
+ mt7915_tm_set_entry(phy);
+ } else {
+ INIT_DELAYED_WORK(&phy->ipi_work, mt7915_tm_ipi_work);
+@@ -1654,7 +1817,7 @@ mt7915_tm_set_tx_frames(struct mt7915_phy *phy, bool en)
+ u32 tx_time = td->tx_time, ipg = td->tx_ipg;
+ u8 duty_cycle = td->tx_duty_cycle;
+
+- if (!phy->test.bf_en)
++ if (!td->bf_en)
+ mt7915_tm_update_channel(phy);
+
+ if (td->tx_spe_idx)
+@@ -1669,7 +1832,7 @@ mt7915_tm_set_tx_frames(struct mt7915_phy *phy, bool en)
+ if (duty_cycle < 100)
+ tx_time = duty_cycle * ipg / (100 - duty_cycle);
+ }
+- mt7915_tm_set_ipg_params(phy, ipg, td->tx_rate_mode);
++ mt7915_tm_set_ipg_params(phy, ipg, td->tx_rate_mode, false);
+ mt7915_tm_set_tx_len(phy, tx_time);
+
+ if (ipg)
+@@ -1688,6 +1851,9 @@ mt7915_tm_set_tx_frames(struct mt7915_phy *phy, bool en)
+ mt7915_tm_tx_frames_mu(phy, en);
+
+ mt7915_tm_set_trx(phy, TM_MAC_TX, en);
++
++ if (td->bf_en)
++ mt7915_tm_set_trx(phy, TM_MAC_RX_RXV, en);
+ }
+
+ static int
+@@ -1779,7 +1945,7 @@ mt7915_tm_set_rx_frames(struct mt7915_phy *phy, bool en)
+ mt7915_tm_set_trx(phy, TM_MAC_RX_RXV, false);
+
+ if (en) {
+- if (!phy->test.bf_en)
++ if (!td->bf_en || !td->is_txbf_dut)
+ mt7915_tm_update_channel(phy);
+ if (td->aid)
+ mt7915_tm_set_rx_user_idx(phy, td->aid);
+@@ -1796,6 +1962,9 @@ mt7915_tm_set_rx_frames(struct mt7915_phy *phy, bool en)
+ mt7915_tm_set_muru_aid(phy, en ? td->aid : 0xf800);
+
+ mt7915_tm_set_trx(phy, TM_MAC_RX_RXV, en);
++
++ if (td->bf_en)
++ mt7915_tm_set_trx(phy, TM_MAC_TX, en);
+ }
+
+ static int
+@@ -1855,34 +2024,7 @@ mt7915_tm_set_tx_cont(struct mt7915_phy *phy, bool en)
+ rate_idx = sband->bitrates[idx].hw_value & 0xff;
+ }
+
+- switch (td->tx_rate_mode) {
+- case MT76_TM_TX_MODE_CCK:
+- mode = MT_PHY_TYPE_CCK;
+- break;
+- case MT76_TM_TX_MODE_OFDM:
+- mode = MT_PHY_TYPE_OFDM;
+- break;
+- case MT76_TM_TX_MODE_HT:
+- mode = MT_PHY_TYPE_HT;
+- break;
+- case MT76_TM_TX_MODE_VHT:
+- mode = MT_PHY_TYPE_VHT;
+- break;
+- case MT76_TM_TX_MODE_HE_SU:
+- mode = MT_PHY_TYPE_HE_SU;
+- break;
+- case MT76_TM_TX_MODE_HE_EXT_SU:
+- mode = MT_PHY_TYPE_HE_EXT_SU;
+- break;
+- case MT76_TM_TX_MODE_HE_TB:
+- mode = MT_PHY_TYPE_HE_TB;
+- break;
+- case MT76_TM_TX_MODE_HE_MU:
+- mode = MT_PHY_TYPE_HE_MU;
+- break;
+- default:
+- return -EINVAL;
+- }
++ mode = mt7915_tm_rate_to_phy(td->tx_rate_mode);
+
+ rateval = mode << 6 | rate_idx;
+ tx_cont->rateval = cpu_to_le16(rateval);
+diff --git a/mt7915/testmode.h b/mt7915/testmode.h
+index d500987..1982369 100644
+--- a/mt7915/testmode.h
++++ b/mt7915/testmode.h
+@@ -311,137 +311,7 @@ struct mt7915_tm_muru {
+
+ #define MAX_PHASE_GROUP_NUM 9
+
+-struct mt7915_tm_txbf_phase {
+- u8 status;
+- struct {
+- u8 r0_uh;
+- u8 r0_h;
+- u8 r0_m;
+- u8 r0_l;
+- u8 r0_ul;
+- u8 r1_uh;
+- u8 r1_h;
+- u8 r1_m;
+- u8 r1_l;
+- u8 r1_ul;
+- u8 r2_uh;
+- u8 r2_h;
+- u8 r2_m;
+- u8 r2_l;
+- u8 r2_ul;
+- u8 r3_uh;
+- u8 r3_h;
+- u8 r3_m;
+- u8 r3_l;
+- u8 r3_ul;
+- u8 r2_uh_sx2;
+- u8 r2_h_sx2;
+- u8 r2_m_sx2;
+- u8 r2_l_sx2;
+- u8 r2_ul_sx2;
+- u8 r3_uh_sx2;
+- u8 r3_h_sx2;
+- u8 r3_m_sx2;
+- u8 r3_l_sx2;
+- u8 r3_ul_sx2;
+- u8 m_t0_h;
+- u8 m_t1_h;
+- u8 m_t2_h;
+- u8 m_t2_h_sx2;
+- u8 r0_reserved;
+- u8 r1_reserved;
+- u8 r2_reserved;
+- u8 r3_reserved;
+- u8 r2_sx2_reserved;
+- u8 r3_sx2_reserved;
+- } phase;
+-};
+-
+-struct mt7915_tm_pfmu_tag1 {
+- __le32 pfmu_idx:10;
+- __le32 ebf:1;
+- __le32 data_bw:2;
+- __le32 lm:2;
+- __le32 is_mu:1;
+- __le32 nr:3, nc:3;
+- __le32 codebook:2;
+- __le32 ngroup:2;
+- __le32 _rsv:2;
+- __le32 invalid_prof:1;
+- __le32 rmsd:3;
+-
+- __le32 col_id1:6, row_id1:10;
+- __le32 col_id2:6, row_id2:10;
+- __le32 col_id3:6, row_id3:10;
+- __le32 col_id4:6, row_id4:10;
+-
+- __le32 ru_start_id:7;
+- __le32 _rsv1:1;
+- __le32 ru_end_id:7;
+- __le32 _rsv2:1;
+- __le32 mob_cal_en:1;
+- __le32 _rsv3:15;
+-
+- __le32 snr_sts0:8, snr_sts1:8, snr_sts2:8, snr_sts3:8;
+- __le32 snr_sts4:8, snr_sts5:8, snr_sts6:8, snr_sts7:8;
+-
+- __le32 _rsv4;
+-} __packed;
+-
+-struct mt7915_tm_pfmu_tag2 {
+- __le32 smart_ant:24;
+- __le32 se_idx:5;
+- __le32 _rsv:3;
+-
+- __le32 _rsv1:8;
+- __le32 rmsd_thres:3;
+- __le32 _rsv2:5;
+- __le32 ibf_timeout:8;
+- __le32 _rsv3:8;
+-
+- __le32 _rsv4:16;
+- __le32 ibf_data_bw:2;
+- __le32 ibf_nc:3;
+- __le32 ibf_nr:3;
+- __le32 ibf_ru:8;
+-
+- __le32 mob_delta_t:8;
+- __le32 mob_lq_result:7;
+- __le32 _rsv5:1;
+- __le32 _rsv6:16;
+-
+- __le32 _rsv7;
+-} __packed;
+-
+-struct mt7915_tm_pfmu_tag {
+- struct mt7915_tm_pfmu_tag1 t1;
+- struct mt7915_tm_pfmu_tag2 t2;
+-};
+-
+-struct mt7915_tm_pfmu_data {
+- __le16 subc_idx;
+- __le16 phi11;
+- __le16 phi21;
+- __le16 phi31;
+-};
+-
+-struct mt7915_tm_ibf_cal_info {
+- u8 format_id;
+- u8 group_l_m_n;
+- u8 group;
+- bool sx2;
+- u8 status;
+- u8 cal_type;
+- u8 _rsv[2];
+- u8 buf[1000];
+-} __packed;
+-
+-enum {
+- IBF_PHASE_CAL_UNSPEC,
+- IBF_PHASE_CAL_NORMAL,
+- IBF_PHASE_CAL_VERIFY,
+- IBF_PHASE_CAL_NORMAL_INSTRUMENT,
+- IBF_PHASE_CAL_VERIFY_INSTRUMENT,
+-};
++#define TXBF_DUT_MAC_SUBADDR 0x22
++#define TXBF_GOLDEN_MAC_SUBADDR 0x11
+
+ #endif
+diff --git a/testmode.c b/testmode.c
+index b19b872..ed5ceee 100644
+--- a/testmode.c
++++ b/testmode.c
+@@ -191,6 +191,7 @@ mt76_testmode_alloc_skb(struct mt76_phy *phy, u32 len,
+
+ hdr = __skb_put_zero(head, sizeof(*hdr));
+ hdr->frame_control = cpu_to_le16(fc);
++
+ memcpy(hdr->addr1, addr[0], ETH_ALEN);
+ memcpy(hdr->addr2, addr[1], ETH_ALEN);
+ memcpy(hdr->addr3, addr[2], ETH_ALEN);
+diff --git a/testmode.h b/testmode.h
+index 27a0095..e1ebbd5 100644
+--- a/testmode.h
++++ b/testmode.h
+@@ -299,7 +299,10 @@ enum mt76_testmode_cfg {
+ };
+
+ enum mt76_testmode_txbf_act {
++ MT76_TM_TXBF_ACT_GOLDEN_INIT,
+ MT76_TM_TXBF_ACT_INIT,
++ MT76_TM_TX_EBF_ACT_GOLDEN_INIT,
++ MT76_TM_TX_EBF_ACT_INIT,
+ MT76_TM_TXBF_ACT_UPDATE_CH,
+ MT76_TM_TXBF_ACT_PHASE_COMP,
+ MT76_TM_TXBF_ACT_TX_PREP,
+@@ -310,6 +313,12 @@ enum mt76_testmode_txbf_act {
+ MT76_TM_TXBF_ACT_PROF_UPDATE_ALL,
+ MT76_TM_TXBF_ACT_PROF_UPDATE_ALL_CMD,
+ MT76_TM_TXBF_ACT_E2P_UPDATE,
++ MT76_TM_TXBF_ACT_TRIGGER_SOUNDING,
++ MT76_TM_TXBF_ACT_STOP_SOUNDING,
++ MT76_TM_TXBF_ACT_PROFILE_TAG_READ,
++ MT76_TM_TXBF_ACT_PROFILE_TAG_WRITE,
++ MT76_TM_TXBF_ACT_PROFILE_TAG_INVALID,
++ MT76_TM_TXBF_ACT_STA_REC_READ,
+
+ /* keep last */
+ NUM_MT76_TM_TXBF_ACT,
+diff --git a/tools/fields.c b/tools/fields.c
+index 6f07eed..7211ec5 100644
+--- a/tools/fields.c
++++ b/tools/fields.c
+@@ -33,7 +33,10 @@ static const char * const testmode_tx_mode[] = {
+ };
+
+ static const char * const testmode_txbf_act[] = {
++ [MT76_TM_TXBF_ACT_GOLDEN_INIT] = "golden_init",
+ [MT76_TM_TXBF_ACT_INIT] = "init",
++ [MT76_TM_TX_EBF_ACT_GOLDEN_INIT] = "ebf_golden_init",
++ [MT76_TM_TX_EBF_ACT_INIT] = "ebf_init",
+ [MT76_TM_TXBF_ACT_UPDATE_CH] = "update_ch",
+ [MT76_TM_TXBF_ACT_PHASE_COMP] = "phase_comp",
+ [MT76_TM_TXBF_ACT_TX_PREP] = "tx_prep",
+@@ -44,6 +47,12 @@ static const char * const testmode_txbf_act[] = {
+ [MT76_TM_TXBF_ACT_PROF_UPDATE_ALL] = "prof_update",
+ [MT76_TM_TXBF_ACT_PROF_UPDATE_ALL_CMD] = "prof_update_all",
+ [MT76_TM_TXBF_ACT_E2P_UPDATE] = "e2p_update",
++ [MT76_TM_TXBF_ACT_TRIGGER_SOUNDING] = "trigger_sounding",
++ [MT76_TM_TXBF_ACT_STOP_SOUNDING] = "stop_sounding",
++ [MT76_TM_TXBF_ACT_PROFILE_TAG_READ] = "pfmu_tag_read",
++ [MT76_TM_TXBF_ACT_PROFILE_TAG_WRITE] = "pfmu_tag_write",
++ [MT76_TM_TXBF_ACT_PROFILE_TAG_INVALID] = "set_invalid_prof",
++ [MT76_TM_TXBF_ACT_STA_REC_READ] = "sta_rec_read",
+ };
+
+ static const char * const testmode_offchan_bw[] = {
+--
+2.36.1
+