projects / deliverable / linux.git / blame_incremental
? search:
summary | shortlog | log | commit | commitdiff | tree
blob | blame (non-incremental) | history | HEAD
batman-adv: iv_ogm_send_to_if, declare char* as const
[deliverable/linux.git] / net / bluetooth / 6lowpan.c
... / ...
CommitLineData
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/af_ieee802154.h> /* to get the address type */
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> /* for the compression support */
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/* The devices list contains those devices that we are acting
49 * as a proxy. The BT 6LoWPAN device is a virtual device that
50 * connects to the Bluetooth LE device. The real connection to
51 * BT device is done via l2cap layer. There exists one
52 * virtual device / one BT 6LoWPAN network (=hciX device).
53 * The list contains struct lowpan_dev elements.
54 */
55static LIST_HEAD(bt_6lowpan_devices);
56static DEFINE_SPINLOCK(devices_lock);
57
58static bool enable_6lowpan;
59
60/* We are listening incoming connections via this channel
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 /* peer addresses in various formats */
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; /* number of items in peers list */
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 netdev_priv(netdev);
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 /* If we have multiple 6lowpan peers, then check where we should
178 * send the packet. If only one peer exists, then we can send the
179 * packet right away.
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 /* We need to remember the address because it is needed
198 * by bt_xmit() when sending the packet. In bt_xmit(), the
199 * destination routing info is not set.
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 /* at least two bytes will be used for the encoding */
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 /* check that it's our buffer */
326 if (skb->data[0] == LOWPAN_DISPATCH_IPV6) {
327 /* Copy the packet so that the IPv6 header is
328 * properly aligned.
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: /* ipv6 datagram */
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/* Packet from BT LE device */
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 /* Is universal(0) or local(1) bit */
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 /* We need to toggle the U/L bit that we got from IPv6 address
440 * so that we get the proper address and type of the BD address.
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 /* Get destination BT device from skb.
467 * If there is no such peer then discard the packet.
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 /* The packet might be sent to 6lowpan interface
477 * because of routing (either via default route
478 * or user set route) so get peer according to
479 * the destination address.
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/* Packet to BT LE device */
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 /* Remember the skb so that we can send EAGAIN to the caller if
531 * we run out of credits.
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 /* We must take a copy of the skb before we modify/replace the ipv6
607 * header as the header could be used elsewhere
608 */
609 skb = skb_unshare(skb, GFP_ATOMIC);
610 if (!skb)
611 return NET_XMIT_DROP;
612
613 /* Return values from setup_header()
614 * <0 - error, packet is dropped
615 * 0 - this is a multicast packet
616 * 1 - this is unicast packet
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 /* We need to send the packet to every device behind this
635 * interface.
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 /* addr is the BT address in little-endian format */
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 /* Universal/local bit set, BT 6lowpan draft ch. 3.2.1 */
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); /* send neighbour adv at startup */
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 /* RFC 2464 ch. 5 */
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 /* IPv6 address needs to have the U/L bit set properly so toggle
830 * it back here.
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 /* Notifying peers about us needs to be done without locks held */
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(sizeof(struct lowpan_dev), IFACE_NAME_TEMPLATE,
852 NET_NAME_UNKNOWN, netdev_setup);
853 if (!netdev)
854 return -ENOMEM;
855
856 set_dev_addr(netdev, &chan->src, chan->src_type);
857
858 netdev->netdev_ops = &netdev_ops;
859 SET_NETDEV_DEV(netdev, &chan->conn->hcon->dev);
860 SET_NETDEV_DEVTYPE(netdev, &bt_type);
861
862 err = register_netdev(netdev);
863 if (err < 0) {
864 BT_INFO("register_netdev failed %d", err);
865 free_netdev(netdev);
866 goto out;
867 }
868
869 BT_DBG("ifindex %d peer bdaddr %pMR type %d my addr %pMR type %d",
870 netdev->ifindex, &chan->dst, chan->dst_type,
871 &chan->src, chan->src_type);
872 set_bit(__LINK_STATE_PRESENT, &netdev->state);
873
874 *dev = netdev_priv(netdev);
875 (*dev)->netdev = netdev;
876 (*dev)->hdev = chan->conn->hcon->hdev;
877 INIT_LIST_HEAD(&(*dev)->peers);
878
879 spin_lock(&devices_lock);
880 INIT_LIST_HEAD(&(*dev)->list);
881 list_add_rcu(&(*dev)->list, &bt_6lowpan_devices);
882 spin_unlock(&devices_lock);
883
884 return 0;
885
886out:
887 return err;
888}
889
890static inline void chan_ready_cb(struct l2cap_chan *chan)
891{
892 struct lowpan_dev *dev;
893
894 dev = lookup_dev(chan->conn);
895
896 BT_DBG("chan %p conn %p dev %p", chan, chan->conn, dev);
897
898 if (!dev) {
899 if (setup_netdev(chan, &dev) < 0) {
900 l2cap_chan_del(chan, -ENOENT);
901 return;
902 }
903 }
904
905 if (!try_module_get(THIS_MODULE))
906 return;
907
908 add_peer_chan(chan, dev);
909 ifup(dev->netdev);
910}
911
912static inline struct l2cap_chan *chan_new_conn_cb(struct l2cap_chan *pchan)
913{
914 struct l2cap_chan *chan;
915
916 chan = chan_open(pchan);
917 chan->ops = pchan->ops;
918
919 BT_DBG("chan %p pchan %p", chan, pchan);
920
921 return chan;
922}
923
924static void delete_netdev(struct work_struct *work)
925{
926 struct lowpan_dev *entry = container_of(work, struct lowpan_dev,
927 delete_netdev);
928
929 unregister_netdev(entry->netdev);
930
931 /* The entry pointer is deleted in device_event() */
932}
933
934static void chan_close_cb(struct l2cap_chan *chan)
935{
936 struct lowpan_dev *entry;
937 struct lowpan_dev *dev = NULL;
938 struct lowpan_peer *peer;
939 int err = -ENOENT;
940 bool last = false, removed = true;
941
942 BT_DBG("chan %p conn %p", chan, chan->conn);
943
944 if (chan->conn && chan->conn->hcon) {
945 if (!is_bt_6lowpan(chan->conn->hcon))
946 return;
947
948 /* If conn is set, then the netdev is also there and we should
949 * not remove it.
950 */
951 removed = false;
952 }
953
954 spin_lock(&devices_lock);
955
956 list_for_each_entry_rcu(entry, &bt_6lowpan_devices, list) {
957 dev = lowpan_dev(entry->netdev);
958 peer = __peer_lookup_chan(dev, chan);
959 if (peer) {
960 last = peer_del(dev, peer);
961 err = 0;
962
963 BT_DBG("dev %p removing %speer %p", dev,
964 last ? "last " : "1 ", peer);
965 BT_DBG("chan %p orig refcnt %d", chan,
966 atomic_read(&chan->kref.refcount));
967
968 l2cap_chan_put(chan);
969 break;
970 }
971 }
972
973 if (!err && last && dev && !atomic_read(&dev->peer_count)) {
974 spin_unlock(&devices_lock);
975
976 cancel_delayed_work_sync(&dev->notify_peers);
977
978 ifdown(dev->netdev);
979
980 if (!removed) {
981 INIT_WORK(&entry->delete_netdev, delete_netdev);
982 schedule_work(&entry->delete_netdev);
983 }
984 } else {
985 spin_unlock(&devices_lock);
986 }
987
988 return;
989}
990
991static void chan_state_change_cb(struct l2cap_chan *chan, int state, int err)
992{
993 BT_DBG("chan %p conn %p state %s err %d", chan, chan->conn,
994 state_to_string(state), err);
995}
996
997static struct sk_buff *chan_alloc_skb_cb(struct l2cap_chan *chan,
998 unsigned long hdr_len,
999 unsigned long len, int nb)
1000{
1001 /* Note that we must allocate using GFP_ATOMIC here as
1002 * this function is called originally from netdev hard xmit
1003 * function in atomic context.
1004 */
1005 return bt_skb_alloc(hdr_len + len, GFP_ATOMIC);
1006}
1007
1008static void chan_suspend_cb(struct l2cap_chan *chan)
1009{
1010 struct sk_buff *skb = chan->data;
1011
1012 BT_DBG("chan %p conn %p skb %p", chan, chan->conn, skb);
1013
1014 if (!skb)
1015 return;
1016
1017 lowpan_cb(skb)->status = -EAGAIN;
1018}
1019
1020static void chan_resume_cb(struct l2cap_chan *chan)
1021{
1022 struct sk_buff *skb = chan->data;
1023
1024 BT_DBG("chan %p conn %p skb %p", chan, chan->conn, skb);
1025
1026 if (!skb)
1027 return;
1028
1029 lowpan_cb(skb)->status = 0;
1030}
1031
1032static long chan_get_sndtimeo_cb(struct l2cap_chan *chan)
1033{
1034 return L2CAP_CONN_TIMEOUT;
1035}
1036
1037static const struct l2cap_ops bt_6lowpan_chan_ops = {
1038 .name = "L2CAP 6LoWPAN channel",
1039 .new_connection = chan_new_conn_cb,
1040 .recv = chan_recv_cb,
1041 .close = chan_close_cb,
1042 .state_change = chan_state_change_cb,
1043 .ready = chan_ready_cb,
1044 .resume = chan_resume_cb,
1045 .suspend = chan_suspend_cb,
1046 .get_sndtimeo = chan_get_sndtimeo_cb,
1047 .alloc_skb = chan_alloc_skb_cb,
1048
1049 .teardown = l2cap_chan_no_teardown,
1050 .defer = l2cap_chan_no_defer,
1051 .set_shutdown = l2cap_chan_no_set_shutdown,
1052};
1053
1054static inline __u8 bdaddr_type(__u8 type)
1055{
1056 if (type == ADDR_LE_DEV_PUBLIC)
1057 return BDADDR_LE_PUBLIC;
1058 else
1059 return BDADDR_LE_RANDOM;
1060}
1061
1062static struct l2cap_chan *chan_get(void)
1063{
1064 struct l2cap_chan *pchan;
1065
1066 pchan = chan_create();
1067 if (!pchan)
1068 return NULL;
1069
1070 pchan->ops = &bt_6lowpan_chan_ops;
1071
1072 return pchan;
1073}
1074
1075static int bt_6lowpan_connect(bdaddr_t *addr, u8 dst_type)
1076{
1077 struct l2cap_chan *pchan;
1078 int err;
1079
1080 pchan = chan_get();
1081 if (!pchan)
1082 return -EINVAL;
1083
1084 err = l2cap_chan_connect(pchan, cpu_to_le16(L2CAP_PSM_IPSP), 0,
1085 addr, dst_type);
1086
1087 BT_DBG("chan %p err %d", pchan, err);
1088 if (err < 0)
1089 l2cap_chan_put(pchan);
1090
1091 return err;
1092}
1093
1094static int bt_6lowpan_disconnect(struct l2cap_conn *conn, u8 dst_type)
1095{
1096 struct lowpan_peer *peer;
1097
1098 BT_DBG("conn %p dst type %d", conn, dst_type);
1099
1100 peer = lookup_peer(conn);
1101 if (!peer)
1102 return -ENOENT;
1103
1104 BT_DBG("peer %p chan %p", peer, peer->chan);
1105
1106 l2cap_chan_close(peer->chan, ENOENT);
1107
1108 return 0;
1109}
1110
1111static struct l2cap_chan *bt_6lowpan_listen(void)
1112{
1113 bdaddr_t *addr = BDADDR_ANY;
1114 struct l2cap_chan *pchan;
1115 int err;
1116
1117 if (!enable_6lowpan)
1118 return NULL;
1119
1120 pchan = chan_get();
1121 if (!pchan)
1122 return NULL;
1123
1124 pchan->state = BT_LISTEN;
1125 pchan->src_type = BDADDR_LE_PUBLIC;
1126
1127 atomic_set(&pchan->nesting, L2CAP_NESTING_PARENT);
1128
1129 BT_DBG("chan %p src type %d", pchan, pchan->src_type);
1130
1131 err = l2cap_add_psm(pchan, addr, cpu_to_le16(L2CAP_PSM_IPSP));
1132 if (err) {
1133 l2cap_chan_put(pchan);
1134 BT_ERR("psm cannot be added err %d", err);
1135 return NULL;
1136 }
1137
1138 return pchan;
1139}
1140
1141static int get_l2cap_conn(char *buf, bdaddr_t *addr, u8 *addr_type,
1142 struct l2cap_conn **conn)
1143{
1144 struct hci_conn *hcon;
1145 struct hci_dev *hdev;
1146 bdaddr_t *src = BDADDR_ANY;
1147 int n;
1148
1149 n = sscanf(buf, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx %hhu",
1150 &addr->b[5], &addr->b[4], &addr->b[3],
1151 &addr->b[2], &addr->b[1], &addr->b[0],
1152 addr_type);
1153
1154 if (n < 7)
1155 return -EINVAL;
1156
1157 hdev = hci_get_route(addr, src);
1158 if (!hdev)
1159 return -ENOENT;
1160
1161 hci_dev_lock(hdev);
1162 hcon = hci_conn_hash_lookup_ba(hdev, LE_LINK, addr);
1163 hci_dev_unlock(hdev);
1164
1165 if (!hcon)
1166 return -ENOENT;
1167
1168 *conn = (struct l2cap_conn *)hcon->l2cap_data;
1169
1170 BT_DBG("conn %p dst %pMR type %d", *conn, &hcon->dst, hcon->dst_type);
1171
1172 return 0;
1173}
1174
1175static void disconnect_all_peers(void)
1176{
1177 struct lowpan_dev *entry;
1178 struct lowpan_peer *peer, *tmp_peer, *new_peer;
1179 struct list_head peers;
1180
1181 INIT_LIST_HEAD(&peers);
1182
1183 /* We make a separate list of peers as the close_cb() will
1184 * modify the device peers list so it is better not to mess
1185 * with the same list at the same time.
1186 */
1187
1188 rcu_read_lock();
1189
1190 list_for_each_entry_rcu(entry, &bt_6lowpan_devices, list) {
1191 list_for_each_entry_rcu(peer, &entry->peers, list) {
1192 new_peer = kmalloc(sizeof(*new_peer), GFP_ATOMIC);
1193 if (!new_peer)
1194 break;
1195
1196 new_peer->chan = peer->chan;
1197 INIT_LIST_HEAD(&new_peer->list);
1198
1199 list_add(&new_peer->list, &peers);
1200 }
1201 }
1202
1203 rcu_read_unlock();
1204
1205 spin_lock(&devices_lock);
1206 list_for_each_entry_safe(peer, tmp_peer, &peers, list) {
1207 l2cap_chan_close(peer->chan, ENOENT);
1208
1209 list_del_rcu(&peer->list);
1210 kfree_rcu(peer, rcu);
1211
1212 module_put(THIS_MODULE);
1213 }
1214 spin_unlock(&devices_lock);
1215}
1216
1217struct set_enable {
1218 struct work_struct work;
1219 bool flag;
1220};
1221
1222static void do_enable_set(struct work_struct *work)
1223{
1224 struct set_enable *set_enable = container_of(work,
1225 struct set_enable, work);
1226
1227 if (!set_enable->flag || enable_6lowpan != set_enable->flag)
1228 /* Disconnect existing connections if 6lowpan is
1229 * disabled
1230 */
1231 disconnect_all_peers();
1232
1233 enable_6lowpan = set_enable->flag;
1234
1235 if (listen_chan) {
1236 l2cap_chan_close(listen_chan, 0);
1237 l2cap_chan_put(listen_chan);
1238 }
1239
1240 listen_chan = bt_6lowpan_listen();
1241
1242 kfree(set_enable);
1243}
1244
1245static int lowpan_enable_set(void *data, u64 val)
1246{
1247 struct set_enable *set_enable;
1248
1249 set_enable = kzalloc(sizeof(*set_enable), GFP_KERNEL);
1250 if (!set_enable)
1251 return -ENOMEM;
1252
1253 set_enable->flag = !!val;
1254 INIT_WORK(&set_enable->work, do_enable_set);
1255
1256 schedule_work(&set_enable->work);
1257
1258 return 0;
1259}
1260
1261static int lowpan_enable_get(void *data, u64 *val)
1262{
1263 *val = enable_6lowpan;
1264 return 0;
1265}
1266
1267DEFINE_SIMPLE_ATTRIBUTE(lowpan_enable_fops, lowpan_enable_get,
1268 lowpan_enable_set, "%llu\n");
1269
1270static ssize_t lowpan_control_write(struct file *fp,
1271 const char __user *user_buffer,
1272 size_t count,
1273 loff_t *position)
1274{
1275 char buf[32];
1276 size_t buf_size = min(count, sizeof(buf) - 1);
1277 int ret;
1278 bdaddr_t addr;
1279 u8 addr_type;
1280 struct l2cap_conn *conn = NULL;
1281
1282 if (copy_from_user(buf, user_buffer, buf_size))
1283 return -EFAULT;
1284
1285 buf[buf_size] = '\0';
1286
1287 if (memcmp(buf, "connect ", 8) == 0) {
1288 ret = get_l2cap_conn(&buf[8], &addr, &addr_type, &conn);
1289 if (ret == -EINVAL)
1290 return ret;
1291
1292 if (listen_chan) {
1293 l2cap_chan_close(listen_chan, 0);
1294 l2cap_chan_put(listen_chan);
1295 listen_chan = NULL;
1296 }
1297
1298 if (conn) {
1299 struct lowpan_peer *peer;
1300
1301 if (!is_bt_6lowpan(conn->hcon))
1302 return -EINVAL;
1303
1304 peer = lookup_peer(conn);
1305 if (peer) {
1306 BT_DBG("6LoWPAN connection already exists");
1307 return -EALREADY;
1308 }
1309
1310 BT_DBG("conn %p dst %pMR type %d user %d", conn,
1311 &conn->hcon->dst, conn->hcon->dst_type,
1312 addr_type);
1313 }
1314
1315 ret = bt_6lowpan_connect(&addr, addr_type);
1316 if (ret < 0)
1317 return ret;
1318
1319 return count;
1320 }
1321
1322 if (memcmp(buf, "disconnect ", 11) == 0) {
1323 ret = get_l2cap_conn(&buf[11], &addr, &addr_type, &conn);
1324 if (ret < 0)
1325 return ret;
1326
1327 ret = bt_6lowpan_disconnect(conn, addr_type);
1328 if (ret < 0)
1329 return ret;
1330
1331 return count;
1332 }
1333
1334 return count;
1335}
1336
1337static int lowpan_control_show(struct seq_file *f, void *ptr)
1338{
1339 struct lowpan_dev *entry;
1340 struct lowpan_peer *peer;
1341
1342 spin_lock(&devices_lock);
1343
1344 list_for_each_entry(entry, &bt_6lowpan_devices, list) {
1345 list_for_each_entry(peer, &entry->peers, list)
1346 seq_printf(f, "%pMR (type %u)\n",
1347 &peer->chan->dst, peer->chan->dst_type);
1348 }
1349
1350 spin_unlock(&devices_lock);
1351
1352 return 0;
1353}
1354
1355static int lowpan_control_open(struct inode *inode, struct file *file)
1356{
1357 return single_open(file, lowpan_control_show, inode->i_private);
1358}
1359
1360static const struct file_operations lowpan_control_fops = {
1361 .open = lowpan_control_open,
1362 .read = seq_read,
1363 .write = lowpan_control_write,
1364 .llseek = seq_lseek,
1365 .release = single_release,
1366};
1367
1368static void disconnect_devices(void)
1369{
1370 struct lowpan_dev *entry, *tmp, *new_dev;
1371 struct list_head devices;
1372
1373 INIT_LIST_HEAD(&devices);
1374
1375 /* We make a separate list of devices because the unregister_netdev()
1376 * will call device_event() which will also want to modify the same
1377 * devices list.
1378 */
1379
1380 rcu_read_lock();
1381
1382 list_for_each_entry_rcu(entry, &bt_6lowpan_devices, list) {
1383 new_dev = kmalloc(sizeof(*new_dev), GFP_ATOMIC);
1384 if (!new_dev)
1385 break;
1386
1387 new_dev->netdev = entry->netdev;
1388 INIT_LIST_HEAD(&new_dev->list);
1389
1390 list_add_rcu(&new_dev->list, &devices);
1391 }
1392
1393 rcu_read_unlock();
1394
1395 list_for_each_entry_safe(entry, tmp, &devices, list) {
1396 ifdown(entry->netdev);
1397 BT_DBG("Unregistering netdev %s %p",
1398 entry->netdev->name, entry->netdev);
1399 unregister_netdev(entry->netdev);
1400 kfree(entry);
1401 }
1402}
1403
1404static int device_event(struct notifier_block *unused,
1405 unsigned long event, void *ptr)
1406{
1407 struct net_device *netdev = netdev_notifier_info_to_dev(ptr);
1408 struct lowpan_dev *entry;
1409
1410 if (netdev->type != ARPHRD_6LOWPAN)
1411 return NOTIFY_DONE;
1412
1413 switch (event) {
1414 case NETDEV_UNREGISTER:
1415 spin_lock(&devices_lock);
1416 list_for_each_entry(entry, &bt_6lowpan_devices, list) {
1417 if (entry->netdev == netdev) {
1418 BT_DBG("Unregistered netdev %s %p",
1419 netdev->name, netdev);
1420 list_del(&entry->list);
1421 kfree(entry);
1422 break;
1423 }
1424 }
1425 spin_unlock(&devices_lock);
1426 break;
1427 }
1428
1429 return NOTIFY_DONE;
1430}
1431
1432static struct notifier_block bt_6lowpan_dev_notifier = {
1433 .notifier_call = device_event,
1434};
1435
1436static int __init bt_6lowpan_init(void)
1437{
1438 lowpan_enable_debugfs = debugfs_create_file("6lowpan_enable", 0644,
1439 bt_debugfs, NULL,
1440 &lowpan_enable_fops);
1441 lowpan_control_debugfs = debugfs_create_file("6lowpan_control", 0644,
1442 bt_debugfs, NULL,
1443 &lowpan_control_fops);
1444
1445 return register_netdevice_notifier(&bt_6lowpan_dev_notifier);
1446}
1447
1448static void __exit bt_6lowpan_exit(void)
1449{
1450 debugfs_remove(lowpan_enable_debugfs);
1451 debugfs_remove(lowpan_control_debugfs);
1452
1453 if (listen_chan) {
1454 l2cap_chan_close(listen_chan, 0);
1455 l2cap_chan_put(listen_chan);
1456 }
1457
1458 disconnect_devices();
1459
1460 unregister_netdevice_notifier(&bt_6lowpan_dev_notifier);
1461}
1462
1463module_init(bt_6lowpan_init);
1464module_exit(bt_6lowpan_exit);
1465
1466MODULE_AUTHOR("Jukka Rissanen <jukka.rissanen@linux.intel.com>");
1467MODULE_DESCRIPTION("Bluetooth 6LoWPAN");
1468MODULE_VERSION(VERSION);
1469MODULE_LICENSE("GPL");
Linux kernel - Deliverables
This page took 0.028849 seconds and 5 git commands to generate.