diff --git a/openwrt_patches-21.02/423-master-mt76-testmode.patch b/openwrt_patches-21.02/423-master-mt76-testmode.patch
new file mode 100644
index 0000000..dcf7db6
--- /dev/null
+++ b/openwrt_patches-21.02/423-master-mt76-testmode.patch
@@ -0,0 +1,2020 @@
+diff --git a/package/kernel/mt76/patches/1006-mt76-mt7915-testmode-patches.patch b/package/kernel/mt76/patches/1006-mt76-mt7915-testmode-patches.patch
+new file mode 100644
+index 00000000..002d1fcb
+--- /dev/null
++++ b/package/kernel/mt76/patches/1006-mt76-mt7915-testmode-patches.patch
+@@ -0,0 +1,2014 @@
++From cdb7bc7976835e2b96ae5f19392a793d1895fd4f 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 1006/1006] mt76: mt7915: testmode patches
++
++---
++ 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   |  26 +-
++ .../net/wireless/mediatek/mt76/mt7915/mcu.c   |  13 +-
++ .../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  | 513 +++++++++++++++---
++ .../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, 1048 insertions(+), 142 deletions(-)
++
++diff --git a/mac80211.c b/mac80211.c
++index 9796419..e473227 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 5e10fe1..4b502c6 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 0a646ae..9158329 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 8903e08..cb7d096 100644
++--- a/mt76_connac_mcu.h
+++++ b/mt76_connac_mcu.h
++@@ -987,6 +987,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 aed4731..7ec48e0 100644
++--- a/mt7915/init.c
+++++ b/mt7915/init.c
++@@ -568,7 +568,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 efdc1b1..9a5bb20 100644
++--- a/mt7915/mac.c
+++++ b/mt7915/mac.c
++@@ -904,16 +904,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:
++@@ -1003,9 +1015,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,
++@@ -1472,6 +1485,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 bb77edc..29ba3ed 100644
++--- a/mt7915/mcu.c
+++++ b/mt7915/mcu.c
++@@ -289,7 +289,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
++@@ -2784,7 +2783,6 @@ int mt7915_mcu_set_chan_info(struct mt7915_phy *phy, int cmd)
++ 	struct mt7915_dev *dev = phy->dev;
++ 	struct cfg80211_chan_def *chandef = &phy->mt76->chandef;
++ 	int freq1 = chandef->center_freq1;
++-	bool ext_phy = phy != &dev->phy;
++ 	struct {
++ 		u8 control_ch;
++ 		u8 center_ch;
++@@ -2814,14 +2812,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 (ext_phy)
++-			req.rx_streams >>= dev->chainshift;
++ 	}
++ #endif
++ 
++@@ -2887,14 +2880,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 30211cb..4b78468 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 1b14bba..207941d 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,
++@@ -125,6 +126,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 b3abe77..db4c6bc 100644
++--- a/mt7915/mt7915.h
+++++ b/mt7915/mt7915.h
++@@ -539,7 +539,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 71f325a..dcfb3f8 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,
++@@ -171,6 +172,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))
++@@ -179,6 +186,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)
++@@ -437,8 +447,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 6605e24..9f13919 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)
++@@ -320,7 +446,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;
++ 
++@@ -332,7 +458,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);
++@@ -344,18 +470,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;
++ 	}
++ 
++@@ -375,8 +511,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 |
++@@ -389,31 +530,124 @@ 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_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;
++ 
++-	mt7915_mcu_add_bss_info(phy, phy->monitor_vif, en);
++-	mt7915_mcu_add_sta(dev, phy->monitor_vif, NULL, en);
+++	if (!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
++@@ -426,59 +660,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
++@@ -487,12 +784,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);
++ 	}
++ }
++@@ -631,6 +931,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)
++ {
++@@ -641,6 +966,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
++@@ -700,9 +1031,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);
++@@ -717,12 +1045,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)
++@@ -766,19 +1090,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);
+++}
+++
+++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;
++ 
++-	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;
+++	if (is_mt7986(&dev->mt76))
+++		goto out;
++ 
++-	return 0;
+++	/* 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 5573ac3..d22aabe 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 382b456..9da490c 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 5e2792d..b360d7a 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 e3f6908..036406c 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 6b8c9dc..ca5e6d9 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
++