[PATCH 1/7] mt76: mt7615: schedule tx tasklet and sta poll on mac tx free

linux-wireless.vger.kernel.org archive mirror
 help / color / mirror / Atom feed

* [PATCH 1/7] mt76: mt7615: schedule tx tasklet and sta poll on mac tx free
@ 2020-06-22 20:13 Felix Fietkau
  2020-06-22 20:13 ` [PATCH 2/7] mt76: mt7615: add support for accessing mapped registers via bus ops Felix Fietkau
                   ` (5 more replies)
  0 siblings, 6 replies; 8+ messages in thread
From: Felix Fietkau @ 2020-06-22 20:13 UTC (permalink / raw)
  To: linux-wireless

Unlike on earlier chips, DMA completion on MT7615 does not imply actually
having sent out any packets.
Since AQL will prevent filling the hardware queues and will only allow more
packets to be passed to the driver after tx completion, it makes much more
sense to schedule the tx tasklet there.
This is also needed for scheduling tx in testmode support

Signed-off-by: Felix Fietkau <nbd@nbd.name>
---
 drivers/net/wireless/mediatek/mt76/mt7615/dma.c | 4 ----
 drivers/net/wireless/mediatek/mt76/mt7615/mac.c | 6 ++++++
 2 files changed, 6 insertions(+), 4 deletions(-)

diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/dma.c b/drivers/net/wireless/mediatek/mt76/mt7615/dma.c
index e5a965df899a..1231a5ddf9ea 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7615/dma.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/dma.c
@@ -122,10 +122,6 @@ static int mt7615_poll_tx(struct napi_struct *napi, int budget)
 
 	mt7615_tx_cleanup(dev);
 
-	rcu_read_lock();
-	mt7615_mac_sta_poll(dev);
-	rcu_read_unlock();
-
 	tasklet_schedule(&dev->mt76.tx_tasklet);
 
 	return 0;
diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/mac.c b/drivers/net/wireless/mediatek/mt76/mt7615/mac.c
index d150fac50c00..1dc291e8b766 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7615/mac.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/mac.c
@@ -1399,6 +1399,12 @@ static void mt7615_mac_tx_free(struct mt7615_dev *dev, struct sk_buff *skb)
 	}
 
 	dev_kfree_skb(skb);
