From ce58ffb18be834e2f62a30a71c9d6f5805517a9e Mon Sep 17 00:00:00 2001
From: StanleyYP Wang <StanleyYP.Wang@mediatek.com>
Date: Wed, 1 Mar 2023 11:59:16 +0800
Subject: [PATCH 36/98] wifi: mt76: testmode: add basic testmode support

Signed-off-by: StanleyYP Wang <StanleyYP.Wang@mediatek.com>
---
 eeprom.c          |   6 +-
 mac80211.c        |   3 +-
 mt76.h            |  35 +++
 mt76_connac_mcu.h |   2 +
 mt7996/Makefile   |   1 +
 mt7996/eeprom.c   |  35 ++-
 mt7996/eeprom.h   |   1 +
 mt7996/init.c     |   8 +
 mt7996/mac.c      |   3 +-
 mt7996/main.c     |  16 ++
 mt7996/mcu.c      |  42 ++-
 mt7996/mcu.h      |  27 ++
 mt7996/mt7996.h   |  23 ++
 mt7996/testmode.c | 674 ++++++++++++++++++++++++++++++++++++++++++++++
 mt7996/testmode.h | 297 ++++++++++++++++++++
 testmode.c        |  78 ++++--
 testmode.h        |  64 +++++
 tools/fields.c    | 102 ++++++-
 18 files changed, 1383 insertions(+), 34 deletions(-)
 create mode 100644 mt7996/testmode.c
 create mode 100644 mt7996/testmode.h

diff --git a/eeprom.c b/eeprom.c
index a07ca84..437d8ca 100644
--- a/eeprom.c
+++ b/eeprom.c
@@ -94,8 +94,10 @@ static int mt76_get_of_epprom_from_mtd(struct mt76_dev *dev, void *eep, int offs
 	}
 
 #ifdef CONFIG_NL80211_TESTMODE
-	dev->test_mtd.name = devm_kstrdup(dev->dev, part, GFP_KERNEL);
-	dev->test_mtd.offset = offset;
+	if (len == dev->eeprom.size) {
+		dev->test_mtd.name = devm_kstrdup(dev->dev, part, GFP_KERNEL);
+		dev->test_mtd.offset = offset;
+	}
 #endif
 
 out_put_node:
diff --git a/mac80211.c b/mac80211.c
index cd102dd..f10ca90 100644
--- a/mac80211.c
+++ b/mac80211.c
@@ -835,7 +835,8 @@ void mt76_rx(struct mt76_dev *dev, enum mt76_rxq_id q, struct sk_buff *skb)
 	}
 
 #ifdef CONFIG_NL80211_TESTMODE
