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, *einfo_old;
 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                         einfo_old = rcu_replace_pointer(e->info, einfo,
 727                                                         lockdep_is_held(&hwsim_phys_lock));
 728                         rcu_read_unlock();
 729                         kfree_rcu(einfo_old, rcu);
 730                         mutex_unlock(&hwsim_phys_lock);
 731                         return 0;
 732                 }
 733         }
 734         rcu_read_unlock();
 735
 736         kfree(einfo);
 737         mutex_unlock(&hwsim_phys_lock);
 738
 739         return -ENOENT;
 740 }
 741
 742 /* MAC802154_HWSIM netlink policy */
 743
 744 static const struct nla_policy hwsim_genl_policy[MAC802154_HWSIM_ATTR_MAX + 1] = {
 745         [MAC802154_HWSIM_ATTR_RADIO_ID] = { .type = NLA_U32 },
 746         [MAC802154_HWSIM_ATTR_RADIO_EDGE] = { .type = NLA_NESTED },
 747         [MAC802154_HWSIM_ATTR_RADIO_EDGES] = { .type = NLA_NESTED },
 748 };
 749
 750 /* Generic Netlink operations array */
 751 static const struct genl_small_ops hwsim_nl_ops[] = {
 752         {
 753                 .cmd = MAC802154_HWSIM_CMD_NEW_RADIO,
 754                 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
 755                 .doit = hwsim_new_radio_nl,
 756                 .flags = GENL_UNS_ADMIN_PERM,
 757         },
 758         {
 759                 .cmd = MAC802154_HWSIM_CMD_DEL_RADIO,
 760                 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
 761                 .doit = hwsim_del_radio_nl,
 762                 .flags = GENL_UNS_ADMIN_PERM,
 763         },
 764         {
 765                 .cmd = MAC802154_HWSIM_CMD_GET_RADIO,
 766                 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
 767                 .doit = hwsim_get_radio_nl,
 768                 .dumpit = hwsim_dump_radio_nl,
 769         },
 770         {
 771                 .cmd = MAC802154_HWSIM_CMD_NEW_EDGE,
 772                 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
 773                 .doit = hwsim_new_edge_nl,
 774                 .flags = GENL_UNS_ADMIN_PERM,
 775         },
 776         {
 777                 .cmd = MAC802154_HWSIM_CMD_DEL_EDGE,
 778                 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
 779                 .doit = hwsim_del_edge_nl,
 780                 .flags = GENL_UNS_ADMIN_PERM,
 781         },
 782         {
 783                 .cmd = MAC802154_HWSIM_CMD_SET_EDGE,
 784                 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
 785                 .doit = hwsim_set_edge_lqi,
 786                 .flags = GENL_UNS_ADMIN_PERM,
 787         },
 788 };
 789
 790 static struct genl_family hwsim_genl_family __ro_after_init = {
 791         .name = "MAC802154_HWSIM",
 792         .version = 1,
 793         .maxattr = MAC802154_HWSIM_ATTR_MAX,
 794         .policy = hwsim_genl_policy,
 795         .module = THIS_MODULE,
 796         .small_ops = hwsim_nl_ops,
 797         .n_small_ops = ARRAY_SIZE(hwsim_nl_ops),
 798         .resv_start_op = MAC802154_HWSIM_CMD_NEW_EDGE + 1,
 799         .mcgrps = hwsim_mcgrps,
 800         .n_mcgrps = ARRAY_SIZE(hwsim_mcgrps),
 801 };
 802
 803 static void hwsim_mcast_config_msg(struct sk_buff *mcast_skb,
 804                                    struct genl_info *info)
 805 {
 806         if (info)
 807                 genl_notify(&hwsim_genl_family, mcast_skb, info,
 808                             HWSIM_MCGRP_CONFIG, GFP_KERNEL);
 809         else
 810                 genlmsg_multicast(&hwsim_genl_family, mcast_skb, 0,
 811                                   HWSIM_MCGRP_CONFIG, GFP_KERNEL);
 812 }
 813
 814 static void hwsim_mcast_new_radio(struct genl_info *info, struct hwsim_phy *phy)
 815 {
 816         struct sk_buff *mcast_skb;
 817         void *data;
 818
 819         mcast_skb = genlmsg_new(GENLMSG_DEFAULT_SIZE, GFP_KERNEL);
 820         if (!mcast_skb)
 821                 return;
 822
 823         data = genlmsg_put(mcast_skb, 0, 0, &hwsim_genl_family, 0,
 824                            MAC802154_HWSIM_CMD_NEW_RADIO);
 825         if (!data)
 826                 goto out_err;
 827
 828         if (append_radio_msg(mcast_skb, phy) < 0)
 829                 goto out_err;
 830
 831         genlmsg_end(mcast_skb, data);
 832
 833         hwsim_mcast_config_msg(mcast_skb, info);
 834         return;
 835
 836 out_err:
 837         genlmsg_cancel(mcast_skb, data);
 838         nlmsg_free(mcast_skb);
 839 }
 840
 841 static void hwsim_edge_unsubscribe_me(struct hwsim_phy *phy)
 842 {
 843         struct hwsim_phy *tmp;
 844         struct hwsim_edge *e;
 845
 846         rcu_read_lock();
 847         /* going to all phy edges and remove phy from it */
 848         list_for_each_entry(tmp, &hwsim_phys, list) {
 849                 list_for_each_entry_rcu(e, &tmp->edges, list) {
 850                         if (e->endpoint->idx == phy->idx) {
 851                                 list_del_rcu(&e->list);
 852                                 hwsim_free_edge(e);
 853                         }
 854                 }
 855         }
 856         rcu_read_unlock();
 857
 858         synchronize_rcu();
 859 }
 860
 861 static int hwsim_subscribe_all_others(struct hwsim_phy *phy)
 862 {
 863         struct hwsim_phy *sub;
 864         struct hwsim_edge *e;
 865
 866         list_for_each_entry(sub, &hwsim_phys, list) {
 867                 e = hwsim_alloc_edge(sub, 0xff);
 868                 if (!e)
 869                         goto me_fail;
 870
 871                 list_add_rcu(&e->list, &phy->edges);
 872         }
 873
 874         list_for_each_entry(sub, &hwsim_phys, list) {
 875                 e = hwsim_alloc_edge(phy, 0xff);
 876                 if (!e)
 877                         goto sub_fail;
 878
 879                 list_add_rcu(&e->list, &sub->edges);
 880         }
 881
 882         return 0;
 883
 884 sub_fail:
 885         hwsim_edge_unsubscribe_me(phy);
 886 me_fail:
 887         rcu_read_lock();
 888         list_for_each_entry_rcu(e, &phy->edges, list) {
 889                 list_del_rcu(&e->list);
 890                 hwsim_free_edge(e);
 891         }
 892         rcu_read_unlock();
 893         return -ENOMEM;
 894 }
 895
 896 static int hwsim_add_one(struct genl_info *info, struct device *dev,
 897                          bool init)
 898 {
 899         struct ieee802154_hw *hw;
 900         struct hwsim_phy *phy;
 901         struct hwsim_pib *pib;
 902         int idx;
 903         int err;
 904
 905         idx = hwsim_radio_idx++;
 906
 907         hw = ieee802154_alloc_hw(sizeof(*phy), &hwsim_ops);
 908         if (!hw)
 909                 return -ENOMEM;
 910
 911         phy = hw->priv;
 912         phy->hw = hw;
 913
 914         /* 868 MHz BPSK 802.15.4-2003 */
 915         hw->phy->supported.channels[0] |= 1;
 916         /* 915 MHz BPSK 802.15.4-2003 */
 917         hw->phy->supported.channels[0] |= 0x7fe;
 918         /* 2.4 GHz O-QPSK 802.15.4-2003 */
 919         hw->phy->supported.channels[0] |= 0x7FFF800;
 920         /* 868 MHz ASK 802.15.4-2006 */
 921         hw->phy->supported.channels[1] |= 1;
 922         /* 915 MHz ASK 802.15.4-2006 */
 923         hw->phy->supported.channels[1] |= 0x7fe;
 924         /* 868 MHz O-QPSK 802.15.4-2006 */
 925         hw->phy->supported.channels[2] |= 1;
 926         /* 915 MHz O-QPSK 802.15.4-2006 */
 927         hw->phy->supported.channels[2] |= 0x7fe;
 928         /* 2.4 GHz CSS 802.15.4a-2007 */
 929         hw->phy->supported.channels[3] |= 0x3fff;
 930         /* UWB Sub-gigahertz 802.15.4a-2007 */
 931         hw->phy->supported.channels[4] |= 1;
 932         /* UWB Low band 802.15.4a-2007 */
 933         hw->phy->supported.channels[4] |= 0x1e;
 934         /* UWB High band 802.15.4a-2007 */
 935         hw->phy->supported.channels[4] |= 0xffe0;
 936         /* 750 MHz O-QPSK 802.15.4c-2009 */
 937         hw->phy->supported.channels[5] |= 0xf;
 938         /* 750 MHz MPSK 802.15.4c-2009 */
 939         hw->phy->supported.channels[5] |= 0xf0;
 940         /* 950 MHz BPSK 802.15.4d-2009 */
 941         hw->phy->supported.channels[6] |= 0x3ff;
 942         /* 950 MHz GFSK 802.15.4d-2009 */
 943         hw->phy->supported.channels[6] |= 0x3ffc00;
 944
 945         ieee802154_random_extended_addr(&hw->phy->perm_extended_addr);
 946
 947         /* hwsim phy channel 13 as default */
 948         hw->phy->current_channel = 13;
 949         pib = kzalloc(sizeof(*pib), GFP_KERNEL);
 950         if (!pib) {
 951                 err = -ENOMEM;
 952                 goto err_pib;
 953         }
 954
 955         pib->channel = 13;
 956         pib->filt.short_addr = cpu_to_le16(IEEE802154_ADDR_BROADCAST);
 957         pib->filt.pan_id = cpu_to_le16(IEEE802154_PANID_BROADCAST);
 958         rcu_assign_pointer(phy->pib, pib);
 959         phy->idx = idx;
 960         INIT_LIST_HEAD(&phy->edges);
 961
 962         hw->flags = IEEE802154_HW_PROMISCUOUS;
 963         hw->parent = dev;
 964
 965         err = ieee802154_register_hw(hw);
 966         if (err)
 967                 goto err_reg;
 968
 969         mutex_lock(&hwsim_phys_lock);
 970         if (init) {
 971                 err = hwsim_subscribe_all_others(phy);
 972                 if (err < 0) {
 973                         mutex_unlock(&hwsim_phys_lock);
 974                         goto err_subscribe;
 975                 }
 976         }
 977         list_add_tail(&phy->list, &hwsim_phys);
 978         mutex_unlock(&hwsim_phys_lock);
 979
 980         hwsim_mcast_new_radio(info, phy);
 981
 982         return idx;
 983
 984 err_subscribe:
 985         ieee802154_unregister_hw(phy->hw);
 986 err_reg:
 987         kfree(pib);
 988 err_pib:
 989         ieee802154_free_hw(phy->hw);
 990         return err;
 991 }
 992
 993 static void hwsim_del(struct hwsim_phy *phy)
 994 {
 995         struct hwsim_pib *pib;
 996         struct hwsim_edge *e;
 997
 998         hwsim_edge_unsubscribe_me(phy);
 999
1000         list_del(&phy->list);
1001
1002         rcu_read_lock();
1003         list_for_each_entry_rcu(e, &phy->edges, list) {
1004                 list_del_rcu(&e->list);
1005                 hwsim_free_edge(e);
1006         }
1007         pib = rcu_dereference(phy->pib);
1008         rcu_read_unlock();
1009
1010         kfree_rcu(pib, rcu);
1011
1012         ieee802154_unregister_hw(phy->hw);
1013         ieee802154_free_hw(phy->hw);
1014 }
1015
1016 static int hwsim_probe(struct platform_device *pdev)
1017 {
1018         struct hwsim_phy *phy, *tmp;
1019         int err, i;
1020
1021         for (i = 0; i < 2; i++) {
1022                 err = hwsim_add_one(NULL, &pdev->dev, true);
1023                 if (err < 0)
1024                         goto err_slave;
1025         }
1026
1027         dev_info(&pdev->dev, "Added 2 mac802154 hwsim hardware radios\n");
1028         return 0;
1029
1030 err_slave:
1031         mutex_lock(&hwsim_phys_lock);
1032         list_for_each_entry_safe(phy, tmp, &hwsim_phys, list)
1033                 hwsim_del(phy);
1034         mutex_unlock(&hwsim_phys_lock);
1035         return err;
1036 }
1037
1038 static int hwsim_remove(struct platform_device *pdev)
1039 {
1040         struct hwsim_phy *phy, *tmp;
1041
1042         mutex_lock(&hwsim_phys_lock);
1043         list_for_each_entry_safe(phy, tmp, &hwsim_phys, list)
1044                 hwsim_del(phy);
1045         mutex_unlock(&hwsim_phys_lock);
1046
1047         return 0;
1048 }
1049
1050 static struct platform_driver mac802154hwsim_driver = {
1051         .probe = hwsim_probe,
1052         .remove = hwsim_remove,
1053         .driver = {
1054                         .name = "mac802154_hwsim",
1055         },
1056 };
1057
1058 static __init int hwsim_init_module(void)
1059 {
1060         int rc;
1061
1062         rc = genl_register_family(&hwsim_genl_family);
1063         if (rc)
1064                 return rc;
1065
1066         mac802154hwsim_dev = platform_device_register_simple("mac802154_hwsim",
1067                                                              -1, NULL, 0);
1068         if (IS_ERR(mac802154hwsim_dev)) {
1069                 rc = PTR_ERR(mac802154hwsim_dev);
1070                 goto platform_dev;
1071         }
1072
1073         rc = platform_driver_register(&mac802154hwsim_driver);
1074         if (rc < 0)
1075                 goto platform_drv;
1076
1077         return 0;
1078
1079 platform_drv:
1080         platform_device_unregister(mac802154hwsim_dev);
1081 platform_dev:
1082         genl_unregister_family(&hwsim_genl_family);
1083         return rc;
1084 }
1085
1086 static __exit void hwsim_remove_module(void)
1087 {
1088         genl_unregister_family(&hwsim_genl_family);
1089         platform_driver_unregister(&mac802154hwsim_driver);
1090         platform_device_unregister(mac802154hwsim_dev);
1091 }
1092
1093 module_init(hwsim_init_module);
1094 module_exit(hwsim_remove_module);