1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26#include <linux/wlp.h>
27
28#include "wlp-internal.h"
29
30static
31const 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
51static 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
58static 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
76static 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
83static 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
92
93
94
95
96
97
98
99
100
101
102
103
104#define wlp_set(type, type_code, name) \
105static size_t wlp_set_##name(struct wlp_attr_##name *attr, type value) \
106{ \
107 wlp_set_attr_hdr(&attr->hdr, type_code, \
108 sizeof(*attr) - sizeof(struct wlp_attr_hdr)); \
109 attr->name = value; \
110 return sizeof(*attr); \
111}
112
113#define wlp_pset(type, type_code, name) \
114static size_t wlp_set_##name(struct wlp_attr_##name *attr, type value) \
115{ \
116 wlp_set_attr_hdr(&attr->hdr, type_code, \
117 sizeof(*attr) - sizeof(struct wlp_attr_hdr)); \
118 attr->name = *value; \
119 return sizeof(*attr); \
120}
121
122
123
124
125
126
127
128
129
130
131#define wlp_vset(type, type_code, name) \
132static size_t wlp_set_##name(struct wlp_attr_##name *attr, type value, \
133 size_t len) \
134{ \
135 wlp_set_attr_hdr(&attr->hdr, type_code, len); \
136 memcpy(attr->name, value, len); \
137 return sizeof(*attr) + len; \
138}
139
140wlp_vset(char *, WLP_ATTR_DEV_NAME, dev_name)
141wlp_vset(char *, WLP_ATTR_MANUF, manufacturer)
142wlp_set(enum wlp_assoc_type, WLP_ATTR_MSG_TYPE, msg_type)
143wlp_vset(char *, WLP_ATTR_MODEL_NAME, model_name)
144wlp_vset(char *, WLP_ATTR_MODEL_NR, model_nr)
145wlp_vset(char *, WLP_ATTR_SERIAL, serial)
146wlp_vset(char *, WLP_ATTR_WSS_NAME, wss_name)
147wlp_pset(struct wlp_uuid *, WLP_ATTR_UUID_E, uuid_e)
148wlp_pset(struct wlp_uuid *, WLP_ATTR_UUID_R, uuid_r)
149wlp_pset(struct wlp_uuid *, WLP_ATTR_WSSID, wssid)
150wlp_pset(struct wlp_dev_type *, WLP_ATTR_PRI_DEV_TYPE, prim_dev_type)
151
152wlp_set(u8, WLP_ATTR_WLP_VER, version)
153wlp_set(enum wlp_assc_error, WLP_ATTR_WLP_ASSC_ERR, wlp_assc_err)
154wlp_set(enum wlp_wss_sel_mthd, WLP_ATTR_WSS_SEL_MTHD, wss_sel_mthd)
155wlp_set(u8, WLP_ATTR_ACC_ENRL, accept_enrl)
156wlp_set(u8, WLP_ATTR_WSS_SEC_STAT, wss_sec_status)
157wlp_pset(struct uwb_mac_addr *, WLP_ATTR_WSS_BCAST, wss_bcast)
158wlp_pset(struct wlp_nonce *, WLP_ATTR_ENRL_NONCE, enonce)
159wlp_pset(struct wlp_nonce *, WLP_ATTR_REG_NONCE, rnonce)
160wlp_set(u8, WLP_ATTR_WSS_TAG, wss_tag)
161wlp_pset(struct uwb_mac_addr *, WLP_ATTR_WSS_VIRT, wss_virt)
162
163
164
165
166
167
168
169static 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);
175
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);
183 return sizeof(*attr) + used;
184}
185
186
187
188
189
190
191
192
193
194
195
196
197static 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
217
218
219
220
221
222
223
224
225
226
227
228
229
230static 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
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;
257out:
258 return result;
259}
260
261
262
263
264
265
266
267
268
269
270
271
272
273#define wlp_get(type, type_code, name) \
274ssize_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
300
301
302
303
304
305
306
307
308
309
310
311#define wlp_vget(type_val, type_code, name, max) \
312static 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
347wlp_get(u8, WLP_ATTR_WLP_VER, version)
348wlp_get_sparse(enum wlp_wss_sel_mthd, WLP_ATTR_WSS_SEL_MTHD, wss_sel_mthd)
349wlp_get_sparse(struct wlp_dev_type, WLP_ATTR_PRI_DEV_TYPE, prim_dev_type)
350wlp_get_sparse(enum wlp_assc_error, WLP_ATTR_WLP_ASSC_ERR, wlp_assc_err)
351wlp_get_sparse(struct wlp_uuid, WLP_ATTR_UUID_E, uuid_e)
352wlp_get_sparse(struct wlp_uuid, WLP_ATTR_UUID_R, uuid_r)
353wlp_get(struct wlp_uuid, WLP_ATTR_WSSID, wssid)
354wlp_get_sparse(u8, WLP_ATTR_ACC_ENRL, accept_enrl)
355wlp_get_sparse(u8, WLP_ATTR_WSS_SEC_STAT, wss_sec_status)
356wlp_get_sparse(struct uwb_mac_addr, WLP_ATTR_WSS_BCAST, wss_bcast)
357wlp_get_sparse(u8, WLP_ATTR_WSS_TAG, wss_tag)
358wlp_get_sparse(struct uwb_mac_addr, WLP_ATTR_WSS_VIRT, wss_virt)
359wlp_get_sparse(struct wlp_nonce, WLP_ATTR_ENRL_NONCE, enonce)
360wlp_get_sparse(struct wlp_nonce, WLP_ATTR_REG_NONCE, rnonce)
361
362
363
364
365
366
367
368
369
370wlp_vget(char, WLP_ATTR_WSS_NAME, wss_name,
371 FIELD_SIZEOF(struct wlp_wss, name) - 1)
372wlp_vget(char, WLP_ATTR_DEV_NAME, dev_name,
373 FIELD_SIZEOF(struct wlp_device_info, name) - 1)
374wlp_vget(char, WLP_ATTR_MANUF, manufacturer,
375 FIELD_SIZEOF(struct wlp_device_info, manufacturer) - 1)
376wlp_vget(char, WLP_ATTR_MODEL_NAME, model_name,
377 FIELD_SIZEOF(struct wlp_device_info, model_name) - 1)
378wlp_vget(char, WLP_ATTR_MODEL_NR, model_nr,
379 FIELD_SIZEOF(struct wlp_device_info, model_nr) - 1)
380wlp_vget(char, WLP_ATTR_SERIAL, serial,
381 FIELD_SIZEOF(struct wlp_device_info, serial) - 1)
382
383
384
385
386
387
388
389
390
391
392
393
394static 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
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;
411
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;
426
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;
441
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;
451error_parse:
452 return result;
453}
454
455
456
457
458
459
460
461
462
463
464
465
466static 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);
487error_alloc:
488 return wssid_e;
489}
490
491
492
493
494
495
496
497
498
499
500
501
502static 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;
519
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;
542out:
543 return result;
544}
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570static 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;
582 struct wlp_wssid_e *wssid_e;
583 char buf[WLP_WSS_UUID_STRSIZE];
584
585 if (buflen < 0)
586 goto out;
587
588 if (neighbor != NULL && wss == NULL)
589 enroll = 0;
590 else if (wss != NULL && neighbor == NULL)
591 enroll = 1;
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);
623 dev_dbg(dev, "WLP: Found WSS %s. Enrolling.\n", buf);
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;
640error_parse:
641 if (result < 0 && !enroll)
642 wlp_remove_neighbor_tmp_info(neighbor);
643out:
644 return result;
645
646}
647
648
649
650
651
652
653
654
655static 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
665
666
667
668
669
670static 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
679
680
681
682
683
684
685
686
687static 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
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;
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;
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);
752 *skb = _skb;
753error:
754 return result;
755}
756
757
758
759
760
761
762
763
764
765
766
767static
768int 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
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;
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;
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);
841 *skb = _skb;
842error:
843 return result;
844}
845
846
847
848
849
850
851
852
853static
854int 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
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;
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;
887error_alloc:
888 return result;
889}
890
891
892
893
894
895
896
897
898int 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;
938error_parse:
939 return result;
940}
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955static
956int 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
1065 goto out;
1066 break;
1067 }
1068 }
1069out:
1070 return used;
1071error_parse:
1072 return -EINVAL;
1073}
1074
1075
1076
1077
1078
1079
1080
1081static
1082int 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;
1132error_parse:
1133 return result;
1134}
1135
1136
1137
1138
1139
1140
1141
1142
1143
1144
1145void 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;
1160 struct sk_buff *resp = NULL;
1161
1162
1163 mutex_lock(&wss->mutex);
1164 mutex_lock(&wlp->mutex);
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 }
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
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
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
1201 dev_kfree_skb_any(resp);
1202 }
1203out:
1204 kfree(frame_ctx);
1205 mutex_unlock(&wlp->mutex);
1206 mutex_unlock(&wss->mutex);
1207}
1208
1209
1210
1211
1212
1213
1214
1215
1216
1217
1218int 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;
1295error_parse:
1296 if (result < 0)
1297 wlp_remove_neighbor_tmp_info(neighbor);
1298 return result;
1299}
1300
1301
1302
1303
1304
1305
1306
1307
1308
1309
1310
1311
1312
1313
1314
1315int 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
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;
1410error_parse:
1411 return result;
1412}
1413
1414
1415
1416
1417
1418
1419
1420
1421
1422
1423
1424
1425
1426
1427int 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;
1436 struct wlp_frame_assoc *assoc = ptr;
1437
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 }
1460error_parse:
1461 return result;
1462}
1463
1464
1465
1466
1467
1468
1469static
1470int 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
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;
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));
1495 *skb = _skb;
1496 result = 0;
1497error_alloc:
1498 return result;
1499}
1500
1501
1502static
1503int 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
1509static
1510int 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
1519
1520
1521
1522static
1523int 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
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;
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));
1552 *skb = _skb;
1553 result = 0;
1554error_alloc:
1555 return result;
1556}
1557
1558static
1559int 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
1565static
1566int 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) \
1574static 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; \
1580 \
1581 \
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 \
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; \
1601error_xmit: \
1602 \
1603 dev_kfree_skb_any(skb); \
1604error_build_assoc: \
1605 return result; \
1606}
1607
1608wlp_send_assoc(d1, WLP_ASSOC_D1)
1609wlp_send_assoc(c1, WLP_ASSOC_C1)
1610wlp_send_assoc(c3, WLP_ASSOC_C3)
1611
1612int 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
1639
1640
1641
1642
1643
1644void 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;
1657 struct sk_buff *resp = NULL;
1658
1659
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 }
1667 if (!memcmp(&wssid, &wss->wssid, sizeof(wssid))
1668 && wss->state == WLP_WSS_STATE_ACTIVE) {
1669
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 {
1676
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
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
1692 dev_kfree_skb_any(resp);
1693 }
1694out:
1695 kfree_skb(frame_ctx->skb);
1696 kfree(frame_ctx);
1697 mutex_unlock(&wss->mutex);
1698}
1699
1700
1701
1702
1703
1704
1705
1706
1707
1708void 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;
1719 struct sk_buff *resp = NULL;
1720 struct wlp_uuid wssid;
1721 u8 tag;
1722 struct uwb_mac_addr virt_addr;
1723
1724
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 }
1731 if (!memcmp(&wssid, &wss->wssid, sizeof(wssid))
1732 && wss->state >= WLP_WSS_STATE_ACTIVE) {
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
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 {
1757
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
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
1773 dev_kfree_skb_any(resp);
1774 }
1775out:
1776 kfree_skb(frame_ctx->skb);
1777 kfree(frame_ctx);
1778 mutex_unlock(&wss->mutex);
1779}
1780
1781
1782