blob: 23f4279b9a43f755893467c1fd60daea23ccb9a2 [file] [log] [blame]
developer92086052022-03-17 16:55:20 +08001diff --git a/package/kernel/mt76/patches/1006-mt76-mt7915-add-testmode.patch b/package/kernel/mt76/patches/1006-mt76-mt7915-add-testmode.patch
developer5f11e9e2022-03-10 15:03:47 +08002new file mode 100644
developer92086052022-03-17 16:55:20 +08003index 00000000..b5fa1b3f
developer5f11e9e2022-03-10 15:03:47 +08004--- /dev/null
developer92086052022-03-17 16:55:20 +08005+++ b/package/kernel/mt76/patches/1006-mt76-mt7915-add-testmode.patch
6@@ -0,0 +1,2048 @@
7+From 55fb2cbfd4666fb983dee99f72afac8e0f827007 Mon Sep 17 00:00:00 2001
developer5f11e9e2022-03-10 15:03:47 +08008+From: Shayne Chen <shayne.chen@mediatek.com>
9+Date: Wed, 19 Jan 2022 15:46:06 +0800
developer92086052022-03-17 16:55:20 +080010+Subject: [PATCH] mt76: mt7915: add testmode
developer5f11e9e2022-03-10 15:03:47 +080011+
12+---
13+ drivers/net/wireless/mediatek/mt76/mac80211.c | 18 +-
developer92086052022-03-17 16:55:20 +080014+ drivers/net/wireless/mediatek/mt76/mt76.h | 123 +++-
developer5f11e9e2022-03-10 15:03:47 +080015+ .../wireless/mediatek/mt76/mt76_connac_mcu.c | 5 +
16+ .../wireless/mediatek/mt76/mt76_connac_mcu.h | 1 +
17+ .../net/wireless/mediatek/mt76/mt7915/init.c | 2 +-
developer92086052022-03-17 16:55:20 +080018+ .../net/wireless/mediatek/mt76/mt7915/mac.c | 29 +-
19+ .../net/wireless/mediatek/mt76/mt7915/mcu.c | 12 +-
developer5f11e9e2022-03-10 15:03:47 +080020+ .../net/wireless/mediatek/mt76/mt7915/mcu.h | 5 +
21+ .../net/wireless/mediatek/mt76/mt7915/mmio.c | 2 +
22+ .../wireless/mediatek/mt76/mt7915/mt7915.h | 2 +-
23+ .../net/wireless/mediatek/mt76/mt7915/regs.h | 16 +-
developer92086052022-03-17 16:55:20 +080024+ .../wireless/mediatek/mt76/mt7915/testmode.c | 529 +++++++++++++++---
developer5f11e9e2022-03-10 15:03:47 +080025+ .../wireless/mediatek/mt76/mt7915/testmode.h | 38 ++
developer92086052022-03-17 16:55:20 +080026+ drivers/net/wireless/mediatek/mt76/testmode.c | 276 +++++++--
developer5f11e9e2022-03-10 15:03:47 +080027+ drivers/net/wireless/mediatek/mt76/testmode.h | 71 +++
28+ .../net/wireless/mediatek/mt76/tools/fields.c | 76 +++
29+ drivers/net/wireless/mediatek/mt76/tx.c | 3 +-
developer92086052022-03-17 16:55:20 +080030+ 17 files changed, 1061 insertions(+), 147 deletions(-)
developer5f11e9e2022-03-10 15:03:47 +080031+
32+diff --git a/mac80211.c b/mac80211.c
developer92086052022-03-17 16:55:20 +080033+index 5b53d008..fcba2894 100644
developer5f11e9e2022-03-10 15:03:47 +080034+--- a/mac80211.c
35++++ b/mac80211.c
36+@@ -45,6 +45,9 @@ static const struct ieee80211_channel mt76_channels_2ghz[] = {
37+ };
38+
39+ static const struct ieee80211_channel mt76_channels_5ghz[] = {
40++ CHAN5G(12, 5060),
41++ CHAN5G(16, 5080),
42++
43+ CHAN5G(36, 5180),
44+ CHAN5G(40, 5200),
45+ CHAN5G(44, 5220),
46+@@ -55,6 +58,13 @@ static const struct ieee80211_channel mt76_channels_5ghz[] = {
47+ CHAN5G(60, 5300),
48+ CHAN5G(64, 5320),
49+
50++ CHAN5G(68, 5340),
51++ CHAN5G(80, 5400),
52++ CHAN5G(84, 5420),
53++ CHAN5G(88, 5440),
54++ CHAN5G(92, 5460),
55++ CHAN5G(96, 5480),
56++
57+ CHAN5G(100, 5500),
58+ CHAN5G(104, 5520),
59+ CHAN5G(108, 5540),
60+@@ -75,6 +85,11 @@ static const struct ieee80211_channel mt76_channels_5ghz[] = {
61+ CHAN5G(165, 5825),
62+ CHAN5G(169, 5845),
63+ CHAN5G(173, 5865),
64++
65++ CHAN5G(184, 4920),
66++ CHAN5G(188, 4940),
67++ CHAN5G(192, 4960),
68++ CHAN5G(196, 4980),
69+ };
70+
71+ static const struct ieee80211_channel mt76_channels_6ghz[] = {
72+@@ -737,7 +752,8 @@ void mt76_rx(struct mt76_dev *dev, enum mt76_rxq_id q, struct sk_buff *skb)
73+ }
74+
75+ #ifdef CONFIG_NL80211_TESTMODE
76+- if (phy->test.state == MT76_TM_STATE_RX_FRAMES) {
77++ if (!(phy->test.flag & MT_TM_FW_RX_COUNT) &&
78++ phy->test.state == MT76_TM_STATE_RX_FRAMES) {
79+ phy->test.rx_stats.packets[q]++;
80+ if (status->flag & RX_FLAG_FAILED_FCS_CRC)
81+ phy->test.rx_stats.fcs_error[q]++;
82+diff --git a/mt76.h b/mt76.h
developer92086052022-03-17 16:55:20 +080083+index 882fb5d2..2df88277 100644
developer5f11e9e2022-03-10 15:03:47 +080084+--- a/mt76.h
85++++ b/mt76.h
86+@@ -581,6 +581,25 @@ struct mt76_testmode_ops {
87+ int (*set_params)(struct mt76_phy *phy, struct nlattr **tb,
88+ enum mt76_testmode_state new_state);
89+ int (*dump_stats)(struct mt76_phy *phy, struct sk_buff *msg);
90++ int (*set_eeprom)(struct mt76_phy *phy, u32 offset, u8 *val, u8 action);
91++};
92++
93++#define MT_TM_FW_RX_COUNT BIT(0)
94++
95++struct mt76_testmode_sta_data {
96++ u16 tx_mpdu_len;
97++ u8 tx_rate_idx;
98++ u8 tx_rate_nss;
99++ u8 tx_rate_ldpc;
100++
101++ u8 aid;
102++ u8 ru_alloc;
103++ u8 ru_idx;
104++};
105++
106++struct mt76_testmode_sta {
107++ struct sk_buff *tx_skb;
108++ struct mt76_testmode_sta_data sd;
109+ };
110+
111+ struct mt76_testmode_data {
112+@@ -590,13 +609,9 @@ struct mt76_testmode_data {
113+ struct sk_buff *tx_skb;
114+
115+ u32 tx_count;
116+- u16 tx_mpdu_len;
117+
118+ u8 tx_rate_mode;
119+- u8 tx_rate_idx;
120+- u8 tx_rate_nss;
121+ u8 tx_rate_sgi;
122+- u8 tx_rate_ldpc;
123+ u8 tx_rate_stbc;
124+ u8 tx_ltf;
125+
126+@@ -614,6 +629,35 @@ struct mt76_testmode_data {
127+
128+ u8 addr[3][ETH_ALEN];
129+
130++ u8 flag;
131++
132++ struct {
133++ u8 type;
134++ u8 enable;
135++ } cfg;
136++
137++ u8 off_ch_scan_ch;
138++ u8 off_ch_scan_center_ch;
139++ u8 off_ch_scan_bw;
140++ u8 off_ch_scan_path;
141++
142++ struct mt76_wcid *tm_wcid[MT76_TM_MAX_STA_NUM + 1];
143++ u8 cur_aid;
144++ u16 tm_sta_mask;
145++ union {
146++ struct mt76_testmode_sta_data sd;
147++ struct {
148++ u16 tx_mpdu_len;
149++ u8 tx_rate_idx;
150++ u8 tx_rate_nss;
151++ u8 tx_rate_ldpc;
152++
153++ u8 aid;
154++ u8 ru_alloc;
155++ u8 ru_idx;
156++ };
157++ };
158++
159+ u32 tx_pending;
160+ u32 tx_queued;
161+ u16 tx_queued_limit;
162+@@ -621,6 +665,7 @@ struct mt76_testmode_data {
163+ struct {
164+ u64 packets[__MT_RXQ_MAX];
165+ u64 fcs_error[__MT_RXQ_MAX];
166++ u64 len_mismatch;
167+ } rx_stats;
168+ };
169+
170+@@ -1091,22 +1136,69 @@ static inline bool mt76_testmode_enabled(struct mt76_phy *phy)
171+ #endif
172+ }
173+
174++#ifdef CONFIG_NL80211_TESTMODE
175++static inline bool
176++mt76_testmode_has_sta(struct mt76_phy *phy)
177++{
178++ return phy->test.tm_sta_mask != 0;
179++}
180++
181++static inline struct mt76_testmode_sta *
182++mt76_testmode_aid_get_sta(struct mt76_phy *phy, u8 aid)
183++{
184++ struct mt76_wcid *wcid = phy->test.tm_wcid[aid];
185++
186++ if (!wcid || !aid)
187++ return NULL;
188++
189++ return (struct mt76_testmode_sta *)((u8 *)wcid + phy->hw->sta_data_size);
190++}
191++
192++#define mt76_testmode_for_each_sta(phy, aid, tm_sta) \
193++ for (aid = 1, tm_sta = mt76_testmode_aid_get_sta(phy, 1); \
194++ aid <= hweight16(phy->test.tm_sta_mask); \
195++ aid = phy->test.tm_sta_mask >> aid ? \
196++ ffs(phy->test.tm_sta_mask >> aid) + aid : \
197++ aid + 1, \
198++ tm_sta = mt76_testmode_aid_get_sta(phy, aid))
199++
200++static inline bool
201++__mt76_testmode_check_skb(struct mt76_phy *phy, struct sk_buff *skb)
202++{
203++ struct mt76_testmode_sta *tm_sta;
204++ int i;
205++
206++ if (!mt76_testmode_has_sta(phy))
207++ return false;
208++
209++ mt76_testmode_for_each_sta(phy, i, tm_sta) {
210++ if (tm_sta->tx_skb == skb)
211++ return true;
212++ }
213++
214++ return false;
215++}
216++
217+ static inline bool mt76_is_testmode_skb(struct mt76_dev *dev,
218+ struct sk_buff *skb,
219+ struct ieee80211_hw **hw)
220+ {
221+-#ifdef CONFIG_NL80211_TESTMODE
222+- if (skb == dev->phy.test.tx_skb)
223+- *hw = dev->phy.hw;
224+- else if (dev->phy2 && skb == dev->phy2->test.tx_skb)
225+- *hw = dev->phy2->hw;
226+- else
227+- return false;
228+- return true;
229+-#else
230++ struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
231++ struct mt76_phy *phy = &dev->phy;
232++
233++ if ((info->hw_queue & MT_TX_HW_QUEUE_EXT_PHY) && dev->phy2)
234++ phy = dev->phy2;
235++
236++ if (mt76_testmode_enabled(phy) &&
237++ (skb == phy->test.tx_skb ||
238++ __mt76_testmode_check_skb(phy, skb))) {
239++ *hw = phy->hw;
240++ return true;
241++ }
242++
243+ return false;
244+-#endif
245+ }
246++#endif
247+
248+ void mt76_rx(struct mt76_dev *dev, enum mt76_rxq_id q, struct sk_buff *skb);
249+ void mt76_tx(struct mt76_phy *dev, struct ieee80211_sta *sta,
250+@@ -1198,7 +1290,7 @@ int mt76_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
251+ int mt76_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *skb,
252+ struct netlink_callback *cb, void *data, int len);
253+ int mt76_testmode_set_state(struct mt76_phy *phy, enum mt76_testmode_state state);
254+-int mt76_testmode_alloc_skb(struct mt76_phy *phy, u32 len);
255++int mt76_testmode_init_skb(struct mt76_phy *phy, u32 len, u8 aid, struct sk_buff **skb);
256+
257+ static inline void mt76_testmode_reset(struct mt76_phy *phy, bool disable)
258+ {
259+@@ -1212,7 +1304,6 @@ static inline void mt76_testmode_reset(struct mt76_phy *phy, bool disable)
260+ #endif
261+ }
262+
263+-
264+ /* internal */
265+ static inline struct ieee80211_hw *
266+ mt76_tx_status_get_hw(struct mt76_dev *dev, struct sk_buff *skb)
267+diff --git a/mt76_connac_mcu.c b/mt76_connac_mcu.c
developer92086052022-03-17 16:55:20 +0800268+index eeb73d14..0725e5dd 100644
developer5f11e9e2022-03-10 15:03:47 +0800269+--- a/mt76_connac_mcu.c
270++++ b/mt76_connac_mcu.c
271+@@ -389,6 +389,7 @@ void mt76_connac_mcu_sta_basic_tlv(struct sk_buff *skb,
272+ switch (vif->type) {
273+ case NL80211_IFTYPE_MESH_POINT:
274+ case NL80211_IFTYPE_AP:
275++ case NL80211_IFTYPE_MONITOR:
276+ if (vif->p2p)
277+ conn_type = CONNECTION_P2P_GC;
278+ else
279+@@ -577,6 +578,10 @@ void mt76_connac_mcu_wtbl_generic_tlv(struct mt76_dev *dev,
280+ wtbl_tlv, sta_wtbl);
281+ spe = (struct wtbl_spe *)tlv;
282+ spe->spe_idx = 24;
283++
284++ /* check */
285++ if (vif->type == NL80211_IFTYPE_MONITOR)
286++ rx->rca1 = 0;
287+ }
288+ EXPORT_SYMBOL_GPL(mt76_connac_mcu_wtbl_generic_tlv);
289+
290+diff --git a/mt76_connac_mcu.h b/mt76_connac_mcu.h
developer92086052022-03-17 16:55:20 +0800291+index c3c93338..54419864 100644
developer5f11e9e2022-03-10 15:03:47 +0800292+--- a/mt76_connac_mcu.h
293++++ b/mt76_connac_mcu.h
developer92086052022-03-17 16:55:20 +0800294+@@ -980,6 +980,7 @@ enum {
developer5f11e9e2022-03-10 15:03:47 +0800295+ MCU_EXT_CMD_OFFCH_SCAN_CTRL = 0x9a,
296+ MCU_EXT_CMD_SET_RDD_TH = 0x9d,
297+ MCU_EXT_CMD_MURU_CTRL = 0x9f,
298++ MCU_EXT_CMD_RX_STAT = 0xa4,
299+ MCU_EXT_CMD_SET_SPR = 0xa8,
300+ MCU_EXT_CMD_GROUP_PRE_CAL_INFO = 0xab,
301+ MCU_EXT_CMD_DPD_PRE_CAL_INFO = 0xac,
302+diff --git a/mt7915/init.c b/mt7915/init.c
developer92086052022-03-17 16:55:20 +0800303+index f57a3d18..db7f029f 100644
developer5f11e9e2022-03-10 15:03:47 +0800304+--- a/mt7915/init.c
305++++ b/mt7915/init.c
developer92086052022-03-17 16:55:20 +0800306+@@ -558,7 +558,7 @@ static void mt7915_init_work(struct work_struct *work)
developer5f11e9e2022-03-10 15:03:47 +0800307+ struct mt7915_dev *dev = container_of(work, struct mt7915_dev,
308+ init_work);
309+
310+- mt7915_mcu_set_eeprom(dev);
311++ mt7915_mcu_set_eeprom(dev, dev->flash_mode);
312+ mt7915_mac_init(dev);
313+ mt7915_init_txpower(dev, &dev->mphy.sband_2g.sband);
314+ mt7915_init_txpower(dev, &dev->mphy.sband_5g.sband);
315+diff --git a/mt7915/mac.c b/mt7915/mac.c
developer92086052022-03-17 16:55:20 +0800316+index 47d5a993..48139ccf 100644
developer5f11e9e2022-03-10 15:03:47 +0800317+--- a/mt7915/mac.c
318++++ b/mt7915/mac.c
developer92086052022-03-17 16:55:20 +0800319+@@ -899,16 +899,28 @@ mt7915_mac_write_txwi_tm(struct mt7915_phy *phy, __le32 *txwi,
developer5f11e9e2022-03-10 15:03:47 +0800320+ {
321+ #ifdef CONFIG_NL80211_TESTMODE
322+ struct mt76_testmode_data *td = &phy->mt76->test;
323++ struct mt76_testmode_sta_data *sd = &td->sd;
324+ const struct ieee80211_rate *r;
325+- u8 bw, mode, nss = td->tx_rate_nss;
326+- u8 rate_idx = td->tx_rate_idx;
327++ u8 bw, mode, nss, rate_idx;
328+ u16 rateval = 0;
329+ u32 val;
330+ bool cck = false;
331+ int band;
332+
333+- if (skb != phy->mt76->test.tx_skb)
334+- return;
335++ if (mt76_testmode_has_sta(phy->mt76)) {
336++ struct mt76_testmode_sta *tm_sta;
337++ int i;
338++
339++ mt76_testmode_for_each_sta(phy->mt76, i, tm_sta) {
340++ if (tm_sta->tx_skb == skb) {
341++ sd = &tm_sta->sd;
342++ break;
343++ }
344++ }
345++ }
346++
347++ nss = sd->tx_rate_nss;
348++ rate_idx = sd->tx_rate_idx;
349+
350+ switch (td->tx_rate_mode) {
351+ case MT76_TM_TX_MODE_HT:
developer92086052022-03-17 16:55:20 +0800352+@@ -998,9 +1010,10 @@ mt7915_mac_write_txwi_tm(struct mt7915_phy *phy, __le32 *txwi,
developer5f11e9e2022-03-10 15:03:47 +0800353+ if (mode >= MT_PHY_TYPE_HE_SU)
354+ val |= FIELD_PREP(MT_TXD6_HELTF, td->tx_ltf);
355+
356+- if (td->tx_rate_ldpc || (bw > 0 && mode >= MT_PHY_TYPE_HE_SU))
357++ if (sd->tx_rate_ldpc || (bw > 0 && mode >= MT_PHY_TYPE_HE_SU))
358+ val |= MT_TXD6_LDPC;
359+
360++ txwi[1] &= ~cpu_to_le32(MT_TXD1_VTA);
361+ txwi[3] &= ~cpu_to_le32(MT_TXD3_SN_VALID);
362+ txwi[6] |= cpu_to_le32(val);
363+ txwi[7] |= cpu_to_le32(FIELD_PREP(MT_TXD7_SPE_IDX,
developer92086052022-03-17 16:55:20 +0800364+@@ -1355,6 +1368,9 @@ mt7915_tx_check_aggr(struct ieee80211_sta *sta, __le32 *txwi)
365+ if (!sta || !(sta->ht_cap.ht_supported || sta->he_cap.has_he))
366+ return;
367+
368++ if (le32_get_bits(txwi[2], MT_TXD2_FIX_RATE))
369++ return;
370++
371+ tid = le32_get_bits(txwi[1], MT_TXD1_TID);
372+ if (tid >= 6) /* skip VO queue */
373+ return;
374+@@ -1462,6 +1478,9 @@ mt7915_mac_tx_free(struct mt7915_dev *dev, void *data, int len)
developer5f11e9e2022-03-10 15:03:47 +0800375+ continue;
376+
377+ msta = container_of(wcid, struct mt7915_sta, wcid);
378++ if (mt76_testmode_enabled(msta->vif->phy->mt76))
379++ continue;
380++
381+ spin_lock_bh(&dev->sta_poll_lock);
382+ if (list_empty(&msta->poll_list))
383+ list_add_tail(&msta->poll_list, &dev->sta_poll_list);
384+diff --git a/mt7915/mcu.c b/mt7915/mcu.c
developer92086052022-03-17 16:55:20 +0800385+index 2aba342c..5c3309be 100644
developer5f11e9e2022-03-10 15:03:47 +0800386+--- a/mt7915/mcu.c
387++++ b/mt7915/mcu.c
developer92086052022-03-17 16:55:20 +0800388+@@ -285,7 +285,6 @@ mt7915_mcu_send_message(struct mt76_dev *mdev, struct sk_buff *skb,
developer5f11e9e2022-03-10 15:03:47 +0800389+ if (mcu_txd->ext_cid) {
390+ mcu_txd->ext_cid_ack = 1;
391+
392+- /* do not use Q_SET for efuse */
393+ if (cmd & __MCU_CMD_FIELD_QUERY)
394+ mcu_txd->set_query = MCU_Q_QUERY;
395+ else
developer92086052022-03-17 16:55:20 +0800396+@@ -2822,14 +2821,9 @@ int mt7915_mcu_set_chan_info(struct mt7915_phy *phy, int cmd)
developer5f11e9e2022-03-10 15:03:47 +0800397+
398+ #ifdef CONFIG_NL80211_TESTMODE
399+ if (phy->mt76->test.tx_antenna_mask &&
400+- (phy->mt76->test.state == MT76_TM_STATE_TX_FRAMES ||
401+- phy->mt76->test.state == MT76_TM_STATE_RX_FRAMES ||
402+- phy->mt76->test.state == MT76_TM_STATE_TX_CONT)) {
403++ mt76_testmode_enabled(phy->mt76)) {
404+ req.tx_streams_num = fls(phy->mt76->test.tx_antenna_mask);
405+ req.rx_streams = phy->mt76->test.tx_antenna_mask;
406+-
developer92086052022-03-17 16:55:20 +0800407+- if (phy != &dev->phy)
developer5f11e9e2022-03-10 15:03:47 +0800408+- req.rx_streams >>= dev->chainshift;
409+ }
410+ #endif
411+
developer92086052022-03-17 16:55:20 +0800412+@@ -2897,14 +2891,14 @@ static int mt7915_mcu_set_eeprom_flash(struct mt7915_dev *dev)
developer5f11e9e2022-03-10 15:03:47 +0800413+ return 0;
414+ }
415+
416+-int mt7915_mcu_set_eeprom(struct mt7915_dev *dev)
417++int mt7915_mcu_set_eeprom(struct mt7915_dev *dev, bool flash_mode)
418+ {
419+ struct mt7915_mcu_eeprom req = {
420+ .buffer_mode = EE_MODE_EFUSE,
421+ .format = EE_FORMAT_WHOLE,
422+ };
423+
424+- if (dev->flash_mode)
425++ if (flash_mode)
426+ return mt7915_mcu_set_eeprom_flash(dev);
427+
428+ return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(EFUSE_BUFFER_MODE),
429+diff --git a/mt7915/mcu.h b/mt7915/mcu.h
developer92086052022-03-17 16:55:20 +0800430+index 960072a4..a5d869ca 100644
developer5f11e9e2022-03-10 15:03:47 +0800431+--- a/mt7915/mcu.h
432++++ b/mt7915/mcu.h
433+@@ -27,7 +27,12 @@ struct mt7915_mcu_txd {
434+
435+ enum {
436+ MCU_ATE_SET_TRX = 0x1,
437++ MCU_ATE_SET_TSSI = 0x5,
438++ MCU_ATE_SET_DPD = 0x6,
439++ MCU_ATE_SET_RATE_POWER_OFFSET = 0x7,
440++ MCU_ATE_SET_THERMAL_COMP = 0x8,
441+ MCU_ATE_SET_FREQ_OFFSET = 0xa,
442++ MCU_ATE_SET_PHY_COUNT = 0x11,
443+ MCU_ATE_SET_SLOT_TIME = 0x13,
444+ MCU_ATE_CLEAN_TXQUEUE = 0x1c,
445+ };
446+diff --git a/mt7915/mmio.c b/mt7915/mmio.c
developer92086052022-03-17 16:55:20 +0800447+index 5062e0d8..2466907e 100644
developer5f11e9e2022-03-10 15:03:47 +0800448+--- a/mt7915/mmio.c
449++++ b/mt7915/mmio.c
450+@@ -53,6 +53,7 @@ static const u32 mt7986_reg[] = {
451+ };
452+
453+ static const u32 mt7915_offs[] = {
454++ [TMAC_TCR2] = 0x05c,
455+ [TMAC_CDTR] = 0x090,
456+ [TMAC_ODTR] = 0x094,
457+ [TMAC_ATCR] = 0x098,
developer92086052022-03-17 16:55:20 +0800458+@@ -126,6 +127,7 @@ static const u32 mt7915_offs[] = {
developer5f11e9e2022-03-10 15:03:47 +0800459+ };
460+
461+ static const u32 mt7916_offs[] = {
462++ [TMAC_TCR2] = 0x004,
463+ [TMAC_CDTR] = 0x0c8,
464+ [TMAC_ODTR] = 0x0cc,
465+ [TMAC_ATCR] = 0x00c,
466+diff --git a/mt7915/mt7915.h b/mt7915/mt7915.h
developer92086052022-03-17 16:55:20 +0800467+index 6efa0a2e..7ad550c2 100644
developer5f11e9e2022-03-10 15:03:47 +0800468+--- a/mt7915/mt7915.h
469++++ b/mt7915/mt7915.h
developer92086052022-03-17 16:55:20 +0800470+@@ -478,7 +478,7 @@ int mt7915_mcu_set_fixed_rate_ctrl(struct mt7915_dev *dev,
developer5f11e9e2022-03-10 15:03:47 +0800471+ struct ieee80211_vif *vif,
472+ struct ieee80211_sta *sta,
473+ void *data, u32 field);
474+-int mt7915_mcu_set_eeprom(struct mt7915_dev *dev);
475++int mt7915_mcu_set_eeprom(struct mt7915_dev *dev, bool flash_mode);
476+ int mt7915_mcu_get_eeprom(struct mt7915_dev *dev, u32 offset);
477+ int mt7915_mcu_get_eeprom_free_block(struct mt7915_dev *dev, u8 *block_num);
478+ int mt7915_mcu_set_mac(struct mt7915_dev *dev, int band, bool enable,
479+diff --git a/mt7915/regs.h b/mt7915/regs.h
developer92086052022-03-17 16:55:20 +0800480+index e5f93c40..999dd7fc 100644
developer5f11e9e2022-03-10 15:03:47 +0800481+--- a/mt7915/regs.h
482++++ b/mt7915/regs.h
483+@@ -34,6 +34,7 @@ enum reg_rev {
484+ };
485+
486+ enum offs_rev {
487++ TMAC_TCR2,
488+ TMAC_CDTR,
489+ TMAC_ODTR,
490+ TMAC_ATCR,
developer92086052022-03-17 16:55:20 +0800491+@@ -172,6 +173,12 @@ enum offs_rev {
developer5f11e9e2022-03-10 15:03:47 +0800492+ #define MT_MDP_TO_HIF 0
493+ #define MT_MDP_TO_WM 1
494+
495++#define MT_MDP_TOP_DBG_WDT_CTRL MT_MDP(0x0d0)
496++#define MT_MDP_TOP_DBG_WDT_CTRL_TDP_DIS_BLK BIT(7)
497++
498++#define MT_MDP_TOP_DBG_CTRL MT_MDP(0x0dc)
499++#define MT_MDP_TOP_DBG_CTRL_ENQ_MODE BIT(30)
500++
501+ /* TMAC: band 0(0x820e4000), band 1(0x820f4000) */
502+ #define MT_WF_TMAC_BASE(_band) ((_band) ? 0x820f4000 : 0x820e4000)
503+ #define MT_WF_TMAC(_band, ofs) (MT_WF_TMAC_BASE(_band) + (ofs))
developer92086052022-03-17 16:55:20 +0800504+@@ -180,6 +187,9 @@ enum offs_rev {
developer5f11e9e2022-03-10 15:03:47 +0800505+ #define MT_TMAC_TCR0_TX_BLINK GENMASK(7, 6)
506+ #define MT_TMAC_TCR0_TBTT_STOP_CTRL BIT(25)
507+
508++#define MT_TMAC_TCR2(_band) MT_WF_TMAC(_band, __OFFS(TMAC_TCR2))
509++#define MT_TMAC_TCR2_SCH_DET_DIS BIT(19)
510++
511+ #define MT_TMAC_CDTR(_band) MT_WF_TMAC(_band, __OFFS(TMAC_CDTR))
512+ #define MT_TMAC_ODTR(_band) MT_WF_TMAC(_band, __OFFS(TMAC_ODTR))
513+ #define MT_TIMEOUT_VAL_PLCP GENMASK(15, 0)
developer92086052022-03-17 16:55:20 +0800514+@@ -451,8 +461,10 @@ enum offs_rev {
developer5f11e9e2022-03-10 15:03:47 +0800515+ #define MT_AGG_PCR0_VHT_PROT BIT(13)
516+ #define MT_AGG_PCR0_PTA_WIN_DIS BIT(15)
517+
518+-#define MT_AGG_PCR1_RTS0_NUM_THRES GENMASK(31, 23)
519+-#define MT_AGG_PCR1_RTS0_LEN_THRES GENMASK(19, 0)
520++#define MT_AGG_PCR1_RTS0_NUM_THRES GENMASK(31, 23)
521++#define MT_AGG_PCR1_RTS0_LEN_THRES GENMASK(19, 0)
522++#define MT_AGG_PCR1_RTS0_NUM_THRES_MT7916 GENMASK(29, 24)
523++#define MT_AGG_PCR1_RTS0_LEN_THRES_MT7916 GENMASK(22, 0)
524+
525+ #define MT_AGG_ACR0(_band) MT_WF_AGG(_band, __OFFS(AGG_ACR0))
526+ #define MT_AGG_ACR_CFEND_RATE GENMASK(13, 0)
527+diff --git a/mt7915/testmode.c b/mt7915/testmode.c
developer92086052022-03-17 16:55:20 +0800528+index 20f63644..a277c16d 100644
developer5f11e9e2022-03-10 15:03:47 +0800529+--- a/mt7915/testmode.c
530++++ b/mt7915/testmode.c
531+@@ -9,6 +9,9 @@
532+ enum {
533+ TM_CHANGED_TXPOWER,
534+ TM_CHANGED_FREQ_OFFSET,
535++ TM_CHANGED_CFG,
536++ TM_CHANGED_OFF_CH_SCAN_CH,
537++ TM_CHANGED_AID,
538+
539+ /* must be last */
540+ NUM_TM_CHANGED
541+@@ -17,6 +20,9 @@ enum {
542+ static const u8 tm_change_map[] = {
543+ [TM_CHANGED_TXPOWER] = MT76_TM_ATTR_TX_POWER,
544+ [TM_CHANGED_FREQ_OFFSET] = MT76_TM_ATTR_FREQ_OFFSET,
545++ [TM_CHANGED_CFG] = MT76_TM_ATTR_CFG,
546++ [TM_CHANGED_OFF_CH_SCAN_CH] = MT76_TM_ATTR_OFF_CH_SCAN_CH,
547++ [TM_CHANGED_AID] = MT76_TM_ATTR_AID,
548+ };
549+
550+ struct reg_band {
551+@@ -30,10 +36,29 @@ struct reg_band {
552+ { _list.band[0] = MT_##_reg(0, _idx); \
553+ _list.band[1] = MT_##_reg(1, _idx); }
554+
555+-#define TM_REG_MAX_ID 17
556++#define TM_REG_MAX_ID 20
557+ static struct reg_band reg_backup_list[TM_REG_MAX_ID];
558+
559+
560++static u8 mt7915_tm_chan_bw(enum nl80211_chan_width width)
561++{
562++ static const u8 width_to_bw[] = {
563++ [NL80211_CHAN_WIDTH_40] = TM_CBW_40MHZ,
564++ [NL80211_CHAN_WIDTH_80] = TM_CBW_80MHZ,
565++ [NL80211_CHAN_WIDTH_80P80] = TM_CBW_8080MHZ,
566++ [NL80211_CHAN_WIDTH_160] = TM_CBW_160MHZ,
567++ [NL80211_CHAN_WIDTH_5] = TM_CBW_5MHZ,
568++ [NL80211_CHAN_WIDTH_10] = TM_CBW_10MHZ,
569++ [NL80211_CHAN_WIDTH_20] = TM_CBW_20MHZ,
570++ [NL80211_CHAN_WIDTH_20_NOHT] = TM_CBW_20MHZ,
571++ };
572++
573++ if (width >= ARRAY_SIZE(width_to_bw))
574++ return 0;
575++
576++ return width_to_bw[width];
577++}
578++
579+ static int
580+ mt7915_tm_set_tx_power(struct mt7915_phy *phy)
581+ {
582+@@ -119,15 +144,45 @@ mt7915_tm_set_trx(struct mt7915_phy *phy, int type, bool en)
583+ }
584+
585+ static int
586+-mt7915_tm_clean_hwq(struct mt7915_phy *phy, u8 wcid)
587++mt7915_tm_clean_hwq(struct mt7915_phy *phy)
588+ {
589+ struct mt7915_dev *dev = phy->dev;
590+ struct mt7915_tm_cmd req = {
591+ .testmode_en = 1,
592+ .param_idx = MCU_ATE_CLEAN_TXQUEUE,
593+- .param.clean.wcid = wcid,
594+ .param.clean.band = phy != &dev->phy,
595+ };
596++ struct mt76_testmode_sta *tm_sta;
597++ int ret, i;
598++
599++ if (!mt76_testmode_has_sta(phy->mt76)) {
600++ req.param.clean.wcid = dev->mt76.global_wcid.idx;
601++
602++ return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(ATE_CTRL),
603++ &req, sizeof(req), false);
604++ }
605++
606++ mt76_testmode_for_each_sta(phy->mt76, i, tm_sta) {
607++ req.param.clean.wcid = phy->mt76->test.tm_wcid[i]->idx;
608++ ret = mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(ATE_CTRL),
609++ &req, sizeof(req), false);
610++ if (ret)
611++ return ret;
612++ }
613++
614++ return 0;
615++}
616++
617++static int
618++mt7915_tm_set_phy_count(struct mt7915_phy *phy, u8 control)
619++{
620++ struct mt7915_dev *dev = phy->dev;
621++ struct mt7915_tm_cmd req = {
622++ .testmode_en = 1,
623++ .param_idx = MCU_ATE_SET_PHY_COUNT,
624++ .param.cfg.enable = control,
625++ .param.cfg.band = phy != &dev->phy,
626++ };
627+
628+ return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(ATE_CTRL), &req,
629+ sizeof(req), false);
630+@@ -167,6 +222,77 @@ mt7915_tm_set_tam_arb(struct mt7915_phy *phy, bool enable, bool mu)
631+ return mt7915_mcu_set_muru_ctrl(dev, MURU_SET_ARB_OP_MODE, op_mode);
632+ }
633+
634++static int
635++mt7915_tm_set_cfg(struct mt7915_phy *phy)
636++{
637++ static const u8 cfg_cmd[] = {
638++ [MT76_TM_CFG_TSSI] = MCU_ATE_SET_TSSI,
639++ [MT76_TM_CFG_DPD] = MCU_ATE_SET_DPD,
640++ [MT76_TM_CFG_RATE_POWER_OFFSET] = MCU_ATE_SET_RATE_POWER_OFFSET,
641++ [MT76_TM_CFG_THERMAL_COMP] = MCU_ATE_SET_THERMAL_COMP,
642++ };
643++ struct mt76_testmode_data *td = &phy->mt76->test;
644++ struct mt7915_dev *dev = phy->dev;
645++ struct mt7915_tm_cmd req = {
646++ .testmode_en = !(phy->mt76->test.state == MT76_TM_STATE_OFF),
647++ .param_idx = cfg_cmd[td->cfg.type],
648++ .param.cfg.enable = td->cfg.enable,
649++ .param.cfg.band = phy != &dev->phy,
650++ };
651++
652++ return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(ATE_CTRL), &req,
653++ sizeof(req), false);
654++}
655++
656++static int
657++mt7915_tm_set_off_channel_scan(struct mt7915_phy *phy)
658++{
659++#define OFF_CH_SCAN_SIMPLE_RX 2
660++ struct mt76_testmode_data *td = &phy->mt76->test;
661++ struct mt7915_dev *dev = phy->dev;
662++ struct cfg80211_chan_def *chandef = &phy->mt76->chandef;
663++ int freq1 = chandef->center_freq1;
664++ struct {
665++ u8 cur_pri_ch;
666++ u8 cur_center_ch;
667++ u8 cur_bw;
668++ u8 cur_tx_path;
669++ u8 cur_rx_path;
670++
671++ u8 scan_pri_ch;
672++ u8 scan_center_ch;
673++ u8 scan_bw;
674++ u8 scan_tx_path;
675++ u8 scan_rx_path;
676++
677++ u8 enable;
678++ u8 band_idx;
679++ u8 type;
680++ u8 is_5g;
681++ u8 _rsv[2];
682++ } __packed req = {
683++ .cur_pri_ch = chandef->chan->hw_value,
684++ .cur_center_ch = ieee80211_frequency_to_channel(freq1),
685++ .cur_bw = mt7915_tm_chan_bw(chandef->width),
686++ .cur_tx_path = td->tx_antenna_mask,
687++ .cur_rx_path = td->tx_antenna_mask,
688++
689++ .scan_pri_ch = td->off_ch_scan_ch,
690++ .scan_center_ch = td->off_ch_scan_center_ch,
691++ .scan_bw = td->off_ch_scan_bw,
692++ .scan_tx_path = td->off_ch_scan_path,
693++ .scan_rx_path = td->off_ch_scan_path,
694++
695++ .enable = !!td->off_ch_scan_ch,
696++ .band_idx = phy != &dev->phy,
697++ .type = OFF_CH_SCAN_SIMPLE_RX,
698++ .is_5g = td->off_ch_scan_ch > 14 ? 1 : 0,
699++ };
700++
701++ return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(OFFCH_SCAN_CTRL), &req,
702++ sizeof(req), false);
703++}
704++
705+ static int
706+ mt7915_tm_set_wmm_qid(struct mt7915_dev *dev, u8 qid, u8 aifs, u8 cw_min,
707+ u16 cw_max, u16 txop)
developer92086052022-03-17 16:55:20 +0800708+@@ -269,10 +395,10 @@ mt7915_tm_set_tx_len(struct mt7915_phy *phy, u32 tx_time)
709+ switch (td->tx_rate_mode) {
710+ case MT76_TM_TX_MODE_CCK:
711+ case MT76_TM_TX_MODE_OFDM:
712+- if (mphy->chandef.chan->band == NL80211_BAND_5GHZ)
713+- sband = &mphy->sband_5g.sband;
714+- else if (mphy->chandef.chan->band == NL80211_BAND_6GHZ)
715++ if (mphy->chandef.chan->band == NL80211_BAND_6GHZ)
716+ sband = &mphy->sband_6g.sband;
717++ else if (mphy->chandef.chan->band == NL80211_BAND_5GHZ)
718++ sband = &mphy->sband_5g.sband;
719+ else
720+ sband = &mphy->sband_2g.sband;
721+
722+@@ -322,7 +448,7 @@ mt7915_tm_set_tx_len(struct mt7915_phy *phy, u32 tx_time)
developer5f11e9e2022-03-10 15:03:47 +0800723+ bitrate = cfg80211_calculate_bitrate(&rate);
724+ tx_len = bitrate * tx_time / 10 / 8;
725+
726+- ret = mt76_testmode_alloc_skb(phy->mt76, tx_len);
727++ ret = mt76_testmode_init_skb(phy->mt76, tx_len, 0, &td->tx_skb);
728+ if (ret)
729+ return ret;
730+
developer92086052022-03-17 16:55:20 +0800731+@@ -334,7 +460,7 @@ mt7915_tm_reg_backup_restore(struct mt7915_phy *phy)
developer5f11e9e2022-03-10 15:03:47 +0800732+ {
733+ int n_regs = ARRAY_SIZE(reg_backup_list);
734+ struct mt7915_dev *dev = phy->dev;
735+- u32 *b = phy->test.reg_backup;
736++ u32 *b = phy->test.reg_backup, val;
737+ int i;
738+
739+ REG_BAND_IDX(reg_backup_list[0], AGG_PCR0, 0);
developer92086052022-03-17 16:55:20 +0800740+@@ -346,18 +472,28 @@ mt7915_tm_reg_backup_restore(struct mt7915_phy *phy)
developer5f11e9e2022-03-10 15:03:47 +0800741+ REG_BAND(reg_backup_list[6], AGG_MRCR);
742+ REG_BAND(reg_backup_list[7], TMAC_TFCR0);
743+ REG_BAND(reg_backup_list[8], TMAC_TCR0);
744+- REG_BAND(reg_backup_list[9], AGG_ATCR1);
745+- REG_BAND(reg_backup_list[10], AGG_ATCR3);
746+- REG_BAND(reg_backup_list[11], TMAC_TRCR0);
747+- REG_BAND(reg_backup_list[12], TMAC_ICR0);
748+- REG_BAND_IDX(reg_backup_list[13], ARB_DRNGR0, 0);
749+- REG_BAND_IDX(reg_backup_list[14], ARB_DRNGR0, 1);
750+- REG_BAND(reg_backup_list[15], WF_RFCR);
751+- REG_BAND(reg_backup_list[16], WF_RFCR1);
752++ REG_BAND(reg_backup_list[9], TMAC_TCR2);
753++ REG_BAND(reg_backup_list[10], AGG_ATCR1);
754++ REG_BAND(reg_backup_list[11], AGG_ATCR3);
755++ REG_BAND(reg_backup_list[12], TMAC_TRCR0);
756++ REG_BAND(reg_backup_list[13], TMAC_ICR0);
757++ REG_BAND_IDX(reg_backup_list[14], ARB_DRNGR0, 0);
758++ REG_BAND_IDX(reg_backup_list[15], ARB_DRNGR0, 1);
759++ REG_BAND(reg_backup_list[16], WF_RFCR);
760++ REG_BAND(reg_backup_list[17], WF_RFCR1);
761++
762++ if (is_mt7916(&dev->mt76)) {
763++ reg_backup_list[18].band[phy->band_idx] = MT_MDP_TOP_DBG_WDT_CTRL;
764++ reg_backup_list[19].band[phy->band_idx] = MT_MDP_TOP_DBG_CTRL;
765++ }
766+
767+ if (phy->mt76->test.state == MT76_TM_STATE_OFF) {
768+- for (i = 0; i < n_regs; i++)
769+- mt76_wr(dev, reg_backup_list[i].band[phy->band_idx], b[i]);
770++ for (i = 0; i < n_regs; i++) {
771++ u8 reg = reg_backup_list[i].band[phy->band_idx];
772++
773++ if (reg)
774++ mt76_wr(dev, reg, b[i]);
775++ }
776+ return;
777+ }
778+
developer92086052022-03-17 16:55:20 +0800779+@@ -377,8 +513,13 @@ mt7915_tm_reg_backup_restore(struct mt7915_phy *phy)
developer5f11e9e2022-03-10 15:03:47 +0800780+ MT_AGG_PCR0_BW40_PROT | MT_AGG_PCR0_BW80_PROT);
781+ mt76_set(dev, MT_AGG_PCR0(phy->band_idx, 0), MT_AGG_PCR0_PTA_WIN_DIS);
782+
783+- mt76_wr(dev, MT_AGG_PCR0(phy->band_idx, 1), MT_AGG_PCR1_RTS0_NUM_THRES |
784+- MT_AGG_PCR1_RTS0_LEN_THRES);
785++ if (is_mt7915(&dev->mt76))
786++ val = MT_AGG_PCR1_RTS0_NUM_THRES | MT_AGG_PCR1_RTS0_LEN_THRES;
787++ else
788++ val = MT_AGG_PCR1_RTS0_NUM_THRES_MT7916 |
789++ MT_AGG_PCR1_RTS0_LEN_THRES_MT7916;
790++
791++ mt76_wr(dev, MT_AGG_PCR0(phy->band_idx, 1), val);
792+
793+ mt76_clear(dev, MT_AGG_MRCR(phy->band_idx), MT_AGG_MRCR_BAR_CNT_LIMIT |
794+ MT_AGG_MRCR_LAST_RTS_CTS_RN | MT_AGG_MRCR_RTS_FAIL_LIMIT |
developer92086052022-03-17 16:55:20 +0800795+@@ -391,31 +532,128 @@ mt7915_tm_reg_backup_restore(struct mt7915_phy *phy)
developer5f11e9e2022-03-10 15:03:47 +0800796+
797+ mt76_wr(dev, MT_TMAC_TFCR0(phy->band_idx), 0);
798+ mt76_clear(dev, MT_TMAC_TCR0(phy->band_idx), MT_TMAC_TCR0_TBTT_STOP_CTRL);
799++ mt76_set(dev, MT_TMAC_TCR2(phy->band_idx), MT_TMAC_TCR2_SCH_DET_DIS);
800+
801+ /* config rx filter for testmode rx */
802+ mt76_wr(dev, MT_WF_RFCR(phy->band_idx), 0xcf70a);
803+ mt76_wr(dev, MT_WF_RFCR1(phy->band_idx), 0);
804++
805++ if (is_mt7916(&dev->mt76)) {
806++ /* enable MDP Tx block mode */
807++ mt76_clear(dev, MT_MDP_TOP_DBG_WDT_CTRL,
808++ MT_MDP_TOP_DBG_WDT_CTRL_TDP_DIS_BLK);
809++ mt76_clear(dev, MT_MDP_TOP_DBG_CTRL,
810++ MT_MDP_TOP_DBG_CTRL_ENQ_MODE);
811++ }
812++}
813++
814++static int
815++mt7915_tm_sta_add(struct mt7915_phy *phy, u8 aid,
816++ struct mt76_testmode_sta_data *sd)
817++{
818++ struct mt76_testmode_data *td = &phy->mt76->test;
819++ struct mt76_testmode_sta *tm_sta;
820++
821++ if (!aid)
822++ return 0;
823++
824++ if (!td->tm_wcid[aid]) {
825++ struct ieee80211_vif *vif = phy->monitor_vif;
826++ struct ieee80211_sband_iftype_data *data;
827++ struct ieee80211_supported_band *sband;
828++ struct ieee80211_sta *sta;
829++ struct mt7915_sta *msta;
830++ int ret;
831++
832++ sta = kzalloc(sizeof(*sta) + phy->mt76->hw->sta_data_size +
833++ sizeof(*tm_sta), GFP_KERNEL);
834++ if (!sta)
835++ return -ENOMEM;
836++
developer92086052022-03-17 16:55:20 +0800837++ if (phy->mt76->chandef.chan->band == NL80211_BAND_6GHZ) {
838++ sband = &phy->mt76->sband_6g.sband;
839++ data = phy->iftype[NL80211_BAND_6GHZ];
840++ } else if (phy->mt76->chandef.chan->band == NL80211_BAND_5GHZ) {
developer5f11e9e2022-03-10 15:03:47 +0800841++ sband = &phy->mt76->sband_5g.sband;
842++ data = phy->iftype[NL80211_BAND_5GHZ];
843++ } else {
844++ sband = &phy->mt76->sband_2g.sband;
845++ data = phy->iftype[NL80211_BAND_2GHZ];
846++ }
847++
848++ ether_addr_copy(sta->addr, phy->mt76->macaddr);
849++ sta->addr[0] += aid * 4;
850++ memcpy(&sta->ht_cap, &sband->ht_cap, sizeof(sta->ht_cap));
851++ memcpy(&sta->vht_cap, &sband->vht_cap, sizeof(sta->vht_cap));
852++ memcpy(&sta->he_cap, &data[NL80211_IFTYPE_STATION].he_cap,
853++ sizeof(sta->he_cap));
854++ sta->aid = aid;
855++ sta->wme = 1;
856++
857++ ret = mt7915_mac_sta_add(&phy->dev->mt76, vif, sta);
858++ if (ret) {
859++ kfree(sta);
860++ return ret;
861++ }
862++
863++ msta = (struct mt7915_sta *)sta->drv_priv;
864++ td->tm_wcid[aid] = &msta->wcid;
865++ td->tm_sta_mask |= BIT(aid - 1);
866++ }
867++
868++ tm_sta = mt76_testmode_aid_get_sta(phy->mt76, aid);
869++ memcpy(&tm_sta->sd, sd, sizeof(tm_sta->sd));
870++
871++ return 0;
872+ }
873+
874+ static void
875+-mt7915_tm_init(struct mt7915_phy *phy, bool en)
876++mt7915_tm_sta_remove(struct mt7915_phy *phy, u8 aid)
877+ {
878++ struct mt76_testmode_data *td = &phy->mt76->test;
879++ struct mt76_wcid *wcid = td->tm_wcid[aid];
880+ struct mt7915_dev *dev = phy->dev;
881++ struct ieee80211_sta *sta = wcid_to_sta(wcid);
882+
883+- if (!test_bit(MT76_STATE_RUNNING, &phy->mt76->state))
884++ mt7915_mac_sta_remove(&dev->mt76, phy->monitor_vif, sta);
885++ mt76_wcid_mask_clear(dev->mt76.wcid_mask, wcid->idx);
886++
887++ kfree(sta);
888++ td->tm_wcid[aid] = NULL;
889++ td->tm_sta_mask &= ~BIT(aid - 1);
890++}
891++
892++static void
893++mt7915_tm_sta_remove_all(struct mt7915_phy *phy)
894++{
895++ int i;
896++
897++ if (!mt76_testmode_has_sta(phy->mt76))
898+ return;
899+
900+- mt7915_mcu_set_sku_en(phy, !en);
901++ for (i = 1; i < ARRAY_SIZE(phy->mt76->test.tm_wcid); i++) {
902++ if (phy->mt76->test.tm_wcid[i])
903++ mt7915_tm_sta_remove(phy, i);
904++ }
905++}
906+
907+- mt7915_tm_mode_ctrl(dev, en);
908+- mt7915_tm_reg_backup_restore(phy);
909+- mt7915_tm_set_trx(phy, TM_MAC_TXRX, !en);
910++static int
911++mt7915_tm_set_sta(struct mt7915_phy *phy)
912++{
913++ struct mt76_testmode_data *td = &phy->mt76->test;
developer92086052022-03-17 16:55:20 +0800914++ bool en = td->state != MT76_TM_STATE_OFF;
developer5f11e9e2022-03-10 15:03:47 +0800915+
916+- mt7915_mcu_add_bss_info(phy, phy->monitor_vif, en);
917+- mt7915_mcu_add_sta(dev, phy->monitor_vif, NULL, en);
developer92086052022-03-17 16:55:20 +0800918++ if (!en || !td->aid) {
developer5f11e9e2022-03-10 15:03:47 +0800919++ mt7915_tm_sta_remove_all(phy);
920++ return 0;
921++ }
922+
923+- if (!en)
924+- mt7915_tm_set_tam_arb(phy, en, 0);
925++ if (td->tx_count == 0) {
926++ mt7915_tm_sta_remove(phy, td->aid);
927++ return 0;
928++ }
929++
930++ return mt7915_tm_sta_add(phy, td->aid, &td->sd);
931+ }
932+
933+ static void
developer92086052022-03-17 16:55:20 +0800934+@@ -428,59 +666,122 @@ mt7915_tm_update_channel(struct mt7915_phy *phy)
developer5f11e9e2022-03-10 15:03:47 +0800935+ mt7915_mcu_set_chan_info(phy, MCU_EXT_CMD(SET_RX_PATH));
936+ }
937+
938++static bool
939++mt7915_tm_check_skb(struct mt7915_phy *phy)
940++{
941++ struct mt76_testmode_data *td = &phy->mt76->test;
942++ struct ieee80211_tx_info *info;
943++
944++ if (!mt76_testmode_has_sta(phy->mt76)) {
945++ if (!td->tx_skb)
946++ return false;
947++
948++ info = IEEE80211_SKB_CB(td->tx_skb);
949++ info->control.vif = phy->monitor_vif;
950++ } else {
951++ struct mt76_testmode_sta *tm_sta;
952++ int i;
953++
954++ mt76_testmode_for_each_sta(phy->mt76, i, tm_sta) {
955++ if (!tm_sta->tx_skb)
956++ return false;
957++
958++ info = IEEE80211_SKB_CB(tm_sta->tx_skb);
959++ info->control.vif = phy->monitor_vif;
960++ }
961++ }
962++
963++ return true;
964++}
965++
966+ static void
967+ mt7915_tm_set_tx_frames(struct mt7915_phy *phy, bool en)
968+ {
969+ static const u8 spe_idx_map[] = {0, 0, 1, 0, 3, 2, 4, 0,
970+ 9, 8, 6, 10, 16, 12, 18, 0};
971+ struct mt76_testmode_data *td = &phy->mt76->test;
972+- struct mt7915_dev *dev = phy->dev;
973+- struct ieee80211_tx_info *info;
974+- u8 duty_cycle = td->tx_duty_cycle;
975+- u32 tx_time = td->tx_time;
976+- u32 ipg = td->tx_ipg;
977+
978+ mt7915_tm_set_trx(phy, TM_MAC_RX_RXV, false);
979+- mt7915_tm_clean_hwq(phy, dev->mt76.global_wcid.idx);
980++ mt7915_tm_set_trx(phy, TM_MAC_TX, false);
981+
982+ if (en) {
983++ u32 tx_time = td->tx_time, ipg = td->tx_ipg;
984++ u8 duty_cycle = td->tx_duty_cycle;
985++
986+ mt7915_tm_update_channel(phy);
987+
988+ if (td->tx_spe_idx) {
989+ phy->test.spe_idx = td->tx_spe_idx;
990+ } else {
991+- u8 tx_ant = td->tx_antenna_mask;
992++ phy->test.spe_idx = spe_idx_map[td->tx_antenna_mask];
993++ }
994+
995+- if (phy != &dev->phy)
996+- tx_ant >>= dev->chainshift;
997+- phy->test.spe_idx = spe_idx_map[tx_ant];
998++ /* if all three params are set, duty_cycle will be ignored */
999++ if (duty_cycle && tx_time && !ipg) {
1000++ ipg = tx_time * 100 / duty_cycle - tx_time;
1001++ } else if (duty_cycle && !tx_time && ipg) {
1002++ if (duty_cycle < 100)
1003++ tx_time = duty_cycle * ipg / (100 - duty_cycle);
1004+ }
1005++
1006++ mt7915_tm_set_ipg_params(phy, ipg, td->tx_rate_mode);
1007++ mt7915_tm_set_tx_len(phy, tx_time);
1008++
1009++ if (ipg)
1010++ td->tx_queued_limit = MT76_TM_TIMEOUT * 1000000 / ipg / 2;
1011++
1012++ if (!mt7915_tm_check_skb(phy))
1013++ return;
1014++ } else {
1015++ mt7915_tm_clean_hwq(phy);
1016+ }
1017+
1018+ mt7915_tm_set_tam_arb(phy, en,
1019+ td->tx_rate_mode == MT76_TM_TX_MODE_HE_MU);
1020+
1021+- /* if all three params are set, duty_cycle will be ignored */
1022+- if (duty_cycle && tx_time && !ipg) {
1023+- ipg = tx_time * 100 / duty_cycle - tx_time;
1024+- } else if (duty_cycle && !tx_time && ipg) {
1025+- if (duty_cycle < 100)
1026+- tx_time = duty_cycle * ipg / (100 - duty_cycle);
1027+- }
1028++ mt7915_tm_set_trx(phy, TM_MAC_TX, en);
1029++}
1030+
1031+- mt7915_tm_set_ipg_params(phy, ipg, td->tx_rate_mode);
1032+- mt7915_tm_set_tx_len(phy, tx_time);
1033++static int
1034++mt7915_tm_get_rx_stats(struct mt7915_phy *phy, bool clear)
1035++{
1036++#define CMD_RX_STAT_BAND 0x3
1037++ struct mt76_testmode_data *td = &phy->mt76->test;
1038++ struct mt7915_tm_rx_stat_band *rs_band;
1039++ struct mt7915_dev *dev = phy->dev;
1040++ struct sk_buff *skb;
1041++ struct {
1042++ u8 format_id;
1043++ u8 band;
1044++ u8 _rsv[2];
1045++ } __packed req = {
1046++ .format_id = CMD_RX_STAT_BAND,
1047++ .band = phy != &dev->phy,
1048++ };
1049++ int ret;
1050+
1051+- if (ipg)
1052+- td->tx_queued_limit = MT76_TM_TIMEOUT * 1000000 / ipg / 2;
1053++ ret = mt76_mcu_send_and_get_msg(&dev->mt76, MCU_EXT_CMD(RX_STAT),
1054++ &req, sizeof(req), true, &skb);
1055++ if (ret)
1056++ return ret;
1057+
1058+- if (!en || !td->tx_skb)
1059+- return;
1060++ rs_band = (struct mt7915_tm_rx_stat_band *)skb->data;
1061++ /* pr_info("mdrdy_cnt = %d\n", le32_to_cpu(rs_band->mdrdy_cnt)); */
1062++ /* pr_info("fcs_err = %d\n", le16_to_cpu(rs_band->fcs_err)); */
1063++ /* pr_info("len_mismatch = %d\n", le16_to_cpu(rs_band->len_mismatch)); */
1064++ /* pr_info("fcs_ok = %d\n", le16_to_cpu(rs_band->fcs_succ)); */
1065+
1066+- info = IEEE80211_SKB_CB(td->tx_skb);
1067+- info->control.vif = phy->monitor_vif;
1068++ if (!clear) {
1069++ enum mt76_rxq_id q = req.band ? MT_RXQ_EXT : MT_RXQ_MAIN;
1070+
1071+- mt7915_tm_set_trx(phy, TM_MAC_TX, en);
1072++ td->rx_stats.packets[q] += le32_to_cpu(rs_band->mdrdy_cnt);
1073++ td->rx_stats.fcs_error[q] += le16_to_cpu(rs_band->fcs_err);
1074++ td->rx_stats.len_mismatch += le16_to_cpu(rs_band->len_mismatch);
1075++ }
1076++
1077++ dev_kfree_skb(skb);
1078++
1079++ return 0;
1080+ }
1081+
1082+ static void
developer92086052022-03-17 16:55:20 +08001083+@@ -489,12 +790,15 @@ mt7915_tm_set_rx_frames(struct mt7915_phy *phy, bool en)
developer5f11e9e2022-03-10 15:03:47 +08001084+ mt7915_tm_set_trx(phy, TM_MAC_RX_RXV, false);
1085+
1086+ if (en) {
1087+- struct mt7915_dev *dev = phy->dev;
1088+-
1089+ mt7915_tm_update_channel(phy);
1090+
1091+ /* read-clear */
1092+- mt76_rr(dev, MT_MIB_SDR3(phy != &dev->phy));
1093++ mt7915_tm_get_rx_stats(phy, true);
1094++
1095++ /* clear fw count */
1096++ mt7915_tm_set_phy_count(phy, 0);
1097++ mt7915_tm_set_phy_count(phy, 1);
1098++
1099+ mt7915_tm_set_trx(phy, TM_MAC_RX_RXV, en);
1100+ }
1101+ }
developer92086052022-03-17 16:55:20 +08001102+@@ -572,10 +876,10 @@ mt7915_tm_set_tx_cont(struct mt7915_phy *phy, bool en)
1103+ struct ieee80211_supported_band *sband;
1104+ u8 idx = rate_idx;
1105+
1106+- if (chandef->chan->band == NL80211_BAND_5GHZ)
1107+- sband = &phy->mt76->sband_5g.sband;
1108+- else if (chandef->chan->band == NL80211_BAND_6GHZ)
1109++ if (chandef->chan->band == NL80211_BAND_6GHZ)
1110+ sband = &phy->mt76->sband_6g.sband;
1111++ else if (chandef->chan->band == NL80211_BAND_5GHZ)
1112++ sband = &phy->mt76->sband_5g.sband;
1113+ else
1114+ sband = &phy->mt76->sband_2g.sband;
1115+
1116+@@ -635,6 +939,31 @@ out:
developer5f11e9e2022-03-10 15:03:47 +08001117+ sizeof(req), true);
1118+ }
1119+
1120++static void
1121++mt7915_tm_init(struct mt7915_phy *phy, bool en)
1122++{
1123++ struct mt7915_dev *dev = phy->dev;
1124++
1125++ if (!test_bit(MT76_STATE_RUNNING, &phy->mt76->state))
1126++ return;
1127++
1128++ mt7915_mcu_set_sku_en(phy, !en);
1129++
1130++ mt7915_tm_mode_ctrl(dev, en);
1131++ mt7915_tm_reg_backup_restore(phy);
1132++ mt7915_tm_set_trx(phy, TM_MAC_TXRX, !en);
1133++
1134++ mt7915_mcu_add_bss_info(phy, phy->monitor_vif, en);
1135++ mt7915_mcu_add_sta(dev, phy->monitor_vif, NULL, en);
1136++
1137++ phy->mt76->test.flag |= MT_TM_FW_RX_COUNT;
1138++
1139++ if (!en) {
1140++ mt7915_tm_set_tam_arb(phy, en, 0);
1141++ mt7915_tm_sta_remove_all(phy);
1142++ }
1143++}
1144++
1145+ static void
1146+ mt7915_tm_update_params(struct mt7915_phy *phy, u32 changed)
1147+ {
developer92086052022-03-17 16:55:20 +08001148+@@ -645,6 +974,12 @@ mt7915_tm_update_params(struct mt7915_phy *phy, u32 changed)
developer5f11e9e2022-03-10 15:03:47 +08001149+ mt7915_tm_set_freq_offset(phy, en, en ? td->freq_offset : 0);
1150+ if (changed & BIT(TM_CHANGED_TXPOWER))
1151+ mt7915_tm_set_tx_power(phy);
1152++ if (changed & BIT(TM_CHANGED_CFG))
1153++ mt7915_tm_set_cfg(phy);
1154++ if (changed & BIT(TM_CHANGED_OFF_CH_SCAN_CH))
1155++ mt7915_tm_set_off_channel_scan(phy);
1156++ if (changed & BIT(TM_CHANGED_AID))
1157++ mt7915_tm_set_sta(phy);
1158+ }
1159+
1160+ static int
developer92086052022-03-17 16:55:20 +08001161+@@ -704,9 +1039,6 @@ mt7915_tm_set_params(struct mt76_phy *mphy, struct nlattr **tb,
developer5f11e9e2022-03-10 15:03:47 +08001162+ td->state == MT76_TM_STATE_OFF)
1163+ return 0;
1164+
1165+- if (td->tx_antenna_mask & ~mphy->chainmask)
1166+- return -EINVAL;
1167+-
1168+ for (i = 0; i < ARRAY_SIZE(tm_change_map); i++) {
1169+ if (tb[tm_change_map[i]])
1170+ changed |= BIT(i);
developer92086052022-03-17 16:55:20 +08001171+@@ -721,12 +1053,8 @@ static int
developer5f11e9e2022-03-10 15:03:47 +08001172+ mt7915_tm_dump_stats(struct mt76_phy *mphy, struct sk_buff *msg)
1173+ {
1174+ struct mt7915_phy *phy = mphy->priv;
1175+- struct mt7915_dev *dev = phy->dev;
1176+- enum mt76_rxq_id q;
1177+ void *rx, *rssi;
1178+- u16 fcs_err;
1179+ int i;
1180+- u32 cnt;
1181+
1182+ rx = nla_nest_start(msg, MT76_TM_STATS_ATTR_LAST_RX);
1183+ if (!rx)
developer92086052022-03-17 16:55:20 +08001184+@@ -770,19 +1098,68 @@ mt7915_tm_dump_stats(struct mt76_phy *mphy, struct sk_buff *msg)
developer5f11e9e2022-03-10 15:03:47 +08001185+
1186+ nla_nest_end(msg, rx);
1187+
1188+- cnt = mt76_rr(dev, MT_MIB_SDR3(phy->band_idx));
1189+- fcs_err = is_mt7915(&dev->mt76) ? FIELD_GET(MT_MIB_SDR3_FCS_ERR_MASK, cnt) :
1190+- FIELD_GET(MT_MIB_SDR3_FCS_ERR_MASK_MT7916, cnt);
1191++ return mt7915_tm_get_rx_stats(phy, false);
1192++}
developer92086052022-03-17 16:55:20 +08001193+
1194+- q = phy->band_idx ? MT_RXQ_EXT : MT_RXQ_MAIN;
1195+- mphy->test.rx_stats.packets[q] += fcs_err;
1196+- mphy->test.rx_stats.fcs_error[q] += fcs_err;
developer5f11e9e2022-03-10 15:03:47 +08001197++static int
1198++mt7915_tm_write_back_to_efuse(struct mt7915_dev *dev)
1199++{
1200++ struct mt7915_mcu_eeprom_info req = {};
1201++ u8 *eeprom = dev->mt76.eeprom.data;
1202++ int i, ret = -EINVAL;
1203+
developer92086052022-03-17 16:55:20 +08001204+- return 0;
developer5f11e9e2022-03-10 15:03:47 +08001205++ if (is_mt7986(&dev->mt76))
1206++ goto out;
developer92086052022-03-17 16:55:20 +08001207++
developer5f11e9e2022-03-10 15:03:47 +08001208++ /* prevent from damaging chip id in efuse */
1209++ if (mt76_chip(&dev->mt76) != get_unaligned_le16(eeprom))
1210++ goto out;
1211++
1212++ for (i = 0; i < MT7915_EEPROM_SIZE; i += MT76_TM_EEPROM_BLOCK_SIZE) {
1213++ req.addr = cpu_to_le32(i);
1214++ memcpy(&req.data, eeprom + i, MT76_TM_EEPROM_BLOCK_SIZE);
1215++
1216++ ret = mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(EFUSE_ACCESS),
1217++ &req, sizeof(req), true);
1218++ if (ret)
1219++ return ret;
1220++ }
1221++
1222++out:
1223++ return ret;
1224++}
1225++
1226++static int
1227++mt7915_tm_set_eeprom(struct mt76_phy *mphy, u32 offset, u8 *val, u8 action)
1228++{
1229++ struct mt7915_phy *phy = mphy->priv;
1230++ struct mt7915_dev *dev = phy->dev;
1231++ u8 *eeprom = dev->mt76.eeprom.data;
1232++ int ret = 0;
1233++
1234++ if (offset >= MT7915_EEPROM_SIZE)
1235++ return -EINVAL;
1236++
1237++ switch (action) {
1238++ case MT76_TM_EEPROM_ACTION_UPDATE_DATA:
1239++ memcpy(eeprom + offset, val, MT76_TM_EEPROM_BLOCK_SIZE);
1240++ break;
1241++ case MT76_TM_EEPROM_ACTION_UPDATE_BUFFER_MODE:
1242++ ret = mt7915_mcu_set_eeprom(dev, true);
1243++ break;
1244++ case MT76_TM_EEPROM_ACTION_WRITE_TO_EFUSE:
1245++ ret = mt7915_tm_write_back_to_efuse(dev);
1246++ break;
1247++ default:
1248++ break;
1249++ }
1250++
1251++ return ret;
1252+ }
1253+
1254+ const struct mt76_testmode_ops mt7915_testmode_ops = {
1255+ .set_state = mt7915_tm_set_state,
1256+ .set_params = mt7915_tm_set_params,
1257+ .dump_stats = mt7915_tm_dump_stats,
1258++ .set_eeprom = mt7915_tm_set_eeprom,
1259+ };
1260+diff --git a/mt7915/testmode.h b/mt7915/testmode.h
developer92086052022-03-17 16:55:20 +08001261+index 5573ac30..d22aabe7 100644
developer5f11e9e2022-03-10 15:03:47 +08001262+--- a/mt7915/testmode.h
1263++++ b/mt7915/testmode.h
1264+@@ -33,6 +33,12 @@ struct mt7915_tm_clean_txq {
1265+ u8 rsv;
1266+ };
1267+
1268++struct mt7915_tm_cfg {
1269++ u8 enable;
1270++ u8 band;
1271++ u8 _rsv[2];
1272++};
1273++
1274+ struct mt7915_tm_cmd {
1275+ u8 testmode_en;
1276+ u8 param_idx;
1277+@@ -43,6 +49,7 @@ struct mt7915_tm_cmd {
1278+ struct mt7915_tm_freq_offset freq;
1279+ struct mt7915_tm_slot_time slot;
1280+ struct mt7915_tm_clean_txq clean;
1281++ struct mt7915_tm_cfg cfg;
1282+ u8 test[72];
1283+ } param;
1284+ } __packed;
1285+@@ -102,4 +109,35 @@ enum {
1286+ TAM_ARB_OP_MODE_FORCE_SU = 5,
1287+ };
1288+
1289++struct mt7915_tm_rx_stat_band {
1290++ u8 category;
1291++
1292++ /* mac */
1293++ __le16 fcs_err;
1294++ __le16 len_mismatch;
1295++ __le16 fcs_succ;
1296++ __le32 mdrdy_cnt;
1297++ /* phy */
1298++ __le16 fcs_err_cck;
1299++ __le16 fcs_err_ofdm;
1300++ __le16 pd_cck;
1301++ __le16 pd_ofdm;
1302++ __le16 sig_err_cck;
1303++ __le16 sfd_err_cck;
1304++ __le16 sig_err_ofdm;
1305++ __le16 tag_err_ofdm;
1306++ __le16 mdrdy_cnt_cck;
1307++ __le16 mdrdy_cnt_ofdm;
1308++};
1309++
1310++enum {
1311++ TM_CBW_20MHZ,
1312++ TM_CBW_40MHZ,
1313++ TM_CBW_80MHZ,
1314++ TM_CBW_10MHZ,
1315++ TM_CBW_5MHZ,
1316++ TM_CBW_160MHZ,
1317++ TM_CBW_8080MHZ,
1318++};
1319++
1320+ #endif
1321+diff --git a/testmode.c b/testmode.c
developer92086052022-03-17 16:55:20 +08001322+index 382b4563..9da490cf 100644
developer5f11e9e2022-03-10 15:03:47 +08001323+--- a/testmode.c
1324++++ b/testmode.c
1325+@@ -25,18 +25,18 @@ const struct nla_policy mt76_tm_policy[NUM_MT76_TM_ATTRS] = {
1326+ };
1327+ EXPORT_SYMBOL_GPL(mt76_tm_policy);
1328+
1329+-void mt76_testmode_tx_pending(struct mt76_phy *phy)
1330++static u16
1331++mt76_testmode_queue_tx(struct mt76_phy *phy, struct mt76_wcid *wcid,
1332++ struct sk_buff *skb, u32 limit)
1333+ {
1334+ struct mt76_testmode_data *td = &phy->test;
1335+ struct mt76_dev *dev = phy->dev;
1336+- struct mt76_wcid *wcid = &dev->global_wcid;
1337+- struct sk_buff *skb = td->tx_skb;
1338+ struct mt76_queue *q;
1339+- u16 tx_queued_limit;
1340++ u16 tx_queued_limit, count = 0;
1341+ int qid;
1342+
1343+- if (!skb || !td->tx_pending)
1344+- return;
1345++ if (!skb)
1346++ return 0;
1347+
1348+ qid = skb_get_queue_mapping(skb);
1349+ q = phy->q_tx[qid];
1350+@@ -45,7 +45,7 @@ void mt76_testmode_tx_pending(struct mt76_phy *phy)
1351+
1352+ spin_lock_bh(&q->lock);
1353+
1354+- while (td->tx_pending > 0 &&
1355++ while (count < limit &&
1356+ td->tx_queued - td->tx_done < tx_queued_limit &&
1357+ q->queued < q->ndesc / 2) {
1358+ int ret;
1359+@@ -55,13 +55,56 @@ void mt76_testmode_tx_pending(struct mt76_phy *phy)
1360+ if (ret < 0)
1361+ break;
1362+
1363+- td->tx_pending--;
1364+ td->tx_queued++;
1365++ count++;
1366+ }
1367+
1368+ dev->queue_ops->kick(dev, q);
1369+
1370+ spin_unlock_bh(&q->lock);
1371++
1372++ return count;
1373++}
1374++
1375++void mt76_testmode_tx_pending(struct mt76_phy *phy)
1376++{
1377++ struct mt76_testmode_data *td = &phy->test;
1378++ u16 count;
1379++
1380++ if (!td->tx_pending)
1381++ return;
1382++
1383++ if (!mt76_testmode_has_sta(phy)) {
1384++ count = mt76_testmode_queue_tx(phy, &phy->dev->global_wcid,
1385++ td->tx_skb, td->tx_pending);
1386++ td->tx_pending -= count;
1387++
1388++ return;
1389++ }
1390++
1391++ while (true) {
1392++ struct mt76_testmode_sta *tm_sta;
1393++ struct mt76_wcid *wcid;
1394++ u32 limit, per_sta_cnt = 1;
1395++
1396++ if (td->tx_rate_mode != MT76_TM_TX_MODE_HE_MU)
1397++ per_sta_cnt = td->tx_count / hweight16(phy->test.tm_sta_mask);
1398++
1399++ limit = td->tx_pending % per_sta_cnt;
1400++ if (limit == 0)
1401++ limit = per_sta_cnt;
1402++
1403++ tm_sta = mt76_testmode_aid_get_sta(phy, td->cur_aid);
1404++ wcid = td->tm_wcid[td->cur_aid];
1405++ count = mt76_testmode_queue_tx(phy, wcid, tm_sta->tx_skb, limit);
1406++
1407++ td->tx_pending -= count;
1408++
1409++ if (td->tx_pending && (td->tx_pending % per_sta_cnt == 0))
1410++ td->cur_aid = ffs(td->tm_sta_mask >> td->cur_aid) + td->cur_aid;
1411++ else
1412++ break;
1413++ }
1414+ }
1415+
1416+ static u32
1417+@@ -87,15 +130,34 @@ mt76_testmode_max_mpdu_len(struct mt76_phy *phy, u8 tx_rate_mode)
1418+ }
1419+
1420+ static void
1421+-mt76_testmode_free_skb(struct mt76_phy *phy)
1422++mt76_testmode_free_skb(struct sk_buff **tx_skb)
1423++{
1424++ dev_kfree_skb(*tx_skb);
1425++ *tx_skb = NULL;
1426++}
1427++
1428++static void
1429++mt76_testmode_free_skb_all(struct mt76_phy *phy)
1430+ {
1431+ struct mt76_testmode_data *td = &phy->test;
1432+
1433+- dev_kfree_skb(td->tx_skb);
1434+- td->tx_skb = NULL;
1435++ if (mt76_testmode_has_sta(phy)) {
1436++ struct mt76_testmode_sta *tm_sta;
1437++ int i;
1438++
1439++ mt76_testmode_for_each_sta(phy, i, tm_sta) {
1440++ mt76_testmode_free_skb(&tm_sta->tx_skb);
1441++ }
1442++
1443++ return;
1444++ }
1445++
1446++ mt76_testmode_free_skb(&td->tx_skb);
1447+ }
1448+
1449+-int mt76_testmode_alloc_skb(struct mt76_phy *phy, u32 len)
1450++static int
1451++mt76_testmode_alloc_skb(struct mt76_phy *phy, u32 len,
1452++ struct sk_buff **tx_skb, u8 *da)
1453+ {
1454+ #define MT_TXP_MAX_LEN 4095
1455+ u16 fc = IEEE80211_FTYPE_DATA | IEEE80211_STYPE_DATA |
1456+@@ -128,7 +190,9 @@ int mt76_testmode_alloc_skb(struct mt76_phy *phy, u32 len)
1457+ hdr->frame_control = cpu_to_le16(fc);
1458+ memcpy(hdr->addr1, td->addr[0], ETH_ALEN);
1459+ memcpy(hdr->addr2, td->addr[1], ETH_ALEN);
1460+- memcpy(hdr->addr3, td->addr[2], ETH_ALEN);
1461++ /* memcpy(hdr->addr3, td->addr[2], ETH_ALEN); */
1462++ memcpy(hdr->addr3, da, ETH_ALEN);
1463++
1464+ skb_set_queue_mapping(head, IEEE80211_AC_BE);
1465+
1466+ info = IEEE80211_SKB_CB(head);
1467+@@ -152,7 +216,7 @@ int mt76_testmode_alloc_skb(struct mt76_phy *phy, u32 len)
1468+
1469+ frag = alloc_skb(frag_len, GFP_KERNEL);
1470+ if (!frag) {
1471+- mt76_testmode_free_skb(phy);
1472++ mt76_testmode_free_skb(tx_skb);
1473+ dev_kfree_skb(head);
1474+ return -ENOMEM;
1475+ }
1476+@@ -165,23 +229,25 @@ int mt76_testmode_alloc_skb(struct mt76_phy *phy, u32 len)
1477+ frag_tail = &(*frag_tail)->next;
1478+ }
1479+
1480+- mt76_testmode_free_skb(phy);
1481+- td->tx_skb = head;
1482++ mt76_testmode_free_skb(tx_skb);
1483++ *tx_skb = head;
1484+
1485+ return 0;
1486+ }
1487+-EXPORT_SYMBOL(mt76_testmode_alloc_skb);
1488+
1489+-static int
1490+-mt76_testmode_tx_init(struct mt76_phy *phy)
1491++int mt76_testmode_init_skb(struct mt76_phy *phy, u32 len, u8 aid,
1492++ struct sk_buff **tx_skb)
1493+ {
1494+ struct mt76_testmode_data *td = &phy->test;
1495+ struct ieee80211_tx_info *info;
1496+ struct ieee80211_tx_rate *rate;
1497+ u8 max_nss = hweight8(phy->antenna_mask);
1498++ u8 da[ETH_ALEN];
1499+ int ret;
1500+
1501+- ret = mt76_testmode_alloc_skb(phy, td->tx_mpdu_len);
1502++ ether_addr_copy(da, phy->macaddr);
1503++ da[0] += aid * 4;
1504++ ret = mt76_testmode_alloc_skb(phy, len, tx_skb, da);
1505+ if (ret)
1506+ return ret;
1507+
1508+@@ -191,7 +257,7 @@ mt76_testmode_tx_init(struct mt76_phy *phy)
1509+ if (td->tx_antenna_mask)
1510+ max_nss = min_t(u8, max_nss, hweight8(td->tx_antenna_mask));
1511+
1512+- info = IEEE80211_SKB_CB(td->tx_skb);
1513++ info = IEEE80211_SKB_CB(*tx_skb);
1514+ rate = &info->control.rates[0];
1515+ rate->count = 1;
1516+ rate->idx = td->tx_rate_idx;
1517+@@ -263,6 +329,28 @@ mt76_testmode_tx_init(struct mt76_phy *phy)
1518+ out:
1519+ return 0;
1520+ }
1521++EXPORT_SYMBOL(mt76_testmode_init_skb);
1522++
1523++static int
1524++mt76_testmode_tx_init(struct mt76_phy *phy)
1525++{
1526++ struct mt76_testmode_data *td = &phy->test;
1527++ struct mt76_testmode_sta *tm_sta;
1528++ int ret, i;
1529++
1530++ if (!mt76_testmode_has_sta(phy))
1531++ return mt76_testmode_init_skb(phy, td->tx_mpdu_len,
1532++ 0, &td->tx_skb);
1533++
1534++ mt76_testmode_for_each_sta(phy, i, tm_sta) {
1535++ ret = mt76_testmode_init_skb(phy, tm_sta->sd.tx_mpdu_len,
1536++ tm_sta->sd.aid, &tm_sta->tx_skb);
1537++ if (ret)
1538++ return ret;
1539++ }
1540++
1541++ return 0;
1542++}
1543+
1544+ static void
1545+ mt76_testmode_tx_start(struct mt76_phy *phy)
1546+@@ -273,6 +361,17 @@ mt76_testmode_tx_start(struct mt76_phy *phy)
1547+ td->tx_queued = 0;
1548+ td->tx_done = 0;
1549+ td->tx_pending = td->tx_count;
1550++
1551++ if (mt76_testmode_has_sta(phy)) {
1552++ td->cur_aid = ffs(td->tm_sta_mask);
1553++
1554++ /* The actual tx count of MU packets will be pass to FW
1555++ * by a mcu command in testmode.
1556++ */
1557++ if (td->tx_rate_mode == MT76_TM_TX_MODE_HE_MU)
1558++ td->tx_pending = hweight16(phy->test.tm_sta_mask);
1559++ }
1560++
1561+ mt76_worker_schedule(&dev->tx_worker);
1562+ }
1563+
1564+@@ -291,7 +390,7 @@ mt76_testmode_tx_stop(struct mt76_phy *phy)
1565+ wait_event_timeout(dev->tx_wait, td->tx_done == td->tx_queued,
1566+ MT76_TM_TIMEOUT * HZ);
1567+
1568+- mt76_testmode_free_skb(phy);
1569++ mt76_testmode_free_skb_all(phy);
1570+ }
1571+
1572+ static inline void
1573+@@ -331,8 +430,11 @@ __mt76_testmode_set_state(struct mt76_phy *phy, enum mt76_testmode_state state)
1574+ struct mt76_dev *dev = phy->dev;
1575+ int err;
1576+
1577+- if (prev_state == MT76_TM_STATE_TX_FRAMES)
1578++ if (prev_state == MT76_TM_STATE_TX_FRAMES) {
1579++ if (phy->test.tx_rate_mode == MT76_TM_TX_MODE_HE_MU)
1580++ dev->test_ops->set_state(phy, MT76_TM_STATE_IDLE);
1581+ mt76_testmode_tx_stop(phy);
1582++ }
1583+
1584+ if (state == MT76_TM_STATE_TX_FRAMES) {
1585+ err = mt76_testmode_tx_init(phy);
1586+@@ -382,7 +484,6 @@ int mt76_testmode_set_state(struct mt76_phy *phy, enum mt76_testmode_state state
1587+ }
1588+
1589+ return __mt76_testmode_set_state(phy, state);
1590+-
1591+ }
1592+ EXPORT_SYMBOL(mt76_testmode_set_state);
1593+
1594+@@ -402,6 +503,44 @@ mt76_tm_get_u8(struct nlattr *attr, u8 *dest, u8 min, u8 max)
1595+ return 0;
1596+ }
1597+
1598++static int
1599++mt76_testmode_set_eeprom(struct mt76_phy *phy, struct nlattr **tb)
1600++{
1601++ struct mt76_dev *dev = phy->dev;
1602++ u8 action, val[MT76_TM_EEPROM_BLOCK_SIZE];
1603++ u32 offset = 0;
1604++ int err = -EINVAL;
1605++
1606++ if (!dev->test_ops->set_eeprom)
1607++ return -EOPNOTSUPP;
1608++
1609++ if (mt76_tm_get_u8(tb[MT76_TM_ATTR_EEPROM_ACTION], &action,
1610++ 0, MT76_TM_EEPROM_ACTION_MAX))
1611++ goto out;
1612++
1613++ if (tb[MT76_TM_ATTR_EEPROM_OFFSET]) {
1614++ struct nlattr *cur;
1615++ int rem, idx = 0;
1616++
1617++ offset = nla_get_u32(tb[MT76_TM_ATTR_EEPROM_OFFSET]);
1618++ if (!!(offset % MT76_TM_EEPROM_BLOCK_SIZE) ||
1619++ !tb[MT76_TM_ATTR_EEPROM_VAL])
1620++ goto out;
1621++
1622++ nla_for_each_nested(cur, tb[MT76_TM_ATTR_EEPROM_VAL], rem) {
1623++ if (nla_len(cur) != 1 || idx >= ARRAY_SIZE(val))
1624++ goto out;
1625++
1626++ val[idx++] = nla_get_u8(cur);
1627++ }
1628++ }
1629++
1630++ err = dev->test_ops->set_eeprom(phy, offset, val, action);
1631++
1632++out:
1633++ return err;
1634++}
1635++
1636+ int mt76_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
1637+ void *data, int len)
1638+ {
1639+@@ -425,6 +564,11 @@ int mt76_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
1640+
1641+ mutex_lock(&dev->mutex);
1642+
1643++ if (tb[MT76_TM_ATTR_EEPROM_ACTION]) {
1644++ err = mt76_testmode_set_eeprom(phy, tb);
1645++ goto out;
1646++ }
1647++
1648+ if (tb[MT76_TM_ATTR_RESET]) {
1649+ mt76_testmode_set_state(phy, MT76_TM_STATE_OFF);
1650+ memset(td, 0, sizeof(*td));
1651+@@ -446,13 +590,16 @@ int mt76_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
1652+ mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_LDPC], &td->tx_rate_ldpc, 0, 1) ||
1653+ mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_STBC], &td->tx_rate_stbc, 0, 1) ||
1654+ mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_LTF], &td->tx_ltf, 0, 2) ||
1655+- mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_ANTENNA],
1656+- &td->tx_antenna_mask, 0, 0xff) ||
1657++ mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_ANTENNA], &td->tx_antenna_mask,
1658++ 1, phy->antenna_mask) ||
1659+ mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_SPE_IDX], &td->tx_spe_idx, 0, 27) ||
1660+ mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_DUTY_CYCLE],
1661+ &td->tx_duty_cycle, 0, 99) ||
1662+ mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_POWER_CONTROL],
1663+- &td->tx_power_control, 0, 1))
1664++ &td->tx_power_control, 0, 1) ||
1665++ mt76_tm_get_u8(tb[MT76_TM_ATTR_AID], &td->aid, 0, 16) ||
1666++ mt76_tm_get_u8(tb[MT76_TM_ATTR_RU_ALLOC], &td->ru_alloc, 0, 0xff) ||
1667++ mt76_tm_get_u8(tb[MT76_TM_ATTR_RU_IDX], &td->ru_idx, 0, 68))
1668+ goto out;
1669+
1670+ if (tb[MT76_TM_ATTR_TX_LENGTH]) {
1671+@@ -484,8 +631,7 @@ int mt76_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
1672+
1673+ if (tb[MT76_TM_ATTR_TX_POWER]) {
1674+ struct nlattr *cur;
1675+- int idx = 0;
1676+- int rem;
1677++ int rem, idx = 0;
1678+
1679+ nla_for_each_nested(cur, tb[MT76_TM_ATTR_TX_POWER], rem) {
1680+ if (nla_len(cur) != 1 ||
1681+@@ -505,11 +651,47 @@ int mt76_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
1682+ if (nla_len(cur) != ETH_ALEN || idx >= 3)
1683+ goto out;
1684+
1685+- memcpy(td->addr[idx], nla_data(cur), ETH_ALEN);
1686++ memcpy(td->addr[idx++], nla_data(cur), ETH_ALEN);
1687++ }
1688++ }
1689++
1690++ if (tb[MT76_TM_ATTR_CFG]) {
1691++ struct nlattr *cur;
1692++ int rem, idx = 0;
1693++
1694++ nla_for_each_nested(cur, tb[MT76_TM_ATTR_CFG], rem) {
1695++ if (nla_len(cur) != 1 || idx >= 2)
1696++ goto out;
1697++
1698++ if (idx == 0)
1699++ td->cfg.type = nla_get_u8(cur);
1700++ else
1701++ td->cfg.enable = nla_get_u8(cur);
1702+ idx++;
1703+ }
1704+ }
1705+
1706++ if (tb[MT76_TM_ATTR_OFF_CH_SCAN_CH]) {
1707++ u8 ch = nla_get_u8(tb[MT76_TM_ATTR_OFF_CH_SCAN_CH]);
1708++ struct ieee80211_supported_band *sband;
1709++
1710++ sband = ch > 14 ? &phy->sband_5g.sband :
1711++ &phy->sband_2g.sband;
1712++ if (ch && (ch < sband->channels[0].hw_value ||
1713++ ch > sband->channels[sband->n_channels - 1].hw_value))
1714++ goto out;
1715++
1716++ td->off_ch_scan_ch = ch;
1717++
1718++ if (mt76_tm_get_u8(tb[MT76_TM_ATTR_OFF_CH_SCAN_CENTER_CH],
1719++ &td->off_ch_scan_center_ch, ch - 6, ch + 6) ||
1720++ mt76_tm_get_u8(tb[MT76_TM_ATTR_OFF_CH_SCAN_BW],
1721++ &td->off_ch_scan_bw, 0, 6) ||
1722++ mt76_tm_get_u8(tb[MT76_TM_ATTR_OFF_CH_SCAN_PATH],
1723++ &td->off_ch_scan_path, 1, 0xff))
1724++ goto out;
1725++ }
1726++
1727+ if (dev->test_ops->set_params) {
1728+ err = dev->test_ops->set_params(phy, tb, state);
1729+ if (err)
1730+@@ -559,6 +741,9 @@ mt76_testmode_dump_stats(struct mt76_phy *phy, struct sk_buff *msg)
1731+ nla_put_u64_64bit(msg, MT76_TM_STATS_ATTR_RX_PACKETS, rx_packets,
1732+ MT76_TM_STATS_ATTR_PAD) ||
1733+ nla_put_u64_64bit(msg, MT76_TM_STATS_ATTR_RX_FCS_ERROR, rx_fcs_error,
1734++ MT76_TM_STATS_ATTR_PAD) ||
1735++ nla_put_u64_64bit(msg, MT76_TM_STATS_ATTR_RX_LEN_MISMATCH,
1736++ td->rx_stats.len_mismatch,
1737+ MT76_TM_STATS_ATTR_PAD))
1738+ return -EMSGSIZE;
1739+
1740+@@ -571,6 +756,7 @@ int mt76_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *msg,
1741+ struct mt76_phy *phy = hw->priv;
1742+ struct mt76_dev *dev = phy->dev;
1743+ struct mt76_testmode_data *td = &phy->test;
1744++ struct mt76_testmode_sta_data *sd = &td->sd;
1745+ struct nlattr *tb[NUM_MT76_TM_ATTRS] = {};
1746+ int err = 0;
1747+ void *a;
1748+@@ -603,6 +789,23 @@ int mt76_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *msg,
1749+ goto out;
1750+ }
1751+
1752++ if (tb[MT76_TM_ATTR_AID]) {
1753++ struct mt76_testmode_sta *tm_sta;
1754++ u8 aid;
1755++
1756++ err = mt76_tm_get_u8(tb[MT76_TM_ATTR_AID], &aid, 1, 16);
1757++ if (err)
1758++ goto out;
1759++
1760++ tm_sta = mt76_testmode_aid_get_sta(phy, aid);
1761++ if (!tm_sta) {
1762++ err = -EINVAL;
1763++ goto out;
1764++ }
1765++
1766++ sd = &tm_sta->sd;
1767++ }
1768++
1769+ mt76_testmode_init_defaults(phy);
1770+
1771+ err = -EMSGSIZE;
1772+@@ -615,12 +818,8 @@ int mt76_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *msg,
1773+ goto out;
1774+
1775+ if (nla_put_u32(msg, MT76_TM_ATTR_TX_COUNT, td->tx_count) ||
1776+- nla_put_u32(msg, MT76_TM_ATTR_TX_LENGTH, td->tx_mpdu_len) ||
1777+ nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_MODE, td->tx_rate_mode) ||
1778+- nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_NSS, td->tx_rate_nss) ||
1779+- nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_IDX, td->tx_rate_idx) ||
1780+ nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_SGI, td->tx_rate_sgi) ||
1781+- nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_LDPC, td->tx_rate_ldpc) ||
1782+ nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_STBC, td->tx_rate_stbc) ||
1783+ (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_LTF) &&
1784+ nla_put_u8(msg, MT76_TM_ATTR_TX_LTF, td->tx_ltf)) ||
1785+@@ -640,6 +839,15 @@ int mt76_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *msg,
1786+ nla_put_u8(msg, MT76_TM_ATTR_FREQ_OFFSET, td->freq_offset)))
1787+ goto out;
1788+
1789++ if (nla_put_u8(msg, MT76_TM_ATTR_AID, sd->aid) ||
1790++ nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_NSS, sd->tx_rate_nss) ||
1791++ nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_IDX, sd->tx_rate_idx) ||
1792++ nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_LDPC, sd->tx_rate_ldpc) ||
1793++ nla_put_u8(msg, MT76_TM_ATTR_RU_ALLOC, sd->ru_alloc) ||
1794++ nla_put_u8(msg, MT76_TM_ATTR_RU_IDX, sd->ru_idx) ||
1795++ nla_put_u32(msg, MT76_TM_ATTR_TX_LENGTH, sd->tx_mpdu_len))
1796++ goto out;
1797++
1798+ if (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_POWER)) {
1799+ a = nla_nest_start(msg, MT76_TM_ATTR_TX_POWER);
1800+ if (!a)
1801+diff --git a/testmode.h b/testmode.h
developer92086052022-03-17 16:55:20 +08001802+index 5e2792d8..b360d7a4 100644
developer5f11e9e2022-03-10 15:03:47 +08001803+--- a/testmode.h
1804++++ b/testmode.h
1805+@@ -6,6 +6,8 @@
1806+ #define __MT76_TESTMODE_H
1807+
1808+ #define MT76_TM_TIMEOUT 10
1809++#define MT76_TM_EEPROM_BLOCK_SIZE 16
1810++#define MT76_TM_MAX_STA_NUM 16
1811+
1812+ /**
1813+ * enum mt76_testmode_attr - testmode attributes inside NL80211_ATTR_TESTDATA
1814+@@ -47,6 +49,20 @@
1815+ * @MT76_TM_ATTR_DRV_DATA: driver specific netlink attrs (nested)
1816+ *
1817+ * @MT76_TM_ATTR_MAC_ADDRS: array of nested MAC addresses (nested)
1818++ *
1819++ * @MT76_TM_ATTR_EEPROM_ACTION: eeprom setting actions
1820++ * (u8, see &enum mt76_testmode_eeprom_action)
1821++ * @MT76_TM_ATTR_EEPROM_OFFSET: offset of eeprom data block for writing (u32)
1822++ * @MT76_TM_ATTR_EEPROM_VAL: values for writing into a 16-byte data block
1823++ * (nested, u8 attrs)
1824++ *
1825++ * @MT76_TM_ATTR_CFG: config testmode rf feature (nested, see &mt76_testmode_cfg)
1826++ *
1827++ * @MT76_TM_ATTR_OFF_CH_SCAN_CH: monitored channel for off channel scan (u8)
1828++ * @MT76_TM_ATTR_OFF_CH_SCAN_CENTER_CH: monitored channel for off channel scan (u8)
1829++ * @MT76_TM_ATTR_OFF_CH_SCAN_BW: monitored bw for off channel scan (u8)
1830++ * @MT76_TM_ATTR_OFF_CH_SCAN_PATH: monitored rx path for off channel scan (u8)
1831++ *
1832+ */
1833+ enum mt76_testmode_attr {
1834+ MT76_TM_ATTR_UNSPEC,
1835+@@ -85,6 +101,21 @@ enum mt76_testmode_attr {
1836+
1837+ MT76_TM_ATTR_MAC_ADDRS,
1838+
1839++ MT76_TM_ATTR_EEPROM_ACTION,
1840++ MT76_TM_ATTR_EEPROM_OFFSET,
1841++ MT76_TM_ATTR_EEPROM_VAL,
1842++
1843++ MT76_TM_ATTR_CFG,
1844++
1845++ MT76_TM_ATTR_OFF_CH_SCAN_CH,
1846++ MT76_TM_ATTR_OFF_CH_SCAN_CENTER_CH,
1847++ MT76_TM_ATTR_OFF_CH_SCAN_BW,
1848++ MT76_TM_ATTR_OFF_CH_SCAN_PATH,
1849++
1850++ MT76_TM_ATTR_AID,
1851++ MT76_TM_ATTR_RU_ALLOC,
1852++ MT76_TM_ATTR_RU_IDX,
1853++
1854+ /* keep last */
1855+ NUM_MT76_TM_ATTRS,
1856+ MT76_TM_ATTR_MAX = NUM_MT76_TM_ATTRS - 1,
1857+@@ -101,6 +132,8 @@ enum mt76_testmode_attr {
1858+ * @MT76_TM_STATS_ATTR_RX_FCS_ERROR: number of rx packets with FCS error (u64)
1859+ * @MT76_TM_STATS_ATTR_LAST_RX: information about the last received packet
1860+ * see &enum mt76_testmode_rx_attr
1861++ * @MT76_TM_STATS_ATTR_RX_LEN_MISMATCH: number of rx packets with length
1862++ * mismatch error (u64)
1863+ */
1864+ enum mt76_testmode_stats_attr {
1865+ MT76_TM_STATS_ATTR_UNSPEC,
1866+@@ -113,6 +146,7 @@ enum mt76_testmode_stats_attr {
1867+ MT76_TM_STATS_ATTR_RX_PACKETS,
1868+ MT76_TM_STATS_ATTR_RX_FCS_ERROR,
1869+ MT76_TM_STATS_ATTR_LAST_RX,
1870++ MT76_TM_STATS_ATTR_RX_LEN_MISMATCH,
1871+
1872+ /* keep last */
1873+ NUM_MT76_TM_STATS_ATTRS,
1874+@@ -195,4 +229,41 @@ enum mt76_testmode_tx_mode {
1875+
1876+ extern const struct nla_policy mt76_tm_policy[NUM_MT76_TM_ATTRS];
1877+
1878++/**
1879++ * enum mt76_testmode_eeprom_action - eeprom setting actions
1880++ *
1881++ * @MT76_TM_EEPROM_ACTION_UPDATE_DATA: update rf values to specific
1882++ * eeprom data block
1883++ * @MT76_TM_EEPROM_ACTION_UPDATE_BUFFER_MODE: send updated eeprom data to fw
1884++ * @MT76_TM_EEPROM_ACTION_WRITE_TO_EFUSE: write eeprom data back to efuse
1885++ */
1886++enum mt76_testmode_eeprom_action {
1887++ MT76_TM_EEPROM_ACTION_UPDATE_DATA,
1888++ MT76_TM_EEPROM_ACTION_UPDATE_BUFFER_MODE,
1889++ MT76_TM_EEPROM_ACTION_WRITE_TO_EFUSE,
1890++
1891++ /* keep last */
1892++ NUM_MT76_TM_EEPROM_ACTION,
1893++ MT76_TM_EEPROM_ACTION_MAX = NUM_MT76_TM_EEPROM_ACTION - 1,
1894++};
1895++
1896++/**
1897++ * enum mt76_testmode_cfg - packet tx phy mode
1898++ *
1899++ * @MT76_TM_EEPROM_ACTION_UPDATE_DATA: update rf values to specific
1900++ * eeprom data block
1901++ * @MT76_TM_EEPROM_ACTION_UPDATE_BUFFER_MODE: send updated eeprom data to fw
1902++ * @MT76_TM_EEPROM_ACTION_WRITE_TO_EFUSE: write eeprom data back to efuse
1903++ */
1904++enum mt76_testmode_cfg {
1905++ MT76_TM_CFG_TSSI,
1906++ MT76_TM_CFG_DPD,
1907++ MT76_TM_CFG_RATE_POWER_OFFSET,
1908++ MT76_TM_CFG_THERMAL_COMP,
1909++
1910++ /* keep last */
1911++ NUM_MT76_TM_CFG,
1912++ MT76_TM_CFG_MAX = NUM_MT76_TM_CFG - 1,
1913++};
1914++
1915+ #endif
1916+diff --git a/tools/fields.c b/tools/fields.c
developer92086052022-03-17 16:55:20 +08001917+index e3f69089..036406c1 100644
developer5f11e9e2022-03-10 15:03:47 +08001918+--- a/tools/fields.c
1919++++ b/tools/fields.c
1920+@@ -10,6 +10,7 @@ static const char * const testmode_state[] = {
1921+ [MT76_TM_STATE_IDLE] = "idle",
1922+ [MT76_TM_STATE_TX_FRAMES] = "tx_frames",
1923+ [MT76_TM_STATE_RX_FRAMES] = "rx_frames",
1924++ [MT76_TM_STATE_TX_CONT] = "tx_cont",
1925+ };
1926+
1927+ static const char * const testmode_tx_mode[] = {
1928+@@ -201,6 +202,63 @@ static void print_extra_stats(const struct tm_field *field, struct nlattr **tb)
1929+ printf("%srx_per=%.02f%%\n", prefix, 100 * failed / total);
1930+ }
1931+
1932++static bool parse_mac(const struct tm_field *field, int idx,
1933++ struct nl_msg *msg, const char *val)
1934++{
1935++#define ETH_ALEN 6
1936++ bool ret = true;
1937++ char *str, *cur, *ap;
1938++ void *a;
1939++
1940++ ap = str = strdup(val);
1941++
1942++ a = nla_nest_start(msg, idx);
1943++
1944++ idx = 0;
1945++ while ((cur = strsep(&ap, ",")) != NULL) {
1946++ unsigned char addr[ETH_ALEN];
1947++ char *val, *tmp = cur;
1948++ int i = 0;
1949++
1950++ while ((val = strsep(&tmp, ":")) != NULL) {
1951++ if (i >= ETH_ALEN)
1952++ break;
1953++
1954++ addr[i++] = strtoul(val, NULL, 16);
1955++ }
1956++
1957++ nla_put(msg, idx, ETH_ALEN, addr);
1958++
1959++ idx++;
1960++ }
1961++
1962++ nla_nest_end(msg, a);
1963++
1964++ free(str);
1965++
1966++ return ret;
1967++}
1968++
1969++static void print_mac(const struct tm_field *field, struct nlattr *attr)
1970++{
1971++#define MAC2STR(a) (a)[0], (a)[1], (a)[2], (a)[3], (a)[4], (a)[5]
1972++#define MACSTR "%02x:%02x:%02x:%02x:%02x:%02x"
1973++ unsigned char addr[3][6];
1974++ struct nlattr *cur;
1975++ int idx = 0;
1976++ int rem;
1977++
1978++ nla_for_each_nested(cur, attr, rem) {
1979++ if (nla_len(cur) != 6)
1980++ continue;
1981++ memcpy(addr[idx++], nla_data(cur), 6);
1982++ }
1983++
1984++ printf("" MACSTR "," MACSTR "," MACSTR "",
1985++ MAC2STR(addr[0]), MAC2STR(addr[1]), MAC2STR(addr[2]));
1986++
1987++ return;
1988++}
1989+
1990+ #define FIELD_GENERIC(_field, _name, ...) \
1991+ [FIELD_NAME(_field)] = { \
1992+@@ -250,6 +308,13 @@ static void print_extra_stats(const struct tm_field *field, struct nlattr **tb)
1993+ ##__VA_ARGS__ \
1994+ )
1995+
1996++#define FIELD_MAC(_field, _name) \
1997++ [FIELD_NAME(_field)] = { \
1998++ .name = _name, \
1999++ .parse = parse_mac, \
2000++ .print = print_mac \
2001++ }
2002++
2003+ #define FIELD_NAME(_field) MT76_TM_RX_ATTR_##_field
2004+ static const struct tm_field rx_fields[NUM_MT76_TM_RX_ATTRS] = {
2005+ FIELD_RO(s32, FREQ_OFFSET, "freq_offset"),
2006+@@ -300,10 +365,16 @@ static const struct tm_field testdata_fields[NUM_MT76_TM_ATTRS] = {
2007+ FIELD(u8, TX_RATE_LDPC, "tx_rate_ldpc"),
2008+ FIELD(u8, TX_RATE_STBC, "tx_rate_stbc"),
2009+ FIELD(u8, TX_LTF, "tx_ltf"),
2010++ FIELD(u8, TX_DUTY_CYCLE, "tx_duty_cycle"),
2011++ FIELD(u32, TX_IPG, "tx_ipg"),
2012++ FIELD(u32, TX_TIME, "tx_time"),
2013+ FIELD(u8, TX_POWER_CONTROL, "tx_power_control"),
2014+ FIELD_ARRAY(u8, TX_POWER, "tx_power"),
2015+ FIELD(u8, TX_ANTENNA, "tx_antenna"),
2016++ FIELD(u8, TX_SPE_IDX, "tx_spe_idx"),
2017+ FIELD(u32, FREQ_OFFSET, "freq_offset"),
2018++ FIELD(u8, AID, "aid"),
2019++ FIELD_MAC(MAC_ADDRS, "mac_addrs"),
2020+ FIELD_NESTED_RO(STATS, stats, "",
2021+ .print_extra = print_extra_stats),
2022+ };
2023+@@ -322,9 +393,14 @@ static struct nla_policy testdata_policy[NUM_MT76_TM_ATTRS] = {
2024+ [MT76_TM_ATTR_TX_RATE_LDPC] = { .type = NLA_U8 },
2025+ [MT76_TM_ATTR_TX_RATE_STBC] = { .type = NLA_U8 },
2026+ [MT76_TM_ATTR_TX_LTF] = { .type = NLA_U8 },
2027++ [MT76_TM_ATTR_TX_DUTY_CYCLE] = { .type = NLA_U8 },
2028++ [MT76_TM_ATTR_TX_IPG] = { .type = NLA_U32 },
2029++ [MT76_TM_ATTR_TX_TIME] = { .type = NLA_U32 },
2030+ [MT76_TM_ATTR_TX_POWER_CONTROL] = { .type = NLA_U8 },
2031+ [MT76_TM_ATTR_TX_ANTENNA] = { .type = NLA_U8 },
2032++ [MT76_TM_ATTR_TX_SPE_IDX] = { .type = NLA_U8 },
2033+ [MT76_TM_ATTR_FREQ_OFFSET] = { .type = NLA_U32 },
2034++ [MT76_TM_ATTR_AID] = { .type = NLA_U8 },
2035+ [MT76_TM_ATTR_STATS] = { .type = NLA_NESTED },
2036+ };
2037+
2038+diff --git a/tx.c b/tx.c
developer92086052022-03-17 16:55:20 +08002039+index 6b8c9dc8..ca5e6d97 100644
developer5f11e9e2022-03-10 15:03:47 +08002040+--- a/tx.c
2041++++ b/tx.c
2042+@@ -245,8 +245,7 @@ void __mt76_tx_complete_skb(struct mt76_dev *dev, u16 wcid_idx, struct sk_buff *
2043+ if (mt76_is_testmode_skb(dev, skb, &hw)) {
2044+ struct mt76_phy *phy = hw->priv;
2045+
2046+- if (skb == phy->test.tx_skb)
2047+- phy->test.tx_done++;
2048++ phy->test.tx_done++;
2049+ if (phy->test.tx_queued == phy->test.tx_done)
2050+ wake_up(&dev->tx_wait);
2051+
2052+--
2053+2.25.1
2054+