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