]>
Commit | Line | Data |
---|---|---|
c8c90873 KV |
1 | /* |
2 | * This file is part of wl1271 | |
3 | * | |
4 | * Copyright (C) 2010 Nokia Corporation | |
5 | * | |
6 | * Contact: Luciano Coelho <luciano.coelho@nokia.com> | |
7 | * | |
8 | * This program is free software; you can redistribute it and/or | |
9 | * modify it under the terms of the GNU General Public License | |
10 | * version 2 as published by the Free Software Foundation. | |
11 | * | |
12 | * This program is distributed in the hope that it will be useful, but | |
13 | * WITHOUT ANY WARRANTY; without even the implied warranty of | |
14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU | |
15 | * General Public License for more details. | |
16 | * | |
17 | * You should have received a copy of the GNU General Public License | |
18 | * along with this program; if not, write to the Free Software | |
19 | * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA | |
20 | * 02110-1301 USA | |
21 | * | |
22 | */ | |
00d20100 | 23 | #include "testmode.h" |
c8c90873 | 24 | |
5a0e3ad6 | 25 | #include <linux/slab.h> |
c8c90873 KV |
26 | #include <net/genetlink.h> |
27 | ||
00d20100 SL |
28 | #include "wl12xx.h" |
29 | #include "acx.h" | |
bc765bf3 | 30 | #include "reg.h" |
c8c90873 KV |
31 | |
32 | #define WL1271_TM_MAX_DATA_LENGTH 1024 | |
33 | ||
34 | enum wl1271_tm_commands { | |
35 | WL1271_TM_CMD_UNSPEC, | |
36 | WL1271_TM_CMD_TEST, | |
37 | WL1271_TM_CMD_INTERROGATE, | |
38 | WL1271_TM_CMD_CONFIGURE, | |
c8c90873 | 39 | WL1271_TM_CMD_SET_PLT_MODE, |
e285a525 | 40 | WL1271_TM_CMD_RECOVER, |
c8c90873 KV |
41 | |
42 | __WL1271_TM_CMD_AFTER_LAST | |
43 | }; | |
44 | #define WL1271_TM_CMD_MAX (__WL1271_TM_CMD_AFTER_LAST - 1) | |
45 | ||
46 | enum wl1271_tm_attrs { | |
47 | WL1271_TM_ATTR_UNSPEC, | |
48 | WL1271_TM_ATTR_CMD_ID, | |
49 | WL1271_TM_ATTR_ANSWER, | |
50 | WL1271_TM_ATTR_DATA, | |
51 | WL1271_TM_ATTR_IE_ID, | |
52 | WL1271_TM_ATTR_PLT_MODE, | |
53 | ||
54 | __WL1271_TM_ATTR_AFTER_LAST | |
55 | }; | |
56 | #define WL1271_TM_ATTR_MAX (__WL1271_TM_ATTR_AFTER_LAST - 1) | |
57 | ||
58 | static struct nla_policy wl1271_tm_policy[WL1271_TM_ATTR_MAX + 1] = { | |
59 | [WL1271_TM_ATTR_CMD_ID] = { .type = NLA_U32 }, | |
60 | [WL1271_TM_ATTR_ANSWER] = { .type = NLA_U8 }, | |
61 | [WL1271_TM_ATTR_DATA] = { .type = NLA_BINARY, | |
62 | .len = WL1271_TM_MAX_DATA_LENGTH }, | |
63 | [WL1271_TM_ATTR_IE_ID] = { .type = NLA_U32 }, | |
64 | [WL1271_TM_ATTR_PLT_MODE] = { .type = NLA_U32 }, | |
65 | }; | |
66 | ||
67 | ||
68 | static int wl1271_tm_cmd_test(struct wl1271 *wl, struct nlattr *tb[]) | |
69 | { | |
70 | int buf_len, ret, len; | |
71 | struct sk_buff *skb; | |
72 | void *buf; | |
73 | u8 answer = 0; | |
74 | ||
75 | wl1271_debug(DEBUG_TESTMODE, "testmode cmd test"); | |
76 | ||
77 | if (!tb[WL1271_TM_ATTR_DATA]) | |
78 | return -EINVAL; | |
79 | ||
80 | buf = nla_data(tb[WL1271_TM_ATTR_DATA]); | |
81 | buf_len = nla_len(tb[WL1271_TM_ATTR_DATA]); | |
82 | ||
83 | if (tb[WL1271_TM_ATTR_ANSWER]) | |
84 | answer = nla_get_u8(tb[WL1271_TM_ATTR_ANSWER]); | |
85 | ||
86 | if (buf_len > sizeof(struct wl1271_command)) | |
87 | return -EMSGSIZE; | |
88 | ||
89 | mutex_lock(&wl->mutex); | |
90 | ret = wl1271_cmd_test(wl, buf, buf_len, answer); | |
91 | mutex_unlock(&wl->mutex); | |
92 | ||
93 | if (ret < 0) { | |
94 | wl1271_warning("testmode cmd test failed: %d", ret); | |
95 | return ret; | |
96 | } | |
97 | ||
98 | if (answer) { | |
99 | len = nla_total_size(buf_len); | |
100 | skb = cfg80211_testmode_alloc_reply_skb(wl->hw->wiphy, len); | |
101 | if (!skb) | |
102 | return -ENOMEM; | |
103 | ||
104 | NLA_PUT(skb, WL1271_TM_ATTR_DATA, buf_len, buf); | |
105 | ret = cfg80211_testmode_reply(skb); | |
106 | if (ret < 0) | |
107 | return ret; | |
108 | } | |
109 | ||
110 | return 0; | |
111 | ||
112 | nla_put_failure: | |
113 | kfree_skb(skb); | |
114 | return -EMSGSIZE; | |
115 | } | |
116 | ||
117 | static int wl1271_tm_cmd_interrogate(struct wl1271 *wl, struct nlattr *tb[]) | |
118 | { | |
119 | int ret; | |
120 | struct wl1271_command *cmd; | |
121 | struct sk_buff *skb; | |
122 | u8 ie_id; | |
123 | ||
124 | wl1271_debug(DEBUG_TESTMODE, "testmode cmd interrogate"); | |
125 | ||
126 | if (!tb[WL1271_TM_ATTR_IE_ID]) | |
127 | return -EINVAL; | |
128 | ||
129 | ie_id = nla_get_u8(tb[WL1271_TM_ATTR_IE_ID]); | |
130 | ||
131 | cmd = kzalloc(sizeof(*cmd), GFP_KERNEL); | |
132 | if (!cmd) | |
133 | return -ENOMEM; | |
134 | ||
135 | mutex_lock(&wl->mutex); | |
136 | ret = wl1271_cmd_interrogate(wl, ie_id, cmd, sizeof(*cmd)); | |
137 | mutex_unlock(&wl->mutex); | |
138 | ||
139 | if (ret < 0) { | |
140 | wl1271_warning("testmode cmd interrogate failed: %d", ret); | |
f8afdf48 | 141 | kfree(cmd); |
c8c90873 KV |
142 | return ret; |
143 | } | |
144 | ||
145 | skb = cfg80211_testmode_alloc_reply_skb(wl->hw->wiphy, sizeof(*cmd)); | |
f8afdf48 JL |
146 | if (!skb) { |
147 | kfree(cmd); | |
c8c90873 | 148 | return -ENOMEM; |
f8afdf48 | 149 | } |
c8c90873 KV |
150 | |
151 | NLA_PUT(skb, WL1271_TM_ATTR_DATA, sizeof(*cmd), cmd); | |
152 | ||
153 | return 0; | |
154 | ||
155 | nla_put_failure: | |
156 | kfree_skb(skb); | |
157 | return -EMSGSIZE; | |
158 | } | |
159 | ||
160 | static int wl1271_tm_cmd_configure(struct wl1271 *wl, struct nlattr *tb[]) | |
161 | { | |
162 | int buf_len, ret; | |
163 | void *buf; | |
164 | u8 ie_id; | |
165 | ||
166 | wl1271_debug(DEBUG_TESTMODE, "testmode cmd configure"); | |
167 | ||
168 | if (!tb[WL1271_TM_ATTR_DATA]) | |
169 | return -EINVAL; | |
170 | if (!tb[WL1271_TM_ATTR_IE_ID]) | |
171 | return -EINVAL; | |
172 | ||
173 | ie_id = nla_get_u8(tb[WL1271_TM_ATTR_IE_ID]); | |
174 | buf = nla_data(tb[WL1271_TM_ATTR_DATA]); | |
175 | buf_len = nla_len(tb[WL1271_TM_ATTR_DATA]); | |
176 | ||
177 | if (buf_len > sizeof(struct wl1271_command)) | |
178 | return -EMSGSIZE; | |
179 | ||
180 | mutex_lock(&wl->mutex); | |
181 | ret = wl1271_cmd_configure(wl, ie_id, buf, buf_len); | |
182 | mutex_unlock(&wl->mutex); | |
183 | ||
184 | if (ret < 0) { | |
185 | wl1271_warning("testmode cmd configure failed: %d", ret); | |
186 | return ret; | |
187 | } | |
188 | ||
189 | return 0; | |
190 | } | |
191 | ||
c8c90873 KV |
192 | static int wl1271_tm_cmd_set_plt_mode(struct wl1271 *wl, struct nlattr *tb[]) |
193 | { | |
194 | u32 val; | |
195 | int ret; | |
196 | ||
197 | wl1271_debug(DEBUG_TESTMODE, "testmode cmd set plt mode"); | |
198 | ||
199 | if (!tb[WL1271_TM_ATTR_PLT_MODE]) | |
200 | return -EINVAL; | |
201 | ||
202 | val = nla_get_u32(tb[WL1271_TM_ATTR_PLT_MODE]); | |
203 | ||
204 | switch (val) { | |
205 | case 0: | |
206 | ret = wl1271_plt_stop(wl); | |
207 | break; | |
208 | case 1: | |
209 | ret = wl1271_plt_start(wl); | |
210 | break; | |
211 | default: | |
212 | ret = -EINVAL; | |
213 | break; | |
214 | } | |
215 | ||
216 | return ret; | |
217 | } | |
218 | ||
e285a525 EP |
219 | static int wl1271_tm_cmd_recover(struct wl1271 *wl, struct nlattr *tb[]) |
220 | { | |
221 | wl1271_debug(DEBUG_TESTMODE, "testmode cmd recover"); | |
222 | ||
baacb9ae | 223 | wl12xx_queue_recovery_work(wl); |
e285a525 EP |
224 | |
225 | return 0; | |
226 | } | |
227 | ||
c8c90873 KV |
228 | int wl1271_tm_cmd(struct ieee80211_hw *hw, void *data, int len) |
229 | { | |
230 | struct wl1271 *wl = hw->priv; | |
231 | struct nlattr *tb[WL1271_TM_ATTR_MAX + 1]; | |
232 | int err; | |
233 | ||
234 | err = nla_parse(tb, WL1271_TM_ATTR_MAX, data, len, wl1271_tm_policy); | |
235 | if (err) | |
236 | return err; | |
237 | ||
238 | if (!tb[WL1271_TM_ATTR_CMD_ID]) | |
239 | return -EINVAL; | |
240 | ||
241 | switch (nla_get_u32(tb[WL1271_TM_ATTR_CMD_ID])) { | |
242 | case WL1271_TM_CMD_TEST: | |
243 | return wl1271_tm_cmd_test(wl, tb); | |
244 | case WL1271_TM_CMD_INTERROGATE: | |
245 | return wl1271_tm_cmd_interrogate(wl, tb); | |
246 | case WL1271_TM_CMD_CONFIGURE: | |
247 | return wl1271_tm_cmd_configure(wl, tb); | |
c8c90873 KV |
248 | case WL1271_TM_CMD_SET_PLT_MODE: |
249 | return wl1271_tm_cmd_set_plt_mode(wl, tb); | |
e285a525 EP |
250 | case WL1271_TM_CMD_RECOVER: |
251 | return wl1271_tm_cmd_recover(wl, tb); | |
c8c90873 KV |
252 | default: |
253 | return -EOPNOTSUPP; | |
254 | } | |
255 | } |