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