cachepc-linux

Fork of AMDESE/linux with modifications for CachePC side-channel attack
git clone https://git.sinitax.com/sinitax/cachepc-linux
Log | Files | Refs | README | LICENSE | sfeed.txt

mac802154_hwsim.c (21088B)


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