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