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