net/bluetooth/bnep/core.c

   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/l2cap.h>
  36 #include <net/bluetooth/hci_core.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 %pMR -> %pMR", a1, a2);
 186
 187                         /* Iterate from a1 to a2 */
 188                         set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter);
 189                         while (memcmp(a1, a2, 6) < 0 && s->mc_filter != ~0LL) {
 190                                 /* Increment a1 */
 191                                 i = 5;
 192                                 while (i >= 0 && ++a1[i--] == 0)
 193                                         ;
 194
 195                                 set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter);
 196                         }
 197                 }
 198         }
 199
 200         BT_DBG("mc filter hash 0x%llx", s->mc_filter);
 201
 202         bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_SUCCESS);
 203 #else
 204         bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_FILTER_UNSUPPORTED_REQ);
 205 #endif
 206         return 0;
 207 }
 208
 209 static int bnep_rx_control(struct bnep_session *s, void *data, int len)
 210 {
 211         u8  cmd = *(u8 *)data;
 212         int err = 0;
 213
 214         data++;
 215         len--;
 216
 217         switch (cmd) {
 218         case BNEP_CMD_NOT_UNDERSTOOD:
 219         case BNEP_SETUP_CONN_RSP:
 220         case BNEP_FILTER_NET_TYPE_RSP:
 221         case BNEP_FILTER_MULTI_ADDR_RSP:
 222                 /* Ignore these for now */
 223                 break;
 224
 225         case BNEP_FILTER_NET_TYPE_SET:
 226                 err = bnep_ctrl_set_netfilter(s, data, len);
 227                 break;
 228
 229         case BNEP_FILTER_MULTI_ADDR_SET:
 230                 err = bnep_ctrl_set_mcfilter(s, data, len);
 231                 break;
 232
 233         case BNEP_SETUP_CONN_REQ:
 234                 /* Successful response should be sent only once */
 235                 if (test_bit(BNEP_SETUP_RESPONSE, &s->flags) &&
 236                     !test_and_set_bit(BNEP_SETUP_RSP_SENT, &s->flags))
 237                         err = bnep_send_rsp(s, BNEP_SETUP_CONN_RSP,
 238                                             BNEP_SUCCESS);
 239                 else
 240                         err = bnep_send_rsp(s, BNEP_SETUP_CONN_RSP,
 241                                             BNEP_CONN_NOT_ALLOWED);
 242                 break;
 243
 244         default: {
 245                         u8 pkt[3];
 246                         pkt[0] = BNEP_CONTROL;
 247                         pkt[1] = BNEP_CMD_NOT_UNDERSTOOD;
 248                         pkt[2] = cmd;
 249                         err = bnep_send(s, pkt, sizeof(pkt));
 250                 }
 251                 break;
 252         }
 253
 254         return err;
 255 }
 256
 257 static int bnep_rx_extension(struct bnep_session *s, struct sk_buff *skb)
 258 {
 259         struct bnep_ext_hdr *h;
 260         int err = 0;
 261
 262         do {
 263                 h = (void *) skb->data;
 264                 if (!skb_pull(skb, sizeof(*h))) {
 265                         err = -EILSEQ;
 266                         break;
 267                 }
 268
 269                 BT_DBG("type 0x%x len %d", h->type, h->len);
 270
 271                 switch (h->type & BNEP_TYPE_MASK) {
 272                 case BNEP_EXT_CONTROL:
 273                         bnep_rx_control(s, skb->data, skb->len);
 274                         break;
 275
 276                 default:
 277                         /* Unknown extension, skip it. */
 278                         break;
 279                 }
 280
 281                 if (!skb_pull(skb, h->len)) {
 282                         err = -EILSEQ;
 283                         break;
 284                 }
 285         } while (!err && (h->type & BNEP_EXT_HEADER));
 286
 287         return err;
 288 }
 289
 290 static u8 __bnep_rx_hlen[] = {
 291         ETH_HLEN,     /* BNEP_GENERAL */
 292         0,            /* BNEP_CONTROL */
 293         2,            /* BNEP_COMPRESSED */
 294         ETH_ALEN + 2, /* BNEP_COMPRESSED_SRC_ONLY */
 295         ETH_ALEN + 2  /* BNEP_COMPRESSED_DST_ONLY */
 296 };
 297
 298 static int bnep_rx_frame(struct bnep_session *s, struct sk_buff *skb)
 299 {
 300         struct net_device *dev = s->dev;
 301         struct sk_buff *nskb;
 302         u8 type, ctrl_type;
 303
 304         dev->stats.rx_bytes += skb->len;
 305
 306         type = *(u8 *) skb->data;
 307         skb_pull(skb, 1);
 308         ctrl_type = *(u8 *)skb->data;
 309
 310         if ((type & BNEP_TYPE_MASK) >= sizeof(__bnep_rx_hlen))
 311                 goto badframe;
 312
 313         if ((type & BNEP_TYPE_MASK) == BNEP_CONTROL) {
 314                 if (bnep_rx_control(s, skb->data, skb->len) < 0) {
 315                         dev->stats.tx_errors++;
 316                         kfree_skb(skb);
 317                         return 0;
 318                 }
 319
 320                 if (!(type & BNEP_EXT_HEADER)) {
 321                         kfree_skb(skb);
 322                         return 0;
 323                 }
 324
 325                 /* Verify and pull ctrl message since it's already processed */
 326                 switch (ctrl_type) {
 327                 case BNEP_SETUP_CONN_REQ:
 328                         /* Pull: ctrl type (1 b), len (1 b), data (len bytes) */
 329                         if (!skb_pull(skb, 2 + *(u8 *)(skb->data + 1) * 2))
 330                                 goto badframe;
 331                         break;
 332                 case BNEP_FILTER_MULTI_ADDR_SET:
 333                 case BNEP_FILTER_NET_TYPE_SET:
 334                         /* Pull: ctrl type (1 b), len (2 b), data (len bytes) */
 335                         if (!skb_pull(skb, 3 + *(u16 *)(skb->data + 1) * 2))
 336                                 goto badframe;
 337                         break;
 338                 default:
 339                         kfree_skb(skb);
 340                         return 0;
 341                 }
 342         } else {
 343                 skb_reset_mac_header(skb);
 344
 345                 /* Verify and pull out header */
 346                 if (!skb_pull(skb, __bnep_rx_hlen[type & BNEP_TYPE_MASK]))
 347                         goto badframe;
 348
 349                 s->eh.h_proto = get_unaligned((__be16 *) (skb->data - 2));
 350         }
 351
 352         if (type & BNEP_EXT_HEADER) {
 353                 if (bnep_rx_extension(s, skb) < 0)
 354                         goto badframe;
 355         }
 356
 357         /* Strip 802.1p header */
 358         if (ntohs(s->eh.h_proto) == ETH_P_8021Q) {
 359                 if (!skb_pull(skb, 4))
 360                         goto badframe;
 361                 s->eh.h_proto = get_unaligned((__be16 *) (skb->data - 2));
 362         }
 363
 364         /* We have to alloc new skb and copy data here :(. Because original skb
 365          * may not be modified and because of the alignment requirements. */
 366         nskb = alloc_skb(2 + ETH_HLEN + skb->len, GFP_KERNEL);
 367         if (!nskb) {
 368                 dev->stats.rx_dropped++;
 369                 kfree_skb(skb);
 370                 return -ENOMEM;
 371         }
 372         skb_reserve(nskb, 2);
 373
 374         /* Decompress header and construct ether frame */
 375         switch (type & BNEP_TYPE_MASK) {
 376         case BNEP_COMPRESSED:
 377                 memcpy(__skb_put(nskb, ETH_HLEN), &s->eh, ETH_HLEN);
 378                 break;
 379
 380         case BNEP_COMPRESSED_SRC_ONLY:
 381                 memcpy(__skb_put(nskb, ETH_ALEN), s->eh.h_dest, ETH_ALEN);
 382                 memcpy(__skb_put(nskb, ETH_ALEN), skb_mac_header(skb), ETH_ALEN);
 383                 put_unaligned(s->eh.h_proto, (__be16 *) __skb_put(nskb, 2));
 384                 break;
 385
 386         case BNEP_COMPRESSED_DST_ONLY:
 387                 memcpy(__skb_put(nskb, ETH_ALEN), skb_mac_header(skb),
 388                                                                 ETH_ALEN);
 389                 memcpy(__skb_put(nskb, ETH_ALEN + 2), s->eh.h_source,
 390                                                                 ETH_ALEN + 2);
 391                 break;
 392
 393         case BNEP_GENERAL:
 394                 memcpy(__skb_put(nskb, ETH_ALEN * 2), skb_mac_header(skb),
 395                                                                 ETH_ALEN * 2);
 396                 put_unaligned(s->eh.h_proto, (__be16 *) __skb_put(nskb, 2));
 397                 break;
 398         }
 399
 400         skb_copy_from_linear_data(skb, __skb_put(nskb, skb->len), skb->len);
 401         kfree_skb(skb);
 402
 403         dev->stats.rx_packets++;
 404         nskb->ip_summed = CHECKSUM_NONE;
 405         nskb->protocol  = eth_type_trans(nskb, dev);
 406         netif_rx_ni(nskb);
 407         return 0;
 408
 409 badframe:
 410         dev->stats.rx_errors++;
 411         kfree_skb(skb);
 412         return 0;
 413 }
 414
 415 static u8 __bnep_tx_types[] = {
 416         BNEP_GENERAL,
 417         BNEP_COMPRESSED_SRC_ONLY,
 418         BNEP_COMPRESSED_DST_ONLY,
 419         BNEP_COMPRESSED
 420 };
 421
 422 static int bnep_tx_frame(struct bnep_session *s, struct sk_buff *skb)
 423 {
 424         struct ethhdr *eh = (void *) skb->data;
 425         struct socket *sock = s->sock;
 426         struct kvec iv[3];
 427         int len = 0, il = 0;
 428         u8 type = 0;
 429
 430         BT_DBG("skb %p dev %p type %d", skb, skb->dev, skb->pkt_type);
 431
 432         if (!skb->dev) {
 433                 /* Control frame sent by us */
 434                 goto send;
 435         }
 436
 437         iv[il++] = (struct kvec) { &type, 1 };
 438         len++;
 439
 440         if (compress_src && ether_addr_equal(eh->h_dest, s->eh.h_source))
 441                 type |= 0x01;
 442
 443         if (compress_dst && ether_addr_equal(eh->h_source, s->eh.h_dest))
 444                 type |= 0x02;
 445
 446         if (type)
 447                 skb_pull(skb, ETH_ALEN * 2);
 448
 449         type = __bnep_tx_types[type];
 450         switch (type) {
 451         case BNEP_COMPRESSED_SRC_ONLY:
 452                 iv[il++] = (struct kvec) { eh->h_source, ETH_ALEN };
 453                 len += ETH_ALEN;
 454                 break;
 455
 456         case BNEP_COMPRESSED_DST_ONLY:
 457                 iv[il++] = (struct kvec) { eh->h_dest, ETH_ALEN };
 458                 len += ETH_ALEN;
 459                 break;
 460         }
 461
 462 send:
 463         iv[il++] = (struct kvec) { skb->data, skb->len };
 464         len += skb->len;
 465
 466         /* FIXME: linearize skb */
 467         {
 468                 len = kernel_sendmsg(sock, &s->msg, iv, il, len);
 469         }
 470         kfree_skb(skb);
 471
 472         if (len > 0) {
 473                 s->dev->stats.tx_bytes += len;
 474                 s->dev->stats.tx_packets++;
 475                 return 0;
 476         }
 477
 478         return len;
 479 }
 480
 481 static int bnep_session(void *arg)
 482 {
 483         struct bnep_session *s = arg;
 484         struct net_device *dev = s->dev;
 485         struct sock *sk = s->sock->sk;
 486         struct sk_buff *skb;
 487         DEFINE_WAIT_FUNC(wait, woken_wake_function);
 488
 489         BT_DBG("");
 490
 491         set_user_nice(current, -15);
 492
 493         add_wait_queue(sk_sleep(sk), &wait);
 494         while (1) {
 495                 /* Ensure session->terminate is updated */
 496                 smp_mb__before_atomic();
 497
 498                 if (atomic_read(&s->terminate))
 499                         break;
 500                 /* RX */
 501                 while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
 502                         skb_orphan(skb);
 503                         if (!skb_linearize(skb))
 504                                 bnep_rx_frame(s, skb);
 505                         else
 506                                 kfree_skb(skb);
 507                 }
 508
 509                 if (sk->sk_state != BT_CONNECTED)
 510                         break;
 511
 512                 /* TX */
 513                 while ((skb = skb_dequeue(&sk->sk_write_queue)))
 514                         if (bnep_tx_frame(s, skb))
 515                                 break;
 516                 netif_wake_queue(dev);
 517
 518                 wait_woken(&wait, TASK_INTERRUPTIBLE, MAX_SCHEDULE_TIMEOUT);
 519         }
 520         remove_wait_queue(sk_sleep(sk), &wait);
 521
 522         /* Cleanup session */
 523         down_write(&bnep_session_sem);
 524
 525         /* Delete network device */
 526         unregister_netdev(dev);
 527
 528         /* Wakeup user-space polling for socket errors */
 529         s->sock->sk->sk_err = EUNATCH;
 530
 531         wake_up_interruptible(sk_sleep(s->sock->sk));
 532
 533         /* Release the socket */
 534         fput(s->sock->file);
 535
 536         __bnep_unlink_session(s);
 537
 538         up_write(&bnep_session_sem);
 539         free_netdev(dev);
 540         module_put_and_exit(0);
 541         return 0;
 542 }
 543
 544 static struct device *bnep_get_device(struct bnep_session *session)
 545 {
 546         struct l2cap_conn *conn = l2cap_pi(session->sock->sk)->chan->conn;
 547
 548         if (!conn || !conn->hcon)
 549                 return NULL;
 550
 551         return &conn->hcon->dev;
 552 }
 553
 554 static struct device_type bnep_type = {
 555         .name   = "bluetooth",
 556 };
 557
 558 int bnep_add_connection(struct bnep_connadd_req *req, struct socket *sock)
 559 {
 560         u32 valid_flags = BIT(BNEP_SETUP_RESPONSE);
 561         struct net_device *dev;
 562         struct bnep_session *s, *ss;
 563         u8 dst[ETH_ALEN], src[ETH_ALEN];
 564         int err;
 565
 566         BT_DBG("");
 567
 568         if (!l2cap_is_socket(sock))
 569                 return -EBADFD;
 570
 571         if (req->flags & ~valid_flags)
 572                 return -EINVAL;
 573
 574         baswap((void *) dst, &l2cap_pi(sock->sk)->chan->dst);
 575         baswap((void *) src, &l2cap_pi(sock->sk)->chan->src);
 576
 577         /* session struct allocated as private part of net_device */
 578         dev = alloc_netdev(sizeof(struct bnep_session),
 579                            (*req->device) ? req->device : "bnep%d",
 580                            NET_NAME_UNKNOWN,
 581                            bnep_net_setup);
 582         if (!dev)
 583                 return -ENOMEM;
 584
 585         down_write(&bnep_session_sem);
 586
 587         ss = __bnep_get_session(dst);
 588         if (ss && ss->state == BT_CONNECTED) {
 589                 err = -EEXIST;
 590                 goto failed;
 591         }
 592
 593         s = netdev_priv(dev);
 594
 595         /* This is rx header therefore addresses are swapped.
 596          * ie. eh.h_dest is our local address. */
 597         memcpy(s->eh.h_dest,   &src, ETH_ALEN);
 598         memcpy(s->eh.h_source, &dst, ETH_ALEN);
 599         memcpy(dev->dev_addr, s->eh.h_dest, ETH_ALEN);
 600
 601         s->dev   = dev;
 602         s->sock  = sock;
 603         s->role  = req->role;
 604         s->state = BT_CONNECTED;
 605         s->flags = req->flags;
 606
 607         s->msg.msg_flags = MSG_NOSIGNAL;
 608
 609 #ifdef CONFIG_BT_BNEP_MC_FILTER
 610         /* Set default mc filter */
 611         set_bit(bnep_mc_hash(dev->broadcast), (ulong *) &s->mc_filter);
 612 #endif
 613
 614 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
 615         /* Set default protocol filter */
 616         bnep_set_default_proto_filter(s);
 617 #endif
 618
 619         SET_NETDEV_DEV(dev, bnep_get_device(s));
 620         SET_NETDEV_DEVTYPE(dev, &bnep_type);
 621
 622         err = register_netdev(dev);
 623         if (err)
 624                 goto failed;
 625
 626         __bnep_link_session(s);
 627
 628         __module_get(THIS_MODULE);
 629         s->task = kthread_run(bnep_session, s, "kbnepd %s", dev->name);
 630         if (IS_ERR(s->task)) {
 631                 /* Session thread start failed, gotta cleanup. */
 632                 module_put(THIS_MODULE);
 633                 unregister_netdev(dev);
 634                 __bnep_unlink_session(s);
 635                 err = PTR_ERR(s->task);
 636                 goto failed;
 637         }
 638
 639         up_write(&bnep_session_sem);
 640         strcpy(req->device, dev->name);
 641         return 0;
 642
 643 failed:
 644         up_write(&bnep_session_sem);
 645         free_netdev(dev);
 646         return err;
 647 }
 648
 649 int bnep_del_connection(struct bnep_conndel_req *req)
 650 {
 651         u32 valid_flags = 0;
 652         struct bnep_session *s;
 653         int  err = 0;
 654
 655         BT_DBG("");
 656
 657         if (req->flags & ~valid_flags)
 658                 return -EINVAL;
 659
 660         down_read(&bnep_session_sem);
 661
 662         s = __bnep_get_session(req->dst);
 663         if (s) {
 664                 atomic_inc(&s->terminate);
 665                 wake_up_interruptible(sk_sleep(s->sock->sk));
 666         } else
 667                 err = -ENOENT;
 668
 669         up_read(&bnep_session_sem);
 670         return err;
 671 }
 672
 673 static void __bnep_copy_ci(struct bnep_conninfo *ci, struct bnep_session *s)
 674 {
 675         u32 valid_flags = BIT(BNEP_SETUP_RESPONSE);
 676
 677         memset(ci, 0, sizeof(*ci));
 678         memcpy(ci->dst, s->eh.h_source, ETH_ALEN);
 679         strcpy(ci->device, s->dev->name);
 680         ci->flags = s->flags & valid_flags;
 681         ci->state = s->state;
 682         ci->role  = s->role;
 683 }
 684
 685 int bnep_get_connlist(struct bnep_connlist_req *req)
 686 {
 687         struct bnep_session *s;
 688         int err = 0, n = 0;
 689
 690         down_read(&bnep_session_sem);
 691
 692         list_for_each_entry(s, &bnep_session_list, list) {
 693                 struct bnep_conninfo ci;
 694
 695                 __bnep_copy_ci(&ci, s);
 696
 697                 if (copy_to_user(req->ci, &ci, sizeof(ci))) {
 698                         err = -EFAULT;
 699                         break;
 700                 }
 701
 702                 if (++n >= req->cnum)
 703                         break;
 704
 705                 req->ci++;
 706         }
 707         req->cnum = n;
 708
 709         up_read(&bnep_session_sem);
 710         return err;
 711 }
 712
 713 int bnep_get_conninfo(struct bnep_conninfo *ci)
 714 {
 715         struct bnep_session *s;
 716         int err = 0;
 717
 718         down_read(&bnep_session_sem);
 719
 720         s = __bnep_get_session(ci->dst);
 721         if (s)
 722                 __bnep_copy_ci(ci, s);
 723         else
 724                 err = -ENOENT;
 725
 726         up_read(&bnep_session_sem);
 727         return err;
 728 }
 729
 730 static int __init bnep_init(void)
 731 {
 732         char flt[50] = "";
 733
 734 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
 735         strcat(flt, "protocol ");
 736 #endif
 737
 738 #ifdef CONFIG_BT_BNEP_MC_FILTER
 739         strcat(flt, "multicast");
 740 #endif
 741
 742         BT_INFO("BNEP (Ethernet Emulation) ver %s", VERSION);
 743         if (flt[0])
 744                 BT_INFO("BNEP filters: %s", flt);
 745
 746         bnep_sock_init();
 747         return 0;
 748 }
 749
 750 static void __exit bnep_exit(void)
 751 {
 752         bnep_sock_cleanup();
 753 }
 754
 755 module_init(bnep_init);
 756 module_exit(bnep_exit);
 757
 758 module_param(compress_src, bool, 0644);
 759 MODULE_PARM_DESC(compress_src, "Compress sources headers");
 760
 761 module_param(compress_dst, bool, 0644);
 762 MODULE_PARM_DESC(compress_dst, "Compress destination headers");
 763
 764 MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
 765 MODULE_DESCRIPTION("Bluetooth BNEP ver " VERSION);
 766 MODULE_VERSION(VERSION);
 767 MODULE_LICENSE("GPL");
 768 MODULE_ALIAS("bt-proto-4");