1 // SPDX-License-Identifier: ISC
2 /* Copyright (C) 2020 Felix Fietkau <nbd@nbd.name> */
3
4 #include <linux/random.h>
5 #include "mt76.h"
6
7 const struct nla_policy mt76_tm_policy[NUM_MT76_TM_ATTRS] = {
8 [MT76_TM_ATTR_RESET] = { .type = NLA_FLAG },
9 [MT76_TM_ATTR_STATE] = { .type = NLA_U8 },
10 [MT76_TM_ATTR_TX_COUNT] = { .type = NLA_U32 },
11 [MT76_TM_ATTR_TX_LENGTH] = { .type = NLA_U32 },
12 [MT76_TM_ATTR_TX_RATE_MODE] = { .type = NLA_U8 },
13 [MT76_TM_ATTR_TX_RATE_NSS] = { .type = NLA_U8 },
14 [MT76_TM_ATTR_TX_RATE_IDX] = { .type = NLA_U8 },
15 [MT76_TM_ATTR_TX_RATE_SGI] = { .type = NLA_U8 },
16 [MT76_TM_ATTR_TX_RATE_LDPC] = { .type = NLA_U8 },
17 [MT76_TM_ATTR_TX_RATE_STBC] = { .type = NLA_U8 },
18 [MT76_TM_ATTR_TX_LTF] = { .type = NLA_U8 },
19 [MT76_TM_ATTR_TX_ANTENNA] = { .type = NLA_U8 },
20 [MT76_TM_ATTR_TX_SPE_IDX] = { .type = NLA_U8 },
21 [MT76_TM_ATTR_TX_POWER_CONTROL] = { .type = NLA_U8 },
22 [MT76_TM_ATTR_TX_POWER] = { .type = NLA_NESTED },
23 [MT76_TM_ATTR_TX_DUTY_CYCLE] = { .type = NLA_U8 },
24 [MT76_TM_ATTR_TX_IPG] = { .type = NLA_U32 },
25 [MT76_TM_ATTR_TX_TIME] = { .type = NLA_U32 },
26 [MT76_TM_ATTR_FREQ_OFFSET] = { .type = NLA_U32 },
27 [MT76_TM_ATTR_DRV_DATA] = { .type = NLA_NESTED },
28 };
29 EXPORT_SYMBOL_GPL(mt76_tm_policy);
30
mt76_testmode_tx_pending(struct mt76_phy * phy)31 void mt76_testmode_tx_pending(struct mt76_phy *phy)
32 {
33 struct mt76_testmode_data *td = &phy->test;
34 struct mt76_dev *dev = phy->dev;
35 struct mt76_wcid *wcid = &dev->global_wcid;
36 struct sk_buff *skb = td->tx_skb;
37 struct mt76_queue *q;
38 u16 tx_queued_limit;
39 int qid;
40
41 if (!skb || !td->tx_pending)
42 return;
43
44 qid = skb_get_queue_mapping(skb);
45 q = phy->q_tx[qid];
46
47 tx_queued_limit = td->tx_queued_limit ? td->tx_queued_limit : 1000;
48
49 spin_lock_bh(&q->lock);
50
51 while (td->tx_pending > 0 &&
52 td->tx_queued - td->tx_done < tx_queued_limit &&
53 q->queued < q->ndesc / 2) {
54 int ret;
55
56 ret = dev->queue_ops->tx_queue_skb(dev, q, qid, skb_get(skb),
57 wcid, NULL);
58 if (ret < 0)
59 break;
60
61 td->tx_pending--;
62 td->tx_queued++;
63 }
64
65 dev->queue_ops->kick(dev, q);
66
67 spin_unlock_bh(&q->lock);
68 }
69
70 static u32
mt76_testmode_max_mpdu_len(struct mt76_phy * phy,u8 tx_rate_mode)71 mt76_testmode_max_mpdu_len(struct mt76_phy *phy, u8 tx_rate_mode)
72 {
73 switch (tx_rate_mode) {
74 case MT76_TM_TX_MODE_HT:
75 return IEEE80211_MAX_MPDU_LEN_HT_7935;
76 case MT76_TM_TX_MODE_VHT:
77 case MT76_TM_TX_MODE_HE_SU:
78 case MT76_TM_TX_MODE_HE_EXT_SU:
79 case MT76_TM_TX_MODE_HE_TB:
80 case MT76_TM_TX_MODE_HE_MU:
81 if (phy->sband_5g.sband.vht_cap.cap &
82 IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_7991)
83 return IEEE80211_MAX_MPDU_LEN_VHT_7991;
84 return IEEE80211_MAX_MPDU_LEN_VHT_11454;
85 case MT76_TM_TX_MODE_CCK:
86 case MT76_TM_TX_MODE_OFDM:
87 default:
88 return IEEE80211_MAX_FRAME_LEN;
89 }
90 }
91
92 static void
mt76_testmode_free_skb(struct mt76_phy * phy)93 mt76_testmode_free_skb(struct mt76_phy *phy)
94 {
95 struct mt76_testmode_data *td = &phy->test;
96
97 dev_kfree_skb(td->tx_skb);
98 td->tx_skb = NULL;
99 }
100
mt76_testmode_alloc_skb(struct mt76_phy * phy,u32 len)101 int mt76_testmode_alloc_skb(struct mt76_phy *phy, u32 len)
102 {
103 #define MT_TXP_MAX_LEN 4095
104 u16 fc = IEEE80211_FTYPE_DATA | IEEE80211_STYPE_DATA |
105 IEEE80211_FCTL_FROMDS;
106 struct mt76_testmode_data *td = &phy->test;
107 struct sk_buff **frag_tail, *head;
108 struct ieee80211_tx_info *info;
109 struct ieee80211_hdr *hdr;
110 u32 max_len, head_len;
111 int nfrags, i;
112
113 max_len = mt76_testmode_max_mpdu_len(phy, td->tx_rate_mode);
114 if (len > max_len)
115 len = max_len;
116 else if (len < sizeof(struct ieee80211_hdr))
117 len = sizeof(struct ieee80211_hdr);
118
119 nfrags = len / MT_TXP_MAX_LEN;
120 head_len = nfrags ? MT_TXP_MAX_LEN : len;
121
122 if (len > IEEE80211_MAX_FRAME_LEN)
123 fc |= IEEE80211_STYPE_QOS_DATA;
124
125 head = alloc_skb(head_len, GFP_KERNEL);
126 if (!head)
127 return -ENOMEM;
128
129 hdr = __skb_put_zero(head, sizeof(*hdr));
130 hdr->frame_control = cpu_to_le16(fc);
131 memcpy(hdr->addr1, td->addr[0], ETH_ALEN);
132 memcpy(hdr->addr2, td->addr[1], ETH_ALEN);
133 memcpy(hdr->addr3, td->addr[2], ETH_ALEN);
134 skb_set_queue_mapping(head, IEEE80211_AC_BE);
135 get_random_bytes(__skb_put(head, head_len - sizeof(*hdr)),
136 head_len - sizeof(*hdr));
137
138 info = IEEE80211_SKB_CB(head);
139 info->flags = IEEE80211_TX_CTL_INJECTED |
140 IEEE80211_TX_CTL_NO_ACK |
141 IEEE80211_TX_CTL_NO_PS_BUFFER;
142
143 info->hw_queue |= FIELD_PREP(MT_TX_HW_QUEUE_PHY, phy->band_idx);
144 frag_tail = &skb_shinfo(head)->frag_list;
145
146 for (i = 0; i < nfrags; i++) {
147 struct sk_buff *frag;
148 u16 frag_len;
149
150 if (i == nfrags - 1)
151 frag_len = len % MT_TXP_MAX_LEN;
152 else
153 frag_len = MT_TXP_MAX_LEN;
154
155 frag = alloc_skb(frag_len, GFP_KERNEL);
156 if (!frag) {
157 mt76_testmode_free_skb(phy);
158 dev_kfree_skb(head);
159 return -ENOMEM;
160 }
161
162 get_random_bytes(__skb_put(frag, frag_len), frag_len);
163 head->len += frag->len;
164 head->data_len += frag->len;
165
166 *frag_tail = frag;
167 frag_tail = &(*frag_tail)->next;
168 }
169
170 mt76_testmode_free_skb(phy);
171 td->tx_skb = head;
172
173 return 0;
174 }
175 EXPORT_SYMBOL(mt76_testmode_alloc_skb);
176
177 static int
mt76_testmode_tx_init(struct mt76_phy * phy)178 mt76_testmode_tx_init(struct mt76_phy *phy)
179 {
180 struct mt76_testmode_data *td = &phy->test;
181 struct ieee80211_tx_info *info;
182 struct ieee80211_tx_rate *rate;
183 u8 max_nss = hweight8(phy->antenna_mask);
184 int ret;
185
186 ret = mt76_testmode_alloc_skb(phy, td->tx_mpdu_len);
187 if (ret)
188 return ret;
189
190 if (td->tx_rate_mode > MT76_TM_TX_MODE_VHT)
191 goto out;
192
193 if (td->tx_antenna_mask)
194 max_nss = min_t(u8, max_nss, hweight8(td->tx_antenna_mask));
195
196 info = IEEE80211_SKB_CB(td->tx_skb);
197 rate = &info->control.rates[0];
198 rate->count = 1;
199 rate->idx = td->tx_rate_idx;
200
201 switch (td->tx_rate_mode) {
202 case MT76_TM_TX_MODE_CCK:
203 if (phy->chandef.chan->band != NL80211_BAND_2GHZ)
204 return -EINVAL;
205
206 if (rate->idx > 4)
207 return -EINVAL;
208 break;
209 case MT76_TM_TX_MODE_OFDM:
210 if (phy->chandef.chan->band != NL80211_BAND_2GHZ)
211 break;
212
213 if (rate->idx > 8)
214 return -EINVAL;
215
216 rate->idx += 4;
217 break;
218 case MT76_TM_TX_MODE_HT:
219 if (rate->idx > 8 * max_nss &&
220 !(rate->idx == 32 &&
221 phy->chandef.width >= NL80211_CHAN_WIDTH_40))
222 return -EINVAL;
223
224 rate->flags |= IEEE80211_TX_RC_MCS;
225 break;
226 case MT76_TM_TX_MODE_VHT:
227 if (rate->idx > 9)
228 return -EINVAL;
229
230 if (td->tx_rate_nss > max_nss)
231 return -EINVAL;
232
233 ieee80211_rate_set_vht(rate, td->tx_rate_idx, td->tx_rate_nss);
234 rate->flags |= IEEE80211_TX_RC_VHT_MCS;
235 break;
236 default:
237 break;
238 }
239
240 if (td->tx_rate_sgi)
241 rate->flags |= IEEE80211_TX_RC_SHORT_GI;
242
243 if (td->tx_rate_ldpc)
244 info->flags |= IEEE80211_TX_CTL_LDPC;
245
246 if (td->tx_rate_stbc)
247 info->flags |= IEEE80211_TX_CTL_STBC;
248
249 if (td->tx_rate_mode >= MT76_TM_TX_MODE_HT) {
250 switch (phy->chandef.width) {
251 case NL80211_CHAN_WIDTH_40:
252 rate->flags |= IEEE80211_TX_RC_40_MHZ_WIDTH;
253 break;
254 case NL80211_CHAN_WIDTH_80:
255 rate->flags |= IEEE80211_TX_RC_80_MHZ_WIDTH;
256 break;
257 case NL80211_CHAN_WIDTH_80P80:
258 case NL80211_CHAN_WIDTH_160:
259 rate->flags |= IEEE80211_TX_RC_160_MHZ_WIDTH;
260 break;
261 default:
262 break;
263 }
264 }
265 out:
266 return 0;
267 }
268
269 static void
mt76_testmode_tx_start(struct mt76_phy * phy)270 mt76_testmode_tx_start(struct mt76_phy *phy)
271 {
272 struct mt76_testmode_data *td = &phy->test;
273 struct mt76_dev *dev = phy->dev;
274
275 td->tx_queued = 0;
276 td->tx_done = 0;
277 td->tx_pending = td->tx_count;
278 mt76_worker_schedule(&dev->tx_worker);
279 }
280
281 static void
mt76_testmode_tx_stop(struct mt76_phy * phy)282 mt76_testmode_tx_stop(struct mt76_phy *phy)
283 {
284 struct mt76_testmode_data *td = &phy->test;
285 struct mt76_dev *dev = phy->dev;
286
287 mt76_worker_disable(&dev->tx_worker);
288
289 td->tx_pending = 0;
290
291 mt76_worker_enable(&dev->tx_worker);
292
293 wait_event_timeout(dev->tx_wait, td->tx_done == td->tx_queued,
294 MT76_TM_TIMEOUT * HZ);
295
296 mt76_testmode_free_skb(phy);
297 }
298
299 static inline void
mt76_testmode_param_set(struct mt76_testmode_data * td,u16 idx)300 mt76_testmode_param_set(struct mt76_testmode_data *td, u16 idx)
301 {
302 td->param_set[idx / 32] |= BIT(idx % 32);
303 }
304
305 static inline bool
mt76_testmode_param_present(struct mt76_testmode_data * td,u16 idx)306 mt76_testmode_param_present(struct mt76_testmode_data *td, u16 idx)
307 {
308 return td->param_set[idx / 32] & BIT(idx % 32);
309 }
310
311 static void
mt76_testmode_init_defaults(struct mt76_phy * phy)312 mt76_testmode_init_defaults(struct mt76_phy *phy)
313 {
314 struct mt76_testmode_data *td = &phy->test;
315
316 if (td->tx_mpdu_len > 0)
317 return;
318
319 td->tx_mpdu_len = 1024;
320 td->tx_count = 1;
321 td->tx_rate_mode = MT76_TM_TX_MODE_OFDM;
322 td->tx_rate_nss = 1;
323
324 memcpy(td->addr[0], phy->macaddr, ETH_ALEN);
325 memcpy(td->addr[1], phy->macaddr, ETH_ALEN);
326 memcpy(td->addr[2], phy->macaddr, ETH_ALEN);
327 }
328
329 static int
__mt76_testmode_set_state(struct mt76_phy * phy,enum mt76_testmode_state state)330 __mt76_testmode_set_state(struct mt76_phy *phy, enum mt76_testmode_state state)
331 {
332 enum mt76_testmode_state prev_state = phy->test.state;
333 struct mt76_dev *dev = phy->dev;
334 int err;
335
336 if (prev_state == MT76_TM_STATE_TX_FRAMES)
337 mt76_testmode_tx_stop(phy);
338
339 if (state == MT76_TM_STATE_TX_FRAMES) {
340 err = mt76_testmode_tx_init(phy);
341 if (err)
342 return err;
343 }
344
345 err = dev->test_ops->set_state(phy, state);
346 if (err) {
347 if (state == MT76_TM_STATE_TX_FRAMES)
348 mt76_testmode_tx_stop(phy);
349
350 return err;
351 }
352
353 if (state == MT76_TM_STATE_TX_FRAMES)
354 mt76_testmode_tx_start(phy);
355 else if (state == MT76_TM_STATE_RX_FRAMES) {
356 memset(&phy->test.rx_stats, 0, sizeof(phy->test.rx_stats));
357 }
358
359 phy->test.state = state;
360
361 return 0;
362 }
363
mt76_testmode_set_state(struct mt76_phy * phy,enum mt76_testmode_state state)364 int mt76_testmode_set_state(struct mt76_phy *phy, enum mt76_testmode_state state)
365 {
366 struct mt76_testmode_data *td = &phy->test;
367 struct ieee80211_hw *hw = phy->hw;
368
369 if (state == td->state && state == MT76_TM_STATE_OFF)
370 return 0;
371
372 if (state > MT76_TM_STATE_OFF &&
373 (!test_bit(MT76_STATE_RUNNING, &phy->state) ||
374 !(hw->conf.flags & IEEE80211_CONF_MONITOR)))
375 return -ENOTCONN;
376
377 if (state != MT76_TM_STATE_IDLE &&
378 td->state != MT76_TM_STATE_IDLE) {
379 int ret;
380
381 ret = __mt76_testmode_set_state(phy, MT76_TM_STATE_IDLE);
382 if (ret)
383 return ret;
384 }
385
386 return __mt76_testmode_set_state(phy, state);
387
388 }
389 EXPORT_SYMBOL(mt76_testmode_set_state);
390
391 static int
mt76_tm_get_u8(struct nlattr * attr,u8 * dest,u8 min,u8 max)392 mt76_tm_get_u8(struct nlattr *attr, u8 *dest, u8 min, u8 max)
393 {
394 u8 val;
395
396 if (!attr)
397 return 0;
398
399 val = nla_get_u8(attr);
400 if (val < min || val > max)
401 return -EINVAL;
402
403 *dest = val;
404 return 0;
405 }
406
mt76_testmode_cmd(struct ieee80211_hw * hw,struct ieee80211_vif * vif,void * data,int len)407 int mt76_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
408 void *data, int len)
409 {
410 struct mt76_phy *phy = hw->priv;
411 struct mt76_dev *dev = phy->dev;
412 struct mt76_testmode_data *td = &phy->test;
413 struct nlattr *tb[NUM_MT76_TM_ATTRS];
414 u32 state;
415 int err;
416 int i;
417
418 if (!dev->test_ops)
419 return -EOPNOTSUPP;
420
421 err = nla_parse_deprecated(tb, MT76_TM_ATTR_MAX, data, len,
422 mt76_tm_policy, NULL);
423 if (err)
424 return err;
425
426 err = -EINVAL;
427
428 mutex_lock(&dev->mutex);
429
430 if (tb[MT76_TM_ATTR_RESET]) {
431 mt76_testmode_set_state(phy, MT76_TM_STATE_OFF);
432 memset(td, 0, sizeof(*td));
433 }
434
435 mt76_testmode_init_defaults(phy);
436
437 if (tb[MT76_TM_ATTR_TX_COUNT])
438 td->tx_count = nla_get_u32(tb[MT76_TM_ATTR_TX_COUNT]);
439
440 if (tb[MT76_TM_ATTR_TX_RATE_IDX])
441 td->tx_rate_idx = nla_get_u8(tb[MT76_TM_ATTR_TX_RATE_IDX]);
442
443 if (mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_MODE], &td->tx_rate_mode,
444 0, MT76_TM_TX_MODE_MAX) ||
445 mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_NSS], &td->tx_rate_nss,
446 1, hweight8(phy->antenna_mask)) ||
447 mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_SGI], &td->tx_rate_sgi, 0, 2) ||
448 mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_LDPC], &td->tx_rate_ldpc, 0, 1) ||
449 mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_STBC], &td->tx_rate_stbc, 0, 1) ||
450 mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_LTF], &td->tx_ltf, 0, 2) ||
451 mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_ANTENNA],
452 &td->tx_antenna_mask, 0, 0xff) ||
453 mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_SPE_IDX], &td->tx_spe_idx, 0, 27) ||
454 mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_DUTY_CYCLE],
455 &td->tx_duty_cycle, 0, 99) ||
456 mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_POWER_CONTROL],
457 &td->tx_power_control, 0, 1))
458 goto out;
459
460 if (tb[MT76_TM_ATTR_TX_LENGTH]) {
461 u32 val = nla_get_u32(tb[MT76_TM_ATTR_TX_LENGTH]);
462
463 if (val > mt76_testmode_max_mpdu_len(phy, td->tx_rate_mode) ||
464 val < sizeof(struct ieee80211_hdr))
465 goto out;
466
467 td->tx_mpdu_len = val;
468 }
469
470 if (tb[MT76_TM_ATTR_TX_IPG])
471 td->tx_ipg = nla_get_u32(tb[MT76_TM_ATTR_TX_IPG]);
472
473 if (tb[MT76_TM_ATTR_TX_TIME])
474 td->tx_time = nla_get_u32(tb[MT76_TM_ATTR_TX_TIME]);
475
476 if (tb[MT76_TM_ATTR_FREQ_OFFSET])
477 td->freq_offset = nla_get_u32(tb[MT76_TM_ATTR_FREQ_OFFSET]);
478
479 if (tb[MT76_TM_ATTR_STATE]) {
480 state = nla_get_u32(tb[MT76_TM_ATTR_STATE]);
481 if (state > MT76_TM_STATE_MAX)
482 goto out;
483 } else {
484 state = td->state;
485 }
486
487 if (tb[MT76_TM_ATTR_TX_POWER]) {
488 struct nlattr *cur;
489 int idx = 0;
490 int rem;
491
492 nla_for_each_nested(cur, tb[MT76_TM_ATTR_TX_POWER], rem) {
493 if (nla_len(cur) != 1 ||
494 idx >= ARRAY_SIZE(td->tx_power))
495 goto out;
496
497 td->tx_power[idx++] = nla_get_u8(cur);
498 }
499 }
500
501 if (tb[MT76_TM_ATTR_MAC_ADDRS]) {
502 struct nlattr *cur;
503 int idx = 0;
504 int rem;
505
506 nla_for_each_nested(cur, tb[MT76_TM_ATTR_MAC_ADDRS], rem) {
507 if (nla_len(cur) != ETH_ALEN || idx >= 3)
508 goto out;
509
510 memcpy(td->addr[idx], nla_data(cur), ETH_ALEN);
511 idx++;
512 }
513 }
514
515 if (dev->test_ops->set_params) {
516 err = dev->test_ops->set_params(phy, tb, state);
517 if (err)
518 goto out;
519 }
520
521 for (i = MT76_TM_ATTR_STATE; i < ARRAY_SIZE(tb); i++)
522 if (tb[i])
523 mt76_testmode_param_set(td, i);
524
525 err = 0;
526 if (tb[MT76_TM_ATTR_STATE])
527 err = mt76_testmode_set_state(phy, state);
528
529 out:
530 mutex_unlock(&dev->mutex);
531
532 return err;
533 }
534 EXPORT_SYMBOL(mt76_testmode_cmd);
535
536 static int
mt76_testmode_dump_stats(struct mt76_phy * phy,struct sk_buff * msg)537 mt76_testmode_dump_stats(struct mt76_phy *phy, struct sk_buff *msg)
538 {
539 struct mt76_testmode_data *td = &phy->test;
540 struct mt76_dev *dev = phy->dev;
541 u64 rx_packets = 0;
542 u64 rx_fcs_error = 0;
543 int i;
544
545 if (dev->test_ops->dump_stats) {
546 int ret;
547
548 ret = dev->test_ops->dump_stats(phy, msg);
549 if (ret)
550 return ret;
551 }
552
553 for (i = 0; i < ARRAY_SIZE(td->rx_stats.packets); i++) {
554 rx_packets += td->rx_stats.packets[i];
555 rx_fcs_error += td->rx_stats.fcs_error[i];
556 }
557
558 if (nla_put_u32(msg, MT76_TM_STATS_ATTR_TX_PENDING, td->tx_pending) ||
559 nla_put_u32(msg, MT76_TM_STATS_ATTR_TX_QUEUED, td->tx_queued) ||
560 nla_put_u32(msg, MT76_TM_STATS_ATTR_TX_DONE, td->tx_done) ||
561 nla_put_u64_64bit(msg, MT76_TM_STATS_ATTR_RX_PACKETS, rx_packets,
562 MT76_TM_STATS_ATTR_PAD) ||
563 nla_put_u64_64bit(msg, MT76_TM_STATS_ATTR_RX_FCS_ERROR, rx_fcs_error,
564 MT76_TM_STATS_ATTR_PAD))
565 return -EMSGSIZE;
566
567 return 0;
568 }
569
mt76_testmode_dump(struct ieee80211_hw * hw,struct sk_buff * msg,struct netlink_callback * cb,void * data,int len)570 int mt76_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *msg,
571 struct netlink_callback *cb, void *data, int len)
572 {
573 struct mt76_phy *phy = hw->priv;
574 struct mt76_dev *dev = phy->dev;
575 struct mt76_testmode_data *td = &phy->test;
576 struct nlattr *tb[NUM_MT76_TM_ATTRS] = {};
577 int err = 0;
578 void *a;
579 int i;
580
581 if (!dev->test_ops)
582 return -EOPNOTSUPP;
583
584 if (cb->args[2]++ > 0)
585 return -ENOENT;
586
587 if (data) {
588 err = nla_parse_deprecated(tb, MT76_TM_ATTR_MAX, data, len,
589 mt76_tm_policy, NULL);
590 if (err)
591 return err;
592 }
593
594 mutex_lock(&dev->mutex);
595
596 if (tb[MT76_TM_ATTR_STATS]) {
597 err = -EINVAL;
598
599 a = nla_nest_start(msg, MT76_TM_ATTR_STATS);
600 if (a) {
601 err = mt76_testmode_dump_stats(phy, msg);
602 nla_nest_end(msg, a);
603 }
604
605 goto out;
606 }
607
608 mt76_testmode_init_defaults(phy);
609
610 err = -EMSGSIZE;
611 if (nla_put_u32(msg, MT76_TM_ATTR_STATE, td->state))
612 goto out;
613
614 if (dev->test_mtd.name &&
615 (nla_put_string(msg, MT76_TM_ATTR_MTD_PART, dev->test_mtd.name) ||
616 nla_put_u32(msg, MT76_TM_ATTR_MTD_OFFSET, dev->test_mtd.offset)))
617 goto out;
618
619 if (nla_put_u32(msg, MT76_TM_ATTR_TX_COUNT, td->tx_count) ||
620 nla_put_u32(msg, MT76_TM_ATTR_TX_LENGTH, td->tx_mpdu_len) ||
621 nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_MODE, td->tx_rate_mode) ||
622 nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_NSS, td->tx_rate_nss) ||
623 nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_IDX, td->tx_rate_idx) ||
624 nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_SGI, td->tx_rate_sgi) ||
625 nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_LDPC, td->tx_rate_ldpc) ||
626 nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_STBC, td->tx_rate_stbc) ||
627 (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_LTF) &&
628 nla_put_u8(msg, MT76_TM_ATTR_TX_LTF, td->tx_ltf)) ||
629 (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_ANTENNA) &&
630 nla_put_u8(msg, MT76_TM_ATTR_TX_ANTENNA, td->tx_antenna_mask)) ||
631 (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_SPE_IDX) &&
632 nla_put_u8(msg, MT76_TM_ATTR_TX_SPE_IDX, td->tx_spe_idx)) ||
633 (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_DUTY_CYCLE) &&
634 nla_put_u8(msg, MT76_TM_ATTR_TX_DUTY_CYCLE, td->tx_duty_cycle)) ||
635 (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_IPG) &&
636 nla_put_u32(msg, MT76_TM_ATTR_TX_IPG, td->tx_ipg)) ||
637 (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_TIME) &&
638 nla_put_u32(msg, MT76_TM_ATTR_TX_TIME, td->tx_time)) ||
639 (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_POWER_CONTROL) &&
640 nla_put_u8(msg, MT76_TM_ATTR_TX_POWER_CONTROL, td->tx_power_control)) ||
641 (mt76_testmode_param_present(td, MT76_TM_ATTR_FREQ_OFFSET) &&
642 nla_put_u8(msg, MT76_TM_ATTR_FREQ_OFFSET, td->freq_offset)))
643 goto out;
644
645 if (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_POWER)) {
646 a = nla_nest_start(msg, MT76_TM_ATTR_TX_POWER);
647 if (!a)
648 goto out;
649
650 for (i = 0; i < ARRAY_SIZE(td->tx_power); i++)
651 if (nla_put_u8(msg, i, td->tx_power[i]))
652 goto out;
653
654 nla_nest_end(msg, a);
655 }
656
657 if (mt76_testmode_param_present(td, MT76_TM_ATTR_MAC_ADDRS)) {
658 a = nla_nest_start(msg, MT76_TM_ATTR_MAC_ADDRS);
659 if (!a)
660 goto out;
661
662 for (i = 0; i < 3; i++)
663 if (nla_put(msg, i, ETH_ALEN, td->addr[i]))
664 goto out;
665
666 nla_nest_end(msg, a);
667 }
668
669 err = 0;
670
671 out:
672 mutex_unlock(&dev->mutex);
673
674 return err;
675 }
676 EXPORT_SYMBOL(mt76_testmode_dump);
677