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 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(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(local->mac_wq, &local->scan_work, 207 msecs_to_jiffies(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); 219 if (ret) { 220 rcu_read_unlock(); 221 goto end_scan; 222 } 223 } while (!ieee802154_chan_is_valid(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, IEEE802154_FILTERING_3_SCAN, &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(wpan_phy, page, channel); 257 scan_duration = mac802154_scan_get_channel_time(scan_req_duration, 258 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(local->mac_wq, &local->scan_work, scan_duration); 263 return; 264 265 end_scan: 266 rtnl_lock(); 267 mac802154_scan_cleanup_locked(local, sdata, false); 268 rtnl_unlock(); 269 } 270 271 int 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(IEEE802154_IS_SCANNING, &local->ongoing); 297 if (request->type == NL802154_SCAN_ACTIVE) 298 mac802154_scan_prepare_beacon_req(local); 299 300 nl802154_scan_started(request->wpan_phy, request->wpan_dev); 301 302 queue_delayed_work(local->mac_wq, &local->scan_work, 0); 303 304 return 0; 305 } 306 307 int 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(skb->data); 331 desc.gts_permit = bh->gts_permit; 332 333 trace_802154_scan_event(&desc); 334 335 rcu_read_lock(); 336 scan_req = rcu_dereference(local->scan_req); 337 if (likely(scan_req)) 338 nl802154_scan_event(scan_req->wpan_phy, scan_req->wpan_dev, &desc); 339 rcu_read_unlock(); 340 341 return 0; 342 } 343 344 static 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(&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(beacon_req->wpan_dev); 368 skb->dev = sdata->dev; 369 370 rcu_read_unlock(); 371 372 ret = ieee802154_beacon_push(skb, &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 399 void 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(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(local->mac_wq, &local->beacon_work, 422 msecs_to_jiffies(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(local->mac_wq, &local->beacon_work, 439 local->beacon_interval); 440 } 441 442 int 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(IEEE802154_IS_BEACONING, &local->ongoing); 454 cancel_delayed_work(&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 465 int 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 470 ASSERT_RTNL(); 471 472 if (mac802154_is_beaconing(local)) 473 mac802154_stop_beacons_locked(local, sdata); 474 475 /* Store beaconing parameters */ 476 rcu_assign_pointer(local->beacon_req, request); 477 478 set_bit(IEEE802154_IS_BEACONING, &local->ongoing); 479 480 memset(&local->beacon, 0, sizeof(local->beacon)); 481 local->beacon.mhr.fc.type = IEEE802154_FC_TYPE_BEACON; 482 local->beacon.mhr.fc.security_enabled = 0; 483 local->beacon.mhr.fc.frame_pending = 0; 484 local->beacon.mhr.fc.ack_request = 0; 485 local->beacon.mhr.fc.intra_pan = 0; 486 local->beacon.mhr.fc.dest_addr_mode = IEEE802154_NO_ADDRESSING; 487 local->beacon.mhr.fc.version = IEEE802154_2003_STD; 488 local->beacon.mhr.fc.source_addr_mode = IEEE802154_EXTENDED_ADDRESSING; 489 atomic_set(&request->wpan_dev->bsn, -1); 490 local->beacon.mhr.source.mode = IEEE802154_ADDR_LONG; 491 local->beacon.mhr.source.pan_id = request->wpan_dev->pan_id; 492 local->beacon.mhr.source.extended_addr = request->wpan_dev->extended_addr; 493 local->beacon.mac_pl.beacon_order = request->interval; 494 if (request->interval <= IEEE802154_MAX_SCAN_DURATION) 495 local->beacon.mac_pl.superframe_order = request->interval; 496 local->beacon.mac_pl.final_cap_slot = 0xf; 497 local->beacon.mac_pl.battery_life_ext = 0; 498 /* TODO: Fill this field with the coordinator situation in the network */ 499 local->beacon.mac_pl.pan_coordinator = 1; 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(request->interval, 508 request->wpan_phy->symbol_duration); 509 queue_delayed_work(local->mac_wq, &local->beacon_work, 0); 510 511 return 0; 512 } 513