-	if (phy->test.state == MT76_TM_STATE_RX_FRAMES) {
+	if (!(phy->test.flag & MT_TM_FW_RX_COUNT) &&
+	    phy->test.state == MT76_TM_STATE_RX_FRAMES) {
 		phy->test.rx_stats.packets[q]++;
 		if (status->flag & RX_FLAG_FAILED_FCS_CRC)
 			phy->test.rx_stats.fcs_error[q]++;
diff --git a/mt76.h b/mt76.h
index 7f93210..feb861c 100644
--- a/mt76.h
+++ b/mt76.h
@@ -692,14 +692,20 @@ struct mt76_testmode_ops {
 	int (*set_params)(struct mt76_phy *phy, struct nlattr **tb,
 			  enum mt76_testmode_state new_state);
 	int (*dump_stats)(struct mt76_phy *phy, struct sk_buff *msg);
+	void (*reset_rx_stats)(struct mt76_phy *phy);
+	void (*tx_stop)(struct mt76_phy *phy);
 };
 
+#define MT_TM_FW_RX_COUNT	BIT(0)
+
 struct mt76_testmode_data {
 	enum mt76_testmode_state state;
 
 	u32 param_set[DIV_ROUND_UP(NUM_MT76_TM_ATTRS, 32)];
 	struct sk_buff *tx_skb;
 
+	u8 sku_en;
+
 	u32 tx_count;
 	u16 tx_mpdu_len;
 
@@ -709,6 +715,7 @@ struct mt76_testmode_data {
 	u8 tx_rate_sgi;
 	u8 tx_rate_ldpc;
 	u8 tx_rate_stbc;
+	u16 tx_preamble_puncture;
 	u8 tx_ltf;
 
 	u8 tx_antenna_mask;
@@ -718,6 +725,9 @@ struct mt76_testmode_data {
 	u32 tx_time;
 	u32 tx_ipg;
 
+	bool ibf;
+	bool ebf;
+
 	u32 freq_offset;
 
 	u8 tx_power[4];
@@ -732,7 +742,16 @@ struct mt76_testmode_data {
 	struct {
 		u64 packets[__MT_RXQ_MAX];
 		u64 fcs_error[__MT_RXQ_MAX];
+		u64 len_mismatch;
 	} rx_stats;
+	u8 flag;
+
+	struct {
+		u8 type;
+		u8 enable;
+	} cfg;
+
+	u8 aid;
 };
 
 struct mt76_vif {
@@ -1418,6 +1437,22 @@ int mt76_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *skb,
 int mt76_testmode_set_state(struct mt76_phy *phy, enum mt76_testmode_state state);
 int mt76_testmode_alloc_skb(struct mt76_phy *phy, u32 len);
 
+static inline void
+mt76_testmode_param_set(struct mt76_testmode_data *td, u16 idx)
+{
+#ifdef CONFIG_NL80211_TESTMODE
+	td->param_set[idx / 32] |= BIT(idx % 32);
+#endif
+}
+
+static inline bool
+mt76_testmode_param_present(struct mt76_testmode_data *td, u16 idx)
+{
+#ifdef CONFIG_NL80211_TESTMODE
+	return td->param_set[idx / 32] & BIT(idx % 32);
+#endif
+}
+
 static inline void mt76_testmode_reset(struct mt76_phy *phy, bool disable)
 {
 #ifdef CONFIG_NL80211_TESTMODE
diff --git a/mt76_connac_mcu.h b/mt76_connac_mcu.h
index ca2e573..4d054bd 100644
--- a/mt76_connac_mcu.h
+++ b/mt76_connac_mcu.h
@@ -1239,12 +1239,14 @@ enum {
 	MCU_UNI_CMD_EFUSE_CTRL = 0x2d,
 	MCU_UNI_CMD_RA = 0x2f,
 	MCU_UNI_CMD_MURU = 0x31,
+	MCU_UNI_CMD_TESTMODE_RX_STAT = 0x32,
 	MCU_UNI_CMD_BF = 0x33,
 	MCU_UNI_CMD_CHANNEL_SWITCH = 0x34,
 	MCU_UNI_CMD_THERMAL = 0x35,
 	MCU_UNI_CMD_VOW = 0x37,
 	MCU_UNI_CMD_PP = 0x38,
 	MCU_UNI_CMD_FIXED_RATE_TABLE = 0x40,
+	MCU_UNI_CMD_TESTMODE_CTRL = 0x46,
 	MCU_UNI_CMD_RRO = 0x57,
 	MCU_UNI_CMD_OFFCH_SCAN_CTRL = 0x58,
 	MCU_UNI_CMD_PER_STA_INFO = 0x6d,
diff --git a/mt7996/Makefile b/mt7996/Makefile
index a056b40..7bb17f4 100644
--- a/mt7996/Makefile
+++ b/mt7996/Makefile
@@ -8,5 +8,6 @@ mt7996e-y := pci.o init.o dma.o eeprom.o main.o mcu.o mac.o \
 	     debugfs.o mmio.o
 
 mt7996e-$(CONFIG_DEV_COREDUMP) += coredump.o
+mt7996e-$(CONFIG_NL80211_TESTMODE) += testmode.o
 
 mt7996e-y += mtk_debugfs.o mtk_mcu.o
diff --git a/mt7996/eeprom.c b/mt7996/eeprom.c
index 7ae92e1..7fd318c 100644
--- a/mt7996/eeprom.c
+++ b/mt7996/eeprom.c
@@ -6,6 +6,11 @@
 #include <linux/firmware.h>
 #include "mt7996.h"
 #include "eeprom.h"
+#include <linux/moduleparam.h>
+
+static bool testmode_enable;
+module_param(testmode_enable, bool, 0644);
+MODULE_PARM_DESC(testmode_enable, "Enable testmode");
 
 static int mt7996_check_eeprom(struct mt7996_dev *dev)
 {
@@ -23,7 +28,10 @@ static int mt7996_check_eeprom(struct mt7996_dev *dev)
 static char *mt7996_eeprom_name(struct mt7996_dev *dev)
 {
 	/* reserve for future variants */
-	return MT7996_EEPROM_DEFAULT;
+	if (dev->testmode_enable)
+		return MT7996_EEPROM_DEFAULT_TM;
+	else
+		return MT7996_EEPROM_DEFAULT;
 }
 
 static int
@@ -52,21 +60,36 @@ out:
 	return ret;
 }
 
-static int mt7996_eeprom_load(struct mt7996_dev *dev)
+int mt7996_eeprom_check_fw_mode(struct mt7996_dev *dev)
 {
+	u8 *eeprom;
 	int ret;
 
+	/* load eeprom in flash or bin file mode to determine fw mode */
 	ret = mt76_eeprom_init(&dev->mt76, MT7996_EEPROM_SIZE);
 	if (ret < 0)
 		return ret;
 
 	if (ret) {
 		dev->flash_mode = true;
-	} else {
-		u8 free_block_num;
-		u32 block_num, i;
-		u32 eeprom_blk_size = MT7996_EEPROM_BLOCK_SIZE;
+		eeprom = dev->mt76.eeprom.data;
+		/* testmode enable priority: eeprom field > module parameter */
+		dev->testmode_enable = !mt7996_check_eeprom(dev) ? eeprom[MT_EE_TESTMODE_EN] :
+								   testmode_enable;
+	}
+
+	return ret;
+}
+
+static int mt7996_eeprom_load(struct mt7996_dev *dev)
+{
+	int ret;
+	u8 free_block_num;
+	u32 block_num, i;
+	u32 eeprom_blk_size = MT7996_EEPROM_BLOCK_SIZE;
 
+	/* flash or bin file mode eeprom is loaded before mcu init */
+	if (!dev->flash_mode) {
 		ret = mt7996_mcu_get_eeprom_free_block(dev, &free_block_num);
 		if (ret < 0)
 			return ret;
diff --git a/mt7996/eeprom.h b/mt7996/eeprom.h
index 412d6e2..9ea3667 100644
--- a/mt7996/eeprom.h
+++ b/mt7996/eeprom.h
@@ -14,6 +14,7 @@ enum mt7996_eeprom_field {
 	MT_EE_MAC_ADDR =	0x004,
 	MT_EE_MAC_ADDR2 =	0x00a,
 	MT_EE_WIFI_CONF =	0x190,
+	MT_EE_TESTMODE_EN =	0x1af,
 	MT_EE_MAC_ADDR3 =	0x2c0,
 	MT_EE_RATE_DELTA_2G =	0x1400,
 	MT_EE_RATE_DELTA_5G =	0x147d,
diff --git a/mt7996/init.c b/mt7996/init.c
index 273d1e7..6d39c3c 100644
--- a/mt7996/init.c
+++ b/mt7996/init.c
@@ -800,6 +800,10 @@ static int mt7996_init_hardware(struct mt7996_dev *dev)
 
 	set_bit(MT76_STATE_INITIALIZED, &dev->mphy.state);
 
+	ret = mt7996_eeprom_check_fw_mode(dev);
+	if (ret < 0)
+		return ret;
+
 	ret = mt7996_mcu_init(dev);
 	if (ret)
 		return ret;
@@ -1217,6 +1221,10 @@ int mt7996_register_device(struct mt7996_dev *dev)
 
 	mt7996_init_wiphy(hw, &dev->mt76.mmio.wed);
 
+#ifdef CONFIG_NL80211_TESTMODE
+	dev->mt76.test_ops = &mt7996_testmode_ops;
+#endif
+
 	ret = mt76_register_device(&dev->mt76, true, mt76_rates,
 				   ARRAY_SIZE(mt76_rates));
 	if (ret)
diff --git a/mt7996/mac.c b/mt7996/mac.c
index 32c52fc..637f0f6 100644
--- a/mt7996/mac.c
+++ b/mt7996/mac.c
@@ -685,7 +685,8 @@ mt7996_mac_fill_rx(struct mt7996_dev *dev, enum mt76_rxq_id q,
 				     *info);
 	}
 
-	if (rxv && mode >= MT_PHY_TYPE_HE_SU && !(status->flag & RX_FLAG_8023))
+	if (rxv && mode >= MT_PHY_TYPE_HE_SU && mode < MT_PHY_TYPE_EHT_SU &&
+	    !(status->flag & RX_FLAG_8023))
 		mt76_connac3_mac_decode_he_radiotap(skb, rxv, mode);
 
 	if (!status->wcid || !ieee80211_is_data_qos(fc) || hw_aggr)
diff --git a/mt7996/main.c b/mt7996/main.c
index 0e51fe0..226235c 100644
--- a/mt7996/main.c
+++ b/mt7996/main.c
@@ -23,6 +23,18 @@ static bool mt7996_dev_running(struct mt7996_dev *dev)
 	return phy && test_bit(MT76_STATE_RUNNING, &phy->mt76->state);
 }
 
+static void mt7996_testmode_disable_all(struct mt7996_dev *dev)
+{
+	struct mt7996_phy *phy;
+	int i;
+
+	for (i = 0; i < __MT_MAX_BAND; i++) {
+		phy = __mt7996_phy(dev, i);
+		if (phy)
+			mt76_testmode_set_state(phy->mt76, MT76_TM_STATE_OFF);
+	}
+}
+
 int mt7996_run(struct ieee80211_hw *hw)
 {
 	struct mt7996_dev *dev = mt7996_hw_dev(hw);
@@ -37,6 +49,8 @@ int mt7996_run(struct ieee80211_hw *hw)
 			goto out;
 	}
 
+	mt7996_testmode_disable_all(dev);
+
 	mt7996_mac_enable_nf(dev, phy->mt76->band_idx);
 
 	ret = mt7996_mcu_set_rts_thresh(phy, 0x92b);
@@ -1478,6 +1492,8 @@ const struct ieee80211_ops mt7996_ops = {
 	.sta_set_decap_offload = mt7996_sta_set_decap_offload,
 	.add_twt_setup = mt7996_mac_add_twt_setup,
 	.twt_teardown_request = mt7996_twt_teardown_request,
+	CFG80211_TESTMODE_CMD(mt76_testmode_cmd)
+	CFG80211_TESTMODE_DUMP(mt76_testmode_dump)
 #ifdef CONFIG_MAC80211_DEBUGFS
 	.sta_add_debugfs = mt7996_sta_add_debugfs,
 #endif
diff --git a/mt7996/mcu.c b/mt7996/mcu.c
index 7d0c511..141b838 100644
--- a/mt7996/mcu.c
+++ b/mt7996/mcu.c
@@ -2681,8 +2681,12 @@ static int mt7996_load_ram(struct mt7996_dev *dev)
 {
 	int ret;
 
-	ret = __mt7996_load_ram(dev, "WM", MT7996_FIRMWARE_WM,
-				MT7996_RAM_TYPE_WM);
+	if (dev->testmode_enable)
+		ret = __mt7996_load_ram(dev, "WM_TM", MT7996_FIRMWARE_WM_TM,
+					MT7996_RAM_TYPE_WM_TM);
+	else
+		ret = __mt7996_load_ram(dev, "WM", MT7996_FIRMWARE_WM,
+					MT7996_RAM_TYPE_WM);
 	if (ret)
 		return ret;
 
@@ -4308,3 +4312,37 @@ int mt7996_mcu_set_pp_en(struct mt7996_phy *phy, bool auto_mode,
 	return mt76_mcu_send_msg(&dev->mt76, MCU_WM_UNI_CMD(PP),
 				 &req, sizeof(req), false);
 }
+
+int mt7996_mcu_set_tx_power_ctrl(struct mt7996_phy *phy, u8 power_ctrl_id, u8 data)
+{
+	struct mt7996_dev *dev = phy->dev;
+	struct tx_power_ctrl req = {
+		.tag = cpu_to_le16(power_ctrl_id),
+		.len = cpu_to_le16(sizeof(req) - 4),
+		.power_ctrl_id = power_ctrl_id,
+		.band_idx = phy->mt76->band_idx,
+	};
+
+	switch (power_ctrl_id) {
+	case UNI_TXPOWER_SKU_POWER_LIMIT_CTRL:
+		req.sku_enable = !!data;
+		break;
+	case UNI_TXPOWER_PERCENTAGE_CTRL:
+		req.percentage_ctrl_enable = !!data;
+		break;
+	case UNI_TXPOWER_PERCENTAGE_DROP_CTRL:
+		req.power_drop_level = data;
+		break;
+	case UNI_TXPOWER_BACKOFF_POWER_LIMIT_CTRL:
+		req.bf_backoff_enable = !!data;
+		break;
+	case UNI_TXPOWER_ATE_MODE_CTRL:
+		req.ate_mode_enable = !!data;
+		break;
+	default:
+		req.sku_enable = !!data;
+	}
+
+	return mt76_mcu_send_msg(&dev->mt76, MCU_WM_UNI_CMD(TXPOWER),
+				 &req, sizeof(req), false);
+}
diff --git a/mt7996/mcu.h b/mt7996/mcu.h
index ca16336..16ffc3d 100644
--- a/mt7996/mcu.h
+++ b/mt7996/mcu.h
@@ -814,6 +814,33 @@ enum {
 	UNI_CMD_THERMAL_PROTECT_DUTY_CONFIG,
 };
 
+struct tx_power_ctrl {
+	u8 _rsv[4];
+
+	__le16 tag;
+	__le16 len;
+
+	u8 power_ctrl_id;
+	union {
+		bool sku_enable;
+		bool ate_mode_enable;
+		bool percentage_ctrl_enable;
+		bool bf_backoff_enable;
+		u8 power_drop_level;
+	};
+	u8 band_idx;
+	u8 rsv[1];
+} __packed;
+
+enum {
+	UNI_TXPOWER_SKU_POWER_LIMIT_CTRL = 0,
+	UNI_TXPOWER_PERCENTAGE_CTRL = 1,
+	UNI_TXPOWER_PERCENTAGE_DROP_CTRL = 2,
+	UNI_TXPOWER_BACKOFF_POWER_LIMIT_CTRL = 3,
+	UNI_TXPOWER_POWER_LIMIT_TABLE_CTRL = 4,
+	UNI_TXPOWER_ATE_MODE_CTRL = 6,
+};
+
 enum {
 	UNI_CMD_ACCESS_REG_BASIC = 0x0,
 	UNI_CMD_ACCESS_RF_REG_BASIC,
diff --git a/mt7996/mt7996.h b/mt7996/mt7996.h
index 8801956..3964035 100644
--- a/mt7996/mt7996.h
+++ b/mt7996/mt7996.h
@@ -32,9 +32,11 @@
 #define MT7996_FIRMWARE_WA		"mediatek/mt7996/mt7996_wa.bin"
 #define MT7996_FIRMWARE_WM		"mediatek/mt7996/mt7996_wm.bin"
 #define MT7996_FIRMWARE_DSP		"mediatek/mt7996/mt7996_dsp.bin"
+#define MT7996_FIRMWARE_WM_TM		"mediatek/mt7996/mt7996_wm_tm.bin"
 #define MT7996_ROM_PATCH		"mediatek/mt7996/mt7996_rom_patch.bin"
 
 #define MT7996_EEPROM_DEFAULT		"mediatek/mt7996/mt7996_eeprom.bin"
+#define MT7996_EEPROM_DEFAULT_TM	"mediatek/mt7996/mt7996_eeprom_tm.bin"
 #define MT7996_EEPROM_SIZE		7680
 #define MT7996_EEPROM_BLOCK_SIZE	16
 #define MT7996_TOKEN_SIZE		16384
@@ -83,6 +85,7 @@ struct mt7996_dfs_pattern;
 
 enum mt7996_ram_type {
 	MT7996_RAM_TYPE_WM,
+	MT7996_RAM_TYPE_WM_TM = MT7996_RAM_TYPE_WM,
 	MT7996_RAM_TYPE_WA,
 	MT7996_RAM_TYPE_DSP,
 	__MT7996_RAM_TYPE_MAX,
@@ -225,6 +228,21 @@ struct mt7996_phy {
 	struct mt76_channel_state state_ts;
 
 	bool has_aux_rx;
+
+#ifdef CONFIG_NL80211_TESTMODE
+	struct {
+		u32 *reg_backup;
+
+		s32 last_freq_offset;
+		u8 last_rcpi[4];
+		s8 last_rssi[4];
+		s8 last_ib_rssi[4];
+		s8 last_wb_rssi[4];
+		u8 last_snr;
+
+		u8 spe_idx;
+	} test;
+#endif
 };
 
 struct mt7996_dev {
@@ -300,6 +318,8 @@ struct mt7996_dev {
 		} session;
 	} wed_rro;
 
+	bool testmode_enable;
+
 	bool ibf;
 	u8 fw_debug_wm;
 	u8 fw_debug_wa;
@@ -414,6 +434,7 @@ mt7996_band_valid(struct mt7996_dev *dev, u8 band)
 extern const struct ieee80211_ops mt7996_ops;
 extern struct pci_driver mt7996_pci_driver;
 extern struct pci_driver mt7996_hif_driver;
+extern const struct mt76_testmode_ops mt7996_testmode_ops;
 
 struct mt7996_dev *mt7996_mmio_probe(struct device *pdev,
 				     void __iomem *mem_base, u32 device_id);
@@ -423,6 +444,7 @@ u64 __mt7996_get_tsf(struct ieee80211_hw *hw, struct mt7996_vif *mvif);
 int mt7996_register_device(struct mt7996_dev *dev);
 void mt7996_unregister_device(struct mt7996_dev *dev);
 int mt7996_eeprom_init(struct mt7996_dev *dev);
+int mt7996_eeprom_check_fw_mode(struct mt7996_dev *dev);
 int mt7996_eeprom_parse_hw_cap(struct mt7996_dev *dev, struct mt7996_phy *phy);
 int mt7996_eeprom_get_target_power(struct mt7996_dev *dev,
 				   struct ieee80211_channel *chan);
@@ -508,6 +530,7 @@ int mt7996_mcu_trigger_assert(struct mt7996_dev *dev);
 void mt7996_mcu_rx_event(struct mt7996_dev *dev, struct sk_buff *skb);
 void mt7996_mcu_exit(struct mt7996_dev *dev);
 int mt7996_mcu_get_all_sta_info(struct mt7996_phy *phy, u16 tag);
+int mt7996_mcu_set_tx_power_ctrl(struct mt7996_phy *phy, u8 power_ctrl_id, u8 data);
 
 static inline u8 mt7996_max_interface_num(struct mt7996_dev *dev)
 {
diff --git a/mt7996/testmode.c b/mt7996/testmode.c
new file mode 100644
index 0000000..fb041c3
--- /dev/null
+++ b/mt7996/testmode.c
@@ -0,0 +1,674 @@
+// SPDX-License-Identifier: ISC
+/*
+ * Copyright (C) 2022 MediaTek Inc.
+ */
+
+#include "mt7996.h"
+#include "mac.h"
+#include "mcu.h"
+#include "testmode.h"
+
+enum {
+	TM_CHANGED_TXPOWER,
+	TM_CHANGED_FREQ_OFFSET,
+	TM_CHANGED_SKU_EN,
+	TM_CHANGED_TX_LENGTH,
+	TM_CHANGED_TX_TIME,
+	TM_CHANGED_CFG,
+
+	/* must be last */
+	NUM_TM_CHANGED
+};
+
+static const u8 tm_change_map[] = {
+	[TM_CHANGED_TXPOWER] = MT76_TM_ATTR_TX_POWER,
+	[TM_CHANGED_FREQ_OFFSET] = MT76_TM_ATTR_FREQ_OFFSET,
+	[TM_CHANGED_SKU_EN] = MT76_TM_ATTR_SKU_EN,
+	[TM_CHANGED_TX_LENGTH] = MT76_TM_ATTR_TX_LENGTH,
+	[TM_CHANGED_TX_TIME] = MT76_TM_ATTR_TX_TIME,
+	[TM_CHANGED_CFG] = MT76_TM_ATTR_CFG,
+};
+
+static u8 mt7996_tm_bw_mapping(enum nl80211_chan_width width, enum bw_mapping_method method)
+{
+	static const u8 width_to_bw[][NUM_BW_MAP] = {
+		[NL80211_CHAN_WIDTH_40] = {FW_CDBW_40MHZ, TM_CBW_40MHZ},
+		[NL80211_CHAN_WIDTH_80] = {FW_CDBW_80MHZ, TM_CBW_80MHZ},
+		[NL80211_CHAN_WIDTH_80P80] = {FW_CDBW_8080MHZ, TM_CBW_8080MHZ},
+		[NL80211_CHAN_WIDTH_160] = {FW_CDBW_160MHZ, TM_CBW_160MHZ},
+		[NL80211_CHAN_WIDTH_5] = {FW_CDBW_5MHZ, TM_CBW_5MHZ},
+		[NL80211_CHAN_WIDTH_10] = {FW_CDBW_10MHZ, TM_CBW_10MHZ},
+		[NL80211_CHAN_WIDTH_20] = {FW_CDBW_20MHZ, TM_CBW_20MHZ},
+		[NL80211_CHAN_WIDTH_20_NOHT] = {FW_CDBW_20MHZ, TM_CBW_20MHZ},
+		[NL80211_CHAN_WIDTH_320] = {FW_CDBW_320MHZ, TM_CBW_320MHZ},
+	};
+
+	if (width >= ARRAY_SIZE(width_to_bw))
+		return 0;
+
+	return width_to_bw[width][method];
+}
+
+static u8 mt7996_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,
+		[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,
+	};
+
+	if (tx_rate_mode > MT76_TM_TX_MODE_MAX)
+		return -EINVAL;
+
+	return rate_to_phy[tx_rate_mode];
+}
+
+static int
+mt7996_tm_check_antenna(struct mt7996_phy *phy)
+{
+	struct mt76_testmode_data *td = &phy->mt76->test;
+	struct mt7996_dev *dev = phy->dev;
+	u8 band_idx = phy->mt76->band_idx;
+	u32 chainmask = phy->mt76->chainmask;
+	u32 aux_rx_mask;
+
+	chainmask = chainmask >> dev->chainshift[band_idx];
+	aux_rx_mask = BIT(fls(chainmask)) * phy->has_aux_rx;
+	if (td->tx_antenna_mask & ~(chainmask | aux_rx_mask)) {
+		dev_err(dev->mt76.dev,
+			"tx antenna mask 0x%x exceeds hw limit (chainmask 0x%x, has aux rx: %s)\n",
+			td->tx_antenna_mask, chainmask, phy->has_aux_rx ? "yes" : "no");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int
+mt7996_tm_set(struct mt7996_dev *dev, u32 func_idx, u32 data)
+{
+	struct mt7996_tm_req req = {
+		.rf_test = {
+			.tag = cpu_to_le16(UNI_RF_TEST_CTRL),
+			.len = cpu_to_le16(sizeof(req.rf_test)),
+			.action = RF_ACTION_SET,
+			.op.rf.func_idx = func_idx,
+			.op.rf.param.func_data = cpu_to_le32(data),
+		},
+	};
+
+	return mt76_mcu_send_msg(&dev->mt76, MCU_WM_UNI_CMD(TESTMODE_CTRL), &req,
+				 sizeof(req), false);
+}
+
+static int
+mt7996_tm_get(struct mt7996_dev *dev, u32 func_idx, u32 data, u32 *result)
+{
+	struct mt7996_tm_req req = {
+		.rf_test = {
+			.tag = cpu_to_le16(UNI_RF_TEST_CTRL),
+			.len = cpu_to_le16(sizeof(req.rf_test)),
+			.action = RF_ACTION_GET,
+			.op.rf.func_idx = func_idx,
+			.op.rf.param.func_data = cpu_to_le32(data),
+		},
+	};
+	struct mt7996_tm_event *event;
+	struct sk_buff *skb;
+	int ret;
+
+	ret = mt76_mcu_send_and_get_msg(&dev->mt76, MCU_WM_UNI_CMD_QUERY(TESTMODE_CTRL),
+					&req, sizeof(req), true, &skb);
+	if (ret)
+		return ret;
+
+	event = (struct mt7996_tm_event *)skb->data;
+	*result = event->result.payload_length;
+
+	dev_kfree_skb(skb);
+
+	return ret;
+}
+
+static void
+mt7996_tm_set_antenna(struct mt7996_phy *phy, u32 func_idx)
+{
+#define SPE_INDEX_MASK		BIT(31)
+#define TX_ANTENNA_MASK		GENMASK(3, 0)
+#define RX_ANTENNA_MASK		GENMASK(20, 16)		/* RX antenna mask at most 5 bit */
+	struct mt7996_dev *dev = phy->dev;
+	struct mt76_testmode_data *td = &phy->mt76->test;
+	u32 antenna_mask;
+
+	if (!mt76_testmode_param_present(td, MT76_TM_ATTR_TX_ANTENNA))
+		return;
+
+	if (func_idx == SET_ID(TX_PATH))
+		antenna_mask = td->tx_spe_idx ? (SPE_INDEX_MASK | td->tx_spe_idx) :
+						td->tx_antenna_mask & TX_ANTENNA_MASK;
+	else if (func_idx == SET_ID(RX_PATH))
+		antenna_mask = u32_encode_bits(td->tx_antenna_mask, RX_ANTENNA_MASK);
+	else
+		return;
+
+	mt7996_tm_set(dev, func_idx, antenna_mask);
+}
+
+static void
+mt7996_tm_set_mac_addr(struct mt7996_dev *dev, u8 *addr, u32 func_idx)
+{
+#define REMAIN_PART_TAG		BIT(18)
+	u32 own_mac_first = 0, own_mac_remain = 0;
+	int len = sizeof(u32);
+
+	memcpy(&own_mac_first, addr, len);
+	mt7996_tm_set(dev, func_idx, own_mac_first);
+	/* Set the remain part of mac address */
+	memcpy(&own_mac_remain, addr + len, ETH_ALEN - len);
+	mt7996_tm_set(dev, func_idx | REMAIN_PART_TAG, own_mac_remain);
+}
+
+static int
+mt7996_tm_rf_switch_mode(struct mt7996_dev *dev, u32 op_mode)
+{
+	struct mt7996_tm_req req = {
+		.rf_test = {
+			.tag = cpu_to_le16(UNI_RF_TEST_CTRL),
+			.len = cpu_to_le16(sizeof(req.rf_test)),
+			.action = RF_ACTION_SWITCH_TO_RF_TEST,
+			.op.op_mode = cpu_to_le32(op_mode),
+		},
+	};
+
+	return mt76_mcu_send_msg(&dev->mt76, MCU_WM_UNI_CMD(TESTMODE_CTRL), &req,
+				 sizeof(req), false);
+}
+
+static void
+mt7996_tm_init(struct mt7996_phy *phy, bool en)
+{
+	struct mt7996_dev *dev = phy->dev;
+	u8 rf_test_mode = en ? RF_OPER_RF_TEST : RF_OPER_NORMAL;
+
+	if (!test_bit(MT76_STATE_RUNNING, &phy->mt76->state))
+		return;
+
+	mt7996_mcu_set_tx_power_ctrl(phy, POWER_CTRL(ATE_MODE), en);
+	mt7996_mcu_set_tx_power_ctrl(phy, POWER_CTRL(SKU_POWER_LIMIT), !en);
+	mt7996_mcu_set_tx_power_ctrl(phy, POWER_CTRL(BACKOFF_POWER_LIMIT), !en);
+
+	mt7996_tm_rf_switch_mode(dev, rf_test_mode);
+
+	mt7996_mcu_add_bss_info(phy, phy->monitor_vif, en);
+	mt7996_mcu_add_sta(dev, phy->monitor_vif, NULL, en);
+
+	mt7996_tm_set(dev, SET_ID(BAND_IDX), phy->mt76->band_idx);
+
+	/* use firmware counter for RX stats */
+	phy->mt76->test.flag |= MT_TM_FW_RX_COUNT;
+}
+
+static void
+mt7996_tm_update_channel(struct mt7996_phy *phy)
+{
+#define CHAN_FREQ_BW_80P80_TAG		(SET_ID(CHAN_FREQ) | BIT(16))
+	struct mt7996_dev *dev = phy->dev;
+	struct cfg80211_chan_def *chandef = &phy->mt76->chandef;
+	struct ieee80211_channel *chan = chandef->chan;
+	u8 width = chandef->width;
+	static const u8 ch_band[] = {
+		[NL80211_BAND_2GHZ] = 0,
+		[NL80211_BAND_5GHZ] = 1,
+		[NL80211_BAND_6GHZ] = 2,
+	};
+
+	if (!chan || !chandef) {
+		dev_info(dev->mt76.dev, "chandef not found, channel update failed!\n");
+		return;
+	}
+
+	/* system bw */
+	mt7996_tm_set(dev, SET_ID(CBW), mt7996_tm_bw_mapping(width, BW_MAP_NL_TO_FW));
+
+	if (width == NL80211_CHAN_WIDTH_80P80) {
+		width = NL80211_CHAN_WIDTH_160;
+		mt7996_tm_set(dev, CHAN_FREQ_BW_80P80_TAG, chandef->center_freq2 * 1000);
+	}
+
+	/* TODO: define per-packet bw */
+	/* per-packet bw */
+	mt7996_tm_set(dev, SET_ID(DBW), mt7996_tm_bw_mapping(width, BW_MAP_NL_TO_FW));
+
+	/* control channel selection index */
+	mt7996_tm_set(dev, SET_ID(PRIMARY_CH), 0);
+	mt7996_tm_set(dev, SET_ID(BAND), ch_band[chan->band]);
+
+	/* trigger switch channel calibration */
+	mt7996_tm_set(dev, SET_ID(CHAN_FREQ), chandef->center_freq1 * 1000);
+
+	// TODO: update power limit table
+}
+
+static void
+mt7996_tm_tx_stop(struct mt76_phy *mphy)
+{
+	struct mt76_testmode_data *td = &mphy->test;
+	struct mt7996_phy *phy = mphy->priv;
+	struct mt7996_dev *dev = phy->dev;
+
+	mt7996_tm_set(dev, SET_ID(COMMAND), RF_CMD(STOP_TEST));
+	td->tx_pending = 0;
+}
+
+static void
+mt7996_tm_set_tx_frames(struct mt7996_phy *phy, bool en)
+{
+#define FRAME_CONTROL		0x88
+	struct mt76_testmode_data *td = &phy->mt76->test;
+	struct mt7996_dev *dev = phy->dev;
+
+	//TODO: RU operation, replace mcs, nss, and ldpc
+	if (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_RATE), td->tx_rate_idx);
+
+		if (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_POWER))
+			mt7996_tm_set(dev, SET_ID(POWER), td->tx_power[0]);
+
+		if (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_TIME)) {
+			mt7996_tm_set(dev, SET_ID(TX_LEN), 0);
+			mt7996_tm_set(dev, SET_ID(TX_TIME), td->tx_time);
+		} else {
+			mt7996_tm_set(dev, SET_ID(TX_LEN), td->tx_mpdu_len);
+			mt7996_tm_set(dev, SET_ID(TX_TIME), 0);
+		}
+
+		mt7996_tm_set_antenna(phy, SET_ID(TX_PATH));
+		mt7996_tm_set_antenna(phy, SET_ID(RX_PATH));
+		mt7996_tm_set(dev, SET_ID(STBC), td->tx_rate_stbc);
+		mt7996_tm_set(dev, SET_ID(ENCODE_MODE), td->tx_rate_ldpc);
+		mt7996_tm_set(dev, SET_ID(IBF_ENABLE), td->ibf);
+		mt7996_tm_set(dev, SET_ID(EBF_ENABLE), td->ebf);
+		mt7996_tm_set(dev, SET_ID(IPG), td->tx_ipg);
+		mt7996_tm_set(dev, SET_ID(GI), td->tx_rate_sgi);
+		mt7996_tm_set(dev, SET_ID(NSS), td->tx_rate_nss);
+		mt7996_tm_set(dev, SET_ID(AID_OFFSET), 0);
+		mt7996_tm_set(dev, SET_ID(PUNCTURE), td->tx_preamble_puncture);
+
+		mt7996_tm_set(dev, SET_ID(MAX_PE), 2);
+		mt7996_tm_set(dev, SET_ID(HW_TX_MODE), 0);
+		mt7996_tm_update_channel(phy);
+
+		/* trigger firmware to start TX */
+		mt7996_tm_set(dev, SET_ID(COMMAND), RF_CMD(START_TX));
+	} else {
+		mt7996_tm_tx_stop(phy->mt76);
+	}
+}
+
+static int
+mt7996_tm_rx_stats_user_ctrl(struct mt7996_phy *phy, u16 user_idx)
+{
+	struct mt7996_dev *dev = phy->dev;
+	struct mt7996_tm_rx_req req = {
+		.band = phy->mt76->band_idx,
+		.user_ctrl = {
+			.tag = cpu_to_le16(UNI_TM_RX_STAT_SET_USER_CTRL),
+			.len = cpu_to_le16(sizeof(req.user_ctrl)),
+			.band_idx = phy->mt76->band_idx,
+			.user_idx = cpu_to_le16(user_idx),
+		},
+	};
+
+	return mt76_mcu_send_msg(&dev->mt76, MCU_WM_UNI_CMD(TESTMODE_RX_STAT), &req,
+				 sizeof(req), false);
+}
+
+static void
+mt7996_tm_set_rx_frames(struct mt7996_phy *phy, bool en)
+{
+#define RX_MU_DISABLE	0xf800
+	struct mt76_testmode_data *td = &phy->mt76->test;
+	struct mt7996_dev *dev = phy->dev;
+	int ret;
+
+	if (en) {
+		ret = mt7996_tm_rx_stats_user_ctrl(phy, td->aid);
+		if (ret) {
+			dev_info(dev->mt76.dev, "Set RX stats user control failed!\n");
+			return;
+		}
+
+		mt7996_tm_update_channel(phy);
+
+		if (td->tx_rate_mode >= MT76_TM_TX_MODE_HE_MU) {
+			if (td->aid)
+				ret = mt7996_tm_set(dev, SET_ID(RX_MU_AID), td->aid);
+			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(GI), td->tx_rate_sgi);
+		mt7996_tm_set_antenna(phy, SET_ID(RX_PATH));
+		mt7996_tm_set(dev, SET_ID(MAX_PE), 2);
+
+		mt7996_tm_set_mac_addr(dev, td->addr[1], SET_ID(SA));
+
+		/* trigger firmware to start RX */
+		mt7996_tm_set(dev, SET_ID(COMMAND), RF_CMD(START_RX));
+	} else {
+		/* trigger firmware to stop RX */
+		mt7996_tm_set(dev, SET_ID(COMMAND), RF_CMD(STOP_TEST));
+	}
+}
+
+static void
+mt7996_tm_set_tx_cont(struct mt7996_phy *phy, bool en)
+{
+#define CONT_WAVE_MODE_OFDM	3
+	struct mt76_testmode_data *td = &phy->mt76->test;
+	struct mt7996_dev *dev = phy->dev;
+
+	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_RATE), td->tx_rate_idx);
+		/* fix payload is OFDM */
+		mt7996_tm_set(dev, SET_ID(CONT_WAVE_MODE), CONT_WAVE_MODE_OFDM);
+		mt7996_tm_set(dev, SET_ID(ANT_MASK), td->tx_antenna_mask);
+
+		/* trigger firmware to start CONT TX */
+		mt7996_tm_set(dev, SET_ID(COMMAND), RF_CMD(CONT_WAVE));
+	} else {
+		/* trigger firmware to stop CONT TX  */
+		mt7996_tm_set(dev, SET_ID(COMMAND), RF_CMD(STOP_TEST));
+	}
+}
+
+static void
+mt7996_tm_update_params(struct mt7996_phy *phy, u32 changed)
+{
+	struct mt76_testmode_data *td = &phy->mt76->test;
+	struct mt7996_dev *dev = phy->dev;
+	bool en = td->state != MT76_TM_STATE_OFF;
+
+	if (changed & BIT(TM_CHANGED_FREQ_OFFSET))
+		mt7996_tm_set(dev, SET_ID(FREQ_OFFSET), en ? td->freq_offset : 0);
+	if (changed & BIT(TM_CHANGED_TXPOWER))
+		mt7996_tm_set(dev, SET_ID(POWER), td->tx_power[0]);
+	if (changed & BIT(TM_CHANGED_SKU_EN)) {
+		mt7996_tm_update_channel(phy);
+		mt7996_mcu_set_tx_power_ctrl(phy, POWER_CTRL(SKU_POWER_LIMIT), td->sku_en);
+		mt7996_mcu_set_tx_power_ctrl(phy, POWER_CTRL(BACKOFF_POWER_LIMIT), td->sku_en);
+		mt7996_mcu_set_txpower_sku(phy);
+	}
+	if (changed & BIT(TM_CHANGED_TX_LENGTH)) {
+		mt7996_tm_set(dev, SET_ID(TX_LEN), td->tx_mpdu_len);
+		mt7996_tm_set(dev, SET_ID(TX_TIME), 0);
+	}
+	if (changed & BIT(TM_CHANGED_TX_TIME)) {
+		mt7996_tm_set(dev, SET_ID(TX_LEN), 0);
+		mt7996_tm_set(dev, SET_ID(TX_TIME), td->tx_time);
+	}
+	if (changed & BIT(TM_CHANGED_CFG)) {
+		u32 func_idx = td->cfg.enable ? SET_ID(CFG_ON) : SET_ID(CFG_OFF);
+
+		mt7996_tm_set(dev, func_idx, td->cfg.type);
+	}
+}
+
+static int
+mt7996_tm_set_state(struct mt76_phy *mphy, enum mt76_testmode_state state)
+{
+	struct mt76_testmode_data *td = &mphy->test;
+	struct mt7996_phy *phy = mphy->priv;
+	enum mt76_testmode_state prev_state = td->state;
+
+	mphy->test.state = state;
+
+	if (prev_state != MT76_TM_STATE_OFF)
+		mt7996_tm_set(phy->dev, SET_ID(BAND_IDX), mphy->band_idx);
+
+	if (prev_state == MT76_TM_STATE_TX_FRAMES ||
+	    state == MT76_TM_STATE_TX_FRAMES)
+		mt7996_tm_set_tx_frames(phy, state == MT76_TM_STATE_TX_FRAMES);
+	else if (prev_state == MT76_TM_STATE_RX_FRAMES ||
+		 state == MT76_TM_STATE_RX_FRAMES)
+		mt7996_tm_set_rx_frames(phy, state == MT76_TM_STATE_RX_FRAMES);
+	else if (prev_state == MT76_TM_STATE_TX_CONT ||
+		 state == MT76_TM_STATE_TX_CONT)
+		mt7996_tm_set_tx_cont(phy, state == MT76_TM_STATE_TX_CONT);
+	else if (prev_state == MT76_TM_STATE_OFF ||
+		 state == MT76_TM_STATE_OFF)
+		mt7996_tm_init(phy, !(state == MT76_TM_STATE_OFF));
+
+	if ((state == MT76_TM_STATE_IDLE &&
+	     prev_state == MT76_TM_STATE_OFF) ||
+	    (state == MT76_TM_STATE_OFF &&
+	     prev_state == MT76_TM_STATE_IDLE)) {
+		u32 changed = 0;
+		int i, ret;
+
+		for (i = 0; i < ARRAY_SIZE(tm_change_map); i++) {
+			u16 cur = tm_change_map[i];
+
+			if (mt76_testmode_param_present(td, cur))
+				changed |= BIT(i);
+		}
+
+		ret = mt7996_tm_check_antenna(phy);
+		if (ret)
+			return ret;
+
+		mt7996_tm_update_params(phy, changed);
+	}
+
+	return 0;
+}
+
+static int
+mt7996_tm_set_params(struct mt76_phy *mphy, struct nlattr **tb,
+		     enum mt76_testmode_state new_state)
+{
+	struct mt76_testmode_data *td = &mphy->test;
+	struct mt7996_phy *phy = mphy->priv;
+	struct mt7996_dev *dev = phy->dev;
+	u32 changed = 0;
+	int i, ret;
+
+	BUILD_BUG_ON(NUM_TM_CHANGED >= 32);
+
+	if (new_state == MT76_TM_STATE_OFF ||
+	    td->state == MT76_TM_STATE_OFF)
+		return 0;
+
+	ret = mt7996_tm_check_antenna(phy);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < ARRAY_SIZE(tm_change_map); i++) {
+		if (tb[tm_change_map[i]])
+			changed |= BIT(i);
+	}
+
+	mt7996_tm_set(dev, SET_ID(BAND_IDX), mphy->band_idx);
+	mt7996_tm_update_params(phy, changed);
+
+	return 0;
+}
+
+static int
+mt7996_tm_get_rx_stats(struct mt7996_phy *phy)
+{
+	struct mt7996_dev *dev = phy->dev;
+	struct mt7996_tm_rx_req req = {
+		.band = phy->mt76->band_idx,
+		.rx_stat_all = {
+			.tag = cpu_to_le16(UNI_TM_RX_STAT_GET_ALL_V2),
+			.len = cpu_to_le16(sizeof(req.rx_stat_all)),
+			.band_idx = phy->mt76->band_idx,
+		},
+	};
+	struct mt76_testmode_data *td = &phy->mt76->test;
+	struct mt7996_tm_rx_event *rx_stats;
+	struct mt7996_tm_rx_event_stat_all *rx_stats_all;
+	struct sk_buff *skb;
+	enum mt76_rxq_id qid;
+	int i, ret = 0;
+	u32 mac_rx_mdrdy_cnt;
+	u16 mac_rx_len_mismatch, fcs_err_count;
+
+	if (td->state != MT76_TM_STATE_RX_FRAMES)
+		return 0;
+
+	ret = mt76_mcu_send_and_get_msg(&dev->mt76, MCU_WM_UNI_CMD_QUERY(TESTMODE_RX_STAT),
+					&req, sizeof(req), true, &skb);
+
+	if (ret)
+		return ret;
+
+	rx_stats = (struct mt7996_tm_rx_event *)skb->data;
+	rx_stats_all = &rx_stats->rx_stat_all;
+
+	phy->test.last_freq_offset = le32_to_cpu(rx_stats_all->user_info[0].freq_offset);
+	phy->test.last_snr = le32_to_cpu(rx_stats_all->user_info[0].snr);
+	for (i = 0; i < ARRAY_SIZE(phy->test.last_rcpi); i++) {
+		phy->test.last_rcpi[i] = le16_to_cpu(rx_stats_all->rxv_info[i].rcpi);
+		phy->test.last_rssi[i] = le16_to_cpu(rx_stats_all->rxv_info[i].rssi);
+		phy->test.last_ib_rssi[i] = rx_stats_all->fagc[i].ib_rssi;
+		phy->test.last_wb_rssi[i] = rx_stats_all->fagc[i].wb_rssi;
+	}
+
+	if (phy->mt76->band_idx == 2)
+		qid = MT_RXQ_BAND2;
+	else if (phy->mt76->band_idx == 1)
+		qid = MT_RXQ_BAND1;
+	else
+		qid = MT_RXQ_MAIN;
+
+	fcs_err_count = le16_to_cpu(rx_stats_all->band_info.mac_rx_fcs_err_cnt);
+	mac_rx_len_mismatch = le16_to_cpu(rx_stats_all->band_info.mac_rx_len_mismatch);
+	mac_rx_mdrdy_cnt = le32_to_cpu(rx_stats_all->band_info.mac_rx_mdrdy_cnt);
+	td->rx_stats.packets[qid] += mac_rx_mdrdy_cnt;
+	td->rx_stats.packets[qid] += fcs_err_count;
+	td->rx_stats.fcs_error[qid] += fcs_err_count;
+	td->rx_stats.len_mismatch += mac_rx_len_mismatch;
+
+	dev_kfree_skb(skb);
+
+	return ret;
+}
+
+static void
+mt7996_tm_reset_trx_stats(struct mt76_phy *mphy)
+{
+	struct mt7996_phy *phy = mphy->priv;
+	struct mt7996_dev *dev = phy->dev;
+
+	memset(&mphy->test.rx_stats, 0, sizeof(mphy->test.rx_stats));
+	mt7996_tm_set(dev, SET_ID(TRX_COUNTER_RESET), 0);
+}
+
+static int
+mt7996_tm_get_tx_stats(struct mt7996_phy *phy)
+{
+	struct mt7996_dev *dev = phy->dev;
+	struct mt76_testmode_data *td = &phy->mt76->test;
+	int ret;
+
+	if (td->state != MT76_TM_STATE_TX_FRAMES)
+		return 0;
+
+	ret = mt7996_tm_get(dev, GET_ID(TXED_COUNT), 0, &td->tx_done);
+	if (ret)
+		return ret;
+
+	td->tx_pending = td->tx_count - td->tx_done;
+
+	return ret;
+}
+
+static int
+mt7996_tm_dump_stats(struct mt76_phy *mphy, struct sk_buff *msg)
+{
+	struct mt7996_phy *phy = mphy->priv;
+	void *rx, *rssi;
+	int i;
+
+	mt7996_tm_set(phy->dev, SET_ID(BAND_IDX), mphy->band_idx);
+	mt7996_tm_get_rx_stats(phy);
+	mt7996_tm_get_tx_stats(phy);
+
+	rx = nla_nest_start(msg, MT76_TM_STATS_ATTR_LAST_RX);
+	if (!rx)
+		return -ENOMEM;
+
+	if (nla_put_s32(msg, MT76_TM_RX_ATTR_FREQ_OFFSET, phy->test.last_freq_offset))
+		return -ENOMEM;
+
+	rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_RCPI);
+	if (!rssi)
+		return -ENOMEM;
+
+	for (i = 0; i < ARRAY_SIZE(phy->test.last_rcpi); i++)
+		if (nla_put_u8(msg, i, phy->test.last_rcpi[i]))
+			return -ENOMEM;
+
+	nla_nest_end(msg, rssi);
+
+	rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_RSSI);
+	if (!rssi)
+		return -ENOMEM;
+
+	for (i = 0; i < ARRAY_SIZE(phy->test.last_rssi); i++)
+		if (nla_put_s8(msg, i, phy->test.last_rssi[i]))
+			return -ENOMEM;
+
+	nla_nest_end(msg, rssi);
+
+	rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_IB_RSSI);
+	if (!rssi)
+		return -ENOMEM;
+
+	for (i = 0; i < ARRAY_SIZE(phy->test.last_ib_rssi); i++)
+		if (nla_put_s8(msg, i, phy->test.last_ib_rssi[i]))
+			return -ENOMEM;
+
+	nla_nest_end(msg, rssi);
+
+	rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_WB_RSSI);
+	if (!rssi)
+		return -ENOMEM;
+
+	for (i = 0; i < ARRAY_SIZE(phy->test.last_wb_rssi); i++)
+		if (nla_put_s8(msg, i, phy->test.last_wb_rssi[i]))
+			return -ENOMEM;
+
+	nla_nest_end(msg, rssi);
+
+	if (nla_put_u8(msg, MT76_TM_RX_ATTR_SNR, phy->test.last_snr))
+		return -ENOMEM;
+
+	nla_nest_end(msg, rx);
+
+	return 0;
+}
+
+const struct mt76_testmode_ops mt7996_testmode_ops = {
+	.set_state = mt7996_tm_set_state,
+	.set_params = mt7996_tm_set_params,
+	.dump_stats = mt7996_tm_dump_stats,
+	.reset_rx_stats = mt7996_tm_reset_trx_stats,
+	.tx_stop = mt7996_tm_tx_stop,
+};
diff --git a/mt7996/testmode.h b/mt7996/testmode.h
new file mode 100644
index 0000000..e4d55a6
--- /dev/null
+++ b/mt7996/testmode.h
@@ -0,0 +1,297 @@
+/* SPDX-License-Identifier: ISC */
+/* Copyright (C) 2020 MediaTek Inc. */
+
+#ifndef __MT7996_TESTMODE_H
+#define __MT7996_TESTMODE_H
+
+enum {
+	TM_CBW_20MHZ,
+	TM_CBW_40MHZ,
+	TM_CBW_80MHZ,
+	TM_CBW_10MHZ,
+	TM_CBW_5MHZ,
+	TM_CBW_160MHZ,
+	TM_CBW_8080MHZ,
+	TM_CBW_320MHZ = 12,
+};
+
+/* BW defined in FW hal_cal_flow_rom.h */
+enum {
+	FW_CDBW_20MHZ,
+	FW_CDBW_40MHZ,
+	FW_CDBW_80MHZ,
+	FW_CDBW_160MHZ,
+	FW_CDBW_320MHZ,
+	FW_CDBW_5MHZ,
+	FW_CDBW_10MHZ,
+	FW_CDBW_8080MHZ,
+};
+
+enum bw_mapping_method {
+	BW_MAP_NL_TO_FW,
+	BW_MAP_NL_TO_TM,
+
+	NUM_BW_MAP,
+};
+
+struct mt7996_tm_rf_test {
+	__le16 tag;
+	__le16 len;
+
+	u8 action;
+	u8 icap_len;
+	u8 _rsv[2];
+	union {
+		__le32 op_mode;
+		__le32 freq;
+
+		struct {
+			__le32 func_idx;
+			union {
+				__le32 func_data;
+				__le32 cal_dump;
+
+				u8 _pad[80];
+			} param;
+		} rf;
+	} op;
+} __packed;
+
+struct mt7996_tm_req {
+	u8 _rsv[4];
+
+	struct mt7996_tm_rf_test rf_test;
+} __packed;
+
+struct mt7996_tm_rf_test_result {
+	__le32 func_idx;
+	__le32 payload_length;
+	u8 event[0];
+};
+
+struct mt7996_tm_event {
+	u8 _rsv[4];
+
+	__le16 tag;
+	__le16 len;
+	struct mt7996_tm_rf_test_result result;
+} __packed;
+
+enum {
+	RF_ACTION_SWITCH_TO_RF_TEST,
+	RF_ACTION_IN_RF_TEST,
+	RF_ACTION_SET = 3,
+	RF_ACTION_GET,
+};
+
+enum {
+	RF_OPER_NORMAL,
+	RF_OPER_RF_TEST,
+	RF_OPER_ICAP,
+	RF_OPER_ICAP_OVERLAP,
+	RF_OPER_WIFI_SPECTRUM,
+};
+
+enum {
+	UNI_RF_TEST_CTRL,
+};
+
+#define RF_CMD(cmd)		RF_TEST_CMD_##cmd
+
+enum {
+	RF_TEST_CMD_STOP_TEST = 0,
+	RF_TEST_CMD_START_TX = 1,
+	RF_TEST_CMD_START_RX = 2,
+	RF_TEST_CMD_CONT_WAVE = 10,
+	RF_TEST_CMD_TX_COMMIT = 18,
+	RF_TEST_CMD_RX_COMMIT = 19,
+};
+
+#define SET_ID(id)		RF_TEST_ID_SET_##id
+#define GET_ID(id)		RF_TEST_ID_GET_##id
+
+enum {
+	RF_TEST_ID_SET_COMMAND = 1,
+	RF_TEST_ID_SET_POWER = 2,
+	RF_TEST_ID_SET_TX_RATE = 3,
+	RF_TEST_ID_SET_TX_MODE = 4,
+	RF_TEST_ID_SET_TX_LEN = 6,
+	RF_TEST_ID_SET_TX_COUNT = 7,
+	RF_TEST_ID_SET_IPG = 8,
+	RF_TEST_ID_SET_GI = 16,
+	RF_TEST_ID_SET_STBC = 17,
+	RF_TEST_ID_SET_CHAN_FREQ = 18,
+	RF_TEST_ID_GET_TXED_COUNT = 32,
+	RF_TEST_ID_SET_CONT_WAVE_MODE = 65,
+	RF_TEST_ID_SET_DA = 68,
+	RF_TEST_ID_SET_SA = 69,
+	RF_TEST_ID_SET_CBW = 71,
+	RF_TEST_ID_SET_DBW = 72,
+	RF_TEST_ID_SET_PRIMARY_CH = 73,
+	RF_TEST_ID_SET_ENCODE_MODE = 74,
+	RF_TEST_ID_SET_BAND = 90,
+	RF_TEST_ID_SET_TRX_COUNTER_RESET = 91,
+	RF_TEST_ID_SET_MAC_HEADER = 101,
+	RF_TEST_ID_SET_SEQ_CTRL = 102,
+	RF_TEST_ID_SET_PAYLOAD = 103,
+	RF_TEST_ID_SET_BAND_IDX = 104,
+	RF_TEST_ID_SET_RX_PATH = 106,
+	RF_TEST_ID_SET_FREQ_OFFSET = 107,
+	RF_TEST_ID_GET_FREQ_OFFSET = 108,
+	RF_TEST_ID_SET_TX_PATH = 113,
+	RF_TEST_ID_SET_NSS = 114,
+	RF_TEST_ID_SET_ANT_MASK = 115,
+	RF_TEST_ID_SET_IBF_ENABLE = 126,
+	RF_TEST_ID_SET_EBF_ENABLE = 127,
+	RF_TEST_ID_GET_TX_POWER = 136,
+	RF_TEST_ID_SET_RX_MU_AID = 157,
+	RF_TEST_ID_SET_HW_TX_MODE = 167,
+	RF_TEST_ID_SET_PUNCTURE = 168,
+	RF_TEST_ID_SET_CFG_ON = 176,
+	RF_TEST_ID_SET_CFG_OFF = 177,
+	RF_TEST_ID_SET_BSSID = 189,
+	RF_TEST_ID_SET_TX_TIME = 190,
+	RF_TEST_ID_SET_MAX_PE = 191,
+	RF_TEST_ID_SET_AID_OFFSET = 204,
+};
+
+#define POWER_CTRL(type)	UNI_TXPOWER_##type##_CTRL
+
+struct mt7996_tm_rx_stat_user_ctrl {
+	__le16 tag;
+	__le16 len;
+
+	u8 band_idx;
+	u8 rsv;
+	__le16 user_idx;
+} __packed;
+
+struct mt7996_tm_rx_stat_all {
+	__le16 tag;
+	__le16 len;
+
+	u8 band_idx;
+	u8 rsv[3];
+} __packed;
+
+struct mt7996_tm_rx_req {
+	u8 band;
+	u8 _rsv[3];
+
+	union {
+		struct mt7996_tm_rx_stat_user_ctrl user_ctrl;
+		struct mt7996_tm_rx_stat_all rx_stat_all;
+	};
+} __packed;
+
+enum {
+	UNI_TM_RX_STAT_SET_USER_CTRL = 7,
+	UNI_TM_RX_STAT_GET_ALL_V2 = 9,
+};
+
+struct rx_band_info {
+	/* mac part */
+	__le16 mac_rx_fcs_err_cnt;
+	__le16 mac_rx_len_mismatch;
+	__le16 mac_rx_fcs_ok_cnt;
+	u8 rsv1[2];
+	__le32 mac_rx_mdrdy_cnt;
+
+	/* phy part */
+	__le16 phy_rx_fcs_err_cnt_cck;
+	__le16 phy_rx_fcs_err_cnt_ofdm;
+	__le16 phy_rx_pd_cck;
+	__le16 phy_rx_pd_ofdm;
+	__le16 phy_rx_sig_err_cck;
+	__le16 phy_rx_sfd_err_cck;
+	__le16 phy_rx_sig_err_ofdm;
+	__le16 phy_rx_tag_err_ofdm;
+	__le16 phy_rx_mdrdy_cnt_cck;
+	__le16 phy_rx_mdrdy_cnt_ofdm;
+} __packed;
+
+struct rx_band_info_ext {
+	/* mac part */
+	__le32 mac_rx_mpdu_cnt;
+
+	/* phy part */
+	u8 rsv[4];
+} __packed;
+
+struct rx_common_info {
+	__le16 rx_fifo_full;
+	u8 rsv[2];
+	__le32 aci_hit_low;
+	__le32 aci_hit_high;
+} __packed;
+
+struct rx_common_info_ext {
+	__le32 driver_rx_count;
+	__le32 sinr;
+	__le32 mu_pkt_count;
+
+	/* mac part */
+	u8 _rsv[4];
+
+	/* phy part */
+	u8 sig_mcs;
+	u8 rsv[3];
+} __packed;
+
+struct rx_rxv_info {
+	__le16 rcpi;
+	s16 rssi;
+	s16 snr;
+	s16 adc_rssi;
+} __packed;
+
+struct rx_rssi_info {
+	s8 ib_rssi;
+	s8 wb_rssi;
+	u8 rsv[2];
+} __packed;
+
+struct rx_user_info {
+	s32 freq_offset;
+	s32 snr;
+	__le32 fcs_err_count;
+} __packed;
+
+struct rx_user_info_ext {
+	s8 ne_var_db_all_user;
+	u8 rsv[3];
+} __packed;
+
+#define MAX_ANTENNA_NUM		8
+#define MAX_USER_NUM		16
+
+struct mt7996_tm_rx_event_stat_all {
+	__le16 tag;
+	__le16 len;
+
+	struct rx_band_info band_info;
+	struct rx_band_info_ext band_info_ext;
+	struct rx_common_info common_info;
+	struct rx_common_info_ext common_info_ext;
+
+	/* RXV info */
+	struct rx_rxv_info rxv_info[MAX_ANTENNA_NUM];
+
+	/* RSSI info */
+	struct rx_rssi_info fagc[MAX_ANTENNA_NUM];
+	struct rx_rssi_info inst[MAX_ANTENNA_NUM];
+
+	/* User info */
+	struct rx_user_info user_info[MAX_USER_NUM];
+	struct rx_user_info_ext user_info_ext[MAX_USER_NUM];
+} __packed;
+
+struct mt7996_tm_rx_event {
+	u8 _rsv[4];
+
+	union {
+		struct mt7996_tm_rx_event_stat_all rx_stat_all;
+	};
+} __packed;
+
+#endif
diff --git a/testmode.c b/testmode.c
index 5c93aa6..bbe8230 100644
--- a/testmode.c
+++ b/testmode.c
@@ -2,11 +2,13 @@
 /* Copyright (C) 2020 Felix Fietkau <nbd@nbd.name> */
 
 #include <linux/random.h>
+#include "mt76_connac.h"
 #include "mt76.h"
 
 const struct nla_policy mt76_tm_policy[NUM_MT76_TM_ATTRS] = {
 	[MT76_TM_ATTR_RESET] = { .type = NLA_FLAG },
 	[MT76_TM_ATTR_STATE] = { .type = NLA_U8 },
+	[MT76_TM_ATTR_SKU_EN] = { .type = NLA_U8 },
 	[MT76_TM_ATTR_TX_COUNT] = { .type = NLA_U32 },
 	[MT76_TM_ATTR_TX_LENGTH] = { .type = NLA_U32 },
 	[MT76_TM_ATTR_TX_RATE_MODE] = { .type = NLA_U8 },
@@ -82,6 +84,11 @@ mt76_testmode_max_mpdu_len(struct mt76_phy *phy, u8 tx_rate_mode)
 		    IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_7991)
 			return IEEE80211_MAX_MPDU_LEN_VHT_7991;
 		return IEEE80211_MAX_MPDU_LEN_VHT_11454;
+	case MT76_TM_TX_MODE_EHT_SU:
+	case MT76_TM_TX_MODE_EHT_TRIG:
+	case MT76_TM_TX_MODE_EHT_MU:
+		/* TODO: check the limit */
+		return UINT_MAX;
 	case MT76_TM_TX_MODE_CCK:
 	case MT76_TM_TX_MODE_OFDM:
 	default:
@@ -183,6 +190,9 @@ mt76_testmode_tx_init(struct mt76_phy *phy)
 	u8 max_nss = hweight8(phy->antenna_mask);
 	int ret;
 
+	if (is_mt7996(phy->dev))
+		return 0;
+
 	ret = mt76_testmode_alloc_skb(phy, td->tx_mpdu_len);
 	if (ret)
 		return ret;
@@ -275,7 +285,9 @@ mt76_testmode_tx_start(struct mt76_phy *phy)
 	td->tx_queued = 0;
 	td->tx_done = 0;
 	td->tx_pending = td->tx_count;
-	mt76_worker_schedule(&dev->tx_worker);
+
+	if (!is_mt7996(dev))
+		mt76_worker_schedule(&dev->tx_worker);
 }
 
 static void
@@ -284,6 +296,11 @@ mt76_testmode_tx_stop(struct mt76_phy *phy)
 	struct mt76_testmode_data *td = &phy->test;
 	struct mt76_dev *dev = phy->dev;
 
+	if (is_mt7996(dev) && dev->test_ops->tx_stop) {
+		dev->test_ops->tx_stop(phy);
+		return;
+	}
+
 	mt76_worker_disable(&dev->tx_worker);
 
 	td->tx_pending = 0;
@@ -296,22 +313,11 @@ mt76_testmode_tx_stop(struct mt76_phy *phy)
 	mt76_testmode_free_skb(phy);
 }
 
-static inline void
-mt76_testmode_param_set(struct mt76_testmode_data *td, u16 idx)
-{
-	td->param_set[idx / 32] |= BIT(idx % 32);
-}
-
-static inline bool
-mt76_testmode_param_present(struct mt76_testmode_data *td, u16 idx)
-{
-	return td->param_set[idx / 32] & BIT(idx % 32);
-}
-
 static void
 mt76_testmode_init_defaults(struct mt76_phy *phy)
 {
 	struct mt76_testmode_data *td = &phy->test;
+	u8 addr[ETH_ALEN] = {phy->band_idx, 0x11, 0x22, 0xaa, 0xbb, 0xcc};
 
 	if (td->tx_mpdu_len > 0)
 		return;
@@ -319,11 +325,18 @@ mt76_testmode_init_defaults(struct mt76_phy *phy)
 	td->tx_mpdu_len = 1024;
 	td->tx_count = 1;
 	td->tx_rate_mode = MT76_TM_TX_MODE_OFDM;
+	td->tx_rate_idx = 7;
 	td->tx_rate_nss = 1;
+	/* 0xffff for OFDMA no puncture */
+	td->tx_preamble_puncture = ~(td->tx_preamble_puncture & 0);
+	td->tx_ipg = 50;
 
-	memcpy(td->addr[0], phy->macaddr, ETH_ALEN);
-	memcpy(td->addr[1], phy->macaddr, ETH_ALEN);
-	memcpy(td->addr[2], phy->macaddr, ETH_ALEN);
+	/* rx stat user config */
+	td->aid = 1;
+
+	memcpy(td->addr[0], addr, ETH_ALEN);
+	memcpy(td->addr[1], addr, ETH_ALEN);
+	memcpy(td->addr[2], addr, ETH_ALEN);
 }
 
 static int
@@ -353,7 +366,7 @@ __mt76_testmode_set_state(struct mt76_phy *phy, enum mt76_testmode_state state)
 	if (state == MT76_TM_STATE_TX_FRAMES)
 		mt76_testmode_tx_start(phy);
 	else if (state == MT76_TM_STATE_RX_FRAMES) {
-		memset(&phy->test.rx_stats, 0, sizeof(phy->test.rx_stats));
+		dev->test_ops->reset_rx_stats(phy);
 	}
 
 	phy->test.state = state;
@@ -434,6 +447,9 @@ int mt76_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 
 	mt76_testmode_init_defaults(phy);
 
+	if (tb[MT76_TM_ATTR_SKU_EN])
+		td->sku_en = nla_get_u8(tb[MT76_TM_ATTR_SKU_EN]);
+
 	if (tb[MT76_TM_ATTR_TX_COUNT])
 		td->tx_count = nla_get_u32(tb[MT76_TM_ATTR_TX_COUNT]);
 
@@ -454,7 +470,8 @@ int mt76_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	    mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_DUTY_CYCLE],
 			   &td->tx_duty_cycle, 0, 99) ||
 	    mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_POWER_CONTROL],
