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