1 /* 2 BNEP implementation for Linux Bluetooth stack (BlueZ). 3 Copyright (C) 2001-2002 Inventel Systemes 4 Written 2001-2002 by 5 Clément Moreau <clement.moreau@inventel.fr> 6 David Libault <david.libault@inventel.fr> 7 8 Copyright (C) 2002 Maxim Krasnyansky <maxk@qualcomm.com> 9 10 This program is free software; you can redistribute it and/or modify 11 it under the terms of the GNU General Public License version 2 as 12 published by the Free Software Foundation; 13 14 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 15 OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. 17 IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY 18 CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES 19 WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 20 ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 21 OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 22 23 ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, 24 COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS 25 SOFTWARE IS DISCLAIMED. 26 */ 27 28 #include <linux/module.h> 29 #include <linux/kthread.h> 30 #include <linux/file.h> 31 #include <linux/etherdevice.h> 32 #include <asm/unaligned.h> 33 34 #include <net/bluetooth/bluetooth.h> 35 #include <net/bluetooth/hci_core.h> 36 #include <net/bluetooth/l2cap.h> 37 38 #include "bnep.h" 39 40 #define VERSION "1.3" 41 42 static bool compress_src = true; 43 static bool compress_dst = true; 44 45 static LIST_HEAD(bnep_session_list); 46 static DECLARE_RWSEM(bnep_session_sem); 47 48 static struct bnep_session *__bnep_get_session(u8 *dst) 49 { 50 struct bnep_session *s; 51 52 BT_DBG(""); 53 54 list_for_each_entry(s, &bnep_session_list, list) 55 if (ether_addr_equal(dst, s->eh.h_source)) 56 return s; 57 58 return NULL; 59 } 60 61 static void __bnep_link_session(struct bnep_session *s) 62 { 63 list_add(&s->list, &bnep_session_list); 64 } 65 66 static void __bnep_unlink_session(struct bnep_session *s) 67 { 68 list_del(&s->list); 69 } 70 71 static int bnep_send(struct bnep_session *s, void *data, size_t len) 72 { 73 struct socket *sock = s->sock; 74 struct kvec iv = { data, len }; 75 76 return kernel_sendmsg(sock, &s->msg, &iv, 1, len); 77 } 78 79 static int bnep_send_rsp(struct bnep_session *s, u8 ctrl, u16 resp) 80 { 81 struct bnep_control_rsp rsp; 82 rsp.type = BNEP_CONTROL; 83 rsp.ctrl = ctrl; 84 rsp.resp = htons(resp); 85 return bnep_send(s, &rsp, sizeof(rsp)); 86 } 87 88 #ifdef CONFIG_BT_BNEP_PROTO_FILTER 89 static inline void bnep_set_default_proto_filter(struct bnep_session *s) 90 { 91 /* (IPv4, ARP) */ 92 s->proto_filter[0].start = ETH_P_IP; 93 s->proto_filter[0].end = ETH_P_ARP; 94 /* (RARP, AppleTalk) */ 95 s->proto_filter[1].start = ETH_P_RARP; 96 s->proto_filter[1].end = ETH_P_AARP; 97 /* (IPX, IPv6) */ 98 s->proto_filter[2].start = ETH_P_IPX; 99 s->proto_filter[2].end = ETH_P_IPV6; 100 } 101 #endif 102 103 static int bnep_ctrl_set_netfilter(struct bnep_session *s, __be16 *data, int len) 104 { 105 int n; 106 107 if (len < 2) 108 return -EILSEQ; 109 110 n = get_unaligned_be16(data); 111 data++; 112 len -= 2; 113 114 if (len < n) 115 return -EILSEQ; 116 117 BT_DBG("filter len %d", n); 118 119 #ifdef CONFIG_BT_BNEP_PROTO_FILTER 120 n /= 4; 121 if (n <= BNEP_MAX_PROTO_FILTERS) { 122 struct bnep_proto_filter *f = s->proto_filter; 123 int i; 124 125 for (i = 0; i < n; i++) { 126 f[i].start = get_unaligned_be16(data++); 127 f[i].end = get_unaligned_be16(data++); 128 129 BT_DBG("proto filter start %d end %d", 130 f[i].start, f[i].end); 131 } 132 133 if (i < BNEP_MAX_PROTO_FILTERS) 134 memset(f + i, 0, sizeof(*f)); 135 136 if (n == 0) 137 bnep_set_default_proto_filter(s); 138 139 bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_SUCCESS); 140 } else { 141 bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_FILTER_LIMIT_REACHED); 142 } 143 #else 144 bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_FILTER_UNSUPPORTED_REQ); 145 #endif 146 return 0; 147 } 148 149 static int bnep_ctrl_set_mcfilter(struct bnep_session *s, u8 *data, int len) 150 { 151 int n; 152 153 if (len < 2) 154 return -EILSEQ; 155 156 n = get_unaligned_be16(data); 157 data += 2; 158 len -= 2; 159 160 if (len < n) 161 return -EILSEQ; 162 163 BT_DBG("filter len %d", n); 164 165 #ifdef CONFIG_BT_BNEP_MC_FILTER 166 n /= (ETH_ALEN * 2); 167 168 if (n > 0) { 169 int i; 170 171 s->mc_filter = 0; 172 173 /* Always send broadcast */ 174 set_bit(bnep_mc_hash(s->dev->broadcast), (ulong *) &s->mc_filter); 175 176 /* Add address ranges to the multicast hash */ 177 for (; n > 0; n--) { 178 u8 a1[6], *a2; 179 180 memcpy(a1, data, ETH_ALEN); 181 data += ETH_ALEN; 182 a2 = data; 183 data += ETH_ALEN; 184 185 BT_DBG("mc filter %s -> %s", 186 batostr((void *) a1), batostr((void *) a2)); 187 188 /* Iterate from a1 to a2 */ 189 set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter); 190 while (memcmp(a1, a2, 6) < 0 && s->mc_filter != ~0LL) { 191 /* Increment a1 */ 192 i = 5; 193 while (i >= 0 && ++a1[i--] == 0) 194 ; 195 196 set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter); 197 } 198 } 199 } 200 201 BT_DBG("mc filter hash 0x%llx", s->mc_filter); 202 203 bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_SUCCESS); 204 #else 205 bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_FILTER_UNSUPPORTED_REQ); 206 #endif 207 return 0; 208 } 209 210 static int bnep_rx_control(struct bnep_session *s, void *data, int len) 211 { 212 u8 cmd = *(u8 *)data; 213 int err = 0; 214 215 data++; 216 len--; 217 218 switch (cmd) { 219 case BNEP_CMD_NOT_UNDERSTOOD: 220 case BNEP_SETUP_CONN_RSP: 221 case BNEP_FILTER_NET_TYPE_RSP: 222 case BNEP_FILTER_MULTI_ADDR_RSP: 223 /* Ignore these for now */ 224 break; 225 226 case BNEP_FILTER_NET_TYPE_SET: 227 err = bnep_ctrl_set_netfilter(s, data, len); 228 break; 229 230 case BNEP_FILTER_MULTI_ADDR_SET: 231 err = bnep_ctrl_set_mcfilter(s, data, len); 232 break; 233 234 case BNEP_SETUP_CONN_REQ: 235 err = bnep_send_rsp(s, BNEP_SETUP_CONN_RSP, BNEP_CONN_NOT_ALLOWED); 236 break; 237 238 default: { 239 u8 pkt[3]; 240 pkt[0] = BNEP_CONTROL; 241 pkt[1] = BNEP_CMD_NOT_UNDERSTOOD; 242 pkt[2] = cmd; 243 bnep_send(s, pkt, sizeof(pkt)); 244 } 245 break; 246 } 247 248 return err; 249 } 250 251 static int bnep_rx_extension(struct bnep_session *s, struct sk_buff *skb) 252 { 253 struct bnep_ext_hdr *h; 254 int err = 0; 255 256 do { 257 h = (void *) skb->data; 258 if (!skb_pull(skb, sizeof(*h))) { 259 err = -EILSEQ; 260 break; 261 } 262 263 BT_DBG("type 0x%x len %d", h->type, h->len); 264 265 switch (h->type & BNEP_TYPE_MASK) { 266 case BNEP_EXT_CONTROL: 267 bnep_rx_control(s, skb->data, skb->len); 268 break; 269 270 default: 271 /* Unknown extension, skip it. */ 272 break; 273 } 274 275 if (!skb_pull(skb, h->len)) { 276 err = -EILSEQ; 277 break; 278 } 279 } while (!err && (h->type & BNEP_EXT_HEADER)); 280 281 return err; 282 } 283 284 static u8 __bnep_rx_hlen[] = { 285 ETH_HLEN, /* BNEP_GENERAL */ 286 0, /* BNEP_CONTROL */ 287 2, /* BNEP_COMPRESSED */ 288 ETH_ALEN + 2, /* BNEP_COMPRESSED_SRC_ONLY */ 289 ETH_ALEN + 2 /* BNEP_COMPRESSED_DST_ONLY */ 290 }; 291 292 static int bnep_rx_frame(struct bnep_session *s, struct sk_buff *skb) 293 { 294 struct net_device *dev = s->dev; 295 struct sk_buff *nskb; 296 u8 type; 297 298 dev->stats.rx_bytes += skb->len; 299 300 type = *(u8 *) skb->data; 301 skb_pull(skb, 1); 302 303 if ((type & BNEP_TYPE_MASK) >= sizeof(__bnep_rx_hlen)) 304 goto badframe; 305 306 if ((type & BNEP_TYPE_MASK) == BNEP_CONTROL) { 307 bnep_rx_control(s, skb->data, skb->len); 308 kfree_skb(skb); 309 return 0; 310 } 311 312 skb_reset_mac_header(skb); 313 314 /* Verify and pull out header */ 315 if (!skb_pull(skb, __bnep_rx_hlen[type & BNEP_TYPE_MASK])) 316 goto badframe; 317 318 s->eh.h_proto = get_unaligned((__be16 *) (skb->data - 2)); 319 320 if (type & BNEP_EXT_HEADER) { 321 if (bnep_rx_extension(s, skb) < 0) 322 goto badframe; 323 } 324 325 /* Strip 802.1p header */ 326 if (ntohs(s->eh.h_proto) == ETH_P_8021Q) { 327 if (!skb_pull(skb, 4)) 328 goto badframe; 329 s->eh.h_proto = get_unaligned((__be16 *) (skb->data - 2)); 330 } 331 332 /* We have to alloc new skb and copy data here :(. Because original skb 333 * may not be modified and because of the alignment requirements. */ 334 nskb = alloc_skb(2 + ETH_HLEN + skb->len, GFP_KERNEL); 335 if (!nskb) { 336 dev->stats.rx_dropped++; 337 kfree_skb(skb); 338 return -ENOMEM; 339 } 340 skb_reserve(nskb, 2); 341 342 /* Decompress header and construct ether frame */ 343 switch (type & BNEP_TYPE_MASK) { 344 case BNEP_COMPRESSED: 345 memcpy(__skb_put(nskb, ETH_HLEN), &s->eh, ETH_HLEN); 346 break; 347 348 case BNEP_COMPRESSED_SRC_ONLY: 349 memcpy(__skb_put(nskb, ETH_ALEN), s->eh.h_dest, ETH_ALEN); 350 memcpy(__skb_put(nskb, ETH_ALEN), skb_mac_header(skb), ETH_ALEN); 351 put_unaligned(s->eh.h_proto, (__be16 *) __skb_put(nskb, 2)); 352 break; 353 354 case BNEP_COMPRESSED_DST_ONLY: 355 memcpy(__skb_put(nskb, ETH_ALEN), skb_mac_header(skb), 356 ETH_ALEN); 357 memcpy(__skb_put(nskb, ETH_ALEN + 2), s->eh.h_source, 358 ETH_ALEN + 2); 359 break; 360 361 case BNEP_GENERAL: 362 memcpy(__skb_put(nskb, ETH_ALEN * 2), skb_mac_header(skb), 363 ETH_ALEN * 2); 364 put_unaligned(s->eh.h_proto, (__be16 *) __skb_put(nskb, 2)); 365 break; 366 } 367 368 skb_copy_from_linear_data(skb, __skb_put(nskb, skb->len), skb->len); 369 kfree_skb(skb); 370 371 dev->stats.rx_packets++; 372 nskb->ip_summed = CHECKSUM_NONE; 373 nskb->protocol = eth_type_trans(nskb, dev); 374 netif_rx_ni(nskb); 375 return 0; 376 377 badframe: 378 dev->stats.rx_errors++; 379 kfree_skb(skb); 380 return 0; 381 } 382 383 static u8 __bnep_tx_types[] = { 384 BNEP_GENERAL, 385 BNEP_COMPRESSED_SRC_ONLY, 386 BNEP_COMPRESSED_DST_ONLY, 387 BNEP_COMPRESSED 388 }; 389 390 static int bnep_tx_frame(struct bnep_session *s, struct sk_buff *skb) 391 { 392 struct ethhdr *eh = (void *) skb->data; 393 struct socket *sock = s->sock; 394 struct kvec iv[3]; 395 int len = 0, il = 0; 396 u8 type = 0; 397 398 BT_DBG("skb %p dev %p type %d", skb, skb->dev, skb->pkt_type); 399 400 if (!skb->dev) { 401 /* Control frame sent by us */ 402 goto send; 403 } 404 405 iv[il++] = (struct kvec) { &type, 1 }; 406 len++; 407 408 if (compress_src && ether_addr_equal(eh->h_dest, s->eh.h_source)) 409 type |= 0x01; 410 411 if (compress_dst && ether_addr_equal(eh->h_source, s->eh.h_dest)) 412 type |= 0x02; 413 414 if (type) 415 skb_pull(skb, ETH_ALEN * 2); 416 417 type = __bnep_tx_types[type]; 418 switch (type) { 419 case BNEP_COMPRESSED_SRC_ONLY: 420 iv[il++] = (struct kvec) { eh->h_source, ETH_ALEN }; 421 len += ETH_ALEN; 422 break; 423 424 case BNEP_COMPRESSED_DST_ONLY: 425 iv[il++] = (struct kvec) { eh->h_dest, ETH_ALEN }; 426 len += ETH_ALEN; 427 break; 428 } 429 430 send: 431 iv[il++] = (struct kvec) { skb->data, skb->len }; 432 len += skb->len; 433 434 /* FIXME: linearize skb */ 435 { 436 len = kernel_sendmsg(sock, &s->msg, iv, il, len); 437 } 438 kfree_skb(skb); 439 440 if (len > 0) { 441 s->dev->stats.tx_bytes += len; 442 s->dev->stats.tx_packets++; 443 return 0; 444 } 445 446 return len; 447 } 448 449 static int bnep_session(void *arg) 450 { 451 struct bnep_session *s = arg; 452 struct net_device *dev = s->dev; 453 struct sock *sk = s->sock->sk; 454 struct sk_buff *skb; 455 wait_queue_t wait; 456 457 BT_DBG(""); 458 459 set_user_nice(current, -15); 460 461 init_waitqueue_entry(&wait, current); 462 add_wait_queue(sk_sleep(sk), &wait); 463 while (1) { 464 set_current_state(TASK_INTERRUPTIBLE); 465 466 if (atomic_read(&s->terminate)) 467 break; 468 /* RX */ 469 while ((skb = skb_dequeue(&sk->sk_receive_queue))) { 470 skb_orphan(skb); 471 if (!skb_linearize(skb)) 472 bnep_rx_frame(s, skb); 473 else 474 kfree_skb(skb); 475 } 476 477 if (sk->sk_state != BT_CONNECTED) 478 break; 479 480 /* TX */ 481 while ((skb = skb_dequeue(&sk->sk_write_queue))) 482 if (bnep_tx_frame(s, skb)) 483 break; 484 netif_wake_queue(dev); 485 486 schedule(); 487 } 488 __set_current_state(TASK_RUNNING); 489 remove_wait_queue(sk_sleep(sk), &wait); 490 491 /* Cleanup session */ 492 down_write(&bnep_session_sem); 493 494 /* Delete network device */ 495 unregister_netdev(dev); 496 497 /* Wakeup user-space polling for socket errors */ 498 s->sock->sk->sk_err = EUNATCH; 499 500 wake_up_interruptible(sk_sleep(s->sock->sk)); 501 502 /* Release the socket */ 503 fput(s->sock->file); 504 505 __bnep_unlink_session(s); 506 507 up_write(&bnep_session_sem); 508 free_netdev(dev); 509 module_put_and_exit(0); 510 return 0; 511 } 512 513 static struct device *bnep_get_device(struct bnep_session *session) 514 { 515 bdaddr_t *src = &bt_sk(session->sock->sk)->src; 516 bdaddr_t *dst = &bt_sk(session->sock->sk)->dst; 517 struct hci_dev *hdev; 518 struct hci_conn *conn; 519 520 hdev = hci_get_route(dst, src); 521 if (!hdev) 522 return NULL; 523 524 conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, dst); 525 526 hci_dev_put(hdev); 527 528 return conn ? &conn->dev : NULL; 529 } 530 531 static struct device_type bnep_type = { 532 .name = "bluetooth", 533 }; 534 535 int bnep_add_connection(struct bnep_connadd_req *req, struct socket *sock) 536 { 537 struct net_device *dev; 538 struct bnep_session *s, *ss; 539 u8 dst[ETH_ALEN], src[ETH_ALEN]; 540 int err; 541 542 BT_DBG(""); 543 544 baswap((void *) dst, &bt_sk(sock->sk)->dst); 545 baswap((void *) src, &bt_sk(sock->sk)->src); 546 547 /* session struct allocated as private part of net_device */ 548 dev = alloc_netdev(sizeof(struct bnep_session), 549 (*req->device) ? req->device : "bnep%d", 550 bnep_net_setup); 551 if (!dev) 552 return -ENOMEM; 553 554 down_write(&bnep_session_sem); 555 556 ss = __bnep_get_session(dst); 557 if (ss && ss->state == BT_CONNECTED) { 558 err = -EEXIST; 559 goto failed; 560 } 561 562 s = netdev_priv(dev); 563 564 /* This is rx header therefore addresses are swapped. 565 * ie. eh.h_dest is our local address. */ 566 memcpy(s->eh.h_dest, &src, ETH_ALEN); 567 memcpy(s->eh.h_source, &dst, ETH_ALEN); 568 memcpy(dev->dev_addr, s->eh.h_dest, ETH_ALEN); 569 570 s->dev = dev; 571 s->sock = sock; 572 s->role = req->role; 573 s->state = BT_CONNECTED; 574 575 s->msg.msg_flags = MSG_NOSIGNAL; 576 577 #ifdef CONFIG_BT_BNEP_MC_FILTER 578 /* Set default mc filter */ 579 set_bit(bnep_mc_hash(dev->broadcast), (ulong *) &s->mc_filter); 580 #endif 581 582 #ifdef CONFIG_BT_BNEP_PROTO_FILTER 583 /* Set default protocol filter */ 584 bnep_set_default_proto_filter(s); 585 #endif 586 587 SET_NETDEV_DEV(dev, bnep_get_device(s)); 588 SET_NETDEV_DEVTYPE(dev, &bnep_type); 589 590 err = register_netdev(dev); 591 if (err) 592 goto failed; 593 594 __bnep_link_session(s); 595 596 __module_get(THIS_MODULE); 597 s->task = kthread_run(bnep_session, s, "kbnepd %s", dev->name); 598 if (IS_ERR(s->task)) { 599 /* Session thread start failed, gotta cleanup. */ 600 module_put(THIS_MODULE); 601 unregister_netdev(dev); 602 __bnep_unlink_session(s); 603 err = PTR_ERR(s->task); 604 goto failed; 605 } 606 607 up_write(&bnep_session_sem); 608 strcpy(req->device, dev->name); 609 return 0; 610 611 failed: 612 up_write(&bnep_session_sem); 613 free_netdev(dev); 614 return err; 615 } 616 617 int bnep_del_connection(struct bnep_conndel_req *req) 618 { 619 struct bnep_session *s; 620 int err = 0; 621 622 BT_DBG(""); 623 624 down_read(&bnep_session_sem); 625 626 s = __bnep_get_session(req->dst); 627 if (s) { 628 atomic_inc(&s->terminate); 629 wake_up_process(s->task); 630 } else 631 err = -ENOENT; 632 633 up_read(&bnep_session_sem); 634 return err; 635 } 636 637 static void __bnep_copy_ci(struct bnep_conninfo *ci, struct bnep_session *s) 638 { 639 memset(ci, 0, sizeof(*ci)); 640 memcpy(ci->dst, s->eh.h_source, ETH_ALEN); 641 strcpy(ci->device, s->dev->name); 642 ci->flags = s->flags; 643 ci->state = s->state; 644 ci->role = s->role; 645 } 646 647 int bnep_get_connlist(struct bnep_connlist_req *req) 648 { 649 struct bnep_session *s; 650 int err = 0, n = 0; 651 652 down_read(&bnep_session_sem); 653 654 list_for_each_entry(s, &bnep_session_list, list) { 655 struct bnep_conninfo ci; 656 657 __bnep_copy_ci(&ci, s); 658 659 if (copy_to_user(req->ci, &ci, sizeof(ci))) { 660 err = -EFAULT; 661 break; 662 } 663 664 if (++n >= req->cnum) 665 break; 666 667 req->ci++; 668 } 669 req->cnum = n; 670 671 up_read(&bnep_session_sem); 672 return err; 673 } 674 675 int bnep_get_conninfo(struct bnep_conninfo *ci) 676 { 677 struct bnep_session *s; 678 int err = 0; 679 680 down_read(&bnep_session_sem); 681 682 s = __bnep_get_session(ci->dst); 683 if (s) 684 __bnep_copy_ci(ci, s); 685 else 686 err = -ENOENT; 687 688 up_read(&bnep_session_sem); 689 return err; 690 } 691 692 static int __init bnep_init(void) 693 { 694 char flt[50] = ""; 695 696 #ifdef CONFIG_BT_BNEP_PROTO_FILTER 697 strcat(flt, "protocol "); 698 #endif 699 700 #ifdef CONFIG_BT_BNEP_MC_FILTER 701 strcat(flt, "multicast"); 702 #endif 703 704 BT_INFO("BNEP (Ethernet Emulation) ver %s", VERSION); 705 if (flt[0]) 706 BT_INFO("BNEP filters: %s", flt); 707 708 bnep_sock_init(); 709 return 0; 710 } 711 712 static void __exit bnep_exit(void) 713 { 714 bnep_sock_cleanup(); 715 } 716 717 module_init(bnep_init); 718 module_exit(bnep_exit); 719 720 module_param(compress_src, bool, 0644); 721 MODULE_PARM_DESC(compress_src, "Compress sources headers"); 722 723 module_param(compress_dst, bool, 0644); 724 MODULE_PARM_DESC(compress_dst, "Compress destination headers"); 725 726 MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>"); 727 MODULE_DESCRIPTION("Bluetooth BNEP ver " VERSION); 728 MODULE_VERSION(VERSION); 729 MODULE_LICENSE("GPL"); 730 MODULE_ALIAS("bt-proto-4"); 731