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

source code of linux/net/mac802154/scan.c