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 26MODULE_DESCRIPTION("Software simulator of IEEE 802.15.4 radio(s) for mac802154"); 27MODULE_LICENSE("GPL"); 28 29static LIST_HEAD(hwsim_phys); 30static DEFINE_MUTEX(hwsim_phys_lock); 31 32static struct platform_device *mac802154hwsim_dev; 33 34/* MAC802154_HWSIM netlink family */ 35static struct genl_family hwsim_genl_family; 36 37static int hwsim_radio_idx; 38 39enum hwsim_multicast_groups { 40 HWSIM_MCGRP_CONFIG, 41}; 42 43static const struct genl_multicast_group hwsim_mcgrps[] = { 44 [HWSIM_MCGRP_CONFIG] = { .name = "config", }, 45}; 46 47struct hwsim_pib { 48 u8 page; 49 u8 channel; 50 51 struct rcu_head rcu; 52}; 53 54struct hwsim_edge_info { 55 u8 lqi; 56 57 struct rcu_head rcu; 58}; 59 60struct 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 68struct 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 80static int hwsim_add_one(struct genl_info *info, struct device *dev, 81 bool init); 82static void hwsim_del(struct hwsim_phy *phy); 83 84static int hwsim_hw_ed(struct ieee802154_hw *hw, u8 *level) 85{ 86 *level = 0xbe; 87 88 return 0; 89} 90 91static 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 109static 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 146static 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 154static void hwsim_hw_stop(struct ieee802154_hw *hw) 155{ 156 struct hwsim_phy *phy = hw->priv; 157 158 phy->suspended = true; 159} 160 161static int 162hwsim_set_promiscuous_mode(struct ieee802154_hw *hw, const bool on) 163{ 164 return 0; 165} 166 167static 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 177static 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 182static 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 205static 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 266static 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 = -EMSGSIZE; 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 288out_err: 289 genlmsg_cancel(skb, hdr); 290 return res; 291} 292 293static 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 325out_err: 326 mutex_unlock(&hwsim_phys_lock); 327 328 return res; 329} 330 331static 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 357done: 358 mutex_unlock(&hwsim_phys_lock); 359 return skb->len; 360} 361 362/* caller need to held hwsim_phys_lock */ 363static 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 375static 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 380static 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 402static 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 414static 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 476static 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 522static 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, *einfo_old; 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 einfo_old = rcu_replace_pointer(e->info, einfo, 564 lockdep_is_held(&hwsim_phys_lock)); 565 rcu_read_unlock(); 566 kfree_rcu(einfo_old, rcu); 567 mutex_unlock(&hwsim_phys_lock); 568 return 0; 569 } 570 } 571 rcu_read_unlock(); 572 573 kfree(einfo); 574 mutex_unlock(&hwsim_phys_lock); 575 576 return -ENOENT; 577} 578 579/* MAC802154_HWSIM netlink policy */ 580 581static const struct nla_policy hwsim_genl_policy[MAC802154_HWSIM_ATTR_MAX + 1] = { 582 [MAC802154_HWSIM_ATTR_RADIO_ID] = { .type = NLA_U32 }, 583 [MAC802154_HWSIM_ATTR_RADIO_EDGE] = { .type = NLA_NESTED }, 584 [MAC802154_HWSIM_ATTR_RADIO_EDGES] = { .type = NLA_NESTED }, 585}; 586 587/* Generic Netlink operations array */ 588static const struct genl_small_ops hwsim_nl_ops[] = { 589 { 590 .cmd = MAC802154_HWSIM_CMD_NEW_RADIO, 591 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP, 592 .doit = hwsim_new_radio_nl, 593 .flags = GENL_UNS_ADMIN_PERM, 594 }, 595 { 596 .cmd = MAC802154_HWSIM_CMD_DEL_RADIO, 597 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP, 598 .doit = hwsim_del_radio_nl, 599 .flags = GENL_UNS_ADMIN_PERM, 600 }, 601 { 602 .cmd = MAC802154_HWSIM_CMD_GET_RADIO, 603 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP, 604 .doit = hwsim_get_radio_nl, 605 .dumpit = hwsim_dump_radio_nl, 606 }, 607 { 608 .cmd = MAC802154_HWSIM_CMD_NEW_EDGE, 609 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP, 610 .doit = hwsim_new_edge_nl, 611 .flags = GENL_UNS_ADMIN_PERM, 612 }, 613 { 614 .cmd = MAC802154_HWSIM_CMD_DEL_EDGE, 615 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP, 616 .doit = hwsim_del_edge_nl, 617 .flags = GENL_UNS_ADMIN_PERM, 618 }, 619 { 620 .cmd = MAC802154_HWSIM_CMD_SET_EDGE, 621 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP, 622 .doit = hwsim_set_edge_lqi, 623 .flags = GENL_UNS_ADMIN_PERM, 624 }, 625}; 626 627static struct genl_family hwsim_genl_family __ro_after_init = { 628 .name = "MAC802154_HWSIM", 629 .version = 1, 630 .maxattr = MAC802154_HWSIM_ATTR_MAX, 631 .policy = hwsim_genl_policy, 632 .module = THIS_MODULE, 633 .small_ops = hwsim_nl_ops, 634 .n_small_ops = ARRAY_SIZE(hwsim_nl_ops), 635 .mcgrps = hwsim_mcgrps, 636 .n_mcgrps = ARRAY_SIZE(hwsim_mcgrps), 637}; 638 639static void hwsim_mcast_config_msg(struct sk_buff *mcast_skb, 640 struct genl_info *info) 641{ 642 if (info) 643 genl_notify(&hwsim_genl_family, mcast_skb, info, 644 HWSIM_MCGRP_CONFIG, GFP_KERNEL); 645 else 646 genlmsg_multicast(&hwsim_genl_family, mcast_skb, 0, 647 HWSIM_MCGRP_CONFIG, GFP_KERNEL); 648} 649 650static void hwsim_mcast_new_radio(struct genl_info *info, struct hwsim_phy *phy) 651{ 652 struct sk_buff *mcast_skb; 653 void *data; 654 655 mcast_skb = genlmsg_new(GENLMSG_DEFAULT_SIZE, GFP_KERNEL); 656 if (!mcast_skb) 657 return; 658 659 data = genlmsg_put(mcast_skb, 0, 0, &hwsim_genl_family, 0, 660 MAC802154_HWSIM_CMD_NEW_RADIO); 661 if (!data) 662 goto out_err; 663 664 if (append_radio_msg(mcast_skb, phy) < 0) 665 goto out_err; 666 667 genlmsg_end(mcast_skb, data); 668 669 hwsim_mcast_config_msg(mcast_skb, info); 670 return; 671 672out_err: 673 genlmsg_cancel(mcast_skb, data); 674 nlmsg_free(mcast_skb); 675} 676 677static void hwsim_edge_unsubscribe_me(struct hwsim_phy *phy) 678{ 679 struct hwsim_phy *tmp; 680 struct hwsim_edge *e; 681 682 rcu_read_lock(); 683 /* going to all phy edges and remove phy from it */ 684 list_for_each_entry(tmp, &hwsim_phys, list) { 685 list_for_each_entry_rcu(e, &tmp->edges, list) { 686 if (e->endpoint->idx == phy->idx) { 687 list_del_rcu(&e->list); 688 hwsim_free_edge(e); 689 } 690 } 691 } 692 rcu_read_unlock(); 693 694 synchronize_rcu(); 695} 696 697static int hwsim_subscribe_all_others(struct hwsim_phy *phy) 698{ 699 struct hwsim_phy *sub; 700 struct hwsim_edge *e; 701 702 list_for_each_entry(sub, &hwsim_phys, list) { 703 e = hwsim_alloc_edge(sub, 0xff); 704 if (!e) 705 goto me_fail; 706 707 list_add_rcu(&e->list, &phy->edges); 708 } 709 710 list_for_each_entry(sub, &hwsim_phys, list) { 711 e = hwsim_alloc_edge(phy, 0xff); 712 if (!e) 713 goto sub_fail; 714 715 list_add_rcu(&e->list, &sub->edges); 716 } 717 718 return 0; 719 720sub_fail: 721 hwsim_edge_unsubscribe_me(phy); 722me_fail: 723 rcu_read_lock(); 724 list_for_each_entry_rcu(e, &phy->edges, list) { 725 list_del_rcu(&e->list); 726 hwsim_free_edge(e); 727 } 728 rcu_read_unlock(); 729 return -ENOMEM; 730} 731 732static int hwsim_add_one(struct genl_info *info, struct device *dev, 733 bool init) 734{ 735 struct ieee802154_hw *hw; 736 struct hwsim_phy *phy; 737 struct hwsim_pib *pib; 738 int idx; 739 int err; 740 741 idx = hwsim_radio_idx++; 742 743 hw = ieee802154_alloc_hw(sizeof(*phy), &hwsim_ops); 744 if (!hw) 745 return -ENOMEM; 746 747 phy = hw->priv; 748 phy->hw = hw; 749 750 /* 868 MHz BPSK 802.15.4-2003 */ 751 hw->phy->supported.channels[0] |= 1; 752 /* 915 MHz BPSK 802.15.4-2003 */ 753 hw->phy->supported.channels[0] |= 0x7fe; 754 /* 2.4 GHz O-QPSK 802.15.4-2003 */ 755 hw->phy->supported.channels[0] |= 0x7FFF800; 756 /* 868 MHz ASK 802.15.4-2006 */ 757 hw->phy->supported.channels[1] |= 1; 758 /* 915 MHz ASK 802.15.4-2006 */ 759 hw->phy->supported.channels[1] |= 0x7fe; 760 /* 868 MHz O-QPSK 802.15.4-2006 */ 761 hw->phy->supported.channels[2] |= 1; 762 /* 915 MHz O-QPSK 802.15.4-2006 */ 763 hw->phy->supported.channels[2] |= 0x7fe; 764 /* 2.4 GHz CSS 802.15.4a-2007 */ 765 hw->phy->supported.channels[3] |= 0x3fff; 766 /* UWB Sub-gigahertz 802.15.4a-2007 */ 767 hw->phy->supported.channels[4] |= 1; 768 /* UWB Low band 802.15.4a-2007 */ 769 hw->phy->supported.channels[4] |= 0x1e; 770 /* UWB High band 802.15.4a-2007 */ 771 hw->phy->supported.channels[4] |= 0xffe0; 772 /* 750 MHz O-QPSK 802.15.4c-2009 */ 773 hw->phy->supported.channels[5] |= 0xf; 774 /* 750 MHz MPSK 802.15.4c-2009 */ 775 hw->phy->supported.channels[5] |= 0xf0; 776 /* 950 MHz BPSK 802.15.4d-2009 */ 777 hw->phy->supported.channels[6] |= 0x3ff; 778 /* 950 MHz GFSK 802.15.4d-2009 */ 779 hw->phy->supported.channels[6] |= 0x3ffc00; 780 781 ieee802154_random_extended_addr(&hw->phy->perm_extended_addr); 782 783 /* hwsim phy channel 13 as default */ 784 hw->phy->current_channel = 13; 785 pib = kzalloc(sizeof(*pib), GFP_KERNEL); 786 if (!pib) { 787 err = -ENOMEM; 788 goto err_pib; 789 } 790 791 pib->channel = 13; 792 rcu_assign_pointer(phy->pib, pib); 793 phy->idx = idx; 794 INIT_LIST_HEAD(&phy->edges); 795 796 hw->flags = IEEE802154_HW_PROMISCUOUS; 797 hw->parent = dev; 798 799 err = ieee802154_register_hw(hw); 800 if (err) 801 goto err_reg; 802 803 mutex_lock(&hwsim_phys_lock); 804 if (init) { 805 err = hwsim_subscribe_all_others(phy); 806 if (err < 0) { 807 mutex_unlock(&hwsim_phys_lock); 808 goto err_subscribe; 809 } 810 } 811 list_add_tail(&phy->list, &hwsim_phys); 812 mutex_unlock(&hwsim_phys_lock); 813 814 hwsim_mcast_new_radio(info, phy); 815 816 return idx; 817 818err_subscribe: 819 ieee802154_unregister_hw(phy->hw); 820err_reg: 821 kfree(pib); 822err_pib: 823 ieee802154_free_hw(phy->hw); 824 return err; 825} 826 827static void hwsim_del(struct hwsim_phy *phy) 828{ 829 struct hwsim_pib *pib; 830 struct hwsim_edge *e; 831 832 hwsim_edge_unsubscribe_me(phy); 833 834 list_del(&phy->list); 835 836 rcu_read_lock(); 837 list_for_each_entry_rcu(e, &phy->edges, list) { 838 list_del_rcu(&e->list); 839 hwsim_free_edge(e); 840 } 841 pib = rcu_dereference(phy->pib); 842 rcu_read_unlock(); 843 844 kfree_rcu(pib, rcu); 845 846 ieee802154_unregister_hw(phy->hw); 847 ieee802154_free_hw(phy->hw); 848} 849 850static int hwsim_probe(struct platform_device *pdev) 851{ 852 struct hwsim_phy *phy, *tmp; 853 int err, i; 854 855 for (i = 0; i < 2; i++) { 856 err = hwsim_add_one(NULL, &pdev->dev, true); 857 if (err < 0) 858 goto err_slave; 859 } 860 861 dev_info(&pdev->dev, "Added 2 mac802154 hwsim hardware radios\n"); 862 return 0; 863 864err_slave: 865 mutex_lock(&hwsim_phys_lock); 866 list_for_each_entry_safe(phy, tmp, &hwsim_phys, list) 867 hwsim_del(phy); 868 mutex_unlock(&hwsim_phys_lock); 869 return err; 870} 871 872static int hwsim_remove(struct platform_device *pdev) 873{ 874 struct hwsim_phy *phy, *tmp; 875 876 mutex_lock(&hwsim_phys_lock); 877 list_for_each_entry_safe(phy, tmp, &hwsim_phys, list) 878 hwsim_del(phy); 879 mutex_unlock(&hwsim_phys_lock); 880 881 return 0; 882} 883 884static struct platform_driver mac802154hwsim_driver = { 885 .probe = hwsim_probe, 886 .remove = hwsim_remove, 887 .driver = { 888 .name = "mac802154_hwsim", 889 }, 890}; 891 892static __init int hwsim_init_module(void) 893{ 894 int rc; 895 896 rc = genl_register_family(&hwsim_genl_family); 897 if (rc) 898 return rc; 899 900 mac802154hwsim_dev = platform_device_register_simple("mac802154_hwsim", 901 -1, NULL, 0); 902 if (IS_ERR(mac802154hwsim_dev)) { 903 rc = PTR_ERR(mac802154hwsim_dev); 904 goto platform_dev; 905 } 906 907 rc = platform_driver_register(&mac802154hwsim_driver); 908 if (rc < 0) 909 goto platform_drv; 910 911 return 0; 912 913platform_drv: 914 platform_device_unregister(mac802154hwsim_dev); 915platform_dev: 916 genl_unregister_family(&hwsim_genl_family); 917 return rc; 918} 919 920static __exit void hwsim_remove_module(void) 921{ 922 genl_unregister_family(&hwsim_genl_family); 923 platform_driver_unregister(&mac802154hwsim_driver); 924 platform_device_unregister(mac802154hwsim_dev); 925} 926 927module_init(hwsim_init_module); 928module_exit(hwsim_remove_module); 929