net/mac802154/scan.c

   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 }