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