drivers/net/ieee802154/mac802154_hwsim.c

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