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