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