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