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, mask: ATH11K_DBG_TESTMODE, NULL, prefix: "" , buf: skb->data, len: skb->len); |
57 | |
58 | ar = ath11k_tm_get_ar(ab); |
59 | if (!ar) { |
60 | ath11k_warn(ab, fmt: "testmode event not handled due to invalid pdev\n" ); |
61 | return; |
62 | } |
63 | |
64 | spin_lock_bh(lock: &ar->data_lock); |
65 | |
66 | nl_skb = cfg80211_testmode_alloc_event_skb(wiphy: ar->hw->wiphy, |
67 | approxlen: 2 * nla_total_size(payload: sizeof(u32)) + |
68 | nla_total_size(payload: skb->len), |
69 | GFP_ATOMIC); |
70 | if (!nl_skb) { |
71 | ath11k_warn(ab, |
72 | fmt: "failed to allocate skb for unsegmented testmode wmi event\n" ); |
73 | goto out; |
74 | } |
75 | |
76 | if (nla_put_u32(skb: nl_skb, attrtype: ATH11K_TM_ATTR_CMD, value: ATH11K_TM_CMD_WMI) || |
77 | nla_put_u32(skb: nl_skb, attrtype: ATH11K_TM_ATTR_WMI_CMDID, value: cmd_id) || |
78 | nla_put(skb: nl_skb, attrtype: ATH11K_TM_ATTR_DATA, attrlen: skb->len, data: skb->data)) { |
79 | ath11k_warn(ab, fmt: "failed to populate testmode unsegmented event\n" ); |
80 | kfree_skb(skb: nl_skb); |
81 | goto out; |
82 | } |
83 | |
84 | cfg80211_testmode_event(skb: nl_skb, GFP_ATOMIC); |
85 | spin_unlock_bh(lock: &ar->data_lock); |
86 | return; |
87 | |
88 | out: |
89 | spin_unlock_bh(lock: &ar->data_lock); |
90 | ath11k_warn(ab, fmt: "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, mask: ATH11K_DBG_TESTMODE, NULL, prefix: "" , buf: ftm_msg, len: 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, fmt: "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, fmt: "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(lock: &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, fmt: "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(wiphy: ar->hw->wiphy, |
165 | approxlen: 2 * nla_total_size(payload: sizeof(u32)) + |
166 | nla_total_size(payload: data_pos), |
167 | GFP_ATOMIC); |
168 | if (!nl_skb) { |
169 | ath11k_warn(ab, |
170 | fmt: "failed to allocate skb for segmented testmode wmi event\n" ); |
171 | ret = -ENOMEM; |
172 | goto out; |
173 | } |
174 | |
175 | if (nla_put_u32(skb: nl_skb, attrtype: ATH11K_TM_ATTR_CMD, |
176 | value: ATH11K_TM_CMD_WMI_FTM) || |
177 | nla_put_u32(skb: nl_skb, attrtype: ATH11K_TM_ATTR_WMI_CMDID, value: cmd_id) || |
178 | nla_put(skb: nl_skb, attrtype: ATH11K_TM_ATTR_DATA, attrlen: data_pos, |
179 | data: &ab->testmode.eventdata[0])) { |
180 | ath11k_warn(ab, fmt: "failed to populate segmented testmode event" ); |
181 | kfree_skb(skb: nl_skb); |
182 | ret = -ENOBUFS; |
183 | goto out; |
184 | } |
185 | |
186 | cfg80211_testmode_event(skb: nl_skb, GFP_ATOMIC); |
187 | |
188 | out: |
189 | spin_unlock_bh(lock: &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, GFP_ATOMIC); |
202 | if (IS_ERR(ptr: tb)) { |
203 | ret = PTR_ERR(ptr: tb); |
204 | ath11k_warn(ab, fmt: "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, fmt: "failed to fetch ftm msg\n" ); |
211 | kfree(objp: tb); |
212 | return; |
213 | } |
214 | |
215 | length = skb->len - TLV_HDR_SIZE; |
216 | ret = ath11k_tm_process_event(ab, cmd_id, ftm_msg: ev, length); |
217 | if (ret) |
218 | ath11k_warn(ab, fmt: "Failed to process ftm event\n" ); |
219 | |
220 | kfree(objp: 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(wiphy: ar->hw->wiphy, |
242 | approxlen: nla_total_size(payload: sizeof(u32))); |
243 | if (!skb) |
244 | return -ENOMEM; |
245 | |
246 | ret = nla_put_u32(skb, attrtype: 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, attrtype: 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(lock: &ar->conf_mutex); |
291 | |
292 | ath11k_dbg(ar->ab, ATH11K_DBG_TESTMODE, "cmd start\n" ); |
293 | return 0; |
294 | |
295 | err: |
296 | mutex_unlock(lock: &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(nla: tb[ATH11K_TM_ATTR_DATA]); |
324 | buf_len = nla_len(nla: tb[ATH11K_TM_ATTR_DATA]); |
325 | if (!buf_len) { |
326 | ath11k_warn(ab: ar->ab, fmt: "No data present in testmode wmi command\n" ); |
327 | ret = -EINVAL; |
328 | goto out; |
329 | } |
330 | |
331 | cmd_id = nla_get_u32(nla: 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 = ath11k_vif_to_arvif(vif); |
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(ab: ar->ab, mask: ATH11K_DBG_TESTMODE, NULL, prefix: "" , buf, len: buf_len); |
366 | |
367 | skb = ath11k_wmi_alloc_skb(wmi_sc: wmi->wmi_ab, len: 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(ab: ar->ab, fmt: "failed to transmit wmi command (testmode): %d\n" , |
379 | ret); |
380 | goto out; |
381 | } |
382 | |
383 | ret = 0; |
384 | |
385 | out: |
386 | mutex_unlock(lock: &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(nr: ATH11K_FLAG_FTM_SEGMENTED, addr: &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(nla: tb[ATH11K_TM_ATTR_DATA]); |
418 | buf_len = nla_len(nla: 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(ab: ar->ab, mask: ATH11K_DBG_TESTMODE, NULL, prefix: "" , buf, len: 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_sc: wmi->wmi_ab, len: (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(ab: ar->ab, fmt: "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(lock: &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, maxtype: ATH11K_TM_ATTR_MAX, head: data, len, policy: 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(nla: 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 | |