]>
Commit | Line | Data |
---|---|---|
e377e9d3 RC |
1 | /* |
2 | * WiMedia Logical Link Control Protocol (WLP) | |
3 | * Message construction and parsing | |
4 | * | |
5 | * Copyright (C) 2007 Intel Corporation | |
6 | * Reinette Chatre <reinette.chatre@intel.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 version | |
10 | * 2 as published by the Free Software Foundation. | |
11 | * | |
12 | * This program is distributed in the hope that it will be useful, | |
13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of | |
14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |
15 | * GNU 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 Street, Fifth Floor, Boston, MA | |
20 | * 02110-1301, USA. | |
21 | * | |
22 | * | |
23 | * FIXME: docs | |
24 | */ | |
25 | ||
26 | #include <linux/wlp.h> | |
bce83697 | 27 | |
e377e9d3 RC |
28 | #include "wlp-internal.h" |
29 | ||
30 | static | |
31 | const char *__wlp_assoc_frame[] = { | |
32 | [WLP_ASSOC_D1] = "WLP_ASSOC_D1", | |
33 | [WLP_ASSOC_D2] = "WLP_ASSOC_D2", | |
34 | [WLP_ASSOC_M1] = "WLP_ASSOC_M1", | |
35 | [WLP_ASSOC_M2] = "WLP_ASSOC_M2", | |
36 | [WLP_ASSOC_M3] = "WLP_ASSOC_M3", | |
37 | [WLP_ASSOC_M4] = "WLP_ASSOC_M4", | |
38 | [WLP_ASSOC_M5] = "WLP_ASSOC_M5", | |
39 | [WLP_ASSOC_M6] = "WLP_ASSOC_M6", | |
40 | [WLP_ASSOC_M7] = "WLP_ASSOC_M7", | |
41 | [WLP_ASSOC_M8] = "WLP_ASSOC_M8", | |
42 | [WLP_ASSOC_F0] = "WLP_ASSOC_F0", | |
43 | [WLP_ASSOC_E1] = "WLP_ASSOC_E1", | |
44 | [WLP_ASSOC_E2] = "WLP_ASSOC_E2", | |
45 | [WLP_ASSOC_C1] = "WLP_ASSOC_C1", | |
46 | [WLP_ASSOC_C2] = "WLP_ASSOC_C2", | |
47 | [WLP_ASSOC_C3] = "WLP_ASSOC_C3", | |
48 | [WLP_ASSOC_C4] = "WLP_ASSOC_C4", | |
49 | }; | |
50 | ||
51 | static const char *wlp_assoc_frame_str(unsigned id) | |
52 | { | |
53 | if (id >= ARRAY_SIZE(__wlp_assoc_frame)) | |
54 | return "unknown association frame"; | |
55 | return __wlp_assoc_frame[id]; | |
56 | } | |
57 | ||
58 | static const char *__wlp_assc_error[] = { | |
59 | "none", | |
60 | "Authenticator Failure", | |
61 | "Rogue activity suspected", | |
62 | "Device busy", | |
63 | "Setup Locked", | |
64 | "Registrar not ready", | |
65 | "Invalid WSS selection", | |
66 | "Message timeout", | |
67 | "Enrollment session timeout", | |
68 | "Device password invalid", | |
69 | "Unsupported version", | |
70 | "Internal error", | |
71 | "Undefined error", | |
72 | "Numeric comparison failure", | |
73 | "Waiting for user input", | |
74 | }; | |
75 | ||
76 | static const char *wlp_assc_error_str(unsigned id) | |
77 | { | |
78 | if (id >= ARRAY_SIZE(__wlp_assc_error)) | |
79 | return "unknown WLP association error"; | |
80 | return __wlp_assc_error[id]; | |
81 | } | |
82 | ||
83 | static inline void wlp_set_attr_hdr(struct wlp_attr_hdr *hdr, unsigned type, | |
84 | size_t len) | |
85 | { | |
86 | hdr->type = cpu_to_le16(type); | |
87 | hdr->length = cpu_to_le16(len); | |
88 | } | |
89 | ||
90 | /* | |
91 | * Populate fields of a constant sized attribute | |
92 | * | |
93 | * @returns: total size of attribute including size of new value | |
94 | * | |
95 | * We have two instances of this function (wlp_pset and wlp_set): one takes | |
96 | * the value as a parameter, the other takes a pointer to the value as | |
97 | * parameter. They thus only differ in how the value is assigned to the | |
98 | * attribute. | |
99 | * | |
100 | * We use sizeof(*attr) - sizeof(struct wlp_attr_hdr) instead of | |
101 | * sizeof(type) to be able to use this same code for the structures that | |
102 | * contain 8bit enum values and be able to deal with pointer types. | |
103 | */ | |
104 | #define wlp_set(type, type_code, name) \ | |
105 | static size_t wlp_set_##name(struct wlp_attr_##name *attr, type value) \ | |
106 | { \ | |
e377e9d3 RC |
107 | wlp_set_attr_hdr(&attr->hdr, type_code, \ |
108 | sizeof(*attr) - sizeof(struct wlp_attr_hdr)); \ | |
109 | attr->name = value; \ | |
e377e9d3 RC |
110 | return sizeof(*attr); \ |
111 | } | |
112 | ||
113 | #define wlp_pset(type, type_code, name) \ | |
114 | static size_t wlp_set_##name(struct wlp_attr_##name *attr, type value) \ | |
115 | { \ | |
e377e9d3 RC |
116 | wlp_set_attr_hdr(&attr->hdr, type_code, \ |
117 | sizeof(*attr) - sizeof(struct wlp_attr_hdr)); \ | |
118 | attr->name = *value; \ | |
e377e9d3 RC |
119 | return sizeof(*attr); \ |
120 | } | |
121 | ||
122 | /** | |
123 | * Populate fields of a variable attribute | |
124 | * | |
125 | * @returns: total size of attribute including size of new value | |
126 | * | |
127 | * Provided with a pointer to the memory area reserved for the | |
128 | * attribute structure, the field is populated with the value. The | |
129 | * reserved memory has to contain enough space for the value. | |
130 | */ | |
131 | #define wlp_vset(type, type_code, name) \ | |
132 | static size_t wlp_set_##name(struct wlp_attr_##name *attr, type value, \ | |
133 | size_t len) \ | |
134 | { \ | |
e377e9d3 RC |
135 | wlp_set_attr_hdr(&attr->hdr, type_code, len); \ |
136 | memcpy(attr->name, value, len); \ | |
e377e9d3 RC |
137 | return sizeof(*attr) + len; \ |
138 | } | |
139 | ||
140 | wlp_vset(char *, WLP_ATTR_DEV_NAME, dev_name) | |
141 | wlp_vset(char *, WLP_ATTR_MANUF, manufacturer) | |
142 | wlp_set(enum wlp_assoc_type, WLP_ATTR_MSG_TYPE, msg_type) | |
143 | wlp_vset(char *, WLP_ATTR_MODEL_NAME, model_name) | |
144 | wlp_vset(char *, WLP_ATTR_MODEL_NR, model_nr) | |
145 | wlp_vset(char *, WLP_ATTR_SERIAL, serial) | |
146 | wlp_vset(char *, WLP_ATTR_WSS_NAME, wss_name) | |
147 | wlp_pset(struct wlp_uuid *, WLP_ATTR_UUID_E, uuid_e) | |
148 | wlp_pset(struct wlp_uuid *, WLP_ATTR_UUID_R, uuid_r) | |
149 | wlp_pset(struct wlp_uuid *, WLP_ATTR_WSSID, wssid) | |
150 | wlp_pset(struct wlp_dev_type *, WLP_ATTR_PRI_DEV_TYPE, prim_dev_type) | |
151 | /*wlp_pset(struct wlp_dev_type *, WLP_ATTR_SEC_DEV_TYPE, sec_dev_type)*/ | |
152 | wlp_set(u8, WLP_ATTR_WLP_VER, version) | |
153 | wlp_set(enum wlp_assc_error, WLP_ATTR_WLP_ASSC_ERR, wlp_assc_err) | |
154 | wlp_set(enum wlp_wss_sel_mthd, WLP_ATTR_WSS_SEL_MTHD, wss_sel_mthd) | |
155 | wlp_set(u8, WLP_ATTR_ACC_ENRL, accept_enrl) | |
156 | wlp_set(u8, WLP_ATTR_WSS_SEC_STAT, wss_sec_status) | |
157 | wlp_pset(struct uwb_mac_addr *, WLP_ATTR_WSS_BCAST, wss_bcast) | |
158 | wlp_pset(struct wlp_nonce *, WLP_ATTR_ENRL_NONCE, enonce) | |
159 | wlp_pset(struct wlp_nonce *, WLP_ATTR_REG_NONCE, rnonce) | |
160 | wlp_set(u8, WLP_ATTR_WSS_TAG, wss_tag) | |
161 | wlp_pset(struct uwb_mac_addr *, WLP_ATTR_WSS_VIRT, wss_virt) | |
162 | ||
163 | /** | |
164 | * Fill in the WSS information attributes | |
165 | * | |
166 | * We currently only support one WSS, and this is assumed in this function | |
167 | * that can populate only one WSS information attribute. | |
168 | */ | |
169 | static size_t wlp_set_wss_info(struct wlp_attr_wss_info *attr, | |
170 | struct wlp_wss *wss) | |
171 | { | |
172 | size_t datalen; | |
173 | void *ptr = attr->wss_info; | |
174 | size_t used = sizeof(*attr); | |
bce83697 | 175 | |
e377e9d3 RC |
176 | datalen = sizeof(struct wlp_wss_info) + strlen(wss->name); |
177 | wlp_set_attr_hdr(&attr->hdr, WLP_ATTR_WSS_INFO, datalen); | |
178 | used = wlp_set_wssid(ptr, &wss->wssid); | |
179 | used += wlp_set_wss_name(ptr + used, wss->name, strlen(wss->name)); | |
180 | used += wlp_set_accept_enrl(ptr + used, wss->accept_enroll); | |
181 | used += wlp_set_wss_sec_status(ptr + used, wss->secure_status); | |
182 | used += wlp_set_wss_bcast(ptr + used, &wss->bcast); | |
e377e9d3 RC |
183 | return sizeof(*attr) + used; |
184 | } | |
185 | ||
186 | /** | |
187 | * Verify attribute header | |
188 | * | |
189 | * @hdr: Pointer to attribute header that will be verified. | |
190 | * @type: Expected attribute type. | |
191 | * @len: Expected length of attribute value (excluding header). | |
192 | * | |
193 | * Most attribute values have a known length even when they do have a | |
194 | * length field. This knowledge can be used via this function to verify | |
195 | * that the length field matches the expected value. | |
196 | */ | |
197 | static int wlp_check_attr_hdr(struct wlp *wlp, struct wlp_attr_hdr *hdr, | |
198 | enum wlp_attr_type type, unsigned len) | |
199 | { | |
200 | struct device *dev = &wlp->rc->uwb_dev.dev; | |
201 | ||
202 | if (le16_to_cpu(hdr->type) != type) { | |
203 | dev_err(dev, "WLP: unexpected header type. Expected " | |
204 | "%u, got %u.\n", type, le16_to_cpu(hdr->type)); | |
205 | return -EINVAL; | |
206 | } | |
207 | if (le16_to_cpu(hdr->length) != len) { | |
208 | dev_err(dev, "WLP: unexpected length in header. Expected " | |
209 | "%u, got %u.\n", len, le16_to_cpu(hdr->length)); | |
210 | return -EINVAL; | |
211 | } | |
212 | return 0; | |
213 | } | |
214 | ||
215 | /** | |
216 | * Check if header of WSS information attribute valid | |
217 | * | |
218 | * @returns: length of WSS attributes (value of length attribute field) if | |
219 | * valid WSS information attribute found | |
220 | * -ENODATA if no WSS information attribute found | |
221 | * -EIO other error occured | |
222 | * | |
223 | * The WSS information attribute is optional. The function will be provided | |
224 | * with a pointer to data that could _potentially_ be a WSS information | |
225 | * attribute. If a valid WSS information attribute is found it will return | |
226 | * 0, if no WSS information attribute is found it will return -ENODATA, and | |
227 | * another error will be returned if it is a WSS information attribute, but | |
228 | * some parsing failure occured. | |
229 | */ | |
230 | static int wlp_check_wss_info_attr_hdr(struct wlp *wlp, | |
231 | struct wlp_attr_hdr *hdr, size_t buflen) | |
232 | { | |
233 | struct device *dev = &wlp->rc->uwb_dev.dev; | |
234 | size_t len; | |
235 | int result = 0; | |
236 | ||
237 | if (buflen < sizeof(*hdr)) { | |
238 | dev_err(dev, "WLP: Not enough space in buffer to parse" | |
239 | " WSS information attribute header.\n"); | |
240 | result = -EIO; | |
241 | goto out; | |
242 | } | |
243 | if (le16_to_cpu(hdr->type) != WLP_ATTR_WSS_INFO) { | |
244 | /* WSS information is optional */ | |
245 | result = -ENODATA; | |
246 | goto out; | |
247 | } | |
248 | len = le16_to_cpu(hdr->length); | |
249 | if (buflen < sizeof(*hdr) + len) { | |
250 | dev_err(dev, "WLP: Not enough space in buffer to parse " | |
251 | "variable data. Got %d, expected %d.\n", | |
252 | (int)buflen, (int)(sizeof(*hdr) + len)); | |
253 | result = -EIO; | |
254 | goto out; | |
255 | } | |
256 | result = len; | |
257 | out: | |
258 | return result; | |
259 | } | |
260 | ||
261 | ||
262 | /** | |
263 | * Get value of attribute from fixed size attribute field. | |
264 | * | |
265 | * @attr: Pointer to attribute field. | |
266 | * @value: Pointer to variable in which attribute value will be placed. | |
267 | * @buflen: Size of buffer in which attribute field (including header) | |
268 | * can be found. | |
269 | * @returns: Amount of given buffer consumed by parsing for this attribute. | |
270 | * | |
271 | * The size and type of the value is known by the type of the attribute. | |
272 | */ | |
273 | #define wlp_get(type, type_code, name) \ | |
274 | ssize_t wlp_get_##name(struct wlp *wlp, struct wlp_attr_##name *attr, \ | |
275 | type *value, ssize_t buflen) \ | |
276 | { \ | |
277 | struct device *dev = &wlp->rc->uwb_dev.dev; \ | |
278 | if (buflen < 0) \ | |
279 | return -EINVAL; \ | |
280 | if (buflen < sizeof(*attr)) { \ | |
281 | dev_err(dev, "WLP: Not enough space in buffer to parse" \ | |
282 | " attribute field. Need %d, received %zu\n", \ | |
283 | (int)sizeof(*attr), buflen); \ | |
284 | return -EIO; \ | |
285 | } \ | |
286 | if (wlp_check_attr_hdr(wlp, &attr->hdr, type_code, \ | |
287 | sizeof(attr->name)) < 0) { \ | |
288 | dev_err(dev, "WLP: Header verification failed. \n"); \ | |
289 | return -EINVAL; \ | |
290 | } \ | |
291 | *value = attr->name; \ | |
292 | return sizeof(*attr); \ | |
293 | } | |
294 | ||
295 | #define wlp_get_sparse(type, type_code, name) \ | |
296 | static wlp_get(type, type_code, name) | |
297 | ||
298 | /** | |
299 | * Get value of attribute from variable sized attribute field. | |
300 | * | |
301 | * @max: The maximum size of this attribute. This value is dictated by | |
302 | * the maximum value from the WLP specification. | |
303 | * | |
304 | * @attr: Pointer to attribute field. | |
305 | * @value: Pointer to variable that will contain the value. The memory | |
306 | * must already have been allocated for this value. | |
307 | * @buflen: Size of buffer in which attribute field (including header) | |
308 | * can be found. | |
309 | * @returns: Amount of given bufferconsumed by parsing for this attribute. | |
310 | */ | |
311 | #define wlp_vget(type_val, type_code, name, max) \ | |
312 | static ssize_t wlp_get_##name(struct wlp *wlp, \ | |
313 | struct wlp_attr_##name *attr, \ | |
314 | type_val *value, ssize_t buflen) \ | |
315 | { \ | |
316 | struct device *dev = &wlp->rc->uwb_dev.dev; \ | |
317 | size_t len; \ | |
318 | if (buflen < 0) \ | |
319 | return -EINVAL; \ | |
320 | if (buflen < sizeof(*attr)) { \ | |
321 | dev_err(dev, "WLP: Not enough space in buffer to parse" \ | |
322 | " header.\n"); \ | |
323 | return -EIO; \ | |
324 | } \ | |
325 | if (le16_to_cpu(attr->hdr.type) != type_code) { \ | |
326 | dev_err(dev, "WLP: Unexpected attribute type. Got %u, " \ | |
327 | "expected %u.\n", le16_to_cpu(attr->hdr.type), \ | |
328 | type_code); \ | |
329 | return -EINVAL; \ | |
330 | } \ | |
331 | len = le16_to_cpu(attr->hdr.length); \ | |
332 | if (len > max) { \ | |
333 | dev_err(dev, "WLP: Attribute larger than maximum " \ | |
334 | "allowed. Received %zu, max is %d.\n", len, \ | |
335 | (int)max); \ | |
336 | return -EFBIG; \ | |
337 | } \ | |
338 | if (buflen < sizeof(*attr) + len) { \ | |
339 | dev_err(dev, "WLP: Not enough space in buffer to parse "\ | |
340 | "variable data.\n"); \ | |
341 | return -EIO; \ | |
342 | } \ | |
343 | memcpy(value, (void *) attr + sizeof(*attr), len); \ | |
344 | return sizeof(*attr) + len; \ | |
345 | } | |
346 | ||
347 | wlp_get(u8, WLP_ATTR_WLP_VER, version) | |
348 | wlp_get_sparse(enum wlp_wss_sel_mthd, WLP_ATTR_WSS_SEL_MTHD, wss_sel_mthd) | |
349 | wlp_get_sparse(struct wlp_dev_type, WLP_ATTR_PRI_DEV_TYPE, prim_dev_type) | |
350 | wlp_get_sparse(enum wlp_assc_error, WLP_ATTR_WLP_ASSC_ERR, wlp_assc_err) | |
351 | wlp_get_sparse(struct wlp_uuid, WLP_ATTR_UUID_E, uuid_e) | |
352 | wlp_get_sparse(struct wlp_uuid, WLP_ATTR_UUID_R, uuid_r) | |
353 | wlp_get(struct wlp_uuid, WLP_ATTR_WSSID, wssid) | |
354 | wlp_get_sparse(u8, WLP_ATTR_ACC_ENRL, accept_enrl) | |
355 | wlp_get_sparse(u8, WLP_ATTR_WSS_SEC_STAT, wss_sec_status) | |
356 | wlp_get_sparse(struct uwb_mac_addr, WLP_ATTR_WSS_BCAST, wss_bcast) | |
357 | wlp_get_sparse(u8, WLP_ATTR_WSS_TAG, wss_tag) | |
358 | wlp_get_sparse(struct uwb_mac_addr, WLP_ATTR_WSS_VIRT, wss_virt) | |
359 | wlp_get_sparse(struct wlp_nonce, WLP_ATTR_ENRL_NONCE, enonce) | |
360 | wlp_get_sparse(struct wlp_nonce, WLP_ATTR_REG_NONCE, rnonce) | |
361 | ||
362 | /* The buffers for the device info attributes can be found in the | |
363 | * wlp_device_info struct. These buffers contain one byte more than the | |
364 | * max allowed by the spec - this is done to be able to add the | |
365 | * terminating \0 for user display. This terminating byte is not required | |
366 | * in the actual attribute field (because it has a length field) so the | |
367 | * maximum allowed for this value is one less than its size in the | |
368 | * structure. | |
369 | */ | |
370 | wlp_vget(char, WLP_ATTR_WSS_NAME, wss_name, | |
371 | FIELD_SIZEOF(struct wlp_wss, name) - 1) | |
372 | wlp_vget(char, WLP_ATTR_DEV_NAME, dev_name, | |
373 | FIELD_SIZEOF(struct wlp_device_info, name) - 1) | |
374 | wlp_vget(char, WLP_ATTR_MANUF, manufacturer, | |
375 | FIELD_SIZEOF(struct wlp_device_info, manufacturer) - 1) | |
376 | wlp_vget(char, WLP_ATTR_MODEL_NAME, model_name, | |
377 | FIELD_SIZEOF(struct wlp_device_info, model_name) - 1) | |
378 | wlp_vget(char, WLP_ATTR_MODEL_NR, model_nr, | |
379 | FIELD_SIZEOF(struct wlp_device_info, model_nr) - 1) | |
380 | wlp_vget(char, WLP_ATTR_SERIAL, serial, | |
381 | FIELD_SIZEOF(struct wlp_device_info, serial) - 1) | |
382 | ||
383 | /** | |
384 | * Retrieve WSS Name, Accept enroll, Secure status, Broadcast from WSS info | |
385 | * | |
386 | * @attr: pointer to WSS name attribute in WSS information attribute field | |
387 | * @info: structure that will be populated with data from WSS information | |
388 | * field (WSS name, Accept enroll, secure status, broadcast address) | |
389 | * @buflen: size of buffer | |
390 | * | |
391 | * Although the WSSID attribute forms part of the WSS info attribute it is | |
392 | * retrieved separately and stored in a different location. | |
393 | */ | |
394 | static ssize_t wlp_get_wss_info_attrs(struct wlp *wlp, | |
395 | struct wlp_attr_hdr *attr, | |
396 | struct wlp_wss_tmp_info *info, | |
397 | ssize_t buflen) | |
398 | { | |
399 | struct device *dev = &wlp->rc->uwb_dev.dev; | |
400 | void *ptr = attr; | |
401 | size_t used = 0; | |
402 | ssize_t result = -EINVAL; | |
403 | ||
e377e9d3 RC |
404 | result = wlp_get_wss_name(wlp, ptr, info->name, buflen); |
405 | if (result < 0) { | |
406 | dev_err(dev, "WLP: unable to obtain WSS name from " | |
407 | "WSS info in D2 message.\n"); | |
408 | goto error_parse; | |
409 | } | |
410 | used += result; | |
bce83697 | 411 | |
e377e9d3 RC |
412 | result = wlp_get_accept_enrl(wlp, ptr + used, &info->accept_enroll, |
413 | buflen - used); | |
414 | if (result < 0) { | |
415 | dev_err(dev, "WLP: unable to obtain accepting " | |
416 | "enrollment from WSS info in D2 message.\n"); | |
417 | goto error_parse; | |
418 | } | |
419 | if (info->accept_enroll != 0 && info->accept_enroll != 1) { | |
420 | dev_err(dev, "WLP: invalid value for accepting " | |
421 | "enrollment in D2 message.\n"); | |
422 | result = -EINVAL; | |
423 | goto error_parse; | |
424 | } | |
425 | used += result; | |
bce83697 | 426 | |
e377e9d3 RC |
427 | result = wlp_get_wss_sec_status(wlp, ptr + used, &info->sec_status, |
428 | buflen - used); | |
429 | if (result < 0) { | |
430 | dev_err(dev, "WLP: unable to obtain secure " | |
431 | "status from WSS info in D2 message.\n"); | |
432 | goto error_parse; | |
433 | } | |
434 | if (info->sec_status != 0 && info->sec_status != 1) { | |
435 | dev_err(dev, "WLP: invalid value for secure " | |
436 | "status in D2 message.\n"); | |
437 | result = -EINVAL; | |
438 | goto error_parse; | |
439 | } | |
440 | used += result; | |
bce83697 | 441 | |
e377e9d3 RC |
442 | result = wlp_get_wss_bcast(wlp, ptr + used, &info->bcast, |
443 | buflen - used); | |
444 | if (result < 0) { | |
445 | dev_err(dev, "WLP: unable to obtain broadcast " | |
446 | "address from WSS info in D2 message.\n"); | |
447 | goto error_parse; | |
448 | } | |
449 | used += result; | |
450 | result = used; | |
451 | error_parse: | |
452 | return result; | |
453 | } | |
454 | ||
455 | /** | |
456 | * Create a new WSSID entry for the neighbor, allocate temporary storage | |
457 | * | |
458 | * Each neighbor can have many WSS active. We maintain a list of WSSIDs | |
459 | * advertised by neighbor. During discovery we also cache information about | |
460 | * these WSS in temporary storage. | |
461 | * | |
462 | * The temporary storage will be removed after it has been used (eg. | |
463 | * displayed to user), the wssid element will be removed from the list when | |
464 | * the neighbor is rediscovered or when it disappears. | |
465 | */ | |
466 | static struct wlp_wssid_e *wlp_create_wssid_e(struct wlp *wlp, | |
467 | struct wlp_neighbor_e *neighbor) | |
468 | { | |
469 | struct device *dev = &wlp->rc->uwb_dev.dev; | |
470 | struct wlp_wssid_e *wssid_e; | |
471 | ||
472 | wssid_e = kzalloc(sizeof(*wssid_e), GFP_KERNEL); | |
473 | if (wssid_e == NULL) { | |
474 | dev_err(dev, "WLP: unable to allocate memory " | |
475 | "for WSS information.\n"); | |
476 | goto error_alloc; | |
477 | } | |
478 | wssid_e->info = kzalloc(sizeof(struct wlp_wss_tmp_info), GFP_KERNEL); | |
479 | if (wssid_e->info == NULL) { | |
480 | dev_err(dev, "WLP: unable to allocate memory " | |
481 | "for temporary WSS information.\n"); | |
482 | kfree(wssid_e); | |
483 | wssid_e = NULL; | |
484 | goto error_alloc; | |
485 | } | |
486 | list_add(&wssid_e->node, &neighbor->wssid); | |
487 | error_alloc: | |
488 | return wssid_e; | |
489 | } | |
490 | ||
491 | /** | |
492 | * Parse WSS information attribute | |
493 | * | |
494 | * @attr: pointer to WSS information attribute header | |
495 | * @buflen: size of buffer in which WSS information attribute appears | |
496 | * @wssid: will place wssid from WSS info attribute in this location | |
497 | * @wss_info: will place other information from WSS information attribute | |
498 | * in this location | |
499 | * | |
500 | * memory for @wssid and @wss_info must be allocated when calling this | |
501 | */ | |
502 | static ssize_t wlp_get_wss_info(struct wlp *wlp, struct wlp_attr_wss_info *attr, | |
503 | size_t buflen, struct wlp_uuid *wssid, | |
504 | struct wlp_wss_tmp_info *wss_info) | |
505 | { | |
506 | struct device *dev = &wlp->rc->uwb_dev.dev; | |
507 | ssize_t result; | |
508 | size_t len; | |
509 | size_t used = 0; | |
510 | void *ptr; | |
511 | ||
512 | result = wlp_check_wss_info_attr_hdr(wlp, (struct wlp_attr_hdr *)attr, | |
513 | buflen); | |
514 | if (result < 0) | |
515 | goto out; | |
516 | len = result; | |
517 | used = sizeof(*attr); | |
518 | ptr = attr; | |
bce83697 | 519 | |
e377e9d3 RC |
520 | result = wlp_get_wssid(wlp, ptr + used, wssid, buflen - used); |
521 | if (result < 0) { | |
522 | dev_err(dev, "WLP: unable to obtain WSSID from WSS info.\n"); | |
523 | goto out; | |
524 | } | |
525 | used += result; | |
526 | result = wlp_get_wss_info_attrs(wlp, ptr + used, wss_info, | |
527 | buflen - used); | |
528 | if (result < 0) { | |
529 | dev_err(dev, "WLP: unable to obtain WSS information " | |
530 | "from WSS information attributes. \n"); | |
531 | goto out; | |
532 | } | |
533 | used += result; | |
534 | if (len + sizeof(*attr) != used) { | |
535 | dev_err(dev, "WLP: Amount of data parsed does not " | |
536 | "match length field. Parsed %zu, length " | |
537 | "field %zu. \n", used, len); | |
538 | result = -EINVAL; | |
539 | goto out; | |
540 | } | |
541 | result = used; | |
e377e9d3 RC |
542 | out: |
543 | return result; | |
544 | } | |
545 | ||
546 | /** | |
547 | * Retrieve WSS info from association frame | |
548 | * | |
549 | * @attr: pointer to WSS information attribute | |
550 | * @neighbor: ptr to neighbor being discovered, NULL if enrollment in | |
551 | * progress | |
552 | * @wss: ptr to WSS being enrolled in, NULL if discovery in progress | |
553 | * @buflen: size of buffer in which WSS information appears | |
554 | * | |
555 | * The WSS information attribute appears in the D2 association message. | |
556 | * This message is used in two ways: to discover all neighbors or to enroll | |
557 | * into a WSS activated by a neighbor. During discovery we only want to | |
558 | * store the WSS info in a cache, to be deleted right after it has been | |
559 | * used (eg. displayed to the user). During enrollment we store the WSS | |
560 | * information for the lifetime of enrollment. | |
561 | * | |
562 | * During discovery we are interested in all WSS information, during | |
563 | * enrollment we are only interested in the WSS being enrolled in. Even so, | |
564 | * when in enrollment we keep parsing the message after finding the WSS of | |
565 | * interest, this simplifies the calling routine in that it can be sure | |
566 | * that all WSS information attributes have been parsed out of the message. | |
567 | * | |
568 | * Association frame is process with nbmutex held. The list access is safe. | |
569 | */ | |
570 | static ssize_t wlp_get_all_wss_info(struct wlp *wlp, | |
571 | struct wlp_attr_wss_info *attr, | |
572 | struct wlp_neighbor_e *neighbor, | |
573 | struct wlp_wss *wss, ssize_t buflen) | |
574 | { | |
575 | struct device *dev = &wlp->rc->uwb_dev.dev; | |
576 | size_t used = 0; | |
577 | ssize_t result = -EINVAL; | |
578 | struct wlp_attr_wss_info *cur; | |
579 | struct wlp_uuid wssid; | |
580 | struct wlp_wss_tmp_info wss_info; | |
581 | unsigned enroll; /* 0 - discovery to cache, 1 - enrollment */ | |
582 | struct wlp_wssid_e *wssid_e; | |
583 | char buf[WLP_WSS_UUID_STRSIZE]; | |
584 | ||
e377e9d3 RC |
585 | if (buflen < 0) |
586 | goto out; | |
587 | ||
588 | if (neighbor != NULL && wss == NULL) | |
589 | enroll = 0; /* discovery */ | |
590 | else if (wss != NULL && neighbor == NULL) | |
591 | enroll = 1; /* enrollment */ | |
592 | else | |
593 | goto out; | |
594 | ||
595 | cur = attr; | |
596 | while (buflen - used > 0) { | |
597 | memset(&wss_info, 0, sizeof(wss_info)); | |
598 | cur = (void *)cur + used; | |
599 | result = wlp_get_wss_info(wlp, cur, buflen - used, &wssid, | |
600 | &wss_info); | |
601 | if (result == -ENODATA) { | |
602 | result = used; | |
603 | goto out; | |
604 | } else if (result < 0) { | |
605 | dev_err(dev, "WLP: Unable to parse WSS information " | |
606 | "from WSS information attribute. \n"); | |
607 | result = -EINVAL; | |
608 | goto error_parse; | |
609 | } | |
610 | if (enroll && !memcmp(&wssid, &wss->wssid, sizeof(wssid))) { | |
611 | if (wss_info.accept_enroll != 1) { | |
612 | dev_err(dev, "WLP: Requested WSS does " | |
613 | "not accept enrollment.\n"); | |
614 | result = -EINVAL; | |
615 | goto out; | |
616 | } | |
617 | memcpy(wss->name, wss_info.name, sizeof(wss->name)); | |
618 | wss->bcast = wss_info.bcast; | |
619 | wss->secure_status = wss_info.sec_status; | |
620 | wss->accept_enroll = wss_info.accept_enroll; | |
621 | wss->state = WLP_WSS_STATE_PART_ENROLLED; | |
622 | wlp_wss_uuid_print(buf, sizeof(buf), &wssid); | |
bce83697 | 623 | dev_dbg(dev, "WLP: Found WSS %s. Enrolling.\n", buf); |
e377e9d3 RC |
624 | } else { |
625 | wssid_e = wlp_create_wssid_e(wlp, neighbor); | |
626 | if (wssid_e == NULL) { | |
627 | dev_err(dev, "WLP: Cannot create new WSSID " | |
628 | "entry for neighbor %02x:%02x.\n", | |
629 | neighbor->uwb_dev->dev_addr.data[1], | |
630 | neighbor->uwb_dev->dev_addr.data[0]); | |
631 | result = -ENOMEM; | |
632 | goto out; | |
633 | } | |
634 | wssid_e->wssid = wssid; | |
635 | *wssid_e->info = wss_info; | |
636 | } | |
637 | used += result; | |
638 | } | |
639 | result = used; | |
640 | error_parse: | |
641 | if (result < 0 && !enroll) /* this was a discovery */ | |
642 | wlp_remove_neighbor_tmp_info(neighbor); | |
643 | out: | |
e377e9d3 RC |
644 | return result; |
645 | ||
646 | } | |
647 | ||
648 | /** | |
649 | * Parse WSS information attributes into cache for discovery | |
650 | * | |
651 | * @attr: the first WSS information attribute in message | |
652 | * @neighbor: the neighbor whose cache will be populated | |
653 | * @buflen: size of the input buffer | |
654 | */ | |
655 | static ssize_t wlp_get_wss_info_to_cache(struct wlp *wlp, | |
656 | struct wlp_attr_wss_info *attr, | |
657 | struct wlp_neighbor_e *neighbor, | |
658 | ssize_t buflen) | |
659 | { | |
660 | return wlp_get_all_wss_info(wlp, attr, neighbor, NULL, buflen); | |
661 | } | |
662 | ||
663 | /** | |
664 | * Parse WSS information attributes into WSS struct for enrollment | |
665 | * | |
666 | * @attr: the first WSS information attribute in message | |
667 | * @wss: the WSS that will be enrolled | |
668 | * @buflen: size of the input buffer | |
669 | */ | |
670 | static ssize_t wlp_get_wss_info_to_enroll(struct wlp *wlp, | |
671 | struct wlp_attr_wss_info *attr, | |
672 | struct wlp_wss *wss, ssize_t buflen) | |
673 | { | |
674 | return wlp_get_all_wss_info(wlp, attr, NULL, wss, buflen); | |
675 | } | |
676 | ||
677 | /** | |
678 | * Construct a D1 association frame | |
679 | * | |
680 | * We use the radio control functions to determine the values of the device | |
681 | * properties. These are of variable length and the total space needed is | |
682 | * tallied first before we start constructing the message. The radio | |
683 | * control functions return strings that are terminated with \0. This | |
684 | * character should not be included in the message (there is a length field | |
685 | * accompanying it in the attribute). | |
686 | */ | |
687 | static int wlp_build_assoc_d1(struct wlp *wlp, struct wlp_wss *wss, | |
688 | struct sk_buff **skb) | |
689 | { | |
690 | ||
691 | struct device *dev = &wlp->rc->uwb_dev.dev; | |
692 | int result = 0; | |
693 | struct wlp_device_info *info; | |
694 | size_t used = 0; | |
695 | struct wlp_frame_assoc *_d1; | |
696 | struct sk_buff *_skb; | |
697 | void *d1_itr; | |
698 | ||
e377e9d3 RC |
699 | if (wlp->dev_info == NULL) { |
700 | result = __wlp_setup_device_info(wlp); | |
701 | if (result < 0) { | |
702 | dev_err(dev, "WLP: Unable to setup device " | |
703 | "information for D1 message.\n"); | |
704 | goto error; | |
705 | } | |
706 | } | |
707 | info = wlp->dev_info; | |
e377e9d3 RC |
708 | _skb = dev_alloc_skb(sizeof(*_d1) |
709 | + sizeof(struct wlp_attr_uuid_e) | |
710 | + sizeof(struct wlp_attr_wss_sel_mthd) | |
711 | + sizeof(struct wlp_attr_dev_name) | |
712 | + strlen(info->name) | |
713 | + sizeof(struct wlp_attr_manufacturer) | |
714 | + strlen(info->manufacturer) | |
715 | + sizeof(struct wlp_attr_model_name) | |
716 | + strlen(info->model_name) | |
717 | + sizeof(struct wlp_attr_model_nr) | |
718 | + strlen(info->model_nr) | |
719 | + sizeof(struct wlp_attr_serial) | |
720 | + strlen(info->serial) | |
721 | + sizeof(struct wlp_attr_prim_dev_type) | |
722 | + sizeof(struct wlp_attr_wlp_assc_err)); | |
723 | if (_skb == NULL) { | |
724 | dev_err(dev, "WLP: Cannot allocate memory for association " | |
725 | "message.\n"); | |
726 | result = -ENOMEM; | |
727 | goto error; | |
728 | } | |
729 | _d1 = (void *) _skb->data; | |
e377e9d3 RC |
730 | _d1->hdr.mux_hdr = cpu_to_le16(WLP_PROTOCOL_ID); |
731 | _d1->hdr.type = WLP_FRAME_ASSOCIATION; | |
732 | _d1->type = WLP_ASSOC_D1; | |
733 | ||
734 | wlp_set_version(&_d1->version, WLP_VERSION); | |
735 | wlp_set_msg_type(&_d1->msg_type, WLP_ASSOC_D1); | |
736 | d1_itr = _d1->attr; | |
737 | used = wlp_set_uuid_e(d1_itr, &wlp->uuid); | |
738 | used += wlp_set_wss_sel_mthd(d1_itr + used, WLP_WSS_REG_SELECT); | |
739 | used += wlp_set_dev_name(d1_itr + used, info->name, | |
740 | strlen(info->name)); | |
741 | used += wlp_set_manufacturer(d1_itr + used, info->manufacturer, | |
742 | strlen(info->manufacturer)); | |
743 | used += wlp_set_model_name(d1_itr + used, info->model_name, | |
744 | strlen(info->model_name)); | |
745 | used += wlp_set_model_nr(d1_itr + used, info->model_nr, | |
746 | strlen(info->model_nr)); | |
747 | used += wlp_set_serial(d1_itr + used, info->serial, | |
748 | strlen(info->serial)); | |
749 | used += wlp_set_prim_dev_type(d1_itr + used, &info->prim_dev_type); | |
750 | used += wlp_set_wlp_assc_err(d1_itr + used, WLP_ASSOC_ERROR_NONE); | |
751 | skb_put(_skb, sizeof(*_d1) + used); | |
e377e9d3 RC |
752 | *skb = _skb; |
753 | error: | |
e377e9d3 RC |
754 | return result; |
755 | } | |
756 | ||
757 | /** | |
758 | * Construct a D2 association frame | |
759 | * | |
760 | * We use the radio control functions to determine the values of the device | |
761 | * properties. These are of variable length and the total space needed is | |
762 | * tallied first before we start constructing the message. The radio | |
763 | * control functions return strings that are terminated with \0. This | |
764 | * character should not be included in the message (there is a length field | |
765 | * accompanying it in the attribute). | |
766 | */ | |
767 | static | |
768 | int wlp_build_assoc_d2(struct wlp *wlp, struct wlp_wss *wss, | |
769 | struct sk_buff **skb, struct wlp_uuid *uuid_e) | |
770 | { | |
771 | ||
772 | struct device *dev = &wlp->rc->uwb_dev.dev; | |
773 | int result = 0; | |
774 | struct wlp_device_info *info; | |
775 | size_t used = 0; | |
776 | struct wlp_frame_assoc *_d2; | |
777 | struct sk_buff *_skb; | |
778 | void *d2_itr; | |
779 | size_t mem_needed; | |
780 | ||
e377e9d3 RC |
781 | if (wlp->dev_info == NULL) { |
782 | result = __wlp_setup_device_info(wlp); | |
783 | if (result < 0) { | |
784 | dev_err(dev, "WLP: Unable to setup device " | |
785 | "information for D2 message.\n"); | |
786 | goto error; | |
787 | } | |
788 | } | |
789 | info = wlp->dev_info; | |
e377e9d3 RC |
790 | mem_needed = sizeof(*_d2) |
791 | + sizeof(struct wlp_attr_uuid_e) | |
792 | + sizeof(struct wlp_attr_uuid_r) | |
793 | + sizeof(struct wlp_attr_dev_name) | |
794 | + strlen(info->name) | |
795 | + sizeof(struct wlp_attr_manufacturer) | |
796 | + strlen(info->manufacturer) | |
797 | + sizeof(struct wlp_attr_model_name) | |
798 | + strlen(info->model_name) | |
799 | + sizeof(struct wlp_attr_model_nr) | |
800 | + strlen(info->model_nr) | |
801 | + sizeof(struct wlp_attr_serial) | |
802 | + strlen(info->serial) | |
803 | + sizeof(struct wlp_attr_prim_dev_type) | |
804 | + sizeof(struct wlp_attr_wlp_assc_err); | |
805 | if (wlp->wss.state >= WLP_WSS_STATE_ACTIVE) | |
806 | mem_needed += sizeof(struct wlp_attr_wss_info) | |
807 | + sizeof(struct wlp_wss_info) | |
808 | + strlen(wlp->wss.name); | |
809 | _skb = dev_alloc_skb(mem_needed); | |
810 | if (_skb == NULL) { | |
811 | dev_err(dev, "WLP: Cannot allocate memory for association " | |
812 | "message.\n"); | |
813 | result = -ENOMEM; | |
814 | goto error; | |
815 | } | |
816 | _d2 = (void *) _skb->data; | |
e377e9d3 RC |
817 | _d2->hdr.mux_hdr = cpu_to_le16(WLP_PROTOCOL_ID); |
818 | _d2->hdr.type = WLP_FRAME_ASSOCIATION; | |
819 | _d2->type = WLP_ASSOC_D2; | |
820 | ||
821 | wlp_set_version(&_d2->version, WLP_VERSION); | |
822 | wlp_set_msg_type(&_d2->msg_type, WLP_ASSOC_D2); | |
823 | d2_itr = _d2->attr; | |
824 | used = wlp_set_uuid_e(d2_itr, uuid_e); | |
825 | used += wlp_set_uuid_r(d2_itr + used, &wlp->uuid); | |
826 | if (wlp->wss.state >= WLP_WSS_STATE_ACTIVE) | |
827 | used += wlp_set_wss_info(d2_itr + used, &wlp->wss); | |
828 | used += wlp_set_dev_name(d2_itr + used, info->name, | |
829 | strlen(info->name)); | |
830 | used += wlp_set_manufacturer(d2_itr + used, info->manufacturer, | |
831 | strlen(info->manufacturer)); | |
832 | used += wlp_set_model_name(d2_itr + used, info->model_name, | |
833 | strlen(info->model_name)); | |
834 | used += wlp_set_model_nr(d2_itr + used, info->model_nr, | |
835 | strlen(info->model_nr)); | |
836 | used += wlp_set_serial(d2_itr + used, info->serial, | |
837 | strlen(info->serial)); | |
838 | used += wlp_set_prim_dev_type(d2_itr + used, &info->prim_dev_type); | |
839 | used += wlp_set_wlp_assc_err(d2_itr + used, WLP_ASSOC_ERROR_NONE); | |
840 | skb_put(_skb, sizeof(*_d2) + used); | |
e377e9d3 RC |
841 | *skb = _skb; |
842 | error: | |
e377e9d3 RC |
843 | return result; |
844 | } | |
845 | ||
846 | /** | |
847 | * Allocate memory for and populate fields of F0 association frame | |
848 | * | |
849 | * Currently (while focusing on unsecure enrollment) we ignore the | |
850 | * nonce's that could be placed in the message. Only the error field is | |
851 | * populated by the value provided by the caller. | |
852 | */ | |
853 | static | |
854 | int wlp_build_assoc_f0(struct wlp *wlp, struct sk_buff **skb, | |
855 | enum wlp_assc_error error) | |
856 | { | |
857 | struct device *dev = &wlp->rc->uwb_dev.dev; | |
858 | int result = -ENOMEM; | |
859 | struct { | |
860 | struct wlp_frame_assoc f0_hdr; | |
861 | struct wlp_attr_enonce enonce; | |
862 | struct wlp_attr_rnonce rnonce; | |
863 | struct wlp_attr_wlp_assc_err assc_err; | |
864 | } *f0; | |
865 | struct sk_buff *_skb; | |
866 | struct wlp_nonce tmp; | |
867 | ||
e377e9d3 RC |
868 | _skb = dev_alloc_skb(sizeof(*f0)); |
869 | if (_skb == NULL) { | |
870 | dev_err(dev, "WLP: Unable to allocate memory for F0 " | |
871 | "association frame. \n"); | |
872 | goto error_alloc; | |
873 | } | |
874 | f0 = (void *) _skb->data; | |
e377e9d3 RC |
875 | f0->f0_hdr.hdr.mux_hdr = cpu_to_le16(WLP_PROTOCOL_ID); |
876 | f0->f0_hdr.hdr.type = WLP_FRAME_ASSOCIATION; | |
877 | f0->f0_hdr.type = WLP_ASSOC_F0; | |
878 | wlp_set_version(&f0->f0_hdr.version, WLP_VERSION); | |
879 | wlp_set_msg_type(&f0->f0_hdr.msg_type, WLP_ASSOC_F0); | |
880 | memset(&tmp, 0, sizeof(tmp)); | |
881 | wlp_set_enonce(&f0->enonce, &tmp); | |
882 | wlp_set_rnonce(&f0->rnonce, &tmp); | |
883 | wlp_set_wlp_assc_err(&f0->assc_err, error); | |
884 | skb_put(_skb, sizeof(*f0)); | |
885 | *skb = _skb; | |
886 | result = 0; | |
887 | error_alloc: | |
e377e9d3 RC |
888 | return result; |
889 | } | |
890 | ||
891 | /** | |
892 | * Parse F0 frame | |
893 | * | |
894 | * We just retrieve the values and print it as an error to the user. | |
895 | * Calling function already knows an error occured (F0 indicates error), so | |
896 | * we just parse the content as debug for higher layers. | |
897 | */ | |
898 | int wlp_parse_f0(struct wlp *wlp, struct sk_buff *skb) | |
899 | { | |
900 | struct device *dev = &wlp->rc->uwb_dev.dev; | |
901 | struct wlp_frame_assoc *f0 = (void *) skb->data; | |
902 | void *ptr = skb->data; | |
903 | size_t len = skb->len; | |
904 | size_t used; | |
905 | ssize_t result; | |
906 | struct wlp_nonce enonce, rnonce; | |
907 | enum wlp_assc_error assc_err; | |
908 | char enonce_buf[WLP_WSS_NONCE_STRSIZE]; | |
909 | char rnonce_buf[WLP_WSS_NONCE_STRSIZE]; | |
910 | ||
911 | used = sizeof(*f0); | |
912 | result = wlp_get_enonce(wlp, ptr + used, &enonce, len - used); | |
913 | if (result < 0) { | |
914 | dev_err(dev, "WLP: unable to obtain Enrollee nonce " | |
915 | "attribute from F0 message.\n"); | |
916 | goto error_parse; | |
917 | } | |
918 | used += result; | |
919 | result = wlp_get_rnonce(wlp, ptr + used, &rnonce, len - used); | |
920 | if (result < 0) { | |
921 | dev_err(dev, "WLP: unable to obtain Registrar nonce " | |
922 | "attribute from F0 message.\n"); | |
923 | goto error_parse; | |
924 | } | |
925 | used += result; | |
926 | result = wlp_get_wlp_assc_err(wlp, ptr + used, &assc_err, len - used); | |
927 | if (result < 0) { | |
928 | dev_err(dev, "WLP: unable to obtain WLP Association error " | |
929 | "attribute from F0 message.\n"); | |
930 | goto error_parse; | |
931 | } | |
932 | wlp_wss_nonce_print(enonce_buf, sizeof(enonce_buf), &enonce); | |
933 | wlp_wss_nonce_print(rnonce_buf, sizeof(rnonce_buf), &rnonce); | |
934 | dev_err(dev, "WLP: Received F0 error frame from neighbor. Enrollee " | |
935 | "nonce: %s, Registrar nonce: %s, WLP Association error: %s.\n", | |
936 | enonce_buf, rnonce_buf, wlp_assc_error_str(assc_err)); | |
937 | result = 0; | |
938 | error_parse: | |
939 | return result; | |
940 | } | |
941 | ||
942 | /** | |
943 | * Retrieve variable device information from association message | |
944 | * | |
945 | * The device information parsed is not required in any message. This | |
946 | * routine will thus not fail if an attribute is not present. | |
947 | * The attributes are expected in a certain order, even if all are not | |
948 | * present. The "attribute type" value is used to ensure the attributes | |
949 | * are parsed in the correct order. | |
950 | * | |
951 | * If an error is encountered during parsing the function will return an | |
952 | * error code, when this happens the given device_info structure may be | |
953 | * partially filled. | |
954 | */ | |
955 | static | |
956 | int wlp_get_variable_info(struct wlp *wlp, void *data, | |
957 | struct wlp_device_info *dev_info, ssize_t len) | |
958 | { | |
959 | struct device *dev = &wlp->rc->uwb_dev.dev; | |
960 | size_t used = 0; | |
961 | struct wlp_attr_hdr *hdr; | |
962 | ssize_t result = 0; | |
963 | unsigned last = 0; | |
964 | ||
965 | while (len - used > 0) { | |
966 | if (len - used < sizeof(*hdr)) { | |
967 | dev_err(dev, "WLP: Partial data in frame, cannot " | |
968 | "parse. \n"); | |
969 | goto error_parse; | |
970 | } | |
971 | hdr = data + used; | |
972 | switch (le16_to_cpu(hdr->type)) { | |
973 | case WLP_ATTR_MANUF: | |
974 | if (last >= WLP_ATTR_MANUF) { | |
975 | dev_err(dev, "WLP: Incorrect order of " | |
976 | "attribute values in D1 msg.\n"); | |
977 | goto error_parse; | |
978 | } | |
979 | result = wlp_get_manufacturer(wlp, data + used, | |
980 | dev_info->manufacturer, | |
981 | len - used); | |
982 | if (result < 0) { | |
983 | dev_err(dev, "WLP: Unable to obtain " | |
984 | "Manufacturer attribute from D1 " | |
985 | "message.\n"); | |
986 | goto error_parse; | |
987 | } | |
988 | last = WLP_ATTR_MANUF; | |
989 | used += result; | |
990 | break; | |
991 | case WLP_ATTR_MODEL_NAME: | |
992 | if (last >= WLP_ATTR_MODEL_NAME) { | |
993 | dev_err(dev, "WLP: Incorrect order of " | |
994 | "attribute values in D1 msg.\n"); | |
995 | goto error_parse; | |
996 | } | |
997 | result = wlp_get_model_name(wlp, data + used, | |
998 | dev_info->model_name, | |
999 | len - used); | |
1000 | if (result < 0) { | |
1001 | dev_err(dev, "WLP: Unable to obtain Model " | |
1002 | "name attribute from D1 message.\n"); | |
1003 | goto error_parse; | |
1004 | } | |
1005 | last = WLP_ATTR_MODEL_NAME; | |
1006 | used += result; | |
1007 | break; | |
1008 | case WLP_ATTR_MODEL_NR: | |
1009 | if (last >= WLP_ATTR_MODEL_NR) { | |
1010 | dev_err(dev, "WLP: Incorrect order of " | |
1011 | "attribute values in D1 msg.\n"); | |
1012 | goto error_parse; | |
1013 | } | |
1014 | result = wlp_get_model_nr(wlp, data + used, | |
1015 | dev_info->model_nr, | |
1016 | len - used); | |
1017 | if (result < 0) { | |
1018 | dev_err(dev, "WLP: Unable to obtain Model " | |
1019 | "number attribute from D1 message.\n"); | |
1020 | goto error_parse; | |
1021 | } | |
1022 | last = WLP_ATTR_MODEL_NR; | |
1023 | used += result; | |
1024 | break; | |
1025 | case WLP_ATTR_SERIAL: | |
1026 | if (last >= WLP_ATTR_SERIAL) { | |
1027 | dev_err(dev, "WLP: Incorrect order of " | |
1028 | "attribute values in D1 msg.\n"); | |
1029 | goto error_parse; | |
1030 | } | |
1031 | result = wlp_get_serial(wlp, data + used, | |
1032 | dev_info->serial, len - used); | |
1033 | if (result < 0) { | |
1034 | dev_err(dev, "WLP: Unable to obtain Serial " | |
1035 | "number attribute from D1 message.\n"); | |
1036 | goto error_parse; | |
1037 | } | |
1038 | last = WLP_ATTR_SERIAL; | |
1039 | used += result; | |
1040 | break; | |
1041 | case WLP_ATTR_PRI_DEV_TYPE: | |
1042 | if (last >= WLP_ATTR_PRI_DEV_TYPE) { | |
1043 | dev_err(dev, "WLP: Incorrect order of " | |
1044 | "attribute values in D1 msg.\n"); | |
1045 | goto error_parse; | |
1046 | } | |
1047 | result = wlp_get_prim_dev_type(wlp, data + used, | |
1048 | &dev_info->prim_dev_type, | |
1049 | len - used); | |
1050 | if (result < 0) { | |
1051 | dev_err(dev, "WLP: Unable to obtain Primary " | |
1052 | "device type attribute from D1 " | |
1053 | "message.\n"); | |
1054 | goto error_parse; | |
1055 | } | |
1056 | dev_info->prim_dev_type.category = | |
1057 | le16_to_cpu(dev_info->prim_dev_type.category); | |
1058 | dev_info->prim_dev_type.subID = | |
1059 | le16_to_cpu(dev_info->prim_dev_type.subID); | |
1060 | last = WLP_ATTR_PRI_DEV_TYPE; | |
1061 | used += result; | |
1062 | break; | |
1063 | default: | |
1064 | /* This is not variable device information. */ | |
1065 | goto out; | |
1066 | break; | |
1067 | } | |
1068 | } | |
1069 | out: | |
1070 | return used; | |
1071 | error_parse: | |
1072 | return -EINVAL; | |
1073 | } | |
1074 | ||
1075 | /** | |
1076 | * Parse incoming D1 frame, populate attribute values | |
1077 | * | |
1078 | * Caller provides pointers to memory already allocated for attributes | |
1079 | * expected in the D1 frame. These variables will be populated. | |
1080 | */ | |
1081 | static | |
1082 | int wlp_parse_d1_frame(struct wlp *wlp, struct sk_buff *skb, | |
1083 | struct wlp_uuid *uuid_e, | |
1084 | enum wlp_wss_sel_mthd *sel_mthd, | |
1085 | struct wlp_device_info *dev_info, | |
1086 | enum wlp_assc_error *assc_err) | |
1087 | { | |
1088 | struct device *dev = &wlp->rc->uwb_dev.dev; | |
1089 | struct wlp_frame_assoc *d1 = (void *) skb->data; | |
1090 | void *ptr = skb->data; | |
1091 | size_t len = skb->len; | |
1092 | size_t used; | |
1093 | ssize_t result; | |
1094 | ||
1095 | used = sizeof(*d1); | |
1096 | result = wlp_get_uuid_e(wlp, ptr + used, uuid_e, len - used); | |
1097 | if (result < 0) { | |
1098 | dev_err(dev, "WLP: unable to obtain UUID-E attribute from D1 " | |
1099 | "message.\n"); | |
1100 | goto error_parse; | |
1101 | } | |
1102 | used += result; | |
1103 | result = wlp_get_wss_sel_mthd(wlp, ptr + used, sel_mthd, len - used); | |
1104 | if (result < 0) { | |
1105 | dev_err(dev, "WLP: unable to obtain WSS selection method " | |
1106 | "from D1 message.\n"); | |
1107 | goto error_parse; | |
1108 | } | |
1109 | used += result; | |
1110 | result = wlp_get_dev_name(wlp, ptr + used, dev_info->name, | |
1111 | len - used); | |
1112 | if (result < 0) { | |
1113 | dev_err(dev, "WLP: unable to obtain Device Name from D1 " | |
1114 | "message.\n"); | |
1115 | goto error_parse; | |
1116 | } | |
1117 | used += result; | |
1118 | result = wlp_get_variable_info(wlp, ptr + used, dev_info, len - used); | |
1119 | if (result < 0) { | |
1120 | dev_err(dev, "WLP: unable to obtain Device Information from " | |
1121 | "D1 message.\n"); | |
1122 | goto error_parse; | |
1123 | } | |
1124 | used += result; | |
1125 | result = wlp_get_wlp_assc_err(wlp, ptr + used, assc_err, len - used); | |
1126 | if (result < 0) { | |
1127 | dev_err(dev, "WLP: unable to obtain WLP Association Error " | |
1128 | "Information from D1 message.\n"); | |
1129 | goto error_parse; | |
1130 | } | |
1131 | result = 0; | |
1132 | error_parse: | |
1133 | return result; | |
1134 | } | |
1135 | /** | |
1136 | * Handle incoming D1 frame | |
1137 | * | |
1138 | * The frame has already been verified to contain an Association header with | |
1139 | * the correct version number. Parse the incoming frame, construct and send | |
1140 | * a D2 frame in response. | |
1141 | * | |
1142 | * It is not clear what to do with most fields in the incoming D1 frame. We | |
1143 | * retrieve and discard the information here for now. | |
1144 | */ | |
1145 | void wlp_handle_d1_frame(struct work_struct *ws) | |
1146 | { | |
1147 | struct wlp_assoc_frame_ctx *frame_ctx = container_of(ws, | |
1148 | struct wlp_assoc_frame_ctx, | |
1149 | ws); | |
1150 | struct wlp *wlp = frame_ctx->wlp; | |
1151 | struct wlp_wss *wss = &wlp->wss; | |
1152 | struct sk_buff *skb = frame_ctx->skb; | |
1153 | struct uwb_dev_addr *src = &frame_ctx->src; | |
1154 | int result; | |
1155 | struct device *dev = &wlp->rc->uwb_dev.dev; | |
1156 | struct wlp_uuid uuid_e; | |
1157 | enum wlp_wss_sel_mthd sel_mthd = 0; | |
1158 | struct wlp_device_info dev_info; | |
1159 | enum wlp_assc_error assc_err; | |
e377e9d3 RC |
1160 | struct sk_buff *resp = NULL; |
1161 | ||
1162 | /* Parse D1 frame */ | |
e377e9d3 RC |
1163 | mutex_lock(&wss->mutex); |
1164 | mutex_lock(&wlp->mutex); /* to access wlp->uuid */ | |
1165 | memset(&dev_info, 0, sizeof(dev_info)); | |
1166 | result = wlp_parse_d1_frame(wlp, skb, &uuid_e, &sel_mthd, &dev_info, | |
1167 | &assc_err); | |
1168 | if (result < 0) { | |
1169 | dev_err(dev, "WLP: Unable to parse incoming D1 frame.\n"); | |
1170 | kfree_skb(skb); | |
1171 | goto out; | |
1172 | } | |
e377e9d3 RC |
1173 | |
1174 | kfree_skb(skb); | |
1175 | if (!wlp_uuid_is_set(&wlp->uuid)) { | |
1176 | dev_err(dev, "WLP: UUID is not set. Set via sysfs to " | |
1177 | "proceed. Respong to D1 message with error F0.\n"); | |
1178 | result = wlp_build_assoc_f0(wlp, &resp, | |
1179 | WLP_ASSOC_ERROR_NOT_READY); | |
1180 | if (result < 0) { | |
1181 | dev_err(dev, "WLP: Unable to construct F0 message.\n"); | |
1182 | goto out; | |
1183 | } | |
1184 | } else { | |
1185 | /* Construct D2 frame */ | |
1186 | result = wlp_build_assoc_d2(wlp, wss, &resp, &uuid_e); | |
1187 | if (result < 0) { | |
1188 | dev_err(dev, "WLP: Unable to construct D2 message.\n"); | |
1189 | goto out; | |
1190 | } | |
1191 | } | |
1192 | /* Send D2 frame */ | |
1193 | BUG_ON(wlp->xmit_frame == NULL); | |
1194 | result = wlp->xmit_frame(wlp, resp, src); | |
1195 | if (result < 0) { | |
1196 | dev_err(dev, "WLP: Unable to transmit D2 association " | |
1197 | "message: %d\n", result); | |
1198 | if (result == -ENXIO) | |
1199 | dev_err(dev, "WLP: Is network interface up? \n"); | |
1200 | /* We could try again ... */ | |
1201 | dev_kfree_skb_any(resp); /* we need to free if tx fails */ | |
1202 | } | |
1203 | out: | |
1204 | kfree(frame_ctx); | |
1205 | mutex_unlock(&wlp->mutex); | |
1206 | mutex_unlock(&wss->mutex); | |
e377e9d3 RC |
1207 | } |
1208 | ||
1209 | /** | |
1210 | * Parse incoming D2 frame, create and populate temporary cache | |
1211 | * | |
1212 | * @skb: socket buffer in which D2 frame can be found | |
1213 | * @neighbor: the neighbor that sent the D2 frame | |
1214 | * | |
1215 | * Will allocate memory for temporary storage of information learned during | |
1216 | * discovery. | |
1217 | */ | |
1218 | int wlp_parse_d2_frame_to_cache(struct wlp *wlp, struct sk_buff *skb, | |
1219 | struct wlp_neighbor_e *neighbor) | |
1220 | { | |
1221 | struct device *dev = &wlp->rc->uwb_dev.dev; | |
1222 | struct wlp_frame_assoc *d2 = (void *) skb->data; | |
1223 | void *ptr = skb->data; | |
1224 | size_t len = skb->len; | |
1225 | size_t used; | |
1226 | ssize_t result; | |
1227 | struct wlp_uuid uuid_e; | |
1228 | struct wlp_device_info *nb_info; | |
1229 | enum wlp_assc_error assc_err; | |
1230 | ||
1231 | used = sizeof(*d2); | |
1232 | result = wlp_get_uuid_e(wlp, ptr + used, &uuid_e, len - used); | |
1233 | if (result < 0) { | |
1234 | dev_err(dev, "WLP: unable to obtain UUID-E attribute from D2 " | |
1235 | "message.\n"); | |
1236 | goto error_parse; | |
1237 | } | |
1238 | if (memcmp(&uuid_e, &wlp->uuid, sizeof(uuid_e))) { | |
1239 | dev_err(dev, "WLP: UUID-E in incoming D2 does not match " | |
1240 | "local UUID sent in D1. \n"); | |
1241 | goto error_parse; | |
1242 | } | |
1243 | used += result; | |
1244 | result = wlp_get_uuid_r(wlp, ptr + used, &neighbor->uuid, len - used); | |
1245 | if (result < 0) { | |
1246 | dev_err(dev, "WLP: unable to obtain UUID-R attribute from D2 " | |
1247 | "message.\n"); | |
1248 | goto error_parse; | |
1249 | } | |
1250 | used += result; | |
1251 | result = wlp_get_wss_info_to_cache(wlp, ptr + used, neighbor, | |
1252 | len - used); | |
1253 | if (result < 0) { | |
1254 | dev_err(dev, "WLP: unable to obtain WSS information " | |
1255 | "from D2 message.\n"); | |
1256 | goto error_parse; | |
1257 | } | |
1258 | used += result; | |
1259 | neighbor->info = kzalloc(sizeof(struct wlp_device_info), GFP_KERNEL); | |
1260 | if (neighbor->info == NULL) { | |
1261 | dev_err(dev, "WLP: cannot allocate memory to store device " | |
1262 | "info.\n"); | |
1263 | result = -ENOMEM; | |
1264 | goto error_parse; | |
1265 | } | |
1266 | nb_info = neighbor->info; | |
1267 | result = wlp_get_dev_name(wlp, ptr + used, nb_info->name, | |
1268 | len - used); | |
1269 | if (result < 0) { | |
1270 | dev_err(dev, "WLP: unable to obtain Device Name from D2 " | |
1271 | "message.\n"); | |
1272 | goto error_parse; | |
1273 | } | |
1274 | used += result; | |
1275 | result = wlp_get_variable_info(wlp, ptr + used, nb_info, len - used); | |
1276 | if (result < 0) { | |
1277 | dev_err(dev, "WLP: unable to obtain Device Information from " | |
1278 | "D2 message.\n"); | |
1279 | goto error_parse; | |
1280 | } | |
1281 | used += result; | |
1282 | result = wlp_get_wlp_assc_err(wlp, ptr + used, &assc_err, len - used); | |
1283 | if (result < 0) { | |
1284 | dev_err(dev, "WLP: unable to obtain WLP Association Error " | |
1285 | "Information from D2 message.\n"); | |
1286 | goto error_parse; | |
1287 | } | |
1288 | if (assc_err != WLP_ASSOC_ERROR_NONE) { | |
1289 | dev_err(dev, "WLP: neighbor device returned association " | |
1290 | "error %d\n", assc_err); | |
1291 | result = -EINVAL; | |
1292 | goto error_parse; | |
1293 | } | |
1294 | result = 0; | |
1295 | error_parse: | |
1296 | if (result < 0) | |
1297 | wlp_remove_neighbor_tmp_info(neighbor); | |
1298 | return result; | |
1299 | } | |
1300 | ||
1301 | /** | |
1302 | * Parse incoming D2 frame, populate attribute values of WSS bein enrolled in | |
1303 | * | |
1304 | * @wss: our WSS that will be enrolled | |
1305 | * @skb: socket buffer in which D2 frame can be found | |
1306 | * @neighbor: the neighbor that sent the D2 frame | |
1307 | * @wssid: the wssid of the WSS in which we want to enroll | |
1308 | * | |
1309 | * Forms part of enrollment sequence. We are trying to enroll in WSS with | |
1310 | * @wssid by using @neighbor as registrar. A D1 message was sent to | |
1311 | * @neighbor and now we need to parse the D2 response. The neighbor's | |
1312 | * response is searched for the requested WSS and if found (and it accepts | |
1313 | * enrollment), we store the information. | |
1314 | */ | |
1315 | int wlp_parse_d2_frame_to_enroll(struct wlp_wss *wss, struct sk_buff *skb, | |
1316 | struct wlp_neighbor_e *neighbor, | |
1317 | struct wlp_uuid *wssid) | |
1318 | { | |
1319 | struct wlp *wlp = container_of(wss, struct wlp, wss); | |
1320 | struct device *dev = &wlp->rc->uwb_dev.dev; | |
1321 | void *ptr = skb->data; | |
1322 | size_t len = skb->len; | |
1323 | size_t used; | |
1324 | ssize_t result; | |
1325 | struct wlp_uuid uuid_e; | |
1326 | struct wlp_uuid uuid_r; | |
1327 | struct wlp_device_info nb_info; | |
1328 | enum wlp_assc_error assc_err; | |
1329 | char uuid_bufA[WLP_WSS_UUID_STRSIZE]; | |
1330 | char uuid_bufB[WLP_WSS_UUID_STRSIZE]; | |
1331 | ||
1332 | used = sizeof(struct wlp_frame_assoc); | |
1333 | result = wlp_get_uuid_e(wlp, ptr + used, &uuid_e, len - used); | |
1334 | if (result < 0) { | |
1335 | dev_err(dev, "WLP: unable to obtain UUID-E attribute from D2 " | |
1336 | "message.\n"); | |
1337 | goto error_parse; | |
1338 | } | |
1339 | if (memcmp(&uuid_e, &wlp->uuid, sizeof(uuid_e))) { | |
1340 | dev_err(dev, "WLP: UUID-E in incoming D2 does not match " | |
1341 | "local UUID sent in D1. \n"); | |
1342 | goto error_parse; | |
1343 | } | |
1344 | used += result; | |
1345 | result = wlp_get_uuid_r(wlp, ptr + used, &uuid_r, len - used); | |
1346 | if (result < 0) { | |
1347 | dev_err(dev, "WLP: unable to obtain UUID-R attribute from D2 " | |
1348 | "message.\n"); | |
1349 | goto error_parse; | |
1350 | } | |
1351 | if (memcmp(&uuid_r, &neighbor->uuid, sizeof(uuid_r))) { | |
1352 | wlp_wss_uuid_print(uuid_bufA, sizeof(uuid_bufA), | |
1353 | &neighbor->uuid); | |
1354 | wlp_wss_uuid_print(uuid_bufB, sizeof(uuid_bufB), &uuid_r); | |
1355 | dev_err(dev, "WLP: UUID of neighbor does not match UUID " | |
1356 | "learned during discovery. Originally discovered: %s, " | |
1357 | "now from D2 message: %s\n", uuid_bufA, uuid_bufB); | |
1358 | result = -EINVAL; | |
1359 | goto error_parse; | |
1360 | } | |
1361 | used += result; | |
1362 | wss->wssid = *wssid; | |
1363 | result = wlp_get_wss_info_to_enroll(wlp, ptr + used, wss, len - used); | |
1364 | if (result < 0) { | |
1365 | dev_err(dev, "WLP: unable to obtain WSS information " | |
1366 | "from D2 message.\n"); | |
1367 | goto error_parse; | |
1368 | } | |
1369 | if (wss->state != WLP_WSS_STATE_PART_ENROLLED) { | |
1370 | dev_err(dev, "WLP: D2 message did not contain information " | |
1371 | "for successful enrollment. \n"); | |
1372 | result = -EINVAL; | |
1373 | goto error_parse; | |
1374 | } | |
1375 | used += result; | |
1376 | /* Place device information on stack to continue parsing of message */ | |
1377 | result = wlp_get_dev_name(wlp, ptr + used, nb_info.name, | |
1378 | len - used); | |
1379 | if (result < 0) { | |
1380 | dev_err(dev, "WLP: unable to obtain Device Name from D2 " | |
1381 | "message.\n"); | |
1382 | goto error_parse; | |
1383 | } | |
1384 | used += result; | |
1385 | result = wlp_get_variable_info(wlp, ptr + used, &nb_info, len - used); | |
1386 | if (result < 0) { | |
1387 | dev_err(dev, "WLP: unable to obtain Device Information from " | |
1388 | "D2 message.\n"); | |
1389 | goto error_parse; | |
1390 | } | |
1391 | used += result; | |
1392 | result = wlp_get_wlp_assc_err(wlp, ptr + used, &assc_err, len - used); | |
1393 | if (result < 0) { | |
1394 | dev_err(dev, "WLP: unable to obtain WLP Association Error " | |
1395 | "Information from D2 message.\n"); | |
1396 | goto error_parse; | |
1397 | } | |
1398 | if (assc_err != WLP_ASSOC_ERROR_NONE) { | |
1399 | dev_err(dev, "WLP: neighbor device returned association " | |
1400 | "error %d\n", assc_err); | |
1401 | if (wss->state == WLP_WSS_STATE_PART_ENROLLED) { | |
1402 | dev_err(dev, "WLP: Enrolled in WSS (should not " | |
1403 | "happen according to spec). Undoing. \n"); | |
1404 | wlp_wss_reset(wss); | |
1405 | } | |
1406 | result = -EINVAL; | |
1407 | goto error_parse; | |
1408 | } | |
1409 | result = 0; | |
1410 | error_parse: | |
1411 | return result; | |
1412 | } | |
1413 | ||
1414 | /** | |
1415 | * Parse C3/C4 frame into provided variables | |
1416 | * | |
1417 | * @wssid: will point to copy of wssid retrieved from C3/C4 frame | |
1418 | * @tag: will point to copy of tag retrieved from C3/C4 frame | |
1419 | * @virt_addr: will point to copy of virtual address retrieved from C3/C4 | |
1420 | * frame. | |
1421 | * | |
1422 | * Calling function has to allocate memory for these values. | |
1423 | * | |
1424 | * skb contains a valid C3/C4 frame, return the individual fields of this | |
1425 | * frame in the provided variables. | |
1426 | */ | |
1427 | int wlp_parse_c3c4_frame(struct wlp *wlp, struct sk_buff *skb, | |
1428 | struct wlp_uuid *wssid, u8 *tag, | |
1429 | struct uwb_mac_addr *virt_addr) | |
1430 | { | |
1431 | struct device *dev = &wlp->rc->uwb_dev.dev; | |
1432 | int result; | |
1433 | void *ptr = skb->data; | |
1434 | size_t len = skb->len; | |
1435 | size_t used; | |
e377e9d3 RC |
1436 | struct wlp_frame_assoc *assoc = ptr; |
1437 | ||
e377e9d3 RC |
1438 | used = sizeof(*assoc); |
1439 | result = wlp_get_wssid(wlp, ptr + used, wssid, len - used); | |
1440 | if (result < 0) { | |
1441 | dev_err(dev, "WLP: unable to obtain WSSID attribute from " | |
1442 | "%s message.\n", wlp_assoc_frame_str(assoc->type)); | |
1443 | goto error_parse; | |
1444 | } | |
1445 | used += result; | |
1446 | result = wlp_get_wss_tag(wlp, ptr + used, tag, len - used); | |
1447 | if (result < 0) { | |
1448 | dev_err(dev, "WLP: unable to obtain WSS tag attribute from " | |
1449 | "%s message.\n", wlp_assoc_frame_str(assoc->type)); | |
1450 | goto error_parse; | |
1451 | } | |
1452 | used += result; | |
1453 | result = wlp_get_wss_virt(wlp, ptr + used, virt_addr, len - used); | |
1454 | if (result < 0) { | |
1455 | dev_err(dev, "WLP: unable to obtain WSS virtual address " | |
1456 | "attribute from %s message.\n", | |
1457 | wlp_assoc_frame_str(assoc->type)); | |
1458 | goto error_parse; | |
1459 | } | |
e377e9d3 | 1460 | error_parse: |
e377e9d3 RC |
1461 | return result; |
1462 | } | |
1463 | ||
1464 | /** | |
1465 | * Allocate memory for and populate fields of C1 or C2 association frame | |
1466 | * | |
1467 | * The C1 and C2 association frames appear identical - except for the type. | |
1468 | */ | |
1469 | static | |
1470 | int wlp_build_assoc_c1c2(struct wlp *wlp, struct wlp_wss *wss, | |
1471 | struct sk_buff **skb, enum wlp_assoc_type type) | |
1472 | { | |
1473 | struct device *dev = &wlp->rc->uwb_dev.dev; | |
1474 | int result = -ENOMEM; | |
1475 | struct { | |
1476 | struct wlp_frame_assoc c_hdr; | |
1477 | struct wlp_attr_wssid wssid; | |
1478 | } *c; | |
1479 | struct sk_buff *_skb; | |
1480 | ||
e377e9d3 RC |
1481 | _skb = dev_alloc_skb(sizeof(*c)); |
1482 | if (_skb == NULL) { | |
1483 | dev_err(dev, "WLP: Unable to allocate memory for C1/C2 " | |
1484 | "association frame. \n"); | |
1485 | goto error_alloc; | |
1486 | } | |
1487 | c = (void *) _skb->data; | |
e377e9d3 RC |
1488 | c->c_hdr.hdr.mux_hdr = cpu_to_le16(WLP_PROTOCOL_ID); |
1489 | c->c_hdr.hdr.type = WLP_FRAME_ASSOCIATION; | |
1490 | c->c_hdr.type = type; | |
1491 | wlp_set_version(&c->c_hdr.version, WLP_VERSION); | |
1492 | wlp_set_msg_type(&c->c_hdr.msg_type, type); | |
1493 | wlp_set_wssid(&c->wssid, &wss->wssid); | |
1494 | skb_put(_skb, sizeof(*c)); | |
e377e9d3 RC |
1495 | *skb = _skb; |
1496 | result = 0; | |
1497 | error_alloc: | |
e377e9d3 RC |
1498 | return result; |
1499 | } | |
1500 | ||
1501 | ||
1502 | static | |
1503 | int wlp_build_assoc_c1(struct wlp *wlp, struct wlp_wss *wss, | |
1504 | struct sk_buff **skb) | |
1505 | { | |
1506 | return wlp_build_assoc_c1c2(wlp, wss, skb, WLP_ASSOC_C1); | |
1507 | } | |
1508 | ||
1509 | static | |
1510 | int wlp_build_assoc_c2(struct wlp *wlp, struct wlp_wss *wss, | |
1511 | struct sk_buff **skb) | |
1512 | { | |
1513 | return wlp_build_assoc_c1c2(wlp, wss, skb, WLP_ASSOC_C2); | |
1514 | } | |
1515 | ||
1516 | ||
1517 | /** | |
1518 | * Allocate memory for and populate fields of C3 or C4 association frame | |
1519 | * | |
1520 | * The C3 and C4 association frames appear identical - except for the type. | |
1521 | */ | |
1522 | static | |
1523 | int wlp_build_assoc_c3c4(struct wlp *wlp, struct wlp_wss *wss, | |
1524 | struct sk_buff **skb, enum wlp_assoc_type type) | |
1525 | { | |
1526 | struct device *dev = &wlp->rc->uwb_dev.dev; | |
1527 | int result = -ENOMEM; | |
1528 | struct { | |
1529 | struct wlp_frame_assoc c_hdr; | |
1530 | struct wlp_attr_wssid wssid; | |
1531 | struct wlp_attr_wss_tag wss_tag; | |
1532 | struct wlp_attr_wss_virt wss_virt; | |
1533 | } *c; | |
1534 | struct sk_buff *_skb; | |
1535 | ||
e377e9d3 RC |
1536 | _skb = dev_alloc_skb(sizeof(*c)); |
1537 | if (_skb == NULL) { | |
1538 | dev_err(dev, "WLP: Unable to allocate memory for C3/C4 " | |
1539 | "association frame. \n"); | |
1540 | goto error_alloc; | |
1541 | } | |
1542 | c = (void *) _skb->data; | |
e377e9d3 RC |
1543 | c->c_hdr.hdr.mux_hdr = cpu_to_le16(WLP_PROTOCOL_ID); |
1544 | c->c_hdr.hdr.type = WLP_FRAME_ASSOCIATION; | |
1545 | c->c_hdr.type = type; | |
1546 | wlp_set_version(&c->c_hdr.version, WLP_VERSION); | |
1547 | wlp_set_msg_type(&c->c_hdr.msg_type, type); | |
1548 | wlp_set_wssid(&c->wssid, &wss->wssid); | |
1549 | wlp_set_wss_tag(&c->wss_tag, wss->tag); | |
1550 | wlp_set_wss_virt(&c->wss_virt, &wss->virtual_addr); | |
1551 | skb_put(_skb, sizeof(*c)); | |
e377e9d3 RC |
1552 | *skb = _skb; |
1553 | result = 0; | |
1554 | error_alloc: | |
e377e9d3 RC |
1555 | return result; |
1556 | } | |
1557 | ||
1558 | static | |
1559 | int wlp_build_assoc_c3(struct wlp *wlp, struct wlp_wss *wss, | |
1560 | struct sk_buff **skb) | |
1561 | { | |
1562 | return wlp_build_assoc_c3c4(wlp, wss, skb, WLP_ASSOC_C3); | |
1563 | } | |
1564 | ||
1565 | static | |
1566 | int wlp_build_assoc_c4(struct wlp *wlp, struct wlp_wss *wss, | |
1567 | struct sk_buff **skb) | |
1568 | { | |
1569 | return wlp_build_assoc_c3c4(wlp, wss, skb, WLP_ASSOC_C4); | |
1570 | } | |
1571 | ||
1572 | ||
1573 | #define wlp_send_assoc(type, id) \ | |
1574 | static int wlp_send_assoc_##type(struct wlp *wlp, struct wlp_wss *wss, \ | |
1575 | struct uwb_dev_addr *dev_addr) \ | |
1576 | { \ | |
1577 | struct device *dev = &wlp->rc->uwb_dev.dev; \ | |
1578 | int result; \ | |
1579 | struct sk_buff *skb = NULL; \ | |
bce83697 | 1580 | \ |
e377e9d3 RC |
1581 | /* Build the frame */ \ |
1582 | result = wlp_build_assoc_##type(wlp, wss, &skb); \ | |
1583 | if (result < 0) { \ | |
1584 | dev_err(dev, "WLP: Unable to construct %s association " \ | |
1585 | "frame: %d\n", wlp_assoc_frame_str(id), result);\ | |
1586 | goto error_build_assoc; \ | |
1587 | } \ | |
1588 | /* Send the frame */ \ | |
e377e9d3 RC |
1589 | BUG_ON(wlp->xmit_frame == NULL); \ |
1590 | result = wlp->xmit_frame(wlp, skb, dev_addr); \ | |
1591 | if (result < 0) { \ | |
1592 | dev_err(dev, "WLP: Unable to transmit %s association " \ | |
1593 | "message: %d\n", wlp_assoc_frame_str(id), \ | |
1594 | result); \ | |
1595 | if (result == -ENXIO) \ | |
1596 | dev_err(dev, "WLP: Is network interface " \ | |
1597 | "up? \n"); \ | |
1598 | goto error_xmit; \ | |
1599 | } \ | |
1600 | return 0; \ | |
1601 | error_xmit: \ | |
1602 | /* We could try again ... */ \ | |
1603 | dev_kfree_skb_any(skb);/*we need to free if tx fails*/ \ | |
1604 | error_build_assoc: \ | |
e377e9d3 RC |
1605 | return result; \ |
1606 | } | |
1607 | ||
1608 | wlp_send_assoc(d1, WLP_ASSOC_D1) | |
1609 | wlp_send_assoc(c1, WLP_ASSOC_C1) | |
1610 | wlp_send_assoc(c3, WLP_ASSOC_C3) | |
1611 | ||
1612 | int wlp_send_assoc_frame(struct wlp *wlp, struct wlp_wss *wss, | |
1613 | struct uwb_dev_addr *dev_addr, | |
1614 | enum wlp_assoc_type type) | |
1615 | { | |
1616 | int result = 0; | |
1617 | struct device *dev = &wlp->rc->uwb_dev.dev; | |
1618 | switch (type) { | |
1619 | case WLP_ASSOC_D1: | |
1620 | result = wlp_send_assoc_d1(wlp, wss, dev_addr); | |
1621 | break; | |
1622 | case WLP_ASSOC_C1: | |
1623 | result = wlp_send_assoc_c1(wlp, wss, dev_addr); | |
1624 | break; | |
1625 | case WLP_ASSOC_C3: | |
1626 | result = wlp_send_assoc_c3(wlp, wss, dev_addr); | |
1627 | break; | |
1628 | default: | |
1629 | dev_err(dev, "WLP: Received request to send unknown " | |
1630 | "association message.\n"); | |
1631 | result = -EINVAL; | |
1632 | break; | |
1633 | } | |
1634 | return result; | |
1635 | } | |
1636 | ||
1637 | /** | |
1638 | * Handle incoming C1 frame | |
1639 | * | |
1640 | * The frame has already been verified to contain an Association header with | |
1641 | * the correct version number. Parse the incoming frame, construct and send | |
1642 | * a C2 frame in response. | |
1643 | */ | |
1644 | void wlp_handle_c1_frame(struct work_struct *ws) | |
1645 | { | |
1646 | struct wlp_assoc_frame_ctx *frame_ctx = container_of(ws, | |
1647 | struct wlp_assoc_frame_ctx, | |
1648 | ws); | |
1649 | struct wlp *wlp = frame_ctx->wlp; | |
1650 | struct wlp_wss *wss = &wlp->wss; | |
1651 | struct device *dev = &wlp->rc->uwb_dev.dev; | |
1652 | struct wlp_frame_assoc *c1 = (void *) frame_ctx->skb->data; | |
1653 | unsigned int len = frame_ctx->skb->len; | |
1654 | struct uwb_dev_addr *src = &frame_ctx->src; | |
1655 | int result; | |
1656 | struct wlp_uuid wssid; | |
e377e9d3 RC |
1657 | struct sk_buff *resp = NULL; |
1658 | ||
1659 | /* Parse C1 frame */ | |
e377e9d3 RC |
1660 | mutex_lock(&wss->mutex); |
1661 | result = wlp_get_wssid(wlp, (void *)c1 + sizeof(*c1), &wssid, | |
1662 | len - sizeof(*c1)); | |
1663 | if (result < 0) { | |
1664 | dev_err(dev, "WLP: unable to obtain WSSID from C1 frame.\n"); | |
1665 | goto out; | |
1666 | } | |
e377e9d3 RC |
1667 | if (!memcmp(&wssid, &wss->wssid, sizeof(wssid)) |
1668 | && wss->state == WLP_WSS_STATE_ACTIVE) { | |
e377e9d3 RC |
1669 | /* Construct C2 frame */ |
1670 | result = wlp_build_assoc_c2(wlp, wss, &resp); | |
1671 | if (result < 0) { | |
1672 | dev_err(dev, "WLP: Unable to construct C2 message.\n"); | |
1673 | goto out; | |
1674 | } | |
1675 | } else { | |
e377e9d3 RC |
1676 | /* Construct F0 frame */ |
1677 | result = wlp_build_assoc_f0(wlp, &resp, WLP_ASSOC_ERROR_INV); | |
1678 | if (result < 0) { | |
1679 | dev_err(dev, "WLP: Unable to construct F0 message.\n"); | |
1680 | goto out; | |
1681 | } | |
1682 | } | |
1683 | /* Send C2 frame */ | |
e377e9d3 RC |
1684 | BUG_ON(wlp->xmit_frame == NULL); |
1685 | result = wlp->xmit_frame(wlp, resp, src); | |
1686 | if (result < 0) { | |
1687 | dev_err(dev, "WLP: Unable to transmit response association " | |
1688 | "message: %d\n", result); | |
1689 | if (result == -ENXIO) | |
1690 | dev_err(dev, "WLP: Is network interface up? \n"); | |
1691 | /* We could try again ... */ | |
1692 | dev_kfree_skb_any(resp); /* we need to free if tx fails */ | |
1693 | } | |
1694 | out: | |
1695 | kfree_skb(frame_ctx->skb); | |
1696 | kfree(frame_ctx); | |
1697 | mutex_unlock(&wss->mutex); | |
e377e9d3 RC |
1698 | } |
1699 | ||
1700 | /** | |
1701 | * Handle incoming C3 frame | |
1702 | * | |
1703 | * The frame has already been verified to contain an Association header with | |
1704 | * the correct version number. Parse the incoming frame, construct and send | |
1705 | * a C4 frame in response. If the C3 frame identifies a WSS that is locally | |
1706 | * active then we connect to this neighbor (add it to our EDA cache). | |
1707 | */ | |
1708 | void wlp_handle_c3_frame(struct work_struct *ws) | |
1709 | { | |
1710 | struct wlp_assoc_frame_ctx *frame_ctx = container_of(ws, | |
1711 | struct wlp_assoc_frame_ctx, | |
1712 | ws); | |
1713 | struct wlp *wlp = frame_ctx->wlp; | |
1714 | struct wlp_wss *wss = &wlp->wss; | |
1715 | struct device *dev = &wlp->rc->uwb_dev.dev; | |
1716 | struct sk_buff *skb = frame_ctx->skb; | |
1717 | struct uwb_dev_addr *src = &frame_ctx->src; | |
1718 | int result; | |
e377e9d3 RC |
1719 | struct sk_buff *resp = NULL; |
1720 | struct wlp_uuid wssid; | |
1721 | u8 tag; | |
1722 | struct uwb_mac_addr virt_addr; | |
1723 | ||
1724 | /* Parse C3 frame */ | |
e377e9d3 RC |
1725 | mutex_lock(&wss->mutex); |
1726 | result = wlp_parse_c3c4_frame(wlp, skb, &wssid, &tag, &virt_addr); | |
1727 | if (result < 0) { | |
1728 | dev_err(dev, "WLP: unable to obtain values from C3 frame.\n"); | |
1729 | goto out; | |
1730 | } | |
e377e9d3 RC |
1731 | if (!memcmp(&wssid, &wss->wssid, sizeof(wssid)) |
1732 | && wss->state >= WLP_WSS_STATE_ACTIVE) { | |
e377e9d3 RC |
1733 | result = wlp_eda_update_node(&wlp->eda, src, wss, |
1734 | (void *) virt_addr.data, tag, | |
1735 | WLP_WSS_CONNECTED); | |
1736 | if (result < 0) { | |
1737 | dev_err(dev, "WLP: Unable to update EDA cache " | |
1738 | "with new connected neighbor information.\n"); | |
1739 | result = wlp_build_assoc_f0(wlp, &resp, | |
1740 | WLP_ASSOC_ERROR_INT); | |
1741 | if (result < 0) { | |
1742 | dev_err(dev, "WLP: Unable to construct F0 " | |
1743 | "message.\n"); | |
1744 | goto out; | |
1745 | } | |
1746 | } else { | |
1747 | wss->state = WLP_WSS_STATE_CONNECTED; | |
1748 | /* Construct C4 frame */ | |
1749 | result = wlp_build_assoc_c4(wlp, wss, &resp); | |
1750 | if (result < 0) { | |
1751 | dev_err(dev, "WLP: Unable to construct C4 " | |
1752 | "message.\n"); | |
1753 | goto out; | |
1754 | } | |
1755 | } | |
1756 | } else { | |
e377e9d3 RC |
1757 | /* Construct F0 frame */ |
1758 | result = wlp_build_assoc_f0(wlp, &resp, WLP_ASSOC_ERROR_INV); | |
1759 | if (result < 0) { | |
1760 | dev_err(dev, "WLP: Unable to construct F0 message.\n"); | |
1761 | goto out; | |
1762 | } | |
1763 | } | |
1764 | /* Send C4 frame */ | |
e377e9d3 RC |
1765 | BUG_ON(wlp->xmit_frame == NULL); |
1766 | result = wlp->xmit_frame(wlp, resp, src); | |
1767 | if (result < 0) { | |
1768 | dev_err(dev, "WLP: Unable to transmit response association " | |
1769 | "message: %d\n", result); | |
1770 | if (result == -ENXIO) | |
1771 | dev_err(dev, "WLP: Is network interface up? \n"); | |
1772 | /* We could try again ... */ | |
1773 | dev_kfree_skb_any(resp); /* we need to free if tx fails */ | |
1774 | } | |
1775 | out: | |
1776 | kfree_skb(frame_ctx->skb); | |
1777 | kfree(frame_ctx); | |
1778 | mutex_unlock(&wss->mutex); | |
e377e9d3 RC |
1779 | } |
1780 | ||
1781 |