openwrt_patches-21.02/423-master-mt76-testmode.patch

diff --git a/package/kernel/mt76/patches/1006-mt76-mt7915-add-testmode.patch b/package/kernel/mt76/patches/1006-mt76-mt7915-add-testmode.patch
new file mode 100644
index 00000000..b5fa1b3f
--- /dev/null
+++ b/package/kernel/mt76/patches/1006-mt76-mt7915-add-testmode.patch
@@ -0,0 +1,2048 @@
+From 55fb2cbfd4666fb983dee99f72afac8e0f827007 Mon Sep 17 00:00:00 2001
+From: Shayne Chen <shayne.chen@mediatek.com>
+Date: Wed, 19 Jan 2022 15:46:06 +0800
+Subject: [PATCH] mt76: mt7915: add testmode
+
+---
+ drivers/net/wireless/mediatek/mt76/mac80211.c |  18 +-
+ drivers/net/wireless/mediatek/mt76/mt76.h     | 123 +++-
+ .../wireless/mediatek/mt76/mt76_connac_mcu.c  |   5 +
+ .../wireless/mediatek/mt76/mt76_connac_mcu.h  |   1 +
+ .../net/wireless/mediatek/mt76/mt7915/init.c  |   2 +-
+ .../net/wireless/mediatek/mt76/mt7915/mac.c   |  29 +-
+ .../net/wireless/mediatek/mt76/mt7915/mcu.c   |  12 +-
+ .../net/wireless/mediatek/mt76/mt7915/mcu.h   |   5 +
+ .../net/wireless/mediatek/mt76/mt7915/mmio.c  |   2 +
+ .../wireless/mediatek/mt76/mt7915/mt7915.h    |   2 +-
+ .../net/wireless/mediatek/mt76/mt7915/regs.h  |  16 +-
+ .../wireless/mediatek/mt76/mt7915/testmode.c  | 529 +++++++++++++++---
+ .../wireless/mediatek/mt76/mt7915/testmode.h  |  38 ++
+ drivers/net/wireless/mediatek/mt76/testmode.c | 276 +++++++--
+ drivers/net/wireless/mediatek/mt76/testmode.h |  71 +++
+ .../net/wireless/mediatek/mt76/tools/fields.c |  76 +++
+ drivers/net/wireless/mediatek/mt76/tx.c       |   3 +-
+ 17 files changed, 1061 insertions(+), 147 deletions(-)
+
+diff --git a/mac80211.c b/mac80211.c
+index 5b53d008..fcba2894 100644
+--- a/mac80211.c
++++ b/mac80211.c
+@@ -45,6 +45,9 @@ static const struct ieee80211_channel mt76_channels_2ghz[] = {
+ };
+ 
+ static const struct ieee80211_channel mt76_channels_5ghz[] = {
++	CHAN5G(12, 5060),
++	CHAN5G(16, 5080),
++
+ 	CHAN5G(36, 5180),
+ 	CHAN5G(40, 5200),
+ 	CHAN5G(44, 5220),
+@@ -55,6 +58,13 @@ static const struct ieee80211_channel mt76_channels_5ghz[] = {
+ 	CHAN5G(60, 5300),
+ 	CHAN5G(64, 5320),
+ 
++	CHAN5G(68, 5340),
++	CHAN5G(80, 5400),
++	CHAN5G(84, 5420),
++	CHAN5G(88, 5440),
++	CHAN5G(92, 5460),
++	CHAN5G(96, 5480),
++
+ 	CHAN5G(100, 5500),
+ 	CHAN5G(104, 5520),
+ 	CHAN5G(108, 5540),
+@@ -75,6 +85,11 @@ static const struct ieee80211_channel mt76_channels_5ghz[] = {
+ 	CHAN5G(165, 5825),
+ 	CHAN5G(169, 5845),
+ 	CHAN5G(173, 5865),
++
++	CHAN5G(184, 4920),
++	CHAN5G(188, 4940),
++	CHAN5G(192, 4960),
++	CHAN5G(196, 4980),
+ };
+ 
+ static const struct ieee80211_channel mt76_channels_6ghz[] = {
+@@ -737,7 +752,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 882fb5d2..2df88277 100644
+--- a/mt76.h
++++ b/mt76.h
+@@ -581,6 +581,25 @@ 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);
++	int (*set_eeprom)(struct mt76_phy *phy, u32 offset, u8 *val, u8 action);
++};
++
++#define MT_TM_FW_RX_COUNT	BIT(0)
++
++struct mt76_testmode_sta_data {
++	u16 tx_mpdu_len;
++	u8 tx_rate_idx;
++	u8 tx_rate_nss;
++	u8 tx_rate_ldpc;
++
++	u8 aid;
++	u8 ru_alloc;
++	u8 ru_idx;
++};
++
++struct mt76_testmode_sta {
++	struct sk_buff *tx_skb;
++	struct mt76_testmode_sta_data sd;
+ };
+ 
+ struct mt76_testmode_data {
+@@ -590,13 +609,9 @@ struct mt76_testmode_data {
+ 	struct sk_buff *tx_skb;
+ 
+ 	u32 tx_count;
+-	u16 tx_mpdu_len;
+ 
+ 	u8 tx_rate_mode;
+-	u8 tx_rate_idx;
+-	u8 tx_rate_nss;
+ 	u8 tx_rate_sgi;
+-	u8 tx_rate_ldpc;
+ 	u8 tx_rate_stbc;
+ 	u8 tx_ltf;
+ 
+@@ -614,6 +629,35 @@ struct mt76_testmode_data {
+ 
+ 	u8 addr[3][ETH_ALEN];
+ 
++	u8 flag;
++
++	struct {
++		u8 type;
++		u8 enable;
++	} cfg;
++
++	u8 off_ch_scan_ch;
++	u8 off_ch_scan_center_ch;
++	u8 off_ch_scan_bw;
++	u8 off_ch_scan_path;
++
++	struct mt76_wcid *tm_wcid[MT76_TM_MAX_STA_NUM + 1];
++	u8 cur_aid;
++	u16 tm_sta_mask;
++	union {
++		struct mt76_testmode_sta_data sd;
++		struct {
++			u16 tx_mpdu_len;
++			u8 tx_rate_idx;
++			u8 tx_rate_nss;
++			u8 tx_rate_ldpc;
++
++			u8 aid;
++			u8 ru_alloc;
++			u8 ru_idx;
++		};
++	};
++
+ 	u32 tx_pending;
+ 	u32 tx_queued;
+ 	u16 tx_queued_limit;
+@@ -621,6 +665,7 @@ struct mt76_testmode_data {
+ 	struct {
+ 		u64 packets[__MT_RXQ_MAX];
+ 		u64 fcs_error[__MT_RXQ_MAX];
++		u64 len_mismatch;
+ 	} rx_stats;
+ };
+ 
+@@ -1091,22 +1136,69 @@ static inline bool mt76_testmode_enabled(struct mt76_phy *phy)
+ #endif
+ }
+ 
++#ifdef CONFIG_NL80211_TESTMODE
++static inline bool
++mt76_testmode_has_sta(struct mt76_phy *phy)
++{
++	return phy->test.tm_sta_mask != 0;
++}
++
++static inline struct mt76_testmode_sta *
++mt76_testmode_aid_get_sta(struct mt76_phy *phy, u8 aid)
++{
++	struct mt76_wcid *wcid = phy->test.tm_wcid[aid];
++
++	if (!wcid || !aid)
++		return NULL;
++
++	return (struct mt76_testmode_sta *)((u8 *)wcid + phy->hw->sta_data_size);
++}
++
++#define mt76_testmode_for_each_sta(phy, aid, tm_sta)	\
++	for (aid = 1, tm_sta = mt76_testmode_aid_get_sta(phy, 1);	\
++	     aid <= hweight16(phy->test.tm_sta_mask);	\
++	     aid = phy->test.tm_sta_mask >> aid ?	\
++		   ffs(phy->test.tm_sta_mask >> aid) + aid :	\
++		   aid + 1,	\
++	     tm_sta = mt76_testmode_aid_get_sta(phy, aid))
++
++static inline bool
++__mt76_testmode_check_skb(struct mt76_phy *phy, struct sk_buff *skb)
++{
++	struct mt76_testmode_sta *tm_sta;
++	int i;
++
++	if (!mt76_testmode_has_sta(phy))
++		return false;
++
++	mt76_testmode_for_each_sta(phy, i, tm_sta) {
++		if (tm_sta->tx_skb == skb)
++			return true;
++	}
++
++	return false;
++}
++
+ static inline bool mt76_is_testmode_skb(struct mt76_dev *dev,
+ 					struct sk_buff *skb,
+ 					struct ieee80211_hw **hw)
+ {
+-#ifdef CONFIG_NL80211_TESTMODE
+-	if (skb == dev->phy.test.tx_skb)
+-		*hw = dev->phy.hw;
+-	else if (dev->phy2 && skb == dev->phy2->test.tx_skb)
+-		*hw = dev->phy2->hw;
+-	else
+-		return false;
+-	return true;
+-#else
++	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
++	struct mt76_phy *phy = &dev->phy;
++
++	if ((info->hw_queue & MT_TX_HW_QUEUE_EXT_PHY) && dev->phy2)
++		phy = dev->phy2;
++
++	if (mt76_testmode_enabled(phy) &&
++	    (skb == phy->test.tx_skb ||
++	    __mt76_testmode_check_skb(phy, skb))) {
++		*hw = phy->hw;
++		return true;
++	}
++
+ 	return false;
+-#endif
+ }
++#endif
+ 
+ void mt76_rx(struct mt76_dev *dev, enum mt76_rxq_id q, struct sk_buff *skb);
+ void mt76_tx(struct mt76_phy *dev, struct ieee80211_sta *sta,
+@@ -1198,7 +1290,7 @@ int mt76_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+ int mt76_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *skb,
+ 		       struct netlink_callback *cb, void *data, int len);
+ 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);
++int mt76_testmode_init_skb(struct mt76_phy *phy, u32 len, u8 aid, struct sk_buff **skb);
+ 
+ static inline void mt76_testmode_reset(struct mt76_phy *phy, bool disable)
+ {
+@@ -1212,7 +1304,6 @@ static inline void mt76_testmode_reset(struct mt76_phy *phy, bool disable)
+ #endif
+ }
+ 
+-
+ /* internal */
+ static inline struct ieee80211_hw *
+ mt76_tx_status_get_hw(struct mt76_dev *dev, struct sk_buff *skb)
+diff --git a/mt76_connac_mcu.c b/mt76_connac_mcu.c
+index eeb73d14..0725e5dd 100644
+--- a/mt76_connac_mcu.c
++++ b/mt76_connac_mcu.c
+@@ -389,6 +389,7 @@ void mt76_connac_mcu_sta_basic_tlv(struct sk_buff *skb,
+ 	switch (vif->type) {
+ 	case NL80211_IFTYPE_MESH_POINT:
+ 	case NL80211_IFTYPE_AP:
++	case NL80211_IFTYPE_MONITOR:
+ 		if (vif->p2p)
+ 			conn_type = CONNECTION_P2P_GC;
+ 		else
+@@ -577,6 +578,10 @@ void mt76_connac_mcu_wtbl_generic_tlv(struct mt76_dev *dev,
+ 					     wtbl_tlv, sta_wtbl);
+ 	spe = (struct wtbl_spe *)tlv;
+ 	spe->spe_idx = 24;
++
++	/* check */
++	if (vif->type == NL80211_IFTYPE_MONITOR)
++		rx->rca1 = 0;
+ }
+ EXPORT_SYMBOL_GPL(mt76_connac_mcu_wtbl_generic_tlv);
+ 
+diff --git a/mt76_connac_mcu.h b/mt76_connac_mcu.h
+index c3c93338..54419864 100644
+--- a/mt76_connac_mcu.h
++++ b/mt76_connac_mcu.h
+@@ -980,6 +980,7 @@ enum {
+ 	MCU_EXT_CMD_OFFCH_SCAN_CTRL = 0x9a,
+ 	MCU_EXT_CMD_SET_RDD_TH = 0x9d,
+ 	MCU_EXT_CMD_MURU_CTRL = 0x9f,
++	MCU_EXT_CMD_RX_STAT = 0xa4,
+ 	MCU_EXT_CMD_SET_SPR = 0xa8,
+ 	MCU_EXT_CMD_GROUP_PRE_CAL_INFO = 0xab,
+ 	MCU_EXT_CMD_DPD_PRE_CAL_INFO = 0xac,
+diff --git a/mt7915/init.c b/mt7915/init.c
+index f57a3d18..db7f029f 100644
+--- a/mt7915/init.c
++++ b/mt7915/init.c
+@@ -558,7 +558,7 @@ static void mt7915_init_work(struct work_struct *work)
+ 	struct mt7915_dev *dev = container_of(work, struct mt7915_dev,
+ 				 init_work);
+ 
+-	mt7915_mcu_set_eeprom(dev);
++	mt7915_mcu_set_eeprom(dev, dev->flash_mode);
+ 	mt7915_mac_init(dev);
+ 	mt7915_init_txpower(dev, &dev->mphy.sband_2g.sband);
+ 	mt7915_init_txpower(dev, &dev->mphy.sband_5g.sband);
+diff --git a/mt7915/mac.c b/mt7915/mac.c
+index 47d5a993..48139ccf 100644
+--- a/mt7915/mac.c
++++ b/mt7915/mac.c
+@@ -899,16 +899,28 @@ mt7915_mac_write_txwi_tm(struct mt7915_phy *phy, __le32 *txwi,
+ {
+ #ifdef CONFIG_NL80211_TESTMODE
+ 	struct mt76_testmode_data *td = &phy->mt76->test;
++	struct mt76_testmode_sta_data *sd = &td->sd;
+ 	const struct ieee80211_rate *r;
+-	u8 bw, mode, nss = td->tx_rate_nss;
+-	u8 rate_idx = td->tx_rate_idx;
++	u8 bw, mode, nss, rate_idx;
+ 	u16 rateval = 0;
+ 	u32 val;
+ 	bool cck = false;
+ 	int band;
+ 
+-	if (skb != phy->mt76->test.tx_skb)
+-		return;
++	if (mt76_testmode_has_sta(phy->mt76)) {
++		struct mt76_testmode_sta *tm_sta;
++		int i;
++
++		mt76_testmode_for_each_sta(phy->mt76, i, tm_sta) {
++			if (tm_sta->tx_skb == skb) {
++				sd = &tm_sta->sd;
++				break;
++			}
++		}
++	}
++
++	nss = sd->tx_rate_nss;
++	rate_idx = sd->tx_rate_idx;
+ 
+ 	switch (td->tx_rate_mode) {
+ 	case MT76_TM_TX_MODE_HT:
+@@ -998,9 +1010,10 @@ mt7915_mac_write_txwi_tm(struct mt7915_phy *phy, __le32 *txwi,
+ 	if (mode >= MT_PHY_TYPE_HE_SU)
+ 		val |= FIELD_PREP(MT_TXD6_HELTF, td->tx_ltf);
+ 
+-	if (td->tx_rate_ldpc || (bw > 0 && mode >= MT_PHY_TYPE_HE_SU))
++	if (sd->tx_rate_ldpc || (bw > 0 && mode >= MT_PHY_TYPE_HE_SU))
+ 		val |= MT_TXD6_LDPC;
+ 
++	txwi[1] &= ~cpu_to_le32(MT_TXD1_VTA);
+ 	txwi[3] &= ~cpu_to_le32(MT_TXD3_SN_VALID);
+ 	txwi[6] |= cpu_to_le32(val);
+ 	txwi[7] |= cpu_to_le32(FIELD_PREP(MT_TXD7_SPE_IDX,
+@@ -1355,6 +1368,9 @@ mt7915_tx_check_aggr(struct ieee80211_sta *sta, __le32 *txwi)
+ 	if (!sta || !(sta->ht_cap.ht_supported || sta->he_cap.has_he))
+ 		return;
+ 
++	if (le32_get_bits(txwi[2], MT_TXD2_FIX_RATE))
++		return;
++
+ 	tid = le32_get_bits(txwi[1], MT_TXD1_TID);
+ 	if (tid >= 6) /* skip VO queue */
+ 		return;
+@@ -1462,6 +1478,9 @@ mt7915_mac_tx_free(struct mt7915_dev *dev, void *data, int len)
+ 				continue;
+ 
+ 			msta = container_of(wcid, struct mt7915_sta, wcid);
++			if (mt76_testmode_enabled(msta->vif->phy->mt76))
++				continue;
++
+ 			spin_lock_bh(&dev->sta_poll_lock);
+ 			if (list_empty(&msta->poll_list))
+ 				list_add_tail(&msta->poll_list, &dev->sta_poll_list);
+diff --git a/mt7915/mcu.c b/mt7915/mcu.c
+index 2aba342c..5c3309be 100644
+--- a/mt7915/mcu.c
++++ b/mt7915/mcu.c
+@@ -285,7 +285,6 @@ mt7915_mcu_send_message(struct mt76_dev *mdev, struct sk_buff *skb,
+ 	if (mcu_txd->ext_cid) {
+ 		mcu_txd->ext_cid_ack = 1;
+ 
+-		/* do not use Q_SET for efuse */
+ 		if (cmd & __MCU_CMD_FIELD_QUERY)
+ 			mcu_txd->set_query = MCU_Q_QUERY;
+ 		else
+@@ -2822,14 +2821,9 @@ int mt7915_mcu_set_chan_info(struct mt7915_phy *phy, int cmd)
+ 
+ #ifdef CONFIG_NL80211_TESTMODE
+ 	if (phy->mt76->test.tx_antenna_mask &&
+-	    (phy->mt76->test.state == MT76_TM_STATE_TX_FRAMES ||
+-	     phy->mt76->test.state == MT76_TM_STATE_RX_FRAMES ||
+-	     phy->mt76->test.state == MT76_TM_STATE_TX_CONT)) {
++	    mt76_testmode_enabled(phy->mt76)) {
+ 		req.tx_streams_num = fls(phy->mt76->test.tx_antenna_mask);
+ 		req.rx_streams = phy->mt76->test.tx_antenna_mask;
+-
+-		if (phy != &dev->phy)
+-			req.rx_streams >>= dev->chainshift;
+ 	}
+ #endif
+ 
+@@ -2897,14 +2891,14 @@ static int mt7915_mcu_set_eeprom_flash(struct mt7915_dev *dev)
+ 	return 0;
+ }
+ 
+-int mt7915_mcu_set_eeprom(struct mt7915_dev *dev)
++int mt7915_mcu_set_eeprom(struct mt7915_dev *dev, bool flash_mode)
+ {
+ 	struct mt7915_mcu_eeprom req = {
+ 		.buffer_mode = EE_MODE_EFUSE,
+ 		.format = EE_FORMAT_WHOLE,
+ 	};
+ 
+-	if (dev->flash_mode)
++	if (flash_mode)
+ 		return mt7915_mcu_set_eeprom_flash(dev);
+ 
+ 	return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(EFUSE_BUFFER_MODE),
+diff --git a/mt7915/mcu.h b/mt7915/mcu.h
+index 960072a4..a5d869ca 100644
+--- a/mt7915/mcu.h
++++ b/mt7915/mcu.h
+@@ -27,7 +27,12 @@ struct mt7915_mcu_txd {
+ 
+ enum {
+ 	MCU_ATE_SET_TRX = 0x1,
++	MCU_ATE_SET_TSSI = 0x5,
++	MCU_ATE_SET_DPD = 0x6,
++	MCU_ATE_SET_RATE_POWER_OFFSET = 0x7,
++	MCU_ATE_SET_THERMAL_COMP = 0x8,
+ 	MCU_ATE_SET_FREQ_OFFSET = 0xa,
++	MCU_ATE_SET_PHY_COUNT = 0x11,
+ 	MCU_ATE_SET_SLOT_TIME = 0x13,
+ 	MCU_ATE_CLEAN_TXQUEUE = 0x1c,
+ };
+diff --git a/mt7915/mmio.c b/mt7915/mmio.c
+index 5062e0d8..2466907e 100644
+--- a/mt7915/mmio.c
++++ b/mt7915/mmio.c
+@@ -53,6 +53,7 @@ static const u32 mt7986_reg[] = {
+ };
+ 
+ static const u32 mt7915_offs[] = {
++	[TMAC_TCR2]		= 0x05c,
+ 	[TMAC_CDTR]		= 0x090,
+ 	[TMAC_ODTR]		= 0x094,
+ 	[TMAC_ATCR]		= 0x098,
+@@ -126,6 +127,7 @@ static const u32 mt7915_offs[] = {
+ };
+ 
+ static const u32 mt7916_offs[] = {
++	[TMAC_TCR2]		= 0x004,
+ 	[TMAC_CDTR]		= 0x0c8,
+ 	[TMAC_ODTR]		= 0x0cc,
+ 	[TMAC_ATCR]		= 0x00c,
+diff --git a/mt7915/mt7915.h b/mt7915/mt7915.h
+index 6efa0a2e..7ad550c2 100644
+--- a/mt7915/mt7915.h
++++ b/mt7915/mt7915.h
+@@ -478,7 +478,7 @@ int mt7915_mcu_set_fixed_rate_ctrl(struct mt7915_dev *dev,
+ 				   struct ieee80211_vif *vif,
+ 				   struct ieee80211_sta *sta,
+ 				   void *data, u32 field);
+-int mt7915_mcu_set_eeprom(struct mt7915_dev *dev);
++int mt7915_mcu_set_eeprom(struct mt7915_dev *dev, bool flash_mode);
+ int mt7915_mcu_get_eeprom(struct mt7915_dev *dev, u32 offset);
+ int mt7915_mcu_get_eeprom_free_block(struct mt7915_dev *dev, u8 *block_num);
+ int mt7915_mcu_set_mac(struct mt7915_dev *dev, int band, bool enable,
+diff --git a/mt7915/regs.h b/mt7915/regs.h
+index e5f93c40..999dd7fc 100644
+--- a/mt7915/regs.h
++++ b/mt7915/regs.h
+@@ -34,6 +34,7 @@ enum reg_rev {
+ };
+ 
+ enum offs_rev {
++	TMAC_TCR2,
+ 	TMAC_CDTR,
+ 	TMAC_ODTR,
+ 	TMAC_ATCR,
+@@ -172,6 +173,12 @@ enum offs_rev {
+ #define MT_MDP_TO_HIF			0
+ #define MT_MDP_TO_WM			1
+ 
++#define MT_MDP_TOP_DBG_WDT_CTRL			MT_MDP(0x0d0)
++#define MT_MDP_TOP_DBG_WDT_CTRL_TDP_DIS_BLK	BIT(7)
++
++#define MT_MDP_TOP_DBG_CTRL			MT_MDP(0x0dc)
++#define MT_MDP_TOP_DBG_CTRL_ENQ_MODE		BIT(30)
++
+ /* TMAC: band 0(0x820e4000), band 1(0x820f4000) */
+ #define MT_WF_TMAC_BASE(_band)		((_band) ? 0x820f4000 : 0x820e4000)
+ #define MT_WF_TMAC(_band, ofs)		(MT_WF_TMAC_BASE(_band) + (ofs))
+@@ -180,6 +187,9 @@ enum offs_rev {
+ #define MT_TMAC_TCR0_TX_BLINK		GENMASK(7, 6)
+ #define MT_TMAC_TCR0_TBTT_STOP_CTRL	BIT(25)
+ 
++#define MT_TMAC_TCR2(_band)		MT_WF_TMAC(_band, __OFFS(TMAC_TCR2))
++#define MT_TMAC_TCR2_SCH_DET_DIS	BIT(19)
++
+ #define MT_TMAC_CDTR(_band)		MT_WF_TMAC(_band, __OFFS(TMAC_CDTR))
+  #define MT_TMAC_ODTR(_band)		MT_WF_TMAC(_band, __OFFS(TMAC_ODTR))
+ #define MT_TIMEOUT_VAL_PLCP		GENMASK(15, 0)
+@@ -451,8 +461,10 @@ enum offs_rev {
+ #define MT_AGG_PCR0_VHT_PROT		BIT(13)
+ #define MT_AGG_PCR0_PTA_WIN_DIS		BIT(15)
+ 
+-#define MT_AGG_PCR1_RTS0_NUM_THRES	GENMASK(31, 23)
+-#define MT_AGG_PCR1_RTS0_LEN_THRES	GENMASK(19, 0)
++#define MT_AGG_PCR1_RTS0_NUM_THRES		GENMASK(31, 23)
++#define MT_AGG_PCR1_RTS0_LEN_THRES		GENMASK(19, 0)
++#define MT_AGG_PCR1_RTS0_NUM_THRES_MT7916	GENMASK(29, 24)
++#define MT_AGG_PCR1_RTS0_LEN_THRES_MT7916	GENMASK(22, 0)
+ 
+ #define MT_AGG_ACR0(_band)		MT_WF_AGG(_band, __OFFS(AGG_ACR0))
+ #define MT_AGG_ACR_CFEND_RATE		GENMASK(13, 0)
+diff --git a/mt7915/testmode.c b/mt7915/testmode.c
+index 20f63644..a277c16d 100644
+--- a/mt7915/testmode.c
++++ b/mt7915/testmode.c
+@@ -9,6 +9,9 @@
+ enum {
+ 	TM_CHANGED_TXPOWER,
+ 	TM_CHANGED_FREQ_OFFSET,
++	TM_CHANGED_CFG,
++	TM_CHANGED_OFF_CH_SCAN_CH,
++	TM_CHANGED_AID,
+ 
+ 	/* must be last */
+ 	NUM_TM_CHANGED
+@@ -17,6 +20,9 @@ enum {
+ 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_CFG] = MT76_TM_ATTR_CFG,
++	[TM_CHANGED_OFF_CH_SCAN_CH] = MT76_TM_ATTR_OFF_CH_SCAN_CH,
++	[TM_CHANGED_AID] = MT76_TM_ATTR_AID,
+ };
+ 
+ struct reg_band {
+@@ -30,10 +36,29 @@ struct reg_band {
+ 		{ _list.band[0] = MT_##_reg(0, _idx);	\
+ 		  _list.band[1] = MT_##_reg(1, _idx); }
+ 
+-#define TM_REG_MAX_ID	17
++#define TM_REG_MAX_ID	20
+ static struct reg_band reg_backup_list[TM_REG_MAX_ID];
+ 
+ 
++static u8 mt7915_tm_chan_bw(enum nl80211_chan_width width)
++{
++	static const u8 width_to_bw[] = {
++		[NL80211_CHAN_WIDTH_40] = TM_CBW_40MHZ,
++		[NL80211_CHAN_WIDTH_80] = TM_CBW_80MHZ,
++		[NL80211_CHAN_WIDTH_80P80] = TM_CBW_8080MHZ,
++		[NL80211_CHAN_WIDTH_160] = TM_CBW_160MHZ,
++		[NL80211_CHAN_WIDTH_5] = TM_CBW_5MHZ,
++		[NL80211_CHAN_WIDTH_10] = TM_CBW_10MHZ,
++		[NL80211_CHAN_WIDTH_20] = TM_CBW_20MHZ,
++		[NL80211_CHAN_WIDTH_20_NOHT] = TM_CBW_20MHZ,
++	};
++
++	if (width >= ARRAY_SIZE(width_to_bw))
++		return 0;
++
++	return width_to_bw[width];
++}
++
+ static int
+ mt7915_tm_set_tx_power(struct mt7915_phy *phy)
+ {
+@@ -119,15 +144,45 @@ mt7915_tm_set_trx(struct mt7915_phy *phy, int type, bool en)
+ }
+ 
+ static int
+-mt7915_tm_clean_hwq(struct mt7915_phy *phy, u8 wcid)
++mt7915_tm_clean_hwq(struct mt7915_phy *phy)
+ {
+ 	struct mt7915_dev *dev = phy->dev;
+ 	struct mt7915_tm_cmd req = {
+ 		.testmode_en = 1,
+ 		.param_idx = MCU_ATE_CLEAN_TXQUEUE,
+-		.param.clean.wcid = wcid,
+ 		.param.clean.band = phy != &dev->phy,
+ 	};
++	struct mt76_testmode_sta *tm_sta;
++	int ret, i;
++
++	if (!mt76_testmode_has_sta(phy->mt76)) {
++		req.param.clean.wcid = dev->mt76.global_wcid.idx;
++
++		return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(ATE_CTRL),
++					 &req, sizeof(req), false);
++	}
++
++	mt76_testmode_for_each_sta(phy->mt76, i, tm_sta) {
++		req.param.clean.wcid = phy->mt76->test.tm_wcid[i]->idx;
++		ret = mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(ATE_CTRL),
++					&req, sizeof(req), false);
++		if (ret)
++			return ret;
++	}
++
++	return 0;
++}
++
++static int
++mt7915_tm_set_phy_count(struct mt7915_phy *phy, u8 control)
++{
++	struct mt7915_dev *dev = phy->dev;
++	struct mt7915_tm_cmd req = {
++		.testmode_en = 1,
++		.param_idx = MCU_ATE_SET_PHY_COUNT,
++		.param.cfg.enable = control,
++		.param.cfg.band = phy != &dev->phy,
++	};
+ 
+ 	return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(ATE_CTRL), &req,
+ 				 sizeof(req), false);
+@@ -167,6 +222,77 @@ mt7915_tm_set_tam_arb(struct mt7915_phy *phy, bool enable, bool mu)
+ 	return mt7915_mcu_set_muru_ctrl(dev, MURU_SET_ARB_OP_MODE, op_mode);
+ }
+ 
++static int
++mt7915_tm_set_cfg(struct mt7915_phy *phy)
++{
++	static const u8 cfg_cmd[] = {
++		[MT76_TM_CFG_TSSI] = MCU_ATE_SET_TSSI,
++		[MT76_TM_CFG_DPD] = MCU_ATE_SET_DPD,
++		[MT76_TM_CFG_RATE_POWER_OFFSET] = MCU_ATE_SET_RATE_POWER_OFFSET,
++		[MT76_TM_CFG_THERMAL_COMP] = MCU_ATE_SET_THERMAL_COMP,
++	};
++	struct mt76_testmode_data *td = &phy->mt76->test;
++	struct mt7915_dev *dev = phy->dev;
++	struct mt7915_tm_cmd req = {
++		.testmode_en = !(phy->mt76->test.state == MT76_TM_STATE_OFF),
++		.param_idx = cfg_cmd[td->cfg.type],
++		.param.cfg.enable = td->cfg.enable,
++		.param.cfg.band = phy != &dev->phy,
++	};
++
++	return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(ATE_CTRL), &req,
++				 sizeof(req), false);
++}
++
++static int
++mt7915_tm_set_off_channel_scan(struct mt7915_phy *phy)
++{
++#define OFF_CH_SCAN_SIMPLE_RX	2
++	struct mt76_testmode_data *td = &phy->mt76->test;
++	struct mt7915_dev *dev = phy->dev;
++	struct cfg80211_chan_def *chandef = &phy->mt76->chandef;
++	int freq1 = chandef->center_freq1;
++	struct {
++		u8 cur_pri_ch;
++		u8 cur_center_ch;
++		u8 cur_bw;
++		u8 cur_tx_path;
++		u8 cur_rx_path;
++
++		u8 scan_pri_ch;
++		u8 scan_center_ch;
++		u8 scan_bw;
++		u8 scan_tx_path;
++		u8 scan_rx_path;
++
++		u8 enable;
++		u8 band_idx;
++		u8 type;
++		u8 is_5g;
++		u8 _rsv[2];
++	} __packed req = {
++		.cur_pri_ch = chandef->chan->hw_value,
++		.cur_center_ch = ieee80211_frequency_to_channel(freq1),
++		.cur_bw = mt7915_tm_chan_bw(chandef->width),
++		.cur_tx_path = td->tx_antenna_mask,
++		.cur_rx_path = td->tx_antenna_mask,
++
++		.scan_pri_ch = td->off_ch_scan_ch,
++		.scan_center_ch = td->off_ch_scan_center_ch,
++		.scan_bw = td->off_ch_scan_bw,
++		.scan_tx_path = td->off_ch_scan_path,
++		.scan_rx_path = td->off_ch_scan_path,
++
++		.enable = !!td->off_ch_scan_ch,
++		.band_idx = phy != &dev->phy,
++		.type = OFF_CH_SCAN_SIMPLE_RX,
++		.is_5g = td->off_ch_scan_ch > 14 ? 1 : 0,
++	};
++
++	return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(OFFCH_SCAN_CTRL), &req,
++				 sizeof(req), false);
++}
++
+ static int
+ mt7915_tm_set_wmm_qid(struct mt7915_dev *dev, u8 qid, u8 aifs, u8 cw_min,
+ 		      u16 cw_max, u16 txop)
+@@ -269,10 +395,10 @@ mt7915_tm_set_tx_len(struct mt7915_phy *phy, u32 tx_time)
+ 	switch (td->tx_rate_mode) {
+ 	case MT76_TM_TX_MODE_CCK:
+ 	case MT76_TM_TX_MODE_OFDM:
+-		if (mphy->chandef.chan->band == NL80211_BAND_5GHZ)
+-			sband = &mphy->sband_5g.sband;
+-		else if (mphy->chandef.chan->band == NL80211_BAND_6GHZ)
++		if (mphy->chandef.chan->band == NL80211_BAND_6GHZ)
+ 			sband = &mphy->sband_6g.sband;
++		else if (mphy->chandef.chan->band == NL80211_BAND_5GHZ)
++			sband = &mphy->sband_5g.sband;
+ 		else
+ 			sband = &mphy->sband_2g.sband;
+ 
+@@ -322,7 +448,7 @@ mt7915_tm_set_tx_len(struct mt7915_phy *phy, u32 tx_time)
+ 	bitrate = cfg80211_calculate_bitrate(&rate);
+ 	tx_len = bitrate * tx_time / 10 / 8;
+ 
+-	ret = mt76_testmode_alloc_skb(phy->mt76, tx_len);
++	ret = mt76_testmode_init_skb(phy->mt76, tx_len, 0, &td->tx_skb);
+ 	if (ret)
+ 		return ret;
+ 
+@@ -334,7 +460,7 @@ mt7915_tm_reg_backup_restore(struct mt7915_phy *phy)
+ {
+ 	int n_regs = ARRAY_SIZE(reg_backup_list);
+ 	struct mt7915_dev *dev = phy->dev;
+-	u32 *b = phy->test.reg_backup;
++	u32 *b = phy->test.reg_backup, val;
+ 	int i;
+ 
+ 	REG_BAND_IDX(reg_backup_list[0], AGG_PCR0, 0);
+@@ -346,18 +472,28 @@ mt7915_tm_reg_backup_restore(struct mt7915_phy *phy)
+ 	REG_BAND(reg_backup_list[6], AGG_MRCR);
+ 	REG_BAND(reg_backup_list[7], TMAC_TFCR0);
+ 	REG_BAND(reg_backup_list[8], TMAC_TCR0);
+-	REG_BAND(reg_backup_list[9], AGG_ATCR1);
+-	REG_BAND(reg_backup_list[10], AGG_ATCR3);
+-	REG_BAND(reg_backup_list[11], TMAC_TRCR0);
+-	REG_BAND(reg_backup_list[12], TMAC_ICR0);
+-	REG_BAND_IDX(reg_backup_list[13], ARB_DRNGR0, 0);
+-	REG_BAND_IDX(reg_backup_list[14], ARB_DRNGR0, 1);
+-	REG_BAND(reg_backup_list[15], WF_RFCR);
+-	REG_BAND(reg_backup_list[16], WF_RFCR1);
++	REG_BAND(reg_backup_list[9], TMAC_TCR2);
++	REG_BAND(reg_backup_list[10], AGG_ATCR1);
++	REG_BAND(reg_backup_list[11], AGG_ATCR3);
++	REG_BAND(reg_backup_list[12], TMAC_TRCR0);
++	REG_BAND(reg_backup_list[13], TMAC_ICR0);
++	REG_BAND_IDX(reg_backup_list[14], ARB_DRNGR0, 0);
++	REG_BAND_IDX(reg_backup_list[15], ARB_DRNGR0, 1);
++	REG_BAND(reg_backup_list[16], WF_RFCR);
++	REG_BAND(reg_backup_list[17], WF_RFCR1);
++
++	if (is_mt7916(&dev->mt76)) {
++		reg_backup_list[18].band[phy->band_idx] = MT_MDP_TOP_DBG_WDT_CTRL;
++		reg_backup_list[19].band[phy->band_idx] = MT_MDP_TOP_DBG_CTRL;
++	}
+ 
+ 	if (phy->mt76->test.state == MT76_TM_STATE_OFF) {
+-		for (i = 0; i < n_regs; i++)
+-			mt76_wr(dev, reg_backup_list[i].band[phy->band_idx], b[i]);
++		for (i = 0; i < n_regs; i++) {
++			u8 reg = reg_backup_list[i].band[phy->band_idx];
++
++			if (reg)
++				mt76_wr(dev, reg, b[i]);
++		}
+ 		return;
+ 	}
+ 
+@@ -377,8 +513,13 @@ mt7915_tm_reg_backup_restore(struct mt7915_phy *phy)
+ 		   MT_AGG_PCR0_BW40_PROT | MT_AGG_PCR0_BW80_PROT);
+ 	mt76_set(dev, MT_AGG_PCR0(phy->band_idx, 0), MT_AGG_PCR0_PTA_WIN_DIS);
+ 
+-	mt76_wr(dev, MT_AGG_PCR0(phy->band_idx, 1), MT_AGG_PCR1_RTS0_NUM_THRES |
+-		MT_AGG_PCR1_RTS0_LEN_THRES);
++	if (is_mt7915(&dev->mt76))
++		val = MT_AGG_PCR1_RTS0_NUM_THRES | MT_AGG_PCR1_RTS0_LEN_THRES;
++	else
++		val = MT_AGG_PCR1_RTS0_NUM_THRES_MT7916 |
++		      MT_AGG_PCR1_RTS0_LEN_THRES_MT7916;
++
++	mt76_wr(dev, MT_AGG_PCR0(phy->band_idx, 1), val);
+ 
+ 	mt76_clear(dev, MT_AGG_MRCR(phy->band_idx), MT_AGG_MRCR_BAR_CNT_LIMIT |
+ 		   MT_AGG_MRCR_LAST_RTS_CTS_RN | MT_AGG_MRCR_RTS_FAIL_LIMIT |
+@@ -391,31 +532,128 @@ mt7915_tm_reg_backup_restore(struct mt7915_phy *phy)
+ 
+ 	mt76_wr(dev, MT_TMAC_TFCR0(phy->band_idx), 0);
+ 	mt76_clear(dev, MT_TMAC_TCR0(phy->band_idx), MT_TMAC_TCR0_TBTT_STOP_CTRL);
++	mt76_set(dev, MT_TMAC_TCR2(phy->band_idx), MT_TMAC_TCR2_SCH_DET_DIS);
+ 
+ 	/* config rx filter for testmode rx */
+ 	mt76_wr(dev, MT_WF_RFCR(phy->band_idx), 0xcf70a);
+ 	mt76_wr(dev, MT_WF_RFCR1(phy->band_idx), 0);
++
++	if (is_mt7916(&dev->mt76)) {
++		/* enable MDP Tx block mode */
++		mt76_clear(dev, MT_MDP_TOP_DBG_WDT_CTRL,
++			   MT_MDP_TOP_DBG_WDT_CTRL_TDP_DIS_BLK);
++		mt76_clear(dev, MT_MDP_TOP_DBG_CTRL,
++			   MT_MDP_TOP_DBG_CTRL_ENQ_MODE);
++	}
++}
++
++static int
++mt7915_tm_sta_add(struct mt7915_phy *phy, u8 aid,
++		  struct mt76_testmode_sta_data *sd)
++{
++	struct mt76_testmode_data *td = &phy->mt76->test;
++	struct mt76_testmode_sta *tm_sta;
++
++	if (!aid)
++		return 0;
++
++	if (!td->tm_wcid[aid]) {
++		struct ieee80211_vif *vif = phy->monitor_vif;
++		struct ieee80211_sband_iftype_data *data;
++		struct ieee80211_supported_band *sband;
++		struct ieee80211_sta *sta;
++		struct mt7915_sta *msta;
++		int ret;
++
++		sta = kzalloc(sizeof(*sta) + phy->mt76->hw->sta_data_size +
++			      sizeof(*tm_sta), GFP_KERNEL);
++		if (!sta)
++			return -ENOMEM;
++
++		if (phy->mt76->chandef.chan->band == NL80211_BAND_6GHZ) {
++			sband = &phy->mt76->sband_6g.sband;
++			data = phy->iftype[NL80211_BAND_6GHZ];
++		} else if (phy->mt76->chandef.chan->band == NL80211_BAND_5GHZ) {
++			sband = &phy->mt76->sband_5g.sband;
++			data = phy->iftype[NL80211_BAND_5GHZ];
++		} else {
++			sband = &phy->mt76->sband_2g.sband;
++			data = phy->iftype[NL80211_BAND_2GHZ];
++		}
++
++		ether_addr_copy(sta->addr, phy->mt76->macaddr);
++		sta->addr[0] += aid * 4;
++		memcpy(&sta->ht_cap, &sband->ht_cap, sizeof(sta->ht_cap));
++		memcpy(&sta->vht_cap, &sband->vht_cap, sizeof(sta->vht_cap));
++		memcpy(&sta->he_cap, &data[NL80211_IFTYPE_STATION].he_cap,
++		       sizeof(sta->he_cap));
++		sta->aid = aid;
++		sta->wme = 1;
++
++		ret = mt7915_mac_sta_add(&phy->dev->mt76, vif, sta);
++		if (ret) {
++			kfree(sta);
++			return ret;
++		}
++
++		msta = (struct mt7915_sta *)sta->drv_priv;
++		td->tm_wcid[aid] = &msta->wcid;
++		td->tm_sta_mask |= BIT(aid - 1);
++	}
++
++	tm_sta = mt76_testmode_aid_get_sta(phy->mt76, aid);
++	memcpy(&tm_sta->sd, sd, sizeof(tm_sta->sd));
++
++	return 0;
+ }
+ 
+ static void
+-mt7915_tm_init(struct mt7915_phy *phy, bool en)
++mt7915_tm_sta_remove(struct mt7915_phy *phy, u8 aid)
+ {
++	struct mt76_testmode_data *td = &phy->mt76->test;
++	struct mt76_wcid *wcid = td->tm_wcid[aid];
+ 	struct mt7915_dev *dev = phy->dev;
++	struct ieee80211_sta *sta = wcid_to_sta(wcid);
+ 
+-	if (!test_bit(MT76_STATE_RUNNING, &phy->mt76->state))
++	mt7915_mac_sta_remove(&dev->mt76, phy->monitor_vif, sta);
++	mt76_wcid_mask_clear(dev->mt76.wcid_mask, wcid->idx);
++
++	kfree(sta);
++	td->tm_wcid[aid] = NULL;
++	td->tm_sta_mask &= ~BIT(aid - 1);
++}
++
++static void
++mt7915_tm_sta_remove_all(struct mt7915_phy *phy)
++{
++	int i;
++
++	if (!mt76_testmode_has_sta(phy->mt76))
+ 		return;
+ 
+-	mt7915_mcu_set_sku_en(phy, !en);
++	for (i = 1; i < ARRAY_SIZE(phy->mt76->test.tm_wcid); i++) {
++		if (phy->mt76->test.tm_wcid[i])
++			mt7915_tm_sta_remove(phy, i);
++	}
++}
+ 
+-	mt7915_tm_mode_ctrl(dev, en);
+-	mt7915_tm_reg_backup_restore(phy);
+-	mt7915_tm_set_trx(phy, TM_MAC_TXRX, !en);
++static int
++mt7915_tm_set_sta(struct mt7915_phy *phy)
++{
++	struct mt76_testmode_data *td = &phy->mt76->test;
++	bool en = td->state != MT76_TM_STATE_OFF;
+ 
+-	mt7915_mcu_add_bss_info(phy, phy->monitor_vif, en);
+-	mt7915_mcu_add_sta(dev, phy->monitor_vif, NULL, en);
++	if (!en || !td->aid) {
++		mt7915_tm_sta_remove_all(phy);
++		return 0;
++	}
+ 
+-	if (!en)
+-		mt7915_tm_set_tam_arb(phy, en, 0);
++	if (td->tx_count == 0) {
++		mt7915_tm_sta_remove(phy, td->aid);
++		return 0;
++	}
++
++	return mt7915_tm_sta_add(phy, td->aid, &td->sd);
+ }
+ 
+ static void
+@@ -428,59 +666,122 @@ mt7915_tm_update_channel(struct mt7915_phy *phy)
+ 	mt7915_mcu_set_chan_info(phy, MCU_EXT_CMD(SET_RX_PATH));
+ }
+ 
++static bool
++mt7915_tm_check_skb(struct mt7915_phy *phy)
++{
++	struct mt76_testmode_data *td = &phy->mt76->test;
++	struct ieee80211_tx_info *info;
++
++	if (!mt76_testmode_has_sta(phy->mt76)) {
++		if (!td->tx_skb)
++			return false;
++
++		info = IEEE80211_SKB_CB(td->tx_skb);
++		info->control.vif = phy->monitor_vif;
++	} else {
++		struct mt76_testmode_sta *tm_sta;
++		int i;
++
++		mt76_testmode_for_each_sta(phy->mt76, i, tm_sta) {
++			if (!tm_sta->tx_skb)
++				return false;
++
++			info = IEEE80211_SKB_CB(tm_sta->tx_skb);
++			info->control.vif = phy->monitor_vif;
++		}
++	}
++
++	return true;
++}
++
+ static void
+ mt7915_tm_set_tx_frames(struct mt7915_phy *phy, bool en)
+ {
+ 	static const u8 spe_idx_map[] = {0, 0, 1, 0, 3, 2, 4, 0,
+ 					 9, 8, 6, 10, 16, 12, 18, 0};
+ 	struct mt76_testmode_data *td = &phy->mt76->test;
+-	struct mt7915_dev *dev = phy->dev;
+-	struct ieee80211_tx_info *info;
+-	u8 duty_cycle = td->tx_duty_cycle;
+-	u32 tx_time = td->tx_time;
+-	u32 ipg = td->tx_ipg;
+ 
+ 	mt7915_tm_set_trx(phy, TM_MAC_RX_RXV, false);
+-	mt7915_tm_clean_hwq(phy, dev->mt76.global_wcid.idx);
++	mt7915_tm_set_trx(phy, TM_MAC_TX, false);
+ 
+ 	if (en) {
++		u32 tx_time = td->tx_time, ipg = td->tx_ipg;
++		u8 duty_cycle = td->tx_duty_cycle;
++
+ 		mt7915_tm_update_channel(phy);
+ 
+ 		if (td->tx_spe_idx) {
+ 			phy->test.spe_idx = td->tx_spe_idx;
+ 		} else {
+-			u8 tx_ant = td->tx_antenna_mask;
++			phy->test.spe_idx = spe_idx_map[td->tx_antenna_mask];
++		}
+ 
+-			if (phy != &dev->phy)
+-				tx_ant >>= dev->chainshift;
+-			phy->test.spe_idx = spe_idx_map[tx_ant];
++		/* if all three params are set, duty_cycle will be ignored */
++		if (duty_cycle && tx_time && !ipg) {
++			ipg = tx_time * 100 / duty_cycle - tx_time;
++		} else if (duty_cycle && !tx_time && ipg) {
++			if (duty_cycle < 100)
++				tx_time = duty_cycle * ipg / (100 - duty_cycle);
+ 		}
++
++		mt7915_tm_set_ipg_params(phy, ipg, td->tx_rate_mode);
++		mt7915_tm_set_tx_len(phy, tx_time);
++
++		if (ipg)
++			td->tx_queued_limit = MT76_TM_TIMEOUT * 1000000 / ipg / 2;
++
++		if (!mt7915_tm_check_skb(phy))
++			return;
++	} else {
++		mt7915_tm_clean_hwq(phy);
+ 	}
+ 
+ 	mt7915_tm_set_tam_arb(phy, en,
+ 			      td->tx_rate_mode == MT76_TM_TX_MODE_HE_MU);
+ 
+-	/* if all three params are set, duty_cycle will be ignored */
+-	if (duty_cycle && tx_time && !ipg) {
+-		ipg = tx_time * 100 / duty_cycle - tx_time;
+-	} else if (duty_cycle && !tx_time && ipg) {
+-		if (duty_cycle < 100)
+-			tx_time = duty_cycle * ipg / (100 - duty_cycle);
+-	}
++	mt7915_tm_set_trx(phy, TM_MAC_TX, en);
++}
+ 
+-	mt7915_tm_set_ipg_params(phy, ipg, td->tx_rate_mode);
+-	mt7915_tm_set_tx_len(phy, tx_time);
++static int
++mt7915_tm_get_rx_stats(struct mt7915_phy *phy, bool clear)
++{
++#define CMD_RX_STAT_BAND	0x3
++	struct mt76_testmode_data *td = &phy->mt76->test;
++	struct mt7915_tm_rx_stat_band *rs_band;
++	struct mt7915_dev *dev = phy->dev;
++	struct sk_buff *skb;
++	struct {
++		u8 format_id;
++		u8 band;
++		u8 _rsv[2];
++	} __packed req = {
++		.format_id = CMD_RX_STAT_BAND,
++		.band = phy != &dev->phy,
++	};
++	int ret;
+ 
+-	if (ipg)
+-		td->tx_queued_limit = MT76_TM_TIMEOUT * 1000000 / ipg / 2;
++	ret = mt76_mcu_send_and_get_msg(&dev->mt76, MCU_EXT_CMD(RX_STAT),
++					&req, sizeof(req), true, &skb);
++	if (ret)
++		return ret;
+ 
+-	if (!en || !td->tx_skb)
+-		return;
++	rs_band = (struct mt7915_tm_rx_stat_band *)skb->data;
++	/* pr_info("mdrdy_cnt = %d\n", le32_to_cpu(rs_band->mdrdy_cnt)); */
++	/* pr_info("fcs_err = %d\n", le16_to_cpu(rs_band->fcs_err)); */
++	/* pr_info("len_mismatch = %d\n", le16_to_cpu(rs_band->len_mismatch)); */
++	/* pr_info("fcs_ok = %d\n", le16_to_cpu(rs_band->fcs_succ)); */
+ 
+-	info = IEEE80211_SKB_CB(td->tx_skb);
+-	info->control.vif = phy->monitor_vif;
++	if (!clear) {
++		enum mt76_rxq_id q = req.band ? MT_RXQ_EXT : MT_RXQ_MAIN;
+ 
+-	mt7915_tm_set_trx(phy, TM_MAC_TX, en);
++		td->rx_stats.packets[q] += le32_to_cpu(rs_band->mdrdy_cnt);
++		td->rx_stats.fcs_error[q] += le16_to_cpu(rs_band->fcs_err);
++		td->rx_stats.len_mismatch += le16_to_cpu(rs_band->len_mismatch);
++	}
++
++	dev_kfree_skb(skb);
++
++	return 0;
+ }
+ 
+ static void
+@@ -489,12 +790,15 @@ mt7915_tm_set_rx_frames(struct mt7915_phy *phy, bool en)
+ 	mt7915_tm_set_trx(phy, TM_MAC_RX_RXV, false);
+ 
+ 	if (en) {
+-		struct mt7915_dev *dev = phy->dev;
+-
+ 		mt7915_tm_update_channel(phy);
+ 
+ 		/* read-clear */
+-		mt76_rr(dev, MT_MIB_SDR3(phy != &dev->phy));
++		mt7915_tm_get_rx_stats(phy, true);
++
++		/* clear fw count */
++		mt7915_tm_set_phy_count(phy, 0);
++		mt7915_tm_set_phy_count(phy, 1);
++
+ 		mt7915_tm_set_trx(phy, TM_MAC_RX_RXV, en);
+ 	}
+ }
+@@ -572,10 +876,10 @@ mt7915_tm_set_tx_cont(struct mt7915_phy *phy, bool en)
+ 		struct ieee80211_supported_band *sband;
+ 		u8 idx = rate_idx;
+ 
+-		if (chandef->chan->band == NL80211_BAND_5GHZ)
+-			sband = &phy->mt76->sband_5g.sband;
+-		else if (chandef->chan->band == NL80211_BAND_6GHZ)
++		if (chandef->chan->band == NL80211_BAND_6GHZ)
+ 			sband = &phy->mt76->sband_6g.sband;
++		else if (chandef->chan->band == NL80211_BAND_5GHZ)
++			sband = &phy->mt76->sband_5g.sband;
+ 		else
+ 			sband = &phy->mt76->sband_2g.sband;
+ 
+@@ -635,6 +939,31 @@ out:
+ 				 sizeof(req), true);
+ }
+ 
++static void
++mt7915_tm_init(struct mt7915_phy *phy, bool en)
++{
++	struct mt7915_dev *dev = phy->dev;
++
++	if (!test_bit(MT76_STATE_RUNNING, &phy->mt76->state))
++		return;
++
++	mt7915_mcu_set_sku_en(phy, !en);
++
++	mt7915_tm_mode_ctrl(dev, en);
++	mt7915_tm_reg_backup_restore(phy);
++	mt7915_tm_set_trx(phy, TM_MAC_TXRX, !en);
++
++	mt7915_mcu_add_bss_info(phy, phy->monitor_vif, en);
++	mt7915_mcu_add_sta(dev, phy->monitor_vif, NULL, en);
++
++	phy->mt76->test.flag |= MT_TM_FW_RX_COUNT;
++
++	if (!en) {
++		mt7915_tm_set_tam_arb(phy, en, 0);
++		mt7915_tm_sta_remove_all(phy);
++	}
++}
++
+ static void
+ mt7915_tm_update_params(struct mt7915_phy *phy, u32 changed)
+ {
+@@ -645,6 +974,12 @@ mt7915_tm_update_params(struct mt7915_phy *phy, u32 changed)
+ 		mt7915_tm_set_freq_offset(phy, en, en ? td->freq_offset : 0);
+ 	if (changed & BIT(TM_CHANGED_TXPOWER))
+ 		mt7915_tm_set_tx_power(phy);
++	if (changed & BIT(TM_CHANGED_CFG))
++		mt7915_tm_set_cfg(phy);
++	if (changed & BIT(TM_CHANGED_OFF_CH_SCAN_CH))
++		mt7915_tm_set_off_channel_scan(phy);
++	if (changed & BIT(TM_CHANGED_AID))
++		mt7915_tm_set_sta(phy);
+ }
+ 
+ static int
+@@ -704,9 +1039,6 @@ mt7915_tm_set_params(struct mt76_phy *mphy, struct nlattr **tb,
+ 	    td->state == MT76_TM_STATE_OFF)
+ 		return 0;
+ 
+-	if (td->tx_antenna_mask & ~mphy->chainmask)
+-		return -EINVAL;
+-
+ 	for (i = 0; i < ARRAY_SIZE(tm_change_map); i++) {
+ 		if (tb[tm_change_map[i]])
+ 			changed |= BIT(i);
+@@ -721,12 +1053,8 @@ static int
+ mt7915_tm_dump_stats(struct mt76_phy *mphy, struct sk_buff *msg)
+ {
+ 	struct mt7915_phy *phy = mphy->priv;
+-	struct mt7915_dev *dev = phy->dev;
+-	enum mt76_rxq_id q;
+ 	void *rx, *rssi;
+-	u16 fcs_err;
+ 	int i;
+-	u32 cnt;
+ 
+ 	rx = nla_nest_start(msg, MT76_TM_STATS_ATTR_LAST_RX);
+ 	if (!rx)
+@@ -770,19 +1098,68 @@ mt7915_tm_dump_stats(struct mt76_phy *mphy, struct sk_buff *msg)
+ 
+ 	nla_nest_end(msg, rx);
+ 
+-	cnt = mt76_rr(dev, MT_MIB_SDR3(phy->band_idx));
+-	fcs_err = is_mt7915(&dev->mt76) ? FIELD_GET(MT_MIB_SDR3_FCS_ERR_MASK, cnt) :
+-		FIELD_GET(MT_MIB_SDR3_FCS_ERR_MASK_MT7916, cnt);
++	return mt7915_tm_get_rx_stats(phy, false);
++}
+ 
+-	q = phy->band_idx ? MT_RXQ_EXT : MT_RXQ_MAIN;
+-	mphy->test.rx_stats.packets[q] += fcs_err;
+-	mphy->test.rx_stats.fcs_error[q] += fcs_err;
++static int
++mt7915_tm_write_back_to_efuse(struct mt7915_dev *dev)
++{
++	struct mt7915_mcu_eeprom_info req = {};
++	u8 *eeprom = dev->mt76.eeprom.data;
++	int i, ret = -EINVAL;
+ 
+-	return 0;
++	if (is_mt7986(&dev->mt76))
++		goto out;
++
++	/* prevent from damaging chip id in efuse */
++	if (mt76_chip(&dev->mt76) != get_unaligned_le16(eeprom))
++		goto out;
++
++	for (i = 0; i < MT7915_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 = mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(EFUSE_ACCESS),
++					&req, sizeof(req), true);
++		if (ret)
++			return ret;
++	}
++
++out:
++	return ret;
++}
++
++static int
++mt7915_tm_set_eeprom(struct mt76_phy *mphy, u32 offset, u8 *val, u8 action)
++{
++	struct mt7915_phy *phy = mphy->priv;
++	struct mt7915_dev *dev = phy->dev;
++	u8 *eeprom = dev->mt76.eeprom.data;
++	int ret = 0;
++
++	if (offset >= MT7915_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 = mt7915_mcu_set_eeprom(dev, true);
++		break;
++	case MT76_TM_EEPROM_ACTION_WRITE_TO_EFUSE:
++		ret = mt7915_tm_write_back_to_efuse(dev);
++		break;
++	default:
++		break;
++	}
++
++	return ret;
+ }
+ 
+ const struct mt76_testmode_ops mt7915_testmode_ops = {
+ 	.set_state = mt7915_tm_set_state,
+ 	.set_params = mt7915_tm_set_params,
+ 	.dump_stats = mt7915_tm_dump_stats,
++	.set_eeprom = mt7915_tm_set_eeprom,
+ };
+diff --git a/mt7915/testmode.h b/mt7915/testmode.h
+index 5573ac30..d22aabe7 100644
+--- a/mt7915/testmode.h
++++ b/mt7915/testmode.h
+@@ -33,6 +33,12 @@ struct mt7915_tm_clean_txq {
+ 	u8 rsv;
+ };
+ 
++struct mt7915_tm_cfg {
++	u8 enable;
++	u8 band;
++	u8 _rsv[2];
++};
++
+ struct mt7915_tm_cmd {
+ 	u8 testmode_en;
+ 	u8 param_idx;
+@@ -43,6 +49,7 @@ struct mt7915_tm_cmd {
+ 		struct mt7915_tm_freq_offset freq;
+ 		struct mt7915_tm_slot_time slot;
+ 		struct mt7915_tm_clean_txq clean;
++		struct mt7915_tm_cfg cfg;
+ 		u8 test[72];
+ 	} param;
+ } __packed;
+@@ -102,4 +109,35 @@ enum {
+ 	TAM_ARB_OP_MODE_FORCE_SU = 5,
+ };
+ 
++struct mt7915_tm_rx_stat_band {
++	u8 category;
++
++	/* mac */
++	__le16 fcs_err;
++	__le16 len_mismatch;
++	__le16 fcs_succ;
++	__le32 mdrdy_cnt;
++	/* phy */
++	__le16 fcs_err_cck;
++	__le16 fcs_err_ofdm;
++	__le16 pd_cck;
++	__le16 pd_ofdm;
++	__le16 sig_err_cck;
++	__le16 sfd_err_cck;
++	__le16 sig_err_ofdm;
++	__le16 tag_err_ofdm;
++	__le16 mdrdy_cnt_cck;
++	__le16 mdrdy_cnt_ofdm;
++};
++
++enum {
++	TM_CBW_20MHZ,
++	TM_CBW_40MHZ,
++	TM_CBW_80MHZ,
++	TM_CBW_10MHZ,
++	TM_CBW_5MHZ,
++	TM_CBW_160MHZ,
++	TM_CBW_8080MHZ,
++};
++
+ #endif
+diff --git a/testmode.c b/testmode.c
+index 382b4563..9da490cf 100644
+--- a/testmode.c
++++ b/testmode.c
+@@ -25,18 +25,18 @@ const struct nla_policy mt76_tm_policy[NUM_MT76_TM_ATTRS] = {
+ };
+ EXPORT_SYMBOL_GPL(mt76_tm_policy);
+ 
+-void mt76_testmode_tx_pending(struct mt76_phy *phy)
++static u16
++mt76_testmode_queue_tx(struct mt76_phy *phy, struct mt76_wcid *wcid,
++		       struct sk_buff *skb, u32 limit)
+ {
+ 	struct mt76_testmode_data *td = &phy->test;
+ 	struct mt76_dev *dev = phy->dev;
+-	struct mt76_wcid *wcid = &dev->global_wcid;
+-	struct sk_buff *skb = td->tx_skb;
+ 	struct mt76_queue *q;
+-	u16 tx_queued_limit;
++	u16 tx_queued_limit, count = 0;
+ 	int qid;
+ 
+-	if (!skb || !td->tx_pending)
+-		return;
++	if (!skb)
++		return 0;
+ 
+ 	qid = skb_get_queue_mapping(skb);
+ 	q = phy->q_tx[qid];
+@@ -45,7 +45,7 @@ void mt76_testmode_tx_pending(struct mt76_phy *phy)
+ 
+ 	spin_lock_bh(&q->lock);
+ 
+-	while (td->tx_pending > 0 &&
++	while (count < limit &&
+ 	       td->tx_queued - td->tx_done < tx_queued_limit &&
+ 	       q->queued < q->ndesc / 2) {
+ 		int ret;
+@@ -55,13 +55,56 @@ void mt76_testmode_tx_pending(struct mt76_phy *phy)
+ 		if (ret < 0)
+ 			break;
+ 
+-		td->tx_pending--;
+ 		td->tx_queued++;
++		count++;
+ 	}
+ 
+ 	dev->queue_ops->kick(dev, q);
+ 
+ 	spin_unlock_bh(&q->lock);
++
++	return count;
++}
++
++void mt76_testmode_tx_pending(struct mt76_phy *phy)
++{
++	struct mt76_testmode_data *td = &phy->test;
++	u16 count;
++
++	if (!td->tx_pending)
++		return;
++
++	if (!mt76_testmode_has_sta(phy)) {
++		count = mt76_testmode_queue_tx(phy, &phy->dev->global_wcid,
++					       td->tx_skb, td->tx_pending);
++		td->tx_pending -= count;
++
++		return;
++	}
++
++	while (true) {
++		struct mt76_testmode_sta *tm_sta;
++		struct mt76_wcid *wcid;
++		u32 limit, per_sta_cnt = 1;
++
++		if (td->tx_rate_mode != MT76_TM_TX_MODE_HE_MU)
++			per_sta_cnt = td->tx_count / hweight16(phy->test.tm_sta_mask);
++
++		limit = td->tx_pending % per_sta_cnt;
++		if (limit == 0)
++			limit = per_sta_cnt;
++
++		tm_sta = mt76_testmode_aid_get_sta(phy, td->cur_aid);
++		wcid = td->tm_wcid[td->cur_aid];
++		count = mt76_testmode_queue_tx(phy, wcid, tm_sta->tx_skb, limit);
++
++		td->tx_pending -= count;
++
++		if (td->tx_pending && (td->tx_pending % per_sta_cnt == 0))
++			td->cur_aid = ffs(td->tm_sta_mask >> td->cur_aid) + td->cur_aid;
++		else
++			break;
++	}
+ }
+ 
+ static u32
+@@ -87,15 +130,34 @@ mt76_testmode_max_mpdu_len(struct mt76_phy *phy, u8 tx_rate_mode)
+ }
+ 
+ static void
+-mt76_testmode_free_skb(struct mt76_phy *phy)
++mt76_testmode_free_skb(struct sk_buff **tx_skb)
++{
++	dev_kfree_skb(*tx_skb);
++	*tx_skb = NULL;
++}
++
++static void
++mt76_testmode_free_skb_all(struct mt76_phy *phy)
+ {
+ 	struct mt76_testmode_data *td = &phy->test;
+ 
+-	dev_kfree_skb(td->tx_skb);
+-	td->tx_skb = NULL;
++	if (mt76_testmode_has_sta(phy)) {
++		struct mt76_testmode_sta *tm_sta;
++		int i;
++
++		mt76_testmode_for_each_sta(phy, i, tm_sta) {
++			mt76_testmode_free_skb(&tm_sta->tx_skb);
++		}
++
++		return;
++	}
++
++	mt76_testmode_free_skb(&td->tx_skb);
+ }
+ 
+-int mt76_testmode_alloc_skb(struct mt76_phy *phy, u32 len)
++static int
++mt76_testmode_alloc_skb(struct mt76_phy *phy, u32 len,
++			struct sk_buff **tx_skb, u8 *da)
+ {
+ #define MT_TXP_MAX_LEN	4095
+ 	u16 fc = IEEE80211_FTYPE_DATA | IEEE80211_STYPE_DATA |
+@@ -128,7 +190,9 @@ int mt76_testmode_alloc_skb(struct mt76_phy *phy, u32 len)
+ 	hdr->frame_control = cpu_to_le16(fc);
+ 	memcpy(hdr->addr1, td->addr[0], ETH_ALEN);
+ 	memcpy(hdr->addr2, td->addr[1], ETH_ALEN);
+-	memcpy(hdr->addr3, td->addr[2], ETH_ALEN);
++	/* memcpy(hdr->addr3, td->addr[2], ETH_ALEN); */
++	memcpy(hdr->addr3, da, ETH_ALEN);
++
+ 	skb_set_queue_mapping(head, IEEE80211_AC_BE);
+ 
+ 	info = IEEE80211_SKB_CB(head);
+@@ -152,7 +216,7 @@ int mt76_testmode_alloc_skb(struct mt76_phy *phy, u32 len)
+ 
+ 		frag = alloc_skb(frag_len, GFP_KERNEL);
+ 		if (!frag) {
+-			mt76_testmode_free_skb(phy);
++			mt76_testmode_free_skb(tx_skb);
+ 			dev_kfree_skb(head);
+ 			return -ENOMEM;
+ 		}
+@@ -165,23 +229,25 @@ int mt76_testmode_alloc_skb(struct mt76_phy *phy, u32 len)
+ 		frag_tail = &(*frag_tail)->next;
+ 	}
+ 
+-	mt76_testmode_free_skb(phy);
+-	td->tx_skb = head;
++	mt76_testmode_free_skb(tx_skb);
++	*tx_skb = head;
+ 
+ 	return 0;
+ }
+-EXPORT_SYMBOL(mt76_testmode_alloc_skb);
+ 
+-static int
+-mt76_testmode_tx_init(struct mt76_phy *phy)
++int mt76_testmode_init_skb(struct mt76_phy *phy, u32 len, u8 aid,
++			   struct sk_buff **tx_skb)
+ {
+ 	struct mt76_testmode_data *td = &phy->test;
+ 	struct ieee80211_tx_info *info;
+ 	struct ieee80211_tx_rate *rate;
+ 	u8 max_nss = hweight8(phy->antenna_mask);
++	u8 da[ETH_ALEN];
+ 	int ret;
+ 
+-	ret = mt76_testmode_alloc_skb(phy, td->tx_mpdu_len);
++	ether_addr_copy(da, phy->macaddr);
++	da[0] += aid * 4;
++	ret = mt76_testmode_alloc_skb(phy, len, tx_skb, da);
+ 	if (ret)
+ 		return ret;
+ 
+@@ -191,7 +257,7 @@ mt76_testmode_tx_init(struct mt76_phy *phy)
+ 	if (td->tx_antenna_mask)
+ 		max_nss = min_t(u8, max_nss, hweight8(td->tx_antenna_mask));
+ 
+-	info = IEEE80211_SKB_CB(td->tx_skb);
++	info = IEEE80211_SKB_CB(*tx_skb);
+ 	rate = &info->control.rates[0];
+ 	rate->count = 1;
+ 	rate->idx = td->tx_rate_idx;
+@@ -263,6 +329,28 @@ mt76_testmode_tx_init(struct mt76_phy *phy)
+ out:
+ 	return 0;
+ }
++EXPORT_SYMBOL(mt76_testmode_init_skb);
++
++static int
++mt76_testmode_tx_init(struct mt76_phy *phy)
++{
++	struct mt76_testmode_data *td = &phy->test;
++	struct mt76_testmode_sta *tm_sta;
++	int ret, i;
++
++	if (!mt76_testmode_has_sta(phy))
++		return mt76_testmode_init_skb(phy, td->tx_mpdu_len,
++					      0, &td->tx_skb);
++
++	mt76_testmode_for_each_sta(phy, i, tm_sta) {
++		ret = mt76_testmode_init_skb(phy, tm_sta->sd.tx_mpdu_len,
++					     tm_sta->sd.aid, &tm_sta->tx_skb);
++		if (ret)
++			return ret;
++	}
++
++	return 0;
++}
+ 
+ static void
+ mt76_testmode_tx_start(struct mt76_phy *phy)
+@@ -273,6 +361,17 @@ mt76_testmode_tx_start(struct mt76_phy *phy)
+ 	td->tx_queued = 0;
+ 	td->tx_done = 0;
+ 	td->tx_pending = td->tx_count;
++
++	if (mt76_testmode_has_sta(phy)) {
++		td->cur_aid = ffs(td->tm_sta_mask);
++
++		/* The actual tx count of MU packets will be pass to FW
++		 * by a mcu command in testmode.
++		 */
++		if (td->tx_rate_mode == MT76_TM_TX_MODE_HE_MU)
++			td->tx_pending = hweight16(phy->test.tm_sta_mask);
++	}
++
+ 	mt76_worker_schedule(&dev->tx_worker);
+ }
+ 
+@@ -291,7 +390,7 @@ mt76_testmode_tx_stop(struct mt76_phy *phy)
+ 	wait_event_timeout(dev->tx_wait, td->tx_done == td->tx_queued,
+ 			   MT76_TM_TIMEOUT * HZ);
+ 
+-	mt76_testmode_free_skb(phy);
++	mt76_testmode_free_skb_all(phy);
+ }
+ 
+ static inline void
+@@ -331,8 +430,11 @@ __mt76_testmode_set_state(struct mt76_phy *phy, enum mt76_testmode_state state)
+ 	struct mt76_dev *dev = phy->dev;
+ 	int err;
+ 
+-	if (prev_state == MT76_TM_STATE_TX_FRAMES)
++	if (prev_state == MT76_TM_STATE_TX_FRAMES) {
++		if (phy->test.tx_rate_mode == MT76_TM_TX_MODE_HE_MU)
++			dev->test_ops->set_state(phy, MT76_TM_STATE_IDLE);
+ 		mt76_testmode_tx_stop(phy);
++	}
+ 
+ 	if (state == MT76_TM_STATE_TX_FRAMES) {
+ 		err = mt76_testmode_tx_init(phy);
+@@ -382,7 +484,6 @@ int mt76_testmode_set_state(struct mt76_phy *phy, enum mt76_testmode_state state
+ 	}
+ 
+ 	return __mt76_testmode_set_state(phy, state);
+-
+ }
+ EXPORT_SYMBOL(mt76_testmode_set_state);
+ 
+@@ -402,6 +503,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)
+ {
+@@ -425,6 +564,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));
+@@ -446,13 +590,16 @@ int mt76_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+ 	    mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_LDPC], &td->tx_rate_ldpc, 0, 1) ||
+ 	    mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_STBC], &td->tx_rate_stbc, 0, 1) ||
+ 	    mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_LTF], &td->tx_ltf, 0, 2) ||
+-	    mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_ANTENNA],
+-			   &td->tx_antenna_mask, 0, 0xff) ||
++	    mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_ANTENNA], &td->tx_antenna_mask,
++			   1, phy->antenna_mask) ||
+ 	    mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_SPE_IDX], &td->tx_spe_idx, 0, 27) ||
+ 	    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) ||
++	    mt76_tm_get_u8(tb[MT76_TM_ATTR_RU_ALLOC], &td->ru_alloc, 0, 0xff) ||
++	    mt76_tm_get_u8(tb[MT76_TM_ATTR_RU_IDX], &td->ru_idx, 0, 68))
+ 		goto out;
+ 
+ 	if (tb[MT76_TM_ATTR_TX_LENGTH]) {
+@@ -484,8 +631,7 @@ int mt76_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+ 
+ 	if (tb[MT76_TM_ATTR_TX_POWER]) {
+ 		struct nlattr *cur;
+-		int idx = 0;
+-		int rem;
++		int rem, idx = 0;
+ 
+ 		nla_for_each_nested(cur, tb[MT76_TM_ATTR_TX_POWER], rem) {
+ 			if (nla_len(cur) != 1 ||
+@@ -505,11 +651,47 @@ int mt76_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+ 			if (nla_len(cur) != ETH_ALEN || idx >= 3)
+ 				goto out;
+ 
+-			memcpy(td->addr[idx], nla_data(cur), ETH_ALEN);
++			memcpy(td->addr[idx++], nla_data(cur), ETH_ALEN);
++		}
++	}
++
++	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 (tb[MT76_TM_ATTR_OFF_CH_SCAN_CH]) {
++		u8 ch = nla_get_u8(tb[MT76_TM_ATTR_OFF_CH_SCAN_CH]);
++		struct ieee80211_supported_band *sband;
++
++		sband = ch > 14 ? &phy->sband_5g.sband :
++				  &phy->sband_2g.sband;
++		if (ch && (ch < sband->channels[0].hw_value ||
++			   ch > sband->channels[sband->n_channels - 1].hw_value))
++			goto out;
++
++		td->off_ch_scan_ch = ch;
++
++		if (mt76_tm_get_u8(tb[MT76_TM_ATTR_OFF_CH_SCAN_CENTER_CH],
++				   &td->off_ch_scan_center_ch, ch - 6, ch + 6) ||
++		    mt76_tm_get_u8(tb[MT76_TM_ATTR_OFF_CH_SCAN_BW],
++				   &td->off_ch_scan_bw, 0, 6) ||
++		    mt76_tm_get_u8(tb[MT76_TM_ATTR_OFF_CH_SCAN_PATH],
++				   &td->off_ch_scan_path, 1, 0xff))
++			goto out;
++	}
++
+ 	if (dev->test_ops->set_params) {
+ 		err = dev->test_ops->set_params(phy, tb, state);
+ 		if (err)
+@@ -559,6 +741,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;
+ 
+@@ -571,6 +756,7 @@ int mt76_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *msg,
+ 	struct mt76_phy *phy = hw->priv;
+ 	struct mt76_dev *dev = phy->dev;
+ 	struct mt76_testmode_data *td = &phy->test;
++	struct mt76_testmode_sta_data *sd = &td->sd;
+ 	struct nlattr *tb[NUM_MT76_TM_ATTRS] = {};
+ 	int err = 0;
+ 	void *a;
+@@ -603,6 +789,23 @@ int mt76_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *msg,
+ 		goto out;
+ 	}
+ 
++	if (tb[MT76_TM_ATTR_AID]) {
++		struct mt76_testmode_sta *tm_sta;
++		u8 aid;
++
++		err = mt76_tm_get_u8(tb[MT76_TM_ATTR_AID], &aid, 1, 16);
++		if (err)
++			goto out;
++
++		tm_sta = mt76_testmode_aid_get_sta(phy, aid);
++		if (!tm_sta) {
++			err = -EINVAL;
++			goto out;
++		}
++
++		sd = &tm_sta->sd;
++	}
++
+ 	mt76_testmode_init_defaults(phy);
+ 
+ 	err = -EMSGSIZE;
+@@ -615,12 +818,8 @@ int mt76_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *msg,
+ 		goto out;
+ 
+ 	if (nla_put_u32(msg, MT76_TM_ATTR_TX_COUNT, td->tx_count) ||
+-	    nla_put_u32(msg, MT76_TM_ATTR_TX_LENGTH, td->tx_mpdu_len) ||
+ 	    nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_MODE, td->tx_rate_mode) ||
+-	    nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_NSS, td->tx_rate_nss) ||
+-	    nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_IDX, td->tx_rate_idx) ||
+ 	    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) ||
+ 	    (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_LTF) &&
+ 	     nla_put_u8(msg, MT76_TM_ATTR_TX_LTF, td->tx_ltf)) ||
+@@ -640,6 +839,15 @@ int mt76_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *msg,
+ 	     nla_put_u8(msg, MT76_TM_ATTR_FREQ_OFFSET, td->freq_offset)))
+ 		goto out;
+ 
++	if (nla_put_u8(msg, MT76_TM_ATTR_AID, sd->aid) ||
++	    nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_NSS, sd->tx_rate_nss) ||
++	    nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_IDX, sd->tx_rate_idx) ||
++	    nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_LDPC, sd->tx_rate_ldpc) ||
++	    nla_put_u8(msg, MT76_TM_ATTR_RU_ALLOC, sd->ru_alloc) ||
++	    nla_put_u8(msg, MT76_TM_ATTR_RU_IDX, sd->ru_idx) ||
++	    nla_put_u32(msg, MT76_TM_ATTR_TX_LENGTH, sd->tx_mpdu_len))
++		goto out;
++
+ 	if (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_POWER)) {
+ 		a = nla_nest_start(msg, MT76_TM_ATTR_TX_POWER);
+ 		if (!a)
+diff --git a/testmode.h b/testmode.h
+index 5e2792d8..b360d7a4 100644
+--- a/testmode.h
++++ b/testmode.h
+@@ -6,6 +6,8 @@
+ #define __MT76_TESTMODE_H
+ 
+ #define MT76_TM_TIMEOUT	10
++#define MT76_TM_EEPROM_BLOCK_SIZE	16
++#define MT76_TM_MAX_STA_NUM	16
+ 
+ /**
+  * enum mt76_testmode_attr - testmode attributes inside NL80211_ATTR_TESTDATA
+@@ -47,6 +49,20 @@
+  * @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_OFF_CH_SCAN_CH: monitored channel for off channel scan (u8)
++ * @MT76_TM_ATTR_OFF_CH_SCAN_CENTER_CH: monitored channel for off channel scan (u8)
++ * @MT76_TM_ATTR_OFF_CH_SCAN_BW: monitored bw for off channel scan (u8)
++ * @MT76_TM_ATTR_OFF_CH_SCAN_PATH: monitored rx path for off channel scan (u8)
++ *
+  */
+ enum mt76_testmode_attr {
+ 	MT76_TM_ATTR_UNSPEC,
+@@ -85,6 +101,21 @@ enum mt76_testmode_attr {
+ 
+ 	MT76_TM_ATTR_MAC_ADDRS,
+ 
++	MT76_TM_ATTR_EEPROM_ACTION,
++	MT76_TM_ATTR_EEPROM_OFFSET,
++	MT76_TM_ATTR_EEPROM_VAL,
++
++	MT76_TM_ATTR_CFG,
++
++	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_AID,
++	MT76_TM_ATTR_RU_ALLOC,
++	MT76_TM_ATTR_RU_IDX,
++
+ 	/* keep last */
+ 	NUM_MT76_TM_ATTRS,
+ 	MT76_TM_ATTR_MAX = NUM_MT76_TM_ATTRS - 1,
+@@ -101,6 +132,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 +146,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,
+@@ -195,4 +229,41 @@ enum mt76_testmode_tx_mode {
+ 
+ extern const struct nla_policy mt76_tm_policy[NUM_MT76_TM_ATTRS];
+ 
++/**
++ * 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,
++};
++
++/**
++ * enum mt76_testmode_cfg - packet tx phy mode
++ *
++ * @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_cfg {
++	MT76_TM_CFG_TSSI,
++	MT76_TM_CFG_DPD,
++	MT76_TM_CFG_RATE_POWER_OFFSET,
++	MT76_TM_CFG_THERMAL_COMP,
++
++	/* keep last */
++	NUM_MT76_TM_CFG,
++	MT76_TM_CFG_MAX = NUM_MT76_TM_CFG - 1,
++};
++
+ #endif
+diff --git a/tools/fields.c b/tools/fields.c
+index e3f69089..036406c1 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[] = {
+@@ -201,6 +202,63 @@ 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;
++
++	ap = str = strdup(val);
++
++	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]));
++
++	return;
++}
+ 
+ #define FIELD_GENERIC(_field, _name, ...)	\
+ 	[FIELD_NAME(_field)] = {			\
+@@ -250,6 +308,13 @@ 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"),
+@@ -300,10 +365,16 @@ 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(u8, TX_SPE_IDX, "tx_spe_idx"),
+ 	FIELD(u32, FREQ_OFFSET, "freq_offset"),
++	FIELD(u8, AID, "aid"),
++	FIELD_MAC(MAC_ADDRS, "mac_addrs"),
+ 	FIELD_NESTED_RO(STATS, stats, "",
+ 			.print_extra = print_extra_stats),
+ };
+@@ -322,9 +393,14 @@ 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_STATS] = { .type = NLA_NESTED },
+ };
+ 
+diff --git a/tx.c b/tx.c
+index 6b8c9dc8..ca5e6d97 100644
+--- a/tx.c
++++ b/tx.c
+@@ -245,8 +245,7 @@ void __mt76_tx_complete_skb(struct mt76_dev *dev, u16 wcid_idx, struct sk_buff *
+ 	if (mt76_is_testmode_skb(dev, skb, &hw)) {
+ 		struct mt76_phy *phy = hw->priv;
+ 
+-		if (skb == phy->test.tx_skb)
+-			phy->test.tx_done++;
++		phy->test.tx_done++;
+ 		if (phy->test.tx_queued == phy->test.tx_done)
+ 			wake_up(&dev->tx_wait);
+ 
+-- 
+2.25.1
+