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 /* 29 * $Id: core.c,v 1.20 2002/08/04 21:23:58 maxk Exp $ 30 */ 31 32 #include <linux/config.h> 33 #include <linux/module.h> 34 35 #include <linux/kernel.h> 36 #include <linux/sched.h> 37 #include <linux/signal.h> 38 #include <linux/init.h> 39 #include <linux/wait.h> 40 #include <linux/errno.h> 41 #include <linux/smp_lock.h> 42 #include <linux/net.h> 43 #include <net/sock.h> 44 45 #include <linux/socket.h> 46 #include <linux/file.h> 47 48 #include <linux/netdevice.h> 49 #include <linux/etherdevice.h> 50 #include <linux/skbuff.h> 51 52 #include <asm/unaligned.h> 53 54 #include <net/bluetooth/bluetooth.h> 55 #include <net/bluetooth/l2cap.h> 56 57 #include "bnep.h" 58 59 #ifndef CONFIG_BT_BNEP_DEBUG 60 #undef BT_DBG 61 #define BT_DBG(D...) 62 #endif 63 64 #define VERSION "1.2" 65 66 static LIST_HEAD(bnep_session_list); 67 static DECLARE_RWSEM(bnep_session_sem); 68 69 static struct bnep_session *__bnep_get_session(u8 *dst) 70 { 71 struct bnep_session *s; 72 struct list_head *p; 73 74 BT_DBG(""); 75 76 list_for_each(p, &bnep_session_list) { 77 s = list_entry(p, struct bnep_session, list); 78 if (!compare_ether_addr(dst, s->eh.h_source)) 79 return s; 80 } 81 return NULL; 82 } 83 84 static void __bnep_link_session(struct bnep_session *s) 85 { 86 /* It's safe to call __module_get() here because sessions are added 87 by the socket layer which has to hold the refference to this module. 88 */ 89 __module_get(THIS_MODULE); 90 list_add(&s->list, &bnep_session_list); 91 } 92 93 static void __bnep_unlink_session(struct bnep_session *s) 94 { 95 list_del(&s->list); 96 module_put(THIS_MODULE); 97 } 98 99 static int bnep_send(struct bnep_session *s, void *data, size_t len) 100 { 101 struct socket *sock = s->sock; 102 struct kvec iv = { data, len }; 103 104 return kernel_sendmsg(sock, &s->msg, &iv, 1, len); 105 } 106 107 static int bnep_send_rsp(struct bnep_session *s, u8 ctrl, u16 resp) 108 { 109 struct bnep_control_rsp rsp; 110 rsp.type = BNEP_CONTROL; 111 rsp.ctrl = ctrl; 112 rsp.resp = htons(resp); 113 return bnep_send(s, &rsp, sizeof(rsp)); 114 } 115 116 #ifdef CONFIG_BT_BNEP_PROTO_FILTER 117 static inline void bnep_set_default_proto_filter(struct bnep_session *s) 118 { 119 /* (IPv4, ARP) */ 120 s->proto_filter[0].start = htons(0x0800); 121 s->proto_filter[0].end = htons(0x0806); 122 /* (RARP, AppleTalk) */ 123 s->proto_filter[1].start = htons(0x8035); 124 s->proto_filter[1].end = htons(0x80F3); 125 /* (IPX, IPv6) */ 126 s->proto_filter[2].start = htons(0x8137); 127 s->proto_filter[2].end = htons(0x86DD); 128 } 129 #endif 130 131 static int bnep_ctrl_set_netfilter(struct bnep_session *s, u16 *data, int len) 132 { 133 int n; 134 135 if (len < 2) 136 return -EILSEQ; 137 138 n = ntohs(get_unaligned(data)); 139 data++; len -= 2; 140 141 if (len < n) 142 return -EILSEQ; 143 144 BT_DBG("filter len %d", n); 145 146 #ifdef CONFIG_BT_BNEP_PROTO_FILTER 147 n /= 4; 148 if (n <= BNEP_MAX_PROTO_FILTERS) { 149 struct bnep_proto_filter *f = s->proto_filter; 150 int i; 151 152 for (i = 0; i < n; i++) { 153 f[i].start = get_unaligned(data++); 154 f[i].end = get_unaligned(data++); 155 156 BT_DBG("proto filter start %d end %d", 157 f[i].start, f[i].end); 158 } 159 160 if (i < BNEP_MAX_PROTO_FILTERS) 161 memset(f + i, 0, sizeof(*f)); 162 163 if (n == 0) 164 bnep_set_default_proto_filter(s); 165 166 bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_SUCCESS); 167 } else { 168 bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_FILTER_LIMIT_REACHED); 169 } 170 #else 171 bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_FILTER_UNSUPPORTED_REQ); 172 #endif 173 return 0; 174 } 175 176 static int bnep_ctrl_set_mcfilter(struct bnep_session *s, u8 *data, int len) 177 { 178 int n; 179 180 if (len < 2) 181 return -EILSEQ; 182 183 n = ntohs(get_unaligned((u16 *) data)); 184 data += 2; len -= 2; 185 186 if (len < n) 187 return -EILSEQ; 188 189 BT_DBG("filter len %d", n); 190 191 #ifdef CONFIG_BT_BNEP_MC_FILTER 192 n /= (ETH_ALEN * 2); 193 194 if (n > 0) { 195 s->mc_filter = 0; 196 197 /* Always send broadcast */ 198 set_bit(bnep_mc_hash(s->dev->broadcast), (ulong *) &s->mc_filter); 199 200 /* Add address ranges to the multicast hash */ 201 for (; n > 0; n--) { 202 u8 a1[6], *a2; 203 204 memcpy(a1, data, ETH_ALEN); data += ETH_ALEN; 205 a2 = data; data += ETH_ALEN; 206 207 BT_DBG("mc filter %s -> %s", 208 batostr((void *) a1), batostr((void *) a2)); 209 210 #define INCA(a) { int i = 5; while (i >=0 && ++a[i--] == 0); } 211 212 /* Iterate from a1 to a2 */ 213 set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter); 214 while (memcmp(a1, a2, 6) < 0 && s->mc_filter != ~0LL) { 215 INCA(a1); 216 set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter); 217 } 218 } 219 } 220 221 BT_DBG("mc filter hash 0x%llx", s->mc_filter); 222 223 bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_SUCCESS); 224 #else 225 bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_FILTER_UNSUPPORTED_REQ); 226 #endif 227 return 0; 228 } 229 230 static int bnep_rx_control(struct bnep_session *s, void *data, int len) 231 { 232 u8 cmd = *(u8 *)data; 233 int err = 0; 234 235 data++; len--; 236 237 switch (cmd) { 238 case BNEP_CMD_NOT_UNDERSTOOD: 239 case BNEP_SETUP_CONN_REQ: 240 case BNEP_SETUP_CONN_RSP: 241 case BNEP_FILTER_NET_TYPE_RSP: 242 case BNEP_FILTER_MULTI_ADDR_RSP: 243 /* Ignore these for now */ 244 break; 245 246 case BNEP_FILTER_NET_TYPE_SET: 247 err = bnep_ctrl_set_netfilter(s, data, len); 248 break; 249 250 case BNEP_FILTER_MULTI_ADDR_SET: 251 err = bnep_ctrl_set_mcfilter(s, data, len); 252 break; 253 254 default: { 255 u8 pkt[3]; 256 pkt[0] = BNEP_CONTROL; 257 pkt[1] = BNEP_CMD_NOT_UNDERSTOOD; 258 pkt[2] = cmd; 259 bnep_send(s, pkt, sizeof(pkt)); 260 } 261 break; 262 } 263 264 return err; 265 } 266 267 static int bnep_rx_extension(struct bnep_session *s, struct sk_buff *skb) 268 { 269 struct bnep_ext_hdr *h; 270 int err = 0; 271 272 do { 273 h = (void *) skb->data; 274 if (!skb_pull(skb, sizeof(*h))) { 275 err = -EILSEQ; 276 break; 277 } 278 279 BT_DBG("type 0x%x len %d", h->type, h->len); 280 281 switch (h->type & BNEP_TYPE_MASK) { 282 case BNEP_EXT_CONTROL: 283 bnep_rx_control(s, skb->data, skb->len); 284 break; 285 286 default: 287 /* Unknown extension, skip it. */ 288 break; 289 } 290 291 if (!skb_pull(skb, h->len)) { 292 err = -EILSEQ; 293 break; 294 } 295 } while (!err && (h->type & BNEP_EXT_HEADER)); 296 297 return err; 298 } 299 300 static u8 __bnep_rx_hlen[] = { 301 ETH_HLEN, /* BNEP_GENERAL */ 302 0, /* BNEP_CONTROL */ 303 2, /* BNEP_COMPRESSED */ 304 ETH_ALEN + 2, /* BNEP_COMPRESSED_SRC_ONLY */ 305 ETH_ALEN + 2 /* BNEP_COMPRESSED_DST_ONLY */ 306 }; 307 #define BNEP_RX_TYPES (sizeof(__bnep_rx_hlen) - 1) 308 309 static inline int bnep_rx_frame(struct bnep_session *s, struct sk_buff *skb) 310 { 311 struct net_device *dev = s->dev; 312 struct sk_buff *nskb; 313 u8 type; 314 315 dev->last_rx = jiffies; 316 s->stats.rx_bytes += skb->len; 317 318 type = *(u8 *) skb->data; skb_pull(skb, 1); 319 320 if ((type & BNEP_TYPE_MASK) > BNEP_RX_TYPES) 321 goto badframe; 322 323 if ((type & BNEP_TYPE_MASK) == BNEP_CONTROL) { 324 bnep_rx_control(s, skb->data, skb->len); 325 kfree_skb(skb); 326 return 0; 327 } 328 329 skb->mac.raw = skb->data; 330 331 /* Verify and pull out header */ 332 if (!skb_pull(skb, __bnep_rx_hlen[type & BNEP_TYPE_MASK])) 333 goto badframe; 334 335 s->eh.h_proto = get_unaligned((u16 *) (skb->data - 2)); 336 337 if (type & BNEP_EXT_HEADER) { 338 if (bnep_rx_extension(s, skb) < 0) 339 goto badframe; 340 } 341 342 /* Strip 802.1p header */ 343 if (ntohs(s->eh.h_proto) == 0x8100) { 344 if (!skb_pull(skb, 4)) 345 goto badframe; 346 s->eh.h_proto = get_unaligned((u16 *) (skb->data - 2)); 347 } 348 349 /* We have to alloc new skb and copy data here :(. Because original skb 350 * may not be modified and because of the alignment requirements. */ 351 nskb = alloc_skb(2 + ETH_HLEN + skb->len, GFP_KERNEL); 352 if (!nskb) { 353 s->stats.rx_dropped++; 354 kfree_skb(skb); 355 return -ENOMEM; 356 } 357 skb_reserve(nskb, 2); 358 359 /* Decompress header and construct ether frame */ 360 switch (type & BNEP_TYPE_MASK) { 361 case BNEP_COMPRESSED: 362 memcpy(__skb_put(nskb, ETH_HLEN), &s->eh, ETH_HLEN); 363 break; 364 365 case BNEP_COMPRESSED_SRC_ONLY: 366 memcpy(__skb_put(nskb, ETH_ALEN), s->eh.h_dest, ETH_ALEN); 367 memcpy(__skb_put(nskb, ETH_ALEN), skb->mac.raw, ETH_ALEN); 368 put_unaligned(s->eh.h_proto, (u16 *) __skb_put(nskb, 2)); 369 break; 370 371 case BNEP_COMPRESSED_DST_ONLY: 372 memcpy(__skb_put(nskb, ETH_ALEN), skb->mac.raw, ETH_ALEN); 373 memcpy(__skb_put(nskb, ETH_ALEN + 2), s->eh.h_source, ETH_ALEN + 2); 374 break; 375 376 case BNEP_GENERAL: 377 memcpy(__skb_put(nskb, ETH_ALEN * 2), skb->mac.raw, ETH_ALEN * 2); 378 put_unaligned(s->eh.h_proto, (u16 *) __skb_put(nskb, 2)); 379 break; 380 } 381 382 memcpy(__skb_put(nskb, skb->len), skb->data, skb->len); 383 kfree_skb(skb); 384 385 s->stats.rx_packets++; 386 nskb->dev = dev; 387 nskb->ip_summed = CHECKSUM_NONE; 388 nskb->protocol = eth_type_trans(nskb, dev); 389 netif_rx_ni(nskb); 390 return 0; 391 392 badframe: 393 s->stats.rx_errors++; 394 kfree_skb(skb); 395 return 0; 396 } 397 398 static u8 __bnep_tx_types[] = { 399 BNEP_GENERAL, 400 BNEP_COMPRESSED_SRC_ONLY, 401 BNEP_COMPRESSED_DST_ONLY, 402 BNEP_COMPRESSED 403 }; 404 405 static inline int bnep_tx_frame(struct bnep_session *s, struct sk_buff *skb) 406 { 407 struct ethhdr *eh = (void *) skb->data; 408 struct socket *sock = s->sock; 409 struct kvec iv[3]; 410 int len = 0, il = 0; 411 u8 type = 0; 412 413 BT_DBG("skb %p dev %p type %d", skb, skb->dev, skb->pkt_type); 414 415 if (!skb->dev) { 416 /* Control frame sent by us */ 417 goto send; 418 } 419 420 iv[il++] = (struct kvec) { &type, 1 }; 421 len++; 422 423 if (!compare_ether_addr(eh->h_dest, s->eh.h_source)) 424 type |= 0x01; 425 426 if (!compare_ether_addr(eh->h_source, s->eh.h_dest)) 427 type |= 0x02; 428 429 if (type) 430 skb_pull(skb, ETH_ALEN * 2); 431 432 type = __bnep_tx_types[type]; 433 switch (type) { 434 case BNEP_COMPRESSED_SRC_ONLY: 435 iv[il++] = (struct kvec) { eh->h_source, ETH_ALEN }; 436 len += ETH_ALEN; 437 break; 438 439 case BNEP_COMPRESSED_DST_ONLY: 440 iv[il++] = (struct kvec) { eh->h_dest, ETH_ALEN }; 441 len += ETH_ALEN; 442 break; 443 } 444 445 send: 446 iv[il++] = (struct kvec) { skb->data, skb->len }; 447 len += skb->len; 448 449 /* FIXME: linearize skb */ 450 { 451 len = kernel_sendmsg(sock, &s->msg, iv, il, len); 452 } 453 kfree_skb(skb); 454 455 if (len > 0) { 456 s->stats.tx_bytes += len; 457 s->stats.tx_packets++; 458 return 0; 459 } 460 461 return len; 462 } 463 464 static int bnep_session(void *arg) 465 { 466 struct bnep_session *s = arg; 467 struct net_device *dev = s->dev; 468 struct sock *sk = s->sock->sk; 469 struct sk_buff *skb; 470 wait_queue_t wait; 471 472 BT_DBG(""); 473 474 daemonize("kbnepd %s", dev->name); 475 set_user_nice(current, -15); 476 current->flags |= PF_NOFREEZE; 477 478 init_waitqueue_entry(&wait, current); 479 add_wait_queue(sk->sk_sleep, &wait); 480 while (!atomic_read(&s->killed)) { 481 set_current_state(TASK_INTERRUPTIBLE); 482 483 // RX 484 while ((skb = skb_dequeue(&sk->sk_receive_queue))) { 485 skb_orphan(skb); 486 bnep_rx_frame(s, skb); 487 } 488 489 if (sk->sk_state != BT_CONNECTED) 490 break; 491 492 // TX 493 while ((skb = skb_dequeue(&sk->sk_write_queue))) 494 if (bnep_tx_frame(s, skb)) 495 break; 496 netif_wake_queue(dev); 497 498 schedule(); 499 } 500 set_current_state(TASK_RUNNING); 501 remove_wait_queue(sk->sk_sleep, &wait); 502 503 /* Cleanup session */ 504 down_write(&bnep_session_sem); 505 506 /* Delete network device */ 507 unregister_netdev(dev); 508 509 /* Release the socket */ 510 fput(s->sock->file); 511 512 __bnep_unlink_session(s); 513 514 up_write(&bnep_session_sem); 515 free_netdev(dev); 516 return 0; 517 } 518 519 int bnep_add_connection(struct bnep_connadd_req *req, struct socket *sock) 520 { 521 struct net_device *dev; 522 struct bnep_session *s, *ss; 523 u8 dst[ETH_ALEN], src[ETH_ALEN]; 524 int err; 525 526 BT_DBG(""); 527 528 baswap((void *) dst, &bt_sk(sock->sk)->dst); 529 baswap((void *) src, &bt_sk(sock->sk)->src); 530 531 /* session struct allocated as private part of net_device */ 532 dev = alloc_netdev(sizeof(struct bnep_session), 533 (*req->device) ? req->device : "bnep%d", 534 bnep_net_setup); 535 if (!dev) 536 return -ENOMEM; 537 538 539 down_write(&bnep_session_sem); 540 541 ss = __bnep_get_session(dst); 542 if (ss && ss->state == BT_CONNECTED) { 543 err = -EEXIST; 544 goto failed; 545 } 546 547 s = dev->priv; 548 549 /* This is rx header therefore addresses are swapped. 550 * ie eh.h_dest is our local address. */ 551 memcpy(s->eh.h_dest, &src, ETH_ALEN); 552 memcpy(s->eh.h_source, &dst, ETH_ALEN); 553 memcpy(dev->dev_addr, s->eh.h_dest, ETH_ALEN); 554 555 s->dev = dev; 556 s->sock = sock; 557 s->role = req->role; 558 s->state = BT_CONNECTED; 559 560 s->msg.msg_flags = MSG_NOSIGNAL; 561 562 #ifdef CONFIG_BT_BNEP_MC_FILTER 563 /* Set default mc filter */ 564 set_bit(bnep_mc_hash(dev->broadcast), (ulong *) &s->mc_filter); 565 #endif 566 567 #ifdef CONFIG_BT_BNEP_PROTO_FILTER 568 /* Set default protocol filter */ 569 bnep_set_default_proto_filter(s); 570 #endif 571 572 err = register_netdev(dev); 573 if (err) { 574 goto failed; 575 } 576 577 __bnep_link_session(s); 578 579 err = kernel_thread(bnep_session, s, CLONE_KERNEL); 580 if (err < 0) { 581 /* Session thread start failed, gotta cleanup. */ 582 unregister_netdev(dev); 583 __bnep_unlink_session(s); 584 goto failed; 585 } 586 587 up_write(&bnep_session_sem); 588 strcpy(req->device, dev->name); 589 return 0; 590 591 failed: 592 up_write(&bnep_session_sem); 593 free_netdev(dev); 594 return err; 595 } 596 597 int bnep_del_connection(struct bnep_conndel_req *req) 598 { 599 struct bnep_session *s; 600 int err = 0; 601 602 BT_DBG(""); 603 604 down_read(&bnep_session_sem); 605 606 s = __bnep_get_session(req->dst); 607 if (s) { 608 /* Wakeup user-space which is polling for socket errors. 609 * This is temporary hack untill we have shutdown in L2CAP */ 610 s->sock->sk->sk_err = EUNATCH; 611 612 /* Kill session thread */ 613 atomic_inc(&s->killed); 614 wake_up_interruptible(s->sock->sk->sk_sleep); 615 } else 616 err = -ENOENT; 617 618 up_read(&bnep_session_sem); 619 return err; 620 } 621 622 static void __bnep_copy_ci(struct bnep_conninfo *ci, struct bnep_session *s) 623 { 624 memcpy(ci->dst, s->eh.h_source, ETH_ALEN); 625 strcpy(ci->device, s->dev->name); 626 ci->flags = s->flags; 627 ci->state = s->state; 628 ci->role = s->role; 629 } 630 631 int bnep_get_connlist(struct bnep_connlist_req *req) 632 { 633 struct list_head *p; 634 int err = 0, n = 0; 635 636 down_read(&bnep_session_sem); 637 638 list_for_each(p, &bnep_session_list) { 639 struct bnep_session *s; 640 struct bnep_conninfo ci; 641 642 s = list_entry(p, struct bnep_session, list); 643 644 __bnep_copy_ci(&ci, s); 645 646 if (copy_to_user(req->ci, &ci, sizeof(ci))) { 647 err = -EFAULT; 648 break; 649 } 650 651 if (++n >= req->cnum) 652 break; 653 654 req->ci++; 655 } 656 req->cnum = n; 657 658 up_read(&bnep_session_sem); 659 return err; 660 } 661 662 int bnep_get_conninfo(struct bnep_conninfo *ci) 663 { 664 struct bnep_session *s; 665 int err = 0; 666 667 down_read(&bnep_session_sem); 668 669 s = __bnep_get_session(ci->dst); 670 if (s) 671 __bnep_copy_ci(ci, s); 672 else 673 err = -ENOENT; 674 675 up_read(&bnep_session_sem); 676 return err; 677 } 678 679 static int __init bnep_init(void) 680 { 681 char flt[50] = ""; 682 683 l2cap_load(); 684 685 #ifdef CONFIG_BT_BNEP_PROTO_FILTER 686 strcat(flt, "protocol "); 687 #endif 688 689 #ifdef CONFIG_BT_BNEP_MC_FILTER 690 strcat(flt, "multicast"); 691 #endif 692 693 BT_INFO("BNEP (Ethernet Emulation) ver %s", VERSION); 694 if (flt[0]) 695 BT_INFO("BNEP filters: %s", flt); 696 697 bnep_sock_init(); 698 return 0; 699 } 700 701 static void __exit bnep_exit(void) 702 { 703 bnep_sock_cleanup(); 704 } 705 706 module_init(bnep_init); 707 module_exit(bnep_exit); 708 709 MODULE_AUTHOR("David Libault <david.libault@inventel.fr>, Maxim Krasnyansky <maxk@qualcomm.com>"); 710 MODULE_DESCRIPTION("Bluetooth BNEP ver " VERSION); 711 MODULE_VERSION(VERSION); 712 MODULE_LICENSE("GPL"); 713 MODULE_ALIAS("bt-proto-4"); 714