-			   &td->tx_power_control, 0, 1))
+			   &td->tx_power_control, 0, 1) ||
+	    mt76_tm_get_u8(tb[MT76_TM_ATTR_AID], &td->aid, 0, 16))
 		goto out;
 
 	if (tb[MT76_TM_ATTR_TX_LENGTH]) {
@@ -494,7 +511,9 @@ int mt76_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 			    idx >= ARRAY_SIZE(td->tx_power))
 				goto out;
 
-			td->tx_power[idx++] = nla_get_u8(cur);
+			err = mt76_tm_get_u8(cur, &td->tx_power[idx++], 0, 63);
+			if (err)
+				return err;
 		}
 	}
 
@@ -512,6 +531,22 @@ int mt76_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 		}
 	}
 
+	if (tb[MT76_TM_ATTR_CFG]) {
+		struct nlattr *cur;
+		int rem, idx = 0;
+
+		nla_for_each_nested(cur, tb[MT76_TM_ATTR_CFG], rem) {
+			if (nla_len(cur) != 1 || idx >= 2)
+				goto out;
+
+			if (idx == 0)
+				td->cfg.type = nla_get_u8(cur);
+			else
+				td->cfg.enable = nla_get_u8(cur);
+			idx++;
+		}
+	}
+
 	if (dev->test_ops->set_params) {
 		err = dev->test_ops->set_params(phy, tb, state);
 		if (err)
@@ -561,6 +596,9 @@ mt76_testmode_dump_stats(struct mt76_phy *phy, struct sk_buff *msg)
 	    nla_put_u64_64bit(msg, MT76_TM_STATS_ATTR_RX_PACKETS, rx_packets,
 			      MT76_TM_STATS_ATTR_PAD) ||
 	    nla_put_u64_64bit(msg, MT76_TM_STATS_ATTR_RX_FCS_ERROR, rx_fcs_error,
+			      MT76_TM_STATS_ATTR_PAD) ||
+	    nla_put_u64_64bit(msg, MT76_TM_STATS_ATTR_RX_LEN_MISMATCH,
+			      td->rx_stats.len_mismatch,
 			      MT76_TM_STATS_ATTR_PAD))
 		return -EMSGSIZE;
 
