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