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_BEACON_SKB_SZ (IEEE802154_BEACON_MHR_SZ + \
22 IEEE802154_BEACON_PL_SZ)
23
24 /* mac802154_scan_cleanup_locked() must be called upon scan completion or abort.
25 * - Completions are asynchronous, not locked by the rtnl and decided by the
26 * scan worker.
27 * - Aborts are decided by userspace, and locked by the rtnl.
28 *
29 * Concurrent modifications to the PHY, the interfaces or the hardware is in
30 * general prevented by the rtnl. So in most cases we don't need additional
31 * protection.
32 *
33 * However, the scan worker get's triggered without anybody noticing and thus we
34 * must ensure the presence of the devices as well as data consistency:
35 * - The sub-interface and device driver module get both their reference
36 * counters incremented whenever we start a scan, so they cannot disappear
37 * during operation.
38 * - Data consistency is achieved by the use of rcu protected pointers.
39 */
mac802154_scan_cleanup_locked(struct ieee802154_local * local,struct ieee802154_sub_if_data * sdata,bool aborted)40 static int mac802154_scan_cleanup_locked(struct ieee802154_local *local,
41 struct ieee802154_sub_if_data *sdata,
42 bool aborted)
43 {
44 struct wpan_dev *wpan_dev = &sdata->wpan_dev;
45 struct wpan_phy *wpan_phy = local->phy;
46 struct cfg802154_scan_request *request;
47 u8 arg;
48
49 /* Prevent any further use of the scan request */
50 clear_bit(IEEE802154_IS_SCANNING, &local->ongoing);
51 cancel_delayed_work(&local->scan_work);
52 request = rcu_replace_pointer(local->scan_req, NULL, 1);
53 if (!request)
54 return 0;
55 kfree_rcu(request);
56
57 /* Advertize first, while we know the devices cannot be removed */
58 if (aborted)
59 arg = NL802154_SCAN_DONE_REASON_ABORTED;
60 else
61 arg = NL802154_SCAN_DONE_REASON_FINISHED;
62 nl802154_scan_done(wpan_phy, wpan_dev, arg);
63
64 /* Cleanup software stack */
65 ieee802154_mlme_op_post(local);
66
67 /* Set the hardware back in its original state */
68 drv_set_channel(local, wpan_phy->current_page,
69 wpan_phy->current_channel);
70 ieee802154_configure_durations(wpan_phy, wpan_phy->current_page,
71 wpan_phy->current_channel);
72 drv_stop(local);
73 synchronize_net();
74 sdata->required_filtering = sdata->iface_default_filtering;
75 drv_start(local, sdata->required_filtering, &local->addr_filt);
76
77 return 0;
78 }
79
mac802154_abort_scan_locked(struct ieee802154_local * local,struct ieee802154_sub_if_data * sdata)80 int mac802154_abort_scan_locked(struct ieee802154_local *local,
81 struct ieee802154_sub_if_data *sdata)
82 {
83 ASSERT_RTNL();
84
85 if (!mac802154_is_scanning(local))
86 return -ESRCH;
87
88 return mac802154_scan_cleanup_locked(local, sdata, true);
89 }
90
mac802154_scan_get_channel_time(u8 duration_order,u8 symbol_duration)91 static unsigned int mac802154_scan_get_channel_time(u8 duration_order,
92 u8 symbol_duration)
93 {
94 u64 base_super_frame_duration = (u64)symbol_duration *
95 IEEE802154_SUPERFRAME_PERIOD * IEEE802154_SLOT_PERIOD;
96
97 return usecs_to_jiffies(base_super_frame_duration *
98 (BIT(duration_order) + 1));
99 }
100
mac802154_flush_queued_beacons(struct ieee802154_local * local)101 static void mac802154_flush_queued_beacons(struct ieee802154_local *local)
102 {
103 struct cfg802154_mac_pkt *mac_pkt, *tmp;
104
105 list_for_each_entry_safe(mac_pkt, tmp, &local->rx_beacon_list, node) {
106 list_del(&mac_pkt->node);
107 kfree_skb(mac_pkt->skb);
108 kfree(mac_pkt);
109 }
110 }
111
112 static void
mac802154_scan_get_next_channel(struct ieee802154_local * local,struct cfg802154_scan_request * scan_req,u8 * channel)113 mac802154_scan_get_next_channel(struct ieee802154_local *local,
114 struct cfg802154_scan_request *scan_req,
115 u8 *channel)
116 {
117 (*channel)++;
118 *channel = find_next_bit((const unsigned long *)&scan_req->channels,
119 IEEE802154_MAX_CHANNEL + 1,
120 *channel);
121 }
122
mac802154_scan_find_next_chan(struct ieee802154_local * local,struct cfg802154_scan_request * scan_req,u8 page,u8 * channel)123 static int mac802154_scan_find_next_chan(struct ieee802154_local *local,
124 struct cfg802154_scan_request *scan_req,
125 u8 page, u8 *channel)
126 {
127 mac802154_scan_get_next_channel(local, scan_req, channel);
128 if (*channel > IEEE802154_MAX_CHANNEL)
129 return -EINVAL;
130
131 return 0;
132 }
133
mac802154_scan_worker(struct work_struct * work)134 void mac802154_scan_worker(struct work_struct *work)
135 {
136 struct ieee802154_local *local =
137 container_of(work, struct ieee802154_local, scan_work.work);
138 struct cfg802154_scan_request *scan_req;
139 struct ieee802154_sub_if_data *sdata;
140 unsigned int scan_duration = 0;
141 struct wpan_phy *wpan_phy;
142 u8 scan_req_duration;
143 u8 page, channel;
144 int ret;
145
146 /* Ensure the device receiver is turned off when changing channels
147 * because there is no atomic way to change the channel and know on
148 * which one a beacon might have been received.
149 */
150 drv_stop(local);
151 synchronize_net();
152 mac802154_flush_queued_beacons(local);
153
154 rcu_read_lock();
155 scan_req = rcu_dereference(local->scan_req);
156 if (unlikely(!scan_req)) {
157 rcu_read_unlock();
158 return;
159 }
160
161 sdata = IEEE802154_WPAN_DEV_TO_SUB_IF(scan_req->wpan_dev);
162
163 /* Wait an arbitrary amount of time in case we cannot use the device */
164 if (local->suspended || !ieee802154_sdata_running(sdata)) {
165 rcu_read_unlock();
166 queue_delayed_work(local->mac_wq, &local->scan_work,
167 msecs_to_jiffies(1000));
168 return;
169 }
170
171 wpan_phy = scan_req->wpan_phy;
172 scan_req_duration = scan_req->duration;
173
174 /* Look for the next valid chan */
175 page = local->scan_page;
176 channel = local->scan_channel;
177 do {
178 ret = mac802154_scan_find_next_chan(local, scan_req, page, &channel);
179 if (ret) {
180 rcu_read_unlock();
181 goto end_scan;
182 }
183 } while (!ieee802154_chan_is_valid(scan_req->wpan_phy, page, channel));
184
185 rcu_read_unlock();
186
187 /* Bypass the stack on purpose when changing the channel */
188 rtnl_lock();
189 ret = drv_set_channel(local, page, channel);
190 rtnl_unlock();
191 if (ret) {
192 dev_err(&sdata->dev->dev,
193 "Channel change failure during scan, aborting (%d)\n", ret);
194 goto end_scan;
195 }
196
197 local->scan_page = page;
198 local->scan_channel = channel;
199
200 rtnl_lock();
201 ret = drv_start(local, IEEE802154_FILTERING_3_SCAN, &local->addr_filt);
202 rtnl_unlock();
203 if (ret) {
204 dev_err(&sdata->dev->dev,
205 "Restarting failure after channel change, aborting (%d)\n", ret);
206 goto end_scan;
207 }
208
209 ieee802154_configure_durations(wpan_phy, page, channel);
210 scan_duration = mac802154_scan_get_channel_time(scan_req_duration,
211 wpan_phy->symbol_duration);
212 dev_dbg(&sdata->dev->dev,
213 "Scan page %u channel %u for %ums\n",
214 page, channel, jiffies_to_msecs(scan_duration));
215 queue_delayed_work(local->mac_wq, &local->scan_work, scan_duration);
216 return;
217
218 end_scan:
219 rtnl_lock();
220 mac802154_scan_cleanup_locked(local, sdata, false);
221 rtnl_unlock();
222 }
223
mac802154_trigger_scan_locked(struct ieee802154_sub_if_data * sdata,struct cfg802154_scan_request * request)224 int mac802154_trigger_scan_locked(struct ieee802154_sub_if_data *sdata,
225 struct cfg802154_scan_request *request)
226 {
227 struct ieee802154_local *local = sdata->local;
228
229 ASSERT_RTNL();
230
231 if (mac802154_is_scanning(local))
232 return -EBUSY;
233
234 /* TODO: support other scanning type */
235 if (request->type != NL802154_SCAN_PASSIVE)
236 return -EOPNOTSUPP;
237
238 /* Store scanning parameters */
239 rcu_assign_pointer(local->scan_req, request);
240
241 /* Software scanning requires to set promiscuous mode, so we need to
242 * pause the Tx queue during the entire operation.
243 */
244 ieee802154_mlme_op_pre(local);
245
246 sdata->required_filtering = IEEE802154_FILTERING_3_SCAN;
247 local->scan_page = request->page;
248 local->scan_channel = -1;
249 set_bit(IEEE802154_IS_SCANNING, &local->ongoing);
250
251 nl802154_scan_started(request->wpan_phy, request->wpan_dev);
252
253 queue_delayed_work(local->mac_wq, &local->scan_work, 0);
254
255 return 0;
256 }
257
mac802154_process_beacon(struct ieee802154_local * local,struct sk_buff * skb,u8 page,u8 channel)258 int mac802154_process_beacon(struct ieee802154_local *local,
259 struct sk_buff *skb,
260 u8 page, u8 channel)
261 {
262 struct ieee802154_beacon_hdr *bh = (void *)skb->data;
263 struct ieee802154_addr *src = &mac_cb(skb)->source;
264 struct cfg802154_scan_request *scan_req;
265 struct ieee802154_coord_desc desc;
266
267 if (skb->len != sizeof(*bh))
268 return -EINVAL;
269
270 if (unlikely(src->mode == IEEE802154_ADDR_NONE))
271 return -EINVAL;
272
273 dev_dbg(&skb->dev->dev,
274 "BEACON received on page %u channel %u\n",
275 page, channel);
276
277 memcpy(&desc.addr, src, sizeof(desc.addr));
278 desc.page = page;
279 desc.channel = channel;
280 desc.link_quality = mac_cb(skb)->lqi;
281 desc.superframe_spec = get_unaligned_le16(skb->data);
282 desc.gts_permit = bh->gts_permit;
283
284 trace_802154_scan_event(&desc);
285
286 rcu_read_lock();
287 scan_req = rcu_dereference(local->scan_req);
288 if (likely(scan_req))
289 nl802154_scan_event(scan_req->wpan_phy, scan_req->wpan_dev, &desc);
290 rcu_read_unlock();
291
292 return 0;
293 }
294
mac802154_transmit_beacon(struct ieee802154_local * local,struct wpan_dev * wpan_dev)295 static int mac802154_transmit_beacon(struct ieee802154_local *local,
296 struct wpan_dev *wpan_dev)
297 {
298 struct cfg802154_beacon_request *beacon_req;
299 struct ieee802154_sub_if_data *sdata;
300 struct sk_buff *skb;
301 int ret;
302
303 /* Update the sequence number */
304 local->beacon.mhr.seq = atomic_inc_return(&wpan_dev->bsn) & 0xFF;
305
306 skb = alloc_skb(IEEE802154_BEACON_SKB_SZ, GFP_KERNEL);
307 if (!skb)
308 return -ENOBUFS;
309
310 rcu_read_lock();
311 beacon_req = rcu_dereference(local->beacon_req);
312 if (unlikely(!beacon_req)) {
313 rcu_read_unlock();
314 kfree_skb(skb);
315 return -EINVAL;
316 }
317
318 sdata = IEEE802154_WPAN_DEV_TO_SUB_IF(beacon_req->wpan_dev);
319 skb->dev = sdata->dev;
320
321 rcu_read_unlock();
322
323 ret = ieee802154_beacon_push(skb, &local->beacon);
324 if (ret) {
325 kfree_skb(skb);
326 return ret;
327 }
328
329 /* Using the MLME transmission helper for sending beacons is a bit
330 * overkill because we do not really care about the final outcome.
331 *
332 * Even though, going through the whole net stack with a regular
333 * dev_queue_xmit() is not relevant either because we want beacons to be
334 * sent "now" rather than go through the whole net stack scheduling
335 * (qdisc & co).
336 *
337 * Finally, using ieee802154_subif_start_xmit() would only be an option
338 * if we had a generic transmit helper which would acquire the
339 * HARD_TX_LOCK() to prevent buffer handling conflicts with regular
340 * packets.
341 *
342 * So for now we keep it simple and send beacons with our MLME helper,
343 * even if it stops the ieee802154 queue entirely during these
344 * transmissions, wich anyway does not have a huge impact on the
345 * performances given the current design of the stack.
346 */
347 return ieee802154_mlme_tx(local, sdata, skb);
348 }
349
mac802154_beacon_worker(struct work_struct * work)350 void mac802154_beacon_worker(struct work_struct *work)
351 {
352 struct ieee802154_local *local =
353 container_of(work, struct ieee802154_local, beacon_work.work);
354 struct cfg802154_beacon_request *beacon_req;
355 struct ieee802154_sub_if_data *sdata;
356 struct wpan_dev *wpan_dev;
357 int ret;
358
359 rcu_read_lock();
360 beacon_req = rcu_dereference(local->beacon_req);
361 if (unlikely(!beacon_req)) {
362 rcu_read_unlock();
363 return;
364 }
365
366 sdata = IEEE802154_WPAN_DEV_TO_SUB_IF(beacon_req->wpan_dev);
367
368 /* Wait an arbitrary amount of time in case we cannot use the device */
369 if (local->suspended || !ieee802154_sdata_running(sdata)) {
370 rcu_read_unlock();
371 queue_delayed_work(local->mac_wq, &local->beacon_work,
372 msecs_to_jiffies(1000));
373 return;
374 }
375
376 wpan_dev = beacon_req->wpan_dev;
377
378 rcu_read_unlock();
379
380 dev_dbg(&sdata->dev->dev, "Sending beacon\n");
381 ret = mac802154_transmit_beacon(local, wpan_dev);
382 if (ret)
383 dev_err(&sdata->dev->dev,
384 "Beacon could not be transmitted (%d)\n", ret);
385
386 queue_delayed_work(local->mac_wq, &local->beacon_work,
387 local->beacon_interval);
388 }
389
mac802154_stop_beacons_locked(struct ieee802154_local * local,struct ieee802154_sub_if_data * sdata)390 int mac802154_stop_beacons_locked(struct ieee802154_local *local,
391 struct ieee802154_sub_if_data *sdata)
392 {
393 struct wpan_dev *wpan_dev = &sdata->wpan_dev;
394 struct cfg802154_beacon_request *request;
395
396 ASSERT_RTNL();
397
398 if (!mac802154_is_beaconing(local))
399 return -ESRCH;
400
401 clear_bit(IEEE802154_IS_BEACONING, &local->ongoing);
402 cancel_delayed_work(&local->beacon_work);
403 request = rcu_replace_pointer(local->beacon_req, NULL, 1);
404 if (!request)
405 return 0;
406 kfree_rcu(request);
407
408 nl802154_beaconing_done(wpan_dev);
409
410 return 0;
411 }
412
mac802154_send_beacons_locked(struct ieee802154_sub_if_data * sdata,struct cfg802154_beacon_request * request)413 int mac802154_send_beacons_locked(struct ieee802154_sub_if_data *sdata,
414 struct cfg802154_beacon_request *request)
415 {
416 struct ieee802154_local *local = sdata->local;
417
418 ASSERT_RTNL();
419
420 if (mac802154_is_beaconing(local))
421 mac802154_stop_beacons_locked(local, sdata);
422
423 /* Store beaconing parameters */
424 rcu_assign_pointer(local->beacon_req, request);
425
426 set_bit(IEEE802154_IS_BEACONING, &local->ongoing);
427
428 memset(&local->beacon, 0, sizeof(local->beacon));
429 local->beacon.mhr.fc.type = IEEE802154_FC_TYPE_BEACON;
430 local->beacon.mhr.fc.security_enabled = 0;
431 local->beacon.mhr.fc.frame_pending = 0;
432 local->beacon.mhr.fc.ack_request = 0;
433 local->beacon.mhr.fc.intra_pan = 0;
434 local->beacon.mhr.fc.dest_addr_mode = IEEE802154_NO_ADDRESSING;
435 local->beacon.mhr.fc.version = IEEE802154_2003_STD;
436 local->beacon.mhr.fc.source_addr_mode = IEEE802154_EXTENDED_ADDRESSING;
437 atomic_set(&request->wpan_dev->bsn, -1);
438 local->beacon.mhr.source.mode = IEEE802154_ADDR_LONG;
439 local->beacon.mhr.source.pan_id = request->wpan_dev->pan_id;
440 local->beacon.mhr.source.extended_addr = request->wpan_dev->extended_addr;
441 local->beacon.mac_pl.beacon_order = request->interval;
442 local->beacon.mac_pl.superframe_order = request->interval;
443 local->beacon.mac_pl.final_cap_slot = 0xf;
444 local->beacon.mac_pl.battery_life_ext = 0;
445 /* TODO: Fill this field depending on the coordinator capacity */
446 local->beacon.mac_pl.pan_coordinator = 1;
447 local->beacon.mac_pl.assoc_permit = 1;
448
449 /* Start the beacon work */
450 local->beacon_interval =
451 mac802154_scan_get_channel_time(request->interval,
452 request->wpan_phy->symbol_duration);
453 queue_delayed_work(local->mac_wq, &local->beacon_work, 0);
454
455 return 0;
456 }
457