1 // SPDX-License-Identifier: ISC
2 /* Copyright (C) 2020 Felix Fietkau <nbd@nbd.name> */
3 #include "mt76.h"
4
5 static const struct nla_policy mt76_tm_policy[NUM_MT76_TM_ATTRS] = {
6 [MT76_TM_ATTR_RESET] = { .type = NLA_FLAG },
7 [MT76_TM_ATTR_STATE] = { .type = NLA_U8 },
8 [MT76_TM_ATTR_TX_COUNT] = { .type = NLA_U32 },
9 [MT76_TM_ATTR_TX_LENGTH] = { .type = NLA_U32 },
10 [MT76_TM_ATTR_TX_RATE_MODE] = { .type = NLA_U8 },
11 [MT76_TM_ATTR_TX_RATE_NSS] = { .type = NLA_U8 },
12 [MT76_TM_ATTR_TX_RATE_IDX] = { .type = NLA_U8 },
13 [MT76_TM_ATTR_TX_RATE_SGI] = { .type = NLA_U8 },
14 [MT76_TM_ATTR_TX_RATE_LDPC] = { .type = NLA_U8 },
15 [MT76_TM_ATTR_TX_ANTENNA] = { .type = NLA_U8 },
16 [MT76_TM_ATTR_TX_POWER_CONTROL] = { .type = NLA_U8 },
17 [MT76_TM_ATTR_TX_POWER] = { .type = NLA_NESTED },
18 [MT76_TM_ATTR_FREQ_OFFSET] = { .type = NLA_U32 },
19 };
20
mt76_testmode_tx_pending(struct mt76_dev * dev)21 void mt76_testmode_tx_pending(struct mt76_dev *dev)
22 {
23 struct mt76_testmode_data *td = &dev->test;
24 struct mt76_wcid *wcid = &dev->global_wcid;
25 struct sk_buff *skb = td->tx_skb;
26 struct mt76_queue *q;
27 int qid;
28
29 if (!skb || !td->tx_pending)
30 return;
31
32 qid = skb_get_queue_mapping(skb);
33 q = dev->q_tx[qid];
34
35 spin_lock_bh(&q->lock);
36
37 while (td->tx_pending > 0 && td->tx_queued - td->tx_done < 1000 &&
38 q->queued < q->ndesc / 2) {
39 int ret;
40
41 ret = dev->queue_ops->tx_queue_skb(dev, qid, skb_get(skb), wcid, NULL);
42 if (ret < 0)
43 break;
44
45 td->tx_pending--;
46 td->tx_queued++;
47 }
48
49 dev->queue_ops->kick(dev, q);
50
51 spin_unlock_bh(&q->lock);
52 }
53
54
55 static int
mt76_testmode_tx_init(struct mt76_dev * dev)56 mt76_testmode_tx_init(struct mt76_dev *dev)
57 {
58 struct mt76_testmode_data *td = &dev->test;
59 struct ieee80211_tx_info *info;
60 struct ieee80211_hdr *hdr;
61 struct sk_buff *skb;
62 u16 fc = IEEE80211_FTYPE_DATA | IEEE80211_STYPE_DATA |
63 IEEE80211_FCTL_FROMDS;
64 struct ieee80211_tx_rate *rate;
65 u8 max_nss = hweight8(dev->phy.antenna_mask);
66
67 if (td->tx_antenna_mask)
68 max_nss = min_t(u8, max_nss, hweight8(td->tx_antenna_mask));
69
70 skb = alloc_skb(td->tx_msdu_len, GFP_KERNEL);
71 if (!skb)
72 return -ENOMEM;
73
74 dev_kfree_skb(td->tx_skb);
75 td->tx_skb = skb;
76 hdr = __skb_put_zero(skb, td->tx_msdu_len);
77 hdr->frame_control = cpu_to_le16(fc);
78 memcpy(hdr->addr1, dev->macaddr, sizeof(dev->macaddr));
79 memcpy(hdr->addr2, dev->macaddr, sizeof(dev->macaddr));
80 memcpy(hdr->addr3, dev->macaddr, sizeof(dev->macaddr));
81
82 info = IEEE80211_SKB_CB(skb);
83 info->flags = IEEE80211_TX_CTL_INJECTED |
84 IEEE80211_TX_CTL_NO_ACK |
85 IEEE80211_TX_CTL_NO_PS_BUFFER;
86 rate = &info->control.rates[0];
87 rate->count = 1;
88 rate->idx = td->tx_rate_idx;
89
90 switch (td->tx_rate_mode) {
91 case MT76_TM_TX_MODE_CCK:
92 if (dev->phy.chandef.chan->band != NL80211_BAND_2GHZ)
93 return -EINVAL;
94
95 if (rate->idx > 4)
96 return -EINVAL;
97 break;
98 case MT76_TM_TX_MODE_OFDM:
99 if (dev->phy.chandef.chan->band != NL80211_BAND_2GHZ)
100 break;
101
102 if (rate->idx > 8)
103 return -EINVAL;
104
105 rate->idx += 4;
106 break;
107 case MT76_TM_TX_MODE_HT:
108 if (rate->idx > 8 * max_nss &&
109 !(rate->idx == 32 &&
110 dev->phy.chandef.width >= NL80211_CHAN_WIDTH_40))
111 return -EINVAL;
112
113 rate->flags |= IEEE80211_TX_RC_MCS;
114 break;
115 case MT76_TM_TX_MODE_VHT:
116 if (rate->idx > 9)
117 return -EINVAL;
118
119 if (td->tx_rate_nss > max_nss)
120 return -EINVAL;
121
122 ieee80211_rate_set_vht(rate, td->tx_rate_idx, td->tx_rate_nss);
123 rate->flags |= IEEE80211_TX_RC_VHT_MCS;
124 break;
125 default:
126 break;
127 }
128
129 if (td->tx_rate_sgi)
130 rate->flags |= IEEE80211_TX_RC_SHORT_GI;
131
132 if (td->tx_rate_ldpc)
133 info->flags |= IEEE80211_TX_CTL_LDPC;
134
135 if (td->tx_rate_mode >= MT76_TM_TX_MODE_HT) {
136 switch (dev->phy.chandef.width) {
137 case NL80211_CHAN_WIDTH_40:
138 rate->flags |= IEEE80211_TX_RC_40_MHZ_WIDTH;
139 break;
140 case NL80211_CHAN_WIDTH_80:
141 rate->flags |= IEEE80211_TX_RC_80_MHZ_WIDTH;
142 break;
143 case NL80211_CHAN_WIDTH_80P80:
144 case NL80211_CHAN_WIDTH_160:
145 rate->flags |= IEEE80211_TX_RC_160_MHZ_WIDTH;
146 break;
147 default:
148 break;
149 }
150 }
151
152 skb_set_queue_mapping(skb, IEEE80211_AC_BE);
153
154 return 0;
155 }
156
157 static void
mt76_testmode_tx_start(struct mt76_dev * dev)158 mt76_testmode_tx_start(struct mt76_dev *dev)
159 {
160 struct mt76_testmode_data *td = &dev->test;
161
162 td->tx_queued = 0;
163 td->tx_done = 0;
164 td->tx_pending = td->tx_count;
165 mt76_worker_schedule(&dev->tx_worker);
166 }
167
168 static void
mt76_testmode_tx_stop(struct mt76_dev * dev)169 mt76_testmode_tx_stop(struct mt76_dev *dev)
170 {
171 struct mt76_testmode_data *td = &dev->test;
172
173 mt76_worker_disable(&dev->tx_worker);
174
175 td->tx_pending = 0;
176
177 mt76_worker_enable(&dev->tx_worker);
178
179 wait_event_timeout(dev->tx_wait, td->tx_done == td->tx_queued, 10 * HZ);
180
181 dev_kfree_skb(td->tx_skb);
182 td->tx_skb = NULL;
183 }
184
185 static inline void
mt76_testmode_param_set(struct mt76_testmode_data * td,u16 idx)186 mt76_testmode_param_set(struct mt76_testmode_data *td, u16 idx)
187 {
188 td->param_set[idx / 32] |= BIT(idx % 32);
189 }
190
191 static inline bool
mt76_testmode_param_present(struct mt76_testmode_data * td,u16 idx)192 mt76_testmode_param_present(struct mt76_testmode_data *td, u16 idx)
193 {
194 return td->param_set[idx / 32] & BIT(idx % 32);
195 }
196
197 static void
mt76_testmode_init_defaults(struct mt76_dev * dev)198 mt76_testmode_init_defaults(struct mt76_dev *dev)
199 {
200 struct mt76_testmode_data *td = &dev->test;
201
202 if (td->tx_msdu_len > 0)
203 return;
204
205 td->tx_msdu_len = 1024;
206 td->tx_count = 1;
207 td->tx_rate_mode = MT76_TM_TX_MODE_OFDM;
208 td->tx_rate_nss = 1;
209 }
210
211 static int
__mt76_testmode_set_state(struct mt76_dev * dev,enum mt76_testmode_state state)212 __mt76_testmode_set_state(struct mt76_dev *dev, enum mt76_testmode_state state)
213 {
214 enum mt76_testmode_state prev_state = dev->test.state;
215 int err;
216
217 if (prev_state == MT76_TM_STATE_TX_FRAMES)
218 mt76_testmode_tx_stop(dev);
219
220 if (state == MT76_TM_STATE_TX_FRAMES) {
221 err = mt76_testmode_tx_init(dev);
222 if (err)
223 return err;
224 }
225
226 err = dev->test_ops->set_state(dev, state);
227 if (err) {
228 if (state == MT76_TM_STATE_TX_FRAMES)
229 mt76_testmode_tx_stop(dev);
230
231 return err;
232 }
233
234 if (state == MT76_TM_STATE_TX_FRAMES)
235 mt76_testmode_tx_start(dev);
236 else if (state == MT76_TM_STATE_RX_FRAMES) {
237 memset(&dev->test.rx_stats, 0, sizeof(dev->test.rx_stats));
238 }
239
240 dev->test.state = state;
241
242 return 0;
243 }
244
mt76_testmode_set_state(struct mt76_dev * dev,enum mt76_testmode_state state)245 int mt76_testmode_set_state(struct mt76_dev *dev, enum mt76_testmode_state state)
246 {
247 struct mt76_testmode_data *td = &dev->test;
248 struct ieee80211_hw *hw = dev->phy.hw;
249
250 if (state == td->state && state == MT76_TM_STATE_OFF)
251 return 0;
252
253 if (state > MT76_TM_STATE_OFF &&
254 (!test_bit(MT76_STATE_RUNNING, &dev->phy.state) ||
255 !(hw->conf.flags & IEEE80211_CONF_MONITOR)))
256 return -ENOTCONN;
257
258 if (state != MT76_TM_STATE_IDLE &&
259 td->state != MT76_TM_STATE_IDLE) {
260 int ret;
261
262 ret = __mt76_testmode_set_state(dev, MT76_TM_STATE_IDLE);
263 if (ret)
264 return ret;
265 }
266
267 return __mt76_testmode_set_state(dev, state);
268
269 }
270 EXPORT_SYMBOL(mt76_testmode_set_state);
271
272 static int
mt76_tm_get_u8(struct nlattr * attr,u8 * dest,u8 min,u8 max)273 mt76_tm_get_u8(struct nlattr *attr, u8 *dest, u8 min, u8 max)
274 {
275 u8 val;
276
277 if (!attr)
278 return 0;
279
280 val = nla_get_u8(attr);
281 if (val < min || val > max)
282 return -EINVAL;
283
284 *dest = val;
285 return 0;
286 }
287
mt76_testmode_cmd(struct ieee80211_hw * hw,struct ieee80211_vif * vif,void * data,int len)288 int mt76_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
289 void *data, int len)
290 {
291 struct mt76_phy *phy = hw->priv;
292 struct mt76_dev *dev = phy->dev;
293 struct mt76_testmode_data *td = &dev->test;
294 struct nlattr *tb[NUM_MT76_TM_ATTRS];
295 u32 state;
296 int err;
297 int i;
298
299 if (!dev->test_ops)
300 return -EOPNOTSUPP;
301
302 err = nla_parse_deprecated(tb, MT76_TM_ATTR_MAX, data, len,
303 mt76_tm_policy, NULL);
304 if (err)
305 return err;
306
307 err = -EINVAL;
308
309 mutex_lock(&dev->mutex);
310
311 if (tb[MT76_TM_ATTR_RESET]) {
312 mt76_testmode_set_state(dev, MT76_TM_STATE_OFF);
313 memset(td, 0, sizeof(*td));
314 }
315
316 mt76_testmode_init_defaults(dev);
317
318 if (tb[MT76_TM_ATTR_TX_COUNT])
319 td->tx_count = nla_get_u32(tb[MT76_TM_ATTR_TX_COUNT]);
320
321 if (tb[MT76_TM_ATTR_TX_LENGTH]) {
322 u32 val = nla_get_u32(tb[MT76_TM_ATTR_TX_LENGTH]);
323
324 if (val > IEEE80211_MAX_FRAME_LEN ||
325 val < sizeof(struct ieee80211_hdr))
326 goto out;
327
328 td->tx_msdu_len = val;
329 }
330
331 if (tb[MT76_TM_ATTR_TX_RATE_IDX])
332 td->tx_rate_idx = nla_get_u8(tb[MT76_TM_ATTR_TX_RATE_IDX]);
333
334 if (mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_MODE], &td->tx_rate_mode,
335 0, MT76_TM_TX_MODE_MAX) ||
336 mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_NSS], &td->tx_rate_nss,
337 1, hweight8(phy->antenna_mask)) ||
338 mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_SGI], &td->tx_rate_sgi, 0, 1) ||
339 mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_LDPC], &td->tx_rate_ldpc, 0, 1) ||
340 mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_ANTENNA], &td->tx_antenna_mask, 1,
341 phy->antenna_mask) ||
342 mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_POWER_CONTROL],
343 &td->tx_power_control, 0, 1))
344 goto out;
345
346 if (tb[MT76_TM_ATTR_FREQ_OFFSET])
347 td->freq_offset = nla_get_u32(tb[MT76_TM_ATTR_FREQ_OFFSET]);
348
349 if (tb[MT76_TM_ATTR_STATE]) {
350 state = nla_get_u32(tb[MT76_TM_ATTR_STATE]);
351 if (state > MT76_TM_STATE_MAX)
352 goto out;
353 } else {
354 state = td->state;
355 }
356
357 if (tb[MT76_TM_ATTR_TX_POWER]) {
358 struct nlattr *cur;
359 int idx = 0;
360 int rem;
361
362 nla_for_each_nested(cur, tb[MT76_TM_ATTR_TX_POWER], rem) {
363 if (nla_len(cur) != 1 ||
364 idx >= ARRAY_SIZE(td->tx_power))
365 goto out;
366
367 td->tx_power[idx++] = nla_get_u8(cur);
368 }
369 }
370
371 if (dev->test_ops->set_params) {
372 err = dev->test_ops->set_params(dev, tb, state);
373 if (err)
374 goto out;
375 }
376
377 for (i = MT76_TM_ATTR_STATE; i < ARRAY_SIZE(tb); i++)
378 if (tb[i])
379 mt76_testmode_param_set(td, i);
380
381 err = 0;
382 if (tb[MT76_TM_ATTR_STATE])
383 err = mt76_testmode_set_state(dev, state);
384
385 out:
386 mutex_unlock(&dev->mutex);
387
388 return err;
389 }
390 EXPORT_SYMBOL(mt76_testmode_cmd);
391
392 static int
mt76_testmode_dump_stats(struct mt76_dev * dev,struct sk_buff * msg)393 mt76_testmode_dump_stats(struct mt76_dev *dev, struct sk_buff *msg)
394 {
395 struct mt76_testmode_data *td = &dev->test;
396 u64 rx_packets = 0;
397 u64 rx_fcs_error = 0;
398 int i;
399
400 for (i = 0; i < ARRAY_SIZE(td->rx_stats.packets); i++) {
401 rx_packets += td->rx_stats.packets[i];
402 rx_fcs_error += td->rx_stats.fcs_error[i];
403 }
404
405 if (nla_put_u32(msg, MT76_TM_STATS_ATTR_TX_PENDING, td->tx_pending) ||
406 nla_put_u32(msg, MT76_TM_STATS_ATTR_TX_QUEUED, td->tx_queued) ||
407 nla_put_u32(msg, MT76_TM_STATS_ATTR_TX_DONE, td->tx_done) ||
408 nla_put_u64_64bit(msg, MT76_TM_STATS_ATTR_RX_PACKETS, rx_packets,
409 MT76_TM_STATS_ATTR_PAD) ||
410 nla_put_u64_64bit(msg, MT76_TM_STATS_ATTR_RX_FCS_ERROR, rx_fcs_error,
411 MT76_TM_STATS_ATTR_PAD))
412 return -EMSGSIZE;
413
414 if (dev->test_ops->dump_stats)
415 return dev->test_ops->dump_stats(dev, msg);
416
417 return 0;
418 }
419
mt76_testmode_dump(struct ieee80211_hw * hw,struct sk_buff * msg,struct netlink_callback * cb,void * data,int len)420 int mt76_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *msg,
421 struct netlink_callback *cb, void *data, int len)
422 {
423 struct mt76_phy *phy = hw->priv;
424 struct mt76_dev *dev = phy->dev;
425 struct mt76_testmode_data *td = &dev->test;
426 struct nlattr *tb[NUM_MT76_TM_ATTRS] = {};
427 int err = 0;
428 void *a;
429 int i;
430
431 if (!dev->test_ops)
432 return -EOPNOTSUPP;
433
434 if (cb->args[2]++ > 0)
435 return -ENOENT;
436
437 if (data) {
438 err = nla_parse_deprecated(tb, MT76_TM_ATTR_MAX, data, len,
439 mt76_tm_policy, NULL);
440 if (err)
441 return err;
442 }
443
444 mutex_lock(&dev->mutex);
445
446 if (tb[MT76_TM_ATTR_STATS]) {
447 err = -EINVAL;
448
449 a = nla_nest_start(msg, MT76_TM_ATTR_STATS);
450 if (a) {
451 err = mt76_testmode_dump_stats(dev, msg);
452 nla_nest_end(msg, a);
453 }
454
455 goto out;
456 }
457
458 mt76_testmode_init_defaults(dev);
459
460 err = -EMSGSIZE;
461 if (nla_put_u32(msg, MT76_TM_ATTR_STATE, td->state))
462 goto out;
463
464 if (td->mtd_name &&
465 (nla_put_string(msg, MT76_TM_ATTR_MTD_PART, td->mtd_name) ||
466 nla_put_u32(msg, MT76_TM_ATTR_MTD_OFFSET, td->mtd_offset)))
467 goto out;
468
469 if (nla_put_u32(msg, MT76_TM_ATTR_TX_COUNT, td->tx_count) ||
470 nla_put_u32(msg, MT76_TM_ATTR_TX_LENGTH, td->tx_msdu_len) ||
471 nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_MODE, td->tx_rate_mode) ||
472 nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_NSS, td->tx_rate_nss) ||
473 nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_IDX, td->tx_rate_idx) ||
474 nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_SGI, td->tx_rate_sgi) ||
475 nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_LDPC, td->tx_rate_ldpc) ||
476 (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_ANTENNA) &&
477 nla_put_u8(msg, MT76_TM_ATTR_TX_ANTENNA, td->tx_antenna_mask)) ||
478 (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_POWER_CONTROL) &&
479 nla_put_u8(msg, MT76_TM_ATTR_TX_POWER_CONTROL, td->tx_power_control)) ||
480 (mt76_testmode_param_present(td, MT76_TM_ATTR_FREQ_OFFSET) &&
481 nla_put_u8(msg, MT76_TM_ATTR_FREQ_OFFSET, td->freq_offset)))
482 goto out;
483
484 if (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_POWER)) {
485 a = nla_nest_start(msg, MT76_TM_ATTR_TX_POWER);
486 if (!a)
487 goto out;
488
489 for (i = 0; i < ARRAY_SIZE(td->tx_power); i++)
490 if (nla_put_u8(msg, i, td->tx_power[i]))
491 goto out;
492
493 nla_nest_end(msg, a);
494 }
495
496 err = 0;
497
498 out:
499 mutex_unlock(&dev->mutex);
500
501 return err;
502 }
503 EXPORT_SYMBOL(mt76_testmode_dump);
504