testmode.c

  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
 19static 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
 28static 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
 45void 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
 88void 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 %p 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
179static 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
207static 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 %p 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
281static 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
300static 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
354int 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}