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 	.mcgrps = hwsim_mcgrps,
634 	.n_mcgrps = ARRAY_SIZE(hwsim_mcgrps),
635 };
636 
637 static 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 
648 static 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 
670 out_err:
671 	genlmsg_cancel(mcast_skb, data);
672 	nlmsg_free(mcast_skb);
673 }
674 
675 static 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 
695 static 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 
718 sub_fail:
719 	hwsim_edge_unsubscribe_me(phy);
720 me_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 
730 static 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 
816 err_subscribe:
817 	ieee802154_unregister_hw(phy->hw);
818 err_reg:
819 	kfree(pib);
820 err_pib:
821 	ieee802154_free_hw(phy->hw);
822 	return err;
823 }
824 
825 static 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 
848 static 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 
862 err_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 
870 static 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 
882 static struct platform_driver mac802154hwsim_driver = {
883 	.probe = hwsim_probe,
884 	.remove = hwsim_remove,
885 	.driver = {
886 			.name = "mac802154_hwsim",
887 	},
888 };
889 
890 static __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 
911 platform_drv:
912 	platform_device_unregister(mac802154hwsim_dev);
913 platform_dev:
914 	genl_unregister_family(&hwsim_genl_family);
915 	return rc;
916 }
917 
918 static __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 
925 module_init(hwsim_init_module);
926 module_exit(hwsim_remove_module);
927