mt76: mt7915: implement testmode tx support
authorShayne Chen <shayne.chen@mediatek.com>
Thu, 22 Oct 2020 02:28:17 +0000 (10:28 +0800)
committerFelix Fietkau <nbd@nbd.name>
Fri, 4 Dec 2020 13:31:13 +0000 (14:31 +0100)
Support testmode tx for MT7915A, including tx streams and HE rate
settings.

Reviewed-by: Ryder Lee <ryder.lee@mediatek.com>
Signed-off-by: Shayne Chen <shayne.chen@mediatek.com>
Signed-off-by: Felix Fietkau <nbd@nbd.name>
drivers/net/wireless/mediatek/mt76/mt7915/Makefile
drivers/net/wireless/mediatek/mt76/mt7915/init.c
drivers/net/wireless/mediatek/mt76/mt7915/mac.c
drivers/net/wireless/mediatek/mt76/mt7915/main.c
drivers/net/wireless/mediatek/mt76/mt7915/mcu.c
drivers/net/wireless/mediatek/mt76/mt7915/mcu.h
drivers/net/wireless/mediatek/mt76/mt7915/mt7915.h
drivers/net/wireless/mediatek/mt76/mt7915/regs.h
drivers/net/wireless/mediatek/mt76/mt7915/testmode.c [new file with mode: 0644]
drivers/net/wireless/mediatek/mt76/mt7915/testmode.h [new file with mode: 0644]

index 57fe726..cc2054d 100644 (file)
@@ -4,3 +4,5 @@ obj-$(CONFIG_MT7915E) += mt7915e.o
 
 mt7915e-y := pci.o init.o dma.o eeprom.o main.o mcu.o mac.o \
             debugfs.o
+
+mt7915e-$(CONFIG_NL80211_TESTMODE) += testmode.o
index 8a3b701..fe369f8 100644 (file)
@@ -264,6 +264,7 @@ mt7915_init_wiphy(struct ieee80211_hw *hw)
 
        ieee80211_hw_set(hw, HAS_RATE_CONTROL);
        ieee80211_hw_set(hw, SUPPORTS_TX_ENCAP_OFFLOAD);
+       ieee80211_hw_set(hw, WANT_MONITOR_VIF);
 
        hw->max_tx_fragments = 4;
 }
@@ -620,9 +621,6 @@ int mt7915_register_ext_phy(struct mt7915_dev *dev)
        mphy->hw->wiphy->perm_addr[0] |= 2;
        mphy->hw->wiphy->perm_addr[0] ^= BIT(7);
 
-       /* The second interface does not get any packets unless it has a vif */
-       ieee80211_hw_set(mphy->hw, WANT_MONITOR_VIF);
-
        ret = mt76_register_phy(mphy);
        if (ret)
                ieee80211_free_hw(mphy->hw);
@@ -678,6 +676,10 @@ int mt7915_register_device(struct mt7915_dev *dev)
        mt7915_cap_dbdc_disable(dev);
        dev->phy.dfs_state = -1;
 
+#ifdef CONFIG_NL80211_TESTMODE
+       dev->mt76.test_ops = &mt7915_testmode_ops;
+#endif
+
        ret = mt76_register_device(&dev->mt76, true, mt7915_rates,
                                   ARRAY_SIZE(mt7915_rates));
        if (ret)
index 521d0c2..923d87b 100644 (file)
@@ -563,6 +563,108 @@ int mt7915_mac_fill_rx(struct mt7915_dev *dev, struct sk_buff *skb)
 }
 
 static void
