1 // SPDX-License-Identifier: BSD-3-Clause-Clear
2 /*
3  * Copyright (c) 2018-2019 The Linux Foundation. All rights reserved.
4  * Copyright (c) 2023 Qualcomm Innovation Center, Inc. All rights reserved.
5  */
6 
7 #include "testmode.h"
8 #include <net/netlink.h>
9 #include "debug.h"
10 #include "wmi.h"
11 #include "hw.h"
12 #include "core.h"
13 #include "testmode_i.h"
14 
15 #define ATH11K_FTM_SEGHDR_CURRENT_SEQ		GENMASK(3, 0)
16 #define ATH11K_FTM_SEGHDR_TOTAL_SEGMENTS	GENMASK(7, 4)
17 
18 static const struct nla_policy ath11k_tm_policy[ATH11K_TM_ATTR_MAX + 1] = {
19 	[ATH11K_TM_ATTR_CMD]		= { .type = NLA_U32 },
20 	[ATH11K_TM_ATTR_DATA]		= { .type = NLA_BINARY,
21 					    .len = ATH11K_TM_DATA_MAX_LEN },
22 	[ATH11K_TM_ATTR_WMI_CMDID]	= { .type = NLA_U32 },
23 	[ATH11K_TM_ATTR_VERSION_MAJOR]	= { .type = NLA_U32 },
24 	[ATH11K_TM_ATTR_VERSION_MINOR]	= { .type = NLA_U32 },
25 };
26 
27 static struct ath11k *ath11k_tm_get_ar(struct ath11k_base *ab)
28 {
29 	struct ath11k_pdev *pdev;
30 	struct ath11k *ar = NULL;
31 	int i;
32 
33 	for (i = 0; i < ab->num_radios; i++) {
34 		pdev = &ab->pdevs[i];
35 		ar = pdev->ar;
36 
37 		if (ar && ar->state == ATH11K_STATE_FTM)
38 			break;
39 	}
40 
41 	return ar;
42 }
43 
44 /* This function handles unsegmented events. Data in various events are aggregated
45  * in application layer, this event is unsegmented from host perspective.
46  */
47 static void ath11k_tm_wmi_event_unsegmented(struct ath11k_base *ab, u32 cmd_id,
48 					    struct sk_buff *skb)
49 {
50 	struct sk_buff *nl_skb;
51 	struct ath11k *ar;
52 
53 	ath11k_dbg(ab, ATH11K_DBG_TESTMODE,
54 		   "event wmi cmd_id %d skb length %d\n",
55 		   cmd_id, skb->len);
56 	ath11k_dbg_dump(ab, ATH11K_DBG_TESTMODE, NULL, "", skb->data, skb->len);
57 
58 	ar = ath11k_tm_get_ar(ab);
59 	if (!ar) {
60 		ath11k_warn(ab, "testmode event not handled due to invalid pdev\n");
61 		return;
62 	}
63 
64 	spin_lock_bh(&ar->data_lock);
65 
66 	nl_skb = cfg80211_testmode_alloc_event_skb(ar->hw->wiphy,
67 						   2 * nla_total_size(sizeof(u32)) +
68 						   nla_total_size(skb->len),
69 						   GFP_ATOMIC);
70 	if (!nl_skb) {
71 		ath11k_warn(ab,
72 			    "failed to allocate skb for unsegmented testmode wmi event\n");
73 		goto out;
74 	}
75 
76 	if (nla_put_u32(nl_skb, ATH11K_TM_ATTR_CMD, ATH11K_TM_CMD_WMI) ||
77 	    nla_put_u32(nl_skb, ATH11K_TM_ATTR_WMI_CMDID, cmd_id) ||
78 	    nla_put(nl_skb, ATH11K_TM_ATTR_DATA, skb->len, skb->data)) {
79 		ath11k_warn(ab, "failed to populate testmode unsegmented event\n");
80 		kfree_skb(nl_skb);
81 		goto out;
82 	}
83 
84 	cfg80211_testmode_event(nl_skb, GFP_ATOMIC);
85 	spin_unlock_bh(&ar->data_lock);
86 	return;
87 
88 out:
89 	spin_unlock_bh(&ar->data_lock);
90 	ath11k_warn(ab, "Failed to send testmode event to higher layers\n");
91 }
92 
93 /* This function handles segmented events. Data of various events received
94  * from firmware is aggregated and sent to application layer
95  */
96 static int ath11k_tm_process_event(struct ath11k_base *ab, u32 cmd_id,
97 				   const struct wmi_ftm_event_msg *ftm_msg,
98 				   u16 length)
99 {
100 	struct sk_buff *nl_skb;
101 	int ret = 0;
102 	struct ath11k *ar;
103 	u8 const *buf_pos;
104 	u16 datalen;
105 	u8 total_segments, current_seq;
106 	u32 data_pos;
107 	u32 pdev_id;
108 
109 	ath11k_dbg(ab, ATH11K_DBG_TESTMODE,
110 		   "event wmi cmd_id %d ftm event msg %pK datalen %d\n",
111 		   cmd_id, ftm_msg, length);
112 	ath11k_dbg_dump(ab, ATH11K_DBG_TESTMODE, NULL, "", ftm_msg, length);
113 	pdev_id = DP_HW2SW_MACID(ftm_msg->seg_hdr.pdev_id);
114 
115 	if (pdev_id >= ab->num_radios) {
116 		ath11k_warn(ab, "testmode event not handled due to invalid pdev id: %d\n",
117 			    pdev_id);
118 		return -EINVAL;
119 	}
120 
121 	ar = ab->pdevs[pdev_id].ar;
122 	if (!ar) {
123 		ath11k_warn(ab, "testmode event not handled due to absence of pdev\n");
124 		return -ENODEV;
125 	}
126 
127 	current_seq = FIELD_GET(ATH11K_FTM_SEGHDR_CURRENT_SEQ,
128 				ftm_msg->seg_hdr.segmentinfo);
129 	total_segments = FIELD_GET(ATH11K_FTM_SEGHDR_TOTAL_SEGMENTS,
130 				   ftm_msg->seg_hdr.segmentinfo);
131 	datalen = length - (sizeof(struct wmi_ftm_seg_hdr));
132 	buf_pos = ftm_msg->data;
133 
134 	spin_lock_bh(&ar->data_lock);
135 
136 	if (current_seq == 0) {
137 		ab->testmode.expected_seq = 0;
138 		ab->testmode.data_pos = 0;
139 	}
140 
141 	data_pos = ab->testmode.data_pos;
142 
143 	if ((data_pos + datalen) > ATH11K_FTM_EVENT_MAX_BUF_LENGTH) {
144 		ath11k_warn(ab, "Invalid ftm event length at %d: %d\n",
145 			    data_pos, datalen);
146 		ret = -EINVAL;
147 		goto out;
148 	}
149 
150 	memcpy(&ab->testmode.eventdata[data_pos], buf_pos, datalen);
151 	data_pos += datalen;
152 
153 	if (++ab->testmode.expected_seq != total_segments) {
154 		ab->testmode.data_pos = data_pos;
155 		ath11k_dbg(ab, ATH11K_DBG_TESTMODE,
156 			   "partial data received current_seq %d total_seg %d\n",
157 			    current_seq, total_segments);
158 		goto out;
159 	}
160 
161 	ath11k_dbg(ab, ATH11K_DBG_TESTMODE,
162 		   "total data length pos %d len %d\n",
163 		    data_pos, ftm_msg->seg_hdr.len);
164 	nl_skb = cfg80211_testmode_alloc_event_skb(ar->hw->wiphy,
165 						   2 * nla_total_size(sizeof(u32)) +
166 						   nla_total_size(data_pos),
167 						   GFP_ATOMIC);
168 	if (!nl_skb) {
169 		ath11k_warn(ab,
170 			    "failed to allocate skb for segmented testmode wmi event\n");
171 		ret = -ENOMEM;
172 		goto out;
173 	}
174 
175 	if (nla_put_u32(nl_skb, ATH11K_TM_ATTR_CMD,
176 			ATH11K_TM_CMD_WMI_FTM) ||
177 	    nla_put_u32(nl_skb, ATH11K_TM_ATTR_WMI_CMDID, cmd_id) ||
178 	    nla_put(nl_skb, ATH11K_TM_ATTR_DATA, data_pos,
179 		    &ab->testmode.eventdata[0])) {
180 		ath11k_warn(ab, "failed to populate segmented testmode event");
181 		kfree_skb(nl_skb);
182 		ret = -ENOBUFS;
183 		goto out;
184 	}
185 
186 	cfg80211_testmode_event(nl_skb, GFP_ATOMIC);
187 
188 out:
189 	spin_unlock_bh(&ar->data_lock);
190 	return ret;
191 }
192 
193 static void ath11k_tm_wmi_event_segmented(struct ath11k_base *ab, u32 cmd_id,
194 					  struct sk_buff *skb)
195 {
196 	const void **tb;
197 	const struct wmi_ftm_event_msg *ev;
198 	u16 length;
199 	int ret;
200 
201 	tb = ath11k_wmi_tlv_parse_alloc(ab, skb->data, skb->len, GFP_ATOMIC);
202 	if (IS_ERR(tb)) {
203 		ret = PTR_ERR(tb);
204 		ath11k_warn(ab, "failed to parse ftm event tlv: %d\n", ret);
205 		return;
206 	}
207 
208 	ev = tb[WMI_TAG_ARRAY_BYTE];
209 	if (!ev) {
210 		ath11k_warn(ab, "failed to fetch ftm msg\n");
211 		kfree(tb);
212 		return;
213 	}
214 
215 	length = skb->len - TLV_HDR_SIZE;
216 	ret = ath11k_tm_process_event(ab, cmd_id, ev, length);
217 	if (ret)
218 		ath11k_warn(ab, "Failed to process ftm event\n");
219 
220 	kfree(tb);
221 }
222 
223 void ath11k_tm_wmi_event(struct ath11k_base *ab, u32 cmd_id, struct sk_buff *skb)
224 {
225 	if (test_bit(ATH11K_FLAG_FTM_SEGMENTED, &ab->dev_flags))
226 		ath11k_tm_wmi_event_segmented(ab, cmd_id, skb);
227 	else
228 		ath11k_tm_wmi_event_unsegmented(ab, cmd_id, skb);
229 }
230 
231 static int ath11k_tm_cmd_get_version(struct ath11k *ar, struct nlattr *tb[])
232 {
233 	struct sk_buff *skb;
234 	int ret;
235 
236 	ath11k_dbg(ar->ab, ATH11K_DBG_TESTMODE,
237 		   "cmd get version_major %d version_minor %d\n",
238 		   ATH11K_TESTMODE_VERSION_MAJOR,
239 		   ATH11K_TESTMODE_VERSION_MINOR);
240 
241 	skb = cfg80211_testmode_alloc_reply_skb(ar->hw->wiphy,
242 						nla_total_size(sizeof(u32)));
243 	if (!skb)
244 		return -ENOMEM;
245 
246 	ret = nla_put_u32(skb, ATH11K_TM_ATTR_VERSION_MAJOR,
247 			  ATH11K_TESTMODE_VERSION_MAJOR);
248 	if (ret) {
249 		kfree_skb(skb);
250 		return ret;
251 	}
252 
253 	ret = nla_put_u32(skb, ATH11K_TM_ATTR_VERSION_MINOR,
254 			  ATH11K_TESTMODE_VERSION_MINOR);
255 	if (ret) {
256 		kfree_skb(skb);
257 		return ret;
258 	}
259 
260 	return cfg80211_testmode_reply(skb);
261 }
262 
263 static int ath11k_tm_cmd_testmode_start(struct ath11k *ar, struct nlattr *tb[])
264 {
265 	int ret;
266 
267 	mutex_lock(&ar->conf_mutex);
268 
269 	if (ar->state == ATH11K_STATE_FTM) {
270 		ret = -EALREADY;
271 		goto err;
272 	}
273 
274 	/* start utf only when the driver is not in use  */
275 	if (ar->state != ATH11K_STATE_OFF) {
276 		ret = -EBUSY;
277 		goto err;
278 	}
279 
280 	ar->ab->testmode.eventdata = kzalloc(ATH11K_FTM_EVENT_MAX_BUF_LENGTH,
281 					     GFP_KERNEL);
282 	if (!ar->ab->testmode.eventdata) {
283 		ret = -ENOMEM;
284 		goto err;
285 	}
286 
287 	ar->state = ATH11K_STATE_FTM;
288 	ar->ftm_msgref = 0;
289 
290 	mutex_unlock(&ar->conf_mutex);
291 
292 	ath11k_dbg(ar->ab, ATH11K_DBG_TESTMODE, "cmd start\n");
293 	return 0;
294 
295 err:
296 	mutex_unlock(&ar->conf_mutex);
297 	return ret;
298 }
299 
300 static int ath11k_tm_cmd_wmi(struct ath11k *ar, struct nlattr *tb[],
301 			     struct ieee80211_vif *vif)
302 {
303 	struct ath11k_pdev_wmi *wmi = ar->wmi;
304 	struct sk_buff *skb;
305 	struct ath11k_vif *arvif;
306 	u32 cmd_id, buf_len;
307 	int ret, tag;
308 	void *buf;
309 	u32 *ptr;
310 
311 	mutex_lock(&ar->conf_mutex);
312 
313 	if (!tb[ATH11K_TM_ATTR_DATA]) {
314 		ret = -EINVAL;
315 		goto out;
316 	}
317 
318 	if (!tb[ATH11K_TM_ATTR_WMI_CMDID]) {
319 		ret = -EINVAL;
320 		goto out;
321 	}
322 
323 	buf = nla_data(tb[ATH11K_TM_ATTR_DATA]);
324 	buf_len = nla_len(tb[ATH11K_TM_ATTR_DATA]);
325 	if (!buf_len) {
326 		ath11k_warn(ar->ab, "No data present in testmode wmi command\n");
327 		ret = -EINVAL;
328 		goto out;
329 	}
330 
331 	cmd_id = nla_get_u32(tb[ATH11K_TM_ATTR_WMI_CMDID]);
332 
333 	/* Make sure that the buffer length is long enough to
334 	 * hold TLV and pdev/vdev id.
335 	 */
336 	if (buf_len < sizeof(struct wmi_tlv) + sizeof(u32)) {
337 		ret = -EINVAL;
338 		goto out;
339 	}
340 
341 	ptr = buf;
342 	tag = FIELD_GET(WMI_TLV_TAG, *ptr);
343 
344 	/* pdev/vdev id start after TLV header */
345 	ptr++;
346 
347 	if (tag == WMI_TAG_PDEV_SET_PARAM_CMD)
348 		*ptr = ar->pdev->pdev_id;
349 
350 	if (ar->ab->fw_mode != ATH11K_FIRMWARE_MODE_FTM &&
351 	    (tag == WMI_TAG_VDEV_SET_PARAM_CMD || tag == WMI_TAG_UNIT_TEST_CMD)) {
352 		if (vif) {
353 			arvif = (struct ath11k_vif *)vif->drv_priv;
354 			*ptr = arvif->vdev_id;
355 		} else {
356 			ret = -EINVAL;
357 			goto out;
358 		}
359 	}
360 
361 	ath11k_dbg(ar->ab, ATH11K_DBG_TESTMODE,
362 		   "cmd wmi cmd_id %d buf length %d\n",
363 		   cmd_id, buf_len);
364 
365 	ath11k_dbg_dump(ar->ab, ATH11K_DBG_TESTMODE, NULL, "", buf, buf_len);
366 
367 	skb = ath11k_wmi_alloc_skb(wmi->wmi_ab, buf_len);
368 	if (!skb) {
369 		ret = -ENOMEM;
370 		goto out;
371 	}
372 
373 	memcpy(skb->data, buf, buf_len);
374 
375 	ret = ath11k_wmi_cmd_send(wmi, skb, cmd_id);
376 	if (ret) {
377 		dev_kfree_skb(skb);
378 		ath11k_warn(ar->ab, "failed to transmit wmi command (testmode): %d\n",
379 			    ret);
380 		goto out;
381 	}
382 
383 	ret = 0;
384 
385 out:
386 	mutex_unlock(&ar->conf_mutex);
387 	return ret;
388 }
389 
390 static int ath11k_tm_cmd_wmi_ftm(struct ath11k *ar, struct nlattr *tb[])
391 {
392 	struct ath11k_pdev_wmi *wmi = ar->wmi;
393 	struct ath11k_base *ab = ar->ab;
394 	struct sk_buff *skb;
395 	u32 cmd_id, buf_len, hdr_info;
396 	int ret;
397 	void *buf;
398 	u8 segnumber = 0, seginfo;
399 	u16 chunk_len, total_bytes, num_segments;
400 	u8 *bufpos;
401 	struct wmi_ftm_cmd *ftm_cmd;
402 
403 	set_bit(ATH11K_FLAG_FTM_SEGMENTED, &ab->dev_flags);
404 
405 	mutex_lock(&ar->conf_mutex);
406 
407 	if (ar->state != ATH11K_STATE_FTM) {
408 		ret = -ENETDOWN;
409 		goto out;
410 	}
411 
412 	if (!tb[ATH11K_TM_ATTR_DATA]) {
413 		ret = -EINVAL;
414 		goto out;
415 	}
416 
417 	buf = nla_data(tb[ATH11K_TM_ATTR_DATA]);
418 	buf_len = nla_len(tb[ATH11K_TM_ATTR_DATA]);
419 	cmd_id = WMI_PDEV_UTF_CMDID;
420 
421 	ath11k_dbg(ar->ab, ATH11K_DBG_TESTMODE,
422 		   "cmd wmi ftm cmd_id %d buffer length %d\n",
423 		   cmd_id, buf_len);
424 	ath11k_dbg_dump(ar->ab, ATH11K_DBG_TESTMODE, NULL, "", buf, buf_len);
425 
426 	bufpos = buf;
427 	total_bytes = buf_len;
428 	num_segments = total_bytes / MAX_WMI_UTF_LEN;
429 
430 	if (buf_len - (num_segments * MAX_WMI_UTF_LEN))
431 		num_segments++;
432 
433 	while (buf_len) {
434 		chunk_len = min_t(u16, buf_len, MAX_WMI_UTF_LEN);
435 
436 		skb = ath11k_wmi_alloc_skb(wmi->wmi_ab, (chunk_len +
437 					   sizeof(struct wmi_ftm_cmd)));
438 		if (!skb) {
439 			ret = -ENOMEM;
440 			goto out;
441 		}
442 
443 		ftm_cmd = (struct wmi_ftm_cmd *)skb->data;
444 		hdr_info = FIELD_PREP(WMI_TLV_TAG, WMI_TAG_ARRAY_BYTE) |
445 			   FIELD_PREP(WMI_TLV_LEN, (chunk_len +
446 				      sizeof(struct wmi_ftm_seg_hdr)));
447 		ftm_cmd->tlv_header = hdr_info;
448 		ftm_cmd->seg_hdr.len = total_bytes;
449 		ftm_cmd->seg_hdr.msgref = ar->ftm_msgref;
450 		seginfo = FIELD_PREP(ATH11K_FTM_SEGHDR_TOTAL_SEGMENTS, num_segments) |
451 			  FIELD_PREP(ATH11K_FTM_SEGHDR_CURRENT_SEQ, segnumber);
452 		ftm_cmd->seg_hdr.segmentinfo = seginfo;
453 		segnumber++;
454 
455 		memcpy(&ftm_cmd->data, bufpos, chunk_len);
456 
457 		ret = ath11k_wmi_cmd_send(wmi, skb, cmd_id);
458 		if (ret) {
459 			ath11k_warn(ar->ab, "failed to send wmi ftm command: %d\n", ret);
460 			goto out;
461 		}
462 
463 		buf_len -= chunk_len;
464 		bufpos += chunk_len;
465 	}
466 
467 	ar->ftm_msgref++;
468 	ret = 0;
469 
470 out:
471 	mutex_unlock(&ar->conf_mutex);
472 	return ret;
473 }
474 
475 int ath11k_tm_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
476 		  void *data, int len)
477 {
478 	struct ath11k *ar = hw->priv;
479 	struct nlattr *tb[ATH11K_TM_ATTR_MAX + 1];
480 	int ret;
481 
482 	ret = nla_parse(tb, ATH11K_TM_ATTR_MAX, data, len, ath11k_tm_policy,
483 			NULL);
484 	if (ret)
485 		return ret;
486 
487 	if (!tb[ATH11K_TM_ATTR_CMD])
488 		return -EINVAL;
489 
490 	switch (nla_get_u32(tb[ATH11K_TM_ATTR_CMD])) {
491 	case ATH11K_TM_CMD_GET_VERSION:
492 		return ath11k_tm_cmd_get_version(ar, tb);
493 	case ATH11K_TM_CMD_WMI:
494 		return ath11k_tm_cmd_wmi(ar, tb, vif);
495 	case ATH11K_TM_CMD_TESTMODE_START:
496 		return ath11k_tm_cmd_testmode_start(ar, tb);
497 	case ATH11K_TM_CMD_WMI_FTM:
498 		return ath11k_tm_cmd_wmi_ftm(ar, tb);
499 	default:
500 		return -EOPNOTSUPP;
501 	}
502 }
503