[][MAC80211][Add testmode patches]
[Description]
Add testmode patches and rebase debug/muru/certification patches.
[Release-log]
N/A
Change-Id: Idd2ca16d6d957b6cb3099dc91dc81898e2fd7337
Reviewed-on: https://gerrit.mediatek.inc/c/openwrt/feeds/mtk_openwrt_feeds/+/5723666
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
++