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, ¤t_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