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