+mt7915_mac_write_txwi_tm(struct mt7915_dev *dev, struct mt76_phy *mphy,
+                        __le32 *txwi, struct sk_buff *skb)
+{
+#ifdef CONFIG_NL80211_TESTMODE
+       struct mt76_testmode_data *td = &dev->mt76.test;
+       u8 rate_idx = td->tx_rate_idx;
+       u8 nss = td->tx_rate_nss;
+       u8 bw, mode;
+       u16 rateval = 0;
+       u32 val;
+
+       if (skb != dev->mt76.test.tx_skb)
+               return;
+
+       switch (td->tx_rate_mode) {
+       case MT76_TM_TX_MODE_CCK:
+               mode = MT_PHY_TYPE_CCK;
+               break;
+       case MT76_TM_TX_MODE_HT:
+               nss = 1 + (rate_idx >> 3);
+               mode = MT_PHY_TYPE_HT;
+               break;
+       case MT76_TM_TX_MODE_VHT:
+               mode = MT_PHY_TYPE_VHT;
+               break;
+       case MT76_TM_TX_MODE_HE_SU:
+               mode = MT_PHY_TYPE_HE_SU;
+               break;
+       case MT76_TM_TX_MODE_HE_EXT_SU:
+               mode = MT_PHY_TYPE_HE_EXT_SU;
+               break;
+       case MT76_TM_TX_MODE_HE_TB:
+               mode = MT_PHY_TYPE_HE_TB;
+               break;
+       case MT76_TM_TX_MODE_HE_MU:
+               mode = MT_PHY_TYPE_HE_MU;
+               break;
+       case MT76_TM_TX_MODE_OFDM:
+       default:
+               mode = MT_PHY_TYPE_OFDM;
+               break;
+       }
+
+       switch (mphy->chandef.width) {
+       case NL80211_CHAN_WIDTH_40:
+               bw = 1;
+               break;
+       case NL80211_CHAN_WIDTH_80:
+               bw = 2;
+               break;
+       case NL80211_CHAN_WIDTH_80P80:
+       case NL80211_CHAN_WIDTH_160:
+               bw = 3;
+               break;
+       default:
+               bw = 0;
+               break;
+       }
+
+       if (td->tx_rate_stbc && nss == 1) {
+               nss++;
+               rateval |= MT_TX_RATE_STBC;
+       }
+
+       rateval |= FIELD_PREP(MT_TX_RATE_IDX, rate_idx) |
+                  FIELD_PREP(MT_TX_RATE_MODE, mode) |
+                  FIELD_PREP(MT_TX_RATE_NSS, nss - 1);
+
+       txwi[2] |= cpu_to_le32(MT_TXD2_FIX_RATE);
+
+       le32p_replace_bits(&txwi[3], 1, MT_TXD3_REM_TX_COUNT);
+       if (td->tx_rate_mode < MT76_TM_TX_MODE_HT)
+               txwi[3] |= cpu_to_le32(MT_TXD3_BA_DISABLE);
+
+       val = MT_TXD6_FIXED_BW |
+             FIELD_PREP(MT_TXD6_BW, bw) |
+             FIELD_PREP(MT_TXD6_TX_RATE, rateval) |
+             FIELD_PREP(MT_TXD6_SGI, td->tx_rate_sgi);
+
+       /* for HE_SU/HE_EXT_SU PPDU
+        * - 1x, 2x, 4x LTF + 0.8us GI
+        * - 2x LTF + 1.6us GI, 4x LTF + 3.2us GI
+        * for HE_MU PPDU
+        * - 2x, 4x LTF + 0.8us GI
+        * - 2x LTF + 1.6us GI, 4x LTF + 3.2us GI
+        * for HE_TB PPDU
+        * - 1x, 2x LTF + 1.6us GI
+        * - 4x LTF + 3.2us GI
+        */
+       if (mode >= MT_PHY_TYPE_HE_SU)
+               val |= FIELD_PREP(MT_TXD6_HELTF, td->tx_ltf);
+
+       if (td->tx_rate_ldpc)
+               val |= MT_TXD6_LDPC;
+
+       txwi[6] |= cpu_to_le32(val);
+       txwi[7] |= cpu_to_le32(FIELD_PREP(MT_TXD7_SPE_IDX,
+                                         dev->test.spe_idx));
+#endif
+}
+
+static void
 mt7915_mac_write_txwi_8023(struct mt7915_dev *dev, __le32 *txwi,
                           struct sk_buff *skb, struct mt76_wcid *wcid)
 {
@@ -761,6 +863,9 @@ void mt7915_mac_write_txwi(struct mt7915_dev *dev, __le32 *txwi,
                txwi[6] |= cpu_to_le32(val);
                txwi[3] |= cpu_to_le32(MT_TXD3_BA_DISABLE);
        }
+
+       if (mt76_testmode_enabled(&dev->mt76))
+               mt7915_mac_write_txwi_tm(dev, mphy, txwi, skb);
 }
 
 int mt7915_tx_prepare_skb(struct mt76_dev *mdev, void *txwi_ptr,
@@ -881,6 +986,18 @@ mt7915_tx_complete_status(struct mt76_dev *mdev, struct sk_buff *skb,
 
        hw = mt76_tx_status_get_hw(mdev, skb);
 
+#ifdef CONFIG_NL80211_TESTMODE
+       if (skb == mdev->test.tx_skb) {
+               struct mt7915_phy *phy = mt7915_hw_phy(hw);
+               struct ieee80211_vif *vif = phy->monitor_vif;
+               struct mt7915_vif *mvif = (struct mt7915_vif *)vif->drv_priv;
+
+               mt76_tx_complete_skb(mdev, mvif->sta.wcid.idx, skb);
+
+               return;
+       }
+#endif
+
        if (info->flags & IEEE80211_TX_CTL_AMPDU)
                info->flags |= IEEE80211_TX_STAT_AMPDU;
 
index 989747c..92f860b 100644 (file)
@@ -44,13 +44,14 @@ static int mt7915_start(struct ieee80211_hw *hw)
                mt7915_mac_enable_nf(dev, 1);
        }
 
-       mt7915_mcu_set_sku_en(phy, true);
+       mt7915_mcu_set_sku_en(phy, !mt76_testmode_enabled(&dev->mt76));
        mt7915_mcu_set_chan_info(phy, MCU_EXT_CMD_SET_RX_PATH);
 
        set_bit(MT76_STATE_RUNNING, &phy->mt76->state);
 
-       ieee80211_queue_delayed_work(hw, &phy->mac_work,
-                                    MT7915_WATCHDOG_TIME);
+       if (!mt76_testmode_enabled(&dev->mt76))
+               ieee80211_queue_delayed_work(hw, &phy->mac_work,
+                                            MT7915_WATCHDOG_TIME);
 
        if (!running)
                mt7915_mac_reset_counters(phy);
@@ -69,6 +70,8 @@ static void mt7915_stop(struct ieee80211_hw *hw)
 
        mutex_lock(&dev->mt76.mutex);
 
+       mt76_testmode_reset(&dev->mt76, true);
+
        clear_bit(MT76_STATE_RUNNING, &phy->mt76->state);
 
        if (phy != &dev->phy) {
@@ -150,6 +153,12 @@ static int mt7915_add_interface(struct ieee80211_hw *hw,
 
        mutex_lock(&dev->mt76.mutex);
 
+       mt76_testmode_reset(&dev->mt76, true);
+
+       if (vif->type == NL80211_IFTYPE_MONITOR &&
+           is_zero_ether_addr(vif->addr))
+               phy->monitor_vif = vif;
+
        mvif->idx = ffs(~phy->mt76->vif_mask) - 1;
        if (mvif->idx >= MT7915_MAX_INTERFACES) {
                ret = -ENOSPC;
@@ -218,6 +227,13 @@ static void mt7915_remove_interface(struct ieee80211_hw *hw,
 
        /* TODO: disable beacon for the bss */
 
+       mutex_lock(&dev->mt76.mutex);
+       mt76_testmode_reset(&dev->mt76, true);
+       mutex_unlock(&dev->mt76.mutex);
+
+       if (vif == phy->monitor_vif)
+               phy->monitor_vif = NULL;
+
        mt7915_mcu_add_dev_info(phy, vif, false);
 
        rcu_assign_pointer(dev->mt76.wcid[idx], NULL);
@@ -252,7 +268,7 @@ static void mt7915_init_dfs_state(struct mt7915_phy *phy)
        phy->dfs_state = -1;
 }
 
-static int mt7915_set_channel(struct mt7915_phy *phy)
+int mt7915_set_channel(struct mt7915_phy *phy)
 {
        struct mt7915_dev *dev = phy->dev;
        int ret;
@@ -281,8 +297,10 @@ out:
        mutex_unlock(&dev->mt76.mutex);
 
        mt76_txq_schedule_all(phy->mt76);
-       ieee80211_queue_delayed_work(phy->mt76->hw, &phy->mac_work,
-                                    MT7915_WATCHDOG_TIME);
+
+       if (!mt76_testmode_enabled(&dev->mt76))
+               ieee80211_queue_delayed_work(phy->mt76->hw, &phy->mac_work,
+                                            MT7915_WATCHDOG_TIME);
 
        return ret;
 }
@@ -346,6 +364,13 @@ static int mt7915_config(struct ieee80211_hw *hw, u32 changed)
        int ret;
 
        if (changed & IEEE80211_CONF_CHANGE_CHANNEL) {
+#ifdef CONFIG_NL80211_TESTMODE
+               if (dev->mt76.test.state != MT76_TM_STATE_OFF) {
+                       mutex_lock(&dev->mt76.mutex);
+                       mt76_testmode_reset(&dev->mt76, false);
+                       mutex_unlock(&dev->mt76.mutex);
+               }
+#endif
                ieee80211_stop_queues(hw);
                ret = mt7915_set_channel(phy);
                if (ret)
@@ -370,6 +395,7 @@ static int mt7915_config(struct ieee80211_hw *hw, u32 changed)
                        phy->rxfilter &= ~MT_WF_RFCR_DROP_OTHER_UC;
 
                mt76_rmw_field(dev, MT_DMA_DCR0, MT_DMA_DCR0_RXD_G5_EN, enabled);
+               mt76_testmode_reset(&dev->mt76, true);
                mt76_wr(dev, MT_WF_RFCR(band), phy->rxfilter);
        }
 
@@ -887,6 +913,8 @@ const struct ieee80211_ops mt7915_ops = {
        .set_coverage_class = mt7915_set_coverage_class,
        .sta_statistics = mt7915_sta_statistics,
        .sta_set_4addr = mt7915_sta_set_4addr,
+       CFG80211_TESTMODE_CMD(mt76_testmode_cmd)
+       CFG80211_TESTMODE_DUMP(mt76_testmode_dump)
 #ifdef CONFIG_MAC80211_DEBUGFS
        .sta_add_debugfs = mt7915_sta_add_debugfs,
 #endif
index 1a630e5..82f80df 100644 (file)
@@ -3166,6 +3166,15 @@ int mt7915_mcu_set_chan_info(struct mt7915_phy *phy, int cmd)
                .channel_band = chandef->chan->band,
        };
 
+#ifdef CONFIG_NL80211_TESTMODE
+       if (dev->mt76.test.tx_antenna_mask &&
+           (dev->mt76.test.state == MT76_TM_STATE_TX_FRAMES ||
+            dev->mt76.test.state == MT76_TM_STATE_RX_FRAMES)) {
+               req.tx_streams_num = fls(dev->mt76.test.tx_antenna_mask);
+               req.rx_streams = dev->mt76.test.tx_antenna_mask;
+       }
+#endif
+
        if (dev->mt76.hw->conf.flags & IEEE80211_CONF_OFFCHANNEL)
                req.switch_reason = CH_SWITCH_SCAN_BYPASS_DPD;
        else if ((chandef->chan->flags & IEEE80211_CHAN_RADAR) &&
@@ -3287,6 +3296,28 @@ int mt7915_mcu_set_sku(struct mt7915_phy *phy)
                                 sizeof(req), true);
 }
 
+int mt7915_mcu_set_test_param(struct mt7915_dev *dev, u8 param, bool test_mode,
+                             u8 en)
+{
+       struct {
+               u8 test_mode_en;
+               u8 param_idx;
+               u8 _rsv[2];
+
+               u8 enable;
+               u8 _rsv2[3];
+
+               u8 pad[8];
+       } __packed req = {
+               .test_mode_en = test_mode,
+               .param_idx = param,
+               .enable = en,
+       };
+
+       return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD_ATE_CTRL, &req,
+                                sizeof(req), false);
+}
+
 int mt7915_mcu_set_sku_en(struct mt7915_phy *phy, bool enable)
 {
        struct mt7915_dev *dev = phy->dev;
index 5f23f27..0a7e9d2 100644 (file)
@@ -46,6 +46,10 @@ enum {
        MCU_EXT_EVENT_RATE_REPORT = 0x87,
 };
 
+enum {
+       MCU_ATE_SET_TRX = 0x1,
+};
+
 struct mt7915_mcu_rxd {
        __le32 rxd[6];
 
@@ -216,6 +220,7 @@ enum {
        MCU_EXT_CMD_WTBL_UPDATE = 0x32,
        MCU_EXT_CMD_SET_DRR_CTRL = 0x36,
        MCU_EXT_CMD_SET_RDD_CTRL = 0x3a,
+       MCU_EXT_CMD_ATE_CTRL = 0x3d,
        MCU_EXT_CMD_PROTECT_CTRL = 0x3e,
        MCU_EXT_CMD_MAC_INIT_CTRL = 0x46,
        MCU_EXT_CMD_RX_HDR_TRANS = 0x47,
index 89cabda..9575285 100644 (file)
@@ -112,6 +112,8 @@ struct mt7915_phy {
 
        struct ieee80211_sband_iftype_data iftype[2][NUM_NL80211_IFTYPES];
 
+       struct ieee80211_vif *monitor_vif;
+
        u32 rxfilter;
        u64 omac_mask;
 
@@ -164,6 +166,14 @@ struct mt7915_dev {
        s8 **rate_power; /* TODO: use mt76_rate_power */
 
        bool fw_debug;
+
+#ifdef CONFIG_NL80211_TESTMODE
+       struct {
+               u32 *reg_backup;
+
+               u8 spe_idx;
+       } test;
+#endif
 };
 
 enum {
@@ -253,6 +263,7 @@ static inline u8 mt7915_lmac_mapping(struct mt7915_dev *dev, u8 ac)
 
 extern const struct ieee80211_ops mt7915_ops;
 extern struct pci_driver mt7915_pci_driver;
+extern const struct mt76_testmode_ops mt7915_testmode_ops;
 
 u32 mt7915_reg_map(struct mt7915_dev *dev, u32 addr);
 
@@ -298,6 +309,7 @@ int mt7915_mcu_add_rate_ctrl(struct mt7915_dev *dev, struct ieee80211_vif *vif,
                             struct ieee80211_sta *sta);
 int mt7915_mcu_add_smps(struct mt7915_dev *dev, struct ieee80211_vif *vif,
                        struct ieee80211_sta *sta);
+int mt7915_set_channel(struct mt7915_phy *phy);
 int mt7915_mcu_set_chan_info(struct mt7915_phy *phy, int cmd);
 int mt7915_mcu_set_tx(struct mt7915_dev *dev, struct ieee80211_vif *vif);
 int mt7915_mcu_set_fixed_rate(struct mt7915_dev *dev,
@@ -306,6 +318,8 @@ int mt7915_mcu_set_eeprom(struct mt7915_dev *dev);
 int mt7915_mcu_get_eeprom(struct mt7915_dev *dev, u32 offset);
 int mt7915_mcu_set_mac(struct mt7915_dev *dev, int band, bool enable,
                       bool hdr_trans);
+int mt7915_mcu_set_test_param(struct mt7915_dev *dev, u8 param, bool test_mode,
+                             u8 en);
 int mt7915_mcu_set_scs(struct mt7915_dev *dev, u8 band, bool enable);
 int mt7915_mcu_set_ser(struct mt7915_dev *dev, u8 action, u8 set, u8 band);
 int mt7915_mcu_set_rts_thresh(struct mt7915_phy *phy, u32 val);
index 234b87d..fded019 100644 (file)
@@ -51,6 +51,9 @@
 #define MT_WF_TMAC_BASE(_band)         ((_band) ? 0xa1000 : 0x21000)
 #define MT_WF_TMAC(_band, ofs)         (MT_WF_TMAC_BASE(_band) + (ofs))
 
+#define MT_TMAC_TCR0(_band)            MT_WF_TMAC(_band, 0)
+#define MT_TMAC_TCR0_TBTT_STOP_CTRL    BIT(25)
+
 #define MT_TMAC_CDTR(_band)            MT_WF_TMAC(_band, 0x090)
 #define MT_TMAC_ODTR(_band)            MT_WF_TMAC(_band, 0x094)
 #define MT_TIMEOUT_VAL_PLCP            GENMASK(15, 0)
@@ -67,6 +70,9 @@
 #define MT_TMAC_CTCR0_INS_DDLMT_EN             BIT(17)
 #define MT_TMAC_CTCR0_INS_DDLMT_VHT_SMPDU_EN   BIT(18)
 
+#define MT_TMAC_TRCR0(_band)           MT_WF_TMAC(_band, 0x09c)
+#define MT_TMAC_TFCR0(_band)           MT_WF_TMAC(_band, 0x1e0)
+
 /* DMA Band 0 */
 #define MT_WF_DMA_BASE                 0x21e00
 #define MT_WF_DMA(ofs)                 (MT_WF_DMA_BASE + (ofs))
 #define MT_WF_AGG_BASE(_band)          ((_band) ? 0xa0800 : 0x20800)
 #define MT_WF_AGG(_band, ofs)          (MT_WF_AGG_BASE(_band) + (ofs))
 
+#define MT_AGG_AWSCR0(_band, _n)       MT_WF_AGG(_band, 0x05c + (_n) * 4)
+#define MT_AGG_PCR0(_band, _n)         MT_WF_AGG(_band, 0x06c + (_n) * 4)
+#define MT_AGG_PCR0_MM_PROT            BIT(0)
+#define MT_AGG_PCR0_GF_PROT            BIT(1)
+#define MT_AGG_PCR0_BW20_PROT          BIT(2)
+#define MT_AGG_PCR0_BW40_PROT          BIT(4)
+#define MT_AGG_PCR0_BW80_PROT          BIT(6)
+#define MT_AGG_PCR0_ERP_PROT           GENMASK(12, 8)
+#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_ACR0(_band)             MT_WF_AGG(_band, 0x084)
 #define MT_AGG_ACR_CFEND_RATE          GENMASK(13, 0)
 #define MT_AGG_ACR_BAR_RATE            GENMASK(29, 16)
 
+#define MT_AGG_MRCR(_band)             MT_WF_AGG(_band, 0x098)
+#define MT_AGG_MRCR_BAR_CNT_LIMIT      GENMASK(15, 12)
+#define MT_AGG_MRCR_LAST_RTS_CTS_RN    BIT(6)
+#define MT_AGG_MRCR_RTS_FAIL_LIMIT     GENMASK(11, 7)
+#define MT_AGG_MRCR_TXCMD_RTS_FAIL_LIMIT       GENMASK(28, 24)
+
+#define MT_AGG_ATCR1(_band)            MT_WF_AGG(_band, 0x0f0)
+#define MT_AGG_ATCR3(_band)            MT_WF_AGG(_band, 0x0f4)
+
 /* ARB: band 0(0x20c00), band 1(0xa0c00) */
 #define MT_WF_ARB_BASE(_band)          ((_band) ? 0xa0c00 : 0x20c00)
 #define MT_WF_ARB(_band, ofs)          (MT_WF_ARB_BASE(_band) + (ofs))
 #define MT_ARB_SCR_TX_DISABLE          BIT(8)
 #define MT_ARB_SCR_RX_DISABLE          BIT(9)
 
+#define MT_ARB_DRNGR0(_band, _n)       MT_WF_ARB(_band, 0x194 + (_n) * 4)
+
 /* RMAC: band 0(0x21400), band 1(0xa1400) */
 #define MT_WF_RMAC_BASE(_band)         ((_band) ? 0xa1400 : 0x21400)
 #define MT_WF_RMAC(_band, ofs)         (MT_WF_RMAC_BASE(_band) + (ofs))
diff --git a/drivers/net/wireless/mediatek/mt76/mt7915/testmode.c b/drivers/net/wireless/mediatek/mt76/mt7915/testmode.c
new file mode 100644 (file)
index 0000000..0b90ecb
--- /dev/null
@@ -0,0 +1,180 @@
+// SPDX-License-Identifier: ISC
+/* Copyright (C) 2020 MediaTek Inc. */
+
+#include "mt7915.h"
+#include "mac.h"
+#include "mcu.h"
+#include "testmode.h"
+
+struct reg_band {
+       u32 band[2];
+};
+
+#define REG_BAND(_reg) \
+       { .band[0] = MT_##_reg(0), .band[1] = MT_##_reg(1) }
+#define REG_BAND_IDX(_reg, _idx) \
+       { .band[0] = MT_##_reg(0, _idx), .band[1] = MT_##_reg(1, _idx) }
+
+static const struct reg_band reg_backup_list[] = {
+       REG_BAND_IDX(AGG_PCR0, 0),
+       REG_BAND_IDX(AGG_PCR0, 1),
+       REG_BAND_IDX(AGG_AWSCR0, 0),
+       REG_BAND_IDX(AGG_AWSCR0, 1),
+       REG_BAND_IDX(AGG_AWSCR0, 2),
+       REG_BAND_IDX(AGG_AWSCR0, 3),
+       REG_BAND(AGG_MRCR),
+       REG_BAND(TMAC_TFCR0),
+       REG_BAND(TMAC_TCR0),
+       REG_BAND(AGG_ATCR1),
+       REG_BAND(AGG_ATCR3),
+       REG_BAND(TMAC_TRCR0),
+       REG_BAND(TMAC_ICR0),
+       REG_BAND_IDX(ARB_DRNGR0, 0),
+       REG_BAND_IDX(ARB_DRNGR0, 1),
+};
+
+static int
+mt7915_tm_mode_ctrl(struct mt7915_dev *dev, bool enable)
+{
+       struct {
+               u8 format_id;
+               bool enable;
+               u8 rsv[2];
+       } __packed req = {
+               .format_id = 0x6,
+               .enable = enable,
+       };
+
+       return mt76_mcu_send_msg(&dev->mt76,
+                                MCU_EXT_CMD_TX_POWER_FEATURE_CTRL,
+                                &req, sizeof(req), false);
+}
+
+static int
+mt7915_tm_set_trx(struct mt7915_dev *dev, struct mt7915_phy *phy,
+                 int type, bool en)
+{
+       struct mt7915_tm_cmd req = {
+               .testmode_en = 1,
+               .param_idx = MCU_ATE_SET_TRX,
+               .param.trx.type = type,
+               .param.trx.enable = en,
+               .param.trx.band = phy != &dev->phy,
+       };
+
+       return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD_ATE_CTRL, &req,
+                                sizeof(req), false);
+}
+
+static void
+mt7915_tm_reg_backup_restore(struct mt7915_dev *dev, struct mt7915_phy *phy)
+{
+       int n_regs = ARRAY_SIZE(reg_backup_list);
+       bool ext_phy = phy != &dev->phy;
+       u32 *b = dev->test.reg_backup;
+       int i;
+
+       if (dev->mt76.test.state == MT76_TM_STATE_OFF) {
+               for (i = 0; i < n_regs; i++)
+                       mt76_wr(dev, reg_backup_list[i].band[ext_phy], b[i]);
+               return;
+       }
+
+       if (b)
+               return;
+
+       b = devm_kzalloc(dev->mt76.dev, 4 * n_regs, GFP_KERNEL);
+       if (!b)
+               return;
+
+       dev->test.reg_backup = b;
+       for (i = 0; i < n_regs; i++)
+               b[i] = mt76_rr(dev, reg_backup_list[i].band[ext_phy]);
+
+       mt76_clear(dev, MT_AGG_PCR0(ext_phy, 0), MT_AGG_PCR0_MM_PROT |
+                  MT_AGG_PCR0_GF_PROT | MT_AGG_PCR0_ERP_PROT |
+                  MT_AGG_PCR0_VHT_PROT | MT_AGG_PCR0_BW20_PROT |
+                  MT_AGG_PCR0_BW40_PROT | MT_AGG_PCR0_BW80_PROT);
+       mt76_set(dev, MT_AGG_PCR0(ext_phy, 0), MT_AGG_PCR0_PTA_WIN_DIS);
+
+       mt76_wr(dev, MT_AGG_PCR0(ext_phy, 1), MT_AGG_PCR1_RTS0_NUM_THRES |
+               MT_AGG_PCR1_RTS0_LEN_THRES);
+
+       mt76_clear(dev, MT_AGG_MRCR(ext_phy), MT_AGG_MRCR_BAR_CNT_LIMIT |
+                  MT_AGG_MRCR_LAST_RTS_CTS_RN | MT_AGG_MRCR_RTS_FAIL_LIMIT |
+                  MT_AGG_MRCR_TXCMD_RTS_FAIL_LIMIT);
+
+       mt76_rmw(dev, MT_AGG_MRCR(ext_phy), MT_AGG_MRCR_RTS_FAIL_LIMIT |
+                MT_AGG_MRCR_TXCMD_RTS_FAIL_LIMIT,
+                FIELD_PREP(MT_AGG_MRCR_RTS_FAIL_LIMIT, 1) |
+                FIELD_PREP(MT_AGG_MRCR_TXCMD_RTS_FAIL_LIMIT, 1));
+
+       mt76_wr(dev, MT_TMAC_TFCR0(ext_phy), 0);
+       mt76_clear(dev, MT_TMAC_TCR0(ext_phy), MT_TMAC_TCR0_TBTT_STOP_CTRL);
+}
+
+static void
+mt7915_tm_init(struct mt7915_dev *dev)
+{
+       bool en = !(dev->mt76.test.state == MT76_TM_STATE_OFF);
+
+       if (!test_bit(MT76_STATE_RUNNING, &dev->phy.mt76->state))
+               return;
+
+       mt7915_tm_mode_ctrl(dev, en);
+       mt7915_tm_reg_backup_restore(dev, &dev->phy);
+       mt7915_tm_set_trx(dev, &dev->phy, TM_MAC_TXRX, !en);
+}
+
+static void
+mt7915_tm_set_tx_frames(struct mt7915_dev *dev, 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 sk_buff *skb = dev->mt76.test.tx_skb;
+       struct ieee80211_tx_info *info;
+
+       mt7915_tm_set_trx(dev, &dev->phy, TM_MAC_RX_RXV, false);
+
+       if (en) {
+               u8 tx_ant = dev->mt76.test.tx_antenna_mask;
+
+               mutex_unlock(&dev->mt76.mutex);
+               mt7915_set_channel(&dev->phy);
+               mutex_lock(&dev->mt76.mutex);
+
+               mt7915_mcu_set_chan_info(&dev->phy, MCU_EXT_CMD_SET_RX_PATH);
+               dev->test.spe_idx = spe_idx_map[tx_ant];
+       }
+
+       mt7915_tm_set_trx(dev, &dev->phy, TM_MAC_TX, en);
+
+       if (!en || !skb)
+               return;
+
+       info = IEEE80211_SKB_CB(skb);
+       info->control.vif = dev->phy.monitor_vif;
+}
+
+static int
+mt7915_tm_set_state(struct mt76_dev *mdev, enum mt76_testmode_state state)
+{
+       struct mt7915_dev *dev = container_of(mdev, struct mt7915_dev, mt76);
+       struct mt76_testmode_data *td = &mdev->test;
+       enum mt76_testmode_state prev_state = td->state;
+
+       mdev->test.state = state;
+
+       if (prev_state == MT76_TM_STATE_TX_FRAMES)
+               mt7915_tm_set_tx_frames(dev, false);
+       else if (state == MT76_TM_STATE_TX_FRAMES)
+               mt7915_tm_set_tx_frames(dev, true);
+       else if (prev_state == MT76_TM_STATE_OFF || state == MT76_TM_STATE_OFF)
+               mt7915_tm_init(dev);
+
+       return 0;
+}
+
+const struct mt76_testmode_ops mt7915_testmode_ops = {
+       .set_state = mt7915_tm_set_state,
+};
diff --git a/drivers/net/wireless/mediatek/mt76/mt7915/testmode.h b/drivers/net/wireless/mediatek/mt76/mt7915/testmode.h
new file mode 100644 (file)
index 0000000..04f4a2c
--- /dev/null
@@ -0,0 +1,34 @@
+// SPDX-License-Identifier: ISC
+/* Copyright (C) 2020 MediaTek Inc. */
+
+#ifndef __MT7915_TESTMODE_H
+#define __MT7915_TESTMODE_H
+
+struct mt7915_tm_trx {
+       u8 type;
+       u8 enable;
+       u8 band;
+       u8 rsv;
+};
+
+struct mt7915_tm_cmd {
+       u8 testmode_en;
+       u8 param_idx;
+       u8 _rsv[2];
+       union {
+               __le32 data;
+               struct mt7915_tm_trx trx;
+               u8 test[72];
+       } param;
+} __packed;
+
+enum {
+       TM_MAC_TX = 1,
+       TM_MAC_RX,
+       TM_MAC_TXRX,
+       TM_MAC_TXRX_RXV,
+       TM_MAC_RXV,
+       TM_MAC_RX_RXV,
+};
+
+#endif