1309124Sdim/* 2292932Sdim BNEP implementation for Linux Bluetooth stack (BlueZ). 3353358Sdim Copyright (C) 2001-2002 Inventel Systemes 4353358Sdim Written 2001-2002 by 5353358Sdim Cl��ment Moreau <clement.moreau@inventel.fr> 6292932Sdim David Libault <david.libault@inventel.fr> 7292932Sdim 8292932Sdim Copyright (C) 2002 Maxim Krasnyansky <maxk@qualcomm.com> 9292932Sdim 10292932Sdim This program is free software; you can redistribute it and/or modify 11292932Sdim it under the terms of the GNU General Public License version 2 as 12292932Sdim published by the Free Software Foundation; 13292932Sdim 14292932Sdim THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 15292932Sdim OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16292932Sdim FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. 17321369Sdim IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY 18321369Sdim CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES 19321369Sdim WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 20321369Sdim ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 21292932Sdim OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 22292932Sdim 23292932Sdim ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, 24292932Sdim COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS 25292932Sdim SOFTWARE IS DISCLAIMED. 26314564Sdim*/ 27309124Sdim 28314564Sdim#include <linux/module.h> 29314564Sdim#include <linux/kthread.h> 30314564Sdim#include <linux/file.h> 31314564Sdim#include <linux/etherdevice.h> 32314564Sdim#include <asm/unaligned.h> 33314564Sdim 34314564Sdim#include <net/bluetooth/bluetooth.h> 35314564Sdim#include <net/bluetooth/l2cap.h> 36314564Sdim#include <net/bluetooth/hci_core.h> 37314564Sdim 38314564Sdim#include "bnep.h" 39314564Sdim 40309124Sdim#define VERSION "1.3" 41292932Sdim 42314564Sdimstatic bool compress_src = true; 43309124Sdimstatic bool compress_dst = true; 44314564Sdim 45314564Sdimstatic LIST_HEAD(bnep_session_list); 46314564Sdimstatic DECLARE_RWSEM(bnep_session_sem); 47314564Sdim 48314564Sdimstatic struct bnep_session *__bnep_get_session(u8 *dst) 49314564Sdim{ 50314564Sdim struct bnep_session *s; 51314564Sdim 52353358Sdim BT_DBG(""); 53314564Sdim 54292932Sdim list_for_each_entry(s, &bnep_session_list, list) 55314564Sdim if (ether_addr_equal(dst, s->eh.h_source)) 56314564Sdim return s; 57314564Sdim 58314564Sdim return NULL; 59292932Sdim} 60292932Sdim 61314564Sdimstatic void __bnep_link_session(struct bnep_session *s) 62309124Sdim{ 63314564Sdim list_add(&s->list, &bnep_session_list); 64292932Sdim} 65314564Sdim 66309124Sdimstatic void __bnep_unlink_session(struct bnep_session *s) 67314564Sdim{ 68309124Sdim list_del(&s->list); 69314564Sdim} 70309124Sdim 71314564Sdimstatic int bnep_send(struct bnep_session *s, void *data, size_t len) 72309124Sdim{ 73353358Sdim struct socket *sock = s->sock; 74309124Sdim struct kvec iv = { data, len }; 75309124Sdim 76309124Sdim return kernel_sendmsg(sock, &s->msg, &iv, 1, len); 77309124Sdim} 78314564Sdim 79314564Sdimstatic int bnep_send_rsp(struct bnep_session *s, u8 ctrl, u16 resp) 80314564Sdim{ 81314564Sdim struct bnep_control_rsp rsp; 82314564Sdim rsp.type = BNEP_CONTROL; 83314564Sdim rsp.ctrl = ctrl; 84292932Sdim rsp.resp = htons(resp); 85292932Sdim return bnep_send(s, &rsp, sizeof(rsp)); 86314564Sdim} 87314564Sdim 88314564Sdim#ifdef CONFIG_BT_BNEP_PROTO_FILTER 89314564Sdimstatic inline void bnep_set_default_proto_filter(struct bnep_session *s) 90314564Sdim{ 91314564Sdim /* (IPv4, ARP) */ 92314564Sdim s->proto_filter[0].start = ETH_P_IP; 93314564Sdim s->proto_filter[0].end = ETH_P_ARP; 94314564Sdim /* (RARP, AppleTalk) */ 95314564Sdim s->proto_filter[1].start = ETH_P_RARP; 96314564Sdim s->proto_filter[1].end = ETH_P_AARP; 97314564Sdim /* (IPX, IPv6) */ 98314564Sdim s->proto_filter[2].start = ETH_P_IPX; 99314564Sdim s->proto_filter[2].end = ETH_P_IPV6; 100314564Sdim} 101314564Sdim#endif 102314564Sdim 103314564Sdimstatic int bnep_ctrl_set_netfilter(struct bnep_session *s, __be16 *data, int len) 104314564Sdim{ 105314564Sdim int n; 106314564Sdim 107314564Sdim if (len < 2) 108314564Sdim return -EILSEQ; 109314564Sdim 110314564Sdim n = get_unaligned_be16(data); 111314564Sdim data++; 112314564Sdim len -= 2; 113314564Sdim 114314564Sdim if (len < n) 115327952Sdim return -EILSEQ; 116327952Sdim 117314564Sdim BT_DBG("filter len %d", n); 118314564Sdim 119314564Sdim#ifdef CONFIG_BT_BNEP_PROTO_FILTER 120314564Sdim n /= 4; 121314564Sdim if (n <= BNEP_MAX_PROTO_FILTERS) { 122314564Sdim struct bnep_proto_filter *f = s->proto_filter; 123292932Sdim int i; 124292932Sdim 125314564Sdim for (i = 0; i < n; i++) { 126314564Sdim f[i].start = get_unaligned_be16(data++); 127292932Sdim f[i].end = get_unaligned_be16(data++); 128292932Sdim 129292932Sdim BT_DBG("proto filter start %u end %u", 130314564Sdim f[i].start, f[i].end); 131314564Sdim } 132314564Sdim 133314564Sdim if (i < BNEP_MAX_PROTO_FILTERS) 134314564Sdim memset(f + i, 0, sizeof(*f)); 135314564Sdim 136314564Sdim if (n == 0) 137314564Sdim bnep_set_default_proto_filter(s); 138314564Sdim 139292932Sdim bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_SUCCESS); 140292932Sdim } else { 141314564Sdim bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_FILTER_LIMIT_REACHED); 142292932Sdim } 143314564Sdim#else 144353358Sdim bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_FILTER_UNSUPPORTED_REQ); 145353358Sdim#endif 146314564Sdim return 0; 147353358Sdim} 148314564Sdim 149314564Sdimstatic int bnep_ctrl_set_mcfilter(struct bnep_session *s, u8 *data, int len) 150292932Sdim{ 151292932Sdim int n; 152314564Sdim 153314564Sdim if (len < 2) 154314564Sdim return -EILSEQ; 155314564Sdim 156314564Sdim n = get_unaligned_be16(data); 157292932Sdim data += 2; 158292932Sdim len -= 2; 159292932Sdim 160292932Sdim if (len < n) 161314564Sdim return -EILSEQ; 162314564Sdim 163292932Sdim BT_DBG("filter len %d", n); 164292932Sdim 165292932Sdim#ifdef CONFIG_BT_BNEP_MC_FILTER 166292932Sdim n /= (ETH_ALEN * 2); 167292932Sdim 168292932Sdim if (n > 0) { 169314564Sdim int i; 170314564Sdim 171314564Sdim s->mc_filter = 0; 172314564Sdim 173314564Sdim /* Always send broadcast */ 174314564Sdim set_bit(bnep_mc_hash(s->dev->broadcast), (ulong *) &s->mc_filter); 175314564Sdim 176314564Sdim /* Add address ranges to the multicast hash */ 177314564Sdim for (; n > 0; n--) { 178292932Sdim u8 a1[6], *a2; 179292932Sdim 180314564Sdim memcpy(a1, data, ETH_ALEN); 181314564Sdim data += ETH_ALEN; 182314564Sdim a2 = data; 183314564Sdim data += ETH_ALEN; 184314564Sdim 185314564Sdim BT_DBG("mc filter %pMR -> %pMR", a1, a2); 186314564Sdim 187292932Sdim /* Iterate from a1 to a2 */ 188292932Sdim set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter); 189314564Sdim while (memcmp(a1, a2, 6) < 0 && s->mc_filter != ~0LL) { 190314564Sdim /* Increment a1 */ 191314564Sdim i = 5; 192314564Sdim while (i >= 0 && ++a1[i--] == 0) 193314564Sdim ; 194292932Sdim 195292932Sdim set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter); 196314564Sdim } 197314564Sdim } 198314564Sdim } 199314564Sdim 200314564Sdim BT_DBG("mc filter hash 0x%llx", s->mc_filter); 201314564Sdim 202314564Sdim bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_SUCCESS); 203314564Sdim#else 204321369Sdim bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_FILTER_UNSUPPORTED_REQ); 205314564Sdim#endif 206314564Sdim return 0; 207314564Sdim} 208314564Sdim 209314564Sdimstatic int bnep_rx_control(struct bnep_session *s, void *data, int len) 210314564Sdim{ 211314564Sdim u8 cmd = *(u8 *)data; 212292932Sdim int err = 0; 213292932Sdim 214314564Sdim data++; 215314564Sdim len--; 216292932Sdim 217314564Sdim switch (cmd) { 218314564Sdim case BNEP_CMD_NOT_UNDERSTOOD: 219314564Sdim case BNEP_SETUP_CONN_RSP: 220314564Sdim case BNEP_FILTER_NET_TYPE_RSP: 221292932Sdim case BNEP_FILTER_MULTI_ADDR_RSP: 222292932Sdim /* Ignore these for now */ 223314564Sdim break; 224292932Sdim 225314564Sdim case BNEP_FILTER_NET_TYPE_SET: 226353358Sdim err = bnep_ctrl_set_netfilter(s, data, len); 227353358Sdim break; 228314564Sdim 229314564Sdim case BNEP_FILTER_MULTI_ADDR_SET: 230292932Sdim err = bnep_ctrl_set_mcfilter(s, data, len); 231292932Sdim break; 232314564Sdim 233314564Sdim case BNEP_SETUP_CONN_REQ: 234314564Sdim /* Successful response should be sent only once */ 235314564Sdim if (test_bit(BNEP_SETUP_RESPONSE, &s->flags) && 236314564Sdim !test_and_set_bit(BNEP_SETUP_RSP_SENT, &s->flags)) 237314564Sdim err = bnep_send_rsp(s, BNEP_SETUP_CONN_RSP, 238314564Sdim BNEP_SUCCESS); 239314564Sdim else 240314564Sdim err = bnep_send_rsp(s, BNEP_SETUP_CONN_RSP, 241314564Sdim BNEP_CONN_NOT_ALLOWED); 242314564Sdim break; 243314564Sdim 244292932Sdim default: { 245314564Sdim u8 pkt[3]; 246321369Sdim pkt[0] = BNEP_CONTROL; 247314564Sdim pkt[1] = BNEP_CMD_NOT_UNDERSTOOD; 248314564Sdim pkt[2] = cmd; 249314564Sdim err = bnep_send(s, pkt, sizeof(pkt)); 250314564Sdim } 251314564Sdim break; 252314564Sdim } 253314564Sdim 254314564Sdim return err; 255314564Sdim} 256314564Sdim 257314564Sdimstatic int bnep_rx_extension(struct bnep_session *s, struct sk_buff *skb) 258314564Sdim{ 259314564Sdim struct bnep_ext_hdr *h; 260314564Sdim int err = 0; 261314564Sdim 262292932Sdim do { 263314564Sdim h = (void *) skb->data; 264314564Sdim if (!skb_pull(skb, sizeof(*h))) { 265314564Sdim err = -EILSEQ; 266314564Sdim break; 267314564Sdim } 268314564Sdim 269314564Sdim BT_DBG("type 0x%x len %u", h->type, h->len); 270314564Sdim 271314564Sdim switch (h->type & BNEP_TYPE_MASK) { 272314564Sdim case BNEP_EXT_CONTROL: 273314564Sdim bnep_rx_control(s, skb->data, skb->len); 274314564Sdim break; 275292932Sdim 276314564Sdim default: 277314564Sdim /* Unknown extension, skip it. */ 278292932Sdim break; 279292932Sdim } 280314564Sdim 281314564Sdim if (!skb_pull(skb, h->len)) { 282314564Sdim err = -EILSEQ; 283314564Sdim break; 284314564Sdim } 285314564Sdim } while (!err && (h->type & BNEP_EXT_HEADER)); 286314564Sdim 287314564Sdim return err; 288314564Sdim} 289314564Sdim 290314564Sdimstatic u8 __bnep_rx_hlen[] = { 291314564Sdim ETH_HLEN, /* BNEP_GENERAL */ 292292932Sdim 0, /* BNEP_CONTROL */ 293314564Sdim 2, /* BNEP_COMPRESSED */ 294314564Sdim ETH_ALEN + 2, /* BNEP_COMPRESSED_SRC_ONLY */ 295292932Sdim ETH_ALEN + 2 /* BNEP_COMPRESSED_DST_ONLY */ 296314564Sdim}; 297314564Sdim 298292932Sdimstatic int bnep_rx_frame(struct bnep_session *s, struct sk_buff *skb) 299344779Sdim{ 300344779Sdim struct net_device *dev = s->dev; 301344779Sdim struct sk_buff *nskb; 302344779Sdim u8 type, ctrl_type; 303344779Sdim 304292932Sdim dev->stats.rx_bytes += skb->len; 305314564Sdim 306321369Sdim type = *(u8 *) skb->data; 307314564Sdim skb_pull(skb, 1); 308314564Sdim ctrl_type = *(u8 *)skb->data; 309314564Sdim 310314564Sdim if ((type & BNEP_TYPE_MASK) >= sizeof(__bnep_rx_hlen)) 311314564Sdim goto badframe; 312314564Sdim 313314564Sdim if ((type & BNEP_TYPE_MASK) == BNEP_CONTROL) { 314314564Sdim if (bnep_rx_control(s, skb->data, skb->len) < 0) { 315314564Sdim dev->stats.tx_errors++; 316314564Sdim kfree_skb(skb); 317314564Sdim return 0; 318314564Sdim } 319314564Sdim 320314564Sdim if (!(type & BNEP_EXT_HEADER)) { 321314564Sdim kfree_skb(skb); 322314564Sdim return 0; 323292932Sdim } 324314564Sdim 325314564Sdim /* Verify and pull ctrl message since it's already processed */ 326314564Sdim switch (ctrl_type) { 327314564Sdim case BNEP_SETUP_CONN_REQ: 328314564Sdim /* Pull: ctrl type (1 b), len (1 b), data (len bytes) */ 329314564Sdim if (!skb_pull(skb, 2 + *(u8 *)(skb->data + 1) * 2)) 330314564Sdim goto badframe; 331314564Sdim break; 332314564Sdim case BNEP_FILTER_MULTI_ADDR_SET: 333314564Sdim case BNEP_FILTER_NET_TYPE_SET: 334314564Sdim /* Pull: ctrl type (1 b), len (2 b), data (len bytes) */ 335314564Sdim if (!skb_pull(skb, 3 + *(u16 *)(skb->data + 1) * 2)) 336314564Sdim goto badframe; 337314564Sdim break; 338314564Sdim default: 339314564Sdim kfree_skb(skb); 340314564Sdim return 0; 341314564Sdim } 342314564Sdim } else { 343314564Sdim skb_reset_mac_header(skb); 344314564Sdim 345292932Sdim /* Verify and pull out header */ 346314564Sdim if (!skb_pull(skb, __bnep_rx_hlen[type & BNEP_TYPE_MASK])) 347314564Sdim goto badframe; 348292932Sdim 349309124Sdim s->eh.h_proto = get_unaligned((__be16 *) (skb->data - 2)); 350314564Sdim } 351314564Sdim 352314564Sdim if (type & BNEP_EXT_HEADER) { 353314564Sdim if (bnep_rx_extension(s, skb) < 0) 354314564Sdim goto badframe; 355309124Sdim } 356309124Sdim 357314564Sdim /* Strip 802.1p header */ 358309124Sdim if (ntohs(s->eh.h_proto) == ETH_P_8021Q) { 359309124Sdim if (!skb_pull(skb, 4)) 360314564Sdim goto badframe; 361314564Sdim s->eh.h_proto = get_unaligned((__be16 *) (skb->data - 2)); 362314564Sdim } 363314564Sdim 364309124Sdim /* We have to alloc new skb and copy data here :(. Because original skb 365314564Sdim * may not be modified and because of the alignment requirements. */ 366314564Sdim nskb = alloc_skb(2 + ETH_HLEN + skb->len, GFP_KERNEL); 367314564Sdim if (!nskb) { 368314564Sdim dev->stats.rx_dropped++; 369309124Sdim kfree_skb(skb); 370309124Sdim return -ENOMEM; 371314564Sdim } 372309124Sdim skb_reserve(nskb, 2); 373314564Sdim 374309124Sdim /* Decompress header and construct ether frame */ 375314564Sdim switch (type & BNEP_TYPE_MASK) { 376353358Sdim case BNEP_COMPRESSED: 377353358Sdim __skb_put_data(nskb, &s->eh, ETH_HLEN); 378314564Sdim break; 379314564Sdim 380309124Sdim case BNEP_COMPRESSED_SRC_ONLY: 381309124Sdim __skb_put_data(nskb, s->eh.h_dest, ETH_ALEN); 382309124Sdim __skb_put_data(nskb, skb_mac_header(skb), ETH_ALEN); 383314564Sdim put_unaligned(s->eh.h_proto, (__be16 *) __skb_put(nskb, 2)); 384314564Sdim break; 385314564Sdim 386314564Sdim case BNEP_COMPRESSED_DST_ONLY: 387309124Sdim __skb_put_data(nskb, skb_mac_header(skb), ETH_ALEN); 388309124Sdim __skb_put_data(nskb, s->eh.h_source, ETH_ALEN); 389314564Sdim put_unaligned(s->eh.h_proto, (__be16 *)__skb_put(nskb, 2)); 390314564Sdim break; 391314564Sdim 392314564Sdim case BNEP_GENERAL: 393314564Sdim __skb_put_data(nskb, skb_mac_header(skb), ETH_ALEN * 2); 394309124Sdim put_unaligned(s->eh.h_proto, (__be16 *) __skb_put(nskb, 2)); 395314564Sdim break; 396314564Sdim } 397314564Sdim 398314564Sdim skb_copy_from_linear_data(skb, __skb_put(nskb, skb->len), skb->len); 399309124Sdim kfree_skb(skb); 400314564Sdim 401314564Sdim dev->stats.rx_packets++; 402314564Sdim nskb->ip_summed = CHECKSUM_NONE; 403314564Sdim nskb->protocol = eth_type_trans(nskb, dev); 404314564Sdim netif_rx(nskb); 405309124Sdim return 0; 406314564Sdim 407314564Sdimbadframe: 408314564Sdim dev->stats.rx_errors++; 409314564Sdim kfree_skb(skb); 410314564Sdim return 0; 411309124Sdim} 412321369Sdim 413314564Sdimstatic u8 __bnep_tx_types[] = { 414314564Sdim BNEP_GENERAL, 415314564Sdim BNEP_COMPRESSED_SRC_ONLY, 416314564Sdim BNEP_COMPRESSED_DST_ONLY, 417314564Sdim BNEP_COMPRESSED 418314564Sdim}; 419314564Sdim 420314564Sdimstatic int bnep_tx_frame(struct bnep_session *s, struct sk_buff *skb) 421309124Sdim{ 422314564Sdim struct ethhdr *eh = (void *) skb->data; 423309124Sdim struct socket *sock = s->sock; 424314564Sdim struct kvec iv[3]; 425314564Sdim int len = 0, il = 0; 426309124Sdim u8 type = 0; 427 428 BT_DBG("skb %p dev %p type %u", skb, skb->dev, skb->pkt_type); 429 430 if (!skb->dev) { 431 /* Control frame sent by us */ 432 goto send; 433 } 434 435 iv[il++] = (struct kvec) { &type, 1 }; 436 len++; 437 438 if (compress_src && ether_addr_equal(eh->h_dest, s->eh.h_source)) 439 type |= 0x01; 440 441 if (compress_dst && ether_addr_equal(eh->h_source, s->eh.h_dest)) 442 type |= 0x02; 443 444 if (type) 445 skb_pull(skb, ETH_ALEN * 2); 446 447 type = __bnep_tx_types[type]; 448 switch (type) { 449 case BNEP_COMPRESSED_SRC_ONLY: 450 iv[il++] = (struct kvec) { eh->h_source, ETH_ALEN }; 451 len += ETH_ALEN; 452 break; 453 454 case BNEP_COMPRESSED_DST_ONLY: 455 iv[il++] = (struct kvec) { eh->h_dest, ETH_ALEN }; 456 len += ETH_ALEN; 457 break; 458 } 459 460send: 461 iv[il++] = (struct kvec) { skb->data, skb->len }; 462 len += skb->len; 463 464 /* FIXME: linearize skb */ 465 { 466 len = kernel_sendmsg(sock, &s->msg, iv, il, len); 467 } 468 kfree_skb(skb); 469 470 if (len > 0) { 471 s->dev->stats.tx_bytes += len; 472 s->dev->stats.tx_packets++; 473 return 0; 474 } 475 476 return len; 477} 478 479static int bnep_session(void *arg) 480{ 481 struct bnep_session *s = arg; 482 struct net_device *dev = s->dev; 483 struct sock *sk = s->sock->sk; 484 struct sk_buff *skb; 485 DEFINE_WAIT_FUNC(wait, woken_wake_function); 486 487 BT_DBG(""); 488 489 set_user_nice(current, -15); 490 491 add_wait_queue(sk_sleep(sk), &wait); 492 while (1) { 493 if (atomic_read(&s->terminate)) 494 break; 495 /* RX */ 496 while ((skb = skb_dequeue(&sk->sk_receive_queue))) { 497 skb_orphan(skb); 498 if (!skb_linearize(skb)) 499 bnep_rx_frame(s, skb); 500 else 501 kfree_skb(skb); 502 } 503 504 if (sk->sk_state != BT_CONNECTED) 505 break; 506 507 /* TX */ 508 while ((skb = skb_dequeue(&sk->sk_write_queue))) 509 if (bnep_tx_frame(s, skb)) 510 break; 511 netif_wake_queue(dev); 512 513 /* 514 * wait_woken() performs the necessary memory barriers 515 * for us; see the header comment for this primitive. 516 */ 517 wait_woken(&wait, TASK_INTERRUPTIBLE, MAX_SCHEDULE_TIMEOUT); 518 } 519 remove_wait_queue(sk_sleep(sk), &wait); 520 521 /* Cleanup session */ 522 down_write(&bnep_session_sem); 523 524 /* Delete network device */ 525 unregister_netdev(dev); 526 527 /* Wakeup user-space polling for socket errors */ 528 s->sock->sk->sk_err = EUNATCH; 529 530 wake_up_interruptible(sk_sleep(s->sock->sk)); 531 532 /* Release the socket */ 533 fput(s->sock->file); 534 535 __bnep_unlink_session(s); 536 537 up_write(&bnep_session_sem); 538 free_netdev(dev); 539 module_put_and_kthread_exit(0); 540 return 0; 541} 542 543static struct device *bnep_get_device(struct bnep_session *session) 544{ 545 struct l2cap_conn *conn = l2cap_pi(session->sock->sk)->chan->conn; 546 547 if (!conn || !conn->hcon) 548 return NULL; 549 550 return &conn->hcon->dev; 551} 552 553static const struct device_type bnep_type = { 554 .name = "bluetooth", 555}; 556 557int bnep_add_connection(struct bnep_connadd_req *req, struct socket *sock) 558{ 559 u32 valid_flags = BIT(BNEP_SETUP_RESPONSE); 560 struct net_device *dev; 561 struct bnep_session *s, *ss; 562 u8 dst[ETH_ALEN], src[ETH_ALEN]; 563 int err; 564 565 BT_DBG(""); 566 567 if (!l2cap_is_socket(sock)) 568 return -EBADFD; 569 570 if (req->flags & ~valid_flags) 571 return -EINVAL; 572 573 baswap((void *) dst, &l2cap_pi(sock->sk)->chan->dst); 574 baswap((void *) src, &l2cap_pi(sock->sk)->chan->src); 575 576 /* session struct allocated as private part of net_device */ 577 dev = alloc_netdev(sizeof(struct bnep_session), 578 (*req->device) ? req->device : "bnep%d", 579 NET_NAME_UNKNOWN, 580 bnep_net_setup); 581 if (!dev) 582 return -ENOMEM; 583 584 down_write(&bnep_session_sem); 585 586 ss = __bnep_get_session(dst); 587 if (ss && ss->state == BT_CONNECTED) { 588 err = -EEXIST; 589 goto failed; 590 } 591 592 s = netdev_priv(dev); 593 594 /* This is rx header therefore addresses are swapped. 595 * ie. eh.h_dest is our local address. */ 596 memcpy(s->eh.h_dest, &src, ETH_ALEN); 597 memcpy(s->eh.h_source, &dst, ETH_ALEN); 598 eth_hw_addr_set(dev, s->eh.h_dest); 599 600 s->dev = dev; 601 s->sock = sock; 602 s->role = req->role; 603 s->state = BT_CONNECTED; 604 s->flags = req->flags; 605 606 s->msg.msg_flags = MSG_NOSIGNAL; 607 608#ifdef CONFIG_BT_BNEP_MC_FILTER 609 /* Set default mc filter to not filter out any mc addresses 610 * as defined in the BNEP specification (revision 0.95a) 611 * http://grouper.ieee.org/groups/802/15/Bluetooth/BNEP.pdf 612 */ 613 s->mc_filter = ~0LL; 614#endif 615 616#ifdef CONFIG_BT_BNEP_PROTO_FILTER 617 /* Set default protocol filter */ 618 bnep_set_default_proto_filter(s); 619#endif 620 621 SET_NETDEV_DEV(dev, bnep_get_device(s)); 622 SET_NETDEV_DEVTYPE(dev, &bnep_type); 623 624 err = register_netdev(dev); 625 if (err) 626 goto failed; 627 628 __bnep_link_session(s); 629 630 __module_get(THIS_MODULE); 631 s->task = kthread_run(bnep_session, s, "kbnepd %s", dev->name); 632 if (IS_ERR(s->task)) { 633 /* Session thread start failed, gotta cleanup. */ 634 module_put(THIS_MODULE); 635 unregister_netdev(dev); 636 __bnep_unlink_session(s); 637 err = PTR_ERR(s->task); 638 goto failed; 639 } 640 641 up_write(&bnep_session_sem); 642 strcpy(req->device, dev->name); 643 return 0; 644 645failed: 646 up_write(&bnep_session_sem); 647 free_netdev(dev); 648 return err; 649} 650 651int bnep_del_connection(struct bnep_conndel_req *req) 652{ 653 u32 valid_flags = 0; 654 struct bnep_session *s; 655 int err = 0; 656 657 BT_DBG(""); 658 659 if (req->flags & ~valid_flags) 660 return -EINVAL; 661 662 down_read(&bnep_session_sem); 663 664 s = __bnep_get_session(req->dst); 665 if (s) { 666 atomic_inc(&s->terminate); 667 wake_up_interruptible(sk_sleep(s->sock->sk)); 668 } else 669 err = -ENOENT; 670 671 up_read(&bnep_session_sem); 672 return err; 673} 674 675static void __bnep_copy_ci(struct bnep_conninfo *ci, struct bnep_session *s) 676{ 677 u32 valid_flags = BIT(BNEP_SETUP_RESPONSE); 678 679 memset(ci, 0, sizeof(*ci)); 680 memcpy(ci->dst, s->eh.h_source, ETH_ALEN); 681 strcpy(ci->device, s->dev->name); 682 ci->flags = s->flags & valid_flags; 683 ci->state = s->state; 684 ci->role = s->role; 685} 686 687int bnep_get_connlist(struct bnep_connlist_req *req) 688{ 689 struct bnep_session *s; 690 int err = 0, n = 0; 691 692 down_read(&bnep_session_sem); 693 694 list_for_each_entry(s, &bnep_session_list, list) { 695 struct bnep_conninfo ci; 696 697 __bnep_copy_ci(&ci, s); 698 699 if (copy_to_user(req->ci, &ci, sizeof(ci))) { 700 err = -EFAULT; 701 break; 702 } 703 704 if (++n >= req->cnum) 705 break; 706 707 req->ci++; 708 } 709 req->cnum = n; 710 711 up_read(&bnep_session_sem); 712 return err; 713} 714 715int bnep_get_conninfo(struct bnep_conninfo *ci) 716{ 717 struct bnep_session *s; 718 int err = 0; 719 720 down_read(&bnep_session_sem); 721 722 s = __bnep_get_session(ci->dst); 723 if (s) 724 __bnep_copy_ci(ci, s); 725 else 726 err = -ENOENT; 727 728 up_read(&bnep_session_sem); 729 return err; 730} 731 732static int __init bnep_init(void) 733{ 734 char flt[50] = ""; 735 736#ifdef CONFIG_BT_BNEP_PROTO_FILTER 737 strcat(flt, "protocol "); 738#endif 739 740#ifdef CONFIG_BT_BNEP_MC_FILTER 741 strcat(flt, "multicast"); 742#endif 743 744 BT_INFO("BNEP (Ethernet Emulation) ver %s", VERSION); 745 if (flt[0]) 746 BT_INFO("BNEP filters: %s", flt); 747 748 bnep_sock_init(); 749 return 0; 750} 751 752static void __exit bnep_exit(void) 753{ 754 bnep_sock_cleanup(); 755} 756 757module_init(bnep_init); 758module_exit(bnep_exit); 759 760module_param(compress_src, bool, 0644); 761MODULE_PARM_DESC(compress_src, "Compress sources headers"); 762 763module_param(compress_dst, bool, 0644); 764MODULE_PARM_DESC(compress_dst, "Compress destination headers"); 765 766MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>"); 767MODULE_DESCRIPTION("Bluetooth BNEP ver " VERSION); 768MODULE_VERSION(VERSION); 769MODULE_LICENSE("GPL"); 770MODULE_ALIAS("bt-proto-4"); 771