[][MAC80211][mt76][Add eBF/iBF certification and calibration with golden]
[Description]
Add eBF certification commands with golden device
Add iBF calibration and verification commands with golden device
Add normal mode bf station record and pfmu tag dump command via debugfs
Fix BW display error in debugfs tmac info dump function
[Release-log]
N/A
Change-Id: I992f52b439cfa467e159dbe54285a902087da5db
Reviewed-on: https://gerrit.mediatek.inc/c/openwrt/feeds/mtk_openwrt_feeds/+/6929902
diff --git a/autobuild_mac80211_release/package/kernel/mt76/patches/1116-mt76-testmode-add-iBF-eBF-cal-and-cert-commands-with.patch b/autobuild_mac80211_release/package/kernel/mt76/patches/1116-mt76-testmode-add-iBF-eBF-cal-and-cert-commands-with.patch
new file mode 100644
index 0000000..db71053
--- /dev/null
+++ b/autobuild_mac80211_release/package/kernel/mt76/patches/1116-mt76-testmode-add-iBF-eBF-cal-and-cert-commands-with.patch
@@ -0,0 +1,1909 @@
+From e2c7b902b076265245b45e60d9554a6ec9432e28 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.18.0
+