drivers/net/ieee802154/mac802154_hwsim.c

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