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