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/capability.h>
  32 #include <linux/errno.h>
  33 #include <linux/kernel.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/list.h>
  43 #include <linux/device.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.8"
  60
  61 static const 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         err = sock_error(sk);
 772         if (err)
 773                 return err;
 774
 775         if (msg->msg_flags & MSG_OOB)
 776                 return -EOPNOTSUPP;
 777
 778         /* Check outgoing MTU */
 779         if (sk->sk_type != SOCK_RAW && len > l2cap_pi(sk)->omtu)
 780                 return -EINVAL;
 781
 782         lock_sock(sk);
 783
 784         if (sk->sk_state == BT_CONNECTED)
 785                 err = l2cap_do_send(sk, msg, len);
 786         else
 787                 err = -ENOTCONN;
 788
 789         release_sock(sk);
 790         return err;
 791 }
 792
 793 static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname, char __user *optval, int optlen)
 794 {
 795         struct sock *sk = sock->sk;
 796         struct l2cap_options opts;
 797         int err = 0, len;
 798         u32 opt;
 799
 800         BT_DBG("sk %p", sk);
 801
 802         lock_sock(sk);
 803
 804         switch (optname) {
 805         case L2CAP_OPTIONS:
 806                 len = min_t(unsigned int, sizeof(opts), optlen);
 807                 if (copy_from_user((char *) &opts, optval, len)) {
 808                         err = -EFAULT;
 809                         break;
 810                 }
 811                 l2cap_pi(sk)->imtu  = opts.imtu;
 812                 l2cap_pi(sk)->omtu  = opts.omtu;
 813                 break;
 814
 815         case L2CAP_LM:
 816                 if (get_user(opt, (u32 __user *) optval)) {
 817                         err = -EFAULT;
 818                         break;
 819                 }
 820
 821                 l2cap_pi(sk)->link_mode = opt;
 822                 break;
 823
 824         default:
 825                 err = -ENOPROTOOPT;
 826                 break;
 827         }
 828
 829         release_sock(sk);
 830         return err;
 831 }
 832
 833 static int l2cap_sock_getsockopt(struct socket *sock, int level, int optname, char __user *optval, int __user *optlen)
 834 {
 835         struct sock *sk = sock->sk;
 836         struct l2cap_options opts;
 837         struct l2cap_conninfo cinfo;
 838         int len, err = 0;
 839
 840         BT_DBG("sk %p", sk);
 841
 842         if (get_user(len, optlen))
 843                 return -EFAULT;
 844
 845         lock_sock(sk);
 846
 847         switch (optname) {
 848         case L2CAP_OPTIONS:
 849                 opts.imtu     = l2cap_pi(sk)->imtu;
 850                 opts.omtu     = l2cap_pi(sk)->omtu;
 851                 opts.flush_to = l2cap_pi(sk)->flush_to;
 852                 opts.mode     = 0x00;
 853
 854                 len = min_t(unsigned int, len, sizeof(opts));
 855                 if (copy_to_user(optval, (char *) &opts, len))
 856                         err = -EFAULT;
 857
 858                 break;
 859
 860         case L2CAP_LM:
 861                 if (put_user(l2cap_pi(sk)->link_mode, (u32 __user *) optval))
 862                         err = -EFAULT;
 863                 break;
 864
 865         case L2CAP_CONNINFO:
 866                 if (sk->sk_state != BT_CONNECTED) {
 867                         err = -ENOTCONN;
 868                         break;
 869                 }
 870
 871                 cinfo.hci_handle = l2cap_pi(sk)->conn->hcon->handle;
 872                 memcpy(cinfo.dev_class, l2cap_pi(sk)->conn->hcon->dev_class, 3);
 873
 874                 len = min_t(unsigned int, len, sizeof(cinfo));
 875                 if (copy_to_user(optval, (char *) &cinfo, len))
 876                         err = -EFAULT;
 877
 878                 break;
 879
 880         default:
 881                 err = -ENOPROTOOPT;
 882                 break;
 883         }
 884
 885         release_sock(sk);
 886         return err;
 887 }
 888
 889 static int l2cap_sock_shutdown(struct socket *sock, int how)
 890 {
 891         struct sock *sk = sock->sk;
 892         int err = 0;
 893
 894         BT_DBG("sock %p, sk %p", sock, sk);
 895
 896         if (!sk)
 897                 return 0;
 898
 899         lock_sock(sk);
 900         if (!sk->sk_shutdown) {
 901                 sk->sk_shutdown = SHUTDOWN_MASK;
 902                 l2cap_sock_clear_timer(sk);
 903                 __l2cap_sock_close(sk, 0);
 904
 905                 if (sock_flag(sk, SOCK_LINGER) && sk->sk_lingertime)
 906                         err = bt_sock_wait_state(sk, BT_CLOSED, sk->sk_lingertime);
 907         }
 908         release_sock(sk);
 909         return err;
 910 }
 911
 912 static int l2cap_sock_release(struct socket *sock)
 913 {
 914         struct sock *sk = sock->sk;
 915         int err;
 916
 917         BT_DBG("sock %p, sk %p", sock, sk);
 918
 919         if (!sk)
 920                 return 0;
 921
 922         err = l2cap_sock_shutdown(sock, 2);
 923
 924         sock_orphan(sk);
 925         l2cap_sock_kill(sk);
 926         return err;
 927 }
 928
 929 /* ---- L2CAP channels ---- */
 930 static struct sock *__l2cap_get_chan_by_dcid(struct l2cap_chan_list *l, u16 cid)
 931 {
 932         struct sock *s;
 933         for (s = l->head; s; s = l2cap_pi(s)->next_c) {
 934                 if (l2cap_pi(s)->dcid == cid)
 935                         break;
 936         }
 937         return s;
 938 }
 939
 940 static struct sock *__l2cap_get_chan_by_scid(struct l2cap_chan_list *l, u16 cid)
 941 {
 942         struct sock *s;
 943         for (s = l->head; s; s = l2cap_pi(s)->next_c) {
 944                 if (l2cap_pi(s)->scid == cid)
 945                         break;
 946         }
 947         return s;
 948 }
 949
 950 /* Find channel with given SCID.
 951  * Returns locked socket */
 952 static inline struct sock *l2cap_get_chan_by_scid(struct l2cap_chan_list *l, u16 cid)
 953 {
 954         struct sock *s;
 955         read_lock(&l->lock);
 956         s = __l2cap_get_chan_by_scid(l, cid);
 957         if (s) bh_lock_sock(s);
 958         read_unlock(&l->lock);
 959         return s;
 960 }
 961
 962 static struct sock *__l2cap_get_chan_by_ident(struct l2cap_chan_list *l, u8 ident)
 963 {
 964         struct sock *s;
 965         for (s = l->head; s; s = l2cap_pi(s)->next_c) {
 966                 if (l2cap_pi(s)->ident == ident)
 967                         break;
 968         }
 969         return s;
 970 }
 971
 972 static inline struct sock *l2cap_get_chan_by_ident(struct l2cap_chan_list *l, u8 ident)
 973 {
 974         struct sock *s;
 975         read_lock(&l->lock);
 976         s = __l2cap_get_chan_by_ident(l, ident);
 977         if (s) bh_lock_sock(s);
 978         read_unlock(&l->lock);
 979         return s;
 980 }
 981
 982 static u16 l2cap_alloc_cid(struct l2cap_chan_list *l)
 983 {
 984         u16 cid = 0x0040;
 985
 986         for (; cid < 0xffff; cid++) {
 987                 if(!__l2cap_get_chan_by_scid(l, cid))
 988                         return cid;
 989         }
 990
 991         return 0;
 992 }
 993
 994 static inline void __l2cap_chan_link(struct l2cap_chan_list *l, struct sock *sk)
 995 {
 996         sock_hold(sk);
 997
 998         if (l->head)
 999                 l2cap_pi(l->head)->prev_c = sk;
1000
1001         l2cap_pi(sk)->next_c = l->head;
1002         l2cap_pi(sk)->prev_c = NULL;
1003         l->head = sk;
1004 }
1005
1006 static inline void l2cap_chan_unlink(struct l2cap_chan_list *l, struct sock *sk)
1007 {
1008         struct sock *next = l2cap_pi(sk)->next_c, *prev = l2cap_pi(sk)->prev_c;
1009
1010         write_lock(&l->lock);
1011         if (sk == l->head)
1012                 l->head = next;
1013
1014         if (next)
1015                 l2cap_pi(next)->prev_c = prev;
1016         if (prev)
1017                 l2cap_pi(prev)->next_c = next;
1018         write_unlock(&l->lock);
1019
1020         __sock_put(sk);
1021 }
1022
1023 static void __l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct sock *parent)
1024 {
1025         struct l2cap_chan_list *l = &conn->chan_list;
1026
1027         BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn, l2cap_pi(sk)->psm, l2cap_pi(sk)->dcid);
1028
1029         l2cap_pi(sk)->conn = conn;
1030
1031         if (sk->sk_type == SOCK_SEQPACKET) {
1032                 /* Alloc CID for connection-oriented socket */
1033                 l2cap_pi(sk)->scid = l2cap_alloc_cid(l);
1034         } else if (sk->sk_type == SOCK_DGRAM) {
1035                 /* Connectionless socket */
1036                 l2cap_pi(sk)->scid = 0x0002;
1037                 l2cap_pi(sk)->dcid = 0x0002;
1038                 l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU;
1039         } else {
1040                 /* Raw socket can send/recv signalling messages only */
1041                 l2cap_pi(sk)->scid = 0x0001;
1042                 l2cap_pi(sk)->dcid = 0x0001;
1043                 l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU;
1044         }
1045
1046         __l2cap_chan_link(l, sk);
1047
1048         if (parent)
1049                 bt_accept_enqueue(parent, sk);
1050 }
1051
1052 /* Delete channel.
1053  * Must be called on the locked socket. */
1054 static void l2cap_chan_del(struct sock *sk, int err)
1055 {
1056         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1057         struct sock *parent = bt_sk(sk)->parent;
1058
1059         l2cap_sock_clear_timer(sk);
1060
1061         BT_DBG("sk %p, conn %p, err %d", sk, conn, err);
1062
1063         if (conn) {
1064                 /* Unlink from channel list */
1065                 l2cap_chan_unlink(&conn->chan_list, sk);
1066                 l2cap_pi(sk)->conn = NULL;
1067                 hci_conn_put(conn->hcon);
1068         }
1069
1070         sk->sk_state  = BT_CLOSED;
1071         sock_set_flag(sk, SOCK_ZAPPED);
1072
1073         if (err)
1074                 sk->sk_err = err;
1075
1076         if (parent) {
1077                 bt_accept_unlink(sk);
1078                 parent->sk_data_ready(parent, 0);
1079         } else
1080                 sk->sk_state_change(sk);
1081 }
1082
1083 static void l2cap_conn_ready(struct l2cap_conn *conn)
1084 {
1085         struct l2cap_chan_list *l = &conn->chan_list;
1086         struct sock *sk;
1087
1088         BT_DBG("conn %p", conn);
1089
1090         read_lock(&l->lock);
1091
1092         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
1093                 bh_lock_sock(sk);
1094
1095                 if (sk->sk_type != SOCK_SEQPACKET) {
1096                         l2cap_sock_clear_timer(sk);
1097                         sk->sk_state = BT_CONNECTED;
1098                         sk->sk_state_change(sk);
1099                 } else if (sk->sk_state == BT_CONNECT) {
1100                         struct l2cap_conn_req req;
1101                         l2cap_pi(sk)->ident = l2cap_get_ident(conn);
1102                         req.scid = __cpu_to_le16(l2cap_pi(sk)->scid);
1103                         req.psm  = l2cap_pi(sk)->psm;
1104                         l2cap_send_cmd(conn, l2cap_pi(sk)->ident, L2CAP_CONN_REQ, sizeof(req), &req);
1105                 }
1106
1107                 bh_unlock_sock(sk);
1108         }
1109
1110         read_unlock(&l->lock);
1111 }
1112
1113 /* Notify sockets that we cannot guaranty reliability anymore */
1114 static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err)
1115 {
1116         struct l2cap_chan_list *l = &conn->chan_list;
1117         struct sock *sk;
1118
1119         BT_DBG("conn %p", conn);
1120
1121         read_lock(&l->lock);
1122         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
1123                 if (l2cap_pi(sk)->link_mode & L2CAP_LM_RELIABLE)
1124                         sk->sk_err = err;
1125         }
1126         read_unlock(&l->lock);
1127 }
1128
1129 static void l2cap_chan_ready(struct sock *sk)
1130 {
1131         struct sock *parent = bt_sk(sk)->parent;
1132
1133         BT_DBG("sk %p, parent %p", sk, parent);
1134
1135         l2cap_pi(sk)->conf_state = 0;
1136         l2cap_sock_clear_timer(sk);
1137
1138         if (!parent) {
1139                 /* Outgoing channel.
1140                  * Wake up socket sleeping on connect.
1141                  */
1142                 sk->sk_state = BT_CONNECTED;
1143                 sk->sk_state_change(sk);
1144         } else {
1145                 /* Incoming channel.
1146                  * Wake up socket sleeping on accept.
1147                  */
1148                 parent->sk_data_ready(parent, 0);
1149         }
1150 }
1151
1152 /* Copy frame to all raw sockets on that connection */
1153 static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
1154 {
1155         struct l2cap_chan_list *l = &conn->chan_list;
1156         struct sk_buff *nskb;
1157         struct sock * sk;
1158
1159         BT_DBG("conn %p", conn);
1160
1161         read_lock(&l->lock);
1162         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
1163                 if (sk->sk_type != SOCK_RAW)
1164                         continue;
1165
1166                 /* Don't send frame to the socket it came from */
1167                 if (skb->sk == sk)
1168                         continue;
1169
1170                 if (!(nskb = skb_clone(skb, GFP_ATOMIC)))
1171                         continue;
1172
1173                 if (sock_queue_rcv_skb(sk, nskb))
1174                         kfree_skb(nskb);
1175         }
1176         read_unlock(&l->lock);
1177 }
1178
1179 /* ---- L2CAP signalling commands ---- */
1180 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
1181                                 u8 code, u8 ident, u16 dlen, void *data)
1182 {
1183         struct sk_buff *skb, **frag;
1184         struct l2cap_cmd_hdr *cmd;
1185         struct l2cap_hdr *lh;
1186         int len, count;
1187
1188         BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %d", conn, code, ident, dlen);
1189
1190         len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen;
1191         count = min_t(unsigned int, conn->mtu, len);
1192
1193         skb = bt_skb_alloc(count, GFP_ATOMIC);
1194         if (!skb)
1195                 return NULL;
1196
1197         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1198         lh->len = __cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen);
1199         lh->cid = __cpu_to_le16(0x0001);
1200
1201         cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE);
1202         cmd->code  = code;
1203         cmd->ident = ident;
1204         cmd->len   = __cpu_to_le16(dlen);
1205
1206         if (dlen) {
1207                 count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE;
1208                 memcpy(skb_put(skb, count), data, count);
1209                 data += count;
1210         }
1211
1212         len -= skb->len;
1213
1214         /* Continuation fragments (no L2CAP header) */
1215         frag = &skb_shinfo(skb)->frag_list;
1216         while (len) {
1217                 count = min_t(unsigned int, conn->mtu, len);
1218
1219                 *frag = bt_skb_alloc(count, GFP_ATOMIC);
1220                 if (!*frag)
1221                         goto fail;
1222
1223                 memcpy(skb_put(*frag, count), data, count);
1224
1225                 len  -= count;
1226                 data += count;
1227
1228                 frag = &(*frag)->next;
1229         }
1230
1231         return skb;
1232
1233 fail:
1234         kfree_skb(skb);
1235         return NULL;
1236 }
1237
1238 static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val)
1239 {
1240         struct l2cap_conf_opt *opt = *ptr;
1241         int len;
1242
1243         len = L2CAP_CONF_OPT_SIZE + opt->len;
1244         *ptr += len;
1245
1246         *type = opt->type;
1247         *olen = opt->len;
1248
1249         switch (opt->len) {
1250         case 1:
1251                 *val = *((u8 *) opt->val);
1252                 break;
1253
1254         case 2:
1255                 *val = __le16_to_cpu(*((u16 *)opt->val));
1256                 break;
1257
1258         case 4:
1259                 *val = __le32_to_cpu(*((u32 *)opt->val));
1260                 break;
1261
1262         default:
1263                 *val = (unsigned long) opt->val;
1264                 break;
1265         }
1266
1267         BT_DBG("type 0x%2.2x len %d val 0x%lx", *type, opt->len, *val);
1268         return len;
1269 }
1270
1271 static inline void l2cap_parse_conf_req(struct sock *sk, void *data, int len)
1272 {
1273         int type, hint, olen;
1274         unsigned long val;
1275         void *ptr = data;
1276
1277         BT_DBG("sk %p len %d", sk, len);
1278
1279         while (len >= L2CAP_CONF_OPT_SIZE) {
1280                 len -= l2cap_get_conf_opt(&ptr, &type, &olen, &val);
1281
1282                 hint  = type & 0x80;
1283                 type &= 0x7f;
1284
1285                 switch (type) {
1286                 case L2CAP_CONF_MTU:
1287                         l2cap_pi(sk)->conf_mtu = val;
1288                         break;
1289
1290                 case L2CAP_CONF_FLUSH_TO:
1291                         l2cap_pi(sk)->flush_to = val;
1292                         break;
1293
1294                 case L2CAP_CONF_QOS:
1295                         break;
1296
1297                 default:
1298                         if (hint)
1299                                 break;
1300
1301                         /* FIXME: Reject unknown option */
1302                         break;
1303                 }
1304         }
1305 }
1306
1307 static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val)
1308 {
1309         struct l2cap_conf_opt *opt = *ptr;
1310
1311         BT_DBG("type 0x%2.2x len %d val 0x%lx", type, len, val);
1312
1313         opt->type = type;
1314         opt->len  = len;
1315
1316         switch (len) {
1317         case 1:
1318                 *((u8 *) opt->val)  = val;
1319                 break;
1320
1321         case 2:
1322                 *((u16 *) opt->val) = __cpu_to_le16(val);
1323                 break;
1324
1325         case 4:
1326                 *((u32 *) opt->val) = __cpu_to_le32(val);
1327                 break;
1328
1329         default:
1330                 memcpy(opt->val, (void *) val, len);
1331                 break;
1332         }
1333
1334         *ptr += L2CAP_CONF_OPT_SIZE + len;
1335 }
1336
1337 static int l2cap_build_conf_req(struct sock *sk, void *data)
1338 {
1339         struct l2cap_pinfo *pi = l2cap_pi(sk);
1340         struct l2cap_conf_req *req = data;
1341         void *ptr = req->data;
1342
1343         BT_DBG("sk %p", sk);
1344
1345         if (pi->imtu != L2CAP_DEFAULT_MTU)
1346                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->imtu);
1347
1348         /* FIXME: Need actual value of the flush timeout */
1349         //if (flush_to != L2CAP_DEFAULT_FLUSH_TO)
1350         //   l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO, 2, pi->flush_to);
1351
1352         req->dcid  = __cpu_to_le16(pi->dcid);
1353         req->flags = __cpu_to_le16(0);
1354
1355         return ptr - data;
1356 }
1357
1358 static inline int l2cap_conf_output(struct sock *sk, void **ptr)
1359 {
1360         struct l2cap_pinfo *pi = l2cap_pi(sk);
1361         int result = 0;
1362
1363         /* Configure output options and let the other side know
1364          * which ones we don't like. */
1365         if (pi->conf_mtu < pi->omtu) {
1366                 l2cap_add_conf_opt(ptr, L2CAP_CONF_MTU, 2, pi->omtu);
1367                 result = L2CAP_CONF_UNACCEPT;
1368         } else {
1369                 pi->omtu = pi->conf_mtu;
1370         }
1371
1372         BT_DBG("sk %p result %d", sk, result);
1373         return result;
1374 }
1375
1376 static int l2cap_build_conf_rsp(struct sock *sk, void *data, int *result)
1377 {
1378         struct l2cap_conf_rsp *rsp = data;
1379         void *ptr = rsp->data;
1380         u16 flags = 0;
1381
1382         BT_DBG("sk %p complete %d", sk, result ? 1 : 0);
1383
1384         if (result)
1385                 *result = l2cap_conf_output(sk, &ptr);
1386         else
1387                 flags = 0x0001;
1388
1389         rsp->scid   = __cpu_to_le16(l2cap_pi(sk)->dcid);
1390         rsp->result = __cpu_to_le16(result ? *result : 0);
1391         rsp->flags  = __cpu_to_le16(flags);
1392
1393         return ptr - data;
1394 }
1395
1396 static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1397 {
1398         struct l2cap_chan_list *list = &conn->chan_list;
1399         struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
1400         struct l2cap_conn_rsp rsp;
1401         struct sock *sk, *parent;
1402         int result = 0, status = 0;
1403
1404         u16 dcid = 0, scid = __le16_to_cpu(req->scid);
1405         u16 psm  = req->psm;
1406
1407         BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid);
1408
1409         /* Check if we have socket listening on psm */
1410         parent = l2cap_get_sock_by_psm(BT_LISTEN, psm, conn->src);
1411         if (!parent) {
1412                 result = L2CAP_CR_BAD_PSM;
1413                 goto sendresp;
1414         }
1415
1416         result = L2CAP_CR_NO_MEM;
1417
1418         /* Check for backlog size */
1419         if (sk_acceptq_is_full(parent)) {
1420                 BT_DBG("backlog full %d", parent->sk_ack_backlog);
1421                 goto response;
1422         }
1423
1424         sk = l2cap_sock_alloc(NULL, BTPROTO_L2CAP, GFP_ATOMIC);
1425         if (!sk)
1426                 goto response;
1427
1428         write_lock(&list->lock);
1429
1430         /* Check if we already have channel with that dcid */
1431         if (__l2cap_get_chan_by_dcid(list, scid)) {
1432                 write_unlock(&list->lock);
1433                 sock_set_flag(sk, SOCK_ZAPPED);
1434                 l2cap_sock_kill(sk);
1435                 goto response;
1436         }
1437
1438         hci_conn_hold(conn->hcon);
1439
1440         l2cap_sock_init(sk, parent);
1441         bacpy(&bt_sk(sk)->src, conn->src);
1442         bacpy(&bt_sk(sk)->dst, conn->dst);
1443         l2cap_pi(sk)->psm  = psm;
1444         l2cap_pi(sk)->dcid = scid;
1445
1446         __l2cap_chan_add(conn, sk, parent);
1447         dcid = l2cap_pi(sk)->scid;
1448
1449         l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
1450
1451         /* Service level security */
1452         result = L2CAP_CR_PEND;
1453         status = L2CAP_CS_AUTHEN_PEND;
1454         sk->sk_state = BT_CONNECT2;
1455         l2cap_pi(sk)->ident = cmd->ident;
1456
1457         if ((l2cap_pi(sk)->link_mode & L2CAP_LM_ENCRYPT) ||
1458                         (l2cap_pi(sk)->link_mode & L2CAP_LM_SECURE)) {
1459                 if (!hci_conn_encrypt(conn->hcon))
1460                         goto done;
1461         } else if (l2cap_pi(sk)->link_mode & L2CAP_LM_AUTH) {
1462                 if (!hci_conn_auth(conn->hcon))
1463                         goto done;
1464         }
1465
1466         sk->sk_state = BT_CONFIG;
1467         result = status = 0;
1468
1469 done:
1470         write_unlock(&list->lock);
1471
1472 response:
1473         bh_unlock_sock(parent);
1474
1475 sendresp:
1476         rsp.scid   = __cpu_to_le16(scid);
1477         rsp.dcid   = __cpu_to_le16(dcid);
1478         rsp.result = __cpu_to_le16(result);
1479         rsp.status = __cpu_to_le16(status);
1480         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
1481         return 0;
1482 }
1483
1484 static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1485 {
1486         struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
1487         u16 scid, dcid, result, status;
1488         struct sock *sk;
1489         u8 req[128];
1490
1491         scid   = __le16_to_cpu(rsp->scid);
1492         dcid   = __le16_to_cpu(rsp->dcid);
1493         result = __le16_to_cpu(rsp->result);
1494         status = __le16_to_cpu(rsp->status);
1495
1496         BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status);
1497
1498         if (scid) {
1499                 if (!(sk = l2cap_get_chan_by_scid(&conn->chan_list, scid)))
1500                         return 0;
1501         } else {
1502                 if (!(sk = l2cap_get_chan_by_ident(&conn->chan_list, cmd->ident)))
1503                         return 0;
1504         }
1505
1506         switch (result) {
1507         case L2CAP_CR_SUCCESS:
1508                 sk->sk_state = BT_CONFIG;
1509                 l2cap_pi(sk)->ident = 0;
1510                 l2cap_pi(sk)->dcid = dcid;
1511                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
1512
1513                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
1514                                         l2cap_build_conf_req(sk, req), req);
1515                 break;
1516
1517         case L2CAP_CR_PEND:
1518                 break;
1519
1520         default:
1521                 l2cap_chan_del(sk, ECONNREFUSED);
1522                 break;
1523         }
1524
1525         bh_unlock_sock(sk);
1526         return 0;
1527 }
1528
1529 static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1530 {
1531         struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
1532         u16 dcid, flags;
1533         u8 rsp[64];
1534         struct sock *sk;
1535         int result;
1536
1537         dcid  = __le16_to_cpu(req->dcid);
1538         flags = __le16_to_cpu(req->flags);
1539
1540         BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
1541
1542         if (!(sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid)))
1543                 return -ENOENT;
1544
1545         l2cap_parse_conf_req(sk, req->data, cmd->len - sizeof(*req));
1546
1547         if (flags & 0x0001) {
1548                 /* Incomplete config. Send empty response. */
1549                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
1550                                 l2cap_build_conf_rsp(sk, rsp, NULL), rsp);
1551                 goto unlock;
1552         }
1553
1554         /* Complete config. */
1555         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
1556                         l2cap_build_conf_rsp(sk, rsp, &result), rsp);
1557
1558         if (result)
1559                 goto unlock;
1560
1561         /* Output config done */
1562         l2cap_pi(sk)->conf_state |= L2CAP_CONF_OUTPUT_DONE;
1563
1564         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_INPUT_DONE) {
1565                 sk->sk_state = BT_CONNECTED;
1566                 l2cap_chan_ready(sk);
1567         } else if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT)) {
1568                 u8 req[64];
1569                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
1570                                         l2cap_build_conf_req(sk, req), req);
1571         }
1572
1573 unlock:
1574         bh_unlock_sock(sk);
1575         return 0;
1576 }
1577
1578 static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1579 {
1580         struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
1581         u16 scid, flags, result;
1582         struct sock *sk;
1583
1584         scid   = __le16_to_cpu(rsp->scid);
1585         flags  = __le16_to_cpu(rsp->flags);
1586         result = __le16_to_cpu(rsp->result);
1587
1588         BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x", scid, flags, result);
1589
1590         if (!(sk = l2cap_get_chan_by_scid(&conn->chan_list, scid)))
1591                 return 0;
1592
1593         switch (result) {
1594         case L2CAP_CONF_SUCCESS:
1595                 break;
1596
1597         case L2CAP_CONF_UNACCEPT:
1598                 if (++l2cap_pi(sk)->conf_retry < L2CAP_CONF_MAX_RETRIES) {
1599                         char req[128];
1600                         /* It does not make sense to adjust L2CAP parameters
1601                          * that are currently defined in the spec. We simply
1602                          * resend config request that we sent earlier. It is
1603                          * stupid, but it helps qualification testing which
1604                          * expects at least some response from us. */
1605                         l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
1606                                                 l2cap_build_conf_req(sk, req), req);
1607                         goto done;
1608                 }
1609
1610         default:
1611                 sk->sk_state = BT_DISCONN;
1612                 sk->sk_err   = ECONNRESET;
1613                 l2cap_sock_set_timer(sk, HZ * 5);
1614                 {
1615                         struct l2cap_disconn_req req;
1616                         req.dcid = __cpu_to_le16(l2cap_pi(sk)->dcid);
1617                         req.scid = __cpu_to_le16(l2cap_pi(sk)->scid);
1618                         l2cap_send_cmd(conn, l2cap_get_ident(conn),
1619                                         L2CAP_DISCONN_REQ, sizeof(req), &req);
1620                 }
1621                 goto done;
1622         }
1623
1624         if (flags & 0x01)
1625                 goto done;
1626
1627         /* Input config done */
1628         l2cap_pi(sk)->conf_state |= L2CAP_CONF_INPUT_DONE;
1629
1630         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE) {
1631                 sk->sk_state = BT_CONNECTED;
1632                 l2cap_chan_ready(sk);
1633         }
1634
1635 done:
1636         bh_unlock_sock(sk);
1637         return 0;
1638 }
1639
1640 static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1641 {
1642         struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
1643         struct l2cap_disconn_rsp rsp;
1644         u16 dcid, scid;
1645         struct sock *sk;
1646
1647         scid = __le16_to_cpu(req->scid);
1648         dcid = __le16_to_cpu(req->dcid);
1649
1650         BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
1651
1652         if (!(sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid)))
1653                 return 0;
1654
1655         rsp.dcid = __cpu_to_le16(l2cap_pi(sk)->scid);
1656         rsp.scid = __cpu_to_le16(l2cap_pi(sk)->dcid);
1657         l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
1658
1659         sk->sk_shutdown = SHUTDOWN_MASK;
1660
1661         l2cap_chan_del(sk, ECONNRESET);
1662         bh_unlock_sock(sk);
1663
1664         l2cap_sock_kill(sk);
1665         return 0;
1666 }
1667
1668 static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1669 {
1670         struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
1671         u16 dcid, scid;
1672         struct sock *sk;
1673
1674         scid = __le16_to_cpu(rsp->scid);
1675         dcid = __le16_to_cpu(rsp->dcid);
1676
1677         BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
1678
1679         if (!(sk = l2cap_get_chan_by_scid(&conn->chan_list, scid)))
1680                 return 0;
1681
1682         l2cap_chan_del(sk, 0);
1683         bh_unlock_sock(sk);
1684
1685         l2cap_sock_kill(sk);
1686         return 0;
1687 }
1688
1689 static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1690 {
1691         struct l2cap_info_req *req = (struct l2cap_info_req *) data;
1692         struct l2cap_info_rsp rsp;
1693         u16 type;
1694
1695         type = __le16_to_cpu(req->type);
1696
1697         BT_DBG("type 0x%4.4x", type);
1698
1699         rsp.type   = __cpu_to_le16(type);
1700         rsp.result = __cpu_to_le16(L2CAP_IR_NOTSUPP);
1701         l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(rsp), &rsp);
1702
1703         return 0;
1704 }
1705
1706 static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1707 {
1708         struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
1709         u16 type, result;
1710
1711         type   = __le16_to_cpu(rsp->type);
1712         result = __le16_to_cpu(rsp->result);
1713
1714         BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
1715
1716         return 0;
1717 }
1718
1719 static inline void l2cap_sig_channel(struct l2cap_conn *conn, struct sk_buff *skb)
1720 {
1721         u8 *data = skb->data;
1722         int len = skb->len;
1723         struct l2cap_cmd_hdr cmd;
1724         int err = 0;
1725
1726         l2cap_raw_recv(conn, skb);
1727
1728         while (len >= L2CAP_CMD_HDR_SIZE) {
1729                 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
1730                 data += L2CAP_CMD_HDR_SIZE;
1731                 len  -= L2CAP_CMD_HDR_SIZE;
1732
1733                 cmd.len = __le16_to_cpu(cmd.len);
1734
1735                 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd.len, cmd.ident);
1736
1737                 if (cmd.len > len || !cmd.ident) {
1738                         BT_DBG("corrupted command");
1739                         break;
1740                 }
1741
1742                 switch (cmd.code) {
1743                 case L2CAP_COMMAND_REJ:
1744                         /* FIXME: We should process this */
1745                         break;
1746
1747                 case L2CAP_CONN_REQ:
1748                         err = l2cap_connect_req(conn, &cmd, data);
1749                         break;
1750
1751                 case L2CAP_CONN_RSP:
1752                         err = l2cap_connect_rsp(conn, &cmd, data);
1753                         break;
1754
1755                 case L2CAP_CONF_REQ:
1756                         err = l2cap_config_req(conn, &cmd, data);
1757                         break;
1758
1759                 case L2CAP_CONF_RSP:
1760                         err = l2cap_config_rsp(conn, &cmd, data);
1761                         break;
1762
1763                 case L2CAP_DISCONN_REQ:
1764                         err = l2cap_disconnect_req(conn, &cmd, data);
1765                         break;
1766
1767                 case L2CAP_DISCONN_RSP:
1768                         err = l2cap_disconnect_rsp(conn, &cmd, data);
1769                         break;
1770
1771                 case L2CAP_ECHO_REQ:
1772                         l2cap_send_cmd(conn, cmd.ident, L2CAP_ECHO_RSP, cmd.len, data);
1773                         break;
1774
1775                 case L2CAP_ECHO_RSP:
1776                         break;
1777
1778                 case L2CAP_INFO_REQ:
1779                         err = l2cap_information_req(conn, &cmd, data);
1780                         break;
1781
1782                 case L2CAP_INFO_RSP:
1783                         err = l2cap_information_rsp(conn, &cmd, data);
1784                         break;
1785
1786                 default:
1787                         BT_ERR("Unknown signaling command 0x%2.2x", cmd.code);
1788                         err = -EINVAL;
1789                         break;
1790                 }
1791
1792                 if (err) {
1793                         struct l2cap_cmd_rej rej;
1794                         BT_DBG("error %d", err);
1795
1796                         /* FIXME: Map err to a valid reason */
1797                         rej.reason = __cpu_to_le16(0);
1798                         l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
1799                 }
1800
1801                 data += cmd.len;
1802                 len  -= cmd.len;
1803         }
1804
1805         kfree_skb(skb);
1806 }
1807
1808 static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
1809 {
1810         struct sock *sk;
1811
1812         sk = l2cap_get_chan_by_scid(&conn->chan_list, cid);
1813         if (!sk) {
1814                 BT_DBG("unknown cid 0x%4.4x", cid);
1815                 goto drop;
1816         }
1817
1818         BT_DBG("sk %p, len %d", sk, skb->len);
1819
1820         if (sk->sk_state != BT_CONNECTED)
1821                 goto drop;
1822
1823         if (l2cap_pi(sk)->imtu < skb->len)
1824                 goto drop;
1825
1826         /* If socket recv buffers overflows we drop data here
1827          * which is *bad* because L2CAP has to be reliable.
1828          * But we don't have any other choice. L2CAP doesn't
1829          * provide flow control mechanism. */
1830
1831         if (!sock_queue_rcv_skb(sk, skb))
1832                 goto done;
1833
1834 drop:
1835         kfree_skb(skb);
1836
1837 done:
1838         if (sk) bh_unlock_sock(sk);
1839         return 0;
1840 }
1841
1842 static inline int l2cap_conless_channel(struct l2cap_conn *conn, u16 psm, struct sk_buff *skb)
1843 {
1844         struct sock *sk;
1845
1846         sk = l2cap_get_sock_by_psm(0, psm, conn->src);
1847         if (!sk)
1848                 goto drop;
1849
1850         BT_DBG("sk %p, len %d", sk, skb->len);
1851
1852         if (sk->sk_state != BT_BOUND && sk->sk_state != BT_CONNECTED)
1853                 goto drop;
1854
1855         if (l2cap_pi(sk)->imtu < skb->len)
1856                 goto drop;
1857
1858         if (!sock_queue_rcv_skb(sk, skb))
1859                 goto done;
1860
1861 drop:
1862         kfree_skb(skb);
1863
1864 done:
1865         if (sk) bh_unlock_sock(sk);
1866         return 0;
1867 }
1868
1869 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
1870 {
1871         struct l2cap_hdr *lh = (void *) skb->data;
1872         u16 cid, psm, len;
1873
1874         skb_pull(skb, L2CAP_HDR_SIZE);
1875         cid = __le16_to_cpu(lh->cid);
1876         len = __le16_to_cpu(lh->len);
1877
1878         BT_DBG("len %d, cid 0x%4.4x", len, cid);
1879
1880         switch (cid) {
1881         case 0x0001:
1882                 l2cap_sig_channel(conn, skb);
1883                 break;
1884
1885         case 0x0002:
1886                 psm = get_unaligned((u16 *) skb->data);
1887                 skb_pull(skb, 2);
1888                 l2cap_conless_channel(conn, psm, skb);
1889                 break;
1890
1891         default:
1892                 l2cap_data_channel(conn, cid, skb);
1893                 break;
1894         }
1895 }
1896
1897 /* ---- L2CAP interface with lower layer (HCI) ---- */
1898
1899 static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
1900 {
1901         int exact = 0, lm1 = 0, lm2 = 0;
1902         register struct sock *sk;
1903         struct hlist_node *node;
1904
1905         if (type != ACL_LINK)
1906                 return 0;
1907
1908         BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
1909
1910         /* Find listening sockets and check their link_mode */
1911         read_lock(&l2cap_sk_list.lock);
1912         sk_for_each(sk, node, &l2cap_sk_list.head) {
1913                 if (sk->sk_state != BT_LISTEN)
1914                         continue;
1915
1916                 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
1917                         lm1 |= (HCI_LM_ACCEPT | l2cap_pi(sk)->link_mode);
1918                         exact++;
1919                 } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
1920                         lm2 |= (HCI_LM_ACCEPT | l2cap_pi(sk)->link_mode);
1921         }
1922         read_unlock(&l2cap_sk_list.lock);
1923
1924         return exact ? lm1 : lm2;
1925 }
1926
1927 static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
1928 {
1929         BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
1930
1931         if (hcon->type != ACL_LINK)
1932                 return 0;
1933
1934         if (!status) {
1935                 struct l2cap_conn *conn;
1936
1937                 conn = l2cap_conn_add(hcon, status);
1938                 if (conn)
1939                         l2cap_conn_ready(conn);
1940         } else
1941                 l2cap_conn_del(hcon, bt_err(status));
1942
1943         return 0;
1944 }
1945
1946 static int l2cap_disconn_ind(struct hci_conn *hcon, u8 reason)
1947 {
1948         BT_DBG("hcon %p reason %d", hcon, reason);
1949
1950         if (hcon->type != ACL_LINK)
1951                 return 0;
1952
1953         l2cap_conn_del(hcon, bt_err(reason));
1954         return 0;
1955 }
1956
1957 static int l2cap_auth_cfm(struct hci_conn *hcon, u8 status)
1958 {
1959         struct l2cap_chan_list *l;
1960         struct l2cap_conn *conn;
1961         struct l2cap_conn_rsp rsp;
1962         struct sock *sk;
1963         int result;
1964
1965         if (!(conn = hcon->l2cap_data))
1966                 return 0;
1967         l = &conn->chan_list;
1968
1969         BT_DBG("conn %p", conn);
1970
1971         read_lock(&l->lock);
1972
1973         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
1974                 bh_lock_sock(sk);
1975
1976                 if (sk->sk_state != BT_CONNECT2 ||
1977                                 (l2cap_pi(sk)->link_mode & L2CAP_LM_ENCRYPT) ||
1978                                 (l2cap_pi(sk)->link_mode & L2CAP_LM_SECURE)) {
1979                         bh_unlock_sock(sk);
1980                         continue;
1981                 }
1982
1983                 if (!status) {
1984                         sk->sk_state = BT_CONFIG;
1985                         result = 0;
1986                 } else {
1987                         sk->sk_state = BT_DISCONN;
1988                         l2cap_sock_set_timer(sk, HZ/10);
1989                         result = L2CAP_CR_SEC_BLOCK;
1990                 }
1991
1992                 rsp.scid   = __cpu_to_le16(l2cap_pi(sk)->dcid);
1993                 rsp.dcid   = __cpu_to_le16(l2cap_pi(sk)->scid);
1994                 rsp.result = __cpu_to_le16(result);
1995                 rsp.status = __cpu_to_le16(0);
1996                 l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
1997                                 L2CAP_CONN_RSP, sizeof(rsp), &rsp);
1998
1999                 bh_unlock_sock(sk);
2000         }
2001
2002         read_unlock(&l->lock);
2003         return 0;
2004 }
2005
2006 static int l2cap_encrypt_cfm(struct hci_conn *hcon, u8 status)
2007 {
2008         struct l2cap_chan_list *l;
2009         struct l2cap_conn *conn;
2010         struct l2cap_conn_rsp rsp;
2011         struct sock *sk;
2012         int result;
2013
2014         if (!(conn = hcon->l2cap_data))
2015                 return 0;
2016         l = &conn->chan_list;
2017
2018         BT_DBG("conn %p", conn);
2019
2020         read_lock(&l->lock);
2021
2022         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
2023                 bh_lock_sock(sk);
2024
2025                 if (sk->sk_state != BT_CONNECT2) {
2026                         bh_unlock_sock(sk);
2027                         continue;
2028                 }
2029
2030                 if (!status) {
2031                         sk->sk_state = BT_CONFIG;
2032                         result = 0;
2033                 } else {
2034                         sk->sk_state = BT_DISCONN;
2035                         l2cap_sock_set_timer(sk, HZ/10);
2036                         result = L2CAP_CR_SEC_BLOCK;
2037                 }
2038
2039                 rsp.scid   = __cpu_to_le16(l2cap_pi(sk)->dcid);
2040                 rsp.dcid   = __cpu_to_le16(l2cap_pi(sk)->scid);
2041                 rsp.result = __cpu_to_le16(result);
2042                 rsp.status = __cpu_to_le16(0);
2043                 l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
2044                                 L2CAP_CONN_RSP, sizeof(rsp), &rsp);
2045
2046                 if (l2cap_pi(sk)->link_mode & L2CAP_LM_SECURE)
2047                         hci_conn_change_link_key(hcon);
2048
2049                 bh_unlock_sock(sk);
2050         }
2051
2052         read_unlock(&l->lock);
2053         return 0;
2054 }
2055
2056 static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
2057 {
2058         struct l2cap_conn *conn = hcon->l2cap_data;
2059
2060         if (!conn && !(conn = l2cap_conn_add(hcon, 0)))
2061                 goto drop;
2062
2063         BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
2064
2065         if (flags & ACL_START) {
2066                 struct l2cap_hdr *hdr;
2067                 int len;
2068
2069                 if (conn->rx_len) {
2070                         BT_ERR("Unexpected start frame (len %d)", skb->len);
2071                         kfree_skb(conn->rx_skb);
2072                         conn->rx_skb = NULL;
2073                         conn->rx_len = 0;
2074                         l2cap_conn_unreliable(conn, ECOMM);
2075                 }
2076
2077                 if (skb->len < 2) {
2078                         BT_ERR("Frame is too short (len %d)", skb->len);
2079                         l2cap_conn_unreliable(conn, ECOMM);
2080                         goto drop;
2081                 }
2082
2083                 hdr = (struct l2cap_hdr *) skb->data;
2084                 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
2085
2086                 if (len == skb->len) {
2087                         /* Complete frame received */
2088                         l2cap_recv_frame(conn, skb);
2089                         return 0;
2090                 }
2091
2092                 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
2093
2094                 if (skb->len > len) {
2095                         BT_ERR("Frame is too long (len %d, expected len %d)",
2096                                 skb->len, len);
2097                         l2cap_conn_unreliable(conn, ECOMM);
2098                         goto drop;
2099                 }
2100
2101                 /* Allocate skb for the complete frame (with header) */
2102                 if (!(conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC)))
2103                         goto drop;
2104
2105                 memcpy(skb_put(conn->rx_skb, skb->len), skb->data, skb->len);
2106                 conn->rx_len = len - skb->len;
2107         } else {
2108                 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
2109
2110                 if (!conn->rx_len) {
2111                         BT_ERR("Unexpected continuation frame (len %d)", skb->len);
2112                         l2cap_conn_unreliable(conn, ECOMM);
2113                         goto drop;
2114                 }
2115
2116                 if (skb->len > conn->rx_len) {
2117                         BT_ERR("Fragment is too long (len %d, expected %d)",
2118                                         skb->len, conn->rx_len);
2119                         kfree_skb(conn->rx_skb);
2120                         conn->rx_skb = NULL;
2121                         conn->rx_len = 0;
2122                         l2cap_conn_unreliable(conn, ECOMM);
2123                         goto drop;
2124                 }
2125
2126                 memcpy(skb_put(conn->rx_skb, skb->len), skb->data, skb->len);
2127                 conn->rx_len -= skb->len;
2128
2129                 if (!conn->rx_len) {
2130                         /* Complete frame received */
2131                         l2cap_recv_frame(conn, conn->rx_skb);
2132                         conn->rx_skb = NULL;
2133                 }
2134         }
2135
2136 drop:
2137         kfree_skb(skb);
2138         return 0;
2139 }
2140
2141 static ssize_t l2cap_sysfs_show(struct class *dev, char *buf)
2142 {
2143         struct sock *sk;
2144         struct hlist_node *node;
2145         char *str = buf;
2146
2147         read_lock_bh(&l2cap_sk_list.lock);
2148
2149         sk_for_each(sk, node, &l2cap_sk_list.head) {
2150                 struct l2cap_pinfo *pi = l2cap_pi(sk);
2151
2152                 str += sprintf(str, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d 0x%x\n",
2153                                 batostr(&bt_sk(sk)->src), batostr(&bt_sk(sk)->dst),
2154                                 sk->sk_state, pi->psm, pi->scid, pi->dcid, pi->imtu,
2155                                 pi->omtu, pi->link_mode);
2156         }
2157
2158         read_unlock_bh(&l2cap_sk_list.lock);
2159
2160         return (str - buf);
2161 }
2162
2163 static CLASS_ATTR(l2cap, S_IRUGO, l2cap_sysfs_show, NULL);
2164
2165 static const struct proto_ops l2cap_sock_ops = {
2166         .family         = PF_BLUETOOTH,
2167         .owner          = THIS_MODULE,
2168         .release        = l2cap_sock_release,
2169         .bind           = l2cap_sock_bind,
2170         .connect        = l2cap_sock_connect,
2171         .listen         = l2cap_sock_listen,
2172         .accept         = l2cap_sock_accept,
2173         .getname        = l2cap_sock_getname,
2174         .sendmsg        = l2cap_sock_sendmsg,
2175         .recvmsg        = bt_sock_recvmsg,
2176         .poll           = bt_sock_poll,
2177         .mmap           = sock_no_mmap,
2178         .socketpair     = sock_no_socketpair,
2179         .ioctl          = sock_no_ioctl,
2180         .shutdown       = l2cap_sock_shutdown,
2181         .setsockopt     = l2cap_sock_setsockopt,
2182         .getsockopt     = l2cap_sock_getsockopt
2183 };
2184
2185 static struct net_proto_family l2cap_sock_family_ops = {
2186         .family = PF_BLUETOOTH,
2187         .owner  = THIS_MODULE,
2188         .create = l2cap_sock_create,
2189 };
2190
2191 static struct hci_proto l2cap_hci_proto = {
2192         .name           = "L2CAP",
2193         .id             = HCI_PROTO_L2CAP,
2194         .connect_ind    = l2cap_connect_ind,
2195         .connect_cfm    = l2cap_connect_cfm,
2196         .disconn_ind    = l2cap_disconn_ind,
2197         .auth_cfm       = l2cap_auth_cfm,
2198         .encrypt_cfm    = l2cap_encrypt_cfm,
2199         .recv_acldata   = l2cap_recv_acldata
2200 };
2201
2202 static int __init l2cap_init(void)
2203 {
2204         int err;
2205
2206         err = proto_register(&l2cap_proto, 0);
2207         if (err < 0)
2208                 return err;
2209
2210         err = bt_sock_register(BTPROTO_L2CAP, &l2cap_sock_family_ops);
2211         if (err < 0) {
2212                 BT_ERR("L2CAP socket registration failed");
2213                 goto error;
2214         }
2215
2216         err = hci_register_proto(&l2cap_hci_proto);
2217         if (err < 0) {
2218                 BT_ERR("L2CAP protocol registration failed");
2219                 bt_sock_unregister(BTPROTO_L2CAP);
2220                 goto error;
2221         }
2222
2223         class_create_file(&bt_class, &class_attr_l2cap);
2224
2225         BT_INFO("L2CAP ver %s", VERSION);
2226         BT_INFO("L2CAP socket layer initialized");
2227
2228         return 0;
2229
2230 error:
2231         proto_unregister(&l2cap_proto);
2232         return err;
2233 }
2234
2235 static void __exit l2cap_exit(void)
2236 {
2237         class_remove_file(&bt_class, &class_attr_l2cap);
2238
2239         if (bt_sock_unregister(BTPROTO_L2CAP) < 0)
2240                 BT_ERR("L2CAP socket unregistration failed");
2241
2242         if (hci_unregister_proto(&l2cap_hci_proto) < 0)
2243                 BT_ERR("L2CAP protocol unregistration failed");
2244
2245         proto_unregister(&l2cap_proto);
2246 }
2247
2248 void l2cap_load(void)
2249 {
2250         /* Dummy function to trigger automatic L2CAP module loading by
2251          * other modules that use L2CAP sockets but don't use any other
2252          * symbols from it. */
2253         return;
2254 }
2255 EXPORT_SYMBOL(l2cap_load);
2256
2257 module_init(l2cap_init);
2258 module_exit(l2cap_exit);
2259
2260 MODULE_AUTHOR("Maxim Krasnyansky <maxk@qualcomm.com>, Marcel Holtmann <marcel@holtmann.org>");
2261 MODULE_DESCRIPTION("Bluetooth L2CAP ver " VERSION);
2262 MODULE_VERSION(VERSION);
2263 MODULE_LICENSE("GPL");
2264 MODULE_ALIAS("bt-proto-0");