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 | |
22 | enum 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 | |
36 | enum 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 | |
48 | static 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 | |
58 | static 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(nla: tb[WL1271_TM_ATTR_DATA]); |
71 | buf_len = nla_len(nla: tb[WL1271_TM_ATTR_DATA]); |
72 | |
73 | if (tb[WL1271_TM_ATTR_ANSWER]) |
74 | answer = nla_get_u8(nla: 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(dev: 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(payload: buf_len); |
112 | skb = cfg80211_testmode_alloc_reply_skb(wiphy: wl->hw->wiphy, approxlen: len); |
113 | if (!skb) { |
114 | ret = -ENOMEM; |
115 | goto out_sleep; |
116 | } |
117 | |
118 | if (nla_put(skb, attrtype: WL1271_TM_ATTR_DATA, attrlen: buf_len, data: 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 | |
129 | out_sleep: |
130 | pm_runtime_mark_last_busy(dev: wl->dev); |
131 | pm_runtime_put_autosuspend(dev: wl->dev); |
132 | out: |
133 | mutex_unlock(lock: &wl->mutex); |
134 | |
135 | return ret; |
136 | } |
137 | |
138 | static int wl1271_tm_cmd_interrogate(struct wl1271 *wl, struct nlattr *tb[]) |
139 | { |
140 | int ret; |
141 | struct wl1271_command *cmd; |
142 | struct sk_buff *skb; |
143 | u8 ie_id; |
144 | |
145 | wl1271_debug(DEBUG_TESTMODE, "testmode cmd interrogate" ); |
146 | |
147 | if (!tb[WL1271_TM_ATTR_IE_ID]) |
148 | return -EINVAL; |
149 | |
150 | ie_id = nla_get_u8(nla: tb[WL1271_TM_ATTR_IE_ID]); |
151 | |
152 | mutex_lock(&wl->mutex); |
153 | |
154 | if (unlikely(wl->state != WLCORE_STATE_ON)) { |
155 | ret = -EINVAL; |
156 | goto out; |
157 | } |
158 | |
159 | ret = pm_runtime_resume_and_get(dev: wl->dev); |
160 | if (ret < 0) |
161 | goto out; |
162 | |
163 | cmd = kzalloc(size: sizeof(*cmd), GFP_KERNEL); |
164 | if (!cmd) { |
165 | ret = -ENOMEM; |
166 | goto out_sleep; |
167 | } |
168 | |
169 | ret = wl1271_cmd_interrogate(wl, id: ie_id, buf: cmd, |
170 | cmd_len: sizeof(struct acx_header), res_len: sizeof(*cmd)); |
171 | if (ret < 0) { |
172 | wl1271_warning("testmode cmd interrogate failed: %d" , ret); |
173 | goto out_free; |
174 | } |
175 | |
176 | skb = cfg80211_testmode_alloc_reply_skb(wiphy: wl->hw->wiphy, approxlen: sizeof(*cmd)); |
177 | if (!skb) { |
178 | ret = -ENOMEM; |
179 | goto out_free; |
180 | } |
181 | |
182 | if (nla_put(skb, attrtype: WL1271_TM_ATTR_DATA, attrlen: sizeof(*cmd), data: cmd)) { |
183 | kfree_skb(skb); |
184 | ret = -EMSGSIZE; |
185 | goto out_free; |
186 | } |
187 | |
188 | ret = cfg80211_testmode_reply(skb); |
189 | if (ret < 0) |
190 | goto out_free; |
191 | |
192 | out_free: |
193 | kfree(objp: cmd); |
194 | out_sleep: |
195 | pm_runtime_mark_last_busy(dev: wl->dev); |
196 | pm_runtime_put_autosuspend(dev: wl->dev); |
197 | out: |
198 | mutex_unlock(lock: &wl->mutex); |
199 | |
200 | return ret; |
201 | } |
202 | |
203 | static int wl1271_tm_cmd_configure(struct wl1271 *wl, struct nlattr *tb[]) |
204 | { |
205 | int buf_len, ret; |
206 | void *buf; |
207 | u8 ie_id; |
208 | |
209 | wl1271_debug(DEBUG_TESTMODE, "testmode cmd configure" ); |
210 | |
211 | if (!tb[WL1271_TM_ATTR_DATA]) |
212 | return -EINVAL; |
213 | if (!tb[WL1271_TM_ATTR_IE_ID]) |
214 | return -EINVAL; |
215 | |
216 | ie_id = nla_get_u8(nla: tb[WL1271_TM_ATTR_IE_ID]); |
217 | buf = nla_data(nla: tb[WL1271_TM_ATTR_DATA]); |
218 | buf_len = nla_len(nla: tb[WL1271_TM_ATTR_DATA]); |
219 | |
220 | if (buf_len > sizeof(struct wl1271_command)) |
221 | return -EMSGSIZE; |
222 | |
223 | mutex_lock(&wl->mutex); |
224 | ret = wl1271_cmd_configure(wl, id: ie_id, buf, len: buf_len); |
225 | mutex_unlock(lock: &wl->mutex); |
226 | |
227 | if (ret < 0) { |
228 | wl1271_warning("testmode cmd configure failed: %d" , ret); |
229 | return ret; |
230 | } |
231 | |
232 | return 0; |
233 | } |
234 | |
235 | static int wl1271_tm_detect_fem(struct wl1271 *wl, struct nlattr *tb[]) |
236 | { |
237 | /* return FEM type */ |
238 | int ret, len; |
239 | struct sk_buff *skb; |
240 | |
241 | ret = wl1271_plt_start(wl, plt_mode: PLT_FEM_DETECT); |
242 | if (ret < 0) |
243 | goto out; |
244 | |
245 | mutex_lock(&wl->mutex); |
246 | |
247 | len = nla_total_size(payload: sizeof(wl->fem_manuf)); |
248 | skb = cfg80211_testmode_alloc_reply_skb(wiphy: wl->hw->wiphy, approxlen: len); |
249 | if (!skb) { |
250 | ret = -ENOMEM; |
251 | goto out_mutex; |
252 | } |
253 | |
254 | if (nla_put(skb, attrtype: WL1271_TM_ATTR_DATA, attrlen: sizeof(wl->fem_manuf), |
255 | data: &wl->fem_manuf)) { |
256 | kfree_skb(skb); |
257 | ret = -EMSGSIZE; |
258 | goto out_mutex; |
259 | } |
260 | |
261 | ret = cfg80211_testmode_reply(skb); |
262 | |
263 | out_mutex: |
264 | mutex_unlock(lock: &wl->mutex); |
265 | |
266 | /* We always stop plt after DETECT mode */ |
267 | wl1271_plt_stop(wl); |
268 | out: |
269 | return ret; |
270 | } |
271 | |
272 | static int wl1271_tm_cmd_set_plt_mode(struct wl1271 *wl, struct nlattr *tb[]) |
273 | { |
274 | u32 val; |
275 | int ret; |
276 | |
277 | wl1271_debug(DEBUG_TESTMODE, "testmode cmd set plt mode" ); |
278 | |
279 | if (!tb[WL1271_TM_ATTR_PLT_MODE]) |
280 | return -EINVAL; |
281 | |
282 | val = nla_get_u32(nla: tb[WL1271_TM_ATTR_PLT_MODE]); |
283 | |
284 | switch (val) { |
285 | case PLT_OFF: |
286 | ret = wl1271_plt_stop(wl); |
287 | break; |
288 | case PLT_ON: |
289 | case PLT_CHIP_AWAKE: |
290 | ret = wl1271_plt_start(wl, plt_mode: val); |
291 | break; |
292 | case PLT_FEM_DETECT: |
293 | ret = wl1271_tm_detect_fem(wl, tb); |
294 | break; |
295 | default: |
296 | ret = -EINVAL; |
297 | break; |
298 | } |
299 | |
300 | return ret; |
301 | } |
302 | |
303 | static int wl12xx_tm_cmd_get_mac(struct wl1271 *wl, struct nlattr *tb[]) |
304 | { |
305 | struct sk_buff *skb; |
306 | u8 mac_addr[ETH_ALEN]; |
307 | int ret = 0; |
308 | |
309 | mutex_lock(&wl->mutex); |
310 | |
311 | if (!wl->plt) { |
312 | ret = -EINVAL; |
313 | goto out; |
314 | } |
315 | |
316 | if (wl->fuse_oui_addr == 0 && wl->fuse_nic_addr == 0) { |
317 | ret = -EOPNOTSUPP; |
318 | goto out; |
319 | } |
320 | |
321 | mac_addr[0] = (u8)(wl->fuse_oui_addr >> 16); |
322 | mac_addr[1] = (u8)(wl->fuse_oui_addr >> 8); |
323 | mac_addr[2] = (u8) wl->fuse_oui_addr; |
324 | mac_addr[3] = (u8)(wl->fuse_nic_addr >> 16); |
325 | mac_addr[4] = (u8)(wl->fuse_nic_addr >> 8); |
326 | mac_addr[5] = (u8) wl->fuse_nic_addr; |
327 | |
328 | skb = cfg80211_testmode_alloc_reply_skb(wiphy: wl->hw->wiphy, ETH_ALEN); |
329 | if (!skb) { |
330 | ret = -ENOMEM; |
331 | goto out; |
332 | } |
333 | |
334 | if (nla_put(skb, attrtype: WL1271_TM_ATTR_DATA, ETH_ALEN, data: mac_addr)) { |
335 | kfree_skb(skb); |
336 | ret = -EMSGSIZE; |
337 | goto out; |
338 | } |
339 | |
340 | ret = cfg80211_testmode_reply(skb); |
341 | if (ret < 0) |
342 | goto out; |
343 | |
344 | out: |
345 | mutex_unlock(lock: &wl->mutex); |
346 | return ret; |
347 | } |
348 | |
349 | int wl1271_tm_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif, |
350 | void *data, int len) |
351 | { |
352 | struct wl1271 *wl = hw->priv; |
353 | struct nlattr *tb[WL1271_TM_ATTR_MAX + 1]; |
354 | u32 nla_cmd; |
355 | int err; |
356 | |
357 | err = nla_parse_deprecated(tb, WL1271_TM_ATTR_MAX, head: data, len, |
358 | policy: wl1271_tm_policy, NULL); |
359 | if (err) |
360 | return err; |
361 | |
362 | if (!tb[WL1271_TM_ATTR_CMD_ID]) |
363 | return -EINVAL; |
364 | |
365 | nla_cmd = nla_get_u32(nla: tb[WL1271_TM_ATTR_CMD_ID]); |
366 | |
367 | /* Only SET_PLT_MODE is allowed in case of mode PLT_CHIP_AWAKE */ |
368 | if (wl->plt_mode == PLT_CHIP_AWAKE && |
369 | nla_cmd != WL1271_TM_CMD_SET_PLT_MODE) |
370 | return -EOPNOTSUPP; |
371 | |
372 | switch (nla_cmd) { |
373 | case WL1271_TM_CMD_TEST: |
374 | return wl1271_tm_cmd_test(wl, tb); |
375 | case WL1271_TM_CMD_INTERROGATE: |
376 | return wl1271_tm_cmd_interrogate(wl, tb); |
377 | case WL1271_TM_CMD_CONFIGURE: |
378 | return wl1271_tm_cmd_configure(wl, tb); |
379 | case WL1271_TM_CMD_SET_PLT_MODE: |
380 | return wl1271_tm_cmd_set_plt_mode(wl, tb); |
381 | case WL1271_TM_CMD_GET_MAC: |
382 | return wl12xx_tm_cmd_get_mac(wl, tb); |
383 | default: |
384 | return -EOPNOTSUPP; |
385 | } |
386 | } |
387 | |