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 | |
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(lock: &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(lock: &q->lock); |
68 | } |
69 | |
70 | static u32 |
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 |
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 | |
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, tx_rate_mode: 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(size: head_len, GFP_KERNEL); |
126 | if (!head) |
127 | return -ENOMEM; |
128 | |
129 | hdr = __skb_put_zero(skb: head, len: 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(skb: head, queue_mapping: IEEE80211_AC_BE); |
135 | get_random_bytes(buf: __skb_put(skb: head, len: head_len - sizeof(*hdr)), |
136 | len: head_len - sizeof(*hdr)); |
137 | |
138 | info = IEEE80211_SKB_CB(skb: 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(size: 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(buf: __skb_put(skb: frag, len: frag_len), 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 |
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(skb: 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, mcs: td->tx_rate_idx, nss: 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 |
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(w: &dev->tx_worker); |
279 | } |
280 | |
281 | static void |
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(w: &dev->tx_worker); |
288 | |
289 | td->tx_pending = 0; |
290 | |
291 | mt76_worker_enable(w: &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 |
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 |
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 |
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 |
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 | |
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, state: 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 |
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(nla: attr); |
400 | if (val < min || val > max) |
401 | return -EINVAL; |
402 | |
403 | *dest = val; |
404 | return 0; |
405 | } |
406 | |
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, maxtype: MT76_TM_ATTR_MAX, head: data, len, |
422 | policy: 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(nla: tb[MT76_TM_ATTR_TX_COUNT]); |
439 | |
440 | if (tb[MT76_TM_ATTR_TX_RATE_IDX]) |
441 | td->tx_rate_idx = nla_get_u8(nla: tb[MT76_TM_ATTR_TX_RATE_IDX]); |
442 | |
443 | if (mt76_tm_get_u8(attr: tb[MT76_TM_ATTR_TX_RATE_MODE], dest: &td->tx_rate_mode, |
444 | min: 0, max: MT76_TM_TX_MODE_MAX) || |
445 | mt76_tm_get_u8(attr: tb[MT76_TM_ATTR_TX_RATE_NSS], dest: &td->tx_rate_nss, |
446 | min: 1, hweight8(phy->antenna_mask)) || |
447 | mt76_tm_get_u8(attr: tb[MT76_TM_ATTR_TX_RATE_SGI], dest: &td->tx_rate_sgi, min: 0, max: 2) || |
448 | mt76_tm_get_u8(attr: tb[MT76_TM_ATTR_TX_RATE_LDPC], dest: &td->tx_rate_ldpc, min: 0, max: 1) || |
449 | mt76_tm_get_u8(attr: tb[MT76_TM_ATTR_TX_RATE_STBC], dest: &td->tx_rate_stbc, min: 0, max: 1) || |
450 | mt76_tm_get_u8(attr: tb[MT76_TM_ATTR_TX_LTF], dest: &td->tx_ltf, min: 0, max: 2) || |
451 | mt76_tm_get_u8(attr: tb[MT76_TM_ATTR_TX_ANTENNA], |
452 | dest: &td->tx_antenna_mask, min: 0, max: 0xff) || |
453 | mt76_tm_get_u8(attr: tb[MT76_TM_ATTR_TX_SPE_IDX], dest: &td->tx_spe_idx, min: 0, max: 27) || |
454 | mt76_tm_get_u8(attr: tb[MT76_TM_ATTR_TX_DUTY_CYCLE], |
455 | dest: &td->tx_duty_cycle, min: 0, max: 99) || |
456 | mt76_tm_get_u8(attr: tb[MT76_TM_ATTR_TX_POWER_CONTROL], |
457 | dest: &td->tx_power_control, min: 0, max: 1)) |
458 | goto out; |
459 | |
460 | if (tb[MT76_TM_ATTR_TX_LENGTH]) { |
461 | u32 val = nla_get_u32(nla: tb[MT76_TM_ATTR_TX_LENGTH]); |
462 | |
463 | if (val > mt76_testmode_max_mpdu_len(phy, tx_rate_mode: 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(nla: tb[MT76_TM_ATTR_TX_IPG]); |
472 | |
473 | if (tb[MT76_TM_ATTR_TX_TIME]) |
474 | td->tx_time = nla_get_u32(nla: tb[MT76_TM_ATTR_TX_TIME]); |
475 | |
476 | if (tb[MT76_TM_ATTR_FREQ_OFFSET]) |
477 | td->freq_offset = nla_get_u32(nla: tb[MT76_TM_ATTR_FREQ_OFFSET]); |
478 | |
479 | if (tb[MT76_TM_ATTR_STATE]) { |
480 | state = nla_get_u32(nla: 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(nla: cur) != 1 || |
494 | idx >= ARRAY_SIZE(td->tx_power)) |
495 | goto out; |
496 | |
497 | td->tx_power[idx++] = nla_get_u8(nla: 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(nla: 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, idx: 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(lock: &dev->mutex); |
531 | |
532 | return err; |
533 | } |
534 | EXPORT_SYMBOL(mt76_testmode_cmd); |
535 | |
536 | static int |
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(skb: msg, attrtype: MT76_TM_STATS_ATTR_TX_PENDING, value: td->tx_pending) || |
559 | nla_put_u32(skb: msg, attrtype: MT76_TM_STATS_ATTR_TX_QUEUED, value: td->tx_queued) || |
560 | nla_put_u32(skb: msg, attrtype: MT76_TM_STATS_ATTR_TX_DONE, value: td->tx_done) || |
561 | nla_put_u64_64bit(skb: msg, attrtype: MT76_TM_STATS_ATTR_RX_PACKETS, value: rx_packets, |
562 | padattr: MT76_TM_STATS_ATTR_PAD) || |
563 | nla_put_u64_64bit(skb: msg, attrtype: MT76_TM_STATS_ATTR_RX_FCS_ERROR, value: rx_fcs_error, |
564 | padattr: MT76_TM_STATS_ATTR_PAD)) |
565 | return -EMSGSIZE; |
566 | |
567 | return 0; |
568 | } |
569 | |
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, maxtype: MT76_TM_ATTR_MAX, head: data, len, |
589 | policy: 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(skb: msg, attrtype: MT76_TM_ATTR_STATS); |
600 | if (a) { |
601 | err = mt76_testmode_dump_stats(phy, msg); |
602 | nla_nest_end(skb: msg, start: a); |
603 | } |
604 | |
605 | goto out; |
606 | } |
607 | |
608 | mt76_testmode_init_defaults(phy); |
609 | |
610 | err = -EMSGSIZE; |
611 | if (nla_put_u32(skb: msg, attrtype: MT76_TM_ATTR_STATE, value: td->state)) |
612 | goto out; |
613 | |
614 | if (dev->test_mtd.name && |
615 | (nla_put_string(skb: msg, attrtype: MT76_TM_ATTR_MTD_PART, str: dev->test_mtd.name) || |
616 | nla_put_u32(skb: msg, attrtype: MT76_TM_ATTR_MTD_OFFSET, value: dev->test_mtd.offset))) |
617 | goto out; |
618 | |
619 | if (nla_put_u32(skb: msg, attrtype: MT76_TM_ATTR_TX_COUNT, value: td->tx_count) || |
620 | nla_put_u32(skb: msg, attrtype: MT76_TM_ATTR_TX_LENGTH, value: td->tx_mpdu_len) || |
621 | nla_put_u8(skb: msg, attrtype: MT76_TM_ATTR_TX_RATE_MODE, value: td->tx_rate_mode) || |
622 | nla_put_u8(skb: msg, attrtype: MT76_TM_ATTR_TX_RATE_NSS, value: td->tx_rate_nss) || |
623 | nla_put_u8(skb: msg, attrtype: MT76_TM_ATTR_TX_RATE_IDX, value: td->tx_rate_idx) || |
624 | nla_put_u8(skb: msg, attrtype: MT76_TM_ATTR_TX_RATE_SGI, value: td->tx_rate_sgi) || |
625 | nla_put_u8(skb: msg, attrtype: MT76_TM_ATTR_TX_RATE_LDPC, value: td->tx_rate_ldpc) || |
626 | nla_put_u8(skb: msg, attrtype: MT76_TM_ATTR_TX_RATE_STBC, value: td->tx_rate_stbc) || |
627 | (mt76_testmode_param_present(td, idx: MT76_TM_ATTR_TX_LTF) && |
628 | nla_put_u8(skb: msg, attrtype: MT76_TM_ATTR_TX_LTF, value: td->tx_ltf)) || |
629 | (mt76_testmode_param_present(td, idx: MT76_TM_ATTR_TX_ANTENNA) && |
630 | nla_put_u8(skb: msg, attrtype: MT76_TM_ATTR_TX_ANTENNA, value: td->tx_antenna_mask)) || |
631 | (mt76_testmode_param_present(td, idx: MT76_TM_ATTR_TX_SPE_IDX) && |
632 | nla_put_u8(skb: msg, attrtype: MT76_TM_ATTR_TX_SPE_IDX, value: td->tx_spe_idx)) || |
633 | (mt76_testmode_param_present(td, idx: MT76_TM_ATTR_TX_DUTY_CYCLE) && |
634 | nla_put_u8(skb: msg, attrtype: MT76_TM_ATTR_TX_DUTY_CYCLE, value: td->tx_duty_cycle)) || |
635 | (mt76_testmode_param_present(td, idx: MT76_TM_ATTR_TX_IPG) && |
636 | nla_put_u32(skb: msg, attrtype: MT76_TM_ATTR_TX_IPG, value: td->tx_ipg)) || |
637 | (mt76_testmode_param_present(td, idx: MT76_TM_ATTR_TX_TIME) && |
638 | nla_put_u32(skb: msg, attrtype: MT76_TM_ATTR_TX_TIME, value: td->tx_time)) || |
639 | (mt76_testmode_param_present(td, idx: MT76_TM_ATTR_TX_POWER_CONTROL) && |
640 | nla_put_u8(skb: msg, attrtype: MT76_TM_ATTR_TX_POWER_CONTROL, value: td->tx_power_control)) || |
641 | (mt76_testmode_param_present(td, idx: MT76_TM_ATTR_FREQ_OFFSET) && |
642 | nla_put_u8(skb: msg, attrtype: MT76_TM_ATTR_FREQ_OFFSET, value: td->freq_offset))) |
643 | goto out; |
644 | |
645 | if (mt76_testmode_param_present(td, idx: MT76_TM_ATTR_TX_POWER)) { |
646 | a = nla_nest_start(skb: msg, attrtype: 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(skb: msg, attrtype: i, value: td->tx_power[i])) |
652 | goto out; |
653 | |
654 | nla_nest_end(skb: msg, start: a); |
655 | } |
656 | |
657 | if (mt76_testmode_param_present(td, idx: MT76_TM_ATTR_MAC_ADDRS)) { |
658 | a = nla_nest_start(skb: msg, attrtype: MT76_TM_ATTR_MAC_ADDRS); |
659 | if (!a) |
660 | goto out; |
661 | |
662 | for (i = 0; i < 3; i++) |
663 | if (nla_put(skb: msg, attrtype: i, ETH_ALEN, data: td->addr[i])) |
664 | goto out; |
665 | |
666 | nla_nest_end(skb: msg, start: a); |
667 | } |
668 | |
669 | err = 0; |
670 | |
671 | out: |
672 | mutex_unlock(lock: &dev->mutex); |
673 | |
674 | return err; |
675 | } |
676 | EXPORT_SYMBOL(mt76_testmode_dump); |
677 | |