1093 lines · c
1// SPDX-License-Identifier: GPL-2.0-only2/*3 * HWSIM IEEE 802.15.4 interface4 *5 * (C) 2018 Mojatau, Alexander Aring <aring@mojatau.com>6 * Copyright 2007-2012 Siemens AG7 *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 27MODULE_DESCRIPTION("Software simulator of IEEE 802.15.4 radio(s) for mac802154");28MODULE_LICENSE("GPL");29 30static LIST_HEAD(hwsim_phys);31static DEFINE_MUTEX(hwsim_phys_lock);32 33static struct platform_device *mac802154hwsim_dev;34 35/* MAC802154_HWSIM netlink family */36static struct genl_family hwsim_genl_family;37 38static int hwsim_radio_idx;39 40enum hwsim_multicast_groups {41 HWSIM_MCGRP_CONFIG,42};43 44static const struct genl_multicast_group hwsim_mcgrps[] = {45 [HWSIM_MCGRP_CONFIG] = { .name = "config", },46};47 48struct 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 57struct hwsim_edge_info {58 u8 lqi;59 60 struct rcu_head rcu;61};62 63struct 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 71struct 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 83static int hwsim_add_one(struct genl_info *info, struct device *dev,84 bool init);85static void hwsim_del(struct hwsim_phy *phy);86 87static int hwsim_hw_ed(struct ieee802154_hw *hw, u8 *level)88{89 *level = 0xbe;90 91 return 0;92}93 94static 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 120static 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 134static 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 150static 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 251drop:252 rcu_read_unlock();253 kfree_skb(skb);254}255 256static 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 a269 * performance tweak. Received radio should drop the frame270 * in mac802154 stack anyway... so we don't need to be271 * 100% of locking here to check on suspended272 */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 292static 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 301static void hwsim_hw_stop(struct ieee802154_hw *hw)302{303 struct hwsim_phy *phy = hw->priv;304 305 phy->suspended = true;306}307 308static int309hwsim_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 else319 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 329static 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 340static 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 345static 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 368static 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 429static 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 451out_err:452 genlmsg_cancel(skb, hdr);453 return res;454}455 456static 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 488out_err:489 mutex_unlock(&hwsim_phys_lock);490 491 return res;492}493 494static 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 520done:521 mutex_unlock(&hwsim_phys_lock);522 return skb->len;523}524 525/* caller need to held hwsim_phys_lock */526static 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 538static 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 543static 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 565static 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 577static 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 lock630 * should prevent of calling this function twice while631 * edges list has not the changes yet.632 */633 synchronize_rcu();634 mutex_unlock(&hwsim_phys_lock);635 636 return 0;637}638 639static 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 685static 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 744static 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 */751static 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 790static 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 803static 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 else810 genlmsg_multicast(&hwsim_genl_family, mcast_skb, 0,811 HWSIM_MCGRP_CONFIG, GFP_KERNEL);812}813 814static 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 836out_err:837 genlmsg_cancel(mcast_skb, data);838 nlmsg_free(mcast_skb);839}840 841static 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 861static 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 884sub_fail:885 hwsim_edge_unsubscribe_me(phy);886me_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 896static 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 984err_subscribe:985 ieee802154_unregister_hw(phy->hw);986err_reg:987 kfree(pib);988err_pib:989 ieee802154_free_hw(phy->hw);990 return err;991}992 993static 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 1016static 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 1030err_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 1038static 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 1048static struct platform_driver mac802154hwsim_driver = {1049 .probe = hwsim_probe,1050 .remove_new = hwsim_remove,1051 .driver = {1052 .name = "mac802154_hwsim",1053 },1054};1055 1056static __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 1077platform_drv:1078 platform_device_unregister(mac802154hwsim_dev);1079platform_dev:1080 genl_unregister_family(&hwsim_genl_family);1081 return rc;1082}1083 1084static __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 1091module_init(hwsim_init_module);1092module_exit(hwsim_remove_module);1093