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 <linux/uaccess.h>
  44 #include <linux/crc16.h>
  45 #include <net/sock.h>
  46
  47 #include <asm/system.h>
  48 #include <asm/unaligned.h>
  49
  50 #include <net/bluetooth/bluetooth.h>
  51 #include <net/bluetooth/hci_core.h>
  52 #include <net/bluetooth/l2cap.h>
  53
  54 #define VERSION "2.14"
  55
  56 static int enable_ertm = 0;
  57
  58 static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN;
  59 static u8 l2cap_fixed_chan[8] = { 0x02, };
  60
  61 static const struct proto_ops l2cap_sock_ops;
  62
  63 static struct bt_sock_list l2cap_sk_list = {
  64         .lock = __RW_LOCK_UNLOCKED(l2cap_sk_list.lock)
  65 };
  66
  67 static void __l2cap_sock_close(struct sock *sk, int reason);
  68 static void l2cap_sock_close(struct sock *sk);
  69 static void l2cap_sock_kill(struct sock *sk);
  70
  71 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
  72                                 u8 code, u8 ident, u16 dlen, void *data);
  73
  74 /* ---- L2CAP timers ---- */
  75 static void l2cap_sock_timeout(unsigned long arg)
  76 {
  77         struct sock *sk = (struct sock *) arg;
  78         int reason;
  79
  80         BT_DBG("sock %p state %d", sk, sk->sk_state);
  81
  82         bh_lock_sock(sk);
  83
  84         if (sk->sk_state == BT_CONNECTED || sk->sk_state == BT_CONFIG)
  85                 reason = ECONNREFUSED;
  86         else if (sk->sk_state == BT_CONNECT &&
  87                                 l2cap_pi(sk)->sec_level != BT_SECURITY_SDP)
  88                 reason = ECONNREFUSED;
  89         else
  90                 reason = ETIMEDOUT;
  91
  92         __l2cap_sock_close(sk, reason);
  93
  94         bh_unlock_sock(sk);
  95
  96         l2cap_sock_kill(sk);
  97         sock_put(sk);
  98 }
  99
 100 static void l2cap_sock_set_timer(struct sock *sk, long timeout)
 101 {
 102         BT_DBG("sk %p state %d timeout %ld", sk, sk->sk_state, timeout);
 103         sk_reset_timer(sk, &sk->sk_timer, jiffies + timeout);
 104 }
 105
 106 static void l2cap_sock_clear_timer(struct sock *sk)
 107 {
 108         BT_DBG("sock %p state %d", sk, sk->sk_state);
 109         sk_stop_timer(sk, &sk->sk_timer);
 110 }
 111
 112 /* ---- L2CAP channels ---- */
 113 static struct sock *__l2cap_get_chan_by_dcid(struct l2cap_chan_list *l, u16 cid)
 114 {
 115         struct sock *s;
 116         for (s = l->head; s; s = l2cap_pi(s)->next_c) {
 117                 if (l2cap_pi(s)->dcid == cid)
 118                         break;
 119         }
 120         return s;
 121 }
 122
 123 static struct sock *__l2cap_get_chan_by_scid(struct l2cap_chan_list *l, u16 cid)
 124 {
 125         struct sock *s;
 126         for (s = l->head; s; s = l2cap_pi(s)->next_c) {
 127                 if (l2cap_pi(s)->scid == cid)
 128                         break;
 129         }
 130         return s;
 131 }
 132
 133 /* Find channel with given SCID.
 134  * Returns locked socket */
 135 static inline struct sock *l2cap_get_chan_by_scid(struct l2cap_chan_list *l, u16 cid)
 136 {
 137         struct sock *s;
 138         read_lock(&l->lock);
 139         s = __l2cap_get_chan_by_scid(l, cid);
 140         if (s)
 141                 bh_lock_sock(s);
 142         read_unlock(&l->lock);
 143         return s;
 144 }
 145
 146 static struct sock *__l2cap_get_chan_by_ident(struct l2cap_chan_list *l, u8 ident)
 147 {
 148         struct sock *s;
 149         for (s = l->head; s; s = l2cap_pi(s)->next_c) {
 150                 if (l2cap_pi(s)->ident == ident)
 151                         break;
 152         }
 153         return s;
 154 }
 155
 156 static inline struct sock *l2cap_get_chan_by_ident(struct l2cap_chan_list *l, u8 ident)
 157 {
 158         struct sock *s;
 159         read_lock(&l->lock);
 160         s = __l2cap_get_chan_by_ident(l, ident);
 161         if (s)
 162                 bh_lock_sock(s);
 163         read_unlock(&l->lock);
 164         return s;
 165 }
 166
 167 static u16 l2cap_alloc_cid(struct l2cap_chan_list *l)
 168 {
 169         u16 cid = L2CAP_CID_DYN_START;
 170
 171         for (; cid < L2CAP_CID_DYN_END; cid++) {
 172                 if (!__l2cap_get_chan_by_scid(l, cid))
 173                         return cid;
 174         }
 175
 176         return 0;
 177 }
 178
 179 static inline void __l2cap_chan_link(struct l2cap_chan_list *l, struct sock *sk)
 180 {
 181         sock_hold(sk);
 182
 183         if (l->head)
 184                 l2cap_pi(l->head)->prev_c = sk;
 185
 186         l2cap_pi(sk)->next_c = l->head;
 187         l2cap_pi(sk)->prev_c = NULL;
 188         l->head = sk;
 189 }
 190
 191 static inline void l2cap_chan_unlink(struct l2cap_chan_list *l, struct sock *sk)
 192 {
 193         struct sock *next = l2cap_pi(sk)->next_c, *prev = l2cap_pi(sk)->prev_c;
 194
 195         write_lock_bh(&l->lock);
 196         if (sk == l->head)
 197                 l->head = next;
 198
 199         if (next)
 200                 l2cap_pi(next)->prev_c = prev;
 201         if (prev)
 202                 l2cap_pi(prev)->next_c = next;
 203         write_unlock_bh(&l->lock);
 204
 205         __sock_put(sk);
 206 }
 207
 208 static void __l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct sock *parent)
 209 {
 210         struct l2cap_chan_list *l = &conn->chan_list;
 211
 212         BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn,
 213                         l2cap_pi(sk)->psm, l2cap_pi(sk)->dcid);
 214
 215         conn->disc_reason = 0x13;
 216
 217         l2cap_pi(sk)->conn = conn;
 218
 219         if (sk->sk_type == SOCK_SEQPACKET) {
 220                 /* Alloc CID for connection-oriented socket */
 221                 l2cap_pi(sk)->scid = l2cap_alloc_cid(l);
 222         } else if (sk->sk_type == SOCK_DGRAM) {
 223                 /* Connectionless socket */
 224                 l2cap_pi(sk)->scid = L2CAP_CID_CONN_LESS;
 225                 l2cap_pi(sk)->dcid = L2CAP_CID_CONN_LESS;
 226                 l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU;
 227         } else {
 228                 /* Raw socket can send/recv signalling messages only */
 229                 l2cap_pi(sk)->scid = L2CAP_CID_SIGNALING;
 230                 l2cap_pi(sk)->dcid = L2CAP_CID_SIGNALING;
 231                 l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU;
 232         }
 233
 234         __l2cap_chan_link(l, sk);
 235
 236         if (parent)
 237                 bt_accept_enqueue(parent, sk);
 238 }
 239
 240 /* Delete channel.
 241  * Must be called on the locked socket. */
 242 static void l2cap_chan_del(struct sock *sk, int err)
 243 {
 244         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
 245         struct sock *parent = bt_sk(sk)->parent;
 246
 247         l2cap_sock_clear_timer(sk);
 248
 249         BT_DBG("sk %p, conn %p, err %d", sk, conn, err);
 250
 251         if (conn) {
 252                 /* Unlink from channel list */
 253                 l2cap_chan_unlink(&conn->chan_list, sk);
 254                 l2cap_pi(sk)->conn = NULL;
 255                 hci_conn_put(conn->hcon);
 256         }
 257
 258         sk->sk_state = BT_CLOSED;
 259         sock_set_flag(sk, SOCK_ZAPPED);
 260
 261         if (err)
 262                 sk->sk_err = err;
 263
 264         if (parent) {
 265                 bt_accept_unlink(sk);
 266                 parent->sk_data_ready(parent, 0);
 267         } else
 268                 sk->sk_state_change(sk);
 269 }
 270
 271 /* Service level security */
 272 static inline int l2cap_check_security(struct sock *sk)
 273 {
 274         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
 275         __u8 auth_type;
 276
 277         if (l2cap_pi(sk)->psm == cpu_to_le16(0x0001)) {
 278                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_HIGH)
 279                         auth_type = HCI_AT_NO_BONDING_MITM;
 280                 else
 281                         auth_type = HCI_AT_NO_BONDING;
 282
 283                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_LOW)
 284                         l2cap_pi(sk)->sec_level = BT_SECURITY_SDP;
 285         } else {
 286                 switch (l2cap_pi(sk)->sec_level) {
 287                 case BT_SECURITY_HIGH:
 288                         auth_type = HCI_AT_GENERAL_BONDING_MITM;
 289                         break;
 290                 case BT_SECURITY_MEDIUM:
 291                         auth_type = HCI_AT_GENERAL_BONDING;
 292                         break;
 293                 default:
 294                         auth_type = HCI_AT_NO_BONDING;
 295                         break;
 296                 }
 297         }
 298
 299         return hci_conn_security(conn->hcon, l2cap_pi(sk)->sec_level,
 300                                                                 auth_type);
 301 }
 302
 303 static inline u8 l2cap_get_ident(struct l2cap_conn *conn)
 304 {
 305         u8 id;
 306
 307         /* Get next available identificator.
 308          *    1 - 128 are used by kernel.
 309          *  129 - 199 are reserved.
 310          *  200 - 254 are used by utilities like l2ping, etc.
 311          */
 312
 313         spin_lock_bh(&conn->lock);
 314
 315         if (++conn->tx_ident > 128)
 316                 conn->tx_ident = 1;
 317
 318         id = conn->tx_ident;
 319
 320         spin_unlock_bh(&conn->lock);
 321
 322         return id;
 323 }
 324
 325 static inline int l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, void *data)
 326 {
 327         struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data);
 328
 329         BT_DBG("code 0x%2.2x", code);
 330
 331         if (!skb)
 332                 return -ENOMEM;
 333
 334         return hci_send_acl(conn->hcon, skb, 0);
 335 }
 336
 337 static inline int l2cap_send_sframe(struct l2cap_pinfo *pi, u16 control)
 338 {
 339         struct sk_buff *skb;
 340         struct l2cap_hdr *lh;
 341         struct l2cap_conn *conn = pi->conn;
 342         int count, hlen = L2CAP_HDR_SIZE + 2;
 343
 344         if (pi->fcs == L2CAP_FCS_CRC16)
 345                 hlen += 2;
 346
 347         BT_DBG("pi %p, control 0x%2.2x", pi, control);
 348
 349         count = min_t(unsigned int, conn->mtu, hlen);
 350         control |= L2CAP_CTRL_FRAME_TYPE;
 351
 352         skb = bt_skb_alloc(count, GFP_ATOMIC);
 353         if (!skb)
 354                 return -ENOMEM;
 355
 356         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
 357         lh->len = cpu_to_le16(hlen - L2CAP_HDR_SIZE);
 358         lh->cid = cpu_to_le16(pi->dcid);
 359         put_unaligned_le16(control, skb_put(skb, 2));
 360
 361         if (pi->fcs == L2CAP_FCS_CRC16) {
 362                 u16 fcs = crc16(0, (u8 *)lh, count - 2);
 363                 put_unaligned_le16(fcs, skb_put(skb, 2));
 364         }
 365
 366         return hci_send_acl(pi->conn->hcon, skb, 0);
 367 }
 368
 369 static void l2cap_do_start(struct sock *sk)
 370 {
 371         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
 372
 373         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) {
 374                 if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE))
 375                         return;
 376
 377                 if (l2cap_check_security(sk)) {
 378                         struct l2cap_conn_req req;
 379                         req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
 380                         req.psm  = l2cap_pi(sk)->psm;
 381
 382                         l2cap_pi(sk)->ident = l2cap_get_ident(conn);
 383
 384                         l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
 385                                         L2CAP_CONN_REQ, sizeof(req), &req);
 386                 }
 387         } else {
 388                 struct l2cap_info_req req;
 389                 req.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
 390
 391                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
 392                 conn->info_ident = l2cap_get_ident(conn);
 393
 394                 mod_timer(&conn->info_timer, jiffies +
 395                                         msecs_to_jiffies(L2CAP_INFO_TIMEOUT));
 396
 397                 l2cap_send_cmd(conn, conn->info_ident,
 398                                         L2CAP_INFO_REQ, sizeof(req), &req);
 399         }
 400 }
 401
 402 static void l2cap_send_disconn_req(struct l2cap_conn *conn, struct sock *sk)
 403 {
 404         struct l2cap_disconn_req req;
 405
 406         req.dcid = cpu_to_le16(l2cap_pi(sk)->dcid);
 407         req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
 408         l2cap_send_cmd(conn, l2cap_get_ident(conn),
 409                         L2CAP_DISCONN_REQ, sizeof(req), &req);
 410 }
 411
 412 /* ---- L2CAP connections ---- */
 413 static void l2cap_conn_start(struct l2cap_conn *conn)
 414 {
 415         struct l2cap_chan_list *l = &conn->chan_list;
 416         struct sock *sk;
 417
 418         BT_DBG("conn %p", conn);
 419
 420         read_lock(&l->lock);
 421
 422         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
 423                 bh_lock_sock(sk);
 424
 425                 if (sk->sk_type != SOCK_SEQPACKET) {
 426                         bh_unlock_sock(sk);
 427                         continue;
 428                 }
 429
 430                 if (sk->sk_state == BT_CONNECT) {
 431                         if (l2cap_check_security(sk)) {
 432                                 struct l2cap_conn_req req;
 433                                 req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
 434                                 req.psm  = l2cap_pi(sk)->psm;
 435
 436                                 l2cap_pi(sk)->ident = l2cap_get_ident(conn);
 437
 438                                 l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
 439                                         L2CAP_CONN_REQ, sizeof(req), &req);
 440                         }
 441                 } else if (sk->sk_state == BT_CONNECT2) {
 442                         struct l2cap_conn_rsp rsp;
 443                         rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid);
 444                         rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid);
 445
 446                         if (l2cap_check_security(sk)) {
 447                                 if (bt_sk(sk)->defer_setup) {
 448                                         struct sock *parent = bt_sk(sk)->parent;
 449                                         rsp.result = cpu_to_le16(L2CAP_CR_PEND);
 450                                         rsp.status = cpu_to_le16(L2CAP_CS_AUTHOR_PEND);
 451                                         parent->sk_data_ready(parent, 0);
 452
 453                                 } else {
 454                                         sk->sk_state = BT_CONFIG;
 455                                         rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
 456                                         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
 457                                 }
 458                         } else {
 459                                 rsp.result = cpu_to_le16(L2CAP_CR_PEND);
 460                                 rsp.status = cpu_to_le16(L2CAP_CS_AUTHEN_PEND);
 461                         }
 462
 463                         l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
 464                                         L2CAP_CONN_RSP, sizeof(rsp), &rsp);
 465                 }
 466
 467                 bh_unlock_sock(sk);
 468         }
 469
 470         read_unlock(&l->lock);
 471 }
 472
 473 static void l2cap_conn_ready(struct l2cap_conn *conn)
 474 {
 475         struct l2cap_chan_list *l = &conn->chan_list;
 476         struct sock *sk;
 477
 478         BT_DBG("conn %p", conn);
 479
 480         read_lock(&l->lock);
 481
 482         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
 483                 bh_lock_sock(sk);
 484
 485                 if (sk->sk_type != SOCK_SEQPACKET) {
 486                         l2cap_sock_clear_timer(sk);
 487                         sk->sk_state = BT_CONNECTED;
 488                         sk->sk_state_change(sk);
 489                 } else if (sk->sk_state == BT_CONNECT)
 490                         l2cap_do_start(sk);
 491
 492                 bh_unlock_sock(sk);
 493         }
 494
 495         read_unlock(&l->lock);
 496 }
 497
 498 /* Notify sockets that we cannot guaranty reliability anymore */
 499 static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err)
 500 {
 501         struct l2cap_chan_list *l = &conn->chan_list;
 502         struct sock *sk;
 503
 504         BT_DBG("conn %p", conn);
 505
 506         read_lock(&l->lock);
 507
 508         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
 509                 if (l2cap_pi(sk)->force_reliable)
 510                         sk->sk_err = err;
 511         }
 512
 513         read_unlock(&l->lock);
 514 }
 515
 516 static void l2cap_info_timeout(unsigned long arg)
 517 {
 518         struct l2cap_conn *conn = (void *) arg;
 519
 520         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
 521         conn->info_ident = 0;
 522
 523         l2cap_conn_start(conn);
 524 }
 525
 526 static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status)
 527 {
 528         struct l2cap_conn *conn = hcon->l2cap_data;
 529
 530         if (conn || status)
 531                 return conn;
 532
 533         conn = kzalloc(sizeof(struct l2cap_conn), GFP_ATOMIC);
 534         if (!conn)
 535                 return NULL;
 536
 537         hcon->l2cap_data = conn;
 538         conn->hcon = hcon;
 539
 540         BT_DBG("hcon %p conn %p", hcon, conn);
 541
 542         conn->mtu = hcon->hdev->acl_mtu;
 543         conn->src = &hcon->hdev->bdaddr;
 544         conn->dst = &hcon->dst;
 545
 546         conn->feat_mask = 0;
 547
 548         setup_timer(&conn->info_timer, l2cap_info_timeout,
 549                                                 (unsigned long) conn);
 550
 551         spin_lock_init(&conn->lock);
 552         rwlock_init(&conn->chan_list.lock);
 553
 554         conn->disc_reason = 0x13;
 555
 556         return conn;
 557 }
 558
 559 static void l2cap_conn_del(struct hci_conn *hcon, int err)
 560 {
 561         struct l2cap_conn *conn = hcon->l2cap_data;
 562         struct sock *sk;
 563
 564         if (!conn)
 565                 return;
 566
 567         BT_DBG("hcon %p conn %p, err %d", hcon, conn, err);
 568
 569         kfree_skb(conn->rx_skb);
 570
 571         /* Kill channels */
 572         while ((sk = conn->chan_list.head)) {
 573                 bh_lock_sock(sk);
 574                 l2cap_chan_del(sk, err);
 575                 bh_unlock_sock(sk);
 576                 l2cap_sock_kill(sk);
 577         }
 578
 579         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT)
 580                 del_timer_sync(&conn->info_timer);
 581
 582         hcon->l2cap_data = NULL;
 583         kfree(conn);
 584 }
 585
 586 static inline void l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct sock *parent)
 587 {
 588         struct l2cap_chan_list *l = &conn->chan_list;
 589         write_lock_bh(&l->lock);
 590         __l2cap_chan_add(conn, sk, parent);
 591         write_unlock_bh(&l->lock);
 592 }
 593
 594 /* ---- Socket interface ---- */
 595 static struct sock *__l2cap_get_sock_by_addr(__le16 psm, bdaddr_t *src)
 596 {
 597         struct sock *sk;
 598         struct hlist_node *node;
 599         sk_for_each(sk, node, &l2cap_sk_list.head)
 600                 if (l2cap_pi(sk)->sport == psm && !bacmp(&bt_sk(sk)->src, src))
 601                         goto found;
 602         sk = NULL;
 603 found:
 604         return sk;
 605 }
 606
 607 /* Find socket with psm and source bdaddr.
 608  * Returns closest match.
 609  */
 610 static struct sock *__l2cap_get_sock_by_psm(int state, __le16 psm, bdaddr_t *src)
 611 {
 612         struct sock *sk = NULL, *sk1 = NULL;
 613         struct hlist_node *node;
 614
 615         sk_for_each(sk, node, &l2cap_sk_list.head) {
 616                 if (state && sk->sk_state != state)
 617                         continue;
 618
 619                 if (l2cap_pi(sk)->psm == psm) {
 620                         /* Exact match. */
 621                         if (!bacmp(&bt_sk(sk)->src, src))
 622                                 break;
 623
 624                         /* Closest match */
 625                         if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
 626                                 sk1 = sk;
 627                 }
 628         }
 629         return node ? sk : sk1;
 630 }
 631
 632 /* Find socket with given address (psm, src).
 633  * Returns locked socket */
 634 static inline struct sock *l2cap_get_sock_by_psm(int state, __le16 psm, bdaddr_t *src)
 635 {
 636         struct sock *s;
 637         read_lock(&l2cap_sk_list.lock);
 638         s = __l2cap_get_sock_by_psm(state, psm, src);
 639         if (s)
 640                 bh_lock_sock(s);
 641         read_unlock(&l2cap_sk_list.lock);
 642         return s;
 643 }
 644
 645 static void l2cap_sock_destruct(struct sock *sk)
 646 {
 647         BT_DBG("sk %p", sk);
 648
 649         skb_queue_purge(&sk->sk_receive_queue);
 650         skb_queue_purge(&sk->sk_write_queue);
 651 }
 652
 653 static void l2cap_sock_cleanup_listen(struct sock *parent)
 654 {
 655         struct sock *sk;
 656
 657         BT_DBG("parent %p", parent);
 658
 659         /* Close not yet accepted channels */
 660         while ((sk = bt_accept_dequeue(parent, NULL)))
 661                 l2cap_sock_close(sk);
 662
 663         parent->sk_state = BT_CLOSED;
 664         sock_set_flag(parent, SOCK_ZAPPED);
 665 }
 666
 667 /* Kill socket (only if zapped and orphan)
 668  * Must be called on unlocked socket.
 669  */
 670 static void l2cap_sock_kill(struct sock *sk)
 671 {
 672         if (!sock_flag(sk, SOCK_ZAPPED) || sk->sk_socket)
 673                 return;
 674
 675         BT_DBG("sk %p state %d", sk, sk->sk_state);
 676
 677         /* Kill poor orphan */
 678         bt_sock_unlink(&l2cap_sk_list, sk);
 679         sock_set_flag(sk, SOCK_DEAD);
 680         sock_put(sk);
 681 }
 682
 683 static void __l2cap_sock_close(struct sock *sk, int reason)
 684 {
 685         BT_DBG("sk %p state %d socket %p", sk, sk->sk_state, sk->sk_socket);
 686
 687         switch (sk->sk_state) {
 688         case BT_LISTEN:
 689                 l2cap_sock_cleanup_listen(sk);
 690                 break;
 691
 692         case BT_CONNECTED:
 693         case BT_CONFIG:
 694                 if (sk->sk_type == SOCK_SEQPACKET) {
 695                         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
 696
 697                         sk->sk_state = BT_DISCONN;
 698                         l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
 699                         l2cap_send_disconn_req(conn, sk);
 700                 } else
 701                         l2cap_chan_del(sk, reason);
 702                 break;
 703
 704         case BT_CONNECT2:
 705                 if (sk->sk_type == SOCK_SEQPACKET) {
 706                         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
 707                         struct l2cap_conn_rsp rsp;
 708                         __u16 result;
 709
 710                         if (bt_sk(sk)->defer_setup)
 711                                 result = L2CAP_CR_SEC_BLOCK;
 712                         else
 713                                 result = L2CAP_CR_BAD_PSM;
 714
 715                         rsp.scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
 716                         rsp.dcid   = cpu_to_le16(l2cap_pi(sk)->scid);
 717                         rsp.result = cpu_to_le16(result);
 718                         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
 719                         l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
 720                                         L2CAP_CONN_RSP, sizeof(rsp), &rsp);
 721                 } else
 722                         l2cap_chan_del(sk, reason);
 723                 break;
 724
 725         case BT_CONNECT:
 726         case BT_DISCONN:
 727                 l2cap_chan_del(sk, reason);
 728                 break;
 729
 730         default:
 731                 sock_set_flag(sk, SOCK_ZAPPED);
 732                 break;
 733         }
 734 }
 735
 736 /* Must be called on unlocked socket. */
 737 static void l2cap_sock_close(struct sock *sk)
 738 {
 739         l2cap_sock_clear_timer(sk);
 740         lock_sock(sk);
 741         __l2cap_sock_close(sk, ECONNRESET);
 742         release_sock(sk);
 743         l2cap_sock_kill(sk);
 744 }
 745
 746 static void l2cap_sock_init(struct sock *sk, struct sock *parent)
 747 {
 748         struct l2cap_pinfo *pi = l2cap_pi(sk);
 749
 750         BT_DBG("sk %p", sk);
 751
 752         if (parent) {
 753                 sk->sk_type = parent->sk_type;
 754                 bt_sk(sk)->defer_setup = bt_sk(parent)->defer_setup;
 755
 756                 pi->imtu = l2cap_pi(parent)->imtu;
 757                 pi->omtu = l2cap_pi(parent)->omtu;
 758                 pi->mode = l2cap_pi(parent)->mode;
 759                 pi->fcs  = l2cap_pi(parent)->fcs;
 760                 pi->sec_level = l2cap_pi(parent)->sec_level;
 761                 pi->role_switch = l2cap_pi(parent)->role_switch;
 762                 pi->force_reliable = l2cap_pi(parent)->force_reliable;
 763         } else {
 764                 pi->imtu = L2CAP_DEFAULT_MTU;
 765                 pi->omtu = 0;
 766                 pi->mode = L2CAP_MODE_BASIC;
 767                 pi->fcs  = L2CAP_FCS_CRC16;
 768                 pi->sec_level = BT_SECURITY_LOW;
 769                 pi->role_switch = 0;
 770                 pi->force_reliable = 0;
 771         }
 772
 773         /* Default config options */
 774         pi->conf_len = 0;
 775         pi->flush_to = L2CAP_DEFAULT_FLUSH_TO;
 776 }
 777
 778 static struct proto l2cap_proto = {
 779         .name           = "L2CAP",
 780         .owner          = THIS_MODULE,
 781         .obj_size       = sizeof(struct l2cap_pinfo)
 782 };
 783
 784 static struct sock *l2cap_sock_alloc(struct net *net, struct socket *sock, int proto, gfp_t prio)
 785 {
 786         struct sock *sk;
 787
 788         sk = sk_alloc(net, PF_BLUETOOTH, prio, &l2cap_proto);
 789         if (!sk)
 790                 return NULL;
 791
 792         sock_init_data(sock, sk);
 793         INIT_LIST_HEAD(&bt_sk(sk)->accept_q);
 794
 795         sk->sk_destruct = l2cap_sock_destruct;
 796         sk->sk_sndtimeo = msecs_to_jiffies(L2CAP_CONN_TIMEOUT);
 797
 798         sock_reset_flag(sk, SOCK_ZAPPED);
 799
 800         sk->sk_protocol = proto;
 801         sk->sk_state = BT_OPEN;
 802
 803         setup_timer(&sk->sk_timer, l2cap_sock_timeout, (unsigned long) sk);
 804
 805         bt_sock_link(&l2cap_sk_list, sk);
 806         return sk;
 807 }
 808
 809 static int l2cap_sock_create(struct net *net, struct socket *sock, int protocol)
 810 {
 811         struct sock *sk;
 812
 813         BT_DBG("sock %p", sock);
 814
 815         sock->state = SS_UNCONNECTED;
 816
 817         if (sock->type != SOCK_SEQPACKET &&
 818                         sock->type != SOCK_DGRAM && sock->type != SOCK_RAW)
 819                 return -ESOCKTNOSUPPORT;
 820
 821         if (sock->type == SOCK_RAW && !capable(CAP_NET_RAW))
 822                 return -EPERM;
 823
 824         sock->ops = &l2cap_sock_ops;
 825
 826         sk = l2cap_sock_alloc(net, sock, protocol, GFP_ATOMIC);
 827         if (!sk)
 828                 return -ENOMEM;
 829
 830         l2cap_sock_init(sk, NULL);
 831         return 0;
 832 }
 833
 834 static int l2cap_sock_bind(struct socket *sock, struct sockaddr *addr, int alen)
 835 {
 836         struct sock *sk = sock->sk;
 837         struct sockaddr_l2 la;
 838         int len, err = 0;
 839
 840         BT_DBG("sk %p", sk);
 841
 842         if (!addr || addr->sa_family != AF_BLUETOOTH)
 843                 return -EINVAL;
 844
 845         memset(&la, 0, sizeof(la));
 846         len = min_t(unsigned int, sizeof(la), alen);
 847         memcpy(&la, addr, len);
 848
 849         if (la.l2_cid)
 850                 return -EINVAL;
 851
 852         lock_sock(sk);
 853
 854         if (sk->sk_state != BT_OPEN) {
 855                 err = -EBADFD;
 856                 goto done;
 857         }
 858
 859         if (la.l2_psm && __le16_to_cpu(la.l2_psm) < 0x1001 &&
 860                                 !capable(CAP_NET_BIND_SERVICE)) {
 861                 err = -EACCES;
 862                 goto done;
 863         }
 864
 865         write_lock_bh(&l2cap_sk_list.lock);
 866
 867         if (la.l2_psm && __l2cap_get_sock_by_addr(la.l2_psm, &la.l2_bdaddr)) {
 868                 err = -EADDRINUSE;
 869         } else {
 870                 /* Save source address */
 871                 bacpy(&bt_sk(sk)->src, &la.l2_bdaddr);
 872                 l2cap_pi(sk)->psm   = la.l2_psm;
 873                 l2cap_pi(sk)->sport = la.l2_psm;
 874                 sk->sk_state = BT_BOUND;
 875
 876                 if (__le16_to_cpu(la.l2_psm) == 0x0001 ||
 877                                         __le16_to_cpu(la.l2_psm) == 0x0003)
 878                         l2cap_pi(sk)->sec_level = BT_SECURITY_SDP;
 879         }
 880
 881         write_unlock_bh(&l2cap_sk_list.lock);
 882
 883 done:
 884         release_sock(sk);
 885         return err;
 886 }
 887
 888 static int l2cap_do_connect(struct sock *sk)
 889 {
 890         bdaddr_t *src = &bt_sk(sk)->src;
 891         bdaddr_t *dst = &bt_sk(sk)->dst;
 892         struct l2cap_conn *conn;
 893         struct hci_conn *hcon;
 894         struct hci_dev *hdev;
 895         __u8 auth_type;
 896         int err;
 897
 898         BT_DBG("%s -> %s psm 0x%2.2x", batostr(src), batostr(dst),
 899                                                         l2cap_pi(sk)->psm);
 900
 901         hdev = hci_get_route(dst, src);
 902         if (!hdev)
 903                 return -EHOSTUNREACH;
 904
 905         hci_dev_lock_bh(hdev);
 906
 907         err = -ENOMEM;
 908
 909         if (sk->sk_type == SOCK_RAW) {
 910                 switch (l2cap_pi(sk)->sec_level) {
 911                 case BT_SECURITY_HIGH:
 912                         auth_type = HCI_AT_DEDICATED_BONDING_MITM;
 913                         break;
 914                 case BT_SECURITY_MEDIUM:
 915                         auth_type = HCI_AT_DEDICATED_BONDING;
 916                         break;
 917                 default:
 918                         auth_type = HCI_AT_NO_BONDING;
 919                         break;
 920                 }
 921         } else if (l2cap_pi(sk)->psm == cpu_to_le16(0x0001)) {
 922                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_HIGH)
 923                         auth_type = HCI_AT_NO_BONDING_MITM;
 924                 else
 925                         auth_type = HCI_AT_NO_BONDING;
 926
 927                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_LOW)
 928                         l2cap_pi(sk)->sec_level = BT_SECURITY_SDP;
 929         } else {
 930                 switch (l2cap_pi(sk)->sec_level) {
 931                 case BT_SECURITY_HIGH:
 932                         auth_type = HCI_AT_GENERAL_BONDING_MITM;
 933                         break;
 934                 case BT_SECURITY_MEDIUM:
 935                         auth_type = HCI_AT_GENERAL_BONDING;
 936                         break;
 937                 default:
 938                         auth_type = HCI_AT_NO_BONDING;
 939                         break;
 940                 }
 941         }
 942
 943         hcon = hci_connect(hdev, ACL_LINK, dst,
 944                                         l2cap_pi(sk)->sec_level, auth_type);
 945         if (!hcon)
 946                 goto done;
 947
 948         conn = l2cap_conn_add(hcon, 0);
 949         if (!conn) {
 950                 hci_conn_put(hcon);
 951                 goto done;
 952         }
 953
 954         err = 0;
 955
 956         /* Update source addr of the socket */
 957         bacpy(src, conn->src);
 958
 959         l2cap_chan_add(conn, sk, NULL);
 960
 961         sk->sk_state = BT_CONNECT;
 962         l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
 963
 964         if (hcon->state == BT_CONNECTED) {
 965                 if (sk->sk_type != SOCK_SEQPACKET) {
 966                         l2cap_sock_clear_timer(sk);
 967                         sk->sk_state = BT_CONNECTED;
 968                 } else
 969                         l2cap_do_start(sk);
 970         }
 971
 972 done:
 973         hci_dev_unlock_bh(hdev);
 974         hci_dev_put(hdev);
 975         return err;
 976 }
 977
 978 static int l2cap_sock_connect(struct socket *sock, struct sockaddr *addr, int alen, int flags)
 979 {
 980         struct sock *sk = sock->sk;
 981         struct sockaddr_l2 la;
 982         int len, err = 0;
 983
 984         BT_DBG("sk %p", sk);
 985
 986         if (!addr || addr->sa_family != AF_BLUETOOTH)
 987                 return -EINVAL;
 988
 989         memset(&la, 0, sizeof(la));
 990         len = min_t(unsigned int, sizeof(la), alen);
 991         memcpy(&la, addr, len);
 992
 993         if (la.l2_cid)
 994                 return -EINVAL;
 995
 996         lock_sock(sk);
 997
 998         if (sk->sk_type == SOCK_SEQPACKET && !la.l2_psm) {
 999                 err = -EINVAL;
1000                 goto done;
1001         }
1002
1003         switch (l2cap_pi(sk)->mode) {
1004         case L2CAP_MODE_BASIC:
1005                 break;
1006         case L2CAP_MODE_ERTM:
1007         case L2CAP_MODE_STREAMING:
1008                 if (enable_ertm)
1009                         break;
1010                 /* fall through */
1011         default:
1012                 err = -ENOTSUPP;
1013                 goto done;
1014         }
1015
1016         switch (sk->sk_state) {
1017         case BT_CONNECT:
1018         case BT_CONNECT2:
1019         case BT_CONFIG:
1020                 /* Already connecting */
1021                 goto wait;
1022
1023         case BT_CONNECTED:
1024                 /* Already connected */
1025                 goto done;
1026
1027         case BT_OPEN:
1028         case BT_BOUND:
1029                 /* Can connect */
1030                 break;
1031
1032         default:
1033                 err = -EBADFD;
1034                 goto done;
1035         }
1036
1037         /* Set destination address and psm */
1038         bacpy(&bt_sk(sk)->dst, &la.l2_bdaddr);
1039         l2cap_pi(sk)->psm = la.l2_psm;
1040
1041         err = l2cap_do_connect(sk);
1042         if (err)
1043                 goto done;
1044
1045 wait:
1046         err = bt_sock_wait_state(sk, BT_CONNECTED,
1047                         sock_sndtimeo(sk, flags & O_NONBLOCK));
1048 done:
1049         release_sock(sk);
1050         return err;
1051 }
1052
1053 static int l2cap_sock_listen(struct socket *sock, int backlog)
1054 {
1055         struct sock *sk = sock->sk;
1056         int err = 0;
1057
1058         BT_DBG("sk %p backlog %d", sk, backlog);
1059
1060         lock_sock(sk);
1061
1062         if (sk->sk_state != BT_BOUND || sock->type != SOCK_SEQPACKET) {
1063                 err = -EBADFD;
1064                 goto done;
1065         }
1066
1067         switch (l2cap_pi(sk)->mode) {
1068         case L2CAP_MODE_BASIC:
1069                 break;
1070         case L2CAP_MODE_ERTM:
1071         case L2CAP_MODE_STREAMING:
1072                 if (enable_ertm)
1073                         break;
1074                 /* fall through */
1075         default:
1076                 err = -ENOTSUPP;
1077                 goto done;
1078         }
1079
1080         if (!l2cap_pi(sk)->psm) {
1081                 bdaddr_t *src = &bt_sk(sk)->src;
1082                 u16 psm;
1083
1084                 err = -EINVAL;
1085
1086                 write_lock_bh(&l2cap_sk_list.lock);
1087
1088                 for (psm = 0x1001; psm < 0x1100; psm += 2)
1089                         if (!__l2cap_get_sock_by_addr(cpu_to_le16(psm), src)) {
1090                                 l2cap_pi(sk)->psm   = cpu_to_le16(psm);
1091                                 l2cap_pi(sk)->sport = cpu_to_le16(psm);
1092                                 err = 0;
1093                                 break;
1094                         }
1095
1096                 write_unlock_bh(&l2cap_sk_list.lock);
1097
1098                 if (err < 0)
1099                         goto done;
1100         }
1101
1102         sk->sk_max_ack_backlog = backlog;
1103         sk->sk_ack_backlog = 0;
1104         sk->sk_state = BT_LISTEN;
1105
1106 done:
1107         release_sock(sk);
1108         return err;
1109 }
1110
1111 static int l2cap_sock_accept(struct socket *sock, struct socket *newsock, int flags)
1112 {
1113         DECLARE_WAITQUEUE(wait, current);
1114         struct sock *sk = sock->sk, *nsk;
1115         long timeo;
1116         int err = 0;
1117
1118         lock_sock_nested(sk, SINGLE_DEPTH_NESTING);
1119
1120         if (sk->sk_state != BT_LISTEN) {
1121                 err = -EBADFD;
1122                 goto done;
1123         }
1124
1125         timeo = sock_rcvtimeo(sk, flags & O_NONBLOCK);
1126
1127         BT_DBG("sk %p timeo %ld", sk, timeo);
1128
1129         /* Wait for an incoming connection. (wake-one). */
1130         add_wait_queue_exclusive(sk->sk_sleep, &wait);
1131         while (!(nsk = bt_accept_dequeue(sk, newsock))) {
1132                 set_current_state(TASK_INTERRUPTIBLE);
1133                 if (!timeo) {
1134                         err = -EAGAIN;
1135                         break;
1136                 }
1137
1138                 release_sock(sk);
1139                 timeo = schedule_timeout(timeo);
1140                 lock_sock_nested(sk, SINGLE_DEPTH_NESTING);
1141
1142                 if (sk->sk_state != BT_LISTEN) {
1143                         err = -EBADFD;
1144                         break;
1145                 }
1146
1147                 if (signal_pending(current)) {
1148                         err = sock_intr_errno(timeo);
1149                         break;
1150                 }
1151         }
1152         set_current_state(TASK_RUNNING);
1153         remove_wait_queue(sk->sk_sleep, &wait);
1154
1155         if (err)
1156                 goto done;
1157
1158         newsock->state = SS_CONNECTED;
1159
1160         BT_DBG("new socket %p", nsk);
1161
1162 done:
1163         release_sock(sk);
1164         return err;
1165 }
1166
1167 static int l2cap_sock_getname(struct socket *sock, struct sockaddr *addr, int *len, int peer)
1168 {
1169         struct sockaddr_l2 *la = (struct sockaddr_l2 *) addr;
1170         struct sock *sk = sock->sk;
1171
1172         BT_DBG("sock %p, sk %p", sock, sk);
1173
1174         addr->sa_family = AF_BLUETOOTH;
1175         *len = sizeof(struct sockaddr_l2);
1176
1177         if (peer) {
1178                 la->l2_psm = l2cap_pi(sk)->psm;
1179                 bacpy(&la->l2_bdaddr, &bt_sk(sk)->dst);
1180                 la->l2_cid = cpu_to_le16(l2cap_pi(sk)->dcid);
1181         } else {
1182                 la->l2_psm = l2cap_pi(sk)->sport;
1183                 bacpy(&la->l2_bdaddr, &bt_sk(sk)->src);
1184                 la->l2_cid = cpu_to_le16(l2cap_pi(sk)->scid);
1185         }
1186
1187         return 0;
1188 }
1189
1190 static void l2cap_monitor_timeout(unsigned long arg)
1191 {
1192         struct sock *sk = (void *) arg;
1193         u16 control;
1194
1195         bh_lock_sock(sk);
1196         if (l2cap_pi(sk)->retry_count >= l2cap_pi(sk)->remote_max_tx) {
1197                 l2cap_send_disconn_req(l2cap_pi(sk)->conn, sk);
1198                 return;
1199         }
1200
1201         l2cap_pi(sk)->retry_count++;
1202         __mod_monitor_timer();
1203
1204         control = L2CAP_CTRL_POLL;
1205         control |= L2CAP_SUPER_RCV_READY;
1206         l2cap_send_sframe(l2cap_pi(sk), control);
1207         bh_unlock_sock(sk);
1208 }
1209
1210 static void l2cap_retrans_timeout(unsigned long arg)
1211 {
1212         struct sock *sk = (void *) arg;
1213         u16 control;
1214
1215         bh_lock_sock(sk);
1216         l2cap_pi(sk)->retry_count = 1;
1217         __mod_monitor_timer();
1218
1219         l2cap_pi(sk)->conn_state |= L2CAP_CONN_WAIT_F;
1220
1221         control = L2CAP_CTRL_POLL;
1222         control |= L2CAP_SUPER_RCV_READY;
1223         l2cap_send_sframe(l2cap_pi(sk), control);
1224         bh_unlock_sock(sk);
1225 }
1226
1227 static void l2cap_drop_acked_frames(struct sock *sk)
1228 {
1229         struct sk_buff *skb;
1230
1231         while ((skb = skb_peek(TX_QUEUE(sk)))) {
1232                 if (bt_cb(skb)->tx_seq == l2cap_pi(sk)->expected_ack_seq)
1233                         break;
1234
1235                 skb = skb_dequeue(TX_QUEUE(sk));
1236                 kfree_skb(skb);
1237
1238                 l2cap_pi(sk)->unacked_frames--;
1239         }
1240
1241         if (!l2cap_pi(sk)->unacked_frames)
1242                 del_timer(&l2cap_pi(sk)->retrans_timer);
1243
1244         return;
1245 }
1246
1247 static inline int l2cap_do_send(struct sock *sk, struct sk_buff *skb)
1248 {
1249         struct l2cap_pinfo *pi = l2cap_pi(sk);
1250         int err;
1251
1252         BT_DBG("sk %p, skb %p len %d", sk, skb, skb->len);
1253
1254         err = hci_send_acl(pi->conn->hcon, skb, 0);
1255         if (err < 0)
1256                 kfree_skb(skb);
1257
1258         return err;
1259 }
1260
1261 static int l2cap_streaming_send(struct sock *sk)
1262 {
1263         struct sk_buff *skb, *tx_skb;
1264         struct l2cap_pinfo *pi = l2cap_pi(sk);
1265         u16 control, fcs;
1266         int err;
1267
1268         while ((skb = sk->sk_send_head)) {
1269                 tx_skb = skb_clone(skb, GFP_ATOMIC);
1270
1271                 control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE);
1272                 control |= pi->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT;
1273                 put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);
1274
1275                 if (l2cap_pi(sk)->fcs == L2CAP_FCS_CRC16) {
1276                         fcs = crc16(0, (u8 *)tx_skb->data, tx_skb->len - 2);
1277                         put_unaligned_le16(fcs, tx_skb->data + tx_skb->len - 2);
1278                 }
1279
1280                 err = l2cap_do_send(sk, tx_skb);
1281                 if (err < 0) {
1282                         l2cap_send_disconn_req(pi->conn, sk);
1283                         return err;
1284                 }
1285
1286                 pi->next_tx_seq = (pi->next_tx_seq + 1) % 64;
1287
1288                 if (skb_queue_is_last(TX_QUEUE(sk), skb))
1289                         sk->sk_send_head = NULL;
1290                 else
1291                         sk->sk_send_head = skb_queue_next(TX_QUEUE(sk), skb);
1292
1293                 skb = skb_dequeue(TX_QUEUE(sk));
1294                 kfree_skb(skb);
1295         }
1296         return 0;
1297 }
1298
1299 static int l2cap_retransmit_frame(struct sock *sk, u8 tx_seq)
1300 {
1301         struct l2cap_pinfo *pi = l2cap_pi(sk);
1302         struct sk_buff *skb, *tx_skb;
1303         u16 control, fcs;
1304         int err;
1305
1306         skb = skb_peek(TX_QUEUE(sk));
1307         do {
1308                 if (bt_cb(skb)->tx_seq != tx_seq) {
1309                         if (skb_queue_is_last(TX_QUEUE(sk), skb))
1310                                 break;
1311                         skb = skb_queue_next(TX_QUEUE(sk), skb);
1312                         continue;
1313                 }
1314
1315                 if (pi->remote_max_tx &&
1316                                 bt_cb(skb)->retries == pi->remote_max_tx) {
1317                         l2cap_send_disconn_req(pi->conn, sk);
1318                         break;
1319                 }
1320
1321                 tx_skb = skb_clone(skb, GFP_ATOMIC);
1322                 bt_cb(skb)->retries++;
1323                 control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE);
1324                 control |= (pi->req_seq << L2CAP_CTRL_REQSEQ_SHIFT)
1325                                 | (tx_seq << L2CAP_CTRL_TXSEQ_SHIFT);
1326                 put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);
1327
1328                 if (l2cap_pi(sk)->fcs == L2CAP_FCS_CRC16) {
1329                         fcs = crc16(0, (u8 *)tx_skb->data, tx_skb->len - 2);
1330                         put_unaligned_le16(fcs, tx_skb->data + tx_skb->len - 2);
1331                 }
1332
1333                 err = l2cap_do_send(sk, tx_skb);
1334                 if (err < 0) {
1335                         l2cap_send_disconn_req(pi->conn, sk);
1336                         return err;
1337                 }
1338                 break;
1339         } while(1);
1340         return 0;
1341 }
1342
1343 static int l2cap_ertm_send(struct sock *sk)
1344 {
1345         struct sk_buff *skb, *tx_skb;
1346         struct l2cap_pinfo *pi = l2cap_pi(sk);
1347         u16 control, fcs;
1348         int err;
1349
1350         if (pi->conn_state & L2CAP_CONN_WAIT_F)
1351                 return 0;
1352
1353         while ((skb = sk->sk_send_head) && (!l2cap_tx_window_full(sk))) {
1354                 tx_skb = skb_clone(skb, GFP_ATOMIC);
1355
1356                 if (pi->remote_max_tx &&
1357                                 bt_cb(skb)->retries == pi->remote_max_tx) {
1358                         l2cap_send_disconn_req(pi->conn, sk);
1359                         break;
1360                 }
1361
1362                 bt_cb(skb)->retries++;
1363
1364                 control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE);
1365                 control |= (pi->req_seq << L2CAP_CTRL_REQSEQ_SHIFT)
1366                                 | (pi->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT);
1367                 put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);
1368
1369
1370                 if (l2cap_pi(sk)->fcs == L2CAP_FCS_CRC16) {
1371                         fcs = crc16(0, (u8 *)skb->data, tx_skb->len - 2);
1372                         put_unaligned_le16(fcs, skb->data + tx_skb->len - 2);
1373                 }
1374
1375                 err = l2cap_do_send(sk, tx_skb);
1376                 if (err < 0) {
1377                         l2cap_send_disconn_req(pi->conn, sk);
1378                         return err;
1379                 }
1380                 __mod_retrans_timer();
1381
1382                 bt_cb(skb)->tx_seq = pi->next_tx_seq;
1383                 pi->next_tx_seq = (pi->next_tx_seq + 1) % 64;
1384
1385                 pi->unacked_frames++;
1386
1387                 if (skb_queue_is_last(TX_QUEUE(sk), skb))
1388                         sk->sk_send_head = NULL;
1389                 else
1390                         sk->sk_send_head = skb_queue_next(TX_QUEUE(sk), skb);
1391         }
1392
1393         return 0;
1394 }
1395
1396 static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, int len, int count, struct sk_buff *skb)
1397 {
1398         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1399         struct sk_buff **frag;
1400         int err, sent = 0;
1401
1402         if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count)) {
1403                 return -EFAULT;
1404         }
1405
1406         sent += count;
1407         len  -= count;
1408
1409         /* Continuation fragments (no L2CAP header) */
1410         frag = &skb_shinfo(skb)->frag_list;
1411         while (len) {
1412                 count = min_t(unsigned int, conn->mtu, len);
1413
1414                 *frag = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err);
1415                 if (!*frag)
1416                         return -EFAULT;
1417                 if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count))
1418                         return -EFAULT;
1419
1420                 sent += count;
1421                 len  -= count;
1422
1423                 frag = &(*frag)->next;
1424         }
1425
1426         return sent;
1427 }
1428
1429 static struct sk_buff *l2cap_create_connless_pdu(struct sock *sk, struct msghdr *msg, size_t len)
1430 {
1431         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1432         struct sk_buff *skb;
1433         int err, count, hlen = L2CAP_HDR_SIZE + 2;
1434         struct l2cap_hdr *lh;
1435
1436         BT_DBG("sk %p len %d", sk, (int)len);
1437
1438         count = min_t(unsigned int, (conn->mtu - hlen), len);
1439         skb = bt_skb_send_alloc(sk, count + hlen,
1440                         msg->msg_flags & MSG_DONTWAIT, &err);
1441         if (!skb)
1442                 return ERR_PTR(-ENOMEM);
1443
1444         /* Create L2CAP header */
1445         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1446         lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
1447         lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1448         put_unaligned_le16(l2cap_pi(sk)->psm, skb_put(skb, 2));
1449
1450         err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1451         if (unlikely(err < 0)) {
1452                 kfree_skb(skb);
1453                 return ERR_PTR(err);
1454         }
1455         return skb;
1456 }
1457
1458 static struct sk_buff *l2cap_create_basic_pdu(struct sock *sk, struct msghdr *msg, size_t len)
1459 {
1460         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1461         struct sk_buff *skb;
1462         int err, count, hlen = L2CAP_HDR_SIZE;
1463         struct l2cap_hdr *lh;
1464
1465         BT_DBG("sk %p len %d", sk, (int)len);
1466
1467         count = min_t(unsigned int, (conn->mtu - hlen), len);
1468         skb = bt_skb_send_alloc(sk, count + hlen,
1469                         msg->msg_flags & MSG_DONTWAIT, &err);
1470         if (!skb)
1471                 return ERR_PTR(-ENOMEM);
1472
1473         /* Create L2CAP header */
1474         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1475         lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
1476         lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1477
1478         err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1479         if (unlikely(err < 0)) {
1480                 kfree_skb(skb);
1481                 return ERR_PTR(err);
1482         }
1483         return skb;
1484 }
1485
1486 static struct sk_buff *l2cap_create_iframe_pdu(struct sock *sk, struct msghdr *msg, size_t len, u16 control, u16 sdulen)
1487 {
1488         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1489         struct sk_buff *skb;
1490         int err, count, hlen = L2CAP_HDR_SIZE + 2;
1491         struct l2cap_hdr *lh;
1492
1493         BT_DBG("sk %p len %d", sk, (int)len);
1494
1495         if (sdulen)
1496                 hlen += 2;
1497
1498         if (l2cap_pi(sk)->fcs == L2CAP_FCS_CRC16)
1499                 hlen += 2;
1500
1501         count = min_t(unsigned int, (conn->mtu - hlen), len);
1502         skb = bt_skb_send_alloc(sk, count + hlen,
1503                         msg->msg_flags & MSG_DONTWAIT, &err);
1504         if (!skb)
1505                 return ERR_PTR(-ENOMEM);
1506
1507         /* Create L2CAP header */
1508         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1509         lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
1510         lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1511         put_unaligned_le16(control, skb_put(skb, 2));
1512         if (sdulen)
1513                 put_unaligned_le16(sdulen, skb_put(skb, 2));
1514
1515         err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1516         if (unlikely(err < 0)) {
1517                 kfree_skb(skb);
1518                 return ERR_PTR(err);
1519         }
1520
1521         if (l2cap_pi(sk)->fcs == L2CAP_FCS_CRC16)
1522                 put_unaligned_le16(0, skb_put(skb, 2));
1523
1524         bt_cb(skb)->retries = 0;
1525         return skb;
1526 }
1527
1528 static inline int l2cap_sar_segment_sdu(struct sock *sk, struct msghdr *msg, size_t len)
1529 {
1530         struct l2cap_pinfo *pi = l2cap_pi(sk);
1531         struct sk_buff *skb;
1532         struct sk_buff_head sar_queue;
1533         u16 control;
1534         size_t size = 0;
1535
1536         __skb_queue_head_init(&sar_queue);
1537         control = L2CAP_SDU_START;
1538         skb = l2cap_create_iframe_pdu(sk, msg, pi->max_pdu_size, control, len);
1539         if (IS_ERR(skb))
1540                 return PTR_ERR(skb);
1541
1542         __skb_queue_tail(&sar_queue, skb);
1543         len -= pi->max_pdu_size;
1544         size +=pi->max_pdu_size;
1545         control = 0;
1546
1547         while (len > 0) {
1548                 size_t buflen;
1549
1550                 if (len > pi->max_pdu_size) {
1551                         control |= L2CAP_SDU_CONTINUE;
1552                         buflen = pi->max_pdu_size;
1553                 } else {
1554                         control |= L2CAP_SDU_END;
1555                         buflen = len;
1556                 }
1557
1558                 skb = l2cap_create_iframe_pdu(sk, msg, buflen, control, 0);
1559                 if (IS_ERR(skb)) {
1560                         skb_queue_purge(&sar_queue);
1561                         return PTR_ERR(skb);
1562                 }
1563
1564                 __skb_queue_tail(&sar_queue, skb);
1565                 len -= buflen;
1566                 size += buflen;
1567                 control = 0;
1568         }
1569         skb_queue_splice_tail(&sar_queue, TX_QUEUE(sk));
1570         if (sk->sk_send_head == NULL)
1571                 sk->sk_send_head = sar_queue.next;
1572
1573         return size;
1574 }
1575
1576 static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len)
1577 {
1578         struct sock *sk = sock->sk;
1579         struct l2cap_pinfo *pi = l2cap_pi(sk);
1580         struct sk_buff *skb;
1581         u16 control;
1582         int err;
1583
1584         BT_DBG("sock %p, sk %p", sock, sk);
1585
1586         err = sock_error(sk);
1587         if (err)
1588                 return err;
1589
1590         if (msg->msg_flags & MSG_OOB)
1591                 return -EOPNOTSUPP;
1592
1593         /* Check outgoing MTU */
1594         if (sk->sk_type == SOCK_SEQPACKET && pi->mode == L2CAP_MODE_BASIC
1595                         && len > pi->omtu)
1596                 return -EINVAL;
1597
1598         lock_sock(sk);
1599
1600         if (sk->sk_state != BT_CONNECTED) {
1601                 err = -ENOTCONN;
1602                 goto done;
1603         }
1604
1605         /* Connectionless channel */
1606         if (sk->sk_type == SOCK_DGRAM) {
1607                 skb = l2cap_create_connless_pdu(sk, msg, len);
1608                 err = l2cap_do_send(sk, skb);
1609                 goto done;
1610         }
1611
1612         switch (pi->mode) {
1613         case L2CAP_MODE_BASIC:
1614                 /* Create a basic PDU */
1615                 skb = l2cap_create_basic_pdu(sk, msg, len);
1616                 if (IS_ERR(skb)) {
1617                         err = PTR_ERR(skb);
1618                         goto done;
1619                 }
1620
1621                 err = l2cap_do_send(sk, skb);
1622                 if (!err)
1623                         err = len;
1624                 break;
1625
1626         case L2CAP_MODE_ERTM:
1627         case L2CAP_MODE_STREAMING:
1628                 /* Entire SDU fits into one PDU */
1629                 if (len <= pi->max_pdu_size) {
1630                         control = L2CAP_SDU_UNSEGMENTED;
1631                         skb = l2cap_create_iframe_pdu(sk, msg, len, control, 0);
1632                         if (IS_ERR(skb)) {
1633                                 err = PTR_ERR(skb);
1634                                 goto done;
1635                         }
1636                         __skb_queue_tail(TX_QUEUE(sk), skb);
1637                         if (sk->sk_send_head == NULL)
1638                                 sk->sk_send_head = skb;
1639                 } else {
1640                 /* Segment SDU into multiples PDUs */
1641                         err = l2cap_sar_segment_sdu(sk, msg, len);
1642                         if (err < 0)
1643                                 goto done;
1644                 }
1645
1646                 if (pi->mode == L2CAP_MODE_STREAMING)
1647                         err = l2cap_streaming_send(sk);
1648                 else
1649                         err = l2cap_ertm_send(sk);
1650
1651                 if (!err)
1652                         err = len;
1653                 break;
1654
1655         default:
1656                 BT_DBG("bad state %1.1x", pi->mode);
1657                 err = -EINVAL;
1658         }
1659
1660 done:
1661         release_sock(sk);
1662         return err;
1663 }
1664
1665 static int l2cap_sock_recvmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len, int flags)
1666 {
1667         struct sock *sk = sock->sk;
1668
1669         lock_sock(sk);
1670
1671         if (sk->sk_state == BT_CONNECT2 && bt_sk(sk)->defer_setup) {
1672                 struct l2cap_conn_rsp rsp;
1673
1674                 sk->sk_state = BT_CONFIG;
1675
1676                 rsp.scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
1677                 rsp.dcid   = cpu_to_le16(l2cap_pi(sk)->scid);
1678                 rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
1679                 rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
1680                 l2cap_send_cmd(l2cap_pi(sk)->conn, l2cap_pi(sk)->ident,
1681                                         L2CAP_CONN_RSP, sizeof(rsp), &rsp);
1682
1683                 release_sock(sk);
1684                 return 0;
1685         }
1686
1687         release_sock(sk);
1688
1689         return bt_sock_recvmsg(iocb, sock, msg, len, flags);
1690 }
1691
1692 static int l2cap_sock_setsockopt_old(struct socket *sock, int optname, char __user *optval, int optlen)
1693 {
1694         struct sock *sk = sock->sk;
1695         struct l2cap_options opts;
1696         int len, err = 0;
1697         u32 opt;
1698
1699         BT_DBG("sk %p", sk);
1700
1701         lock_sock(sk);
1702
1703         switch (optname) {
1704         case L2CAP_OPTIONS:
1705                 opts.imtu     = l2cap_pi(sk)->imtu;
1706                 opts.omtu     = l2cap_pi(sk)->omtu;
1707                 opts.flush_to = l2cap_pi(sk)->flush_to;
1708                 opts.mode     = l2cap_pi(sk)->mode;
1709                 opts.fcs      = l2cap_pi(sk)->fcs;
1710
1711                 len = min_t(unsigned int, sizeof(opts), optlen);
1712                 if (copy_from_user((char *) &opts, optval, len)) {
1713                         err = -EFAULT;
1714                         break;
1715                 }
1716
1717                 l2cap_pi(sk)->imtu = opts.imtu;
1718                 l2cap_pi(sk)->omtu = opts.omtu;
1719                 l2cap_pi(sk)->mode = opts.mode;
1720                 l2cap_pi(sk)->fcs  = opts.fcs;
1721                 break;
1722
1723         case L2CAP_LM:
1724                 if (get_user(opt, (u32 __user *) optval)) {
1725                         err = -EFAULT;
1726                         break;
1727                 }
1728
1729                 if (opt & L2CAP_LM_AUTH)
1730                         l2cap_pi(sk)->sec_level = BT_SECURITY_LOW;
1731                 if (opt & L2CAP_LM_ENCRYPT)
1732                         l2cap_pi(sk)->sec_level = BT_SECURITY_MEDIUM;
1733                 if (opt & L2CAP_LM_SECURE)
1734                         l2cap_pi(sk)->sec_level = BT_SECURITY_HIGH;
1735
1736                 l2cap_pi(sk)->role_switch    = (opt & L2CAP_LM_MASTER);
1737                 l2cap_pi(sk)->force_reliable = (opt & L2CAP_LM_RELIABLE);
1738                 break;
1739
1740         default:
1741                 err = -ENOPROTOOPT;
1742                 break;
1743         }
1744
1745         release_sock(sk);
1746         return err;
1747 }
1748
1749 static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname, char __user *optval, int optlen)
1750 {
1751         struct sock *sk = sock->sk;
1752         struct bt_security sec;
1753         int len, err = 0;
1754         u32 opt;
1755
1756         BT_DBG("sk %p", sk);
1757
1758         if (level == SOL_L2CAP)
1759                 return l2cap_sock_setsockopt_old(sock, optname, optval, optlen);
1760
1761         if (level != SOL_BLUETOOTH)
1762                 return -ENOPROTOOPT;
1763
1764         lock_sock(sk);
1765
1766         switch (optname) {
1767         case BT_SECURITY:
1768                 if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_RAW) {
1769                         err = -EINVAL;
1770                         break;
1771                 }
1772
1773                 sec.level = BT_SECURITY_LOW;
1774
1775                 len = min_t(unsigned int, sizeof(sec), optlen);
1776                 if (copy_from_user((char *) &sec, optval, len)) {
1777                         err = -EFAULT;
1778                         break;
1779                 }
1780
1781                 if (sec.level < BT_SECURITY_LOW ||
1782                                         sec.level > BT_SECURITY_HIGH) {
1783                         err = -EINVAL;
1784                         break;
1785                 }
1786
1787                 l2cap_pi(sk)->sec_level = sec.level;
1788                 break;
1789
1790         case BT_DEFER_SETUP:
1791                 if (sk->sk_state != BT_BOUND && sk->sk_state != BT_LISTEN) {
1792                         err = -EINVAL;
1793                         break;
1794                 }
1795
1796                 if (get_user(opt, (u32 __user *) optval)) {
1797                         err = -EFAULT;
1798                         break;
1799                 }
1800
1801                 bt_sk(sk)->defer_setup = opt;
1802                 break;
1803
1804         default:
1805                 err = -ENOPROTOOPT;
1806                 break;
1807         }
1808
1809         release_sock(sk);
1810         return err;
1811 }
1812
1813 static int l2cap_sock_getsockopt_old(struct socket *sock, int optname, char __user *optval, int __user *optlen)
1814 {
1815         struct sock *sk = sock->sk;
1816         struct l2cap_options opts;
1817         struct l2cap_conninfo cinfo;
1818         int len, err = 0;
1819         u32 opt;
1820
1821         BT_DBG("sk %p", sk);
1822
1823         if (get_user(len, optlen))
1824                 return -EFAULT;
1825
1826         lock_sock(sk);
1827
1828         switch (optname) {
1829         case L2CAP_OPTIONS:
1830                 opts.imtu     = l2cap_pi(sk)->imtu;
1831                 opts.omtu     = l2cap_pi(sk)->omtu;
1832                 opts.flush_to = l2cap_pi(sk)->flush_to;
1833                 opts.mode     = l2cap_pi(sk)->mode;
1834                 opts.fcs      = l2cap_pi(sk)->fcs;
1835
1836                 len = min_t(unsigned int, len, sizeof(opts));
1837                 if (copy_to_user(optval, (char *) &opts, len))
1838                         err = -EFAULT;
1839
1840                 break;
1841
1842         case L2CAP_LM:
1843                 switch (l2cap_pi(sk)->sec_level) {
1844                 case BT_SECURITY_LOW:
1845                         opt = L2CAP_LM_AUTH;
1846                         break;
1847                 case BT_SECURITY_MEDIUM:
1848                         opt = L2CAP_LM_AUTH | L2CAP_LM_ENCRYPT;
1849                         break;
1850                 case BT_SECURITY_HIGH:
1851                         opt = L2CAP_LM_AUTH | L2CAP_LM_ENCRYPT |
1852                                                         L2CAP_LM_SECURE;
1853                         break;
1854                 default:
1855                         opt = 0;
1856                         break;
1857                 }
1858
1859                 if (l2cap_pi(sk)->role_switch)
1860                         opt |= L2CAP_LM_MASTER;
1861
1862                 if (l2cap_pi(sk)->force_reliable)
1863                         opt |= L2CAP_LM_RELIABLE;
1864
1865                 if (put_user(opt, (u32 __user *) optval))
1866                         err = -EFAULT;
1867                 break;
1868
1869         case L2CAP_CONNINFO:
1870                 if (sk->sk_state != BT_CONNECTED &&
1871                                         !(sk->sk_state == BT_CONNECT2 &&
1872                                                 bt_sk(sk)->defer_setup)) {
1873                         err = -ENOTCONN;
1874                         break;
1875                 }
1876
1877                 cinfo.hci_handle = l2cap_pi(sk)->conn->hcon->handle;
1878                 memcpy(cinfo.dev_class, l2cap_pi(sk)->conn->hcon->dev_class, 3);
1879
1880                 len = min_t(unsigned int, len, sizeof(cinfo));
1881                 if (copy_to_user(optval, (char *) &cinfo, len))
1882                         err = -EFAULT;
1883
1884                 break;
1885
1886         default:
1887                 err = -ENOPROTOOPT;
1888                 break;
1889         }
1890
1891         release_sock(sk);
1892         return err;
1893 }
1894
1895 static int l2cap_sock_getsockopt(struct socket *sock, int level, int optname, char __user *optval, int __user *optlen)
1896 {
1897         struct sock *sk = sock->sk;
1898         struct bt_security sec;
1899         int len, err = 0;
1900
1901         BT_DBG("sk %p", sk);
1902
1903         if (level == SOL_L2CAP)
1904                 return l2cap_sock_getsockopt_old(sock, optname, optval, optlen);
1905
1906         if (level != SOL_BLUETOOTH)
1907                 return -ENOPROTOOPT;
1908
1909         if (get_user(len, optlen))
1910                 return -EFAULT;
1911
1912         lock_sock(sk);
1913
1914         switch (optname) {
1915         case BT_SECURITY:
1916                 if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_RAW) {
1917                         err = -EINVAL;
1918                         break;
1919                 }
1920
1921                 sec.level = l2cap_pi(sk)->sec_level;
1922
1923                 len = min_t(unsigned int, len, sizeof(sec));
1924                 if (copy_to_user(optval, (char *) &sec, len))
1925                         err = -EFAULT;
1926
1927                 break;
1928
1929         case BT_DEFER_SETUP:
1930                 if (sk->sk_state != BT_BOUND && sk->sk_state != BT_LISTEN) {
1931                         err = -EINVAL;
1932                         break;
1933                 }
1934
1935                 if (put_user(bt_sk(sk)->defer_setup, (u32 __user *) optval))
1936                         err = -EFAULT;
1937
1938                 break;
1939
1940         default:
1941                 err = -ENOPROTOOPT;
1942                 break;
1943         }
1944
1945         release_sock(sk);
1946         return err;
1947 }
1948
1949 static int l2cap_sock_shutdown(struct socket *sock, int how)
1950 {
1951         struct sock *sk = sock->sk;
1952         int err = 0;
1953
1954         BT_DBG("sock %p, sk %p", sock, sk);
1955
1956         if (!sk)
1957                 return 0;
1958
1959         lock_sock(sk);
1960         if (!sk->sk_shutdown) {
1961                 sk->sk_shutdown = SHUTDOWN_MASK;
1962                 l2cap_sock_clear_timer(sk);
1963                 __l2cap_sock_close(sk, 0);
1964
1965                 if (sock_flag(sk, SOCK_LINGER) && sk->sk_lingertime)
1966                         err = bt_sock_wait_state(sk, BT_CLOSED,
1967                                                         sk->sk_lingertime);
1968         }
1969         release_sock(sk);
1970         return err;
1971 }
1972
1973 static int l2cap_sock_release(struct socket *sock)
1974 {
1975         struct sock *sk = sock->sk;
1976         int err;
1977
1978         BT_DBG("sock %p, sk %p", sock, sk);
1979
1980         if (!sk)
1981                 return 0;
1982
1983         err = l2cap_sock_shutdown(sock, 2);
1984
1985         sock_orphan(sk);
1986         l2cap_sock_kill(sk);
1987         return err;
1988 }
1989
1990 static void l2cap_chan_ready(struct sock *sk)
1991 {
1992         struct sock *parent = bt_sk(sk)->parent;
1993
1994         BT_DBG("sk %p, parent %p", sk, parent);
1995
1996         l2cap_pi(sk)->conf_state = 0;
1997         l2cap_sock_clear_timer(sk);
1998
1999         if (!parent) {
2000                 /* Outgoing channel.
2001                  * Wake up socket sleeping on connect.
2002                  */
2003                 sk->sk_state = BT_CONNECTED;
2004                 sk->sk_state_change(sk);
2005         } else {
2006                 /* Incoming channel.
2007                  * Wake up socket sleeping on accept.
2008                  */
2009                 parent->sk_data_ready(parent, 0);
2010         }
2011 }
2012
2013 /* Copy frame to all raw sockets on that connection */
2014 static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
2015 {
2016         struct l2cap_chan_list *l = &conn->chan_list;
2017         struct sk_buff *nskb;
2018         struct sock *sk;
2019
2020         BT_DBG("conn %p", conn);
2021
2022         read_lock(&l->lock);
2023         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
2024                 if (sk->sk_type != SOCK_RAW)
2025                         continue;
2026
2027                 /* Don't send frame to the socket it came from */
2028                 if (skb->sk == sk)
2029                         continue;
2030                 nskb = skb_clone(skb, GFP_ATOMIC);
2031                 if (!nskb)
2032                         continue;
2033
2034                 if (sock_queue_rcv_skb(sk, nskb))
2035                         kfree_skb(nskb);
2036         }
2037         read_unlock(&l->lock);
2038 }
2039
2040 /* ---- L2CAP signalling commands ---- */
2041 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
2042                                 u8 code, u8 ident, u16 dlen, void *data)
2043 {
2044         struct sk_buff *skb, **frag;
2045         struct l2cap_cmd_hdr *cmd;
2046         struct l2cap_hdr *lh;
2047         int len, count;
2048
2049         BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %d",
2050                         conn, code, ident, dlen);
2051
2052         len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen;
2053         count = min_t(unsigned int, conn->mtu, len);
2054
2055         skb = bt_skb_alloc(count, GFP_ATOMIC);
2056         if (!skb)
2057                 return NULL;
2058
2059         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
2060         lh->len = cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen);
2061         lh->cid = cpu_to_le16(L2CAP_CID_SIGNALING);
2062
2063         cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE);
2064         cmd->code  = code;
2065         cmd->ident = ident;
2066         cmd->len   = cpu_to_le16(dlen);
2067
2068         if (dlen) {
2069                 count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE;
2070                 memcpy(skb_put(skb, count), data, count);
2071                 data += count;
2072         }
2073
2074         len -= skb->len;
2075
2076         /* Continuation fragments (no L2CAP header) */
2077         frag = &skb_shinfo(skb)->frag_list;
2078         while (len) {
2079                 count = min_t(unsigned int, conn->mtu, len);
2080
2081                 *frag = bt_skb_alloc(count, GFP_ATOMIC);
2082                 if (!*frag)
2083                         goto fail;
2084
2085                 memcpy(skb_put(*frag, count), data, count);
2086
2087                 len  -= count;
2088                 data += count;
2089
2090                 frag = &(*frag)->next;
2091         }
2092
2093         return skb;
2094
2095 fail:
2096         kfree_skb(skb);
2097         return NULL;
2098 }
2099
2100 static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val)
2101 {
2102         struct l2cap_conf_opt *opt = *ptr;
2103         int len;
2104
2105         len = L2CAP_CONF_OPT_SIZE + opt->len;
2106         *ptr += len;
2107
2108         *type = opt->type;
2109         *olen = opt->len;
2110
2111         switch (opt->len) {
2112         case 1:
2113                 *val = *((u8 *) opt->val);
2114                 break;
2115
2116         case 2:
2117                 *val = __le16_to_cpu(*((__le16 *) opt->val));
2118                 break;
2119
2120         case 4:
2121                 *val = __le32_to_cpu(*((__le32 *) opt->val));
2122                 break;
2123
2124         default:
2125                 *val = (unsigned long) opt->val;
2126                 break;
2127         }
2128
2129         BT_DBG("type 0x%2.2x len %d val 0x%lx", *type, opt->len, *val);
2130         return len;
2131 }
2132
2133 static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val)
2134 {
2135         struct l2cap_conf_opt *opt = *ptr;
2136
2137         BT_DBG("type 0x%2.2x len %d val 0x%lx", type, len, val);
2138
2139         opt->type = type;
2140         opt->len  = len;
2141
2142         switch (len) {
2143         case 1:
2144                 *((u8 *) opt->val)  = val;
2145                 break;
2146
2147         case 2:
2148                 *((__le16 *) opt->val) = cpu_to_le16(val);
2149                 break;
2150
2151         case 4:
2152                 *((__le32 *) opt->val) = cpu_to_le32(val);
2153                 break;
2154
2155         default:
2156                 memcpy(opt->val, (void *) val, len);
2157                 break;
2158         }
2159
2160         *ptr += L2CAP_CONF_OPT_SIZE + len;
2161 }
2162
2163 static int l2cap_mode_supported(__u8 mode, __u32 feat_mask)
2164 {
2165         u32 local_feat_mask = l2cap_feat_mask;
2166         if (enable_ertm)
2167                 local_feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING;
2168
2169         switch (mode) {
2170         case L2CAP_MODE_ERTM:
2171                 return L2CAP_FEAT_ERTM & feat_mask & local_feat_mask;
2172         case L2CAP_MODE_STREAMING:
2173                 return L2CAP_FEAT_STREAMING & feat_mask & local_feat_mask;
2174         default:
2175                 return 0x00;
2176         }
2177 }
2178
2179 static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
2180 {
2181         switch (mode) {
2182         case L2CAP_MODE_STREAMING:
2183         case L2CAP_MODE_ERTM:
2184                 if (l2cap_mode_supported(mode, remote_feat_mask))
2185                         return mode;
2186                 /* fall through */
2187         default:
2188                 return L2CAP_MODE_BASIC;
2189         }
2190 }
2191
2192 static int l2cap_build_conf_req(struct sock *sk, void *data)
2193 {
2194         struct l2cap_pinfo *pi = l2cap_pi(sk);
2195         struct l2cap_conf_req *req = data;
2196         struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_ERTM };
2197         void *ptr = req->data;
2198
2199         BT_DBG("sk %p", sk);
2200
2201         if (pi->num_conf_req || pi->num_conf_rsp)
2202                 goto done;
2203
2204         switch (pi->mode) {
2205         case L2CAP_MODE_STREAMING:
2206         case L2CAP_MODE_ERTM:
2207                 pi->conf_state |= L2CAP_CONF_STATE2_DEVICE;
2208                 if (!l2cap_mode_supported(pi->mode, pi->conn->feat_mask))
2209                         l2cap_send_disconn_req(pi->conn, sk);
2210                 break;
2211         default:
2212                 pi->mode = l2cap_select_mode(rfc.mode, pi->conn->feat_mask);
2213                 break;
2214         }
2215
2216 done:
2217         switch (pi->mode) {
2218         case L2CAP_MODE_BASIC:
2219                 if (pi->imtu != L2CAP_DEFAULT_MTU)
2220                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->imtu);
2221                 break;
2222
2223         case L2CAP_MODE_ERTM:
2224                 rfc.mode            = L2CAP_MODE_ERTM;
2225                 rfc.txwin_size      = L2CAP_DEFAULT_TX_WINDOW;
2226                 rfc.max_transmit    = L2CAP_DEFAULT_MAX_TX;
2227                 rfc.retrans_timeout = 0;
2228                 rfc.monitor_timeout = 0;
2229                 rfc.max_pdu_size    = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE);
2230
2231                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2232                                         sizeof(rfc), (unsigned long) &rfc);
2233
2234                 if (!(pi->conn->feat_mask & L2CAP_FEAT_FCS))
2235                         break;
2236
2237                 if (pi->fcs == L2CAP_FCS_NONE ||
2238                                 pi->conf_state & L2CAP_CONF_NO_FCS_RECV) {
2239                         pi->fcs = L2CAP_FCS_NONE;
2240                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, pi->fcs);
2241                 }
2242                 break;
2243
2244         case L2CAP_MODE_STREAMING:
2245                 rfc.mode            = L2CAP_MODE_STREAMING;
2246                 rfc.txwin_size      = 0;
2247                 rfc.max_transmit    = 0;
2248                 rfc.retrans_timeout = 0;
2249                 rfc.monitor_timeout = 0;
2250                 rfc.max_pdu_size    = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE);
2251
2252                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2253                                         sizeof(rfc), (unsigned long) &rfc);
2254
2255                 if (!(pi->conn->feat_mask & L2CAP_FEAT_FCS))
2256                         break;
2257
2258                 if (pi->fcs == L2CAP_FCS_NONE ||
2259                                 pi->conf_state & L2CAP_CONF_NO_FCS_RECV) {
2260                         pi->fcs = L2CAP_FCS_NONE;
2261                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, pi->fcs);
2262                 }
2263                 break;
2264         }
2265
2266         /* FIXME: Need actual value of the flush timeout */
2267         //if (flush_to != L2CAP_DEFAULT_FLUSH_TO)
2268         //   l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO, 2, pi->flush_to);
2269
2270         req->dcid  = cpu_to_le16(pi->dcid);
2271         req->flags = cpu_to_le16(0);
2272
2273         return ptr - data;
2274 }
2275
2276 static int l2cap_parse_conf_req(struct sock *sk, void *data)
2277 {
2278         struct l2cap_pinfo *pi = l2cap_pi(sk);
2279         struct l2cap_conf_rsp *rsp = data;
2280         void *ptr = rsp->data;
2281         void *req = pi->conf_req;
2282         int len = pi->conf_len;
2283         int type, hint, olen;
2284         unsigned long val;
2285         struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
2286         u16 mtu = L2CAP_DEFAULT_MTU;
2287         u16 result = L2CAP_CONF_SUCCESS;
2288
2289         BT_DBG("sk %p", sk);
2290
2291         while (len >= L2CAP_CONF_OPT_SIZE) {
2292                 len -= l2cap_get_conf_opt(&req, &type, &olen, &val);
2293
2294                 hint  = type & L2CAP_CONF_HINT;
2295                 type &= L2CAP_CONF_MASK;
2296
2297                 switch (type) {
2298                 case L2CAP_CONF_MTU:
2299                         mtu = val;
2300                         break;
2301
2302                 case L2CAP_CONF_FLUSH_TO:
2303                         pi->flush_to = val;
2304                         break;
2305
2306                 case L2CAP_CONF_QOS:
2307                         break;
2308
2309                 case L2CAP_CONF_RFC:
2310                         if (olen == sizeof(rfc))
2311                                 memcpy(&rfc, (void *) val, olen);
2312                         break;
2313
2314                 case L2CAP_CONF_FCS:
2315                         if (val == L2CAP_FCS_NONE)
2316                                 pi->conf_state |= L2CAP_CONF_NO_FCS_RECV;
2317
2318                         break;
2319
2320                 default:
2321                         if (hint)
2322                                 break;
2323
2324                         result = L2CAP_CONF_UNKNOWN;
2325                         *((u8 *) ptr++) = type;
2326                         break;
2327                 }
2328         }
2329
2330         if (pi->num_conf_rsp || pi->num_conf_req)
2331                 goto done;
2332
2333         switch (pi->mode) {
2334         case L2CAP_MODE_STREAMING:
2335         case L2CAP_MODE_ERTM:
2336                 pi->conf_state |= L2CAP_CONF_STATE2_DEVICE;
2337                 if (!l2cap_mode_supported(pi->mode, pi->conn->feat_mask))
2338                         return -ECONNREFUSED;
2339                 break;
2340         default:
2341                 pi->mode = l2cap_select_mode(rfc.mode, pi->conn->feat_mask);
2342                 break;
2343         }
2344
2345 done:
2346         if (pi->mode != rfc.mode) {
2347                 result = L2CAP_CONF_UNACCEPT;
2348                 rfc.mode = pi->mode;
2349
2350                 if (pi->num_conf_rsp == 1)
2351                         return -ECONNREFUSED;
2352
2353                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2354                                         sizeof(rfc), (unsigned long) &rfc);
2355         }
2356
2357
2358         if (result == L2CAP_CONF_SUCCESS) {
2359                 /* Configure output options and let the other side know
2360                  * which ones we don't like. */
2361
2362                 if (mtu < L2CAP_DEFAULT_MIN_MTU)
2363                         result = L2CAP_CONF_UNACCEPT;
2364                 else {
2365                         pi->omtu = mtu;
2366                         pi->conf_state |= L2CAP_CONF_MTU_DONE;
2367                 }
2368                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->omtu);
2369
2370                 switch (rfc.mode) {
2371                 case L2CAP_MODE_BASIC:
2372                         pi->fcs = L2CAP_FCS_NONE;
2373                         pi->conf_state |= L2CAP_CONF_MODE_DONE;
2374                         break;
2375
2376                 case L2CAP_MODE_ERTM:
2377                         pi->remote_tx_win = rfc.txwin_size;
2378                         pi->remote_max_tx = rfc.max_transmit;
2379                         pi->max_pdu_size = rfc.max_pdu_size;
2380
2381                         rfc.retrans_timeout = L2CAP_DEFAULT_RETRANS_TO;
2382                         rfc.monitor_timeout = L2CAP_DEFAULT_MONITOR_TO;
2383
2384                         pi->conf_state |= L2CAP_CONF_MODE_DONE;
2385                         break;
2386
2387                 case L2CAP_MODE_STREAMING:
2388                         pi->remote_tx_win = rfc.txwin_size;
2389                         pi->max_pdu_size = rfc.max_pdu_size;
2390
2391                         pi->conf_state |= L2CAP_CONF_MODE_DONE;
2392                         break;
2393
2394                 default:
2395                         result = L2CAP_CONF_UNACCEPT;
2396
2397                         memset(&rfc, 0, sizeof(rfc));
2398                         rfc.mode = pi->mode;
2399                 }
2400
2401                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2402                                         sizeof(rfc), (unsigned long) &rfc);
2403
2404                 if (result == L2CAP_CONF_SUCCESS)
2405                         pi->conf_state |= L2CAP_CONF_OUTPUT_DONE;
2406         }
2407         rsp->scid   = cpu_to_le16(pi->dcid);
2408         rsp->result = cpu_to_le16(result);
2409         rsp->flags  = cpu_to_le16(0x0000);
2410
2411         return ptr - data;
2412 }
2413
2414 static int l2cap_parse_conf_rsp(struct sock *sk, void *rsp, int len, void *data, u16 *result)
2415 {
2416         struct l2cap_pinfo *pi = l2cap_pi(sk);
2417         struct l2cap_conf_req *req = data;
2418         void *ptr = req->data;
2419         int type, olen;
2420         unsigned long val;
2421         struct l2cap_conf_rfc rfc;
2422
2423         BT_DBG("sk %p, rsp %p, len %d, req %p", sk, rsp, len, data);
2424
2425         while (len >= L2CAP_CONF_OPT_SIZE) {
2426                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
2427
2428                 switch (type) {
2429                 case L2CAP_CONF_MTU:
2430                         if (val < L2CAP_DEFAULT_MIN_MTU) {
2431                                 *result = L2CAP_CONF_UNACCEPT;
2432                                 pi->omtu = L2CAP_DEFAULT_MIN_MTU;
2433                         } else
2434                                 pi->omtu = val;
2435                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->omtu);
2436                         break;
2437
2438                 case L2CAP_CONF_FLUSH_TO:
2439                         pi->flush_to = val;
2440                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
2441                                                         2, pi->flush_to);
2442                         break;
2443
2444                 case L2CAP_CONF_RFC:
2445                         if (olen == sizeof(rfc))
2446                                 memcpy(&rfc, (void *)val, olen);
2447
2448                         if ((pi->conf_state & L2CAP_CONF_STATE2_DEVICE) &&
2449                                                         rfc.mode != pi->mode)
2450                                 return -ECONNREFUSED;
2451
2452                         pi->mode = rfc.mode;
2453                         pi->fcs = 0;
2454
2455                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2456                                         sizeof(rfc), (unsigned long) &rfc);
2457                         break;
2458                 }
2459         }
2460
2461         if (*result == L2CAP_CONF_SUCCESS) {
2462                 switch (rfc.mode) {
2463                 case L2CAP_MODE_ERTM:
2464                         pi->remote_tx_win   = rfc.txwin_size;
2465                         pi->retrans_timeout = rfc.retrans_timeout;
2466                         pi->monitor_timeout = rfc.monitor_timeout;
2467                         pi->max_pdu_size    = le16_to_cpu(rfc.max_pdu_size);
2468                         break;
2469                 case L2CAP_MODE_STREAMING:
2470                         pi->max_pdu_size    = le16_to_cpu(rfc.max_pdu_size);
2471                         break;
2472                 }
2473         }
2474
2475         req->dcid   = cpu_to_le16(pi->dcid);
2476         req->flags  = cpu_to_le16(0x0000);
2477
2478         return ptr - data;
2479 }
2480
2481 static int l2cap_build_conf_rsp(struct sock *sk, void *data, u16 result, u16 flags)
2482 {
2483         struct l2cap_conf_rsp *rsp = data;
2484         void *ptr = rsp->data;
2485
2486         BT_DBG("sk %p", sk);
2487
2488         rsp->scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
2489         rsp->result = cpu_to_le16(result);
2490         rsp->flags  = cpu_to_le16(flags);
2491
2492         return ptr - data;
2493 }
2494
2495 static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2496 {
2497         struct l2cap_cmd_rej *rej = (struct l2cap_cmd_rej *) data;
2498
2499         if (rej->reason != 0x0000)
2500                 return 0;
2501
2502         if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
2503                                         cmd->ident == conn->info_ident) {
2504                 del_timer(&conn->info_timer);
2505
2506                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2507                 conn->info_ident = 0;
2508
2509                 l2cap_conn_start(conn);
2510         }
2511
2512         return 0;
2513 }
2514
2515 static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2516 {
2517         struct l2cap_chan_list *list = &conn->chan_list;
2518         struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
2519         struct l2cap_conn_rsp rsp;
2520         struct sock *sk, *parent;
2521         int result, status = L2CAP_CS_NO_INFO;
2522
2523         u16 dcid = 0, scid = __le16_to_cpu(req->scid);
2524         __le16 psm = req->psm;
2525
2526         BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid);
2527
2528         /* Check if we have socket listening on psm */
2529         parent = l2cap_get_sock_by_psm(BT_LISTEN, psm, conn->src);
2530         if (!parent) {
2531                 result = L2CAP_CR_BAD_PSM;
2532                 goto sendresp;
2533         }
2534
2535         /* Check if the ACL is secure enough (if not SDP) */
2536         if (psm != cpu_to_le16(0x0001) &&
2537                                 !hci_conn_check_link_mode(conn->hcon)) {
2538                 conn->disc_reason = 0x05;
2539                 result = L2CAP_CR_SEC_BLOCK;
2540                 goto response;
2541         }
2542
2543         result = L2CAP_CR_NO_MEM;
2544
2545         /* Check for backlog size */
2546         if (sk_acceptq_is_full(parent)) {
2547                 BT_DBG("backlog full %d", parent->sk_ack_backlog);
2548                 goto response;
2549         }
2550
2551         sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, GFP_ATOMIC);
2552         if (!sk)
2553                 goto response;
2554
2555         write_lock_bh(&list->lock);
2556
2557         /* Check if we already have channel with that dcid */
2558         if (__l2cap_get_chan_by_dcid(list, scid)) {
2559                 write_unlock_bh(&list->lock);
2560                 sock_set_flag(sk, SOCK_ZAPPED);
2561                 l2cap_sock_kill(sk);
2562                 goto response;
2563         }
2564
2565         hci_conn_hold(conn->hcon);
2566
2567         l2cap_sock_init(sk, parent);
2568         bacpy(&bt_sk(sk)->src, conn->src);
2569         bacpy(&bt_sk(sk)->dst, conn->dst);
2570         l2cap_pi(sk)->psm  = psm;
2571         l2cap_pi(sk)->dcid = scid;
2572
2573         __l2cap_chan_add(conn, sk, parent);
2574         dcid = l2cap_pi(sk)->scid;
2575
2576         l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
2577
2578         l2cap_pi(sk)->ident = cmd->ident;
2579
2580         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
2581                 if (l2cap_check_security(sk)) {
2582                         if (bt_sk(sk)->defer_setup) {
2583                                 sk->sk_state = BT_CONNECT2;
2584                                 result = L2CAP_CR_PEND;
2585                                 status = L2CAP_CS_AUTHOR_PEND;
2586                                 parent->sk_data_ready(parent, 0);
2587                         } else {
2588                                 sk->sk_state = BT_CONFIG;
2589                                 result = L2CAP_CR_SUCCESS;
2590                                 status = L2CAP_CS_NO_INFO;
2591                         }
2592                 } else {
2593                         sk->sk_state = BT_CONNECT2;
2594                         result = L2CAP_CR_PEND;
2595                         status = L2CAP_CS_AUTHEN_PEND;
2596                 }
2597         } else {
2598                 sk->sk_state = BT_CONNECT2;
2599                 result = L2CAP_CR_PEND;
2600                 status = L2CAP_CS_NO_INFO;
2601         }
2602
2603         write_unlock_bh(&list->lock);
2604
2605 response:
2606         bh_unlock_sock(parent);
2607
2608 sendresp:
2609         rsp.scid   = cpu_to_le16(scid);
2610         rsp.dcid   = cpu_to_le16(dcid);
2611         rsp.result = cpu_to_le16(result);
2612         rsp.status = cpu_to_le16(status);
2613         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
2614
2615         if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
2616                 struct l2cap_info_req info;
2617                 info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
2618
2619                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
2620                 conn->info_ident = l2cap_get_ident(conn);
2621
2622                 mod_timer(&conn->info_timer, jiffies +
2623                                         msecs_to_jiffies(L2CAP_INFO_TIMEOUT));
2624
2625                 l2cap_send_cmd(conn, conn->info_ident,
2626                                         L2CAP_INFO_REQ, sizeof(info), &info);
2627         }
2628
2629         return 0;
2630 }
2631
2632 static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2633 {
2634         struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
2635         u16 scid, dcid, result, status;
2636         struct sock *sk;
2637         u8 req[128];
2638
2639         scid   = __le16_to_cpu(rsp->scid);
2640         dcid   = __le16_to_cpu(rsp->dcid);
2641         result = __le16_to_cpu(rsp->result);
2642         status = __le16_to_cpu(rsp->status);
2643
2644         BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status);
2645
2646         if (scid) {
2647                 sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
2648                 if (!sk)
2649                         return 0;
2650         } else {
2651                 sk = l2cap_get_chan_by_ident(&conn->chan_list, cmd->ident);
2652                 if (!sk)
2653                         return 0;
2654         }
2655
2656         switch (result) {
2657         case L2CAP_CR_SUCCESS:
2658                 sk->sk_state = BT_CONFIG;
2659                 l2cap_pi(sk)->ident = 0;
2660                 l2cap_pi(sk)->dcid = dcid;
2661                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
2662
2663                 l2cap_pi(sk)->conf_state &= ~L2CAP_CONF_CONNECT_PEND;
2664
2665                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2666                                         l2cap_build_conf_req(sk, req), req);
2667                 l2cap_pi(sk)->num_conf_req++;
2668                 break;
2669
2670         case L2CAP_CR_PEND:
2671                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND;
2672                 break;
2673
2674         default:
2675                 l2cap_chan_del(sk, ECONNREFUSED);
2676                 break;
2677         }
2678
2679         bh_unlock_sock(sk);
2680         return 0;
2681 }
2682
2683 static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
2684 {
2685         struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
2686         u16 dcid, flags;
2687         u8 rsp[64];
2688         struct sock *sk;
2689         int len;
2690
2691         dcid  = __le16_to_cpu(req->dcid);
2692         flags = __le16_to_cpu(req->flags);
2693
2694         BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
2695
2696         sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid);
2697         if (!sk)
2698                 return -ENOENT;
2699
2700         if (sk->sk_state == BT_DISCONN)
2701                 goto unlock;
2702
2703         /* Reject if config buffer is too small. */
2704         len = cmd_len - sizeof(*req);
2705         if (l2cap_pi(sk)->conf_len + len > sizeof(l2cap_pi(sk)->conf_req)) {
2706                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2707                                 l2cap_build_conf_rsp(sk, rsp,
2708                                         L2CAP_CONF_REJECT, flags), rsp);
2709                 goto unlock;
2710         }
2711
2712         /* Store config. */
2713         memcpy(l2cap_pi(sk)->conf_req + l2cap_pi(sk)->conf_len, req->data, len);
2714         l2cap_pi(sk)->conf_len += len;
2715
2716         if (flags & 0x0001) {
2717                 /* Incomplete config. Send empty response. */
2718                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2719                                 l2cap_build_conf_rsp(sk, rsp,
2720                                         L2CAP_CONF_SUCCESS, 0x0001), rsp);
2721                 goto unlock;
2722         }
2723
2724         /* Complete config. */
2725         len = l2cap_parse_conf_req(sk, rsp);
2726         if (len < 0) {
2727                 l2cap_send_disconn_req(conn, sk);
2728                 goto unlock;
2729         }
2730
2731         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
2732         l2cap_pi(sk)->num_conf_rsp++;
2733
2734         /* Reset config buffer. */
2735         l2cap_pi(sk)->conf_len = 0;
2736
2737         if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE))
2738                 goto unlock;
2739
2740         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_INPUT_DONE) {
2741                 if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_NO_FCS_RECV)
2742                                 || l2cap_pi(sk)->fcs != L2CAP_FCS_NONE)
2743                         l2cap_pi(sk)->fcs = L2CAP_FCS_CRC16;
2744
2745                 sk->sk_state = BT_CONNECTED;
2746                 l2cap_pi(sk)->next_tx_seq = 0;
2747                 l2cap_pi(sk)->expected_ack_seq = 0;
2748                 l2cap_pi(sk)->unacked_frames = 0;
2749
2750                 setup_timer(&l2cap_pi(sk)->retrans_timer,
2751                                 l2cap_retrans_timeout, (unsigned long) sk);
2752                 setup_timer(&l2cap_pi(sk)->monitor_timer,
2753                                 l2cap_monitor_timeout, (unsigned long) sk);
2754
2755                 __skb_queue_head_init(TX_QUEUE(sk));
2756                 __skb_queue_head_init(SREJ_QUEUE(sk));
2757                 l2cap_chan_ready(sk);
2758                 goto unlock;
2759         }
2760
2761         if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT)) {
2762                 u8 buf[64];
2763                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2764                                         l2cap_build_conf_req(sk, buf), buf);
2765                 l2cap_pi(sk)->num_conf_req++;
2766         }
2767
2768 unlock:
2769         bh_unlock_sock(sk);
2770         return 0;
2771 }
2772
2773 static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2774 {
2775         struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
2776         u16 scid, flags, result;
2777         struct sock *sk;
2778
2779         scid   = __le16_to_cpu(rsp->scid);
2780         flags  = __le16_to_cpu(rsp->flags);
2781         result = __le16_to_cpu(rsp->result);
2782
2783         BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x",
2784                         scid, flags, result);
2785
2786         sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
2787         if (!sk)
2788                 return 0;
2789
2790         switch (result) {
2791         case L2CAP_CONF_SUCCESS:
2792                 break;
2793
2794         case L2CAP_CONF_UNACCEPT:
2795                 if (l2cap_pi(sk)->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
2796                         int len = cmd->len - sizeof(*rsp);
2797                         char req[64];
2798
2799                         /* throw out any old stored conf requests */
2800                         result = L2CAP_CONF_SUCCESS;
2801                         len = l2cap_parse_conf_rsp(sk, rsp->data,
2802                                                         len, req, &result);
2803                         if (len < 0) {
2804                                 l2cap_send_disconn_req(conn, sk);
2805                                 goto done;
2806                         }
2807
2808                         l2cap_send_cmd(conn, l2cap_get_ident(conn),
2809                                                 L2CAP_CONF_REQ, len, req);
2810                         l2cap_pi(sk)->num_conf_req++;
2811                         if (result != L2CAP_CONF_SUCCESS)
2812                                 goto done;
2813                         break;
2814                 }
2815
2816         default:
2817                 sk->sk_state = BT_DISCONN;
2818                 sk->sk_err = ECONNRESET;
2819                 l2cap_sock_set_timer(sk, HZ * 5);
2820                 l2cap_send_disconn_req(conn, sk);
2821                 goto done;
2822         }
2823
2824         if (flags & 0x01)
2825                 goto done;
2826
2827         l2cap_pi(sk)->conf_state |= L2CAP_CONF_INPUT_DONE;
2828
2829         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE) {
2830                 if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_NO_FCS_RECV)
2831                                 || l2cap_pi(sk)->fcs != L2CAP_FCS_NONE)
2832                         l2cap_pi(sk)->fcs = L2CAP_FCS_CRC16;
2833
2834                 sk->sk_state = BT_CONNECTED;
2835                 l2cap_pi(sk)->expected_tx_seq = 0;
2836                 l2cap_pi(sk)->buffer_seq = 0;
2837                 l2cap_pi(sk)->num_to_ack = 0;
2838                 __skb_queue_head_init(TX_QUEUE(sk));
2839                 __skb_queue_head_init(SREJ_QUEUE(sk));
2840                 l2cap_chan_ready(sk);
2841         }
2842
2843 done:
2844         bh_unlock_sock(sk);
2845         return 0;
2846 }
2847
2848 static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2849 {
2850         struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
2851         struct l2cap_disconn_rsp rsp;
2852         u16 dcid, scid;
2853         struct sock *sk;
2854
2855         scid = __le16_to_cpu(req->scid);
2856         dcid = __le16_to_cpu(req->dcid);
2857
2858         BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
2859
2860         sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid);
2861         if (!sk)
2862                 return 0;
2863
2864         rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid);
2865         rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid);
2866         l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
2867
2868         sk->sk_shutdown = SHUTDOWN_MASK;
2869
2870         skb_queue_purge(TX_QUEUE(sk));
2871         skb_queue_purge(SREJ_QUEUE(sk));
2872         del_timer(&l2cap_pi(sk)->retrans_timer);
2873         del_timer(&l2cap_pi(sk)->monitor_timer);
2874
2875         l2cap_chan_del(sk, ECONNRESET);
2876         bh_unlock_sock(sk);
2877
2878         l2cap_sock_kill(sk);
2879         return 0;
2880 }
2881
2882 static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2883 {
2884         struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
2885         u16 dcid, scid;
2886         struct sock *sk;
2887
2888         scid = __le16_to_cpu(rsp->scid);
2889         dcid = __le16_to_cpu(rsp->dcid);
2890
2891         BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
2892
2893         sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
2894         if (!sk)
2895                 return 0;
2896
2897         skb_queue_purge(TX_QUEUE(sk));
2898         skb_queue_purge(SREJ_QUEUE(sk));
2899         del_timer(&l2cap_pi(sk)->retrans_timer);
2900         del_timer(&l2cap_pi(sk)->monitor_timer);
2901
2902         l2cap_chan_del(sk, 0);
2903         bh_unlock_sock(sk);
2904
2905         l2cap_sock_kill(sk);
2906         return 0;
2907 }
2908
2909 static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2910 {
2911         struct l2cap_info_req *req = (struct l2cap_info_req *) data;
2912         u16 type;
2913
2914         type = __le16_to_cpu(req->type);
2915
2916         BT_DBG("type 0x%4.4x", type);
2917
2918         if (type == L2CAP_IT_FEAT_MASK) {
2919                 u8 buf[8];
2920                 u32 feat_mask = l2cap_feat_mask;
2921                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
2922                 rsp->type   = cpu_to_le16(L2CAP_IT_FEAT_MASK);
2923                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
2924                 if (enable_ertm)
2925                         feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
2926                                                          | L2CAP_FEAT_FCS;
2927                 put_unaligned_le32(feat_mask, rsp->data);
2928                 l2cap_send_cmd(conn, cmd->ident,
2929                                         L2CAP_INFO_RSP, sizeof(buf), buf);
2930         } else if (type == L2CAP_IT_FIXED_CHAN) {
2931                 u8 buf[12];
2932                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
2933                 rsp->type   = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
2934                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
2935                 memcpy(buf + 4, l2cap_fixed_chan, 8);
2936                 l2cap_send_cmd(conn, cmd->ident,
2937                                         L2CAP_INFO_RSP, sizeof(buf), buf);
2938         } else {
2939                 struct l2cap_info_rsp rsp;
2940                 rsp.type   = cpu_to_le16(type);
2941                 rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP);
2942                 l2cap_send_cmd(conn, cmd->ident,
2943                                         L2CAP_INFO_RSP, sizeof(rsp), &rsp);
2944         }
2945
2946         return 0;
2947 }
2948
2949 static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2950 {
2951         struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
2952         u16 type, result;
2953
2954         type   = __le16_to_cpu(rsp->type);
2955         result = __le16_to_cpu(rsp->result);
2956
2957         BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
2958
2959         del_timer(&conn->info_timer);
2960
2961         if (type == L2CAP_IT_FEAT_MASK) {
2962                 conn->feat_mask = get_unaligned_le32(rsp->data);
2963
2964                 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
2965                         struct l2cap_info_req req;
2966                         req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
2967
2968                         conn->info_ident = l2cap_get_ident(conn);
2969
2970                         l2cap_send_cmd(conn, conn->info_ident,
2971                                         L2CAP_INFO_REQ, sizeof(req), &req);
2972                 } else {
2973                         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2974                         conn->info_ident = 0;
2975
2976                         l2cap_conn_start(conn);
2977                 }
2978         } else if (type == L2CAP_IT_FIXED_CHAN) {
2979                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2980                 conn->info_ident = 0;
2981
2982                 l2cap_conn_start(conn);
2983         }
2984
2985         return 0;
2986 }
2987
2988 static inline void l2cap_sig_channel(struct l2cap_conn *conn, struct sk_buff *skb)
2989 {
2990         u8 *data = skb->data;
2991         int len = skb->len;
2992         struct l2cap_cmd_hdr cmd;
2993         int err = 0;
2994
2995         l2cap_raw_recv(conn, skb);
2996
2997         while (len >= L2CAP_CMD_HDR_SIZE) {
2998                 u16 cmd_len;
2999                 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
3000                 data += L2CAP_CMD_HDR_SIZE;
3001                 len  -= L2CAP_CMD_HDR_SIZE;
3002
3003                 cmd_len = le16_to_cpu(cmd.len);
3004
3005                 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, cmd.ident);
3006
3007                 if (cmd_len > len || !cmd.ident) {
3008                         BT_DBG("corrupted command");
3009                         break;
3010                 }
3011
3012                 switch (cmd.code) {
3013                 case L2CAP_COMMAND_REJ:
3014                         l2cap_command_rej(conn, &cmd, data);
3015                         break;
3016
3017                 case L2CAP_CONN_REQ:
3018                         err = l2cap_connect_req(conn, &cmd, data);
3019                         break;
3020
3021                 case L2CAP_CONN_RSP:
3022                         err = l2cap_connect_rsp(conn, &cmd, data);
3023                         break;
3024
3025                 case L2CAP_CONF_REQ:
3026                         err = l2cap_config_req(conn, &cmd, cmd_len, data);
3027                         break;
3028
3029                 case L2CAP_CONF_RSP:
3030                         err = l2cap_config_rsp(conn, &cmd, data);
3031                         break;
3032
3033                 case L2CAP_DISCONN_REQ:
3034                         err = l2cap_disconnect_req(conn, &cmd, data);
3035                         break;
3036
3037                 case L2CAP_DISCONN_RSP:
3038                         err = l2cap_disconnect_rsp(conn, &cmd, data);
3039                         break;
3040
3041                 case L2CAP_ECHO_REQ:
3042                         l2cap_send_cmd(conn, cmd.ident, L2CAP_ECHO_RSP, cmd_len, data);
3043                         break;
3044
3045                 case L2CAP_ECHO_RSP:
3046                         break;
3047
3048                 case L2CAP_INFO_REQ:
3049                         err = l2cap_information_req(conn, &cmd, data);
3050                         break;
3051
3052                 case L2CAP_INFO_RSP:
3053                         err = l2cap_information_rsp(conn, &cmd, data);
3054                         break;
3055
3056                 default:
3057                         BT_ERR("Unknown signaling command 0x%2.2x", cmd.code);
3058                         err = -EINVAL;
3059                         break;
3060                 }
3061
3062                 if (err) {
3063                         struct l2cap_cmd_rej rej;
3064                         BT_DBG("error %d", err);
3065
3066                         /* FIXME: Map err to a valid reason */
3067                         rej.reason = cpu_to_le16(0);
3068                         l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
3069                 }
3070
3071                 data += cmd_len;
3072                 len  -= cmd_len;
3073         }
3074
3075         kfree_skb(skb);
3076 }
3077
3078 static int l2cap_check_fcs(struct l2cap_pinfo *pi,  struct sk_buff *skb)
3079 {
3080         u16 our_fcs, rcv_fcs;
3081         int hdr_size = L2CAP_HDR_SIZE + 2;
3082
3083         if (pi->fcs == L2CAP_FCS_CRC16) {
3084                 skb_trim(skb, skb->len - 2);
3085                 rcv_fcs = get_unaligned_le16(skb->data + skb->len);
3086                 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
3087
3088                 if (our_fcs != rcv_fcs)
3089                         return -EINVAL;
3090         }
3091         return 0;
3092 }
3093
3094 static void l2cap_add_to_srej_queue(struct sock *sk, struct sk_buff *skb, u8 tx_seq, u8 sar)
3095 {
3096         struct sk_buff *next_skb;
3097
3098         bt_cb(skb)->tx_seq = tx_seq;
3099         bt_cb(skb)->sar = sar;
3100
3101         next_skb = skb_peek(SREJ_QUEUE(sk));
3102         if (!next_skb) {
3103                 __skb_queue_tail(SREJ_QUEUE(sk), skb);
3104                 return;
3105         }
3106
3107         do {
3108                 if (bt_cb(next_skb)->tx_seq > tx_seq) {
3109                         __skb_queue_before(SREJ_QUEUE(sk), next_skb, skb);
3110                         return;
3111                 }
3112
3113                 if (skb_queue_is_last(SREJ_QUEUE(sk), next_skb))
3114                         break;
3115
3116         } while((next_skb = skb_queue_next(SREJ_QUEUE(sk), next_skb)));
3117
3118         __skb_queue_tail(SREJ_QUEUE(sk), skb);
3119 }
3120
3121 static int l2cap_sar_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)
3122 {
3123         struct l2cap_pinfo *pi = l2cap_pi(sk);
3124         struct sk_buff *_skb;
3125         int err = -EINVAL;
3126
3127         switch (control & L2CAP_CTRL_SAR) {
3128         case L2CAP_SDU_UNSEGMENTED:
3129                 if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
3130                         kfree_skb(pi->sdu);
3131                         break;
3132                 }
3133
3134                 err = sock_queue_rcv_skb(sk, skb);
3135                 if (!err)
3136                         return 0;
3137
3138                 break;
3139
3140         case L2CAP_SDU_START:
3141                 if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
3142                         kfree_skb(pi->sdu);
3143                         break;
3144                 }
3145
3146                 pi->sdu_len = get_unaligned_le16(skb->data);
3147                 skb_pull(skb, 2);
3148
3149                 pi->sdu = bt_skb_alloc(pi->sdu_len, GFP_ATOMIC);
3150                 if (!pi->sdu) {
3151                         err = -ENOMEM;
3152                         break;
3153                 }
3154
3155                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3156
3157                 pi->conn_state |= L2CAP_CONN_SAR_SDU;
3158                 pi->partial_sdu_len = skb->len;
3159                 err = 0;
3160                 break;
3161
3162         case L2CAP_SDU_CONTINUE:
3163                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
3164                         break;
3165
3166                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3167
3168                 pi->partial_sdu_len += skb->len;
3169                 if (pi->partial_sdu_len > pi->sdu_len)
3170                         kfree_skb(pi->sdu);
3171                 else
3172                         err = 0;
3173
3174                 break;
3175
3176         case L2CAP_SDU_END:
3177                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
3178                         break;
3179
3180                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3181
3182                 pi->conn_state &= ~L2CAP_CONN_SAR_SDU;
3183                 pi->partial_sdu_len += skb->len;
3184
3185                 if (pi->partial_sdu_len == pi->sdu_len) {
3186                         _skb = skb_clone(pi->sdu, GFP_ATOMIC);
3187                         err = sock_queue_rcv_skb(sk, _skb);
3188                         if (err < 0)
3189                                 kfree_skb(_skb);
3190                 }
3191                 kfree_skb(pi->sdu);
3192                 err = 0;
3193
3194                 break;
3195         }
3196
3197         kfree_skb(skb);
3198         return err;
3199 }
3200
3201 static void l2cap_check_srej_gap(struct sock *sk, u8 tx_seq)
3202 {
3203         struct sk_buff *skb;
3204         u16 control = 0;
3205
3206         while((skb = skb_peek(SREJ_QUEUE(sk)))) {
3207                 if (bt_cb(skb)->tx_seq != tx_seq)
3208                         break;
3209
3210                 skb = skb_dequeue(SREJ_QUEUE(sk));
3211                 control |= bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
3212                 l2cap_sar_reassembly_sdu(sk, skb, control);
3213                 l2cap_pi(sk)->buffer_seq_srej =
3214                         (l2cap_pi(sk)->buffer_seq_srej + 1) % 64;
3215                 tx_seq++;
3216         }
3217 }
3218
3219 static void l2cap_resend_srejframe(struct sock *sk, u8 tx_seq)
3220 {
3221         struct l2cap_pinfo *pi = l2cap_pi(sk);
3222         struct srej_list *l, *tmp;
3223         u16 control;
3224
3225         list_for_each_entry_safe(l,tmp, SREJ_LIST(sk), list) {
3226                 if (l->tx_seq == tx_seq) {
3227                         list_del(&l->list);
3228                         kfree(l);
3229                         return;
3230                 }
3231                 control = L2CAP_SUPER_SELECT_REJECT;
3232                 control |= l->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3233                 l2cap_send_sframe(pi, control);
3234                 list_del(&l->list);
3235                 list_add_tail(&l->list, SREJ_LIST(sk));
3236         }
3237 }
3238
3239 static void l2cap_send_srejframe(struct sock *sk, u8 tx_seq)
3240 {
3241         struct l2cap_pinfo *pi = l2cap_pi(sk);
3242         struct srej_list *new;
3243         u16 control;
3244
3245         while (tx_seq != pi->expected_tx_seq) {
3246                 control = L2CAP_SUPER_SELECT_REJECT;
3247                 control |= pi->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3248                 if (pi->conn_state & L2CAP_CONN_SEND_PBIT) {
3249                         control |= L2CAP_CTRL_POLL;
3250                         pi->conn_state &= ~L2CAP_CONN_SEND_PBIT;
3251                 }
3252                 l2cap_send_sframe(pi, control);
3253
3254                 new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC);
3255                 new->tx_seq = pi->expected_tx_seq++;
3256                 list_add_tail(&new->list, SREJ_LIST(sk));
3257         }
3258         pi->expected_tx_seq++;
3259 }
3260
3261 static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
3262 {
3263         struct l2cap_pinfo *pi = l2cap_pi(sk);
3264         u8 tx_seq = __get_txseq(rx_control);
3265         u16 tx_control = 0;
3266         u8 sar = rx_control >> L2CAP_CTRL_SAR_SHIFT;
3267         int err = 0;
3268
3269         BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
3270
3271         if (tx_seq == pi->expected_tx_seq)
3272                 goto expected;
3273
3274         if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
3275                 struct srej_list *first;
3276
3277                 first = list_first_entry(SREJ_LIST(sk),
3278                                 struct srej_list, list);
3279                 if (tx_seq == first->tx_seq) {
3280                         l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
3281                         l2cap_check_srej_gap(sk, tx_seq);
3282
3283                         list_del(&first->list);
3284                         kfree(first);
3285
3286                         if (list_empty(SREJ_LIST(sk))) {
3287                                 pi->buffer_seq = pi->buffer_seq_srej;
3288                                 pi->conn_state &= ~L2CAP_CONN_SREJ_SENT;
3289                         }
3290                 } else {
3291                         struct srej_list *l;
3292                         l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
3293
3294                         list_for_each_entry(l, SREJ_LIST(sk), list) {
3295                                 if (l->tx_seq == tx_seq) {
3296                                         l2cap_resend_srejframe(sk, tx_seq);
3297                                         return 0;
3298                                 }
3299                         }
3300                         l2cap_send_srejframe(sk, tx_seq);
3301                 }
3302         } else {
3303                 pi->conn_state |= L2CAP_CONN_SREJ_SENT;
3304
3305                 INIT_LIST_HEAD(SREJ_LIST(sk));
3306                 pi->buffer_seq_srej = pi->buffer_seq;
3307
3308                 __skb_queue_head_init(SREJ_QUEUE(sk));
3309                 l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
3310
3311                 pi->conn_state |= L2CAP_CONN_SEND_PBIT;
3312
3313                 l2cap_send_srejframe(sk, tx_seq);
3314         }
3315         return 0;
3316
3317 expected:
3318         pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3319
3320         if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
3321                 l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
3322                 return 0;
3323         }
3324
3325         pi->buffer_seq = (pi->buffer_seq + 1) % 64;
3326
3327         err = l2cap_sar_reassembly_sdu(sk, skb, rx_control);
3328         if (err < 0)
3329                 return err;
3330
3331         pi->num_to_ack = (pi->num_to_ack + 1) % L2CAP_DEFAULT_NUM_TO_ACK;
3332         if (pi->num_to_ack == L2CAP_DEFAULT_NUM_TO_ACK - 1) {
3333                 tx_control |= L2CAP_SUPER_RCV_READY;
3334                 tx_control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3335                 l2cap_send_sframe(pi, tx_control);
3336         }
3337         return 0;
3338 }
3339
3340 static inline int l2cap_data_channel_sframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
3341 {
3342         struct l2cap_pinfo *pi = l2cap_pi(sk);
3343         u8 tx_seq = __get_reqseq(rx_control);
3344
3345         BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
3346
3347         switch (rx_control & L2CAP_CTRL_SUPERVISE) {
3348         case L2CAP_SUPER_RCV_READY:
3349                 if (rx_control & L2CAP_CTRL_POLL) {
3350                         u16 control = L2CAP_CTRL_FINAL;
3351                         control |= L2CAP_SUPER_RCV_READY |
3352                                 (pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT);
3353                         l2cap_send_sframe(l2cap_pi(sk), control);
3354                 } else if (rx_control & L2CAP_CTRL_FINAL) {
3355                         pi->expected_ack_seq = tx_seq;
3356                         l2cap_drop_acked_frames(sk);
3357
3358                         if (!(pi->conn_state & L2CAP_CONN_WAIT_F))
3359                                 break;
3360
3361                         pi->conn_state &= ~L2CAP_CONN_WAIT_F;
3362                         del_timer(&pi->monitor_timer);
3363
3364                         if (pi->unacked_frames > 0)
3365                                 __mod_retrans_timer();
3366                 } else {
3367                         pi->expected_ack_seq = tx_seq;
3368                         l2cap_drop_acked_frames(sk);
3369                         if (pi->unacked_frames > 0)
3370                                 __mod_retrans_timer();
3371                         l2cap_ertm_send(sk);
3372                 }
3373                 break;
3374
3375         case L2CAP_SUPER_REJECT:
3376                 pi->expected_ack_seq = __get_reqseq(rx_control);
3377                 l2cap_drop_acked_frames(sk);
3378
3379                 sk->sk_send_head = TX_QUEUE(sk)->next;
3380                 pi->next_tx_seq = pi->expected_ack_seq;
3381
3382                 l2cap_ertm_send(sk);
3383
3384                 break;
3385
3386         case L2CAP_SUPER_SELECT_REJECT:
3387                 if (rx_control & L2CAP_CTRL_POLL) {
3388                         l2cap_retransmit_frame(sk, tx_seq);
3389                         pi->expected_ack_seq = tx_seq;
3390                         l2cap_drop_acked_frames(sk);
3391                         l2cap_ertm_send(sk);
3392                         if (pi->conn_state & L2CAP_CONN_WAIT_F) {
3393                                 pi->srej_save_reqseq = tx_seq;
3394                                 pi->conn_state |= L2CAP_CONN_SREJ_ACT;
3395                         }
3396                 } else if (rx_control & L2CAP_CTRL_FINAL) {
3397                         if ((pi->conn_state & L2CAP_CONN_SREJ_ACT) &&
3398                                         pi->srej_save_reqseq == tx_seq)
3399                                 pi->srej_save_reqseq &= ~L2CAP_CONN_SREJ_ACT;
3400                         else
3401                                 l2cap_retransmit_frame(sk, tx_seq);
3402                 }
3403                 else {
3404                         l2cap_retransmit_frame(sk, tx_seq);
3405                         if (pi->conn_state & L2CAP_CONN_WAIT_F) {
3406                                 pi->srej_save_reqseq = tx_seq;
3407                                 pi->conn_state |= L2CAP_CONN_SREJ_ACT;
3408                         }
3409                 }
3410                 break;
3411
3412         case L2CAP_SUPER_RCV_NOT_READY:
3413                 break;
3414         }
3415
3416         return 0;
3417 }
3418
3419 static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
3420 {
3421         struct sock *sk;
3422         struct l2cap_pinfo *pi;
3423         u16 control, len;
3424         u8 tx_seq;
3425         int err;
3426
3427         sk = l2cap_get_chan_by_scid(&conn->chan_list, cid);
3428         if (!sk) {
3429                 BT_DBG("unknown cid 0x%4.4x", cid);
3430                 goto drop;
3431         }
3432
3433         pi = l2cap_pi(sk);
3434
3435         BT_DBG("sk %p, len %d", sk, skb->len);
3436
3437         if (sk->sk_state != BT_CONNECTED)
3438                 goto drop;
3439
3440         switch (pi->mode) {
3441         case L2CAP_MODE_BASIC:
3442                 /* If socket recv buffers overflows we drop data here
3443                  * which is *bad* because L2CAP has to be reliable.
3444                  * But we don't have any other choice. L2CAP doesn't
3445                  * provide flow control mechanism. */
3446
3447                 if (pi->imtu < skb->len)
3448                         goto drop;
3449
3450                 if (!sock_queue_rcv_skb(sk, skb))
3451                         goto done;
3452                 break;
3453
3454         case L2CAP_MODE_ERTM:
3455                 control = get_unaligned_le16(skb->data);
3456                 skb_pull(skb, 2);
3457                 len = skb->len;
3458
3459                 if (__is_sar_start(control))
3460                         len -= 2;
3461
3462                 if (pi->fcs == L2CAP_FCS_CRC16)
3463                         len -= 2;
3464
3465                 /*
3466                  * We can just drop the corrupted I-frame here.
3467                  * Receiver will miss it and start proper recovery
3468                  * procedures and ask retransmission.
3469                  */
3470                 if (len > L2CAP_DEFAULT_MAX_PDU_SIZE)
3471                         goto drop;
3472
3473                 if (l2cap_check_fcs(pi, skb))
3474                         goto drop;
3475
3476                 if (__is_iframe(control))
3477                         err = l2cap_data_channel_iframe(sk, control, skb);
3478                 else
3479                         err = l2cap_data_channel_sframe(sk, control, skb);
3480
3481                 if (!err)
3482                         goto done;
3483                 break;
3484
3485         case L2CAP_MODE_STREAMING:
3486                 control = get_unaligned_le16(skb->data);
3487                 skb_pull(skb, 2);
3488                 len = skb->len;
3489
3490                 if (__is_sar_start(control))
3491                         len -= 2;
3492
3493                 if (pi->fcs == L2CAP_FCS_CRC16)
3494                         len -= 2;
3495
3496                 if (len > L2CAP_DEFAULT_MAX_PDU_SIZE || __is_sframe(control))
3497                         goto drop;
3498
3499                 if (l2cap_check_fcs(pi, skb))
3500                         goto drop;
3501
3502                 tx_seq = __get_txseq(control);
3503
3504                 if (pi->expected_tx_seq == tx_seq)
3505                         pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3506                 else
3507                         pi->expected_tx_seq = tx_seq + 1;
3508
3509                 err = l2cap_sar_reassembly_sdu(sk, skb, control);
3510
3511                 goto done;
3512
3513         default:
3514                 BT_DBG("sk %p: bad mode 0x%2.2x", sk, l2cap_pi(sk)->mode);
3515                 break;
3516         }
3517
3518 drop:
3519         kfree_skb(skb);
3520
3521 done:
3522         if (sk)
3523                 bh_unlock_sock(sk);
3524
3525         return 0;
3526 }
3527
3528 static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb)
3529 {
3530         struct sock *sk;
3531
3532         sk = l2cap_get_sock_by_psm(0, psm, conn->src);
3533         if (!sk)
3534                 goto drop;
3535
3536         BT_DBG("sk %p, len %d", sk, skb->len);
3537
3538         if (sk->sk_state != BT_BOUND && sk->sk_state != BT_CONNECTED)
3539                 goto drop;
3540
3541         if (l2cap_pi(sk)->imtu < skb->len)
3542                 goto drop;
3543
3544         if (!sock_queue_rcv_skb(sk, skb))
3545                 goto done;
3546
3547 drop:
3548         kfree_skb(skb);
3549
3550 done:
3551         if (sk)
3552                 bh_unlock_sock(sk);
3553         return 0;
3554 }
3555
3556 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
3557 {
3558         struct l2cap_hdr *lh = (void *) skb->data;
3559         u16 cid, len;
3560         __le16 psm;
3561
3562         skb_pull(skb, L2CAP_HDR_SIZE);
3563         cid = __le16_to_cpu(lh->cid);
3564         len = __le16_to_cpu(lh->len);
3565
3566         if (len != skb->len) {
3567                 kfree_skb(skb);
3568                 return;
3569         }
3570
3571         BT_DBG("len %d, cid 0x%4.4x", len, cid);
3572
3573         switch (cid) {
3574         case L2CAP_CID_SIGNALING:
3575                 l2cap_sig_channel(conn, skb);
3576                 break;
3577
3578         case L2CAP_CID_CONN_LESS:
3579                 psm = get_unaligned_le16(skb->data);
3580                 skb_pull(skb, 2);
3581                 l2cap_conless_channel(conn, psm, skb);
3582                 break;
3583
3584         default:
3585                 l2cap_data_channel(conn, cid, skb);
3586                 break;
3587         }
3588 }
3589
3590 /* ---- L2CAP interface with lower layer (HCI) ---- */
3591
3592 static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
3593 {
3594         int exact = 0, lm1 = 0, lm2 = 0;
3595         register struct sock *sk;
3596         struct hlist_node *node;
3597
3598         if (type != ACL_LINK)
3599                 return 0;
3600
3601         BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
3602
3603         /* Find listening sockets and check their link_mode */
3604         read_lock(&l2cap_sk_list.lock);
3605         sk_for_each(sk, node, &l2cap_sk_list.head) {
3606                 if (sk->sk_state != BT_LISTEN)
3607                         continue;
3608
3609                 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
3610                         lm1 |= HCI_LM_ACCEPT;
3611                         if (l2cap_pi(sk)->role_switch)
3612                                 lm1 |= HCI_LM_MASTER;
3613                         exact++;
3614                 } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
3615                         lm2 |= HCI_LM_ACCEPT;
3616                         if (l2cap_pi(sk)->role_switch)
3617                                 lm2 |= HCI_LM_MASTER;
3618                 }
3619         }
3620         read_unlock(&l2cap_sk_list.lock);
3621
3622         return exact ? lm1 : lm2;
3623 }
3624
3625 static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
3626 {
3627         struct l2cap_conn *conn;
3628
3629         BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
3630
3631         if (hcon->type != ACL_LINK)
3632                 return 0;
3633
3634         if (!status) {
3635                 conn = l2cap_conn_add(hcon, status);
3636                 if (conn)
3637                         l2cap_conn_ready(conn);
3638         } else
3639                 l2cap_conn_del(hcon, bt_err(status));
3640
3641         return 0;
3642 }
3643
3644 static int l2cap_disconn_ind(struct hci_conn *hcon)
3645 {
3646         struct l2cap_conn *conn = hcon->l2cap_data;
3647
3648         BT_DBG("hcon %p", hcon);
3649
3650         if (hcon->type != ACL_LINK || !conn)
3651                 return 0x13;
3652
3653         return conn->disc_reason;
3654 }
3655
3656 static int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
3657 {
3658         BT_DBG("hcon %p reason %d", hcon, reason);
3659
3660         if (hcon->type != ACL_LINK)
3661                 return 0;
3662
3663         l2cap_conn_del(hcon, bt_err(reason));
3664
3665         return 0;
3666 }
3667
3668 static inline void l2cap_check_encryption(struct sock *sk, u8 encrypt)
3669 {
3670         if (sk->sk_type != SOCK_SEQPACKET)
3671                 return;
3672
3673         if (encrypt == 0x00) {
3674                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_MEDIUM) {
3675                         l2cap_sock_clear_timer(sk);
3676                         l2cap_sock_set_timer(sk, HZ * 5);
3677                 } else if (l2cap_pi(sk)->sec_level == BT_SECURITY_HIGH)
3678                         __l2cap_sock_close(sk, ECONNREFUSED);
3679         } else {
3680                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_MEDIUM)
3681                         l2cap_sock_clear_timer(sk);
3682         }
3683 }
3684
3685 static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
3686 {
3687         struct l2cap_chan_list *l;
3688         struct l2cap_conn *conn = hcon->l2cap_data;
3689         struct sock *sk;
3690
3691         if (!conn)
3692                 return 0;
3693
3694         l = &conn->chan_list;
3695
3696         BT_DBG("conn %p", conn);
3697
3698         read_lock(&l->lock);
3699
3700         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
3701                 bh_lock_sock(sk);
3702
3703                 if (l2cap_pi(sk)->conf_state & L2CAP_CONF_CONNECT_PEND) {
3704                         bh_unlock_sock(sk);
3705                         continue;
3706                 }
3707
3708                 if (!status && (sk->sk_state == BT_CONNECTED ||
3709                                                 sk->sk_state == BT_CONFIG)) {
3710                         l2cap_check_encryption(sk, encrypt);
3711                         bh_unlock_sock(sk);
3712                         continue;
3713                 }
3714
3715                 if (sk->sk_state == BT_CONNECT) {
3716                         if (!status) {
3717                                 struct l2cap_conn_req req;
3718                                 req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
3719                                 req.psm  = l2cap_pi(sk)->psm;
3720
3721                                 l2cap_pi(sk)->ident = l2cap_get_ident(conn);
3722
3723                                 l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
3724                                         L2CAP_CONN_REQ, sizeof(req), &req);
3725                         } else {
3726                                 l2cap_sock_clear_timer(sk);
3727                                 l2cap_sock_set_timer(sk, HZ / 10);
3728                         }
3729                 } else if (sk->sk_state == BT_CONNECT2) {
3730                         struct l2cap_conn_rsp rsp;
3731                         __u16 result;
3732
3733                         if (!status) {
3734                                 sk->sk_state = BT_CONFIG;
3735                                 result = L2CAP_CR_SUCCESS;
3736                         } else {
3737                                 sk->sk_state = BT_DISCONN;
3738                                 l2cap_sock_set_timer(sk, HZ / 10);
3739                                 result = L2CAP_CR_SEC_BLOCK;
3740                         }
3741
3742                         rsp.scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
3743                         rsp.dcid   = cpu_to_le16(l2cap_pi(sk)->scid);
3744                         rsp.result = cpu_to_le16(result);
3745                         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
3746                         l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
3747                                         L2CAP_CONN_RSP, sizeof(rsp), &rsp);
3748                 }
3749
3750                 bh_unlock_sock(sk);
3751         }
3752
3753         read_unlock(&l->lock);
3754
3755         return 0;
3756 }
3757
3758 static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
3759 {
3760         struct l2cap_conn *conn = hcon->l2cap_data;
3761
3762         if (!conn && !(conn = l2cap_conn_add(hcon, 0)))
3763                 goto drop;
3764
3765         BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
3766
3767         if (flags & ACL_START) {
3768                 struct l2cap_hdr *hdr;
3769                 int len;
3770
3771                 if (conn->rx_len) {
3772                         BT_ERR("Unexpected start frame (len %d)", skb->len);
3773                         kfree_skb(conn->rx_skb);
3774                         conn->rx_skb = NULL;
3775                         conn->rx_len = 0;
3776                         l2cap_conn_unreliable(conn, ECOMM);
3777                 }
3778
3779                 if (skb->len < 2) {
3780                         BT_ERR("Frame is too short (len %d)", skb->len);
3781                         l2cap_conn_unreliable(conn, ECOMM);
3782                         goto drop;
3783                 }
3784
3785                 hdr = (struct l2cap_hdr *) skb->data;
3786                 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
3787
3788                 if (len == skb->len) {
3789                         /* Complete frame received */
3790                         l2cap_recv_frame(conn, skb);
3791                         return 0;
3792                 }
3793
3794                 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
3795
3796                 if (skb->len > len) {
3797                         BT_ERR("Frame is too long (len %d, expected len %d)",
3798                                 skb->len, len);
3799                         l2cap_conn_unreliable(conn, ECOMM);
3800                         goto drop;
3801                 }
3802
3803                 /* Allocate skb for the complete frame (with header) */
3804                 conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC);
3805                 if (!conn->rx_skb)
3806                         goto drop;
3807
3808                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
3809                                                                 skb->len);
3810                 conn->rx_len = len - skb->len;
3811         } else {
3812                 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
3813
3814                 if (!conn->rx_len) {
3815                         BT_ERR("Unexpected continuation frame (len %d)", skb->len);
3816                         l2cap_conn_unreliable(conn, ECOMM);
3817                         goto drop;
3818                 }
3819
3820                 if (skb->len > conn->rx_len) {
3821                         BT_ERR("Fragment is too long (len %d, expected %d)",
3822                                         skb->len, conn->rx_len);
3823                         kfree_skb(conn->rx_skb);
3824                         conn->rx_skb = NULL;
3825                         conn->rx_len = 0;
3826                         l2cap_conn_unreliable(conn, ECOMM);
3827                         goto drop;
3828                 }
3829
3830                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
3831                                                                 skb->len);
3832                 conn->rx_len -= skb->len;
3833
3834                 if (!conn->rx_len) {
3835                         /* Complete frame received */
3836                         l2cap_recv_frame(conn, conn->rx_skb);
3837                         conn->rx_skb = NULL;
3838                 }
3839         }
3840
3841 drop:
3842         kfree_skb(skb);
3843         return 0;
3844 }
3845
3846 static ssize_t l2cap_sysfs_show(struct class *dev, char *buf)
3847 {
3848         struct sock *sk;
3849         struct hlist_node *node;
3850         char *str = buf;
3851
3852         read_lock_bh(&l2cap_sk_list.lock);
3853
3854         sk_for_each(sk, node, &l2cap_sk_list.head) {
3855                 struct l2cap_pinfo *pi = l2cap_pi(sk);
3856
3857                 str += sprintf(str, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d\n",
3858                                 batostr(&bt_sk(sk)->src), batostr(&bt_sk(sk)->dst),
3859                                 sk->sk_state, __le16_to_cpu(pi->psm), pi->scid,
3860                                 pi->dcid, pi->imtu, pi->omtu, pi->sec_level);
3861         }
3862
3863         read_unlock_bh(&l2cap_sk_list.lock);
3864
3865         return str - buf;
3866 }
3867
3868 static CLASS_ATTR(l2cap, S_IRUGO, l2cap_sysfs_show, NULL);
3869
3870 static const struct proto_ops l2cap_sock_ops = {
3871         .family         = PF_BLUETOOTH,
3872         .owner          = THIS_MODULE,
3873         .release        = l2cap_sock_release,
3874         .bind           = l2cap_sock_bind,
3875         .connect        = l2cap_sock_connect,
3876         .listen         = l2cap_sock_listen,
3877         .accept         = l2cap_sock_accept,
3878         .getname        = l2cap_sock_getname,
3879         .sendmsg        = l2cap_sock_sendmsg,
3880         .recvmsg        = l2cap_sock_recvmsg,
3881         .poll           = bt_sock_poll,
3882         .ioctl          = bt_sock_ioctl,
3883         .mmap           = sock_no_mmap,
3884         .socketpair     = sock_no_socketpair,
3885         .shutdown       = l2cap_sock_shutdown,
3886         .setsockopt     = l2cap_sock_setsockopt,
3887         .getsockopt     = l2cap_sock_getsockopt
3888 };
3889
3890 static struct net_proto_family l2cap_sock_family_ops = {
3891         .family = PF_BLUETOOTH,
3892         .owner  = THIS_MODULE,
3893         .create = l2cap_sock_create,
3894 };
3895
3896 static struct hci_proto l2cap_hci_proto = {
3897         .name           = "L2CAP",
3898         .id             = HCI_PROTO_L2CAP,
3899         .connect_ind    = l2cap_connect_ind,
3900         .connect_cfm    = l2cap_connect_cfm,
3901         .disconn_ind    = l2cap_disconn_ind,
3902         .disconn_cfm    = l2cap_disconn_cfm,
3903         .security_cfm   = l2cap_security_cfm,
3904         .recv_acldata   = l2cap_recv_acldata
3905 };
3906
3907 static int __init l2cap_init(void)
3908 {
3909         int err;
3910
3911         err = proto_register(&l2cap_proto, 0);
3912         if (err < 0)
3913                 return err;
3914
3915         err = bt_sock_register(BTPROTO_L2CAP, &l2cap_sock_family_ops);
3916         if (err < 0) {
3917                 BT_ERR("L2CAP socket registration failed");
3918                 goto error;
3919         }
3920
3921         err = hci_register_proto(&l2cap_hci_proto);
3922         if (err < 0) {
3923                 BT_ERR("L2CAP protocol registration failed");
3924                 bt_sock_unregister(BTPROTO_L2CAP);
3925                 goto error;
3926         }
3927
3928         if (class_create_file(bt_class, &class_attr_l2cap) < 0)
3929                 BT_ERR("Failed to create L2CAP info file");
3930
3931         BT_INFO("L2CAP ver %s", VERSION);
3932         BT_INFO("L2CAP socket layer initialized");
3933
3934         return 0;
3935
3936 error:
3937         proto_unregister(&l2cap_proto);
3938         return err;
3939 }
3940
3941 static void __exit l2cap_exit(void)
3942 {
3943         class_remove_file(bt_class, &class_attr_l2cap);
3944
3945         if (bt_sock_unregister(BTPROTO_L2CAP) < 0)
3946                 BT_ERR("L2CAP socket unregistration failed");
3947
3948         if (hci_unregister_proto(&l2cap_hci_proto) < 0)
3949                 BT_ERR("L2CAP protocol unregistration failed");
3950
3951         proto_unregister(&l2cap_proto);
3952 }
3953
3954 void l2cap_load(void)
3955 {
3956         /* Dummy function to trigger automatic L2CAP module loading by
3957          * other modules that use L2CAP sockets but don't use any other
3958          * symbols from it. */
3959         return;
3960 }
3961 EXPORT_SYMBOL(l2cap_load);
3962
3963 module_init(l2cap_init);
3964 module_exit(l2cap_exit);
3965
3966 module_param(enable_ertm, bool, 0644);
3967 MODULE_PARM_DESC(enable_ertm, "Enable enhanced retransmission mode");
3968
3969 MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
3970 MODULE_DESCRIPTION("Bluetooth L2CAP ver " VERSION);
3971 MODULE_VERSION(VERSION);
3972 MODULE_LICENSE("GPL");
3973 MODULE_ALIAS("bt-proto-0");