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