recipes-wifi/linux-mt76/files/patches-3.x/0027-mtk-mt76-mt7996-add-basic-testmode-support.patch

From c051b319e725ca566a990b375c089bf74d53e021 Mon Sep 17 00:00:00 2001
From: StanleyYP Wang <StanleyYP.Wang@mediatek.com>
Date: Wed, 28 Dec 2022 22:24:25 +0800
Subject: [PATCH 027/199] mtk: mt76: mt7996: add basic testmode support

Add testmode eeprom buffer mode support

Fix power & freq offset issue for iTest power cal & tx/rx verifcation
1. Wait for fw to tx. Otherwise, iTest testing tool cannot get the
accurate tx power.
2. In crystal mode, freq offset is set in 6G band and forwarded to 5G
and 2G band. Therefore, we should avoid reseting freq offset to 0 when
6G interface is off.

edcca return err in testmode; therefore, bypass it when we are in testmode idle state or testmode bf is on

Signed-off-by: StanleyYP Wang <StanleyYP.Wang@mediatek.com>
---
 eeprom.c          |   6 +-
 mac80211.c        |   3 +-
 mt76.h            |  36 +++
 mt76_connac_mcu.h |   2 +
 mt7996/Makefile   |   1 +
 mt7996/eeprom.c   |  37 ++-
 mt7996/eeprom.h   |   1 +
 mt7996/init.c     |   8 +
 mt7996/mac.c      |   3 +-
 mt7996/main.c     |  27 ++
 mt7996/mcu.c      |  59 +++-
 mt7996/mcu.h      |  33 +++
 mt7996/mt7996.h   |  27 +-
 mt7996/testmode.c | 740 ++++++++++++++++++++++++++++++++++++++++++++++
 mt7996/testmode.h | 299 +++++++++++++++++++
 testmode.c        | 126 ++++++--
 testmode.h        |  87 +++++-
 tools/fields.c    | 102 ++++++-
 18 files changed, 1547 insertions(+), 50 deletions(-)
 create mode 100644 mt7996/testmode.c
 create mode 100644 mt7996/testmode.h

diff --git a/eeprom.c b/eeprom.c
index 0bc66cc1..a0047d79 100644
--- a/eeprom.c
+++ b/eeprom.c
@@ -94,8 +94,10 @@ int mt76_get_of_data_from_mtd(struct mt76_dev *dev, void *eep, int offset, int l
 	}
 
 #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 9df9109f..7e4ee83f 100644
--- a/mac80211.c
+++ b/mac80211.c
@@ -846,7 +846,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 beba1d91..27635c49 100644
--- a/mt76.h
+++ b/mt76.h
@@ -704,14 +704,21 @@ 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);
+	int (*set_eeprom)(struct mt76_phy *phy, u32 offset, u8 *val, u8 action);
 };
 
+#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;
 
@@ -721,6 +728,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;
@@ -730,6 +738,9 @@ struct mt76_testmode_data {
 	u32 tx_time;
 	u32 tx_ipg;
 
+	bool ibf;
+	bool ebf;
+
 	u32 freq_offset;
 
 	u8 tx_power[4];
@@ -744,7 +755,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 {
@@ -1453,6 +1473,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 a7cb33d2..57e0e9de 100644
--- a/mt76_connac_mcu.h
+++ b/mt76_connac_mcu.h
@@ -1279,12 +1279,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 a056b40e..7bb17f44 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 bd02eb00..e2790109 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)
 {
@@ -43,6 +48,9 @@ static int mt7996_check_eeprom(struct mt7996_dev *dev)
 
 static char *mt7996_eeprom_name(struct mt7996_dev *dev)
 {
+	if (dev->testmode_enable)
+		return MT7996_EEPROM_DEFAULT_TM;
+
 	switch (mt76_chip(&dev->mt76)) {
 	case 0x7990:
 		if (dev->chip_sku == MT7996_SKU_404)
@@ -92,21 +100,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;
@@ -118,8 +141,8 @@ static int mt7996_eeprom_load(struct mt7996_dev *dev)
 		/* read eeprom data from efuse */
 		block_num = DIV_ROUND_UP(MT7996_EEPROM_SIZE, eeprom_blk_size);
 		for (i = 0; i < block_num; i++) {
-			ret = mt7996_mcu_get_eeprom(dev, i * eeprom_blk_size);
-			if (ret < 0)
+			ret = mt7996_mcu_get_eeprom(dev, i * eeprom_blk_size, NULL);
+			if (ret && ret != -EINVAL)
 				return ret;
 		}
 	}
diff --git a/mt7996/eeprom.h b/mt7996/eeprom.h
index 72c38ad3..de3ff4e2 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 f80fba28..ed7c7040 100644
--- a/mt7996/init.c
+++ b/mt7996/init.c
@@ -971,6 +971,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;
@@ -1420,6 +1424,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 52ea6796..a0fcd8aa 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 72232994..be50f7f3 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);
@@ -45,6 +57,8 @@ int mt7996_run(struct ieee80211_hw *hw)
 		}
 	}
 
