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