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