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