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 spin_lock_bh(&local->rx_lock);
108 mac802154_flush_list(&local->rx_beacon_list, NULL);
109 spin_unlock_bh(&local->rx_lock);
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_prepare_beacon_req(struct ieee802154_local * local)134 static int mac802154_scan_prepare_beacon_req(struct ieee802154_local *local)
135 {
136 memset(&local->scan_beacon_req, 0, sizeof(local->scan_beacon_req));
137 local->scan_beacon_req.mhr.fc.type = IEEE802154_FC_TYPE_MAC_CMD;
138 local->scan_beacon_req.mhr.fc.dest_addr_mode = IEEE802154_SHORT_ADDRESSING;
139 local->scan_beacon_req.mhr.fc.version = IEEE802154_2003_STD;
140 local->scan_beacon_req.mhr.fc.source_addr_mode = IEEE802154_NO_ADDRESSING;
141 local->scan_beacon_req.mhr.dest.mode = IEEE802154_ADDR_SHORT;
142 local->scan_beacon_req.mhr.dest.pan_id = cpu_to_le16(IEEE802154_PANID_BROADCAST);
143 local->scan_beacon_req.mhr.dest.short_addr = cpu_to_le16(IEEE802154_ADDR_BROADCAST);
144 local->scan_beacon_req.mac_pl.cmd_id = IEEE802154_CMD_BEACON_REQ;
145
146 return 0;
147 }
148
mac802154_transmit_beacon_req(struct ieee802154_local * local,struct ieee802154_sub_if_data * sdata)149 static int mac802154_transmit_beacon_req(struct ieee802154_local *local,
150 struct ieee802154_sub_if_data *sdata)
151 {
152 struct sk_buff *skb;
153 int ret;
154
155 skb = alloc_skb(IEEE802154_MAC_CMD_SKB_SZ, GFP_KERNEL);
156 if (!skb)
157 return -ENOBUFS;
158
159 skb->dev = sdata->dev;
160
161 ret = ieee802154_mac_cmd_push(skb, &local->scan_beacon_req, NULL, 0);
162 if (ret) {
163 kfree_skb(skb);
164 return ret;
165 }
166
167 return ieee802154_mlme_tx(local, sdata, skb);
168 }
169
mac802154_scan_worker(struct work_struct * work)170 void mac802154_scan_worker(struct work_struct *work)
171 {
172 struct ieee802154_local *local =
173 container_of(work, struct ieee802154_local, scan_work.work);
174 struct cfg802154_scan_request *scan_req;
175 enum nl802154_scan_types scan_req_type;
176 struct ieee802154_sub_if_data *sdata;
177 unsigned int scan_duration = 0;
178 netdevice_tracker dev_tracker;
179 struct wpan_phy *wpan_phy;
180 u8 scan_req_duration;
181 u8 page, channel;
182 int ret;
183
184 /* Ensure the device receiver is turned off when changing channels
185 * because there is no atomic way to change the channel and know on
186 * which one a beacon might have been received.
187 */
188 drv_stop(local);
189 synchronize_net();
190 mac802154_flush_queued_beacons(local);
191
192 rcu_read_lock();
193 scan_req = rcu_dereference(local->scan_req);
194 if (unlikely(!scan_req)) {
195 rcu_read_unlock();
196 return;
197 }
198
199 sdata = IEEE802154_WPAN_DEV_TO_SUB_IF(scan_req->wpan_dev);
200
201 /* Wait an arbitrary amount of time in case we cannot use the device */
202 if (local->suspended || !ieee802154_sdata_running(sdata)) {
203 rcu_read_unlock();
204 queue_delayed_work(local->mac_wq, &local->scan_work,
205 msecs_to_jiffies(1000));
206 return;
207 }
208
209 /*
210 * sdata->dev is dereferenced below after rcu_read_unlock() and outside
211 * the rtnl, and a concurrent DEL_INTERFACE / PHY teardown can free it
212 * asynchronously from netdev_run_todo(). Pin it with a reference taken
213 * while the RCU read lock is still held, and drop it at every exit.
214 */
215 netdev_hold(sdata->dev, &dev_tracker, GFP_ATOMIC);
216
217 wpan_phy = scan_req->wpan_phy;
218 scan_req_type = scan_req->type;
219 scan_req_duration = scan_req->duration;
220
221 /* Look for the next valid chan */
222 page = local->scan_page;
223 channel = local->scan_channel;
224 do {
225 ret = mac802154_scan_find_next_chan(local, scan_req, page, &channel);
226 if (ret) {
227 rcu_read_unlock();
228 goto end_scan;
229 }
230 } while (!ieee802154_chan_is_valid(scan_req->wpan_phy, page, channel));
231
232 rcu_read_unlock();
233
234 /* Bypass the stack on purpose when changing the channel */
235 rtnl_lock();
236 ret = drv_set_channel(local, page, channel);
237 rtnl_unlock();
238 if (ret) {
239 dev_err(&sdata->dev->dev,
240 "Channel change failure during scan, aborting (%d)\n", ret);
241 goto end_scan;
242 }
243
244 local->scan_page = page;
245 local->scan_channel = channel;
246
247 rtnl_lock();
248 ret = drv_start(local, IEEE802154_FILTERING_3_SCAN, &local->addr_filt);
249 rtnl_unlock();
250 if (ret) {
251 dev_err(&sdata->dev->dev,
252 "Restarting failure after channel change, aborting (%d)\n", ret);
253 goto end_scan;
254 }
255
256 if (scan_req_type == NL802154_SCAN_ACTIVE) {
257 ret = mac802154_transmit_beacon_req(local, sdata);
258 if (ret)
259 dev_err(&sdata->dev->dev,
260 "Error when transmitting beacon request (%d)\n", ret);
261 }
262
263 ieee802154_configure_durations(wpan_phy, page, channel);
264 scan_duration = mac802154_scan_get_channel_time(scan_req_duration,
265 wpan_phy->symbol_duration);
266 dev_dbg(&sdata->dev->dev,
267 "Scan page %u channel %u for %ums\n",
268 page, channel, jiffies_to_msecs(scan_duration));
269 queue_delayed_work(local->mac_wq, &local->scan_work, scan_duration);
270 netdev_put(sdata->dev, &dev_tracker);
271 return;
272
273 end_scan:
274 rtnl_lock();
275 mac802154_scan_cleanup_locked(local, sdata, false);
276 rtnl_unlock();
277 netdev_put(sdata->dev, &dev_tracker);
278 }
279
mac802154_trigger_scan_locked(struct ieee802154_sub_if_data * sdata,struct cfg802154_scan_request * request)280 int mac802154_trigger_scan_locked(struct ieee802154_sub_if_data *sdata,
281 struct cfg802154_scan_request *request)
282 {
283 struct ieee802154_local *local = sdata->local;
284
285 ASSERT_RTNL();
286
287 if (mac802154_is_scanning(local))
288 return -EBUSY;
289
290 if (request->type != NL802154_SCAN_PASSIVE &&
291 request->type != NL802154_SCAN_ACTIVE)
292 return -EOPNOTSUPP;
293
294 /* Store scanning parameters */
295 rcu_assign_pointer(local->scan_req, request);
296
297 /* Software scanning requires to set promiscuous mode, so we need to
298 * pause the Tx queue during the entire operation.
299 */
300 ieee802154_mlme_op_pre(local);
301
302 sdata->required_filtering = IEEE802154_FILTERING_3_SCAN;
303 local->scan_page = request->page;
304 local->scan_channel = -1;
305 set_bit(IEEE802154_IS_SCANNING, &local->ongoing);
306 if (request->type == NL802154_SCAN_ACTIVE)
307 mac802154_scan_prepare_beacon_req(local);
308
309 nl802154_scan_started(request->wpan_phy, request->wpan_dev);
310
311 queue_delayed_work(local->mac_wq, &local->scan_work, 0);
312
313 return 0;
314 }
315
mac802154_process_beacon(struct ieee802154_local * local,struct sk_buff * skb,u8 page,u8 channel)316 int mac802154_process_beacon(struct ieee802154_local *local,
317 struct sk_buff *skb,
318 u8 page, u8 channel)
319 {
320 struct ieee802154_beacon_hdr *bh = (void *)skb->data;
321 struct ieee802154_addr *src = &mac_cb(skb)->source;
322 struct cfg802154_scan_request *scan_req;
323 struct ieee802154_coord_desc desc;
324
325 if (skb->len != sizeof(*bh))
326 return -EINVAL;
327
328 if (unlikely(src->mode == IEEE802154_ADDR_NONE))
329 return -EINVAL;
330
331 dev_dbg(&skb->dev->dev,
332 "BEACON received on page %u channel %u\n",
333 page, channel);
334
335 memcpy(&desc.addr, src, sizeof(desc.addr));
336 desc.page = page;
337 desc.channel = channel;
338 desc.link_quality = mac_cb(skb)->lqi;
339 desc.superframe_spec = get_unaligned_le16(skb->data);
340 desc.gts_permit = bh->gts_permit;
341
342 trace_802154_scan_event(&desc);
343
344 rcu_read_lock();
345 scan_req = rcu_dereference(local->scan_req);
346 if (likely(scan_req))
347 nl802154_scan_event(scan_req->wpan_phy, scan_req->wpan_dev, &desc);
348 rcu_read_unlock();
349
350 return 0;
351 }
352
mac802154_transmit_beacon(struct ieee802154_local * local,struct wpan_dev * wpan_dev)353 static int mac802154_transmit_beacon(struct ieee802154_local *local,
354 struct wpan_dev *wpan_dev)
355 {
356 struct cfg802154_beacon_request *beacon_req;
357 struct ieee802154_sub_if_data *sdata;
358 struct sk_buff *skb;
359 int ret;
360
361 /* Update the sequence number */
362 local->beacon.mhr.seq = atomic_inc_return(&wpan_dev->bsn) & 0xFF;
363
364 skb = alloc_skb(IEEE802154_BEACON_SKB_SZ, GFP_KERNEL);
365 if (!skb)
366 return -ENOBUFS;
367
368 rcu_read_lock();
369 beacon_req = rcu_dereference(local->beacon_req);
370 if (unlikely(!beacon_req)) {
371 rcu_read_unlock();
372 kfree_skb(skb);
373 return -EINVAL;
374 }
375
376 sdata = IEEE802154_WPAN_DEV_TO_SUB_IF(beacon_req->wpan_dev);
377 skb->dev = sdata->dev;
378
379 rcu_read_unlock();
380
381 ret = ieee802154_beacon_push(skb, &local->beacon);
382 if (ret) {
383 kfree_skb(skb);
384 return ret;
385 }
386
387 /* Using the MLME transmission helper for sending beacons is a bit
388 * overkill because we do not really care about the final outcome.
389 *
390 * Even though, going through the whole net stack with a regular
391 * dev_queue_xmit() is not relevant either because we want beacons to be
392 * sent "now" rather than go through the whole net stack scheduling
393 * (qdisc & co).
394 *
395 * Finally, using ieee802154_subif_start_xmit() would only be an option
396 * if we had a generic transmit helper which would acquire the
397 * HARD_TX_LOCK() to prevent buffer handling conflicts with regular
398 * packets.
399 *
400 * So for now we keep it simple and send beacons with our MLME helper,
401 * even if it stops the ieee802154 queue entirely during these
402 * transmissions, wich anyway does not have a huge impact on the
403 * performances given the current design of the stack.
404 */
405 return ieee802154_mlme_tx(local, sdata, skb);
406 }
407
mac802154_beacon_worker(struct work_struct * work)408 void mac802154_beacon_worker(struct work_struct *work)
409 {
410 struct ieee802154_local *local =
411 container_of(work, struct ieee802154_local, beacon_work.work);
412 struct cfg802154_beacon_request *beacon_req;
413 struct ieee802154_sub_if_data *sdata;
414 netdevice_tracker dev_tracker;
415 struct wpan_dev *wpan_dev;
416 u8 interval;
417 int ret;
418
419 rcu_read_lock();
420 beacon_req = rcu_dereference(local->beacon_req);
421 if (unlikely(!beacon_req)) {
422 rcu_read_unlock();
423 return;
424 }
425
426 sdata = IEEE802154_WPAN_DEV_TO_SUB_IF(beacon_req->wpan_dev);
427 netdev_hold(sdata->dev, &dev_tracker, GFP_ATOMIC);
428
429 /* Wait an arbitrary amount of time in case we cannot use the device */
430 if (local->suspended || !ieee802154_sdata_running(sdata)) {
431 rcu_read_unlock();
432 queue_delayed_work(local->mac_wq, &local->beacon_work,
433 msecs_to_jiffies(1000));
434 netdev_put(sdata->dev, &dev_tracker);
435 return;
436 }
437
438 wpan_dev = beacon_req->wpan_dev;
439 interval = beacon_req->interval;
440
441 rcu_read_unlock();
442
443 dev_dbg(&sdata->dev->dev, "Sending beacon\n");
444 ret = mac802154_transmit_beacon(local, wpan_dev);
445 if (ret)
446 dev_err(&sdata->dev->dev,
447 "Beacon could not be transmitted (%d)\n", ret);
448
449 if (interval < IEEE802154_ACTIVE_SCAN_DURATION)
450 queue_delayed_work(local->mac_wq, &local->beacon_work,
451 local->beacon_interval);
452 netdev_put(sdata->dev, &dev_tracker);
453 }
454
mac802154_stop_beacons_locked(struct ieee802154_local * local,struct ieee802154_sub_if_data * sdata)455 int mac802154_stop_beacons_locked(struct ieee802154_local *local,
456 struct ieee802154_sub_if_data *sdata)
457 {
458 struct wpan_dev *wpan_dev = &sdata->wpan_dev;
459 struct cfg802154_beacon_request *request;
460
461 ASSERT_RTNL();
462
463 if (!mac802154_is_beaconing(local))
464 return -ESRCH;
465
466 clear_bit(IEEE802154_IS_BEACONING, &local->ongoing);
467 cancel_delayed_work(&local->beacon_work);
468 request = rcu_replace_pointer(local->beacon_req, NULL, 1);
469 if (!request)
470 return 0;
471 kvfree_rcu_mightsleep(request);
472
473 nl802154_beaconing_done(wpan_dev);
474
475 return 0;
476 }
477
mac802154_send_beacons_locked(struct ieee802154_sub_if_data * sdata,struct cfg802154_beacon_request * request)478 int mac802154_send_beacons_locked(struct ieee802154_sub_if_data *sdata,
479 struct cfg802154_beacon_request *request)
480 {
481 struct ieee802154_local *local = sdata->local;
482 struct wpan_dev *wpan_dev = &sdata->wpan_dev;
483
484 ASSERT_RTNL();
485
486 if (mac802154_is_beaconing(local))
487 mac802154_stop_beacons_locked(local, sdata);
488
489 /* Store beaconing parameters */
490 rcu_assign_pointer(local->beacon_req, request);
491
492 set_bit(IEEE802154_IS_BEACONING, &local->ongoing);
493
494 memset(&local->beacon, 0, sizeof(local->beacon));
495 local->beacon.mhr.fc.type = IEEE802154_FC_TYPE_BEACON;
496 local->beacon.mhr.fc.security_enabled = 0;
497 local->beacon.mhr.fc.frame_pending = 0;
498 local->beacon.mhr.fc.ack_request = 0;
499 local->beacon.mhr.fc.intra_pan = 0;
500 local->beacon.mhr.fc.dest_addr_mode = IEEE802154_NO_ADDRESSING;
501 local->beacon.mhr.fc.version = IEEE802154_2003_STD;
502 local->beacon.mhr.fc.source_addr_mode = IEEE802154_EXTENDED_ADDRESSING;
503 atomic_set(&request->wpan_dev->bsn, -1);
504 local->beacon.mhr.source.mode = IEEE802154_ADDR_LONG;
505 local->beacon.mhr.source.pan_id = request->wpan_dev->pan_id;
506 local->beacon.mhr.source.extended_addr = request->wpan_dev->extended_addr;
507 local->beacon.mac_pl.beacon_order = request->interval;
508 if (request->interval <= IEEE802154_MAX_SCAN_DURATION)
509 local->beacon.mac_pl.superframe_order = request->interval;
510 local->beacon.mac_pl.final_cap_slot = 0xf;
511 local->beacon.mac_pl.battery_life_ext = 0;
512 local->beacon.mac_pl.pan_coordinator = !wpan_dev->parent;
513 local->beacon.mac_pl.assoc_permit = 1;
514
515 if (request->interval == IEEE802154_ACTIVE_SCAN_DURATION)
516 return 0;
517
518 /* Start the beacon work */
519 local->beacon_interval =
520 mac802154_scan_get_channel_time(request->interval,
521 request->wpan_phy->symbol_duration);
522 queue_delayed_work(local->mac_wq, &local->beacon_work, 0);
523
524 return 0;
525 }
526
mac802154_perform_association(struct ieee802154_sub_if_data * sdata,struct ieee802154_pan_device * coord,__le16 * short_addr)527 int mac802154_perform_association(struct ieee802154_sub_if_data *sdata,
528 struct ieee802154_pan_device *coord,
529 __le16 *short_addr)
530 {
531 u64 ceaddr = swab64((__force u64)coord->extended_addr);
532 struct ieee802154_association_req_frame frame = {};
533 struct ieee802154_local *local = sdata->local;
534 struct wpan_dev *wpan_dev = &sdata->wpan_dev;
535 __le16 resp_short_addr;
536 struct sk_buff *skb;
537 u8 resp_status;
538 int ret;
539
540 frame.mhr.fc.type = IEEE802154_FC_TYPE_MAC_CMD;
541 frame.mhr.fc.security_enabled = 0;
542 frame.mhr.fc.frame_pending = 0;
543 frame.mhr.fc.ack_request = 1; /* We always expect an ack here */
544 frame.mhr.fc.intra_pan = 0;
545 frame.mhr.fc.dest_addr_mode = (coord->mode == IEEE802154_ADDR_LONG) ?
546 IEEE802154_EXTENDED_ADDRESSING : IEEE802154_SHORT_ADDRESSING;
547 frame.mhr.fc.version = IEEE802154_2003_STD;
548 frame.mhr.fc.source_addr_mode = IEEE802154_EXTENDED_ADDRESSING;
549 frame.mhr.source.mode = IEEE802154_ADDR_LONG;
550 frame.mhr.source.pan_id = cpu_to_le16(IEEE802154_PANID_BROADCAST);
551 frame.mhr.source.extended_addr = wpan_dev->extended_addr;
552 frame.mhr.dest.mode = coord->mode;
553 frame.mhr.dest.pan_id = coord->pan_id;
554 if (coord->mode == IEEE802154_ADDR_LONG)
555 frame.mhr.dest.extended_addr = coord->extended_addr;
556 else
557 frame.mhr.dest.short_addr = coord->short_addr;
558 frame.mhr.seq = atomic_inc_return(&wpan_dev->dsn) & 0xFF;
559 frame.mac_pl.cmd_id = IEEE802154_CMD_ASSOCIATION_REQ;
560 frame.assoc_req_pl.device_type = 1;
561 frame.assoc_req_pl.power_source = 1;
562 frame.assoc_req_pl.rx_on_when_idle = 1;
563 frame.assoc_req_pl.alloc_addr = 1;
564
565 skb = alloc_skb(IEEE802154_MAC_CMD_SKB_SZ + sizeof(frame.assoc_req_pl),
566 GFP_KERNEL);
567 if (!skb)
568 return -ENOBUFS;
569
570 skb->dev = sdata->dev;
571
572 ret = ieee802154_mac_cmd_push(skb, &frame, &frame.assoc_req_pl,
573 sizeof(frame.assoc_req_pl));
574 if (ret) {
575 kfree_skb(skb);
576 return ret;
577 }
578
579 spin_lock(&local->assoc_lock);
580 reinit_completion(&local->assoc_done);
581 local->assoc_dev_extended_addr = coord->extended_addr;
582 set_bit(IEEE802154_IS_ASSOCIATING, &local->ongoing);
583 spin_unlock(&local->assoc_lock);
584
585 ret = ieee802154_mlme_tx_one_locked(local, sdata, skb);
586 if (ret) {
587 if (ret > 0)
588 ret = (ret == IEEE802154_NO_ACK) ? -EREMOTEIO : -EIO;
589 dev_warn(&sdata->dev->dev,
590 "No ASSOC REQ ACK received from %8phC\n", &ceaddr);
591 goto clear_assoc;
592 }
593
594 ret = wait_for_completion_killable_timeout(&local->assoc_done, 10 * HZ);
595 if (ret <= 0) {
596 dev_warn(&sdata->dev->dev,
597 "No ASSOC RESP received from %8phC\n", &ceaddr);
598 ret = -ETIMEDOUT;
599 goto clear_assoc;
600 }
601
602 /* The association is complete: mac802154_process_association_resp()
603 * cleared the associating bit before waking us, so a second (e.g.
604 * malicious) ASSOC RESP can no longer pass the recheck and overwrite
605 * the result. Snapshot assoc_status/assoc_addr under the lock.
606 */
607 spin_lock(&local->assoc_lock);
608 resp_status = local->assoc_status;
609 resp_short_addr = local->assoc_addr;
610 spin_unlock(&local->assoc_lock);
611
612 if (resp_status != IEEE802154_ASSOCIATION_SUCCESSFUL) {
613 if (resp_status == IEEE802154_PAN_AT_CAPACITY)
614 ret = -ERANGE;
615 else
616 ret = -EPERM;
617
618 dev_warn(&sdata->dev->dev,
619 "Negative ASSOC RESP received from %8phC: %s\n", &ceaddr,
620 resp_status == IEEE802154_PAN_AT_CAPACITY ?
621 "PAN at capacity" : "access denied");
622 return ret;
623 }
624
625 *short_addr = resp_short_addr;
626
627 return 0;
628
629 clear_assoc:
630 spin_lock(&local->assoc_lock);
631 clear_bit(IEEE802154_IS_ASSOCIATING, &local->ongoing);
632 spin_unlock(&local->assoc_lock);
633
634 return ret;
635 }
636
mac802154_process_association_resp(struct ieee802154_sub_if_data * sdata,struct sk_buff * skb)637 int mac802154_process_association_resp(struct ieee802154_sub_if_data *sdata,
638 struct sk_buff *skb)
639 {
640 struct ieee802154_addr *src = &mac_cb(skb)->source;
641 struct ieee802154_addr *dest = &mac_cb(skb)->dest;
642 u64 deaddr = swab64((__force u64)dest->extended_addr);
643 struct ieee802154_local *local = sdata->local;
644 struct wpan_dev *wpan_dev = &sdata->wpan_dev;
645 struct ieee802154_assoc_resp_pl resp_pl = {};
646
647 if (skb->len != sizeof(resp_pl))
648 return -EINVAL;
649
650 if (unlikely(src->mode != IEEE802154_EXTENDED_ADDRESSING ||
651 dest->mode != IEEE802154_EXTENDED_ADDRESSING))
652 return -EINVAL;
653
654 spin_lock(&local->assoc_lock);
655 if (unlikely(!test_bit(IEEE802154_IS_ASSOCIATING, &local->ongoing) ||
656 dest->extended_addr != wpan_dev->extended_addr ||
657 src->extended_addr != local->assoc_dev_extended_addr)) {
658 spin_unlock(&local->assoc_lock);
659 return -ENODEV;
660 }
661
662 memcpy(&resp_pl, skb->data, sizeof(resp_pl));
663 local->assoc_addr = resp_pl.short_addr;
664 local->assoc_status = resp_pl.status;
665 /* Clear the associating bit before waking the waiter: once the result
666 * is saved, any subsequent (e.g. malicious) ASSOC RESP must fail the
667 * test_bit() recheck above and can no longer overwrite the result.
668 */
669 clear_bit(IEEE802154_IS_ASSOCIATING, &local->ongoing);
670 complete(&local->assoc_done);
671 spin_unlock(&local->assoc_lock);
672
673 dev_dbg(&skb->dev->dev,
674 "ASSOC RESP 0x%x received from %8phC, getting short address %04x\n",
675 resp_pl.status, &deaddr, resp_pl.short_addr);
676
677 return 0;
678 }
679
mac802154_send_disassociation_notif(struct ieee802154_sub_if_data * sdata,struct ieee802154_pan_device * target,u8 reason)680 int mac802154_send_disassociation_notif(struct ieee802154_sub_if_data *sdata,
681 struct ieee802154_pan_device *target,
682 u8 reason)
683 {
684 struct ieee802154_disassociation_notif_frame frame = {};
685 u64 teaddr = swab64((__force u64)target->extended_addr);
686 struct ieee802154_local *local = sdata->local;
687 struct wpan_dev *wpan_dev = &sdata->wpan_dev;
688 struct sk_buff *skb;
689 int ret;
690
691 frame.mhr.fc.type = IEEE802154_FC_TYPE_MAC_CMD;
692 frame.mhr.fc.security_enabled = 0;
693 frame.mhr.fc.frame_pending = 0;
694 frame.mhr.fc.ack_request = 1;
695 frame.mhr.fc.intra_pan = 1;
696 frame.mhr.fc.dest_addr_mode = (target->mode == IEEE802154_ADDR_LONG) ?
697 IEEE802154_EXTENDED_ADDRESSING : IEEE802154_SHORT_ADDRESSING;
698 frame.mhr.fc.version = IEEE802154_2003_STD;
699 frame.mhr.fc.source_addr_mode = IEEE802154_EXTENDED_ADDRESSING;
700 frame.mhr.source.mode = IEEE802154_ADDR_LONG;
701 frame.mhr.source.pan_id = wpan_dev->pan_id;
702 frame.mhr.source.extended_addr = wpan_dev->extended_addr;
703 frame.mhr.dest.mode = target->mode;
704 frame.mhr.dest.pan_id = wpan_dev->pan_id;
705 if (target->mode == IEEE802154_ADDR_LONG)
706 frame.mhr.dest.extended_addr = target->extended_addr;
707 else
708 frame.mhr.dest.short_addr = target->short_addr;
709 frame.mhr.seq = atomic_inc_return(&wpan_dev->dsn) & 0xFF;
710 frame.mac_pl.cmd_id = IEEE802154_CMD_DISASSOCIATION_NOTIFY;
711 frame.disassoc_pl = reason;
712
713 skb = alloc_skb(IEEE802154_MAC_CMD_SKB_SZ + sizeof(frame.disassoc_pl),
714 GFP_KERNEL);
715 if (!skb)
716 return -ENOBUFS;
717
718 skb->dev = sdata->dev;
719
720 ret = ieee802154_mac_cmd_push(skb, &frame, &frame.disassoc_pl,
721 sizeof(frame.disassoc_pl));
722 if (ret) {
723 kfree_skb(skb);
724 return ret;
725 }
726
727 ret = ieee802154_mlme_tx_one_locked(local, sdata, skb);
728 if (ret) {
729 dev_warn(&sdata->dev->dev,
730 "No DISASSOC ACK received from %8phC\n", &teaddr);
731 if (ret > 0)
732 ret = (ret == IEEE802154_NO_ACK) ? -EREMOTEIO : -EIO;
733 return ret;
734 }
735
736 dev_dbg(&sdata->dev->dev, "DISASSOC ACK received from %8phC\n", &teaddr);
737 return 0;
738 }
739
740 static int
mac802154_send_association_resp_locked(struct ieee802154_sub_if_data * sdata,struct ieee802154_pan_device * target,struct ieee802154_assoc_resp_pl * assoc_resp_pl)741 mac802154_send_association_resp_locked(struct ieee802154_sub_if_data *sdata,
742 struct ieee802154_pan_device *target,
743 struct ieee802154_assoc_resp_pl *assoc_resp_pl)
744 {
745 u64 teaddr = swab64((__force u64)target->extended_addr);
746 struct ieee802154_association_resp_frame frame = {};
747 struct ieee802154_local *local = sdata->local;
748 struct wpan_dev *wpan_dev = &sdata->wpan_dev;
749 struct sk_buff *skb;
750 int ret;
751
752 frame.mhr.fc.type = IEEE802154_FC_TYPE_MAC_CMD;
753 frame.mhr.fc.security_enabled = 0;
754 frame.mhr.fc.frame_pending = 0;
755 frame.mhr.fc.ack_request = 1; /* We always expect an ack here */
756 frame.mhr.fc.intra_pan = 1;
757 frame.mhr.fc.dest_addr_mode = IEEE802154_EXTENDED_ADDRESSING;
758 frame.mhr.fc.version = IEEE802154_2003_STD;
759 frame.mhr.fc.source_addr_mode = IEEE802154_EXTENDED_ADDRESSING;
760 frame.mhr.source.mode = IEEE802154_ADDR_LONG;
761 frame.mhr.source.extended_addr = wpan_dev->extended_addr;
762 frame.mhr.dest.mode = IEEE802154_ADDR_LONG;
763 frame.mhr.dest.pan_id = wpan_dev->pan_id;
764 frame.mhr.dest.extended_addr = target->extended_addr;
765 frame.mhr.seq = atomic_inc_return(&wpan_dev->dsn) & 0xFF;
766 frame.mac_pl.cmd_id = IEEE802154_CMD_ASSOCIATION_RESP;
767
768 skb = alloc_skb(IEEE802154_MAC_CMD_SKB_SZ + sizeof(*assoc_resp_pl),
769 GFP_KERNEL);
770 if (!skb)
771 return -ENOBUFS;
772
773 skb->dev = sdata->dev;
774
775 ret = ieee802154_mac_cmd_push(skb, &frame, assoc_resp_pl,
776 sizeof(*assoc_resp_pl));
777 if (ret) {
778 kfree_skb(skb);
779 return ret;
780 }
781
782 ret = ieee802154_mlme_tx_locked(local, sdata, skb);
783 if (ret) {
784 dev_warn(&sdata->dev->dev,
785 "No ASSOC RESP ACK received from %8phC\n", &teaddr);
786 if (ret > 0)
787 ret = (ret == IEEE802154_NO_ACK) ? -EREMOTEIO : -EIO;
788 return ret;
789 }
790
791 return 0;
792 }
793
mac802154_process_association_req(struct ieee802154_sub_if_data * sdata,struct sk_buff * skb)794 int mac802154_process_association_req(struct ieee802154_sub_if_data *sdata,
795 struct sk_buff *skb)
796 {
797 struct wpan_dev *wpan_dev = &sdata->wpan_dev;
798 struct ieee802154_addr *src = &mac_cb(skb)->source;
799 struct ieee802154_addr *dest = &mac_cb(skb)->dest;
800 struct ieee802154_assoc_resp_pl assoc_resp_pl = {};
801 struct ieee802154_assoc_req_pl assoc_req_pl;
802 struct ieee802154_pan_device *child, *exchild;
803 struct ieee802154_addr tmp = {};
804 u64 ceaddr;
805 int ret;
806
807 if (skb->len != sizeof(assoc_req_pl))
808 return -EINVAL;
809
810 if (unlikely(src->mode != IEEE802154_EXTENDED_ADDRESSING))
811 return -EINVAL;
812
813 if (unlikely(dest->pan_id != wpan_dev->pan_id))
814 return -ENODEV;
815
816 if (dest->mode == IEEE802154_EXTENDED_ADDRESSING &&
817 unlikely(dest->extended_addr != wpan_dev->extended_addr))
818 return -ENODEV;
819 else if (dest->mode == IEEE802154_SHORT_ADDRESSING &&
820 unlikely(dest->short_addr != wpan_dev->short_addr))
821 return -ENODEV;
822
823 if (wpan_dev->parent) {
824 dev_dbg(&sdata->dev->dev,
825 "Ignoring ASSOC REQ, not the PAN coordinator\n");
826 return -ENODEV;
827 }
828
829 mutex_lock(&wpan_dev->association_lock);
830
831 memcpy(&assoc_req_pl, skb->data, sizeof(assoc_req_pl));
832 if (assoc_req_pl.assoc_type) {
833 dev_err(&skb->dev->dev, "Fast associations not supported yet\n");
834 ret = -EOPNOTSUPP;
835 goto unlock;
836 }
837
838 child = kzalloc_obj(*child);
839 if (!child) {
840 ret = -ENOMEM;
841 goto unlock;
842 }
843
844 child->extended_addr = src->extended_addr;
845 child->mode = IEEE802154_EXTENDED_ADDRESSING;
846 ceaddr = swab64((__force u64)child->extended_addr);
847
848 if (wpan_dev->nchildren >= wpan_dev->max_associations) {
849 if (!wpan_dev->max_associations)
850 assoc_resp_pl.status = IEEE802154_PAN_ACCESS_DENIED;
851 else
852 assoc_resp_pl.status = IEEE802154_PAN_AT_CAPACITY;
853 assoc_resp_pl.short_addr = cpu_to_le16(IEEE802154_ADDR_SHORT_BROADCAST);
854 dev_dbg(&sdata->dev->dev,
855 "Refusing ASSOC REQ from child %8phC, %s\n", &ceaddr,
856 assoc_resp_pl.status == IEEE802154_PAN_ACCESS_DENIED ?
857 "access denied" : "too many children");
858 } else {
859 assoc_resp_pl.status = IEEE802154_ASSOCIATION_SUCCESSFUL;
860 if (assoc_req_pl.alloc_addr) {
861 assoc_resp_pl.short_addr = cfg802154_get_free_short_addr(wpan_dev);
862 child->mode = IEEE802154_SHORT_ADDRESSING;
863 } else {
864 assoc_resp_pl.short_addr = cpu_to_le16(IEEE802154_ADDR_SHORT_UNSPEC);
865 }
866 child->short_addr = assoc_resp_pl.short_addr;
867 dev_dbg(&sdata->dev->dev,
868 "Accepting ASSOC REQ from child %8phC, providing short address 0x%04x\n",
869 &ceaddr, le16_to_cpu(child->short_addr));
870 }
871
872 ret = mac802154_send_association_resp_locked(sdata, child, &assoc_resp_pl);
873 if (ret || assoc_resp_pl.status != IEEE802154_ASSOCIATION_SUCCESSFUL) {
874 kfree(child);
875 goto unlock;
876 }
877
878 dev_dbg(&sdata->dev->dev,
879 "Successful association with new child %8phC\n", &ceaddr);
880
881 /* Ensure this child is not already associated (might happen due to
882 * retransmissions), in this case drop the ex structure.
883 */
884 tmp.mode = child->mode;
885 tmp.extended_addr = child->extended_addr;
886 exchild = cfg802154_device_is_child(wpan_dev, &tmp);
887 if (exchild) {
888 dev_dbg(&sdata->dev->dev,
889 "Child %8phC was already known\n", &ceaddr);
890 list_del(&exchild->node);
891 }
892
893 list_add(&child->node, &wpan_dev->children);
894 wpan_dev->nchildren++;
895
896 unlock:
897 mutex_unlock(&wpan_dev->association_lock);
898 return ret;
899 }
900
mac802154_process_disassociation_notif(struct ieee802154_sub_if_data * sdata,struct sk_buff * skb)901 int mac802154_process_disassociation_notif(struct ieee802154_sub_if_data *sdata,
902 struct sk_buff *skb)
903 {
904 struct ieee802154_addr *src = &mac_cb(skb)->source;
905 struct ieee802154_addr *dest = &mac_cb(skb)->dest;
906 struct wpan_dev *wpan_dev = &sdata->wpan_dev;
907 struct ieee802154_pan_device *child;
908 struct ieee802154_addr target;
909 bool parent;
910 u64 teaddr;
911
912 if (skb->len != sizeof(u8))
913 return -EINVAL;
914
915 if (unlikely(src->mode != IEEE802154_EXTENDED_ADDRESSING))
916 return -EINVAL;
917
918 if (dest->mode == IEEE802154_EXTENDED_ADDRESSING &&
919 unlikely(dest->extended_addr != wpan_dev->extended_addr))
920 return -ENODEV;
921 else if (dest->mode == IEEE802154_SHORT_ADDRESSING &&
922 unlikely(dest->short_addr != wpan_dev->short_addr))
923 return -ENODEV;
924
925 if (dest->pan_id != wpan_dev->pan_id)
926 return -ENODEV;
927
928 target.mode = IEEE802154_EXTENDED_ADDRESSING;
929 target.extended_addr = src->extended_addr;
930 teaddr = swab64((__force u64)target.extended_addr);
931 dev_dbg(&skb->dev->dev, "Processing DISASSOC NOTIF from %8phC\n", &teaddr);
932
933 mutex_lock(&wpan_dev->association_lock);
934 parent = cfg802154_device_is_parent(wpan_dev, &target);
935 if (!parent)
936 child = cfg802154_device_is_child(wpan_dev, &target);
937 if (!parent && !child) {
938 mutex_unlock(&wpan_dev->association_lock);
939 return -EINVAL;
940 }
941
942 if (parent) {
943 kfree(wpan_dev->parent);
944 wpan_dev->parent = NULL;
945 } else {
946 list_del(&child->node);
947 kfree(child);
948 wpan_dev->nchildren--;
949 }
950
951 mutex_unlock(&wpan_dev->association_lock);
952
953 return 0;
954 }
955