drivers/net/ieee802154/mac802154_hwsim.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * HWSIM IEEE 802.15.4 interface
   4  *
   5  * (C) 2018 Mojatau, Alexander Aring <aring@mojatau.com>
   6  * Copyright 2007-2012 Siemens AG
   7  *
   8  * Based on fakelb, original Written by:
   9  * Sergey Lapin <slapin@ossfans.org>
  10  * Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>
  11  * Alexander Smirnov <alex.bluesman.smirnov@gmail.com>
  12  */
  13
  14 #include <linux/module.h>
  15 #include <linux/timer.h>
  16 #include <linux/platform_device.h>
  17 #include <linux/rtnetlink.h>
  18 #include <linux/netdevice.h>
  19 #include <linux/device.h>
  20 #include <linux/spinlock.h>
  21 #include <net/ieee802154_netdev.h>
  22 #include <net/mac802154.h>
  23 #include <net/cfg802154.h>
  24 #include <net/genetlink.h>
  25 #include "mac802154_hwsim.h"
  26
  27 MODULE_DESCRIPTION("Software simulator of IEEE 802.15.4 radio(s) for mac802154");
  28 MODULE_LICENSE("GPL");
  29
  30 static LIST_HEAD(hwsim_phys);
  31 static DEFINE_MUTEX(hwsim_phys_lock);
  32
  33 static struct platform_device *mac802154hwsim_dev;
  34
  35 /* MAC802154_HWSIM netlink family */
  36 static struct genl_family hwsim_genl_family;
  37
  38 static int hwsim_radio_idx;
  39
  40 enum hwsim_multicast_groups {
  41         HWSIM_MCGRP_CONFIG,
  42 };
  43
  44 static const struct genl_multicast_group hwsim_mcgrps[] = {
  45         [HWSIM_MCGRP_CONFIG] = { .name = "config", },
  46 };
  47
  48 struct hwsim_pib {
  49         u8 page;
  50         u8 channel;
  51         struct ieee802154_hw_addr_filt filt;
  52         enum ieee802154_filtering_level filt_level;
  53
  54         struct rcu_head rcu;
  55 };
  56
  57 struct hwsim_edge_info {
  58         u8 lqi;
  59
  60         struct rcu_head rcu;
  61 };
  62
  63 struct hwsim_edge {
  64         struct hwsim_phy *endpoint;
  65         struct hwsim_edge_info __rcu *info;
  66
  67         struct list_head list;
  68         struct rcu_head rcu;
  69 };
  70
  71 struct hwsim_phy {
  72         struct ieee802154_hw *hw;
  73         u32 idx;
  74
  75         struct hwsim_pib __rcu *pib;
  76
  77         bool suspended;
  78         struct list_head edges;
  79
  80         struct list_head list;
  81 };
  82
  83 static int hwsim_add_one(struct genl_info *info, struct device *dev,
  84                          bool init);
  85 static void hwsim_del(struct hwsim_phy *phy);
  86
  87 static int hwsim_hw_ed(struct ieee802154_hw *hw, u8 *level)
  88 {
  89         *level = 0xbe;
  90
  91         return 0;
  92 }
  93
  94 static int hwsim_update_pib(struct ieee802154_hw *hw, u8 page, u8 channel,
  95                             struct ieee802154_hw_addr_filt *filt,
  96                             enum ieee802154_filtering_level filt_level)
  97 {
  98         struct hwsim_phy *phy = hw->priv;
  99         struct hwsim_pib *pib, *pib_old;
 100
 101         pib = kzalloc(sizeof(*pib), GFP_ATOMIC);
 102         if (!pib)
 103                 return -ENOMEM;
 104
 105         pib_old = rtnl_dereference(phy->pib);
 106
 107         pib->page = page;
 108         pib->channel = channel;
 109         pib->filt.short_addr = filt->short_addr;
 110         pib->filt.pan_id = filt->pan_id;
 111         pib->filt.ieee_addr = filt->ieee_addr;
 112         pib->filt.pan_coord = filt->pan_coord;
 113         pib->filt_level = filt_level;
 114
 115         rcu_assign_pointer(phy->pib, pib);
 116         kfree_rcu(pib_old, rcu);
 117         return 0;
 118 }
 119
 120 static int hwsim_hw_channel(struct ieee802154_hw *hw, u8 page, u8 channel)
 121 {
 122         struct hwsim_phy *phy = hw->priv;
 123         struct hwsim_pib *pib;
 124         int ret;
 125
 126         rcu_read_lock();
 127         pib = rcu_dereference(phy->pib);
 128         ret = hwsim_update_pib(hw, page, channel, &pib->filt, pib->filt_level);
 129         rcu_read_unlock();
 130
 131         return ret;
 132 }
 133
 134 static int hwsim_hw_addr_filt(struct ieee802154_hw *hw,
 135                               struct ieee802154_hw_addr_filt *filt,
 136                               unsigned long changed)
 137 {
 138         struct hwsim_phy *phy = hw->priv;
 139         struct hwsim_pib *pib;
 140         int ret;
 141
 142         rcu_read_lock();
 143         pib = rcu_dereference(phy->pib);
 144         ret = hwsim_update_pib(hw, pib->page, pib->channel, filt, pib->filt_level);
 145         rcu_read_unlock();
 146
 147         return ret;
 148 }
 149
 150 static void hwsim_hw_receive(struct ieee802154_hw *hw, struct sk_buff *skb,
 151                              u8 lqi)
 152 {
 153         struct ieee802154_hdr hdr;
 154         struct hwsim_phy *phy = hw->priv;
 155         struct hwsim_pib *pib;
 156
 157         rcu_read_lock();
 158         pib = rcu_dereference(phy->pib);
 159
 160         if (!pskb_may_pull(skb, 3)) {
 161                 dev_dbg(hw->parent, "invalid frame\n");
 162                 goto drop;
 163         }
 164
 165         memcpy(&hdr, skb->data, 3);
 166
 167         /* Level 4 filtering: Frame fields validity */
 168         if (pib->filt_level == IEEE802154_FILTERING_4_FRAME_FIELDS) {
 169                 /* a) Drop reserved frame types */
 170                 switch (mac_cb(skb)->type) {
 171                 case IEEE802154_FC_TYPE_BEACON:
 172                 case IEEE802154_FC_TYPE_DATA:
 173                 case IEEE802154_FC_TYPE_ACK:
 174                 case IEEE802154_FC_TYPE_MAC_CMD:
 175                         break;
 176                 default:
 177                         dev_dbg(hw->parent, "unrecognized frame type 0x%x\n",
 178                                 mac_cb(skb)->type);
 179                         goto drop;
 180                 }
 181
 182                 /* b) Drop reserved frame versions */
 183                 switch (hdr.fc.version) {
 184                 case IEEE802154_2003_STD:
 185                 case IEEE802154_2006_STD:
 186                 case IEEE802154_STD:
 187                         break;
 188                 default:
 189                         dev_dbg(hw->parent,
 190                                 "unrecognized frame version 0x%x\n",
 191                                 hdr.fc.version);
 192                         goto drop;
 193                 }
 194
 195                 /* c) PAN ID constraints */
 196                 if ((mac_cb(skb)->dest.mode == IEEE802154_ADDR_LONG ||
 197                      mac_cb(skb)->dest.mode == IEEE802154_ADDR_SHORT) &&
 198                     mac_cb(skb)->dest.pan_id != pib->filt.pan_id &&
 199                     mac_cb(skb)->dest.pan_id != cpu_to_le16(IEEE802154_PANID_BROADCAST)) {
 200                         dev_dbg(hw->parent,
 201                                 "unrecognized PAN ID %04x\n",
 202                                 le16_to_cpu(mac_cb(skb)->dest.pan_id));
 203                         goto drop;
 204                 }
 205
 206                 /* d1) Short address constraints */
 207                 if (mac_cb(skb)->dest.mode == IEEE802154_ADDR_SHORT &&
 208                     mac_cb(skb)->dest.short_addr != pib->filt.short_addr &&
 209                     mac_cb(skb)->dest.short_addr != cpu_to_le16(IEEE802154_ADDR_BROADCAST)) {
 210                         dev_dbg(hw->parent,
 211                                 "unrecognized short address %04x\n",
 212                                 le16_to_cpu(mac_cb(skb)->dest.short_addr));
 213                         goto drop;
 214                 }
 215
 216                 /* d2) Extended address constraints */
 217                 if (mac_cb(skb)->dest.mode == IEEE802154_ADDR_LONG &&
 218                     mac_cb(skb)->dest.extended_addr != pib->filt.ieee_addr) {
 219                         dev_dbg(hw->parent,
 220                                 "unrecognized long address 0x%016llx\n",
 221                                 mac_cb(skb)->dest.extended_addr);
 222                         goto drop;
 223                 }
 224
 225                 /* d4) Specific PAN coordinator case (no parent) */
 226                 if ((mac_cb(skb)->type == IEEE802154_FC_TYPE_DATA ||
 227                      mac_cb(skb)->type == IEEE802154_FC_TYPE_MAC_CMD) &&
 228                     mac_cb(skb)->dest.mode == IEEE802154_ADDR_NONE) {
 229                         dev_dbg(hw->parent,
 230                                 "relaying is not supported\n");
 231                         goto drop;
 232                 }
 233
 234                 /* e) Beacon frames follow specific PAN ID rules */
 235                 if (mac_cb(skb)->type == IEEE802154_FC_TYPE_BEACON &&
 236                     pib->filt.pan_id != cpu_to_le16(IEEE802154_PANID_BROADCAST) &&
 237                     mac_cb(skb)->dest.pan_id != pib->filt.pan_id) {
 238                         dev_dbg(hw->parent,
 239                                 "invalid beacon PAN ID %04x\n",
 240                                 le16_to_cpu(mac_cb(skb)->dest.pan_id));
 241                         goto drop;
 242                 }
 243         }
 244
 245         rcu_read_unlock();
 246
 247         ieee802154_rx_irqsafe(hw, skb, lqi);
 248
 249         return;
 250
 251 drop:
 252         rcu_read_unlock();
 253         kfree_skb(skb);
 254 }
 255
 256 static int hwsim_hw_xmit(struct ieee802154_hw *hw, struct sk_buff *skb)
 257 {
 258         struct hwsim_phy *current_phy = hw->priv;
 259         struct hwsim_pib *current_pib, *endpoint_pib;
 260         struct hwsim_edge_info *einfo;
 261         struct hwsim_edge *e;
 262
 263         WARN_ON(current_phy->suspended);
 264
 265         rcu_read_lock();
 266         current_pib = rcu_dereference(current_phy->pib);
 267         list_for_each_entry_rcu(e, &current_phy->edges, list) {
 268                 /* Can be changed later in rx_irqsafe, but this is only a
 269                  * performance tweak. Received radio should drop the frame
 270                  * in mac802154 stack anyway... so we don't need to be
 271                  * 100% of locking here to check on suspended
 272                  */
 273                 if (e->endpoint->suspended)
 274                         continue;
 275
 276                 endpoint_pib = rcu_dereference(e->endpoint->pib);
 277                 if (current_pib->page == endpoint_pib->page &&
 278                     current_pib->channel == endpoint_pib->channel) {
 279                         struct sk_buff *newskb = pskb_copy(skb, GFP_ATOMIC);
 280
 281                         einfo = rcu_dereference(e->info);
 282                         if (newskb)
 283                                 hwsim_hw_receive(e->endpoint->hw, newskb, einfo->lqi);
 284                 }
 285         }
 286         rcu_read_unlock();
 287
 288         ieee802154_xmit_complete(hw, skb, false);
 289         return 0;
 290 }
 291
 292 static int hwsim_hw_start(struct ieee802154_hw *hw)
 293 {
 294         struct hwsim_phy *phy = hw->priv;
 295
 296         phy->suspended = false;
 297
 298         return 0;
 299 }
 300
 301 static void hwsim_hw_stop(struct ieee802154_hw *hw)
 302 {
 303         struct hwsim_phy *phy = hw->priv;
 304
 305         phy->suspended = true;
 306 }
 307
 308 static int
 309 hwsim_set_promiscuous_mode(struct ieee802154_hw *hw, const bool on)
 310 {
 311         enum ieee802154_filtering_level filt_level;
 312         struct hwsim_phy *phy = hw->priv;
 313         struct hwsim_pib *pib;
 314         int ret;
 315
 316         if (on)
 317                 filt_level = IEEE802154_FILTERING_NONE;
 318         else
 319                 filt_level = IEEE802154_FILTERING_4_FRAME_FIELDS;
 320
 321         rcu_read_lock();
 322         pib = rcu_dereference(phy->pib);
 323         ret = hwsim_update_pib(hw, pib->page, pib->channel, &pib->filt, filt_level);
 324         rcu_read_unlock();
 325
 326         return ret;
 327 }
 328
 329 static const struct ieee802154_ops hwsim_ops = {
 330         .owner = THIS_MODULE,
 331         .xmit_async = hwsim_hw_xmit,
 332         .ed = hwsim_hw_ed,
 333         .set_channel = hwsim_hw_channel,
 334         .start = hwsim_hw_start,
 335         .stop = hwsim_hw_stop,
 336         .set_promiscuous_mode = hwsim_set_promiscuous_mode,
 337         .set_hw_addr_filt = hwsim_hw_addr_filt,
 338 };
 339
 340 static int hwsim_new_radio_nl(struct sk_buff *msg, struct genl_info *info)
 341 {
 342         return hwsim_add_one(info, &mac802154hwsim_dev->dev, false);
 343 }
 344
 345 static int hwsim_del_radio_nl(struct sk_buff *msg, struct genl_info *info)
 346 {
 347         struct hwsim_phy *phy, *tmp;
 348         s64 idx = -1;
 349
 350         if (!info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID])
 351                 return -EINVAL;
 352
 353         idx = nla_get_u32(info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID]);
 354
 355         mutex_lock(&hwsim_phys_lock);
 356         list_for_each_entry_safe(phy, tmp, &hwsim_phys, list) {
 357                 if (idx == phy->idx) {
 358                         hwsim_del(phy);
 359                         mutex_unlock(&hwsim_phys_lock);
 360                         return 0;
 361                 }
 362         }
 363         mutex_unlock(&hwsim_phys_lock);
 364
 365         return -ENODEV;
 366 }
 367
 368 static int append_radio_msg(struct sk_buff *skb, struct hwsim_phy *phy)
 369 {
 370         struct nlattr *nl_edges, *nl_edge;
 371         struct hwsim_edge_info *einfo;
 372         struct hwsim_edge *e;
 373         int ret;
 374
 375         ret = nla_put_u32(skb, MAC802154_HWSIM_ATTR_RADIO_ID, phy->idx);
 376         if (ret < 0)
 377                 return ret;
 378
 379         rcu_read_lock();
 380         if (list_empty(&phy->edges)) {
 381                 rcu_read_unlock();
 382                 return 0;
 383         }
 384
 385         nl_edges = nla_nest_start_noflag(skb,
 386                                          MAC802154_HWSIM_ATTR_RADIO_EDGES);
 387         if (!nl_edges) {
 388                 rcu_read_unlock();
 389                 return -ENOBUFS;
 390         }
 391
 392         list_for_each_entry_rcu(e, &phy->edges, list) {
 393                 nl_edge = nla_nest_start_noflag(skb,
 394                                                 MAC802154_HWSIM_ATTR_RADIO_EDGE);
 395                 if (!nl_edge) {
 396                         rcu_read_unlock();
 397                         nla_nest_cancel(skb, nl_edges);
 398                         return -ENOBUFS;
 399                 }
 400
 401                 ret = nla_put_u32(skb, MAC802154_HWSIM_EDGE_ATTR_ENDPOINT_ID,
 402                                   e->endpoint->idx);
 403                 if (ret < 0) {
 404                         rcu_read_unlock();
 405                         nla_nest_cancel(skb, nl_edge);
 406                         nla_nest_cancel(skb, nl_edges);
 407                         return ret;
 408                 }
 409
 410                 einfo = rcu_dereference(e->info);
 411                 ret = nla_put_u8(skb, MAC802154_HWSIM_EDGE_ATTR_LQI,
 412                                  einfo->lqi);
 413                 if (ret < 0) {
 414                         rcu_read_unlock();
 415                         nla_nest_cancel(skb, nl_edge);
 416                         nla_nest_cancel(skb, nl_edges);
 417                         return ret;
 418                 }
 419
 420                 nla_nest_end(skb, nl_edge);
 421         }
 422         rcu_read_unlock();
 423
 424         nla_nest_end(skb, nl_edges);
 425
 426         return 0;
 427 }
 428
 429 static int hwsim_get_radio(struct sk_buff *skb, struct hwsim_phy *phy,
 430                            u32 portid, u32 seq,
 431                            struct netlink_callback *cb, int flags)
 432 {
 433         void *hdr;
 434         int res;
 435
 436         hdr = genlmsg_put(skb, portid, seq, &hwsim_genl_family, flags,
 437                           MAC802154_HWSIM_CMD_GET_RADIO);
 438         if (!hdr)
 439                 return -EMSGSIZE;
 440
 441         if (cb)
 442                 genl_dump_check_consistent(cb, hdr);
 443
 444         res = append_radio_msg(skb, phy);
 445         if (res < 0)
 446                 goto out_err;
 447
 448         genlmsg_end(skb, hdr);
 449         return 0;
 450
 451 out_err:
 452         genlmsg_cancel(skb, hdr);
 453         return res;
 454 }
 455
 456 static int hwsim_get_radio_nl(struct sk_buff *msg, struct genl_info *info)
 457 {
 458         struct hwsim_phy *phy;
 459         struct sk_buff *skb;
 460         int idx, res = -ENODEV;
 461
 462         if (!info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID])
 463                 return -EINVAL;
 464         idx = nla_get_u32(info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID]);
 465
 466         mutex_lock(&hwsim_phys_lock);
 467         list_for_each_entry(phy, &hwsim_phys, list) {
 468                 if (phy->idx != idx)
 469                         continue;
 470
 471                 skb = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
 472                 if (!skb) {
 473                         res = -ENOMEM;
 474                         goto out_err;
 475                 }
 476
 477                 res = hwsim_get_radio(skb, phy, info->snd_portid,
 478                                       info->snd_seq, NULL, 0);
 479                 if (res < 0) {
 480                         nlmsg_free(skb);
 481                         goto out_err;
 482                 }
 483
 484                 res = genlmsg_reply(skb, info);
 485                 break;
 486         }
 487
 488 out_err:
 489         mutex_unlock(&hwsim_phys_lock);
 490
 491         return res;
 492 }
 493
 494 static int hwsim_dump_radio_nl(struct sk_buff *skb,
 495                                struct netlink_callback *cb)
 496 {
 497         int idx = cb->args[0];
 498         struct hwsim_phy *phy;
 499         int res;
 500
 501         mutex_lock(&hwsim_phys_lock);
 502
 503         if (idx == hwsim_radio_idx)
 504                 goto done;
 505
 506         list_for_each_entry(phy, &hwsim_phys, list) {
 507                 if (phy->idx < idx)
 508                         continue;
 509
 510                 res = hwsim_get_radio(skb, phy, NETLINK_CB(cb->skb).portid,
 511                                       cb->nlh->nlmsg_seq, cb, NLM_F_MULTI);
 512                 if (res < 0)
 513                         break;
 514
 515                 idx = phy->idx + 1;
 516         }
 517
 518         cb->args[0] = idx;
 519
 520 done:
 521         mutex_unlock(&hwsim_phys_lock);
 522         return skb->len;
 523 }
 524
 525 /* caller need to held hwsim_phys_lock */
 526 static struct hwsim_phy *hwsim_get_radio_by_id(uint32_t idx)
 527 {
 528         struct hwsim_phy *phy;
 529
 530         list_for_each_entry(phy, &hwsim_phys, list) {
 531                 if (phy->idx == idx)
 532                         return phy;
 533         }
 534
 535         return NULL;
 536 }
 537
 538 static const struct nla_policy hwsim_edge_policy[MAC802154_HWSIM_EDGE_ATTR_MAX + 1] = {
 539         [MAC802154_HWSIM_EDGE_ATTR_ENDPOINT_ID] = { .type = NLA_U32 },
 540         [MAC802154_HWSIM_EDGE_ATTR_LQI] = { .type = NLA_U8 },
 541 };
 542
 543 static struct hwsim_edge *hwsim_alloc_edge(struct hwsim_phy *endpoint, u8 lqi)
 544 {
 545         struct hwsim_edge_info *einfo;
 546         struct hwsim_edge *e;
 547
 548         e = kzalloc(sizeof(*e), GFP_KERNEL);
 549         if (!e)
 550                 return NULL;
 551
 552         einfo = kzalloc(sizeof(*einfo), GFP_KERNEL);
 553         if (!einfo) {
 554                 kfree(e);
 555                 return NULL;
 556         }
 557
 558         einfo->lqi = 0xff;
 559         rcu_assign_pointer(e->info, einfo);
 560         e->endpoint = endpoint;
 561
 562         return e;
 563 }
 564
 565 static void hwsim_free_edge(struct hwsim_edge *e)
 566 {
 567         struct hwsim_edge_info *einfo;
 568
 569         rcu_read_lock();
 570         einfo = rcu_dereference(e->info);
 571         rcu_read_unlock();
 572
 573         kfree_rcu(einfo, rcu);
 574         kfree_rcu(e, rcu);
 575 }
 576
 577 static int hwsim_new_edge_nl(struct sk_buff *msg, struct genl_info *info)
 578 {
 579         struct nlattr *edge_attrs[MAC802154_HWSIM_EDGE_ATTR_MAX + 1];
 580         struct hwsim_phy *phy_v0, *phy_v1;
 581         struct hwsim_edge *e;
 582         u32 v0, v1;
 583
 584         if (!info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID] ||
 585             !info->attrs[MAC802154_HWSIM_ATTR_RADIO_EDGE])
 586                 return -EINVAL;
 587
 588         if (nla_parse_nested_deprecated(edge_attrs, MAC802154_HWSIM_EDGE_ATTR_MAX, info->attrs[MAC802154_HWSIM_ATTR_RADIO_EDGE], hwsim_edge_policy, NULL))
 589                 return -EINVAL;
 590
 591         if (!edge_attrs[MAC802154_HWSIM_EDGE_ATTR_ENDPOINT_ID])
 592                 return -EINVAL;
 593
 594         v0 = nla_get_u32(info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID]);
 595         v1 = nla_get_u32(edge_attrs[MAC802154_HWSIM_EDGE_ATTR_ENDPOINT_ID]);
 596
 597         if (v0 == v1)
 598                 return -EINVAL;
 599
 600         mutex_lock(&hwsim_phys_lock);
 601         phy_v0 = hwsim_get_radio_by_id(v0);
 602         if (!phy_v0) {
 603                 mutex_unlock(&hwsim_phys_lock);
 604                 return -ENOENT;
 605         }
 606
 607         phy_v1 = hwsim_get_radio_by_id(v1);
 608         if (!phy_v1) {
 609                 mutex_unlock(&hwsim_phys_lock);
 610                 return -ENOENT;
 611         }
 612
 613         rcu_read_lock();
 614         list_for_each_entry_rcu(e, &phy_v0->edges, list) {
 615                 if (e->endpoint->idx == v1) {
 616                         mutex_unlock(&hwsim_phys_lock);
 617                         rcu_read_unlock();
 618                         return -EEXIST;
 619                 }
 620         }
 621         rcu_read_unlock();
 622
 623         e = hwsim_alloc_edge(phy_v1, 0xff);
 624         if (!e) {
 625                 mutex_unlock(&hwsim_phys_lock);
 626                 return -ENOMEM;
 627         }
 628         list_add_rcu(&e->list, &phy_v0->edges);
 629         /* wait until changes are done under hwsim_phys_lock lock
 630          * should prevent of calling this function twice while
 631          * edges list has not the changes yet.
 632          */
 633         synchronize_rcu();
 634         mutex_unlock(&hwsim_phys_lock);
 635
 636         return 0;
 637 }
 638
 639 static int hwsim_del_edge_nl(struct sk_buff *msg, struct genl_info *info)
 640 {
 641         struct nlattr *edge_attrs[MAC802154_HWSIM_EDGE_ATTR_MAX + 1];
 642         struct hwsim_phy *phy_v0;
 643         struct hwsim_edge *e;
 644         u32 v0, v1;
 645
 646         if (!info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID] ||
 647             !info->attrs[MAC802154_HWSIM_ATTR_RADIO_EDGE])
 648                 return -EINVAL;
 649
 650         if (nla_parse_nested_deprecated(edge_attrs, MAC802154_HWSIM_EDGE_ATTR_MAX, info->attrs[MAC802154_HWSIM_ATTR_RADIO_EDGE], hwsim_edge_policy, NULL))
 651                 return -EINVAL;
 652
 653         if (!edge_attrs[MAC802154_HWSIM_EDGE_ATTR_ENDPOINT_ID])
 654                 return -EINVAL;
 655
 656         v0 = nla_get_u32(info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID]);
 657         v1 = nla_get_u32(edge_attrs[MAC802154_HWSIM_EDGE_ATTR_ENDPOINT_ID]);
 658
 659         mutex_lock(&hwsim_phys_lock);
 660         phy_v0 = hwsim_get_radio_by_id(v0);
 661         if (!phy_v0) {
 662                 mutex_unlock(&hwsim_phys_lock);
 663                 return -ENOENT;
 664         }
 665
 666         rcu_read_lock();
 667         list_for_each_entry_rcu(e, &phy_v0->edges, list) {
 668                 if (e->endpoint->idx == v1) {
 669                         rcu_read_unlock();
 670                         list_del_rcu(&e->list);
 671                         hwsim_free_edge(e);
 672                         /* same again - wait until list changes are done */
 673                         synchronize_rcu();
 674                         mutex_unlock(&hwsim_phys_lock);
 675                         return 0;
 676                 }
 677         }
 678         rcu_read_unlock();
 679
 680         mutex_unlock(&hwsim_phys_lock);
 681
 682         return -ENOENT;
 683 }
 684
 685 static int hwsim_set_edge_lqi(struct sk_buff *msg, struct genl_info *info)
 686 {
 687         struct nlattr *edge_attrs[MAC802154_HWSIM_EDGE_ATTR_MAX + 1];
 688         struct hwsim_edge_info *einfo;
 689         struct hwsim_phy *phy_v0;
 690         struct hwsim_edge *e;
 691         u32 v0, v1;
 692         u8 lqi;
 693
 694         if (!info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID] ||
 695             !info->attrs[MAC802154_HWSIM_ATTR_RADIO_EDGE])
 696                 return -EINVAL;
 697
 698         if (nla_parse_nested_deprecated(edge_attrs, MAC802154_HWSIM_EDGE_ATTR_MAX, info->attrs[MAC802154_HWSIM_ATTR_RADIO_EDGE], hwsim_edge_policy, NULL))
 699                 return -EINVAL;
 700
 701         if (!edge_attrs[MAC802154_HWSIM_EDGE_ATTR_ENDPOINT_ID] ||
 702             !edge_attrs[MAC802154_HWSIM_EDGE_ATTR_LQI])
 703                 return -EINVAL;
 704
 705         v0 = nla_get_u32(info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID]);
 706         v1 = nla_get_u32(edge_attrs[MAC802154_HWSIM_EDGE_ATTR_ENDPOINT_ID]);
 707         lqi = nla_get_u8(edge_attrs[MAC802154_HWSIM_EDGE_ATTR_LQI]);
 708
 709         mutex_lock(&hwsim_phys_lock);
 710         phy_v0 = hwsim_get_radio_by_id(v0);
 711         if (!phy_v0) {
 712                 mutex_unlock(&hwsim_phys_lock);
 713                 return -ENOENT;
 714         }
 715
 716         einfo = kzalloc(sizeof(*einfo), GFP_KERNEL);
 717         if (!einfo) {
 718                 mutex_unlock(&hwsim_phys_lock);
 719                 return -ENOMEM;
 720         }
 721
 722         rcu_read_lock();
 723         list_for_each_entry_rcu(e, &phy_v0->edges, list) {
 724                 if (e->endpoint->idx == v1) {
 725                         einfo->lqi = lqi;
 726                         rcu_assign_pointer(e->info, einfo);
 727                         rcu_read_unlock();
 728                         mutex_unlock(&hwsim_phys_lock);
 729                         return 0;
 730                 }
 731         }
 732         rcu_read_unlock();
 733
 734         kfree(einfo);
 735         mutex_unlock(&hwsim_phys_lock);
 736
 737         return -ENOENT;
 738 }
 739
 740 /* MAC802154_HWSIM netlink policy */
 741
 742 static const struct nla_policy hwsim_genl_policy[MAC802154_HWSIM_ATTR_MAX + 1] = {
 743         [MAC802154_HWSIM_ATTR_RADIO_ID] = { .type = NLA_U32 },
 744         [MAC802154_HWSIM_ATTR_RADIO_EDGE] = { .type = NLA_NESTED },
 745         [MAC802154_HWSIM_ATTR_RADIO_EDGES] = { .type = NLA_NESTED },
 746 };
 747
 748 /* Generic Netlink operations array */
 749 static const struct genl_small_ops hwsim_nl_ops[] = {
 750         {
 751                 .cmd = MAC802154_HWSIM_CMD_NEW_RADIO,
 752                 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
 753                 .doit = hwsim_new_radio_nl,
 754                 .flags = GENL_UNS_ADMIN_PERM,
 755         },
 756         {
 757                 .cmd = MAC802154_HWSIM_CMD_DEL_RADIO,
 758                 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
 759                 .doit = hwsim_del_radio_nl,
 760                 .flags = GENL_UNS_ADMIN_PERM,
 761         },
 762         {
 763                 .cmd = MAC802154_HWSIM_CMD_GET_RADIO,
 764                 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
 765                 .doit = hwsim_get_radio_nl,
 766                 .dumpit = hwsim_dump_radio_nl,
 767         },
 768         {
 769                 .cmd = MAC802154_HWSIM_CMD_NEW_EDGE,
 770                 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
 771                 .doit = hwsim_new_edge_nl,
 772                 .flags = GENL_UNS_ADMIN_PERM,
 773         },
 774         {
 775                 .cmd = MAC802154_HWSIM_CMD_DEL_EDGE,
 776                 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
 777                 .doit = hwsim_del_edge_nl,
 778                 .flags = GENL_UNS_ADMIN_PERM,
 779         },
 780         {
 781                 .cmd = MAC802154_HWSIM_CMD_SET_EDGE,
 782                 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
 783                 .doit = hwsim_set_edge_lqi,
 784                 .flags = GENL_UNS_ADMIN_PERM,
 785         },
 786 };
 787
 788 static struct genl_family hwsim_genl_family __ro_after_init = {
 789         .name = "MAC802154_HWSIM",
 790         .version = 1,
 791         .maxattr = MAC802154_HWSIM_ATTR_MAX,
 792         .policy = hwsim_genl_policy,
 793         .module = THIS_MODULE,
 794         .small_ops = hwsim_nl_ops,
 795         .n_small_ops = ARRAY_SIZE(hwsim_nl_ops),
 796         .resv_start_op = MAC802154_HWSIM_CMD_NEW_EDGE + 1,
 797         .mcgrps = hwsim_mcgrps,
 798         .n_mcgrps = ARRAY_SIZE(hwsim_mcgrps),
 799 };
 800
 801 static void hwsim_mcast_config_msg(struct sk_buff *mcast_skb,
 802                                    struct genl_info *info)
 803 {
 804         if (info)
 805                 genl_notify(&hwsim_genl_family, mcast_skb, info,
 806                             HWSIM_MCGRP_CONFIG, GFP_KERNEL);
 807         else
 808                 genlmsg_multicast(&hwsim_genl_family, mcast_skb, 0,
 809                                   HWSIM_MCGRP_CONFIG, GFP_KERNEL);
 810 }
 811
 812 static void hwsim_mcast_new_radio(struct genl_info *info, struct hwsim_phy *phy)
 813 {
 814         struct sk_buff *mcast_skb;
 815         void *data;
 816
 817         mcast_skb = genlmsg_new(GENLMSG_DEFAULT_SIZE, GFP_KERNEL);
 818         if (!mcast_skb)
 819                 return;
 820
 821         data = genlmsg_put(mcast_skb, 0, 0, &hwsim_genl_family, 0,
 822                            MAC802154_HWSIM_CMD_NEW_RADIO);
 823         if (!data)
 824                 goto out_err;
 825
 826         if (append_radio_msg(mcast_skb, phy) < 0)
 827                 goto out_err;
 828
 829         genlmsg_end(mcast_skb, data);
 830
 831         hwsim_mcast_config_msg(mcast_skb, info);
 832         return;
 833
 834 out_err:
 835         genlmsg_cancel(mcast_skb, data);
 836         nlmsg_free(mcast_skb);
 837 }
 838
 839 static void hwsim_edge_unsubscribe_me(struct hwsim_phy *phy)
 840 {
 841         struct hwsim_phy *tmp;
 842         struct hwsim_edge *e;
 843
 844         rcu_read_lock();
 845         /* going to all phy edges and remove phy from it */
 846         list_for_each_entry(tmp, &hwsim_phys, list) {
 847                 list_for_each_entry_rcu(e, &tmp->edges, list) {
 848                         if (e->endpoint->idx == phy->idx) {
 849                                 list_del_rcu(&e->list);
 850                                 hwsim_free_edge(e);
 851                         }
 852                 }
 853         }
 854         rcu_read_unlock();
 855
 856         synchronize_rcu();
 857 }
 858
 859 static int hwsim_subscribe_all_others(struct hwsim_phy *phy)
 860 {
 861         struct hwsim_phy *sub;
 862         struct hwsim_edge *e;
 863
 864         list_for_each_entry(sub, &hwsim_phys, list) {
 865                 e = hwsim_alloc_edge(sub, 0xff);
 866                 if (!e)
 867                         goto me_fail;
 868
 869                 list_add_rcu(&e->list, &phy->edges);
 870         }
 871
 872         list_for_each_entry(sub, &hwsim_phys, list) {
 873                 e = hwsim_alloc_edge(phy, 0xff);
 874                 if (!e)
 875                         goto sub_fail;
 876
 877                 list_add_rcu(&e->list, &sub->edges);
 878         }
 879
 880         return 0;
 881
 882 sub_fail:
 883         hwsim_edge_unsubscribe_me(phy);
 884 me_fail:
 885         rcu_read_lock();
 886         list_for_each_entry_rcu(e, &phy->edges, list) {
 887                 list_del_rcu(&e->list);
 888                 hwsim_free_edge(e);
 889         }
 890         rcu_read_unlock();
 891         return -ENOMEM;
 892 }
 893
 894 static int hwsim_add_one(struct genl_info *info, struct device *dev,
 895                          bool init)
 896 {
 897         struct ieee802154_hw *hw;
 898         struct hwsim_phy *phy;
 899         struct hwsim_pib *pib;
 900         int idx;
 901         int err;
 902
 903         idx = hwsim_radio_idx++;
 904
 905         hw = ieee802154_alloc_hw(sizeof(*phy), &hwsim_ops);
 906         if (!hw)
 907                 return -ENOMEM;
 908
 909         phy = hw->priv;
 910         phy->hw = hw;
 911
 912         /* 868 MHz BPSK 802.15.4-2003 */
 913         hw->phy->supported.channels[0] |= 1;
 914         /* 915 MHz BPSK 802.15.4-2003 */
 915         hw->phy->supported.channels[0] |= 0x7fe;
 916         /* 2.4 GHz O-QPSK 802.15.4-2003 */
 917         hw->phy->supported.channels[0] |= 0x7FFF800;
 918         /* 868 MHz ASK 802.15.4-2006 */
 919         hw->phy->supported.channels[1] |= 1;
 920         /* 915 MHz ASK 802.15.4-2006 */
 921         hw->phy->supported.channels[1] |= 0x7fe;
 922         /* 868 MHz O-QPSK 802.15.4-2006 */
 923         hw->phy->supported.channels[2] |= 1;
 924         /* 915 MHz O-QPSK 802.15.4-2006 */
 925         hw->phy->supported.channels[2] |= 0x7fe;
 926         /* 2.4 GHz CSS 802.15.4a-2007 */
 927         hw->phy->supported.channels[3] |= 0x3fff;
 928         /* UWB Sub-gigahertz 802.15.4a-2007 */
 929         hw->phy->supported.channels[4] |= 1;
 930         /* UWB Low band 802.15.4a-2007 */
 931         hw->phy->supported.channels[4] |= 0x1e;
 932         /* UWB High band 802.15.4a-2007 */
 933         hw->phy->supported.channels[4] |= 0xffe0;
 934         /* 750 MHz O-QPSK 802.15.4c-2009 */
 935         hw->phy->supported.channels[5] |= 0xf;
 936         /* 750 MHz MPSK 802.15.4c-2009 */
 937         hw->phy->supported.channels[5] |= 0xf0;
 938         /* 950 MHz BPSK 802.15.4d-2009 */
 939         hw->phy->supported.channels[6] |= 0x3ff;
 940         /* 950 MHz GFSK 802.15.4d-2009 */
 941         hw->phy->supported.channels[6] |= 0x3ffc00;
 942
 943         ieee802154_random_extended_addr(&hw->phy->perm_extended_addr);
 944
 945         /* hwsim phy channel 13 as default */
 946         hw->phy->current_channel = 13;
 947         pib = kzalloc(sizeof(*pib), GFP_KERNEL);
 948         if (!pib) {
 949                 err = -ENOMEM;
 950                 goto err_pib;
 951         }
 952
 953         pib->channel = 13;
 954         pib->filt.short_addr = cpu_to_le16(IEEE802154_ADDR_BROADCAST);
 955         pib->filt.pan_id = cpu_to_le16(IEEE802154_PANID_BROADCAST);
 956         rcu_assign_pointer(phy->pib, pib);
 957         phy->idx = idx;
 958         INIT_LIST_HEAD(&phy->edges);
 959
 960         hw->flags = IEEE802154_HW_PROMISCUOUS;
 961         hw->parent = dev;
 962
 963         err = ieee802154_register_hw(hw);
 964         if (err)
 965                 goto err_reg;
 966
 967         mutex_lock(&hwsim_phys_lock);
 968         if (init) {
 969                 err = hwsim_subscribe_all_others(phy);
 970                 if (err < 0) {
 971                         mutex_unlock(&hwsim_phys_lock);
 972                         goto err_subscribe;
 973                 }
 974         }
 975         list_add_tail(&phy->list, &hwsim_phys);
 976         mutex_unlock(&hwsim_phys_lock);
 977
 978         hwsim_mcast_new_radio(info, phy);
 979
 980         return idx;
 981
 982 err_subscribe:
 983         ieee802154_unregister_hw(phy->hw);
 984 err_reg:
 985         kfree(pib);
 986 err_pib:
 987         ieee802154_free_hw(phy->hw);
 988         return err;
 989 }
 990
 991 static void hwsim_del(struct hwsim_phy *phy)
 992 {
 993         struct hwsim_pib *pib;
 994         struct hwsim_edge *e;
 995
 996         hwsim_edge_unsubscribe_me(phy);
 997
 998         list_del(&phy->list);
 999
1000         rcu_read_lock();
1001         list_for_each_entry_rcu(e, &phy->edges, list) {
1002                 list_del_rcu(&e->list);
1003                 hwsim_free_edge(e);
1004         }
1005         pib = rcu_dereference(phy->pib);
1006         rcu_read_unlock();
1007
1008         kfree_rcu(pib, rcu);
1009
1010         ieee802154_unregister_hw(phy->hw);
1011         ieee802154_free_hw(phy->hw);
1012 }
1013
1014 static int hwsim_probe(struct platform_device *pdev)
1015 {
1016         struct hwsim_phy *phy, *tmp;
1017         int err, i;
1018
1019         for (i = 0; i < 2; i++) {
1020                 err = hwsim_add_one(NULL, &pdev->dev, true);
1021                 if (err < 0)
1022                         goto err_slave;
1023         }
1024
1025         dev_info(&pdev->dev, "Added 2 mac802154 hwsim hardware radios\n");
1026         return 0;
1027
1028 err_slave:
1029         mutex_lock(&hwsim_phys_lock);
1030         list_for_each_entry_safe(phy, tmp, &hwsim_phys, list)
1031                 hwsim_del(phy);
1032         mutex_unlock(&hwsim_phys_lock);
1033         return err;
1034 }
1035
1036 static int hwsim_remove(struct platform_device *pdev)
1037 {
1038         struct hwsim_phy *phy, *tmp;
1039
1040         mutex_lock(&hwsim_phys_lock);
1041         list_for_each_entry_safe(phy, tmp, &hwsim_phys, list)
1042                 hwsim_del(phy);
1043         mutex_unlock(&hwsim_phys_lock);
1044
1045         return 0;
1046 }
1047
1048 static struct platform_driver mac802154hwsim_driver = {
1049         .probe = hwsim_probe,
1050         .remove = hwsim_remove,
1051         .driver = {
1052                         .name = "mac802154_hwsim",
1053         },
1054 };
1055
1056 static __init int hwsim_init_module(void)
1057 {
1058         int rc;
1059
1060         rc = genl_register_family(&hwsim_genl_family);
1061         if (rc)
1062                 return rc;
1063
1064         mac802154hwsim_dev = platform_device_register_simple("mac802154_hwsim",
1065                                                              -1, NULL, 0);
1066         if (IS_ERR(mac802154hwsim_dev)) {
1067                 rc = PTR_ERR(mac802154hwsim_dev);
1068                 goto platform_dev;
1069         }
1070
1071         rc = platform_driver_register(&mac802154hwsim_driver);
1072         if (rc < 0)
1073                 goto platform_drv;
1074
1075         return 0;
1076
1077 platform_drv:
1078         platform_device_unregister(mac802154hwsim_dev);
1079 platform_dev:
1080         genl_unregister_family(&hwsim_genl_family);
1081         return rc;
1082 }
1083
1084 static __exit void hwsim_remove_module(void)
1085 {
1086         genl_unregister_family(&hwsim_genl_family);
1087         platform_driver_unregister(&mac802154hwsim_driver);
1088         platform_device_unregister(mac802154hwsim_dev);
1089 }
1090
1091 module_init(hwsim_init_module);
1092 module_exit(hwsim_remove_module);