1
2
3
4
5
6
7
8
9
10
11
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
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
122
123
124
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;
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
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
467
468
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
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
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
586static const struct genl_small_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 .small_ops = hwsim_nl_ops,
632 .n_small_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
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
718sub_fail:
719 hwsim_edge_unsubscribe_me(phy);
720me_fail:
721 rcu_read_lock();
722 list_for_each_entry_rcu(e, &phy->edges, list) {
723 list_del_rcu(&e->list);
724 hwsim_free_edge(e);
725 }
726 rcu_read_unlock();
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
749 hw->phy->supported.channels[0] |= 1;
750
751 hw->phy->supported.channels[0] |= 0x7fe;
752
753 hw->phy->supported.channels[0] |= 0x7FFF800;
754
755 hw->phy->supported.channels[1] |= 1;
756
757 hw->phy->supported.channels[1] |= 0x7fe;
758
759 hw->phy->supported.channels[2] |= 1;
760
761 hw->phy->supported.channels[2] |= 0x7fe;
762
763 hw->phy->supported.channels[3] |= 0x3fff;
764
765 hw->phy->supported.channels[4] |= 1;
766
767 hw->phy->supported.channels[4] |= 0x1e;
768
769 hw->phy->supported.channels[4] |= 0xffe0;
770
771 hw->phy->supported.channels[5] |= 0xf;
772
773 hw->phy->supported.channels[5] |= 0xf0;
774
775 hw->phy->supported.channels[6] |= 0x3ff;
776
777 hw->phy->supported.channels[6] |= 0x3ffc00;
778
779 ieee802154_random_extended_addr(&hw->phy->perm_extended_addr);
780
781
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 struct hwsim_edge *e;
828
829 hwsim_edge_unsubscribe_me(phy);
830
831 list_del(&phy->list);
832
833 rcu_read_lock();
834 list_for_each_entry_rcu(e, &phy->edges, list) {
835 list_del_rcu(&e->list);
836 hwsim_free_edge(e);
837 }
838 pib = rcu_dereference(phy->pib);
839 rcu_read_unlock();
840
841 kfree_rcu(pib, rcu);
842
843 ieee802154_unregister_hw(phy->hw);
844 ieee802154_free_hw(phy->hw);
845}
846
847static int hwsim_probe(struct platform_device *pdev)
848{
849 struct hwsim_phy *phy, *tmp;
850 int err, i;
851
852 for (i = 0; i < 2; i++) {
853 err = hwsim_add_one(NULL, &pdev->dev, true);
854 if (err < 0)
855 goto err_slave;
856 }
857
858 dev_info(&pdev->dev, "Added 2 mac802154 hwsim hardware radios\n");
859 return 0;
860
861err_slave:
862 mutex_lock(&hwsim_phys_lock);
863 list_for_each_entry_safe(phy, tmp, &hwsim_phys, list)
864 hwsim_del(phy);
865 mutex_unlock(&hwsim_phys_lock);
866 return err;
867}
868
869static int hwsim_remove(struct platform_device *pdev)
870{
871 struct hwsim_phy *phy, *tmp;
872
873 mutex_lock(&hwsim_phys_lock);
874 list_for_each_entry_safe(phy, tmp, &hwsim_phys, list)
875 hwsim_del(phy);
876 mutex_unlock(&hwsim_phys_lock);
877
878 return 0;
879}
880
881static struct platform_driver mac802154hwsim_driver = {
882 .probe = hwsim_probe,
883 .remove = hwsim_remove,
884 .driver = {
885 .name = "mac802154_hwsim",
886 },
887};
888
889static __init int hwsim_init_module(void)
890{
891 int rc;
892
893 rc = genl_register_family(&hwsim_genl_family);
894 if (rc)
895 return rc;
896
897 mac802154hwsim_dev = platform_device_register_simple("mac802154_hwsim",
898 -1, NULL, 0);
899 if (IS_ERR(mac802154hwsim_dev)) {
900 rc = PTR_ERR(mac802154hwsim_dev);
901 goto platform_dev;
902 }
903
904 rc = platform_driver_register(&mac802154hwsim_driver);
905 if (rc < 0)
906 goto platform_drv;
907
908 return 0;
909
910platform_drv:
911 platform_device_unregister(mac802154hwsim_dev);
912platform_dev:
913 genl_unregister_family(&hwsim_genl_family);
914 return rc;
915}
916
917static __exit void hwsim_remove_module(void)
918{
919 genl_unregister_family(&hwsim_genl_family);
920 platform_driver_unregister(&mac802154hwsim_driver);
921 platform_device_unregister(mac802154hwsim_dev);
922}
923
924module_init(hwsim_init_module);
925module_exit(hwsim_remove_module);
926
