1 /* 2 Copyright (c) 2013 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 18 #include <net/ipv6.h> 19 #include <net/ip6_route.h> 20 #include <net/addrconf.h> 21 22 #include <net/af_ieee802154.h> /* to get the address type */ 23 24 #include <net/bluetooth/bluetooth.h> 25 #include <net/bluetooth/hci_core.h> 26 #include <net/bluetooth/l2cap.h> 27 28 #include "6lowpan.h" 29 30 #include <net/6lowpan.h> /* for the compression support */ 31 32 #define IFACE_NAME_TEMPLATE "bt%d" 33 #define EUI64_ADDR_LEN 8 34 35 struct skb_cb { 36 struct in6_addr addr; 37 struct l2cap_conn *conn; 38 }; 39 #define lowpan_cb(skb) ((struct skb_cb *)((skb)->cb)) 40 41 /* The devices list contains those devices that we are acting 42 * as a proxy. The BT 6LoWPAN device is a virtual device that 43 * connects to the Bluetooth LE device. The real connection to 44 * BT device is done via l2cap layer. There exists one 45 * virtual device / one BT 6LoWPAN network (=hciX device). 46 * The list contains struct lowpan_dev elements. 47 */ 48 static LIST_HEAD(bt_6lowpan_devices); 49 static DEFINE_RWLOCK(devices_lock); 50 51 struct lowpan_peer { 52 struct list_head list; 53 struct l2cap_conn *conn; 54 55 /* peer addresses in various formats */ 56 unsigned char eui64_addr[EUI64_ADDR_LEN]; 57 struct in6_addr peer_addr; 58 }; 59 60 struct lowpan_dev { 61 struct list_head list; 62 63 struct hci_dev *hdev; 64 struct net_device *netdev; 65 struct list_head peers; 66 atomic_t peer_count; /* number of items in peers list */ 67 68 struct work_struct delete_netdev; 69 struct delayed_work notify_peers; 70 }; 71 72 static inline struct lowpan_dev *lowpan_dev(const struct net_device *netdev) 73 { 74 return netdev_priv(netdev); 75 } 76 77 static inline void peer_add(struct lowpan_dev *dev, struct lowpan_peer *peer) 78 { 79 list_add(&peer->list, &dev->peers); 80 atomic_inc(&dev->peer_count); 81 } 82 83 static inline bool peer_del(struct lowpan_dev *dev, struct lowpan_peer *peer) 84 { 85 list_del(&peer->list); 86 87 if (atomic_dec_and_test(&dev->peer_count)) { 88 BT_DBG("last peer"); 89 return true; 90 } 91 92 return false; 93 } 94 95 static inline struct lowpan_peer *peer_lookup_ba(struct lowpan_dev *dev, 96 bdaddr_t *ba, __u8 type) 97 { 98 struct lowpan_peer *peer, *tmp; 99 100 BT_DBG("peers %d addr %pMR type %d", atomic_read(&dev->peer_count), 101 ba, type); 102 103 list_for_each_entry_safe(peer, tmp, &dev->peers, list) { 104 BT_DBG("addr %pMR type %d", 105 &peer->conn->hcon->dst, peer->conn->hcon->dst_type); 106 107 if (bacmp(&peer->conn->hcon->dst, ba)) 108 continue; 109 110 if (type == peer->conn->hcon->dst_type) 111 return peer; 112 } 113 114 return NULL; 115 } 116 117 static inline struct lowpan_peer *peer_lookup_conn(struct lowpan_dev *dev, 118 struct l2cap_conn *conn) 119 { 120 struct lowpan_peer *peer, *tmp; 121 122 list_for_each_entry_safe(peer, tmp, &dev->peers, list) { 123 if (peer->conn == conn) 124 return peer; 125 } 126 127 return NULL; 128 } 129 130 static struct lowpan_peer *lookup_peer(struct l2cap_conn *conn) 131 { 132 struct lowpan_dev *entry, *tmp; 133 struct lowpan_peer *peer = NULL; 134 unsigned long flags; 135 136 read_lock_irqsave(&devices_lock, flags); 137 138 list_for_each_entry_safe(entry, tmp, &bt_6lowpan_devices, list) { 139 peer = peer_lookup_conn(entry, conn); 140 if (peer) 141 break; 142 } 143 144 read_unlock_irqrestore(&devices_lock, flags); 145 146 return peer; 147 } 148 149 static struct lowpan_dev *lookup_dev(struct l2cap_conn *conn) 150 { 151 struct lowpan_dev *entry, *tmp; 152 struct lowpan_dev *dev = NULL; 153 unsigned long flags; 154 155 read_lock_irqsave(&devices_lock, flags); 156 157 list_for_each_entry_safe(entry, tmp, &bt_6lowpan_devices, list) { 158 if (conn->hcon->hdev == entry->hdev) { 159 dev = entry; 160 break; 161 } 162 } 163 164 read_unlock_irqrestore(&devices_lock, flags); 165 166 return dev; 167 } 168 169 static int give_skb_to_upper(struct sk_buff *skb, struct net_device *dev) 170 { 171 struct sk_buff *skb_cp; 172 int ret; 173 174 skb_cp = skb_copy(skb, GFP_ATOMIC); 175 if (!skb_cp) 176 return -ENOMEM; 177 178 ret = netif_rx(skb_cp); 179 180 BT_DBG("receive skb %d", ret); 181 if (ret < 0) 182 return NET_RX_DROP; 183 184 return ret; 185 } 186 187 static int process_data(struct sk_buff *skb, struct net_device *netdev, 188 struct l2cap_conn *conn) 189 { 190 const u8 *saddr, *daddr; 191 u8 iphc0, iphc1; 192 struct lowpan_dev *dev; 193 struct lowpan_peer *peer; 194 unsigned long flags; 195 196 dev = lowpan_dev(netdev); 197 198 read_lock_irqsave(&devices_lock, flags); 199 peer = peer_lookup_conn(dev, conn); 200 read_unlock_irqrestore(&devices_lock, flags); 201 if (!peer) 202 goto drop; 203 204 saddr = peer->eui64_addr; 205 daddr = dev->netdev->dev_addr; 206 207 /* at least two bytes will be used for the encoding */ 208 if (skb->len < 2) 209 goto drop; 210 211 if (lowpan_fetch_skb_u8(skb, &iphc0)) 212 goto drop; 213 214 if (lowpan_fetch_skb_u8(skb, &iphc1)) 215 goto drop; 216 217 return lowpan_process_data(skb, netdev, 218 saddr, IEEE802154_ADDR_LONG, EUI64_ADDR_LEN, 219 daddr, IEEE802154_ADDR_LONG, EUI64_ADDR_LEN, 220 iphc0, iphc1, give_skb_to_upper); 221 222 drop: 223 kfree_skb(skb); 224 return -EINVAL; 225 } 226 227 static int recv_pkt(struct sk_buff *skb, struct net_device *dev, 228 struct l2cap_conn *conn) 229 { 230 struct sk_buff *local_skb; 231 int ret; 232 233 if (!netif_running(dev)) 234 goto drop; 235 236 if (dev->type != ARPHRD_6LOWPAN) 237 goto drop; 238 239 /* check that it's our buffer */ 240 if (skb->data[0] == LOWPAN_DISPATCH_IPV6) { 241 /* Copy the packet so that the IPv6 header is 242 * properly aligned. 243 */ 244 local_skb = skb_copy_expand(skb, NET_SKB_PAD - 1, 245 skb_tailroom(skb), GFP_ATOMIC); 246 if (!local_skb) 247 goto drop; 248 249 local_skb->protocol = htons(ETH_P_IPV6); 250 local_skb->pkt_type = PACKET_HOST; 251 252 skb_reset_network_header(local_skb); 253 skb_set_transport_header(local_skb, sizeof(struct ipv6hdr)); 254 255 if (give_skb_to_upper(local_skb, dev) != NET_RX_SUCCESS) { 256 kfree_skb(local_skb); 257 goto drop; 258 } 259 260 dev->stats.rx_bytes += skb->len; 261 dev->stats.rx_packets++; 262 263 kfree_skb(local_skb); 264 kfree_skb(skb); 265 } else { 266 switch (skb->data[0] & 0xe0) { 267 case LOWPAN_DISPATCH_IPHC: /* ipv6 datagram */ 268 local_skb = skb_clone(skb, GFP_ATOMIC); 269 if (!local_skb) 270 goto drop; 271 272 ret = process_data(local_skb, dev, conn); 273 if (ret != NET_RX_SUCCESS) 274 goto drop; 275 276 dev->stats.rx_bytes += skb->len; 277 dev->stats.rx_packets++; 278 279 kfree_skb(skb); 280 break; 281 default: 282 break; 283 } 284 } 285 286 return NET_RX_SUCCESS; 287 288 drop: 289 kfree_skb(skb); 290 return NET_RX_DROP; 291 } 292 293 /* Packet from BT LE device */ 294 int bt_6lowpan_recv(struct l2cap_conn *conn, struct sk_buff *skb) 295 { 296 struct lowpan_dev *dev; 297 struct lowpan_peer *peer; 298 int err; 299 300 peer = lookup_peer(conn); 301 if (!peer) 302 return -ENOENT; 303 304 dev = lookup_dev(conn); 305 if (!dev || !dev->netdev) 306 return -ENOENT; 307 308 err = recv_pkt(skb, dev->netdev, conn); 309 BT_DBG("recv pkt %d", err); 310 311 return err; 312 } 313 314 static inline int skbuff_copy(void *msg, int len, int count, int mtu, 315 struct sk_buff *skb, struct net_device *dev) 316 { 317 struct sk_buff **frag; 318 int sent = 0; 319 320 memcpy(skb_put(skb, count), msg, count); 321 322 sent += count; 323 msg += count; 324 len -= count; 325 326 dev->stats.tx_bytes += count; 327 dev->stats.tx_packets++; 328 329 raw_dump_table(__func__, "Sending", skb->data, skb->len); 330 331 /* Continuation fragments (no L2CAP header) */ 332 frag = &skb_shinfo(skb)->frag_list; 333 while (len > 0) { 334 struct sk_buff *tmp; 335 336 count = min_t(unsigned int, mtu, len); 337 338 tmp = bt_skb_alloc(count, GFP_ATOMIC); 339 if (!tmp) 340 return -ENOMEM; 341 342 *frag = tmp; 343 344 memcpy(skb_put(*frag, count), msg, count); 345 346 raw_dump_table(__func__, "Sending fragment", 347 (*frag)->data, count); 348 349 (*frag)->priority = skb->priority; 350 351 sent += count; 352 msg += count; 353 len -= count; 354 355 skb->len += (*frag)->len; 356 skb->data_len += (*frag)->len; 357 358 frag = &(*frag)->next; 359 360 dev->stats.tx_bytes += count; 361 dev->stats.tx_packets++; 362 } 363 364 return sent; 365 } 366 367 static struct sk_buff *create_pdu(struct l2cap_conn *conn, void *msg, 368 size_t len, u32 priority, 369 struct net_device *dev) 370 { 371 struct sk_buff *skb; 372 int err, count; 373 struct l2cap_hdr *lh; 374 375 /* FIXME: This mtu check should be not needed and atm is only used for 376 * testing purposes 377 */ 378 if (conn->mtu > (L2CAP_LE_MIN_MTU + L2CAP_HDR_SIZE)) 379 conn->mtu = L2CAP_LE_MIN_MTU + L2CAP_HDR_SIZE; 380 381 count = min_t(unsigned int, (conn->mtu - L2CAP_HDR_SIZE), len); 382 383 BT_DBG("conn %p len %zu mtu %d count %d", conn, len, conn->mtu, count); 384 385 skb = bt_skb_alloc(count + L2CAP_HDR_SIZE, GFP_ATOMIC); 386 if (!skb) 387 return ERR_PTR(-ENOMEM); 388 389 skb->priority = priority; 390 391 lh = (struct l2cap_hdr *)skb_put(skb, L2CAP_HDR_SIZE); 392 lh->cid = cpu_to_le16(L2CAP_FC_6LOWPAN); 393 lh->len = cpu_to_le16(len); 394 395 err = skbuff_copy(msg, len, count, conn->mtu, skb, dev); 396 if (unlikely(err < 0)) { 397 kfree_skb(skb); 398 BT_DBG("skbuff copy %d failed", err); 399 return ERR_PTR(err); 400 } 401 402 return skb; 403 } 404 405 static int conn_send(struct l2cap_conn *conn, 406 void *msg, size_t len, u32 priority, 407 struct net_device *dev) 408 { 409 struct sk_buff *skb; 410 411 skb = create_pdu(conn, msg, len, priority, dev); 412 if (IS_ERR(skb)) 413 return -EINVAL; 414 415 BT_DBG("conn %p skb %p len %d priority %u", conn, skb, skb->len, 416 skb->priority); 417 418 hci_send_acl(conn->hchan, skb, ACL_START); 419 420 return 0; 421 } 422 423 static u8 get_addr_type_from_eui64(u8 byte) 424 { 425 /* Is universal(0) or local(1) bit, */ 426 if (byte & 0x02) 427 return ADDR_LE_DEV_RANDOM; 428 429 return ADDR_LE_DEV_PUBLIC; 430 } 431 432 static void copy_to_bdaddr(struct in6_addr *ip6_daddr, bdaddr_t *addr) 433 { 434 u8 *eui64 = ip6_daddr->s6_addr + 8; 435 436 addr->b[0] = eui64[7]; 437 addr->b[1] = eui64[6]; 438 addr->b[2] = eui64[5]; 439 addr->b[3] = eui64[2]; 440 addr->b[4] = eui64[1]; 441 addr->b[5] = eui64[0]; 442 } 443 444 static void convert_dest_bdaddr(struct in6_addr *ip6_daddr, 445 bdaddr_t *addr, u8 *addr_type) 446 { 447 copy_to_bdaddr(ip6_daddr, addr); 448 449 /* We need to toggle the U/L bit that we got from IPv6 address 450 * so that we get the proper address and type of the BD address. 451 */ 452 addr->b[5] ^= 0x02; 453 454 *addr_type = get_addr_type_from_eui64(addr->b[5]); 455 } 456 457 static int header_create(struct sk_buff *skb, struct net_device *netdev, 458 unsigned short type, const void *_daddr, 459 const void *_saddr, unsigned int len) 460 { 461 struct ipv6hdr *hdr; 462 struct lowpan_dev *dev; 463 struct lowpan_peer *peer; 464 bdaddr_t addr, *any = BDADDR_ANY; 465 u8 *saddr, *daddr = any->b; 466 u8 addr_type; 467 468 if (type != ETH_P_IPV6) 469 return -EINVAL; 470 471 hdr = ipv6_hdr(skb); 472 473 dev = lowpan_dev(netdev); 474 475 if (ipv6_addr_is_multicast(&hdr->daddr)) { 476 memcpy(&lowpan_cb(skb)->addr, &hdr->daddr, 477 sizeof(struct in6_addr)); 478 lowpan_cb(skb)->conn = NULL; 479 } else { 480 unsigned long flags; 481 482 /* Get destination BT device from skb. 483 * If there is no such peer then discard the packet. 484 */ 485 convert_dest_bdaddr(&hdr->daddr, &addr, &addr_type); 486 487 BT_DBG("dest addr %pMR type %s IP %pI6c", &addr, 488 addr_type == ADDR_LE_DEV_PUBLIC ? "PUBLIC" : "RANDOM", 489 &hdr->daddr); 490 491 read_lock_irqsave(&devices_lock, flags); 492 peer = peer_lookup_ba(dev, &addr, addr_type); 493 read_unlock_irqrestore(&devices_lock, flags); 494 495 if (!peer) { 496 BT_DBG("no such peer %pMR found", &addr); 497 return -ENOENT; 498 } 499 500 daddr = peer->eui64_addr; 501 502 memcpy(&lowpan_cb(skb)->addr, &hdr->daddr, 503 sizeof(struct in6_addr)); 504 lowpan_cb(skb)->conn = peer->conn; 505 } 506 507 saddr = dev->netdev->dev_addr; 508 509 return lowpan_header_compress(skb, netdev, type, daddr, saddr, len); 510 } 511 512 /* Packet to BT LE device */ 513 static int send_pkt(struct l2cap_conn *conn, const void *saddr, 514 const void *daddr, struct sk_buff *skb, 515 struct net_device *netdev) 516 { 517 raw_dump_table(__func__, "raw skb data dump before fragmentation", 518 skb->data, skb->len); 519 520 return conn_send(conn, skb->data, skb->len, 0, netdev); 521 } 522 523 static void send_mcast_pkt(struct sk_buff *skb, struct net_device *netdev) 524 { 525 struct sk_buff *local_skb; 526 struct lowpan_dev *entry, *tmp; 527 unsigned long flags; 528 529 read_lock_irqsave(&devices_lock, flags); 530 531 list_for_each_entry_safe(entry, tmp, &bt_6lowpan_devices, list) { 532 struct lowpan_peer *pentry, *ptmp; 533 struct lowpan_dev *dev; 534 535 if (entry->netdev != netdev) 536 continue; 537 538 dev = lowpan_dev(entry->netdev); 539 540 list_for_each_entry_safe(pentry, ptmp, &dev->peers, list) { 541 local_skb = skb_clone(skb, GFP_ATOMIC); 542 543 send_pkt(pentry->conn, netdev->dev_addr, 544 pentry->eui64_addr, local_skb, netdev); 545 546 kfree_skb(local_skb); 547 } 548 } 549 550 read_unlock_irqrestore(&devices_lock, flags); 551 } 552 553 static netdev_tx_t bt_xmit(struct sk_buff *skb, struct net_device *netdev) 554 { 555 int err = 0; 556 unsigned char *eui64_addr; 557 struct lowpan_dev *dev; 558 struct lowpan_peer *peer; 559 bdaddr_t addr; 560 u8 addr_type; 561 562 if (ipv6_addr_is_multicast(&lowpan_cb(skb)->addr)) { 563 /* We need to send the packet to every device 564 * behind this interface. 565 */ 566 send_mcast_pkt(skb, netdev); 567 } else { 568 unsigned long flags; 569 570 convert_dest_bdaddr(&lowpan_cb(skb)->addr, &addr, &addr_type); 571 eui64_addr = lowpan_cb(skb)->addr.s6_addr + 8; 572 dev = lowpan_dev(netdev); 573 574 read_lock_irqsave(&devices_lock, flags); 575 peer = peer_lookup_ba(dev, &addr, addr_type); 576 read_unlock_irqrestore(&devices_lock, flags); 577 578 BT_DBG("xmit %s to %pMR type %s IP %pI6c peer %p", 579 netdev->name, &addr, 580 addr_type == ADDR_LE_DEV_PUBLIC ? "PUBLIC" : "RANDOM", 581 &lowpan_cb(skb)->addr, peer); 582 583 if (peer && peer->conn) 584 err = send_pkt(peer->conn, netdev->dev_addr, 585 eui64_addr, skb, netdev); 586 } 587 dev_kfree_skb(skb); 588 589 if (err) 590 BT_DBG("ERROR: xmit failed (%d)", err); 591 592 return (err < 0) ? NET_XMIT_DROP : err; 593 } 594 595 static const struct net_device_ops netdev_ops = { 596 .ndo_start_xmit = bt_xmit, 597 }; 598 599 static struct header_ops header_ops = { 600 .create = header_create, 601 }; 602 603 static void netdev_setup(struct net_device *dev) 604 { 605 dev->addr_len = EUI64_ADDR_LEN; 606 dev->type = ARPHRD_6LOWPAN; 607 608 dev->hard_header_len = 0; 609 dev->needed_tailroom = 0; 610 dev->mtu = IPV6_MIN_MTU; 611 dev->tx_queue_len = 0; 612 dev->flags = IFF_RUNNING | IFF_POINTOPOINT; 613 dev->watchdog_timeo = 0; 614 615 dev->netdev_ops = &netdev_ops; 616 dev->header_ops = &header_ops; 617 dev->destructor = free_netdev; 618 } 619 620 static struct device_type bt_type = { 621 .name = "bluetooth", 622 }; 623 624 static void set_addr(u8 *eui, u8 *addr, u8 addr_type) 625 { 626 /* addr is the BT address in little-endian format */ 627 eui[0] = addr[5]; 628 eui[1] = addr[4]; 629 eui[2] = addr[3]; 630 eui[3] = 0xFF; 631 eui[4] = 0xFE; 632 eui[5] = addr[2]; 633 eui[6] = addr[1]; 634 eui[7] = addr[0]; 635 636 /* Universal/local bit set, BT 6lowpan draft ch. 3.2.1 */ 637 if (addr_type == ADDR_LE_DEV_PUBLIC) 638 eui[0] &= ~0x02; 639 else 640 eui[0] |= 0x02; 641 642 BT_DBG("type %d addr %*phC", addr_type, 8, eui); 643 } 644 645 static void set_dev_addr(struct net_device *netdev, bdaddr_t *addr, 646 u8 addr_type) 647 { 648 netdev->addr_assign_type = NET_ADDR_PERM; 649 set_addr(netdev->dev_addr, addr->b, addr_type); 650 } 651 652 static void ifup(struct net_device *netdev) 653 { 654 int err; 655 656 rtnl_lock(); 657 err = dev_open(netdev); 658 if (err < 0) 659 BT_INFO("iface %s cannot be opened (%d)", netdev->name, err); 660 rtnl_unlock(); 661 } 662 663 static void do_notify_peers(struct work_struct *work) 664 { 665 struct lowpan_dev *dev = container_of(work, struct lowpan_dev, 666 notify_peers.work); 667 668 netdev_notify_peers(dev->netdev); /* send neighbour adv at startup */ 669 } 670 671 static bool is_bt_6lowpan(struct hci_conn *hcon) 672 { 673 if (hcon->type != LE_LINK) 674 return false; 675 676 return test_bit(HCI_CONN_6LOWPAN, &hcon->flags); 677 } 678 679 static int add_peer_conn(struct l2cap_conn *conn, struct lowpan_dev *dev) 680 { 681 struct lowpan_peer *peer; 682 unsigned long flags; 683 684 peer = kzalloc(sizeof(*peer), GFP_ATOMIC); 685 if (!peer) 686 return -ENOMEM; 687 688 peer->conn = conn; 689 memset(&peer->peer_addr, 0, sizeof(struct in6_addr)); 690 691 /* RFC 2464 ch. 5 */ 692 peer->peer_addr.s6_addr[0] = 0xFE; 693 peer->peer_addr.s6_addr[1] = 0x80; 694 set_addr((u8 *)&peer->peer_addr.s6_addr + 8, conn->hcon->dst.b, 695 conn->hcon->dst_type); 696 697 memcpy(&peer->eui64_addr, (u8 *)&peer->peer_addr.s6_addr + 8, 698 EUI64_ADDR_LEN); 699 700 write_lock_irqsave(&devices_lock, flags); 701 INIT_LIST_HEAD(&peer->list); 702 peer_add(dev, peer); 703 write_unlock_irqrestore(&devices_lock, flags); 704 705 /* Notifying peers about us needs to be done without locks held */ 706 INIT_DELAYED_WORK(&dev->notify_peers, do_notify_peers); 707 schedule_delayed_work(&dev->notify_peers, msecs_to_jiffies(100)); 708 709 return 0; 710 } 711 712 /* This gets called when BT LE 6LoWPAN device is connected. We then 713 * create network device that acts as a proxy between BT LE device 714 * and kernel network stack. 715 */ 716 int bt_6lowpan_add_conn(struct l2cap_conn *conn) 717 { 718 struct lowpan_peer *peer = NULL; 719 struct lowpan_dev *dev; 720 struct net_device *netdev; 721 int err = 0; 722 unsigned long flags; 723 724 if (!is_bt_6lowpan(conn->hcon)) 725 return 0; 726 727 peer = lookup_peer(conn); 728 if (peer) 729 return -EEXIST; 730 731 dev = lookup_dev(conn); 732 if (dev) 733 return add_peer_conn(conn, dev); 734 735 netdev = alloc_netdev(sizeof(*dev), IFACE_NAME_TEMPLATE, netdev_setup); 736 if (!netdev) 737 return -ENOMEM; 738 739 set_dev_addr(netdev, &conn->hcon->src, conn->hcon->src_type); 740 741 netdev->netdev_ops = &netdev_ops; 742 SET_NETDEV_DEV(netdev, &conn->hcon->dev); 743 SET_NETDEV_DEVTYPE(netdev, &bt_type); 744 745 err = register_netdev(netdev); 746 if (err < 0) { 747 BT_INFO("register_netdev failed %d", err); 748 free_netdev(netdev); 749 goto out; 750 } 751 752 BT_DBG("ifindex %d peer bdaddr %pMR my addr %pMR", 753 netdev->ifindex, &conn->hcon->dst, &conn->hcon->src); 754 set_bit(__LINK_STATE_PRESENT, &netdev->state); 755 756 dev = netdev_priv(netdev); 757 dev->netdev = netdev; 758 dev->hdev = conn->hcon->hdev; 759 INIT_LIST_HEAD(&dev->peers); 760 761 write_lock_irqsave(&devices_lock, flags); 762 INIT_LIST_HEAD(&dev->list); 763 list_add(&dev->list, &bt_6lowpan_devices); 764 write_unlock_irqrestore(&devices_lock, flags); 765 766 ifup(netdev); 767 768 return add_peer_conn(conn, dev); 769 770 out: 771 return err; 772 } 773 774 static void delete_netdev(struct work_struct *work) 775 { 776 struct lowpan_dev *entry = container_of(work, struct lowpan_dev, 777 delete_netdev); 778 779 unregister_netdev(entry->netdev); 780 781 /* The entry pointer is deleted in device_event() */ 782 } 783 784 int bt_6lowpan_del_conn(struct l2cap_conn *conn) 785 { 786 struct lowpan_dev *entry, *tmp; 787 struct lowpan_dev *dev = NULL; 788 struct lowpan_peer *peer; 789 int err = -ENOENT; 790 unsigned long flags; 791 bool last = false; 792 793 if (!conn || !is_bt_6lowpan(conn->hcon)) 794 return 0; 795 796 write_lock_irqsave(&devices_lock, flags); 797 798 list_for_each_entry_safe(entry, tmp, &bt_6lowpan_devices, list) { 799 dev = lowpan_dev(entry->netdev); 800 peer = peer_lookup_conn(dev, conn); 801 if (peer) { 802 last = peer_del(dev, peer); 803 err = 0; 804 break; 805 } 806 } 807 808 if (!err && last && dev && !atomic_read(&dev->peer_count)) { 809 write_unlock_irqrestore(&devices_lock, flags); 810 811 cancel_delayed_work_sync(&dev->notify_peers); 812 813 /* bt_6lowpan_del_conn() is called with hci dev lock held which 814 * means that we must delete the netdevice in worker thread. 815 */ 816 INIT_WORK(&entry->delete_netdev, delete_netdev); 817 schedule_work(&entry->delete_netdev); 818 } else { 819 write_unlock_irqrestore(&devices_lock, flags); 820 } 821 822 return err; 823 } 824 825 static int device_event(struct notifier_block *unused, 826 unsigned long event, void *ptr) 827 { 828 struct net_device *netdev = netdev_notifier_info_to_dev(ptr); 829 struct lowpan_dev *entry, *tmp; 830 unsigned long flags; 831 832 if (netdev->type != ARPHRD_6LOWPAN) 833 return NOTIFY_DONE; 834 835 switch (event) { 836 case NETDEV_UNREGISTER: 837 write_lock_irqsave(&devices_lock, flags); 838 list_for_each_entry_safe(entry, tmp, &bt_6lowpan_devices, 839 list) { 840 if (entry->netdev == netdev) { 841 list_del(&entry->list); 842 kfree(entry); 843 break; 844 } 845 } 846 write_unlock_irqrestore(&devices_lock, flags); 847 break; 848 } 849 850 return NOTIFY_DONE; 851 } 852 853 static struct notifier_block bt_6lowpan_dev_notifier = { 854 .notifier_call = device_event, 855 }; 856 857 int bt_6lowpan_init(void) 858 { 859 return register_netdevice_notifier(&bt_6lowpan_dev_notifier); 860 } 861 862 void bt_6lowpan_cleanup(void) 863 { 864 unregister_netdevice_notifier(&bt_6lowpan_dev_notifier); 865 } 866