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 
hwsim_hw_ed(struct ieee802154_hw * hw,u8 * level)87 static int hwsim_hw_ed(struct ieee802154_hw *hw, u8 *level)
88 {
89 	*level = 0xbe;
90 
91 	return 0;
92 }
93 
hwsim_update_pib(struct ieee802154_hw * hw,u8 page,u8 channel,struct ieee802154_hw_addr_filt * filt,enum ieee802154_filtering_level filt_level)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 
hwsim_hw_channel(struct ieee802154_hw * hw,u8 page,u8 channel)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 
hwsim_hw_addr_filt(struct ieee802154_hw * hw,struct ieee802154_hw_addr_filt * filt,unsigned long changed)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 
hwsim_hw_receive(struct ieee802154_hw * hw,struct sk_buff * skb,u8 lqi)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 
hwsim_hw_xmit(struct ieee802154_hw * hw,struct sk_buff * skb)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 
hwsim_hw_start(struct ieee802154_hw * hw)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 
hwsim_hw_stop(struct ieee802154_hw * hw)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
hwsim_set_promiscuous_mode(struct ieee802154_hw * hw,const bool on)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 
hwsim_new_radio_nl(struct sk_buff * msg,struct genl_info * info)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 
hwsim_del_radio_nl(struct sk_buff * msg,struct genl_info * info)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 
append_radio_msg(struct sk_buff * skb,struct hwsim_phy * phy)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 
hwsim_get_radio(struct sk_buff * skb,struct hwsim_phy * phy,u32 portid,u32 seq,struct netlink_callback * cb,int flags)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 
hwsim_get_radio_nl(struct sk_buff * msg,struct genl_info * info)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 
hwsim_dump_radio_nl(struct sk_buff * skb,struct netlink_callback * cb)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 */
hwsim_get_radio_by_id(uint32_t idx)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 
hwsim_alloc_edge(struct hwsim_phy * endpoint,u8 lqi)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 
hwsim_free_edge(struct hwsim_edge * e)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 
hwsim_new_edge_nl(struct sk_buff * msg,struct genl_info * info)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 
hwsim_del_edge_nl(struct sk_buff * msg,struct genl_info * info)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 
hwsim_set_edge_lqi(struct sk_buff * msg,struct genl_info * info)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, *einfo_old;
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 			einfo_old = rcu_replace_pointer(e->info, einfo,
727 							lockdep_is_held(&hwsim_phys_lock));
728 			rcu_read_unlock();
729 			kfree_rcu(einfo_old, rcu);
730 			mutex_unlock(&hwsim_phys_lock);
731 			return 0;
732 		}
733 	}
734 	rcu_read_unlock();
735 
736 	kfree(einfo);
737 	mutex_unlock(&hwsim_phys_lock);
738 
739 	return -ENOENT;
740 }
741 
742 /* MAC802154_HWSIM netlink policy */
743 
744 static const struct nla_policy hwsim_genl_policy[MAC802154_HWSIM_ATTR_MAX + 1] = {
745 	[MAC802154_HWSIM_ATTR_RADIO_ID] = { .type = NLA_U32 },
746 	[MAC802154_HWSIM_ATTR_RADIO_EDGE] = { .type = NLA_NESTED },
747 	[MAC802154_HWSIM_ATTR_RADIO_EDGES] = { .type = NLA_NESTED },
748 };
749 
750 /* Generic Netlink operations array */
751 static const struct genl_small_ops hwsim_nl_ops[] = {
752 	{
753 		.cmd = MAC802154_HWSIM_CMD_NEW_RADIO,
754 		.validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
755 		.doit = hwsim_new_radio_nl,
756 		.flags = GENL_UNS_ADMIN_PERM,
757 	},
758 	{
759 		.cmd = MAC802154_HWSIM_CMD_DEL_RADIO,
760 		.validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
761 		.doit = hwsim_del_radio_nl,
762 		.flags = GENL_UNS_ADMIN_PERM,
763 	},
764 	{
765 		.cmd = MAC802154_HWSIM_CMD_GET_RADIO,
766 		.validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
767 		.doit = hwsim_get_radio_nl,
768 		.dumpit = hwsim_dump_radio_nl,
769 	},
770 	{
771 		.cmd = MAC802154_HWSIM_CMD_NEW_EDGE,
772 		.validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
773 		.doit = hwsim_new_edge_nl,
774 		.flags = GENL_UNS_ADMIN_PERM,
775 	},
776 	{
777 		.cmd = MAC802154_HWSIM_CMD_DEL_EDGE,
778 		.validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
779 		.doit = hwsim_del_edge_nl,
780 		.flags = GENL_UNS_ADMIN_PERM,
781 	},
782 	{
783 		.cmd = MAC802154_HWSIM_CMD_SET_EDGE,
784 		.validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
785 		.doit = hwsim_set_edge_lqi,
786 		.flags = GENL_UNS_ADMIN_PERM,
787 	},
788 };
789 
790 static struct genl_family hwsim_genl_family __ro_after_init = {
791 	.name = "MAC802154_HWSIM",
792 	.version = 1,
793 	.maxattr = MAC802154_HWSIM_ATTR_MAX,
794 	.policy = hwsim_genl_policy,
795 	.module = THIS_MODULE,
796 	.small_ops = hwsim_nl_ops,
797 	.n_small_ops = ARRAY_SIZE(hwsim_nl_ops),
798 	.resv_start_op = MAC802154_HWSIM_CMD_NEW_EDGE + 1,
799 	.mcgrps = hwsim_mcgrps,
800 	.n_mcgrps = ARRAY_SIZE(hwsim_mcgrps),
801 };
802 
hwsim_mcast_config_msg(struct sk_buff * mcast_skb,struct genl_info * info)803 static void hwsim_mcast_config_msg(struct sk_buff *mcast_skb,
804 				   struct genl_info *info)
805 {
806 	if (info)
807 		genl_notify(&hwsim_genl_family, mcast_skb, info,
808 			    HWSIM_MCGRP_CONFIG, GFP_KERNEL);
809 	else
810 		genlmsg_multicast(&hwsim_genl_family, mcast_skb, 0,
811 				  HWSIM_MCGRP_CONFIG, GFP_KERNEL);
812 }
813 
hwsim_mcast_new_radio(struct genl_info * info,struct hwsim_phy * phy)814 static void hwsim_mcast_new_radio(struct genl_info *info, struct hwsim_phy *phy)
815 {
816 	struct sk_buff *mcast_skb;
817 	void *data;
818 
819 	mcast_skb = genlmsg_new(GENLMSG_DEFAULT_SIZE, GFP_KERNEL);
820 	if (!mcast_skb)
821 		return;
822 
823 	data = genlmsg_put(mcast_skb, 0, 0, &hwsim_genl_family, 0,
824 			   MAC802154_HWSIM_CMD_NEW_RADIO);
825 	if (!data)
826 		goto out_err;
827 
828 	if (append_radio_msg(mcast_skb, phy) < 0)
829 		goto out_err;
830 
831 	genlmsg_end(mcast_skb, data);
832 
833 	hwsim_mcast_config_msg(mcast_skb, info);
834 	return;
835 
836 out_err:
837 	genlmsg_cancel(mcast_skb, data);
838 	nlmsg_free(mcast_skb);
839 }
840 
hwsim_edge_unsubscribe_me(struct hwsim_phy * phy)841 static void hwsim_edge_unsubscribe_me(struct hwsim_phy *phy)
842 {
843 	struct hwsim_phy *tmp;
844 	struct hwsim_edge *e;
845 
846 	rcu_read_lock();
847 	/* going to all phy edges and remove phy from it */
848 	list_for_each_entry(tmp, &hwsim_phys, list) {
849 		list_for_each_entry_rcu(e, &tmp->edges, list) {
850 			if (e->endpoint->idx == phy->idx) {
851 				list_del_rcu(&e->list);
852 				hwsim_free_edge(e);
853 			}
854 		}
855 	}
856 	rcu_read_unlock();
857 
858 	synchronize_rcu();
859 }
860 
hwsim_subscribe_all_others(struct hwsim_phy * phy)861 static int hwsim_subscribe_all_others(struct hwsim_phy *phy)
862 {
863 	struct hwsim_phy *sub;
864 	struct hwsim_edge *e;
865 
866 	list_for_each_entry(sub, &hwsim_phys, list) {
867 		e = hwsim_alloc_edge(sub, 0xff);
868 		if (!e)
869 			goto me_fail;
870 
871 		list_add_rcu(&e->list, &phy->edges);
872 	}
873 
874 	list_for_each_entry(sub, &hwsim_phys, list) {
875 		e = hwsim_alloc_edge(phy, 0xff);
876 		if (!e)
877 			goto sub_fail;
878 
879 		list_add_rcu(&e->list, &sub->edges);
880 	}
881 
882 	return 0;
883 
884 sub_fail:
885 	hwsim_edge_unsubscribe_me(phy);
886 me_fail:
887 	rcu_read_lock();
888 	list_for_each_entry_rcu(e, &phy->edges, list) {
889 		list_del_rcu(&e->list);
890 		hwsim_free_edge(e);
891 	}
892 	rcu_read_unlock();
893 	return -ENOMEM;
894 }
895 
hwsim_add_one(struct genl_info * info,struct device * dev,bool init)896 static int hwsim_add_one(struct genl_info *info, struct device *dev,
897 			 bool init)
898 {
899 	struct ieee802154_hw *hw;
900 	struct hwsim_phy *phy;
901 	struct hwsim_pib *pib;
902 	int idx;
903 	int err;
904 
905 	idx = hwsim_radio_idx++;
906 
907 	hw = ieee802154_alloc_hw(sizeof(*phy), &hwsim_ops);
908 	if (!hw)
909 		return -ENOMEM;
910 
911 	phy = hw->priv;
912 	phy->hw = hw;
913 
914 	/* 868 MHz BPSK	802.15.4-2003 */
915 	hw->phy->supported.channels[0] |= 1;
916 	/* 915 MHz BPSK	802.15.4-2003 */
917 	hw->phy->supported.channels[0] |= 0x7fe;
918 	/* 2.4 GHz O-QPSK 802.15.4-2003 */
919 	hw->phy->supported.channels[0] |= 0x7FFF800;
920 	/* 868 MHz ASK 802.15.4-2006 */
921 	hw->phy->supported.channels[1] |= 1;
922 	/* 915 MHz ASK 802.15.4-2006 */
923 	hw->phy->supported.channels[1] |= 0x7fe;
924 	/* 868 MHz O-QPSK 802.15.4-2006 */
925 	hw->phy->supported.channels[2] |= 1;
926 	/* 915 MHz O-QPSK 802.15.4-2006 */
927 	hw->phy->supported.channels[2] |= 0x7fe;
928 	/* 2.4 GHz CSS 802.15.4a-2007 */
929 	hw->phy->supported.channels[3] |= 0x3fff;
930 	/* UWB Sub-gigahertz 802.15.4a-2007 */
931 	hw->phy->supported.channels[4] |= 1;
932 	/* UWB Low band 802.15.4a-2007 */
933 	hw->phy->supported.channels[4] |= 0x1e;
934 	/* UWB High band 802.15.4a-2007 */
935 	hw->phy->supported.channels[4] |= 0xffe0;
936 	/* 750 MHz O-QPSK 802.15.4c-2009 */
937 	hw->phy->supported.channels[5] |= 0xf;
938 	/* 750 MHz MPSK 802.15.4c-2009 */
939 	hw->phy->supported.channels[5] |= 0xf0;
940 	/* 950 MHz BPSK 802.15.4d-2009 */
941 	hw->phy->supported.channels[6] |= 0x3ff;
942 	/* 950 MHz GFSK 802.15.4d-2009 */
943 	hw->phy->supported.channels[6] |= 0x3ffc00;
944 
945 	ieee802154_random_extended_addr(&hw->phy->perm_extended_addr);
946 
947 	/* hwsim phy channel 13 as default */
948 	hw->phy->current_channel = 13;
949 	pib = kzalloc(sizeof(*pib), GFP_KERNEL);
950 	if (!pib) {
951 		err = -ENOMEM;
952 		goto err_pib;
953 	}
954 
955 	pib->channel = 13;
956 	pib->filt.short_addr = cpu_to_le16(IEEE802154_ADDR_BROADCAST);
957 	pib->filt.pan_id = cpu_to_le16(IEEE802154_PANID_BROADCAST);
958 	rcu_assign_pointer(phy->pib, pib);
959 	phy->idx = idx;
960 	INIT_LIST_HEAD(&phy->edges);
961 
962 	hw->flags = IEEE802154_HW_PROMISCUOUS;
963 	hw->parent = dev;
964 
965 	err = ieee802154_register_hw(hw);
966 	if (err)
967 		goto err_reg;
968 
969 	mutex_lock(&hwsim_phys_lock);
970 	if (init) {
971 		err = hwsim_subscribe_all_others(phy);
972 		if (err < 0) {
973 			mutex_unlock(&hwsim_phys_lock);
974 			goto err_subscribe;
975 		}
976 	}
977 	list_add_tail(&phy->list, &hwsim_phys);
978 	mutex_unlock(&hwsim_phys_lock);
979 
980 	hwsim_mcast_new_radio(info, phy);
981 
982 	return idx;
983 
984 err_subscribe:
985 	ieee802154_unregister_hw(phy->hw);
986 err_reg:
987 	kfree(pib);
988 err_pib:
989 	ieee802154_free_hw(phy->hw);
990 	return err;
991 }
992 
hwsim_del(struct hwsim_phy * phy)993 static void hwsim_del(struct hwsim_phy *phy)
994 {
995 	struct hwsim_pib *pib;
996 	struct hwsim_edge *e;
997 
998 	hwsim_edge_unsubscribe_me(phy);
999 
1000 	list_del(&phy->list);
1001 
1002 	rcu_read_lock();
1003 	list_for_each_entry_rcu(e, &phy->edges, list) {
1004 		list_del_rcu(&e->list);
1005 		hwsim_free_edge(e);
1006 	}
1007 	pib = rcu_dereference(phy->pib);
1008 	rcu_read_unlock();
1009 
1010 	kfree_rcu(pib, rcu);
1011 
1012 	ieee802154_unregister_hw(phy->hw);
1013 	ieee802154_free_hw(phy->hw);
1014 }
1015 
hwsim_probe(struct platform_device * pdev)1016 static int hwsim_probe(struct platform_device *pdev)
1017 {
1018 	struct hwsim_phy *phy, *tmp;
1019 	int err, i;
1020 
1021 	for (i = 0; i < 2; i++) {
1022 		err = hwsim_add_one(NULL, &pdev->dev, true);
1023 		if (err < 0)
1024 			goto err_slave;
1025 	}
1026 
1027 	dev_info(&pdev->dev, "Added 2 mac802154 hwsim hardware radios\n");
1028 	return 0;
1029 
1030 err_slave:
1031 	mutex_lock(&hwsim_phys_lock);
1032 	list_for_each_entry_safe(phy, tmp, &hwsim_phys, list)
1033 		hwsim_del(phy);
1034 	mutex_unlock(&hwsim_phys_lock);
1035 	return err;
1036 }
1037 
hwsim_remove(struct platform_device * pdev)1038 static int hwsim_remove(struct platform_device *pdev)
1039 {
1040 	struct hwsim_phy *phy, *tmp;
1041 
1042 	mutex_lock(&hwsim_phys_lock);
1043 	list_for_each_entry_safe(phy, tmp, &hwsim_phys, list)
1044 		hwsim_del(phy);
1045 	mutex_unlock(&hwsim_phys_lock);
1046 
1047 	return 0;
1048 }
1049 
1050 static struct platform_driver mac802154hwsim_driver = {
1051 	.probe = hwsim_probe,
1052 	.remove = hwsim_remove,
1053 	.driver = {
1054 			.name = "mac802154_hwsim",
1055 	},
1056 };
1057 
hwsim_init_module(void)1058 static __init int hwsim_init_module(void)
1059 {
1060 	int rc;
1061 
1062 	rc = genl_register_family(&hwsim_genl_family);
1063 	if (rc)
1064 		return rc;
1065 
1066 	mac802154hwsim_dev = platform_device_register_simple("mac802154_hwsim",
1067 							     -1, NULL, 0);
1068 	if (IS_ERR(mac802154hwsim_dev)) {
1069 		rc = PTR_ERR(mac802154hwsim_dev);
1070 		goto platform_dev;
1071 	}
1072 
1073 	rc = platform_driver_register(&mac802154hwsim_driver);
1074 	if (rc < 0)
1075 		goto platform_drv;
1076 
1077 	return 0;
1078 
1079 platform_drv:
1080 	platform_device_unregister(mac802154hwsim_dev);
1081 platform_dev:
1082 	genl_unregister_family(&hwsim_genl_family);
1083 	return rc;
1084 }
1085 
hwsim_remove_module(void)1086 static __exit void hwsim_remove_module(void)
1087 {
1088 	genl_unregister_family(&hwsim_genl_family);
1089 	platform_driver_unregister(&mac802154hwsim_driver);
1090 	platform_device_unregister(mac802154hwsim_dev);
1091 }
1092 
1093 module_init(hwsim_init_module);
1094 module_exit(hwsim_remove_module);
1095