+	mt7996_testmode_disable_all(dev);
+
 	mt7996_mac_enable_nf(dev, phy->mt76->band_idx);
 
 	ret = mt7996_mcu_set_rts_thresh(phy, 0x92b);
@@ -303,6 +317,11 @@ int mt7996_set_channel(struct mt7996_phy *phy)
 
 	mt76_set_channel(phy->mt76);
 
+	if (mt76_testmode_enabled(phy->mt76) || phy->mt76->test.bf_en) {
+		mt7996_tm_update_channel(phy);
+		goto out;
+	}
+
 	ret = mt7996_mcu_set_chan_info(phy, UNI_CHANNEL_SWITCH);
 	if (ret)
 		goto out;
@@ -415,6 +434,12 @@ static int mt7996_config(struct ieee80211_hw *hw, u32 changed)
 	int ret;
 
 	if (changed & IEEE80211_CONF_CHANGE_CHANNEL) {
+		if (!mt76_testmode_enabled(phy->mt76) && !phy->mt76->test.bf_en) {
+			ret = mt7996_mcu_edcca_enable(phy, true);
+			if (ret)
+				return ret;
+		}
+
 		ret = mt7996_mcu_set_pp_en(phy, PP_USR_MODE,
 					   phy->mt76->chandef.punctured);
 		if (ret)
@@ -1531,6 +1556,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 4ff1af78..61b46370 100644
--- a/mt7996/mcu.c
+++ b/mt7996/mcu.c
@@ -2868,8 +2868,12 @@ static int mt7996_load_ram(struct mt7996_dev *dev)
 {
 	int ret;
 
-	ret = __mt7996_load_ram(dev, "WM", fw_name(dev, FIRMWARE_WM),
-				MT7996_RAM_TYPE_WM);
+	if (dev->testmode_enable)
+		ret = __mt7996_load_ram(dev, "WM_TM", fw_name(dev, FIRMWARE_WM_TM),
+					MT7996_RAM_TYPE_WM_TM);
+	else
+		ret = __mt7996_load_ram(dev, "WM", fw_name(dev, FIRMWARE_WM),
+					MT7996_RAM_TYPE_WM);
 	if (ret)
 		return ret;
 
@@ -3560,17 +3564,9 @@ int mt7996_mcu_set_eeprom(struct mt7996_dev *dev)
 				 &req, sizeof(req), true);
 }
 
