2 #include <linux/kernel.h>
3 #include <linux/init.h>
4 #include <linux/module.h>
5 #include <linux/device.h>
7 #include <linux/kdev_t.h>
8 #include <linux/slab.h>
10 #include "uwb-internal.h"
12 /* Start Beaconing command structure */
13 struct uwb_rc_cmd_start_beacon
{
17 } __attribute__((packed
));
20 static int uwb_rc_start_beacon(struct uwb_rc
*rc
, u16 bpst_offset
, u8 channel
)
23 struct uwb_rc_cmd_start_beacon
*cmd
;
24 struct uwb_rc_evt_confirm reply
;
26 cmd
= kzalloc(sizeof(*cmd
), GFP_KERNEL
);
29 cmd
->rccb
.bCommandType
= UWB_RC_CET_GENERAL
;
30 cmd
->rccb
.wCommand
= cpu_to_le16(UWB_RC_CMD_START_BEACON
);
31 cmd
->wBPSTOffset
= cpu_to_le16(bpst_offset
);
32 cmd
->bChannelNumber
= channel
;
33 reply
.rceb
.bEventType
= UWB_RC_CET_GENERAL
;
34 reply
.rceb
.wEvent
= UWB_RC_CMD_START_BEACON
;
35 result
= uwb_rc_cmd(rc
, "START-BEACON", &cmd
->rccb
, sizeof(*cmd
),
36 &reply
.rceb
, sizeof(reply
));
39 if (reply
.bResultCode
!= UWB_RC_RES_SUCCESS
) {
40 dev_err(&rc
->uwb_dev
.dev
,
41 "START-BEACON: command execution failed: %s (%d)\n",
42 uwb_rc_strerror(reply
.bResultCode
), reply
.bResultCode
);
50 static int uwb_rc_stop_beacon(struct uwb_rc
*rc
)
54 struct uwb_rc_evt_confirm reply
;
56 cmd
= kzalloc(sizeof(*cmd
), GFP_KERNEL
);
59 cmd
->bCommandType
= UWB_RC_CET_GENERAL
;
60 cmd
->wCommand
= cpu_to_le16(UWB_RC_CMD_STOP_BEACON
);
61 reply
.rceb
.bEventType
= UWB_RC_CET_GENERAL
;
62 reply
.rceb
.wEvent
= UWB_RC_CMD_STOP_BEACON
;
63 result
= uwb_rc_cmd(rc
, "STOP-BEACON", cmd
, sizeof(*cmd
),
64 &reply
.rceb
, sizeof(reply
));
67 if (reply
.bResultCode
!= UWB_RC_RES_SUCCESS
) {
68 dev_err(&rc
->uwb_dev
.dev
,
69 "STOP-BEACON: command execution failed: %s (%d)\n",
70 uwb_rc_strerror(reply
.bResultCode
), reply
.bResultCode
);
78 int uwb_rc_beacon(struct uwb_rc
*rc
, int channel
, unsigned bpst_offset
)
81 struct device
*dev
= &rc
->uwb_dev
.dev
;
86 result
= uwb_rc_stop_beacon(rc
);
88 /* channel >= 0...dah */
89 result
= uwb_rc_start_beacon(rc
, bpst_offset
, channel
);
92 if (le16_to_cpu(rc
->ies
->wIELength
) > 0) {
93 result
= uwb_rc_set_ie(rc
, rc
->ies
);
95 dev_err(dev
, "Cannot set new IE on device: "
97 result
= uwb_rc_stop_beacon(rc
);
105 rc
->beaconing
= channel
;
110 void uwb_bce_kfree(struct kref
*_bce
)
112 struct uwb_beca_e
*bce
= container_of(_bce
, struct uwb_beca_e
, refcnt
);
119 /* Find a beacon by dev addr in the cache */
121 struct uwb_beca_e
*__uwb_beca_find_bydev(struct uwb_rc
*rc
,
122 const struct uwb_dev_addr
*dev_addr
)
124 struct uwb_beca_e
*bce
, *next
;
125 list_for_each_entry_safe(bce
, next
, &rc
->uwb_beca
.list
, node
) {
126 if (!memcmp(&bce
->dev_addr
, dev_addr
, sizeof(bce
->dev_addr
)))
134 /* Find a beacon by dev addr in the cache */
136 struct uwb_beca_e
*__uwb_beca_find_bymac(struct uwb_rc
*rc
,
137 const struct uwb_mac_addr
*mac_addr
)
139 struct uwb_beca_e
*bce
, *next
;
140 list_for_each_entry_safe(bce
, next
, &rc
->uwb_beca
.list
, node
) {
141 if (!memcmp(bce
->mac_addr
, mac_addr
->data
,
142 sizeof(struct uwb_mac_addr
)))
151 * uwb_dev_get_by_devaddr - get a UWB device with a specific DevAddr
152 * @rc: the radio controller that saw the device
153 * @devaddr: DevAddr of the UWB device to find
155 * There may be more than one matching device (in the case of a
156 * DevAddr conflict), but only the first one is returned.
158 struct uwb_dev
*uwb_dev_get_by_devaddr(struct uwb_rc
*rc
,
159 const struct uwb_dev_addr
*devaddr
)
161 struct uwb_dev
*found
= NULL
;
162 struct uwb_beca_e
*bce
;
164 mutex_lock(&rc
->uwb_beca
.mutex
);
165 bce
= __uwb_beca_find_bydev(rc
, devaddr
);
167 found
= uwb_dev_try_get(rc
, bce
->uwb_dev
);
168 mutex_unlock(&rc
->uwb_beca
.mutex
);
174 * uwb_dev_get_by_macaddr - get a UWB device with a specific EUI-48
175 * @rc: the radio controller that saw the device
176 * @devaddr: EUI-48 of the UWB device to find
178 struct uwb_dev
*uwb_dev_get_by_macaddr(struct uwb_rc
*rc
,
179 const struct uwb_mac_addr
*macaddr
)
181 struct uwb_dev
*found
= NULL
;
182 struct uwb_beca_e
*bce
;
184 mutex_lock(&rc
->uwb_beca
.mutex
);
185 bce
= __uwb_beca_find_bymac(rc
, macaddr
);
187 found
= uwb_dev_try_get(rc
, bce
->uwb_dev
);
188 mutex_unlock(&rc
->uwb_beca
.mutex
);
193 /* Initialize a beacon cache entry */
194 static void uwb_beca_e_init(struct uwb_beca_e
*bce
)
196 mutex_init(&bce
->mutex
);
197 kref_init(&bce
->refcnt
);
198 stats_init(&bce
->lqe_stats
);
199 stats_init(&bce
->rssi_stats
);
203 * Add a beacon to the cache
205 * @be: Beacon event information
206 * @bf: Beacon frame (part of b, really)
207 * @ts_jiffies: Timestamp (in jiffies) when the beacon was received
210 struct uwb_beca_e
*__uwb_beca_add(struct uwb_rc
*rc
,
211 struct uwb_rc_evt_beacon
*be
,
212 struct uwb_beacon_frame
*bf
,
213 unsigned long ts_jiffies
)
215 struct uwb_beca_e
*bce
;
217 bce
= kzalloc(sizeof(*bce
), GFP_KERNEL
);
220 uwb_beca_e_init(bce
);
221 bce
->ts_jiffies
= ts_jiffies
;
223 list_add(&bce
->node
, &rc
->uwb_beca
.list
);
228 * Wipe out beacon entries that became stale
230 * Remove associated devicest too.
232 void uwb_beca_purge(struct uwb_rc
*rc
)
234 struct uwb_beca_e
*bce
, *next
;
235 unsigned long expires
;
237 mutex_lock(&rc
->uwb_beca
.mutex
);
238 list_for_each_entry_safe(bce
, next
, &rc
->uwb_beca
.list
, node
) {
239 expires
= bce
->ts_jiffies
+ msecs_to_jiffies(beacon_timeout_ms
);
240 if (time_after(jiffies
, expires
)) {
241 uwbd_dev_offair(bce
);
244 mutex_unlock(&rc
->uwb_beca
.mutex
);
247 /* Clean up the whole beacon cache. Called on shutdown */
248 void uwb_beca_release(struct uwb_rc
*rc
)
250 struct uwb_beca_e
*bce
, *next
;
252 mutex_lock(&rc
->uwb_beca
.mutex
);
253 list_for_each_entry_safe(bce
, next
, &rc
->uwb_beca
.list
, node
) {
254 list_del(&bce
->node
);
257 mutex_unlock(&rc
->uwb_beca
.mutex
);
260 static void uwb_beacon_print(struct uwb_rc
*rc
, struct uwb_rc_evt_beacon
*be
,
261 struct uwb_beacon_frame
*bf
)
263 char macbuf
[UWB_ADDR_STRSIZE
];
264 char devbuf
[UWB_ADDR_STRSIZE
];
265 char dstbuf
[UWB_ADDR_STRSIZE
];
267 uwb_mac_addr_print(macbuf
, sizeof(macbuf
), &bf
->Device_Identifier
);
268 uwb_dev_addr_print(devbuf
, sizeof(devbuf
), &bf
->hdr
.SrcAddr
);
269 uwb_dev_addr_print(dstbuf
, sizeof(dstbuf
), &bf
->hdr
.DestAddr
);
270 dev_info(&rc
->uwb_dev
.dev
,
271 "BEACON from %s to %s (ch%u offset %u slot %u MAC %s)\n",
272 devbuf
, dstbuf
, be
->bChannelNumber
, be
->wBPSTOffset
,
273 bf
->Beacon_Slot_Number
, macbuf
);
277 * @bce: beacon cache entry, referenced
279 ssize_t
uwb_bce_print_IEs(struct uwb_dev
*uwb_dev
, struct uwb_beca_e
*bce
,
280 char *buf
, size_t size
)
283 struct uwb_rc_evt_beacon
*be
;
284 struct uwb_beacon_frame
*bf
;
286 struct uwb_ie_hdr
*ies
;
288 mutex_lock(&bce
->mutex
);
292 bf
= (struct uwb_beacon_frame
*)bce
->be
->BeaconInfo
;
293 ies_len
= be
->wBeaconInfoLength
- sizeof(struct uwb_beacon_frame
);
294 ies
= (struct uwb_ie_hdr
*)bf
->IEData
;
296 result
= uwb_ie_dump_hex(ies
, ies_len
, buf
, size
);
299 mutex_unlock(&bce
->mutex
);
305 * Verify that the beacon event, frame and IEs are ok
307 static int uwb_verify_beacon(struct uwb_rc
*rc
, struct uwb_event
*evt
,
308 struct uwb_rc_evt_beacon
*be
)
310 int result
= -EINVAL
;
311 struct uwb_beacon_frame
*bf
;
312 struct device
*dev
= &rc
->uwb_dev
.dev
;
314 /* Is there enough data to decode a beacon frame? */
315 if (evt
->notif
.size
< sizeof(*be
) + sizeof(*bf
)) {
316 dev_err(dev
, "BEACON event: Not enough data to decode "
317 "(%zu vs %zu bytes needed)\n", evt
->notif
.size
,
318 sizeof(*be
) + sizeof(*bf
));
327 * Handle UWB_RC_EVT_BEACON events
329 * We check the beacon cache to see how the received beacon fares. If
330 * is there already we refresh the timestamp. If not we create a new
333 * According to the WHCI and WUSB specs, only one beacon frame is
334 * allowed per notification block, so we don't bother about scanning
337 int uwbd_evt_handle_rc_beacon(struct uwb_event
*evt
)
339 int result
= -EINVAL
;
341 struct uwb_rc_evt_beacon
*be
;
342 struct uwb_beacon_frame
*bf
;
343 struct uwb_beca_e
*bce
;
344 unsigned long last_ts
;
347 be
= container_of(evt
->notif
.rceb
, struct uwb_rc_evt_beacon
, rceb
);
348 result
= uwb_verify_beacon(rc
, evt
, be
);
352 if (be
->bBeaconType
== UWB_RC_BEACON_TYPE_OL_ALIEN
||
353 be
->bBeaconType
== UWB_RC_BEACON_TYPE_NOL_ALIEN
) {
357 bf
= (struct uwb_beacon_frame
*) be
->BeaconInfo
;
360 * Drop beacons from devices with a NULL EUI-48 -- they cannot
361 * be uniquely identified.
363 * It's expected that these will all be WUSB devices and they
364 * have a WUSB specific connection method so ignoring them
365 * here shouldn't be a problem.
367 if (uwb_mac_addr_bcast(&bf
->Device_Identifier
))
370 mutex_lock(&rc
->uwb_beca
.mutex
);
371 bce
= __uwb_beca_find_bymac(rc
, &bf
->Device_Identifier
);
373 /* Not in there, a new device is pinging */
374 uwb_beacon_print(evt
->rc
, be
, bf
);
375 bce
= __uwb_beca_add(rc
, be
, bf
, evt
->ts_jiffies
);
377 mutex_unlock(&rc
->uwb_beca
.mutex
);
381 mutex_unlock(&rc
->uwb_beca
.mutex
);
383 mutex_lock(&bce
->mutex
);
384 /* purge old beacon data */
387 last_ts
= bce
->ts_jiffies
;
389 /* Update commonly used fields */
390 bce
->ts_jiffies
= evt
->ts_jiffies
;
392 bce
->dev_addr
= bf
->hdr
.SrcAddr
;
393 bce
->mac_addr
= &bf
->Device_Identifier
;
394 be
->wBPSTOffset
= le16_to_cpu(be
->wBPSTOffset
);
395 be
->wBeaconInfoLength
= le16_to_cpu(be
->wBeaconInfoLength
);
396 stats_add_sample(&bce
->lqe_stats
, be
->bLQI
- 7);
397 stats_add_sample(&bce
->rssi_stats
, be
->bRSSI
+ 18);
400 * This might be a beacon from a new device.
402 if (bce
->uwb_dev
== NULL
)
403 uwbd_dev_onair(evt
->rc
, bce
);
405 mutex_unlock(&bce
->mutex
);
407 return 1; /* we keep the event data */
411 * Handle UWB_RC_EVT_BEACON_SIZE events
415 int uwbd_evt_handle_rc_beacon_size(struct uwb_event
*evt
)
417 int result
= -EINVAL
;
418 struct device
*dev
= &evt
->rc
->uwb_dev
.dev
;
419 struct uwb_rc_evt_beacon_size
*bs
;
421 /* Is there enough data to decode the event? */
422 if (evt
->notif
.size
< sizeof(*bs
)) {
423 dev_err(dev
, "BEACON SIZE notification: Not enough data to "
424 "decode (%zu vs %zu bytes needed)\n",
425 evt
->notif
.size
, sizeof(*bs
));
428 bs
= container_of(evt
->notif
.rceb
, struct uwb_rc_evt_beacon_size
, rceb
);
430 dev_info(dev
, "Beacon size changed to %u bytes "
431 "(FIXME: action?)\n", le16_to_cpu(bs
->wNewBeaconSize
));
433 /* temporary hack until we do something with this message... */
434 static unsigned count
;
435 if (++count
% 1000 == 0)
436 dev_info(dev
, "Beacon size changed %u times "
437 "(FIXME: action?)\n", count
);
445 * uwbd_evt_handle_rc_bp_slot_change - handle a BP_SLOT_CHANGE event
446 * @evt: the BP_SLOT_CHANGE notification from the radio controller
448 * If the event indicates that no beacon period slots were available
449 * then radio controller has transitioned to a non-beaconing state.
450 * Otherwise, simply save the current beacon slot.
452 int uwbd_evt_handle_rc_bp_slot_change(struct uwb_event
*evt
)
454 struct uwb_rc
*rc
= evt
->rc
;
455 struct device
*dev
= &rc
->uwb_dev
.dev
;
456 struct uwb_rc_evt_bp_slot_change
*bpsc
;
458 if (evt
->notif
.size
< sizeof(*bpsc
)) {
459 dev_err(dev
, "BP SLOT CHANGE event: Not enough data\n");
462 bpsc
= container_of(evt
->notif
.rceb
, struct uwb_rc_evt_bp_slot_change
, rceb
);
464 mutex_lock(&rc
->uwb_dev
.mutex
);
465 if (uwb_rc_evt_bp_slot_change_no_slot(bpsc
)) {
466 dev_info(dev
, "stopped beaconing: No free slots in BP\n");
469 rc
->uwb_dev
.beacon_slot
= uwb_rc_evt_bp_slot_change_slot_num(bpsc
);
470 mutex_unlock(&rc
->uwb_dev
.mutex
);
476 * Handle UWB_RC_EVT_BPOIE_CHANGE events
481 struct uwb_ie_hdr hdr
;
484 } __attribute__((packed
));
486 int uwbd_evt_handle_rc_bpoie_change(struct uwb_event
*evt
)
488 int result
= -EINVAL
;
489 struct device
*dev
= &evt
->rc
->uwb_dev
.dev
;
490 struct uwb_rc_evt_bpoie_change
*bpoiec
;
491 struct uwb_ie_bpo
*bpoie
;
492 static unsigned count
;
495 /* Is there enough data to decode it? */
496 if (evt
->notif
.size
< sizeof(*bpoiec
)) {
497 dev_err(dev
, "BPOIEC notification: Not enough data to "
498 "decode (%zu vs %zu bytes needed)\n",
499 evt
->notif
.size
, sizeof(*bpoiec
));
502 bpoiec
= container_of(evt
->notif
.rceb
, struct uwb_rc_evt_bpoie_change
, rceb
);
503 iesize
= le16_to_cpu(bpoiec
->wBPOIELength
);
504 if (iesize
< sizeof(*bpoie
)) {
505 dev_err(dev
, "BPOIEC notification: Not enough IE data to "
506 "decode (%zu vs %zu bytes needed)\n",
507 iesize
, sizeof(*bpoie
));
510 if (++count
% 1000 == 0) /* Lame placeholder */
511 dev_info(dev
, "BPOIE: %u changes received\n", count
);
518 * Print beaconing state.
520 static ssize_t
uwb_rc_beacon_show(struct device
*dev
,
521 struct device_attribute
*attr
, char *buf
)
523 struct uwb_dev
*uwb_dev
= to_uwb_dev(dev
);
524 struct uwb_rc
*rc
= uwb_dev
->rc
;
527 mutex_lock(&rc
->uwb_dev
.mutex
);
528 result
= sprintf(buf
, "%d\n", rc
->beaconing
);
529 mutex_unlock(&rc
->uwb_dev
.mutex
);
534 * Start beaconing on the specified channel, or stop beaconing.
536 static ssize_t
uwb_rc_beacon_store(struct device
*dev
,
537 struct device_attribute
*attr
,
538 const char *buf
, size_t size
)
540 struct uwb_dev
*uwb_dev
= to_uwb_dev(dev
);
541 struct uwb_rc
*rc
= uwb_dev
->rc
;
543 ssize_t result
= -EINVAL
;
545 result
= sscanf(buf
, "%d", &channel
);
547 result
= uwb_radio_force_channel(rc
, channel
);
549 return result
< 0 ? result
: size
;
551 DEVICE_ATTR(beacon
, S_IRUGO
| S_IWUSR
, uwb_rc_beacon_show
, uwb_rc_beacon_store
);