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