recipes-wifi/linux-mt76/files/patches-3.x/1034-mtk-wifi-mt76-testmode-add-testmode-bf-support.patch

From e5de8b93a62c62b5c44b10022e53d410f93c70d6 Mon Sep 17 00:00:00 2001
From: StanleyYP Wang <StanleyYP.Wang@mediatek.com>
Date: Thu, 6 Apr 2023 16:40:28 +0800
Subject: [PATCH 1034/1044] mtk: wifi: mt76: testmode: add testmode bf support

Signed-off-by: StanleyYP Wang <StanleyYP.Wang@mediatek.com>

Add iTest additional bf command

Signed-off-by: StanleyYP Wang <StanleyYP.Wang@mediatek.com>

fw return Gx (5g band) ibf cal struct for both 2G and 5G ibf.
Therefore, memcpy cannot be used for 2G ibf cal.
https://gerrit.mediatek.inc/c/neptune/wlan_driver/logan/+/8206056

Signed-off-by: StanleyYP Wang <StanleyYP.Wang@mediatek.com>
---
 mt76.h               |   5 +
 mt76_connac_mcu.h    |   4 +-
 mt7996/mcu.c         |   8 +-
 mt7996/mcu.h         |  45 ++-
 mt7996/mt7996.h      |  12 +-
 mt7996/mtk_debugfs.c |   6 +-
 mt7996/mtk_mcu.c     |  79 ++++-
 mt7996/mtk_mcu.h     | 338 +++++++++++++++++++-
 mt7996/regs.h        |   3 +
 mt7996/testmode.c    | 744 +++++++++++++++++++++++++++++++++++++++++--
 mt7996/testmode.h    |  19 ++
 testmode.c           |  60 ++++
 testmode.h           |  53 +++
 tools/fields.c       |  37 +++
 14 files changed, 1354 insertions(+), 59 deletions(-)

