1 // SPDX-License-Identifier: BSD-3-Clause-Clear
2 /*
3 * Copyright (c) 2018-2021 The Linux Foundation. All rights reserved.
4 * Copyright (c) 2021-2025 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 "hif.h"
14 #include "../testmode_i.h"
15
16 #define ATH12K_FTM_SEGHDR_CURRENT_SEQ GENMASK(3, 0)
17 #define ATH12K_FTM_SEGHDR_TOTAL_SEGMENTS GENMASK(7, 4)
18
19 static const struct nla_policy ath12k_tm_policy[ATH_TM_ATTR_MAX + 1] = {
20 [ATH_TM_ATTR_CMD] = { .type = NLA_U32 },
21 [ATH_TM_ATTR_DATA] = { .type = NLA_BINARY,
22 .len = ATH_TM_DATA_MAX_LEN },
23 [ATH_TM_ATTR_WMI_CMDID] = { .type = NLA_U32 },
24 [ATH_TM_ATTR_VERSION_MAJOR] = { .type = NLA_U32 },
25 [ATH_TM_ATTR_VERSION_MINOR] = { .type = NLA_U32 },
26 };
27
ath12k_tm_get_ar(struct ath12k_base * ab)28 static struct ath12k *ath12k_tm_get_ar(struct ath12k_base *ab)
29 {
30 struct ath12k_pdev *pdev;
31 struct ath12k *ar;
32 int i;
33
34 for (i = 0; i < ab->num_radios; i++) {
35 pdev = &ab->pdevs[i];
36 ar = pdev->ar;
37
38 if (ar && ar->ah->state == ATH12K_HW_STATE_TM)
39 return ar;
40 }
41
42 return NULL;
43 }
44
ath12k_tm_wmi_event_unsegmented(struct ath12k_base * ab,u32 cmd_id,struct sk_buff * skb)45 void ath12k_tm_wmi_event_unsegmented(struct ath12k_base *ab, u32 cmd_id,
46 struct sk_buff *skb)
47 {
48 struct sk_buff *nl_skb;
49 struct ath12k *ar;
50
51 ath12k_dbg(ab, ATH12K_DBG_TESTMODE,
52 "testmode event wmi cmd_id %d skb length %d\n",
53 cmd_id, skb->len);
54
55 ath12k_dbg_dump(ab, ATH12K_DBG_TESTMODE, NULL, "", skb->data, skb->len);
56
57 ar = ath12k_tm_get_ar(ab);
58 if (!ar) {
59 ath12k_warn(ab, "testmode event not handled due to invalid pdev\n");
60 return;
61 }
62
63 spin_lock_bh(&ar->data_lock);
64
65 nl_skb = cfg80211_testmode_alloc_event_skb(ar->ah->hw->wiphy,
66 2 * nla_total_size(sizeof(u32)) +
67 nla_total_size(skb->len),
68 GFP_ATOMIC);
69 spin_unlock_bh(&ar->data_lock);
70
71 if (!nl_skb) {
72 ath12k_warn(ab,
73 "failed to allocate skb for unsegmented testmode wmi event\n");
74 return;
75 }
76
77 if (nla_put_u32(nl_skb, ATH_TM_ATTR_CMD, ATH_TM_CMD_WMI) ||
78 nla_put_u32(nl_skb, ATH_TM_ATTR_WMI_CMDID, cmd_id) ||
79 nla_put(nl_skb, ATH_TM_ATTR_DATA, skb->len, skb->data)) {
80 ath12k_warn(ab, "failed to populate testmode unsegmented event\n");
81 kfree_skb(nl_skb);
82 return;
83 }
84
85 cfg80211_testmode_event(nl_skb, GFP_ATOMIC);
86 }
87
ath12k_tm_process_event(struct ath12k_base * ab,u32 cmd_id,const struct ath12k_wmi_ftm_event * ftm_msg,u16 length)88 void ath12k_tm_process_event(struct ath12k_base *ab, u32 cmd_id,
89 const struct ath12k_wmi_ftm_event *ftm_msg,
90 u16 length)
91 {
92 struct sk_buff *nl_skb;
93 struct ath12k *ar;
94 u32 data_pos, pdev_id;
95 u16 datalen;
96 u8 total_segments, current_seq;
97 u8 const *buf_pos;
98
99 ath12k_dbg(ab, ATH12K_DBG_TESTMODE,
100 "testmode event wmi cmd_id %d ftm event msg %pK datalen %d\n",
101 cmd_id, ftm_msg, length);
102 ath12k_dbg_dump(ab, ATH12K_DBG_TESTMODE, NULL, "", ftm_msg, length);
103 pdev_id = DP_HW2SW_MACID(le32_to_cpu(ftm_msg->seg_hdr.pdev_id));
104
105 if (pdev_id >= ab->num_radios) {
106 ath12k_warn(ab, "testmode event not handled due to invalid pdev id\n");
107 return;
108 }
109
110 ar = ab->pdevs[pdev_id].ar;
111
112 if (!ar) {
113 ath12k_warn(ab, "testmode event not handled due to absence of pdev\n");
114 return;
115 }
116
117 current_seq = le32_get_bits(ftm_msg->seg_hdr.segmentinfo,
118 ATH12K_FTM_SEGHDR_CURRENT_SEQ);
119 total_segments = le32_get_bits(ftm_msg->seg_hdr.segmentinfo,
120 ATH12K_FTM_SEGHDR_TOTAL_SEGMENTS);
121 datalen = length - (sizeof(struct ath12k_wmi_ftm_seg_hdr_params));
122 buf_pos = ftm_msg->data;
123
124 if (current_seq == 0) {
125 ab->ftm_event_obj.expected_seq = 0;
126 ab->ftm_event_obj.data_pos = 0;
127 }
128
129 data_pos = ab->ftm_event_obj.data_pos;
130
131 if ((data_pos + datalen) > ATH_FTM_EVENT_MAX_BUF_LENGTH) {
132 ath12k_warn(ab,
133 "Invalid event length date_pos[%d] datalen[%d]\n",
134 data_pos, datalen);
135 return;
136 }
137
138 memcpy(&ab->ftm_event_obj.eventdata[data_pos], buf_pos, datalen);
139 data_pos += datalen;
140
141 if (++ab->ftm_event_obj.expected_seq != total_segments) {
142 ab->ftm_event_obj.data_pos = data_pos;
143 ath12k_dbg(ab, ATH12K_DBG_TESTMODE,
144 "partial data received current_seq[%d], total_seg[%d]\n",
145 current_seq, total_segments);
146 return;
147 }
148
149 ath12k_dbg(ab, ATH12K_DBG_TESTMODE,
150 "total data length[%d] = [%d]\n",
151 data_pos, ftm_msg->seg_hdr.len);
152
153 spin_lock_bh(&ar->data_lock);
154 nl_skb = cfg80211_testmode_alloc_event_skb(ar->ah->hw->wiphy,
155 2 * nla_total_size(sizeof(u32)) +
156 nla_total_size(data_pos),
157 GFP_ATOMIC);
158 spin_unlock_bh(&ar->data_lock);
159
160 if (!nl_skb) {
161 ath12k_warn(ab,
162 "failed to allocate skb for testmode wmi event\n");
163 return;
164 }
165
166 if (nla_put_u32(nl_skb, ATH_TM_ATTR_CMD,
167 ATH_TM_CMD_WMI_FTM) ||
168 nla_put_u32(nl_skb, ATH_TM_ATTR_WMI_CMDID, cmd_id) ||
169 nla_put(nl_skb, ATH_TM_ATTR_DATA, data_pos,
170 &ab->ftm_event_obj.eventdata[0])) {
171 ath12k_warn(ab, "failed to populate testmode event");
172 kfree_skb(nl_skb);
173 return;
174 }
175
176 cfg80211_testmode_event(nl_skb, GFP_ATOMIC);
177 }
178
ath12k_tm_cmd_get_version(struct ath12k * ar,struct nlattr * tb[])179 static int ath12k_tm_cmd_get_version(struct ath12k *ar, struct nlattr *tb[])
180 {
181 struct sk_buff *skb;
182
183 ath12k_dbg(ar->ab, ATH12K_DBG_TESTMODE,
184 "testmode cmd get version_major %d version_minor %d\n",
185 ATH_TESTMODE_VERSION_MAJOR,
186 ATH_TESTMODE_VERSION_MINOR);
187
188 spin_lock_bh(&ar->data_lock);
189 skb = cfg80211_testmode_alloc_reply_skb(ar->ah->hw->wiphy,
190 2 * nla_total_size(sizeof(u32)));
191 spin_unlock_bh(&ar->data_lock);
192
193 if (!skb)
194 return -ENOMEM;
195
196 if (nla_put_u32(skb, ATH_TM_ATTR_VERSION_MAJOR,
197 ATH_TESTMODE_VERSION_MAJOR) ||
198 nla_put_u32(skb, ATH_TM_ATTR_VERSION_MINOR,
199 ATH_TESTMODE_VERSION_MINOR)) {
200 kfree_skb(skb);
201 return -ENOBUFS;
202 }
203
204 return cfg80211_testmode_reply(skb);
205 }
206
ath12k_tm_cmd_process_ftm(struct ath12k * ar,struct nlattr * tb[])207 static int ath12k_tm_cmd_process_ftm(struct ath12k *ar, struct nlattr *tb[])
208 {
209 struct ath12k_wmi_pdev *wmi = ar->wmi;
210 struct sk_buff *skb;
211 struct ath12k_wmi_ftm_cmd *ftm_cmd;
212 int ret = 0;
213 void *buf;
214 size_t aligned_len;
215 u32 cmd_id, buf_len;
216 u16 chunk_len, total_bytes, num_segments;
217 u8 segnumber = 0, *bufpos;
218
219 ath12k_dbg(ar->ab, ATH12K_DBG_TESTMODE, "ah->state %d\n", ar->ah->state);
220 if (ar->ah->state != ATH12K_HW_STATE_TM)
221 return -ENETDOWN;
222
223 if (!tb[ATH_TM_ATTR_DATA])
224 return -EINVAL;
225
226 buf = nla_data(tb[ATH_TM_ATTR_DATA]);
227 buf_len = nla_len(tb[ATH_TM_ATTR_DATA]);
228 cmd_id = WMI_PDEV_UTF_CMDID;
229 ath12k_dbg(ar->ab, ATH12K_DBG_TESTMODE,
230 "testmode cmd wmi cmd_id %d buf %pK buf_len %d\n",
231 cmd_id, buf, buf_len);
232 ath12k_dbg_dump(ar->ab, ATH12K_DBG_TESTMODE, NULL, "", buf, buf_len);
233 bufpos = buf;
234 total_bytes = buf_len;
235 num_segments = total_bytes / MAX_WMI_UTF_LEN;
236
237 if (buf_len - (num_segments * MAX_WMI_UTF_LEN))
238 num_segments++;
239
240 while (buf_len) {
241 if (buf_len > MAX_WMI_UTF_LEN)
242 chunk_len = MAX_WMI_UTF_LEN; /* MAX message */
243 else
244 chunk_len = buf_len;
245
246 skb = ath12k_wmi_alloc_skb(wmi->wmi_ab, (chunk_len +
247 sizeof(struct ath12k_wmi_ftm_cmd)));
248
249 if (!skb)
250 return -ENOMEM;
251
252 ftm_cmd = (struct ath12k_wmi_ftm_cmd *)skb->data;
253 aligned_len = chunk_len + sizeof(struct ath12k_wmi_ftm_seg_hdr_params);
254 ftm_cmd->tlv_header = ath12k_wmi_tlv_hdr(WMI_TAG_ARRAY_BYTE, aligned_len);
255 ftm_cmd->seg_hdr.len = cpu_to_le32(total_bytes);
256 ftm_cmd->seg_hdr.msgref = cpu_to_le32(ar->ftm_msgref);
257 ftm_cmd->seg_hdr.segmentinfo =
258 le32_encode_bits(num_segments,
259 ATH12K_FTM_SEGHDR_TOTAL_SEGMENTS) |
260 le32_encode_bits(segnumber,
261 ATH12K_FTM_SEGHDR_CURRENT_SEQ);
262 ftm_cmd->seg_hdr.pdev_id = cpu_to_le32(ar->pdev->pdev_id);
263 segnumber++;
264 memcpy(&ftm_cmd->data, bufpos, chunk_len);
265 ret = ath12k_wmi_cmd_send(wmi, skb, cmd_id);
266
267 if (ret) {
268 ath12k_warn(ar->ab, "ftm wmi command fail: %d\n", ret);
269 kfree_skb(skb);
270 return ret;
271 }
272
273 buf_len -= chunk_len;
274 bufpos += chunk_len;
275 }
276
277 ++ar->ftm_msgref;
278 return ret;
279 }
280
ath12k_tm_cmd_testmode_start(struct ath12k * ar,struct nlattr * tb[])281 static int ath12k_tm_cmd_testmode_start(struct ath12k *ar, struct nlattr *tb[])
282 {
283 if (ar->ah->state == ATH12K_HW_STATE_TM)
284 return -EALREADY;
285
286 if (ar->ah->state != ATH12K_HW_STATE_OFF)
287 return -EBUSY;
288
289 ar->ab->ftm_event_obj.eventdata = kzalloc(ATH_FTM_EVENT_MAX_BUF_LENGTH,
290 GFP_KERNEL);
291
292 if (!ar->ab->ftm_event_obj.eventdata)
293 return -ENOMEM;
294
295 ar->ah->state = ATH12K_HW_STATE_TM;
296 ar->ftm_msgref = 0;
297 return 0;
298 }
299
ath12k_tm_cmd_wmi(struct ath12k * ar,struct nlattr * tb[])300 static int ath12k_tm_cmd_wmi(struct ath12k *ar, struct nlattr *tb[])
301 {
302 struct ath12k_wmi_pdev *wmi = ar->wmi;
303 struct sk_buff *skb;
304 struct wmi_pdev_set_param_cmd *cmd;
305 int ret = 0, tag;
306 void *buf;
307 u32 cmd_id, buf_len;
308
309 if (!tb[ATH_TM_ATTR_DATA])
310 return -EINVAL;
311
312 if (!tb[ATH_TM_ATTR_WMI_CMDID])
313 return -EINVAL;
314
315 buf = nla_data(tb[ATH_TM_ATTR_DATA]);
316 buf_len = nla_len(tb[ATH_TM_ATTR_DATA]);
317
318 if (!buf_len) {
319 ath12k_warn(ar->ab, "No data present in testmode command\n");
320 return -EINVAL;
321 }
322
323 cmd_id = nla_get_u32(tb[ATH_TM_ATTR_WMI_CMDID]);
324
325 cmd = buf;
326 tag = le32_get_bits(cmd->tlv_header, WMI_TLV_TAG);
327
328 if (tag == WMI_TAG_PDEV_SET_PARAM_CMD)
329 cmd->pdev_id = cpu_to_le32(ar->pdev->pdev_id);
330
331 ath12k_dbg(ar->ab, ATH12K_DBG_TESTMODE,
332 "testmode cmd wmi cmd_id %d buf length %d\n",
333 cmd_id, buf_len);
334
335 ath12k_dbg_dump(ar->ab, ATH12K_DBG_TESTMODE, NULL, "", buf, buf_len);
336
337 skb = ath12k_wmi_alloc_skb(wmi->wmi_ab, buf_len);
338
339 if (!skb)
340 return -ENOMEM;
341
342 memcpy(skb->data, buf, buf_len);
343
344 ret = ath12k_wmi_cmd_send(wmi, skb, cmd_id);
345 if (ret) {
346 dev_kfree_skb(skb);
347 ath12k_warn(ar->ab, "failed to transmit wmi command (testmode): %d\n",
348 ret);
349 }
350
351 return ret;
352 }
353
ath12k_tm_cmd(struct ieee80211_hw * hw,struct ieee80211_vif * vif,void * data,int len)354 int ath12k_tm_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
355 void *data, int len)
356 {
357 struct ath12k_hw *ah = hw->priv;
358 struct ath12k *ar = NULL;
359 struct nlattr *tb[ATH_TM_ATTR_MAX + 1];
360 struct ath12k_base *ab;
361 struct wiphy *wiphy = hw->wiphy;
362 int ret;
363
364 lockdep_assert_held(&wiphy->mtx);
365
366 ret = nla_parse(tb, ATH_TM_ATTR_MAX, data, len, ath12k_tm_policy,
367 NULL);
368 if (ret)
369 return ret;
370
371 if (!tb[ATH_TM_ATTR_CMD])
372 return -EINVAL;
373
374 /* TODO: have to handle ar for MLO case */
375 if (ah->num_radio)
376 ar = ah->radio;
377
378 if (!ar)
379 return -EINVAL;
380
381 ab = ar->ab;
382 switch (nla_get_u32(tb[ATH_TM_ATTR_CMD])) {
383 case ATH_TM_CMD_WMI:
384 return ath12k_tm_cmd_wmi(ar, tb);
385 case ATH_TM_CMD_TESTMODE_START:
386 return ath12k_tm_cmd_testmode_start(ar, tb);
387 case ATH_TM_CMD_GET_VERSION:
388 return ath12k_tm_cmd_get_version(ar, tb);
389 case ATH_TM_CMD_WMI_FTM:
390 set_bit(ATH12K_FLAG_FTM_SEGMENTED, &ab->dev_flags);
391 return ath12k_tm_cmd_process_ftm(ar, tb);
392 default:
393 return -EOPNOTSUPP;
394 }
395 }
396