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