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