Commit | Line | Data |
---|---|---|
57588c71 MR |
1 | // SPDX-License-Identifier: GPL-2.0-only |
2 | /* | |
3 | * IEEE 802.15.4 scanning management | |
4 | * | |
5 | * Copyright (C) 2021 Qorvo US, Inc | |
6 | * Authors: | |
7 | * - David Girault <david.girault@qorvo.com> | |
8 | * - Miquel Raynal <miquel.raynal@bootlin.com> | |
9 | */ | |
10 | ||
11 | #include <linux/module.h> | |
12 | #include <linux/rtnetlink.h> | |
13 | #include <net/mac802154.h> | |
14 | ||
15 | #include "ieee802154_i.h" | |
16 | #include "driver-ops.h" | |
17 | #include "../ieee802154/nl802154.h" | |
18 | ||
3accf476 MR |
19 | #define IEEE802154_BEACON_MHR_SZ 13 |
20 | #define IEEE802154_BEACON_PL_SZ 4 | |
e2c3e6f5 MR |
21 | #define IEEE802154_MAC_CMD_MHR_SZ 23 |
22 | #define IEEE802154_MAC_CMD_PL_SZ 1 | |
3accf476 MR |
23 | #define IEEE802154_BEACON_SKB_SZ (IEEE802154_BEACON_MHR_SZ + \ |
24 | IEEE802154_BEACON_PL_SZ) | |
e2c3e6f5 MR |
25 | #define IEEE802154_MAC_CMD_SKB_SZ (IEEE802154_MAC_CMD_MHR_SZ + \ |
26 | IEEE802154_MAC_CMD_PL_SZ) | |
3accf476 | 27 | |
57588c71 MR |
28 | /* mac802154_scan_cleanup_locked() must be called upon scan completion or abort. |
29 | * - Completions are asynchronous, not locked by the rtnl and decided by the | |
30 | * scan worker. | |
31 | * - Aborts are decided by userspace, and locked by the rtnl. | |
32 | * | |
33 | * Concurrent modifications to the PHY, the interfaces or the hardware is in | |
34 | * general prevented by the rtnl. So in most cases we don't need additional | |
35 | * protection. | |
36 | * | |
37 | * However, the scan worker get's triggered without anybody noticing and thus we | |
38 | * must ensure the presence of the devices as well as data consistency: | |
39 | * - The sub-interface and device driver module get both their reference | |
40 | * counters incremented whenever we start a scan, so they cannot disappear | |
41 | * during operation. | |
42 | * - Data consistency is achieved by the use of rcu protected pointers. | |
43 | */ | |
44 | static int mac802154_scan_cleanup_locked(struct ieee802154_local *local, | |
45 | struct ieee802154_sub_if_data *sdata, | |
46 | bool aborted) | |
47 | { | |
48 | struct wpan_dev *wpan_dev = &sdata->wpan_dev; | |
49 | struct wpan_phy *wpan_phy = local->phy; | |
50 | struct cfg802154_scan_request *request; | |
51 | u8 arg; | |
52 | ||
53 | /* Prevent any further use of the scan request */ | |
54 | clear_bit(IEEE802154_IS_SCANNING, &local->ongoing); | |
55 | cancel_delayed_work(&local->scan_work); | |
56 | request = rcu_replace_pointer(local->scan_req, NULL, 1); | |
57 | if (!request) | |
58 | return 0; | |
eb56a4cb | 59 | kvfree_rcu_mightsleep(request); |
57588c71 MR |
60 | |
61 | /* Advertize first, while we know the devices cannot be removed */ | |
62 | if (aborted) | |
63 | arg = NL802154_SCAN_DONE_REASON_ABORTED; | |
64 | else | |
65 | arg = NL802154_SCAN_DONE_REASON_FINISHED; | |
66 | nl802154_scan_done(wpan_phy, wpan_dev, arg); | |
67 | ||
68 | /* Cleanup software stack */ | |
69 | ieee802154_mlme_op_post(local); | |
70 | ||
71 | /* Set the hardware back in its original state */ | |
72 | drv_set_channel(local, wpan_phy->current_page, | |
73 | wpan_phy->current_channel); | |
74 | ieee802154_configure_durations(wpan_phy, wpan_phy->current_page, | |
75 | wpan_phy->current_channel); | |
76 | drv_stop(local); | |
77 | synchronize_net(); | |
78 | sdata->required_filtering = sdata->iface_default_filtering; | |
79 | drv_start(local, sdata->required_filtering, &local->addr_filt); | |
80 | ||
81 | return 0; | |
82 | } | |
83 | ||
84 | int mac802154_abort_scan_locked(struct ieee802154_local *local, | |
85 | struct ieee802154_sub_if_data *sdata) | |
86 | { | |
87 | ASSERT_RTNL(); | |
88 | ||
89 | if (!mac802154_is_scanning(local)) | |
90 | return -ESRCH; | |
91 | ||
92 | return mac802154_scan_cleanup_locked(local, sdata, true); | |
93 | } | |
94 | ||
95 | static unsigned int mac802154_scan_get_channel_time(u8 duration_order, | |
96 | u8 symbol_duration) | |
97 | { | |
98 | u64 base_super_frame_duration = (u64)symbol_duration * | |
99 | IEEE802154_SUPERFRAME_PERIOD * IEEE802154_SLOT_PERIOD; | |
100 | ||
101 | return usecs_to_jiffies(base_super_frame_duration * | |
102 | (BIT(duration_order) + 1)); | |
103 | } | |
104 | ||
105 | static void mac802154_flush_queued_beacons(struct ieee802154_local *local) | |
106 | { | |
107 | struct cfg802154_mac_pkt *mac_pkt, *tmp; | |
108 | ||
109 | list_for_each_entry_safe(mac_pkt, tmp, &local->rx_beacon_list, node) { | |
110 | list_del(&mac_pkt->node); | |
111 | kfree_skb(mac_pkt->skb); | |
112 | kfree(mac_pkt); | |
113 | } | |
114 | } | |
115 | ||
116 | static void | |
117 | mac802154_scan_get_next_channel(struct ieee802154_local *local, | |
118 | struct cfg802154_scan_request *scan_req, | |
119 | u8 *channel) | |
120 | { | |
121 | (*channel)++; | |
122 | *channel = find_next_bit((const unsigned long *)&scan_req->channels, | |
123 | IEEE802154_MAX_CHANNEL + 1, | |
124 | *channel); | |
125 | } | |
126 | ||
127 | static int mac802154_scan_find_next_chan(struct ieee802154_local *local, | |
128 | struct cfg802154_scan_request *scan_req, | |
129 | u8 page, u8 *channel) | |
130 | { | |
131 | mac802154_scan_get_next_channel(local, scan_req, channel); | |
132 | if (*channel > IEEE802154_MAX_CHANNEL) | |
133 | return -EINVAL; | |
134 | ||
135 | return 0; | |
136 | } | |
137 | ||
e2c3e6f5 MR |
138 | static int mac802154_scan_prepare_beacon_req(struct ieee802154_local *local) |
139 | { | |
140 | memset(&local->scan_beacon_req, 0, sizeof(local->scan_beacon_req)); | |
141 | local->scan_beacon_req.mhr.fc.type = IEEE802154_FC_TYPE_MAC_CMD; | |
142 | local->scan_beacon_req.mhr.fc.dest_addr_mode = IEEE802154_SHORT_ADDRESSING; | |
143 | local->scan_beacon_req.mhr.fc.version = IEEE802154_2003_STD; | |
144 | local->scan_beacon_req.mhr.fc.source_addr_mode = IEEE802154_NO_ADDRESSING; | |
145 | local->scan_beacon_req.mhr.dest.mode = IEEE802154_ADDR_SHORT; | |
146 | local->scan_beacon_req.mhr.dest.pan_id = cpu_to_le16(IEEE802154_PANID_BROADCAST); | |
147 | local->scan_beacon_req.mhr.dest.short_addr = cpu_to_le16(IEEE802154_ADDR_BROADCAST); | |
148 | local->scan_beacon_req.mac_pl.cmd_id = IEEE802154_CMD_BEACON_REQ; | |
149 | ||
150 | return 0; | |
151 | } | |
152 | ||
153 | static int mac802154_transmit_beacon_req(struct ieee802154_local *local, | |
154 | struct ieee802154_sub_if_data *sdata) | |
155 | { | |
156 | struct sk_buff *skb; | |
157 | int ret; | |
158 | ||
159 | skb = alloc_skb(IEEE802154_MAC_CMD_SKB_SZ, GFP_KERNEL); | |
160 | if (!skb) | |
161 | return -ENOBUFS; | |
162 | ||
163 | skb->dev = sdata->dev; | |
164 | ||
165 | ret = ieee802154_mac_cmd_push(skb, &local->scan_beacon_req, NULL, 0); | |
166 | if (ret) { | |
167 | kfree_skb(skb); | |
168 | return ret; | |
169 | } | |
170 | ||
171 | return ieee802154_mlme_tx(local, sdata, skb); | |
172 | } | |
173 | ||
57588c71 MR |
174 | void mac802154_scan_worker(struct work_struct *work) |
175 | { | |
176 | struct ieee802154_local *local = | |
177 | container_of(work, struct ieee802154_local, scan_work.work); | |
178 | struct cfg802154_scan_request *scan_req; | |
179 | struct ieee802154_sub_if_data *sdata; | |
180 | unsigned int scan_duration = 0; | |
181 | struct wpan_phy *wpan_phy; | |
182 | u8 scan_req_duration; | |
183 | u8 page, channel; | |
184 | int ret; | |
185 | ||
186 | /* Ensure the device receiver is turned off when changing channels | |
187 | * because there is no atomic way to change the channel and know on | |
188 | * which one a beacon might have been received. | |
189 | */ | |
190 | drv_stop(local); | |
191 | synchronize_net(); | |
192 | mac802154_flush_queued_beacons(local); | |
193 | ||
194 | rcu_read_lock(); | |
195 | scan_req = rcu_dereference(local->scan_req); | |
196 | if (unlikely(!scan_req)) { | |
197 | rcu_read_unlock(); | |
198 | return; | |
199 | } | |
200 | ||
201 | sdata = IEEE802154_WPAN_DEV_TO_SUB_IF(scan_req->wpan_dev); | |
202 | ||
203 | /* Wait an arbitrary amount of time in case we cannot use the device */ | |
204 | if (local->suspended || !ieee802154_sdata_running(sdata)) { | |
205 | rcu_read_unlock(); | |
206 | queue_delayed_work(local->mac_wq, &local->scan_work, | |
207 | msecs_to_jiffies(1000)); | |
208 | return; | |
209 | } | |
210 | ||
211 | wpan_phy = scan_req->wpan_phy; | |
212 | scan_req_duration = scan_req->duration; | |
213 | ||
214 | /* Look for the next valid chan */ | |
215 | page = local->scan_page; | |
216 | channel = local->scan_channel; | |
217 | do { | |
218 | ret = mac802154_scan_find_next_chan(local, scan_req, page, &channel); | |
219 | if (ret) { | |
220 | rcu_read_unlock(); | |
221 | goto end_scan; | |
222 | } | |
223 | } while (!ieee802154_chan_is_valid(scan_req->wpan_phy, page, channel)); | |
224 | ||
225 | rcu_read_unlock(); | |
226 | ||
227 | /* Bypass the stack on purpose when changing the channel */ | |
228 | rtnl_lock(); | |
229 | ret = drv_set_channel(local, page, channel); | |
230 | rtnl_unlock(); | |
231 | if (ret) { | |
232 | dev_err(&sdata->dev->dev, | |
233 | "Channel change failure during scan, aborting (%d)\n", ret); | |
234 | goto end_scan; | |
235 | } | |
236 | ||
237 | local->scan_page = page; | |
238 | local->scan_channel = channel; | |
239 | ||
240 | rtnl_lock(); | |
241 | ret = drv_start(local, IEEE802154_FILTERING_3_SCAN, &local->addr_filt); | |
242 | rtnl_unlock(); | |
243 | if (ret) { | |
244 | dev_err(&sdata->dev->dev, | |
245 | "Restarting failure after channel change, aborting (%d)\n", ret); | |
246 | goto end_scan; | |
247 | } | |
248 | ||
e2c3e6f5 MR |
249 | if (scan_req->type == NL802154_SCAN_ACTIVE) { |
250 | ret = mac802154_transmit_beacon_req(local, sdata); | |
251 | if (ret) | |
252 | dev_err(&sdata->dev->dev, | |
253 | "Error when transmitting beacon request (%d)\n", ret); | |
254 | } | |
255 | ||
57588c71 MR |
256 | ieee802154_configure_durations(wpan_phy, page, channel); |
257 | scan_duration = mac802154_scan_get_channel_time(scan_req_duration, | |
258 | wpan_phy->symbol_duration); | |
259 | dev_dbg(&sdata->dev->dev, | |
260 | "Scan page %u channel %u for %ums\n", | |
261 | page, channel, jiffies_to_msecs(scan_duration)); | |
262 | queue_delayed_work(local->mac_wq, &local->scan_work, scan_duration); | |
263 | return; | |
264 | ||
265 | end_scan: | |
266 | rtnl_lock(); | |
267 | mac802154_scan_cleanup_locked(local, sdata, false); | |
268 | rtnl_unlock(); | |
269 | } | |
270 | ||
271 | int mac802154_trigger_scan_locked(struct ieee802154_sub_if_data *sdata, | |
272 | struct cfg802154_scan_request *request) | |
273 | { | |
274 | struct ieee802154_local *local = sdata->local; | |
275 | ||
276 | ASSERT_RTNL(); | |
277 | ||
278 | if (mac802154_is_scanning(local)) | |
279 | return -EBUSY; | |
280 | ||
e2c3e6f5 MR |
281 | if (request->type != NL802154_SCAN_PASSIVE && |
282 | request->type != NL802154_SCAN_ACTIVE) | |
57588c71 MR |
283 | return -EOPNOTSUPP; |
284 | ||
285 | /* Store scanning parameters */ | |
286 | rcu_assign_pointer(local->scan_req, request); | |
287 | ||
288 | /* Software scanning requires to set promiscuous mode, so we need to | |
289 | * pause the Tx queue during the entire operation. | |
290 | */ | |
291 | ieee802154_mlme_op_pre(local); | |
292 | ||
293 | sdata->required_filtering = IEEE802154_FILTERING_3_SCAN; | |
294 | local->scan_page = request->page; | |
295 | local->scan_channel = -1; | |
296 | set_bit(IEEE802154_IS_SCANNING, &local->ongoing); | |
e2c3e6f5 MR |
297 | if (request->type == NL802154_SCAN_ACTIVE) |
298 | mac802154_scan_prepare_beacon_req(local); | |
57588c71 MR |
299 | |
300 | nl802154_scan_started(request->wpan_phy, request->wpan_dev); | |
301 | ||
302 | queue_delayed_work(local->mac_wq, &local->scan_work, 0); | |
303 | ||
304 | return 0; | |
305 | } | |
306 | ||
307 | int mac802154_process_beacon(struct ieee802154_local *local, | |
308 | struct sk_buff *skb, | |
309 | u8 page, u8 channel) | |
310 | { | |
311 | struct ieee802154_beacon_hdr *bh = (void *)skb->data; | |
312 | struct ieee802154_addr *src = &mac_cb(skb)->source; | |
313 | struct cfg802154_scan_request *scan_req; | |
314 | struct ieee802154_coord_desc desc; | |
315 | ||
316 | if (skb->len != sizeof(*bh)) | |
317 | return -EINVAL; | |
318 | ||
319 | if (unlikely(src->mode == IEEE802154_ADDR_NONE)) | |
320 | return -EINVAL; | |
321 | ||
322 | dev_dbg(&skb->dev->dev, | |
323 | "BEACON received on page %u channel %u\n", | |
324 | page, channel); | |
325 | ||
326 | memcpy(&desc.addr, src, sizeof(desc.addr)); | |
327 | desc.page = page; | |
328 | desc.channel = channel; | |
329 | desc.link_quality = mac_cb(skb)->lqi; | |
330 | desc.superframe_spec = get_unaligned_le16(skb->data); | |
331 | desc.gts_permit = bh->gts_permit; | |
332 | ||
333 | trace_802154_scan_event(&desc); | |
334 | ||
335 | rcu_read_lock(); | |
336 | scan_req = rcu_dereference(local->scan_req); | |
337 | if (likely(scan_req)) | |
338 | nl802154_scan_event(scan_req->wpan_phy, scan_req->wpan_dev, &desc); | |
339 | rcu_read_unlock(); | |
340 | ||
341 | return 0; | |
342 | } | |
3accf476 MR |
343 | |
344 | static int mac802154_transmit_beacon(struct ieee802154_local *local, | |
345 | struct wpan_dev *wpan_dev) | |
346 | { | |
347 | struct cfg802154_beacon_request *beacon_req; | |
348 | struct ieee802154_sub_if_data *sdata; | |
349 | struct sk_buff *skb; | |
350 | int ret; | |
351 | ||
352 | /* Update the sequence number */ | |
353 | local->beacon.mhr.seq = atomic_inc_return(&wpan_dev->bsn) & 0xFF; | |
354 | ||
355 | skb = alloc_skb(IEEE802154_BEACON_SKB_SZ, GFP_KERNEL); | |
356 | if (!skb) | |
357 | return -ENOBUFS; | |
358 | ||
359 | rcu_read_lock(); | |
360 | beacon_req = rcu_dereference(local->beacon_req); | |
361 | if (unlikely(!beacon_req)) { | |
362 | rcu_read_unlock(); | |
363 | kfree_skb(skb); | |
364 | return -EINVAL; | |
365 | } | |
366 | ||
367 | sdata = IEEE802154_WPAN_DEV_TO_SUB_IF(beacon_req->wpan_dev); | |
368 | skb->dev = sdata->dev; | |
369 | ||
370 | rcu_read_unlock(); | |
371 | ||
372 | ret = ieee802154_beacon_push(skb, &local->beacon); | |
373 | if (ret) { | |
374 | kfree_skb(skb); | |
375 | return ret; | |
376 | } | |
377 | ||
1375e3ba MR |
378 | /* Using the MLME transmission helper for sending beacons is a bit |
379 | * overkill because we do not really care about the final outcome. | |
380 | * | |
381 | * Even though, going through the whole net stack with a regular | |
382 | * dev_queue_xmit() is not relevant either because we want beacons to be | |
383 | * sent "now" rather than go through the whole net stack scheduling | |
384 | * (qdisc & co). | |
385 | * | |
386 | * Finally, using ieee802154_subif_start_xmit() would only be an option | |
387 | * if we had a generic transmit helper which would acquire the | |
388 | * HARD_TX_LOCK() to prevent buffer handling conflicts with regular | |
389 | * packets. | |
390 | * | |
391 | * So for now we keep it simple and send beacons with our MLME helper, | |
392 | * even if it stops the ieee802154 queue entirely during these | |
393 | * transmissions, wich anyway does not have a huge impact on the | |
394 | * performances given the current design of the stack. | |
395 | */ | |
396 | return ieee802154_mlme_tx(local, sdata, skb); | |
3accf476 MR |
397 | } |
398 | ||
399 | void mac802154_beacon_worker(struct work_struct *work) | |
400 | { | |
401 | struct ieee802154_local *local = | |
402 | container_of(work, struct ieee802154_local, beacon_work.work); | |
403 | struct cfg802154_beacon_request *beacon_req; | |
404 | struct ieee802154_sub_if_data *sdata; | |
405 | struct wpan_dev *wpan_dev; | |
d021d218 | 406 | u8 interval; |
3accf476 MR |
407 | int ret; |
408 | ||
409 | rcu_read_lock(); | |
410 | beacon_req = rcu_dereference(local->beacon_req); | |
411 | if (unlikely(!beacon_req)) { | |
412 | rcu_read_unlock(); | |
413 | return; | |
414 | } | |
415 | ||
416 | sdata = IEEE802154_WPAN_DEV_TO_SUB_IF(beacon_req->wpan_dev); | |
417 | ||
418 | /* Wait an arbitrary amount of time in case we cannot use the device */ | |
419 | if (local->suspended || !ieee802154_sdata_running(sdata)) { | |
420 | rcu_read_unlock(); | |
421 | queue_delayed_work(local->mac_wq, &local->beacon_work, | |
422 | msecs_to_jiffies(1000)); | |
423 | return; | |
424 | } | |
425 | ||
426 | wpan_dev = beacon_req->wpan_dev; | |
d021d218 | 427 | interval = beacon_req->interval; |
3accf476 MR |
428 | |
429 | rcu_read_unlock(); | |
430 | ||
431 | dev_dbg(&sdata->dev->dev, "Sending beacon\n"); | |
432 | ret = mac802154_transmit_beacon(local, wpan_dev); | |
433 | if (ret) | |
434 | dev_err(&sdata->dev->dev, | |
435 | "Beacon could not be transmitted (%d)\n", ret); | |
436 | ||
d021d218 MR |
437 | if (interval < IEEE802154_ACTIVE_SCAN_DURATION) |
438 | queue_delayed_work(local->mac_wq, &local->beacon_work, | |
439 | local->beacon_interval); | |
3accf476 MR |
440 | } |
441 | ||
442 | int mac802154_stop_beacons_locked(struct ieee802154_local *local, | |
443 | struct ieee802154_sub_if_data *sdata) | |
444 | { | |
445 | struct wpan_dev *wpan_dev = &sdata->wpan_dev; | |
446 | struct cfg802154_beacon_request *request; | |
447 | ||
448 | ASSERT_RTNL(); | |
449 | ||
450 | if (!mac802154_is_beaconing(local)) | |
451 | return -ESRCH; | |
452 | ||
453 | clear_bit(IEEE802154_IS_BEACONING, &local->ongoing); | |
454 | cancel_delayed_work(&local->beacon_work); | |
455 | request = rcu_replace_pointer(local->beacon_req, NULL, 1); | |
456 | if (!request) | |
457 | return 0; | |
eb56a4cb | 458 | kvfree_rcu_mightsleep(request); |
3accf476 MR |
459 | |
460 | nl802154_beaconing_done(wpan_dev); | |
461 | ||
462 | return 0; | |
463 | } | |
464 | ||
465 | int mac802154_send_beacons_locked(struct ieee802154_sub_if_data *sdata, | |
466 | struct cfg802154_beacon_request *request) | |
467 | { | |
468 | struct ieee802154_local *local = sdata->local; | |
cf1b830e | 469 | struct wpan_dev *wpan_dev = &sdata->wpan_dev; |
3accf476 MR |
470 | |
471 | ASSERT_RTNL(); | |
472 | ||
473 | if (mac802154_is_beaconing(local)) | |
474 | mac802154_stop_beacons_locked(local, sdata); | |
475 | ||
476 | /* Store beaconing parameters */ | |
477 | rcu_assign_pointer(local->beacon_req, request); | |
478 | ||
479 | set_bit(IEEE802154_IS_BEACONING, &local->ongoing); | |
480 | ||
481 | memset(&local->beacon, 0, sizeof(local->beacon)); | |
482 | local->beacon.mhr.fc.type = IEEE802154_FC_TYPE_BEACON; | |
483 | local->beacon.mhr.fc.security_enabled = 0; | |
484 | local->beacon.mhr.fc.frame_pending = 0; | |
485 | local->beacon.mhr.fc.ack_request = 0; | |
486 | local->beacon.mhr.fc.intra_pan = 0; | |
487 | local->beacon.mhr.fc.dest_addr_mode = IEEE802154_NO_ADDRESSING; | |
488 | local->beacon.mhr.fc.version = IEEE802154_2003_STD; | |
489 | local->beacon.mhr.fc.source_addr_mode = IEEE802154_EXTENDED_ADDRESSING; | |
490 | atomic_set(&request->wpan_dev->bsn, -1); | |
491 | local->beacon.mhr.source.mode = IEEE802154_ADDR_LONG; | |
8338304c MR |
492 | local->beacon.mhr.source.pan_id = request->wpan_dev->pan_id; |
493 | local->beacon.mhr.source.extended_addr = request->wpan_dev->extended_addr; | |
3accf476 | 494 | local->beacon.mac_pl.beacon_order = request->interval; |
d021d218 MR |
495 | if (request->interval <= IEEE802154_MAX_SCAN_DURATION) |
496 | local->beacon.mac_pl.superframe_order = request->interval; | |
3accf476 MR |
497 | local->beacon.mac_pl.final_cap_slot = 0xf; |
498 | local->beacon.mac_pl.battery_life_ext = 0; | |
cf1b830e | 499 | local->beacon.mac_pl.pan_coordinator = !wpan_dev->parent; |
3accf476 MR |
500 | local->beacon.mac_pl.assoc_permit = 1; |
501 | ||
d021d218 MR |
502 | if (request->interval == IEEE802154_ACTIVE_SCAN_DURATION) |
503 | return 0; | |
504 | ||
3accf476 MR |
505 | /* Start the beacon work */ |
506 | local->beacon_interval = | |
507 | mac802154_scan_get_channel_time(request->interval, | |
508 | request->wpan_phy->symbol_duration); | |
509 | queue_delayed_work(local->mac_wq, &local->beacon_work, 0); | |
510 | ||
511 | return 0; | |
512 | } | |
fefd1980 MR |
513 | |
514 | int mac802154_perform_association(struct ieee802154_sub_if_data *sdata, | |
515 | struct ieee802154_pan_device *coord, | |
516 | __le16 *short_addr) | |
517 | { | |
518 | u64 ceaddr = swab64((__force u64)coord->extended_addr); | |
519 | struct ieee802154_association_req_frame frame = {}; | |
520 | struct ieee802154_local *local = sdata->local; | |
521 | struct wpan_dev *wpan_dev = &sdata->wpan_dev; | |
522 | struct sk_buff *skb; | |
523 | int ret; | |
524 | ||
525 | frame.mhr.fc.type = IEEE802154_FC_TYPE_MAC_CMD; | |
526 | frame.mhr.fc.security_enabled = 0; | |
527 | frame.mhr.fc.frame_pending = 0; | |
528 | frame.mhr.fc.ack_request = 1; /* We always expect an ack here */ | |
529 | frame.mhr.fc.intra_pan = 0; | |
530 | frame.mhr.fc.dest_addr_mode = (coord->mode == IEEE802154_ADDR_LONG) ? | |
531 | IEEE802154_EXTENDED_ADDRESSING : IEEE802154_SHORT_ADDRESSING; | |
532 | frame.mhr.fc.version = IEEE802154_2003_STD; | |
533 | frame.mhr.fc.source_addr_mode = IEEE802154_EXTENDED_ADDRESSING; | |
534 | frame.mhr.source.mode = IEEE802154_ADDR_LONG; | |
535 | frame.mhr.source.pan_id = cpu_to_le16(IEEE802154_PANID_BROADCAST); | |
536 | frame.mhr.source.extended_addr = wpan_dev->extended_addr; | |
537 | frame.mhr.dest.mode = coord->mode; | |
538 | frame.mhr.dest.pan_id = coord->pan_id; | |
539 | if (coord->mode == IEEE802154_ADDR_LONG) | |
540 | frame.mhr.dest.extended_addr = coord->extended_addr; | |
541 | else | |
542 | frame.mhr.dest.short_addr = coord->short_addr; | |
543 | frame.mhr.seq = atomic_inc_return(&wpan_dev->dsn) & 0xFF; | |
544 | frame.mac_pl.cmd_id = IEEE802154_CMD_ASSOCIATION_REQ; | |
545 | frame.assoc_req_pl.device_type = 1; | |
546 | frame.assoc_req_pl.power_source = 1; | |
547 | frame.assoc_req_pl.rx_on_when_idle = 1; | |
548 | frame.assoc_req_pl.alloc_addr = 1; | |
549 | ||
550 | skb = alloc_skb(IEEE802154_MAC_CMD_SKB_SZ + sizeof(frame.assoc_req_pl), | |
551 | GFP_KERNEL); | |
552 | if (!skb) | |
553 | return -ENOBUFS; | |
554 | ||
555 | skb->dev = sdata->dev; | |
556 | ||
557 | ret = ieee802154_mac_cmd_push(skb, &frame, &frame.assoc_req_pl, | |
558 | sizeof(frame.assoc_req_pl)); | |
559 | if (ret) { | |
560 | kfree_skb(skb); | |
561 | return ret; | |
562 | } | |
563 | ||
564 | local->assoc_dev = coord; | |
565 | reinit_completion(&local->assoc_done); | |
566 | set_bit(IEEE802154_IS_ASSOCIATING, &local->ongoing); | |
567 | ||
568 | ret = ieee802154_mlme_tx_one_locked(local, sdata, skb); | |
569 | if (ret) { | |
570 | if (ret > 0) | |
571 | ret = (ret == IEEE802154_NO_ACK) ? -EREMOTEIO : -EIO; | |
572 | dev_warn(&sdata->dev->dev, | |
573 | "No ASSOC REQ ACK received from %8phC\n", &ceaddr); | |
574 | goto clear_assoc; | |
575 | } | |
576 | ||
577 | ret = wait_for_completion_killable_timeout(&local->assoc_done, 10 * HZ); | |
578 | if (ret <= 0) { | |
579 | dev_warn(&sdata->dev->dev, | |
580 | "No ASSOC RESP received from %8phC\n", &ceaddr); | |
581 | ret = -ETIMEDOUT; | |
582 | goto clear_assoc; | |
583 | } | |
584 | ||
585 | if (local->assoc_status != IEEE802154_ASSOCIATION_SUCCESSFUL) { | |
586 | if (local->assoc_status == IEEE802154_PAN_AT_CAPACITY) | |
587 | ret = -ERANGE; | |
588 | else | |
589 | ret = -EPERM; | |
590 | ||
591 | dev_warn(&sdata->dev->dev, | |
592 | "Negative ASSOC RESP received from %8phC: %s\n", &ceaddr, | |
593 | local->assoc_status == IEEE802154_PAN_AT_CAPACITY ? | |
594 | "PAN at capacity" : "access denied"); | |
595 | } | |
596 | ||
597 | ret = 0; | |
598 | *short_addr = local->assoc_addr; | |
599 | ||
600 | clear_assoc: | |
601 | clear_bit(IEEE802154_IS_ASSOCIATING, &local->ongoing); | |
602 | local->assoc_dev = NULL; | |
603 | ||
604 | return ret; | |
605 | } | |
606 | ||
607 | int mac802154_process_association_resp(struct ieee802154_sub_if_data *sdata, | |
608 | struct sk_buff *skb) | |
609 | { | |
610 | struct ieee802154_addr *src = &mac_cb(skb)->source; | |
611 | struct ieee802154_addr *dest = &mac_cb(skb)->dest; | |
612 | u64 deaddr = swab64((__force u64)dest->extended_addr); | |
613 | struct ieee802154_local *local = sdata->local; | |
614 | struct wpan_dev *wpan_dev = &sdata->wpan_dev; | |
615 | struct ieee802154_assoc_resp_pl resp_pl = {}; | |
616 | ||
617 | if (skb->len != sizeof(resp_pl)) | |
618 | return -EINVAL; | |
619 | ||
620 | if (unlikely(src->mode != IEEE802154_EXTENDED_ADDRESSING || | |
621 | dest->mode != IEEE802154_EXTENDED_ADDRESSING)) | |
622 | return -EINVAL; | |
623 | ||
624 | if (unlikely(dest->extended_addr != wpan_dev->extended_addr || | |
625 | src->extended_addr != local->assoc_dev->extended_addr)) | |
626 | return -ENODEV; | |
627 | ||
628 | memcpy(&resp_pl, skb->data, sizeof(resp_pl)); | |
629 | local->assoc_addr = resp_pl.short_addr; | |
630 | local->assoc_status = resp_pl.status; | |
631 | ||
632 | dev_dbg(&skb->dev->dev, | |
633 | "ASSOC RESP 0x%x received from %8phC, getting short address %04x\n", | |
634 | local->assoc_status, &deaddr, local->assoc_addr); | |
635 | ||
636 | complete(&local->assoc_done); | |
637 | ||
638 | return 0; | |
639 | } | |
9860d9be MR |
640 | |
641 | int mac802154_send_disassociation_notif(struct ieee802154_sub_if_data *sdata, | |
642 | struct ieee802154_pan_device *target, | |
643 | u8 reason) | |
644 | { | |
645 | struct ieee802154_disassociation_notif_frame frame = {}; | |
646 | u64 teaddr = swab64((__force u64)target->extended_addr); | |
647 | struct ieee802154_local *local = sdata->local; | |
648 | struct wpan_dev *wpan_dev = &sdata->wpan_dev; | |
649 | struct sk_buff *skb; | |
650 | int ret; | |
651 | ||
652 | frame.mhr.fc.type = IEEE802154_FC_TYPE_MAC_CMD; | |
653 | frame.mhr.fc.security_enabled = 0; | |
654 | frame.mhr.fc.frame_pending = 0; | |
655 | frame.mhr.fc.ack_request = 1; | |
656 | frame.mhr.fc.intra_pan = 1; | |
657 | frame.mhr.fc.dest_addr_mode = (target->mode == IEEE802154_ADDR_LONG) ? | |
658 | IEEE802154_EXTENDED_ADDRESSING : IEEE802154_SHORT_ADDRESSING; | |
659 | frame.mhr.fc.version = IEEE802154_2003_STD; | |
660 | frame.mhr.fc.source_addr_mode = IEEE802154_EXTENDED_ADDRESSING; | |
661 | frame.mhr.source.mode = IEEE802154_ADDR_LONG; | |
662 | frame.mhr.source.pan_id = wpan_dev->pan_id; | |
663 | frame.mhr.source.extended_addr = wpan_dev->extended_addr; | |
664 | frame.mhr.dest.mode = target->mode; | |
665 | frame.mhr.dest.pan_id = wpan_dev->pan_id; | |
666 | if (target->mode == IEEE802154_ADDR_LONG) | |
667 | frame.mhr.dest.extended_addr = target->extended_addr; | |
668 | else | |
669 | frame.mhr.dest.short_addr = target->short_addr; | |
670 | frame.mhr.seq = atomic_inc_return(&wpan_dev->dsn) & 0xFF; | |
671 | frame.mac_pl.cmd_id = IEEE802154_CMD_DISASSOCIATION_NOTIFY; | |
672 | frame.disassoc_pl = reason; | |
673 | ||
674 | skb = alloc_skb(IEEE802154_MAC_CMD_SKB_SZ + sizeof(frame.disassoc_pl), | |
675 | GFP_KERNEL); | |
676 | if (!skb) | |
677 | return -ENOBUFS; | |
678 | ||
679 | skb->dev = sdata->dev; | |
680 | ||
681 | ret = ieee802154_mac_cmd_push(skb, &frame, &frame.disassoc_pl, | |
682 | sizeof(frame.disassoc_pl)); | |
683 | if (ret) { | |
684 | kfree_skb(skb); | |
685 | return ret; | |
686 | } | |
687 | ||
688 | ret = ieee802154_mlme_tx_one_locked(local, sdata, skb); | |
689 | if (ret) { | |
690 | dev_warn(&sdata->dev->dev, | |
691 | "No DISASSOC ACK received from %8phC\n", &teaddr); | |
692 | if (ret > 0) | |
693 | ret = (ret == IEEE802154_NO_ACK) ? -EREMOTEIO : -EIO; | |
694 | return ret; | |
695 | } | |
696 | ||
697 | dev_dbg(&sdata->dev->dev, "DISASSOC ACK received from %8phC\n", &teaddr); | |
698 | return 0; | |
699 | } | |
601f160b MR |
700 | |
701 | static int | |
702 | mac802154_send_association_resp_locked(struct ieee802154_sub_if_data *sdata, | |
703 | struct ieee802154_pan_device *target, | |
704 | struct ieee802154_assoc_resp_pl *assoc_resp_pl) | |
705 | { | |
706 | u64 teaddr = swab64((__force u64)target->extended_addr); | |
707 | struct ieee802154_association_resp_frame frame = {}; | |
708 | struct ieee802154_local *local = sdata->local; | |
709 | struct wpan_dev *wpan_dev = &sdata->wpan_dev; | |
710 | struct sk_buff *skb; | |
711 | int ret; | |
712 | ||
713 | frame.mhr.fc.type = IEEE802154_FC_TYPE_MAC_CMD; | |
714 | frame.mhr.fc.security_enabled = 0; | |
715 | frame.mhr.fc.frame_pending = 0; | |
716 | frame.mhr.fc.ack_request = 1; /* We always expect an ack here */ | |
717 | frame.mhr.fc.intra_pan = 1; | |
718 | frame.mhr.fc.dest_addr_mode = IEEE802154_EXTENDED_ADDRESSING; | |
719 | frame.mhr.fc.version = IEEE802154_2003_STD; | |
720 | frame.mhr.fc.source_addr_mode = IEEE802154_EXTENDED_ADDRESSING; | |
721 | frame.mhr.source.mode = IEEE802154_ADDR_LONG; | |
722 | frame.mhr.source.extended_addr = wpan_dev->extended_addr; | |
723 | frame.mhr.dest.mode = IEEE802154_ADDR_LONG; | |
724 | frame.mhr.dest.pan_id = wpan_dev->pan_id; | |
725 | frame.mhr.dest.extended_addr = target->extended_addr; | |
726 | frame.mhr.seq = atomic_inc_return(&wpan_dev->dsn) & 0xFF; | |
727 | frame.mac_pl.cmd_id = IEEE802154_CMD_ASSOCIATION_RESP; | |
728 | ||
729 | skb = alloc_skb(IEEE802154_MAC_CMD_SKB_SZ + sizeof(*assoc_resp_pl), | |
730 | GFP_KERNEL); | |
731 | if (!skb) | |
732 | return -ENOBUFS; | |
733 | ||
734 | skb->dev = sdata->dev; | |
735 | ||
736 | ret = ieee802154_mac_cmd_push(skb, &frame, assoc_resp_pl, | |
737 | sizeof(*assoc_resp_pl)); | |
738 | if (ret) { | |
739 | kfree_skb(skb); | |
740 | return ret; | |
741 | } | |
742 | ||
743 | ret = ieee802154_mlme_tx_locked(local, sdata, skb); | |
744 | if (ret) { | |
745 | dev_warn(&sdata->dev->dev, | |
746 | "No ASSOC RESP ACK received from %8phC\n", &teaddr); | |
747 | if (ret > 0) | |
748 | ret = (ret == IEEE802154_NO_ACK) ? -EREMOTEIO : -EIO; | |
749 | return ret; | |
750 | } | |
751 | ||
752 | return 0; | |
753 | } | |
754 | ||
755 | int mac802154_process_association_req(struct ieee802154_sub_if_data *sdata, | |
756 | struct sk_buff *skb) | |
757 | { | |
758 | struct wpan_dev *wpan_dev = &sdata->wpan_dev; | |
759 | struct ieee802154_addr *src = &mac_cb(skb)->source; | |
760 | struct ieee802154_addr *dest = &mac_cb(skb)->dest; | |
761 | struct ieee802154_assoc_resp_pl assoc_resp_pl = {}; | |
762 | struct ieee802154_assoc_req_pl assoc_req_pl; | |
763 | struct ieee802154_pan_device *child, *exchild; | |
764 | struct ieee802154_addr tmp = {}; | |
765 | u64 ceaddr; | |
766 | int ret; | |
767 | ||
768 | if (skb->len != sizeof(assoc_req_pl)) | |
769 | return -EINVAL; | |
770 | ||
771 | if (unlikely(src->mode != IEEE802154_EXTENDED_ADDRESSING)) | |
772 | return -EINVAL; | |
773 | ||
774 | if (unlikely(dest->pan_id != wpan_dev->pan_id)) | |
775 | return -ENODEV; | |
776 | ||
777 | if (dest->mode == IEEE802154_EXTENDED_ADDRESSING && | |
778 | unlikely(dest->extended_addr != wpan_dev->extended_addr)) | |
779 | return -ENODEV; | |
780 | else if (dest->mode == IEEE802154_SHORT_ADDRESSING && | |
781 | unlikely(dest->short_addr != wpan_dev->short_addr)) | |
782 | return -ENODEV; | |
783 | ||
95d92505 MR |
784 | if (wpan_dev->parent) { |
785 | dev_dbg(&sdata->dev->dev, | |
786 | "Ignoring ASSOC REQ, not the PAN coordinator\n"); | |
787 | return -ENODEV; | |
788 | } | |
789 | ||
601f160b MR |
790 | mutex_lock(&wpan_dev->association_lock); |
791 | ||
792 | memcpy(&assoc_req_pl, skb->data, sizeof(assoc_req_pl)); | |
793 | if (assoc_req_pl.assoc_type) { | |
794 | dev_err(&skb->dev->dev, "Fast associations not supported yet\n"); | |
795 | ret = -EOPNOTSUPP; | |
796 | goto unlock; | |
797 | } | |
798 | ||
799 | child = kzalloc(sizeof(*child), GFP_KERNEL); | |
800 | if (!child) { | |
801 | ret = -ENOMEM; | |
802 | goto unlock; | |
803 | } | |
804 | ||
805 | child->extended_addr = src->extended_addr; | |
806 | child->mode = IEEE802154_EXTENDED_ADDRESSING; | |
807 | ceaddr = swab64((__force u64)child->extended_addr); | |
808 | ||
80f8bf9a MR |
809 | if (wpan_dev->nchildren >= wpan_dev->max_associations) { |
810 | if (!wpan_dev->max_associations) | |
811 | assoc_resp_pl.status = IEEE802154_PAN_ACCESS_DENIED; | |
812 | else | |
813 | assoc_resp_pl.status = IEEE802154_PAN_AT_CAPACITY; | |
814 | assoc_resp_pl.short_addr = cpu_to_le16(IEEE802154_ADDR_SHORT_BROADCAST); | |
815 | dev_dbg(&sdata->dev->dev, | |
816 | "Refusing ASSOC REQ from child %8phC, %s\n", &ceaddr, | |
817 | assoc_resp_pl.status == IEEE802154_PAN_ACCESS_DENIED ? | |
818 | "access denied" : "too many children"); | |
601f160b | 819 | } else { |
80f8bf9a MR |
820 | assoc_resp_pl.status = IEEE802154_ASSOCIATION_SUCCESSFUL; |
821 | if (assoc_req_pl.alloc_addr) { | |
822 | assoc_resp_pl.short_addr = cfg802154_get_free_short_addr(wpan_dev); | |
823 | child->mode = IEEE802154_SHORT_ADDRESSING; | |
824 | } else { | |
825 | assoc_resp_pl.short_addr = cpu_to_le16(IEEE802154_ADDR_SHORT_UNSPEC); | |
826 | } | |
827 | child->short_addr = assoc_resp_pl.short_addr; | |
828 | dev_dbg(&sdata->dev->dev, | |
829 | "Accepting ASSOC REQ from child %8phC, providing short address 0x%04x\n", | |
830 | &ceaddr, le16_to_cpu(child->short_addr)); | |
601f160b | 831 | } |
601f160b MR |
832 | |
833 | ret = mac802154_send_association_resp_locked(sdata, child, &assoc_resp_pl); | |
80f8bf9a | 834 | if (ret || assoc_resp_pl.status != IEEE802154_ASSOCIATION_SUCCESSFUL) { |
601f160b MR |
835 | kfree(child); |
836 | goto unlock; | |
837 | } | |
838 | ||
839 | dev_dbg(&sdata->dev->dev, | |
840 | "Successful association with new child %8phC\n", &ceaddr); | |
841 | ||
842 | /* Ensure this child is not already associated (might happen due to | |
843 | * retransmissions), in this case drop the ex structure. | |
844 | */ | |
845 | tmp.mode = child->mode; | |
846 | tmp.extended_addr = child->extended_addr; | |
847 | exchild = cfg802154_device_is_child(wpan_dev, &tmp); | |
848 | if (exchild) { | |
849 | dev_dbg(&sdata->dev->dev, | |
850 | "Child %8phC was already known\n", &ceaddr); | |
851 | list_del(&exchild->node); | |
852 | } | |
853 | ||
854 | list_add(&child->node, &wpan_dev->children); | |
80f8bf9a | 855 | wpan_dev->nchildren++; |
601f160b MR |
856 | |
857 | unlock: | |
858 | mutex_unlock(&wpan_dev->association_lock); | |
859 | return ret; | |
860 | } | |
1e2a45f1 MR |
861 | |
862 | int mac802154_process_disassociation_notif(struct ieee802154_sub_if_data *sdata, | |
863 | struct sk_buff *skb) | |
864 | { | |
865 | struct ieee802154_addr *src = &mac_cb(skb)->source; | |
866 | struct ieee802154_addr *dest = &mac_cb(skb)->dest; | |
867 | struct wpan_dev *wpan_dev = &sdata->wpan_dev; | |
868 | struct ieee802154_pan_device *child; | |
869 | struct ieee802154_addr target; | |
870 | bool parent; | |
871 | u64 teaddr; | |
872 | ||
873 | if (skb->len != sizeof(u8)) | |
874 | return -EINVAL; | |
875 | ||
876 | if (unlikely(src->mode != IEEE802154_EXTENDED_ADDRESSING)) | |
877 | return -EINVAL; | |
878 | ||
879 | if (dest->mode == IEEE802154_EXTENDED_ADDRESSING && | |
880 | unlikely(dest->extended_addr != wpan_dev->extended_addr)) | |
881 | return -ENODEV; | |
882 | else if (dest->mode == IEEE802154_SHORT_ADDRESSING && | |
883 | unlikely(dest->short_addr != wpan_dev->short_addr)) | |
884 | return -ENODEV; | |
885 | ||
886 | if (dest->pan_id != wpan_dev->pan_id) | |
887 | return -ENODEV; | |
888 | ||
889 | target.mode = IEEE802154_EXTENDED_ADDRESSING; | |
890 | target.extended_addr = src->extended_addr; | |
891 | teaddr = swab64((__force u64)target.extended_addr); | |
892 | dev_dbg(&skb->dev->dev, "Processing DISASSOC NOTIF from %8phC\n", &teaddr); | |
893 | ||
894 | mutex_lock(&wpan_dev->association_lock); | |
895 | parent = cfg802154_device_is_parent(wpan_dev, &target); | |
896 | if (!parent) | |
897 | child = cfg802154_device_is_child(wpan_dev, &target); | |
898 | if (!parent && !child) { | |
899 | mutex_unlock(&wpan_dev->association_lock); | |
900 | return -EINVAL; | |
901 | } | |
902 | ||
903 | if (parent) { | |
904 | kfree(wpan_dev->parent); | |
905 | wpan_dev->parent = NULL; | |
906 | } else { | |
907 | list_del(&child->node); | |
908 | kfree(child); | |
909 | wpan_dev->nchildren--; | |
910 | } | |
911 | ||
912 | mutex_unlock(&wpan_dev->association_lock); | |
913 | ||
914 | return 0; | |
915 | } |