From: Lorenzo Bianconi <[email protected]>
We add the testmode support to access the testmode feature provided
by the MT7921 firmware.
Co-developed-by: Sean Wang <[email protected]>
Signed-off-by: Sean Wang <[email protected]>
Signed-off-by: Lorenzo Bianconi <[email protected]>
---
.../wireless/mediatek/mt76/mt76_connac_mcu.h | 1 +
.../wireless/mediatek/mt76/mt7921/Makefile | 1 +
.../net/wireless/mediatek/mt76/mt7921/init.c | 2 +
.../net/wireless/mediatek/mt76/mt7921/main.c | 2 +
.../net/wireless/mediatek/mt76/mt7921/mcu.h | 26 +++
.../wireless/mediatek/mt76/mt7921/mt7921.h | 4 +
.../wireless/mediatek/mt76/mt7921/testmode.c | 173 ++++++++++++++++++
drivers/net/wireless/mediatek/mt76/testmode.h | 2 +
8 files changed, 211 insertions(+)
create mode 100644 drivers/net/wireless/mediatek/mt76/mt7921/testmode.c
diff --git a/drivers/net/wireless/mediatek/mt76/mt76_connac_mcu.h b/drivers/net/wireless/mediatek/mt76/mt76_connac_mcu.h
index 1c73beb22677..a37090cac019 100644
--- a/drivers/net/wireless/mediatek/mt76/mt76_connac_mcu.h
+++ b/drivers/net/wireless/mediatek/mt76/mt76_connac_mcu.h
@@ -548,6 +548,7 @@ enum {
/* offload mcu commands */
enum {
+ MCU_CMD_TEST_CTRL = MCU_CE_PREFIX | 0x01,
MCU_CMD_START_HW_SCAN = MCU_CE_PREFIX | 0x03,
MCU_CMD_SET_PS_PROFILE = MCU_CE_PREFIX | 0x05,
MCU_CMD_SET_CHAN_DOMAIN = MCU_CE_PREFIX | 0x0f,
diff --git a/drivers/net/wireless/mediatek/mt76/mt7921/Makefile b/drivers/net/wireless/mediatek/mt76/mt7921/Makefile
index e531666f9fb4..8361f9558ea1 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7921/Makefile
+++ b/drivers/net/wireless/mediatek/mt76/mt7921/Makefile
@@ -5,3 +5,4 @@ obj-$(CONFIG_MT7921E) += mt7921e.o
CFLAGS_trace.o := -I$(src)
mt7921e-y := pci.o mac.o mcu.o dma.o eeprom.o main.o init.o debugfs.o trace.o
+mt7921e-$(CONFIG_NL80211_TESTMODE) += testmode.o
diff --git a/drivers/net/wireless/mediatek/mt76/mt7921/init.c b/drivers/net/wireless/mediatek/mt76/mt7921/init.c
index 2caa5096a419..c37084796525 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7921/init.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7921/init.c
@@ -127,6 +127,8 @@ int mt7921_mac_init(struct mt7921_dev *dev)
for (i = 0; i < 2; i++)
mt7921_mac_init_band(dev, i);
+ dev->mt76.rxfilter = mt76_rr(dev, MT_WF_RFCR(0));
+
return mt76_connac_mcu_set_rts_thresh(&dev->mt76, 0x92b, 0);
}
diff --git a/drivers/net/wireless/mediatek/mt76/mt7921/main.c b/drivers/net/wireless/mediatek/mt76/mt7921/main.c
index bc5643f485c5..c4fd670b89b0 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7921/main.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7921/main.c
@@ -1191,6 +1191,8 @@ const struct ieee80211_ops mt7921_ops = {
.sta_statistics = mt7921_sta_statistics,
.sched_scan_start = mt7921_start_sched_scan,
.sched_scan_stop = mt7921_stop_sched_scan,
+ CFG80211_TESTMODE_CMD(mt7921_testmode_cmd)
+ CFG80211_TESTMODE_DUMP(mt7921_testmode_dump)
#ifdef CONFIG_PM
.suspend = mt7921_suspend,
.resume = mt7921_resume,
diff --git a/drivers/net/wireless/mediatek/mt76/mt7921/mcu.h b/drivers/net/wireless/mediatek/mt76/mt7921/mcu.h
index 89fed2f71161..e964a06359d3 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7921/mcu.h
+++ b/drivers/net/wireless/mediatek/mt76/mt7921/mcu.h
@@ -320,4 +320,30 @@ struct mt7921_mcu_tx_done_event {
u8 rsv1[32];
} __packed;
+
+enum {
+ TM_SWITCH_MODE,
+ TM_SET_AT_CMD,
+ TM_QUERY_AT_CMD,
+};
+
+enum {
+ MT7921_TM_NORMAL,
+ MT7921_TM_TESTMODE,
+ MT7921_TM_ICAP,
+ MT7921_TM_ICAP_OVERLAP,
+ MT7921_TM_WIFISPECTRUM,
+};
+
+struct mt7921_rftest_cmd {
+ u8 action;
+ u8 rsv[3];
+ __le32 param0;
+ __le32 param1;
+} __packed;
+
+struct mt7921_rftest_evt {
+ __le32 param0;
+ __le32 param1;
+} __packed;
#endif
diff --git a/drivers/net/wireless/mediatek/mt76/mt7921/mt7921.h b/drivers/net/wireless/mediatek/mt76/mt7921/mt7921.h
index a249ce34b44b..34f0e3cca211 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7921/mt7921.h
+++ b/drivers/net/wireless/mediatek/mt76/mt7921/mt7921.h
@@ -387,4 +387,8 @@ void mt7921_pm_interface_iter(void *priv, u8 *mac, struct ieee80211_vif *vif);
void mt7921_coredump_work(struct work_struct *work);
int mt7921_wfsys_reset(struct mt7921_dev *dev);
int mt7921_get_txpwr_info(struct mt7921_dev *dev, struct mt7921_txpwr *txpwr);
+int mt7921_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+ void *data, int len);
+int mt7921_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *msg,
+ struct netlink_callback *cb, void *data, int len);
#endif
diff --git a/drivers/net/wireless/mediatek/mt76/mt7921/testmode.c b/drivers/net/wireless/mediatek/mt76/mt7921/testmode.c
new file mode 100644
index 000000000000..e1ba536008fa
--- /dev/null
+++ b/drivers/net/wireless/mediatek/mt76/mt7921/testmode.c
@@ -0,0 +1,173 @@
+// SPDX-License-Identifier: ISC
+
+#include "mt7921.h"
+#include "mcu.h"
+
+enum mt7921_testmode_attr {
+ MT7921_TM_ATTR_UNSPEC,
+ MT7921_TM_ATTR_SET,
+ MT7921_TM_ATTR_QUERY,
+ MT7921_TM_ATTR_RSP,
+
+ /* keep last */
+ NUM_MT7921_TM_ATTRS,
+ MT7921_TM_ATTR_MAX = NUM_MT7921_TM_ATTRS - 1,
+};
+
+struct mt7921_tm_cmd {
+ u8 action;
+ u32 param0;
+ u32 param1;
+};
+
+struct mt7921_tm_evt {
+ u32 param0;
+ u32 param1;
+};
+
+static const struct nla_policy mt7921_tm_policy[NUM_MT7921_TM_ATTRS] = {
+ [MT7921_TM_ATTR_SET] = NLA_POLICY_EXACT_LEN(sizeof(struct mt7921_tm_cmd)),
+ [MT7921_TM_ATTR_QUERY] = NLA_POLICY_EXACT_LEN(sizeof(struct mt7921_tm_cmd)),
+};
+
+static int
+mt7921_tm_set(struct mt7921_dev *dev, struct mt7921_tm_cmd *req)
+{
+ struct mt7921_rftest_cmd cmd = {
+ .action = req->action,
+ .param0 = cpu_to_le32(req->param0),
+ .param1 = cpu_to_le32(req->param1),
+ };
+ bool to_testmode = false, to_normal = false;
+ struct mt76_connac_pm *pm = &dev->pm;
+ struct mt76_phy *phy = &dev->mphy;
+ int ret = -ENOTCONN;
+
+ mutex_lock(&dev->mt76.mutex);
+
+ if (req->action == TM_SWITCH_MODE) {
+ if (req->param0 != MT7921_TM_NORMAL)
+ to_testmode = true;
+ else
+ to_normal = true;
+ }
+
+ if (to_testmode) {
+ mt76_wr(dev, MT_WF_RFCR(0), dev->mt76.rxfilter);
+
+ /* Make sure testmode running on full power mode */
+ pm->enable = false;
+ cancel_delayed_work_sync(&pm->ps_work);
+ cancel_work_sync(&pm->wake_work);
+ __mt7921_mcu_drv_pmctrl(dev);
+
+ phy->test.state = MT76_TM_STATE_ON;
+ }
+
+ if (!mt76_testmode_enabled(phy))
+ goto out;
+
+ ret = mt76_mcu_send_msg(&dev->mt76, MCU_CMD_TEST_CTRL, &cmd,
+ sizeof(cmd), false);
+
+ if (to_normal) {
+ /* Switch back to the normal world */
+ phy->test.state = MT76_TM_STATE_OFF;
+ pm->enable = true;
+ }
+out:
+ mutex_unlock(&dev->mt76.mutex);
+
+ return ret;
+}
+
+static int
+mt7921_tm_query(struct mt7921_dev *dev, struct mt7921_tm_cmd *req,
+ struct mt7921_tm_evt *resp)
+{
+ struct mt7921_rftest_cmd cmd = {
+ .action = req->action,
+ .param0 = cpu_to_le32(req->param0),
+ .param1 = cpu_to_le32(req->param1),
+ };
+ struct mt7921_rftest_evt *evt;
+ struct sk_buff *skb;
+ int ret;
+
+ mutex_lock(&dev->mt76.mutex);
+
+ ret = mt76_mcu_send_and_get_msg(&dev->mt76, MCU_CMD_TEST_CTRL,
+ &cmd, sizeof(cmd), true, &skb);
+ mutex_unlock(&dev->mt76.mutex);
+
+ if (ret)
+ goto out;
+
+ evt = (struct mt7921_rftest_evt *)(skb->data);
+ resp->param0 = le32_to_cpu(evt->param0);
+ resp->param1 = le32_to_cpu(evt->param1);
+out:
+ dev_kfree_skb(skb);
+
+ return ret;
+}
+
+int mt7921_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+ void *data, int len)
+{
+ struct nlattr *tb[NUM_MT7921_TM_ATTRS];
+ struct mt76_phy *mphy = hw->priv;
+ struct mt7921_phy *phy = mphy->priv;
+ struct mt7921_dev *dev = phy->dev;
+ int err;
+
+ if (!test_bit(MT76_STATE_RUNNING, &mphy->state) ||
+ !(hw->conf.flags & IEEE80211_CONF_MONITOR))
+ return -ENOTCONN;
+
+ err = nla_parse_deprecated(tb, MT7921_TM_ATTR_MAX, data, len,
+ mt7921_tm_policy, NULL);
+ if (err)
+ return err;
+
+ err = -EINVAL;
+ if (tb[MT7921_TM_ATTR_SET])
+ err = mt7921_tm_set(dev, nla_data(tb[MT7921_TM_ATTR_SET]));
+
+ return err;
+}
+
+int mt7921_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *msg,
+ struct netlink_callback *cb, void *data, int len)
+{
+ struct nlattr *tb[NUM_MT7921_TM_ATTRS];
+ struct mt76_phy *mphy = hw->priv;
+ struct mt7921_phy *phy = mphy->priv;
+ struct mt7921_dev *dev = phy->dev;
+ struct mt7921_tm_evt resp;
+ int err;
+
+ if (!test_bit(MT76_STATE_RUNNING, &mphy->state) ||
+ !(hw->conf.flags & IEEE80211_CONF_MONITOR) ||
+ !mt76_testmode_enabled(mphy))
+ return -ENOTCONN;
+
+ if (cb->args[2]++ > 0)
+ return -ENOENT;
+
+ err = nla_parse_deprecated(tb, MT7921_TM_ATTR_MAX, data, len,
+ mt7921_tm_policy, NULL);
+ if (err)
+ return err;
+
+ err = -EINVAL;
+ if (tb[MT7921_TM_ATTR_QUERY]) {
+ err = mt7921_tm_query(dev, nla_data(tb[MT7921_TM_ATTR_QUERY]), &resp);
+ if (err)
+ goto out;
+
+ err = nla_put(msg, MT7921_TM_ATTR_RSP, sizeof(resp), &resp);
+ }
+out:
+ return err;
+}
diff --git a/drivers/net/wireless/mediatek/mt76/testmode.h b/drivers/net/wireless/mediatek/mt76/testmode.h
index d32a7654c47e..127c13105ee9 100644
--- a/drivers/net/wireless/mediatek/mt76/testmode.h
+++ b/drivers/net/wireless/mediatek/mt76/testmode.h
@@ -140,6 +140,7 @@ enum mt76_testmode_rx_attr {
* enum mt76_testmode_state - phy test state
*
* @MT76_TM_STATE_OFF: test mode disabled (normal operation)
+ * @MT76_TM_STATE_ON: test mode enabled used in offload firmware
* @MT76_TM_STATE_IDLE: test mode enabled, but idle
* @MT76_TM_STATE_TX_FRAMES: send a fixed number of test frames
* @MT76_TM_STATE_RX_FRAMES: receive packets and keep statistics
@@ -147,6 +148,7 @@ enum mt76_testmode_rx_attr {
*/
enum mt76_testmode_state {
MT76_TM_STATE_OFF,
+ MT76_TM_STATE_ON,
MT76_TM_STATE_IDLE,
MT76_TM_STATE_TX_FRAMES,
MT76_TM_STATE_RX_FRAMES,
--
2.25.1