+
+	rcu_read_lock();
+	mt7615_mac_sta_poll(dev);
+	rcu_read_unlock();
+
+	tasklet_schedule(&dev->mt76.tx_tasklet);
 }
 
 void mt7615_queue_rx_skb(struct mt76_dev *mdev, enum mt76_rxq_id q,
-- 
2.24.0


^ permalink raw reply related	[flat|nested] 8+ messages in thread

* [PATCH 2/7] mt76: mt7615: add support for accessing mapped registers via bus ops
  2020-06-22 20:13 [PATCH 1/7] mt76: mt7615: schedule tx tasklet and sta poll on mac tx free Felix Fietkau
@ 2020-06-22 20:13 ` Felix Fietkau
  2020-06-22 20:13 ` [PATCH 3/7] mt76: mt7615: add support for accessing RF registers via MCU Felix Fietkau
                   ` (4 subsequent siblings)
  5 siblings, 0 replies; 8+ messages in thread
From: Felix Fietkau @ 2020-06-22 20:13 UTC (permalink / raw)
  To: linux-wireless

Makes it possible to read/write them via debugfs, similar to mt7603

Signed-off-by: Felix Fietkau <nbd@nbd.name>
---
 .../net/wireless/mediatek/mt76/mt7615/mmio.c  | 46 +++++++++++++++++++
 .../wireless/mediatek/mt76/mt7615/mt7615.h    |  1 +
 2 files changed, 47 insertions(+)

diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/mmio.c b/drivers/net/wireless/mediatek/mt76/mt7615/mmio.c
index 2e99845b9c96..07ad64e4a77a 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7615/mmio.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/mmio.c
@@ -140,6 +140,38 @@ static void mt7615_irq_tasklet(unsigned long data)
 	mt76_set_irq_mask(&dev->mt76, MT_INT_MASK_CSR, mask, 0);
 }
 
+static u32 __mt7615_reg_addr(struct mt7615_dev *dev, u32 addr)
+{
+	if (addr < 0x100000)
+		return addr;
+
+	return mt7615_reg_map(dev, addr);
+}
+
+static u32 mt7615_rr(struct mt76_dev *mdev, u32 offset)
+{
+	struct mt7615_dev *dev = container_of(mdev, struct mt7615_dev, mt76);
+	u32 addr = __mt7615_reg_addr(dev, offset);
+
+	return dev->bus_ops->rr(mdev, addr);
+}
+
+static void mt7615_wr(struct mt76_dev *mdev, u32 offset, u32 val)
+{
+	struct mt7615_dev *dev = container_of(mdev, struct mt7615_dev, mt76);
+	u32 addr = __mt7615_reg_addr(dev, offset);
+
+	dev->bus_ops->wr(mdev, addr, val);
+}
+
+static u32 mt7615_rmw(struct mt76_dev *mdev, u32 offset, u32 mask, u32 val)
+{
+	struct mt7615_dev *dev = container_of(mdev, struct mt7615_dev, mt76);
+	u32 addr = __mt7615_reg_addr(dev, offset);
+
+	return dev->bus_ops->rmw(mdev, addr, mask, val);
+}
+
 int mt7615_mmio_probe(struct device *pdev, void __iomem *mem_base,
 		      int irq, const u32 *map)
 {
@@ -159,6 +191,7 @@ int mt7615_mmio_probe(struct device *pdev, void __iomem *mem_base,
 		.sta_remove = mt7615_mac_sta_remove,
 		.update_survey = mt7615_update_channel,
 	};
+	struct mt76_bus_ops *bus_ops;
 	struct ieee80211_ops *ops;
 	struct mt7615_dev *dev;
 	struct mt76_dev *mdev;
@@ -182,6 +215,19 @@ int mt7615_mmio_probe(struct device *pdev, void __iomem *mem_base,
 		    (mt76_rr(dev, MT_HW_REV) & 0xff);
 	dev_dbg(mdev->dev, "ASIC revision: %04x\n", mdev->rev);
 
+	dev->bus_ops = dev->mt76.bus;
+	bus_ops = devm_kmemdup(dev->mt76.dev, dev->bus_ops, sizeof(*bus_ops),
+			       GFP_KERNEL);
+	if (!bus_ops) {
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	bus_ops->rr = mt7615_rr;
+	bus_ops->wr = mt7615_wr;
+	bus_ops->rmw = mt7615_rmw;
+	dev->mt76.bus = bus_ops;
+
 	ret = devm_request_irq(mdev->dev, irq, mt7615_irq_handler,
 			       IRQF_SHARED, KBUILD_MODNAME, dev);
 	if (ret)
diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/mt7615.h b/drivers/net/wireless/mediatek/mt76/mt7615/mt7615.h
index 913dac5c3006..78cc2a2c31f7 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7615/mt7615.h
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/mt7615.h
@@ -240,6 +240,7 @@ struct mt7615_dev {
 		struct mt76_phy mphy;
 	};
 
+	const struct mt76_bus_ops *bus_ops;
 	struct tasklet_struct irq_tasklet;
 
 	struct mt7615_phy phy;
-- 
2.24.0


^ permalink raw reply related	[flat|nested] 8+ messages in thread

* [PATCH 3/7] mt76: mt7615: add support for accessing RF registers via MCU
  2020-06-22 20:13 [PATCH 1/7] mt76: mt7615: schedule tx tasklet and sta poll on mac tx free Felix Fietkau
  2020-06-22 20:13 ` [PATCH 2/7] mt76: mt7615: add support for accessing mapped registers via bus ops Felix Fietkau
@ 2020-06-22 20:13 ` Felix Fietkau
  2020-06-22 20:13 ` [PATCH 4/7] mt76: mt7615: use full on-chip memory address for WF_PHY registers Felix Fietkau
                   ` (3 subsequent siblings)
  5 siblings, 0 replies; 8+ messages in thread
From: Felix Fietkau @ 2020-06-22 20:13 UTC (permalink / raw)
  To: linux-wireless

Includes debugfs files for testing it.
Will be used for testmode support.

Signed-off-by: Felix Fietkau <nbd@nbd.name>
---
 .../wireless/mediatek/mt76/mt7615/debugfs.c   | 28 ++++++++++++
 .../net/wireless/mediatek/mt76/mt7615/mcu.c   | 43 ++++++++++++++++++-
 .../net/wireless/mediatek/mt76/mt7615/mcu.h   |  4 +-
 .../wireless/mediatek/mt76/mt7615/mt7615.h    |  5 +++
 4 files changed, 77 insertions(+), 3 deletions(-)

diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/debugfs.c b/drivers/net/wireless/mediatek/mt76/mt7615/debugfs.c
index d06afcf46d67..8bb7c64db738 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7615/debugfs.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/debugfs.c
@@ -285,6 +285,29 @@ mt7615_queues_read(struct seq_file *s, void *data)
 	return 0;
 }
 
+static int
+mt7615_rf_reg_set(void *data, u64 val)
+{
+	struct mt7615_dev *dev = data;
+
+	mt7615_rf_wr(dev, dev->debugfs_rf_wf, dev->debugfs_rf_reg, val);
+
+	return 0;
+}
+
+static int
+mt7615_rf_reg_get(void *data, u64 *val)
+{
+	struct mt7615_dev *dev = data;
+
+	*val = mt7615_rf_rr(dev, dev->debugfs_rf_wf, dev->debugfs_rf_reg);
+
+	return 0;
+}
+
+DEFINE_DEBUGFS_ATTRIBUTE(fops_rf_reg, mt7615_rf_reg_get, mt7615_rf_reg_set,
+			 "0x%08llx\n");
+
 int mt7615_init_debugfs(struct mt7615_dev *dev)
 {
 	struct dentry *dir;
@@ -324,6 +347,11 @@ int mt7615_init_debugfs(struct mt7615_dev *dev)
 	debugfs_create_devm_seqfile(dev->mt76.dev, "temperature", dir,
 				    mt7615_read_temperature);
 
+	debugfs_create_u32("rf_wfidx", 0600, dir, &dev->debugfs_rf_wf);
+	debugfs_create_u32("rf_regidx", 0600, dir, &dev->debugfs_rf_reg);
+	debugfs_create_file_unsafe("rf_regval", 0600, dir, dev,
+				   &fops_rf_reg);
+
 	return 0;
 }
 EXPORT_SYMBOL_GPL(mt7615_init_debugfs);
diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/mcu.c b/drivers/net/wireless/mediatek/mt76/mt7615/mcu.c
index d3a8ada3b779..b4440f131925 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7615/mcu.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/mcu.c
@@ -151,8 +151,11 @@ void mt7615_mcu_fill_msg(struct mt7615_dev *dev, struct sk_buff *skb,
 		break;
 	default:
 		mcu_txd->cid = MCU_CMD_EXT_CID;
-		mcu_txd->set_query = MCU_Q_SET;
-		mcu_txd->ext_cid = cmd;
+		if (cmd & MCU_QUERY_PREFIX)
+			mcu_txd->set_query = MCU_Q_QUERY;
+		else
+			mcu_txd->set_query = MCU_Q_SET;
+		mcu_txd->ext_cid = mcu_cmd;
 		mcu_txd->ext_cid_ack = 1;
 		break;
 	}
@@ -192,6 +195,10 @@ mt7615_mcu_parse_response(struct mt7615_dev *dev, int cmd,
 		skb_pull(skb, sizeof(*rxd));
 		ret = le32_to_cpu(*(__le32 *)skb->data);
 		break;
+	case MCU_EXT_CMD_RF_REG_ACCESS | MCU_QUERY_PREFIX:
+		skb_pull(skb, sizeof(*rxd));
+		ret = le32_to_cpu(*(__le32 *)&skb->data[8]);
+		break;
 	case MCU_UNI_CMD_DEV_INFO_UPDATE:
 	case MCU_UNI_CMD_BSS_INFO_UPDATE:
 	case MCU_UNI_CMD_STA_REC_UPDATE:
@@ -271,6 +278,38 @@ int mt7615_mcu_msg_send(struct mt76_dev *mdev, int cmd, const void *data,
 }
 EXPORT_SYMBOL_GPL(mt7615_mcu_msg_send);
 
+u32 mt7615_rf_rr(struct mt7615_dev *dev, u32 wf, u32 reg)
+{
+	struct {
+		__le32 wifi_stream;
+		__le32 address;
+		__le32 data;
+	} req = {
+		.wifi_stream = cpu_to_le32(wf),
+		.address = cpu_to_le32(reg),
+	};
+
+	return __mt76_mcu_send_msg(&dev->mt76,
+				   MCU_EXT_CMD_RF_REG_ACCESS | MCU_QUERY_PREFIX,
+				   &req, sizeof(req), true);
+}
+
+int mt7615_rf_wr(struct mt7615_dev *dev, u32 wf, u32 reg, u32 val)
+{
+	struct {
+		__le32 wifi_stream;
+		__le32 address;
+		__le32 data;
+	} req = {
+		.wifi_stream = cpu_to_le32(wf),
+		.address = cpu_to_le32(reg),
+		.data = cpu_to_le32(val),
+	};
+
+	return __mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD_RF_REG_ACCESS, &req,
+				   sizeof(req), false);
+}
+
 static void
 mt7615_mcu_csa_finish(void *priv, u8 *mac, struct ieee80211_vif *vif)
 {
diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/mcu.h b/drivers/net/wireless/mediatek/mt76/mt7615/mcu.h
index 4f70c4de69a4..8e2be150a556 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7615/mcu.h
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/mcu.h
@@ -238,8 +238,9 @@ enum {
 #define MCU_FW_PREFIX		BIT(31)
 #define MCU_UNI_PREFIX		BIT(30)
 #define MCU_CE_PREFIX		BIT(29)
+#define MCU_QUERY_PREFIX	BIT(28)
 #define MCU_CMD_MASK		~(MCU_FW_PREFIX | MCU_UNI_PREFIX |	\
-				  MCU_CE_PREFIX)
+				  MCU_CE_PREFIX | MCU_QUERY_PREFIX)
 
 enum {
 	MCU_CMD_TARGET_ADDRESS_LEN_REQ = MCU_FW_PREFIX | 0x01,
@@ -254,6 +255,7 @@ enum {
 };
 
 enum {
+	MCU_EXT_CMD_RF_REG_ACCESS = 0x02,
 	MCU_EXT_CMD_PM_STATE_CTRL = 0x07,
 	MCU_EXT_CMD_CHANNEL_SWITCH = 0x08,
 	MCU_EXT_CMD_SET_TX_POWER_CTRL = 0x11,
diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/mt7615.h b/drivers/net/wireless/mediatek/mt76/mt7615/mt7615.h
index 78cc2a2c31f7..6e423f45b63f 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7615/mt7615.h
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/mt7615.h
@@ -281,6 +281,9 @@ struct mt7615_dev {
 
 	struct work_struct wtbl_work;
 	struct list_head wd_head;
+
+	u32 debugfs_rf_wf;
+	u32 debugfs_rf_reg;
 };
 
 enum tx_pkt_queue_idx {
@@ -522,6 +525,8 @@ u32 mt7615_mac_get_sta_tid_sn(struct mt7615_dev *dev, int wcid, u8 tid);
 int mt7615_mcu_wait_response(struct mt7615_dev *dev, int cmd, int seq);
 int mt7615_mcu_msg_send(struct mt76_dev *mdev, int cmd, const void *data,
 			int len, bool wait_resp);
+u32 mt7615_rf_rr(struct mt7615_dev *dev, u32 wf, u32 reg);
+int mt7615_rf_wr(struct mt7615_dev *dev, u32 wf, u32 reg, u32 val);
 int mt7615_mcu_set_dbdc(struct mt7615_dev *dev);
 int mt7615_mcu_set_eeprom(struct mt7615_dev *dev);
 int mt7615_mcu_set_mac_enable(struct mt7615_dev *dev, int band, bool enable);
-- 
2.24.0


^ permalink raw reply related	[flat|nested] 8+ messages in thread

* [PATCH 4/7] mt76: mt7615: use full on-chip memory address for WF_PHY registers
  2020-06-22 20:13 [PATCH 1/7] mt76: mt7615: schedule tx tasklet and sta poll on mac tx free Felix Fietkau
  2020-06-22 20:13 ` [PATCH 2/7] mt76: mt7615: add support for accessing mapped registers via bus ops Felix Fietkau
  2020-06-22 20:13 ` [PATCH 3/7] mt76: mt7615: add support for accessing RF registers via MCU Felix Fietkau
@ 2020-06-22 20:13 ` Felix Fietkau
  2020-06-22 20:13 ` [PATCH 5/7] mt76: vif_mask to struct mt76_phy Felix Fietkau
                   ` (2 subsequent siblings)
  5 siblings, 0 replies; 8+ messages in thread
From: Felix Fietkau @ 2020-06-22 20:13 UTC (permalink / raw)
  To: linux-wireless

Now that the bus access functions can use mapping for accessing full
register addresses, use it for WF_PHY registers to keep them constant.
Needed for follow-up work on testmode support

Signed-off-by: Felix Fietkau <nbd@nbd.name>
---
 drivers/net/wireless/mediatek/mt76/mt7615/mmio.c | 2 --
 drivers/net/wireless/mediatek/mt76/mt7615/regs.h | 3 +--
 drivers/net/wireless/mediatek/mt76/mt7615/usb.c  | 1 -
 3 files changed, 1 insertion(+), 5 deletions(-)

diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/mmio.c b/drivers/net/wireless/mediatek/mt76/mt7615/mmio.c
index 07ad64e4a77a..461ac6c42271 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7615/mmio.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/mmio.c
@@ -17,7 +17,6 @@ const u32 mt7615e_reg_map[] = {
 	[MT_CSR_BASE]		= 0x07000,
 	[MT_PLE_BASE]		= 0x08000,
 	[MT_PSE_BASE]		= 0x0c000,
-	[MT_PHY_BASE]		= 0x10000,
 	[MT_CFG_BASE]		= 0x20200,
 	[MT_AGG_BASE]		= 0x20a00,
 	[MT_TMAC_BASE]		= 0x21000,
@@ -44,7 +43,6 @@ const u32 mt7663e_reg_map[] = {
 	[MT_CSR_BASE]		= 0x07000,
 	[MT_PLE_BASE]		= 0x08000,
 	[MT_PSE_BASE]		= 0x0c000,
-	[MT_PHY_BASE]		= 0x10000,
 	[MT_CFG_BASE]		= 0x20000,
 	[MT_AGG_BASE]		= 0x22000,
 	[MT_TMAC_BASE]		= 0x24000,
diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/regs.h b/drivers/net/wireless/mediatek/mt76/mt7615/regs.h
index f0b36b162bf3..4743c12005a4 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7615/regs.h
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/regs.h
@@ -14,7 +14,6 @@ enum mt7615_reg_base {
 	MT_CSR_BASE,
 	MT_PLE_BASE,
 	MT_PSE_BASE,
-	MT_PHY_BASE,
 	MT_CFG_BASE,
 	MT_AGG_BASE,
 	MT_TMAC_BASE,
@@ -169,7 +168,7 @@ enum mt7615_reg_base {
 #define MT_PSE_PG_INFO			MT_PSE(0x194)
 #define MT_PSE_SRC_CNT			GENMASK(27, 16)
 
-#define MT_WF_PHY_BASE			((dev)->reg_map[MT_PHY_BASE])
+#define MT_WF_PHY_BASE			0x82070000
 #define MT_WF_PHY(ofs)			(MT_WF_PHY_BASE + (ofs))
 
 #define MT_WF_PHY_WF2_RFCTRL0(n)	MT_WF_PHY(0x1900 + (n) * 0x400)
diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/usb.c b/drivers/net/wireless/mediatek/mt76/mt7615/usb.c
index f5dc1b828518..0ba28d37c414 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7615/usb.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/usb.c
@@ -25,7 +25,6 @@ static const u32 mt7663u_reg_map[] = {
 	[MT_TOP_MISC_BASE]	= 0x81020000,
 	[MT_PLE_BASE]		= 0x82060000,
 	[MT_PSE_BASE]		= 0x82068000,
-	[MT_PHY_BASE]		= 0x82070000,
 	[MT_WTBL_BASE_ADDR]	= 0x820e0000,
 	[MT_CFG_BASE]		= 0x820f0000,
 	[MT_AGG_BASE]		= 0x820f2000,
-- 
2.24.0


^ permalink raw reply related	[flat|nested] 8+ messages in thread

* [PATCH 5/7] mt76: vif_mask to struct mt76_phy
  2020-06-22 20:13 [PATCH 1/7] mt76: mt7615: schedule tx tasklet and sta poll on mac tx free Felix Fietkau
                   ` (2 preceding siblings ...)
  2020-06-22 20:13 ` [PATCH 4/7] mt76: mt7615: use full on-chip memory address for WF_PHY registers Felix Fietkau
@ 2020-06-22 20:13 ` Felix Fietkau
  2020-06-22 20:13 ` [PATCH 6/7] mt76: add API for testmode support Felix Fietkau
  2020-06-22 20:13 ` [PATCH 7/7] mt76: mt7615: implement " Felix Fietkau
  5 siblings, 0 replies; 8+ messages in thread
From: Felix Fietkau @ 2020-06-22 20:13 UTC (permalink / raw)
  To: linux-wireless

All drivers use this in pretty much the same way. Moving it to core helps with
some checks for the upcoming testmode support

Signed-off-by: Felix Fietkau <nbd@nbd.name>
---
 drivers/net/wireless/mediatek/mt76/mt76.h          | 2 ++
 drivers/net/wireless/mediatek/mt76/mt7603/main.c   | 6 +++---
 drivers/net/wireless/mediatek/mt76/mt7603/mt7603.h | 2 --
 drivers/net/wireless/mediatek/mt76/mt7615/main.c   | 6 +++---
 drivers/net/wireless/mediatek/mt76/mt7615/mt7615.h | 1 -
 drivers/net/wireless/mediatek/mt76/mt76x02.h       | 1 -
 drivers/net/wireless/mediatek/mt76/mt76x02_mmio.c  | 2 +-
 drivers/net/wireless/mediatek/mt76/mt76x02_util.c  | 8 ++++----
 drivers/net/wireless/mediatek/mt76/mt7915/main.c   | 6 +++---
 drivers/net/wireless/mediatek/mt76/mt7915/mt7915.h | 1 -
 10 files changed, 16 insertions(+), 19 deletions(-)

diff --git a/drivers/net/wireless/mediatek/mt76/mt76.h b/drivers/net/wireless/mediatek/mt76/mt76.h
index 3d7db6ffb599..ece462e9c9ab 100644
--- a/drivers/net/wireless/mediatek/mt76/mt76.h
+++ b/drivers/net/wireless/mediatek/mt76/mt76.h
@@ -491,6 +491,8 @@ struct mt76_phy {
 	struct mt76_sband sband_2g;
 	struct mt76_sband sband_5g;
 
+	u32 vif_mask;
+
 	int txpower_cur;
 	u8 antenna_mask;
 };
diff --git a/drivers/net/wireless/mediatek/mt76/mt7603/main.c b/drivers/net/wireless/mediatek/mt76/mt7603/main.c
index 83dfa6da4761..447f2c63ef38 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7603/main.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7603/main.c
@@ -44,7 +44,7 @@ mt7603_add_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 
 	mutex_lock(&dev->mt76.mutex);
 
-	mvif->idx = ffs(~dev->vif_mask) - 1;
+	mvif->idx = ffs(~dev->mphy.vif_mask) - 1;
 	if (mvif->idx >= MT7603_MAX_INTERFACES) {
 		ret = -ENOSPC;
 		goto out;
@@ -65,7 +65,7 @@ mt7603_add_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 	}
 
 	idx = MT7603_WTBL_RESERVED - 1 - mvif->idx;
-	dev->vif_mask |= BIT(mvif->idx);
+	dev->mphy.vif_mask |= BIT(mvif->idx);
 	INIT_LIST_HEAD(&mvif->sta.poll_list);
 	mvif->sta.wcid.idx = idx;
 	mvif->sta.wcid.hw_key_idx = -1;
@@ -107,7 +107,7 @@ mt7603_remove_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 	spin_unlock_bh(&dev->sta_poll_lock);
 
 	mutex_lock(&dev->mt76.mutex);
-	dev->vif_mask &= ~BIT(mvif->idx);
+	dev->mphy.vif_mask &= ~BIT(mvif->idx);
 	mutex_unlock(&dev->mt76.mutex);
 }
 
diff --git a/drivers/net/wireless/mediatek/mt76/mt7603/mt7603.h b/drivers/net/wireless/mediatek/mt76/mt7603/mt7603.h
index 7fadf094e9be..c86305241e66 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7603/mt7603.h
+++ b/drivers/net/wireless/mediatek/mt76/mt7603/mt7603.h
@@ -108,8 +108,6 @@ struct mt7603_dev {
 
 	u32 rxfilter;
 
-	u8 vif_mask;
-
 	struct list_head sta_poll_list;
 	spinlock_t sta_poll_lock;
 
diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/main.c b/drivers/net/wireless/mediatek/mt76/mt7615/main.c
index f8ce485c8d19..abb9853eaa8b 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7615/main.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/main.c
@@ -137,7 +137,7 @@ static int mt7615_add_interface(struct ieee80211_hw *hw,
 
 	mutex_lock(&dev->mt76.mutex);
 
-	mvif->idx = ffs(~dev->vif_mask) - 1;
+	mvif->idx = ffs(~dev->mphy.vif_mask) - 1;
 	if (mvif->idx >= MT7615_MAX_INTERFACES) {
 		ret = -ENOSPC;
 		goto out;
@@ -157,7 +157,7 @@ static int mt7615_add_interface(struct ieee80211_hw *hw,
 	else
 		mvif->wmm_idx = mvif->idx % MT7615_MAX_WMM_SETS;
 
-	dev->vif_mask |= BIT(mvif->idx);
+	dev->mphy.vif_mask |= BIT(mvif->idx);
 	dev->omac_mask |= BIT(mvif->omac_idx);
 	phy->omac_mask |= BIT(mvif->omac_idx);
 
@@ -204,7 +204,7 @@ static void mt7615_remove_interface(struct ieee80211_hw *hw,
 		mt76_txq_remove(&dev->mt76, vif->txq);
 
 	mutex_lock(&dev->mt76.mutex);
-	dev->vif_mask &= ~BIT(mvif->idx);
+	dev->mphy.vif_mask &= ~BIT(mvif->idx);
 	dev->omac_mask &= ~BIT(mvif->omac_idx);
 	phy->omac_mask &= ~BIT(mvif->omac_idx);
 	mutex_unlock(&dev->mt76.mutex);
diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/mt7615.h b/drivers/net/wireless/mediatek/mt76/mt7615/mt7615.h
index 6e423f45b63f..af86cf6925dd 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7615/mt7615.h
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/mt7615.h
@@ -244,7 +244,6 @@ struct mt7615_dev {
 	struct tasklet_struct irq_tasklet;
 
 	struct mt7615_phy phy;
-	u32 vif_mask;
 	u32 omac_mask;
 
 	u16 chainmask;
diff --git a/drivers/net/wireless/mediatek/mt76/mt76x02.h b/drivers/net/wireless/mediatek/mt76/mt76x02.h
index 4c9bbc7ce023..4660b9691ec3 100644
--- a/drivers/net/wireless/mediatek/mt76/mt76x02.h
+++ b/drivers/net/wireless/mediatek/mt76/mt76x02.h
@@ -80,7 +80,6 @@ struct mt76x02_dev {
 
 	struct mutex phy_mutex;
 
-	u16 vif_mask;
 	u16 chainmask;
 
 	u8 txdone_seq;
diff --git a/drivers/net/wireless/mediatek/mt76/mt76x02_mmio.c b/drivers/net/wireless/mediatek/mt76/mt76x02_mmio.c
index cbbe986655fe..99611515a093 100644
--- a/drivers/net/wireless/mediatek/mt76/mt76x02_mmio.c
+++ b/drivers/net/wireless/mediatek/mt76/mt76x02_mmio.c
@@ -439,7 +439,7 @@ static void mt76x02_reset_state(struct mt76x02_dev *dev)
 		memset(msta, 0, sizeof(*msta));
 	}
 
-	dev->vif_mask = 0;
+	dev->mphy.vif_mask = 0;
 	dev->mt76.beacon_mask = 0;
 }
 
diff --git a/drivers/net/wireless/mediatek/mt76/mt76x02_util.c b/drivers/net/wireless/mediatek/mt76/mt76x02_util.c
index 44822a849eb1..dbd4077ea283 100644
--- a/drivers/net/wireless/mediatek/mt76/mt76x02_util.c
+++ b/drivers/net/wireless/mediatek/mt76/mt76x02_util.c
@@ -305,7 +305,7 @@ mt76x02_add_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 	unsigned int idx = 0;
 
 	/* Allow to change address in HW if we create first interface. */
-	if (!dev->vif_mask &&
+	if (!dev->mphy.vif_mask &&
 	    (((vif->addr[0] ^ dev->mt76.macaddr[0]) & ~GENMASK(4, 1)) ||
 	     memcmp(vif->addr + 1, dev->mt76.macaddr + 1, ETH_ALEN - 1)))
 		mt76x02_mac_setaddr(dev, vif->addr);
@@ -330,11 +330,11 @@ mt76x02_add_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 		idx += 8;
 
 	/* vif is already set or idx is 8 for AP/Mesh/... */
-	if (dev->vif_mask & BIT(idx) ||
+	if (dev->mphy.vif_mask & BIT(idx) ||
 	    (vif->type != NL80211_IFTYPE_STATION && idx > 7))
 		return -EBUSY;
 
-	dev->vif_mask |= BIT(idx);
+	dev->mphy.vif_mask |= BIT(idx);
 
 	mt76x02_vif_init(dev, vif, idx);
 	return 0;
@@ -348,7 +348,7 @@ void mt76x02_remove_interface(struct ieee80211_hw *hw,
 	struct mt76x02_vif *mvif = (struct mt76x02_vif *)vif->drv_priv;
 
 	mt76_txq_remove(&dev->mt76, vif->txq);
-	dev->vif_mask &= ~BIT(mvif->idx);
+	dev->mphy.vif_mask &= ~BIT(mvif->idx);
 }
 EXPORT_SYMBOL_GPL(mt76x02_remove_interface);
 
diff --git a/drivers/net/wireless/mediatek/mt76/mt7915/main.c b/drivers/net/wireless/mediatek/mt76/mt7915/main.c
index d0af025d2db9..889f5f93ce30 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7915/main.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7915/main.c
@@ -125,7 +125,7 @@ static int mt7915_add_interface(struct ieee80211_hw *hw,
 
 	mutex_lock(&dev->mt76.mutex);
 
-	mvif->idx = ffs(~phy->vif_mask) - 1;
+	mvif->idx = ffs(~phy->mt76->vif_mask) - 1;
 	if (mvif->idx >= MT7915_MAX_INTERFACES) {
 		ret = -ENOSPC;
 		goto out;
@@ -150,7 +150,7 @@ static int mt7915_add_interface(struct ieee80211_hw *hw,
 	if (ret)
 		goto out;
 
-	phy->vif_mask |= BIT(mvif->idx);
+	phy->mt76->vif_mask |= BIT(mvif->idx);
 	phy->omac_mask |= BIT(mvif->omac_idx);
 
 	idx = MT7915_WTBL_RESERVED - mvif->idx;
@@ -194,7 +194,7 @@ static void mt7915_remove_interface(struct ieee80211_hw *hw,
 		mt76_txq_remove(&dev->mt76, vif->txq);
 
 	mutex_lock(&dev->mt76.mutex);
-	phy->vif_mask &= ~BIT(mvif->idx);
+	phy->mt76->vif_mask &= ~BIT(mvif->idx);
 	phy->omac_mask &= ~BIT(mvif->omac_idx);
 	mutex_unlock(&dev->mt76.mutex);
 
diff --git a/drivers/net/wireless/mediatek/mt76/mt7915/mt7915.h b/drivers/net/wireless/mediatek/mt76/mt7915/mt7915.h
index 6d038deee0ea..3eaf5e64b0b4 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7915/mt7915.h
+++ b/drivers/net/wireless/mediatek/mt76/mt7915/mt7915.h
@@ -116,7 +116,6 @@ struct mt7915_phy {
 	struct ieee80211_sband_iftype_data iftype[2][NUM_NL80211_IFTYPES];
 
 	u32 rxfilter;
-	u32 vif_mask;
 	u32 omac_mask;
 
 	u16 noise;
-- 
2.24.0


^ permalink raw reply related	[flat|nested] 8+ messages in thread

* [PATCH 6/7] mt76: add API for testmode support
  2020-06-22 20:13 [PATCH 1/7] mt76: mt7615: schedule tx tasklet and sta poll on mac tx free Felix Fietkau
                   ` (3 preceding siblings ...)
  2020-06-22 20:13 ` [PATCH 5/7] mt76: vif_mask to struct mt76_phy Felix Fietkau
@ 2020-06-22 20:13 ` Felix Fietkau
  2020-06-22 20:13 ` [PATCH 7/7] mt76: mt7615: implement " Felix Fietkau
  5 siblings, 0 replies; 8+ messages in thread
From: Felix Fietkau @ 2020-06-22 20:13 UTC (permalink / raw)
  To: linux-wireless

This can be used for calibration in the manufacturing process.
It supports sending a configurable number of packets with a specific rate
and configurable tx power levels / antenna settings.
It also supports receiving packets and showing some statistics, including
packet counters and detailed RSSI information.
It will only be compiled in if CONFIG_NL80211_TESTMODE is enabled

Signed-off-by: Felix Fietkau <nbd@nbd.name>
---
 drivers/net/wireless/mediatek/mt76/Makefile   |   1 +
 drivers/net/wireless/mediatek/mt76/eeprom.c   |   5 +
 drivers/net/wireless/mediatek/mt76/mac80211.c |   7 +
 drivers/net/wireless/mediatek/mt76/mt76.h     |  75 +++
 drivers/net/wireless/mediatek/mt76/testmode.c | 497 ++++++++++++++++++
 drivers/net/wireless/mediatek/mt76/testmode.h | 156 ++++++
 drivers/net/wireless/mediatek/mt76/tx.c       |  18 +
 7 files changed, 759 insertions(+)
 create mode 100644 drivers/net/wireless/mediatek/mt76/testmode.c
 create mode 100644 drivers/net/wireless/mediatek/mt76/testmode.h

diff --git a/drivers/net/wireless/mediatek/mt76/Makefile b/drivers/net/wireless/mediatek/mt76/Makefile
index ef663b873b0b..949b3c8ffa1b 100644
--- a/drivers/net/wireless/mediatek/mt76/Makefile
+++ b/drivers/net/wireless/mediatek/mt76/Makefile
@@ -9,6 +9,7 @@ mt76-y := \
 	tx.o agg-rx.o mcu.o
 
 mt76-$(CONFIG_PCI) += pci.o
+mt76-$(CONFIG_NL80211_TESTMODE) += testmode.o
 
 mt76-usb-y := usb.o usb_trace.o
 
diff --git a/drivers/net/wireless/mediatek/mt76/eeprom.c b/drivers/net/wireless/mediatek/mt76/eeprom.c
index c236e303ccfd..3044e0069991 100644
--- a/drivers/net/wireless/mediatek/mt76/eeprom.c
+++ b/drivers/net/wireless/mediatek/mt76/eeprom.c
@@ -74,6 +74,11 @@ mt76_get_of_eeprom(struct mt76_dev *dev, int len)
 					   &data[i]);
 	}
 
+#ifdef CONFIG_NL80211_TESTMODE
+	dev->test.mtd_name = devm_kstrdup(dev->dev, part, GFP_KERNEL);
+	dev->test.mtd_offset = offset;
+#endif
+
 out_put_node:
 	of_node_put(np);
 	return ret;
diff --git a/drivers/net/wireless/mediatek/mt76/mac80211.c b/drivers/net/wireless/mediatek/mt76/mac80211.c
index d8533d0c6e81..10e6dbb64996 100644
--- a/drivers/net/wireless/mediatek/mt76/mac80211.c
+++ b/drivers/net/wireless/mediatek/mt76/mac80211.c
@@ -505,6 +505,13 @@ void mt76_rx(struct mt76_dev *dev, enum mt76_rxq_id q, struct sk_buff *skb)
 		return;
 	}
 
+#ifdef CONFIG_NL80211_TESTMODE
+	if (dev->test.state == MT76_TM_STATE_RX_FRAMES) {
+		dev->test.rx_stats.packets[q]++;
+		if (status->flag & RX_FLAG_FAILED_FCS_CRC)
+			dev->test.rx_stats.fcs_error[q]++;
+	}
+#endif
 	__skb_queue_tail(&dev->rx_skb[q], skb);
 }
 EXPORT_SYMBOL_GPL(mt76_rx);
diff --git a/drivers/net/wireless/mediatek/mt76/mt76.h b/drivers/net/wireless/mediatek/mt76/mt76.h
index ece462e9c9ab..14b3a39cd55a 100644
--- a/drivers/net/wireless/mediatek/mt76/mt76.h
+++ b/drivers/net/wireless/mediatek/mt76/mt76.h
@@ -15,6 +15,7 @@
 #include <linux/average.h>
 #include <net/mac80211.h>
 #include "util.h"
+#include "testmode.h"
 
 #define MT_TX_RING_SIZE     256
 #define MT_MCU_RING_SIZE    32
@@ -475,6 +476,47 @@ struct mt76_rx_status {
 	s8 chain_signal[IEEE80211_MAX_CHAINS];
 };
 
+struct mt76_testmode_ops {
+	int (*set_state)(struct mt76_dev *dev, enum mt76_testmode_state state);
+	int (*set_params)(struct mt76_dev *dev, struct nlattr **tb,
+			  enum mt76_testmode_state new_state);
+	int (*dump_stats)(struct mt76_dev *dev, struct sk_buff *msg);
+};
+
+struct mt76_testmode_data {
+	enum mt76_testmode_state state;
+
+	u32 param_set[DIV_ROUND_UP(NUM_MT76_TM_ATTRS, 32)];
+	struct sk_buff *tx_skb;
+
+	u32 tx_count;
+	u16 tx_msdu_len;
+
+	u8 tx_rate_mode;
+	u8 tx_rate_idx;
+	u8 tx_rate_nss;
+	u8 tx_rate_sgi;
+	u8 tx_rate_ldpc;
+
+	u8 tx_antenna_mask;
+
+	u32 freq_offset;
+
+	u8 tx_power[4];
+	u8 tx_power_control;
+
+	const char *mtd_name;
+	u32 mtd_offset;
+
+	u32 tx_pending;
+	u32 tx_queued;
+	u32 tx_done;
+	struct {
+		u64 packets[__MT_RXQ_MAX];
+		u64 fcs_error[__MT_RXQ_MAX];
+	} rx_stats;
+};
+
 struct mt76_phy {
 	struct ieee80211_hw *hw;
 	struct mt76_dev *dev;
@@ -574,6 +616,11 @@ struct mt76_dev {
 
 	u32 rxfilter;
 
+#ifdef CONFIG_NL80211_TESTMODE
+	const struct mt76_testmode_ops *test_ops;
+	struct mt76_testmode_data test;
+#endif
+
 	union {
 		struct mt76_mmio mmio;
 		struct mt76_usb usb;
@@ -807,6 +854,15 @@ static inline u8 mt76_tx_power_nss_delta(u8 nss)
 	return nss_delta[nss - 1];
 }
 
+static inline bool mt76_testmode_enabled(struct mt76_dev *dev)
+{
+#ifdef CONFIG_NL80211_TESTMODE
+	return dev->test.state != MT76_TM_STATE_OFF;
+#else
+	return false;
+#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,
 	     struct mt76_wcid *wcid, struct sk_buff *skb);
@@ -879,6 +935,24 @@ void mt76_sw_scan(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 		  const u8 *mac);
 void mt76_sw_scan_complete(struct ieee80211_hw *hw,
 			   struct ieee80211_vif *vif);
+int mt76_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+		      void *data, int len);
+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_dev *dev, enum mt76_testmode_state state);
+
+static inline void mt76_testmode_reset(struct mt76_dev *dev, bool disable)
+{
+#ifdef CONFIG_NL80211_TESTMODE
+	enum mt76_testmode_state state = MT76_TM_STATE_IDLE;
+
+	if (disable || dev->test.state == MT76_TM_STATE_OFF)
+		state = MT76_TM_STATE_OFF;
+
+	mt76_testmode_set_state(dev, state);
+#endif
+}
+
 
 /* internal */
 static inline struct ieee80211_hw *
@@ -903,6 +977,7 @@ void mt76_rx_complete(struct mt76_dev *dev, struct sk_buff_head *frames,
 void mt76_rx_poll_complete(struct mt76_dev *dev, enum mt76_rxq_id q,
 			   struct napi_struct *napi);
 void mt76_rx_aggr_reorder(struct sk_buff *skb, struct sk_buff_head *frames);
+void mt76_testmode_tx_pending(struct mt76_dev *dev);
 
 /* usb */
 static inline bool mt76u_urb_error(struct urb *urb)
diff --git a/drivers/net/wireless/mediatek/mt76/testmode.c b/drivers/net/wireless/mediatek/mt76/testmode.c
new file mode 100644
index 000000000000..a78388fb35eb
--- /dev/null
+++ b/drivers/net/wireless/mediatek/mt76/testmode.c
@@ -0,0 +1,497 @@
+// SPDX-License-Identifier: ISC
+/* Copyright (C) 2020 Felix Fietkau <nbd@nbd.name> */
+#include "mt76.h"
+
+static const struct nla_policy mt76_tm_policy[NUM_MT76_TM_ATTRS] = {
+	[MT76_TM_ATTR_RESET] = { .type = NLA_FLAG },
+	[MT76_TM_ATTR_STATE] = { .type = NLA_U8 },
+	[MT76_TM_ATTR_TX_COUNT] = { .type = NLA_U32 },
+	[MT76_TM_ATTR_TX_RATE_MODE] = { .type = NLA_U8 },
+	[MT76_TM_ATTR_TX_RATE_NSS] = { .type = NLA_U8 },
+	[MT76_TM_ATTR_TX_RATE_IDX] = { .type = NLA_U8 },
+	[MT76_TM_ATTR_TX_RATE_SGI] = { .type = NLA_U8 },
+	[MT76_TM_ATTR_TX_RATE_LDPC] = { .type = NLA_U8 },
+	[MT76_TM_ATTR_TX_ANTENNA] = { .type = NLA_U8 },
+	[MT76_TM_ATTR_TX_POWER_CONTROL] = { .type = NLA_U8 },
+	[MT76_TM_ATTR_TX_POWER] = { .type = NLA_NESTED },
+	[MT76_TM_ATTR_FREQ_OFFSET] = { .type = NLA_U32 },
+};
+
+void mt76_testmode_tx_pending(struct mt76_dev *dev)
+{
+	struct mt76_testmode_data *td = &dev->test;
+	struct mt76_wcid *wcid = &dev->global_wcid;
+	struct sk_buff *skb = td->tx_skb;
+	struct mt76_queue *q;
+	int qid;
+
+	if (!skb || !td->tx_pending)
+		return;
+
+	qid = skb_get_queue_mapping(skb);
+	q = dev->q_tx[qid].q;
+
+	spin_lock_bh(&q->lock);
+
+	while (td->tx_pending > 0 && q->queued < q->ndesc / 2) {
+		int ret;
+
+		ret = dev->queue_ops->tx_queue_skb(dev, qid, skb_get(skb), wcid, NULL);
+		if (ret < 0)
+			break;
+
+		td->tx_pending--;
+		td->tx_queued++;
+	}
+
+	dev->queue_ops->kick(dev, q);
+
+	spin_unlock_bh(&q->lock);
+}
+
+
+static int
+mt76_testmode_tx_init(struct mt76_dev *dev)
+{
+	struct mt76_testmode_data *td = &dev->test;
+	struct ieee80211_tx_info *info;
+	struct ieee80211_hdr *hdr;
+	struct sk_buff *skb;
+	u16 fc = IEEE80211_FTYPE_DATA | IEEE80211_STYPE_DATA |
+		 IEEE80211_FCTL_FROMDS;
+	struct ieee80211_tx_rate *rate;
+	u8 max_nss = hweight8(dev->phy.antenna_mask);
+
+	if (td->tx_antenna_mask)
+		max_nss = min_t(u8, max_nss, hweight8(td->tx_antenna_mask));
+
+	skb = alloc_skb(td->tx_msdu_len, GFP_KERNEL);
+	if (!skb)
+		return -ENOMEM;
+
+	dev_kfree_skb(td->tx_skb);
+	td->tx_skb = skb;
+	hdr = __skb_put_zero(skb, td->tx_msdu_len);
+	hdr->frame_control = cpu_to_le16(fc);
+	memcpy(hdr->addr1, dev->macaddr, sizeof(dev->macaddr));
+	memcpy(hdr->addr2, dev->macaddr, sizeof(dev->macaddr));
+	memcpy(hdr->addr3, dev->macaddr, sizeof(dev->macaddr));
+
+	info = IEEE80211_SKB_CB(skb);
+	info->flags = IEEE80211_TX_CTL_INJECTED |
+		      IEEE80211_TX_CTL_NO_ACK |
+		      IEEE80211_TX_CTL_NO_PS_BUFFER;
+	rate = &info->control.rates[0];
+	rate->count = 1;
+	rate->idx = td->tx_rate_idx;
+
+	switch (td->tx_rate_mode) {
+	case MT76_TM_TX_MODE_CCK:
+		if (dev->phy.chandef.chan->band != NL80211_BAND_2GHZ)
+			return -EINVAL;
+
+		if (rate->idx > 4)
+			return -EINVAL;
+		break;
+	case MT76_TM_TX_MODE_OFDM:
+		if (dev->phy.chandef.chan->band != NL80211_BAND_2GHZ)
+			break;
+
+		if (rate->idx > 8)
+			return -EINVAL;
+
+		rate->idx += 4;
+		break;
+	case MT76_TM_TX_MODE_HT:
+		if (rate->idx > 8 * max_nss &&
+			!(rate->idx == 32 &&
+			  dev->phy.chandef.width >= NL80211_CHAN_WIDTH_40))
+			return -EINVAL;
+
+		rate->flags |= IEEE80211_TX_RC_MCS;
+		break;
+	case MT76_TM_TX_MODE_VHT:
+		if (rate->idx > 9)
+			return -EINVAL;
+
+		if (td->tx_rate_nss > max_nss)
+			return -EINVAL;
+
+		ieee80211_rate_set_vht(rate, td->tx_rate_idx, td->tx_rate_nss);
+		rate->flags |= IEEE80211_TX_RC_VHT_MCS;
+		break;
+	default:
+		break;
+	}
+
+	if (td->tx_rate_sgi)
+		rate->flags |= IEEE80211_TX_RC_SHORT_GI;
+
+	if (td->tx_rate_ldpc)
+		info->flags |= IEEE80211_TX_CTL_LDPC;
+
+	if (td->tx_rate_mode >= MT76_TM_TX_MODE_HT) {
+		switch (dev->phy.chandef.width) {
+		case NL80211_CHAN_WIDTH_40:
+			rate->flags |= IEEE80211_TX_RC_40_MHZ_WIDTH;
+			break;
+		case NL80211_CHAN_WIDTH_80:
+			rate->flags |= IEEE80211_TX_RC_80_MHZ_WIDTH;
+			break;
+		case NL80211_CHAN_WIDTH_80P80:
+		case NL80211_CHAN_WIDTH_160:
+			rate->flags |= IEEE80211_TX_RC_160_MHZ_WIDTH;
+			break;
+		default:
+			break;
+		}
+	}
+
+	skb_set_queue_mapping(skb, IEEE80211_AC_BE);
+
+	return 0;
+}
+
+static void
+mt76_testmode_tx_start(struct mt76_dev *dev)
+{
+	struct mt76_testmode_data *td = &dev->test;
+
+	td->tx_queued = 0;
+	td->tx_done = 0;
+	td->tx_pending = td->tx_count;
+	tasklet_schedule(&dev->tx_tasklet);
+}
+
+static void
+mt76_testmode_tx_stop(struct mt76_dev *dev)
+{
+	struct mt76_testmode_data *td = &dev->test;
+
+	tasklet_disable(&dev->tx_tasklet);
+
+	td->tx_pending = 0;
+
+	tasklet_enable(&dev->tx_tasklet);
+
+	wait_event_timeout(dev->tx_wait, td->tx_done == td->tx_queued, 10 * HZ);
+
+	dev_kfree_skb(td->tx_skb);
+	td->tx_skb = NULL;
+}
+
+static inline void
+mt76_testmode_param_set(struct mt76_testmode_data *td, u16 idx)
+{
+	td->param_set[idx / 32] |= BIT(idx % 32);
+}
+
+static inline bool
+mt76_testmode_param_present(struct mt76_testmode_data *td, u16 idx)
+{
+	return td->param_set[idx / 32] & BIT(idx % 32);
+}
+
+static void
+mt76_testmode_init_defaults(struct mt76_dev *dev)
+{
+	struct mt76_testmode_data *td = &dev->test;
+
+	if (td->tx_msdu_len > 0)
+		return;
+
+	td->tx_msdu_len = 1024;
+	td->tx_count = 1;
+	td->tx_rate_mode = MT76_TM_TX_MODE_OFDM;
+	td->tx_rate_nss = 1;
+}
+
+static int
+__mt76_testmode_set_state(struct mt76_dev *dev, enum mt76_testmode_state state)
+{
+	enum mt76_testmode_state prev_state = dev->test.state;
+	int err;
+
+	if (prev_state == MT76_TM_STATE_TX_FRAMES)
+		mt76_testmode_tx_stop(dev);
+
+	if (state == MT76_TM_STATE_TX_FRAMES) {
+		err = mt76_testmode_tx_init(dev);
+		if (err)
+			return err;
+	}
+
+	err = dev->test_ops->set_state(dev, state);
+	if (err) {
+		if (state == MT76_TM_STATE_TX_FRAMES)
+			mt76_testmode_tx_stop(dev);
+
+		return err;
+	}
+
+	if (state == MT76_TM_STATE_TX_FRAMES)
+		mt76_testmode_tx_start(dev);
+	else if (state == MT76_TM_STATE_RX_FRAMES) {
+		memset(&dev->test.rx_stats, 0, sizeof(dev->test.rx_stats));
+	}
+
+	dev->test.state = state;
+
+	return 0;
+}
+
+int mt76_testmode_set_state(struct mt76_dev *dev, enum mt76_testmode_state state)
+{
+	struct mt76_testmode_data *td = &dev->test;
+	struct ieee80211_hw *hw = dev->phy.hw;
+
+	if (state == td->state && state == MT76_TM_STATE_OFF)
+		return 0;
+
+	if (state > MT76_TM_STATE_OFF &&
+	    (!test_bit(MT76_STATE_RUNNING, &dev->phy.state) ||
+	     !(hw->conf.flags & IEEE80211_CONF_MONITOR)))
+		return -ENOTCONN;
+
+	if (state != MT76_TM_STATE_IDLE &&
+	    td->state != MT76_TM_STATE_IDLE) {
+		int ret;
+
+		ret = __mt76_testmode_set_state(dev, MT76_TM_STATE_IDLE);
+		if (ret)
+			return ret;
+	}
+
+	return __mt76_testmode_set_state(dev, state);
+
+}
+EXPORT_SYMBOL(mt76_testmode_set_state);
+
+static int
+mt76_tm_get_u8(struct nlattr *attr, u8 *dest, u8 min, u8 max)
+{
+	u8 val;
+
+	if (!attr)
+		return 0;
+
+	val = nla_get_u8(attr);
+	if (val < min || val > max)
+		return -EINVAL;
+
+	*dest = val;
+	return 0;
+}
+
+int mt76_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+		      void *data, int len)
+{
+	struct mt76_phy *phy = hw->priv;
+	struct mt76_dev *dev = phy->dev;
+	struct mt76_testmode_data *td = &dev->test;
+	struct nlattr *tb[NUM_MT76_TM_ATTRS];
+	u32 state;
+	int err;
+	int i;
+
+	if (!dev->test_ops)
+		return -EOPNOTSUPP;
+
+	err = nla_parse_deprecated(tb, MT76_TM_ATTR_MAX, data, len,
+				   mt76_tm_policy, NULL);
+	if (err)
+		return err;
+
+	err = -EINVAL;
+
+	mutex_lock(&dev->mutex);
+
+	if (tb[MT76_TM_ATTR_RESET]) {
+		mt76_testmode_set_state(dev, MT76_TM_STATE_OFF);
+		memset(td, 0, sizeof(*td));
+	}
+
+	mt76_testmode_init_defaults(dev);
+
+	if (tb[MT76_TM_ATTR_TX_COUNT])
+		td->tx_count = nla_get_u32(tb[MT76_TM_ATTR_TX_COUNT]);
+
+	if (tb[MT76_TM_ATTR_TX_LENGTH]) {
+		u32 val = nla_get_u32(tb[MT76_TM_ATTR_TX_COUNT]);
+
+		if (val > IEEE80211_MAX_FRAME_LEN ||
+		    val < sizeof(struct ieee80211_hdr))
+			goto out;
+
+		td->tx_msdu_len = val;
+	}
+
+	if (tb[MT76_TM_ATTR_TX_RATE_IDX])
+		td->tx_rate_idx = nla_get_u8(tb[MT76_TM_ATTR_TX_RATE_IDX]);
+
+	if (mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_MODE], &td->tx_rate_mode,
+			   0, MT76_TM_TX_MODE_MAX) ||
+	    mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_NSS], &td->tx_rate_nss,
+			   1, hweight8(phy->antenna_mask)) ||
+	    mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_SGI], &td->tx_rate_sgi, 0, 1) ||
+	    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_ANTENNA], &td->tx_antenna_mask, 1,
+			   phy->antenna_mask) ||
+	    mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_POWER_CONTROL],
+			   &td->tx_power_control, 0, 1))
+		goto out;
+
+	if (tb[MT76_TM_ATTR_FREQ_OFFSET])
+		td->freq_offset = nla_get_u32(tb[MT76_TM_ATTR_FREQ_OFFSET]);
+
+	if (tb[MT76_TM_ATTR_STATE]) {
+		state = nla_get_u32(tb[MT76_TM_ATTR_STATE]);
+		if (state > MT76_TM_STATE_MAX)
+			goto out;
+	} else {
+		state = td->state;
+	}
+
+	if (tb[MT76_TM_ATTR_TX_POWER]) {
+		struct nlattr *cur;
+		int idx = 0;
+		int rem;
+
+		nla_for_each_nested(cur, tb[MT76_TM_ATTR_TX_POWER], rem) {
+			if (nla_len(cur) != 1 ||
+			    idx >= ARRAY_SIZE(td->tx_power))
+				goto out;
+
+			td->tx_power[idx++] = nla_get_u8(cur);
+		}
+	}
+
+	if (dev->test_ops->set_params) {
+		err = dev->test_ops->set_params(dev, tb, state);
+		if (err)
+			goto out;
+	}
+
+	for (i = MT76_TM_ATTR_STATE; i < ARRAY_SIZE(tb); i++)
+		if (tb[i])
+			mt76_testmode_param_set(td, i);
+
+	err = 0;
+	if (tb[MT76_TM_ATTR_STATE])
+		err = mt76_testmode_set_state(dev, state);
+
+out:
+	mutex_unlock(&dev->mutex);
+
+	return err;
+}
+EXPORT_SYMBOL(mt76_testmode_cmd);
+
+static int
+mt76_testmode_dump_stats(struct mt76_dev *dev, struct sk_buff *msg)
+{
+	struct mt76_testmode_data *td = &dev->test;
+	u64 rx_packets = 0;
+	u64 rx_fcs_error = 0;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(td->rx_stats.packets); i++) {
+		rx_packets += td->rx_stats.packets[i];
+		rx_fcs_error += td->rx_stats.fcs_error[i];
+	}
+
+	if (nla_put_u32(msg, MT76_TM_STATS_ATTR_TX_PENDING, td->tx_pending) ||
+	    nla_put_u32(msg, MT76_TM_STATS_ATTR_TX_QUEUED, td->tx_queued) ||
+	    nla_put_u32(msg, MT76_TM_STATS_ATTR_TX_DONE, td->tx_done) ||
+	    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))
+		return -EMSGSIZE;
+
+	if (dev->test_ops->dump_stats)
+		return dev->test_ops->dump_stats(dev, msg);
+
+	return 0;
+}
+
+int mt76_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *msg,
+		       struct netlink_callback *cb, void *data, int len)
+{
+	struct mt76_phy *phy = hw->priv;
+	struct mt76_dev *dev = phy->dev;
+	struct mt76_testmode_data *td = &dev->test;
+	struct nlattr *tb[NUM_MT76_TM_ATTRS] = {};
+	int err = 0;
+	void *a;
+	int i;
+
+	if (!dev->test_ops)
+		return -EOPNOTSUPP;
+
+	if (cb->args[2]++ > 0)
+		return -ENOENT;
+
+	if (data) {
+		err = nla_parse_deprecated(tb, MT76_TM_ATTR_MAX, data, len,
+					   mt76_tm_policy, NULL);
+		if (err)
+			return err;
+	}
+
+	mutex_lock(&dev->mutex);
+
+	if (tb[MT76_TM_ATTR_STATS]) {
+		a = nla_nest_start(msg, MT76_TM_ATTR_STATS);
+		err = mt76_testmode_dump_stats(dev, msg);
+		nla_nest_end(msg, a);
+
+		goto out;
+	}
+
+	mt76_testmode_init_defaults(dev);
+
+	err = -EMSGSIZE;
+	if (nla_put_u32(msg, MT76_TM_ATTR_STATE, td->state))
+		goto out;
+
+	if (td->mtd_name &&
+	    (nla_put_string(msg, MT76_TM_ATTR_MTD_PART, td->mtd_name) ||
+	     nla_put_u32(msg, MT76_TM_ATTR_MTD_OFFSET, td->mtd_offset)))
+		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_msdu_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) ||
+	    (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_ANTENNA) &&
+	     nla_put_u8(msg, MT76_TM_ATTR_TX_ANTENNA, td->tx_antenna_mask)) ||
+	    (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_POWER_CONTROL) &&
+	     nla_put_u8(msg, MT76_TM_ATTR_TX_POWER_CONTROL, td->tx_power_control)) ||
+	    (mt76_testmode_param_present(td, MT76_TM_ATTR_FREQ_OFFSET) &&
+	     nla_put_u8(msg, MT76_TM_ATTR_FREQ_OFFSET, td->freq_offset)))
+		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)
+			goto out;
+
+		for (i = 0; i < ARRAY_SIZE(td->tx_power); i++)
+			if (nla_put_u8(msg, i, td->tx_power[i]))
+				goto out;
+
+		nla_nest_end(msg, a);
+	}
+
+	err = 0;
+
+out:
+	mutex_unlock(&dev->mutex);
+
+	return err;
+}
+EXPORT_SYMBOL(mt76_testmode_dump);
diff --git a/drivers/net/wireless/mediatek/mt76/testmode.h b/drivers/net/wireless/mediatek/mt76/testmode.h
new file mode 100644
index 000000000000..691fe5773244
--- /dev/null
+++ b/drivers/net/wireless/mediatek/mt76/testmode.h
@@ -0,0 +1,156 @@
+/* SPDX-License-Identifier: ISC */
+/*
+ * Copyright (C) 2020 Felix Fietkau <nbd@nbd.name>
+ */
+#ifndef __MT76_TESTMODE_H
+#define __MT76_TESTMODE_H
+
+/**
+ * enum mt76_testmode_attr - testmode attributes inside NL80211_ATTR_TESTDATA
+ *
+ * @MT76_TM_ATTR_UNSPEC: (invalid attribute)
+ *
+ * @MT76_TM_ATTR_RESET: reset parameters to default (flag)
+ * @MT76_TM_ATTR_STATE: test state (u32), see &enum mt76_testmode_state
+ *
+ * @MT76_TM_ATTR_MTD_PART: mtd partition used for eeprom data (string)
+ * @MT76_TM_ATTR_MTD_OFFSET: offset of eeprom data within the partition (u32)
+ *
+ * @MT76_TM_ATTR_TX_COUNT: configured number of frames to send when setting
+ *	state to MT76_TM_STATE_TX_FRAMES (u32)
+ * @MT76_TM_ATTR_TX_PENDING: pending frames during MT76_TM_STATE_TX_FRAMES (u32)
+ * @MT76_TM_ATTR_TX_LENGTH: packet tx msdu length (u32)
+ * @MT76_TM_ATTR_TX_RATE_MODE: packet tx mode (u8, see &enum mt76_testmode_tx_mode)
+ * @MT76_TM_ATTR_TX_RATE_NSS: packet tx number of spatial streams (u8)
+ * @MT76_TM_ATTR_TX_RATE_IDX: packet tx rate/MCS index (u8)
+ * @MT76_TM_ATTR_TX_RATE_SGI: packet tx use short guard interval (u8)
+ * @MT76_TM_ATTR_TX_RATE_LDPC: packet tx enable LDPC (u8)
+ *
+ * @MT76_TM_ATTR_TX_ANTENNA: tx antenna mask (u8)
+ * @MT76_TM_ATTR_TX_POWER_CONTROL: enable tx power control (u8)
+ * @MT76_TM_ATTR_TX_POWER: per-antenna tx power array (nested, u8 attrs)
+ *
+ * @MT76_TM_ATTR_FREQ_OFFSET: RF frequency offset (u32)
+ *
+ * @MT76_TM_ATTR_STATS: statistics (nested, see &enum mt76_testmode_stats_attr)
+ */
+enum mt76_testmode_attr {
+	MT76_TM_ATTR_UNSPEC,
+
+	MT76_TM_ATTR_RESET,
+	MT76_TM_ATTR_STATE,
+
+	MT76_TM_ATTR_MTD_PART,
+	MT76_TM_ATTR_MTD_OFFSET,
+
+	MT76_TM_ATTR_TX_COUNT,
+	MT76_TM_ATTR_TX_LENGTH,
+	MT76_TM_ATTR_TX_RATE_MODE,
+	MT76_TM_ATTR_TX_RATE_NSS,
+	MT76_TM_ATTR_TX_RATE_IDX,
+	MT76_TM_ATTR_TX_RATE_SGI,
+	MT76_TM_ATTR_TX_RATE_LDPC,
+
+	MT76_TM_ATTR_TX_ANTENNA,
+	MT76_TM_ATTR_TX_POWER_CONTROL,
+	MT76_TM_ATTR_TX_POWER,
+
+	MT76_TM_ATTR_FREQ_OFFSET,
+
+	MT76_TM_ATTR_STATS,
+
+	/* keep last */
+	NUM_MT76_TM_ATTRS,
+	MT76_TM_ATTR_MAX = NUM_MT76_TM_ATTRS - 1,
+};
+
+/**
+ * enum mt76_testmode_state - statistics attributes
+ *
+ * @MT76_TM_STATS_ATTR_TX_PENDING: pending tx frames (u32)
+ * @MT76_TM_STATS_ATTR_TX_QUEUED: queued tx frames (u32)
+ * @MT76_TM_STATS_ATTR_TX_QUEUED: completed tx frames (u32)
+ *
+ * @MT76_TM_STATS_ATTR_RX_PACKETS: number of rx packets (u64)
+ * @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
+ */
+enum mt76_testmode_stats_attr {
+	MT76_TM_STATS_ATTR_UNSPEC,
+	MT76_TM_STATS_ATTR_PAD,
+
+	MT76_TM_STATS_ATTR_TX_PENDING,
+	MT76_TM_STATS_ATTR_TX_QUEUED,
+	MT76_TM_STATS_ATTR_TX_DONE,
+
+	MT76_TM_STATS_ATTR_RX_PACKETS,
+	MT76_TM_STATS_ATTR_RX_FCS_ERROR,
+	MT76_TM_STATS_ATTR_LAST_RX,
+
+	/* keep last */
+	NUM_MT76_TM_STATS_ATTRS,
+	MT76_TM_STATS_ATTR_MAX = NUM_MT76_TM_STATS_ATTRS - 1,
+};
+
+
+/**
+ * enum mt76_testmode_rx_attr - packet rx information
+ *
+ * @MT76_TM_RX_ATTR_FREQ_OFFSET: frequency offset (s32)
+ * @MT76_TM_RX_ATTR_RCPI: received channel power indicator (array, u8)
+ * @MT76_TM_RX_ATTR_IB_RSSI: internal inband RSSI (s8)
+ * @MT76_TM_RX_ATTR_WB_RSSI: internal wideband RSSI (s8)
+ */
+enum mt76_testmode_rx_attr {
+	MT76_TM_RX_ATTR_UNSPEC,
+
+	MT76_TM_RX_ATTR_FREQ_OFFSET,
+	MT76_TM_RX_ATTR_RCPI,
+	MT76_TM_RX_ATTR_IB_RSSI,
+	MT76_TM_RX_ATTR_WB_RSSI,
+
+	/* keep last */
+	NUM_MT76_TM_RX_ATTRS,
+	MT76_TM_RX_ATTR_MAX = NUM_MT76_TM_RX_ATTRS - 1,
+};
+
+/**
+ * enum mt76_testmode_state - phy test state
+ *
+ * @MT76_TM_STATE_OFF: test mode disabled (normal operation)
+ * @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
+ */
+enum mt76_testmode_state {
+	MT76_TM_STATE_OFF,
+	MT76_TM_STATE_IDLE,
+	MT76_TM_STATE_TX_FRAMES,
+	MT76_TM_STATE_RX_FRAMES,
+
+	/* keep last */
+	NUM_MT76_TM_STATES,
+	MT76_TM_STATE_MAX = NUM_MT76_TM_STATES - 1,
+};
+
+/**
+ * enum mt76_testmode_tx_mode - packet tx phy mode
+ *
+ * @MT76_TM_TX_MODE_CCK: legacy CCK mode
+ * @MT76_TM_TX_MODE_OFDM: legacy OFDM mode
+ * @MT76_TM_TX_MODE_HT: 802.11n MCS
+ * @MT76_TM_TX_MODE_VHT: 802.11ac MCS
+ */
+enum mt76_testmode_tx_mode {
+	MT76_TM_TX_MODE_CCK,
+	MT76_TM_TX_MODE_OFDM,
+	MT76_TM_TX_MODE_HT,
+	MT76_TM_TX_MODE_VHT,
+
+	/* keep last */
+	NUM_MT76_TM_TX_MODES,
+	MT76_TM_TX_MODE_MAX = NUM_MT76_TM_TX_MODES - 1,
+};
+
+#endif
diff --git a/drivers/net/wireless/mediatek/mt76/tx.c b/drivers/net/wireless/mediatek/mt76/tx.c
index f10c98aa883c..5adf92d7a9f3 100644
--- a/drivers/net/wireless/mediatek/mt76/tx.c
+++ b/drivers/net/wireless/mediatek/mt76/tx.c
@@ -236,6 +236,14 @@ void mt76_tx_complete_skb(struct mt76_dev *dev, struct sk_buff *skb)
 	struct ieee80211_hw *hw;
 	struct sk_buff_head list;
 
+#ifdef CONFIG_NL80211_TESTMODE
+	if (skb == dev->test.tx_skb) {
+		dev->test.tx_done++;
+		if (dev->test.tx_queued == dev->test.tx_done)
+			wake_up(&dev->tx_wait);
+	}
+#endif
+
 	if (!skb->prev) {
 		hw = mt76_tx_status_get_hw(dev, skb);
 		ieee80211_free_txskb(hw, skb);
@@ -259,6 +267,11 @@ mt76_tx(struct mt76_phy *phy, struct ieee80211_sta *sta,
 	int qid = skb_get_queue_mapping(skb);
 	bool ext_phy = phy != &dev->phy;
 
+	if (mt76_testmode_enabled(dev)) {
+		ieee80211_free_txskb(phy->hw, skb);
+		return;
+	}
+
 	if (WARN_ON(qid >= MT_TXQ_PSD)) {
 		qid = MT_TXQ_BE;
 		skb_set_queue_mapping(skb, qid);
@@ -579,6 +592,11 @@ void mt76_tx_tasklet(unsigned long data)
 	mt76_txq_schedule_all(&dev->phy);
 	if (dev->phy2)
 		mt76_txq_schedule_all(dev->phy2);
+
+#ifdef CONFIG_NL80211_TESTMODE
+	if (dev->test.tx_pending)
+		mt76_testmode_tx_pending(dev);
+#endif
 }
 
 void mt76_stop_tx_queues(struct mt76_dev *dev, struct ieee80211_sta *sta,
-- 
2.24.0


^ permalink raw reply related	[flat|nested] 8+ messages in thread

* [PATCH 7/7] mt76: mt7615: implement testmode support
  2020-06-22 20:13 [PATCH 1/7] mt76: mt7615: schedule tx tasklet and sta poll on mac tx free Felix Fietkau
                   ` (4 preceding siblings ...)
  2020-06-22 20:13 ` [PATCH 6/7] mt76: add API for testmode support Felix Fietkau
@ 2020-06-22 20:13 ` Felix Fietkau
  2020-07-10 12:44   ` [PATCH v2 " Felix Fietkau
  5 siblings, 1 reply; 8+ messages in thread
From: Felix Fietkau @ 2020-06-22 20:13 UTC (permalink / raw)
  To: linux-wireless

Supports sending a configurable number of packets with a specific rate
and configurable tx power levels / antenna settings, as well as displaying
rx statistics.

Signed-off-by: Felix Fietkau <nbd@nbd.name>
---
 .../wireless/mediatek/mt76/mt7615/Makefile    |   1 +
 .../net/wireless/mediatek/mt76/mt7615/init.c  |   8 +-
 .../net/wireless/mediatek/mt76/mt7615/mac.c   |  36 ++
 .../net/wireless/mediatek/mt76/mt7615/mac.h   |   5 +
 .../net/wireless/mediatek/mt76/mt7615/main.c  |  44 ++-
 .../net/wireless/mediatek/mt76/mt7615/mcu.c   |  35 +-
 .../net/wireless/mediatek/mt76/mt7615/mcu.h   |   6 +
 .../wireless/mediatek/mt76/mt7615/mt7615.h    |  18 +
 .../net/wireless/mediatek/mt76/mt7615/regs.h  |  16 +
 .../wireless/mediatek/mt76/mt7615/testmode.c  | 363 ++++++++++++++++++
 10 files changed, 523 insertions(+), 9 deletions(-)
 create mode 100644 drivers/net/wireless/mediatek/mt76/mt7615/testmode.c

diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/Makefile b/drivers/net/wireless/mediatek/mt76/mt7615/Makefile
index 99f353b8b9aa..d2ad84cb5244 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7615/Makefile
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/Makefile
@@ -8,6 +8,7 @@ CFLAGS_trace.o := -I$(src)
 
 mt7615-common-y := main.o init.o mcu.o eeprom.o mac.o \
 		   debugfs.o trace.o
+mt7615-common-$(CONFIG_NL80211_TESTMODE) += testmode.o
 
 mt7615e-y := pci.o pci_init.o dma.o pci_mac.o mmio.o
 mt7615e-$(CONFIG_MT7622_WMAC) += soc.o
diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/init.c b/drivers/net/wireless/mediatek/mt76/mt7615/init.c
index e2d80518e5af..2e0840b0e902 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7615/init.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/init.c
@@ -321,6 +321,7 @@ mt7615_init_wiphy(struct ieee80211_hw *hw)
 
 	ieee80211_hw_set(hw, SINGLE_SCAN_ON_ALL_BANDS);
 	ieee80211_hw_set(hw, TX_STATUS_NO_AMPDU_LEN);
+	ieee80211_hw_set(hw, WANT_MONITOR_VIF);
 
 	if (is_mt7615(&phy->dev->mt76))
 		hw->max_tx_fragments = MT_TXP_MAX_BUF_NUM;
@@ -405,9 +406,6 @@ int mt7615_register_ext_phy(struct mt7615_dev *dev)
 	mphy->sband_2g.sband.n_channels = 0;
 	mphy->hw->wiphy->bands[NL80211_BAND_2GHZ] = NULL;
 
-	/* 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);
@@ -457,5 +455,9 @@ void mt7615_init_device(struct mt7615_dev *dev)
 			IEEE80211_VHT_CAP_MAX_A_MPDU_LENGTH_EXPONENT_MASK;
 	mt7615_cap_dbdc_disable(dev);
 	dev->phy.dfs_state = -1;
+
+#ifdef CONFIG_NL80211_TESTMODE
+	dev->mt76.test_ops = &mt7615_testmode_ops;
+#endif
 }
 EXPORT_SYMBOL_GPL(mt7615_init_device);
diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/mac.c b/drivers/net/wireless/mediatek/mt76/mt7615/mac.c
index 1dc291e8b766..9d74a6550cf0 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7615/mac.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/mac.c
@@ -186,6 +186,40 @@ mt7615_get_status_freq_info(struct mt7615_dev *dev, struct mt76_phy *mphy,
 	status->freq = ieee80211_channel_to_frequency(chfreq, status->band);
 }
 
+static void mt7615_mac_fill_tm_rx(struct mt7615_dev *dev, __le32 *rxv)
+{
+#ifdef CONFIG_NL80211_TESTMODE
+	u32 rxv1 = le32_to_cpu(rxv[0]);
+	u32 rxv3 = le32_to_cpu(rxv[2]);
+	u32 rxv4 = le32_to_cpu(rxv[3]);
+	u32 rxv5 = le32_to_cpu(rxv[4]);
+	u8 cbw = FIELD_GET(MT_RXV1_FRAME_MODE, rxv1);
+	u8 mode = FIELD_GET(MT_RXV1_TX_MODE, rxv1);
+	s16 foe = FIELD_GET(MT_RXV5_FOE, rxv5);
+	u32 foe_const = (BIT(cbw + 1) & 0xf) * 10000;
+
+	if (!mode) {
+		/* CCK */
+		foe &= ~BIT(11);
+		foe *= 1000;
+		foe >>= 11;
+	} else {
+		if (foe > 2048)
+			foe -= 4096;
+
+		foe = (foe * foe_const) >> 15;
+	}
+
+	dev->test.last_freq_offset = foe;
+	dev->test.last_rcpi[0] = FIELD_GET(MT_RXV4_RCPI0, rxv4);
+	dev->test.last_rcpi[1] = FIELD_GET(MT_RXV4_RCPI1, rxv4);
+	dev->test.last_rcpi[2] = FIELD_GET(MT_RXV4_RCPI2, rxv4);
+	dev->test.last_rcpi[3] = FIELD_GET(MT_RXV4_RCPI3, rxv4);
+	dev->test.last_ib_rssi = FIELD_GET(MT_RXV3_IB_RSSI, rxv3);
+	dev->test.last_ib_rssi = FIELD_GET(MT_RXV3_WB_RSSI, rxv3);
+#endif
+}
+
 static int mt7615_mac_fill_rx(struct mt7615_dev *dev, struct sk_buff *skb)
 {
 	struct mt76_rx_status *status = (struct mt76_rx_status *)skb->cb;
@@ -401,6 +435,8 @@ static int mt7615_mac_fill_rx(struct mt7615_dev *dev, struct sk_buff *skb)
 					     status->chain_signal[i]);
 		}
 
+		mt7615_mac_fill_tm_rx(dev, rxd);
+
 		rxd += 6;
 		if ((u8 *)rxd - skb->data >= skb->len)
 			return -EINVAL;
diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/mac.h b/drivers/net/wireless/mediatek/mt76/mt7615/mac.h
index 81608ab656b8..169f4e17b5b4 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7615/mac.h
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/mac.h
@@ -100,11 +100,16 @@ enum rx_pkt_type {
 #define MT_RXV2_GROUP_ID		GENMASK(26, 21)
 #define MT_RXV2_LENGTH			GENMASK(20, 0)
 
+#define MT_RXV3_WB_RSSI			GENMASK(31, 24)
+#define MT_RXV3_IB_RSSI			GENMASK(23, 16)
+
 #define MT_RXV4_RCPI3			GENMASK(31, 24)
 #define MT_RXV4_RCPI2			GENMASK(23, 16)
 #define MT_RXV4_RCPI1			GENMASK(15, 8)
 #define MT_RXV4_RCPI0			GENMASK(7, 0)
 
+#define MT_RXV5_FOE			GENMASK(11, 0)
+
 #define MT_RXV6_NF3			GENMASK(31, 24)
 #define MT_RXV6_NF2			GENMASK(23, 16)
 #define MT_RXV6_NF1			GENMASK(15, 8)
diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/main.c b/drivers/net/wireless/mediatek/mt76/mt7615/main.c
index abb9853eaa8b..3abb33ddab7d 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7615/main.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/main.c
@@ -76,6 +76,8 @@ static void mt7615_stop(struct ieee80211_hw *hw)
 
 	mutex_lock(&dev->mt76.mutex);
 
+	mt76_testmode_reset(&dev->mt76, true);
+
 	clear_bit(MT76_STATE_RUNNING, &phy->mt76->state);
 	cancel_delayed_work_sync(&phy->scan_work);
 
@@ -137,6 +139,12 @@ static int mt7615_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(~dev->mphy.vif_mask) - 1;
 	if (mvif->idx >= MT7615_MAX_INTERFACES) {
 		ret = -ENOSPC;
@@ -197,6 +205,13 @@ static void mt7615_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;
+
 	mt7615_mcu_add_dev_info(dev, vif, false);
 
 	rcu_assign_pointer(dev->mt76.wcid[idx], NULL);
@@ -234,7 +249,7 @@ static void mt7615_init_dfs_state(struct mt7615_phy *phy)
 	phy->dfs_state = -1;
 }
 
-static int mt7615_set_channel(struct mt7615_phy *phy)
+int mt7615_set_channel(struct mt7615_phy *phy)
 {
 	struct mt7615_dev *dev = phy->dev;
 	bool ext_phy = phy != &dev->phy;
@@ -260,7 +275,7 @@ static int mt7615_set_channel(struct mt7615_phy *phy)
 	mt7615_mac_set_timing(phy);
 	ret = mt7615_dfs_init_radar_detector(phy);
 	mt7615_mac_cca_stats_reset(phy);
-	mt7615_mcu_set_sku_en(phy, true);
+	mt7615_mcu_set_sku_en(phy, !mt76_testmode_enabled(&dev->mt76));
 
 	mt7615_mac_reset_counters(dev);
 	phy->noise = 0;
@@ -271,8 +286,11 @@ static int mt7615_set_channel(struct mt7615_phy *phy)
 	mutex_unlock(&dev->mt76.mutex);
 
 	mt76_txq_schedule_all(phy->mt76);
-	ieee80211_queue_delayed_work(phy->mt76->hw, &phy->mac_work,
-				     MT7615_WATCHDOG_TIME);
+
+	if (!mt76_testmode_enabled(&dev->mt76))
+		ieee80211_queue_delayed_work(phy->mt76->hw, &phy->mac_work,
+					     MT7615_WATCHDOG_TIME);
+
 	return ret;
 }
 
@@ -369,6 +387,13 @@ static int mt7615_config(struct ieee80211_hw *hw, u32 changed)
 
 	if (changed & (IEEE80211_CONF_CHANGE_CHANNEL |
 		       IEEE80211_CONF_CHANGE_POWER)) {
+#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 = mt7615_set_channel(phy);
 		ieee80211_wake_queues(hw);
@@ -377,6 +402,8 @@ static int mt7615_config(struct ieee80211_hw *hw, u32 changed)
 	mutex_lock(&dev->mt76.mutex);
 
 	if (changed & IEEE80211_CONF_CHANGE_MONITOR) {
+		mt76_testmode_reset(&dev->mt76, true);
+
 		if (!(hw->conf.flags & IEEE80211_CONF_MONITOR))
 			phy->rxfilter |= MT_WF_RFCR_DROP_OTHER_UC;
 		else
@@ -419,10 +446,13 @@ static void mt7615_configure_filter(struct ieee80211_hw *hw,
 			MT_WF_RFCR1_DROP_CFACK;
 	u32 flags = 0;
 
+	mutex_lock(&dev->mt76.mutex);
+
 #define MT76_FILTER(_flag, _hw) do { \
 		flags |= *total_flags & FIF_##_flag;			\
 		phy->rxfilter &= ~(_hw);				\
-		phy->rxfilter |= !(flags & FIF_##_flag) * (_hw);	\
+		if (!mt76_testmode_enabled(&dev->mt76))			\
+			phy->rxfilter |= !(flags & FIF_##_flag) * (_hw);\
 	} while (0)
 
 	phy->rxfilter &= ~(MT_WF_RFCR_DROP_OTHER_BSS |
@@ -455,6 +485,8 @@ static void mt7615_configure_filter(struct ieee80211_hw *hw,
 		mt76_clear(dev, MT_WF_RFCR1(band), ctl_flags);
 	else
 		mt76_set(dev, MT_WF_RFCR1(band), ctl_flags);
+
+	mutex_unlock(&dev->mt76.mutex);
 }
 
 static void mt7615_bss_info_changed(struct ieee80211_hw *hw,
@@ -1069,6 +1101,8 @@ const struct ieee80211_ops mt7615_ops = {
 	.sched_scan_stop = mt7615_stop_sched_scan,
 	.remain_on_channel = mt7615_remain_on_channel,
 	.cancel_remain_on_channel = mt7615_cancel_remain_on_channel,
+	CFG80211_TESTMODE_CMD(mt76_testmode_cmd)
+	CFG80211_TESTMODE_DUMP(mt76_testmode_dump)
 #ifdef CONFIG_PM
 	.suspend = mt7615_suspend,
 	.resume = mt7615_resume,
diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/mcu.c b/drivers/net/wireless/mediatek/mt76/mt7615/mcu.c
index b4440f131925..026bf938c675 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7615/mcu.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/mcu.c
@@ -2806,6 +2806,14 @@ int mt7615_mcu_set_chan_info(struct mt7615_phy *phy, int cmd)
 		.center_chan2 = ieee80211_frequency_to_channel(freq2),
 	};
 
+#ifdef CONFIG_NL80211_TESTMODE
+	if (dev->mt76.test.state == MT76_TM_STATE_TX_FRAMES &&
+	    dev->mt76.test.tx_antenna_mask) {
+		req.tx_streams = hweight8(dev->mt76.test.tx_antenna_mask);
+		req.rx_streams_mask = 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) &&
@@ -2817,7 +2825,10 @@ int mt7615_mcu_set_chan_info(struct mt7615_phy *phy, int cmd)
 	req.band_idx = phy != &dev->phy;
 	req.bw = mt7615_mcu_chan_bw(chandef);
 
-	mt7615_mcu_set_txpower_sku(phy, req.txpower_sku);
+	if (mt76_testmode_enabled(&dev->mt76))
+		memset(req.txpower_sku, 0x3f, 49);
+	else
+		mt7615_mcu_set_txpower_sku(phy, req.txpower_sku);
 
 	return __mt76_mcu_send_msg(&dev->mt76, cmd, &req, sizeof(req), true);
 }
@@ -2835,6 +2846,28 @@ int mt7615_mcu_get_temperature(struct mt7615_dev *dev, int index)
 				   sizeof(req), true);
 }
 
+int mt7615_mcu_set_test_param(struct mt7615_dev *dev, u8 param, bool test_mode,
+			      bool en)
+{
+	struct {
+		u8 test_mode_en;
+		u8 param_idx;
+		u8 _rsv[2];
+
+		u8 enable;
+		u8 _rsv2[3];
+
+		u8 pad[8];
+	} 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 mt7615_mcu_set_sku_en(struct mt7615_phy *phy, bool enable)
 {
 	struct mt7615_dev *dev = phy->dev;
diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/mcu.h b/drivers/net/wireless/mediatek/mt76/mt7615/mcu.h
index 8e2be150a556..e8c7aa5c35ce 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7615/mcu.h
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/mcu.h
@@ -268,6 +268,7 @@ enum {
 	MCU_EXT_CMD_GET_TEMP = 0x2c,
 	MCU_EXT_CMD_WTBL_UPDATE = 0x32,
 	MCU_EXT_CMD_SET_RDD_CTRL = 0x3a,
+	MCU_EXT_CMD_ATE_CTRL = 0x3d,
 	MCU_EXT_CMD_PROTECT_CTRL = 0x3e,
 	MCU_EXT_CMD_DBDC_CTRL = 0x45,
 	MCU_EXT_CMD_MAC_INIT_CTRL = 0x46,
@@ -289,6 +290,11 @@ enum {
 	MCU_UNI_CMD_HIF_CTRL = MCU_UNI_PREFIX | 0x07,
 };
 
+enum {
+	MCU_ATE_SET_FREQ_OFFSET = 0xa,
+	MCU_ATE_SET_TX_POWER_CONTROL = 0x15,
+};
+
 struct mt7615_mcu_uni_event {
 	u8 cid;
 	u8 pad[3];
diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/mt7615.h b/drivers/net/wireless/mediatek/mt76/mt7615/mt7615.h
index af86cf6925dd..1ae6ea5b4f16 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7615/mt7615.h
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/mt7615.h
@@ -169,6 +169,8 @@ struct mt7615_phy {
 	struct mt76_phy *mt76;
 	struct mt7615_dev *dev;
 
+	struct ieee80211_vif *monitor_vif;
+
 	u32 rxfilter;
 	u32 omac_mask;
 
@@ -283,6 +285,17 @@ struct mt7615_dev {
 
 	u32 debugfs_rf_wf;
 	u32 debugfs_rf_reg;
+
+#ifdef CONFIG_NL80211_TESTMODE
+	struct {
+		u32 *reg_backup;
+
+		s16 last_freq_offset;
+		u8 last_rcpi[4];
+		s8 last_ib_rssi;
+		s8 last_wb_rssi;
+	} test;
+#endif
 };
 
 enum tx_pkt_queue_idx {
@@ -377,6 +390,7 @@ extern const u32 mt7615e_reg_map[__MT_BASE_MAX];
 extern const u32 mt7663e_reg_map[__MT_BASE_MAX];
 extern struct pci_driver mt7615_pci_driver;
 extern struct platform_driver mt7622_wmac_driver;
+extern const struct mt76_testmode_ops mt7615_testmode_ops;
 
 #ifdef CONFIG_MT7622_WMAC
 int mt7622_wmac_init(struct mt7615_dev *dev);
@@ -488,6 +502,7 @@ void mt7615_init_txpower(struct mt7615_dev *dev,
 			 struct ieee80211_supported_band *sband);
 void mt7615_phy_init(struct mt7615_dev *dev);
 void mt7615_mac_init(struct mt7615_dev *dev);
+int mt7615_set_channel(struct mt7615_phy *phy);
 
 int mt7615_mcu_restart(struct mt76_dev *dev);
 void mt7615_update_channel(struct mt76_dev *mdev);
@@ -531,6 +546,7 @@ int mt7615_mcu_set_eeprom(struct mt7615_dev *dev);
 int mt7615_mcu_set_mac_enable(struct mt7615_dev *dev, int band, bool enable);
 int mt7615_mcu_set_rts_thresh(struct mt7615_phy *phy, u32 val);
 int mt7615_mcu_get_temperature(struct mt7615_dev *dev, int index);
+int mt7615_mcu_set_tx_power(struct mt7615_phy *phy);
 void mt7615_mcu_exit(struct mt7615_dev *dev);
 void mt7615_mcu_fill_msg(struct mt7615_dev *dev, struct sk_buff *skb,
 			 int cmd, int *wait_seq);
@@ -569,6 +585,8 @@ int mt7615_mcu_set_pulse_th(struct mt7615_dev *dev,
 			    const struct mt7615_dfs_pulse *pulse);
 int mt7615_mcu_set_radar_th(struct mt7615_dev *dev, int index,
 			    const struct mt7615_dfs_pattern *pattern);
+int mt7615_mcu_set_test_param(struct mt7615_dev *dev, u8 param, bool test_mode,
+			      bool en);
 int mt7615_mcu_set_sku_en(struct mt7615_phy *phy, bool enable);
 int mt7615_mcu_apply_rx_dcoc(struct mt7615_phy *phy);
 int mt7615_mcu_apply_tx_dpd(struct mt7615_phy *phy);
diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/regs.h b/drivers/net/wireless/mediatek/mt76/mt7615/regs.h
index 4743c12005a4..8d2c54918e68 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7615/regs.h
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/regs.h
@@ -212,6 +212,9 @@ enum mt7615_reg_base {
 #define MT_WF_PHY_RXTD2_BASE		MT_WF_PHY(0x2a00)
 #define MT_WF_PHY_RXTD2(_n)		(MT_WF_PHY_RXTD2_BASE + ((_n) << 2))
 
+#define MT_WF_PHY_RFINTF3_0(_n)		MT_WF_PHY(0x1100 + (_n) * 0x400)
+#define MT_WF_PHY_RFINTF3_0_ANT		GENMASK(7, 4)
+
 #define MT_WF_CFG_BASE			((dev)->reg_map[MT_CFG_BASE])
 #define MT_WF_CFG(ofs)			(MT_WF_CFG_BASE + (ofs))
 
@@ -255,6 +258,13 @@ enum mt7615_reg_base {
 #define MT_WF_ARB_BASE			((dev)->reg_map[MT_ARB_BASE])
 #define MT_WF_ARB(ofs)			(MT_WF_ARB_BASE + (ofs))
 
+#define MT_ARB_RQCR			MT_WF_ARB(0x070)
+#define MT_ARB_RQCR_RX_START		BIT(0)
+#define MT_ARB_RQCR_RXV_START		BIT(4)
+#define MT_ARB_RQCR_RXV_R_EN		BIT(7)
+#define MT_ARB_RQCR_RXV_T_EN		BIT(8)
+#define MT_ARB_RQCR_BAND_SHIFT		16
+
 #define MT_ARB_SCR			MT_WF_ARB(0x080)
 #define MT_ARB_SCR_TX0_DISABLE		BIT(8)
 #define MT_ARB_SCR_RX0_DISABLE		BIT(9)
@@ -550,4 +560,10 @@ enum mt7615_reg_base {
 #define MT_WL_RX_BUSY			BIT(30)
 #define MT_WL_TX_BUSY			BIT(31)
 
+#define MT_MCU_PTA_BASE			0x81060000
+#define MT_MCU_PTA(_n)			(MT_MCU_PTA_BASE + (_n))
+
+#define MT_ANT_SWITCH_CON(n)		MT_MCU_PTA(0x0c8)
+#define MT_ANT_SWITCH_CON_MODE(_n)	(GENMASK(4, 0) << (_n * 8))
+
 #endif
diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/testmode.c b/drivers/net/wireless/mediatek/mt76/mt7615/testmode.c
new file mode 100644
index 000000000000..55ebb1ec8c70
--- /dev/null
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/testmode.c
@@ -0,0 +1,363 @@
+// SPDX-License-Identifier: ISC
+/* Copyright (C) 2020 Felix Fietkau <nbd@nbd.name> */
+
+#include "mt7615.h"
+#include "eeprom.h"
+#include "mcu.h"
+
+enum {
+	TM_CHANGED_TXPOWER_CTRL,
+	TM_CHANGED_TXPOWER,
+	TM_CHANGED_FREQ_OFFSET,
+
+	/* must be last */
+	NUM_TM_CHANGED
+};
+
+
+static const u8 tm_change_map[] = {
+	[TM_CHANGED_TXPOWER_CTRL] = MT76_TM_ATTR_TX_POWER_CONTROL,
+	[TM_CHANGED_TXPOWER] = MT76_TM_ATTR_TX_POWER,
+	[TM_CHANGED_FREQ_OFFSET] = MT76_TM_ATTR_FREQ_OFFSET,
+};
+
+static const u32 reg_backup_list[] = {
+	MT_WF_PHY_RFINTF3_0(0),
+	MT_WF_PHY_RFINTF3_0(1),
+	MT_WF_PHY_RFINTF3_0(2),
+	MT_WF_PHY_RFINTF3_0(3),
+	MT_ANT_SWITCH_CON(2),
+	MT_ANT_SWITCH_CON(3),
+	MT_ANT_SWITCH_CON(4),
+	MT_ANT_SWITCH_CON(6),
+	MT_ANT_SWITCH_CON(7),
+	MT_ANT_SWITCH_CON(8),
+};
+
+static const struct {
+	u16 wf;
+	u16 reg;
+} rf_backup_list[] = {
+	{ 0, 0x48 },
+	{ 1, 0x48 },
+	{ 2, 0x48 },
+	{ 3, 0x48 },
+};
+
+static int
+mt7615_tm_set_tx_power(struct mt7615_phy *phy)
+{
+	struct mt7615_dev *dev = phy->dev;
+	struct mt76_phy *mphy = phy->mt76;
+	int i, ret, n_chains = hweight8(mphy->antenna_mask);
+	struct cfg80211_chan_def *chandef = &mphy->chandef;
+	int freq = chandef->center_freq1, len, target_chains;
+	u8 *data, *eep = (u8 *)dev->mt76.eeprom.data;
+	enum nl80211_band band = chandef->chan->band;
+	struct sk_buff *skb;
+	struct {
+		u8 center_chan;
+		u8 dbdc_idx;
+		u8 band;
+		u8 rsv;
+	} __packed req_hdr = {
+		.center_chan = ieee80211_frequency_to_channel(freq),
+		.band = band,
+		.dbdc_idx = phy != &dev->phy,
+	};
+	u8 *tx_power = NULL;
+
+	if (dev->mt76.test.state != MT76_TM_STATE_OFF)
+		tx_power = dev->mt76.test.tx_power;
+
+	len = sizeof(req_hdr) + MT7615_EE_MAX - MT_EE_NIC_CONF_0;
+	skb = mt76_mcu_msg_alloc(&dev->mt76, NULL, sizeof(req_hdr) + len);
+	if (!skb)
+		return -ENOMEM;
+
+	skb_put_data(skb, &req_hdr, sizeof(req_hdr));
+	data = skb_put_data(skb, eep + MT_EE_NIC_CONF_0, len);
+
+	target_chains = mt7615_ext_pa_enabled(dev, band) ? 1 : n_chains;
+	for (i = 0; i < target_chains; i++) {
+		int index;
+
+		ret = mt7615_eeprom_get_target_power_index(dev, chandef->chan, i);
+		if (ret < 0)
+			return -EINVAL;
+
+		index = ret - MT_EE_NIC_CONF_0;
+		if (tx_power && tx_power[i])
+			data[ret - MT_EE_NIC_CONF_0] = tx_power[i];
+	}
+
+	return __mt76_mcu_skb_send_msg(&dev->mt76, skb,
+				       MCU_EXT_CMD_SET_TX_POWER_CTRL, false);
+}
+
+static void
+mt7615_tm_reg_backup_restore(struct mt7615_dev *dev)
+{
+	u32 *b = dev->test.reg_backup;
+	int n_regs = ARRAY_SIZE(reg_backup_list);
+	int n_rf_regs = ARRAY_SIZE(rf_backup_list);
+	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], b[i]);
+
+		for (i = 0; i < n_rf_regs; i++)
+			mt7615_rf_wr(dev, rf_backup_list[i].wf,
+				     rf_backup_list[i].reg, b[n_regs + i]);
+		return;
+	}
+
+	if (b)
+		return;
+
+	b = devm_kzalloc(dev->mt76.dev, 4 * (n_regs + n_rf_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]);
+	for (i = 0; i < n_rf_regs; i++)
+		b[n_regs + i] = mt7615_rf_rr(dev, rf_backup_list[i].wf,
+					     rf_backup_list[i].reg);
+}
+
+
+static void
+mt7615_tm_init_phy(struct mt7615_dev *dev, struct mt7615_phy *phy)
+{
+	unsigned int total_flags = ~0;
+
+	if (!test_bit(MT76_STATE_RUNNING, &phy->mt76->state))
+		return;
+
+	mutex_unlock(&dev->mt76.mutex);
+	mt7615_set_channel(phy);
+	mt7615_ops.configure_filter(phy->mt76->hw, 0, &total_flags, 0);
+	mutex_lock(&dev->mt76.mutex);
+
+	mt7615_tm_reg_backup_restore(dev);
+}
+
+static void
+mt7615_tm_init(struct mt7615_dev *dev)
+{
+	mt7615_tm_init_phy(dev, &dev->phy);
+
+	if (dev->mt76.phy2)
+		mt7615_tm_init_phy(dev, dev->mt76.phy2->priv);
+}
+
+static void
+mt7615_tm_set_rx_enable(struct mt7615_dev *dev, bool en)
+{
+	u32 rqcr_mask = (MT_ARB_RQCR_RX_START |
+			 MT_ARB_RQCR_RXV_START |
+			 MT_ARB_RQCR_RXV_R_EN |
+			 MT_ARB_RQCR_RXV_T_EN) *
+			(BIT(0) | BIT(MT_ARB_RQCR_BAND_SHIFT));
+
+	if (en) {
+		mt76_clear(dev, MT_ARB_SCR,
+			   MT_ARB_SCR_RX0_DISABLE | MT_ARB_SCR_RX1_DISABLE);
+		mt76_set(dev, MT_ARB_RQCR, rqcr_mask);
+	} else {
+		mt76_set(dev, MT_ARB_SCR,
+			 MT_ARB_SCR_RX0_DISABLE | MT_ARB_SCR_RX1_DISABLE);
+		mt76_clear(dev, MT_ARB_RQCR, rqcr_mask);
+	}
+}
+
+static void
+mt7615_tm_set_tx_antenna(struct mt7615_dev *dev, bool en)
+{
+	struct mt76_testmode_data *td = &dev->mt76.test;
+	u8 mask = td->tx_antenna_mask;
+	int i;
+
+	if (!mask)
+		return;
+
+	if (!en)
+		mask = dev->phy.chainmask;
+
+	for (i = 0; i < 4; i++) {
+		mt76_rmw_field(dev, MT_WF_PHY_RFINTF3_0(i),
+			       MT_WF_PHY_RFINTF3_0_ANT,
+			       td->tx_antenna_mask & BIT(i) ? 0xa : 0);
+
+	}
+
+	/* 2.4 GHz band */
+	mt76_rmw_field(dev, MT_ANT_SWITCH_CON(3), MT_ANT_SWITCH_CON_MODE(0),
+		       (td->tx_antenna_mask & BIT(0)) ? 0x8 : 0x1b);
+	mt76_rmw_field(dev, MT_ANT_SWITCH_CON(4), MT_ANT_SWITCH_CON_MODE(2),
+		       (td->tx_antenna_mask & BIT(1)) ? 0xe : 0x1b);
+	mt76_rmw_field(dev, MT_ANT_SWITCH_CON(6), MT_ANT_SWITCH_CON_MODE(0),
+		       (td->tx_antenna_mask & BIT(2)) ? 0x0 : 0xf);
+	mt76_rmw_field(dev, MT_ANT_SWITCH_CON(7), MT_ANT_SWITCH_CON_MODE(2),
+		       (td->tx_antenna_mask & BIT(3)) ? 0x6 : 0xf);
+
+	/* 5 GHz band */
+	mt76_rmw_field(dev, MT_ANT_SWITCH_CON(4), MT_ANT_SWITCH_CON_MODE(1),
+		       (td->tx_antenna_mask & BIT(0)) ? 0xd : 0x1b);
+	mt76_rmw_field(dev, MT_ANT_SWITCH_CON(2), MT_ANT_SWITCH_CON_MODE(3),
+		       (td->tx_antenna_mask & BIT(1)) ? 0x13 : 0x1b);
+	mt76_rmw_field(dev, MT_ANT_SWITCH_CON(7), MT_ANT_SWITCH_CON_MODE(1),
+		       (td->tx_antenna_mask & BIT(2)) ? 0x5 : 0xf);
+	mt76_rmw_field(dev, MT_ANT_SWITCH_CON(8), MT_ANT_SWITCH_CON_MODE(3),
+		       (td->tx_antenna_mask & BIT(3)) ? 0xb : 0xf);
+
+	for (i = 0; i < 4; i++) {
+		u32 val;
+
+		val = mt7615_rf_rr(dev, i, 0x48);
+		val &= ~(0x3ff << 20);
+		if (td->tx_antenna_mask & BIT(i))
+			val |= 3 << 20;
+		else
+			val |= (2 << 28) | (2 << 26) | (8 << 20);
+		mt7615_rf_wr(dev, i, 0x48, val);
+	}
+}
+
+static void
+mt7615_tm_set_tx_frames(struct mt7615_dev *dev, bool en)
+{
+	struct ieee80211_tx_info *info;
+	struct sk_buff *skb = dev->mt76.test.tx_skb;
+
+	mt7615_mcu_set_chan_info(&dev->phy, MCU_EXT_CMD_SET_RX_PATH);
+	mt7615_tm_set_tx_antenna(dev, en);
+	mt7615_tm_set_rx_enable(dev, !en);
+	if (!en || !skb)
+		return;
+
+	info = IEEE80211_SKB_CB(skb);
+	info->control.vif = dev->phy.monitor_vif;
+}
+
+static void
+mt7615_tm_update_params(struct mt7615_dev *dev, u32 changed)
+{
+	struct mt76_testmode_data *td = &dev->mt76.test;
+	bool en = dev->mt76.test.state != MT76_TM_STATE_OFF;
+
+	if (changed & BIT(TM_CHANGED_TXPOWER_CTRL))
+		mt7615_mcu_set_test_param(dev, MCU_ATE_SET_TX_POWER_CONTROL,
+					  en, en && td->tx_power_control);
+	if (changed & BIT(TM_CHANGED_FREQ_OFFSET))
+		mt7615_mcu_set_test_param(dev, MCU_ATE_SET_FREQ_OFFSET,
+					  en, en ? td->freq_offset : 0);
+	if (changed & BIT(TM_CHANGED_TXPOWER))
+		mt7615_tm_set_tx_power(&dev->phy);
+}
+
+static int
+mt7615_tm_set_state(struct mt76_dev *mdev, enum mt76_testmode_state state)
+{
+	struct mt7615_dev *dev = container_of(mdev, struct mt7615_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)
+		mt7615_tm_set_tx_frames(dev, false);
+	else if (state == MT76_TM_STATE_TX_FRAMES)
+		mt7615_tm_set_rx_enable(dev, true);
+
+	if (state <= MT76_TM_STATE_IDLE)
+		mt7615_tm_init(dev);
+
+	if ((state == MT76_TM_STATE_IDLE &&
+	     prev_state == MT76_TM_STATE_OFF) ||
+	    (state == MT76_TM_STATE_OFF &&
+	     prev_state == MT76_TM_STATE_IDLE)) {
+		u32 changed = 0;
+		int i;
+
+		for (i = 0; i < ARRAY_SIZE(tm_change_map); i++) {
+			u16 cur = tm_change_map[i];
+
+			if (td->param_set[cur / 32] & BIT(cur % 32))
+				changed |= BIT(i);
+		}
+
+		mt7615_tm_update_params(dev, changed);
+	}
+
+	return 0;
+}
+
+static int
+mt7615_tm_set_params(struct mt76_dev *mdev, struct nlattr **tb,
+		     enum mt76_testmode_state new_state)
+{
+	struct mt7615_dev *dev = container_of(mdev, struct mt7615_dev, mt76);
+	struct mt76_testmode_data *td = &dev->mt76.test;
+	u32 changed = 0;
+	int i;
+
+	BUILD_BUG_ON(NUM_TM_CHANGED >= 32);
+
+	if (new_state == MT76_TM_STATE_OFF ||
+	    td->state == MT76_TM_STATE_OFF)
+		return 0;
+
+	if (td->tx_antenna_mask & ~dev->phy.chainmask)
+		return -EINVAL;
+
+	for (i = 0; i < ARRAY_SIZE(tm_change_map); i++) {
+		if (tb[tm_change_map[i]])
+			changed |= BIT(i);
+	}
+
+	mt7615_tm_update_params(dev, changed);
+
+	return 0;
+}
+
+static int
+mt7615_tm_dump_stats(struct mt76_dev *mdev, struct sk_buff *msg)
+{
+	struct mt7615_dev *dev = container_of(mdev, struct mt7615_dev, mt76);
+	void *rx, *rssi;
+	int i;
+
+	rx = nla_nest_start(msg, MT76_TM_STATS_ATTR_LAST_RX);
+	if (!rx)
+		return -ENOMEM;
+
+	if (nla_put_s32(msg, MT76_TM_RX_ATTR_FREQ_OFFSET, dev->test.last_freq_offset) ||
+	    nla_put_s32(msg, MT76_TM_RX_ATTR_IB_RSSI, dev->test.last_ib_rssi) ||
+	    nla_put_s32(msg, MT76_TM_RX_ATTR_WB_RSSI, dev->test.last_wb_rssi))
+		return -ENOMEM;
+
+	rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_RCPI);
+	if (!rssi)
+		return -ENOMEM;
+
+	for (i = 0; i < ARRAY_SIZE(dev->test.last_rcpi); i++)
+		if (nla_put_u8(msg, i, dev->test.last_rcpi[i]))
+			return -ENOMEM;
+
+	nla_nest_end(msg, rssi);
+
+	nla_nest_end(msg, rx);
+
+	return 0;
+}
+
+const struct mt76_testmode_ops mt7615_testmode_ops = {
+	.set_state = mt7615_tm_set_state,
+	.set_params = mt7615_tm_set_params,
+	.dump_stats = mt7615_tm_dump_stats,
+};
-- 
2.24.0


^ permalink raw reply related	[flat|nested] 8+ messages in thread

* [PATCH v2 7/7] mt76: mt7615: implement testmode support
  2020-06-22 20:13 ` [PATCH 7/7] mt76: mt7615: implement " Felix Fietkau
@ 2020-07-10 12:44   ` Felix Fietkau
  0 siblings, 0 replies; 8+ messages in thread
From: Felix Fietkau @ 2020-07-10 12:44 UTC (permalink / raw)
  To: linux-wireless

Supports sending a configurable number of packets with a specific rate
and configurable tx power levels / antenna settings, as well as displaying
rx statistics.

Signed-off-by: Felix Fietkau <nbd@nbd.name>
---
v2: fix reporting wideband rssi

 .../wireless/mediatek/mt76/mt7615/Makefile    |   1 +
 .../net/wireless/mediatek/mt76/mt7615/init.c  |   8 +-
 .../net/wireless/mediatek/mt76/mt7615/mac.c   |  36 ++
 .../net/wireless/mediatek/mt76/mt7615/mac.h   |   5 +
 .../net/wireless/mediatek/mt76/mt7615/main.c  |  44 ++-
 .../net/wireless/mediatek/mt76/mt7615/mcu.c   |  35 +-
 .../net/wireless/mediatek/mt76/mt7615/mcu.h   |   6 +
 .../wireless/mediatek/mt76/mt7615/mt7615.h    |  18 +
 .../net/wireless/mediatek/mt76/mt7615/regs.h  |  16 +
 .../wireless/mediatek/mt76/mt7615/testmode.c  | 363 ++++++++++++++++++
 10 files changed, 523 insertions(+), 9 deletions(-)
 create mode 100644 drivers/net/wireless/mediatek/mt76/mt7615/testmode.c

diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/Makefile b/drivers/net/wireless/mediatek/mt76/mt7615/Makefile
index 99f353b8b9aa..d2ad84cb5244 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7615/Makefile
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/Makefile
@@ -8,6 +8,7 @@ CFLAGS_trace.o := -I$(src)
 
 mt7615-common-y := main.o init.o mcu.o eeprom.o mac.o \
 		   debugfs.o trace.o
+mt7615-common-$(CONFIG_NL80211_TESTMODE) += testmode.o
 
 mt7615e-y := pci.o pci_init.o dma.o pci_mac.o mmio.o
 mt7615e-$(CONFIG_MT7622_WMAC) += soc.o
diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/init.c b/drivers/net/wireless/mediatek/mt76/mt7615/init.c
index b04fe7301d4a..7daaab277b26 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7615/init.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/init.c
@@ -330,6 +330,7 @@ mt7615_init_wiphy(struct ieee80211_hw *hw)
 
 	ieee80211_hw_set(hw, SINGLE_SCAN_ON_ALL_BANDS);
 	ieee80211_hw_set(hw, TX_STATUS_NO_AMPDU_LEN);
+	ieee80211_hw_set(hw, WANT_MONITOR_VIF);
 
 	if (is_mt7615(&phy->dev->mt76))
 		hw->max_tx_fragments = MT_TXP_MAX_BUF_NUM;
@@ -414,9 +415,6 @@ int mt7615_register_ext_phy(struct mt7615_dev *dev)
 	mphy->sband_2g.sband.n_channels = 0;
 	mphy->hw->wiphy->bands[NL80211_BAND_2GHZ] = NULL;
 
-	/* 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);
@@ -466,5 +464,9 @@ void mt7615_init_device(struct mt7615_dev *dev)
 			IEEE80211_VHT_CAP_MAX_A_MPDU_LENGTH_EXPONENT_MASK;
 	mt7615_cap_dbdc_disable(dev);
 	dev->phy.dfs_state = -1;
+
+#ifdef CONFIG_NL80211_TESTMODE
+	dev->mt76.test_ops = &mt7615_testmode_ops;
+#endif
 }
 EXPORT_SYMBOL_GPL(mt7615_init_device);
diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/mac.c b/drivers/net/wireless/mediatek/mt76/mt7615/mac.c
index 1dc291e8b766..2d432d052780 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7615/mac.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/mac.c
@@ -186,6 +186,40 @@ mt7615_get_status_freq_info(struct mt7615_dev *dev, struct mt76_phy *mphy,
 	status->freq = ieee80211_channel_to_frequency(chfreq, status->band);
 }
 
+static void mt7615_mac_fill_tm_rx(struct mt7615_dev *dev, __le32 *rxv)
+{
+#ifdef CONFIG_NL80211_TESTMODE
+	u32 rxv1 = le32_to_cpu(rxv[0]);
+	u32 rxv3 = le32_to_cpu(rxv[2]);
+	u32 rxv4 = le32_to_cpu(rxv[3]);
+	u32 rxv5 = le32_to_cpu(rxv[4]);
+	u8 cbw = FIELD_GET(MT_RXV1_FRAME_MODE, rxv1);
+	u8 mode = FIELD_GET(MT_RXV1_TX_MODE, rxv1);
+	s16 foe = FIELD_GET(MT_RXV5_FOE, rxv5);
+	u32 foe_const = (BIT(cbw + 1) & 0xf) * 10000;
+
+	if (!mode) {
+		/* CCK */
+		foe &= ~BIT(11);
+		foe *= 1000;
+		foe >>= 11;
+	} else {
+		if (foe > 2048)
+			foe -= 4096;
+
+		foe = (foe * foe_const) >> 15;
+	}
+
+	dev->test.last_freq_offset = foe;
+	dev->test.last_rcpi[0] = FIELD_GET(MT_RXV4_RCPI0, rxv4);
+	dev->test.last_rcpi[1] = FIELD_GET(MT_RXV4_RCPI1, rxv4);
+	dev->test.last_rcpi[2] = FIELD_GET(MT_RXV4_RCPI2, rxv4);
+	dev->test.last_rcpi[3] = FIELD_GET(MT_RXV4_RCPI3, rxv4);
+	dev->test.last_ib_rssi = FIELD_GET(MT_RXV3_IB_RSSI, rxv3);
+	dev->test.last_wb_rssi = FIELD_GET(MT_RXV3_WB_RSSI, rxv3);
+#endif
+}
+
 static int mt7615_mac_fill_rx(struct mt7615_dev *dev, struct sk_buff *skb)
 {
 	struct mt76_rx_status *status = (struct mt76_rx_status *)skb->cb;
@@ -401,6 +435,8 @@ static int mt7615_mac_fill_rx(struct mt7615_dev *dev, struct sk_buff *skb)
 					     status->chain_signal[i]);
 		}
 
+		mt7615_mac_fill_tm_rx(dev, rxd);
+
 		rxd += 6;
 		if ((u8 *)rxd - skb->data >= skb->len)
 			return -EINVAL;
diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/mac.h b/drivers/net/wireless/mediatek/mt76/mt7615/mac.h
index 81608ab656b8..169f4e17b5b4 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7615/mac.h
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/mac.h
@@ -100,11 +100,16 @@ enum rx_pkt_type {
 #define MT_RXV2_GROUP_ID		GENMASK(26, 21)
 #define MT_RXV2_LENGTH			GENMASK(20, 0)
 
+#define MT_RXV3_WB_RSSI			GENMASK(31, 24)
+#define MT_RXV3_IB_RSSI			GENMASK(23, 16)
+
 #define MT_RXV4_RCPI3			GENMASK(31, 24)
 #define MT_RXV4_RCPI2			GENMASK(23, 16)
 #define MT_RXV4_RCPI1			GENMASK(15, 8)
 #define MT_RXV4_RCPI0			GENMASK(7, 0)
 
+#define MT_RXV5_FOE			GENMASK(11, 0)
+
 #define MT_RXV6_NF3			GENMASK(31, 24)
 #define MT_RXV6_NF2			GENMASK(23, 16)
 #define MT_RXV6_NF1			GENMASK(15, 8)
diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/main.c b/drivers/net/wireless/mediatek/mt76/mt7615/main.c
index abb9853eaa8b..3abb33ddab7d 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7615/main.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/main.c
@@ -76,6 +76,8 @@ static void mt7615_stop(struct ieee80211_hw *hw)
 
 	mutex_lock(&dev->mt76.mutex);
 
+	mt76_testmode_reset(&dev->mt76, true);
+
 	clear_bit(MT76_STATE_RUNNING, &phy->mt76->state);
 	cancel_delayed_work_sync(&phy->scan_work);
 
@@ -137,6 +139,12 @@ static int mt7615_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(~dev->mphy.vif_mask) - 1;
 	if (mvif->idx >= MT7615_MAX_INTERFACES) {
 		ret = -ENOSPC;
@@ -197,6 +205,13 @@ static void mt7615_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;
+
 	mt7615_mcu_add_dev_info(dev, vif, false);
 
 	rcu_assign_pointer(dev->mt76.wcid[idx], NULL);
@@ -234,7 +249,7 @@ static void mt7615_init_dfs_state(struct mt7615_phy *phy)
 	phy->dfs_state = -1;
 }
 
-static int mt7615_set_channel(struct mt7615_phy *phy)
+int mt7615_set_channel(struct mt7615_phy *phy)
 {
 	struct mt7615_dev *dev = phy->dev;
 	bool ext_phy = phy != &dev->phy;
@@ -260,7 +275,7 @@ static int mt7615_set_channel(struct mt7615_phy *phy)
 	mt7615_mac_set_timing(phy);
 	ret = mt7615_dfs_init_radar_detector(phy);
 	mt7615_mac_cca_stats_reset(phy);
-	mt7615_mcu_set_sku_en(phy, true);
+	mt7615_mcu_set_sku_en(phy, !mt76_testmode_enabled(&dev->mt76));
 
 	mt7615_mac_reset_counters(dev);
 	phy->noise = 0;
@@ -271,8 +286,11 @@ static int mt7615_set_channel(struct mt7615_phy *phy)
 	mutex_unlock(&dev->mt76.mutex);
 
 	mt76_txq_schedule_all(phy->mt76);
-	ieee80211_queue_delayed_work(phy->mt76->hw, &phy->mac_work,
-				     MT7615_WATCHDOG_TIME);
+
+	if (!mt76_testmode_enabled(&dev->mt76))
+		ieee80211_queue_delayed_work(phy->mt76->hw, &phy->mac_work,
+					     MT7615_WATCHDOG_TIME);
+
 	return ret;
 }
 
@@ -369,6 +387,13 @@ static int mt7615_config(struct ieee80211_hw *hw, u32 changed)
 
 	if (changed & (IEEE80211_CONF_CHANGE_CHANNEL |
 		       IEEE80211_CONF_CHANGE_POWER)) {
+#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 = mt7615_set_channel(phy);
 		ieee80211_wake_queues(hw);
@@ -377,6 +402,8 @@ static int mt7615_config(struct ieee80211_hw *hw, u32 changed)
 	mutex_lock(&dev->mt76.mutex);
 
 	if (changed & IEEE80211_CONF_CHANGE_MONITOR) {
+		mt76_testmode_reset(&dev->mt76, true);
+
 		if (!(hw->conf.flags & IEEE80211_CONF_MONITOR))
 			phy->rxfilter |= MT_WF_RFCR_DROP_OTHER_UC;
 		else
@@ -419,10 +446,13 @@ static void mt7615_configure_filter(struct ieee80211_hw *hw,
 			MT_WF_RFCR1_DROP_CFACK;
 	u32 flags = 0;
 
+	mutex_lock(&dev->mt76.mutex);
+
 #define MT76_FILTER(_flag, _hw) do { \
 		flags |= *total_flags & FIF_##_flag;			\
 		phy->rxfilter &= ~(_hw);				\
-		phy->rxfilter |= !(flags & FIF_##_flag) * (_hw);	\
+		if (!mt76_testmode_enabled(&dev->mt76))			\
+			phy->rxfilter |= !(flags & FIF_##_flag) * (_hw);\
 	} while (0)
 
 	phy->rxfilter &= ~(MT_WF_RFCR_DROP_OTHER_BSS |
@@ -455,6 +485,8 @@ static void mt7615_configure_filter(struct ieee80211_hw *hw,
 		mt76_clear(dev, MT_WF_RFCR1(band), ctl_flags);
 	else
 		mt76_set(dev, MT_WF_RFCR1(band), ctl_flags);
+
+	mutex_unlock(&dev->mt76.mutex);
 }
 
 static void mt7615_bss_info_changed(struct ieee80211_hw *hw,
@@ -1069,6 +1101,8 @@ const struct ieee80211_ops mt7615_ops = {
 	.sched_scan_stop = mt7615_stop_sched_scan,
 	.remain_on_channel = mt7615_remain_on_channel,
 	.cancel_remain_on_channel = mt7615_cancel_remain_on_channel,
+	CFG80211_TESTMODE_CMD(mt76_testmode_cmd)
+	CFG80211_TESTMODE_DUMP(mt76_testmode_dump)
 #ifdef CONFIG_PM
 	.suspend = mt7615_suspend,
 	.resume = mt7615_resume,
diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/mcu.c b/drivers/net/wireless/mediatek/mt76/mt7615/mcu.c
index 16a74d92b540..1b46cccd93c5 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7615/mcu.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/mcu.c
@@ -2865,6 +2865,14 @@ int mt7615_mcu_set_chan_info(struct mt7615_phy *phy, int cmd)
 		.center_chan2 = ieee80211_frequency_to_channel(freq2),
 	};
 
+#ifdef CONFIG_NL80211_TESTMODE
+	if (dev->mt76.test.state == MT76_TM_STATE_TX_FRAMES &&
+	    dev->mt76.test.tx_antenna_mask) {
+		req.tx_streams = hweight8(dev->mt76.test.tx_antenna_mask);
+		req.rx_streams_mask = 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) &&
@@ -2876,7 +2884,10 @@ int mt7615_mcu_set_chan_info(struct mt7615_phy *phy, int cmd)
 	req.band_idx = phy != &dev->phy;
 	req.bw = mt7615_mcu_chan_bw(chandef);
 
-	mt7615_mcu_set_txpower_sku(phy, req.txpower_sku);
+	if (mt76_testmode_enabled(&dev->mt76))
+		memset(req.txpower_sku, 0x3f, 49);
+	else
+		mt7615_mcu_set_txpower_sku(phy, req.txpower_sku);
 
 	return __mt76_mcu_send_msg(&dev->mt76, cmd, &req, sizeof(req), true);
 }
@@ -2894,6 +2905,28 @@ int mt7615_mcu_get_temperature(struct mt7615_dev *dev, int index)
 				   sizeof(req), true);
 }
 
+int mt7615_mcu_set_test_param(struct mt7615_dev *dev, u8 param, bool test_mode,
+			      bool en)
+{
+	struct {
+		u8 test_mode_en;
+		u8 param_idx;
+		u8 _rsv[2];
+
+		u8 enable;
+		u8 _rsv2[3];
+
+		u8 pad[8];
+	} 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 mt7615_mcu_set_sku_en(struct mt7615_phy *phy, bool enable)
 {
 	struct mt7615_dev *dev = phy->dev;
diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/mcu.h b/drivers/net/wireless/mediatek/mt76/mt7615/mcu.h
index 8e2be150a556..e8c7aa5c35ce 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7615/mcu.h
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/mcu.h
@@ -268,6 +268,7 @@ enum {
 	MCU_EXT_CMD_GET_TEMP = 0x2c,
 	MCU_EXT_CMD_WTBL_UPDATE = 0x32,
 	MCU_EXT_CMD_SET_RDD_CTRL = 0x3a,
+	MCU_EXT_CMD_ATE_CTRL = 0x3d,
 	MCU_EXT_CMD_PROTECT_CTRL = 0x3e,
 	MCU_EXT_CMD_DBDC_CTRL = 0x45,
 	MCU_EXT_CMD_MAC_INIT_CTRL = 0x46,
@@ -289,6 +290,11 @@ enum {
 	MCU_UNI_CMD_HIF_CTRL = MCU_UNI_PREFIX | 0x07,
 };
 
+enum {
+	MCU_ATE_SET_FREQ_OFFSET = 0xa,
+	MCU_ATE_SET_TX_POWER_CONTROL = 0x15,
+};
+
 struct mt7615_mcu_uni_event {
 	u8 cid;
 	u8 pad[3];
diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/mt7615.h b/drivers/net/wireless/mediatek/mt76/mt7615/mt7615.h
index af86cf6925dd..1ae6ea5b4f16 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7615/mt7615.h
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/mt7615.h
@@ -169,6 +169,8 @@ struct mt7615_phy {
 	struct mt76_phy *mt76;
 	struct mt7615_dev *dev;
 
+	struct ieee80211_vif *monitor_vif;
+
 	u32 rxfilter;
 	u32 omac_mask;
 
@@ -283,6 +285,17 @@ struct mt7615_dev {
 
 	u32 debugfs_rf_wf;
 	u32 debugfs_rf_reg;
+
+#ifdef CONFIG_NL80211_TESTMODE
+	struct {
+		u32 *reg_backup;
+
+		s16 last_freq_offset;
+		u8 last_rcpi[4];
+		s8 last_ib_rssi;
+		s8 last_wb_rssi;
+	} test;
+#endif
 };
 
 enum tx_pkt_queue_idx {
@@ -377,6 +390,7 @@ extern const u32 mt7615e_reg_map[__MT_BASE_MAX];
 extern const u32 mt7663e_reg_map[__MT_BASE_MAX];
 extern struct pci_driver mt7615_pci_driver;
 extern struct platform_driver mt7622_wmac_driver;
+extern const struct mt76_testmode_ops mt7615_testmode_ops;
 
 #ifdef CONFIG_MT7622_WMAC
 int mt7622_wmac_init(struct mt7615_dev *dev);
@@ -488,6 +502,7 @@ void mt7615_init_txpower(struct mt7615_dev *dev,
 			 struct ieee80211_supported_band *sband);
 void mt7615_phy_init(struct mt7615_dev *dev);
 void mt7615_mac_init(struct mt7615_dev *dev);
+int mt7615_set_channel(struct mt7615_phy *phy);
 
 int mt7615_mcu_restart(struct mt76_dev *dev);
 void mt7615_update_channel(struct mt76_dev *mdev);
@@ -531,6 +546,7 @@ int mt7615_mcu_set_eeprom(struct mt7615_dev *dev);
 int mt7615_mcu_set_mac_enable(struct mt7615_dev *dev, int band, bool enable);
 int mt7615_mcu_set_rts_thresh(struct mt7615_phy *phy, u32 val);
 int mt7615_mcu_get_temperature(struct mt7615_dev *dev, int index);
+int mt7615_mcu_set_tx_power(struct mt7615_phy *phy);
 void mt7615_mcu_exit(struct mt7615_dev *dev);
 void mt7615_mcu_fill_msg(struct mt7615_dev *dev, struct sk_buff *skb,
 			 int cmd, int *wait_seq);
@@ -569,6 +585,8 @@ int mt7615_mcu_set_pulse_th(struct mt7615_dev *dev,
 			    const struct mt7615_dfs_pulse *pulse);
 int mt7615_mcu_set_radar_th(struct mt7615_dev *dev, int index,
 			    const struct mt7615_dfs_pattern *pattern);
+int mt7615_mcu_set_test_param(struct mt7615_dev *dev, u8 param, bool test_mode,
+			      bool en);
 int mt7615_mcu_set_sku_en(struct mt7615_phy *phy, bool enable);
 int mt7615_mcu_apply_rx_dcoc(struct mt7615_phy *phy);
 int mt7615_mcu_apply_tx_dpd(struct mt7615_phy *phy);
diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/regs.h b/drivers/net/wireless/mediatek/mt76/mt7615/regs.h
index 4743c12005a4..8d2c54918e68 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7615/regs.h
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/regs.h
@@ -212,6 +212,9 @@ enum mt7615_reg_base {
 #define MT_WF_PHY_RXTD2_BASE		MT_WF_PHY(0x2a00)
 #define MT_WF_PHY_RXTD2(_n)		(MT_WF_PHY_RXTD2_BASE + ((_n) << 2))
 
+#define MT_WF_PHY_RFINTF3_0(_n)		MT_WF_PHY(0x1100 + (_n) * 0x400)
+#define MT_WF_PHY_RFINTF3_0_ANT		GENMASK(7, 4)
+
 #define MT_WF_CFG_BASE			((dev)->reg_map[MT_CFG_BASE])
 #define MT_WF_CFG(ofs)			(MT_WF_CFG_BASE + (ofs))
 
@@ -255,6 +258,13 @@ enum mt7615_reg_base {
 #define MT_WF_ARB_BASE			((dev)->reg_map[MT_ARB_BASE])
 #define MT_WF_ARB(ofs)			(MT_WF_ARB_BASE + (ofs))
 
+#define MT_ARB_RQCR			MT_WF_ARB(0x070)
+#define MT_ARB_RQCR_RX_START		BIT(0)
+#define MT_ARB_RQCR_RXV_START		BIT(4)
+#define MT_ARB_RQCR_RXV_R_EN		BIT(7)
+#define MT_ARB_RQCR_RXV_T_EN		BIT(8)
+#define MT_ARB_RQCR_BAND_SHIFT		16
+
 #define MT_ARB_SCR			MT_WF_ARB(0x080)
 #define MT_ARB_SCR_TX0_DISABLE		BIT(8)
 #define MT_ARB_SCR_RX0_DISABLE		BIT(9)
@@ -550,4 +560,10 @@ enum mt7615_reg_base {
 #define MT_WL_RX_BUSY			BIT(30)
 #define MT_WL_TX_BUSY			BIT(31)
 
+#define MT_MCU_PTA_BASE			0x81060000
+#define MT_MCU_PTA(_n)			(MT_MCU_PTA_BASE + (_n))
+
+#define MT_ANT_SWITCH_CON(n)		MT_MCU_PTA(0x0c8)
+#define MT_ANT_SWITCH_CON_MODE(_n)	(GENMASK(4, 0) << (_n * 8))
+
 #endif
diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/testmode.c b/drivers/net/wireless/mediatek/mt76/mt7615/testmode.c
new file mode 100644
index 000000000000..55ebb1ec8c70
--- /dev/null
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/testmode.c
@@ -0,0 +1,363 @@
+// SPDX-License-Identifier: ISC
+/* Copyright (C) 2020 Felix Fietkau <nbd@nbd.name> */
+
+#include "mt7615.h"
+#include "eeprom.h"
+#include "mcu.h"
+
+enum {
+	TM_CHANGED_TXPOWER_CTRL,
+	TM_CHANGED_TXPOWER,
+	TM_CHANGED_FREQ_OFFSET,
+
+	/* must be last */
+	NUM_TM_CHANGED
+};
+
+
+static const u8 tm_change_map[] = {
+	[TM_CHANGED_TXPOWER_CTRL] = MT76_TM_ATTR_TX_POWER_CONTROL,
+	[TM_CHANGED_TXPOWER] = MT76_TM_ATTR_TX_POWER,
+	[TM_CHANGED_FREQ_OFFSET] = MT76_TM_ATTR_FREQ_OFFSET,
+};
+
+static const u32 reg_backup_list[] = {
+	MT_WF_PHY_RFINTF3_0(0),
+	MT_WF_PHY_RFINTF3_0(1),
+	MT_WF_PHY_RFINTF3_0(2),
+	MT_WF_PHY_RFINTF3_0(3),
+	MT_ANT_SWITCH_CON(2),
+	MT_ANT_SWITCH_CON(3),
+	MT_ANT_SWITCH_CON(4),
+	MT_ANT_SWITCH_CON(6),
+	MT_ANT_SWITCH_CON(7),
+	MT_ANT_SWITCH_CON(8),
+};
+
+static const struct {
+	u16 wf;
+	u16 reg;
+} rf_backup_list[] = {
+	{ 0, 0x48 },
+	{ 1, 0x48 },
+	{ 2, 0x48 },
+	{ 3, 0x48 },
+};
+
+static int
+mt7615_tm_set_tx_power(struct mt7615_phy *phy)
+{
+	struct mt7615_dev *dev = phy->dev;
+	struct mt76_phy *mphy = phy->mt76;
+	int i, ret, n_chains = hweight8(mphy->antenna_mask);
+	struct cfg80211_chan_def *chandef = &mphy->chandef;
+	int freq = chandef->center_freq1, len, target_chains;
+	u8 *data, *eep = (u8 *)dev->mt76.eeprom.data;
+	enum nl80211_band band = chandef->chan->band;
+	struct sk_buff *skb;
+	struct {
+		u8 center_chan;
+		u8 dbdc_idx;
+		u8 band;
+		u8 rsv;
+	} __packed req_hdr = {
+		.center_chan = ieee80211_frequency_to_channel(freq),
+		.band = band,
+		.dbdc_idx = phy != &dev->phy,
+	};
+	u8 *tx_power = NULL;
+
+	if (dev->mt76.test.state != MT76_TM_STATE_OFF)
+		tx_power = dev->mt76.test.tx_power;
+
+	len = sizeof(req_hdr) + MT7615_EE_MAX - MT_EE_NIC_CONF_0;
+	skb = mt76_mcu_msg_alloc(&dev->mt76, NULL, sizeof(req_hdr) + len);
+	if (!skb)
+		return -ENOMEM;
+
+	skb_put_data(skb, &req_hdr, sizeof(req_hdr));
+	data = skb_put_data(skb, eep + MT_EE_NIC_CONF_0, len);
+
+	target_chains = mt7615_ext_pa_enabled(dev, band) ? 1 : n_chains;
+	for (i = 0; i < target_chains; i++) {
+		int index;
+
+		ret = mt7615_eeprom_get_target_power_index(dev, chandef->chan, i);
+		if (ret < 0)
+			return -EINVAL;
+
+		index = ret - MT_EE_NIC_CONF_0;
+		if (tx_power && tx_power[i])
+			data[ret - MT_EE_NIC_CONF_0] = tx_power[i];
+	}
+
+	return __mt76_mcu_skb_send_msg(&dev->mt76, skb,
+				       MCU_EXT_CMD_SET_TX_POWER_CTRL, false);
+}
+
+static void
+mt7615_tm_reg_backup_restore(struct mt7615_dev *dev)
+{
+	u32 *b = dev->test.reg_backup;
+	int n_regs = ARRAY_SIZE(reg_backup_list);
+	int n_rf_regs = ARRAY_SIZE(rf_backup_list);
+	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], b[i]);
+
+		for (i = 0; i < n_rf_regs; i++)
+			mt7615_rf_wr(dev, rf_backup_list[i].wf,
+				     rf_backup_list[i].reg, b[n_regs + i]);
+		return;
+	}
+
+	if (b)
+		return;
+
+	b = devm_kzalloc(dev->mt76.dev, 4 * (n_regs + n_rf_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]);
+	for (i = 0; i < n_rf_regs; i++)
+		b[n_regs + i] = mt7615_rf_rr(dev, rf_backup_list[i].wf,
+					     rf_backup_list[i].reg);
+}
+
+
+static void
+mt7615_tm_init_phy(struct mt7615_dev *dev, struct mt7615_phy *phy)
+{
+	unsigned int total_flags = ~0;
+
+	if (!test_bit(MT76_STATE_RUNNING, &phy->mt76->state))
+		return;
+
+	mutex_unlock(&dev->mt76.mutex);
+	mt7615_set_channel(phy);
+	mt7615_ops.configure_filter(phy->mt76->hw, 0, &total_flags, 0);
+	mutex_lock(&dev->mt76.mutex);
+
+	mt7615_tm_reg_backup_restore(dev);
+}
+
+static void
+mt7615_tm_init(struct mt7615_dev *dev)
+{
+	mt7615_tm_init_phy(dev, &dev->phy);
+
+	if (dev->mt76.phy2)
+		mt7615_tm_init_phy(dev, dev->mt76.phy2->priv);
+}
+
+static void
+mt7615_tm_set_rx_enable(struct mt7615_dev *dev, bool en)
+{
+	u32 rqcr_mask = (MT_ARB_RQCR_RX_START |
+			 MT_ARB_RQCR_RXV_START |
+			 MT_ARB_RQCR_RXV_R_EN |
+			 MT_ARB_RQCR_RXV_T_EN) *
+			(BIT(0) | BIT(MT_ARB_RQCR_BAND_SHIFT));
+
+	if (en) {
+		mt76_clear(dev, MT_ARB_SCR,
+			   MT_ARB_SCR_RX0_DISABLE | MT_ARB_SCR_RX1_DISABLE);
+		mt76_set(dev, MT_ARB_RQCR, rqcr_mask);
+	} else {
+		mt76_set(dev, MT_ARB_SCR,
+			 MT_ARB_SCR_RX0_DISABLE | MT_ARB_SCR_RX1_DISABLE);
+		mt76_clear(dev, MT_ARB_RQCR, rqcr_mask);
+	}
+}
+
+static void
+mt7615_tm_set_tx_antenna(struct mt7615_dev *dev, bool en)
+{
+	struct mt76_testmode_data *td = &dev->mt76.test;
+	u8 mask = td->tx_antenna_mask;
+	int i;
+
+	if (!mask)
+		return;
+
+	if (!en)
+		mask = dev->phy.chainmask;
+
+	for (i = 0; i < 4; i++) {
+		mt76_rmw_field(dev, MT_WF_PHY_RFINTF3_0(i),
+			       MT_WF_PHY_RFINTF3_0_ANT,
+			       td->tx_antenna_mask & BIT(i) ? 0xa : 0);
+
+	}
+
+	/* 2.4 GHz band */
+	mt76_rmw_field(dev, MT_ANT_SWITCH_CON(3), MT_ANT_SWITCH_CON_MODE(0),
+		       (td->tx_antenna_mask & BIT(0)) ? 0x8 : 0x1b);
+	mt76_rmw_field(dev, MT_ANT_SWITCH_CON(4), MT_ANT_SWITCH_CON_MODE(2),
+		       (td->tx_antenna_mask & BIT(1)) ? 0xe : 0x1b);
+	mt76_rmw_field(dev, MT_ANT_SWITCH_CON(6), MT_ANT_SWITCH_CON_MODE(0),
+		       (td->tx_antenna_mask & BIT(2)) ? 0x0 : 0xf);
+	mt76_rmw_field(dev, MT_ANT_SWITCH_CON(7), MT_ANT_SWITCH_CON_MODE(2),
+		       (td->tx_antenna_mask & BIT(3)) ? 0x6 : 0xf);
+
+	/* 5 GHz band */
+	mt76_rmw_field(dev, MT_ANT_SWITCH_CON(4), MT_ANT_SWITCH_CON_MODE(1),
+		       (td->tx_antenna_mask & BIT(0)) ? 0xd : 0x1b);
+	mt76_rmw_field(dev, MT_ANT_SWITCH_CON(2), MT_ANT_SWITCH_CON_MODE(3),
+		       (td->tx_antenna_mask & BIT(1)) ? 0x13 : 0x1b);
+	mt76_rmw_field(dev, MT_ANT_SWITCH_CON(7), MT_ANT_SWITCH_CON_MODE(1),
+		       (td->tx_antenna_mask & BIT(2)) ? 0x5 : 0xf);
+	mt76_rmw_field(dev, MT_ANT_SWITCH_CON(8), MT_ANT_SWITCH_CON_MODE(3),
+		       (td->tx_antenna_mask & BIT(3)) ? 0xb : 0xf);
+
+	for (i = 0; i < 4; i++) {
+		u32 val;
+
+		val = mt7615_rf_rr(dev, i, 0x48);
+		val &= ~(0x3ff << 20);
+		if (td->tx_antenna_mask & BIT(i))
+			val |= 3 << 20;
+		else
+			val |= (2 << 28) | (2 << 26) | (8 << 20);
+		mt7615_rf_wr(dev, i, 0x48, val);
+	}
+}
+
+static void
+mt7615_tm_set_tx_frames(struct mt7615_dev *dev, bool en)
+{
+	struct ieee80211_tx_info *info;
+	struct sk_buff *skb = dev->mt76.test.tx_skb;
+
+	mt7615_mcu_set_chan_info(&dev->phy, MCU_EXT_CMD_SET_RX_PATH);
+	mt7615_tm_set_tx_antenna(dev, en);
+	mt7615_tm_set_rx_enable(dev, !en);
+	if (!en || !skb)
+		return;
+
+	info = IEEE80211_SKB_CB(skb);
+	info->control.vif = dev->phy.monitor_vif;
+}
+
+static void
+mt7615_tm_update_params(struct mt7615_dev *dev, u32 changed)
+{
+	struct mt76_testmode_data *td = &dev->mt76.test;
+	bool en = dev->mt76.test.state != MT76_TM_STATE_OFF;
+
+	if (changed & BIT(TM_CHANGED_TXPOWER_CTRL))
+		mt7615_mcu_set_test_param(dev, MCU_ATE_SET_TX_POWER_CONTROL,
+					  en, en && td->tx_power_control);
+	if (changed & BIT(TM_CHANGED_FREQ_OFFSET))
+		mt7615_mcu_set_test_param(dev, MCU_ATE_SET_FREQ_OFFSET,
+					  en, en ? td->freq_offset : 0);
+	if (changed & BIT(TM_CHANGED_TXPOWER))
+		mt7615_tm_set_tx_power(&dev->phy);
+}
+
+static int
+mt7615_tm_set_state(struct mt76_dev *mdev, enum mt76_testmode_state state)
+{
+	struct mt7615_dev *dev = container_of(mdev, struct mt7615_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)
+		mt7615_tm_set_tx_frames(dev, false);
+	else if (state == MT76_TM_STATE_TX_FRAMES)
+		mt7615_tm_set_rx_enable(dev, true);
+
+	if (state <= MT76_TM_STATE_IDLE)
+		mt7615_tm_init(dev);
+
+	if ((state == MT76_TM_STATE_IDLE &&
+	     prev_state == MT76_TM_STATE_OFF) ||
+	    (state == MT76_TM_STATE_OFF &&
+	     prev_state == MT76_TM_STATE_IDLE)) {
+		u32 changed = 0;
+		int i;
+
+		for (i = 0; i < ARRAY_SIZE(tm_change_map); i++) {
+			u16 cur = tm_change_map[i];
+
+			if (td->param_set[cur / 32] & BIT(cur % 32))
+				changed |= BIT(i);
+		}
+
+		mt7615_tm_update_params(dev, changed);
+	}
+
+	return 0;
+}
+
+static int
+mt7615_tm_set_params(struct mt76_dev *mdev, struct nlattr **tb,
+		     enum mt76_testmode_state new_state)
+{
+	struct mt7615_dev *dev = container_of(mdev, struct mt7615_dev, mt76);
+	struct mt76_testmode_data *td = &dev->mt76.test;
+	u32 changed = 0;
+	int i;
+
+	BUILD_BUG_ON(NUM_TM_CHANGED >= 32);
+
+	if (new_state == MT76_TM_STATE_OFF ||
+	    td->state == MT76_TM_STATE_OFF)
+		return 0;
+
+	if (td->tx_antenna_mask & ~dev->phy.chainmask)
+		return -EINVAL;
+
+	for (i = 0; i < ARRAY_SIZE(tm_change_map); i++) {
+		if (tb[tm_change_map[i]])
+			changed |= BIT(i);
+	}
+
+	mt7615_tm_update_params(dev, changed);
+
+	return 0;
+}
+
+static int
+mt7615_tm_dump_stats(struct mt76_dev *mdev, struct sk_buff *msg)
+{
+	struct mt7615_dev *dev = container_of(mdev, struct mt7615_dev, mt76);
+	void *rx, *rssi;
+	int i;
+
+	rx = nla_nest_start(msg, MT76_TM_STATS_ATTR_LAST_RX);
+	if (!rx)
+		return -ENOMEM;
+
+	if (nla_put_s32(msg, MT76_TM_RX_ATTR_FREQ_OFFSET, dev->test.last_freq_offset) ||
+	    nla_put_s32(msg, MT76_TM_RX_ATTR_IB_RSSI, dev->test.last_ib_rssi) ||
+	    nla_put_s32(msg, MT76_TM_RX_ATTR_WB_RSSI, dev->test.last_wb_rssi))
+		return -ENOMEM;
+
+	rssi = nla_nest_start(msg, MT76_TM_RX_ATTR_RCPI);
+	if (!rssi)
+		return -ENOMEM;
+
+	for (i = 0; i < ARRAY_SIZE(dev->test.last_rcpi); i++)
+		if (nla_put_u8(msg, i, dev->test.last_rcpi[i]))
+			return -ENOMEM;
+
+	nla_nest_end(msg, rssi);
+
+	nla_nest_end(msg, rx);
+
+	return 0;
+}
+
+const struct mt76_testmode_ops mt7615_testmode_ops = {
+	.set_state = mt7615_tm_set_state,
+	.set_params = mt7615_tm_set_params,
+	.dump_stats = mt7615_tm_dump_stats,
+};
-- 
2.24.0


^ permalink raw reply related	[flat|nested] 8+ messages in thread

end of thread, other threads:[~2020-07-10 12:44 UTC | newest]

Thread overview: 8+ messages (download: mbox.gz / follow: Atom feed)
-- links below jump to the message on this page --
2020-06-22 20:13 [PATCH 1/7] mt76: mt7615: schedule tx tasklet and sta poll on mac tx free Felix Fietkau
2020-06-22 20:13 ` [PATCH 2/7] mt76: mt7615: add support for accessing mapped registers via bus ops Felix Fietkau
2020-06-22 20:13 ` [PATCH 3/7] mt76: mt7615: add support for accessing RF registers via MCU Felix Fietkau
2020-06-22 20:13 ` [PATCH 4/7] mt76: mt7615: use full on-chip memory address for WF_PHY registers Felix Fietkau
2020-06-22 20:13 ` [PATCH 5/7] mt76: vif_mask to struct mt76_phy Felix Fietkau
2020-06-22 20:13 ` [PATCH 6/7] mt76: add API for testmode support Felix Fietkau
2020-06-22 20:13 ` [PATCH 7/7] mt76: mt7615: implement " Felix Fietkau
2020-07-10 12:44   ` [PATCH v2 " Felix Fietkau

This is a public inbox, see mirroring instructions
for how to clone and mirror all data and code used for this inbox;
as well as URLs for NNTP newsgroup(s).