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 
26 MODULE_DESCRIPTION("Software simulator of IEEE 802.15.4 radio(s) for mac802154");
27 MODULE_LICENSE("GPL");
28 
29 static LIST_HEAD(hwsim_phys);
30 static DEFINE_MUTEX(hwsim_phys_lock);
31 
32 static struct platform_device *mac802154hwsim_dev;
33 
34 /* MAC802154_HWSIM netlink family */
35 static struct genl_family hwsim_genl_family;
36 
37 static int hwsim_radio_idx;
38 
39 enum hwsim_multicast_groups {
40 	HWSIM_MCGRP_CONFIG,
41 };
42 
43 static const struct genl_multicast_group hwsim_mcgrps[] = {
44 	[HWSIM_MCGRP_CONFIG] = { .name = "config", },
45 };
46 
47 struct hwsim_pib {
48 	u8 page;
49 	u8 channel;
50 
51 	struct rcu_head rcu;
52 };
53 
54 struct hwsim_edge_info {
55 	u8 lqi;
56 
57 	struct rcu_head rcu;
58 };
59 
60 struct 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 
68 struct 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 
80 static int hwsim_add_one(struct genl_info *info, struct device *dev,
81 			 bool init);
82 static void hwsim_del(struct hwsim_phy *phy);
83 
84 static int hwsim_hw_ed(struct ieee802154_hw *hw, u8 *level)
85 {
86 	*level = 0xbe;
87 
88 	return 0;
89 }
90 
91 static 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 
109 static 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 
146 static 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 
154 static void hwsim_hw_stop(struct ieee802154_hw *hw)
155 {
156 	struct hwsim_phy *phy = hw->priv;
157 
158 	phy->suspended = true;
159 }
160 
161 static int
162 hwsim_set_promiscuous_mode(struct ieee802154_hw *hw, const bool on)
163 {
164 	return 0;
165 }
166 
167 static 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 
177 static 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 
182 static 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 
205 static 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 
266 static 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 
288 out_err:
289 	genlmsg_cancel(skb, hdr);
290 	return res;
291 }
292 
293 static 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 
325 out_err:
326 	mutex_unlock(&hwsim_phys_lock);
327 
328 	return res;
329 }
330 
331 static 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 
357 done:
358 	mutex_unlock(&hwsim_phys_lock);
359 	return skb->len;
360 }
361 
362 /* caller need to held hwsim_phys_lock */
363 static 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 
375 static 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 
380 static 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 
402 static 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 
414 static 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 
476 static 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 
522 static 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 
579 static 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 */
586 static 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 
625 static 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 	.resv_start_op = MAC802154_HWSIM_CMD_NEW_EDGE + 1,
634 	.mcgrps = hwsim_mcgrps,
635 	.n_mcgrps = ARRAY_SIZE(hwsim_mcgrps),
636 };
637 
638 static void hwsim_mcast_config_msg(struct sk_buff *mcast_skb,
639 				   struct genl_info *info)
640 {
641 	if (info)
642 		genl_notify(&hwsim_genl_family, mcast_skb, info,
643 			    HWSIM_MCGRP_CONFIG, GFP_KERNEL);
644 	else
645 		genlmsg_multicast(&hwsim_genl_family, mcast_skb, 0,
646 				  HWSIM_MCGRP_CONFIG, GFP_KERNEL);
647 }
648 
649 static void hwsim_mcast_new_radio(struct genl_info *info, struct hwsim_phy *phy)
650 {
651 	struct sk_buff *mcast_skb;
652 	void *data;
653 
654 	mcast_skb = genlmsg_new(GENLMSG_DEFAULT_SIZE, GFP_KERNEL);
655 	if (!mcast_skb)
656 		return;
657 
658 	data = genlmsg_put(mcast_skb, 0, 0, &hwsim_genl_family, 0,
659 			   MAC802154_HWSIM_CMD_NEW_RADIO);
660 	if (!data)
661 		goto out_err;
662 
663 	if (append_radio_msg(mcast_skb, phy) < 0)
664 		goto out_err;
665 
666 	genlmsg_end(mcast_skb, data);
667 
668 	hwsim_mcast_config_msg(mcast_skb, info);
669 	return;
670 
671 out_err:
672 	genlmsg_cancel(mcast_skb, data);
673 	nlmsg_free(mcast_skb);
674 }
675 
676 static void hwsim_edge_unsubscribe_me(struct hwsim_phy *phy)
677 {
678 	struct hwsim_phy *tmp;
679 	struct hwsim_edge *e;
680 
681 	rcu_read_lock();
682 	/* going to all phy edges and remove phy from it */
683 	list_for_each_entry(tmp, &hwsim_phys, list) {
684 		list_for_each_entry_rcu(e, &tmp->edges, list) {
685 			if (e->endpoint->idx == phy->idx) {
686 				list_del_rcu(&e->list);
687 				hwsim_free_edge(e);
688 			}
689 		}
690 	}
691 	rcu_read_unlock();
692 
693 	synchronize_rcu();
694 }
695 
696 static int hwsim_subscribe_all_others(struct hwsim_phy *phy)
697 {
698 	struct hwsim_phy *sub;
699 	struct hwsim_edge *e;
700 
701 	list_for_each_entry(sub, &hwsim_phys, list) {
702 		e = hwsim_alloc_edge(sub, 0xff);
703 		if (!e)
704 			goto me_fail;
705 
706 		list_add_rcu(&e->list, &phy->edges);
707 	}
708 
709 	list_for_each_entry(sub, &hwsim_phys, list) {
710 		e = hwsim_alloc_edge(phy, 0xff);
711 		if (!e)
712 			goto sub_fail;
713 
714 		list_add_rcu(&e->list, &sub->edges);
715 	}
716 
717 	return 0;
718 
719 sub_fail:
720 	hwsim_edge_unsubscribe_me(phy);
721 me_fail:
722 	rcu_read_lock();
723 	list_for_each_entry_rcu(e, &phy->edges, list) {
724 		list_del_rcu(&e->list);
725 		hwsim_free_edge(e);
726 	}
727 	rcu_read_unlock();
728 	return -ENOMEM;
729 }
730 
731 static int hwsim_add_one(struct genl_info *info, struct device *dev,
732 			 bool init)
733 {
734 	struct ieee802154_hw *hw;
735 	struct hwsim_phy *phy;
736 	struct hwsim_pib *pib;
737 	int idx;
738 	int err;
739 
740 	idx = hwsim_radio_idx++;
741 
742 	hw = ieee802154_alloc_hw(sizeof(*phy), &hwsim_ops);
743 	if (!hw)
744 		return -ENOMEM;
745 
746 	phy = hw->priv;
747 	phy->hw = hw;
748 
749 	/* 868 MHz BPSK	802.15.4-2003 */
750 	hw->phy->supported.channels[0] |= 1;
751 	/* 915 MHz BPSK	802.15.4-2003 */
752 	hw->phy->supported.channels[0] |= 0x7fe;
753 	/* 2.4 GHz O-QPSK 802.15.4-2003 */
754 	hw->phy->supported.channels[0] |= 0x7FFF800;
755 	/* 868 MHz ASK 802.15.4-2006 */
756 	hw->phy->supported.channels[1] |= 1;
757 	/* 915 MHz ASK 802.15.4-2006 */
758 	hw->phy->supported.channels[1] |= 0x7fe;
759 	/* 868 MHz O-QPSK 802.15.4-2006 */
760 	hw->phy->supported.channels[2] |= 1;
761 	/* 915 MHz O-QPSK 802.15.4-2006 */
762 	hw->phy->supported.channels[2] |= 0x7fe;
763 	/* 2.4 GHz CSS 802.15.4a-2007 */
764 	hw->phy->supported.channels[3] |= 0x3fff;
765 	/* UWB Sub-gigahertz 802.15.4a-2007 */
766 	hw->phy->supported.channels[4] |= 1;
767 	/* UWB Low band 802.15.4a-2007 */
768 	hw->phy->supported.channels[4] |= 0x1e;
769 	/* UWB High band 802.15.4a-2007 */
770 	hw->phy->supported.channels[4] |= 0xffe0;
771 	/* 750 MHz O-QPSK 802.15.4c-2009 */
772 	hw->phy->supported.channels[5] |= 0xf;
773 	/* 750 MHz MPSK 802.15.4c-2009 */
774 	hw->phy->supported.channels[5] |= 0xf0;
775 	/* 950 MHz BPSK 802.15.4d-2009 */
776 	hw->phy->supported.channels[6] |= 0x3ff;
777 	/* 950 MHz GFSK 802.15.4d-2009 */
778 	hw->phy->supported.channels[6] |= 0x3ffc00;
779 
780 	ieee802154_random_extended_addr(&hw->phy->perm_extended_addr);
781 
782 	/* hwsim phy channel 13 as default */
783 	hw->phy->current_channel = 13;
784 	pib = kzalloc(sizeof(*pib), GFP_KERNEL);
785 	if (!pib) {
786 		err = -ENOMEM;
787 		goto err_pib;
788 	}
789 
790 	pib->channel = 13;
791 	rcu_assign_pointer(phy->pib, pib);
792 	phy->idx = idx;
793 	INIT_LIST_HEAD(&phy->edges);
794 
795 	hw->flags = IEEE802154_HW_PROMISCUOUS | IEEE802154_HW_RX_DROP_BAD_CKSUM;
796 	hw->parent = dev;
797 
798 	err = ieee802154_register_hw(hw);
799 	if (err)
800 		goto err_reg;
801 
802 	mutex_lock(&hwsim_phys_lock);
803 	if (init) {
804 		err = hwsim_subscribe_all_others(phy);
805 		if (err < 0) {
806 			mutex_unlock(&hwsim_phys_lock);
807 			goto err_subscribe;
808 		}
809 	}
810 	list_add_tail(&phy->list, &hwsim_phys);
811 	mutex_unlock(&hwsim_phys_lock);
812 
813 	hwsim_mcast_new_radio(info, phy);
814 
815 	return idx;
816 
817 err_subscribe:
818 	ieee802154_unregister_hw(phy->hw);
819 err_reg:
820 	kfree(pib);
821 err_pib:
822 	ieee802154_free_hw(phy->hw);
823 	return err;
824 }
825 
826 static void hwsim_del(struct hwsim_phy *phy)
827 {
828 	struct hwsim_pib *pib;
829 	struct hwsim_edge *e;
830 
831 	hwsim_edge_unsubscribe_me(phy);
832 
833 	list_del(&phy->list);
834 
835 	rcu_read_lock();
836 	list_for_each_entry_rcu(e, &phy->edges, list) {
837 		list_del_rcu(&e->list);
838 		hwsim_free_edge(e);
839 	}
840 	pib = rcu_dereference(phy->pib);
841 	rcu_read_unlock();
842 
843 	kfree_rcu(pib, rcu);
844 
845 	ieee802154_unregister_hw(phy->hw);
846 	ieee802154_free_hw(phy->hw);
847 }
848 
849 static int hwsim_probe(struct platform_device *pdev)
850 {
851 	struct hwsim_phy *phy, *tmp;
852 	int err, i;
853 
854 	for (i = 0; i < 2; i++) {
855 		err = hwsim_add_one(NULL, &pdev->dev, true);
856 		if (err < 0)
857 			goto err_slave;
858 	}
859 
860 	dev_info(&pdev->dev, "Added 2 mac802154 hwsim hardware radios\n");
861 	return 0;
862 
863 err_slave:
864 	mutex_lock(&hwsim_phys_lock);
865 	list_for_each_entry_safe(phy, tmp, &hwsim_phys, list)
866 		hwsim_del(phy);
867 	mutex_unlock(&hwsim_phys_lock);
868 	return err;
869 }
870 
871 static int hwsim_remove(struct platform_device *pdev)
872 {
873 	struct hwsim_phy *phy, *tmp;
874 
875 	mutex_lock(&hwsim_phys_lock);
876 	list_for_each_entry_safe(phy, tmp, &hwsim_phys, list)
877 		hwsim_del(phy);
878 	mutex_unlock(&hwsim_phys_lock);
879 
880 	return 0;
881 }
882 
883 static struct platform_driver mac802154hwsim_driver = {
884 	.probe = hwsim_probe,
885 	.remove = hwsim_remove,
886 	.driver = {
887 			.name = "mac802154_hwsim",
888 	},
889 };
890 
891 static __init int hwsim_init_module(void)
892 {
893 	int rc;
894 
895 	rc = genl_register_family(&hwsim_genl_family);
896 	if (rc)
897 		return rc;
898 
899 	mac802154hwsim_dev = platform_device_register_simple("mac802154_hwsim",
900 							     -1, NULL, 0);
901 	if (IS_ERR(mac802154hwsim_dev)) {
902 		rc = PTR_ERR(mac802154hwsim_dev);
903 		goto platform_dev;
904 	}
905 
906 	rc = platform_driver_register(&mac802154hwsim_driver);
907 	if (rc < 0)
908 		goto platform_drv;
909 
910 	return 0;
911 
912 platform_drv:
913 	platform_device_unregister(mac802154hwsim_dev);
914 platform_dev:
915 	genl_unregister_family(&hwsim_genl_family);
916 	return rc;
917 }
918 
919 static __exit void hwsim_remove_module(void)
920 {
921 	genl_unregister_family(&hwsim_genl_family);
922 	platform_driver_unregister(&mac802154hwsim_driver);
923 	platform_device_unregister(mac802154hwsim_dev);
924 }
925 
926 module_init(hwsim_init_module);
927 module_exit(hwsim_remove_module);
928