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