This source file includes following definitions.
- uwb_rc_start_beacon
- uwb_rc_stop_beacon
- uwb_rc_beacon
- uwb_bce_kfree
- __uwb_beca_find_bydev
- __uwb_beca_find_bymac
- uwb_dev_get_by_devaddr
- uwb_dev_get_by_macaddr
- uwb_beca_e_init
- __uwb_beca_add
- uwb_beca_purge
- uwb_beca_release
- uwb_beacon_print
- uwb_bce_print_IEs
- uwb_verify_beacon
- uwbd_evt_handle_rc_beacon
- uwbd_evt_handle_rc_beacon_size
- uwbd_evt_handle_rc_bp_slot_change
- uwbd_evt_handle_rc_bpoie_change
- uwb_rc_beacon_show
- uwb_rc_beacon_store
1
2
3
4
5
6
7
8
9
10
11 #include <linux/kernel.h>
12 #include <linux/init.h>
13 #include <linux/module.h>
14 #include <linux/device.h>
15 #include <linux/err.h>
16 #include <linux/kdev_t.h>
17 #include <linux/slab.h>
18
19 #include "uwb-internal.h"
20
21
22 struct uwb_rc_cmd_start_beacon {
23 struct uwb_rccb rccb;
24 __le16 wBPSTOffset;
25 u8 bChannelNumber;
26 } __attribute__((packed));
27
28
29 static int uwb_rc_start_beacon(struct uwb_rc *rc, u16 bpst_offset, u8 channel)
30 {
31 int result;
32 struct uwb_rc_cmd_start_beacon *cmd;
33 struct uwb_rc_evt_confirm reply;
34
35 cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
36 if (cmd == NULL)
37 return -ENOMEM;
38 cmd->rccb.bCommandType = UWB_RC_CET_GENERAL;
39 cmd->rccb.wCommand = cpu_to_le16(UWB_RC_CMD_START_BEACON);
40 cmd->wBPSTOffset = cpu_to_le16(bpst_offset);
41 cmd->bChannelNumber = channel;
42 reply.rceb.bEventType = UWB_RC_CET_GENERAL;
43 reply.rceb.wEvent = UWB_RC_CMD_START_BEACON;
44 result = uwb_rc_cmd(rc, "START-BEACON", &cmd->rccb, sizeof(*cmd),
45 &reply.rceb, sizeof(reply));
46 if (result < 0)
47 goto error_cmd;
48 if (reply.bResultCode != UWB_RC_RES_SUCCESS) {
49 dev_err(&rc->uwb_dev.dev,
50 "START-BEACON: command execution failed: %s (%d)\n",
51 uwb_rc_strerror(reply.bResultCode), reply.bResultCode);
52 result = -EIO;
53 }
54 error_cmd:
55 kfree(cmd);
56 return result;
57 }
58
59 static int uwb_rc_stop_beacon(struct uwb_rc *rc)
60 {
61 int result;
62 struct uwb_rccb *cmd;
63 struct uwb_rc_evt_confirm reply;
64
65 cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
66 if (cmd == NULL)
67 return -ENOMEM;
68 cmd->bCommandType = UWB_RC_CET_GENERAL;
69 cmd->wCommand = cpu_to_le16(UWB_RC_CMD_STOP_BEACON);
70 reply.rceb.bEventType = UWB_RC_CET_GENERAL;
71 reply.rceb.wEvent = UWB_RC_CMD_STOP_BEACON;
72 result = uwb_rc_cmd(rc, "STOP-BEACON", cmd, sizeof(*cmd),
73 &reply.rceb, sizeof(reply));
74 if (result < 0)
75 goto error_cmd;
76 if (reply.bResultCode != UWB_RC_RES_SUCCESS) {
77 dev_err(&rc->uwb_dev.dev,
78 "STOP-BEACON: command execution failed: %s (%d)\n",
79 uwb_rc_strerror(reply.bResultCode), reply.bResultCode);
80 result = -EIO;
81 }
82 error_cmd:
83 kfree(cmd);
84 return result;
85 }
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101 int uwb_rc_beacon(struct uwb_rc *rc, int channel, unsigned bpst_offset)
102 {
103 int result;
104 struct device *dev = &rc->uwb_dev.dev;
105
106 dev_dbg(dev, "%s: channel = %d\n", __func__, channel);
107 if (channel < 0)
108 channel = -1;
109 if (channel == -1)
110 result = uwb_rc_stop_beacon(rc);
111 else {
112
113 result = uwb_rc_start_beacon(rc, bpst_offset, channel);
114 if (result < 0) {
115 dev_err(dev, "Cannot start beaconing: %d\n", result);
116 return result;
117 }
118 if (le16_to_cpu(rc->ies->wIELength) > 0) {
119 result = uwb_rc_set_ie(rc, rc->ies);
120 if (result < 0) {
121 dev_err(dev, "Cannot set new IE on device: "
122 "%d\n", result);
123 result = uwb_rc_stop_beacon(rc);
124 channel = -1;
125 bpst_offset = 0;
126 }
127 }
128 }
129
130 if (result >= 0)
131 rc->beaconing = channel;
132 return result;
133 }
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150 void uwb_bce_kfree(struct kref *_bce)
151 {
152 struct uwb_beca_e *bce = container_of(_bce, struct uwb_beca_e, refcnt);
153
154 kfree(bce->be);
155 kfree(bce);
156 }
157
158
159
160 static
161 struct uwb_beca_e *__uwb_beca_find_bydev(struct uwb_rc *rc,
162 const struct uwb_dev_addr *dev_addr)
163 {
164 struct uwb_beca_e *bce, *next;
165 list_for_each_entry_safe(bce, next, &rc->uwb_beca.list, node) {
166 if (!memcmp(&bce->dev_addr, dev_addr, sizeof(bce->dev_addr)))
167 goto out;
168 }
169 bce = NULL;
170 out:
171 return bce;
172 }
173
174
175 static
176 struct uwb_beca_e *__uwb_beca_find_bymac(struct uwb_rc *rc,
177 const struct uwb_mac_addr *mac_addr)
178 {
179 struct uwb_beca_e *bce, *next;
180 list_for_each_entry_safe(bce, next, &rc->uwb_beca.list, node) {
181 if (!memcmp(bce->mac_addr, mac_addr->data,
182 sizeof(struct uwb_mac_addr)))
183 goto out;
184 }
185 bce = NULL;
186 out:
187 return bce;
188 }
189
190
191
192
193
194
195
196
197
198 struct uwb_dev *uwb_dev_get_by_devaddr(struct uwb_rc *rc,
199 const struct uwb_dev_addr *devaddr)
200 {
201 struct uwb_dev *found = NULL;
202 struct uwb_beca_e *bce;
203
204 mutex_lock(&rc->uwb_beca.mutex);
205 bce = __uwb_beca_find_bydev(rc, devaddr);
206 if (bce)
207 found = uwb_dev_try_get(rc, bce->uwb_dev);
208 mutex_unlock(&rc->uwb_beca.mutex);
209
210 return found;
211 }
212
213
214
215
216
217
218 struct uwb_dev *uwb_dev_get_by_macaddr(struct uwb_rc *rc,
219 const struct uwb_mac_addr *macaddr)
220 {
221 struct uwb_dev *found = NULL;
222 struct uwb_beca_e *bce;
223
224 mutex_lock(&rc->uwb_beca.mutex);
225 bce = __uwb_beca_find_bymac(rc, macaddr);
226 if (bce)
227 found = uwb_dev_try_get(rc, bce->uwb_dev);
228 mutex_unlock(&rc->uwb_beca.mutex);
229
230 return found;
231 }
232
233
234 static void uwb_beca_e_init(struct uwb_beca_e *bce)
235 {
236 mutex_init(&bce->mutex);
237 kref_init(&bce->refcnt);
238 stats_init(&bce->lqe_stats);
239 stats_init(&bce->rssi_stats);
240 }
241
242
243
244
245
246
247
248
249 static
250 struct uwb_beca_e *__uwb_beca_add(struct uwb_rc *rc,
251 struct uwb_rc_evt_beacon *be,
252 struct uwb_beacon_frame *bf,
253 unsigned long ts_jiffies)
254 {
255 struct uwb_beca_e *bce;
256
257 bce = kzalloc(sizeof(*bce), GFP_KERNEL);
258 if (bce == NULL)
259 return NULL;
260 uwb_beca_e_init(bce);
261 bce->ts_jiffies = ts_jiffies;
262 bce->uwb_dev = NULL;
263 list_add(&bce->node, &rc->uwb_beca.list);
264 return bce;
265 }
266
267
268
269
270
271
272 void uwb_beca_purge(struct uwb_rc *rc)
273 {
274 struct uwb_beca_e *bce, *next;
275 unsigned long expires;
276
277 mutex_lock(&rc->uwb_beca.mutex);
278 list_for_each_entry_safe(bce, next, &rc->uwb_beca.list, node) {
279 expires = bce->ts_jiffies + msecs_to_jiffies(beacon_timeout_ms);
280 if (time_after(jiffies, expires)) {
281 uwbd_dev_offair(bce);
282 }
283 }
284 mutex_unlock(&rc->uwb_beca.mutex);
285 }
286
287
288 void uwb_beca_release(struct uwb_rc *rc)
289 {
290 struct uwb_beca_e *bce, *next;
291
292 mutex_lock(&rc->uwb_beca.mutex);
293 list_for_each_entry_safe(bce, next, &rc->uwb_beca.list, node) {
294 list_del(&bce->node);
295 uwb_bce_put(bce);
296 }
297 mutex_unlock(&rc->uwb_beca.mutex);
298 }
299
300 static void uwb_beacon_print(struct uwb_rc *rc, struct uwb_rc_evt_beacon *be,
301 struct uwb_beacon_frame *bf)
302 {
303 char macbuf[UWB_ADDR_STRSIZE];
304 char devbuf[UWB_ADDR_STRSIZE];
305 char dstbuf[UWB_ADDR_STRSIZE];
306
307 uwb_mac_addr_print(macbuf, sizeof(macbuf), &bf->Device_Identifier);
308 uwb_dev_addr_print(devbuf, sizeof(devbuf), &bf->hdr.SrcAddr);
309 uwb_dev_addr_print(dstbuf, sizeof(dstbuf), &bf->hdr.DestAddr);
310 dev_info(&rc->uwb_dev.dev,
311 "BEACON from %s to %s (ch%u offset %u slot %u MAC %s)\n",
312 devbuf, dstbuf, be->bChannelNumber, be->wBPSTOffset,
313 bf->Beacon_Slot_Number, macbuf);
314 }
315
316
317
318
319 ssize_t uwb_bce_print_IEs(struct uwb_dev *uwb_dev, struct uwb_beca_e *bce,
320 char *buf, size_t size)
321 {
322 ssize_t result = 0;
323 struct uwb_rc_evt_beacon *be;
324 struct uwb_beacon_frame *bf;
325 int ies_len;
326 struct uwb_ie_hdr *ies;
327
328 mutex_lock(&bce->mutex);
329
330 be = bce->be;
331 if (be) {
332 bf = (struct uwb_beacon_frame *)bce->be->BeaconInfo;
333 ies_len = be->wBeaconInfoLength - sizeof(struct uwb_beacon_frame);
334 ies = (struct uwb_ie_hdr *)bf->IEData;
335
336 result = uwb_ie_dump_hex(ies, ies_len, buf, size);
337 }
338
339 mutex_unlock(&bce->mutex);
340
341 return result;
342 }
343
344
345
346
347 static int uwb_verify_beacon(struct uwb_rc *rc, struct uwb_event *evt,
348 struct uwb_rc_evt_beacon *be)
349 {
350 int result = -EINVAL;
351 struct uwb_beacon_frame *bf;
352 struct device *dev = &rc->uwb_dev.dev;
353
354
355 if (evt->notif.size < sizeof(*be) + sizeof(*bf)) {
356 dev_err(dev, "BEACON event: Not enough data to decode "
357 "(%zu vs %zu bytes needed)\n", evt->notif.size,
358 sizeof(*be) + sizeof(*bf));
359 goto error;
360 }
361
362
363 result = 0;
364 error:
365 return result;
366 }
367
368
369
370
371
372
373
374
375
376
377
378
379 int uwbd_evt_handle_rc_beacon(struct uwb_event *evt)
380 {
381 int result = -EINVAL;
382 struct uwb_rc *rc;
383 struct uwb_rc_evt_beacon *be;
384 struct uwb_beacon_frame *bf;
385 struct uwb_beca_e *bce;
386
387 rc = evt->rc;
388 be = container_of(evt->notif.rceb, struct uwb_rc_evt_beacon, rceb);
389 result = uwb_verify_beacon(rc, evt, be);
390 if (result < 0)
391 return result;
392
393
394 if (be->bBeaconType == UWB_RC_BEACON_TYPE_OL_ALIEN ||
395 be->bBeaconType == UWB_RC_BEACON_TYPE_NOL_ALIEN) {
396 return -ENOSYS;
397 }
398
399 bf = (struct uwb_beacon_frame *) be->BeaconInfo;
400
401
402
403
404
405
406
407
408
409 if (uwb_mac_addr_bcast(&bf->Device_Identifier))
410 return 0;
411
412 mutex_lock(&rc->uwb_beca.mutex);
413 bce = __uwb_beca_find_bymac(rc, &bf->Device_Identifier);
414 if (bce == NULL) {
415
416 uwb_beacon_print(evt->rc, be, bf);
417 bce = __uwb_beca_add(rc, be, bf, evt->ts_jiffies);
418 if (bce == NULL) {
419 mutex_unlock(&rc->uwb_beca.mutex);
420 return -ENOMEM;
421 }
422 }
423 mutex_unlock(&rc->uwb_beca.mutex);
424
425 mutex_lock(&bce->mutex);
426
427 kfree(bce->be);
428
429
430 bce->ts_jiffies = evt->ts_jiffies;
431 bce->be = be;
432 bce->dev_addr = bf->hdr.SrcAddr;
433 bce->mac_addr = &bf->Device_Identifier;
434 be->wBPSTOffset = le16_to_cpu(be->wBPSTOffset);
435 be->wBeaconInfoLength = le16_to_cpu(be->wBeaconInfoLength);
436 stats_add_sample(&bce->lqe_stats, be->bLQI - 7);
437 stats_add_sample(&bce->rssi_stats, be->bRSSI + 18);
438
439
440
441
442 if (bce->uwb_dev == NULL)
443 uwbd_dev_onair(evt->rc, bce);
444
445 mutex_unlock(&bce->mutex);
446
447 return 1;
448 }
449
450
451
452
453
454
455 int uwbd_evt_handle_rc_beacon_size(struct uwb_event *evt)
456 {
457 int result = -EINVAL;
458 struct device *dev = &evt->rc->uwb_dev.dev;
459 struct uwb_rc_evt_beacon_size *bs;
460
461
462 if (evt->notif.size < sizeof(*bs)) {
463 dev_err(dev, "BEACON SIZE notification: Not enough data to "
464 "decode (%zu vs %zu bytes needed)\n",
465 evt->notif.size, sizeof(*bs));
466 goto error;
467 }
468 bs = container_of(evt->notif.rceb, struct uwb_rc_evt_beacon_size, rceb);
469 if (0)
470 dev_info(dev, "Beacon size changed to %u bytes "
471 "(FIXME: action?)\n", le16_to_cpu(bs->wNewBeaconSize));
472 else {
473
474 static unsigned count;
475 if (++count % 1000 == 0)
476 dev_info(dev, "Beacon size changed %u times "
477 "(FIXME: action?)\n", count);
478 }
479 result = 0;
480 error:
481 return result;
482 }
483
484
485
486
487
488
489
490
491
492 int uwbd_evt_handle_rc_bp_slot_change(struct uwb_event *evt)
493 {
494 struct uwb_rc *rc = evt->rc;
495 struct device *dev = &rc->uwb_dev.dev;
496 struct uwb_rc_evt_bp_slot_change *bpsc;
497
498 if (evt->notif.size < sizeof(*bpsc)) {
499 dev_err(dev, "BP SLOT CHANGE event: Not enough data\n");
500 return -EINVAL;
501 }
502 bpsc = container_of(evt->notif.rceb, struct uwb_rc_evt_bp_slot_change, rceb);
503
504 if (uwb_rc_evt_bp_slot_change_no_slot(bpsc)) {
505 dev_err(dev, "stopped beaconing: No free slots in BP\n");
506 mutex_lock(&rc->uwb_dev.mutex);
507 rc->beaconing = -1;
508 mutex_unlock(&rc->uwb_dev.mutex);
509 } else
510 rc->uwb_dev.beacon_slot = uwb_rc_evt_bp_slot_change_slot_num(bpsc);
511
512 return 0;
513 }
514
515
516
517
518
519
520 struct uwb_ie_bpo {
521 struct uwb_ie_hdr hdr;
522 u8 bp_length;
523 u8 data[];
524 } __attribute__((packed));
525
526 int uwbd_evt_handle_rc_bpoie_change(struct uwb_event *evt)
527 {
528 int result = -EINVAL;
529 struct device *dev = &evt->rc->uwb_dev.dev;
530 struct uwb_rc_evt_bpoie_change *bpoiec;
531 struct uwb_ie_bpo *bpoie;
532 static unsigned count;
533 size_t iesize;
534
535
536 if (evt->notif.size < sizeof(*bpoiec)) {
537 dev_err(dev, "BPOIEC notification: Not enough data to "
538 "decode (%zu vs %zu bytes needed)\n",
539 evt->notif.size, sizeof(*bpoiec));
540 goto error;
541 }
542 bpoiec = container_of(evt->notif.rceb, struct uwb_rc_evt_bpoie_change, rceb);
543 iesize = le16_to_cpu(bpoiec->wBPOIELength);
544 if (iesize < sizeof(*bpoie)) {
545 dev_err(dev, "BPOIEC notification: Not enough IE data to "
546 "decode (%zu vs %zu bytes needed)\n",
547 iesize, sizeof(*bpoie));
548 goto error;
549 }
550 if (++count % 1000 == 0)
551 dev_info(dev, "BPOIE: %u changes received\n", count);
552
553
554
555
556 result = 0;
557 error:
558 return result;
559 }
560
561
562
563
564 static ssize_t uwb_rc_beacon_show(struct device *dev,
565 struct device_attribute *attr, char *buf)
566 {
567 struct uwb_dev *uwb_dev = to_uwb_dev(dev);
568 struct uwb_rc *rc = uwb_dev->rc;
569 ssize_t result;
570
571 mutex_lock(&rc->uwb_dev.mutex);
572 result = sprintf(buf, "%d\n", rc->beaconing);
573 mutex_unlock(&rc->uwb_dev.mutex);
574 return result;
575 }
576
577
578
579
580 static ssize_t uwb_rc_beacon_store(struct device *dev,
581 struct device_attribute *attr,
582 const char *buf, size_t size)
583 {
584 struct uwb_dev *uwb_dev = to_uwb_dev(dev);
585 struct uwb_rc *rc = uwb_dev->rc;
586 int channel;
587 ssize_t result = -EINVAL;
588
589 result = sscanf(buf, "%d", &channel);
590 if (result >= 1)
591 result = uwb_radio_force_channel(rc, channel);
592
593 return result < 0 ? result : size;
594 }
595 DEVICE_ATTR(beacon, S_IRUGO | S_IWUSR, uwb_rc_beacon_show, uwb_rc_beacon_store);