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 | |
19 | #define IEEE802154_BEACON_MHR_SZ 13 |
20 | #define IEEE802154_BEACON_PL_SZ 4 |
21 | #define IEEE802154_MAC_CMD_MHR_SZ 23 |
22 | #define IEEE802154_MAC_CMD_PL_SZ 1 |
23 | #define IEEE802154_BEACON_SKB_SZ (IEEE802154_BEACON_MHR_SZ + \ |
24 | IEEE802154_BEACON_PL_SZ) |
25 | #define IEEE802154_MAC_CMD_SKB_SZ (IEEE802154_MAC_CMD_MHR_SZ + \ |
26 | IEEE802154_MAC_CMD_PL_SZ) |
27 | |
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(nr: IEEE802154_IS_SCANNING, addr: &local->ongoing); |
55 | cancel_delayed_work(dwork: &local->scan_work); |
56 | request = rcu_replace_pointer(local->scan_req, NULL, 1); |
57 | if (!request) |
58 | return 0; |
59 | kvfree_rcu_mightsleep(request); |
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, reason: 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, page: wpan_phy->current_page, |
73 | channel: wpan_phy->current_channel); |
74 | ieee802154_configure_durations(phy: wpan_phy, page: wpan_phy->current_page, |
75 | channel: wpan_phy->current_channel); |
76 | drv_stop(local); |
77 | synchronize_net(); |
78 | sdata->required_filtering = sdata->iface_default_filtering; |
79 | drv_start(local, level: sdata->required_filtering, addr_filt: &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, aborted: 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(u: 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(entry: &mac_pkt->node); |
111 | kfree_skb(skb: mac_pkt->skb); |
112 | kfree(objp: 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(addr: (const unsigned long *)&scan_req->channels, |
123 | IEEE802154_MAX_CHANNEL + 1, |
124 | offset: *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 | |
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, frame: &local->scan_beacon_req, NULL, pl_len: 0); |
166 | if (ret) { |
167 | kfree_skb(skb); |
168 | return ret; |
169 | } |
170 | |
171 | return ieee802154_mlme_tx(local, sdata, skb); |
172 | } |
173 | |
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(wpan_dev: 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(wq: local->mac_wq, dwork: &local->scan_work, |
207 | delay: msecs_to_jiffies(m: 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: &channel); |
219 | if (ret) { |
220 | rcu_read_unlock(); |
221 | goto end_scan; |
222 | } |
223 | } while (!ieee802154_chan_is_valid(phy: 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, level: IEEE802154_FILTERING_3_SCAN, addr_filt: &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 | |
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 | |
256 | ieee802154_configure_durations(phy: wpan_phy, page, channel); |
257 | scan_duration = mac802154_scan_get_channel_time(duration_order: scan_req_duration, |
258 | symbol_duration: 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(wq: local->mac_wq, dwork: &local->scan_work, delay: scan_duration); |
263 | return; |
264 | |
265 | end_scan: |
266 | rtnl_lock(); |
267 | mac802154_scan_cleanup_locked(local, sdata, aborted: 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 | |
281 | if (request->type != NL802154_SCAN_PASSIVE && |
282 | request->type != NL802154_SCAN_ACTIVE) |
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(nr: IEEE802154_IS_SCANNING, addr: &local->ongoing); |
297 | if (request->type == NL802154_SCAN_ACTIVE) |
298 | mac802154_scan_prepare_beacon_req(local); |
299 | |
300 | nl802154_scan_started(wpan_phy: request->wpan_phy, wpan_dev: request->wpan_dev); |
301 | |
302 | queue_delayed_work(wq: local->mac_wq, dwork: &local->scan_work, delay: 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(p: skb->data); |
331 | desc.gts_permit = bh->gts_permit; |
332 | |
333 | trace_802154_scan_event(desc: &desc); |
334 | |
335 | rcu_read_lock(); |
336 | scan_req = rcu_dereference(local->scan_req); |
337 | if (likely(scan_req)) |
338 | nl802154_scan_event(wpan_phy: scan_req->wpan_phy, wpan_dev: scan_req->wpan_dev, desc: &desc); |
339 | rcu_read_unlock(); |
340 | |
341 | return 0; |
342 | } |
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(v: &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(wpan_dev: beacon_req->wpan_dev); |
368 | skb->dev = sdata->dev; |
369 | |
370 | rcu_read_unlock(); |
371 | |
372 | ret = ieee802154_beacon_push(skb, beacon: &local->beacon); |
373 | if (ret) { |
374 | kfree_skb(skb); |
375 | return ret; |
376 | } |
377 | |
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); |
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; |
406 | u8 interval; |
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(wpan_dev: 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(wq: local->mac_wq, dwork: &local->beacon_work, |
422 | delay: msecs_to_jiffies(m: 1000)); |
423 | return; |
424 | } |
425 | |
426 | wpan_dev = beacon_req->wpan_dev; |
427 | interval = beacon_req->interval; |
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 | |
437 | if (interval < IEEE802154_ACTIVE_SCAN_DURATION) |
438 | queue_delayed_work(wq: local->mac_wq, dwork: &local->beacon_work, |
439 | delay: local->beacon_interval); |
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(nr: IEEE802154_IS_BEACONING, addr: &local->ongoing); |
454 | cancel_delayed_work(dwork: &local->beacon_work); |
455 | request = rcu_replace_pointer(local->beacon_req, NULL, 1); |
456 | if (!request) |
457 | return 0; |
458 | kvfree_rcu_mightsleep(request); |
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; |
469 | |
470 | ASSERT_RTNL(); |
471 | |
472 | if (mac802154_is_beaconing(local)) |
473 | mac802154_stop_beacons_locked(local, sdata); |
474 | |
475 | /* Store beaconing parameters */ |
476 | rcu_assign_pointer(local->beacon_req, request); |
477 | |
478 | set_bit(nr: IEEE802154_IS_BEACONING, addr: &local->ongoing); |
479 | |
480 | memset(&local->beacon, 0, sizeof(local->beacon)); |
481 | local->beacon.mhr.fc.type = IEEE802154_FC_TYPE_BEACON; |
482 | local->beacon.mhr.fc.security_enabled = 0; |
483 | local->beacon.mhr.fc.frame_pending = 0; |
484 | local->beacon.mhr.fc.ack_request = 0; |
485 | local->beacon.mhr.fc.intra_pan = 0; |
486 | local->beacon.mhr.fc.dest_addr_mode = IEEE802154_NO_ADDRESSING; |
487 | local->beacon.mhr.fc.version = IEEE802154_2003_STD; |
488 | local->beacon.mhr.fc.source_addr_mode = IEEE802154_EXTENDED_ADDRESSING; |
489 | atomic_set(v: &request->wpan_dev->bsn, i: -1); |
490 | local->beacon.mhr.source.mode = IEEE802154_ADDR_LONG; |
491 | local->beacon.mhr.source.pan_id = request->wpan_dev->pan_id; |
492 | local->beacon.mhr.source.extended_addr = request->wpan_dev->extended_addr; |
493 | local->beacon.mac_pl.beacon_order = request->interval; |
494 | if (request->interval <= IEEE802154_MAX_SCAN_DURATION) |
495 | local->beacon.mac_pl.superframe_order = request->interval; |
496 | local->beacon.mac_pl.final_cap_slot = 0xf; |
497 | local->beacon.mac_pl.battery_life_ext = 0; |
498 | /* TODO: Fill this field with the coordinator situation in the network */ |
499 | local->beacon.mac_pl.pan_coordinator = 1; |
500 | local->beacon.mac_pl.assoc_permit = 1; |
501 | |
502 | if (request->interval == IEEE802154_ACTIVE_SCAN_DURATION) |
503 | return 0; |
504 | |
505 | /* Start the beacon work */ |
506 | local->beacon_interval = |
507 | mac802154_scan_get_channel_time(duration_order: request->interval, |
508 | symbol_duration: request->wpan_phy->symbol_duration); |
509 | queue_delayed_work(wq: local->mac_wq, dwork: &local->beacon_work, delay: 0); |
510 | |
511 | return 0; |
512 | } |
513 | |