-int mt7996_mcu_get_eeprom(struct mt7996_dev *dev, u32 offset)
+int mt7996_mcu_get_eeprom(struct mt7996_dev *dev, u32 offset, u8 *read_buf)
 {
-	struct {
-		u8 _rsv[4];
-
-		__le16 tag;
-		__le16 len;
-		__le32 addr;
-		__le32 valid;
-		u8 data[16];
-	} __packed req = {
+	struct mt7996_mcu_eeprom_info req = {
 		.tag = cpu_to_le16(UNI_EFUSE_ACCESS),
 		.len = cpu_to_le16(sizeof(req) - 4),
 		.addr = cpu_to_le32(round_down(offset,
@@ -3579,6 +3575,7 @@ int mt7996_mcu_get_eeprom(struct mt7996_dev *dev, u32 offset)
 	struct sk_buff *skb;
 	bool valid;
 	int ret;
+	u8 *buf = read_buf;
 
 	ret = mt76_mcu_send_and_get_msg(&dev->mt76,
 					MCU_WM_UNI_CMD_QUERY(EFUSE_CTRL),
@@ -3589,7 +3586,9 @@ int mt7996_mcu_get_eeprom(struct mt7996_dev *dev, u32 offset)
 	valid = le32_to_cpu(*(__le32 *)(skb->data + 16));
 	if (valid) {
 		u32 addr = le32_to_cpu(*(__le32 *)(skb->data + 12));
-		u8 *buf = (u8 *)dev->mt76.eeprom.data + addr;
+
+		if (!buf)
+			buf = (u8 *)dev->mt76.eeprom.data + addr;
 
 		skb_pull(skb, 48);
 		memcpy(buf, skb->data, MT7996_EEPROM_BLOCK_SIZE);
@@ -4612,3 +4611,37 @@ int mt7996_mcu_set_pp_en(struct mt7996_phy *phy, u8 mode, u16 bitmap)
 	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 df42c0f8..abf8ef48 100644
--- a/mt7996/mcu.h
+++ b/mt7996/mcu.h
@@ -157,6 +157,16 @@ struct mt7996_mcu_eeprom {
 	__le16 buf_len;
 } __packed;
 
+struct mt7996_mcu_eeprom_info {
+	u8 _rsv[4];
+
+	__le16 tag;
+	__le16 len;
+	__le32 addr;
+	__le32 valid;
+	u8 data[MT7996_EEPROM_BLOCK_SIZE];
+} __packed;
+
 struct mt7996_mcu_phy_rx_info {
 	u8 category;
 	u8 rate;
@@ -889,8 +899,31 @@ 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 {
diff --git a/mt7996/mt7996.h b/mt7996/mt7996.h
index bb7536ff..1e1e638a 100644
--- a/mt7996/mt7996.h
+++ b/mt7996/mt7996.h
@@ -32,21 +32,25 @@
 #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 MT7992_FIRMWARE_WA		"mediatek/mt7996/mt7992_wa.bin"
 #define MT7992_FIRMWARE_WM		"mediatek/mt7996/mt7992_wm.bin"
 #define MT7992_FIRMWARE_DSP		"mediatek/mt7996/mt7992_dsp.bin"
+#define MT7992_FIRMWARE_WM_TM		"mediatek/mt7996/mt7992_wm_tm.bin"
 #define MT7992_ROM_PATCH		"mediatek/mt7996/mt7992_rom_patch.bin"
 
 #define MT7992_FIRMWARE_WA_24		"mediatek/mt7996/mt7992_wa_24.bin"
 #define MT7992_FIRMWARE_WM_24		"mediatek/mt7996/mt7992_wm_24.bin"
 #define MT7992_FIRMWARE_DSP_24		"mediatek/mt7996/mt7992_dsp_24.bin"
+#define MT7992_FIRMWARE_WM_TM_24	"mediatek/mt7996/mt7992_wm_tm_24.bin"
 #define MT7992_ROM_PATCH_24		"mediatek/mt7996/mt7992_rom_patch_24.bin"
 
 #define MT7992_FIRMWARE_WA_23		"mediatek/mt7996/mt7992_wa_23.bin"
 #define MT7992_FIRMWARE_WM_23		"mediatek/mt7996/mt7992_wm_23.bin"
 #define MT7992_FIRMWARE_DSP_23		"mediatek/mt7996/mt7992_dsp_23.bin"
+#define MT7992_FIRMWARE_WM_TM_23	"mediatek/mt7996/mt7992_wm_tm_23.bin"
 #define MT7992_ROM_PATCH_23		"mediatek/mt7996/mt7992_rom_patch_23.bin"
 
 #define MT7996_EEPROM_DEFAULT		"mediatek/mt7996/mt7996_eeprom.bin"
@@ -127,6 +131,7 @@ enum mt7992_sku_type {
 
 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,
@@ -277,6 +282,21 @@ struct mt7996_phy {
 
 	u8 pp_mode;
 	u16 punct_bitmap;
+
+#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 {
@@ -357,6 +377,8 @@ struct mt7996_dev {
 		spinlock_t lock;
 	} wed_rro;
 
+	bool testmode_enable;
+
 	bool ibf;
 	u8 fw_debug_wm;
 	u8 fw_debug_wa;
@@ -472,6 +494,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);
@@ -481,6 +504,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);
@@ -533,7 +557,7 @@ int mt7996_mcu_set_fixed_rate_ctrl(struct mt7996_dev *dev,
 int mt7996_mcu_set_fixed_field(struct mt7996_dev *dev, struct ieee80211_vif *vif,
 			       struct ieee80211_sta *sta, void *data, u32 field);
 int mt7996_mcu_set_eeprom(struct mt7996_dev *dev);
-int mt7996_mcu_get_eeprom(struct mt7996_dev *dev, u32 offset);
+int mt7996_mcu_get_eeprom(struct mt7996_dev *dev, u32 offset, u8 *read_buf);
 int mt7996_mcu_get_eeprom_free_block(struct mt7996_dev *dev, u8 *block_num);
 int mt7996_mcu_get_chip_config(struct mt7996_dev *dev, u32 *cap);
 int mt7996_mcu_set_ser(struct mt7996_dev *dev, u8 action, u8 set, u8 band);
@@ -568,6 +592,7 @@ 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_wed_rro_reset_sessions(struct mt7996_dev *dev, u16 id);
+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 00000000..98eebcee
--- /dev/null
+++ b/mt7996/testmode.c
@@ -0,0 +1,740 @@
+// 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),
+		},
+	};
+	bool wait = (data == RF_CMD(START_TX)) ? true : false;
+
+	return mt76_mcu_send_msg(&dev->mt76, MCU_WM_UNI_CMD(TESTMODE_CTRL), &req,
+				 sizeof(req), wait);
+}
+
+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, false);
+
+	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;
+
+	if (changed & BIT(TM_CHANGED_FREQ_OFFSET)) {
+		mt7996_tm_set(dev, SET_ID(FREQ_OFFSET), td->freq_offset);
+		mt7996_tm_set(dev, SET_ID(FREQ_OFFSET_C2), td->freq_offset);
+	}
+	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;
+}
+
+static int
+mt7996_tm_write_back_to_efuse(struct mt7996_dev *dev)
+{
+	struct mt7996_mcu_eeprom_info req = {
+		.tag = cpu_to_le16(UNI_EFUSE_ACCESS),
+		.len = cpu_to_le16(sizeof(req) - 4),
+	};
+	u8 read_buf[MT76_TM_EEPROM_BLOCK_SIZE], *eeprom = dev->mt76.eeprom.data;
+	int i, ret = -EINVAL;
+
+	/* prevent from damaging chip id in efuse */
+	if (mt76_chip(&dev->mt76) != get_unaligned_le16(eeprom))
+		goto out;
+
+	for (i = 0; i < MT7996_EEPROM_SIZE; i += MT76_TM_EEPROM_BLOCK_SIZE) {
+		req.addr = cpu_to_le32(i);
+		memcpy(req.data, eeprom + i, MT76_TM_EEPROM_BLOCK_SIZE);
+
+		ret = mt7996_mcu_get_eeprom(dev, i, read_buf);
+		if (ret < 0)
+			return ret;
+
+		if (!memcmp(req.data, read_buf, MT76_TM_EEPROM_BLOCK_SIZE))
+			continue;
+
+		ret = mt76_mcu_send_msg(&dev->mt76, MCU_WM_UNI_CMD(EFUSE_CTRL),
+					&req, sizeof(req), true);
+		if (ret)
+			return ret;
+	}
+
+out:
+	return ret;
+}
+
+static int
+mt7996_tm_set_eeprom(struct mt76_phy *mphy, u32 offset, u8 *val, u8 action)
+{
+	struct mt7996_phy *phy = mphy->priv;
+	struct mt7996_dev *dev = phy->dev;
+	u8 *eeprom = dev->mt76.eeprom.data;
+	int ret = 0;
+
+	if (offset >= MT7996_EEPROM_SIZE)
+		return -EINVAL;
+
+	switch (action) {
+	case MT76_TM_EEPROM_ACTION_UPDATE_DATA:
+		memcpy(eeprom + offset, val, MT76_TM_EEPROM_BLOCK_SIZE);
+		break;
+	case MT76_TM_EEPROM_ACTION_UPDATE_BUFFER_MODE:
+		ret = mt7996_mcu_set_eeprom(dev);
+		break;
+	case MT76_TM_EEPROM_ACTION_WRITE_TO_EFUSE:
+		ret = mt7996_tm_write_back_to_efuse(dev);
+		break;
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+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,
+	.set_eeprom = mt7996_tm_set_eeprom,
+};
diff --git a/mt7996/testmode.h b/mt7996/testmode.h
new file mode 100644
index 00000000..319ef257
--- /dev/null
+++ b/mt7996/testmode.h
@@ -0,0 +1,299 @@
+/* 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_FREQ_OFFSET_C2 = 171,
+	RF_TEST_ID_GET_FREQ_OFFSET_C2 = 172,
+	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 ca4feccf..44f3a5bf 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;
@@ -404,6 +417,44 @@ mt76_tm_get_u8(struct nlattr *attr, u8 *dest, u8 min, u8 max)
 	return 0;
 }
 
+static int
+mt76_testmode_set_eeprom(struct mt76_phy *phy, struct nlattr **tb)
+{
+	struct mt76_dev *dev = phy->dev;
+	u8 action, val[MT76_TM_EEPROM_BLOCK_SIZE];
+	u32 offset = 0;
+	int err = -EINVAL;
+
+	if (!dev->test_ops->set_eeprom)
+		return -EOPNOTSUPP;
+
+	if (mt76_tm_get_u8(tb[MT76_TM_ATTR_EEPROM_ACTION], &action,
+			   0, MT76_TM_EEPROM_ACTION_MAX))
+		goto out;
+
+	if (tb[MT76_TM_ATTR_EEPROM_OFFSET]) {
+		struct nlattr *cur;
+		int rem, idx = 0;
+
+		offset = nla_get_u32(tb[MT76_TM_ATTR_EEPROM_OFFSET]);
+		if (!!(offset % MT76_TM_EEPROM_BLOCK_SIZE) ||
+		    !tb[MT76_TM_ATTR_EEPROM_VAL])
+			goto out;
+
+		nla_for_each_nested(cur, tb[MT76_TM_ATTR_EEPROM_VAL], rem) {
+			if (nla_len(cur) != 1 || idx >= ARRAY_SIZE(val))
+				goto out;
+
+			val[idx++] = nla_get_u8(cur);
+		}
+	}
+
+	err = dev->test_ops->set_eeprom(phy, offset, val, action);
+
+out:
+	return err;
+}
+
 int mt76_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 		      void *data, int len)
 {
@@ -427,6 +478,11 @@ int mt76_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 
 	mutex_lock(&dev->mutex);
 
+	if (tb[MT76_TM_ATTR_EEPROM_ACTION]) {
+		err = mt76_testmode_set_eeprom(phy, tb);
+		goto out;
+	}
+
 	if (tb[MT76_TM_ATTR_RESET]) {
 		mt76_testmode_set_state(phy, MT76_TM_STATE_OFF);
 		memset(td, 0, sizeof(*td));
@@ -434,6 +490,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 +513,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 +554,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 +574,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 +639,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;
 
@@ -613,7 +694,8 @@ int mt76_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *msg,
 
 	if (dev->test_mtd.name &&
 	    (nla_put_string(msg, MT76_TM_ATTR_MTD_PART, dev->test_mtd.name) ||
-	     nla_put_u32(msg, MT76_TM_ATTR_MTD_OFFSET, dev->test_mtd.offset)))
+	     nla_put_u32(msg, MT76_TM_ATTR_MTD_OFFSET, dev->test_mtd.offset) ||
+	     nla_put_u8(msg, MT76_TM_ATTR_BAND_IDX, phy->band_idx)))
 		goto out;
 
 	if (nla_put_u32(msg, MT76_TM_ATTR_TX_COUNT, td->tx_count) ||
@@ -624,6 +706,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) &&
@@ -639,7 +723,7 @@ int mt76_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *msg,
 	    (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_POWER_CONTROL) &&
 	     nla_put_u8(msg, MT76_TM_ATTR_TX_POWER_CONTROL, td->tx_power_control)) ||
 	    (mt76_testmode_param_present(td, MT76_TM_ATTR_FREQ_OFFSET) &&
-	     nla_put_u8(msg, MT76_TM_ATTR_FREQ_OFFSET, td->freq_offset)))
+	     nla_put_u32(msg, MT76_TM_ATTR_FREQ_OFFSET, td->freq_offset)))
 		goto out;
 
 	if (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_POWER)) {
diff --git a/testmode.h b/testmode.h
index 5e2792d8..96872e8c 100644
--- a/testmode.h
+++ b/testmode.h
@@ -5,7 +5,8 @@
 #ifndef __MT76_TESTMODE_H
 #define __MT76_TESTMODE_H
 
-#define MT76_TM_TIMEOUT	10
+#define MT76_TM_TIMEOUT			10
+#define MT76_TM_EEPROM_BLOCK_SIZE	16
 
 /**
  * enum mt76_testmode_attr - testmode attributes inside NL80211_ATTR_TESTDATA
@@ -17,7 +18,9 @@
  *
  * @MT76_TM_ATTR_MTD_PART: mtd partition used for eeprom data (string)
  * @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)
@@ -38,6 +41,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)
@@ -47,6 +55,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,
@@ -56,7 +87,9 @@ enum mt76_testmode_attr {
 
 	MT76_TM_ATTR_MTD_PART,
 	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,
@@ -74,6 +107,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,
 
@@ -84,6 +119,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,
@@ -101,6 +157,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,
@@ -113,6 +171,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,
@@ -125,6 +184,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)
@@ -134,6 +194,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,
@@ -177,6 +238,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,
@@ -187,12 +251,33 @@ 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,
 	MT76_TM_TX_MODE_MAX = NUM_MT76_TM_TX_MODES - 1,
 };
 
+/**
+ * enum mt76_testmode_eeprom_action - eeprom setting actions
+ *
+ * @MT76_TM_EEPROM_ACTION_UPDATE_DATA: update rf values to specific
+ *	eeprom data block
+ * @MT76_TM_EEPROM_ACTION_UPDATE_BUFFER_MODE: send updated eeprom data to fw
+ * @MT76_TM_EEPROM_ACTION_WRITE_TO_EFUSE: write eeprom data back to efuse
+ */
+enum mt76_testmode_eeprom_action {
+	MT76_TM_EEPROM_ACTION_UPDATE_DATA,
+	MT76_TM_EEPROM_ACTION_UPDATE_BUFFER_MODE,
+	MT76_TM_EEPROM_ACTION_WRITE_TO_EFUSE,
+
+	/* keep last */
+	NUM_MT76_TM_EEPROM_ACTION,
+	MT76_TM_EEPROM_ACTION_MAX = NUM_MT76_TM_EEPROM_ACTION - 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 e3f69089..055f90f3 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