net/bluetooth/l2cap_core.c

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