@@ -625,6 +663,8 @@ int mt76_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *msg,
 	    nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_SGI, td->tx_rate_sgi) ||
 	    nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_LDPC, td->tx_rate_ldpc) ||
 	    nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_STBC, td->tx_rate_stbc) ||
+	    nla_put_u8(msg, MT76_TM_ATTR_SKU_EN, td->sku_en) ||
+	    nla_put_u8(msg, MT76_TM_ATTR_AID, td->aid) ||
 	    (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_LTF) &&
 	     nla_put_u8(msg, MT76_TM_ATTR_TX_LTF, td->tx_ltf)) ||
 	    (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_ANTENNA) &&
diff --git a/testmode.h b/testmode.h
index a40cd74..141bb86 100644
--- a/testmode.h
+++ b/testmode.h
@@ -19,6 +19,7 @@
  * @MT76_TM_ATTR_MTD_OFFSET: offset of eeprom data within the partition (u32)
  * @MT76_TM_ATTR_BAND_IDX: band idx of the chip (u8)
  *
+ * @MT76_TM_ATTR_SKU_EN: config txpower sku is enabled or disabled in testmode (u8)
  * @MT76_TM_ATTR_TX_COUNT: configured number of frames to send when setting
  *	state to MT76_TM_STATE_TX_FRAMES (u32)
  * @MT76_TM_ATTR_TX_PENDING: pending frames during MT76_TM_STATE_TX_FRAMES (u32)
