Gitiles
Code Review Sign In
git01.mediatek.com / openwrt / feeds / mtk-openwrt-feeds / a1c200a1695d28e9c02d00c01cd0633da5834893 / . / openwrt_patches-21.02 / 423-master-mt76-testmode.patch
blob: dcf7db6fc52589b17b43e10f39b9ae44e87db581 [file] [log] [blame]
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
+