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