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