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