@@ -39,6 +40,11 @@
  *
  * @MT76_TM_ATTR_STATS: statistics (nested, see &enum mt76_testmode_stats_attr)
  *
+ * @MT76_TM_ATTR_PRECAL: Pre-cal data (u8)
+ * @MT76_TM_ATTR_PRECAL_INFO: group size, dpd size, dpd_info, transmit size,
+ *                            eeprom cal indicator (u32),
+ *                            dpd_info = [dpd_per_chan_size, chan_num_2g,
+ *                                        chan_num_5g, chan_num_6g]
  * @MT76_TM_ATTR_TX_SPE_IDX: tx spatial extension index (u8)
  *
  * @MT76_TM_ATTR_TX_DUTY_CYCLE: packet tx duty cycle (u8)
@@ -48,6 +54,29 @@
  * @MT76_TM_ATTR_DRV_DATA: driver specific netlink attrs (nested)
  *
  * @MT76_TM_ATTR_MAC_ADDRS: array of nested MAC addresses (nested)
+ *
+ * @MT76_TM_ATTR_EEPROM_ACTION: eeprom setting actions
+ *	(u8, see &enum mt76_testmode_eeprom_action)
+ * @MT76_TM_ATTR_EEPROM_OFFSET: offset of eeprom data block for writing (u32)
+ * @MT76_TM_ATTR_EEPROM_VAL: values for writing into a 16-byte data block
+ *	(nested, u8 attrs)
+ *
+ * @MT76_TM_ATTR_CFG: config testmode rf feature (nested, see &mt76_testmode_cfg)
+ * @MT76_TM_ATTR_TXBF_ACT: txbf setting actions (u8)
+ * @MT76_TM_ATTR_TXBF_PARAM: txbf parameters (nested)
+ *
+ * @MT76_TM_ATTR_OFF_CH_SCAN_CH: config the channel of background chain (ZWDFS) (u8)
+ * @MT76_TM_ATTR_OFF_CH_SCAN_CENTER_CH: config the center channel of background chain (ZWDFS) (u8)
+ * @MT76_TM_ATTR_OFF_CH_SCAN_BW: config the bandwidth of background chain (ZWDFS) (u8)
+ * @MT76_TM_ATTR_OFF_CH_SCAN_PATH: config the tx path of background chain (ZWDFS) (u8)
+ *
+ * @MT76_TM_ATTR_IPI_THRESHOLD: config the IPI index you want to read (u8)
+ * @MT76_TM_ATTR_IPI_PERIOD: config the time period for reading
+ *			     the histogram of specific IPI index (u8)
+ * @MT76_TM_ATTR_IPI_ANTENNA_INDEX: config the antenna index for reading
+ *				    the histogram of specific IPI index (u8)
+ * @MT76_TM_ATTR_IPI_RESET: Reset the IPI counter
+ *
  */
 enum mt76_testmode_attr {
 	MT76_TM_ATTR_UNSPEC,
@@ -59,6 +88,7 @@ enum mt76_testmode_attr {
 	MT76_TM_ATTR_MTD_OFFSET,
 	MT76_TM_ATTR_BAND_IDX,
 
+	MT76_TM_ATTR_SKU_EN,
 	MT76_TM_ATTR_TX_COUNT,
 	MT76_TM_ATTR_TX_LENGTH,
 	MT76_TM_ATTR_TX_RATE_MODE,
@@ -76,6 +106,8 @@ enum mt76_testmode_attr {
 	MT76_TM_ATTR_FREQ_OFFSET,
 
 	MT76_TM_ATTR_STATS,
+	MT76_TM_ATTR_PRECAL,
+	MT76_TM_ATTR_PRECAL_INFO,
 
 	MT76_TM_ATTR_TX_SPE_IDX,
 
@@ -86,6 +118,27 @@ enum mt76_testmode_attr {
 	MT76_TM_ATTR_DRV_DATA,
 
 	MT76_TM_ATTR_MAC_ADDRS,
+	MT76_TM_ATTR_AID,
+	MT76_TM_ATTR_RU_ALLOC,
+	MT76_TM_ATTR_RU_IDX,
+
+	MT76_TM_ATTR_EEPROM_ACTION,
+	MT76_TM_ATTR_EEPROM_OFFSET,
+	MT76_TM_ATTR_EEPROM_VAL,
+
+	MT76_TM_ATTR_CFG,
+	MT76_TM_ATTR_TXBF_ACT,
+	MT76_TM_ATTR_TXBF_PARAM,
+
+	MT76_TM_ATTR_OFF_CH_SCAN_CH,
+	MT76_TM_ATTR_OFF_CH_SCAN_CENTER_CH,
+	MT76_TM_ATTR_OFF_CH_SCAN_BW,
+	MT76_TM_ATTR_OFF_CH_SCAN_PATH,
+
+	MT76_TM_ATTR_IPI_THRESHOLD,
+	MT76_TM_ATTR_IPI_PERIOD,
+	MT76_TM_ATTR_IPI_ANTENNA_INDEX,
+	MT76_TM_ATTR_IPI_RESET,
 
 	/* keep last */
 	NUM_MT76_TM_ATTRS,
@@ -103,6 +156,8 @@ enum mt76_testmode_attr {
  * @MT76_TM_STATS_ATTR_RX_FCS_ERROR: number of rx packets with FCS error (u64)
  * @MT76_TM_STATS_ATTR_LAST_RX: information about the last received packet
  *	see &enum mt76_testmode_rx_attr
+ * @MT76_TM_STATS_ATTR_RX_LEN_MISMATCH: number of rx packets with length
+ *	mismatch error (u64)
  */
 enum mt76_testmode_stats_attr {
 	MT76_TM_STATS_ATTR_UNSPEC,
@@ -115,6 +170,7 @@ enum mt76_testmode_stats_attr {
 	MT76_TM_STATS_ATTR_RX_PACKETS,
 	MT76_TM_STATS_ATTR_RX_FCS_ERROR,
 	MT76_TM_STATS_ATTR_LAST_RX,
+	MT76_TM_STATS_ATTR_RX_LEN_MISMATCH,
 
 	/* keep last */
 	NUM_MT76_TM_STATS_ATTRS,
@@ -127,6 +183,7 @@ enum mt76_testmode_stats_attr {
  *
  * @MT76_TM_RX_ATTR_FREQ_OFFSET: frequency offset (s32)
  * @MT76_TM_RX_ATTR_RCPI: received channel power indicator (array, u8)
+ * @MT76_TM_RX_ATTR_RSSI: received signal strength indicator (array, s8)
  * @MT76_TM_RX_ATTR_IB_RSSI: internal inband RSSI (array, s8)
  * @MT76_TM_RX_ATTR_WB_RSSI: internal wideband RSSI (array, s8)
  * @MT76_TM_RX_ATTR_SNR: signal-to-noise ratio (u8)
@@ -136,6 +193,7 @@ enum mt76_testmode_rx_attr {
 
 	MT76_TM_RX_ATTR_FREQ_OFFSET,
 	MT76_TM_RX_ATTR_RCPI,
+	MT76_TM_RX_ATTR_RSSI,
 	MT76_TM_RX_ATTR_IB_RSSI,
 	MT76_TM_RX_ATTR_WB_RSSI,
 	MT76_TM_RX_ATTR_SNR,
@@ -179,6 +237,9 @@ enum mt76_testmode_state {
  * @MT76_TM_TX_MODE_HE_EXT_SU: 802.11ax extended-range SU
  * @MT76_TM_TX_MODE_HE_TB: 802.11ax trigger-based
  * @MT76_TM_TX_MODE_HE_MU: 802.11ax multi-user MIMO
+ * @MT76_TM_TX_MODE_EHT_SU: 802.11be single-user MIMO
+ * @MT76_TM_TX_MODE_EHT_TRIG: 802.11be trigger-based
+ * @MT76_TM_TX_MODE_EHT_MU: 802.11be multi-user MIMO
  */
 enum mt76_testmode_tx_mode {
 	MT76_TM_TX_MODE_CCK,
@@ -189,6 +250,9 @@ enum mt76_testmode_tx_mode {
 	MT76_TM_TX_MODE_HE_EXT_SU,
 	MT76_TM_TX_MODE_HE_TB,
 	MT76_TM_TX_MODE_HE_MU,
+	MT76_TM_TX_MODE_EHT_SU,
+	MT76_TM_TX_MODE_EHT_TRIG,
+	MT76_TM_TX_MODE_EHT_MU,
 
 	/* keep last */
 	NUM_MT76_TM_TX_MODES,
diff --git a/tools/fields.c b/tools/fields.c
index e3f6908..055f90f 100644
--- a/tools/fields.c
+++ b/tools/fields.c
@@ -10,6 +10,7 @@ static const char * const testmode_state[] = {
 	[MT76_TM_STATE_IDLE] = "idle",
 	[MT76_TM_STATE_TX_FRAMES] = "tx_frames",
 	[MT76_TM_STATE_RX_FRAMES] = "rx_frames",
+	[MT76_TM_STATE_TX_CONT] = "tx_cont",
 };
 
 static const char * const testmode_tx_mode[] = {
@@ -21,6 +22,9 @@ static const char * const testmode_tx_mode[] = {
 	[MT76_TM_TX_MODE_HE_EXT_SU] = "he_ext_su",
 	[MT76_TM_TX_MODE_HE_TB] = "he_tb",
 	[MT76_TM_TX_MODE_HE_MU] = "he_mu",
+	[MT76_TM_TX_MODE_EHT_SU] = "eht_su",
+	[MT76_TM_TX_MODE_EHT_TRIG] = "eht_tb",
+	[MT76_TM_TX_MODE_EHT_MU] = "eht_mu",
 };
 
 static void print_enum(const struct tm_field *field, struct nlattr *attr)
@@ -65,7 +69,7 @@ static bool parse_u8(const struct tm_field *field, int idx,
 
 static void print_u8(const struct tm_field *field, struct nlattr *attr)
 {
-	printf("%d", nla_get_u8(attr));
+	printf("%u", nla_get_u8(attr));
 }
 
 static void print_s8(const struct tm_field *field, struct nlattr *attr)
@@ -86,12 +90,12 @@ static void print_s32(const struct tm_field *field, struct nlattr *attr)
 
 static void print_u32(const struct tm_field *field, struct nlattr *attr)
 {
-	printf("%d", nla_get_u32(attr));
+	printf("%u", nla_get_u32(attr));
 }
 
 static void print_u64(const struct tm_field *field, struct nlattr *attr)
 {
-	printf("%lld", (unsigned long long)nla_get_u64(attr));
+	printf("%llu", (unsigned long long)nla_get_u64(attr));
 }
 
 static bool parse_flag(const struct tm_field *field, int idx,
@@ -201,6 +205,62 @@ static void print_extra_stats(const struct tm_field *field, struct nlattr **tb)
 	printf("%srx_per=%.02f%%\n", prefix, 100 * failed / total);
 }
 
+static bool parse_mac(const struct tm_field *field, int idx,
+		      struct nl_msg *msg, const char *val)
+{
+#define ETH_ALEN	6
+	bool ret = true;
+	char *str, *cur, *ap;
+	void *a;
+
+	str = strdup(val);
+	ap = str;
+
+	a = nla_nest_start(msg, idx);
+
+	idx = 0;
+	while ((cur = strsep(&ap, ",")) != NULL) {
+		unsigned char addr[ETH_ALEN];
+		char *val, *tmp = cur;
+		int i = 0;
+
+		while ((val = strsep(&tmp, ":")) != NULL) {
+			if (i >= ETH_ALEN)
+				break;
+
+			addr[i++] = strtoul(val, NULL, 16);
+		}
+
+		nla_put(msg, idx, ETH_ALEN, addr);
+
+		idx++;
+	}
+
+	nla_nest_end(msg, a);
+
+	free(str);
+
+	return ret;
+}
+
+static void print_mac(const struct tm_field *field, struct nlattr *attr)
+{
+#define MAC2STR(a) (a)[0], (a)[1], (a)[2], (a)[3], (a)[4], (a)[5]
+#define MACSTR "%02x:%02x:%02x:%02x:%02x:%02x"
+	unsigned char addr[3][6];
+	struct nlattr *cur;
+	int idx = 0;
+	int rem;
+
+	nla_for_each_nested(cur, attr, rem) {
+		if (nla_len(cur) != 6)
+			continue;
+		memcpy(addr[idx++], nla_data(cur), 6);
+	}
+
+	printf("" MACSTR "," MACSTR "," MACSTR "",
+	       MAC2STR(addr[0]), MAC2STR(addr[1]), MAC2STR(addr[2]));
+}
 
 #define FIELD_GENERIC(_field, _name, ...)	\
 	[FIELD_NAME(_field)] = {			\
@@ -250,10 +310,18 @@ static void print_extra_stats(const struct tm_field *field, struct nlattr **tb)
 		 ##__VA_ARGS__				\
 	)
 
+#define FIELD_MAC(_field, _name)			\
+	[FIELD_NAME(_field)] = {			\
+		.name = _name,				\
+		.parse = parse_mac,			\
+		.print = print_mac			\
+	}
+
 #define FIELD_NAME(_field) MT76_TM_RX_ATTR_##_field
 static const struct tm_field rx_fields[NUM_MT76_TM_RX_ATTRS] = {
 	FIELD_RO(s32, FREQ_OFFSET, "freq_offset"),
 	FIELD_ARRAY_RO(u8, RCPI, "rcpi"),
+	FIELD_ARRAY_RO(s8, RSSI, "rssi"),
 	FIELD_ARRAY_RO(s8, IB_RSSI, "ib_rssi"),
 	FIELD_ARRAY_RO(s8, WB_RSSI, "wb_rssi"),
 	FIELD_RO(s8, SNR, "snr"),
@@ -261,6 +329,7 @@ static const struct tm_field rx_fields[NUM_MT76_TM_RX_ATTRS] = {
 static struct nla_policy rx_policy[NUM_MT76_TM_RX_ATTRS] = {
 	[MT76_TM_RX_ATTR_FREQ_OFFSET] = { .type = NLA_U32 },
 	[MT76_TM_RX_ATTR_RCPI] = { .type = NLA_NESTED },
+	[MT76_TM_RX_ATTR_RSSI] = { .type = NLA_NESTED },
 	[MT76_TM_RX_ATTR_IB_RSSI] = { .type = NLA_NESTED },
 	[MT76_TM_RX_ATTR_WB_RSSI] = { .type = NLA_NESTED },
 	[MT76_TM_RX_ATTR_SNR] = { .type = NLA_U8 },
@@ -274,6 +343,7 @@ static const struct tm_field stats_fields[NUM_MT76_TM_STATS_ATTRS] = {
 	FIELD_RO(u32, TX_DONE, "tx_done"),
 	FIELD_RO(u64, RX_PACKETS, "rx_packets"),
 	FIELD_RO(u64, RX_FCS_ERROR, "rx_fcs_error"),
+	FIELD_RO(u64, RX_LEN_MISMATCH, "rx_len_mismatch"),
 	FIELD_NESTED_RO(LAST_RX, rx, "last_"),
 };
 static struct nla_policy stats_policy[NUM_MT76_TM_STATS_ATTRS] = {
@@ -282,6 +352,7 @@ static struct nla_policy stats_policy[NUM_MT76_TM_STATS_ATTRS] = {
 	[MT76_TM_STATS_ATTR_TX_DONE] = { .type = NLA_U32 },
 	[MT76_TM_STATS_ATTR_RX_PACKETS] = { .type = NLA_U64 },
 	[MT76_TM_STATS_ATTR_RX_FCS_ERROR] = { .type = NLA_U64 },
+	[MT76_TM_STATS_ATTR_RX_LEN_MISMATCH] = { .type = NLA_U64 },
 };
 #undef FIELD_NAME
 
@@ -291,6 +362,7 @@ static const struct tm_field testdata_fields[NUM_MT76_TM_ATTRS] = {
 	FIELD_ENUM(STATE, "state", testmode_state),
 	FIELD_RO(string, MTD_PART, "mtd_part"),
 	FIELD_RO(u32, MTD_OFFSET, "mtd_offset"),
+	FIELD(u8, SKU_EN, "sku_en"),
 	FIELD(u32, TX_COUNT, "tx_count"),
 	FIELD(u32, TX_LENGTH, "tx_length"),
 	FIELD_ENUM(TX_RATE_MODE, "tx_rate_mode", testmode_tx_mode),
@@ -300,12 +372,20 @@ static const struct tm_field testdata_fields[NUM_MT76_TM_ATTRS] = {
 	FIELD(u8, TX_RATE_LDPC, "tx_rate_ldpc"),
 	FIELD(u8, TX_RATE_STBC, "tx_rate_stbc"),
 	FIELD(u8, TX_LTF, "tx_ltf"),
+	FIELD(u8, TX_DUTY_CYCLE, "tx_duty_cycle"),
+	FIELD(u32, TX_IPG, "tx_ipg"),
+	FIELD(u32, TX_TIME, "tx_time"),
 	FIELD(u8, TX_POWER_CONTROL, "tx_power_control"),
 	FIELD_ARRAY(u8, TX_POWER, "tx_power"),
 	FIELD(u8, TX_ANTENNA, "tx_antenna"),
 	FIELD(u32, FREQ_OFFSET, "freq_offset"),
+	FIELD(u8, AID, "aid"),
+	FIELD(u8, RU_ALLOC, "ru_alloc"),
+	FIELD(u8, RU_IDX, "ru_idx"),
+	FIELD_MAC(MAC_ADDRS, "mac_addrs"),
 	FIELD_NESTED_RO(STATS, stats, "",
 			.print_extra = print_extra_stats),
+
 };
 #undef FIELD_NAME
 
@@ -313,6 +393,7 @@ static struct nla_policy testdata_policy[NUM_MT76_TM_ATTRS] = {
 	[MT76_TM_ATTR_STATE] = { .type = NLA_U8 },
 	[MT76_TM_ATTR_MTD_PART] = { .type = NLA_STRING },
 	[MT76_TM_ATTR_MTD_OFFSET] = { .type = NLA_U32 },
+	[MT76_TM_ATTR_SKU_EN] = { .type = NLA_U8 },
 	[MT76_TM_ATTR_TX_COUNT] = { .type = NLA_U32 },
 	[MT76_TM_ATTR_TX_LENGTH] = { .type = NLA_U32 },
 	[MT76_TM_ATTR_TX_RATE_MODE] = { .type = NLA_U8 },
@@ -322,10 +403,25 @@ static struct nla_policy testdata_policy[NUM_MT76_TM_ATTRS] = {
 	[MT76_TM_ATTR_TX_RATE_LDPC] = { .type = NLA_U8 },
 	[MT76_TM_ATTR_TX_RATE_STBC] = { .type = NLA_U8 },
 	[MT76_TM_ATTR_TX_LTF] = { .type = NLA_U8 },
+	[MT76_TM_ATTR_TX_DUTY_CYCLE] = { .type = NLA_U8 },
+	[MT76_TM_ATTR_TX_IPG] = { .type = NLA_U32 },
+	[MT76_TM_ATTR_TX_TIME] = { .type = NLA_U32 },
 	[MT76_TM_ATTR_TX_POWER_CONTROL] = { .type = NLA_U8 },
 	[MT76_TM_ATTR_TX_ANTENNA] = { .type = NLA_U8 },
+	[MT76_TM_ATTR_TX_SPE_IDX] = { .type = NLA_U8 },
 	[MT76_TM_ATTR_FREQ_OFFSET] = { .type = NLA_U32 },
+	[MT76_TM_ATTR_AID] = { .type = NLA_U8 },
+	[MT76_TM_ATTR_RU_ALLOC] = { .type = NLA_U8 },
+	[MT76_TM_ATTR_RU_IDX] = { .type = NLA_U8 },
 	[MT76_TM_ATTR_STATS] = { .type = NLA_NESTED },
+	[MT76_TM_ATTR_TXBF_ACT] = { .type = NLA_U8 },
+	[MT76_TM_ATTR_OFF_CH_SCAN_CH] = { .type = NLA_U8 },
+	[MT76_TM_ATTR_OFF_CH_SCAN_CENTER_CH] = { .type = NLA_U8 },
+	[MT76_TM_ATTR_OFF_CH_SCAN_BW] = { .type = NLA_U8 },
+	[MT76_TM_ATTR_IPI_THRESHOLD] = { .type = NLA_U8 },
+	[MT76_TM_ATTR_IPI_PERIOD] = { .type = NLA_U32 },
+	[MT76_TM_ATTR_IPI_ANTENNA_INDEX] = { .type = NLA_U8 },
+	[MT76_TM_ATTR_IPI_RESET] = { .type = NLA_U8 },
 };
 
 const struct tm_field msg_field = {
-- 
2.18.0