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
  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, &current_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
 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 int
 309hwsim_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
 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 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
 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	else
 810		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 = 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);