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