net/bluetooth/l2cap_core.c

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