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