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