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