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
26 MODULE_DESCRIPTION("Software simulator of IEEE 802.15.4 radio(s) for mac802154");
27 MODULE_LICENSE("GPL");
28
29 static LIST_HEAD(hwsim_phys);
30 static DEFINE_MUTEX(hwsim_phys_lock);
31
32 static struct platform_device *mac802154hwsim_dev;
33
34 /* MAC802154_HWSIM netlink family */
35 static struct genl_family hwsim_genl_family;
36
37 static int hwsim_radio_idx;
38
39 enum hwsim_multicast_groups {
40 HWSIM_MCGRP_CONFIG,
41 };
42
43 static const struct genl_multicast_group hwsim_mcgrps[] = {
44 [HWSIM_MCGRP_CONFIG] = { .name = "config", },
45 };
46
47 struct hwsim_pib {
48 u8 page;
49 u8 channel;
50
51 struct rcu_head rcu;
52 };
53
54 struct hwsim_edge_info {
55 u8 lqi;
56
57 struct rcu_head rcu;
58 };
59
60 struct 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
68 struct 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
80 static int hwsim_add_one(struct genl_info *info, struct device *dev,
81 bool init);
82 static void hwsim_del(struct hwsim_phy *phy);
83
hwsim_hw_ed(struct ieee802154_hw * hw,u8 * level)84 static int hwsim_hw_ed(struct ieee802154_hw *hw, u8 *level)
85 {
86 *level = 0xbe;
87
88 return 0;
89 }
90
hwsim_hw_channel(struct ieee802154_hw * hw,u8 page,u8 channel)91 static 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
hwsim_hw_xmit(struct ieee802154_hw * hw,struct sk_buff * skb)109 static 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
hwsim_hw_start(struct ieee802154_hw * hw)146 static 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
hwsim_hw_stop(struct ieee802154_hw * hw)154 static void hwsim_hw_stop(struct ieee802154_hw *hw)
155 {
156 struct hwsim_phy *phy = hw->priv;
157
158 phy->suspended = true;
159 }
160
161 static int
hwsim_set_promiscuous_mode(struct ieee802154_hw * hw,const bool on)162 hwsim_set_promiscuous_mode(struct ieee802154_hw *hw, const bool on)
163 {
164 return 0;
165 }
166
167 static 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
hwsim_new_radio_nl(struct sk_buff * msg,struct genl_info * info)177 static 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
hwsim_del_radio_nl(struct sk_buff * msg,struct genl_info * info)182 static 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
append_radio_msg(struct sk_buff * skb,struct hwsim_phy * phy)205 static 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
hwsim_get_radio(struct sk_buff * skb,struct hwsim_phy * phy,u32 portid,u32 seq,struct netlink_callback * cb,int flags)266 static 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
288 out_err:
289 genlmsg_cancel(skb, hdr);
290 return res;
291 }
292
hwsim_get_radio_nl(struct sk_buff * msg,struct genl_info * info)293 static 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
325 out_err:
326 mutex_unlock(&hwsim_phys_lock);
327
328 return res;
329 }
330
hwsim_dump_radio_nl(struct sk_buff * skb,struct netlink_callback * cb)331 static 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
357 done:
358 mutex_unlock(&hwsim_phys_lock);
359 return skb->len;
360 }
361
362 /* caller need to held hwsim_phys_lock */
hwsim_get_radio_by_id(uint32_t idx)363 static 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
375 static 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
hwsim_alloc_edge(struct hwsim_phy * endpoint,u8 lqi)380 static 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
hwsim_free_edge(struct hwsim_edge * e)402 static 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
hwsim_new_edge_nl(struct sk_buff * msg,struct genl_info * info)414 static 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
hwsim_del_edge_nl(struct sk_buff * msg,struct genl_info * info)476 static 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
hwsim_set_edge_lqi(struct sk_buff * msg,struct genl_info * info)522 static 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, *einfo_old;
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 einfo_old = rcu_replace_pointer(e->info, einfo,
564 lockdep_is_held(&hwsim_phys_lock));
565 rcu_read_unlock();
566 kfree_rcu(einfo_old, rcu);
567 mutex_unlock(&hwsim_phys_lock);
568 return 0;
569 }
570 }
571 rcu_read_unlock();
572
573 kfree(einfo);
574 mutex_unlock(&hwsim_phys_lock);
575
576 return -ENOENT;
577 }
578
579 /* MAC802154_HWSIM netlink policy */
580
581 static const struct nla_policy hwsim_genl_policy[MAC802154_HWSIM_ATTR_MAX + 1] = {
582 [MAC802154_HWSIM_ATTR_RADIO_ID] = { .type = NLA_U32 },
583 [MAC802154_HWSIM_ATTR_RADIO_EDGE] = { .type = NLA_NESTED },
584 [MAC802154_HWSIM_ATTR_RADIO_EDGES] = { .type = NLA_NESTED },
585 };
586
587 /* Generic Netlink operations array */
588 static const struct genl_small_ops hwsim_nl_ops[] = {
589 {
590 .cmd = MAC802154_HWSIM_CMD_NEW_RADIO,
591 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
592 .doit = hwsim_new_radio_nl,
593 .flags = GENL_UNS_ADMIN_PERM,
594 },
595 {
596 .cmd = MAC802154_HWSIM_CMD_DEL_RADIO,
597 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
598 .doit = hwsim_del_radio_nl,
599 .flags = GENL_UNS_ADMIN_PERM,
600 },
601 {
602 .cmd = MAC802154_HWSIM_CMD_GET_RADIO,
603 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
604 .doit = hwsim_get_radio_nl,
605 .dumpit = hwsim_dump_radio_nl,
606 },
607 {
608 .cmd = MAC802154_HWSIM_CMD_NEW_EDGE,
609 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
610 .doit = hwsim_new_edge_nl,
611 .flags = GENL_UNS_ADMIN_PERM,
612 },
613 {
614 .cmd = MAC802154_HWSIM_CMD_DEL_EDGE,
615 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
616 .doit = hwsim_del_edge_nl,
617 .flags = GENL_UNS_ADMIN_PERM,
618 },
619 {
620 .cmd = MAC802154_HWSIM_CMD_SET_EDGE,
621 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
622 .doit = hwsim_set_edge_lqi,
623 .flags = GENL_UNS_ADMIN_PERM,
624 },
625 };
626
627 static struct genl_family hwsim_genl_family __ro_after_init = {
628 .name = "MAC802154_HWSIM",
629 .version = 1,
630 .maxattr = MAC802154_HWSIM_ATTR_MAX,
631 .policy = hwsim_genl_policy,
632 .module = THIS_MODULE,
633 .small_ops = hwsim_nl_ops,
634 .n_small_ops = ARRAY_SIZE(hwsim_nl_ops),
635 .mcgrps = hwsim_mcgrps,
636 .n_mcgrps = ARRAY_SIZE(hwsim_mcgrps),
637 };
638
hwsim_mcast_config_msg(struct sk_buff * mcast_skb,struct genl_info * info)639 static void hwsim_mcast_config_msg(struct sk_buff *mcast_skb,
640 struct genl_info *info)
641 {
642 if (info)
643 genl_notify(&hwsim_genl_family, mcast_skb, info,
644 HWSIM_MCGRP_CONFIG, GFP_KERNEL);
645 else
646 genlmsg_multicast(&hwsim_genl_family, mcast_skb, 0,
647 HWSIM_MCGRP_CONFIG, GFP_KERNEL);
648 }
649
hwsim_mcast_new_radio(struct genl_info * info,struct hwsim_phy * phy)650 static void hwsim_mcast_new_radio(struct genl_info *info, struct hwsim_phy *phy)
651 {
652 struct sk_buff *mcast_skb;
653 void *data;
654
655 mcast_skb = genlmsg_new(GENLMSG_DEFAULT_SIZE, GFP_KERNEL);
656 if (!mcast_skb)
657 return;
658
659 data = genlmsg_put(mcast_skb, 0, 0, &hwsim_genl_family, 0,
660 MAC802154_HWSIM_CMD_NEW_RADIO);
661 if (!data)
662 goto out_err;
663
664 if (append_radio_msg(mcast_skb, phy) < 0)
665 goto out_err;
666
667 genlmsg_end(mcast_skb, data);
668
669 hwsim_mcast_config_msg(mcast_skb, info);
670 return;
671
672 out_err:
673 genlmsg_cancel(mcast_skb, data);
674 nlmsg_free(mcast_skb);
675 }
676
hwsim_edge_unsubscribe_me(struct hwsim_phy * phy)677 static void hwsim_edge_unsubscribe_me(struct hwsim_phy *phy)
678 {
679 struct hwsim_phy *tmp;
680 struct hwsim_edge *e;
681
682 rcu_read_lock();
683 /* going to all phy edges and remove phy from it */
684 list_for_each_entry(tmp, &hwsim_phys, list) {
685 list_for_each_entry_rcu(e, &tmp->edges, list) {
686 if (e->endpoint->idx == phy->idx) {
687 list_del_rcu(&e->list);
688 hwsim_free_edge(e);
689 }
690 }
691 }
692 rcu_read_unlock();
693
694 synchronize_rcu();
695 }
696
hwsim_subscribe_all_others(struct hwsim_phy * phy)697 static int hwsim_subscribe_all_others(struct hwsim_phy *phy)
698 {
699 struct hwsim_phy *sub;
700 struct hwsim_edge *e;
701
702 list_for_each_entry(sub, &hwsim_phys, list) {
703 e = hwsim_alloc_edge(sub, 0xff);
704 if (!e)
705 goto me_fail;
706
707 list_add_rcu(&e->list, &phy->edges);
708 }
709
710 list_for_each_entry(sub, &hwsim_phys, list) {
711 e = hwsim_alloc_edge(phy, 0xff);
712 if (!e)
713 goto sub_fail;
714
715 list_add_rcu(&e->list, &sub->edges);
716 }
717
718 return 0;
719
720 sub_fail:
721 hwsim_edge_unsubscribe_me(phy);
722 me_fail:
723 rcu_read_lock();
724 list_for_each_entry_rcu(e, &phy->edges, list) {
725 list_del_rcu(&e->list);
726 hwsim_free_edge(e);
727 }
728 rcu_read_unlock();
729 return -ENOMEM;
730 }
731
hwsim_add_one(struct genl_info * info,struct device * dev,bool init)732 static int hwsim_add_one(struct genl_info *info, struct device *dev,
733 bool init)
734 {
735 struct ieee802154_hw *hw;
736 struct hwsim_phy *phy;
737 struct hwsim_pib *pib;
738 int idx;
739 int err;
740
741 idx = hwsim_radio_idx++;
742
743 hw = ieee802154_alloc_hw(sizeof(*phy), &hwsim_ops);
744 if (!hw)
745 return -ENOMEM;
746
747 phy = hw->priv;
748 phy->hw = hw;
749
750 /* 868 MHz BPSK 802.15.4-2003 */
751 hw->phy->supported.channels[0] |= 1;
752 /* 915 MHz BPSK 802.15.4-2003 */
753 hw->phy->supported.channels[0] |= 0x7fe;
754 /* 2.4 GHz O-QPSK 802.15.4-2003 */
755 hw->phy->supported.channels[0] |= 0x7FFF800;
756 /* 868 MHz ASK 802.15.4-2006 */
757 hw->phy->supported.channels[1] |= 1;
758 /* 915 MHz ASK 802.15.4-2006 */
759 hw->phy->supported.channels[1] |= 0x7fe;
760 /* 868 MHz O-QPSK 802.15.4-2006 */
761 hw->phy->supported.channels[2] |= 1;
762 /* 915 MHz O-QPSK 802.15.4-2006 */
763 hw->phy->supported.channels[2] |= 0x7fe;
764 /* 2.4 GHz CSS 802.15.4a-2007 */
765 hw->phy->supported.channels[3] |= 0x3fff;
766 /* UWB Sub-gigahertz 802.15.4a-2007 */
767 hw->phy->supported.channels[4] |= 1;
768 /* UWB Low band 802.15.4a-2007 */
769 hw->phy->supported.channels[4] |= 0x1e;
770 /* UWB High band 802.15.4a-2007 */
771 hw->phy->supported.channels[4] |= 0xffe0;
772 /* 750 MHz O-QPSK 802.15.4c-2009 */
773 hw->phy->supported.channels[5] |= 0xf;
774 /* 750 MHz MPSK 802.15.4c-2009 */
775 hw->phy->supported.channels[5] |= 0xf0;
776 /* 950 MHz BPSK 802.15.4d-2009 */
777 hw->phy->supported.channels[6] |= 0x3ff;
778 /* 950 MHz GFSK 802.15.4d-2009 */
779 hw->phy->supported.channels[6] |= 0x3ffc00;
780
781 ieee802154_random_extended_addr(&hw->phy->perm_extended_addr);
782
783 /* hwsim phy channel 13 as default */
784 hw->phy->current_channel = 13;
785 pib = kzalloc(sizeof(*pib), GFP_KERNEL);
786 if (!pib) {
787 err = -ENOMEM;
788 goto err_pib;
789 }
790
791 pib->channel = 13;
792 rcu_assign_pointer(phy->pib, pib);
793 phy->idx = idx;
794 INIT_LIST_HEAD(&phy->edges);
795
796 hw->flags = IEEE802154_HW_PROMISCUOUS;
797 hw->parent = dev;
798
799 err = ieee802154_register_hw(hw);
800 if (err)
801 goto err_reg;
802
803 mutex_lock(&hwsim_phys_lock);
804 if (init) {
805 err = hwsim_subscribe_all_others(phy);
806 if (err < 0) {
807 mutex_unlock(&hwsim_phys_lock);
808 goto err_subscribe;
809 }
810 }
811 list_add_tail(&phy->list, &hwsim_phys);
812 mutex_unlock(&hwsim_phys_lock);
813
814 hwsim_mcast_new_radio(info, phy);
815
816 return idx;
817
818 err_subscribe:
819 ieee802154_unregister_hw(phy->hw);
820 err_reg:
821 kfree(pib);
822 err_pib:
823 ieee802154_free_hw(phy->hw);
824 return err;
825 }
826
hwsim_del(struct hwsim_phy * phy)827 static void hwsim_del(struct hwsim_phy *phy)
828 {
829 struct hwsim_pib *pib;
830 struct hwsim_edge *e;
831
832 hwsim_edge_unsubscribe_me(phy);
833
834 list_del(&phy->list);
835
836 rcu_read_lock();
837 list_for_each_entry_rcu(e, &phy->edges, list) {
838 list_del_rcu(&e->list);
839 hwsim_free_edge(e);
840 }
841 pib = rcu_dereference(phy->pib);
842 rcu_read_unlock();
843
844 kfree_rcu(pib, rcu);
845
846 ieee802154_unregister_hw(phy->hw);
847 ieee802154_free_hw(phy->hw);
848 }
849
hwsim_probe(struct platform_device * pdev)850 static int hwsim_probe(struct platform_device *pdev)
851 {
852 struct hwsim_phy *phy, *tmp;
853 int err, i;
854
855 for (i = 0; i < 2; i++) {
856 err = hwsim_add_one(NULL, &pdev->dev, true);
857 if (err < 0)
858 goto err_slave;
859 }
860
861 dev_info(&pdev->dev, "Added 2 mac802154 hwsim hardware radios\n");
862 return 0;
863
864 err_slave:
865 mutex_lock(&hwsim_phys_lock);
866 list_for_each_entry_safe(phy, tmp, &hwsim_phys, list)
867 hwsim_del(phy);
868 mutex_unlock(&hwsim_phys_lock);
869 return err;
870 }
871
hwsim_remove(struct platform_device * pdev)872 static int hwsim_remove(struct platform_device *pdev)
873 {
874 struct hwsim_phy *phy, *tmp;
875
876 mutex_lock(&hwsim_phys_lock);
877 list_for_each_entry_safe(phy, tmp, &hwsim_phys, list)
878 hwsim_del(phy);
879 mutex_unlock(&hwsim_phys_lock);
880
881 return 0;
882 }
883
884 static struct platform_driver mac802154hwsim_driver = {
885 .probe = hwsim_probe,
886 .remove = hwsim_remove,
887 .driver = {
888 .name = "mac802154_hwsim",
889 },
890 };
891
hwsim_init_module(void)892 static __init int hwsim_init_module(void)
893 {
894 int rc;
895
896 rc = genl_register_family(&hwsim_genl_family);
897 if (rc)
898 return rc;
899
900 mac802154hwsim_dev = platform_device_register_simple("mac802154_hwsim",
901 -1, NULL, 0);
902 if (IS_ERR(mac802154hwsim_dev)) {
903 rc = PTR_ERR(mac802154hwsim_dev);
904 goto platform_dev;
905 }
906
907 rc = platform_driver_register(&mac802154hwsim_driver);
908 if (rc < 0)
909 goto platform_drv;
910
911 return 0;
912
913 platform_drv:
914 platform_device_unregister(mac802154hwsim_dev);
915 platform_dev:
916 genl_unregister_family(&hwsim_genl_family);
917 return rc;
918 }
919
hwsim_remove_module(void)920 static __exit void hwsim_remove_module(void)
921 {
922 genl_unregister_family(&hwsim_genl_family);
923 platform_driver_unregister(&mac802154hwsim_driver);
924 platform_device_unregister(mac802154hwsim_dev);
925 }
926
927 module_init(hwsim_init_module);
928 module_exit(hwsim_remove_module);
929