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