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