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