1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3 Copyright (c) 2013-2014 Intel Corp.
4
5 */
6
7 #include <linux/if_arp.h>
8 #include <linux/netdevice.h>
9 #include <linux/etherdevice.h>
10 #include <linux/module.h>
11 #include <linux/debugfs.h>
12
13 #include <net/ipv6.h>
14 #include <net/ip6_route.h>
15 #include <net/addrconf.h>
16 #include <net/netdev_lock.h>
17 #include <net/pkt_sched.h>
18
19 #include <net/bluetooth/bluetooth.h>
20 #include <net/bluetooth/hci_core.h>
21 #include <net/bluetooth/l2cap.h>
22
23 #include <net/6lowpan.h> /* for the compression support */
24
25 #define VERSION "0.1"
26
27 static struct dentry *lowpan_enable_debugfs;
28 static struct dentry *lowpan_control_debugfs;
29
30 #define IFACE_NAME_TEMPLATE "bt%d"
31
32 struct skb_cb {
33 struct in6_addr addr;
34 struct in6_addr gw;
35 struct l2cap_chan *chan;
36 };
37 #define lowpan_cb(skb) ((struct skb_cb *)((skb)->cb))
38
39 /* The devices list contains those devices that we are acting
40 * as a proxy. The BT 6LoWPAN device is a virtual device that
41 * connects to the Bluetooth LE device. The real connection to
42 * BT device is done via l2cap layer. There exists one
43 * virtual device / one BT 6LoWPAN network (=hciX device).
44 * The list contains struct lowpan_dev elements.
45 */
46 static LIST_HEAD(bt_6lowpan_devices);
47 static DEFINE_SPINLOCK(devices_lock);
48
49 static bool enable_6lowpan;
50
51 /* We are listening incoming connections via this channel
52 */
53 static struct l2cap_chan *listen_chan;
54 static DEFINE_MUTEX(set_lock);
55
56 enum {
57 LOWPAN_PEER_CLOSING,
58 LOWPAN_PEER_MAXBITS
59 };
60
61 struct lowpan_peer {
62 struct list_head list;
63 struct rcu_head rcu;
64 struct l2cap_chan *chan;
65
66 /* peer addresses in various formats */
67 unsigned char lladdr[ETH_ALEN];
68 struct in6_addr peer_addr;
69
70 DECLARE_BITMAP(flags, LOWPAN_PEER_MAXBITS);
71 };
72
73 struct lowpan_btle_dev {
74 struct list_head list;
75
76 struct hci_dev *hdev;
77 struct net_device *netdev;
78 struct list_head peers;
79 atomic_t peer_count; /* number of items in peers list */
80
81 struct work_struct delete_netdev;
82 struct delayed_work notify_peers;
83 };
84
85 static inline struct lowpan_btle_dev *
lowpan_btle_dev(const struct net_device * netdev)86 lowpan_btle_dev(const struct net_device *netdev)
87 {
88 return (struct lowpan_btle_dev *)lowpan_dev(netdev)->priv;
89 }
90
peer_add(struct lowpan_btle_dev * dev,struct lowpan_peer * peer)91 static inline void peer_add(struct lowpan_btle_dev *dev,
92 struct lowpan_peer *peer)
93 {
94 list_add_rcu(&peer->list, &dev->peers);
95 atomic_inc(&dev->peer_count);
96 }
97
peer_del(struct lowpan_btle_dev * dev,struct lowpan_peer * peer)98 static inline bool peer_del(struct lowpan_btle_dev *dev,
99 struct lowpan_peer *peer)
100 {
101 list_del_rcu(&peer->list);
102 kfree_rcu(peer, rcu);
103
104 module_put(THIS_MODULE);
105
106 if (atomic_dec_and_test(&dev->peer_count)) {
107 BT_DBG("last peer");
108 return true;
109 }
110
111 return false;
112 }
113
114 static inline struct lowpan_peer *
__peer_lookup_chan(struct lowpan_btle_dev * dev,struct l2cap_chan * chan)115 __peer_lookup_chan(struct lowpan_btle_dev *dev, struct l2cap_chan *chan)
116 {
117 struct lowpan_peer *peer;
118
119 list_for_each_entry_rcu(peer, &dev->peers, list) {
120 if (peer->chan == chan)
121 return peer;
122 }
123
124 return NULL;
125 }
126
127 static inline struct lowpan_peer *
__peer_lookup_conn(struct lowpan_btle_dev * dev,struct l2cap_conn * conn)128 __peer_lookup_conn(struct lowpan_btle_dev *dev, struct l2cap_conn *conn)
129 {
130 struct lowpan_peer *peer;
131
132 list_for_each_entry_rcu(peer, &dev->peers, list) {
133 if (peer->chan->conn == conn)
134 return peer;
135 }
136
137 return NULL;
138 }
139
peer_lookup_dst(struct lowpan_btle_dev * dev,struct in6_addr * daddr,struct sk_buff * skb)140 static inline struct lowpan_peer *peer_lookup_dst(struct lowpan_btle_dev *dev,
141 struct in6_addr *daddr,
142 struct sk_buff *skb)
143 {
144 struct rt6_info *rt = dst_rt6_info(skb_dst(skb));
145 int count = atomic_read(&dev->peer_count);
146 const struct in6_addr *nexthop;
147 struct lowpan_peer *peer;
148 struct neighbour *neigh;
149
150 BT_DBG("peers %d addr %pI6c rt %p", count, daddr, rt);
151
152 if (!rt) {
153 if (ipv6_addr_any(&lowpan_cb(skb)->gw)) {
154 /* There is neither route nor gateway,
155 * probably the destination is a direct peer.
156 */
157 nexthop = daddr;
158 } else {
159 /* There is a known gateway
160 */
161 nexthop = &lowpan_cb(skb)->gw;
162 }
163 } else {
164 nexthop = rt6_nexthop(rt, daddr);
165
166 /* We need to remember the address because it is needed
167 * by bt_xmit() when sending the packet. In bt_xmit(), the
168 * destination routing info is not set.
169 */
170 memcpy(&lowpan_cb(skb)->gw, nexthop, sizeof(struct in6_addr));
171 }
172
173 BT_DBG("gw %pI6c", nexthop);
174
175 rcu_read_lock();
176
177 list_for_each_entry_rcu(peer, &dev->peers, list) {
178 BT_DBG("dst addr %pMR dst type %u ip %pI6c",
179 &peer->chan->dst, peer->chan->dst_type,
180 &peer->peer_addr);
181
182 if (!ipv6_addr_cmp(&peer->peer_addr, nexthop)) {
183 rcu_read_unlock();
184 return peer;
185 }
186 }
187
188 /* use the neighbour cache for matching addresses assigned by SLAAC */
189 neigh = __ipv6_neigh_lookup(dev->netdev, nexthop);
190 if (neigh) {
191 list_for_each_entry_rcu(peer, &dev->peers, list) {
192 if (!memcmp(neigh->ha, peer->lladdr, ETH_ALEN)) {
193 neigh_release(neigh);
194 rcu_read_unlock();
195 return peer;
196 }
197 }
198 neigh_release(neigh);
199 }
200
201 rcu_read_unlock();
202
203 return NULL;
204 }
205
lookup_peer(struct l2cap_conn * conn)206 static struct lowpan_peer *lookup_peer(struct l2cap_conn *conn)
207 {
208 struct lowpan_btle_dev *entry;
209 struct lowpan_peer *peer = NULL;
210
211 rcu_read_lock();
212
213 list_for_each_entry_rcu(entry, &bt_6lowpan_devices, list) {
214 peer = __peer_lookup_conn(entry, conn);
215 if (peer)
216 break;
217 }
218
219 rcu_read_unlock();
220
221 return peer;
222 }
223
lookup_dev(struct l2cap_conn * conn)224 static struct lowpan_btle_dev *lookup_dev(struct l2cap_conn *conn)
225 {
226 struct lowpan_btle_dev *entry;
227 struct lowpan_btle_dev *dev = NULL;
228
229 rcu_read_lock();
230
231 list_for_each_entry_rcu(entry, &bt_6lowpan_devices, list) {
232 if (conn->hcon->hdev == entry->hdev) {
233 dev = entry;
234 break;
235 }
236 }
237
238 rcu_read_unlock();
239
240 return dev;
241 }
242
give_skb_to_upper(struct sk_buff * skb,struct net_device * dev)243 static int give_skb_to_upper(struct sk_buff *skb, struct net_device *dev)
244 {
245 struct sk_buff *skb_cp;
246
247 skb_cp = skb_copy(skb, GFP_ATOMIC);
248 if (!skb_cp)
249 return NET_RX_DROP;
250
251 return netif_rx(skb_cp);
252 }
253
iphc_decompress(struct sk_buff * skb,struct net_device * netdev,struct lowpan_peer * peer)254 static int iphc_decompress(struct sk_buff *skb, struct net_device *netdev,
255 struct lowpan_peer *peer)
256 {
257 const u8 *saddr;
258
259 saddr = peer->lladdr;
260
261 return lowpan_header_decompress(skb, netdev, netdev->dev_addr, saddr);
262 }
263
recv_pkt(struct sk_buff * skb,struct net_device * dev,struct lowpan_peer * peer)264 static int recv_pkt(struct sk_buff *skb, struct net_device *dev,
265 struct lowpan_peer *peer)
266 {
267 struct sk_buff *local_skb;
268 int ret;
269
270 if (!netif_running(dev))
271 goto drop;
272
273 if (dev->type != ARPHRD_6LOWPAN || !skb->len)
274 goto drop;
275
276 skb_reset_network_header(skb);
277
278 skb = skb_share_check(skb, GFP_ATOMIC);
279 if (!skb)
280 goto drop;
281
282 /* check that it's our buffer */
283 if (lowpan_is_ipv6(*skb_network_header(skb))) {
284 /* Pull off the 1-byte of 6lowpan header. */
285 skb_pull(skb, 1);
286
287 /* Copy the packet so that the IPv6 header is
288 * properly aligned.
289 */
290 local_skb = skb_copy_expand(skb, NET_SKB_PAD - 1,
291 skb_tailroom(skb), GFP_ATOMIC);
292 if (!local_skb)
293 goto drop;
294
295 local_skb->protocol = htons(ETH_P_IPV6);
296 local_skb->pkt_type = PACKET_HOST;
297 local_skb->dev = dev;
298
299 skb_reset_mac_header(local_skb);
300 skb_set_transport_header(local_skb, sizeof(struct ipv6hdr));
301
302 if (give_skb_to_upper(local_skb, dev) != NET_RX_SUCCESS) {
303 kfree_skb(local_skb);
304 goto drop;
305 }
306
307 dev->stats.rx_bytes += skb->len;
308 dev->stats.rx_packets++;
309
310 consume_skb(local_skb);
311 consume_skb(skb);
312 } else if (lowpan_is_iphc(*skb_network_header(skb))) {
313 local_skb = skb_clone(skb, GFP_ATOMIC);
314 if (!local_skb)
315 goto drop;
316
317 local_skb->dev = dev;
318
319 ret = iphc_decompress(local_skb, dev, peer);
320 if (ret < 0) {
321 BT_DBG("iphc_decompress failed: %d", ret);
322 kfree_skb(local_skb);
323 goto drop;
324 }
325
326 local_skb->protocol = htons(ETH_P_IPV6);
327 local_skb->pkt_type = PACKET_HOST;
328
329 if (give_skb_to_upper(local_skb, dev)
330 != NET_RX_SUCCESS) {
331 kfree_skb(local_skb);
332 goto drop;
333 }
334
335 dev->stats.rx_bytes += skb->len;
336 dev->stats.rx_packets++;
337
338 consume_skb(local_skb);
339 consume_skb(skb);
340 } else {
341 BT_DBG("unknown packet type");
342 goto drop;
343 }
344
345 return NET_RX_SUCCESS;
346
347 drop:
348 dev->stats.rx_dropped++;
349 return NET_RX_DROP;
350 }
351
352 /* Packet from BT LE device */
chan_recv_cb(struct l2cap_chan * chan,struct sk_buff * skb)353 static int chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
354 {
355 struct lowpan_btle_dev *dev;
356 struct lowpan_peer *peer;
357 int err;
358
359 peer = lookup_peer(chan->conn);
360 if (!peer)
361 return -ENOENT;
362
363 dev = lookup_dev(chan->conn);
364 if (!dev || !dev->netdev)
365 return -ENOENT;
366
367 err = recv_pkt(skb, dev->netdev, peer);
368 if (err) {
369 BT_DBG("recv pkt %d", err);
370 err = -EAGAIN;
371 }
372
373 return err;
374 }
375
setup_header(struct sk_buff * skb,struct net_device * netdev,bdaddr_t * peer_addr,u8 * peer_addr_type)376 static int setup_header(struct sk_buff *skb, struct net_device *netdev,
377 bdaddr_t *peer_addr, u8 *peer_addr_type)
378 {
379 struct in6_addr ipv6_daddr;
380 struct ipv6hdr *hdr;
381 struct lowpan_btle_dev *dev;
382 struct lowpan_peer *peer;
383 u8 *daddr;
384 int err, status = 0;
385
386 hdr = ipv6_hdr(skb);
387
388 dev = lowpan_btle_dev(netdev);
389
390 memcpy(&ipv6_daddr, &hdr->daddr, sizeof(ipv6_daddr));
391
392 if (ipv6_addr_is_multicast(&ipv6_daddr)) {
393 lowpan_cb(skb)->chan = NULL;
394 daddr = NULL;
395 } else {
396 BT_DBG("dest IP %pI6c", &ipv6_daddr);
397
398 /* The packet might be sent to 6lowpan interface
399 * because of routing (either via default route
400 * or user set route) so get peer according to
401 * the destination address.
402 */
403 peer = peer_lookup_dst(dev, &ipv6_daddr, skb);
404 if (!peer) {
405 BT_DBG("no such peer");
406 return -ENOENT;
407 }
408
409 daddr = peer->lladdr;
410 *peer_addr = peer->chan->dst;
411 *peer_addr_type = peer->chan->dst_type;
412 lowpan_cb(skb)->chan = peer->chan;
413
414 status = 1;
415 }
416
417 lowpan_header_compress(skb, netdev, daddr, dev->netdev->dev_addr);
418
419 err = dev_hard_header(skb, netdev, ETH_P_IPV6, NULL, NULL, 0);
420 if (err < 0)
421 return err;
422
423 return status;
424 }
425
header_create(struct sk_buff * skb,struct net_device * netdev,unsigned short type,const void * _daddr,const void * _saddr,unsigned int len)426 static int header_create(struct sk_buff *skb, struct net_device *netdev,
427 unsigned short type, const void *_daddr,
428 const void *_saddr, unsigned int len)
429 {
430 if (type != ETH_P_IPV6)
431 return -EINVAL;
432
433 return 0;
434 }
435
436 /* Packet to BT LE device */
send_pkt(struct l2cap_chan * chan,struct sk_buff * skb,struct net_device * netdev)437 static int send_pkt(struct l2cap_chan *chan, struct sk_buff *skb,
438 struct net_device *netdev)
439 {
440 struct msghdr msg;
441 struct kvec iv;
442 int err;
443
444 /* Remember the skb so that we can send EAGAIN to the caller if
445 * we run out of credits.
446 */
447 chan->data = skb;
448
449 iv.iov_base = skb->data;
450 iv.iov_len = skb->len;
451
452 memset(&msg, 0, sizeof(msg));
453 iov_iter_kvec(&msg.msg_iter, ITER_SOURCE, &iv, 1, skb->len);
454
455 err = l2cap_chan_send(chan, &msg, skb->len, NULL);
456 if (err > 0) {
457 netdev->stats.tx_bytes += err;
458 netdev->stats.tx_packets++;
459 return 0;
460 }
461
462 if (err < 0)
463 netdev->stats.tx_errors++;
464
465 return err;
466 }
467
send_mcast_pkt(struct sk_buff * skb,struct net_device * netdev)468 static int send_mcast_pkt(struct sk_buff *skb, struct net_device *netdev)
469 {
470 struct sk_buff *local_skb;
471 struct lowpan_btle_dev *entry;
472 int err = 0;
473
474 rcu_read_lock();
475
476 list_for_each_entry_rcu(entry, &bt_6lowpan_devices, list) {
477 struct lowpan_peer *pentry;
478 struct lowpan_btle_dev *dev;
479
480 if (entry->netdev != netdev)
481 continue;
482
483 dev = lowpan_btle_dev(entry->netdev);
484
485 list_for_each_entry_rcu(pentry, &dev->peers, list) {
486 int ret;
487
488 local_skb = skb_clone(skb, GFP_ATOMIC);
489 if (!local_skb)
490 continue;
491
492 BT_DBG("xmit %s to %pMR type %u IP %pI6c chan %p",
493 netdev->name,
494 &pentry->chan->dst, pentry->chan->dst_type,
495 &pentry->peer_addr, pentry->chan);
496 ret = send_pkt(pentry->chan, local_skb, netdev);
497 if (ret < 0)
498 err = ret;
499
500 kfree_skb(local_skb);
501 }
502 }
503
504 rcu_read_unlock();
505
506 return err;
507 }
508
bt_xmit(struct sk_buff * skb,struct net_device * netdev)509 static netdev_tx_t bt_xmit(struct sk_buff *skb, struct net_device *netdev)
510 {
511 int err = 0;
512 bdaddr_t addr;
513 u8 addr_type;
514
515 /* We must take a copy of the skb before we modify/replace the ipv6
516 * header as the header could be used elsewhere
517 */
518 skb = skb_unshare(skb, GFP_ATOMIC);
519 if (!skb)
520 return NET_XMIT_DROP;
521
522 /* Return values from setup_header()
523 * <0 - error, packet is dropped
524 * 0 - this is a multicast packet
525 * 1 - this is unicast packet
526 */
527 err = setup_header(skb, netdev, &addr, &addr_type);
528 if (err < 0) {
529 kfree_skb(skb);
530 return NET_XMIT_DROP;
531 }
532
533 if (err) {
534 if (lowpan_cb(skb)->chan) {
535 BT_DBG("xmit %s to %pMR type %u IP %pI6c chan %p",
536 netdev->name, &addr, addr_type,
537 &lowpan_cb(skb)->addr, lowpan_cb(skb)->chan);
538 err = send_pkt(lowpan_cb(skb)->chan, skb, netdev);
539 } else {
540 err = -ENOENT;
541 }
542 } else {
543 /* We need to send the packet to every device behind this
544 * interface.
545 */
546 err = send_mcast_pkt(skb, netdev);
547 }
548
549 dev_kfree_skb(skb);
550
551 if (err)
552 BT_DBG("ERROR: xmit failed (%d)", err);
553
554 return err < 0 ? NET_XMIT_DROP : err;
555 }
556
bt_dev_init(struct net_device * dev)557 static int bt_dev_init(struct net_device *dev)
558 {
559 netdev_lockdep_set_classes(dev);
560
561 return 0;
562 }
563
564 static const struct net_device_ops netdev_ops = {
565 .ndo_init = bt_dev_init,
566 .ndo_start_xmit = bt_xmit,
567 };
568
569 static const struct header_ops header_ops = {
570 .create = header_create,
571 };
572
netdev_setup(struct net_device * dev)573 static void netdev_setup(struct net_device *dev)
574 {
575 dev->hard_header_len = 0;
576 dev->needed_tailroom = 0;
577 dev->flags = IFF_RUNNING | IFF_MULTICAST;
578 dev->watchdog_timeo = 0;
579 dev->tx_queue_len = DEFAULT_TX_QUEUE_LEN;
580
581 dev->netdev_ops = &netdev_ops;
582 dev->header_ops = &header_ops;
583 dev->needs_free_netdev = true;
584 }
585
586 static const struct device_type bt_type = {
587 .name = "bluetooth",
588 };
589
ifup(struct net_device * netdev)590 static void ifup(struct net_device *netdev)
591 {
592 int err;
593
594 rtnl_lock();
595 err = dev_open(netdev, NULL);
596 if (err < 0)
597 BT_INFO("iface %s cannot be opened (%d)", netdev->name, err);
598 rtnl_unlock();
599 }
600
ifdown(struct net_device * netdev)601 static void ifdown(struct net_device *netdev)
602 {
603 rtnl_lock();
604 dev_close(netdev);
605 rtnl_unlock();
606 }
607
do_notify_peers(struct work_struct * work)608 static void do_notify_peers(struct work_struct *work)
609 {
610 struct lowpan_btle_dev *dev = container_of(work, struct lowpan_btle_dev,
611 notify_peers.work);
612
613 netdev_notify_peers(dev->netdev); /* send neighbour adv at startup */
614 }
615
is_bt_6lowpan(struct hci_conn * hcon)616 static bool is_bt_6lowpan(struct hci_conn *hcon)
617 {
618 if (hcon->type != LE_LINK)
619 return false;
620
621 if (!enable_6lowpan)
622 return false;
623
624 return true;
625 }
626
chan_create(void)627 static struct l2cap_chan *chan_create(void)
628 {
629 struct l2cap_chan *chan;
630
631 chan = l2cap_chan_create();
632 if (!chan)
633 return NULL;
634
635 l2cap_chan_set_defaults(chan);
636
637 chan->chan_type = L2CAP_CHAN_CONN_ORIENTED;
638 chan->mode = L2CAP_MODE_LE_FLOWCTL;
639 chan->imtu = 1280;
640
641 return chan;
642 }
643
add_peer_chan(struct l2cap_chan * chan,struct lowpan_btle_dev * dev,bool new_netdev)644 static struct l2cap_chan *add_peer_chan(struct l2cap_chan *chan,
645 struct lowpan_btle_dev *dev,
646 bool new_netdev)
647 {
648 struct lowpan_peer *peer;
649
650 peer = kzalloc_obj(*peer, GFP_ATOMIC);
651 if (!peer)
652 return NULL;
653
654 peer->chan = chan;
655
656 baswap((void *)peer->lladdr, &chan->dst);
657
658 lowpan_iphc_uncompress_eui48_lladdr(&peer->peer_addr, peer->lladdr);
659
660 spin_lock(&devices_lock);
661 INIT_LIST_HEAD(&peer->list);
662 peer_add(dev, peer);
663 spin_unlock(&devices_lock);
664
665 /* Notifying peers about us needs to be done without locks held */
666 if (new_netdev)
667 INIT_DELAYED_WORK(&dev->notify_peers, do_notify_peers);
668 schedule_delayed_work(&dev->notify_peers, msecs_to_jiffies(100));
669
670 return peer->chan;
671 }
672
setup_netdev(struct l2cap_chan * chan,struct lowpan_btle_dev ** dev)673 static int setup_netdev(struct l2cap_chan *chan, struct lowpan_btle_dev **dev)
674 {
675 struct net_device *netdev;
676 bdaddr_t addr;
677 int err;
678
679 netdev = alloc_netdev(LOWPAN_PRIV_SIZE(sizeof(struct lowpan_btle_dev)),
680 IFACE_NAME_TEMPLATE, NET_NAME_UNKNOWN,
681 netdev_setup);
682 if (!netdev)
683 return -ENOMEM;
684
685 netdev->addr_assign_type = NET_ADDR_PERM;
686 baswap(&addr, &chan->src);
687 __dev_addr_set(netdev, &addr, sizeof(addr));
688
689 netdev->netdev_ops = &netdev_ops;
690 SET_NETDEV_DEV(netdev, &chan->conn->hcon->hdev->dev);
691 SET_NETDEV_DEVTYPE(netdev, &bt_type);
692
693 *dev = lowpan_btle_dev(netdev);
694 (*dev)->netdev = netdev;
695 (*dev)->hdev = chan->conn->hcon->hdev;
696 INIT_LIST_HEAD(&(*dev)->peers);
697
698 spin_lock(&devices_lock);
699 INIT_LIST_HEAD(&(*dev)->list);
700 list_add_rcu(&(*dev)->list, &bt_6lowpan_devices);
701 spin_unlock(&devices_lock);
702
703 err = lowpan_register_netdev(netdev, LOWPAN_LLTYPE_BTLE);
704 if (err < 0) {
705 BT_INFO("register_netdev failed %d", err);
706 spin_lock(&devices_lock);
707 list_del_rcu(&(*dev)->list);
708 spin_unlock(&devices_lock);
709 free_netdev(netdev);
710 goto out;
711 }
712
713 BT_DBG("ifindex %d peer bdaddr %pMR type %d my addr %pMR type %d",
714 netdev->ifindex, &chan->dst, chan->dst_type,
715 &chan->src, chan->src_type);
716 set_bit(__LINK_STATE_PRESENT, &netdev->state);
717
718 return 0;
719
720 out:
721 return err;
722 }
723
chan_ready_cb(struct l2cap_chan * chan)724 static inline void chan_ready_cb(struct l2cap_chan *chan)
725 {
726 struct lowpan_btle_dev *dev;
727 bool new_netdev = false;
728
729 dev = lookup_dev(chan->conn);
730
731 BT_DBG("chan %p conn %p dev %p", chan, chan->conn, dev);
732
733 if (!dev) {
734 if (setup_netdev(chan, &dev) < 0) {
735 l2cap_chan_del(chan, -ENOENT);
736 return;
737 }
738 new_netdev = true;
739 }
740
741 if (!try_module_get(THIS_MODULE))
742 return;
743
744 add_peer_chan(chan, dev, new_netdev);
745 ifup(dev->netdev);
746 }
747
chan_new_conn_cb(struct l2cap_chan * pchan)748 static inline struct l2cap_chan *chan_new_conn_cb(struct l2cap_chan *pchan)
749 {
750 struct l2cap_chan *chan;
751
752 chan = chan_create();
753 if (!chan)
754 return NULL;
755
756 chan->ops = pchan->ops;
757
758 BT_DBG("chan %p pchan %p", chan, pchan);
759
760 return chan;
761 }
762
delete_netdev(struct work_struct * work)763 static void delete_netdev(struct work_struct *work)
764 {
765 struct lowpan_btle_dev *entry = container_of(work,
766 struct lowpan_btle_dev,
767 delete_netdev);
768
769 lowpan_unregister_netdev(entry->netdev);
770
771 /* The entry pointer is deleted by the netdev destructor. */
772 }
773
chan_close_cb(struct l2cap_chan * chan)774 static void chan_close_cb(struct l2cap_chan *chan)
775 {
776 struct lowpan_btle_dev *entry;
777 struct lowpan_btle_dev *dev = NULL;
778 struct lowpan_peer *peer;
779 int err = -ENOENT;
780 bool last = false, remove = true;
781
782 BT_DBG("chan %p conn %p", chan, chan->conn);
783
784 if (chan->conn && chan->conn->hcon) {
785 if (!is_bt_6lowpan(chan->conn->hcon))
786 return;
787
788 /* If conn is set, then the netdev is also there and we should
789 * not remove it.
790 */
791 remove = false;
792 }
793
794 spin_lock(&devices_lock);
795
796 list_for_each_entry_rcu(entry, &bt_6lowpan_devices, list) {
797 dev = lowpan_btle_dev(entry->netdev);
798 peer = __peer_lookup_chan(dev, chan);
799 if (peer) {
800 last = peer_del(dev, peer);
801 err = 0;
802
803 BT_DBG("dev %p removing %speer %p", dev,
804 last ? "last " : "1 ", peer);
805 BT_DBG("chan %p orig refcnt %u", chan,
806 kref_read(&chan->kref));
807
808 l2cap_chan_put(chan);
809 break;
810 }
811 }
812
813 if (!err && last && dev && !atomic_read(&dev->peer_count)) {
814 spin_unlock(&devices_lock);
815
816 cancel_delayed_work_sync(&dev->notify_peers);
817
818 ifdown(dev->netdev);
819
820 if (remove) {
821 INIT_WORK(&entry->delete_netdev, delete_netdev);
822 schedule_work(&entry->delete_netdev);
823 }
824 } else {
825 spin_unlock(&devices_lock);
826 }
827 }
828
chan_state_change_cb(struct l2cap_chan * chan,int state,int err)829 static void chan_state_change_cb(struct l2cap_chan *chan, int state, int err)
830 {
831 BT_DBG("chan %p conn %p state %s err %d", chan, chan->conn,
832 state_to_string(state), err);
833 }
834
chan_alloc_skb_cb(struct l2cap_chan * chan,unsigned long hdr_len,unsigned long len,int nb)835 static struct sk_buff *chan_alloc_skb_cb(struct l2cap_chan *chan,
836 unsigned long hdr_len,
837 unsigned long len, int nb)
838 {
839 struct sk_buff *skb;
840
841 /* Note that we must allocate using GFP_ATOMIC here as
842 * this function is called originally from netdev hard xmit
843 * function in atomic context.
844 */
845 skb = bt_skb_alloc(hdr_len + len, GFP_ATOMIC);
846 if (!skb)
847 return ERR_PTR(-ENOMEM);
848 return skb;
849 }
850
chan_suspend_cb(struct l2cap_chan * chan)851 static void chan_suspend_cb(struct l2cap_chan *chan)
852 {
853 struct lowpan_btle_dev *dev;
854
855 BT_DBG("chan %p suspend", chan);
856
857 dev = lookup_dev(chan->conn);
858 if (!dev || !dev->netdev)
859 return;
860
861 netif_stop_queue(dev->netdev);
862 }
863
chan_resume_cb(struct l2cap_chan * chan)864 static void chan_resume_cb(struct l2cap_chan *chan)
865 {
866 struct lowpan_btle_dev *dev;
867
868 BT_DBG("chan %p resume", chan);
869
870 dev = lookup_dev(chan->conn);
871 if (!dev || !dev->netdev)
872 return;
873
874 netif_wake_queue(dev->netdev);
875 }
876
chan_get_sndtimeo_cb(struct l2cap_chan * chan)877 static long chan_get_sndtimeo_cb(struct l2cap_chan *chan)
878 {
879 return L2CAP_CONN_TIMEOUT;
880 }
881
882 static const struct l2cap_ops bt_6lowpan_chan_ops = {
883 .name = "L2CAP 6LoWPAN channel",
884 .new_connection = chan_new_conn_cb,
885 .recv = chan_recv_cb,
886 .close = chan_close_cb,
887 .state_change = chan_state_change_cb,
888 .ready = chan_ready_cb,
889 .resume = chan_resume_cb,
890 .suspend = chan_suspend_cb,
891 .get_sndtimeo = chan_get_sndtimeo_cb,
892 .alloc_skb = chan_alloc_skb_cb,
893
894 .teardown = l2cap_chan_no_teardown,
895 .defer = l2cap_chan_no_defer,
896 .set_shutdown = l2cap_chan_no_set_shutdown,
897 };
898
bt_6lowpan_connect(bdaddr_t * addr,u8 dst_type)899 static int bt_6lowpan_connect(bdaddr_t *addr, u8 dst_type)
900 {
901 struct l2cap_chan *chan;
902 int err;
903
904 chan = chan_create();
905 if (!chan)
906 return -EINVAL;
907
908 chan->ops = &bt_6lowpan_chan_ops;
909
910 err = l2cap_chan_connect(chan, cpu_to_le16(L2CAP_PSM_IPSP), 0,
911 addr, dst_type, L2CAP_CONN_TIMEOUT);
912
913 BT_DBG("chan %p err %d", chan, err);
914 if (err < 0)
915 l2cap_chan_put(chan);
916
917 return err;
918 }
919
bt_6lowpan_disconnect(struct l2cap_conn * conn,u8 dst_type)920 static int bt_6lowpan_disconnect(struct l2cap_conn *conn, u8 dst_type)
921 {
922 struct lowpan_peer *peer;
923
924 BT_DBG("conn %p dst type %u", conn, dst_type);
925
926 peer = lookup_peer(conn);
927 if (!peer)
928 return -ENOENT;
929
930 BT_DBG("peer %p chan %p", peer, peer->chan);
931
932 l2cap_chan_lock(peer->chan);
933 l2cap_chan_close(peer->chan, ENOENT);
934 l2cap_chan_unlock(peer->chan);
935
936 return 0;
937 }
938
bt_6lowpan_listen(void)939 static struct l2cap_chan *bt_6lowpan_listen(void)
940 {
941 bdaddr_t *addr = BDADDR_ANY;
942 struct l2cap_chan *chan;
943 int err;
944
945 if (!enable_6lowpan)
946 return NULL;
947
948 chan = chan_create();
949 if (!chan)
950 return NULL;
951
952 chan->ops = &bt_6lowpan_chan_ops;
953 chan->state = BT_LISTEN;
954 chan->src_type = BDADDR_LE_PUBLIC;
955
956 atomic_set(&chan->nesting, L2CAP_NESTING_PARENT);
957
958 BT_DBG("chan %p src type %u", chan, chan->src_type);
959
960 err = l2cap_add_psm(chan, addr, cpu_to_le16(L2CAP_PSM_IPSP));
961 if (err) {
962 l2cap_chan_put(chan);
963 BT_ERR("psm cannot be added err %d", err);
964 return NULL;
965 }
966
967 return chan;
968 }
969
get_l2cap_conn(char * buf,bdaddr_t * addr,u8 * addr_type,struct l2cap_conn ** conn,bool disconnect)970 static int get_l2cap_conn(char *buf, bdaddr_t *addr, u8 *addr_type,
971 struct l2cap_conn **conn, bool disconnect)
972 {
973 struct hci_conn *hcon;
974 struct hci_dev *hdev;
975 int le_addr_type;
976 int n;
977
978 n = sscanf(buf, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx %hhu",
979 &addr->b[5], &addr->b[4], &addr->b[3],
980 &addr->b[2], &addr->b[1], &addr->b[0],
981 addr_type);
982
983 if (n < 7)
984 return -EINVAL;
985
986 if (disconnect) {
987 /* The "disconnect" debugfs command has used different address
988 * type constants than "connect" since 2015. Let's retain that
989 * for now even though it's obviously buggy...
990 */
991 *addr_type += 1;
992 }
993
994 switch (*addr_type) {
995 case BDADDR_LE_PUBLIC:
996 le_addr_type = ADDR_LE_DEV_PUBLIC;
997 break;
998 case BDADDR_LE_RANDOM:
999 le_addr_type = ADDR_LE_DEV_RANDOM;
1000 break;
1001 default:
1002 return -EINVAL;
1003 }
1004
1005 /* The LE_PUBLIC address type is ignored because of BDADDR_ANY */
1006 hdev = hci_get_route(addr, BDADDR_ANY, BDADDR_LE_PUBLIC);
1007 if (!hdev)
1008 return -ENOENT;
1009
1010 hci_dev_lock(hdev);
1011 hcon = hci_conn_hash_lookup_le(hdev, addr, le_addr_type);
1012 hci_dev_unlock(hdev);
1013 hci_dev_put(hdev);
1014
1015 if (!hcon)
1016 return -ENOENT;
1017
1018 *conn = (struct l2cap_conn *)hcon->l2cap_data;
1019
1020 BT_DBG("conn %p dst %pMR type %u", *conn, &hcon->dst, hcon->dst_type);
1021
1022 return 0;
1023 }
1024
disconnect_all_peers(void)1025 static void disconnect_all_peers(void)
1026 {
1027 struct lowpan_btle_dev *entry;
1028 struct lowpan_peer *peer;
1029 int nchans;
1030
1031 /* l2cap_chan_close() cannot be called from RCU, and lock ordering
1032 * chan->lock > devices_lock prevents taking write side lock, so copy
1033 * then close.
1034 */
1035
1036 rcu_read_lock();
1037 list_for_each_entry_rcu(entry, &bt_6lowpan_devices, list)
1038 list_for_each_entry_rcu(peer, &entry->peers, list)
1039 clear_bit(LOWPAN_PEER_CLOSING, peer->flags);
1040 rcu_read_unlock();
1041
1042 do {
1043 struct l2cap_chan *chans[32];
1044 int i;
1045
1046 nchans = 0;
1047
1048 spin_lock(&devices_lock);
1049
1050 list_for_each_entry_rcu(entry, &bt_6lowpan_devices, list) {
1051 list_for_each_entry_rcu(peer, &entry->peers, list) {
1052 if (test_and_set_bit(LOWPAN_PEER_CLOSING,
1053 peer->flags))
1054 continue;
1055
1056 l2cap_chan_hold(peer->chan);
1057 chans[nchans++] = peer->chan;
1058
1059 if (nchans >= ARRAY_SIZE(chans))
1060 goto done;
1061 }
1062 }
1063
1064 done:
1065 spin_unlock(&devices_lock);
1066
1067 for (i = 0; i < nchans; ++i) {
1068 l2cap_chan_lock(chans[i]);
1069 l2cap_chan_close(chans[i], ENOENT);
1070 l2cap_chan_unlock(chans[i]);
1071 l2cap_chan_put(chans[i]);
1072 }
1073 } while (nchans);
1074 }
1075
1076 struct set_enable {
1077 struct work_struct work;
1078 bool flag;
1079 };
1080
do_enable_set(struct work_struct * work)1081 static void do_enable_set(struct work_struct *work)
1082 {
1083 struct set_enable *set_enable = container_of(work,
1084 struct set_enable, work);
1085
1086 if (!set_enable->flag || enable_6lowpan != set_enable->flag)
1087 /* Disconnect existing connections if 6lowpan is
1088 * disabled
1089 */
1090 disconnect_all_peers();
1091
1092 enable_6lowpan = set_enable->flag;
1093
1094 mutex_lock(&set_lock);
1095 if (listen_chan) {
1096 l2cap_chan_lock(listen_chan);
1097 l2cap_chan_close(listen_chan, 0);
1098 l2cap_chan_unlock(listen_chan);
1099 l2cap_chan_put(listen_chan);
1100 }
1101
1102 listen_chan = bt_6lowpan_listen();
1103 mutex_unlock(&set_lock);
1104
1105 kfree(set_enable);
1106 }
1107
lowpan_enable_set(void * data,u64 val)1108 static int lowpan_enable_set(void *data, u64 val)
1109 {
1110 struct set_enable *set_enable;
1111
1112 set_enable = kzalloc_obj(*set_enable);
1113 if (!set_enable)
1114 return -ENOMEM;
1115
1116 set_enable->flag = !!val;
1117 INIT_WORK(&set_enable->work, do_enable_set);
1118
1119 schedule_work(&set_enable->work);
1120
1121 return 0;
1122 }
1123
lowpan_enable_get(void * data,u64 * val)1124 static int lowpan_enable_get(void *data, u64 *val)
1125 {
1126 *val = enable_6lowpan;
1127 return 0;
1128 }
1129
1130 DEFINE_DEBUGFS_ATTRIBUTE(lowpan_enable_fops, lowpan_enable_get,
1131 lowpan_enable_set, "%llu\n");
1132
lowpan_control_write(struct file * fp,const char __user * user_buffer,size_t count,loff_t * position)1133 static ssize_t lowpan_control_write(struct file *fp,
1134 const char __user *user_buffer,
1135 size_t count,
1136 loff_t *position)
1137 {
1138 char buf[32];
1139 size_t buf_size = min(count, sizeof(buf) - 1);
1140 int ret;
1141 bdaddr_t addr;
1142 u8 addr_type;
1143 struct l2cap_conn *conn = NULL;
1144
1145 if (copy_from_user(buf, user_buffer, buf_size))
1146 return -EFAULT;
1147
1148 buf[buf_size] = '\0';
1149
1150 if (memcmp(buf, "connect ", 8) == 0) {
1151 ret = get_l2cap_conn(&buf[8], &addr, &addr_type, &conn, false);
1152 if (ret == -EINVAL)
1153 return ret;
1154
1155 mutex_lock(&set_lock);
1156 if (listen_chan) {
1157 l2cap_chan_lock(listen_chan);
1158 l2cap_chan_close(listen_chan, 0);
1159 l2cap_chan_unlock(listen_chan);
1160 l2cap_chan_put(listen_chan);
1161 listen_chan = NULL;
1162 }
1163 mutex_unlock(&set_lock);
1164
1165 if (conn) {
1166 struct lowpan_peer *peer;
1167
1168 if (!is_bt_6lowpan(conn->hcon))
1169 return -EINVAL;
1170
1171 peer = lookup_peer(conn);
1172 if (peer) {
1173 BT_DBG("6LoWPAN connection already exists");
1174 return -EALREADY;
1175 }
1176
1177 BT_DBG("conn %p dst %pMR type %d user %u", conn,
1178 &conn->hcon->dst, conn->hcon->dst_type,
1179 addr_type);
1180 }
1181
1182 ret = bt_6lowpan_connect(&addr, addr_type);
1183 if (ret < 0)
1184 return ret;
1185
1186 return count;
1187 }
1188
1189 if (memcmp(buf, "disconnect ", 11) == 0) {
1190 ret = get_l2cap_conn(&buf[11], &addr, &addr_type, &conn, true);
1191 if (ret < 0)
1192 return ret;
1193
1194 ret = bt_6lowpan_disconnect(conn, addr_type);
1195 if (ret < 0)
1196 return ret;
1197
1198 return count;
1199 }
1200
1201 return count;
1202 }
1203
lowpan_control_show(struct seq_file * f,void * ptr)1204 static int lowpan_control_show(struct seq_file *f, void *ptr)
1205 {
1206 struct lowpan_btle_dev *entry;
1207 struct lowpan_peer *peer;
1208
1209 spin_lock(&devices_lock);
1210
1211 list_for_each_entry(entry, &bt_6lowpan_devices, list) {
1212 list_for_each_entry(peer, &entry->peers, list)
1213 seq_printf(f, "%pMR (type %u)\n",
1214 &peer->chan->dst, peer->chan->dst_type);
1215 }
1216
1217 spin_unlock(&devices_lock);
1218
1219 return 0;
1220 }
1221
lowpan_control_open(struct inode * inode,struct file * file)1222 static int lowpan_control_open(struct inode *inode, struct file *file)
1223 {
1224 return single_open(file, lowpan_control_show, inode->i_private);
1225 }
1226
1227 static const struct file_operations lowpan_control_fops = {
1228 .open = lowpan_control_open,
1229 .read = seq_read,
1230 .write = lowpan_control_write,
1231 .llseek = seq_lseek,
1232 .release = single_release,
1233 };
1234
disconnect_devices(void)1235 static void disconnect_devices(void)
1236 {
1237 struct lowpan_btle_dev *entry, *tmp, *new_dev;
1238 struct list_head devices;
1239
1240 INIT_LIST_HEAD(&devices);
1241
1242 /* We make a separate list of devices because the unregister_netdev()
1243 * will call device_event() which will also want to modify the same
1244 * devices list.
1245 */
1246
1247 rcu_read_lock();
1248
1249 list_for_each_entry_rcu(entry, &bt_6lowpan_devices, list) {
1250 new_dev = kmalloc_obj(*new_dev, GFP_ATOMIC);
1251 if (!new_dev)
1252 break;
1253
1254 new_dev->netdev = entry->netdev;
1255 INIT_LIST_HEAD(&new_dev->list);
1256
1257 list_add_rcu(&new_dev->list, &devices);
1258 }
1259
1260 rcu_read_unlock();
1261
1262 list_for_each_entry_safe(entry, tmp, &devices, list) {
1263 ifdown(entry->netdev);
1264 BT_DBG("Unregistering netdev %s %p",
1265 entry->netdev->name, entry->netdev);
1266 lowpan_unregister_netdev(entry->netdev);
1267 kfree(entry);
1268 }
1269 }
1270
device_event(struct notifier_block * unused,unsigned long event,void * ptr)1271 static int device_event(struct notifier_block *unused,
1272 unsigned long event, void *ptr)
1273 {
1274 struct net_device *netdev = netdev_notifier_info_to_dev(ptr);
1275 struct lowpan_btle_dev *entry;
1276
1277 if (netdev->type != ARPHRD_6LOWPAN)
1278 return NOTIFY_DONE;
1279
1280 switch (event) {
1281 case NETDEV_UNREGISTER:
1282 spin_lock(&devices_lock);
1283 list_for_each_entry(entry, &bt_6lowpan_devices, list) {
1284 if (entry->netdev == netdev) {
1285 BT_DBG("Unregistered netdev %s %p",
1286 netdev->name, netdev);
1287 list_del(&entry->list);
1288 break;
1289 }
1290 }
1291 spin_unlock(&devices_lock);
1292 break;
1293 }
1294
1295 return NOTIFY_DONE;
1296 }
1297
1298 static struct notifier_block bt_6lowpan_dev_notifier = {
1299 .notifier_call = device_event,
1300 };
1301
bt_6lowpan_init(void)1302 static int __init bt_6lowpan_init(void)
1303 {
1304 lowpan_enable_debugfs = debugfs_create_file_unsafe("6lowpan_enable",
1305 0644, bt_debugfs,
1306 NULL,
1307 &lowpan_enable_fops);
1308 lowpan_control_debugfs = debugfs_create_file("6lowpan_control", 0644,
1309 bt_debugfs, NULL,
1310 &lowpan_control_fops);
1311
1312 return register_netdevice_notifier(&bt_6lowpan_dev_notifier);
1313 }
1314
bt_6lowpan_exit(void)1315 static void __exit bt_6lowpan_exit(void)
1316 {
1317 debugfs_remove(lowpan_enable_debugfs);
1318 debugfs_remove(lowpan_control_debugfs);
1319
1320 if (listen_chan) {
1321 l2cap_chan_lock(listen_chan);
1322 l2cap_chan_close(listen_chan, 0);
1323 l2cap_chan_unlock(listen_chan);
1324 l2cap_chan_put(listen_chan);
1325 }
1326
1327 disconnect_devices();
1328
1329 unregister_netdevice_notifier(&bt_6lowpan_dev_notifier);
1330 }
1331
1332 module_init(bt_6lowpan_init);
1333 module_exit(bt_6lowpan_exit);
1334
1335 MODULE_AUTHOR("Jukka Rissanen <jukka.rissanen@linux.intel.com>");
1336 MODULE_DESCRIPTION("Bluetooth 6LoWPAN");
1337 MODULE_VERSION(VERSION);
1338 MODULE_LICENSE("GPL");
1339