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