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