net/bluetooth/l2cap.c

   1 /*
   2    BlueZ - Bluetooth protocol stack for Linux
   3    Copyright (C) 2000-2001 Qualcomm Incorporated
   4
   5    Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.com>
   6
   7    This program is free software; you can redistribute it and/or modify
   8    it under the terms of the GNU General Public License version 2 as
   9    published by the Free Software Foundation;
  10
  11    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
  12    OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  13    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
  14    IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY
  15    CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES
  16    WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
  17    ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
  18    OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  19
  20    ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS,
  21    COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS
  22    SOFTWARE IS DISCLAIMED.
  23 */
  24
  25 /*
  26  * BlueZ L2CAP core and sockets.
  27  *
  28  * $Id: l2cap.c,v 1.15 2002/09/09 01:14:52 maxk Exp $
  29  */
  30 #define VERSION "2.3"
  31
  32 #include <linux/config.h>
  33 #include <linux/module.h>
  34
  35 #include <linux/types.h>
  36 #include <linux/errno.h>
  37 #include <linux/kernel.h>
  38 #include <linux/major.h>
  39 #include <linux/sched.h>
  40 #include <linux/slab.h>
  41 #include <linux/poll.h>
  42 #include <linux/fcntl.h>
  43 #include <linux/init.h>
  44 #include <linux/skbuff.h>
  45 #include <linux/interrupt.h>
  46 #include <linux/socket.h>
  47 #include <linux/skbuff.h>
  48 #include <linux/proc_fs.h>
  49 #include <linux/list.h>
  50 #include <net/sock.h>
  51
  52 #include <asm/system.h>
  53 #include <asm/uaccess.h>
  54 #include <asm/unaligned.h>
  55
  56 #include <net/bluetooth/bluetooth.h>
  57 #include <net/bluetooth/hci_core.h>
  58 #include <net/bluetooth/l2cap.h>
  59
  60 #ifndef L2CAP_DEBUG
  61 #undef  BT_DBG
  62 #define BT_DBG( A... )
  63 #endif
  64
  65 static struct proto_ops l2cap_sock_ops;
  66
  67 struct bluez_sock_list l2cap_sk_list = {
  68         lock: RW_LOCK_UNLOCKED
  69 };
  70
  71 static int l2cap_conn_del(struct hci_conn *conn, int err);
  72
  73 static void __l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct sock *parent);
  74 static void l2cap_chan_del(struct sock *sk, int err);
  75 static int  l2cap_chan_send(struct sock *sk, struct msghdr *msg, int len);
  76
  77 static void __l2cap_sock_close(struct sock *sk, int reason);
  78 static void l2cap_sock_close(struct sock *sk);
  79 static void l2cap_sock_kill(struct sock *sk);
  80
  81 static int l2cap_send_req(struct l2cap_conn *conn, __u8 code, __u16 len, void *data);
  82 static int l2cap_send_rsp(struct l2cap_conn *conn, __u8 ident, __u8 code, __u16 len, void *data);
  83
  84 /* ----- L2CAP timers ------ */
  85 static void l2cap_sock_timeout(unsigned long arg)
  86 {
  87         struct sock *sk = (struct sock *) arg;
  88
  89         BT_DBG("sock %p state %d", sk, sk->state);
  90
  91         bh_lock_sock(sk);
  92         __l2cap_sock_close(sk, ETIMEDOUT);
  93         bh_unlock_sock(sk);
  94
  95         l2cap_sock_kill(sk);
  96         sock_put(sk);
  97 }
  98
  99 static void l2cap_sock_set_timer(struct sock *sk, long timeout)
 100 {
 101         BT_DBG("sk %p state %d timeout %ld", sk, sk->state, timeout);
 102
 103         if (!mod_timer(&sk->timer, jiffies + timeout))
 104                 sock_hold(sk);
 105 }
 106
 107 static void l2cap_sock_clear_timer(struct sock *sk)
 108 {
 109         BT_DBG("sock %p state %d", sk, sk->state);
 110
 111         if (timer_pending(&sk->timer) && del_timer(&sk->timer))
 112                 __sock_put(sk);
 113 }
 114
 115 static void l2cap_sock_init_timer(struct sock *sk)
 116 {
 117         init_timer(&sk->timer);
 118         sk->timer.function = l2cap_sock_timeout;
 119         sk->timer.data = (unsigned long)sk;
 120 }
 121
 122 /* -------- L2CAP connections --------- */
 123 static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, __u8 status)
 124 {
 125         struct l2cap_conn *conn;
 126
 127         if ((conn = hcon->l2cap_data))
 128                 return conn;
 129
 130         if (status)
 131                 return conn;
 132
 133         if (!(conn = kmalloc(sizeof(struct l2cap_conn), GFP_ATOMIC)))
 134                 return NULL;
 135         memset(conn, 0, sizeof(struct l2cap_conn));
 136
 137         hcon->l2cap_data = conn;
 138         conn->hcon = hcon;
 139
 140         conn->mtu = hcon->hdev->acl_mtu;
 141         conn->src = &hcon->hdev->bdaddr;
 142         conn->dst = &hcon->dst;
 143
 144         spin_lock_init(&conn->lock);
 145         conn->chan_list.lock = RW_LOCK_UNLOCKED;
 146
 147         BT_DBG("hcon %p conn %p", hcon, conn);
 148
 149         MOD_INC_USE_COUNT;
 150         return conn;
 151 }
 152
 153 static int l2cap_conn_del(struct hci_conn *hcon, int err)
 154 {
 155         struct l2cap_conn *conn;
 156         struct sock *sk;
 157
 158         if (!(conn = hcon->l2cap_data))
 159                 return 0;
 160
 161         BT_DBG("hcon %p conn %p, err %d", hcon, conn, err);
 162
 163         if (conn->rx_skb)
 164                 kfree_skb(conn->rx_skb);
 165
 166         /* Kill channels */
 167         while ((sk = conn->chan_list.head)) {
 168                 bh_lock_sock(sk);
 169                 l2cap_chan_del(sk, err);
 170                 bh_unlock_sock(sk);
 171                 l2cap_sock_kill(sk);
 172         }
 173
 174         hcon->l2cap_data = NULL;
 175         kfree(conn);
 176
 177         MOD_DEC_USE_COUNT;
 178         return 0;
 179 }
 180
 181 /* -------- Socket interface ---------- */
 182 static struct sock *__l2cap_get_sock_by_addr(__u16 psm, bdaddr_t *src)
 183 {
 184         struct sock *sk;
 185         for (sk = l2cap_sk_list.head; sk; sk = sk->next) {
 186                 if (sk->sport == psm && !bacmp(&bluez_pi(sk)->src, src))
 187                         break;
 188         }
 189         return sk;
 190 }
 191
 192 /* Find socket with psm and source bdaddr.
 193  * Returns closest match.
 194  */
 195 static struct sock *__l2cap_get_sock_by_psm(int state, __u16 psm, bdaddr_t *src)
 196 {
 197         struct sock *sk, *sk1 = NULL;
 198
 199         for (sk = l2cap_sk_list.head; sk; sk = sk->next) {
 200                 if (state && sk->state != state)
 201                         continue;
 202
 203                 if (l2cap_pi(sk)->psm == psm) {
 204                         /* Exact match. */
 205                         if (!bacmp(&bluez_pi(sk)->src, src))
 206                                 break;
 207
 208                         /* Closest match */
 209                         if (!bacmp(&bluez_pi(sk)->src, BDADDR_ANY))
 210                                 sk1 = sk;
 211                 }
 212         }
 213         return sk ? sk : sk1;
 214 }
 215
 216 /* Find socket with given address (psm, src).
 217  * Returns locked socket */
 218 static inline struct sock *l2cap_get_sock_by_psm(int state, __u16 psm, bdaddr_t *src)
 219 {
 220         struct sock *s;
 221         read_lock(&l2cap_sk_list.lock);
 222         s = __l2cap_get_sock_by_psm(state, psm, src);
 223         if (s) bh_lock_sock(s);
 224         read_unlock(&l2cap_sk_list.lock);
 225         return s;
 226 }
 227
 228 static void l2cap_sock_destruct(struct sock *sk)
 229 {
 230         BT_DBG("sk %p", sk);
 231
 232         skb_queue_purge(&sk->receive_queue);
 233         skb_queue_purge(&sk->write_queue);
 234
 235         MOD_DEC_USE_COUNT;
 236 }
 237
 238 static void l2cap_sock_cleanup_listen(struct sock *parent)
 239 {
 240         struct sock *sk;
 241
 242         BT_DBG("parent %p", parent);
 243
 244         /* Close not yet accepted channels */
 245         while ((sk = bluez_accept_dequeue(parent, NULL)))
 246                 l2cap_sock_close(sk);
 247
 248         parent->state  = BT_CLOSED;
 249         parent->zapped = 1;
 250 }
 251
 252 /* Kill socket (only if zapped and orphan)
 253  * Must be called on unlocked socket.
 254  */
 255 static void l2cap_sock_kill(struct sock *sk)
 256 {
 257         if (!sk->zapped || sk->socket)
 258                 return;
 259
 260         BT_DBG("sk %p state %d", sk, sk->state);
 261
 262         /* Kill poor orphan */
 263         bluez_sock_unlink(&l2cap_sk_list, sk);
 264         sk->dead = 1;
 265         sock_put(sk);
 266 }
 267
 268 /* Close socket.
 269  */
 270 static void __l2cap_sock_close(struct sock *sk, int reason)
 271 {
 272         BT_DBG("sk %p state %d socket %p", sk, sk->state, sk->socket);
 273
 274         switch (sk->state) {
 275         case BT_LISTEN:
 276                 l2cap_sock_cleanup_listen(sk);
 277                 break;
 278
 279         case BT_CONNECTED:
 280         case BT_CONFIG:
 281         case BT_CONNECT2:
 282                 if (sk->type == SOCK_SEQPACKET) {
 283                         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
 284                         l2cap_disconn_req req;
 285
 286                         sk->state = BT_DISCONN;
 287                         l2cap_sock_set_timer(sk, sk->sndtimeo);
 288
 289                         req.dcid = __cpu_to_le16(l2cap_pi(sk)->dcid);
 290                         req.scid = __cpu_to_le16(l2cap_pi(sk)->scid);
 291                         l2cap_send_req(conn, L2CAP_DISCONN_REQ, L2CAP_DISCONN_REQ_SIZE, &req);
 292                 } else {
 293                         l2cap_chan_del(sk, reason);
 294                 }
 295                 break;
 296
 297         case BT_CONNECT:
 298         case BT_DISCONN:
 299                 l2cap_chan_del(sk, reason);
 300                 break;
 301
 302         default:
 303                 sk->zapped = 1;
 304                 break;
 305         };
 306 }
 307
 308 /* Must be called on unlocked socket. */
 309 static void l2cap_sock_close(struct sock *sk)
 310 {
 311         l2cap_sock_clear_timer(sk);
 312         lock_sock(sk);
 313         __l2cap_sock_close(sk, ECONNRESET);
 314         release_sock(sk);
 315         l2cap_sock_kill(sk);
 316 }
 317
 318 static void l2cap_sock_init(struct sock *sk, struct sock *parent)
 319 {
 320         struct l2cap_pinfo *pi = l2cap_pi(sk);
 321
 322         BT_DBG("sk %p", sk);
 323
 324         if (parent) {
 325                 sk->type = parent->type;
 326                 pi->imtu = l2cap_pi(parent)->imtu;
 327                 pi->omtu = l2cap_pi(parent)->omtu;
 328                 pi->link_mode = l2cap_pi(parent)->link_mode;
 329         } else {
 330                 pi->imtu = L2CAP_DEFAULT_MTU;
 331                 pi->omtu = 0;
 332                 pi->link_mode = 0;
 333         }
 334
 335         /* Default config options */
 336         pi->conf_mtu = L2CAP_DEFAULT_MTU;
 337         pi->flush_to = L2CAP_DEFAULT_FLUSH_TO;
 338 }
 339
 340 static struct sock *l2cap_sock_alloc(struct socket *sock, int proto, int prio)
 341 {
 342         struct sock *sk;
 343
 344         if (!(sk = sk_alloc(PF_BLUETOOTH, prio, 1)))
 345                 return NULL;
 346
 347         bluez_sock_init(sock, sk);
 348
 349         sk->zapped   = 0;
 350
 351         sk->destruct = l2cap_sock_destruct;
 352         sk->sndtimeo = L2CAP_CONN_TIMEOUT;
 353
 354         sk->protocol = proto;
 355         sk->state    = BT_OPEN;
 356
 357         l2cap_sock_init_timer(sk);
 358
 359         bluez_sock_link(&l2cap_sk_list, sk);
 360
 361         MOD_INC_USE_COUNT;
 362         return sk;
 363 }
 364
 365 static int l2cap_sock_create(struct socket *sock, int protocol)
 366 {
 367         struct sock *sk;
 368
 369         BT_DBG("sock %p", sock);
 370
 371         sock->state = SS_UNCONNECTED;
 372
 373         if (sock->type != SOCK_SEQPACKET && sock->type != SOCK_DGRAM && sock->type != SOCK_RAW)
 374                 return -ESOCKTNOSUPPORT;
 375
 376         if (sock->type == SOCK_RAW && !capable(CAP_NET_RAW))
 377                 return -EPERM;
 378
 379         sock->ops = &l2cap_sock_ops;
 380
 381         if (!(sk = l2cap_sock_alloc(sock, protocol, GFP_KERNEL)))
 382                 return -ENOMEM;
 383
 384         l2cap_sock_init(sk, NULL);
 385         return 0;
 386 }
 387
 388 static int l2cap_sock_bind(struct socket *sock, struct sockaddr *addr, int addr_len)
 389 {
 390         struct sockaddr_l2 *la = (struct sockaddr_l2 *) addr;
 391         struct sock *sk = sock->sk;
 392         int err = 0;
 393
 394         BT_DBG("sk %p, %s %d", sk, batostr(&la->l2_bdaddr), la->l2_psm);
 395
 396         if (!addr || addr->sa_family != AF_BLUETOOTH)
 397                 return -EINVAL;
 398
 399         lock_sock(sk);
 400
 401         if (sk->state != BT_OPEN) {
 402                 err = -EBADFD;
 403                 goto done;
 404         }
 405
 406         write_lock_bh(&l2cap_sk_list.lock);
 407         if (la->l2_psm && __l2cap_get_sock_by_addr(la->l2_psm, &la->l2_bdaddr)) {
 408                 err = -EADDRINUSE;
 409         } else {
 410                 /* Save source address */
 411                 bacpy(&bluez_pi(sk)->src, &la->l2_bdaddr);
 412                 l2cap_pi(sk)->psm = la->l2_psm;
 413                 sk->sport = la->l2_psm;
 414                 sk->state = BT_BOUND;
 415         }
 416         write_unlock_bh(&l2cap_sk_list.lock);
 417
 418 done:
 419         release_sock(sk);
 420         return err;
 421 }
 422
 423 static inline void l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct sock *parent)
 424 {
 425         struct l2cap_chan_list *l = &conn->chan_list;
 426         write_lock(&l->lock);
 427         __l2cap_chan_add(conn, sk, parent);
 428         write_unlock(&l->lock);
 429 }
 430
 431 static int l2cap_do_connect(struct sock *sk)
 432 {
 433         bdaddr_t *src = &bluez_pi(sk)->src;
 434         bdaddr_t *dst = &bluez_pi(sk)->dst;
 435         struct l2cap_conn *conn;
 436         struct hci_conn   *hcon;
 437         struct hci_dev    *hdev;
 438         int err = 0;
 439
 440         BT_DBG("%s -> %s psm 0x%2.2x", batostr(src), batostr(dst), l2cap_pi(sk)->psm);
 441
 442         if (!(hdev = hci_get_route(dst, src)))
 443                 return -EHOSTUNREACH;
 444
 445         hci_dev_lock_bh(hdev);
 446
 447         err = -ENOMEM;
 448
 449         hcon = hci_connect(hdev, ACL_LINK, dst);
 450         if (!hcon)
 451                 goto done;
 452
 453         conn = l2cap_conn_add(hcon, 0);
 454         if (!conn) {
 455                 hci_conn_put(hcon);
 456                 goto done;
 457         }
 458
 459         err = 0;
 460
 461         /* Update source addr of the socket */
 462         bacpy(src, conn->src);
 463
 464         l2cap_chan_add(conn, sk, NULL);
 465
 466         sk->state = BT_CONNECT;
 467         l2cap_sock_set_timer(sk, sk->sndtimeo);
 468
 469         if (hcon->state == BT_CONNECTED) {
 470                 if (sk->type == SOCK_SEQPACKET) {
 471                         l2cap_conn_req req;
 472                         req.scid = __cpu_to_le16(l2cap_pi(sk)->scid);
 473                         req.psm  = l2cap_pi(sk)->psm;
 474                         l2cap_send_req(conn, L2CAP_CONN_REQ, L2CAP_CONN_REQ_SIZE, &req);
 475                 } else {
 476                         l2cap_sock_clear_timer(sk);
 477                         sk->state = BT_CONNECTED;
 478                 }
 479         }
 480
 481 done:
 482         hci_dev_unlock_bh(hdev);
 483         hci_dev_put(hdev);
 484         return err;
 485 }
 486
 487 static int l2cap_sock_connect(struct socket *sock, struct sockaddr *addr, int alen, int flags)
 488 {
 489         struct sockaddr_l2 *la = (struct sockaddr_l2 *) addr;
 490         struct sock *sk = sock->sk;
 491         int err = 0;
 492
 493         lock_sock(sk);
 494
 495         BT_DBG("sk %p", sk);
 496
 497         if (addr->sa_family != AF_BLUETOOTH || alen < sizeof(struct sockaddr_l2)) {
 498                 err = -EINVAL;
 499                 goto done;
 500         }
 501
 502         if (sk->type == SOCK_SEQPACKET && !la->l2_psm) {
 503                 err = -EINVAL;
 504                 goto done;
 505         }
 506
 507         switch(sk->state) {
 508         case BT_CONNECT:
 509         case BT_CONNECT2:
 510         case BT_CONFIG:
 511                 /* Already connecting */
 512                 goto wait;
 513
 514         case BT_CONNECTED:
 515                 /* Already connected */
 516                 goto done;
 517
 518         case BT_OPEN:
 519         case BT_BOUND:
 520                 /* Can connect */
 521                 break;
 522
 523         default:
 524                 err = -EBADFD;
 525                 goto done;
 526         }
 527
 528         /* Set destination address and psm */
 529         bacpy(&bluez_pi(sk)->dst, &la->l2_bdaddr);
 530         l2cap_pi(sk)->psm = la->l2_psm;
 531
 532         if ((err = l2cap_do_connect(sk)))
 533                 goto done;
 534
 535 wait:
 536         err = bluez_sock_wait_state(sk, BT_CONNECTED,
 537                         sock_sndtimeo(sk, flags & O_NONBLOCK));
 538
 539 done:
 540         release_sock(sk);
 541         return err;
 542 }
 543
 544 int l2cap_sock_listen(struct socket *sock, int backlog)
 545 {
 546         struct sock *sk = sock->sk;
 547         int err = 0;
 548
 549         BT_DBG("sk %p backlog %d", sk, backlog);
 550
 551         lock_sock(sk);
 552
 553         if (sk->state != BT_BOUND || sock->type != SOCK_SEQPACKET) {
 554                 err = -EBADFD;
 555                 goto done;
 556         }
 557
 558         if (!l2cap_pi(sk)->psm) {
 559                 err = -EINVAL;
 560                 goto done;
 561         }
 562
 563         sk->max_ack_backlog = backlog;
 564         sk->ack_backlog = 0;
 565         sk->state = BT_LISTEN;
 566
 567 done:
 568         release_sock(sk);
 569         return err;
 570 }
 571
 572 int l2cap_sock_accept(struct socket *sock, struct socket *newsock, int flags)
 573 {
 574         DECLARE_WAITQUEUE(wait, current);
 575         struct sock *sk = sock->sk, *nsk;
 576         long timeo;
 577         int err = 0;
 578
 579         lock_sock(sk);
 580
 581         if (sk->state != BT_LISTEN) {
 582                 err = -EBADFD;
 583                 goto done;
 584         }
 585
 586         timeo = sock_rcvtimeo(sk, flags & O_NONBLOCK);
 587
 588         BT_DBG("sk %p timeo %ld", sk, timeo);
 589
 590         /* Wait for an incoming connection. (wake-one). */
 591         add_wait_queue_exclusive(sk->sleep, &wait);
 592         while (!(nsk = bluez_accept_dequeue(sk, newsock))) {
 593                 set_current_state(TASK_INTERRUPTIBLE);
 594                 if (!timeo) {
 595                         err = -EAGAIN;
 596                         break;
 597                 }
 598
 599                 release_sock(sk);
 600                 timeo = schedule_timeout(timeo);
 601                 lock_sock(sk);
 602
 603                 if (sk->state != BT_LISTEN) {
 604                         err = -EBADFD;
 605                         break;
 606                 }
 607
 608                 if (signal_pending(current)) {
 609                         err = sock_intr_errno(timeo);
 610                         break;
 611                 }
 612         }
 613         set_current_state(TASK_RUNNING);
 614         remove_wait_queue(sk->sleep, &wait);
 615
 616         if (err)
 617                 goto done;
 618
 619         newsock->state = SS_CONNECTED;
 620
 621         BT_DBG("new socket %p", nsk);
 622
 623 done:
 624         release_sock(sk);
 625         return err;
 626 }
 627
 628 static int l2cap_sock_getname(struct socket *sock, struct sockaddr *addr, int *len, int peer)
 629 {
 630         struct sockaddr_l2 *la = (struct sockaddr_l2 *) addr;
 631         struct sock *sk = sock->sk;
 632
 633         BT_DBG("sock %p, sk %p", sock, sk);
 634
 635         addr->sa_family = AF_BLUETOOTH;
 636         *len = sizeof(struct sockaddr_l2);
 637
 638         if (peer)
 639                 bacpy(&la->l2_bdaddr, &bluez_pi(sk)->dst);
 640         else
 641                 bacpy(&la->l2_bdaddr, &bluez_pi(sk)->src);
 642
 643         la->l2_psm = l2cap_pi(sk)->psm;
 644         return 0;
 645 }
 646
 647 static int l2cap_sock_sendmsg(struct socket *sock, struct msghdr *msg, int len, struct scm_cookie *scm)
 648 {
 649         struct sock *sk = sock->sk;
 650         int err = 0;
 651
 652         BT_DBG("sock %p, sk %p", sock, sk);
 653
 654         if (sk->err)
 655                 return sock_error(sk);
 656
 657         if (msg->msg_flags & MSG_OOB)
 658                 return -EOPNOTSUPP;
 659
 660         /* Check outgoing MTU */
 661         if (len > l2cap_pi(sk)->omtu)
 662                 return -EINVAL;
 663
 664         lock_sock(sk);
 665
 666         if (sk->state == BT_CONNECTED)
 667                 err = l2cap_chan_send(sk, msg, len);
 668         else
 669                 err = -ENOTCONN;
 670
 671         release_sock(sk);
 672         return err;
 673 }
 674
 675 static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname, char *optval, int optlen)
 676 {
 677         struct sock *sk = sock->sk;
 678         struct l2cap_options opts;
 679         int err = 0, len;
 680         __u32 opt;
 681
 682         BT_DBG("sk %p", sk);
 683
 684         lock_sock(sk);
 685
 686         switch (optname) {
 687         case L2CAP_OPTIONS:
 688                 opts.imtu     = l2cap_pi(sk)->imtu;
 689                 opts.omtu     = l2cap_pi(sk)->omtu;
 690                 opts.flush_to = l2cap_pi(sk)->flush_to;
 691
 692                 len = MIN(sizeof(opts), optlen);
 693                 if (copy_from_user((char *)&opts, optval, len)) {
 694                         err = -EFAULT;
 695                         break;
 696                 }
 697                 l2cap_pi(sk)->imtu  = opts.imtu;
 698                 l2cap_pi(sk)->omtu  = opts.omtu;
 699                 break;
 700
 701         case L2CAP_LM:
 702                 if (get_user(opt, (__u32 *)optval)) {
 703                         err = -EFAULT;
 704                         break;
 705                 }
 706
 707                 l2cap_pi(sk)->link_mode = opt;
 708                 break;
 709
 710         default:
 711                 err = -ENOPROTOOPT;
 712                 break;
 713         }
 714
 715         release_sock(sk);
 716         return err;
 717 }
 718
 719 static int l2cap_sock_getsockopt(struct socket *sock, int level, int optname, char *optval, int *optlen)
 720 {
 721         struct sock *sk = sock->sk;
 722         struct l2cap_options opts;
 723         struct l2cap_conninfo cinfo;
 724         int len, err = 0;
 725
 726         if (get_user(len, optlen))
 727                 return -EFAULT;
 728
 729         lock_sock(sk);
 730
 731         switch (optname) {
 732         case L2CAP_OPTIONS:
 733                 opts.imtu     = l2cap_pi(sk)->imtu;
 734                 opts.omtu     = l2cap_pi(sk)->omtu;
 735                 opts.flush_to = l2cap_pi(sk)->flush_to;
 736
 737                 len = MIN(len, sizeof(opts));
 738                 if (copy_to_user(optval, (char *)&opts, len))
 739                         err = -EFAULT;
 740
 741                 break;
 742
 743         case L2CAP_LM:
 744                 if (put_user(l2cap_pi(sk)->link_mode, (__u32 *)optval))
 745                         err = -EFAULT;
 746                 break;
 747
 748         case L2CAP_CONNINFO:
 749                 if (sk->state != BT_CONNECTED) {
 750                         err = -ENOTCONN;
 751                         break;
 752                 }
 753
 754                 cinfo.hci_handle = l2cap_pi(sk)->conn->hcon->handle;
 755
 756                 len = MIN(len, sizeof(cinfo));
 757                 if (copy_to_user(optval, (char *)&cinfo, len))
 758                         err = -EFAULT;
 759
 760                 break;
 761
 762         default:
 763                 err = -ENOPROTOOPT;
 764                 break;
 765         }
 766
 767         release_sock(sk);
 768         return err;
 769 }
 770
 771 static int l2cap_sock_shutdown(struct socket *sock, int how)
 772 {
 773         struct sock *sk = sock->sk;
 774         int err = 0;
 775
 776         BT_DBG("sock %p, sk %p", sock, sk);
 777
 778         if (!sk) return 0;
 779
 780         lock_sock(sk);
 781         if (!sk->shutdown) {
 782                 sk->shutdown = SHUTDOWN_MASK;
 783                 l2cap_sock_clear_timer(sk);
 784                 __l2cap_sock_close(sk, 0);
 785
 786                 if (sk->linger)
 787                         err = bluez_sock_wait_state(sk, BT_CLOSED, sk->lingertime);
 788         }
 789         release_sock(sk);
 790         return err;
 791 }
 792
 793 static int l2cap_sock_release(struct socket *sock)
 794 {
 795         struct sock *sk = sock->sk;
 796         int err;
 797
 798         BT_DBG("sock %p, sk %p", sock, sk);
 799
 800         if (!sk) return 0;
 801
 802         err = l2cap_sock_shutdown(sock, 2);
 803
 804         sock_orphan(sk);
 805         l2cap_sock_kill(sk);
 806         return err;
 807 }
 808
 809 /* --------- L2CAP channels --------- */
 810 static struct sock * __l2cap_get_chan_by_dcid(struct l2cap_chan_list *l, __u16 cid)
 811 {
 812         struct sock *s;
 813         for (s = l->head; s; s = l2cap_pi(s)->next_c) {
 814                 if (l2cap_pi(s)->dcid == cid)
 815                         break;
 816         }
 817         return s;
 818 }
 819
 820 static struct sock *__l2cap_get_chan_by_scid(struct l2cap_chan_list *l, __u16 cid)
 821 {
 822         struct sock *s;
 823         for (s = l->head; s; s = l2cap_pi(s)->next_c) {
 824                 if (l2cap_pi(s)->scid == cid)
 825                         break;
 826         }
 827         return s;
 828 }
 829
 830 /* Find channel with given SCID.
 831  * Returns locked socket */
 832 static inline struct sock *l2cap_get_chan_by_scid(struct l2cap_chan_list *l, __u16 cid)
 833 {
 834         struct sock *s;
 835         read_lock(&l->lock);
 836         s = __l2cap_get_chan_by_scid(l, cid);
 837         if (s) bh_lock_sock(s);
 838         read_unlock(&l->lock);
 839         return s;
 840 }
 841
 842 static __u16 l2cap_alloc_cid(struct l2cap_chan_list *l)
 843 {
 844         __u16 cid = 0x0040;
 845
 846         for (; cid < 0xffff; cid++) {
 847                 if(!__l2cap_get_chan_by_scid(l, cid))
 848                         return cid;
 849         }
 850
 851         return 0;
 852 }
 853
 854 static inline void __l2cap_chan_link(struct l2cap_chan_list *l, struct sock *sk)
 855 {
 856         sock_hold(sk);
 857
 858         if (l->head)
 859                 l2cap_pi(l->head)->prev_c = sk;
 860
 861         l2cap_pi(sk)->next_c = l->head;
 862         l2cap_pi(sk)->prev_c = NULL;
 863         l->head = sk;
 864 }
 865
 866 static inline void l2cap_chan_unlink(struct l2cap_chan_list *l, struct sock *sk)
 867 {
 868         struct sock *next = l2cap_pi(sk)->next_c, *prev = l2cap_pi(sk)->prev_c;
 869
 870         write_lock(&l->lock);
 871         if (sk == l->head)
 872                 l->head = next;
 873
 874         if (next)
 875                 l2cap_pi(next)->prev_c = prev;
 876         if (prev)
 877                 l2cap_pi(prev)->next_c = next;
 878         write_unlock(&l->lock);
 879
 880         __sock_put(sk);
 881 }
 882
 883 static void __l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct sock *parent)
 884 {
 885         struct l2cap_chan_list *l = &conn->chan_list;
 886
 887         BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn, l2cap_pi(sk)->psm, l2cap_pi(sk)->dcid);
 888
 889         l2cap_pi(sk)->conn = conn;
 890
 891         if (sk->type == SOCK_SEQPACKET) {
 892                 /* Alloc CID for connection-oriented socket */
 893                 l2cap_pi(sk)->scid = l2cap_alloc_cid(l);
 894         } else if (sk->type == SOCK_DGRAM) {
 895                 /* Connectionless socket */
 896                 l2cap_pi(sk)->scid = 0x0002;
 897                 l2cap_pi(sk)->dcid = 0x0002;
 898                 l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU;
 899         } else {
 900                 /* Raw socket can send/recv signalling messages only */
 901                 l2cap_pi(sk)->scid = 0x0001;
 902                 l2cap_pi(sk)->dcid = 0x0001;
 903                 l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU;
 904         }
 905
 906         __l2cap_chan_link(l, sk);
 907
 908         if (parent)
 909                 bluez_accept_enqueue(parent, sk);
 910 }
 911
 912 /* Delete channel.
 913  * Must be called on the locked socket. */
 914 static void l2cap_chan_del(struct sock *sk, int err)
 915 {
 916         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
 917         struct sock *parent = bluez_pi(sk)->parent;
 918
 919         l2cap_sock_clear_timer(sk);
 920
 921         BT_DBG("sk %p, conn %p, err %d", sk, conn, err);
 922
 923         if (conn) {
 924                 /* Unlink from channel list */
 925                 l2cap_chan_unlink(&conn->chan_list, sk);
 926                 l2cap_pi(sk)->conn = NULL;
 927                 hci_conn_put(conn->hcon);
 928         }
 929
 930         sk->state  = BT_CLOSED;
 931         sk->zapped = 1;
 932
 933         if (err)
 934                 sk->err = err;
 935
 936         if (parent)
 937                 parent->data_ready(parent, 0);
 938         else
 939                 sk->state_change(sk);
 940 }
 941
 942 static void l2cap_conn_ready(struct l2cap_conn *conn)
 943 {
 944         struct l2cap_chan_list *l = &conn->chan_list;
 945         struct sock *sk;
 946
 947         BT_DBG("conn %p", conn);
 948
 949         read_lock(&l->lock);
 950
 951         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
 952                 bh_lock_sock(sk);
 953
 954                 if (sk->type != SOCK_SEQPACKET) {
 955                         l2cap_sock_clear_timer(sk);
 956                         sk->state = BT_CONNECTED;
 957                         sk->state_change(sk);
 958                 } else if (sk->state == BT_CONNECT) {
 959                         l2cap_conn_req req;
 960                         req.scid = __cpu_to_le16(l2cap_pi(sk)->scid);
 961                         req.psm  = l2cap_pi(sk)->psm;
 962                         l2cap_send_req(conn, L2CAP_CONN_REQ, L2CAP_CONN_REQ_SIZE, &req);
 963                 }
 964
 965                 bh_unlock_sock(sk);
 966         }
 967
 968         read_unlock(&l->lock);
 969 }
 970
 971 /* Notify sockets that we cannot guaranty reliability anymore */
 972 static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err)
 973 {
 974         struct l2cap_chan_list *l = &conn->chan_list;
 975         struct sock *sk;
 976
 977         BT_DBG("conn %p", conn);
 978
 979         read_lock(&l->lock);
 980         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
 981                 if (l2cap_pi(sk)->link_mode & L2CAP_LM_RELIABLE)
 982                         sk->err = err;
 983         }
 984         read_unlock(&l->lock);
 985 }
 986
 987 static void l2cap_chan_ready(struct sock *sk)
 988 {
 989         struct sock *parent = bluez_pi(sk)->parent;
 990
 991         BT_DBG("sk %p, parent %p", sk, parent);
 992
 993         l2cap_pi(sk)->conf_state = 0;
 994         l2cap_sock_clear_timer(sk);
 995
 996         if (!parent) {
 997                 /* Outgoing channel.
 998                  * Wake up socket sleeping on connect.
 999                  */
1000                 sk->state = BT_CONNECTED;
1001                 sk->state_change(sk);
1002         } else {
1003                 /* Incomming channel.
1004                  * Wake up socket sleeping on accept.
1005                  */
1006                 parent->data_ready(parent, 0);
1007         }
1008 }
1009
1010 /* Copy frame to all raw sockets on that connection */
1011 void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
1012 {
1013         struct l2cap_chan_list *l = &conn->chan_list;
1014         struct sk_buff *nskb;
1015         struct sock * sk;
1016
1017         BT_DBG("conn %p", conn);
1018
1019         read_lock(&l->lock);
1020         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
1021                 if (sk->type != SOCK_RAW)
1022                         continue;
1023
1024                 /* Don't send frame to the socket it came from */
1025                 if (skb->sk == sk)
1026                         continue;
1027
1028                 if (!(nskb = skb_clone(skb, GFP_ATOMIC)))
1029                         continue;
1030
1031                 if (sock_queue_rcv_skb(sk, nskb))
1032                         kfree_skb(nskb);
1033         }
1034         read_unlock(&l->lock);
1035 }
1036
1037 static int l2cap_chan_send(struct sock *sk, struct msghdr *msg, int len)
1038 {
1039         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1040         struct sk_buff *skb, **frag;
1041         int err, hlen, count, sent=0;
1042         l2cap_hdr *lh;
1043
1044         BT_DBG("sk %p len %d", sk, len);
1045
1046         /* First fragment (with L2CAP header) */
1047         if (sk->type == SOCK_DGRAM)
1048                 hlen = L2CAP_HDR_SIZE + 2;
1049         else
1050                 hlen = L2CAP_HDR_SIZE;
1051
1052         count = MIN(conn->mtu - hlen, len);
1053
1054         skb = bluez_skb_send_alloc(sk, hlen + count,
1055                         msg->msg_flags & MSG_DONTWAIT, &err);
1056         if (!skb)
1057                 return err;
1058
1059         /* Create L2CAP header */
1060         lh = (l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1061         lh->cid = __cpu_to_le16(l2cap_pi(sk)->dcid);
1062         lh->len = __cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1063
1064         if (sk->type == SOCK_DGRAM)
1065                 put_unaligned(l2cap_pi(sk)->psm, (__u16 *) skb_put(skb, 2));
1066
1067         if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count)) {
1068                 err = -EFAULT;
1069                 goto fail;
1070         }
1071
1072         sent += count;
1073         len  -= count;
1074
1075         /* Continuation fragments (no L2CAP header) */
1076         frag = &skb_shinfo(skb)->frag_list;
1077         while (len) {
1078                 count = MIN(conn->mtu, len);
1079
1080                 *frag = bluez_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err);
1081                 if (!*frag)
1082                         goto fail;
1083
1084                 if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count)) {
1085                         err = -EFAULT;
1086                         goto fail;
1087                 }
1088
1089                 sent += count;
1090                 len  -= count;
1091
1092                 frag = &(*frag)->next;
1093         }
1094
1095         if ((err = hci_send_acl(conn->hcon, skb, 0)) < 0)
1096                 goto fail;
1097
1098         return sent;
1099
1100 fail:
1101         kfree_skb(skb);
1102         return err;
1103 }
1104
1105 /* --------- L2CAP signalling commands --------- */
1106 static inline __u8 l2cap_get_ident(struct l2cap_conn *conn)
1107 {
1108         __u8 id;
1109
1110         /* Get next available identificator.
1111          *    1 - 199 are used by kernel.
1112          *  200 - 254 are used by utilities like l2ping, etc
1113          */
1114
1115         spin_lock(&conn->lock);
1116
1117         if (++conn->tx_ident > 199)
1118                 conn->tx_ident = 1;
1119
1120         id = conn->tx_ident;
1121
1122         spin_unlock(&conn->lock);
1123
1124         return id;
1125 }
1126
1127 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
1128                                 __u8 code, __u8 ident, __u16 dlen, void *data)
1129 {
1130         struct sk_buff *skb, **frag;
1131         l2cap_cmd_hdr *cmd;
1132         l2cap_hdr *lh;
1133         int len, count;
1134
1135         BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %d", conn, code, ident, dlen);
1136
1137         len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen;
1138         count = MIN(conn->mtu, len);
1139
1140         skb = bluez_skb_alloc(count, GFP_ATOMIC);
1141         if (!skb)
1142                 return NULL;
1143
1144         lh = (l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1145         lh->len = __cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen);
1146         lh->cid = __cpu_to_le16(0x0001);
1147
1148         cmd = (l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE);
1149         cmd->code  = code;
1150         cmd->ident = ident;
1151         cmd->len   = __cpu_to_le16(dlen);
1152
1153         if (dlen) {
1154                 count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE;
1155                 memcpy(skb_put(skb, count), data, count);
1156                 data += count;
1157         }
1158
1159         len -= skb->len;
1160
1161         /* Continuation fragments (no L2CAP header) */
1162         frag = &skb_shinfo(skb)->frag_list;
1163         while (len) {
1164                 count = MIN(conn->mtu, len);
1165
1166                 *frag = bluez_skb_alloc(count, GFP_ATOMIC);
1167                 if (!*frag)
1168                         goto fail;
1169
1170                 memcpy(skb_put(*frag, count), data, count);
1171
1172                 len  -= count;
1173                 data += count;
1174
1175                 frag = &(*frag)->next;
1176         }
1177
1178         return skb;
1179
1180 fail:
1181         kfree_skb(skb);
1182         return NULL;
1183 }
1184
1185 static int l2cap_send_req(struct l2cap_conn *conn, __u8 code, __u16 len, void *data)
1186 {
1187         __u8 ident = l2cap_get_ident(conn);
1188         struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data);
1189
1190         BT_DBG("code 0x%2.2x", code);
1191
1192         if (!skb)
1193                 return -ENOMEM;
1194         return hci_send_acl(conn->hcon, skb, 0);
1195 }
1196
1197 static int l2cap_send_rsp(struct l2cap_conn *conn, __u8 ident, __u8 code, __u16 len, void *data)
1198 {
1199         struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data);
1200
1201         BT_DBG("code 0x%2.2x", code);
1202
1203         if (!skb)
1204                 return -ENOMEM;
1205         return hci_send_acl(conn->hcon, skb, 0);
1206 }
1207
1208 static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val)
1209 {
1210         l2cap_conf_opt *opt = *ptr;
1211         int len;
1212
1213         len = L2CAP_CONF_OPT_SIZE + opt->len;
1214         *ptr += len;
1215
1216         *type = opt->type;
1217         *olen = opt->len;
1218
1219         switch (opt->len) {
1220         case 1:
1221                 *val = *((__u8 *) opt->val);
1222                 break;
1223
1224         case 2:
1225                 *val = __le16_to_cpu(*((__u16 *)opt->val));
1226                 break;
1227
1228         case 4:
1229                 *val = __le32_to_cpu(*((__u32 *)opt->val));
1230                 break;
1231
1232         default:
1233                 *val = (unsigned long) opt->val;
1234                 break;
1235         };
1236
1237         BT_DBG("type 0x%2.2x len %d val 0x%lx", *type, opt->len, *val);
1238         return len;
1239 }
1240
1241 static inline void l2cap_parse_conf_req(struct sock *sk, void *data, int len)
1242 {
1243         int type, hint, olen;
1244         unsigned long val;
1245         void *ptr = data;
1246
1247         BT_DBG("sk %p len %d", sk, len);
1248
1249         while (len >= L2CAP_CONF_OPT_SIZE) {
1250                 len -= l2cap_get_conf_opt(&ptr, &type, &olen, &val);
1251
1252                 hint  = type & 0x80;
1253                 type &= 0x7f;
1254
1255                 switch (type) {
1256                 case L2CAP_CONF_MTU:
1257                         l2cap_pi(sk)->conf_mtu = val;
1258                         break;
1259
1260                 case L2CAP_CONF_FLUSH_TO:
1261                         l2cap_pi(sk)->flush_to = val;
1262                         break;
1263
1264                 case L2CAP_CONF_QOS:
1265                         break;
1266
1267                 default:
1268                         if (hint)
1269                                 break;
1270
1271                         /* FIXME: Reject unknown option */
1272                         break;
1273                 };
1274         }
1275 }
1276
1277 static void l2cap_add_conf_opt(void **ptr, __u8 type, __u8 len, unsigned long val)
1278 {
1279         register l2cap_conf_opt *opt = *ptr;
1280
1281         BT_DBG("type 0x%2.2x len %d val 0x%lx", type, len, val);
1282
1283         opt->type = type;
1284         opt->len  = len;
1285
1286         switch (len) {
1287         case 1:
1288                 *((__u8 *) opt->val)  = val;
1289                 break;
1290
1291         case 2:
1292                 *((__u16 *) opt->val) = __cpu_to_le16(val);
1293                 break;
1294
1295         case 4:
1296                 *((__u32 *) opt->val) = __cpu_to_le32(val);
1297                 break;
1298
1299         default:
1300                 memcpy(opt->val, (void *) val, len);
1301                 break;
1302         };
1303
1304         *ptr += L2CAP_CONF_OPT_SIZE + len;
1305 }
1306
1307 static int l2cap_build_conf_req(struct sock *sk, void *data)
1308 {
1309         struct l2cap_pinfo *pi = l2cap_pi(sk);
1310         l2cap_conf_req *req = (l2cap_conf_req *) data;
1311         void *ptr = req->data;
1312
1313         BT_DBG("sk %p", sk);
1314
1315         if (pi->imtu != L2CAP_DEFAULT_MTU)
1316                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->imtu);
1317
1318         /* FIXME. Need actual value of the flush timeout */
1319         //if (flush_to != L2CAP_DEFAULT_FLUSH_TO)
1320         //   l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO, 2, pi->flush_to);
1321
1322         req->dcid  = __cpu_to_le16(pi->dcid);
1323         req->flags = __cpu_to_le16(0);
1324
1325         return ptr - data;
1326 }
1327
1328 static inline int l2cap_conf_output(struct sock *sk, void **ptr)
1329 {
1330         struct l2cap_pinfo *pi = l2cap_pi(sk);
1331         int result = 0;
1332
1333         /* Configure output options and let the other side know
1334          * which ones we don't like.
1335          */
1336         if (pi->conf_mtu < pi->omtu) {
1337                 l2cap_add_conf_opt(ptr, L2CAP_CONF_MTU, 2, pi->omtu);
1338                 result = L2CAP_CONF_UNACCEPT;
1339         } else {
1340                 pi->omtu = pi->conf_mtu;
1341         }
1342
1343         BT_DBG("sk %p result %d", sk, result);
1344         return result;
1345 }
1346
1347 static int l2cap_build_conf_rsp(struct sock *sk, void *data, int *result)
1348 {
1349         l2cap_conf_rsp *rsp = (l2cap_conf_rsp *) data;
1350         void *ptr = rsp->data;
1351         u16 flags = 0;
1352
1353         BT_DBG("sk %p complete %d", sk, result ? 1 : 0);
1354
1355         if (result)
1356                 *result = l2cap_conf_output(sk, &ptr);
1357         else
1358                 flags |= 0x0001;
1359
1360         rsp->scid   = __cpu_to_le16(l2cap_pi(sk)->dcid);
1361         rsp->result = __cpu_to_le16(result ? *result : 0);
1362         rsp->flags  = __cpu_to_le16(flags);
1363
1364         return ptr - data;
1365 }
1366
1367 static inline int l2cap_connect_req(struct l2cap_conn *conn, l2cap_cmd_hdr *cmd, __u8 *data)
1368 {
1369         struct l2cap_chan_list *list = &conn->chan_list;
1370         l2cap_conn_req *req = (l2cap_conn_req *) data;
1371         l2cap_conn_rsp rsp;
1372         struct sock *sk, *parent;
1373         int result = 0, status = 0;
1374
1375         __u16 dcid = 0, scid = __le16_to_cpu(req->scid);
1376         __u16 psm  = req->psm;
1377
1378         BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid);
1379
1380         /* Check if we have socket listening on psm */
1381         parent = l2cap_get_sock_by_psm(BT_LISTEN, psm, conn->src);
1382         if (!parent) {
1383                 result = L2CAP_CR_BAD_PSM;
1384                 goto sendresp;
1385         }
1386
1387         result = L2CAP_CR_NO_MEM;
1388
1389         /* Check for backlog size */
1390         if (parent->ack_backlog > parent->max_ack_backlog) {
1391                 BT_DBG("backlog full %d", parent->ack_backlog);
1392                 goto response;
1393         }
1394
1395         sk = l2cap_sock_alloc(NULL, BTPROTO_L2CAP, GFP_ATOMIC);
1396         if (!sk)
1397                 goto response;
1398
1399         write_lock(&list->lock);
1400
1401         /* Check if we already have channel with that dcid */
1402         if (__l2cap_get_chan_by_dcid(list, scid)) {
1403                 write_unlock(&list->lock);
1404                 sk->zapped = 1;
1405                 l2cap_sock_kill(sk);
1406                 goto response;
1407         }
1408
1409         hci_conn_hold(conn->hcon);
1410
1411         l2cap_sock_init(sk, parent);
1412         bacpy(&bluez_pi(sk)->src, conn->src);
1413         bacpy(&bluez_pi(sk)->dst, conn->dst);
1414         l2cap_pi(sk)->psm  = psm;
1415         l2cap_pi(sk)->dcid = scid;
1416
1417         __l2cap_chan_add(conn, sk, parent);
1418         dcid = l2cap_pi(sk)->scid;
1419
1420         l2cap_sock_set_timer(sk, sk->sndtimeo);
1421
1422         /* Service level security */
1423         result = L2CAP_CR_PEND;
1424         status = L2CAP_CS_AUTHEN_PEND;
1425         sk->state = BT_CONNECT2;
1426         l2cap_pi(sk)->ident = cmd->ident;
1427
1428         if (l2cap_pi(sk)->link_mode & L2CAP_LM_ENCRYPT) {
1429                 if (!hci_conn_encrypt(conn->hcon))
1430                         goto done;
1431         } else if (l2cap_pi(sk)->link_mode & L2CAP_LM_AUTH) {
1432                 if (!hci_conn_auth(conn->hcon))
1433                         goto done;
1434         }
1435
1436         sk->state = BT_CONFIG;
1437         result = status = 0;
1438
1439 done:
1440         write_unlock(&list->lock);
1441
1442 response:
1443         bh_unlock_sock(parent);
1444
1445 sendresp:
1446         rsp.scid   = __cpu_to_le16(scid);
1447         rsp.dcid   = __cpu_to_le16(dcid);
1448         rsp.result = __cpu_to_le16(result);
1449         rsp.status = __cpu_to_le16(status);
1450         l2cap_send_rsp(conn, cmd->ident, L2CAP_CONN_RSP, L2CAP_CONN_RSP_SIZE, &rsp);
1451         return 0;
1452 }
1453
1454 static inline int l2cap_connect_rsp(struct l2cap_conn *conn, l2cap_cmd_hdr *cmd, __u8 *data)
1455 {
1456         l2cap_conn_rsp *rsp = (l2cap_conn_rsp *) data;
1457         __u16 scid, dcid, result, status;
1458         struct sock *sk;
1459         char req[128];
1460
1461         scid   = __le16_to_cpu(rsp->scid);
1462         dcid   = __le16_to_cpu(rsp->dcid);
1463         result = __le16_to_cpu(rsp->result);
1464         status = __le16_to_cpu(rsp->status);
1465
1466         BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status);
1467
1468         if (!(sk = l2cap_get_chan_by_scid(&conn->chan_list, scid)))
1469                 return -ENOENT;
1470
1471         switch (result) {
1472         case L2CAP_CR_SUCCESS:
1473                 sk->state = BT_CONFIG;
1474                 l2cap_pi(sk)->dcid = dcid;
1475                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
1476
1477                 l2cap_send_req(conn, L2CAP_CONF_REQ, l2cap_build_conf_req(sk, req), req);
1478                 break;
1479
1480         case L2CAP_CR_PEND:
1481                 break;
1482
1483         default:
1484                 l2cap_chan_del(sk, ECONNREFUSED);
1485                 break;
1486         }
1487
1488         bh_unlock_sock(sk);
1489         return 0;
1490 }
1491
1492 static inline int l2cap_config_req(struct l2cap_conn *conn, l2cap_cmd_hdr *cmd, __u8 *data)
1493 {
1494         l2cap_conf_req * req = (l2cap_conf_req *) data;
1495         __u16 dcid, flags;
1496         __u8 rsp[64];
1497         struct sock *sk;
1498         int result;
1499
1500         dcid  = __le16_to_cpu(req->dcid);
1501         flags = __le16_to_cpu(req->flags);
1502
1503         BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
1504
1505         if (!(sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid)))
1506                 return -ENOENT;
1507
1508         l2cap_parse_conf_req(sk, req->data, cmd->len - L2CAP_CONF_REQ_SIZE);
1509
1510         if (flags & 0x0001) {
1511                 /* Incomplete config. Send empty response. */
1512                 l2cap_send_rsp(conn, cmd->ident, L2CAP_CONF_RSP, l2cap_build_conf_rsp(sk, rsp, NULL), rsp);
1513                 goto unlock;
1514         }
1515
1516         /* Complete config. */
1517         l2cap_send_rsp(conn, cmd->ident, L2CAP_CONF_RSP, l2cap_build_conf_rsp(sk, rsp, &result), rsp);
1518
1519         if (result)
1520                 goto unlock;
1521
1522         /* Output config done */
1523         l2cap_pi(sk)->conf_state |= L2CAP_CONF_OUTPUT_DONE;
1524
1525         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_INPUT_DONE) {
1526                 sk->state = BT_CONNECTED;
1527                 l2cap_chan_ready(sk);
1528         } else if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT)) {
1529                 char req[64];
1530                 l2cap_send_req(conn, L2CAP_CONF_REQ, l2cap_build_conf_req(sk, req), req);
1531         }
1532
1533 unlock:
1534         bh_unlock_sock(sk);
1535         return 0;
1536 }
1537
1538 static inline int l2cap_config_rsp(struct l2cap_conn *conn, l2cap_cmd_hdr *cmd, __u8 *data)
1539 {
1540         l2cap_conf_rsp *rsp = (l2cap_conf_rsp *)data;
1541         __u16 scid, flags, result;
1542         struct sock *sk;
1543         int err = 0;
1544
1545         scid   = __le16_to_cpu(rsp->scid);
1546         flags  = __le16_to_cpu(rsp->flags);
1547         result = __le16_to_cpu(rsp->result);
1548
1549         BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x", scid, flags, result);
1550
1551         if (!(sk = l2cap_get_chan_by_scid(&conn->chan_list, scid)))
1552                 return -ENOENT;
1553
1554         switch (result) {
1555         case L2CAP_CONF_SUCCESS:
1556                 break;
1557
1558         case L2CAP_CONF_UNACCEPT:
1559                 if (++l2cap_pi(sk)->conf_retry < L2CAP_CONF_MAX_RETRIES) {
1560                         char req[128];
1561                         /*
1562                            It does not make sense to adjust L2CAP parameters
1563                            that are currently defined in the spec. We simply
1564                            resend config request that we sent earlier. It is
1565                            stupid :) but it helps qualification testing
1566                            which expects at least some response from us.
1567                         */
1568                         l2cap_send_req(conn, L2CAP_CONF_REQ,
1569                                 l2cap_build_conf_req(sk, req), req);
1570                         goto done;
1571                 }
1572         default:
1573                 sk->state = BT_DISCONN;
1574                 sk->err   = ECONNRESET;
1575                 l2cap_sock_set_timer(sk, HZ * 5);
1576                 {
1577                         l2cap_disconn_req req;
1578                         req.dcid = __cpu_to_le16(l2cap_pi(sk)->dcid);
1579                         req.scid = __cpu_to_le16(l2cap_pi(sk)->scid);
1580                         l2cap_send_req(conn, L2CAP_DISCONN_REQ, L2CAP_DISCONN_REQ_SIZE, &req);
1581                 }
1582                 goto done;
1583         }
1584
1585         if (flags & 0x01)
1586                 goto done;
1587
1588         /* Input config done */
1589         l2cap_pi(sk)->conf_state |= L2CAP_CONF_INPUT_DONE;
1590
1591         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE) {
1592                 sk->state = BT_CONNECTED;
1593                 l2cap_chan_ready(sk);
1594         }
1595
1596 done:
1597         bh_unlock_sock(sk);
1598         return err;
1599 }
1600
1601 static inline int l2cap_disconnect_req(struct l2cap_conn *conn, l2cap_cmd_hdr *cmd, __u8 *data)
1602 {
1603         l2cap_disconn_req *req = (l2cap_disconn_req *) data;
1604         l2cap_disconn_rsp rsp;
1605         __u16 dcid, scid;
1606         struct sock *sk;
1607
1608         scid = __le16_to_cpu(req->scid);
1609         dcid = __le16_to_cpu(req->dcid);
1610
1611         BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
1612
1613         if (!(sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid)))
1614                 return 0;
1615
1616         rsp.dcid = __cpu_to_le16(l2cap_pi(sk)->scid);
1617         rsp.scid = __cpu_to_le16(l2cap_pi(sk)->dcid);
1618         l2cap_send_rsp(conn, cmd->ident, L2CAP_DISCONN_RSP, L2CAP_DISCONN_RSP_SIZE, &rsp);
1619
1620         sk->shutdown = SHUTDOWN_MASK;
1621
1622         l2cap_chan_del(sk, ECONNRESET);
1623         bh_unlock_sock(sk);
1624
1625         l2cap_sock_kill(sk);
1626         return 0;
1627 }
1628
1629 static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, l2cap_cmd_hdr *cmd, __u8 *data)
1630 {
1631         l2cap_disconn_rsp *rsp = (l2cap_disconn_rsp *) data;
1632         __u16 dcid, scid;
1633         struct sock *sk;
1634
1635         scid = __le16_to_cpu(rsp->scid);
1636         dcid = __le16_to_cpu(rsp->dcid);
1637
1638         BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
1639
1640         if (!(sk = l2cap_get_chan_by_scid(&conn->chan_list, scid)))
1641                 return 0;
1642         l2cap_chan_del(sk, 0);
1643         bh_unlock_sock(sk);
1644
1645         l2cap_sock_kill(sk);
1646         return 0;
1647 }
1648
1649 static inline int l2cap_information_req(struct l2cap_conn *conn, l2cap_cmd_hdr *cmd, u8 *data)
1650 {
1651         l2cap_info_req *req = (l2cap_info_req *) data;
1652         l2cap_info_rsp rsp;
1653         u16 type;
1654
1655         type = __le16_to_cpu(req->type);
1656
1657         BT_DBG("type 0x%4.4x", type);
1658
1659         rsp.type   = __cpu_to_le16(type);
1660         rsp.result = __cpu_to_le16(L2CAP_IR_NOTSUPP);
1661         l2cap_send_rsp(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(rsp), &rsp);
1662         return 0;
1663 }
1664
1665 static inline int l2cap_information_rsp(struct l2cap_conn *conn, l2cap_cmd_hdr *cmd, u8 *data)
1666 {
1667         l2cap_info_rsp *rsp = (l2cap_info_rsp *) data;
1668         u16 type, result;
1669
1670         type   = __le16_to_cpu(rsp->type);
1671         result = __le16_to_cpu(rsp->result);
1672
1673         BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
1674
1675         return 0;
1676 }
1677
1678 static inline void l2cap_sig_channel(struct l2cap_conn *conn, struct sk_buff *skb)
1679 {
1680         __u8 *data = skb->data;
1681         int len = skb->len;
1682         l2cap_cmd_hdr cmd;
1683         int err = 0;
1684
1685         l2cap_raw_recv(conn, skb);
1686
1687         while (len >= L2CAP_CMD_HDR_SIZE) {
1688                 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
1689                 data += L2CAP_CMD_HDR_SIZE;
1690                 len  -= L2CAP_CMD_HDR_SIZE;
1691
1692                 cmd.len = __le16_to_cpu(cmd.len);
1693
1694                 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd.len, cmd.ident);
1695
1696                 if (cmd.len > len || !cmd.ident) {
1697                         BT_DBG("corrupted command");
1698                         break;
1699                 }
1700
1701                 switch (cmd.code) {
1702                 case L2CAP_COMMAND_REJ:
1703                         /* FIXME: We should process this */
1704                         break;
1705
1706                 case L2CAP_CONN_REQ:
1707                         err = l2cap_connect_req(conn, &cmd, data);
1708                         break;
1709
1710                 case L2CAP_CONN_RSP:
1711                         err = l2cap_connect_rsp(conn, &cmd, data);
1712                         break;
1713
1714                 case L2CAP_CONF_REQ:
1715                         err = l2cap_config_req(conn, &cmd, data);
1716                         break;
1717
1718                 case L2CAP_CONF_RSP:
1719                         err = l2cap_config_rsp(conn, &cmd, data);
1720                         break;
1721
1722                 case L2CAP_DISCONN_REQ:
1723                         err = l2cap_disconnect_req(conn, &cmd, data);
1724                         break;
1725
1726                 case L2CAP_DISCONN_RSP:
1727                         err = l2cap_disconnect_rsp(conn, &cmd, data);
1728                         break;
1729
1730                 case L2CAP_ECHO_REQ:
1731                         l2cap_send_rsp(conn, cmd.ident, L2CAP_ECHO_RSP, cmd.len, data);
1732                         break;
1733
1734                 case L2CAP_ECHO_RSP:
1735                         break;
1736
1737                 case L2CAP_INFO_REQ:
1738                         err = l2cap_information_req(conn, &cmd, data);
1739                         break;
1740
1741                 case L2CAP_INFO_RSP:
1742                         err = l2cap_information_rsp(conn, &cmd, data);
1743                         break;
1744
1745                 default:
1746                         BT_ERR("Unknown signaling command 0x%2.2x", cmd.code);
1747                         err = -EINVAL;
1748                         break;
1749                 };
1750
1751                 if (err) {
1752                         l2cap_cmd_rej rej;
1753                         BT_DBG("error %d", err);
1754
1755                         /* FIXME: Map err to a valid reason */
1756                         rej.reason = __cpu_to_le16(0);
1757                         l2cap_send_rsp(conn, cmd.ident, L2CAP_COMMAND_REJ, L2CAP_CMD_REJ_SIZE, &rej);
1758                 }
1759
1760                 data += cmd.len;
1761                 len  -= cmd.len;
1762         }
1763
1764         kfree_skb(skb);
1765 }
1766
1767 static inline int l2cap_data_channel(struct l2cap_conn *conn, __u16 cid, struct sk_buff *skb)
1768 {
1769         struct sock *sk;
1770
1771         sk = l2cap_get_chan_by_scid(&conn->chan_list, cid);
1772         if (!sk) {
1773                 BT_DBG("unknown cid 0x%4.4x", cid);
1774                 goto drop;
1775         }
1776
1777         BT_DBG("sk %p, len %d", sk, skb->len);
1778
1779         if (sk->state != BT_CONNECTED)
1780                 goto drop;
1781
1782         if (l2cap_pi(sk)->imtu < skb->len)
1783                 goto drop;
1784
1785         /* If socket recv buffers overflows we drop data here
1786          * which is *bad* because L2CAP has to be reliable.
1787          * But we don't have any other choice. L2CAP doesn't
1788          * provide flow control mechanism */
1789
1790         if (!sock_queue_rcv_skb(sk, skb))
1791                 goto done;
1792
1793 drop:
1794         kfree_skb(skb);
1795
1796 done:
1797         if (sk) bh_unlock_sock(sk);
1798         return 0;
1799 }
1800
1801 static inline int l2cap_conless_channel(struct l2cap_conn *conn, __u16 psm, struct sk_buff *skb)
1802 {
1803         struct sock *sk;
1804
1805         sk = l2cap_get_sock_by_psm(0, psm, conn->src);
1806         if (!sk)
1807                 goto drop;
1808
1809         BT_DBG("sk %p, len %d", sk, skb->len);
1810
1811         if (sk->state != BT_BOUND && sk->state != BT_CONNECTED)
1812                 goto drop;
1813
1814         if (l2cap_pi(sk)->imtu < skb->len)
1815                 goto drop;
1816
1817         if (!sock_queue_rcv_skb(sk, skb))
1818                 goto done;
1819
1820 drop:
1821         kfree_skb(skb);
1822
1823 done:
1824         if (sk) bh_unlock_sock(sk);
1825         return 0;
1826 }
1827
1828 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
1829 {
1830         l2cap_hdr *lh = (l2cap_hdr *) skb->data;
1831         __u16 cid, psm, len;
1832
1833         skb_pull(skb, L2CAP_HDR_SIZE);
1834         cid = __le16_to_cpu(lh->cid);
1835         len = __le16_to_cpu(lh->len);
1836
1837         BT_DBG("len %d, cid 0x%4.4x", len, cid);
1838
1839         switch (cid) {
1840         case 0x0001:
1841                 l2cap_sig_channel(conn, skb);
1842                 break;
1843
1844         case 0x0002:
1845                 psm = get_unaligned((__u16 *) skb->data);
1846                 skb_pull(skb, 2);
1847                 l2cap_conless_channel(conn, psm, skb);
1848                 break;
1849
1850         default:
1851                 l2cap_data_channel(conn, cid, skb);
1852                 break;
1853         }
1854 }
1855
1856 /* ------------ L2CAP interface with lower layer (HCI) ------------- */
1857
1858 static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, __u8 type)
1859 {
1860         int exact = 0, lm1 = 0, lm2 = 0;
1861         register struct sock *sk;
1862
1863         if (type != ACL_LINK)
1864                 return 0;
1865
1866         BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
1867
1868         /* Find listening sockets and check their link_mode */
1869         read_lock(&l2cap_sk_list.lock);
1870         for (sk = l2cap_sk_list.head; sk; sk = sk->next) {
1871                 if (sk->state != BT_LISTEN)
1872                         continue;
1873
1874                 if (!bacmp(&bluez_pi(sk)->src, &hdev->bdaddr)) {
1875                         lm1 |= (HCI_LM_ACCEPT | l2cap_pi(sk)->link_mode);
1876                         exact++;
1877                 } else if (!bacmp(&bluez_pi(sk)->src, BDADDR_ANY))
1878                         lm2 |= (HCI_LM_ACCEPT | l2cap_pi(sk)->link_mode);
1879         }
1880         read_unlock(&l2cap_sk_list.lock);
1881
1882         return exact ? lm1 : lm2;
1883 }
1884
1885 static int l2cap_connect_cfm(struct hci_conn *hcon, __u8 status)
1886 {
1887         BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
1888
1889         if (hcon->type != ACL_LINK)
1890                 return 0;
1891
1892         if (!status) {
1893                 struct l2cap_conn *conn;
1894
1895                 conn = l2cap_conn_add(hcon, status);
1896                 if (conn)
1897                         l2cap_conn_ready(conn);
1898         } else
1899                 l2cap_conn_del(hcon, bterr(status));
1900
1901         return 0;
1902 }
1903
1904 static int l2cap_disconn_ind(struct hci_conn *hcon, __u8 reason)
1905 {
1906         BT_DBG("hcon %p reason %d", hcon, reason);
1907
1908         if (hcon->type != ACL_LINK)
1909                 return 0;
1910
1911         l2cap_conn_del(hcon, bterr(reason));
1912         return 0;
1913 }
1914
1915 static int l2cap_auth_cfm(struct hci_conn *hcon, __u8 status)
1916 {
1917         struct l2cap_chan_list *l;
1918         struct l2cap_conn *conn;
1919         l2cap_conn_rsp rsp;
1920         struct sock *sk;
1921         int result;
1922
1923         if (!(conn = hcon->l2cap_data))
1924                 return 0;
1925         l = &conn->chan_list;
1926
1927         BT_DBG("conn %p", conn);
1928
1929         read_lock(&l->lock);
1930
1931         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
1932                 bh_lock_sock(sk);
1933
1934                 if (sk->state != BT_CONNECT2 ||
1935                                 (l2cap_pi(sk)->link_mode & L2CAP_LM_ENCRYPT)) {
1936                         bh_unlock_sock(sk);
1937                         continue;
1938                 }
1939
1940                 if (!status) {
1941                         sk->state = BT_CONFIG;
1942                         result = 0;
1943                 } else {
1944                         sk->state = BT_DISCONN;
1945                         l2cap_sock_set_timer(sk, HZ/10);
1946                         result = L2CAP_CR_SEC_BLOCK;
1947                 }
1948
1949                 rsp.scid   = __cpu_to_le16(l2cap_pi(sk)->dcid);
1950                 rsp.dcid   = __cpu_to_le16(l2cap_pi(sk)->scid);
1951                 rsp.result = __cpu_to_le16(result);
1952                 rsp.status = __cpu_to_le16(0);
1953                 l2cap_send_rsp(conn, l2cap_pi(sk)->ident, L2CAP_CONN_RSP,
1954                         L2CAP_CONN_RSP_SIZE, &rsp);
1955
1956                 bh_unlock_sock(sk);
1957         }
1958
1959         read_unlock(&l->lock);
1960         return 0;
1961 }
1962
1963 static int l2cap_encrypt_cfm(struct hci_conn *hcon, __u8 status)
1964 {
1965         struct l2cap_chan_list *l;
1966         struct l2cap_conn *conn;
1967         l2cap_conn_rsp rsp;
1968         struct sock *sk;
1969         int result;
1970
1971         if (!(conn = hcon->l2cap_data))
1972                 return 0;
1973         l = &conn->chan_list;
1974
1975         BT_DBG("conn %p", conn);
1976
1977         read_lock(&l->lock);
1978
1979         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
1980                 bh_lock_sock(sk);
1981
1982                 if (sk->state != BT_CONNECT2) {
1983                         bh_unlock_sock(sk);
1984                         continue;
1985                 }
1986
1987                 if (!status) {
1988                         sk->state = BT_CONFIG;
1989                         result = 0;
1990                 } else {
1991                         sk->state = BT_DISCONN;
1992                         l2cap_sock_set_timer(sk, HZ/10);
1993                         result = L2CAP_CR_SEC_BLOCK;
1994                 }
1995
1996                 rsp.scid   = __cpu_to_le16(l2cap_pi(sk)->dcid);
1997                 rsp.dcid   = __cpu_to_le16(l2cap_pi(sk)->scid);
1998                 rsp.result = __cpu_to_le16(result);
1999                 rsp.status = __cpu_to_le16(0);
2000                 l2cap_send_rsp(conn, l2cap_pi(sk)->ident, L2CAP_CONN_RSP,
2001                         L2CAP_CONN_RSP_SIZE, &rsp);
2002
2003                 bh_unlock_sock(sk);
2004         }
2005
2006         read_unlock(&l->lock);
2007         return 0;
2008 }
2009
2010 static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, __u16 flags)
2011 {
2012         struct l2cap_conn *conn = hcon->l2cap_data;
2013
2014         if (!conn && !(conn = l2cap_conn_add(hcon, 0)))
2015                 goto drop;
2016
2017         BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
2018
2019         if (flags & ACL_START) {
2020                 l2cap_hdr *hdr;
2021                 int len;
2022
2023                 if (conn->rx_len) {
2024                         BT_ERR("Unexpected start frame (len %d)", skb->len);
2025                         kfree_skb(conn->rx_skb);
2026                         conn->rx_skb = NULL;
2027                         conn->rx_len = 0;
2028                         l2cap_conn_unreliable(conn, ECOMM);
2029                 }
2030
2031                 if (skb->len < 2) {
2032                         BT_ERR("Frame is too short (len %d)", skb->len);
2033                         l2cap_conn_unreliable(conn, ECOMM);
2034                         goto drop;
2035                 }
2036
2037                 hdr = (l2cap_hdr *) skb->data;
2038                 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
2039
2040                 if (len == skb->len) {
2041                         /* Complete frame received */
2042                         l2cap_recv_frame(conn, skb);
2043                         return 0;
2044                 }
2045
2046                 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
2047
2048                 if (skb->len > len) {
2049                         BT_ERR("Frame is too long (len %d, expected len %d)",
2050                                 skb->len, len);
2051                         l2cap_conn_unreliable(conn, ECOMM);
2052                         goto drop;
2053                 }
2054
2055                 /* Allocate skb for the complete frame including header */
2056                 conn->rx_skb = bluez_skb_alloc(len, GFP_ATOMIC);
2057                 if (!conn->rx_skb)
2058                         goto drop;
2059
2060                 memcpy(skb_put(conn->rx_skb, skb->len), skb->data, skb->len);
2061                 conn->rx_len = len - skb->len;
2062         } else {
2063                 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
2064
2065                 if (!conn->rx_len) {
2066                         BT_ERR("Unexpected continuation frame (len %d)", skb->len);
2067                         l2cap_conn_unreliable(conn, ECOMM);
2068                         goto drop;
2069                 }
2070
2071                 if (skb->len > conn->rx_len) {
2072                         BT_ERR("Fragment is too long (len %d, expected %d)",
2073                                         skb->len, conn->rx_len);
2074                         kfree_skb(conn->rx_skb);
2075                         conn->rx_skb = NULL;
2076                         conn->rx_len = 0;
2077                         l2cap_conn_unreliable(conn, ECOMM);
2078                         goto drop;
2079                 }
2080
2081                 memcpy(skb_put(conn->rx_skb, skb->len), skb->data, skb->len);
2082                 conn->rx_len -= skb->len;
2083
2084                 if (!conn->rx_len) {
2085                         /* Complete frame received */
2086                         l2cap_recv_frame(conn, conn->rx_skb);
2087                         conn->rx_skb = NULL;
2088                 }
2089         }
2090
2091 drop:
2092         kfree_skb(skb);
2093         return 0;
2094 }
2095
2096 /* ----- Proc fs support ------ */
2097 static int l2cap_sock_dump(char *buf, struct bluez_sock_list *list)
2098 {
2099         struct l2cap_pinfo *pi;
2100         struct sock *sk;
2101         char *ptr = buf;
2102
2103         read_lock_bh(&list->lock);
2104
2105         for (sk = list->head; sk; sk = sk->next) {
2106                 pi = l2cap_pi(sk);
2107                 ptr += sprintf(ptr, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d 0x%x\n",
2108                                 batostr(&bluez_pi(sk)->src), batostr(&bluez_pi(sk)->dst),
2109                                 sk->state, pi->psm, pi->scid, pi->dcid, pi->imtu, pi->omtu,
2110                                 pi->link_mode);
2111         }
2112
2113         read_unlock_bh(&list->lock);
2114
2115         ptr += sprintf(ptr, "\n");
2116         return ptr - buf;
2117 }
2118
2119 static int l2cap_read_proc(char *buf, char **start, off_t offset, int count, int *eof, void *priv)
2120 {
2121         char *ptr = buf;
2122         int len;
2123
2124         BT_DBG("count %d, offset %ld", count, offset);
2125
2126         ptr += l2cap_sock_dump(ptr, &l2cap_sk_list);
2127         len  = ptr - buf;
2128
2129         if (len <= count + offset)
2130                 *eof = 1;
2131
2132         *start = buf + offset;
2133         len -= offset;
2134
2135         if (len > count)
2136                 len = count;
2137         if (len < 0)
2138                 len = 0;
2139
2140         return len;
2141 }
2142
2143 static struct proto_ops l2cap_sock_ops = {
2144         family:         PF_BLUETOOTH,
2145         release:        l2cap_sock_release,
2146         bind:           l2cap_sock_bind,
2147         connect:        l2cap_sock_connect,
2148         listen:         l2cap_sock_listen,
2149         accept:         l2cap_sock_accept,
2150         getname:        l2cap_sock_getname,
2151         sendmsg:        l2cap_sock_sendmsg,
2152         recvmsg:        bluez_sock_recvmsg,
2153         poll:           bluez_sock_poll,
2154         socketpair:     sock_no_socketpair,
2155         ioctl:          sock_no_ioctl,
2156         shutdown:       l2cap_sock_shutdown,
2157         setsockopt:     l2cap_sock_setsockopt,
2158         getsockopt:     l2cap_sock_getsockopt,
2159         mmap:           sock_no_mmap
2160 };
2161
2162 static struct net_proto_family l2cap_sock_family_ops = {
2163         family:         PF_BLUETOOTH,
2164         create:         l2cap_sock_create
2165 };
2166
2167 static struct hci_proto l2cap_hci_proto = {
2168         name:           "L2CAP",
2169         id:             HCI_PROTO_L2CAP,
2170         connect_ind:    l2cap_connect_ind,
2171         connect_cfm:    l2cap_connect_cfm,
2172         disconn_ind:    l2cap_disconn_ind,
2173         recv_acldata:   l2cap_recv_acldata,
2174         auth_cfm:       l2cap_auth_cfm,
2175         encrypt_cfm:    l2cap_encrypt_cfm
2176 };
2177
2178 int __init l2cap_init(void)
2179 {
2180         int err;
2181
2182         if ((err = bluez_sock_register(BTPROTO_L2CAP, &l2cap_sock_family_ops))) {
2183                 BT_ERR("Can't register L2CAP socket");
2184                 return err;
2185         }
2186
2187         if ((err = hci_register_proto(&l2cap_hci_proto))) {
2188                 BT_ERR("Can't register L2CAP protocol");
2189                 return err;
2190         }
2191
2192         create_proc_read_entry("bluetooth/l2cap", 0, 0, l2cap_read_proc, NULL);
2193
2194         BT_INFO("BlueZ L2CAP ver %s Copyright (C) 2000,2001 Qualcomm Inc", VERSION);
2195         BT_INFO("Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.com>");
2196         return 0;
2197 }
2198
2199 void l2cap_cleanup(void)
2200 {
2201         remove_proc_entry("bluetooth/l2cap", NULL);
2202
2203         /* Unregister socket and protocol */
2204         if (bluez_sock_unregister(BTPROTO_L2CAP))
2205                 BT_ERR("Can't unregister L2CAP socket");
2206
2207         if (hci_unregister_proto(&l2cap_hci_proto))
2208                 BT_ERR("Can't unregister L2CAP protocol");
2209 }
2210
2211 void l2cap_load(void)
2212 {
2213         /* Dummy function to trigger automatic L2CAP module loading by
2214            other modules that use L2CAP sockets but do not use any other
2215            symbols from it. */
2216         return;
2217 }
2218
2219 EXPORT_SYMBOL(l2cap_load);
2220
2221 module_init(l2cap_init);
2222 module_exit(l2cap_cleanup);
2223
2224 MODULE_AUTHOR("Maxim Krasnyansky <maxk@qualcomm.com>");
2225 MODULE_DESCRIPTION("BlueZ L2CAP ver " VERSION);
2226 MODULE_LICENSE("GPL");