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_noflag(skb,
 231                                          MAC802154_HWSIM_ATTR_RADIO_EDGES);
 232         if (!nl_edges) {
 233                 rcu_read_unlock();
 234                 return -ENOBUFS;
 235         }
 236
 237         list_for_each_entry_rcu(e, &phy->edges, list) {
 238                 nl_edge = nla_nest_start_noflag(skb,
 239                                                 MAC802154_HWSIM_ATTR_RADIO_EDGE);
 240                 if (!nl_edge) {
 241                         rcu_read_unlock();
 242                         nla_nest_cancel(skb, nl_edges);
 243                         return -ENOBUFS;
 244                 }
 245
 246                 ret = nla_put_u32(skb, MAC802154_HWSIM_EDGE_ATTR_ENDPOINT_ID,
 247                                   e->endpoint->idx);
 248                 if (ret < 0) {
 249                         rcu_read_unlock();
 250                         nla_nest_cancel(skb, nl_edge);
 251                         nla_nest_cancel(skb, nl_edges);
 252                         return ret;
 253                 }
 254
 255                 einfo = rcu_dereference(e->info);
 256                 ret = nla_put_u8(skb, MAC802154_HWSIM_EDGE_ATTR_LQI,
 257                                  einfo->lqi);
 258                 if (ret < 0) {
 259                         rcu_read_unlock();
 260                         nla_nest_cancel(skb, nl_edge);
 261                         nla_nest_cancel(skb, nl_edges);
 262                         return ret;
 263                 }
 264
 265                 nla_nest_end(skb, nl_edge);
 266         }
 267         rcu_read_unlock();
 268
 269         nla_nest_end(skb, nl_edges);
 270
 271         return 0;
 272 }
 273
 274 static int hwsim_get_radio(struct sk_buff *skb, struct hwsim_phy *phy,
 275                            u32 portid, u32 seq,
 276                            struct netlink_callback *cb, int flags)
 277 {
 278         void *hdr;
 279         int res = -EMSGSIZE;
 280
 281         hdr = genlmsg_put(skb, portid, seq, &hwsim_genl_family, flags,
 282                           MAC802154_HWSIM_CMD_GET_RADIO);
 283         if (!hdr)
 284                 return -EMSGSIZE;
 285
 286         if (cb)
 287                 genl_dump_check_consistent(cb, hdr);
 288
 289         res = append_radio_msg(skb, phy);
 290         if (res < 0)
 291                 goto out_err;
 292
 293         genlmsg_end(skb, hdr);
 294         return 0;
 295
 296 out_err:
 297         genlmsg_cancel(skb, hdr);
 298         return res;
 299 }
 300
 301 static int hwsim_get_radio_nl(struct sk_buff *msg, struct genl_info *info)
 302 {
 303         struct hwsim_phy *phy;
 304         struct sk_buff *skb;
 305         int idx, res = -ENODEV;
 306
 307         if (!info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID])
 308                 return -EINVAL;
 309         idx = nla_get_u32(info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID]);
 310
 311         mutex_lock(&hwsim_phys_lock);
 312         list_for_each_entry(phy, &hwsim_phys, list) {
 313                 if (phy->idx != idx)
 314                         continue;
 315
 316                 skb = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
 317                 if (!skb) {
 318                         res = -ENOMEM;
 319                         goto out_err;
 320                 }
 321
 322                 res = hwsim_get_radio(skb, phy, info->snd_portid,
 323                                       info->snd_seq, NULL, 0);
 324                 if (res < 0) {
 325                         nlmsg_free(skb);
 326                         goto out_err;
 327                 }
 328
 329                 res = genlmsg_reply(skb, info);
 330                 break;
 331         }
 332
 333 out_err:
 334         mutex_unlock(&hwsim_phys_lock);
 335
 336         return res;
 337 }
 338
 339 static int hwsim_dump_radio_nl(struct sk_buff *skb,
 340                                struct netlink_callback *cb)
 341 {
 342         int idx = cb->args[0];
 343         struct hwsim_phy *phy;
 344         int res;
 345
 346         mutex_lock(&hwsim_phys_lock);
 347
 348         if (idx == hwsim_radio_idx)
 349                 goto done;
 350
 351         list_for_each_entry(phy, &hwsim_phys, list) {
 352                 if (phy->idx < idx)
 353                         continue;
 354
 355                 res = hwsim_get_radio(skb, phy, NETLINK_CB(cb->skb).portid,
 356                                       cb->nlh->nlmsg_seq, cb, NLM_F_MULTI);
 357                 if (res < 0)
 358                         break;
 359
 360                 idx = phy->idx + 1;
 361         }
 362
 363         cb->args[0] = idx;
 364
 365 done:
 366         mutex_unlock(&hwsim_phys_lock);
 367         return skb->len;
 368 }
 369
 370 /* caller need to held hwsim_phys_lock */
 371 static struct hwsim_phy *hwsim_get_radio_by_id(uint32_t idx)
 372 {
 373         struct hwsim_phy *phy;
 374
 375         list_for_each_entry(phy, &hwsim_phys, list) {
 376                 if (phy->idx == idx)
 377                         return phy;
 378         }
 379
 380         return NULL;
 381 }
 382
 383 static const struct nla_policy hwsim_edge_policy[MAC802154_HWSIM_EDGE_ATTR_MAX + 1] = {
 384         [MAC802154_HWSIM_EDGE_ATTR_ENDPOINT_ID] = { .type = NLA_U32 },
 385         [MAC802154_HWSIM_EDGE_ATTR_LQI] = { .type = NLA_U8 },
 386 };
 387
 388 static struct hwsim_edge *hwsim_alloc_edge(struct hwsim_phy *endpoint, u8 lqi)
 389 {
 390         struct hwsim_edge_info *einfo;
 391         struct hwsim_edge *e;
 392
 393         e = kzalloc(sizeof(*e), GFP_KERNEL);
 394         if (!e)
 395                 return NULL;
 396
 397         einfo = kzalloc(sizeof(*einfo), GFP_KERNEL);
 398         if (!einfo) {
 399                 kfree(e);
 400                 return NULL;
 401         }
 402
 403         einfo->lqi = 0xff;
 404         rcu_assign_pointer(e->info, einfo);
 405         e->endpoint = endpoint;
 406
 407         return e;
 408 }
 409
 410 static void hwsim_free_edge(struct hwsim_edge *e)
 411 {
 412         struct hwsim_edge_info *einfo;
 413
 414         rcu_read_lock();
 415         einfo = rcu_dereference(e->info);
 416         rcu_read_unlock();
 417
 418         kfree_rcu(einfo, rcu);
 419         kfree_rcu(e, rcu);
 420 }
 421
 422 static int hwsim_new_edge_nl(struct sk_buff *msg, struct genl_info *info)
 423 {
 424         struct nlattr *edge_attrs[MAC802154_HWSIM_EDGE_ATTR_MAX + 1];
 425         struct hwsim_phy *phy_v0, *phy_v1;
 426         struct hwsim_edge *e;
 427         u32 v0, v1;
 428
 429         if (!info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID] &&
 430             !info->attrs[MAC802154_HWSIM_ATTR_RADIO_EDGE])
 431                 return -EINVAL;
 432
 433         if (nla_parse_nested_deprecated(edge_attrs, MAC802154_HWSIM_EDGE_ATTR_MAX, info->attrs[MAC802154_HWSIM_ATTR_RADIO_EDGE], 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_deprecated(edge_attrs, MAC802154_HWSIM_EDGE_ATTR_MAX, info->attrs[MAC802154_HWSIM_ATTR_RADIO_EDGE], hwsim_edge_policy, NULL))
 496                 return -EINVAL;
 497
 498         if (!edge_attrs[MAC802154_HWSIM_EDGE_ATTR_ENDPOINT_ID])
 499                 return -EINVAL;
 500
 501         v0 = nla_get_u32(info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID]);
 502         v1 = nla_get_u32(edge_attrs[MAC802154_HWSIM_EDGE_ATTR_ENDPOINT_ID]);
 503
 504         mutex_lock(&hwsim_phys_lock);
 505         phy_v0 = hwsim_get_radio_by_id(v0);
 506         if (!phy_v0) {
 507                 mutex_unlock(&hwsim_phys_lock);
 508                 return -ENOENT;
 509         }
 510
 511         rcu_read_lock();
 512         list_for_each_entry_rcu(e, &phy_v0->edges, list) {
 513                 if (e->endpoint->idx == v1) {
 514                         rcu_read_unlock();
 515                         list_del_rcu(&e->list);
 516                         hwsim_free_edge(e);
 517                         /* same again - wait until list changes are done */
 518                         synchronize_rcu();
 519                         mutex_unlock(&hwsim_phys_lock);
 520                         return 0;
 521                 }
 522         }
 523         rcu_read_unlock();
 524
 525         mutex_unlock(&hwsim_phys_lock);
 526
 527         return -ENOENT;
 528 }
 529
 530 static int hwsim_set_edge_lqi(struct sk_buff *msg, struct genl_info *info)
 531 {
 532         struct nlattr *edge_attrs[MAC802154_HWSIM_EDGE_ATTR_MAX + 1];
 533         struct hwsim_edge_info *einfo;
 534         struct hwsim_phy *phy_v0;
 535         struct hwsim_edge *e;
 536         u32 v0, v1;
 537         u8 lqi;
 538
 539         if (!info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID] &&
 540             !info->attrs[MAC802154_HWSIM_ATTR_RADIO_EDGE])
 541                 return -EINVAL;
 542
 543         if (nla_parse_nested_deprecated(edge_attrs, MAC802154_HWSIM_EDGE_ATTR_MAX, info->attrs[MAC802154_HWSIM_ATTR_RADIO_EDGE], hwsim_edge_policy, NULL))
 544                 return -EINVAL;
 545
 546         if (!edge_attrs[MAC802154_HWSIM_EDGE_ATTR_ENDPOINT_ID] &&
 547             !edge_attrs[MAC802154_HWSIM_EDGE_ATTR_LQI])
 548                 return -EINVAL;
 549
 550         v0 = nla_get_u32(info->attrs[MAC802154_HWSIM_ATTR_RADIO_ID]);
 551         v1 = nla_get_u32(edge_attrs[MAC802154_HWSIM_EDGE_ATTR_ENDPOINT_ID]);
 552         lqi = nla_get_u8(edge_attrs[MAC802154_HWSIM_EDGE_ATTR_LQI]);
 553
 554         mutex_lock(&hwsim_phys_lock);
 555         phy_v0 = hwsim_get_radio_by_id(v0);
 556         if (!phy_v0) {
 557                 mutex_unlock(&hwsim_phys_lock);
 558                 return -ENOENT;
 559         }
 560
 561         einfo = kzalloc(sizeof(*einfo), GFP_KERNEL);
 562         if (!einfo) {
 563                 mutex_unlock(&hwsim_phys_lock);
 564                 return -ENOMEM;
 565         }
 566
 567         rcu_read_lock();
 568         list_for_each_entry_rcu(e, &phy_v0->edges, list) {
 569                 if (e->endpoint->idx == v1) {
 570                         einfo->lqi = lqi;
 571                         rcu_assign_pointer(e->info, einfo);
 572                         rcu_read_unlock();
 573                         mutex_unlock(&hwsim_phys_lock);
 574                         return 0;
 575                 }
 576         }
 577         rcu_read_unlock();
 578
 579         kfree(einfo);
 580         mutex_unlock(&hwsim_phys_lock);
 581
 582         return -ENOENT;
 583 }
 584
 585 /* MAC802154_HWSIM netlink policy */
 586
 587 static const struct nla_policy hwsim_genl_policy[MAC802154_HWSIM_ATTR_MAX + 1] = {
 588         [MAC802154_HWSIM_ATTR_RADIO_ID] = { .type = NLA_U32 },
 589         [MAC802154_HWSIM_ATTR_RADIO_EDGE] = { .type = NLA_NESTED },
 590         [MAC802154_HWSIM_ATTR_RADIO_EDGES] = { .type = NLA_NESTED },
 591 };
 592
 593 /* Generic Netlink operations array */
 594 static const struct genl_ops hwsim_nl_ops[] = {
 595         {
 596                 .cmd = MAC802154_HWSIM_CMD_NEW_RADIO,
 597                 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
 598                 .doit = hwsim_new_radio_nl,
 599                 .flags = GENL_UNS_ADMIN_PERM,
 600         },
 601         {
 602                 .cmd = MAC802154_HWSIM_CMD_DEL_RADIO,
 603                 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
 604                 .doit = hwsim_del_radio_nl,
 605                 .flags = GENL_UNS_ADMIN_PERM,
 606         },
 607         {
 608                 .cmd = MAC802154_HWSIM_CMD_GET_RADIO,
 609                 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
 610                 .doit = hwsim_get_radio_nl,
 611                 .dumpit = hwsim_dump_radio_nl,
 612         },
 613         {
 614                 .cmd = MAC802154_HWSIM_CMD_NEW_EDGE,
 615                 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
 616                 .doit = hwsim_new_edge_nl,
 617                 .flags = GENL_UNS_ADMIN_PERM,
 618         },
 619         {
 620                 .cmd = MAC802154_HWSIM_CMD_DEL_EDGE,
 621                 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
 622                 .doit = hwsim_del_edge_nl,
 623                 .flags = GENL_UNS_ADMIN_PERM,
 624         },
 625         {
 626                 .cmd = MAC802154_HWSIM_CMD_SET_EDGE,
 627                 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
 628                 .doit = hwsim_set_edge_lqi,
 629                 .flags = GENL_UNS_ADMIN_PERM,
 630         },
 631 };
 632
 633 static struct genl_family hwsim_genl_family __ro_after_init = {
 634         .name = "MAC802154_HWSIM",
 635         .version = 1,
 636         .maxattr = MAC802154_HWSIM_ATTR_MAX,
 637         .policy = hwsim_genl_policy,
 638         .module = THIS_MODULE,
 639         .ops = hwsim_nl_ops,
 640         .n_ops = ARRAY_SIZE(hwsim_nl_ops),
 641         .mcgrps = hwsim_mcgrps,
 642         .n_mcgrps = ARRAY_SIZE(hwsim_mcgrps),
 643 };
 644
 645 static void hwsim_mcast_config_msg(struct sk_buff *mcast_skb,
 646                                    struct genl_info *info)
 647 {
 648         if (info)
 649                 genl_notify(&hwsim_genl_family, mcast_skb, info,
 650                             HWSIM_MCGRP_CONFIG, GFP_KERNEL);
 651         else
 652                 genlmsg_multicast(&hwsim_genl_family, mcast_skb, 0,
 653                                   HWSIM_MCGRP_CONFIG, GFP_KERNEL);
 654 }
 655
 656 static void hwsim_mcast_new_radio(struct genl_info *info, struct hwsim_phy *phy)
 657 {
 658         struct sk_buff *mcast_skb;
 659         void *data;
 660
 661         mcast_skb = genlmsg_new(GENLMSG_DEFAULT_SIZE, GFP_KERNEL);
 662         if (!mcast_skb)
 663                 return;
 664
 665         data = genlmsg_put(mcast_skb, 0, 0, &hwsim_genl_family, 0,
 666                            MAC802154_HWSIM_CMD_NEW_RADIO);
 667         if (!data)
 668                 goto out_err;
 669
 670         if (append_radio_msg(mcast_skb, phy) < 0)
 671                 goto out_err;
 672
 673         genlmsg_end(mcast_skb, data);
 674
 675         hwsim_mcast_config_msg(mcast_skb, info);
 676         return;
 677
 678 out_err:
 679         genlmsg_cancel(mcast_skb, data);
 680         nlmsg_free(mcast_skb);
 681 }
 682
 683 static void hwsim_edge_unsubscribe_me(struct hwsim_phy *phy)
 684 {
 685         struct hwsim_phy *tmp;
 686         struct hwsim_edge *e;
 687
 688         rcu_read_lock();
 689         /* going to all phy edges and remove phy from it */
 690         list_for_each_entry(tmp, &hwsim_phys, list) {
 691                 list_for_each_entry_rcu(e, &tmp->edges, list) {
 692                         if (e->endpoint->idx == phy->idx) {
 693                                 list_del_rcu(&e->list);
 694                                 hwsim_free_edge(e);
 695                         }
 696                 }
 697         }
 698         rcu_read_unlock();
 699
 700         synchronize_rcu();
 701 }
 702
 703 static int hwsim_subscribe_all_others(struct hwsim_phy *phy)
 704 {
 705         struct hwsim_phy *sub;
 706         struct hwsim_edge *e;
 707
 708         list_for_each_entry(sub, &hwsim_phys, list) {
 709                 e = hwsim_alloc_edge(sub, 0xff);
 710                 if (!e)
 711                         goto me_fail;
 712
 713                 list_add_rcu(&e->list, &phy->edges);
 714         }
 715
 716         list_for_each_entry(sub, &hwsim_phys, list) {
 717                 e = hwsim_alloc_edge(phy, 0xff);
 718                 if (!e)
 719                         goto sub_fail;
 720
 721                 list_add_rcu(&e->list, &sub->edges);
 722         }
 723
 724         return 0;
 725
 726 me_fail:
 727         rcu_read_lock();
 728         list_for_each_entry_rcu(e, &phy->edges, list) {
 729                 list_del_rcu(&e->list);
 730                 hwsim_free_edge(e);
 731         }
 732         rcu_read_unlock();
 733 sub_fail:
 734         hwsim_edge_unsubscribe_me(phy);
 735         return -ENOMEM;
 736 }
 737
 738 static int hwsim_add_one(struct genl_info *info, struct device *dev,
 739                          bool init)
 740 {
 741         struct ieee802154_hw *hw;
 742         struct hwsim_phy *phy;
 743         struct hwsim_pib *pib;
 744         int idx;
 745         int err;
 746
 747         idx = hwsim_radio_idx++;
 748
 749         hw = ieee802154_alloc_hw(sizeof(*phy), &hwsim_ops);
 750         if (!hw)
 751                 return -ENOMEM;
 752
 753         phy = hw->priv;
 754         phy->hw = hw;
 755
 756         /* 868 MHz BPSK 802.15.4-2003 */
 757         hw->phy->supported.channels[0] |= 1;
 758         /* 915 MHz BPSK 802.15.4-2003 */
 759         hw->phy->supported.channels[0] |= 0x7fe;
 760         /* 2.4 GHz O-QPSK 802.15.4-2003 */
 761         hw->phy->supported.channels[0] |= 0x7FFF800;
 762         /* 868 MHz ASK 802.15.4-2006 */
 763         hw->phy->supported.channels[1] |= 1;
 764         /* 915 MHz ASK 802.15.4-2006 */
 765         hw->phy->supported.channels[1] |= 0x7fe;
 766         /* 868 MHz O-QPSK 802.15.4-2006 */
 767         hw->phy->supported.channels[2] |= 1;
 768         /* 915 MHz O-QPSK 802.15.4-2006 */
 769         hw->phy->supported.channels[2] |= 0x7fe;
 770         /* 2.4 GHz CSS 802.15.4a-2007 */
 771         hw->phy->supported.channels[3] |= 0x3fff;
 772         /* UWB Sub-gigahertz 802.15.4a-2007 */
 773         hw->phy->supported.channels[4] |= 1;
 774         /* UWB Low band 802.15.4a-2007 */
 775         hw->phy->supported.channels[4] |= 0x1e;
 776         /* UWB High band 802.15.4a-2007 */
 777         hw->phy->supported.channels[4] |= 0xffe0;
 778         /* 750 MHz O-QPSK 802.15.4c-2009 */
 779         hw->phy->supported.channels[5] |= 0xf;
 780         /* 750 MHz MPSK 802.15.4c-2009 */
 781         hw->phy->supported.channels[5] |= 0xf0;
 782         /* 950 MHz BPSK 802.15.4d-2009 */
 783         hw->phy->supported.channels[6] |= 0x3ff;
 784         /* 950 MHz GFSK 802.15.4d-2009 */
 785         hw->phy->supported.channels[6] |= 0x3ffc00;
 786
 787         ieee802154_random_extended_addr(&hw->phy->perm_extended_addr);
 788
 789         /* hwsim phy channel 13 as default */
 790         hw->phy->current_channel = 13;
 791         pib = kzalloc(sizeof(*pib), GFP_KERNEL);
 792         if (!pib) {
 793                 err = -ENOMEM;
 794                 goto err_pib;
 795         }
 796
 797         rcu_assign_pointer(phy->pib, pib);
 798         phy->idx = idx;
 799         INIT_LIST_HEAD(&phy->edges);
 800
 801         hw->flags = IEEE802154_HW_PROMISCUOUS;
 802         hw->parent = dev;
 803
 804         err = ieee802154_register_hw(hw);
 805         if (err)
 806                 goto err_reg;
 807
 808         mutex_lock(&hwsim_phys_lock);
 809         if (init) {
 810                 err = hwsim_subscribe_all_others(phy);
 811                 if (err < 0) {
 812                         mutex_unlock(&hwsim_phys_lock);
 813                         goto err_reg;
 814                 }
 815         }
 816         list_add_tail(&phy->list, &hwsim_phys);
 817         mutex_unlock(&hwsim_phys_lock);
 818
 819         hwsim_mcast_new_radio(info, phy);
 820
 821         return idx;
 822
 823 err_reg:
 824         kfree(pib);
 825 err_pib:
 826         ieee802154_free_hw(phy->hw);
 827         return err;
 828 }
 829
 830 static void hwsim_del(struct hwsim_phy *phy)
 831 {
 832         struct hwsim_pib *pib;
 833
 834         hwsim_edge_unsubscribe_me(phy);
 835
 836         list_del(&phy->list);
 837
 838         rcu_read_lock();
 839         pib = rcu_dereference(phy->pib);
 840         rcu_read_unlock();
 841
 842         kfree_rcu(pib, rcu);
 843
 844         ieee802154_unregister_hw(phy->hw);
 845         ieee802154_free_hw(phy->hw);
 846 }
 847
 848 static int hwsim_probe(struct platform_device *pdev)
 849 {
 850         struct hwsim_phy *phy, *tmp;
 851         int err, i;
 852
 853         for (i = 0; i < 2; i++) {
 854                 err = hwsim_add_one(NULL, &pdev->dev, true);
 855                 if (err < 0)
 856                         goto err_slave;
 857         }
 858
 859         dev_info(&pdev->dev, "Added 2 mac802154 hwsim hardware radios\n");
 860         return 0;
 861
 862 err_slave:
 863         mutex_lock(&hwsim_phys_lock);
 864         list_for_each_entry_safe(phy, tmp, &hwsim_phys, list)
 865                 hwsim_del(phy);
 866         mutex_unlock(&hwsim_phys_lock);
 867         return err;
 868 }
 869
 870 static int hwsim_remove(struct platform_device *pdev)
 871 {
 872         struct hwsim_phy *phy, *tmp;
 873
 874         mutex_lock(&hwsim_phys_lock);
 875         list_for_each_entry_safe(phy, tmp, &hwsim_phys, list)
 876                 hwsim_del(phy);
 877         mutex_unlock(&hwsim_phys_lock);
 878
 879         return 0;
 880 }
 881
 882 static struct platform_driver mac802154hwsim_driver = {
 883         .probe = hwsim_probe,
 884         .remove = hwsim_remove,
 885         .driver = {
 886                         .name = "mac802154_hwsim",
 887         },
 888 };
 889
 890 static __init int hwsim_init_module(void)
 891 {
 892         int rc;
 893
 894         rc = genl_register_family(&hwsim_genl_family);
 895         if (rc)
 896                 return rc;
 897
 898         mac802154hwsim_dev = platform_device_register_simple("mac802154_hwsim",
 899                                                              -1, NULL, 0);
 900         if (IS_ERR(mac802154hwsim_dev)) {
 901                 rc = PTR_ERR(mac802154hwsim_dev);
 902                 goto platform_dev;
 903         }
 904
 905         rc = platform_driver_register(&mac802154hwsim_driver);
 906         if (rc < 0)
 907                 goto platform_drv;
 908
 909         return 0;
 910
 911 platform_drv:
 912         genl_unregister_family(&hwsim_genl_family);
 913 platform_dev:
 914         platform_device_unregister(mac802154hwsim_dev);
 915         return rc;
 916 }
 917
 918 static __exit void hwsim_remove_module(void)
 919 {
 920         genl_unregister_family(&hwsim_genl_family);
 921         platform_driver_unregister(&mac802154hwsim_driver);
 922         platform_device_unregister(mac802154hwsim_dev);
 923 }
 924
 925 module_init(hwsim_init_module);
 926 module_exit(hwsim_remove_module);