mt76: add API for testmode support

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

@@ -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
 

drivers/net/wireless/mediatek/mt76/dma.c

@@ -370,6 +370,12 @@ mt76_dma_tx_queue_skb(struct mt76_dev *dev, enum mt76_txq_id qid,
 				 tx_info.buf[n].len, DMA_TO_DEVICE);
 
 free:
+#ifdef CONFIG_NL80211_TESTMODE
+	/* fix tx_done accounting on queue overflow */
+	if (tx_info.skb == dev->test.tx_skb)
+		dev->test.tx_done--;
+#endif
+
 	e.skb = tx_info.skb;
 	e.txwi = t;
 	dev->drv->tx_complete_skb(dev, qid, &e);

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;

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);

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)

drivers/net/wireless/mediatek/mt76/testmode.h

+/* 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

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,