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