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