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