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