testmode.c

  1// SPDX-License-Identifier: GPL-2.0-only
  2/*
  3 * This file is part of wl1271
  4 *
  5 * Copyright (C) 2010 Nokia Corporation
  6 *
  7 * Contact: Luciano Coelho <luciano.coelho@nokia.com>
  8 */
  9#include "testmode.h"
 10
 11#include <linux/pm_runtime.h>
 12#include <linux/slab.h>
 13#include <net/genetlink.h>
 14
 15#include "wlcore.h"
 16#include "debug.h"
 17#include "acx.h"
 18#include "io.h"
 19
 20#define WL1271_TM_MAX_DATA_LENGTH 1024
 21
 22enum wl1271_tm_commands {
 23	WL1271_TM_CMD_UNSPEC,
 24	WL1271_TM_CMD_TEST,
 25	WL1271_TM_CMD_INTERROGATE,
 26	WL1271_TM_CMD_CONFIGURE,
 27	WL1271_TM_CMD_NVS_PUSH,		/* Not in use. Keep to not break ABI */
 28	WL1271_TM_CMD_SET_PLT_MODE,
 29	WL1271_TM_CMD_RECOVER,		/* Not in use. Keep to not break ABI */
 30	WL1271_TM_CMD_GET_MAC,
 31
 32	__WL1271_TM_CMD_AFTER_LAST
 33};
 34#define WL1271_TM_CMD_MAX (__WL1271_TM_CMD_AFTER_LAST - 1)
 35
 36enum wl1271_tm_attrs {
 37	WL1271_TM_ATTR_UNSPEC,
 38	WL1271_TM_ATTR_CMD_ID,
 39	WL1271_TM_ATTR_ANSWER,
 40	WL1271_TM_ATTR_DATA,
 41	WL1271_TM_ATTR_IE_ID,
 42	WL1271_TM_ATTR_PLT_MODE,
 43
 44	__WL1271_TM_ATTR_AFTER_LAST
 45};
 46#define WL1271_TM_ATTR_MAX (__WL1271_TM_ATTR_AFTER_LAST - 1)
 47
 48static const struct nla_policy wl1271_tm_policy[WL1271_TM_ATTR_MAX + 1] = {
 49	[WL1271_TM_ATTR_CMD_ID] =	{ .type = NLA_U32 },
 50	[WL1271_TM_ATTR_ANSWER] =	{ .type = NLA_U8 },
 51	[WL1271_TM_ATTR_DATA] =		{ .type = NLA_BINARY,
 52					  .len = WL1271_TM_MAX_DATA_LENGTH },
 53	[WL1271_TM_ATTR_IE_ID] =	{ .type = NLA_U32 },
 54	[WL1271_TM_ATTR_PLT_MODE] =	{ .type = NLA_U32 },
 55};
 56
 57
 58static int wl1271_tm_cmd_test(struct wl1271 *wl, struct nlattr *tb[])
 59{
 60	int buf_len, ret, len;
 61	struct sk_buff *skb;
 62	void *buf;
 63	u8 answer = 0;
 64
 65	wl1271_debug(DEBUG_TESTMODE, "testmode cmd test");
 66
 67	if (!tb[WL1271_TM_ATTR_DATA])
 68		return -EINVAL;
 69
 70	buf = nla_data(tb[WL1271_TM_ATTR_DATA]);
 71	buf_len = nla_len(tb[WL1271_TM_ATTR_DATA]);
 72
 73	if (tb[WL1271_TM_ATTR_ANSWER])
 74		answer = nla_get_u8(tb[WL1271_TM_ATTR_ANSWER]);
 75
 76	if (buf_len > sizeof(struct wl1271_command))
 77		return -EMSGSIZE;
 78
 79	mutex_lock(&wl->mutex);
 80
 81	if (unlikely(wl->state != WLCORE_STATE_ON)) {
 82		ret = -EINVAL;
 83		goto out;
 84	}
 85
 86	ret = pm_runtime_resume_and_get(wl->dev);
 87	if (ret < 0)
 88		goto out;
 89
 90	ret = wl1271_cmd_test(wl, buf, buf_len, answer);
 91	if (ret < 0) {
 92		wl1271_warning("testmode cmd test failed: %d", ret);
 93		goto out_sleep;
 94	}
 95
 96	if (answer) {
 97		/* If we got bip calibration answer print radio status */
 98		struct wl1271_cmd_cal_p2g *params =
 99			(struct wl1271_cmd_cal_p2g *) buf;
100
101		s16 radio_status = (s16) le16_to_cpu(params->radio_status);
102
103		if (params->test.id == TEST_CMD_P2G_CAL &&
104		    radio_status < 0)
105			wl1271_warning("testmode cmd: radio status=%d",
106					radio_status);
107		else
108			wl1271_info("testmode cmd: radio status=%d",
109					radio_status);
110
111		len = nla_total_size(buf_len);
112		skb = cfg80211_testmode_alloc_reply_skb(wl->hw->wiphy, len);
113		if (!skb) {
114			ret = -ENOMEM;
115			goto out_sleep;
116		}
117
118		if (nla_put(skb, WL1271_TM_ATTR_DATA, buf_len, buf)) {
119			kfree_skb(skb);
120			ret = -EMSGSIZE;
121			goto out_sleep;
122		}
123
124		ret = cfg80211_testmode_reply(skb);
125		if (ret < 0)
126			goto out_sleep;
127	}
128
129out_sleep:
130	pm_runtime_put_autosuspend(wl->dev);
131out:
132	mutex_unlock(&wl->mutex);
133
134	return ret;
135}
136
137static int wl1271_tm_cmd_interrogate(struct wl1271 *wl, struct nlattr *tb[])
138{
139	int ret;
140	struct wl1271_command *cmd;
141	struct sk_buff *skb;
142	u8 ie_id;
143
144	wl1271_debug(DEBUG_TESTMODE, "testmode cmd interrogate");
145
146	if (!tb[WL1271_TM_ATTR_IE_ID])
147		return -EINVAL;
148
149	ie_id = nla_get_u8(tb[WL1271_TM_ATTR_IE_ID]);
150
151	mutex_lock(&wl->mutex);
152
153	if (unlikely(wl->state != WLCORE_STATE_ON)) {
154		ret = -EINVAL;
155		goto out;
156	}
157
158	ret = pm_runtime_resume_and_get(wl->dev);
159	if (ret < 0)
160		goto out;
161
162	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
163	if (!cmd) {
164		ret = -ENOMEM;
165		goto out_sleep;
166	}
167
168	ret = wl1271_cmd_interrogate(wl, ie_id, cmd,
169				     sizeof(struct acx_header), sizeof(*cmd));
170	if (ret < 0) {
171		wl1271_warning("testmode cmd interrogate failed: %d", ret);
172		goto out_free;
173	}
174
175	skb = cfg80211_testmode_alloc_reply_skb(wl->hw->wiphy, sizeof(*cmd));
176	if (!skb) {
177		ret = -ENOMEM;
178		goto out_free;
179	}
180
181	if (nla_put(skb, WL1271_TM_ATTR_DATA, sizeof(*cmd), cmd)) {
182		kfree_skb(skb);
183		ret = -EMSGSIZE;
184		goto out_free;
185	}
186
187	ret = cfg80211_testmode_reply(skb);
188	if (ret < 0)
189		goto out_free;
190
191out_free:
192	kfree(cmd);
193out_sleep:
194	pm_runtime_put_autosuspend(wl->dev);
195out:
196	mutex_unlock(&wl->mutex);
197
198	return ret;
199}
200
201static int wl1271_tm_cmd_configure(struct wl1271 *wl, struct nlattr *tb[])
202{
203	int buf_len, ret;
204	void *buf;
205	u8 ie_id;
206
207	wl1271_debug(DEBUG_TESTMODE, "testmode cmd configure");
208
209	if (!tb[WL1271_TM_ATTR_DATA])
210		return -EINVAL;
211	if (!tb[WL1271_TM_ATTR_IE_ID])
212		return -EINVAL;
213
214	ie_id = nla_get_u8(tb[WL1271_TM_ATTR_IE_ID]);
215	buf = nla_data(tb[WL1271_TM_ATTR_DATA]);
216	buf_len = nla_len(tb[WL1271_TM_ATTR_DATA]);
217
218	if (buf_len > sizeof(struct wl1271_command))
219		return -EMSGSIZE;
220
221	mutex_lock(&wl->mutex);
222	ret = wl1271_cmd_configure(wl, ie_id, buf, buf_len);
223	mutex_unlock(&wl->mutex);
224
225	if (ret < 0) {
226		wl1271_warning("testmode cmd configure failed: %d", ret);
227		return ret;
228	}
229
230	return 0;
231}
232
233static int wl1271_tm_detect_fem(struct wl1271 *wl, struct nlattr *tb[])
234{
235	/* return FEM type */
236	int ret, len;
237	struct sk_buff *skb;
238
239	ret = wl1271_plt_start(wl, PLT_FEM_DETECT);
240	if (ret < 0)
241		goto out;
242
243	mutex_lock(&wl->mutex);
244
245	len = nla_total_size(sizeof(wl->fem_manuf));
246	skb = cfg80211_testmode_alloc_reply_skb(wl->hw->wiphy, len);
247	if (!skb) {
248		ret = -ENOMEM;
249		goto out_mutex;
250	}
251
252	if (nla_put(skb, WL1271_TM_ATTR_DATA, sizeof(wl->fem_manuf),
253					      &wl->fem_manuf)) {
254		kfree_skb(skb);
255		ret = -EMSGSIZE;
256		goto out_mutex;
257	}
258
259	ret = cfg80211_testmode_reply(skb);
260
261out_mutex:
262	mutex_unlock(&wl->mutex);
263
264	/* We always stop plt after DETECT mode */
265	wl1271_plt_stop(wl);
266out:
267	return ret;
268}
269
270static int wl1271_tm_cmd_set_plt_mode(struct wl1271 *wl, struct nlattr *tb[])
271{
272	u32 val;
273	int ret;
274
275	wl1271_debug(DEBUG_TESTMODE, "testmode cmd set plt mode");
276
277	if (!tb[WL1271_TM_ATTR_PLT_MODE])
278		return -EINVAL;
279
280	val = nla_get_u32(tb[WL1271_TM_ATTR_PLT_MODE]);
281
282	switch (val) {
283	case PLT_OFF:
284		ret = wl1271_plt_stop(wl);
285		break;
286	case PLT_ON:
287	case PLT_CHIP_AWAKE:
288		ret = wl1271_plt_start(wl, val);
289		break;
290	case PLT_FEM_DETECT:
291		ret = wl1271_tm_detect_fem(wl, tb);
292		break;
293	default:
294		ret = -EINVAL;
295		break;
296	}
297
298	return ret;
299}
300
301static int wl12xx_tm_cmd_get_mac(struct wl1271 *wl, struct nlattr *tb[])
302{
303	struct sk_buff *skb;
304	u8 mac_addr[ETH_ALEN];
305	int ret = 0;
306
307	mutex_lock(&wl->mutex);
308
309	if (!wl->plt) {
310		ret = -EINVAL;
311		goto out;
312	}
313
314	if (wl->fuse_oui_addr == 0 && wl->fuse_nic_addr == 0) {
315		ret = -EOPNOTSUPP;
316		goto out;
317	}
318
319	mac_addr[0] = (u8)(wl->fuse_oui_addr >> 16);
320	mac_addr[1] = (u8)(wl->fuse_oui_addr >> 8);
321	mac_addr[2] = (u8) wl->fuse_oui_addr;
322	mac_addr[3] = (u8)(wl->fuse_nic_addr >> 16);
323	mac_addr[4] = (u8)(wl->fuse_nic_addr >> 8);
324	mac_addr[5] = (u8) wl->fuse_nic_addr;
325
326	skb = cfg80211_testmode_alloc_reply_skb(wl->hw->wiphy, ETH_ALEN);
327	if (!skb) {
328		ret = -ENOMEM;
329		goto out;
330	}
331
332	if (nla_put(skb, WL1271_TM_ATTR_DATA, ETH_ALEN, mac_addr)) {
333		kfree_skb(skb);
334		ret = -EMSGSIZE;
335		goto out;
336	}
337
338	ret = cfg80211_testmode_reply(skb);
339	if (ret < 0)
340		goto out;
341
342out:
343	mutex_unlock(&wl->mutex);
344	return ret;
345}
346
347int wl1271_tm_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
348		  void *data, int len)
349{
350	struct wl1271 *wl = hw->priv;
351	struct nlattr *tb[WL1271_TM_ATTR_MAX + 1];
352	u32 nla_cmd;
353	int err;
354
355	err = nla_parse_deprecated(tb, WL1271_TM_ATTR_MAX, data, len,
356				   wl1271_tm_policy, NULL);
357	if (err)
358		return err;
359
360	if (!tb[WL1271_TM_ATTR_CMD_ID])
361		return -EINVAL;
362
363	nla_cmd = nla_get_u32(tb[WL1271_TM_ATTR_CMD_ID]);
364
365	/* Only SET_PLT_MODE is allowed in case of mode PLT_CHIP_AWAKE */
366	if (wl->plt_mode == PLT_CHIP_AWAKE &&
367	    nla_cmd != WL1271_TM_CMD_SET_PLT_MODE)
368		return -EOPNOTSUPP;
369
370	switch (nla_cmd) {
371	case WL1271_TM_CMD_TEST:
372		return wl1271_tm_cmd_test(wl, tb);
373	case WL1271_TM_CMD_INTERROGATE:
374		return wl1271_tm_cmd_interrogate(wl, tb);
375	case WL1271_TM_CMD_CONFIGURE:
376		return wl1271_tm_cmd_configure(wl, tb);
377	case WL1271_TM_CMD_SET_PLT_MODE:
378		return wl1271_tm_cmd_set_plt_mode(wl, tb);
379	case WL1271_TM_CMD_GET_MAC:
380		return wl12xx_tm_cmd_get_mac(wl, tb);
381	default:
382		return -EOPNOTSUPP;
383	}
384}