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 */
mac802154_scan_cleanup_locked(struct ieee802154_local * local,struct ieee802154_sub_if_data * sdata,bool aborted)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;
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, 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
mac802154_abort_scan_locked(struct ieee802154_local * local,struct ieee802154_sub_if_data * sdata)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
mac802154_scan_get_channel_time(u8 duration_order,u8 symbol_duration)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
mac802154_flush_queued_beacons(struct ieee802154_local * local)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
mac802154_scan_get_next_channel(struct ieee802154_local * local,struct cfg802154_scan_request * scan_req,u8 * channel)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
mac802154_scan_find_next_chan(struct ieee802154_local * local,struct cfg802154_scan_request * scan_req,u8 page,u8 * channel)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
mac802154_scan_prepare_beacon_req(struct ieee802154_local * local)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
mac802154_transmit_beacon_req(struct ieee802154_local * local,struct ieee802154_sub_if_data * sdata)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
mac802154_scan_worker(struct work_struct * work)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
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(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
mac802154_trigger_scan_locked(struct ieee802154_sub_if_data * sdata,struct cfg802154_scan_request * request)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(IEEE802154_IS_SCANNING, &local->ongoing);
297 if (request->type == NL802154_SCAN_ACTIVE)
298 mac802154_scan_prepare_beacon_req(local);
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
mac802154_process_beacon(struct ieee802154_local * local,struct sk_buff * skb,u8 page,u8 channel)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 }
343
mac802154_transmit_beacon(struct ieee802154_local * local,struct wpan_dev * wpan_dev)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
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
mac802154_beacon_worker(struct work_struct * work)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(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;
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(local->mac_wq, &local->beacon_work,
439 local->beacon_interval);
440 }
441
mac802154_stop_beacons_locked(struct ieee802154_local * local,struct ieee802154_sub_if_data * sdata)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;
458 kvfree_rcu_mightsleep(request);
459
460 nl802154_beaconing_done(wpan_dev);
461
462 return 0;
463 }
464
mac802154_send_beacons_locked(struct ieee802154_sub_if_data * sdata,struct cfg802154_beacon_request * request)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(IEEE802154_IS_BEACONING, &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(&request->wpan_dev->bsn, -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(request->interval,
508 request->wpan_phy->symbol_duration);
509 queue_delayed_work(local->mac_wq, &local->beacon_work, 0);
510
511 return 0;
512 }
513