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