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, *einfo_old;
 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                         einfo_old = rcu_replace_pointer(e->info, einfo,
 584                                                         lockdep_is_held(&hwsim_phys_lock));
 585                         rcu_read_unlock();
 586                         kfree_rcu(einfo_old, rcu);
 587                         mutex_unlock(&hwsim_phys_lock);
 588                         return 0;
 589                 }
 590         }
 591         rcu_read_unlock();
 592
 593         kfree(einfo);
 594         mutex_unlock(&hwsim_phys_lock);
 595
 596         return -ENOENT;
 597 }
 598
 599 /* MAC802154_HWSIM netlink policy */
 600
 601 static const struct nla_policy hwsim_genl_policy[MAC802154_HWSIM_ATTR_MAX + 1] = {
 602         [MAC802154_HWSIM_ATTR_RADIO_ID] = { .type = NLA_U32 },
 603         [MAC802154_HWSIM_ATTR_RADIO_EDGE] = { .type = NLA_NESTED },
 604         [MAC802154_HWSIM_ATTR_RADIO_EDGES] = { .type = NLA_NESTED },
 605 };
 606
 607 /* Generic Netlink operations array */
 608 static const struct genl_ops hwsim_nl_ops[] = {
 609         {
 610                 .cmd = MAC802154_HWSIM_CMD_NEW_RADIO,
 611                 .policy = hwsim_genl_policy,
 612                 .doit = hwsim_new_radio_nl,
 613                 .flags = GENL_UNS_ADMIN_PERM,
 614         },
 615         {
 616                 .cmd = MAC802154_HWSIM_CMD_DEL_RADIO,
 617                 .policy = hwsim_genl_policy,
 618                 .doit = hwsim_del_radio_nl,
 619                 .flags = GENL_UNS_ADMIN_PERM,
 620         },
 621         {
 622                 .cmd = MAC802154_HWSIM_CMD_GET_RADIO,
 623                 .policy = hwsim_genl_policy,
 624                 .doit = hwsim_get_radio_nl,
 625                 .dumpit = hwsim_dump_radio_nl,
 626         },
 627         {
 628                 .cmd = MAC802154_HWSIM_CMD_NEW_EDGE,
 629                 .policy = hwsim_genl_policy,
 630                 .doit = hwsim_new_edge_nl,
 631                 .flags = GENL_UNS_ADMIN_PERM,
 632         },
 633         {
 634                 .cmd = MAC802154_HWSIM_CMD_DEL_EDGE,
 635                 .policy = hwsim_genl_policy,
 636                 .doit = hwsim_del_edge_nl,
 637                 .flags = GENL_UNS_ADMIN_PERM,
 638         },
 639         {
 640                 .cmd = MAC802154_HWSIM_CMD_SET_EDGE,
 641                 .policy = hwsim_genl_policy,
 642                 .doit = hwsim_set_edge_lqi,
 643                 .flags = GENL_UNS_ADMIN_PERM,
 644         },
 645 };
 646
 647 static struct genl_family hwsim_genl_family __ro_after_init = {
 648         .name = "MAC802154_HWSIM",
 649         .version = 1,
 650         .maxattr = MAC802154_HWSIM_ATTR_MAX,
 651         .module = THIS_MODULE,
 652         .ops = hwsim_nl_ops,
 653         .n_ops = ARRAY_SIZE(hwsim_nl_ops),
 654         .mcgrps = hwsim_mcgrps,
 655         .n_mcgrps = ARRAY_SIZE(hwsim_mcgrps),
 656 };
 657
 658 static void hwsim_mcast_config_msg(struct sk_buff *mcast_skb,
 659                                    struct genl_info *info)
 660 {
 661         if (info)
 662                 genl_notify(&hwsim_genl_family, mcast_skb, info,
 663                             HWSIM_MCGRP_CONFIG, GFP_KERNEL);
 664         else
 665                 genlmsg_multicast(&hwsim_genl_family, mcast_skb, 0,
 666                                   HWSIM_MCGRP_CONFIG, GFP_KERNEL);
 667 }
 668
 669 static void hwsim_mcast_new_radio(struct genl_info *info, struct hwsim_phy *phy)
 670 {
 671         struct sk_buff *mcast_skb;
 672         void *data;
 673
 674         mcast_skb = genlmsg_new(GENLMSG_DEFAULT_SIZE, GFP_KERNEL);
 675         if (!mcast_skb)
 676                 return;
 677
 678         data = genlmsg_put(mcast_skb, 0, 0, &hwsim_genl_family, 0,
 679                            MAC802154_HWSIM_CMD_NEW_RADIO);
 680         if (!data)
 681                 goto out_err;
 682
 683         if (append_radio_msg(mcast_skb, phy) < 0)
 684                 goto out_err;
 685
 686         genlmsg_end(mcast_skb, data);
 687
 688         hwsim_mcast_config_msg(mcast_skb, info);
 689         return;
 690
 691 out_err:
 692         genlmsg_cancel(mcast_skb, data);
 693         nlmsg_free(mcast_skb);
 694 }
 695
 696 static void hwsim_edge_unsubscribe_me(struct hwsim_phy *phy)
 697 {
 698         struct hwsim_phy *tmp;
 699         struct hwsim_edge *e;
 700
 701         rcu_read_lock();
 702         /* going to all phy edges and remove phy from it */
 703         list_for_each_entry(tmp, &hwsim_phys, list) {
 704                 list_for_each_entry_rcu(e, &tmp->edges, list) {
 705                         if (e->endpoint->idx == phy->idx) {
 706                                 list_del_rcu(&e->list);
 707                                 hwsim_free_edge(e);
 708                         }
 709                 }
 710         }
 711         rcu_read_unlock();
 712
 713         synchronize_rcu();
 714 }
 715
 716 static int hwsim_subscribe_all_others(struct hwsim_phy *phy)
 717 {
 718         struct hwsim_phy *sub;
 719         struct hwsim_edge *e;
 720
 721         list_for_each_entry(sub, &hwsim_phys, list) {
 722                 e = hwsim_alloc_edge(sub, 0xff);
 723                 if (!e)
 724                         goto me_fail;
 725
 726                 list_add_rcu(&e->list, &phy->edges);
 727         }
 728
 729         list_for_each_entry(sub, &hwsim_phys, list) {
 730                 e = hwsim_alloc_edge(phy, 0xff);
 731                 if (!e)
 732                         goto sub_fail;
 733
 734                 list_add_rcu(&e->list, &sub->edges);
 735         }
 736
 737         return 0;
 738
 739 sub_fail:
 740         hwsim_edge_unsubscribe_me(phy);
 741 me_fail:
 742         rcu_read_lock();
 743         list_for_each_entry_rcu(e, &phy->edges, list) {
 744                 list_del_rcu(&e->list);
 745                 hwsim_free_edge(e);
 746         }
 747         rcu_read_unlock();
 748         return -ENOMEM;
 749 }
 750
 751 static int hwsim_add_one(struct genl_info *info, struct device *dev,
 752                          bool init)
 753 {
 754         struct ieee802154_hw *hw;
 755         struct hwsim_phy *phy;
 756         struct hwsim_pib *pib;
 757         int idx;
 758         int err;
 759
 760         idx = hwsim_radio_idx++;
 761
 762         hw = ieee802154_alloc_hw(sizeof(*phy), &hwsim_ops);
 763         if (!hw)
 764                 return -ENOMEM;
 765
 766         phy = hw->priv;
 767         phy->hw = hw;
 768
 769         /* 868 MHz BPSK 802.15.4-2003 */
 770         hw->phy->supported.channels[0] |= 1;
 771         /* 915 MHz BPSK 802.15.4-2003 */
 772         hw->phy->supported.channels[0] |= 0x7fe;
 773         /* 2.4 GHz O-QPSK 802.15.4-2003 */
 774         hw->phy->supported.channels[0] |= 0x7FFF800;
 775         /* 868 MHz ASK 802.15.4-2006 */
 776         hw->phy->supported.channels[1] |= 1;
 777         /* 915 MHz ASK 802.15.4-2006 */
 778         hw->phy->supported.channels[1] |= 0x7fe;
 779         /* 868 MHz O-QPSK 802.15.4-2006 */
 780         hw->phy->supported.channels[2] |= 1;
 781         /* 915 MHz O-QPSK 802.15.4-2006 */
 782         hw->phy->supported.channels[2] |= 0x7fe;
 783         /* 2.4 GHz CSS 802.15.4a-2007 */
 784         hw->phy->supported.channels[3] |= 0x3fff;
 785         /* UWB Sub-gigahertz 802.15.4a-2007 */
 786         hw->phy->supported.channels[4] |= 1;
 787         /* UWB Low band 802.15.4a-2007 */
 788         hw->phy->supported.channels[4] |= 0x1e;
 789         /* UWB High band 802.15.4a-2007 */
 790         hw->phy->supported.channels[4] |= 0xffe0;
 791         /* 750 MHz O-QPSK 802.15.4c-2009 */
 792         hw->phy->supported.channels[5] |= 0xf;
 793         /* 750 MHz MPSK 802.15.4c-2009 */
 794         hw->phy->supported.channels[5] |= 0xf0;
 795         /* 950 MHz BPSK 802.15.4d-2009 */
 796         hw->phy->supported.channels[6] |= 0x3ff;
 797         /* 950 MHz GFSK 802.15.4d-2009 */
 798         hw->phy->supported.channels[6] |= 0x3ffc00;
 799
 800         ieee802154_random_extended_addr(&hw->phy->perm_extended_addr);
 801
 802         /* hwsim phy channel 13 as default */
 803         hw->phy->current_channel = 13;
 804         pib = kzalloc(sizeof(*pib), GFP_KERNEL);
 805         if (!pib) {
 806                 err = -ENOMEM;
 807                 goto err_pib;
 808         }
 809
 810         pib->channel = 13;
 811         rcu_assign_pointer(phy->pib, pib);
 812         phy->idx = idx;
 813         INIT_LIST_HEAD(&phy->edges);
 814
 815         hw->flags = IEEE802154_HW_PROMISCUOUS;
 816         hw->parent = dev;
 817
 818         err = ieee802154_register_hw(hw);
 819         if (err)
 820                 goto err_reg;
 821
 822         mutex_lock(&hwsim_phys_lock);
 823         if (init) {
 824                 err = hwsim_subscribe_all_others(phy);
 825                 if (err < 0) {
 826                         mutex_unlock(&hwsim_phys_lock);
 827                         goto err_subscribe;
 828                 }
 829         }
 830         list_add_tail(&phy->list, &hwsim_phys);
 831         mutex_unlock(&hwsim_phys_lock);
 832
 833         hwsim_mcast_new_radio(info, phy);
 834
 835         return idx;
 836
 837 err_subscribe:
 838         ieee802154_unregister_hw(phy->hw);
 839 err_reg:
 840         kfree(pib);
 841 err_pib:
 842         ieee802154_free_hw(phy->hw);
 843         return err;
 844 }
 845
 846 static void hwsim_del(struct hwsim_phy *phy)
 847 {
 848         struct hwsim_pib *pib;
 849         struct hwsim_edge *e;
 850
 851         hwsim_edge_unsubscribe_me(phy);
 852
 853         list_del(&phy->list);
 854
 855         rcu_read_lock();
 856         list_for_each_entry_rcu(e, &phy->edges, list) {
 857                 list_del_rcu(&e->list);
 858                 hwsim_free_edge(e);
 859         }
 860         pib = rcu_dereference(phy->pib);
 861         rcu_read_unlock();
 862
 863         kfree_rcu(pib, rcu);
 864
 865         ieee802154_unregister_hw(phy->hw);
 866         ieee802154_free_hw(phy->hw);
 867 }
 868
 869 static int hwsim_probe(struct platform_device *pdev)
 870 {
 871         struct hwsim_phy *phy, *tmp;
 872         int err, i;
 873
 874         for (i = 0; i < 2; i++) {
 875                 err = hwsim_add_one(NULL, &pdev->dev, true);
 876                 if (err < 0)
 877                         goto err_slave;
 878         }
 879
 880         dev_info(&pdev->dev, "Added 2 mac802154 hwsim hardware radios\n");
 881         return 0;
 882
 883 err_slave:
 884         mutex_lock(&hwsim_phys_lock);
 885         list_for_each_entry_safe(phy, tmp, &hwsim_phys, list)
 886                 hwsim_del(phy);
 887         mutex_unlock(&hwsim_phys_lock);
 888         return err;
 889 }
 890
 891 static int hwsim_remove(struct platform_device *pdev)
 892 {
 893         struct hwsim_phy *phy, *tmp;
 894
 895         mutex_lock(&hwsim_phys_lock);
 896         list_for_each_entry_safe(phy, tmp, &hwsim_phys, list)
 897                 hwsim_del(phy);
 898         mutex_unlock(&hwsim_phys_lock);
 899
 900         return 0;
 901 }
 902
 903 static struct platform_driver mac802154hwsim_driver = {
 904         .probe = hwsim_probe,
 905         .remove = hwsim_remove,
 906         .driver = {
 907                         .name = "mac802154_hwsim",
 908         },
 909 };
 910
 911 static __init int hwsim_init_module(void)
 912 {
 913         int rc;
 914
 915         rc = genl_register_family(&hwsim_genl_family);
 916         if (rc)
 917                 return rc;
 918
 919         mac802154hwsim_dev = platform_device_register_simple("mac802154_hwsim",
 920                                                              -1, NULL, 0);
 921         if (IS_ERR(mac802154hwsim_dev)) {
 922                 rc = PTR_ERR(mac802154hwsim_dev);
 923                 goto platform_dev;
 924         }
 925
 926         rc = platform_driver_register(&mac802154hwsim_driver);
 927         if (rc < 0)
 928                 goto platform_drv;
 929
 930         return 0;
 931
 932 platform_drv:
 933         platform_device_unregister(mac802154hwsim_dev);
 934 platform_dev:
 935         genl_unregister_family(&hwsim_genl_family);
 936         return rc;
 937 }
 938
 939 static __exit void hwsim_remove_module(void)
 940 {
 941         genl_unregister_family(&hwsim_genl_family);
 942         platform_driver_unregister(&mac802154hwsim_driver);
 943         platform_device_unregister(mac802154hwsim_dev);
 944 }
 945
 946 module_init(hwsim_init_module);
 947 module_exit(hwsim_remove_module);