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