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