diff --git a/mt76.h b/mt76.h
index 58fd55b1..543d9de5 100644
--- a/mt76.h
+++ b/mt76.h
@@ -752,6 +752,11 @@ struct mt76_testmode_data {
 	u32 tx_time;
 	u32 tx_ipg;
 
+	u8 txbf_act;
+	u16 txbf_param[8];
+	bool is_txbf_dut;
+	bool bf_en;
+	bool bf_ever_en;
 	bool ibf;
 	bool ebf;
 
diff --git a/mt76_connac_mcu.h b/mt76_connac_mcu.h
index a59e5a0b..266ee711 100644
--- a/mt76_connac_mcu.h
+++ b/mt76_connac_mcu.h
@@ -488,7 +488,8 @@ struct sta_rec_bf {
 	bool codebook75_mu;
 
 	u8 he_ltf;
-	u8 rsv[3];
+	u8 pp_fd_val;
+	u8 rsv[2];
 } __packed;
 
 struct sta_rec_bfee {
@@ -1278,6 +1279,7 @@ enum {
 	MCU_UNI_CMD_VOW = 0x37,
 	MCU_UNI_CMD_PP = 0x38,
 	MCU_UNI_CMD_FIXED_RATE_TABLE = 0x40,
+	MCU_UNI_CMD_TESTMODE_TRX_PARAM = 0x42,
 	MCU_UNI_CMD_TESTMODE_CTRL = 0x46,
 	MCU_UNI_CMD_PRECAL_RESULT = 0x47,
 	MCU_UNI_CMD_RRO = 0x57,
diff --git a/mt7996/mcu.c b/mt7996/mcu.c
index 3a34afd9..6e3047ba 100644
--- a/mt7996/mcu.c
+++ b/mt7996/mcu.c
@@ -1070,7 +1070,12 @@ mt7996_mcu_bss_basic_tlv(struct sk_buff *skb,
 	bss->hw_bss_idx = idx;
 
 	if (vif->type == NL80211_IFTYPE_MONITOR) {
-		memcpy(bss->bssid, phy->macaddr, ETH_ALEN);
+		struct mt76_testmode_data *td = &phy->test;
+
+		if (!td->bf_en)
+			memcpy(bss->bssid, phy->macaddr, ETH_ALEN);
+		else
+			memcpy(bss->bssid, td->addr[2], ETH_ALEN);
 		return 0;
 	}
 
@@ -4108,7 +4113,6 @@ int mt7996_mcu_set_ser(struct mt7996_dev *dev, u8 action, u8 val, u8 band)
 int mt7996_mcu_set_txbf(struct mt7996_dev *dev, u8 action)
 {
 #define MT7996_BF_MAX_SIZE	sizeof(union bf_tag_tlv)
-#define BF_PROCESSING	4
 	struct uni_header hdr;
 	struct sk_buff *skb;
 	struct tlv *tlv;
diff --git a/mt7996/mcu.h b/mt7996/mcu.h
index af078edd..054a616b 100644
--- a/mt7996/mcu.h
+++ b/mt7996/mcu.h
@@ -685,6 +685,22 @@ struct bf_sounding_on {
 	__le32 snd_period;
 } __packed;
 
+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 bf_hw_en_status_update {
 	__le16 tag;
 	__le16 len;
@@ -710,6 +726,24 @@ union bf_tag_tlv {
 	struct bf_mod_en_ctrl bf_mod_en;
 };
 
+enum {
+	BF_SOUNDING_OFF = 0,
+	BF_SOUNDING_ON = 1,
+	BF_DATA_PACKET_APPLY = 2,
+	BF_PFMU_TAG_READ = 5,
+	BF_PFMU_TAG_WRITE = 6,
+	BF_STA_REC_READ = 11,
+	BF_PHASE_CALIBRATION = 12,
+	BF_IBF_PHASE_COMP = 13,
+	BF_PROFILE_WRITE_20M_ALL = 15,
+	BF_HW_EN_UPDATE = 17,
+	BF_MOD_EN_CTRL = 20,
+	BF_FBRPT_DBG_INFO_READ = 23,
+	BF_TXSND_INFO = 24,
+	BF_CMD_TXCMD = 27,
+	BF_CFG_PHY = 28,
+};
+
 struct ra_rate {
 	__le16 wlan_idx;
 	u8 mode;
@@ -771,17 +805,6 @@ enum {
 #define MUMIMO_UL                      BIT(3)
 #define MUMIMO_DL_CERT                 BIT(4)
 
-enum {
-	BF_SOUNDING_ON = 1,
-	BF_PFMU_TAG_READ = 5,
-	BF_STA_REC_READ = 11,
-	BF_HW_EN_UPDATE = 17,
-	BF_MOD_EN_CTRL = 20,
-	BF_FBRPT_DBG_INFO_READ = 23,
-	BF_TXSND_INFO = 24,
-	BF_CFG_PHY = 28,
-};
-
 enum {
 	CMD_BAND_NONE,
 	CMD_BAND_24G,
diff --git a/mt7996/mt7996.h b/mt7996/mt7996.h
index b9c37612..2f76a0af 100644
--- a/mt7996/mt7996.h
+++ b/mt7996/mt7996.h
@@ -480,6 +480,14 @@ struct mt7996_dev {
 #ifdef CONFIG_MTK_VENDOR
 	bool cert_mode;
 #endif
+
+#if defined CONFIG_NL80211_TESTMODE || defined CONFIG_MTK_DEBUG
+	struct {
+		void *txbf_phase_cal;
+		void *txbf_pfmu_data;
+		void *txbf_pfmu_tag;
+	} test;
+#endif
 };
 
 enum {
@@ -819,7 +827,7 @@ int mt7996_mcu_muru_dbg_info(struct mt7996_dev *dev, u16 item, u8 val);
 int mt7996_mcu_set_sr_enable(struct mt7996_phy *phy, u8 action, u64 val, bool set);
 void mt7996_mcu_rx_sr_event(struct mt7996_dev *dev, struct sk_buff *skb);
 int mt7996_mcu_set_dup_wtbl(struct mt7996_dev *dev);
-int mt7996_mcu_set_txbf_internal(struct mt7996_phy *phy, u8 action, int idx);
+int mt7996_mcu_set_txbf_internal(struct mt7996_phy *phy, u8 action, int idx, bool bfer);
 void mt7996_mcu_rx_bf_event(struct mt7996_dev *dev, struct sk_buff *skb);
 int mt7996_mcu_set_muru_fixed_rate_enable(struct mt7996_dev *dev, u8 action, int val);
 int mt7996_mcu_set_muru_fixed_rate_parameter(struct mt7996_dev *dev, u8 action, void *para);
@@ -831,10 +839,12 @@ int mt7996_mcu_set_rfeature_trig_type(struct mt7996_phy *phy, u8 enable, u8 trig
 void mt7996_mcu_set_ppdu_tx_type(struct mt7996_phy *phy, u8 ppdu_type);
 void mt7996_mcu_set_nusers_ofdma(struct mt7996_phy *phy, u8 type, u8 ofdma_user_cnt);
 void mt7996_mcu_set_cert(struct mt7996_phy *phy, u8 type);
+void mt7996_tm_update_channel(struct mt7996_phy *phy);
 #endif
 
 #ifdef CONFIG_NET_MEDIATEK_SOC_WED
 int mt7996_dma_rro_init(struct mt7996_dev *dev);
 #endif /* CONFIG_NET_MEDIATEK_SOC_WED */
 
+
 #endif
diff --git a/mt7996/mtk_debugfs.c b/mt7996/mtk_debugfs.c
index 9e7acd4b..c4cdbcdc 100644
--- a/mt7996/mtk_debugfs.c
+++ b/mt7996/mtk_debugfs.c
@@ -2899,7 +2899,7 @@ mt7996_starec_bf_read_set(void *data, u64 wlan_idx)
 {
 	struct mt7996_phy *phy = data;
 
-	return mt7996_mcu_set_txbf_internal(phy, BF_STA_REC_READ, wlan_idx);
+	return mt7996_mcu_set_txbf_internal(phy, BF_STA_REC_READ, wlan_idx, 0);
 }
 DEFINE_DEBUGFS_ATTRIBUTE(fops_starec_bf_read, NULL,
 			 mt7996_starec_bf_read_set, "%lld\n");
@@ -2943,7 +2943,7 @@ mt7996_bf_fbk_rpt_set(void *data, u64 wlan_idx)
 {
 	struct mt7996_phy *phy = data;
 
-	return mt7996_mcu_set_txbf_internal(phy, BF_FBRPT_DBG_INFO_READ, wlan_idx);
+	return mt7996_mcu_set_txbf_internal(phy, BF_FBRPT_DBG_INFO_READ, wlan_idx, 0);
 }
 DEFINE_DEBUGFS_ATTRIBUTE(fops_bf_fbk_rpt, NULL,
 			 mt7996_bf_fbk_rpt_set, "%lld\n");
@@ -2953,7 +2953,7 @@ mt7996_bf_pfmu_tag_read_set(void *data, u64 wlan_idx)
 {
 	struct mt7996_phy *phy = data;
 
-	return mt7996_mcu_set_txbf_internal(phy, BF_PFMU_TAG_READ, wlan_idx);
+	return mt7996_mcu_set_txbf_internal(phy, BF_PFMU_TAG_READ, wlan_idx, 1);
 }
 DEFINE_DEBUGFS_ATTRIBUTE(fops_bf_pfmu_tag_read, NULL,
 			 mt7996_bf_pfmu_tag_read_set, "%lld\n");
diff --git a/mt7996/mtk_mcu.c b/mt7996/mtk_mcu.c
index cacca449..f70bd0be 100644
--- a/mt7996/mtk_mcu.c
+++ b/mt7996/mtk_mcu.c
@@ -295,7 +295,7 @@ __mt7996_mcu_add_uni_tlv(struct sk_buff *skb, u16 tag, u16 len)
 	return ptlv;
 }
 
-int mt7996_mcu_set_txbf_internal(struct mt7996_phy *phy, u8 action, int idx)
+int mt7996_mcu_set_txbf_internal(struct mt7996_phy *phy, u8 action, int idx, bool bfer)
 {
 	struct mt7996_dev *dev = phy->dev;
 #define MT7996_MTK_BF_MAX_SIZE	sizeof(struct bf_starec_read)
@@ -318,9 +318,8 @@ int mt7996_mcu_set_txbf_internal(struct mt7996_phy *phy, u8 action, int idx)
 
 		tlv = __mt7996_mcu_add_uni_tlv(skb, action, sizeof(*req));
 		req = (struct bf_pfmu_tag *)tlv;
-#define BFER 1
 		req->pfmu_id = idx;
-		req->bfer = BFER;
+		req->bfer = bfer;
 		req->band_idx = phy->mt76->band_idx;
 		break;
 	}
@@ -432,10 +431,36 @@ int mt7996_mcu_set_txbf_snd_info(struct mt7996_phy *phy, void *para)
 	return mt76_mcu_skb_send_msg(&phy->dev->mt76, skb, MCU_WM_UNI_CMD(BF), false);
 }
 
+static inline void
+mt7996_ibf_phase_assign(struct mt7996_dev *dev,
+			struct mt7996_ibf_cal_info *cal,
+			struct mt7996_txbf_phase *phase)
+{
+	/* fw return ibf calibrated data with
+	 * the mt7996_txbf_phase_info_5g struct for both 2G and 5G.
+	 * Therefore, memcpy cannot be used here.
+	 */
+	phase_assign(cal->group, m_t0_h, true);
+	phase_assign(cal->group, m_t1_h, true);
+	phase_assign(cal->group, m_t2_h, true);
+	phase_assign(cal->group, m_t2_h_sx2, false);
+	phase_assign_rx(cal->group, r0);
+	phase_assign_rx(cal->group, r1);
+	phase_assign_rx(cal->group, r2);
+	phase_assign_rx(cal->group, r3);
+	phase_assign_rx_g0(cal->group, r2_sx2);
+	phase_assign_rx_g0(cal->group, r3_sx2);
+	phase_assign(cal->group, r0_reserved, false);
+	phase_assign(cal->group, r1_reserved, false);
+	phase_assign(cal->group, r2_reserved, false);
+	phase_assign(cal->group, r3_reserved, false);
+	phase_assign(cal->group, r2_sx2_reserved, false);
+	phase_assign(cal->group, r3_sx2_reserved, false);
+}
+
 void
 mt7996_mcu_rx_bf_event(struct mt7996_dev *dev, struct sk_buff *skb)
 {
-#define HE_MODE 3
 	struct mt7996_mcu_bf_basic_event *event;
 
 	event = (struct mt7996_mcu_bf_basic_event *)skb->data;
@@ -470,13 +495,12 @@ mt7996_mcu_rx_bf_event(struct mt7996_dev *dev, struct sk_buff *skb)
 			 tag->t1.nr, tag->t1.nc, tag->t1.ngroup, tag->t1.lm, tag->t1.codebook,
 			 tag->t1.mob_cal_en);
 
-		if (tag->t1.lm <= HE_MODE) {
+		if (tag->t1.lm <= BF_LM_HE)
 			dev_info(dev->mt76.dev, "RU start = %d, RU end = %d\n",
 				 tag->t1.field.ru_start_id, tag->t1.field.ru_end_id);
-		} else {
+		else
 			dev_info(dev->mt76.dev, "PartialBW = %d\n",
 				 tag->t1.bw_info.partial_bw_info);
-		}
 
 		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);
@@ -730,6 +754,47 @@ mt7996_mcu_rx_bf_event(struct mt7996_dev *dev, struct sk_buff *skb)
 
 		break;
 	}
+	case UNI_EVENT_BF_CAL_PHASE: {
+		struct mt7996_ibf_cal_info *cal;
+		struct mt7996_txbf_phase_out phase_out;
+		struct mt7996_txbf_phase *phase;
+
+		cal = (struct mt7996_ibf_cal_info *)skb->data;
+		phase = (struct mt7996_txbf_phase *)dev->test.txbf_phase_cal;
+		memcpy(&phase_out, &cal->phase_out, sizeof(phase_out));
+		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];
+			phase->status = cal->status;
+			dev_info(dev->mt76.dev, "Calibrated result = %d\n", phase->status);
+			dev_info(dev->mt76.dev, "Group %d and Group M\n", cal->group);
+			mt7996_ibf_phase_assign(dev, cal, phase);
+			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_uh = %d, c1_uh = %d, c2_uh = %d, c3_uh = %d\n",
+			 phase_out.c0_uh, phase_out.c1_uh, phase_out.c2_uh, phase_out.c3_uh);
+		dev_info(dev->mt76.dev, "c0_h = %d, c1_h = %d, c2_h = %d, c3_h = %d\n",
+			 phase_out.c0_h, phase_out.c1_h, phase_out.c2_h, phase_out.c3_h);
+		dev_info(dev->mt76.dev, "c0_mh = %d, c1_mh = %d, c2_mh = %d, c3_mh = %d\n",
+			 phase_out.c0_mh, phase_out.c1_mh, phase_out.c2_mh, phase_out.c3_mh);
+		dev_info(dev->mt76.dev, "c0_m = %d, c1_m = %d, c2_m = %d, c3_m = %d\n",
+			 phase_out.c0_m, phase_out.c1_m, phase_out.c2_m, phase_out.c3_m);
+		dev_info(dev->mt76.dev, "c0_l = %d, c1_l = %d, c2_l = %d, c3_l = %d\n",
+			 phase_out.c0_l, phase_out.c1_l, phase_out.c2_l, phase_out.c3_l);
+
+		break;
+	}
 	default:
 		dev_info(dev->mt76.dev, "%s: unknown bf event tag %d\n",
 			 __func__, event->tag);
diff --git a/mt7996/mtk_mcu.h b/mt7996/mtk_mcu.h
index 98c9660a..252ae98e 100644
--- a/mt7996/mtk_mcu.h
+++ b/mt7996/mtk_mcu.h
@@ -189,6 +189,164 @@ struct bf_txsnd_info {
 	u8 __rsv[2];
 } __packed;
 
+#define MAX_PHASE_GROUP_NUM	9
+
+struct bf_phase_comp {
+	__le16 tag;
+	__le16 len;
+
+	u8 bw;
+	u8 jp_band;
+	u8 band_idx;
+	bool read_from_e2p;
+	bool disable;
+	u8 group;
+	u8 rsv[2];
+	u8 buf[44];
+} __packed;
+
+struct bf_tx_apply {
+	__le16 tag;
+	__le16 len;
+
+	__le16 wlan_idx;
+	bool ebf;
+	bool ibf;
+	bool mu_txbf;
+	bool phase_cal;
+	u8 rsv[2];
+} __packed;
+
+struct bf_phase_cal {
+	__le16 tag;
+	__le16 len;
+
+	u8 group_l_m_n;
+	u8 group;
+	u8 sx2;
+	u8 cal_type;
+	u8 lna_gain_level;
+	u8 band_idx;
+	u8 rsv[2];
+} __packed;
+
+struct bf_txcmd {
+	__le16 tag;
+	__le16 len;
+
+	u8 action;
+	u8 bf_manual;
+	u8 bf_bit;
+	u8 rsv[5];
+} __packed;
+
+struct bf_pfmu_data_all {
+	__le16 tag;
+	__le16 len;
+
+	u8 pfmu_id;
+	u8 band_idx;
+	u8 rsv[2];
+
+	u8 buf[512];
+} __packed;
+
+#define TXBF_DUT_MAC_SUBADDR		0x22
+#define TXBF_GOLDEN_MAC_SUBADDR		0x11
+
+struct mt7996_tm_bf_req {
+	u8 _rsv[4];
+
+	union {
+		struct bf_sounding_on sounding;
+		struct bf_tx_apply tx_apply;
+		struct bf_pfmu_tag pfmu_tag;
+		struct bf_pfmu_data_all pfmu_data_all;
+		struct bf_phase_cal phase_cal;
+		struct bf_phase_comp phase_comp;
+		struct bf_txcmd txcmd;
+	};
+} __packed;
+
+enum tm_trx_mac_type {
+	TM_TRX_MAC_TX = 1,
+	TM_TRX_MAC_RX,
+	TM_TRX_MAC_TXRX,
+	TM_TRX_MAC_TXRX_RXV,
+	TM_TRX_MAC_RXV,
+	TM_TRX_MAC_RX_RXV,
+};
+
+enum tm_trx_param_idx {
+	TM_TRX_PARAM_RSV,
+	/* MAC */
+	TM_TRX_PARAM_SET_TRX,
+	TM_TRX_PARAM_RX_FILTER,
+	TM_TRX_PARAM_RX_FILTER_PKT_LEN,
+	TM_TRX_PARAM_SLOT_TIME,
+	TM_TRX_PARAM_CLEAN_PERSTA_TXQUEUE,
+	TM_TRX_PARAM_AMPDU_WTBL,
+	TM_TRX_PARAM_MU_RX_AID,
+	TM_TRX_PARAM_PHY_MANUAL_TX,
+
+	/* PHY */
+	TM_TRX_PARAM_RX_PATH,
+	TM_TRX_PARAM_TX_STREAM,
+	TM_TRX_PARAM_TSSI_STATUS,
+	TM_TRX_PARAM_DPD_STATUS,
+	TM_TRX_PARAM_RATE_POWER_OFFSET_ON_OFF,
+	TM_TRX_PARAM_THERMO_COMP_STATUS,
+	TM_TRX_PARAM_FREQ_OFFSET,
+	TM_TRX_PARAM_FAGC_RSSI_PATH,
+	TM_TRX_PARAM_PHY_STATUS_COUNT,
+	TM_TRX_PARAM_RXV_INDEX,
+
+	TM_TRX_PARAM_ANTENNA_PORT,
+	TM_TRX_PARAM_THERMAL_ONOFF,
+	TM_TRX_PARAM_TX_POWER_CONTROL_ALL_RF,
+	TM_TRX_PARAM_RATE_POWER_OFFSET,
+	TM_TRX_PARAM_SLT_CMD_TEST,
+	TM_TRX_PARAM_SKU,
+	TM_TRX_PARAM_POWER_PERCENTAGE_ON_OFF,
+	TM_TRX_PARAM_BF_BACKOFF_ON_OFF,
+	TM_TRX_PARAM_POWER_PERCENTAGE_LEVEL,
+	TM_TRX_PARAM_FRTBL_CFG,
+	TM_TRX_PARAM_PREAMBLE_PUNC_ON_OFF,
+
+	TM_TRX_PARAM_MAX_NUM,
+};
+
+enum trx_action {
+	TM_TRX_ACTION_SET,
+	TM_TRX_ACTION_GET,
+};
+
+struct tm_trx_set {
+	u8 type;
+	u8 enable;
+	u8 band_idx;
+	u8 rsv;
+} __packed;
+
+struct mt7996_tm_trx_req {
+	u8 param_num;
+	u8 _rsv[3];
+
+	__le16 tag;
+	__le16 len;
+
+	__le16 param_idx;
+	u8 band_idx;
+	u8 testmode_en;
+	u8 action;
+	u8 rsv[3];
+
+	u32 data;
+	struct tm_trx_set set_trx;
+
+	u8 buf[220];
+} __packed;
+
 struct mt7996_mcu_bf_basic_event {
 	struct mt7996_mcu_rxd rxd;
 
@@ -394,6 +552,181 @@ struct mt7996_pfmu_tag_event {
 	struct mt7996_pfmu_tag2 t2;
 };
 
+struct mt7996_pfmu_tag {
+	struct mt7996_pfmu_tag1 t1;
+	struct mt7996_pfmu_tag2 t2;
+};
+
+enum bf_lm_type {
+	BF_LM_LEGACY,
+	BF_LM_HT,
+	BF_LM_VHT,
+	BF_LM_HE,
+	BF_LM_EHT,
+};
+
+struct mt7996_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_mh;
+	u8 c1_mh;
+	u8 c2_mh;
+	u8 c3_mh;
+	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 mt7996_txbf_rx_phase_2g {
+	u8 rx_uh;
+	u8 rx_h;
+	u8 rx_m;
+	u8 rx_l;
+	u8 rx_ul;
+};
+
+struct mt7996_txbf_rx_phase_5g {
+	u8 rx_uh;
+	u8 rx_h;
+	u8 rx_mh;
+	u8 rx_m;
+	u8 rx_l;
+	u8 rx_ul;
+};
+
+struct mt7996_txbf_phase_info_2g {
+	struct mt7996_txbf_rx_phase_2g r0;
+	struct mt7996_txbf_rx_phase_2g r1;
+	struct mt7996_txbf_rx_phase_2g r2;
+	struct mt7996_txbf_rx_phase_2g r3;
+	struct mt7996_txbf_rx_phase_2g r2_sx2;
+	struct mt7996_txbf_rx_phase_2g r3_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;
+};
+
+struct mt7996_txbf_phase_info_5g {
+	struct mt7996_txbf_rx_phase_5g r0;
+	struct mt7996_txbf_rx_phase_5g r1;
+	struct mt7996_txbf_rx_phase_5g r2;
+	struct mt7996_txbf_rx_phase_5g r3;
+	struct mt7996_txbf_rx_phase_2g r2_sx2;	/* no middle-high in r2_sx2 */
+	struct mt7996_txbf_rx_phase_2g r3_sx2;	/* no middle-high in r3_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;
+};
+
+struct mt7996_txbf_phase {
+	u8 status;
+	union {
+		struct mt7996_txbf_phase_info_2g phase_2g;
+		struct mt7996_txbf_phase_info_5g phase_5g;
+	};
+};
+
+#define phase_assign(group, field, dump, ...)	({						\
+	if (group) {										\
+		phase->phase_5g.field = cal->phase_5g.field;					\
+		if (dump)									\
+			dev_info(dev->mt76.dev, "%s = %d\n", #field, phase->phase_5g.field);	\
+	} else {										\
+		phase->phase_2g.field = cal->phase_5g.field;					\
+		if (dump)									\
+			dev_info(dev->mt76.dev, "%s = %d\n", #field, phase->phase_2g.field);	\
+	}											\
+})
+
+#define phase_assign_rx_g0(group, rx, ...)	({						\
+	phase_assign(group, rx.rx_uh, false);							\
+	phase_assign(group, rx.rx_h, false);							\
+	phase_assign(group, rx.rx_m, false);							\
+	phase_assign(group, rx.rx_l, false);							\
+	phase_assign(group, rx.rx_ul, false);							\
+})
+
+#define phase_assign_rx(group, rx, ...)	({							\
+	if (group) {										\
+		phase_assign(group, rx.rx_uh, true);						\
+		phase_assign(group, rx.rx_h, true);						\
+		phase->phase_5g.rx.rx_mh = cal->phase_5g.rx.rx_mh;				\
+		dev_info(dev->mt76.dev, "%s.rx_mh = %d\n", #rx, phase->phase_5g.rx.rx_mh);	\
+		phase_assign(group, rx.rx_m, true);						\
+		phase_assign(group, rx.rx_l, true);						\
+		phase_assign(group, rx.rx_ul, true);						\
+	} else {										\
+		phase_assign(group, rx.rx_uh, true);						\
+		phase_assign(group, rx.rx_h, true);						\
+		phase_assign(group, rx.rx_m, true);						\
+		phase_assign(group, rx.rx_l, true);						\
+		phase_assign(group, rx.rx_ul, true);						\
+	}											\
+})
+
+#define GROUP_L		0
+#define GROUP_M		1
+#define GROUP_H		2
+
+struct mt7996_pfmu_data {
+	__le16 subc_idx;
+	__le16 phi11;
+	__le16 phi21;
+	__le16 phi31;
+};
+
+struct mt7996_ibf_cal_info {
+	struct mt7996_mcu_bf_basic_event event;
+
+	u8 category_id;
+	u8 group_l_m_n;
+	u8 group;
+	bool sx2;
+	u8 status;
+	u8 cal_type;
+	u8 _rsv[2];
+	struct mt7996_txbf_phase_out phase_out;
+	union {
+		struct mt7996_txbf_phase_info_2g phase_2g;
+		struct mt7996_txbf_phase_info_5g phase_5g;
+	};
+} __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 MT7996_TXBF_SUBCAR_NUM	64
+
 enum {
 	UNI_EVENT_BF_PFMU_TAG = 0x5,
 	UNI_EVENT_BF_PFMU_DATA = 0x7,
@@ -408,11 +741,6 @@ enum {
 	UNI_EVENT_BF_MAX_NUM
 };
 
-enum {
-	UNI_CMD_MURU_FIXED_RATE_CTRL = 0x11,
-	UNI_CMD_MURU_FIXED_GROUP_RATE_CTRL,
-};
-
 struct uni_muru_mum_set_group_tbl_entry {
 	__le16 wlan_idx0;
 	__le16 wlan_idx1;
diff --git a/mt7996/regs.h b/mt7996/regs.h
index e94f9a90..aa04d8d2 100644
--- a/mt7996/regs.h
+++ b/mt7996/regs.h
@@ -326,6 +326,9 @@ enum offs_rev {
 #define MT_ARB_SCR_TX_DISABLE			BIT(8)
 #define MT_ARB_SCR_RX_DISABLE			BIT(9)
 
+#define MT_ARB_TQSAXM0(_band)			MT_WF_ARB(_band, 0x180)
+#define MT_ARB_TQSAXM_ALTX_START_MASK		GENMASK(12, 8)
+
 /* RMAC: band 0(0x820e5000), band 1(0x820f5000), band 2(0x830e5000), */
 #define MT_WF_RMAC_BASE(_band)			__BASE(WF_RMAC_BASE, (_band))
 #define MT_WF_RMAC(_band, ofs)			(MT_WF_RMAC_BASE(_band) + (ofs))
diff --git a/mt7996/testmode.c b/mt7996/testmode.c
index 26ae5827..2fb36a97 100644
--- a/mt7996/testmode.c
+++ b/mt7996/testmode.c
@@ -23,6 +23,7 @@ enum {
 	TM_CHANGED_IPI_THRESHOLD,
 	TM_CHANGED_IPI_PERIOD,
 	TM_CHANGED_IPI_RESET,
+	TM_CHANGED_TXBF_ACT,
 
 	/* must be last */
 	NUM_TM_CHANGED
@@ -41,25 +42,31 @@ static const u8 tm_change_map[] = {
 	[TM_CHANGED_IPI_THRESHOLD] = MT76_TM_ATTR_IPI_THRESHOLD,
 	[TM_CHANGED_IPI_PERIOD] = MT76_TM_ATTR_IPI_PERIOD,
 	[TM_CHANGED_IPI_RESET] = MT76_TM_ATTR_IPI_RESET,
+	[TM_CHANGED_TXBF_ACT] = MT76_TM_ATTR_TXBF_ACT,
 };
 
 static void mt7996_tm_ipi_work(struct work_struct *work);
+static int mt7996_tm_txbf_apply_tx(struct mt7996_phy *phy, u16 wlan_idx,
+				   bool ebf, bool ibf, bool phase_cal);
 
 static u32 mt7996_tm_bw_mapping(enum nl80211_chan_width width, enum bw_mapping_method method)
 {
 	static const u32 width_to_bw[][NUM_BW_MAP] = {
-		[NL80211_CHAN_WIDTH_40] = {FW_CDBW_40MHZ, TM_CBW_40MHZ, 40,
+		[NL80211_CHAN_WIDTH_40] = {FW_CDBW_40MHZ, TM_CBW_40MHZ, BF_CDBW_40MHZ, 40,
 					   FIRST_CONTROL_CHAN_BITMAP_BW40},
-		[NL80211_CHAN_WIDTH_80] = {FW_CDBW_80MHZ, TM_CBW_80MHZ, 80,
+		[NL80211_CHAN_WIDTH_80] = {FW_CDBW_80MHZ, TM_CBW_80MHZ, BF_CDBW_80MHZ, 80,
 					   FIRST_CONTROL_CHAN_BITMAP_BW80},
-		[NL80211_CHAN_WIDTH_80P80] = {FW_CDBW_8080MHZ, TM_CBW_8080MHZ, 80, 0x0},
-		[NL80211_CHAN_WIDTH_160] = {FW_CDBW_160MHZ, TM_CBW_160MHZ, 160,
+		[NL80211_CHAN_WIDTH_80P80] = {FW_CDBW_8080MHZ, TM_CBW_8080MHZ, BF_CDBW_8080MHZ,
+					      80, 0x0},
+		[NL80211_CHAN_WIDTH_160] = {FW_CDBW_160MHZ, TM_CBW_160MHZ, BF_CDBW_160MHZ, 160,
 					    FIRST_CONTROL_CHAN_BITMAP_BW160},
-		[NL80211_CHAN_WIDTH_5] = {FW_CDBW_5MHZ, TM_CBW_5MHZ, 5, 0x0},
-		[NL80211_CHAN_WIDTH_10] = {FW_CDBW_10MHZ, TM_CBW_10MHZ, 10, 0x0},
-		[NL80211_CHAN_WIDTH_20] = {FW_CDBW_20MHZ, TM_CBW_20MHZ, 20, 0x0},
-		[NL80211_CHAN_WIDTH_20_NOHT] = {FW_CDBW_20MHZ, TM_CBW_20MHZ, 20, 0x0},
-		[NL80211_CHAN_WIDTH_320] = {FW_CDBW_320MHZ, TM_CBW_320MHZ, 320, 0x0},
+		[NL80211_CHAN_WIDTH_5] = {FW_CDBW_5MHZ, TM_CBW_5MHZ, BF_CDBW_5MHZ, 5, 0x0},
+		[NL80211_CHAN_WIDTH_10] = {FW_CDBW_10MHZ, TM_CBW_10MHZ, BF_CDBW_10MHZ, 10, 0x0},
+		[NL80211_CHAN_WIDTH_20] = {FW_CDBW_20MHZ, TM_CBW_20MHZ, BF_CDBW_20MHZ, 20, 0x0},
+		[NL80211_CHAN_WIDTH_20_NOHT] = {FW_CDBW_20MHZ, TM_CBW_20MHZ, BF_CDBW_20MHZ,
+						20, 0x0},
+		[NL80211_CHAN_WIDTH_320] = {FW_CDBW_320MHZ, TM_CBW_320MHZ, BF_CDBW_320MHZ,
+					    320, 0x0},
 	};
 
 	if (width >= ARRAY_SIZE(width_to_bw))
@@ -68,26 +75,26 @@ static u32 mt7996_tm_bw_mapping(enum nl80211_chan_width width, enum bw_mapping_m
 	return width_to_bw[width][method];
 }
 
-static u8 mt7996_tm_rate_to_phy(u8 tx_rate_mode)
+static u8 mt7996_tm_rate_mapping(u8 tx_rate_mode, enum rate_mapping_type type)
 {
-	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,
-		[MT76_TM_TX_MODE_EHT_SU] = MT_PHY_TYPE_EHT_SU,
-		[MT76_TM_TX_MODE_EHT_TRIG] = MT_PHY_TYPE_EHT_TRIG,
-		[MT76_TM_TX_MODE_EHT_MU] = MT_PHY_TYPE_EHT_MU,
+	static const u8 rate_to_phy[][NUM_RATE_MAP] = {
+		[MT76_TM_TX_MODE_CCK] = {MT_PHY_TYPE_CCK, BF_LM_LEGACY},
+		[MT76_TM_TX_MODE_OFDM] = {MT_PHY_TYPE_OFDM, BF_LM_LEGACY},
+		[MT76_TM_TX_MODE_HT] = {MT_PHY_TYPE_HT, BF_LM_HT},
+		[MT76_TM_TX_MODE_VHT] = {MT_PHY_TYPE_VHT, BF_LM_VHT},
+		[MT76_TM_TX_MODE_HE_SU] = {MT_PHY_TYPE_HE_SU, BF_LM_HE},
+		[MT76_TM_TX_MODE_HE_EXT_SU] = {MT_PHY_TYPE_HE_EXT_SU, BF_LM_HE},
+		[MT76_TM_TX_MODE_HE_TB] = {MT_PHY_TYPE_HE_TB, BF_LM_HE},
+		[MT76_TM_TX_MODE_HE_MU] = {MT_PHY_TYPE_HE_MU, BF_LM_HE},
+		[MT76_TM_TX_MODE_EHT_SU] = {MT_PHY_TYPE_EHT_SU, BF_LM_EHT},
+		[MT76_TM_TX_MODE_EHT_TRIG] = {MT_PHY_TYPE_EHT_TRIG, BF_LM_EHT},
+		[MT76_TM_TX_MODE_EHT_MU] = {MT_PHY_TYPE_EHT_MU, BF_LM_EHT},
 	};
 
 	if (tx_rate_mode > MT76_TM_TX_MODE_MAX)
 		return -EINVAL;
 
-	return rate_to_phy[tx_rate_mode];
+	return rate_to_phy[tx_rate_mode][type];
 }
 
 static int
@@ -239,7 +246,7 @@ mt7996_tm_init(struct mt7996_phy *phy, bool en)
 		INIT_DELAYED_WORK(&phy->ipi_work, mt7996_tm_ipi_work);
 }
 
-static void
+void
 mt7996_tm_update_channel(struct mt7996_phy *phy)
 {
 #define CHAN_FREQ_BW_80P80_TAG		(SET_ID(CHAN_FREQ) | BIT(16))
@@ -303,7 +310,8 @@ mt7996_tm_set_tx_frames(struct mt7996_phy *phy, bool en)
 		mt7996_tm_set(dev, SET_ID(MAC_HEADER), FRAME_CONTROL);
 		mt7996_tm_set(dev, SET_ID(SEQ_CTRL), 0);
 		mt7996_tm_set(dev, SET_ID(TX_COUNT), td->tx_count);
-		mt7996_tm_set(dev, SET_ID(TX_MODE), mt7996_tm_rate_to_phy(td->tx_rate_mode));
+		mt7996_tm_set(dev, SET_ID(TX_MODE),
+			      mt7996_tm_rate_mapping(td->tx_rate_mode, RATE_MODE_TO_PHY));
 		mt7996_tm_set(dev, SET_ID(TX_RATE), td->tx_rate_idx);
 
 		if (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_POWER))
@@ -331,7 +339,8 @@ mt7996_tm_set_tx_frames(struct mt7996_phy *phy, bool en)
 
 		mt7996_tm_set(dev, SET_ID(MAX_PE), 2);
 		mt7996_tm_set(dev, SET_ID(HW_TX_MODE), 0);
-		mt7996_tm_update_channel(phy);
+		if (!td->bf_en)
+			mt7996_tm_update_channel(phy);
 
 		/* trigger firmware to start TX */
 		mt7996_tm_set(dev, SET_ID(COMMAND), RF_CMD(START_TX));
@@ -373,7 +382,8 @@ mt7996_tm_set_rx_frames(struct mt7996_phy *phy, bool en)
 			return;
 		}
 
-		mt7996_tm_update_channel(phy);
+		if (!td->bf_en)
+			mt7996_tm_update_channel(phy);
 
 		if (td->tx_rate_mode >= MT76_TM_TX_MODE_HE_MU) {
 			if (td->aid)
@@ -381,7 +391,8 @@ mt7996_tm_set_rx_frames(struct mt7996_phy *phy, bool en)
 			else
 				ret = mt7996_tm_set(dev, SET_ID(RX_MU_AID), RX_MU_DISABLE);
 		}
-		mt7996_tm_set(dev, SET_ID(TX_MODE), mt7996_tm_rate_to_phy(td->tx_rate_mode));
+		mt7996_tm_set(dev, SET_ID(TX_MODE),
+			      mt7996_tm_rate_mapping(td->tx_rate_mode, RATE_MODE_TO_PHY));
 		mt7996_tm_set(dev, SET_ID(GI), td->tx_rate_sgi);
 		mt7996_tm_set_antenna(phy, SET_ID(RX_PATH));
 		mt7996_tm_set(dev, SET_ID(MAX_PE), 2);
@@ -405,7 +416,8 @@ mt7996_tm_set_tx_cont(struct mt7996_phy *phy, bool en)
 
 	if (en) {
 		mt7996_tm_update_channel(phy);
-		mt7996_tm_set(dev, SET_ID(TX_MODE), mt7996_tm_rate_to_phy(td->tx_rate_mode));
+		mt7996_tm_set(dev, SET_ID(TX_MODE),
+			      mt7996_tm_rate_mapping(td->tx_rate_mode, RATE_MODE_TO_PHY));
 		mt7996_tm_set(dev, SET_ID(TX_RATE), td->tx_rate_idx);
 		/* fix payload is OFDM */
 		mt7996_tm_set(dev, SET_ID(CONT_WAVE_MODE), CONT_WAVE_MODE_OFDM);
@@ -1047,6 +1059,678 @@ mt7996_tm_set_ipi(struct mt7996_phy *phy)
 	return 0;
 }
 
+static int
+mt7996_tm_set_trx_mac(struct mt7996_phy *phy, u8 type, bool en)
+{
+#define UNI_TM_TRX_CTRL 0
+	struct mt7996_dev *dev = phy->dev;
+	struct mt7996_tm_trx_req req = {
+		.param_num = 1,
+		.tag = cpu_to_le16(UNI_TM_TRX_CTRL),
+		.len = cpu_to_le16(sizeof(req) - 4),
+		.param_idx = cpu_to_le16(TM_TRX_PARAM_SET_TRX),
+		.band_idx = phy->mt76->band_idx,
+		.testmode_en = 1,
+		.action = TM_TRX_ACTION_SET,
+		.set_trx = {
+			.type = type,
+			.enable = en,
+			.band_idx = phy->mt76->band_idx,
+		}
+	};
+
+	return mt76_mcu_send_msg(&dev->mt76, MCU_WM_UNI_CMD(TESTMODE_TRX_PARAM),
+				 &req, sizeof(req), false);
+}
+
+static int
+mt7996_tm_txbf_init(struct mt7996_phy *phy, u16 *val)
+{
+#define EBF_BBP_RX_OFFSET	0x10280
+#define EBF_BBP_RX_ENABLE	(BIT(0) | BIT(15))
+	struct mt7996_dev *dev = phy->dev;
+	struct mt76_testmode_data *td = &phy->mt76->test;
+	bool enable = val[0];
+	void *phase_cal, *pfmu_data, *pfmu_tag;
+	u8 nss, band_idx = phy->mt76->band_idx;
+	enum nl80211_chan_width width = NL80211_CHAN_WIDTH_20;
+	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};
+	struct mt7996_vif *mvif = (struct mt7996_vif *)phy->monitor_vif->drv_priv;
+
+	if (!enable) {
+		td->bf_en = false;
+		return 0;
+	}
+
+	if (!dev->test.txbf_phase_cal) {
+		phase_cal = devm_kzalloc(dev->mt76.dev,
+					 sizeof(struct mt7996_txbf_phase) *
+					 MAX_PHASE_GROUP_NUM,
+					 GFP_KERNEL);
+		if (!phase_cal)
+			return -ENOMEM;
+
+		dev->test.txbf_phase_cal = phase_cal;
+	}
+
+	if (!dev->test.txbf_pfmu_data) {
+		pfmu_data = devm_kzalloc(dev->mt76.dev,
+					 sizeof(struct mt7996_pfmu_data) *
+					 MT7996_TXBF_SUBCAR_NUM,
+					 GFP_KERNEL);
+		if (!pfmu_data)
+			return -ENOMEM;
+
+		dev->test.txbf_pfmu_data = pfmu_data;
+	}
+
+	if (!dev->test.txbf_pfmu_tag) {
+		pfmu_tag = devm_kzalloc(dev->mt76.dev,
+					sizeof(struct mt7996_pfmu_tag), GFP_KERNEL);
+		if (!pfmu_tag)
+			return -ENOMEM;
+
+		dev->test.txbf_pfmu_tag = pfmu_tag;
+	}
+
+	td->bf_en = true;
+	dev->ibf = td->ibf;
+	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);
+	mt7996_tm_set_mac_addr(dev, td->addr[0], SET_ID(DA));
+	mt7996_tm_set_mac_addr(dev, td->addr[1], SET_ID(SA));
+	mt7996_tm_set_mac_addr(dev, td->addr[2], SET_ID(BSSID));
+
+	/* bss idx & omac idx should be set to band idx for ibf cal */
+	mvif->mt76.idx = band_idx;
+	dev->mt76.vif_mask |= BIT_ULL(mvif->mt76.idx);
+	mvif->mt76.omac_idx = band_idx;
+	phy->omac_mask |= BIT_ULL(mvif->mt76.omac_idx);
+
+	mt7996_mcu_add_dev_info(phy, phy->monitor_vif, true);
+	mt7996_mcu_add_bss_info(phy, phy->monitor_vif, true);
+
+	if (td->ibf) {
+		if (td->is_txbf_dut) {
+			/* Enable ITxBF Capability */
+			mt7996_mcu_set_txbf(dev, BF_HW_EN_UPDATE);
+			mt7996_tm_set_trx_mac(phy, TM_TRX_MAC_TX, true);
+
+			td->tx_ipg = 999;
+			td->tx_mpdu_len = 1024;
+			td->tx_antenna_mask = phy->mt76->chainmask >> dev->chainshift[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;
+			td->tx_mpdu_len = 1024;
+			td->tx_rate_idx = 0;
+			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_rate_sgi = 0;
+	} else {
+		if (td->is_txbf_dut) {
+			/* Enable ETxBF Capability */
+			mt7996_mcu_set_txbf(dev, BF_HW_EN_UPDATE);
+			td->tx_antenna_mask = phy->mt76->chainmask >> dev->chainshift[band_idx];
+			td->tx_spe_idx = 24 + phy->mt76->band_idx;
+			if (td->tx_rate_mode == MT76_TM_TX_MODE_VHT ||
+			    td->tx_rate_mode == MT76_TM_TX_MODE_HE_SU)
+				mt7996_tm_set(dev, SET_ID(NSS), td->tx_rate_nss);
+
+			mt7996_tm_set(dev, SET_ID(ENCODE_MODE), td->tx_rate_ldpc);
+			mt7996_tm_set(dev, SET_ID(TX_COUNT), td->tx_count);
+		} else {
+			/* 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));
+
+			td->tx_antenna_mask = 1;
+		}
+		width = phy->mt76->chandef.width;
+
+		if (td->tx_rate_mode == MT76_TM_TX_MODE_EHT_MU)
+			td->tx_rate_mode = MT76_TM_TX_MODE_EHT_SU;
+	}
+	mt76_testmode_param_set(td, MT76_TM_ATTR_TX_ANTENNA);
+
+	mt7996_tm_set(dev, SET_ID(TX_MODE),
+		      mt7996_tm_rate_mapping(td->tx_rate_mode, RATE_MODE_TO_PHY));
+	mt7996_tm_set(dev, SET_ID(TX_RATE), td->tx_rate_idx);
+	mt7996_tm_set(dev, SET_ID(GI), td->tx_rate_sgi);
+	mt7996_tm_set(dev, SET_ID(CBW),
+		      mt7996_tm_bw_mapping(width, BW_MAP_NL_TO_FW));
+	mt7996_tm_set(dev, SET_ID(DBW),
+		      mt7996_tm_bw_mapping(width, BW_MAP_NL_TO_FW));
+	mt7996_tm_set_antenna(phy, SET_ID(TX_PATH));
+	mt7996_tm_set_antenna(phy, SET_ID(RX_PATH));
+	mt7996_tm_set(dev, SET_ID(IPG), td->tx_ipg);
+	mt7996_tm_set(dev, SET_ID(TX_LEN), td->tx_mpdu_len);
+	mt7996_tm_set(dev, SET_ID(TX_TIME), 0);
+	mt7996_tm_set(dev, SET_ID(COMMAND), RF_CMD(TX_COMMIT));
+
+	return 0;
+}
+
+static int
+mt7996_tm_txbf_phase_comp(struct mt7996_phy *phy, u16 *val)
+{
+	struct mt7996_dev *dev = phy->dev;
+	struct mt7996_tm_bf_req req = {
+		.phase_comp = {
+			.tag = cpu_to_le16(BF_IBF_PHASE_COMP),
+			.len = cpu_to_le16(sizeof(req.phase_comp)),
+			.bw = val[0],
+			.jp_band = (val[2] == 1) ? 1 : 0,
+			.band_idx = phy->mt76->band_idx,
+			.read_from_e2p = val[3],
+			.disable = val[4],
+			.group = val[2],
+		}
+	};
+	struct mt7996_txbf_phase *phase = (struct mt7996_txbf_phase *)dev->test.txbf_phase_cal;
+
+	wait_event_timeout(dev->mt76.tx_wait, phase[val[2]].status != 0, HZ);
+	if (val[2])
+		memcpy(req.phase_comp.buf, &phase[val[2]].phase_5g, sizeof(req.phase_comp.buf));
+	else
+		memcpy(req.phase_comp.buf, &phase[val[2]].phase_2g, sizeof(req.phase_comp.buf));
+
+	pr_info("ibf cal process: phase comp info\n");
+	print_hex_dump(KERN_INFO, "", DUMP_PREFIX_NONE, 16, 1,
+		       &req, sizeof(req), 0);
+
+	return mt76_mcu_send_msg(&dev->mt76, MCU_WM_UNI_CMD(BF), &req,
+				 sizeof(req), false);
+}
+
+static int
+mt7996_tm_txbf_profile_tag_write(struct mt7996_phy *phy, u8 pfmu_idx, struct mt7996_pfmu_tag *tag)
+{
+	struct mt7996_dev *dev = phy->dev;
+	struct mt7996_tm_bf_req req = {
+		.pfmu_tag = {
+			.tag = cpu_to_le16(BF_PFMU_TAG_WRITE),
+			.len = cpu_to_le16(sizeof(req.pfmu_tag)),
+			.pfmu_id = pfmu_idx,
+			.bfer = true,
+			.band_idx = phy->mt76->band_idx,
+		}
+	};
+
+	memcpy(req.pfmu_tag.buf, tag, sizeof(*tag));
+	wait_event_timeout(dev->mt76.tx_wait, tag->t1.pfmu_idx != 0, HZ);
+
+	return mt76_mcu_send_msg(&dev->mt76, MCU_WM_UNI_CMD(BF), &req,
+				 sizeof(req), false);
+}
+
+static int
+mt7996_tm_add_txbf_sta(struct mt7996_phy *phy, u8 pfmu_idx, u8 nr, u8 nc, bool ebf)
+{
+	struct mt7996_dev *dev = phy->dev;
+	struct mt76_testmode_data *td = &phy->mt76->test;
+	struct {
+		struct sta_req_hdr hdr;
+		struct sta_rec_bf bf;
+	} __packed req = {
+		.hdr = {
+			.bss_idx = phy->mt76->band_idx,
+			.wlan_idx_lo = to_wcid_lo(phy->mt76->band_idx + 1),
+			.tlv_num = 1,
+			.is_tlv_append = 1,
+			.muar_idx = 0,
+			.wlan_idx_hi = to_wcid_hi(phy->mt76->band_idx + 1),
+		},
+		.bf = {
+			.tag = cpu_to_le16(STA_REC_BF),
+			.len = cpu_to_le16(sizeof(req.bf)),
+			.pfmu = cpu_to_le16(pfmu_idx),
+			.sounding_phy = 1,
+			.bf_cap = ebf,
+			.ncol = nc,
+			.nrow = nr,
+			.ibf_timeout = 0xff,
+			.tx_mode = mt7996_tm_rate_mapping(td->tx_rate_mode, RATE_MODE_TO_PHY),
+		},
+	};
+	u8 ndp_rate, ndpa_rate, rept_poll_rate, bf_bw;
+
+	if (td->tx_rate_mode == MT76_TM_TX_MODE_HE_SU ||
+	    td->tx_rate_mode == MT76_TM_TX_MODE_EHT_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;
+	}
+
+	bf_bw = mt7996_tm_bw_mapping(phy->mt76->chandef.width, BW_MAP_NL_TO_BF);
+	req.bf.ndp_rate = ndp_rate;
+	req.bf.ndpa_rate = ndpa_rate;
+	req.bf.rept_poll_rate = rept_poll_rate;
+	req.bf.bw = bf_bw;
+	req.bf.tx_mode = (td->tx_rate_mode == MT76_TM_TX_MODE_EHT_SU) ? 0xf : req.bf.tx_mode;
+
+	if (ebf) {
+		req.bf.mem[0].row = 0;
+		req.bf.mem[1].row = 1;
+		req.bf.mem[2].row = 2;
+		req.bf.mem[3].row = 3;
+	} else {
+		req.bf.mem[0].row = 4;
+		req.bf.mem[1].row = 5;
+		req.bf.mem[2].row = 6;
+		req.bf.mem[3].row = 7;
+	}
+
+	return mt76_mcu_send_msg(&dev->mt76, MCU_WMWA_UNI_CMD(STA_REC_UPDATE), &req,
+				 sizeof(req), true);
+}
+
+static int
+mt7996_tm_txbf_profile_update(struct mt7996_phy *phy, u16 *val, bool ebf)
+{
+#define MT_ARB_IBF_ENABLE			(BIT(0) | GENMASK(9, 8))
+	struct mt76_testmode_data *td = &phy->mt76->test;
+	struct mt7996_dev *dev = phy->dev;
+	struct mt7996_pfmu_tag *tag = dev->test.txbf_pfmu_tag;
+	u8 pfmu_idx = val[0], nc = val[2], nr;
+	int ret;
+	bool is_atenl = val[5];
+
+	if (td->tx_antenna_mask == 3)
+		nr = 1;
+	else if (td->tx_antenna_mask == 7)
+		nr = 2;
+	else
+		nr = 3;
+
+	memset(tag, 0, sizeof(*tag));
+	tag->t1.pfmu_idx = pfmu_idx;
+	tag->t1.ebf = ebf;
+	tag->t1.nr = nr;
+	tag->t1.nc = nc;
+	tag->t1.invalid_prof = true;
+	tag->t1.data_bw = mt7996_tm_bw_mapping(phy->mt76->chandef.width, BW_MAP_NL_TO_BF);
+	tag->t2.se_idx = td->tx_spe_idx;
+
+	if (ebf) {
+		tag->t1.row_id1 = 0;
+		tag->t1.row_id2 = 1;
+		tag->t1.row_id3 = 2;
+		tag->t1.row_id4 = 3;
+		tag->t1.lm = mt7996_tm_rate_mapping(td->tx_rate_mode, RATE_MODE_TO_LM);
+	} else {
+		tag->t1.row_id1 = 4;
+		tag->t1.row_id2 = 5;
+		tag->t1.row_id3 = 6;
+		tag->t1.row_id4 = 7;
+		tag->t1.lm = mt7996_tm_rate_mapping(MT76_TM_TX_MODE_OFDM, RATE_MODE_TO_LM);
+
+		tag->t2.ibf_timeout = 0xff;
+		tag->t2.ibf_nr = nr;
+		tag->t2.ibf_nc = nc;
+	}
+
+	ret = mt7996_tm_txbf_profile_tag_write(phy, pfmu_idx, tag);
+	if (ret)
+		return ret;
+
+	ret = mt7996_tm_add_txbf_sta(phy, pfmu_idx, nr, nc, ebf);
+	if (ret)
+		return ret;
+
+	if (!is_atenl && !td->ibf) {
+		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 (!is_atenl && td->ibf && 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 mt7996_tm_txbf_apply_tx(phy, 1, false, true, true);
+
+	return 0;
+}
+
+static int
+mt7996_tm_txbf_phase_cal(struct mt7996_phy *phy, u16 *val)
+{
+	struct mt7996_dev *dev = phy->dev;
+	struct mt7996_tm_bf_req req = {
+		.phase_cal = {
+			.tag = cpu_to_le16(BF_PHASE_CALIBRATION),
+			.len = cpu_to_le16(sizeof(req.phase_cal)),
+			.group = val[0],
+			.group_l_m_n = val[1],
+			.sx2 = val[2],
+			.cal_type = val[3],
+			.lna_gain_level = val[4],
+			.band_idx = phy->mt76->band_idx,
+		},
+	};
+	struct mt7996_txbf_phase *phase = (struct mt7996_txbf_phase *)dev->test.txbf_phase_cal;
+
+	/* reset phase status before update phase cal data */
+	phase[req.phase_cal.group].status = 0;
+
+	return mt76_mcu_send_msg(&dev->mt76, MCU_WM_UNI_CMD(BF), &req,
+				 sizeof(req), false);
+}
+
+static int
+mt7996_tm_txbf_profile_update_all(struct mt7996_phy *phy, u16 *val)
+{
+#define MT7996_TXBF_PFMU_DATA_LEN	(MT7996_TXBF_SUBCAR_NUM * sizeof(struct mt7996_pfmu_data))
+	struct mt76_testmode_data *td = &phy->mt76->test;
+	u8 nss = hweight8(td->tx_antenna_mask);
+	u16 pfmu_idx = val[0];
+	u16 subc_id = val[1];
+	u16 angle11 = val[2];
+	u16 angle21 = val[3];
+	u16 angle31 = val[4];
+	u16 angle41 = val[5];
+	s16 phi11 = 0, phi21 = 0, phi31 = 0;
+	struct mt7996_pfmu_data *pfmu_data;
+
+	if (subc_id > MT7996_TXBF_SUBCAR_NUM - 1)
+		return -EINVAL;
+
+	if (nss == 2) {
+		phi11 = (s16)(angle21 - angle11);
+	} else if (nss == 3) {
+		phi11 = (s16)(angle31 - angle11);
+		phi21 = (s16)(angle31 - angle21);
+	} else {
+		phi11 = (s16)(angle41 - angle11);
+		phi21 = (s16)(angle41 - angle21);
+		phi31 = (s16)(angle41 - angle31);
+	}
+
+	pfmu_data = (struct mt7996_pfmu_data *)phy->dev->test.txbf_pfmu_data;
+	pfmu_data = &pfmu_data[subc_id];
+
+	if (subc_id < 32)
+		pfmu_data->subc_idx = cpu_to_le16(subc_id + 224);
+	else
+		pfmu_data->subc_idx = cpu_to_le16(subc_id - 32);
+
+	pfmu_data->phi11 = cpu_to_le16(phi11);
+	pfmu_data->phi21 = cpu_to_le16(phi21);
+	pfmu_data->phi31 = cpu_to_le16(phi31);
+	if (subc_id == MT7996_TXBF_SUBCAR_NUM - 1) {
+		struct mt7996_dev *dev = phy->dev;
+		struct mt7996_tm_bf_req req = {
+			.pfmu_data_all = {
+				.tag = cpu_to_le16(BF_PROFILE_WRITE_20M_ALL),
+				.len = cpu_to_le16(sizeof(req.pfmu_data_all)),
+				.pfmu_id = pfmu_idx,
+				.band_idx = phy->mt76->band_idx,
+			},
+		};
+
+		memcpy(req.pfmu_data_all.buf, dev->test.txbf_pfmu_data, MT7996_TXBF_PFMU_DATA_LEN);
+
+		return mt76_mcu_send_msg(&dev->mt76, MCU_WM_UNI_CMD(BF),
+					 &req, sizeof(req), true);
+	}
+
+	return 0;
+}
+
+static int
+mt7996_tm_txbf_e2p_update(struct mt7996_phy *phy)
+{
+#define TXBF_PHASE_EEPROM_START_OFFSET		0xc00
+#define TXBF_PHASE_GROUP_EEPROM_OFFSET		0x2e
+	struct mt7996_txbf_phase *phase, *p;
+	struct mt7996_dev *dev = phy->dev;
+	u8 *eeprom = dev->mt76.eeprom.data;
+	u16 offset;
+	int i;
+
+	offset = TXBF_PHASE_EEPROM_START_OFFSET;
+	phase = (struct mt7996_txbf_phase *)dev->test.txbf_phase_cal;
+	for (i = 0; i < MAX_PHASE_GROUP_NUM; i++) {
+		p = &phase[i];
+
+		if (!p->status)
+			continue;
+
+		/* copy phase cal data to eeprom */
+		if (i)
+			memcpy(eeprom + offset, &p->phase_5g, sizeof(p->phase_5g));
+		else
+			memcpy(eeprom + offset, &p->phase_2g, sizeof(p->phase_2g));
+		offset += TXBF_PHASE_GROUP_EEPROM_OFFSET;
+	}
+
+	return 0;
+}
+
+static int
+mt7996_tm_txbf_apply_tx(struct mt7996_phy *phy, u16 wlan_idx, bool ebf,
+			bool ibf, bool phase_cal)
+{
+	struct mt7996_dev *dev = phy->dev;
+	struct mt7996_tm_bf_req req = {
+		.tx_apply = {
+			.tag = cpu_to_le16(BF_DATA_PACKET_APPLY),
+			.len = cpu_to_le16(sizeof(req.tx_apply)),
+			.wlan_idx = cpu_to_le16(wlan_idx),
+			.ebf = ebf,
+			.ibf = ibf,
+			.phase_cal = phase_cal,
+		},
+	};
+
+	return mt76_mcu_send_msg(&dev->mt76, MCU_WM_UNI_CMD(BF), &req, sizeof(req), false);
+}
+
+static int
+mt7996_tm_txbf_set_tx(struct mt7996_phy *phy, u16 *val)
+{
+	bool bf_on = val[0], update = val[3];
+	struct mt7996_dev *dev = phy->dev;
+	struct mt7996_pfmu_tag *tag = dev->test.txbf_pfmu_tag;
+	struct mt76_testmode_data *td = &phy->mt76->test;
+
+	if (bf_on) {
+		mt7996_tm_set_rx_frames(phy, false);
+		mt7996_tm_set_tx_frames(phy, false);
+		mt7996_mcu_set_txbf_internal(phy, BF_PFMU_TAG_READ, 2, true);
+		tag->t1.invalid_prof = false;
+		mt7996_tm_txbf_profile_tag_write(phy, 2, tag);
+		td->bf_ever_en = true;
+
+		if (update)
+			mt7996_tm_txbf_apply_tx(phy, 1, 0, 1, 1);
+	} else {
+		if (!td->bf_ever_en) {
+			mt7996_tm_set_rx_frames(phy, false);
+			mt7996_tm_set_tx_frames(phy, false);
+			td->ibf = false;
+			td->ebf = false;
+
+			if (update)
+				mt7996_tm_txbf_apply_tx(phy, 1, 0, 0, 0);
+		} else {
+			td->bf_ever_en = false;
+
+			mt7996_mcu_set_txbf_internal(phy, BF_PFMU_TAG_READ, 2, true);
+			tag->t1.invalid_prof = true;
+			mt7996_tm_txbf_profile_tag_write(phy, 2, tag);
+		}
+	}
+
+	return 0;
+}
+
+static int
+mt7996_tm_trigger_sounding(struct mt7996_phy *phy, u16 *val, bool en)
+{
+	struct mt7996_dev *dev = phy->dev;
+	u8 sounding_mode = val[0];
+	u8 sta_num = val[1];
+	u32 sounding_interval = (u32)val[2] << 2;	/* input unit: 4ms */
+	u16 tag = en ? BF_SOUNDING_ON : BF_SOUNDING_OFF;
+	struct mt7996_tm_bf_req req = {
+		.sounding = {
+			.tag = cpu_to_le16(tag),
+			.len = cpu_to_le16(sizeof(req.sounding)),
+			.snd_mode = sounding_mode,
+			.sta_num = sta_num,
+			.wlan_id = {
+				cpu_to_le16(val[3]),
+				cpu_to_le16(val[4]),
+				cpu_to_le16(val[5]),
+				cpu_to_le16(val[6])
+			},
+			.snd_period = cpu_to_le32(sounding_interval),
+		},
+	};
+
+	if (sounding_mode > SOUNDING_MODE_MAX)
+		return -EINVAL;
+
+	return mt76_mcu_send_msg(&dev->mt76, MCU_WM_UNI_CMD(BF),
+				 &req, sizeof(req), false);
+}
+
+static int
+mt7996_tm_txbf_txcmd(struct mt7996_phy *phy, u16 *val)
+{
+	struct mt7996_dev *dev = phy->dev;
+	struct mt7996_tm_bf_req req = {
+		.txcmd = {
+			.tag = cpu_to_le16(BF_CMD_TXCMD),
+			.len = cpu_to_le16(sizeof(req.txcmd)),
+			.action = val[0],
+			.bf_manual = val[1],
+			.bf_bit = val[2],
+		},
+	};
+
+	return mt76_mcu_send_msg(&dev->mt76, MCU_WM_UNI_CMD(BF), &req, sizeof(req), false);
+}
+
+static int
+mt7996_tm_set_txbf(struct mt7996_phy *phy)
+{
+#define TXBF_IS_DUT_MASK	BIT(0)
+#define TXBF_IBF_MASK		BIT(1)
+	struct mt76_testmode_data *td = &phy->mt76->test;
+	u16 *val = td->txbf_param;
+
+	dev_info(phy->dev->mt76.dev,
+		 "ibf cal process: act = %u, val = %u, %u, %u, %u, %u, %u, %u, %u\n",
+		 td->txbf_act, val[0], val[1], val[2], val[3], val[4], val[5], val[6], val[7]);
+
+	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->ibf = !u32_get_bits(td->txbf_act, TXBF_IBF_MASK);
+		td->ebf = true;
+		td->is_txbf_dut = !!u32_get_bits(td->txbf_act, TXBF_IS_DUT_MASK);
+		return mt7996_tm_txbf_init(phy, val);
+	case MT76_TM_TXBF_ACT_UPDATE_CH:
+		mt7996_tm_update_channel(phy);
+		break;
+	case MT76_TM_TXBF_ACT_PHASE_COMP:
+		return mt7996_tm_txbf_phase_comp(phy, val);
+	case MT76_TM_TXBF_ACT_TX_PREP:
+		return mt7996_tm_txbf_set_tx(phy, val);
+	case MT76_TM_TXBF_ACT_IBF_PROF_UPDATE:
+		return mt7996_tm_txbf_profile_update(phy, val, false);
+	case MT76_TM_TXBF_ACT_EBF_PROF_UPDATE:
+		return mt7996_tm_txbf_profile_update(phy, val, true);
+	case MT76_TM_TXBF_ACT_PHASE_CAL:
+		return mt7996_tm_txbf_phase_cal(phy, val);
+	case MT76_TM_TXBF_ACT_PROF_UPDATE_ALL_CMD:
+	case MT76_TM_TXBF_ACT_PROF_UPDATE_ALL:
+		return mt7996_tm_txbf_profile_update_all(phy, val);
+	case MT76_TM_TXBF_ACT_E2P_UPDATE:
+		return mt7996_tm_txbf_e2p_update(phy);
+	case MT76_TM_TXBF_ACT_APPLY_TX: {
+		u16 wlan_idx = val[0];
+		bool ebf = !!val[1], ibf = !!val[2], phase_cal = !!val[4];
+
+		return mt7996_tm_txbf_apply_tx(phy, wlan_idx, ebf, ibf, phase_cal);
+	}
+	case MT76_TM_TXBF_ACT_TRIGGER_SOUNDING:
+		return mt7996_tm_trigger_sounding(phy, val, true);
+	case MT76_TM_TXBF_ACT_STOP_SOUNDING:
+		memset(val, 0, sizeof(td->txbf_param));
+		return mt7996_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 mt7996_dev *dev = phy->dev;
+		struct mt7996_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 mt7996_tm_txbf_profile_tag_write(phy, pfmu_idx, tag);
+		else if (td->txbf_act == MT76_TM_TXBF_ACT_PROFILE_TAG_READ)
+			return mt7996_mcu_set_txbf_internal(phy, BF_PFMU_TAG_READ, pfmu_idx, bfer);
+
+		tag->t1.invalid_prof = !!val[0];
+
+		return 0;
+	}
+	case MT76_TM_TXBF_ACT_STA_REC_READ:
+		return mt7996_mcu_set_txbf_internal(phy, BF_STA_REC_READ, val[0], 0);
+	case MT76_TM_TXBF_ACT_TXCMD:
+		return mt7996_tm_txbf_txcmd(phy, val);
+	default:
+		break;
+	};
+
+	return 0;
+}
+
 static void
 mt7996_tm_update_params(struct mt7996_phy *phy, u32 changed)
 {
@@ -1086,6 +1770,8 @@ mt7996_tm_update_params(struct mt7996_phy *phy, u32 changed)
 		mt7996_tm_set_ipi(phy);
 	if (changed & BIT(TM_CHANGED_IPI_RESET))
 		mt7996_tm_ipi_hist_ctrl(phy, NULL, RDD_SET_IPI_HIST_RESET);
+	if (changed & BIT(TM_CHANGED_TXBF_ACT))
+		mt7996_tm_set_txbf(phy);
 }
 
 static int
diff --git a/mt7996/testmode.h b/mt7996/testmode.h
index 78662b2e..f97ccb26 100644
--- a/mt7996/testmode.h
+++ b/mt7996/testmode.h
@@ -27,6 +27,17 @@ enum {
 	FW_CDBW_8080MHZ,
 };
 
+enum {
+	BF_CDBW_20MHZ,
+	BF_CDBW_40MHZ,
+	BF_CDBW_80MHZ,
+	BF_CDBW_160MHZ,
+	BF_CDBW_320MHZ,
+	BF_CDBW_10MHZ = BF_CDBW_320MHZ,
+	BF_CDBW_5MHZ,
+	BF_CDBW_8080MHZ,
+};
+
 #define FIRST_CONTROL_CHAN_BITMAP_BW40		0x5555555
 #define FIRST_CONTROL_CHAN_BITMAP_BW80		0x111111
 #define FIRST_CONTROL_CHAN_BITMAP_BW160		0x100101
@@ -34,12 +45,20 @@ enum {
 enum bw_mapping_method {
 	BW_MAP_NL_TO_FW,
 	BW_MAP_NL_TO_TM,
+	BW_MAP_NL_TO_BF,
 	BW_MAP_NL_TO_MHZ,
 	BW_MAP_NL_TO_CONTROL_BITMAP_5G,
 
 	NUM_BW_MAP,
 };
 
+enum rate_mapping_type {
+	RATE_MODE_TO_PHY,
+	RATE_MODE_TO_LM,
+
+	NUM_RATE_MAP,
+};
+
 struct tm_cal_param {
 	__le32 func_data;
 	u8 band_idx;
diff --git a/testmode.c b/testmode.c
index 69147f86..a5c07f8d 100644
--- a/testmode.c
+++ b/testmode.c
@@ -462,6 +462,42 @@ out:
 	return err;
 }
 
+static int
+mt76_testmode_txbf_profile_update_all_cmd(struct mt76_phy *phy, struct nlattr **tb, u32 state)
+{
+#define PARAM_UNIT	5
+	static u8 pfmu_idx;
+	struct mt76_testmode_data *td = &phy->test;
+	struct mt76_dev *dev = phy->dev;
+	struct nlattr *cur;
+	u16 tmp_val[PARAM_UNIT], *val = td->txbf_param;
+	int idx, rem, ret, i = 0;
+
+	memset(td->txbf_param, 0, sizeof(td->txbf_param));
+	nla_for_each_nested(cur, tb[MT76_TM_ATTR_TXBF_PARAM], rem) {
+		if (nla_len(cur) != 2)
+			return -EINVAL;
+		idx = i % PARAM_UNIT;
+		tmp_val[idx] = nla_get_u16(cur);
+		if (idx == 1 && (tmp_val[idx] == 0xf0 || tmp_val[idx] == 0xff)) {
+			pfmu_idx = tmp_val[0];
+			return 0;
+		}
+		if (idx == PARAM_UNIT - 1) {
+			val[0] = pfmu_idx;
+			memcpy(val + 1, tmp_val, sizeof(tmp_val));
+			if (dev->test_ops->set_params) {
+				ret = dev->test_ops->set_params(phy, tb, state);
+				if (ret)
+					return ret;
+			}
+		}
+		i++;
+	}
+
+	return 0;
+}
+
 int mt76_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 		      void *data, int len)
 {
@@ -607,6 +643,30 @@ int mt76_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 		}
 	}
 
+	if (tb[MT76_TM_ATTR_TXBF_ACT]) {
+		struct nlattr *cur;
+		int rem, idx = 0;
+
+		if (!tb[MT76_TM_ATTR_TXBF_PARAM] ||
+		    mt76_tm_get_u8(tb[MT76_TM_ATTR_TXBF_ACT], &td->txbf_act,
+				   0, MT76_TM_TXBF_ACT_MAX))
+			goto out;
+
+		if (td->txbf_act == MT76_TM_TXBF_ACT_PROF_UPDATE_ALL_CMD) {
+			err = mt76_testmode_txbf_profile_update_all_cmd(phy, tb, state);
+			goto out;
+		}
+
+		memset(td->txbf_param, 0, sizeof(td->txbf_param));
+		nla_for_each_nested(cur, tb[MT76_TM_ATTR_TXBF_PARAM], rem) {
+			if (nla_len(cur) != 2 ||
+			    idx >= ARRAY_SIZE(td->txbf_param))
+				goto out;
+
+			td->txbf_param[idx++] = nla_get_u16(cur);
+		}
+	}
+
 	if (dev->test_ops->set_params) {
 		err = dev->test_ops->set_params(phy, tb, state);
 		if (err)
diff --git a/testmode.h b/testmode.h
index 5d677f8c..bda7624a 100644
--- a/testmode.h
+++ b/testmode.h
@@ -286,6 +286,59 @@ enum mt76_testmode_eeprom_action {
 	MT76_TM_EEPROM_ACTION_MAX = NUM_MT76_TM_EEPROM_ACTION - 1,
 };
 
+/**
+ * enum mt76_testmode_txbf_act - txbf action
+ *
+ * @MT76_TM_TXBF_ACT_GOLDEN_INIT: init ibf setting for golden device
+ * @MT76_TM_TXBF_ACT_INIT: init ibf setting for DUT
+ * @MT76_TM_TX_EBF_ACT_GOLDEN_INIT: init ebf setting for golden device
+ * @MT76_TM_TX_EBF_ACT_INIT: init ebf setting for DUT
+ * @MT76_TM_TXBF_ACT_UPDATE_CH: update channel info
+ * @MT76_TM_TXBF_ACT_PHASE_COMP: txbf phase compensation
+ * @MT76_TM_TXBF_ACT_TX_PREP: TX preparation for txbf
+ * @MT76_TM_TXBF_ACT_IBF_PROF_UPDATE: update ibf profile (pfmu tag, bf sta record)
+ * @MT76_TM_TXBF_ACT_EBF_PROF_UPDATE: update ebf profile
+ * @MT76_TM_TXBF_ACT_APPLY_TX: apply TX setting for txbf
+ * @MT76_TM_TXBF_ACT_PHASE_CAL: perform txbf phase calibration
+ * @MT76_TM_TXBF_ACT_PROF_UPDATE_ALL: update bf profile via instrument
+ * @MT76_TM_TXBF_ACT_PROF_UPDATE_ALL_CMD: update bf profile via instrument
+ * @MT76_TM_TXBF_ACT_E2P_UPDATE: write back txbf calibration result to eeprom
+ * @MT76_TM_TXBF_ACT_TRIGGER_SOUNDING: trigger beamformer to send sounding packet
+ * @MT76_TM_TXBF_ACT_STOP_SOUNDING: stop sending sounding packet
+ * @MT76_TM_TXBF_ACT_PROFILE_TAG_READ: read pfmu tag
+ * @MT76_TM_TXBF_ACT_PROFILE_TAG_WRITE: update pfmu tag
+ * @MT76_TM_TXBF_ACT_PROFILE_TAG_INVALID: invalidate pfmu tag
+ * @MT76_TM_TXBF_ACT_STA_REC_READ: read bf sta record
+ * @MT76_TM_TXBF_ACT_TXCMD: configure txcmd bf bit manually
+ */
+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,
+	MT76_TM_TXBF_ACT_IBF_PROF_UPDATE,
+	MT76_TM_TXBF_ACT_EBF_PROF_UPDATE,
+	MT76_TM_TXBF_ACT_APPLY_TX,
+	MT76_TM_TXBF_ACT_PHASE_CAL,
+	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,
+	MT76_TM_TXBF_ACT_TXCMD,
+
+	/* keep last */
+	NUM_MT76_TM_TXBF_ACT,
+	MT76_TM_TXBF_ACT_MAX = NUM_MT76_TM_TXBF_ACT - 1,
+};
+
 extern const struct nla_policy mt76_tm_policy[NUM_MT76_TM_ATTRS];
 
 #endif
diff --git a/tools/fields.c b/tools/fields.c
index 77696ce7..f793d1a5 100644
--- a/tools/fields.c
+++ b/tools/fields.c
@@ -44,6 +44,30 @@ static const char * const testmode_offchan_bw[] = {
 	[NL80211_CHAN_WIDTH_160] = "160",
 };
 
+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",
+	[MT76_TM_TXBF_ACT_IBF_PROF_UPDATE] = "ibf_prof_update",
+	[MT76_TM_TXBF_ACT_EBF_PROF_UPDATE] = "ebf_prof_update",
+	[MT76_TM_TXBF_ACT_APPLY_TX] = "apply_tx",
+	[MT76_TM_TXBF_ACT_PHASE_CAL] = "phase_cal",
+	[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",
+	[MT76_TM_TXBF_ACT_TXCMD] = "txcmd",
+};
+
 static void print_enum(const struct tm_field *field, struct nlattr *attr)
 {
 	unsigned int i = nla_get_u8(attr);
@@ -94,6 +118,17 @@ static void print_s8(const struct tm_field *field, struct nlattr *attr)
 	printf("%d", (int8_t)nla_get_u8(attr));
 }
 
+static bool parse_u16_hex(const struct tm_field *field, int idx,
+			  struct nl_msg *msg, const char *val)
+{
+	return !nla_put_u16(msg, idx, strtoul(val, NULL, 16));
+}
+
+static void print_u16_hex(const struct tm_field *field, struct nlattr *attr)
+{
+	printf("%d", nla_get_u16(attr));
+}
+
 static bool parse_u32(const struct tm_field *field, int idx,
 		      struct nl_msg *msg, const char *val)
 {
@@ -399,6 +434,8 @@ static const struct tm_field testdata_fields[NUM_MT76_TM_ATTRS] = {
 	FIELD(u8, AID, "aid"),
 	FIELD(u8, RU_ALLOC, "ru_alloc"),
 	FIELD(u8, RU_IDX, "ru_idx"),
+	FIELD_ENUM(TXBF_ACT, "txbf_act", testmode_txbf_act),
+	FIELD_ARRAY(u16_hex, TXBF_PARAM, "txbf_param"),
 	FIELD(u8, OFF_CH_SCAN_CH, "offchan_ch"),
 	FIELD(u8, OFF_CH_SCAN_CENTER_CH, "offchan_center_ch"),
 	FIELD_ENUM(OFF_CH_SCAN_BW, "offchan_bw", testmode_offchan_bw),
-- 
2.18.0