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 #include <linux/filter.h>
  36
  37 #include <net/bluetooth/bluetooth.h>
  38 #include <net/bluetooth/hci_core.h>
  39 #include <net/bluetooth/l2cap.h>
  40
  41 #include "smp.h"
  42 #include "a2mp.h"
  43 #include "amp.h"
  44
  45 #define LE_FLOWCTL_MAX_CREDITS 65535
  46
  47 bool disable_ertm;
  48
  49 static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN | L2CAP_FEAT_UCD;
  50
  51 static LIST_HEAD(chan_list);
  52 static DEFINE_RWLOCK(chan_list_lock);
  53
  54 static u16 le_max_credits = L2CAP_LE_MAX_CREDITS;
  55 static u16 le_default_mps = L2CAP_LE_DEFAULT_MPS;
  56
  57 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
  58                                        u8 code, u8 ident, u16 dlen, void *data);
  59 static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len,
  60                            void *data);
  61 static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data, size_t data_size);
  62 static void l2cap_send_disconn_req(struct l2cap_chan *chan, int err);
  63
  64 static void l2cap_tx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
  65                      struct sk_buff_head *skbs, u8 event);
  66
  67 static inline u8 bdaddr_type(u8 link_type, u8 bdaddr_type)
  68 {
  69         if (link_type == LE_LINK) {
  70                 if (bdaddr_type == ADDR_LE_DEV_PUBLIC)
  71                         return BDADDR_LE_PUBLIC;
  72                 else
  73                         return BDADDR_LE_RANDOM;
  74         }
  75
  76         return BDADDR_BREDR;
  77 }
  78
  79 static inline u8 bdaddr_src_type(struct hci_conn *hcon)
  80 {
  81         return bdaddr_type(hcon->type, hcon->src_type);
  82 }
  83
  84 static inline u8 bdaddr_dst_type(struct hci_conn *hcon)
  85 {
  86         return bdaddr_type(hcon->type, hcon->dst_type);
  87 }
  88
  89 /* ---- L2CAP channels ---- */
  90
  91 static struct l2cap_chan *__l2cap_get_chan_by_dcid(struct l2cap_conn *conn,
  92                                                    u16 cid)
  93 {
  94         struct l2cap_chan *c;
  95
  96         list_for_each_entry(c, &conn->chan_l, list) {
  97                 if (c->dcid == cid)
  98                         return c;
  99         }
 100         return NULL;
 101 }
 102
 103 static struct l2cap_chan *__l2cap_get_chan_by_scid(struct l2cap_conn *conn,
 104                                                    u16 cid)
 105 {
 106         struct l2cap_chan *c;
 107
 108         list_for_each_entry(c, &conn->chan_l, list) {
 109                 if (c->scid == cid)
 110                         return c;
 111         }
 112         return NULL;
 113 }
 114
 115 /* Find channel with given SCID.
 116  * Returns locked channel. */
 117 static struct l2cap_chan *l2cap_get_chan_by_scid(struct l2cap_conn *conn,
 118                                                  u16 cid)
 119 {
 120         struct l2cap_chan *c;
 121
 122         mutex_lock(&conn->chan_lock);
 123         c = __l2cap_get_chan_by_scid(conn, cid);
 124         if (c)
 125                 l2cap_chan_lock(c);
 126         mutex_unlock(&conn->chan_lock);
 127
 128         return c;
 129 }
 130
 131 /* Find channel with given DCID.
 132  * Returns locked channel.
 133  */
 134 static struct l2cap_chan *l2cap_get_chan_by_dcid(struct l2cap_conn *conn,
 135                                                  u16 cid)
 136 {
 137         struct l2cap_chan *c;
 138
 139         mutex_lock(&conn->chan_lock);
 140         c = __l2cap_get_chan_by_dcid(conn, cid);
 141         if (c)
 142                 l2cap_chan_lock(c);
 143         mutex_unlock(&conn->chan_lock);
 144
 145         return c;
 146 }
 147
 148 static struct l2cap_chan *__l2cap_get_chan_by_ident(struct l2cap_conn *conn,
 149                                                     u8 ident)
 150 {
 151         struct l2cap_chan *c;
 152
 153         list_for_each_entry(c, &conn->chan_l, list) {
 154                 if (c->ident == ident)
 155                         return c;
 156         }
 157         return NULL;
 158 }
 159
 160 static struct l2cap_chan *l2cap_get_chan_by_ident(struct l2cap_conn *conn,
 161                                                   u8 ident)
 162 {
 163         struct l2cap_chan *c;
 164
 165         mutex_lock(&conn->chan_lock);
 166         c = __l2cap_get_chan_by_ident(conn, ident);
 167         if (c)
 168                 l2cap_chan_lock(c);
 169         mutex_unlock(&conn->chan_lock);
 170
 171         return c;
 172 }
 173
 174 static struct l2cap_chan *__l2cap_global_chan_by_addr(__le16 psm, bdaddr_t *src)
 175 {
 176         struct l2cap_chan *c;
 177
 178         list_for_each_entry(c, &chan_list, global_l) {
 179                 if (c->sport == psm && !bacmp(&c->src, src))
 180                         return c;
 181         }
 182         return NULL;
 183 }
 184
 185 int l2cap_add_psm(struct l2cap_chan *chan, bdaddr_t *src, __le16 psm)
 186 {
 187         int err;
 188
 189         write_lock(&chan_list_lock);
 190
 191         if (psm && __l2cap_global_chan_by_addr(psm, src)) {
 192                 err = -EADDRINUSE;
 193                 goto done;
 194         }
 195
 196         if (psm) {
 197                 chan->psm = psm;
 198                 chan->sport = psm;
 199                 err = 0;
 200         } else {
 201                 u16 p, start, end, incr;
 202
 203                 if (chan->src_type == BDADDR_BREDR) {
 204                         start = L2CAP_PSM_DYN_START;
 205                         end = L2CAP_PSM_AUTO_END;
 206                         incr = 2;
 207                 } else {
 208                         start = L2CAP_PSM_LE_DYN_START;
 209                         end = L2CAP_PSM_LE_DYN_END;
 210                         incr = 1;
 211                 }
 212
 213                 err = -EINVAL;
 214                 for (p = start; p <= end; p += incr)
 215                         if (!__l2cap_global_chan_by_addr(cpu_to_le16(p), src)) {
 216                                 chan->psm   = cpu_to_le16(p);
 217                                 chan->sport = cpu_to_le16(p);
 218                                 err = 0;
 219                                 break;
 220                         }
 221         }
 222
 223 done:
 224         write_unlock(&chan_list_lock);
 225         return err;
 226 }
 227 EXPORT_SYMBOL_GPL(l2cap_add_psm);
 228
 229 int l2cap_add_scid(struct l2cap_chan *chan,  __u16 scid)
 230 {
 231         write_lock(&chan_list_lock);
 232
 233         /* Override the defaults (which are for conn-oriented) */
 234         chan->omtu = L2CAP_DEFAULT_MTU;
 235         chan->chan_type = L2CAP_CHAN_FIXED;
 236
 237         chan->scid = scid;
 238
 239         write_unlock(&chan_list_lock);
 240
 241         return 0;
 242 }
 243
 244 static u16 l2cap_alloc_cid(struct l2cap_conn *conn)
 245 {
 246         u16 cid, dyn_end;
 247
 248         if (conn->hcon->type == LE_LINK)
 249                 dyn_end = L2CAP_CID_LE_DYN_END;
 250         else
 251                 dyn_end = L2CAP_CID_DYN_END;
 252
 253         for (cid = L2CAP_CID_DYN_START; cid <= dyn_end; cid++) {
 254                 if (!__l2cap_get_chan_by_scid(conn, cid))
 255                         return cid;
 256         }
 257
 258         return 0;
 259 }
 260
 261 static void l2cap_state_change(struct l2cap_chan *chan, int state)
 262 {
 263         BT_DBG("chan %p %s -> %s", chan, state_to_string(chan->state),
 264                state_to_string(state));
 265
 266         chan->state = state;
 267         chan->ops->state_change(chan, state, 0);
 268 }
 269
 270 static inline void l2cap_state_change_and_error(struct l2cap_chan *chan,
 271                                                 int state, int err)
 272 {
 273         chan->state = state;
 274         chan->ops->state_change(chan, chan->state, err);
 275 }
 276
 277 static inline void l2cap_chan_set_err(struct l2cap_chan *chan, int err)
 278 {
 279         chan->ops->state_change(chan, chan->state, err);
 280 }
 281
 282 static void __set_retrans_timer(struct l2cap_chan *chan)
 283 {
 284         if (!delayed_work_pending(&chan->monitor_timer) &&
 285             chan->retrans_timeout) {
 286                 l2cap_set_timer(chan, &chan->retrans_timer,
 287                                 msecs_to_jiffies(chan->retrans_timeout));
 288         }
 289 }
 290
 291 static void __set_monitor_timer(struct l2cap_chan *chan)
 292 {
 293         __clear_retrans_timer(chan);
 294         if (chan->monitor_timeout) {
 295                 l2cap_set_timer(chan, &chan->monitor_timer,
 296                                 msecs_to_jiffies(chan->monitor_timeout));
 297         }
 298 }
 299
 300 static struct sk_buff *l2cap_ertm_seq_in_queue(struct sk_buff_head *head,
 301                                                u16 seq)
 302 {
 303         struct sk_buff *skb;
 304
 305         skb_queue_walk(head, skb) {
 306                 if (bt_cb(skb)->l2cap.txseq == seq)
 307                         return skb;
 308         }
 309
 310         return NULL;
 311 }
 312
 313 /* ---- L2CAP sequence number lists ---- */
 314
 315 /* For ERTM, ordered lists of sequence numbers must be tracked for
 316  * SREJ requests that are received and for frames that are to be
 317  * retransmitted. These seq_list functions implement a singly-linked
 318  * list in an array, where membership in the list can also be checked
 319  * in constant time. Items can also be added to the tail of the list
 320  * and removed from the head in constant time, without further memory
 321  * allocs or frees.
 322  */
 323
 324 static int l2cap_seq_list_init(struct l2cap_seq_list *seq_list, u16 size)
 325 {
 326         size_t alloc_size, i;
 327
 328         /* Allocated size is a power of 2 to map sequence numbers
 329          * (which may be up to 14 bits) in to a smaller array that is
 330          * sized for the negotiated ERTM transmit windows.
 331          */
 332         alloc_size = roundup_pow_of_two(size);
 333
 334         seq_list->list = kmalloc(sizeof(u16) * alloc_size, GFP_KERNEL);
 335         if (!seq_list->list)
 336                 return -ENOMEM;
 337
 338         seq_list->mask = alloc_size - 1;
 339         seq_list->head = L2CAP_SEQ_LIST_CLEAR;
 340         seq_list->tail = L2CAP_SEQ_LIST_CLEAR;
 341         for (i = 0; i < alloc_size; i++)
 342                 seq_list->list[i] = L2CAP_SEQ_LIST_CLEAR;
 343
 344         return 0;
 345 }
 346
 347 static inline void l2cap_seq_list_free(struct l2cap_seq_list *seq_list)
 348 {
 349         kfree(seq_list->list);
 350 }
 351
 352 static inline bool l2cap_seq_list_contains(struct l2cap_seq_list *seq_list,
 353                                            u16 seq)
 354 {
 355         /* Constant-time check for list membership */
 356         return seq_list->list[seq & seq_list->mask] != L2CAP_SEQ_LIST_CLEAR;
 357 }
 358
 359 static inline u16 l2cap_seq_list_pop(struct l2cap_seq_list *seq_list)
 360 {
 361         u16 seq = seq_list->head;
 362         u16 mask = seq_list->mask;
 363
 364         seq_list->head = seq_list->list[seq & mask];
 365         seq_list->list[seq & mask] = L2CAP_SEQ_LIST_CLEAR;
 366
 367         if (seq_list->head == L2CAP_SEQ_LIST_TAIL) {
 368                 seq_list->head = L2CAP_SEQ_LIST_CLEAR;
 369                 seq_list->tail = L2CAP_SEQ_LIST_CLEAR;
 370         }
 371
 372         return seq;
 373 }
 374
 375 static void l2cap_seq_list_clear(struct l2cap_seq_list *seq_list)
 376 {
 377         u16 i;
 378
 379         if (seq_list->head == L2CAP_SEQ_LIST_CLEAR)
 380                 return;
 381
 382         for (i = 0; i <= seq_list->mask; i++)
 383                 seq_list->list[i] = L2CAP_SEQ_LIST_CLEAR;
 384
 385         seq_list->head = L2CAP_SEQ_LIST_CLEAR;
 386         seq_list->tail = L2CAP_SEQ_LIST_CLEAR;
 387 }
 388
 389 static void l2cap_seq_list_append(struct l2cap_seq_list *seq_list, u16 seq)
 390 {
 391         u16 mask = seq_list->mask;
 392
 393         /* All appends happen in constant time */
 394
 395         if (seq_list->list[seq & mask] != L2CAP_SEQ_LIST_CLEAR)
 396                 return;
 397
 398         if (seq_list->tail == L2CAP_SEQ_LIST_CLEAR)
 399                 seq_list->head = seq;
 400         else
 401                 seq_list->list[seq_list->tail & mask] = seq;
 402
 403         seq_list->tail = seq;
 404         seq_list->list[seq & mask] = L2CAP_SEQ_LIST_TAIL;
 405 }
 406
 407 static void l2cap_chan_timeout(struct work_struct *work)
 408 {
 409         struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
 410                                                chan_timer.work);
 411         struct l2cap_conn *conn = chan->conn;
 412         int reason;
 413
 414         BT_DBG("chan %p state %s", chan, state_to_string(chan->state));
 415
 416         mutex_lock(&conn->chan_lock);
 417         l2cap_chan_lock(chan);
 418
 419         if (chan->state == BT_CONNECTED || chan->state == BT_CONFIG)
 420                 reason = ECONNREFUSED;
 421         else if (chan->state == BT_CONNECT &&
 422                  chan->sec_level != BT_SECURITY_SDP)
 423                 reason = ECONNREFUSED;
 424         else
 425                 reason = ETIMEDOUT;
 426
 427         l2cap_chan_close(chan, reason);
 428
 429         l2cap_chan_unlock(chan);
 430
 431         chan->ops->close(chan);
 432         mutex_unlock(&conn->chan_lock);
 433
 434         l2cap_chan_put(chan);
 435 }
 436
 437 struct l2cap_chan *l2cap_chan_create(void)
 438 {
 439         struct l2cap_chan *chan;
 440
 441         chan = kzalloc(sizeof(*chan), GFP_ATOMIC);
 442         if (!chan)
 443                 return NULL;
 444
 445         mutex_init(&chan->lock);
 446
 447         /* Set default lock nesting level */
 448         atomic_set(&chan->nesting, L2CAP_NESTING_NORMAL);
 449
 450         write_lock(&chan_list_lock);
 451         list_add(&chan->global_l, &chan_list);
 452         write_unlock(&chan_list_lock);
 453
 454         INIT_DELAYED_WORK(&chan->chan_timer, l2cap_chan_timeout);
 455
 456         chan->state = BT_OPEN;
 457
 458         kref_init(&chan->kref);
 459
 460         /* This flag is cleared in l2cap_chan_ready() */
 461         set_bit(CONF_NOT_COMPLETE, &chan->conf_state);
 462
 463         BT_DBG("chan %p", chan);
 464
 465         return chan;
 466 }
 467 EXPORT_SYMBOL_GPL(l2cap_chan_create);
 468
 469 static void l2cap_chan_destroy(struct kref *kref)
 470 {
 471         struct l2cap_chan *chan = container_of(kref, struct l2cap_chan, kref);
 472
 473         BT_DBG("chan %p", chan);
 474
 475         write_lock(&chan_list_lock);
 476         list_del(&chan->global_l);
 477         write_unlock(&chan_list_lock);
 478
 479         kfree(chan);
 480 }
 481
 482 void l2cap_chan_hold(struct l2cap_chan *c)
 483 {
 484         BT_DBG("chan %p orig refcnt %d", c, kref_read(&c->kref));
 485
 486         kref_get(&c->kref);
 487 }
 488
 489 void l2cap_chan_put(struct l2cap_chan *c)
 490 {
 491         BT_DBG("chan %p orig refcnt %d", c, kref_read(&c->kref));
 492
 493         kref_put(&c->kref, l2cap_chan_destroy);
 494 }
 495 EXPORT_SYMBOL_GPL(l2cap_chan_put);
 496
 497 void l2cap_chan_set_defaults(struct l2cap_chan *chan)
 498 {
 499         chan->fcs  = L2CAP_FCS_CRC16;
 500         chan->max_tx = L2CAP_DEFAULT_MAX_TX;
 501         chan->tx_win = L2CAP_DEFAULT_TX_WINDOW;
 502         chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
 503         chan->remote_max_tx = chan->max_tx;
 504         chan->remote_tx_win = chan->tx_win;
 505         chan->ack_win = L2CAP_DEFAULT_TX_WINDOW;
 506         chan->sec_level = BT_SECURITY_LOW;
 507         chan->flush_to = L2CAP_DEFAULT_FLUSH_TO;
 508         chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO;
 509         chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO;
 510         chan->conf_state = 0;
 511
 512         set_bit(FLAG_FORCE_ACTIVE, &chan->flags);
 513 }
 514 EXPORT_SYMBOL_GPL(l2cap_chan_set_defaults);
 515
 516 static void l2cap_le_flowctl_init(struct l2cap_chan *chan)
 517 {
 518         chan->sdu = NULL;
 519         chan->sdu_last_frag = NULL;
 520         chan->sdu_len = 0;
 521         chan->tx_credits = 0;
 522         chan->rx_credits = le_max_credits;
 523         chan->mps = min_t(u16, chan->imtu, le_default_mps);
 524
 525         skb_queue_head_init(&chan->tx_q);
 526 }
 527
 528 void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 529 {
 530         BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn,
 531                __le16_to_cpu(chan->psm), chan->dcid);
 532
 533         conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM;
 534
 535         chan->conn = conn;
 536
 537         switch (chan->chan_type) {
 538         case L2CAP_CHAN_CONN_ORIENTED:
 539                 /* Alloc CID for connection-oriented socket */
 540                 chan->scid = l2cap_alloc_cid(conn);
 541                 if (conn->hcon->type == ACL_LINK)
 542                         chan->omtu = L2CAP_DEFAULT_MTU;
 543                 break;
 544
 545         case L2CAP_CHAN_CONN_LESS:
 546                 /* Connectionless socket */
 547                 chan->scid = L2CAP_CID_CONN_LESS;
 548                 chan->dcid = L2CAP_CID_CONN_LESS;
 549                 chan->omtu = L2CAP_DEFAULT_MTU;
 550                 break;
 551
 552         case L2CAP_CHAN_FIXED:
 553                 /* Caller will set CID and CID specific MTU values */
 554                 break;
 555
 556         default:
 557                 /* Raw socket can send/recv signalling messages only */
 558                 chan->scid = L2CAP_CID_SIGNALING;
 559                 chan->dcid = L2CAP_CID_SIGNALING;
 560                 chan->omtu = L2CAP_DEFAULT_MTU;
 561         }
 562
 563         chan->local_id          = L2CAP_BESTEFFORT_ID;
 564         chan->local_stype       = L2CAP_SERV_BESTEFFORT;
 565         chan->local_msdu        = L2CAP_DEFAULT_MAX_SDU_SIZE;
 566         chan->local_sdu_itime   = L2CAP_DEFAULT_SDU_ITIME;
 567         chan->local_acc_lat     = L2CAP_DEFAULT_ACC_LAT;
 568         chan->local_flush_to    = L2CAP_EFS_DEFAULT_FLUSH_TO;
 569
 570         l2cap_chan_hold(chan);
 571
 572         /* Only keep a reference for fixed channels if they requested it */
 573         if (chan->chan_type != L2CAP_CHAN_FIXED ||
 574             test_bit(FLAG_HOLD_HCI_CONN, &chan->flags))
 575                 hci_conn_hold(conn->hcon);
 576
 577         list_add(&chan->list, &conn->chan_l);
 578 }
 579
 580 void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 581 {
 582         mutex_lock(&conn->chan_lock);
 583         __l2cap_chan_add(conn, chan);
 584         mutex_unlock(&conn->chan_lock);
 585 }
 586
 587 void l2cap_chan_del(struct l2cap_chan *chan, int err)
 588 {
 589         struct l2cap_conn *conn = chan->conn;
 590
 591         __clear_chan_timer(chan);
 592
 593         BT_DBG("chan %p, conn %p, err %d, state %s", chan, conn, err,
 594                state_to_string(chan->state));
 595
 596         chan->ops->teardown(chan, err);
 597
 598         if (conn) {
 599                 struct amp_mgr *mgr = conn->hcon->amp_mgr;
 600                 /* Delete from channel list */
 601                 list_del(&chan->list);
 602
 603                 l2cap_chan_put(chan);
 604
 605                 chan->conn = NULL;
 606
 607                 /* Reference was only held for non-fixed channels or
 608                  * fixed channels that explicitly requested it using the
 609                  * FLAG_HOLD_HCI_CONN flag.
 610                  */
 611                 if (chan->chan_type != L2CAP_CHAN_FIXED ||
 612                     test_bit(FLAG_HOLD_HCI_CONN, &chan->flags))
 613                         hci_conn_drop(conn->hcon);
 614
 615                 if (mgr && mgr->bredr_chan == chan)
 616                         mgr->bredr_chan = NULL;
 617         }
 618
 619         if (chan->hs_hchan) {
 620                 struct hci_chan *hs_hchan = chan->hs_hchan;
 621
 622                 BT_DBG("chan %p disconnect hs_hchan %p", chan, hs_hchan);
 623                 amp_disconnect_logical_link(hs_hchan);
 624         }
 625
 626         if (test_bit(CONF_NOT_COMPLETE, &chan->conf_state))
 627                 return;
 628
 629         switch(chan->mode) {
 630         case L2CAP_MODE_BASIC:
 631                 break;
 632
 633         case L2CAP_MODE_LE_FLOWCTL:
 634                 skb_queue_purge(&chan->tx_q);
 635                 break;
 636
 637         case L2CAP_MODE_ERTM:
 638                 __clear_retrans_timer(chan);
 639                 __clear_monitor_timer(chan);
 640                 __clear_ack_timer(chan);
 641
 642                 skb_queue_purge(&chan->srej_q);
 643
 644                 l2cap_seq_list_free(&chan->srej_list);
 645                 l2cap_seq_list_free(&chan->retrans_list);
 646
 647                 /* fall through */
 648
 649         case L2CAP_MODE_STREAMING:
 650                 skb_queue_purge(&chan->tx_q);
 651                 break;
 652         }
 653
 654         return;
 655 }
 656 EXPORT_SYMBOL_GPL(l2cap_chan_del);
 657
 658 static void l2cap_conn_update_id_addr(struct work_struct *work)
 659 {
 660         struct l2cap_conn *conn = container_of(work, struct l2cap_conn,
 661                                                id_addr_update_work);
 662         struct hci_conn *hcon = conn->hcon;
 663         struct l2cap_chan *chan;
 664
 665         mutex_lock(&conn->chan_lock);
 666
 667         list_for_each_entry(chan, &conn->chan_l, list) {
 668                 l2cap_chan_lock(chan);
 669                 bacpy(&chan->dst, &hcon->dst);
 670                 chan->dst_type = bdaddr_dst_type(hcon);
 671                 l2cap_chan_unlock(chan);
 672         }
 673
 674         mutex_unlock(&conn->chan_lock);
 675 }
 676
 677 static void l2cap_chan_le_connect_reject(struct l2cap_chan *chan)
 678 {
 679         struct l2cap_conn *conn = chan->conn;
 680         struct l2cap_le_conn_rsp rsp;
 681         u16 result;
 682
 683         if (test_bit(FLAG_DEFER_SETUP, &chan->flags))
 684                 result = L2CAP_CR_AUTHORIZATION;
 685         else
 686                 result = L2CAP_CR_BAD_PSM;
 687
 688         l2cap_state_change(chan, BT_DISCONN);
 689
 690         rsp.dcid    = cpu_to_le16(chan->scid);
 691         rsp.mtu     = cpu_to_le16(chan->imtu);
 692         rsp.mps     = cpu_to_le16(chan->mps);
 693         rsp.credits = cpu_to_le16(chan->rx_credits);
 694         rsp.result  = cpu_to_le16(result);
 695
 696         l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CONN_RSP, sizeof(rsp),
 697                        &rsp);
 698 }
 699
 700 static void l2cap_chan_connect_reject(struct l2cap_chan *chan)
 701 {
 702         struct l2cap_conn *conn = chan->conn;
 703         struct l2cap_conn_rsp rsp;
 704         u16 result;
 705
 706         if (test_bit(FLAG_DEFER_SETUP, &chan->flags))
 707                 result = L2CAP_CR_SEC_BLOCK;
 708         else
 709                 result = L2CAP_CR_BAD_PSM;
 710
 711         l2cap_state_change(chan, BT_DISCONN);
 712
 713         rsp.scid   = cpu_to_le16(chan->dcid);
 714         rsp.dcid   = cpu_to_le16(chan->scid);
 715         rsp.result = cpu_to_le16(result);
 716         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
 717
 718         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
 719 }
 720
 721 void l2cap_chan_close(struct l2cap_chan *chan, int reason)
 722 {
 723         struct l2cap_conn *conn = chan->conn;
 724
 725         BT_DBG("chan %p state %s", chan, state_to_string(chan->state));
 726
 727         switch (chan->state) {
 728         case BT_LISTEN:
 729                 chan->ops->teardown(chan, 0);
 730                 break;
 731
 732         case BT_CONNECTED:
 733         case BT_CONFIG:
 734                 if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED) {
 735                         __set_chan_timer(chan, chan->ops->get_sndtimeo(chan));
 736                         l2cap_send_disconn_req(chan, reason);
 737                 } else
 738                         l2cap_chan_del(chan, reason);
 739                 break;
 740
 741         case BT_CONNECT2:
 742                 if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED) {
 743                         if (conn->hcon->type == ACL_LINK)
 744                                 l2cap_chan_connect_reject(chan);
 745                         else if (conn->hcon->type == LE_LINK)
 746                                 l2cap_chan_le_connect_reject(chan);
 747                 }
 748
 749                 l2cap_chan_del(chan, reason);
 750                 break;
 751
 752         case BT_CONNECT:
 753         case BT_DISCONN:
 754                 l2cap_chan_del(chan, reason);
 755                 break;
 756
 757         default:
 758                 chan->ops->teardown(chan, 0);
 759                 break;
 760         }
 761 }
 762 EXPORT_SYMBOL(l2cap_chan_close);
 763
 764 static inline u8 l2cap_get_auth_type(struct l2cap_chan *chan)
 765 {
 766         switch (chan->chan_type) {
 767         case L2CAP_CHAN_RAW:
 768                 switch (chan->sec_level) {
 769                 case BT_SECURITY_HIGH:
 770                 case BT_SECURITY_FIPS:
 771                         return HCI_AT_DEDICATED_BONDING_MITM;
 772                 case BT_SECURITY_MEDIUM:
 773                         return HCI_AT_DEDICATED_BONDING;
 774                 default:
 775                         return HCI_AT_NO_BONDING;
 776                 }
 777                 break;
 778         case L2CAP_CHAN_CONN_LESS:
 779                 if (chan->psm == cpu_to_le16(L2CAP_PSM_3DSP)) {
 780                         if (chan->sec_level == BT_SECURITY_LOW)
 781                                 chan->sec_level = BT_SECURITY_SDP;
 782                 }
 783                 if (chan->sec_level == BT_SECURITY_HIGH ||
 784                     chan->sec_level == BT_SECURITY_FIPS)
 785                         return HCI_AT_NO_BONDING_MITM;
 786                 else
 787                         return HCI_AT_NO_BONDING;
 788                 break;
 789         case L2CAP_CHAN_CONN_ORIENTED:
 790                 if (chan->psm == cpu_to_le16(L2CAP_PSM_SDP)) {
 791                         if (chan->sec_level == BT_SECURITY_LOW)
 792                                 chan->sec_level = BT_SECURITY_SDP;
 793
 794                         if (chan->sec_level == BT_SECURITY_HIGH ||
 795                             chan->sec_level == BT_SECURITY_FIPS)
 796                                 return HCI_AT_NO_BONDING_MITM;
 797                         else
 798                                 return HCI_AT_NO_BONDING;
 799                 }
 800                 /* fall through */
 801         default:
 802                 switch (chan->sec_level) {
 803                 case BT_SECURITY_HIGH:
 804                 case BT_SECURITY_FIPS:
 805                         return HCI_AT_GENERAL_BONDING_MITM;
 806                 case BT_SECURITY_MEDIUM:
 807                         return HCI_AT_GENERAL_BONDING;
 808                 default:
 809                         return HCI_AT_NO_BONDING;
 810                 }
 811                 break;
 812         }
 813 }
 814
 815 /* Service level security */
 816 int l2cap_chan_check_security(struct l2cap_chan *chan, bool initiator)
 817 {
 818         struct l2cap_conn *conn = chan->conn;
 819         __u8 auth_type;
 820
 821         if (conn->hcon->type == LE_LINK)
 822                 return smp_conn_security(conn->hcon, chan->sec_level);
 823
 824         auth_type = l2cap_get_auth_type(chan);
 825
 826         return hci_conn_security(conn->hcon, chan->sec_level, auth_type,
 827                                  initiator);
 828 }
 829
 830 static u8 l2cap_get_ident(struct l2cap_conn *conn)
 831 {
 832         u8 id;
 833
 834         /* Get next available identificator.
 835          *    1 - 128 are used by kernel.
 836          *  129 - 199 are reserved.
 837          *  200 - 254 are used by utilities like l2ping, etc.
 838          */
 839
 840         mutex_lock(&conn->ident_lock);
 841
 842         if (++conn->tx_ident > 128)
 843                 conn->tx_ident = 1;
 844
 845         id = conn->tx_ident;
 846
 847         mutex_unlock(&conn->ident_lock);
 848
 849         return id;
 850 }
 851
 852 static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len,
 853                            void *data)
 854 {
 855         struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data);
 856         u8 flags;
 857
 858         BT_DBG("code 0x%2.2x", code);
 859
 860         if (!skb)
 861                 return;
 862
 863         /* Use NO_FLUSH if supported or we have an LE link (which does
 864          * not support auto-flushing packets) */
 865         if (lmp_no_flush_capable(conn->hcon->hdev) ||
 866             conn->hcon->type == LE_LINK)
 867                 flags = ACL_START_NO_FLUSH;
 868         else
 869                 flags = ACL_START;
 870
 871         bt_cb(skb)->force_active = BT_POWER_FORCE_ACTIVE_ON;
 872         skb->priority = HCI_PRIO_MAX;
 873
 874         hci_send_acl(conn->hchan, skb, flags);
 875 }
 876
 877 static bool __chan_is_moving(struct l2cap_chan *chan)
 878 {
 879         return chan->move_state != L2CAP_MOVE_STABLE &&
 880                chan->move_state != L2CAP_MOVE_WAIT_PREPARE;
 881 }
 882
 883 static void l2cap_do_send(struct l2cap_chan *chan, struct sk_buff *skb)
 884 {
 885         struct hci_conn *hcon = chan->conn->hcon;
 886         u16 flags;
 887
 888         BT_DBG("chan %p, skb %p len %d priority %u", chan, skb, skb->len,
 889                skb->priority);
 890
 891         if (chan->hs_hcon && !__chan_is_moving(chan)) {
 892                 if (chan->hs_hchan)
 893                         hci_send_acl(chan->hs_hchan, skb, ACL_COMPLETE);
 894                 else
 895                         kfree_skb(skb);
 896
 897                 return;
 898         }
 899
 900         /* Use NO_FLUSH for LE links (where this is the only option) or
 901          * if the BR/EDR link supports it and flushing has not been
 902          * explicitly requested (through FLAG_FLUSHABLE).
 903          */
 904         if (hcon->type == LE_LINK ||
 905             (!test_bit(FLAG_FLUSHABLE, &chan->flags) &&
 906              lmp_no_flush_capable(hcon->hdev)))
 907                 flags = ACL_START_NO_FLUSH;
 908         else
 909                 flags = ACL_START;
 910
 911         bt_cb(skb)->force_active = test_bit(FLAG_FORCE_ACTIVE, &chan->flags);
 912         hci_send_acl(chan->conn->hchan, skb, flags);
 913 }
 914
 915 static void __unpack_enhanced_control(u16 enh, struct l2cap_ctrl *control)
 916 {
 917         control->reqseq = (enh & L2CAP_CTRL_REQSEQ) >> L2CAP_CTRL_REQSEQ_SHIFT;
 918         control->final = (enh & L2CAP_CTRL_FINAL) >> L2CAP_CTRL_FINAL_SHIFT;
 919
 920         if (enh & L2CAP_CTRL_FRAME_TYPE) {
 921                 /* S-Frame */
 922                 control->sframe = 1;
 923                 control->poll = (enh & L2CAP_CTRL_POLL) >> L2CAP_CTRL_POLL_SHIFT;
 924                 control->super = (enh & L2CAP_CTRL_SUPERVISE) >> L2CAP_CTRL_SUPER_SHIFT;
 925
 926                 control->sar = 0;
 927                 control->txseq = 0;
 928         } else {
 929                 /* I-Frame */
 930                 control->sframe = 0;
 931                 control->sar = (enh & L2CAP_CTRL_SAR) >> L2CAP_CTRL_SAR_SHIFT;
 932                 control->txseq = (enh & L2CAP_CTRL_TXSEQ) >> L2CAP_CTRL_TXSEQ_SHIFT;
 933
 934                 control->poll = 0;
 935                 control->super = 0;
 936         }
 937 }
 938
 939 static void __unpack_extended_control(u32 ext, struct l2cap_ctrl *control)
 940 {
 941         control->reqseq = (ext & L2CAP_EXT_CTRL_REQSEQ) >> L2CAP_EXT_CTRL_REQSEQ_SHIFT;
 942         control->final = (ext & L2CAP_EXT_CTRL_FINAL) >> L2CAP_EXT_CTRL_FINAL_SHIFT;
 943
 944         if (ext & L2CAP_EXT_CTRL_FRAME_TYPE) {
 945                 /* S-Frame */
 946                 control->sframe = 1;
 947                 control->poll = (ext & L2CAP_EXT_CTRL_POLL) >> L2CAP_EXT_CTRL_POLL_SHIFT;
 948                 control->super = (ext & L2CAP_EXT_CTRL_SUPERVISE) >> L2CAP_EXT_CTRL_SUPER_SHIFT;
 949
 950                 control->sar = 0;
 951                 control->txseq = 0;
 952         } else {
 953                 /* I-Frame */
 954                 control->sframe = 0;
 955                 control->sar = (ext & L2CAP_EXT_CTRL_SAR) >> L2CAP_EXT_CTRL_SAR_SHIFT;
 956                 control->txseq = (ext & L2CAP_EXT_CTRL_TXSEQ) >> L2CAP_EXT_CTRL_TXSEQ_SHIFT;
 957
 958                 control->poll = 0;
 959                 control->super = 0;
 960         }
 961 }
 962
 963 static inline void __unpack_control(struct l2cap_chan *chan,
 964                                     struct sk_buff *skb)
 965 {
 966         if (test_bit(FLAG_EXT_CTRL, &chan->flags)) {
 967                 __unpack_extended_control(get_unaligned_le32(skb->data),
 968                                           &bt_cb(skb)->l2cap);
 969                 skb_pull(skb, L2CAP_EXT_CTRL_SIZE);
 970         } else {
 971                 __unpack_enhanced_control(get_unaligned_le16(skb->data),
 972                                           &bt_cb(skb)->l2cap);
 973                 skb_pull(skb, L2CAP_ENH_CTRL_SIZE);
 974         }
 975 }
 976
 977 static u32 __pack_extended_control(struct l2cap_ctrl *control)
 978 {
 979         u32 packed;
 980
 981         packed = control->reqseq << L2CAP_EXT_CTRL_REQSEQ_SHIFT;
 982         packed |= control->final << L2CAP_EXT_CTRL_FINAL_SHIFT;
 983
 984         if (control->sframe) {
 985                 packed |= control->poll << L2CAP_EXT_CTRL_POLL_SHIFT;
 986                 packed |= control->super << L2CAP_EXT_CTRL_SUPER_SHIFT;
 987                 packed |= L2CAP_EXT_CTRL_FRAME_TYPE;
 988         } else {
 989                 packed |= control->sar << L2CAP_EXT_CTRL_SAR_SHIFT;
 990                 packed |= control->txseq << L2CAP_EXT_CTRL_TXSEQ_SHIFT;
 991         }
 992
 993         return packed;
 994 }
 995
 996 static u16 __pack_enhanced_control(struct l2cap_ctrl *control)
 997 {
 998         u16 packed;
 999
1000         packed = control->reqseq << L2CAP_CTRL_REQSEQ_SHIFT;
1001         packed |= control->final << L2CAP_CTRL_FINAL_SHIFT;
1002
1003         if (control->sframe) {
1004                 packed |= control->poll << L2CAP_CTRL_POLL_SHIFT;
1005                 packed |= control->super << L2CAP_CTRL_SUPER_SHIFT;
1006                 packed |= L2CAP_CTRL_FRAME_TYPE;
1007         } else {
1008                 packed |= control->sar << L2CAP_CTRL_SAR_SHIFT;
1009                 packed |= control->txseq << L2CAP_CTRL_TXSEQ_SHIFT;
1010         }
1011
1012         return packed;
1013 }
1014
1015 static inline void __pack_control(struct l2cap_chan *chan,
1016                                   struct l2cap_ctrl *control,
1017                                   struct sk_buff *skb)
1018 {
1019         if (test_bit(FLAG_EXT_CTRL, &chan->flags)) {
1020                 put_unaligned_le32(__pack_extended_control(control),
1021                                    skb->data + L2CAP_HDR_SIZE);
1022         } else {
1023                 put_unaligned_le16(__pack_enhanced_control(control),
1024                                    skb->data + L2CAP_HDR_SIZE);
1025         }
1026 }
1027
1028 static inline unsigned int __ertm_hdr_size(struct l2cap_chan *chan)
1029 {
1030         if (test_bit(FLAG_EXT_CTRL, &chan->flags))
1031                 return L2CAP_EXT_HDR_SIZE;
1032         else
1033                 return L2CAP_ENH_HDR_SIZE;
1034 }
1035
1036 static struct sk_buff *l2cap_create_sframe_pdu(struct l2cap_chan *chan,
1037                                                u32 control)
1038 {
1039         struct sk_buff *skb;
1040         struct l2cap_hdr *lh;
1041         int hlen = __ertm_hdr_size(chan);
1042
1043         if (chan->fcs == L2CAP_FCS_CRC16)
1044                 hlen += L2CAP_FCS_SIZE;
1045
1046         skb = bt_skb_alloc(hlen, GFP_KERNEL);
1047
1048         if (!skb)
1049                 return ERR_PTR(-ENOMEM);
1050
1051         lh = skb_put(skb, L2CAP_HDR_SIZE);
1052         lh->len = cpu_to_le16(hlen - L2CAP_HDR_SIZE);
1053         lh->cid = cpu_to_le16(chan->dcid);
1054
1055         if (test_bit(FLAG_EXT_CTRL, &chan->flags))
1056                 put_unaligned_le32(control, skb_put(skb, L2CAP_EXT_CTRL_SIZE));
1057         else
1058                 put_unaligned_le16(control, skb_put(skb, L2CAP_ENH_CTRL_SIZE));
1059
1060         if (chan->fcs == L2CAP_FCS_CRC16) {
1061                 u16 fcs = crc16(0, (u8 *)skb->data, skb->len);
1062                 put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE));
1063         }
1064
1065         skb->priority = HCI_PRIO_MAX;
1066         return skb;
1067 }
1068
1069 static void l2cap_send_sframe(struct l2cap_chan *chan,
1070                               struct l2cap_ctrl *control)
1071 {
1072         struct sk_buff *skb;
1073         u32 control_field;
1074
1075         BT_DBG("chan %p, control %p", chan, control);
1076
1077         if (!control->sframe)
1078                 return;
1079
1080         if (__chan_is_moving(chan))
1081                 return;
1082
1083         if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state) &&
1084             !control->poll)
1085                 control->final = 1;
1086
1087         if (control->super == L2CAP_SUPER_RR)
1088                 clear_bit(CONN_RNR_SENT, &chan->conn_state);
1089         else if (control->super == L2CAP_SUPER_RNR)
1090                 set_bit(CONN_RNR_SENT, &chan->conn_state);
1091
1092         if (control->super != L2CAP_SUPER_SREJ) {
1093                 chan->last_acked_seq = control->reqseq;
1094                 __clear_ack_timer(chan);
1095         }
1096
1097         BT_DBG("reqseq %d, final %d, poll %d, super %d", control->reqseq,
1098                control->final, control->poll, control->super);
1099
1100         if (test_bit(FLAG_EXT_CTRL, &chan->flags))
1101                 control_field = __pack_extended_control(control);
1102         else
1103                 control_field = __pack_enhanced_control(control);
1104
1105         skb = l2cap_create_sframe_pdu(chan, control_field);
1106         if (!IS_ERR(skb))
1107                 l2cap_do_send(chan, skb);
1108 }
1109
1110 static void l2cap_send_rr_or_rnr(struct l2cap_chan *chan, bool poll)
1111 {
1112         struct l2cap_ctrl control;
1113
1114         BT_DBG("chan %p, poll %d", chan, poll);
1115
1116         memset(&control, 0, sizeof(control));
1117         control.sframe = 1;
1118         control.poll = poll;
1119
1120         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state))
1121                 control.super = L2CAP_SUPER_RNR;
1122         else
1123                 control.super = L2CAP_SUPER_RR;
1124
1125         control.reqseq = chan->buffer_seq;
1126         l2cap_send_sframe(chan, &control);
1127 }
1128
1129 static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan)
1130 {
1131         if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED)
1132                 return true;
1133
1134         return !test_bit(CONF_CONNECT_PEND, &chan->conf_state);
1135 }
1136
1137 static bool __amp_capable(struct l2cap_chan *chan)
1138 {
1139         struct l2cap_conn *conn = chan->conn;
1140         struct hci_dev *hdev;
1141         bool amp_available = false;
1142
1143         if (!(conn->local_fixed_chan & L2CAP_FC_A2MP))
1144                 return false;
1145
1146         if (!(conn->remote_fixed_chan & L2CAP_FC_A2MP))
1147                 return false;
1148
1149         read_lock(&hci_dev_list_lock);
1150         list_for_each_entry(hdev, &hci_dev_list, list) {
1151                 if (hdev->amp_type != AMP_TYPE_BREDR &&
1152                     test_bit(HCI_UP, &hdev->flags)) {
1153                         amp_available = true;
1154                         break;
1155                 }
1156         }
1157         read_unlock(&hci_dev_list_lock);
1158
1159         if (chan->chan_policy == BT_CHANNEL_POLICY_AMP_PREFERRED)
1160                 return amp_available;
1161
1162         return false;
1163 }
1164
1165 static bool l2cap_check_efs(struct l2cap_chan *chan)
1166 {
1167         /* Check EFS parameters */
1168         return true;
1169 }
1170
1171 void l2cap_send_conn_req(struct l2cap_chan *chan)
1172 {
1173         struct l2cap_conn *conn = chan->conn;
1174         struct l2cap_conn_req req;
1175
1176         req.scid = cpu_to_le16(chan->scid);
1177         req.psm  = chan->psm;
1178
1179         chan->ident = l2cap_get_ident(conn);
1180
1181         set_bit(CONF_CONNECT_PEND, &chan->conf_state);
1182
1183         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_REQ, sizeof(req), &req);
1184 }
1185
1186 static void l2cap_send_create_chan_req(struct l2cap_chan *chan, u8 amp_id)
1187 {
1188         struct l2cap_create_chan_req req;
1189         req.scid = cpu_to_le16(chan->scid);
1190         req.psm  = chan->psm;
1191         req.amp_id = amp_id;
1192
1193         chan->ident = l2cap_get_ident(chan->conn);
1194
1195         l2cap_send_cmd(chan->conn, chan->ident, L2CAP_CREATE_CHAN_REQ,
1196                        sizeof(req), &req);
1197 }
1198
1199 static void l2cap_move_setup(struct l2cap_chan *chan)
1200 {
1201         struct sk_buff *skb;
1202
1203         BT_DBG("chan %p", chan);
1204
1205         if (chan->mode != L2CAP_MODE_ERTM)
1206                 return;
1207
1208         __clear_retrans_timer(chan);
1209         __clear_monitor_timer(chan);
1210         __clear_ack_timer(chan);
1211
1212         chan->retry_count = 0;
1213         skb_queue_walk(&chan->tx_q, skb) {
1214                 if (bt_cb(skb)->l2cap.retries)
1215                         bt_cb(skb)->l2cap.retries = 1;
1216                 else
1217                         break;
1218         }
1219
1220         chan->expected_tx_seq = chan->buffer_seq;
1221
1222         clear_bit(CONN_REJ_ACT, &chan->conn_state);
1223         clear_bit(CONN_SREJ_ACT, &chan->conn_state);
1224         l2cap_seq_list_clear(&chan->retrans_list);
1225         l2cap_seq_list_clear(&chan->srej_list);
1226         skb_queue_purge(&chan->srej_q);
1227
1228         chan->tx_state = L2CAP_TX_STATE_XMIT;
1229         chan->rx_state = L2CAP_RX_STATE_MOVE;
1230
1231         set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
1232 }
1233
1234 static void l2cap_move_done(struct l2cap_chan *chan)
1235 {
1236         u8 move_role = chan->move_role;
1237         BT_DBG("chan %p", chan);
1238
1239         chan->move_state = L2CAP_MOVE_STABLE;
1240         chan->move_role = L2CAP_MOVE_ROLE_NONE;
1241
1242         if (chan->mode != L2CAP_MODE_ERTM)
1243                 return;
1244
1245         switch (move_role) {
1246         case L2CAP_MOVE_ROLE_INITIATOR:
1247                 l2cap_tx(chan, NULL, NULL, L2CAP_EV_EXPLICIT_POLL);
1248                 chan->rx_state = L2CAP_RX_STATE_WAIT_F;
1249                 break;
1250         case L2CAP_MOVE_ROLE_RESPONDER:
1251                 chan->rx_state = L2CAP_RX_STATE_WAIT_P;
1252                 break;
1253         }
1254 }
1255
1256 static void l2cap_chan_ready(struct l2cap_chan *chan)
1257 {
1258         /* The channel may have already been flagged as connected in
1259          * case of receiving data before the L2CAP info req/rsp
1260          * procedure is complete.
1261          */
1262         if (chan->state == BT_CONNECTED)
1263                 return;
1264
1265         /* This clears all conf flags, including CONF_NOT_COMPLETE */
1266         chan->conf_state = 0;
1267         __clear_chan_timer(chan);
1268
1269         if (chan->mode == L2CAP_MODE_LE_FLOWCTL && !chan->tx_credits)
1270                 chan->ops->suspend(chan);
1271
1272         chan->state = BT_CONNECTED;
1273
1274         chan->ops->ready(chan);
1275 }
1276
1277 static void l2cap_le_connect(struct l2cap_chan *chan)
1278 {
1279         struct l2cap_conn *conn = chan->conn;
1280         struct l2cap_le_conn_req req;
1281
1282         if (test_and_set_bit(FLAG_LE_CONN_REQ_SENT, &chan->flags))
1283                 return;
1284
1285         req.psm     = chan->psm;
1286         req.scid    = cpu_to_le16(chan->scid);
1287         req.mtu     = cpu_to_le16(chan->imtu);
1288         req.mps     = cpu_to_le16(chan->mps);
1289         req.credits = cpu_to_le16(chan->rx_credits);
1290
1291         chan->ident = l2cap_get_ident(conn);
1292
1293         l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CONN_REQ,
1294                        sizeof(req), &req);
1295 }
1296
1297 static void l2cap_le_start(struct l2cap_chan *chan)
1298 {
1299         struct l2cap_conn *conn = chan->conn;
1300
1301         if (!smp_conn_security(conn->hcon, chan->sec_level))
1302                 return;
1303
1304         if (!chan->psm) {
1305                 l2cap_chan_ready(chan);
1306                 return;
1307         }
1308
1309         if (chan->state == BT_CONNECT)
1310                 l2cap_le_connect(chan);
1311 }
1312
1313 static void l2cap_start_connection(struct l2cap_chan *chan)
1314 {
1315         if (__amp_capable(chan)) {
1316                 BT_DBG("chan %p AMP capable: discover AMPs", chan);
1317                 a2mp_discover_amp(chan);
1318         } else if (chan->conn->hcon->type == LE_LINK) {
1319                 l2cap_le_start(chan);
1320         } else {
1321                 l2cap_send_conn_req(chan);
1322         }
1323 }
1324
1325 static void l2cap_request_info(struct l2cap_conn *conn)
1326 {
1327         struct l2cap_info_req req;
1328
1329         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT)
1330                 return;
1331
1332         req.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
1333
1334         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
1335         conn->info_ident = l2cap_get_ident(conn);
1336
1337         schedule_delayed_work(&conn->info_timer, L2CAP_INFO_TIMEOUT);
1338
1339         l2cap_send_cmd(conn, conn->info_ident, L2CAP_INFO_REQ,
1340                        sizeof(req), &req);
1341 }
1342
1343 static void l2cap_do_start(struct l2cap_chan *chan)
1344 {
1345         struct l2cap_conn *conn = chan->conn;
1346
1347         if (conn->hcon->type == LE_LINK) {
1348                 l2cap_le_start(chan);
1349                 return;
1350         }
1351
1352         if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT)) {
1353                 l2cap_request_info(conn);
1354                 return;
1355         }
1356
1357         if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE))
1358                 return;
1359
1360         if (l2cap_chan_check_security(chan, true) &&
1361             __l2cap_no_conn_pending(chan))
1362                 l2cap_start_connection(chan);
1363 }
1364
1365 static inline int l2cap_mode_supported(__u8 mode, __u32 feat_mask)
1366 {
1367         u32 local_feat_mask = l2cap_feat_mask;
1368         if (!disable_ertm)
1369                 local_feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING;
1370
1371         switch (mode) {
1372         case L2CAP_MODE_ERTM:
1373                 return L2CAP_FEAT_ERTM & feat_mask & local_feat_mask;
1374         case L2CAP_MODE_STREAMING:
1375                 return L2CAP_FEAT_STREAMING & feat_mask & local_feat_mask;
1376         default:
1377                 return 0x00;
1378         }
1379 }
1380
1381 static void l2cap_send_disconn_req(struct l2cap_chan *chan, int err)
1382 {
1383         struct l2cap_conn *conn = chan->conn;
1384         struct l2cap_disconn_req req;
1385
1386         if (!conn)
1387                 return;
1388
1389         if (chan->mode == L2CAP_MODE_ERTM && chan->state == BT_CONNECTED) {
1390                 __clear_retrans_timer(chan);
1391                 __clear_monitor_timer(chan);
1392                 __clear_ack_timer(chan);
1393         }
1394
1395         if (chan->scid == L2CAP_CID_A2MP) {
1396                 l2cap_state_change(chan, BT_DISCONN);
1397                 return;
1398         }
1399
1400         req.dcid = cpu_to_le16(chan->dcid);
1401         req.scid = cpu_to_le16(chan->scid);
1402         l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_DISCONN_REQ,
1403                        sizeof(req), &req);
1404
1405         l2cap_state_change_and_error(chan, BT_DISCONN, err);
1406 }
1407
1408 /* ---- L2CAP connections ---- */
1409 static void l2cap_conn_start(struct l2cap_conn *conn)
1410 {
1411         struct l2cap_chan *chan, *tmp;
1412
1413         BT_DBG("conn %p", conn);
1414
1415         mutex_lock(&conn->chan_lock);
1416
1417         list_for_each_entry_safe(chan, tmp, &conn->chan_l, list) {
1418                 l2cap_chan_lock(chan);
1419
1420                 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
1421                         l2cap_chan_ready(chan);
1422                         l2cap_chan_unlock(chan);
1423                         continue;
1424                 }
1425
1426                 if (chan->state == BT_CONNECT) {
1427                         if (!l2cap_chan_check_security(chan, true) ||
1428                             !__l2cap_no_conn_pending(chan)) {
1429                                 l2cap_chan_unlock(chan);
1430                                 continue;
1431                         }
1432
1433                         if (!l2cap_mode_supported(chan->mode, conn->feat_mask)
1434                             && test_bit(CONF_STATE2_DEVICE,
1435                                         &chan->conf_state)) {
1436                                 l2cap_chan_close(chan, ECONNRESET);
1437                                 l2cap_chan_unlock(chan);
1438                                 continue;
1439                         }
1440
1441                         l2cap_start_connection(chan);
1442
1443                 } else if (chan->state == BT_CONNECT2) {
1444                         struct l2cap_conn_rsp rsp;
1445                         char buf[128];
1446                         rsp.scid = cpu_to_le16(chan->dcid);
1447                         rsp.dcid = cpu_to_le16(chan->scid);
1448
1449                         if (l2cap_chan_check_security(chan, false)) {
1450                                 if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
1451                                         rsp.result = cpu_to_le16(L2CAP_CR_PEND);
1452                                         rsp.status = cpu_to_le16(L2CAP_CS_AUTHOR_PEND);
1453                                         chan->ops->defer(chan);
1454
1455                                 } else {
1456                                         l2cap_state_change(chan, BT_CONFIG);
1457                                         rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
1458                                         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
1459                                 }
1460                         } else {
1461                                 rsp.result = cpu_to_le16(L2CAP_CR_PEND);
1462                                 rsp.status = cpu_to_le16(L2CAP_CS_AUTHEN_PEND);
1463                         }
1464
1465                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
1466                                        sizeof(rsp), &rsp);
1467
1468                         if (test_bit(CONF_REQ_SENT, &chan->conf_state) ||
1469                             rsp.result != L2CAP_CR_SUCCESS) {
1470                                 l2cap_chan_unlock(chan);
1471                                 continue;
1472                         }
1473
1474                         set_bit(CONF_REQ_SENT, &chan->conf_state);
1475                         l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
1476                                        l2cap_build_conf_req(chan, buf, sizeof(buf)), buf);
1477                         chan->num_conf_req++;
1478                 }
1479
1480                 l2cap_chan_unlock(chan);
1481         }
1482
1483         mutex_unlock(&conn->chan_lock);
1484 }
1485
1486 static void l2cap_le_conn_ready(struct l2cap_conn *conn)
1487 {
1488         struct hci_conn *hcon = conn->hcon;
1489         struct hci_dev *hdev = hcon->hdev;
1490
1491         BT_DBG("%s conn %p", hdev->name, conn);
1492
1493         /* For outgoing pairing which doesn't necessarily have an
1494          * associated socket (e.g. mgmt_pair_device).
1495          */
1496         if (hcon->out)
1497                 smp_conn_security(hcon, hcon->pending_sec_level);
1498
1499         /* For LE slave connections, make sure the connection interval
1500          * is in the range of the minium and maximum interval that has
1501          * been configured for this connection. If not, then trigger
1502          * the connection update procedure.
1503          */
1504         if (hcon->role == HCI_ROLE_SLAVE &&
1505             (hcon->le_conn_interval < hcon->le_conn_min_interval ||
1506              hcon->le_conn_interval > hcon->le_conn_max_interval)) {
1507                 struct l2cap_conn_param_update_req req;
1508
1509                 req.min = cpu_to_le16(hcon->le_conn_min_interval);
1510                 req.max = cpu_to_le16(hcon->le_conn_max_interval);
1511                 req.latency = cpu_to_le16(hcon->le_conn_latency);
1512                 req.to_multiplier = cpu_to_le16(hcon->le_supv_timeout);
1513
1514                 l2cap_send_cmd(conn, l2cap_get_ident(conn),
1515                                L2CAP_CONN_PARAM_UPDATE_REQ, sizeof(req), &req);
1516         }
1517 }
1518
1519 static void l2cap_conn_ready(struct l2cap_conn *conn)
1520 {
1521         struct l2cap_chan *chan;
1522         struct hci_conn *hcon = conn->hcon;
1523
1524         BT_DBG("conn %p", conn);
1525
1526         if (hcon->type == ACL_LINK)
1527                 l2cap_request_info(conn);
1528
1529         mutex_lock(&conn->chan_lock);
1530
1531         list_for_each_entry(chan, &conn->chan_l, list) {
1532
1533                 l2cap_chan_lock(chan);
1534
1535                 if (chan->scid == L2CAP_CID_A2MP) {
1536                         l2cap_chan_unlock(chan);
1537                         continue;
1538                 }
1539
1540                 if (hcon->type == LE_LINK) {
1541                         l2cap_le_start(chan);
1542                 } else if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
1543                         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)
1544                                 l2cap_chan_ready(chan);
1545                 } else if (chan->state == BT_CONNECT) {
1546                         l2cap_do_start(chan);
1547                 }
1548
1549                 l2cap_chan_unlock(chan);
1550         }
1551
1552         mutex_unlock(&conn->chan_lock);
1553
1554         if (hcon->type == LE_LINK)
1555                 l2cap_le_conn_ready(conn);
1556
1557         queue_work(hcon->hdev->workqueue, &conn->pending_rx_work);
1558 }
1559
1560 /* Notify sockets that we cannot guaranty reliability anymore */
1561 static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err)
1562 {
1563         struct l2cap_chan *chan;
1564
1565         BT_DBG("conn %p", conn);
1566
1567         mutex_lock(&conn->chan_lock);
1568
1569         list_for_each_entry(chan, &conn->chan_l, list) {
1570                 if (test_bit(FLAG_FORCE_RELIABLE, &chan->flags))
1571                         l2cap_chan_set_err(chan, err);
1572         }
1573
1574         mutex_unlock(&conn->chan_lock);
1575 }
1576
1577 static void l2cap_info_timeout(struct work_struct *work)
1578 {
1579         struct l2cap_conn *conn = container_of(work, struct l2cap_conn,
1580                                                info_timer.work);
1581
1582         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
1583         conn->info_ident = 0;
1584
1585         l2cap_conn_start(conn);
1586 }
1587
1588 /*
1589  * l2cap_user
1590  * External modules can register l2cap_user objects on l2cap_conn. The ->probe
1591  * callback is called during registration. The ->remove callback is called
1592  * during unregistration.
1593  * An l2cap_user object can either be explicitly unregistered or when the
1594  * underlying l2cap_conn object is deleted. This guarantees that l2cap->hcon,
1595  * l2cap->hchan, .. are valid as long as the remove callback hasn't been called.
1596  * External modules must own a reference to the l2cap_conn object if they intend
1597  * to call l2cap_unregister_user(). The l2cap_conn object might get destroyed at
1598  * any time if they don't.
1599  */
1600
1601 int l2cap_register_user(struct l2cap_conn *conn, struct l2cap_user *user)
1602 {
1603         struct hci_dev *hdev = conn->hcon->hdev;
1604         int ret;
1605
1606         /* We need to check whether l2cap_conn is registered. If it is not, we
1607          * must not register the l2cap_user. l2cap_conn_del() is unregisters
1608          * l2cap_conn objects, but doesn't provide its own locking. Instead, it
1609          * relies on the parent hci_conn object to be locked. This itself relies
1610          * on the hci_dev object to be locked. So we must lock the hci device
1611          * here, too. */
1612
1613         hci_dev_lock(hdev);
1614
1615         if (!list_empty(&user->list)) {
1616                 ret = -EINVAL;
1617                 goto out_unlock;
1618         }
1619
1620         /* conn->hchan is NULL after l2cap_conn_del() was called */
1621         if (!conn->hchan) {
1622                 ret = -ENODEV;
1623                 goto out_unlock;
1624         }
1625
1626         ret = user->probe(conn, user);
1627         if (ret)
1628                 goto out_unlock;
1629
1630         list_add(&user->list, &conn->users);
1631         ret = 0;
1632
1633 out_unlock:
1634         hci_dev_unlock(hdev);
1635         return ret;
1636 }
1637 EXPORT_SYMBOL(l2cap_register_user);
1638
1639 void l2cap_unregister_user(struct l2cap_conn *conn, struct l2cap_user *user)
1640 {
1641         struct hci_dev *hdev = conn->hcon->hdev;
1642
1643         hci_dev_lock(hdev);
1644
1645         if (list_empty(&user->list))
1646                 goto out_unlock;
1647
1648         list_del_init(&user->list);
1649         user->remove(conn, user);
1650
1651 out_unlock:
1652         hci_dev_unlock(hdev);
1653 }
1654 EXPORT_SYMBOL(l2cap_unregister_user);
1655
1656 static void l2cap_unregister_all_users(struct l2cap_conn *conn)
1657 {
1658         struct l2cap_user *user;
1659
1660         while (!list_empty(&conn->users)) {
1661                 user = list_first_entry(&conn->users, struct l2cap_user, list);
1662                 list_del_init(&user->list);
1663                 user->remove(conn, user);
1664         }
1665 }
1666
1667 static void l2cap_conn_del(struct hci_conn *hcon, int err)
1668 {
1669         struct l2cap_conn *conn = hcon->l2cap_data;
1670         struct l2cap_chan *chan, *l;
1671
1672         if (!conn)
1673                 return;
1674
1675         BT_DBG("hcon %p conn %p, err %d", hcon, conn, err);
1676
1677         kfree_skb(conn->rx_skb);
1678
1679         skb_queue_purge(&conn->pending_rx);
1680
1681         /* We can not call flush_work(&conn->pending_rx_work) here since we
1682          * might block if we are running on a worker from the same workqueue
1683          * pending_rx_work is waiting on.
1684          */
1685         if (work_pending(&conn->pending_rx_work))
1686                 cancel_work_sync(&conn->pending_rx_work);
1687
1688         if (work_pending(&conn->id_addr_update_work))
1689                 cancel_work_sync(&conn->id_addr_update_work);
1690
1691         l2cap_unregister_all_users(conn);
1692
1693         /* Force the connection to be immediately dropped */
1694         hcon->disc_timeout = 0;
1695
1696         mutex_lock(&conn->chan_lock);
1697
1698         /* Kill channels */
1699         list_for_each_entry_safe(chan, l, &conn->chan_l, list) {
1700                 l2cap_chan_hold(chan);
1701                 l2cap_chan_lock(chan);
1702
1703                 l2cap_chan_del(chan, err);
1704
1705                 l2cap_chan_unlock(chan);
1706
1707                 chan->ops->close(chan);
1708                 l2cap_chan_put(chan);
1709         }
1710
1711         mutex_unlock(&conn->chan_lock);
1712
1713         hci_chan_del(conn->hchan);
1714
1715         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT)
1716                 cancel_delayed_work_sync(&conn->info_timer);
1717
1718         hcon->l2cap_data = NULL;
1719         conn->hchan = NULL;
1720         l2cap_conn_put(conn);
1721 }
1722
1723 static void l2cap_conn_free(struct kref *ref)
1724 {
1725         struct l2cap_conn *conn = container_of(ref, struct l2cap_conn, ref);
1726
1727         hci_conn_put(conn->hcon);
1728         kfree(conn);
1729 }
1730
1731 struct l2cap_conn *l2cap_conn_get(struct l2cap_conn *conn)
1732 {
1733         kref_get(&conn->ref);
1734         return conn;
1735 }
1736 EXPORT_SYMBOL(l2cap_conn_get);
1737
1738 void l2cap_conn_put(struct l2cap_conn *conn)
1739 {
1740         kref_put(&conn->ref, l2cap_conn_free);
1741 }
1742 EXPORT_SYMBOL(l2cap_conn_put);
1743
1744 /* ---- Socket interface ---- */
1745
1746 /* Find socket with psm and source / destination bdaddr.
1747  * Returns closest match.
1748  */
1749 static struct l2cap_chan *l2cap_global_chan_by_psm(int state, __le16 psm,
1750                                                    bdaddr_t *src,
1751                                                    bdaddr_t *dst,
1752                                                    u8 link_type)
1753 {
1754         struct l2cap_chan *c, *c1 = NULL;
1755
1756         read_lock(&chan_list_lock);
1757
1758         list_for_each_entry(c, &chan_list, global_l) {
1759                 if (state && c->state != state)
1760                         continue;
1761
1762                 if (link_type == ACL_LINK && c->src_type != BDADDR_BREDR)
1763                         continue;
1764
1765                 if (link_type == LE_LINK && c->src_type == BDADDR_BREDR)
1766                         continue;
1767
1768                 if (c->psm == psm) {
1769                         int src_match, dst_match;
1770                         int src_any, dst_any;
1771
1772                         /* Exact match. */
1773                         src_match = !bacmp(&c->src, src);
1774                         dst_match = !bacmp(&c->dst, dst);
1775                         if (src_match && dst_match) {
1776                                 l2cap_chan_hold(c);
1777                                 read_unlock(&chan_list_lock);
1778                                 return c;
1779                         }
1780
1781                         /* Closest match */
1782                         src_any = !bacmp(&c->src, BDADDR_ANY);
1783                         dst_any = !bacmp(&c->dst, BDADDR_ANY);
1784                         if ((src_match && dst_any) || (src_any && dst_match) ||
1785                             (src_any && dst_any))
1786                                 c1 = c;
1787                 }
1788         }
1789
1790         if (c1)
1791                 l2cap_chan_hold(c1);
1792
1793         read_unlock(&chan_list_lock);
1794
1795         return c1;
1796 }
1797
1798 static void l2cap_monitor_timeout(struct work_struct *work)
1799 {
1800         struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
1801                                                monitor_timer.work);
1802
1803         BT_DBG("chan %p", chan);
1804
1805         l2cap_chan_lock(chan);
1806
1807         if (!chan->conn) {
1808                 l2cap_chan_unlock(chan);
1809                 l2cap_chan_put(chan);
1810                 return;
1811         }
1812
1813         l2cap_tx(chan, NULL, NULL, L2CAP_EV_MONITOR_TO);
1814
1815         l2cap_chan_unlock(chan);
1816         l2cap_chan_put(chan);
1817 }
1818
1819 static void l2cap_retrans_timeout(struct work_struct *work)
1820 {
1821         struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
1822                                                retrans_timer.work);
1823
1824         BT_DBG("chan %p", chan);
1825
1826         l2cap_chan_lock(chan);
1827
1828         if (!chan->conn) {
1829                 l2cap_chan_unlock(chan);
1830                 l2cap_chan_put(chan);
1831                 return;
1832         }
1833
1834         l2cap_tx(chan, NULL, NULL, L2CAP_EV_RETRANS_TO);
1835         l2cap_chan_unlock(chan);
1836         l2cap_chan_put(chan);
1837 }
1838
1839 static void l2cap_streaming_send(struct l2cap_chan *chan,
1840                                  struct sk_buff_head *skbs)
1841 {
1842         struct sk_buff *skb;
1843         struct l2cap_ctrl *control;
1844
1845         BT_DBG("chan %p, skbs %p", chan, skbs);
1846
1847         if (__chan_is_moving(chan))
1848                 return;
1849
1850         skb_queue_splice_tail_init(skbs, &chan->tx_q);
1851
1852         while (!skb_queue_empty(&chan->tx_q)) {
1853
1854                 skb = skb_dequeue(&chan->tx_q);
1855
1856                 bt_cb(skb)->l2cap.retries = 1;
1857                 control = &bt_cb(skb)->l2cap;
1858
1859                 control->reqseq = 0;
1860                 control->txseq = chan->next_tx_seq;
1861
1862                 __pack_control(chan, control, skb);
1863
1864                 if (chan->fcs == L2CAP_FCS_CRC16) {
1865                         u16 fcs = crc16(0, (u8 *) skb->data, skb->len);
1866                         put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE));
1867                 }
1868
1869                 l2cap_do_send(chan, skb);
1870
1871                 BT_DBG("Sent txseq %u", control->txseq);
1872
1873                 chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq);
1874                 chan->frames_sent++;
1875         }
1876 }
1877
1878 static int l2cap_ertm_send(struct l2cap_chan *chan)
1879 {
1880         struct sk_buff *skb, *tx_skb;
1881         struct l2cap_ctrl *control;
1882         int sent = 0;
1883
1884         BT_DBG("chan %p", chan);
1885
1886         if (chan->state != BT_CONNECTED)
1887                 return -ENOTCONN;
1888
1889         if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
1890                 return 0;
1891
1892         if (__chan_is_moving(chan))
1893                 return 0;
1894
1895         while (chan->tx_send_head &&
1896                chan->unacked_frames < chan->remote_tx_win &&
1897                chan->tx_state == L2CAP_TX_STATE_XMIT) {
1898
1899                 skb = chan->tx_send_head;
1900
1901                 bt_cb(skb)->l2cap.retries = 1;
1902                 control = &bt_cb(skb)->l2cap;
1903
1904                 if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
1905                         control->final = 1;
1906
1907                 control->reqseq = chan->buffer_seq;
1908                 chan->last_acked_seq = chan->buffer_seq;
1909                 control->txseq = chan->next_tx_seq;
1910
1911                 __pack_control(chan, control, skb);
1912
1913                 if (chan->fcs == L2CAP_FCS_CRC16) {
1914                         u16 fcs = crc16(0, (u8 *) skb->data, skb->len);
1915                         put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE));
1916                 }
1917
1918                 /* Clone after data has been modified. Data is assumed to be
1919                    read-only (for locking purposes) on cloned sk_buffs.
1920                  */
1921                 tx_skb = skb_clone(skb, GFP_KERNEL);
1922
1923                 if (!tx_skb)
1924                         break;
1925
1926                 __set_retrans_timer(chan);
1927
1928                 chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq);
1929                 chan->unacked_frames++;
1930                 chan->frames_sent++;
1931                 sent++;
1932
1933                 if (skb_queue_is_last(&chan->tx_q, skb))
1934                         chan->tx_send_head = NULL;
1935                 else
1936                         chan->tx_send_head = skb_queue_next(&chan->tx_q, skb);
1937
1938                 l2cap_do_send(chan, tx_skb);
1939                 BT_DBG("Sent txseq %u", control->txseq);
1940         }
1941
1942         BT_DBG("Sent %d, %u unacked, %u in ERTM queue", sent,
1943                chan->unacked_frames, skb_queue_len(&chan->tx_q));
1944
1945         return sent;
1946 }
1947
1948 static void l2cap_ertm_resend(struct l2cap_chan *chan)
1949 {
1950         struct l2cap_ctrl control;
1951         struct sk_buff *skb;
1952         struct sk_buff *tx_skb;
1953         u16 seq;
1954
1955         BT_DBG("chan %p", chan);
1956
1957         if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
1958                 return;
1959
1960         if (__chan_is_moving(chan))
1961                 return;
1962
1963         while (chan->retrans_list.head != L2CAP_SEQ_LIST_CLEAR) {
1964                 seq = l2cap_seq_list_pop(&chan->retrans_list);
1965
1966                 skb = l2cap_ertm_seq_in_queue(&chan->tx_q, seq);
1967                 if (!skb) {
1968                         BT_DBG("Error: Can't retransmit seq %d, frame missing",
1969                                seq);
1970                         continue;
1971                 }
1972
1973                 bt_cb(skb)->l2cap.retries++;
1974                 control = bt_cb(skb)->l2cap;
1975
1976                 if (chan->max_tx != 0 &&
1977                     bt_cb(skb)->l2cap.retries > chan->max_tx) {
1978                         BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
1979                         l2cap_send_disconn_req(chan, ECONNRESET);
1980                         l2cap_seq_list_clear(&chan->retrans_list);
1981                         break;
1982                 }
1983
1984                 control.reqseq = chan->buffer_seq;
1985                 if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
1986                         control.final = 1;
1987                 else
1988                         control.final = 0;
1989
1990                 if (skb_cloned(skb)) {
1991                         /* Cloned sk_buffs are read-only, so we need a
1992                          * writeable copy
1993                          */
1994                         tx_skb = skb_copy(skb, GFP_KERNEL);
1995                 } else {
1996                         tx_skb = skb_clone(skb, GFP_KERNEL);
1997                 }
1998
1999                 if (!tx_skb) {
2000                         l2cap_seq_list_clear(&chan->retrans_list);
2001                         break;
2002                 }
2003
2004                 /* Update skb contents */
2005                 if (test_bit(FLAG_EXT_CTRL, &chan->flags)) {
2006                         put_unaligned_le32(__pack_extended_control(&control),
2007                                            tx_skb->data + L2CAP_HDR_SIZE);
2008                 } else {
2009                         put_unaligned_le16(__pack_enhanced_control(&control),
2010                                            tx_skb->data + L2CAP_HDR_SIZE);
2011                 }
2012
2013                 /* Update FCS */
2014                 if (chan->fcs == L2CAP_FCS_CRC16) {
2015                         u16 fcs = crc16(0, (u8 *) tx_skb->data,
2016                                         tx_skb->len - L2CAP_FCS_SIZE);
2017                         put_unaligned_le16(fcs, skb_tail_pointer(tx_skb) -
2018                                                 L2CAP_FCS_SIZE);
2019                 }
2020
2021                 l2cap_do_send(chan, tx_skb);
2022
2023                 BT_DBG("Resent txseq %d", control.txseq);
2024
2025                 chan->last_acked_seq = chan->buffer_seq;
2026         }
2027 }
2028
2029 static void l2cap_retransmit(struct l2cap_chan *chan,
2030                              struct l2cap_ctrl *control)
2031 {
2032         BT_DBG("chan %p, control %p", chan, control);
2033
2034         l2cap_seq_list_append(&chan->retrans_list, control->reqseq);
2035         l2cap_ertm_resend(chan);
2036 }
2037
2038 static void l2cap_retransmit_all(struct l2cap_chan *chan,
2039                                  struct l2cap_ctrl *control)
2040 {
2041         struct sk_buff *skb;
2042
2043         BT_DBG("chan %p, control %p", chan, control);
2044
2045         if (control->poll)
2046                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
2047
2048         l2cap_seq_list_clear(&chan->retrans_list);
2049
2050         if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
2051                 return;
2052
2053         if (chan->unacked_frames) {
2054                 skb_queue_walk(&chan->tx_q, skb) {
2055                         if (bt_cb(skb)->l2cap.txseq == control->reqseq ||
2056                             skb == chan->tx_send_head)
2057                                 break;
2058                 }
2059
2060                 skb_queue_walk_from(&chan->tx_q, skb) {
2061                         if (skb == chan->tx_send_head)
2062                                 break;
2063
2064                         l2cap_seq_list_append(&chan->retrans_list,
2065                                               bt_cb(skb)->l2cap.txseq);
2066                 }
2067
2068                 l2cap_ertm_resend(chan);
2069         }
2070 }
2071
2072 static void l2cap_send_ack(struct l2cap_chan *chan)
2073 {
2074         struct l2cap_ctrl control;
2075         u16 frames_to_ack = __seq_offset(chan, chan->buffer_seq,
2076                                          chan->last_acked_seq);
2077         int threshold;
2078
2079         BT_DBG("chan %p last_acked_seq %d buffer_seq %d",
2080                chan, chan->last_acked_seq, chan->buffer_seq);
2081
2082         memset(&control, 0, sizeof(control));
2083         control.sframe = 1;
2084
2085         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
2086             chan->rx_state == L2CAP_RX_STATE_RECV) {
2087                 __clear_ack_timer(chan);
2088                 control.super = L2CAP_SUPER_RNR;
2089                 control.reqseq = chan->buffer_seq;
2090                 l2cap_send_sframe(chan, &control);
2091         } else {
2092                 if (!test_bit(CONN_REMOTE_BUSY, &chan->conn_state)) {
2093                         l2cap_ertm_send(chan);
2094                         /* If any i-frames were sent, they included an ack */
2095                         if (chan->buffer_seq == chan->last_acked_seq)
2096                                 frames_to_ack = 0;
2097                 }
2098
2099                 /* Ack now if the window is 3/4ths full.
2100                  * Calculate without mul or div
2101                  */
2102                 threshold = chan->ack_win;
2103                 threshold += threshold << 1;
2104                 threshold >>= 2;
2105
2106                 BT_DBG("frames_to_ack %u, threshold %d", frames_to_ack,
2107                        threshold);
2108
2109                 if (frames_to_ack >= threshold) {
2110                         __clear_ack_timer(chan);
2111                         control.super = L2CAP_SUPER_RR;
2112                         control.reqseq = chan->buffer_seq;
2113                         l2cap_send_sframe(chan, &control);
2114                         frames_to_ack = 0;
2115                 }
2116
2117                 if (frames_to_ack)
2118                         __set_ack_timer(chan);
2119         }
2120 }
2121
2122 static inline int l2cap_skbuff_fromiovec(struct l2cap_chan *chan,
2123                                          struct msghdr *msg, int len,
2124                                          int count, struct sk_buff *skb)
2125 {
2126         struct l2cap_conn *conn = chan->conn;
2127         struct sk_buff **frag;
2128         int sent = 0;
2129
2130         if (!copy_from_iter_full(skb_put(skb, count), count, &msg->msg_iter))
2131                 return -EFAULT;
2132
2133         sent += count;
2134         len  -= count;
2135
2136         /* Continuation fragments (no L2CAP header) */
2137         frag = &skb_shinfo(skb)->frag_list;
2138         while (len) {
2139                 struct sk_buff *tmp;
2140
2141                 count = min_t(unsigned int, conn->mtu, len);
2142
2143                 tmp = chan->ops->alloc_skb(chan, 0, count,
2144                                            msg->msg_flags & MSG_DONTWAIT);
2145                 if (IS_ERR(tmp))
2146                         return PTR_ERR(tmp);
2147
2148                 *frag = tmp;
2149
2150                 if (!copy_from_iter_full(skb_put(*frag, count), count,
2151                                    &msg->msg_iter))
2152                         return -EFAULT;
2153
2154                 sent += count;
2155                 len  -= count;
2156
2157                 skb->len += (*frag)->len;
2158                 skb->data_len += (*frag)->len;
2159
2160                 frag = &(*frag)->next;
2161         }
2162
2163         return sent;
2164 }
2165
2166 static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan,
2167                                                  struct msghdr *msg, size_t len)
2168 {
2169         struct l2cap_conn *conn = chan->conn;
2170         struct sk_buff *skb;
2171         int err, count, hlen = L2CAP_HDR_SIZE + L2CAP_PSMLEN_SIZE;
2172         struct l2cap_hdr *lh;
2173
2174         BT_DBG("chan %p psm 0x%2.2x len %zu", chan,
2175                __le16_to_cpu(chan->psm), len);
2176
2177         count = min_t(unsigned int, (conn->mtu - hlen), len);
2178
2179         skb = chan->ops->alloc_skb(chan, hlen, count,
2180                                    msg->msg_flags & MSG_DONTWAIT);
2181         if (IS_ERR(skb))
2182                 return skb;
2183
2184         /* Create L2CAP header */
2185         lh = skb_put(skb, L2CAP_HDR_SIZE);
2186         lh->cid = cpu_to_le16(chan->dcid);
2187         lh->len = cpu_to_le16(len + L2CAP_PSMLEN_SIZE);
2188         put_unaligned(chan->psm, (__le16 *) skb_put(skb, L2CAP_PSMLEN_SIZE));
2189
2190         err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb);
2191         if (unlikely(err < 0)) {
2192                 kfree_skb(skb);
2193                 return ERR_PTR(err);
2194         }
2195         return skb;
2196 }
2197
2198 static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan,
2199                                               struct msghdr *msg, size_t len)
2200 {
2201         struct l2cap_conn *conn = chan->conn;
2202         struct sk_buff *skb;
2203         int err, count;
2204         struct l2cap_hdr *lh;
2205
2206         BT_DBG("chan %p len %zu", chan, len);
2207
2208         count = min_t(unsigned int, (conn->mtu - L2CAP_HDR_SIZE), len);
2209
2210         skb = chan->ops->alloc_skb(chan, L2CAP_HDR_SIZE, count,
2211                                    msg->msg_flags & MSG_DONTWAIT);
2212         if (IS_ERR(skb))
2213                 return skb;
2214
2215         /* Create L2CAP header */
2216         lh = skb_put(skb, L2CAP_HDR_SIZE);
2217         lh->cid = cpu_to_le16(chan->dcid);
2218         lh->len = cpu_to_le16(len);
2219
2220         err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb);
2221         if (unlikely(err < 0)) {
2222                 kfree_skb(skb);
2223                 return ERR_PTR(err);
2224         }
2225         return skb;
2226 }
2227
2228 static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan,
2229                                                struct msghdr *msg, size_t len,
2230                                                u16 sdulen)
2231 {
2232         struct l2cap_conn *conn = chan->conn;
2233         struct sk_buff *skb;
2234         int err, count, hlen;
2235         struct l2cap_hdr *lh;
2236
2237         BT_DBG("chan %p len %zu", chan, len);
2238
2239         if (!conn)
2240                 return ERR_PTR(-ENOTCONN);
2241
2242         hlen = __ertm_hdr_size(chan);
2243
2244         if (sdulen)
2245                 hlen += L2CAP_SDULEN_SIZE;
2246
2247         if (chan->fcs == L2CAP_FCS_CRC16)
2248                 hlen += L2CAP_FCS_SIZE;
2249
2250         count = min_t(unsigned int, (conn->mtu - hlen), len);
2251
2252         skb = chan->ops->alloc_skb(chan, hlen, count,
2253                                    msg->msg_flags & MSG_DONTWAIT);
2254         if (IS_ERR(skb))
2255                 return skb;
2256
2257         /* Create L2CAP header */
2258         lh = skb_put(skb, L2CAP_HDR_SIZE);
2259         lh->cid = cpu_to_le16(chan->dcid);
2260         lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
2261
2262         /* Control header is populated later */
2263         if (test_bit(FLAG_EXT_CTRL, &chan->flags))
2264                 put_unaligned_le32(0, skb_put(skb, L2CAP_EXT_CTRL_SIZE));
2265         else
2266                 put_unaligned_le16(0, skb_put(skb, L2CAP_ENH_CTRL_SIZE));
2267
2268         if (sdulen)
2269                 put_unaligned_le16(sdulen, skb_put(skb, L2CAP_SDULEN_SIZE));
2270
2271         err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb);
2272         if (unlikely(err < 0)) {
2273                 kfree_skb(skb);
2274                 return ERR_PTR(err);
2275         }
2276
2277         bt_cb(skb)->l2cap.fcs = chan->fcs;
2278         bt_cb(skb)->l2cap.retries = 0;
2279         return skb;
2280 }
2281
2282 static int l2cap_segment_sdu(struct l2cap_chan *chan,
2283                              struct sk_buff_head *seg_queue,
2284                              struct msghdr *msg, size_t len)
2285 {
2286         struct sk_buff *skb;
2287         u16 sdu_len;
2288         size_t pdu_len;
2289         u8 sar;
2290
2291         BT_DBG("chan %p, msg %p, len %zu", chan, msg, len);
2292
2293         /* It is critical that ERTM PDUs fit in a single HCI fragment,
2294          * so fragmented skbs are not used.  The HCI layer's handling
2295          * of fragmented skbs is not compatible with ERTM's queueing.
2296          */
2297
2298         /* PDU size is derived from the HCI MTU */
2299         pdu_len = chan->conn->mtu;
2300
2301         /* Constrain PDU size for BR/EDR connections */
2302         if (!chan->hs_hcon)
2303                 pdu_len = min_t(size_t, pdu_len, L2CAP_BREDR_MAX_PAYLOAD);
2304
2305         /* Adjust for largest possible L2CAP overhead. */
2306         if (chan->fcs)
2307                 pdu_len -= L2CAP_FCS_SIZE;
2308
2309         pdu_len -= __ertm_hdr_size(chan);
2310
2311         /* Remote device may have requested smaller PDUs */
2312         pdu_len = min_t(size_t, pdu_len, chan->remote_mps);
2313
2314         if (len <= pdu_len) {
2315                 sar = L2CAP_SAR_UNSEGMENTED;
2316                 sdu_len = 0;
2317                 pdu_len = len;
2318         } else {
2319                 sar = L2CAP_SAR_START;
2320                 sdu_len = len;
2321         }
2322
2323         while (len > 0) {
2324                 skb = l2cap_create_iframe_pdu(chan, msg, pdu_len, sdu_len);
2325
2326                 if (IS_ERR(skb)) {
2327                         __skb_queue_purge(seg_queue);
2328                         return PTR_ERR(skb);
2329                 }
2330
2331                 bt_cb(skb)->l2cap.sar = sar;
2332                 __skb_queue_tail(seg_queue, skb);
2333
2334                 len -= pdu_len;
2335                 if (sdu_len)
2336                         sdu_len = 0;
2337
2338                 if (len <= pdu_len) {
2339                         sar = L2CAP_SAR_END;
2340                         pdu_len = len;
2341                 } else {
2342                         sar = L2CAP_SAR_CONTINUE;
2343                 }
2344         }
2345
2346         return 0;
2347 }
2348
2349 static struct sk_buff *l2cap_create_le_flowctl_pdu(struct l2cap_chan *chan,
2350                                                    struct msghdr *msg,
2351                                                    size_t len, u16 sdulen)
2352 {
2353         struct l2cap_conn *conn = chan->conn;
2354         struct sk_buff *skb;
2355         int err, count, hlen;
2356         struct l2cap_hdr *lh;
2357
2358         BT_DBG("chan %p len %zu", chan, len);
2359
2360         if (!conn)
2361                 return ERR_PTR(-ENOTCONN);
2362
2363         hlen = L2CAP_HDR_SIZE;
2364
2365         if (sdulen)
2366                 hlen += L2CAP_SDULEN_SIZE;
2367
2368         count = min_t(unsigned int, (conn->mtu - hlen), len);
2369
2370         skb = chan->ops->alloc_skb(chan, hlen, count,
2371                                    msg->msg_flags & MSG_DONTWAIT);
2372         if (IS_ERR(skb))
2373                 return skb;
2374
2375         /* Create L2CAP header */
2376         lh = skb_put(skb, L2CAP_HDR_SIZE);
2377         lh->cid = cpu_to_le16(chan->dcid);
2378         lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
2379
2380         if (sdulen)
2381                 put_unaligned_le16(sdulen, skb_put(skb, L2CAP_SDULEN_SIZE));
2382
2383         err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb);
2384         if (unlikely(err < 0)) {
2385                 kfree_skb(skb);
2386                 return ERR_PTR(err);
2387         }
2388
2389         return skb;
2390 }
2391
2392 static int l2cap_segment_le_sdu(struct l2cap_chan *chan,
2393                                 struct sk_buff_head *seg_queue,
2394                                 struct msghdr *msg, size_t len)
2395 {
2396         struct sk_buff *skb;
2397         size_t pdu_len;
2398         u16 sdu_len;
2399
2400         BT_DBG("chan %p, msg %p, len %zu", chan, msg, len);
2401
2402         sdu_len = len;
2403         pdu_len = chan->remote_mps - L2CAP_SDULEN_SIZE;
2404
2405         while (len > 0) {
2406                 if (len <= pdu_len)
2407                         pdu_len = len;
2408
2409                 skb = l2cap_create_le_flowctl_pdu(chan, msg, pdu_len, sdu_len);
2410                 if (IS_ERR(skb)) {
2411                         __skb_queue_purge(seg_queue);
2412                         return PTR_ERR(skb);
2413                 }
2414
2415                 __skb_queue_tail(seg_queue, skb);
2416
2417                 len -= pdu_len;
2418
2419                 if (sdu_len) {
2420                         sdu_len = 0;
2421                         pdu_len += L2CAP_SDULEN_SIZE;
2422                 }
2423         }
2424
2425         return 0;
2426 }
2427
2428 static void l2cap_le_flowctl_send(struct l2cap_chan *chan)
2429 {
2430         int sent = 0;
2431
2432         BT_DBG("chan %p", chan);
2433
2434         while (chan->tx_credits && !skb_queue_empty(&chan->tx_q)) {
2435                 l2cap_do_send(chan, skb_dequeue(&chan->tx_q));
2436                 chan->tx_credits--;
2437                 sent++;
2438         }
2439
2440         BT_DBG("Sent %d credits %u queued %u", sent, chan->tx_credits,
2441                skb_queue_len(&chan->tx_q));
2442 }
2443
2444 int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len)
2445 {
2446         struct sk_buff *skb;
2447         int err;
2448         struct sk_buff_head seg_queue;
2449
2450         if (!chan->conn)
2451                 return -ENOTCONN;
2452
2453         /* Connectionless channel */
2454         if (chan->chan_type == L2CAP_CHAN_CONN_LESS) {
2455                 skb = l2cap_create_connless_pdu(chan, msg, len);
2456                 if (IS_ERR(skb))
2457                         return PTR_ERR(skb);
2458
2459                 /* Channel lock is released before requesting new skb and then
2460                  * reacquired thus we need to recheck channel state.
2461                  */
2462                 if (chan->state != BT_CONNECTED) {
2463                         kfree_skb(skb);
2464                         return -ENOTCONN;
2465                 }
2466
2467                 l2cap_do_send(chan, skb);
2468                 return len;
2469         }
2470
2471         switch (chan->mode) {
2472         case L2CAP_MODE_LE_FLOWCTL:
2473                 /* Check outgoing MTU */
2474                 if (len > chan->omtu)
2475                         return -EMSGSIZE;
2476
2477                 __skb_queue_head_init(&seg_queue);
2478
2479                 err = l2cap_segment_le_sdu(chan, &seg_queue, msg, len);
2480
2481                 if (chan->state != BT_CONNECTED) {
2482                         __skb_queue_purge(&seg_queue);
2483                         err = -ENOTCONN;
2484                 }
2485
2486                 if (err)
2487                         return err;
2488
2489                 skb_queue_splice_tail_init(&seg_queue, &chan->tx_q);
2490
2491                 l2cap_le_flowctl_send(chan);
2492
2493                 if (!chan->tx_credits)
2494                         chan->ops->suspend(chan);
2495
2496                 err = len;
2497
2498                 break;
2499
2500         case L2CAP_MODE_BASIC:
2501                 /* Check outgoing MTU */
2502                 if (len > chan->omtu)
2503                         return -EMSGSIZE;
2504
2505                 /* Create a basic PDU */
2506                 skb = l2cap_create_basic_pdu(chan, msg, len);
2507                 if (IS_ERR(skb))
2508                         return PTR_ERR(skb);
2509
2510                 /* Channel lock is released before requesting new skb and then
2511                  * reacquired thus we need to recheck channel state.
2512                  */
2513                 if (chan->state != BT_CONNECTED) {
2514                         kfree_skb(skb);
2515                         return -ENOTCONN;
2516                 }
2517
2518                 l2cap_do_send(chan, skb);
2519                 err = len;
2520                 break;
2521
2522         case L2CAP_MODE_ERTM:
2523         case L2CAP_MODE_STREAMING:
2524                 /* Check outgoing MTU */
2525                 if (len > chan->omtu) {
2526                         err = -EMSGSIZE;
2527                         break;
2528                 }
2529
2530                 __skb_queue_head_init(&seg_queue);
2531
2532                 /* Do segmentation before calling in to the state machine,
2533                  * since it's possible to block while waiting for memory
2534                  * allocation.
2535                  */
2536                 err = l2cap_segment_sdu(chan, &seg_queue, msg, len);
2537
2538                 /* The channel could have been closed while segmenting,
2539                  * check that it is still connected.
2540                  */
2541                 if (chan->state != BT_CONNECTED) {
2542                         __skb_queue_purge(&seg_queue);
2543                         err = -ENOTCONN;
2544                 }
2545
2546                 if (err)
2547                         break;
2548
2549                 if (chan->mode == L2CAP_MODE_ERTM)
2550                         l2cap_tx(chan, NULL, &seg_queue, L2CAP_EV_DATA_REQUEST);
2551                 else
2552                         l2cap_streaming_send(chan, &seg_queue);
2553
2554                 err = len;
2555
2556                 /* If the skbs were not queued for sending, they'll still be in
2557                  * seg_queue and need to be purged.
2558                  */
2559                 __skb_queue_purge(&seg_queue);
2560                 break;
2561
2562         default:
2563                 BT_DBG("bad state %1.1x", chan->mode);
2564                 err = -EBADFD;
2565         }
2566
2567         return err;
2568 }
2569 EXPORT_SYMBOL_GPL(l2cap_chan_send);
2570
2571 static void l2cap_send_srej(struct l2cap_chan *chan, u16 txseq)
2572 {
2573         struct l2cap_ctrl control;
2574         u16 seq;
2575
2576         BT_DBG("chan %p, txseq %u", chan, txseq);
2577
2578         memset(&control, 0, sizeof(control));
2579         control.sframe = 1;
2580         control.super = L2CAP_SUPER_SREJ;
2581
2582         for (seq = chan->expected_tx_seq; seq != txseq;
2583              seq = __next_seq(chan, seq)) {
2584                 if (!l2cap_ertm_seq_in_queue(&chan->srej_q, seq)) {
2585                         control.reqseq = seq;
2586                         l2cap_send_sframe(chan, &control);
2587                         l2cap_seq_list_append(&chan->srej_list, seq);
2588                 }
2589         }
2590
2591         chan->expected_tx_seq = __next_seq(chan, txseq);
2592 }
2593
2594 static void l2cap_send_srej_tail(struct l2cap_chan *chan)
2595 {
2596         struct l2cap_ctrl control;
2597
2598         BT_DBG("chan %p", chan);
2599
2600         if (chan->srej_list.tail == L2CAP_SEQ_LIST_CLEAR)
2601                 return;
2602
2603         memset(&control, 0, sizeof(control));
2604         control.sframe = 1;
2605         control.super = L2CAP_SUPER_SREJ;
2606         control.reqseq = chan->srej_list.tail;
2607         l2cap_send_sframe(chan, &control);
2608 }
2609
2610 static void l2cap_send_srej_list(struct l2cap_chan *chan, u16 txseq)
2611 {
2612         struct l2cap_ctrl control;
2613         u16 initial_head;
2614         u16 seq;
2615
2616         BT_DBG("chan %p, txseq %u", chan, txseq);
2617
2618         memset(&control, 0, sizeof(control));
2619         control.sframe = 1;
2620         control.super = L2CAP_SUPER_SREJ;
2621
2622         /* Capture initial list head to allow only one pass through the list. */
2623         initial_head = chan->srej_list.head;
2624
2625         do {
2626                 seq = l2cap_seq_list_pop(&chan->srej_list);
2627                 if (seq == txseq || seq == L2CAP_SEQ_LIST_CLEAR)
2628                         break;
2629
2630                 control.reqseq = seq;
2631                 l2cap_send_sframe(chan, &control);
2632                 l2cap_seq_list_append(&chan->srej_list, seq);
2633         } while (chan->srej_list.head != initial_head);
2634 }
2635
2636 static void l2cap_process_reqseq(struct l2cap_chan *chan, u16 reqseq)
2637 {
2638         struct sk_buff *acked_skb;
2639         u16 ackseq;
2640
2641         BT_DBG("chan %p, reqseq %u", chan, reqseq);
2642
2643         if (chan->unacked_frames == 0 || reqseq == chan->expected_ack_seq)
2644                 return;
2645
2646         BT_DBG("expected_ack_seq %u, unacked_frames %u",
2647                chan->expected_ack_seq, chan->unacked_frames);
2648
2649         for (ackseq = chan->expected_ack_seq; ackseq != reqseq;
2650              ackseq = __next_seq(chan, ackseq)) {
2651
2652                 acked_skb = l2cap_ertm_seq_in_queue(&chan->tx_q, ackseq);
2653                 if (acked_skb) {
2654                         skb_unlink(acked_skb, &chan->tx_q);
2655                         kfree_skb(acked_skb);
2656                         chan->unacked_frames--;
2657                 }
2658         }
2659
2660         chan->expected_ack_seq = reqseq;
2661
2662         if (chan->unacked_frames == 0)
2663                 __clear_retrans_timer(chan);
2664
2665         BT_DBG("unacked_frames %u", chan->unacked_frames);
2666 }
2667
2668 static void l2cap_abort_rx_srej_sent(struct l2cap_chan *chan)
2669 {
2670         BT_DBG("chan %p", chan);
2671
2672         chan->expected_tx_seq = chan->buffer_seq;
2673         l2cap_seq_list_clear(&chan->srej_list);
2674         skb_queue_purge(&chan->srej_q);
2675         chan->rx_state = L2CAP_RX_STATE_RECV;
2676 }
2677
2678 static void l2cap_tx_state_xmit(struct l2cap_chan *chan,
2679                                 struct l2cap_ctrl *control,
2680                                 struct sk_buff_head *skbs, u8 event)
2681 {
2682         BT_DBG("chan %p, control %p, skbs %p, event %d", chan, control, skbs,
2683                event);
2684
2685         switch (event) {
2686         case L2CAP_EV_DATA_REQUEST:
2687                 if (chan->tx_send_head == NULL)
2688                         chan->tx_send_head = skb_peek(skbs);
2689
2690                 skb_queue_splice_tail_init(skbs, &chan->tx_q);
2691                 l2cap_ertm_send(chan);
2692                 break;
2693         case L2CAP_EV_LOCAL_BUSY_DETECTED:
2694                 BT_DBG("Enter LOCAL_BUSY");
2695                 set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2696
2697                 if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
2698                         /* The SREJ_SENT state must be aborted if we are to
2699                          * enter the LOCAL_BUSY state.
2700                          */
2701                         l2cap_abort_rx_srej_sent(chan);
2702                 }
2703
2704                 l2cap_send_ack(chan);
2705
2706                 break;
2707         case L2CAP_EV_LOCAL_BUSY_CLEAR:
2708                 BT_DBG("Exit LOCAL_BUSY");
2709                 clear_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2710
2711                 if (test_bit(CONN_RNR_SENT, &chan->conn_state)) {
2712                         struct l2cap_ctrl local_control;
2713
2714                         memset(&local_control, 0, sizeof(local_control));
2715                         local_control.sframe = 1;
2716                         local_control.super = L2CAP_SUPER_RR;
2717                         local_control.poll = 1;
2718                         local_control.reqseq = chan->buffer_seq;
2719                         l2cap_send_sframe(chan, &local_control);
2720
2721                         chan->retry_count = 1;
2722                         __set_monitor_timer(chan);
2723                         chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2724                 }
2725                 break;
2726         case L2CAP_EV_RECV_REQSEQ_AND_FBIT:
2727                 l2cap_process_reqseq(chan, control->reqseq);
2728                 break;
2729         case L2CAP_EV_EXPLICIT_POLL:
2730                 l2cap_send_rr_or_rnr(chan, 1);
2731                 chan->retry_count = 1;
2732                 __set_monitor_timer(chan);
2733                 __clear_ack_timer(chan);
2734                 chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2735                 break;
2736         case L2CAP_EV_RETRANS_TO:
2737                 l2cap_send_rr_or_rnr(chan, 1);
2738                 chan->retry_count = 1;
2739                 __set_monitor_timer(chan);
2740                 chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2741                 break;
2742         case L2CAP_EV_RECV_FBIT:
2743                 /* Nothing to process */
2744                 break;
2745         default:
2746                 break;
2747         }
2748 }
2749
2750 static void l2cap_tx_state_wait_f(struct l2cap_chan *chan,
2751                                   struct l2cap_ctrl *control,
2752                                   struct sk_buff_head *skbs, u8 event)
2753 {
2754         BT_DBG("chan %p, control %p, skbs %p, event %d", chan, control, skbs,
2755                event);
2756
2757         switch (event) {
2758         case L2CAP_EV_DATA_REQUEST:
2759                 if (chan->tx_send_head == NULL)
2760                         chan->tx_send_head = skb_peek(skbs);
2761                 /* Queue data, but don't send. */
2762                 skb_queue_splice_tail_init(skbs, &chan->tx_q);
2763                 break;
2764         case L2CAP_EV_LOCAL_BUSY_DETECTED:
2765                 BT_DBG("Enter LOCAL_BUSY");
2766                 set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2767
2768                 if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
2769                         /* The SREJ_SENT state must be aborted if we are to
2770                          * enter the LOCAL_BUSY state.
2771                          */
2772                         l2cap_abort_rx_srej_sent(chan);
2773                 }
2774
2775                 l2cap_send_ack(chan);
2776
2777                 break;
2778         case L2CAP_EV_LOCAL_BUSY_CLEAR:
2779                 BT_DBG("Exit LOCAL_BUSY");
2780                 clear_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2781
2782                 if (test_bit(CONN_RNR_SENT, &chan->conn_state)) {
2783                         struct l2cap_ctrl local_control;
2784                         memset(&local_control, 0, sizeof(local_control));
2785                         local_control.sframe = 1;
2786                         local_control.super = L2CAP_SUPER_RR;
2787                         local_control.poll = 1;
2788                         local_control.reqseq = chan->buffer_seq;
2789                         l2cap_send_sframe(chan, &local_control);
2790
2791                         chan->retry_count = 1;
2792                         __set_monitor_timer(chan);
2793                         chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2794                 }
2795                 break;
2796         case L2CAP_EV_RECV_REQSEQ_AND_FBIT:
2797                 l2cap_process_reqseq(chan, control->reqseq);
2798
2799                 /* Fall through */
2800
2801         case L2CAP_EV_RECV_FBIT:
2802                 if (control && control->final) {
2803                         __clear_monitor_timer(chan);
2804                         if (chan->unacked_frames > 0)
2805                                 __set_retrans_timer(chan);
2806                         chan->retry_count = 0;
2807                         chan->tx_state = L2CAP_TX_STATE_XMIT;
2808                         BT_DBG("recv fbit tx_state 0x2.2%x", chan->tx_state);
2809                 }
2810                 break;
2811         case L2CAP_EV_EXPLICIT_POLL:
2812                 /* Ignore */
2813                 break;
2814         case L2CAP_EV_MONITOR_TO:
2815                 if (chan->max_tx == 0 || chan->retry_count < chan->max_tx) {
2816                         l2cap_send_rr_or_rnr(chan, 1);
2817                         __set_monitor_timer(chan);
2818                         chan->retry_count++;
2819                 } else {
2820                         l2cap_send_disconn_req(chan, ECONNABORTED);
2821                 }
2822                 break;
2823         default:
2824                 break;
2825         }
2826 }
2827
2828 static void l2cap_tx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
2829                      struct sk_buff_head *skbs, u8 event)
2830 {
2831         BT_DBG("chan %p, control %p, skbs %p, event %d, state %d",
2832                chan, control, skbs, event, chan->tx_state);
2833
2834         switch (chan->tx_state) {
2835         case L2CAP_TX_STATE_XMIT:
2836                 l2cap_tx_state_xmit(chan, control, skbs, event);
2837                 break;
2838         case L2CAP_TX_STATE_WAIT_F:
2839                 l2cap_tx_state_wait_f(chan, control, skbs, event);
2840                 break;
2841         default:
2842                 /* Ignore event */
2843                 break;
2844         }
2845 }
2846
2847 static void l2cap_pass_to_tx(struct l2cap_chan *chan,
2848                              struct l2cap_ctrl *control)
2849 {
2850         BT_DBG("chan %p, control %p", chan, control);
2851         l2cap_tx(chan, control, NULL, L2CAP_EV_RECV_REQSEQ_AND_FBIT);
2852 }
2853
2854 static void l2cap_pass_to_tx_fbit(struct l2cap_chan *chan,
2855                                   struct l2cap_ctrl *control)
2856 {
2857         BT_DBG("chan %p, control %p", chan, control);
2858         l2cap_tx(chan, control, NULL, L2CAP_EV_RECV_FBIT);
2859 }
2860
2861 /* Copy frame to all raw sockets on that connection */
2862 static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
2863 {
2864         struct sk_buff *nskb;
2865         struct l2cap_chan *chan;
2866
2867         BT_DBG("conn %p", conn);
2868
2869         mutex_lock(&conn->chan_lock);
2870
2871         list_for_each_entry(chan, &conn->chan_l, list) {
2872                 if (chan->chan_type != L2CAP_CHAN_RAW)
2873                         continue;
2874
2875                 /* Don't send frame to the channel it came from */
2876                 if (bt_cb(skb)->l2cap.chan == chan)
2877                         continue;
2878
2879                 nskb = skb_clone(skb, GFP_KERNEL);
2880                 if (!nskb)
2881                         continue;
2882                 if (chan->ops->recv(chan, nskb))
2883                         kfree_skb(nskb);
2884         }
2885
2886         mutex_unlock(&conn->chan_lock);
2887 }
2888
2889 /* ---- L2CAP signalling commands ---- */
2890 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn, u8 code,
2891                                        u8 ident, u16 dlen, void *data)
2892 {
2893         struct sk_buff *skb, **frag;
2894         struct l2cap_cmd_hdr *cmd;
2895         struct l2cap_hdr *lh;
2896         int len, count;
2897
2898         BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %u",
2899                conn, code, ident, dlen);
2900
2901         if (conn->mtu < L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE)
2902                 return NULL;
2903
2904         len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen;
2905         count = min_t(unsigned int, conn->mtu, len);
2906
2907         skb = bt_skb_alloc(count, GFP_KERNEL);
2908         if (!skb)
2909                 return NULL;
2910
2911         lh = skb_put(skb, L2CAP_HDR_SIZE);
2912         lh->len = cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen);
2913
2914         if (conn->hcon->type == LE_LINK)
2915                 lh->cid = cpu_to_le16(L2CAP_CID_LE_SIGNALING);
2916         else
2917                 lh->cid = cpu_to_le16(L2CAP_CID_SIGNALING);
2918
2919         cmd = skb_put(skb, L2CAP_CMD_HDR_SIZE);
2920         cmd->code  = code;
2921         cmd->ident = ident;
2922         cmd->len   = cpu_to_le16(dlen);
2923
2924         if (dlen) {
2925                 count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE;
2926                 skb_put_data(skb, data, count);
2927                 data += count;
2928         }
2929
2930         len -= skb->len;
2931
2932         /* Continuation fragments (no L2CAP header) */
2933         frag = &skb_shinfo(skb)->frag_list;
2934         while (len) {
2935                 count = min_t(unsigned int, conn->mtu, len);
2936
2937                 *frag = bt_skb_alloc(count, GFP_KERNEL);
2938                 if (!*frag)
2939                         goto fail;
2940
2941                 skb_put_data(*frag, data, count);
2942
2943                 len  -= count;
2944                 data += count;
2945
2946                 frag = &(*frag)->next;
2947         }
2948
2949         return skb;
2950
2951 fail:
2952         kfree_skb(skb);
2953         return NULL;
2954 }
2955
2956 static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen,
2957                                      unsigned long *val)
2958 {
2959         struct l2cap_conf_opt *opt = *ptr;
2960         int len;
2961
2962         len = L2CAP_CONF_OPT_SIZE + opt->len;
2963         *ptr += len;
2964
2965         *type = opt->type;
2966         *olen = opt->len;
2967
2968         switch (opt->len) {
2969         case 1:
2970                 *val = *((u8 *) opt->val);
2971                 break;
2972
2973         case 2:
2974                 *val = get_unaligned_le16(opt->val);
2975                 break;
2976
2977         case 4:
2978                 *val = get_unaligned_le32(opt->val);
2979                 break;
2980
2981         default:
2982                 *val = (unsigned long) opt->val;
2983                 break;
2984         }
2985
2986         BT_DBG("type 0x%2.2x len %u val 0x%lx", *type, opt->len, *val);
2987         return len;
2988 }
2989
2990 static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val, size_t size)
2991 {
2992         struct l2cap_conf_opt *opt = *ptr;
2993
2994         BT_DBG("type 0x%2.2x len %u val 0x%lx", type, len, val);
2995
2996         if (size < L2CAP_CONF_OPT_SIZE + len)
2997                 return;
2998
2999         opt->type = type;
3000         opt->len  = len;
3001
3002         switch (len) {
3003         case 1:
3004                 *((u8 *) opt->val)  = val;
3005                 break;
3006
3007         case 2:
3008                 put_unaligned_le16(val, opt->val);
3009                 break;
3010
3011         case 4:
3012                 put_unaligned_le32(val, opt->val);
3013                 break;
3014
3015         default:
3016                 memcpy(opt->val, (void *) val, len);
3017                 break;
3018         }
3019
3020         *ptr += L2CAP_CONF_OPT_SIZE + len;
3021 }
3022
3023 static void l2cap_add_opt_efs(void **ptr, struct l2cap_chan *chan, size_t size)
3024 {
3025         struct l2cap_conf_efs efs;
3026
3027         switch (chan->mode) {
3028         case L2CAP_MODE_ERTM:
3029                 efs.id          = chan->local_id;
3030                 efs.stype       = chan->local_stype;
3031                 efs.msdu        = cpu_to_le16(chan->local_msdu);
3032                 efs.sdu_itime   = cpu_to_le32(chan->local_sdu_itime);
3033                 efs.acc_lat     = cpu_to_le32(L2CAP_DEFAULT_ACC_LAT);
3034                 efs.flush_to    = cpu_to_le32(L2CAP_EFS_DEFAULT_FLUSH_TO);
3035                 break;
3036
3037         case L2CAP_MODE_STREAMING:
3038                 efs.id          = 1;
3039                 efs.stype       = L2CAP_SERV_BESTEFFORT;
3040                 efs.msdu        = cpu_to_le16(chan->local_msdu);
3041                 efs.sdu_itime   = cpu_to_le32(chan->local_sdu_itime);
3042                 efs.acc_lat     = 0;
3043                 efs.flush_to    = 0;
3044                 break;
3045
3046         default:
3047                 return;
3048         }
3049
3050         l2cap_add_conf_opt(ptr, L2CAP_CONF_EFS, sizeof(efs),
3051                            (unsigned long) &efs, size);
3052 }
3053
3054 static void l2cap_ack_timeout(struct work_struct *work)
3055 {
3056         struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
3057                                                ack_timer.work);
3058         u16 frames_to_ack;
3059
3060         BT_DBG("chan %p", chan);
3061
3062         l2cap_chan_lock(chan);
3063
3064         frames_to_ack = __seq_offset(chan, chan->buffer_seq,
3065                                      chan->last_acked_seq);
3066
3067         if (frames_to_ack)
3068                 l2cap_send_rr_or_rnr(chan, 0);
3069
3070         l2cap_chan_unlock(chan);
3071         l2cap_chan_put(chan);
3072 }
3073
3074 int l2cap_ertm_init(struct l2cap_chan *chan)
3075 {
3076         int err;
3077
3078         chan->next_tx_seq = 0;
3079         chan->expected_tx_seq = 0;
3080         chan->expected_ack_seq = 0;
3081         chan->unacked_frames = 0;
3082         chan->buffer_seq = 0;
3083         chan->frames_sent = 0;
3084         chan->last_acked_seq = 0;
3085         chan->sdu = NULL;
3086         chan->sdu_last_frag = NULL;
3087         chan->sdu_len = 0;
3088
3089         skb_queue_head_init(&chan->tx_q);
3090
3091         chan->local_amp_id = AMP_ID_BREDR;
3092         chan->move_id = AMP_ID_BREDR;
3093         chan->move_state = L2CAP_MOVE_STABLE;
3094         chan->move_role = L2CAP_MOVE_ROLE_NONE;
3095
3096         if (chan->mode != L2CAP_MODE_ERTM)
3097                 return 0;
3098
3099         chan->rx_state = L2CAP_RX_STATE_RECV;
3100         chan->tx_state = L2CAP_TX_STATE_XMIT;
3101
3102         INIT_DELAYED_WORK(&chan->retrans_timer, l2cap_retrans_timeout);
3103         INIT_DELAYED_WORK(&chan->monitor_timer, l2cap_monitor_timeout);
3104         INIT_DELAYED_WORK(&chan->ack_timer, l2cap_ack_timeout);
3105
3106         skb_queue_head_init(&chan->srej_q);
3107
3108         err = l2cap_seq_list_init(&chan->srej_list, chan->tx_win);
3109         if (err < 0)
3110                 return err;
3111
3112         err = l2cap_seq_list_init(&chan->retrans_list, chan->remote_tx_win);
3113         if (err < 0)
3114                 l2cap_seq_list_free(&chan->srej_list);
3115
3116         return err;
3117 }
3118
3119 static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
3120 {
3121         switch (mode) {
3122         case L2CAP_MODE_STREAMING:
3123         case L2CAP_MODE_ERTM:
3124                 if (l2cap_mode_supported(mode, remote_feat_mask))
3125                         return mode;
3126                 /* fall through */
3127         default:
3128                 return L2CAP_MODE_BASIC;
3129         }
3130 }
3131
3132 static inline bool __l2cap_ews_supported(struct l2cap_conn *conn)
3133 {
3134         return ((conn->local_fixed_chan & L2CAP_FC_A2MP) &&
3135                 (conn->feat_mask & L2CAP_FEAT_EXT_WINDOW));
3136 }
3137
3138 static inline bool __l2cap_efs_supported(struct l2cap_conn *conn)
3139 {
3140         return ((conn->local_fixed_chan & L2CAP_FC_A2MP) &&
3141                 (conn->feat_mask & L2CAP_FEAT_EXT_FLOW));
3142 }
3143
3144 static void __l2cap_set_ertm_timeouts(struct l2cap_chan *chan,
3145                                       struct l2cap_conf_rfc *rfc)
3146 {
3147         if (chan->local_amp_id != AMP_ID_BREDR && chan->hs_hcon) {
3148                 u64 ertm_to = chan->hs_hcon->hdev->amp_be_flush_to;
3149
3150                 /* Class 1 devices have must have ERTM timeouts
3151                  * exceeding the Link Supervision Timeout.  The
3152                  * default Link Supervision Timeout for AMP
3153                  * controllers is 10 seconds.
3154                  *
3155                  * Class 1 devices use 0xffffffff for their
3156                  * best-effort flush timeout, so the clamping logic
3157                  * will result in a timeout that meets the above
3158                  * requirement.  ERTM timeouts are 16-bit values, so
3159                  * the maximum timeout is 65.535 seconds.
3160                  */
3161
3162                 /* Convert timeout to milliseconds and round */
3163                 ertm_to = DIV_ROUND_UP_ULL(ertm_to, 1000);
3164
3165                 /* This is the recommended formula for class 2 devices
3166                  * that start ERTM timers when packets are sent to the
3167                  * controller.
3168                  */
3169                 ertm_to = 3 * ertm_to + 500;
3170
3171                 if (ertm_to > 0xffff)
3172                         ertm_to = 0xffff;
3173
3174                 rfc->retrans_timeout = cpu_to_le16((u16) ertm_to);
3175                 rfc->monitor_timeout = rfc->retrans_timeout;
3176         } else {
3177                 rfc->retrans_timeout = cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO);
3178                 rfc->monitor_timeout = cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO);
3179         }
3180 }
3181
3182 static inline void l2cap_txwin_setup(struct l2cap_chan *chan)
3183 {
3184         if (chan->tx_win > L2CAP_DEFAULT_TX_WINDOW &&
3185             __l2cap_ews_supported(chan->conn)) {
3186                 /* use extended control field */
3187                 set_bit(FLAG_EXT_CTRL, &chan->flags);
3188                 chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
3189         } else {
3190                 chan->tx_win = min_t(u16, chan->tx_win,
3191                                      L2CAP_DEFAULT_TX_WINDOW);
3192                 chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
3193         }
3194         chan->ack_win = chan->tx_win;
3195 }
3196
3197 static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data, size_t data_size)
3198 {
3199         struct l2cap_conf_req *req = data;
3200         struct l2cap_conf_rfc rfc = { .mode = chan->mode };
3201         void *ptr = req->data;
3202         void *endptr = data + data_size;
3203         u16 size;
3204
3205         BT_DBG("chan %p", chan);
3206
3207         if (chan->num_conf_req || chan->num_conf_rsp)
3208                 goto done;
3209
3210         switch (chan->mode) {
3211         case L2CAP_MODE_STREAMING:
3212         case L2CAP_MODE_ERTM:
3213                 if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state))
3214                         break;
3215
3216                 if (__l2cap_efs_supported(chan->conn))
3217                         set_bit(FLAG_EFS_ENABLE, &chan->flags);
3218
3219                 /* fall through */
3220         default:
3221                 chan->mode = l2cap_select_mode(rfc.mode, chan->conn->feat_mask);
3222                 break;
3223         }
3224
3225 done:
3226         if (chan->imtu != L2CAP_DEFAULT_MTU)
3227                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu, endptr - ptr);
3228
3229         switch (chan->mode) {
3230         case L2CAP_MODE_BASIC:
3231                 if (disable_ertm)
3232                         break;
3233
3234                 if (!(chan->conn->feat_mask & L2CAP_FEAT_ERTM) &&
3235                     !(chan->conn->feat_mask & L2CAP_FEAT_STREAMING))
3236                         break;
3237
3238                 rfc.mode            = L2CAP_MODE_BASIC;
3239                 rfc.txwin_size      = 0;
3240                 rfc.max_transmit    = 0;
3241                 rfc.retrans_timeout = 0;
3242                 rfc.monitor_timeout = 0;
3243                 rfc.max_pdu_size    = 0;
3244
3245                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3246                                    (unsigned long) &rfc, endptr - ptr);
3247                 break;
3248
3249         case L2CAP_MODE_ERTM:
3250                 rfc.mode            = L2CAP_MODE_ERTM;
3251                 rfc.max_transmit    = chan->max_tx;
3252
3253                 __l2cap_set_ertm_timeouts(chan, &rfc);
3254
3255                 size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
3256                              L2CAP_EXT_HDR_SIZE - L2CAP_SDULEN_SIZE -
3257                              L2CAP_FCS_SIZE);
3258                 rfc.max_pdu_size = cpu_to_le16(size);
3259
3260                 l2cap_txwin_setup(chan);
3261
3262                 rfc.txwin_size = min_t(u16, chan->tx_win,
3263                                        L2CAP_DEFAULT_TX_WINDOW);
3264
3265                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3266                                    (unsigned long) &rfc, endptr - ptr);
3267
3268                 if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
3269                         l2cap_add_opt_efs(&ptr, chan, endptr - ptr);
3270
3271                 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
3272                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
3273                                            chan->tx_win, endptr - ptr);
3274
3275                 if (chan->conn->feat_mask & L2CAP_FEAT_FCS)
3276                         if (chan->fcs == L2CAP_FCS_NONE ||
3277                             test_bit(CONF_RECV_NO_FCS, &chan->conf_state)) {
3278                                 chan->fcs = L2CAP_FCS_NONE;
3279                                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1,
3280                                                    chan->fcs, endptr - ptr);
3281                         }
3282                 break;
3283
3284         case L2CAP_MODE_STREAMING:
3285                 l2cap_txwin_setup(chan);
3286                 rfc.mode            = L2CAP_MODE_STREAMING;
3287                 rfc.txwin_size      = 0;
3288                 rfc.max_transmit    = 0;
3289                 rfc.retrans_timeout = 0;
3290                 rfc.monitor_timeout = 0;
3291
3292                 size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
3293                              L2CAP_EXT_HDR_SIZE - L2CAP_SDULEN_SIZE -
3294                              L2CAP_FCS_SIZE);
3295                 rfc.max_pdu_size = cpu_to_le16(size);
3296
3297                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3298                                    (unsigned long) &rfc, endptr - ptr);
3299
3300                 if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
3301                         l2cap_add_opt_efs(&ptr, chan, endptr - ptr);
3302
3303                 if (chan->conn->feat_mask & L2CAP_FEAT_FCS)
3304                         if (chan->fcs == L2CAP_FCS_NONE ||
3305                             test_bit(CONF_RECV_NO_FCS, &chan->conf_state)) {
3306                                 chan->fcs = L2CAP_FCS_NONE;
3307                                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1,
3308                                                    chan->fcs, endptr - ptr);
3309                         }
3310                 break;
3311         }
3312
3313         req->dcid  = cpu_to_le16(chan->dcid);
3314         req->flags = cpu_to_le16(0);
3315
3316         return ptr - data;
3317 }
3318
3319 static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data, size_t data_size)
3320 {
3321         struct l2cap_conf_rsp *rsp = data;
3322         void *ptr = rsp->data;
3323         void *endptr = data + data_size;
3324         void *req = chan->conf_req;
3325         int len = chan->conf_len;
3326         int type, hint, olen;
3327         unsigned long val;
3328         struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
3329         struct l2cap_conf_efs efs;
3330         u8 remote_efs = 0;
3331         u16 mtu = L2CAP_DEFAULT_MTU;
3332         u16 result = L2CAP_CONF_SUCCESS;
3333         u16 size;
3334
3335         BT_DBG("chan %p", chan);
3336
3337         while (len >= L2CAP_CONF_OPT_SIZE) {
3338                 len -= l2cap_get_conf_opt(&req, &type, &olen, &val);
3339
3340                 hint  = type & L2CAP_CONF_HINT;
3341                 type &= L2CAP_CONF_MASK;
3342
3343                 switch (type) {
3344                 case L2CAP_CONF_MTU:
3345                         mtu = val;
3346                         break;
3347
3348                 case L2CAP_CONF_FLUSH_TO:
3349                         chan->flush_to = val;
3350                         break;
3351
3352                 case L2CAP_CONF_QOS:
3353                         break;
3354
3355                 case L2CAP_CONF_RFC:
3356                         if (olen == sizeof(rfc))
3357                                 memcpy(&rfc, (void *) val, olen);
3358                         break;
3359
3360                 case L2CAP_CONF_FCS:
3361                         if (val == L2CAP_FCS_NONE)
3362                                 set_bit(CONF_RECV_NO_FCS, &chan->conf_state);
3363                         break;
3364
3365                 case L2CAP_CONF_EFS:
3366                         remote_efs = 1;
3367                         if (olen == sizeof(efs))
3368                                 memcpy(&efs, (void *) val, olen);
3369                         break;
3370
3371                 case L2CAP_CONF_EWS:
3372                         if (!(chan->conn->local_fixed_chan & L2CAP_FC_A2MP))
3373                                 return -ECONNREFUSED;
3374
3375                         set_bit(FLAG_EXT_CTRL, &chan->flags);
3376                         set_bit(CONF_EWS_RECV, &chan->conf_state);
3377                         chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
3378                         chan->remote_tx_win = val;
3379                         break;
3380
3381                 default:
3382                         if (hint)
3383                                 break;
3384
3385                         result = L2CAP_CONF_UNKNOWN;
3386                         *((u8 *) ptr++) = type;
3387                         break;
3388                 }
3389         }
3390
3391         if (chan->num_conf_rsp || chan->num_conf_req > 1)
3392                 goto done;
3393
3394         switch (chan->mode) {
3395         case L2CAP_MODE_STREAMING:
3396         case L2CAP_MODE_ERTM:
3397                 if (!test_bit(CONF_STATE2_DEVICE, &chan->conf_state)) {
3398                         chan->mode = l2cap_select_mode(rfc.mode,
3399                                                        chan->conn->feat_mask);
3400                         break;
3401                 }
3402
3403                 if (remote_efs) {
3404                         if (__l2cap_efs_supported(chan->conn))
3405                                 set_bit(FLAG_EFS_ENABLE, &chan->flags);
3406                         else
3407                                 return -ECONNREFUSED;
3408                 }
3409
3410                 if (chan->mode != rfc.mode)
3411                         return -ECONNREFUSED;
3412
3413                 break;
3414         }
3415
3416 done:
3417         if (chan->mode != rfc.mode) {
3418                 result = L2CAP_CONF_UNACCEPT;
3419                 rfc.mode = chan->mode;
3420
3421                 if (chan->num_conf_rsp == 1)
3422                         return -ECONNREFUSED;
3423
3424                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3425                                    (unsigned long) &rfc, endptr - ptr);
3426         }
3427
3428         if (result == L2CAP_CONF_SUCCESS) {
3429                 /* Configure output options and let the other side know
3430                  * which ones we don't like. */
3431
3432                 if (mtu < L2CAP_DEFAULT_MIN_MTU)
3433                         result = L2CAP_CONF_UNACCEPT;
3434                 else {
3435                         chan->omtu = mtu;
3436                         set_bit(CONF_MTU_DONE, &chan->conf_state);
3437                 }
3438                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->omtu, endptr - ptr);
3439
3440                 if (remote_efs) {
3441                         if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
3442                             efs.stype != L2CAP_SERV_NOTRAFIC &&
3443                             efs.stype != chan->local_stype) {
3444
3445                                 result = L2CAP_CONF_UNACCEPT;
3446
3447                                 if (chan->num_conf_req >= 1)
3448                                         return -ECONNREFUSED;
3449
3450                                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
3451                                                    sizeof(efs),
3452                                                    (unsigned long) &efs, endptr - ptr);
3453                         } else {
3454                                 /* Send PENDING Conf Rsp */
3455                                 result = L2CAP_CONF_PENDING;
3456                                 set_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
3457                         }
3458                 }
3459
3460                 switch (rfc.mode) {
3461                 case L2CAP_MODE_BASIC:
3462                         chan->fcs = L2CAP_FCS_NONE;
3463                         set_bit(CONF_MODE_DONE, &chan->conf_state);
3464                         break;
3465
3466                 case L2CAP_MODE_ERTM:
3467                         if (!test_bit(CONF_EWS_RECV, &chan->conf_state))
3468                                 chan->remote_tx_win = rfc.txwin_size;
3469                         else
3470                                 rfc.txwin_size = L2CAP_DEFAULT_TX_WINDOW;
3471
3472                         chan->remote_max_tx = rfc.max_transmit;
3473
3474                         size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
3475                                      chan->conn->mtu - L2CAP_EXT_HDR_SIZE -
3476                                      L2CAP_SDULEN_SIZE - L2CAP_FCS_SIZE);
3477                         rfc.max_pdu_size = cpu_to_le16(size);
3478                         chan->remote_mps = size;
3479
3480                         __l2cap_set_ertm_timeouts(chan, &rfc);
3481
3482                         set_bit(CONF_MODE_DONE, &chan->conf_state);
3483
3484                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
3485                                            sizeof(rfc), (unsigned long) &rfc, endptr - ptr);
3486
3487                         if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
3488                                 chan->remote_id = efs.id;
3489                                 chan->remote_stype = efs.stype;
3490                                 chan->remote_msdu = le16_to_cpu(efs.msdu);
3491                                 chan->remote_flush_to =
3492                                         le32_to_cpu(efs.flush_to);
3493                                 chan->remote_acc_lat =
3494                                         le32_to_cpu(efs.acc_lat);
3495                                 chan->remote_sdu_itime =
3496                                         le32_to_cpu(efs.sdu_itime);
3497                                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
3498                                                    sizeof(efs),
3499                                                    (unsigned long) &efs, endptr - ptr);
3500                         }
3501                         break;
3502
3503                 case L2CAP_MODE_STREAMING:
3504                         size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
3505                                      chan->conn->mtu - L2CAP_EXT_HDR_SIZE -
3506                                      L2CAP_SDULEN_SIZE - L2CAP_FCS_SIZE);
3507                         rfc.max_pdu_size = cpu_to_le16(size);
3508                         chan->remote_mps = size;
3509
3510                         set_bit(CONF_MODE_DONE, &chan->conf_state);
3511
3512                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3513                                            (unsigned long) &rfc, endptr - ptr);
3514
3515                         break;
3516
3517                 default:
3518                         result = L2CAP_CONF_UNACCEPT;
3519
3520                         memset(&rfc, 0, sizeof(rfc));
3521                         rfc.mode = chan->mode;
3522                 }
3523
3524                 if (result == L2CAP_CONF_SUCCESS)
3525                         set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
3526         }
3527         rsp->scid   = cpu_to_le16(chan->dcid);
3528         rsp->result = cpu_to_le16(result);
3529         rsp->flags  = cpu_to_le16(0);
3530
3531         return ptr - data;
3532 }
3533
3534 static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len,
3535                                 void *data, size_t size, u16 *result)
3536 {
3537         struct l2cap_conf_req *req = data;
3538         void *ptr = req->data;
3539         void *endptr = data + size;
3540         int type, olen;
3541         unsigned long val;
3542         struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
3543         struct l2cap_conf_efs efs;
3544
3545         BT_DBG("chan %p, rsp %p, len %d, req %p", chan, rsp, len, data);
3546
3547         while (len >= L2CAP_CONF_OPT_SIZE) {
3548                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
3549
3550                 switch (type) {
3551                 case L2CAP_CONF_MTU:
3552                         if (val < L2CAP_DEFAULT_MIN_MTU) {
3553                                 *result = L2CAP_CONF_UNACCEPT;
3554                                 chan->imtu = L2CAP_DEFAULT_MIN_MTU;
3555                         } else
3556                                 chan->imtu = val;
3557                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu, endptr - ptr);
3558                         break;
3559
3560                 case L2CAP_CONF_FLUSH_TO:
3561                         chan->flush_to = val;
3562                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
3563                                            2, chan->flush_to, endptr - ptr);
3564                         break;
3565
3566                 case L2CAP_CONF_RFC:
3567                         if (olen == sizeof(rfc))
3568                                 memcpy(&rfc, (void *)val, olen);
3569
3570                         if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state) &&
3571                             rfc.mode != chan->mode)
3572                                 return -ECONNREFUSED;
3573
3574                         chan->fcs = 0;
3575
3576                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
3577                                            sizeof(rfc), (unsigned long) &rfc, endptr - ptr);
3578                         break;
3579
3580                 case L2CAP_CONF_EWS:
3581                         chan->ack_win = min_t(u16, val, chan->ack_win);
3582                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
3583                                            chan->tx_win, endptr - ptr);
3584                         break;
3585
3586                 case L2CAP_CONF_EFS:
3587                         if (olen == sizeof(efs))
3588                                 memcpy(&efs, (void *)val, olen);
3589
3590                         if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
3591                             efs.stype != L2CAP_SERV_NOTRAFIC &&
3592                             efs.stype != chan->local_stype)
3593                                 return -ECONNREFUSED;
3594
3595                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS, sizeof(efs),
3596                                            (unsigned long) &efs, endptr - ptr);
3597                         break;
3598
3599                 case L2CAP_CONF_FCS:
3600                         if (*result == L2CAP_CONF_PENDING)
3601                                 if (val == L2CAP_FCS_NONE)
3602                                         set_bit(CONF_RECV_NO_FCS,
3603                                                 &chan->conf_state);
3604                         break;
3605                 }
3606         }
3607
3608         if (chan->mode == L2CAP_MODE_BASIC && chan->mode != rfc.mode)
3609                 return -ECONNREFUSED;
3610
3611         chan->mode = rfc.mode;
3612
3613         if (*result == L2CAP_CONF_SUCCESS || *result == L2CAP_CONF_PENDING) {
3614                 switch (rfc.mode) {
3615                 case L2CAP_MODE_ERTM:
3616                         chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
3617                         chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
3618                         chan->mps    = le16_to_cpu(rfc.max_pdu_size);
3619                         if (!test_bit(FLAG_EXT_CTRL, &chan->flags))
3620                                 chan->ack_win = min_t(u16, chan->ack_win,
3621                                                       rfc.txwin_size);
3622
3623                         if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
3624                                 chan->local_msdu = le16_to_cpu(efs.msdu);
3625                                 chan->local_sdu_itime =
3626                                         le32_to_cpu(efs.sdu_itime);
3627                                 chan->local_acc_lat = le32_to_cpu(efs.acc_lat);
3628                                 chan->local_flush_to =
3629                                         le32_to_cpu(efs.flush_to);
3630                         }
3631                         break;
3632
3633                 case L2CAP_MODE_STREAMING:
3634                         chan->mps    = le16_to_cpu(rfc.max_pdu_size);
3635                 }
3636         }
3637
3638         req->dcid   = cpu_to_le16(chan->dcid);
3639         req->flags  = cpu_to_le16(0);
3640
3641         return ptr - data;
3642 }
3643
3644 static int l2cap_build_conf_rsp(struct l2cap_chan *chan, void *data,
3645                                 u16 result, u16 flags)
3646 {
3647         struct l2cap_conf_rsp *rsp = data;
3648         void *ptr = rsp->data;
3649
3650         BT_DBG("chan %p", chan);
3651
3652         rsp->scid   = cpu_to_le16(chan->dcid);
3653         rsp->result = cpu_to_le16(result);
3654         rsp->flags  = cpu_to_le16(flags);
3655
3656         return ptr - data;
3657 }
3658
3659 void __l2cap_le_connect_rsp_defer(struct l2cap_chan *chan)
3660 {
3661         struct l2cap_le_conn_rsp rsp;
3662         struct l2cap_conn *conn = chan->conn;
3663
3664         BT_DBG("chan %p", chan);
3665
3666         rsp.dcid    = cpu_to_le16(chan->scid);
3667         rsp.mtu     = cpu_to_le16(chan->imtu);
3668         rsp.mps     = cpu_to_le16(chan->mps);
3669         rsp.credits = cpu_to_le16(chan->rx_credits);
3670         rsp.result  = cpu_to_le16(L2CAP_CR_SUCCESS);
3671
3672         l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CONN_RSP, sizeof(rsp),
3673                        &rsp);
3674 }
3675
3676 void __l2cap_connect_rsp_defer(struct l2cap_chan *chan)
3677 {
3678         struct l2cap_conn_rsp rsp;
3679         struct l2cap_conn *conn = chan->conn;
3680         u8 buf[128];
3681         u8 rsp_code;
3682
3683         rsp.scid   = cpu_to_le16(chan->dcid);
3684         rsp.dcid   = cpu_to_le16(chan->scid);
3685         rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
3686         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
3687
3688         if (chan->hs_hcon)
3689                 rsp_code = L2CAP_CREATE_CHAN_RSP;
3690         else
3691                 rsp_code = L2CAP_CONN_RSP;
3692
3693         BT_DBG("chan %p rsp_code %u", chan, rsp_code);
3694
3695         l2cap_send_cmd(conn, chan->ident, rsp_code, sizeof(rsp), &rsp);
3696
3697         if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
3698                 return;
3699
3700         l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3701                        l2cap_build_conf_req(chan, buf, sizeof(buf)), buf);
3702         chan->num_conf_req++;
3703 }
3704
3705 static void l2cap_conf_rfc_get(struct l2cap_chan *chan, void *rsp, int len)
3706 {
3707         int type, olen;
3708         unsigned long val;
3709         /* Use sane default values in case a misbehaving remote device
3710          * did not send an RFC or extended window size option.
3711          */
3712         u16 txwin_ext = chan->ack_win;
3713         struct l2cap_conf_rfc rfc = {
3714                 .mode = chan->mode,
3715                 .retrans_timeout = cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO),
3716                 .monitor_timeout = cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO),
3717                 .max_pdu_size = cpu_to_le16(chan->imtu),
3718                 .txwin_size = min_t(u16, chan->ack_win, L2CAP_DEFAULT_TX_WINDOW),
3719         };
3720
3721         BT_DBG("chan %p, rsp %p, len %d", chan, rsp, len);
3722
3723         if ((chan->mode != L2CAP_MODE_ERTM) && (chan->mode != L2CAP_MODE_STREAMING))
3724                 return;
3725
3726         while (len >= L2CAP_CONF_OPT_SIZE) {
3727                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
3728
3729                 switch (type) {
3730                 case L2CAP_CONF_RFC:
3731                         if (olen == sizeof(rfc))
3732                                 memcpy(&rfc, (void *)val, olen);
3733                         break;
3734                 case L2CAP_CONF_EWS:
3735                         txwin_ext = val;
3736                         break;
3737                 }
3738         }
3739
3740         switch (rfc.mode) {
3741         case L2CAP_MODE_ERTM:
3742                 chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
3743                 chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
3744                 chan->mps = le16_to_cpu(rfc.max_pdu_size);
3745                 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
3746                         chan->ack_win = min_t(u16, chan->ack_win, txwin_ext);
3747                 else
3748                         chan->ack_win = min_t(u16, chan->ack_win,
3749                                               rfc.txwin_size);
3750                 break;
3751         case L2CAP_MODE_STREAMING:
3752                 chan->mps    = le16_to_cpu(rfc.max_pdu_size);
3753         }
3754 }
3755
3756 static inline int l2cap_command_rej(struct l2cap_conn *conn,
3757                                     struct l2cap_cmd_hdr *cmd, u16 cmd_len,
3758                                     u8 *data)
3759 {
3760         struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data;
3761
3762         if (cmd_len < sizeof(*rej))
3763                 return -EPROTO;
3764
3765         if (rej->reason != L2CAP_REJ_NOT_UNDERSTOOD)
3766                 return 0;
3767
3768         if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
3769             cmd->ident == conn->info_ident) {
3770                 cancel_delayed_work(&conn->info_timer);
3771
3772                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3773                 conn->info_ident = 0;
3774
3775                 l2cap_conn_start(conn);
3776         }
3777
3778         return 0;
3779 }
3780
3781 static struct l2cap_chan *l2cap_connect(struct l2cap_conn *conn,
3782                                         struct l2cap_cmd_hdr *cmd,
3783                                         u8 *data, u8 rsp_code, u8 amp_id)
3784 {
3785         struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
3786         struct l2cap_conn_rsp rsp;
3787         struct l2cap_chan *chan = NULL, *pchan;
3788         int result, status = L2CAP_CS_NO_INFO;
3789
3790         u16 dcid = 0, scid = __le16_to_cpu(req->scid);
3791         __le16 psm = req->psm;
3792
3793         BT_DBG("psm 0x%2.2x scid 0x%4.4x", __le16_to_cpu(psm), scid);
3794
3795         /* Check if we have socket listening on psm */
3796         pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, &conn->hcon->src,
3797                                          &conn->hcon->dst, ACL_LINK);
3798         if (!pchan) {
3799                 result = L2CAP_CR_BAD_PSM;
3800                 goto sendresp;
3801         }
3802
3803         mutex_lock(&conn->chan_lock);
3804         l2cap_chan_lock(pchan);
3805
3806         /* Check if the ACL is secure enough (if not SDP) */
3807         if (psm != cpu_to_le16(L2CAP_PSM_SDP) &&
3808             !hci_conn_check_link_mode(conn->hcon)) {
3809                 conn->disc_reason = HCI_ERROR_AUTH_FAILURE;
3810                 result = L2CAP_CR_SEC_BLOCK;
3811                 goto response;
3812         }
3813
3814         result = L2CAP_CR_NO_MEM;
3815
3816         /* Check if we already have channel with that dcid */
3817         if (__l2cap_get_chan_by_dcid(conn, scid))
3818                 goto response;
3819
3820         chan = pchan->ops->new_connection(pchan);
3821         if (!chan)
3822                 goto response;
3823
3824         /* For certain devices (ex: HID mouse), support for authentication,
3825          * pairing and bonding is optional. For such devices, inorder to avoid
3826          * the ACL alive for too long after L2CAP disconnection, reset the ACL
3827          * disc_timeout back to HCI_DISCONN_TIMEOUT during L2CAP connect.
3828          */
3829         conn->hcon->disc_timeout = HCI_DISCONN_TIMEOUT;
3830
3831         bacpy(&chan->src, &conn->hcon->src);
3832         bacpy(&chan->dst, &conn->hcon->dst);
3833         chan->src_type = bdaddr_src_type(conn->hcon);
3834         chan->dst_type = bdaddr_dst_type(conn->hcon);
3835         chan->psm  = psm;
3836         chan->dcid = scid;
3837         chan->local_amp_id = amp_id;
3838
3839         __l2cap_chan_add(conn, chan);
3840
3841         dcid = chan->scid;
3842
3843         __set_chan_timer(chan, chan->ops->get_sndtimeo(chan));
3844
3845         chan->ident = cmd->ident;
3846
3847         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
3848                 if (l2cap_chan_check_security(chan, false)) {
3849                         if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
3850                                 l2cap_state_change(chan, BT_CONNECT2);
3851                                 result = L2CAP_CR_PEND;
3852                                 status = L2CAP_CS_AUTHOR_PEND;
3853                                 chan->ops->defer(chan);
3854                         } else {
3855                                 /* Force pending result for AMP controllers.
3856                                  * The connection will succeed after the
3857                                  * physical link is up.
3858                                  */
3859                                 if (amp_id == AMP_ID_BREDR) {
3860                                         l2cap_state_change(chan, BT_CONFIG);
3861                                         result = L2CAP_CR_SUCCESS;
3862                                 } else {
3863                                         l2cap_state_change(chan, BT_CONNECT2);
3864                                         result = L2CAP_CR_PEND;
3865                                 }
3866                                 status = L2CAP_CS_NO_INFO;
3867                         }
3868                 } else {
3869                         l2cap_state_change(chan, BT_CONNECT2);
3870                         result = L2CAP_CR_PEND;
3871                         status = L2CAP_CS_AUTHEN_PEND;
3872                 }
3873         } else {
3874                 l2cap_state_change(chan, BT_CONNECT2);
3875                 result = L2CAP_CR_PEND;
3876                 status = L2CAP_CS_NO_INFO;
3877         }
3878
3879 response:
3880         l2cap_chan_unlock(pchan);
3881         mutex_unlock(&conn->chan_lock);
3882         l2cap_chan_put(pchan);
3883
3884 sendresp:
3885         rsp.scid   = cpu_to_le16(scid);
3886         rsp.dcid   = cpu_to_le16(dcid);
3887         rsp.result = cpu_to_le16(result);
3888         rsp.status = cpu_to_le16(status);
3889         l2cap_send_cmd(conn, cmd->ident, rsp_code, sizeof(rsp), &rsp);
3890
3891         if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
3892                 struct l2cap_info_req info;
3893                 info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
3894
3895                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
3896                 conn->info_ident = l2cap_get_ident(conn);
3897
3898                 schedule_delayed_work(&conn->info_timer, L2CAP_INFO_TIMEOUT);
3899
3900                 l2cap_send_cmd(conn, conn->info_ident, L2CAP_INFO_REQ,
3901                                sizeof(info), &info);
3902         }
3903
3904         if (chan && !test_bit(CONF_REQ_SENT, &chan->conf_state) &&
3905             result == L2CAP_CR_SUCCESS) {
3906                 u8 buf[128];
3907                 set_bit(CONF_REQ_SENT, &chan->conf_state);
3908                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3909                                l2cap_build_conf_req(chan, buf, sizeof(buf)), buf);
3910                 chan->num_conf_req++;
3911         }
3912
3913         return chan;
3914 }
3915
3916 static int l2cap_connect_req(struct l2cap_conn *conn,
3917                              struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
3918 {
3919         struct hci_dev *hdev = conn->hcon->hdev;
3920         struct hci_conn *hcon = conn->hcon;
3921
3922         if (cmd_len < sizeof(struct l2cap_conn_req))
3923                 return -EPROTO;
3924
3925         hci_dev_lock(hdev);
3926         if (hci_dev_test_flag(hdev, HCI_MGMT) &&
3927             !test_and_set_bit(HCI_CONN_MGMT_CONNECTED, &hcon->flags))
3928                 mgmt_device_connected(hdev, hcon, 0, NULL, 0);
3929         hci_dev_unlock(hdev);
3930
3931         l2cap_connect(conn, cmd, data, L2CAP_CONN_RSP, 0);
3932         return 0;
3933 }
3934
3935 static int l2cap_connect_create_rsp(struct l2cap_conn *conn,
3936                                     struct l2cap_cmd_hdr *cmd, u16 cmd_len,
3937                                     u8 *data)
3938 {
3939         struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
3940         u16 scid, dcid, result, status;
3941         struct l2cap_chan *chan;
3942         u8 req[128];
3943         int err;
3944
3945         if (cmd_len < sizeof(*rsp))
3946                 return -EPROTO;
3947
3948         scid   = __le16_to_cpu(rsp->scid);
3949         dcid   = __le16_to_cpu(rsp->dcid);
3950         result = __le16_to_cpu(rsp->result);
3951         status = __le16_to_cpu(rsp->status);
3952
3953         BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x",
3954                dcid, scid, result, status);
3955
3956         mutex_lock(&conn->chan_lock);
3957
3958         if (scid) {
3959                 chan = __l2cap_get_chan_by_scid(conn, scid);
3960                 if (!chan) {
3961                         err = -EBADSLT;
3962                         goto unlock;
3963                 }
3964         } else {
3965                 chan = __l2cap_get_chan_by_ident(conn, cmd->ident);
3966                 if (!chan) {
3967                         err = -EBADSLT;
3968                         goto unlock;
3969                 }
3970         }
3971
3972         err = 0;
3973
3974         l2cap_chan_lock(chan);
3975
3976         switch (result) {
3977         case L2CAP_CR_SUCCESS:
3978                 l2cap_state_change(chan, BT_CONFIG);
3979                 chan->ident = 0;
3980                 chan->dcid = dcid;
3981                 clear_bit(CONF_CONNECT_PEND, &chan->conf_state);
3982
3983                 if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
3984                         break;
3985
3986                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3987                                l2cap_build_conf_req(chan, req, sizeof(req)), req);
3988                 chan->num_conf_req++;
3989                 break;
3990
3991         case L2CAP_CR_PEND:
3992                 set_bit(CONF_CONNECT_PEND, &chan->conf_state);
3993                 break;
3994
3995         default:
3996                 l2cap_chan_del(chan, ECONNREFUSED);
3997                 break;
3998         }
3999
4000         l2cap_chan_unlock(chan);
4001
4002 unlock:
4003         mutex_unlock(&conn->chan_lock);
4004
4005         return err;
4006 }
4007
4008 static inline void set_default_fcs(struct l2cap_chan *chan)
4009 {
4010         /* FCS is enabled only in ERTM or streaming mode, if one or both
4011          * sides request it.
4012          */
4013         if (chan->mode != L2CAP_MODE_ERTM && chan->mode != L2CAP_MODE_STREAMING)
4014                 chan->fcs = L2CAP_FCS_NONE;
4015         else if (!test_bit(CONF_RECV_NO_FCS, &chan->conf_state))
4016                 chan->fcs = L2CAP_FCS_CRC16;
4017 }
4018
4019 static void l2cap_send_efs_conf_rsp(struct l2cap_chan *chan, void *data,
4020                                     u8 ident, u16 flags)
4021 {
4022         struct l2cap_conn *conn = chan->conn;
4023
4024         BT_DBG("conn %p chan %p ident %d flags 0x%4.4x", conn, chan, ident,
4025                flags);
4026
4027         clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
4028         set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
4029
4030         l2cap_send_cmd(conn, ident, L2CAP_CONF_RSP,
4031                        l2cap_build_conf_rsp(chan, data,
4032                                             L2CAP_CONF_SUCCESS, flags), data);
4033 }
4034
4035 static void cmd_reject_invalid_cid(struct l2cap_conn *conn, u8 ident,
4036                                    u16 scid, u16 dcid)
4037 {
4038         struct l2cap_cmd_rej_cid rej;
4039
4040         rej.reason = cpu_to_le16(L2CAP_REJ_INVALID_CID);
4041         rej.scid = __cpu_to_le16(scid);
4042         rej.dcid = __cpu_to_le16(dcid);
4043
4044         l2cap_send_cmd(conn, ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
4045 }
4046
4047 static inline int l2cap_config_req(struct l2cap_conn *conn,
4048                                    struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4049                                    u8 *data)
4050 {
4051         struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
4052         u16 dcid, flags;
4053         u8 rsp[64];
4054         struct l2cap_chan *chan;
4055         int len, err = 0;
4056
4057         if (cmd_len < sizeof(*req))
4058                 return -EPROTO;
4059
4060         dcid  = __le16_to_cpu(req->dcid);
4061         flags = __le16_to_cpu(req->flags);
4062
4063         BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
4064
4065         chan = l2cap_get_chan_by_scid(conn, dcid);
4066         if (!chan) {
4067                 cmd_reject_invalid_cid(conn, cmd->ident, dcid, 0);
4068                 return 0;
4069         }
4070
4071         if (chan->state != BT_CONFIG && chan->state != BT_CONNECT2) {
4072                 cmd_reject_invalid_cid(conn, cmd->ident, chan->scid,
4073                                        chan->dcid);
4074                 goto unlock;
4075         }
4076
4077         /* Reject if config buffer is too small. */
4078         len = cmd_len - sizeof(*req);
4079         if (chan->conf_len + len > sizeof(chan->conf_req)) {
4080                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
4081                                l2cap_build_conf_rsp(chan, rsp,
4082                                L2CAP_CONF_REJECT, flags), rsp);
4083                 goto unlock;
4084         }
4085
4086         /* Store config. */
4087         memcpy(chan->conf_req + chan->conf_len, req->data, len);
4088         chan->conf_len += len;
4089
4090         if (flags & L2CAP_CONF_FLAG_CONTINUATION) {
4091                 /* Incomplete config. Send empty response. */
4092                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
4093                                l2cap_build_conf_rsp(chan, rsp,
4094                                L2CAP_CONF_SUCCESS, flags), rsp);
4095                 goto unlock;
4096         }
4097
4098         /* Complete config. */
4099         len = l2cap_parse_conf_req(chan, rsp, sizeof(rsp));
4100         if (len < 0) {
4101                 l2cap_send_disconn_req(chan, ECONNRESET);
4102                 goto unlock;
4103         }
4104
4105         chan->ident = cmd->ident;
4106         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
4107         chan->num_conf_rsp++;
4108
4109         /* Reset config buffer. */
4110         chan->conf_len = 0;
4111
4112         if (!test_bit(CONF_OUTPUT_DONE, &chan->conf_state))
4113                 goto unlock;
4114
4115         if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
4116                 set_default_fcs(chan);
4117
4118                 if (chan->mode == L2CAP_MODE_ERTM ||
4119                     chan->mode == L2CAP_MODE_STREAMING)
4120                         err = l2cap_ertm_init(chan);
4121
4122                 if (err < 0)
4123                         l2cap_send_disconn_req(chan, -err);
4124                 else
4125                         l2cap_chan_ready(chan);
4126
4127                 goto unlock;
4128         }
4129
4130         if (!test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) {
4131                 u8 buf[64];
4132                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
4133                                l2cap_build_conf_req(chan, buf, sizeof(buf)), buf);
4134                 chan->num_conf_req++;
4135         }
4136
4137         /* Got Conf Rsp PENDING from remote side and assume we sent
4138            Conf Rsp PENDING in the code above */
4139         if (test_bit(CONF_REM_CONF_PEND, &chan->conf_state) &&
4140             test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
4141
4142                 /* check compatibility */
4143
4144                 /* Send rsp for BR/EDR channel */
4145                 if (!chan->hs_hcon)
4146                         l2cap_send_efs_conf_rsp(chan, rsp, cmd->ident, flags);
4147                 else
4148                         chan->ident = cmd->ident;
4149         }
4150
4151 unlock:
4152         l2cap_chan_unlock(chan);
4153         return err;
4154 }
4155
4156 static inline int l2cap_config_rsp(struct l2cap_conn *conn,
4157                                    struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4158                                    u8 *data)
4159 {
4160         struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
4161         u16 scid, flags, result;
4162         struct l2cap_chan *chan;
4163         int len = cmd_len - sizeof(*rsp);
4164         int err = 0;
4165
4166         if (cmd_len < sizeof(*rsp))
4167                 return -EPROTO;
4168
4169         scid   = __le16_to_cpu(rsp->scid);
4170         flags  = __le16_to_cpu(rsp->flags);
4171         result = __le16_to_cpu(rsp->result);
4172
4173         BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x len %d", scid, flags,
4174                result, len);
4175
4176         chan = l2cap_get_chan_by_scid(conn, scid);
4177         if (!chan)
4178                 return 0;
4179
4180         switch (result) {
4181         case L2CAP_CONF_SUCCESS:
4182                 l2cap_conf_rfc_get(chan, rsp->data, len);
4183                 clear_bit(CONF_REM_CONF_PEND, &chan->conf_state);
4184                 break;
4185
4186         case L2CAP_CONF_PENDING:
4187                 set_bit(CONF_REM_CONF_PEND, &chan->conf_state);
4188
4189                 if (test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
4190                         char buf[64];
4191
4192                         len = l2cap_parse_conf_rsp(chan, rsp->data, len,
4193                                                    buf, sizeof(buf), &result);
4194                         if (len < 0) {
4195                                 l2cap_send_disconn_req(chan, ECONNRESET);
4196                                 goto done;
4197                         }
4198
4199                         if (!chan->hs_hcon) {
4200                                 l2cap_send_efs_conf_rsp(chan, buf, cmd->ident,
4201                                                         0);
4202                         } else {
4203                                 if (l2cap_check_efs(chan)) {
4204                                         amp_create_logical_link(chan);
4205                                         chan->ident = cmd->ident;
4206                                 }
4207                         }
4208                 }
4209                 goto done;
4210
4211         case L2CAP_CONF_UNACCEPT:
4212                 if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
4213                         char req[64];
4214
4215                         if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
4216                                 l2cap_send_disconn_req(chan, ECONNRESET);
4217                                 goto done;
4218                         }
4219
4220                         /* throw out any old stored conf requests */
4221                         result = L2CAP_CONF_SUCCESS;
4222                         len = l2cap_parse_conf_rsp(chan, rsp->data, len,
4223                                                    req, sizeof(req), &result);
4224                         if (len < 0) {
4225                                 l2cap_send_disconn_req(chan, ECONNRESET);
4226                                 goto done;
4227                         }
4228
4229                         l2cap_send_cmd(conn, l2cap_get_ident(conn),
4230                                        L2CAP_CONF_REQ, len, req);
4231                         chan->num_conf_req++;
4232                         if (result != L2CAP_CONF_SUCCESS)
4233                                 goto done;
4234                         break;
4235                 }
4236
4237         default:
4238                 l2cap_chan_set_err(chan, ECONNRESET);
4239
4240                 __set_chan_timer(chan, L2CAP_DISC_REJ_TIMEOUT);
4241                 l2cap_send_disconn_req(chan, ECONNRESET);
4242                 goto done;
4243         }
4244
4245         if (flags & L2CAP_CONF_FLAG_CONTINUATION)
4246                 goto done;
4247
4248         set_bit(CONF_INPUT_DONE, &chan->conf_state);
4249
4250         if (test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) {
4251                 set_default_fcs(chan);
4252
4253                 if (chan->mode == L2CAP_MODE_ERTM ||
4254                     chan->mode == L2CAP_MODE_STREAMING)
4255                         err = l2cap_ertm_init(chan);
4256
4257                 if (err < 0)
4258                         l2cap_send_disconn_req(chan, -err);
4259                 else
4260                         l2cap_chan_ready(chan);
4261         }
4262
4263 done:
4264         l2cap_chan_unlock(chan);
4265         return err;
4266 }
4267
4268 static inline int l2cap_disconnect_req(struct l2cap_conn *conn,
4269                                        struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4270                                        u8 *data)
4271 {
4272         struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
4273         struct l2cap_disconn_rsp rsp;
4274         u16 dcid, scid;
4275         struct l2cap_chan *chan;
4276
4277         if (cmd_len != sizeof(*req))
4278                 return -EPROTO;
4279
4280         scid = __le16_to_cpu(req->scid);
4281         dcid = __le16_to_cpu(req->dcid);
4282
4283         BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
4284
4285         mutex_lock(&conn->chan_lock);
4286
4287         chan = __l2cap_get_chan_by_scid(conn, dcid);
4288         if (!chan) {
4289                 mutex_unlock(&conn->chan_lock);
4290                 cmd_reject_invalid_cid(conn, cmd->ident, dcid, scid);
4291                 return 0;
4292         }
4293
4294         l2cap_chan_lock(chan);
4295
4296         rsp.dcid = cpu_to_le16(chan->scid);
4297         rsp.scid = cpu_to_le16(chan->dcid);
4298         l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
4299
4300         chan->ops->set_shutdown(chan);
4301
4302         l2cap_chan_hold(chan);
4303         l2cap_chan_del(chan, ECONNRESET);
4304
4305         l2cap_chan_unlock(chan);
4306
4307         chan->ops->close(chan);
4308         l2cap_chan_put(chan);
4309
4310         mutex_unlock(&conn->chan_lock);
4311
4312         return 0;
4313 }
4314
4315 static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn,
4316                                        struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4317                                        u8 *data)
4318 {
4319         struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
4320         u16 dcid, scid;
4321         struct l2cap_chan *chan;
4322
4323         if (cmd_len != sizeof(*rsp))
4324                 return -EPROTO;
4325
4326         scid = __le16_to_cpu(rsp->scid);
4327         dcid = __le16_to_cpu(rsp->dcid);
4328
4329         BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
4330
4331         mutex_lock(&conn->chan_lock);
4332
4333         chan = __l2cap_get_chan_by_scid(conn, scid);
4334         if (!chan) {
4335                 mutex_unlock(&conn->chan_lock);
4336                 return 0;
4337         }
4338
4339         l2cap_chan_lock(chan);
4340
4341         l2cap_chan_hold(chan);
4342         l2cap_chan_del(chan, 0);
4343
4344         l2cap_chan_unlock(chan);
4345
4346         chan->ops->close(chan);
4347         l2cap_chan_put(chan);
4348
4349         mutex_unlock(&conn->chan_lock);
4350
4351         return 0;
4352 }
4353
4354 static inline int l2cap_information_req(struct l2cap_conn *conn,
4355                                         struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4356                                         u8 *data)
4357 {
4358         struct l2cap_info_req *req = (struct l2cap_info_req *) data;
4359         u16 type;
4360
4361         if (cmd_len != sizeof(*req))
4362                 return -EPROTO;
4363
4364         type = __le16_to_cpu(req->type);
4365
4366         BT_DBG("type 0x%4.4x", type);
4367
4368         if (type == L2CAP_IT_FEAT_MASK) {
4369                 u8 buf[8];
4370                 u32 feat_mask = l2cap_feat_mask;
4371                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
4372                 rsp->type   = cpu_to_le16(L2CAP_IT_FEAT_MASK);
4373                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
4374                 if (!disable_ertm)
4375                         feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
4376                                 | L2CAP_FEAT_FCS;
4377                 if (conn->local_fixed_chan & L2CAP_FC_A2MP)
4378                         feat_mask |= L2CAP_FEAT_EXT_FLOW
4379                                 | L2CAP_FEAT_EXT_WINDOW;
4380
4381                 put_unaligned_le32(feat_mask, rsp->data);
4382                 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf),
4383                                buf);
4384         } else if (type == L2CAP_IT_FIXED_CHAN) {
4385                 u8 buf[12];
4386                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
4387
4388                 rsp->type   = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
4389                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
4390                 rsp->data[0] = conn->local_fixed_chan;
4391                 memset(rsp->data + 1, 0, 7);
4392                 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf),
4393                                buf);
4394         } else {
4395                 struct l2cap_info_rsp rsp;
4396                 rsp.type   = cpu_to_le16(type);
4397                 rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP);
4398                 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(rsp),
4399                                &rsp);
4400         }
4401
4402         return 0;
4403 }
4404
4405 static inline int l2cap_information_rsp(struct l2cap_conn *conn,
4406                                         struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4407                                         u8 *data)
4408 {
4409         struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
4410         u16 type, result;
4411
4412         if (cmd_len < sizeof(*rsp))
4413                 return -EPROTO;
4414
4415         type   = __le16_to_cpu(rsp->type);
4416         result = __le16_to_cpu(rsp->result);
4417
4418         BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
4419
4420         /* L2CAP Info req/rsp are unbound to channels, add extra checks */
4421         if (cmd->ident != conn->info_ident ||
4422             conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)
4423                 return 0;
4424
4425         cancel_delayed_work(&conn->info_timer);
4426
4427         if (result != L2CAP_IR_SUCCESS) {
4428                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4429                 conn->info_ident = 0;
4430
4431                 l2cap_conn_start(conn);
4432
4433                 return 0;
4434         }
4435
4436         switch (type) {
4437         case L2CAP_IT_FEAT_MASK:
4438                 conn->feat_mask = get_unaligned_le32(rsp->data);
4439
4440                 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
4441                         struct l2cap_info_req req;
4442                         req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
4443
4444                         conn->info_ident = l2cap_get_ident(conn);
4445
4446                         l2cap_send_cmd(conn, conn->info_ident,
4447                                        L2CAP_INFO_REQ, sizeof(req), &req);
4448                 } else {
4449                         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4450                         conn->info_ident = 0;
4451
4452                         l2cap_conn_start(conn);
4453                 }
4454                 break;
4455
4456         case L2CAP_IT_FIXED_CHAN:
4457                 conn->remote_fixed_chan = rsp->data[0];
4458                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4459                 conn->info_ident = 0;
4460
4461                 l2cap_conn_start(conn);
4462                 break;
4463         }
4464
4465         return 0;
4466 }
4467
4468 static int l2cap_create_channel_req(struct l2cap_conn *conn,
4469                                     struct l2cap_cmd_hdr *cmd,
4470                                     u16 cmd_len, void *data)
4471 {
4472         struct l2cap_create_chan_req *req = data;
4473         struct l2cap_create_chan_rsp rsp;
4474         struct l2cap_chan *chan;
4475         struct hci_dev *hdev;
4476         u16 psm, scid;
4477
4478         if (cmd_len != sizeof(*req))
4479                 return -EPROTO;
4480
4481         if (!(conn->local_fixed_chan & L2CAP_FC_A2MP))
4482                 return -EINVAL;
4483
4484         psm = le16_to_cpu(req->psm);
4485         scid = le16_to_cpu(req->scid);
4486
4487         BT_DBG("psm 0x%2.2x, scid 0x%4.4x, amp_id %d", psm, scid, req->amp_id);
4488
4489         /* For controller id 0 make BR/EDR connection */
4490         if (req->amp_id == AMP_ID_BREDR) {
4491                 l2cap_connect(conn, cmd, data, L2CAP_CREATE_CHAN_RSP,
4492                               req->amp_id);
4493                 return 0;
4494         }
4495
4496         /* Validate AMP controller id */
4497         hdev = hci_dev_get(req->amp_id);
4498         if (!hdev)
4499                 goto error;
4500
4501         if (hdev->dev_type != HCI_AMP || !test_bit(HCI_UP, &hdev->flags)) {
4502                 hci_dev_put(hdev);
4503                 goto error;
4504         }
4505
4506         chan = l2cap_connect(conn, cmd, data, L2CAP_CREATE_CHAN_RSP,
4507                              req->amp_id);
4508         if (chan) {
4509                 struct amp_mgr *mgr = conn->hcon->amp_mgr;
4510                 struct hci_conn *hs_hcon;
4511
4512                 hs_hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK,
4513                                                   &conn->hcon->dst);
4514                 if (!hs_hcon) {
4515                         hci_dev_put(hdev);
4516                         cmd_reject_invalid_cid(conn, cmd->ident, chan->scid,
4517                                                chan->dcid);
4518                         return 0;
4519                 }
4520
4521                 BT_DBG("mgr %p bredr_chan %p hs_hcon %p", mgr, chan, hs_hcon);
4522
4523                 mgr->bredr_chan = chan;
4524                 chan->hs_hcon = hs_hcon;
4525                 chan->fcs = L2CAP_FCS_NONE;
4526                 conn->mtu = hdev->block_mtu;
4527         }
4528
4529         hci_dev_put(hdev);
4530
4531         return 0;
4532
4533 error:
4534         rsp.dcid = 0;
4535         rsp.scid = cpu_to_le16(scid);
4536         rsp.result = cpu_to_le16(L2CAP_CR_BAD_AMP);
4537         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
4538
4539         l2cap_send_cmd(conn, cmd->ident, L2CAP_CREATE_CHAN_RSP,
4540                        sizeof(rsp), &rsp);
4541
4542         return 0;
4543 }
4544
4545 static void l2cap_send_move_chan_req(struct l2cap_chan *chan, u8 dest_amp_id)
4546 {
4547         struct l2cap_move_chan_req req;
4548         u8 ident;
4549
4550         BT_DBG("chan %p, dest_amp_id %d", chan, dest_amp_id);
4551
4552         ident = l2cap_get_ident(chan->conn);
4553         chan->ident = ident;
4554
4555         req.icid = cpu_to_le16(chan->scid);
4556         req.dest_amp_id = dest_amp_id;
4557
4558         l2cap_send_cmd(chan->conn, ident, L2CAP_MOVE_CHAN_REQ, sizeof(req),
4559                        &req);
4560
4561         __set_chan_timer(chan, L2CAP_MOVE_TIMEOUT);
4562 }
4563
4564 static void l2cap_send_move_chan_rsp(struct l2cap_chan *chan, u16 result)
4565 {
4566         struct l2cap_move_chan_rsp rsp;
4567
4568         BT_DBG("chan %p, result 0x%4.4x", chan, result);
4569
4570         rsp.icid = cpu_to_le16(chan->dcid);
4571         rsp.result = cpu_to_le16(result);
4572
4573         l2cap_send_cmd(chan->conn, chan->ident, L2CAP_MOVE_CHAN_RSP,
4574                        sizeof(rsp), &rsp);
4575 }
4576
4577 static void l2cap_send_move_chan_cfm(struct l2cap_chan *chan, u16 result)
4578 {
4579         struct l2cap_move_chan_cfm cfm;
4580
4581         BT_DBG("chan %p, result 0x%4.4x", chan, result);
4582
4583         chan->ident = l2cap_get_ident(chan->conn);
4584
4585         cfm.icid = cpu_to_le16(chan->scid);
4586         cfm.result = cpu_to_le16(result);
4587
4588         l2cap_send_cmd(chan->conn, chan->ident, L2CAP_MOVE_CHAN_CFM,
4589                        sizeof(cfm), &cfm);
4590
4591         __set_chan_timer(chan, L2CAP_MOVE_TIMEOUT);
4592 }
4593
4594 static void l2cap_send_move_chan_cfm_icid(struct l2cap_conn *conn, u16 icid)
4595 {
4596         struct l2cap_move_chan_cfm cfm;
4597
4598         BT_DBG("conn %p, icid 0x%4.4x", conn, icid);
4599
4600         cfm.icid = cpu_to_le16(icid);
4601         cfm.result = cpu_to_le16(L2CAP_MC_UNCONFIRMED);
4602
4603         l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_MOVE_CHAN_CFM,
4604                        sizeof(cfm), &cfm);
4605 }
4606
4607 static void l2cap_send_move_chan_cfm_rsp(struct l2cap_conn *conn, u8 ident,
4608                                          u16 icid)
4609 {
4610         struct l2cap_move_chan_cfm_rsp rsp;
4611
4612         BT_DBG("icid 0x%4.4x", icid);
4613
4614         rsp.icid = cpu_to_le16(icid);
4615         l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM_RSP, sizeof(rsp), &rsp);
4616 }
4617
4618 static void __release_logical_link(struct l2cap_chan *chan)
4619 {
4620         chan->hs_hchan = NULL;
4621         chan->hs_hcon = NULL;
4622
4623         /* Placeholder - release the logical link */
4624 }
4625
4626 static void l2cap_logical_fail(struct l2cap_chan *chan)
4627 {
4628         /* Logical link setup failed */
4629         if (chan->state != BT_CONNECTED) {
4630                 /* Create channel failure, disconnect */
4631                 l2cap_send_disconn_req(chan, ECONNRESET);
4632                 return;
4633         }
4634
4635         switch (chan->move_role) {
4636         case L2CAP_MOVE_ROLE_RESPONDER:
4637                 l2cap_move_done(chan);
4638                 l2cap_send_move_chan_rsp(chan, L2CAP_MR_NOT_SUPP);
4639                 break;
4640         case L2CAP_MOVE_ROLE_INITIATOR:
4641                 if (chan->move_state == L2CAP_MOVE_WAIT_LOGICAL_COMP ||
4642                     chan->move_state == L2CAP_MOVE_WAIT_LOGICAL_CFM) {
4643                         /* Remote has only sent pending or
4644                          * success responses, clean up
4645                          */
4646                         l2cap_move_done(chan);
4647                 }
4648
4649                 /* Other amp move states imply that the move
4650                  * has already aborted
4651                  */
4652                 l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
4653                 break;
4654         }
4655 }
4656
4657 static void l2cap_logical_finish_create(struct l2cap_chan *chan,
4658                                         struct hci_chan *hchan)
4659 {
4660         struct l2cap_conf_rsp rsp;
4661
4662         chan->hs_hchan = hchan;
4663         chan->hs_hcon->l2cap_data = chan->conn;
4664
4665         l2cap_send_efs_conf_rsp(chan, &rsp, chan->ident, 0);
4666
4667         if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
4668                 int err;
4669
4670                 set_default_fcs(chan);
4671
4672                 err = l2cap_ertm_init(chan);
4673                 if (err < 0)
4674                         l2cap_send_disconn_req(chan, -err);
4675                 else
4676                         l2cap_chan_ready(chan);
4677         }
4678 }
4679
4680 static void l2cap_logical_finish_move(struct l2cap_chan *chan,
4681                                       struct hci_chan *hchan)
4682 {
4683         chan->hs_hcon = hchan->conn;
4684         chan->hs_hcon->l2cap_data = chan->conn;
4685
4686         BT_DBG("move_state %d", chan->move_state);
4687
4688         switch (chan->move_state) {
4689         case L2CAP_MOVE_WAIT_LOGICAL_COMP:
4690                 /* Move confirm will be sent after a success
4691                  * response is received
4692                  */
4693                 chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
4694                 break;
4695         case L2CAP_MOVE_WAIT_LOGICAL_CFM:
4696                 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
4697                         chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
4698                 } else if (chan->move_role == L2CAP_MOVE_ROLE_INITIATOR) {
4699                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM_RSP;
4700                         l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
4701                 } else if (chan->move_role == L2CAP_MOVE_ROLE_RESPONDER) {
4702                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
4703                         l2cap_send_move_chan_rsp(chan, L2CAP_MR_SUCCESS);
4704                 }
4705                 break;
4706         default:
4707                 /* Move was not in expected state, free the channel */
4708                 __release_logical_link(chan);
4709
4710                 chan->move_state = L2CAP_MOVE_STABLE;
4711         }
4712 }
4713
4714 /* Call with chan locked */
4715 void l2cap_logical_cfm(struct l2cap_chan *chan, struct hci_chan *hchan,
4716                        u8 status)
4717 {
4718         BT_DBG("chan %p, hchan %p, status %d", chan, hchan, status);
4719
4720         if (status) {
4721                 l2cap_logical_fail(chan);
4722                 __release_logical_link(chan);
4723                 return;
4724         }
4725
4726         if (chan->state != BT_CONNECTED) {
4727                 /* Ignore logical link if channel is on BR/EDR */
4728                 if (chan->local_amp_id != AMP_ID_BREDR)
4729                         l2cap_logical_finish_create(chan, hchan);
4730         } else {
4731                 l2cap_logical_finish_move(chan, hchan);
4732         }
4733 }
4734
4735 void l2cap_move_start(struct l2cap_chan *chan)
4736 {
4737         BT_DBG("chan %p", chan);
4738
4739         if (chan->local_amp_id == AMP_ID_BREDR) {
4740                 if (chan->chan_policy != BT_CHANNEL_POLICY_AMP_PREFERRED)
4741                         return;
4742                 chan->move_role = L2CAP_MOVE_ROLE_INITIATOR;
4743                 chan->move_state = L2CAP_MOVE_WAIT_PREPARE;
4744                 /* Placeholder - start physical link setup */
4745         } else {
4746                 chan->move_role = L2CAP_MOVE_ROLE_INITIATOR;
4747                 chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
4748                 chan->move_id = 0;
4749                 l2cap_move_setup(chan);
4750                 l2cap_send_move_chan_req(chan, 0);
4751         }
4752 }
4753
4754 static void l2cap_do_create(struct l2cap_chan *chan, int result,
4755                             u8 local_amp_id, u8 remote_amp_id)
4756 {
4757         BT_DBG("chan %p state %s %u -> %u", chan, state_to_string(chan->state),
4758                local_amp_id, remote_amp_id);
4759
4760         chan->fcs = L2CAP_FCS_NONE;
4761
4762         /* Outgoing channel on AMP */
4763         if (chan->state == BT_CONNECT) {
4764                 if (result == L2CAP_CR_SUCCESS) {
4765                         chan->local_amp_id = local_amp_id;
4766                         l2cap_send_create_chan_req(chan, remote_amp_id);
4767                 } else {
4768                         /* Revert to BR/EDR connect */
4769                         l2cap_send_conn_req(chan);
4770                 }
4771
4772                 return;
4773         }
4774
4775         /* Incoming channel on AMP */
4776         if (__l2cap_no_conn_pending(chan)) {
4777                 struct l2cap_conn_rsp rsp;
4778                 char buf[128];
4779                 rsp.scid = cpu_to_le16(chan->dcid);
4780                 rsp.dcid = cpu_to_le16(chan->scid);
4781
4782                 if (result == L2CAP_CR_SUCCESS) {
4783                         /* Send successful response */
4784                         rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
4785                         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
4786                 } else {
4787                         /* Send negative response */
4788                         rsp.result = cpu_to_le16(L2CAP_CR_NO_MEM);
4789                         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
4790                 }
4791
4792                 l2cap_send_cmd(chan->conn, chan->ident, L2CAP_CREATE_CHAN_RSP,
4793                                sizeof(rsp), &rsp);
4794
4795                 if (result == L2CAP_CR_SUCCESS) {
4796                         l2cap_state_change(chan, BT_CONFIG);
4797                         set_bit(CONF_REQ_SENT, &chan->conf_state);
4798                         l2cap_send_cmd(chan->conn, l2cap_get_ident(chan->conn),
4799                                        L2CAP_CONF_REQ,
4800                                        l2cap_build_conf_req(chan, buf, sizeof(buf)), buf);
4801                         chan->num_conf_req++;
4802                 }
4803         }
4804 }
4805
4806 static void l2cap_do_move_initiate(struct l2cap_chan *chan, u8 local_amp_id,
4807                                    u8 remote_amp_id)
4808 {
4809         l2cap_move_setup(chan);
4810         chan->move_id = local_amp_id;
4811         chan->move_state = L2CAP_MOVE_WAIT_RSP;
4812
4813         l2cap_send_move_chan_req(chan, remote_amp_id);
4814 }
4815
4816 static void l2cap_do_move_respond(struct l2cap_chan *chan, int result)
4817 {
4818         struct hci_chan *hchan = NULL;
4819
4820         /* Placeholder - get hci_chan for logical link */
4821
4822         if (hchan) {
4823                 if (hchan->state == BT_CONNECTED) {
4824                         /* Logical link is ready to go */
4825                         chan->hs_hcon = hchan->conn;
4826                         chan->hs_hcon->l2cap_data = chan->conn;
4827                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
4828                         l2cap_send_move_chan_rsp(chan, L2CAP_MR_SUCCESS);
4829
4830                         l2cap_logical_cfm(chan, hchan, L2CAP_MR_SUCCESS);
4831                 } else {
4832                         /* Wait for logical link to be ready */
4833                         chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
4834                 }
4835         } else {
4836                 /* Logical link not available */
4837                 l2cap_send_move_chan_rsp(chan, L2CAP_MR_NOT_ALLOWED);
4838         }
4839 }
4840
4841 static void l2cap_do_move_cancel(struct l2cap_chan *chan, int result)
4842 {
4843         if (chan->move_role == L2CAP_MOVE_ROLE_RESPONDER) {
4844                 u8 rsp_result;
4845                 if (result == -EINVAL)
4846                         rsp_result = L2CAP_MR_BAD_ID;
4847                 else
4848                         rsp_result = L2CAP_MR_NOT_ALLOWED;
4849
4850                 l2cap_send_move_chan_rsp(chan, rsp_result);
4851         }
4852
4853         chan->move_role = L2CAP_MOVE_ROLE_NONE;
4854         chan->move_state = L2CAP_MOVE_STABLE;
4855
4856         /* Restart data transmission */
4857         l2cap_ertm_send(chan);
4858 }
4859
4860 /* Invoke with locked chan */
4861 void __l2cap_physical_cfm(struct l2cap_chan *chan, int result)
4862 {
4863         u8 local_amp_id = chan->local_amp_id;
4864         u8 remote_amp_id = chan->remote_amp_id;
4865
4866         BT_DBG("chan %p, result %d, local_amp_id %d, remote_amp_id %d",
4867                chan, result, local_amp_id, remote_amp_id);
4868
4869         if (chan->state == BT_DISCONN || chan->state == BT_CLOSED) {
4870                 l2cap_chan_unlock(chan);
4871                 return;
4872         }
4873
4874         if (chan->state != BT_CONNECTED) {
4875                 l2cap_do_create(chan, result, local_amp_id, remote_amp_id);
4876         } else if (result != L2CAP_MR_SUCCESS) {
4877                 l2cap_do_move_cancel(chan, result);
4878         } else {
4879                 switch (chan->move_role) {
4880                 case L2CAP_MOVE_ROLE_INITIATOR:
4881                         l2cap_do_move_initiate(chan, local_amp_id,
4882                                                remote_amp_id);
4883                         break;
4884                 case L2CAP_MOVE_ROLE_RESPONDER:
4885                         l2cap_do_move_respond(chan, result);
4886                         break;
4887                 default:
4888                         l2cap_do_move_cancel(chan, result);
4889                         break;
4890                 }
4891         }
4892 }
4893
4894 static inline int l2cap_move_channel_req(struct l2cap_conn *conn,
4895                                          struct l2cap_cmd_hdr *cmd,
4896                                          u16 cmd_len, void *data)
4897 {
4898         struct l2cap_move_chan_req *req = data;
4899         struct l2cap_move_chan_rsp rsp;
4900         struct l2cap_chan *chan;
4901         u16 icid = 0;
4902         u16 result = L2CAP_MR_NOT_ALLOWED;
4903
4904         if (cmd_len != sizeof(*req))
4905                 return -EPROTO;
4906
4907         icid = le16_to_cpu(req->icid);
4908
4909         BT_DBG("icid 0x%4.4x, dest_amp_id %d", icid, req->dest_amp_id);
4910
4911         if (!(conn->local_fixed_chan & L2CAP_FC_A2MP))
4912                 return -EINVAL;
4913
4914         chan = l2cap_get_chan_by_dcid(conn, icid);
4915         if (!chan) {
4916                 rsp.icid = cpu_to_le16(icid);
4917                 rsp.result = cpu_to_le16(L2CAP_MR_NOT_ALLOWED);
4918                 l2cap_send_cmd(conn, cmd->ident, L2CAP_MOVE_CHAN_RSP,
4919                                sizeof(rsp), &rsp);
4920                 return 0;
4921         }
4922
4923         chan->ident = cmd->ident;
4924
4925         if (chan->scid < L2CAP_CID_DYN_START ||
4926             chan->chan_policy == BT_CHANNEL_POLICY_BREDR_ONLY ||
4927             (chan->mode != L2CAP_MODE_ERTM &&
4928              chan->mode != L2CAP_MODE_STREAMING)) {
4929                 result = L2CAP_MR_NOT_ALLOWED;
4930                 goto send_move_response;
4931         }
4932
4933         if (chan->local_amp_id == req->dest_amp_id) {
4934                 result = L2CAP_MR_SAME_ID;
4935                 goto send_move_response;
4936         }
4937
4938         if (req->dest_amp_id != AMP_ID_BREDR) {
4939                 struct hci_dev *hdev;
4940                 hdev = hci_dev_get(req->dest_amp_id);
4941                 if (!hdev || hdev->dev_type != HCI_AMP ||
4942                     !test_bit(HCI_UP, &hdev->flags)) {
4943                         if (hdev)
4944                                 hci_dev_put(hdev);
4945
4946                         result = L2CAP_MR_BAD_ID;
4947                         goto send_move_response;
4948                 }
4949                 hci_dev_put(hdev);
4950         }
4951
4952         /* Detect a move collision.  Only send a collision response
4953          * if this side has "lost", otherwise proceed with the move.
4954          * The winner has the larger bd_addr.
4955          */
4956         if ((__chan_is_moving(chan) ||
4957              chan->move_role != L2CAP_MOVE_ROLE_NONE) &&
4958             bacmp(&conn->hcon->src, &conn->hcon->dst) > 0) {
4959                 result = L2CAP_MR_COLLISION;
4960                 goto send_move_response;
4961         }
4962
4963         chan->move_role = L2CAP_MOVE_ROLE_RESPONDER;
4964         l2cap_move_setup(chan);
4965         chan->move_id = req->dest_amp_id;
4966         icid = chan->dcid;
4967
4968         if (req->dest_amp_id == AMP_ID_BREDR) {
4969                 /* Moving to BR/EDR */
4970                 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
4971                         chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
4972                         result = L2CAP_MR_PEND;
4973                 } else {
4974                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
4975                         result = L2CAP_MR_SUCCESS;
4976                 }
4977         } else {
4978                 chan->move_state = L2CAP_MOVE_WAIT_PREPARE;
4979                 /* Placeholder - uncomment when amp functions are available */
4980                 /*amp_accept_physical(chan, req->dest_amp_id);*/
4981                 result = L2CAP_MR_PEND;
4982         }
4983
4984 send_move_response:
4985         l2cap_send_move_chan_rsp(chan, result);
4986
4987         l2cap_chan_unlock(chan);
4988
4989         return 0;
4990 }
4991
4992 static void l2cap_move_continue(struct l2cap_conn *conn, u16 icid, u16 result)
4993 {
4994         struct l2cap_chan *chan;
4995         struct hci_chan *hchan = NULL;
4996
4997         chan = l2cap_get_chan_by_scid(conn, icid);
4998         if (!chan) {
4999                 l2cap_send_move_chan_cfm_icid(conn, icid);
5000                 return;
5001         }
5002
5003         __clear_chan_timer(chan);
5004         if (result == L2CAP_MR_PEND)
5005                 __set_chan_timer(chan, L2CAP_MOVE_ERTX_TIMEOUT);
5006
5007         switch (chan->move_state) {
5008         case L2CAP_MOVE_WAIT_LOGICAL_COMP:
5009                 /* Move confirm will be sent when logical link
5010                  * is complete.
5011                  */
5012                 chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
5013                 break;
5014         case L2CAP_MOVE_WAIT_RSP_SUCCESS:
5015                 if (result == L2CAP_MR_PEND) {
5016                         break;
5017                 } else if (test_bit(CONN_LOCAL_BUSY,
5018                                     &chan->conn_state)) {
5019                         chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
5020                 } else {
5021                         /* Logical link is up or moving to BR/EDR,
5022                          * proceed with move
5023                          */
5024                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM_RSP;
5025                         l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
5026                 }
5027                 break;
5028         case L2CAP_MOVE_WAIT_RSP:
5029                 /* Moving to AMP */
5030                 if (result == L2CAP_MR_SUCCESS) {
5031                         /* Remote is ready, send confirm immediately
5032                          * after logical link is ready
5033                          */
5034                         chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
5035                 } else {
5036                         /* Both logical link and move success
5037                          * are required to confirm
5038                          */
5039                         chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_COMP;
5040                 }
5041
5042                 /* Placeholder - get hci_chan for logical link */
5043                 if (!hchan) {
5044                         /* Logical link not available */
5045                         l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
5046                         break;
5047                 }
5048
5049                 /* If the logical link is not yet connected, do not
5050                  * send confirmation.
5051                  */
5052                 if (hchan->state != BT_CONNECTED)
5053                         break;
5054
5055                 /* Logical link is already ready to go */
5056
5057                 chan->hs_hcon = hchan->conn;
5058                 chan->hs_hcon->l2cap_data = chan->conn;
5059
5060                 if (result == L2CAP_MR_SUCCESS) {
5061                         /* Can confirm now */
5062                         l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
5063                 } else {
5064                         /* Now only need move success
5065                          * to confirm
5066                          */
5067                         chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
5068                 }
5069
5070                 l2cap_logical_cfm(chan, hchan, L2CAP_MR_SUCCESS);
5071                 break;
5072         default:
5073                 /* Any other amp move state means the move failed. */
5074                 chan->move_id = chan->local_amp_id;
5075                 l2cap_move_done(chan);
5076                 l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
5077         }
5078
5079         l2cap_chan_unlock(chan);
5080 }
5081
5082 static void l2cap_move_fail(struct l2cap_conn *conn, u8 ident, u16 icid,
5083                             u16 result)
5084 {
5085         struct l2cap_chan *chan;
5086
5087         chan = l2cap_get_chan_by_ident(conn, ident);
5088         if (!chan) {
5089                 /* Could not locate channel, icid is best guess */
5090                 l2cap_send_move_chan_cfm_icid(conn, icid);
5091                 return;
5092         }
5093
5094         __clear_chan_timer(chan);
5095
5096         if (chan->move_role == L2CAP_MOVE_ROLE_INITIATOR) {
5097                 if (result == L2CAP_MR_COLLISION) {
5098                         chan->move_role = L2CAP_MOVE_ROLE_RESPONDER;
5099                 } else {
5100                         /* Cleanup - cancel move */
5101                         chan->move_id = chan->local_amp_id;
5102                         l2cap_move_done(chan);
5103                 }
5104         }
5105
5106         l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
5107
5108         l2cap_chan_unlock(chan);
5109 }
5110
5111 static int l2cap_move_channel_rsp(struct l2cap_conn *conn,
5112                                   struct l2cap_cmd_hdr *cmd,
5113                                   u16 cmd_len, void *data)
5114 {
5115         struct l2cap_move_chan_rsp *rsp = data;
5116         u16 icid, result;
5117
5118         if (cmd_len != sizeof(*rsp))
5119                 return -EPROTO;
5120
5121         icid = le16_to_cpu(rsp->icid);
5122         result = le16_to_cpu(rsp->result);
5123
5124         BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result);
5125
5126         if (result == L2CAP_MR_SUCCESS || result == L2CAP_MR_PEND)
5127                 l2cap_move_continue(conn, icid, result);
5128         else
5129                 l2cap_move_fail(conn, cmd->ident, icid, result);
5130
5131         return 0;
5132 }
5133
5134 static int l2cap_move_channel_confirm(struct l2cap_conn *conn,
5135                                       struct l2cap_cmd_hdr *cmd,
5136                                       u16 cmd_len, void *data)
5137 {
5138         struct l2cap_move_chan_cfm *cfm = data;
5139         struct l2cap_chan *chan;
5140         u16 icid, result;
5141
5142         if (cmd_len != sizeof(*cfm))
5143                 return -EPROTO;
5144
5145         icid = le16_to_cpu(cfm->icid);
5146         result = le16_to_cpu(cfm->result);
5147
5148         BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result);
5149
5150         chan = l2cap_get_chan_by_dcid(conn, icid);
5151         if (!chan) {
5152                 /* Spec requires a response even if the icid was not found */
5153                 l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid);
5154                 return 0;
5155         }
5156
5157         if (chan->move_state == L2CAP_MOVE_WAIT_CONFIRM) {
5158                 if (result == L2CAP_MC_CONFIRMED) {
5159                         chan->local_amp_id = chan->move_id;
5160                         if (chan->local_amp_id == AMP_ID_BREDR)
5161                                 __release_logical_link(chan);
5162                 } else {
5163                         chan->move_id = chan->local_amp_id;
5164                 }
5165
5166                 l2cap_move_done(chan);
5167         }
5168
5169         l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid);
5170
5171         l2cap_chan_unlock(chan);
5172
5173         return 0;
5174 }
5175
5176 static inline int l2cap_move_channel_confirm_rsp(struct l2cap_conn *conn,
5177                                                  struct l2cap_cmd_hdr *cmd,
5178                                                  u16 cmd_len, void *data)
5179 {
5180         struct l2cap_move_chan_cfm_rsp *rsp = data;
5181         struct l2cap_chan *chan;
5182         u16 icid;
5183
5184         if (cmd_len != sizeof(*rsp))
5185                 return -EPROTO;
5186
5187         icid = le16_to_cpu(rsp->icid);
5188
5189         BT_DBG("icid 0x%4.4x", icid);
5190
5191         chan = l2cap_get_chan_by_scid(conn, icid);
5192         if (!chan)
5193                 return 0;
5194
5195         __clear_chan_timer(chan);
5196
5197         if (chan->move_state == L2CAP_MOVE_WAIT_CONFIRM_RSP) {
5198                 chan->local_amp_id = chan->move_id;
5199
5200                 if (chan->local_amp_id == AMP_ID_BREDR && chan->hs_hchan)
5201                         __release_logical_link(chan);
5202
5203                 l2cap_move_done(chan);
5204         }
5205
5206         l2cap_chan_unlock(chan);
5207
5208         return 0;
5209 }
5210
5211 static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
5212                                               struct l2cap_cmd_hdr *cmd,
5213                                               u16 cmd_len, u8 *data)
5214 {
5215         struct hci_conn *hcon = conn->hcon;
5216         struct l2cap_conn_param_update_req *req;
5217         struct l2cap_conn_param_update_rsp rsp;
5218         u16 min, max, latency, to_multiplier;
5219         int err;
5220
5221         if (hcon->role != HCI_ROLE_MASTER)
5222                 return -EINVAL;
5223
5224         if (cmd_len != sizeof(struct l2cap_conn_param_update_req))
5225                 return -EPROTO;
5226
5227         req = (struct l2cap_conn_param_update_req *) data;
5228         min             = __le16_to_cpu(req->min);
5229         max             = __le16_to_cpu(req->max);
5230         latency         = __le16_to_cpu(req->latency);
5231         to_multiplier   = __le16_to_cpu(req->to_multiplier);
5232
5233         BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x",
5234                min, max, latency, to_multiplier);
5235
5236         memset(&rsp, 0, sizeof(rsp));
5237
5238         err = hci_check_conn_params(min, max, latency, to_multiplier);
5239         if (err)
5240                 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_REJECTED);
5241         else
5242                 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED);
5243
5244         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP,
5245                        sizeof(rsp), &rsp);
5246
5247         if (!err) {
5248                 u8 store_hint;
5249
5250                 store_hint = hci_le_conn_update(hcon, min, max, latency,
5251                                                 to_multiplier);
5252                 mgmt_new_conn_param(hcon->hdev, &hcon->dst, hcon->dst_type,
5253                                     store_hint, min, max, latency,
5254                                     to_multiplier);
5255
5256         }
5257
5258         return 0;
5259 }
5260
5261 static int l2cap_le_connect_rsp(struct l2cap_conn *conn,
5262                                 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5263                                 u8 *data)
5264 {
5265         struct l2cap_le_conn_rsp *rsp = (struct l2cap_le_conn_rsp *) data;
5266         struct hci_conn *hcon = conn->hcon;
5267         u16 dcid, mtu, mps, credits, result;
5268         struct l2cap_chan *chan;
5269         int err, sec_level;
5270
5271         if (cmd_len < sizeof(*rsp))
5272                 return -EPROTO;
5273
5274         dcid    = __le16_to_cpu(rsp->dcid);
5275         mtu     = __le16_to_cpu(rsp->mtu);
5276         mps     = __le16_to_cpu(rsp->mps);
5277         credits = __le16_to_cpu(rsp->credits);
5278         result  = __le16_to_cpu(rsp->result);
5279
5280         if (result == L2CAP_CR_SUCCESS && (mtu < 23 || mps < 23 ||
5281                                            dcid < L2CAP_CID_DYN_START ||
5282                                            dcid > L2CAP_CID_LE_DYN_END))
5283                 return -EPROTO;
5284
5285         BT_DBG("dcid 0x%4.4x mtu %u mps %u credits %u result 0x%2.2x",
5286                dcid, mtu, mps, credits, result);
5287
5288         mutex_lock(&conn->chan_lock);
5289
5290         chan = __l2cap_get_chan_by_ident(conn, cmd->ident);
5291         if (!chan) {
5292                 err = -EBADSLT;
5293                 goto unlock;
5294         }
5295
5296         err = 0;
5297
5298         l2cap_chan_lock(chan);
5299
5300         switch (result) {
5301         case L2CAP_CR_SUCCESS:
5302                 if (__l2cap_get_chan_by_dcid(conn, dcid)) {
5303                         err = -EBADSLT;
5304                         break;
5305                 }
5306
5307                 chan->ident = 0;
5308                 chan->dcid = dcid;
5309                 chan->omtu = mtu;
5310                 chan->remote_mps = mps;
5311                 chan->tx_credits = credits;
5312                 l2cap_chan_ready(chan);
5313                 break;
5314
5315         case L2CAP_CR_AUTHENTICATION:
5316         case L2CAP_CR_ENCRYPTION:
5317                 /* If we already have MITM protection we can't do
5318                  * anything.
5319                  */
5320                 if (hcon->sec_level > BT_SECURITY_MEDIUM) {
5321                         l2cap_chan_del(chan, ECONNREFUSED);
5322                         break;
5323                 }
5324
5325                 sec_level = hcon->sec_level + 1;
5326                 if (chan->sec_level < sec_level)
5327                         chan->sec_level = sec_level;
5328
5329                 /* We'll need to send a new Connect Request */
5330                 clear_bit(FLAG_LE_CONN_REQ_SENT, &chan->flags);
5331
5332                 smp_conn_security(hcon, chan->sec_level);
5333                 break;
5334
5335         default:
5336                 l2cap_chan_del(chan, ECONNREFUSED);
5337                 break;
5338         }
5339
5340         l2cap_chan_unlock(chan);
5341
5342 unlock:
5343         mutex_unlock(&conn->chan_lock);
5344
5345         return err;
5346 }
5347
5348 static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
5349                                       struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5350                                       u8 *data)
5351 {
5352         int err = 0;
5353
5354         switch (cmd->code) {
5355         case L2CAP_COMMAND_REJ:
5356                 l2cap_command_rej(conn, cmd, cmd_len, data);
5357                 break;
5358
5359         case L2CAP_CONN_REQ:
5360                 err = l2cap_connect_req(conn, cmd, cmd_len, data);
5361                 break;
5362
5363         case L2CAP_CONN_RSP:
5364         case L2CAP_CREATE_CHAN_RSP:
5365                 l2cap_connect_create_rsp(conn, cmd, cmd_len, data);
5366                 break;
5367
5368         case L2CAP_CONF_REQ:
5369                 err = l2cap_config_req(conn, cmd, cmd_len, data);
5370                 break;
5371
5372         case L2CAP_CONF_RSP:
5373                 l2cap_config_rsp(conn, cmd, cmd_len, data);
5374                 break;
5375
5376         case L2CAP_DISCONN_REQ:
5377                 err = l2cap_disconnect_req(conn, cmd, cmd_len, data);
5378                 break;
5379
5380         case L2CAP_DISCONN_RSP:
5381                 l2cap_disconnect_rsp(conn, cmd, cmd_len, data);
5382                 break;
5383
5384         case L2CAP_ECHO_REQ:
5385                 l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data);
5386                 break;
5387
5388         case L2CAP_ECHO_RSP:
5389                 break;
5390
5391         case L2CAP_INFO_REQ:
5392                 err = l2cap_information_req(conn, cmd, cmd_len, data);
5393                 break;
5394
5395         case L2CAP_INFO_RSP:
5396                 l2cap_information_rsp(conn, cmd, cmd_len, data);
5397                 break;
5398
5399         case L2CAP_CREATE_CHAN_REQ:
5400                 err = l2cap_create_channel_req(conn, cmd, cmd_len, data);
5401                 break;
5402
5403         case L2CAP_MOVE_CHAN_REQ:
5404                 err = l2cap_move_channel_req(conn, cmd, cmd_len, data);
5405                 break;
5406
5407         case L2CAP_MOVE_CHAN_RSP:
5408                 l2cap_move_channel_rsp(conn, cmd, cmd_len, data);
5409                 break;
5410
5411         case L2CAP_MOVE_CHAN_CFM:
5412                 err = l2cap_move_channel_confirm(conn, cmd, cmd_len, data);
5413                 break;
5414
5415         case L2CAP_MOVE_CHAN_CFM_RSP:
5416                 l2cap_move_channel_confirm_rsp(conn, cmd, cmd_len, data);
5417                 break;
5418
5419         default:
5420                 BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code);
5421                 err = -EINVAL;
5422                 break;
5423         }
5424
5425         return err;
5426 }
5427
5428 static int l2cap_le_connect_req(struct l2cap_conn *conn,
5429                                 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5430                                 u8 *data)
5431 {
5432         struct l2cap_le_conn_req *req = (struct l2cap_le_conn_req *) data;
5433         struct l2cap_le_conn_rsp rsp;
5434         struct l2cap_chan *chan, *pchan;
5435         u16 dcid, scid, credits, mtu, mps;
5436         __le16 psm;
5437         u8 result;
5438
5439         if (cmd_len != sizeof(*req))
5440                 return -EPROTO;
5441
5442         scid = __le16_to_cpu(req->scid);
5443         mtu  = __le16_to_cpu(req->mtu);
5444         mps  = __le16_to_cpu(req->mps);
5445         psm  = req->psm;
5446         dcid = 0;
5447         credits = 0;
5448
5449         if (mtu < 23 || mps < 23)
5450                 return -EPROTO;
5451
5452         BT_DBG("psm 0x%2.2x scid 0x%4.4x mtu %u mps %u", __le16_to_cpu(psm),
5453                scid, mtu, mps);
5454
5455         /* Check if we have socket listening on psm */
5456         pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, &conn->hcon->src,
5457                                          &conn->hcon->dst, LE_LINK);
5458         if (!pchan) {
5459                 result = L2CAP_CR_BAD_PSM;
5460                 chan = NULL;
5461                 goto response;
5462         }
5463
5464         mutex_lock(&conn->chan_lock);
5465         l2cap_chan_lock(pchan);
5466
5467         if (!smp_sufficient_security(conn->hcon, pchan->sec_level,
5468                                      SMP_ALLOW_STK)) {
5469                 result = L2CAP_CR_AUTHENTICATION;
5470                 chan = NULL;
5471                 goto response_unlock;
5472         }
5473
5474         /* Check for valid dynamic CID range */
5475         if (scid < L2CAP_CID_DYN_START || scid > L2CAP_CID_LE_DYN_END) {
5476                 result = L2CAP_CR_INVALID_SCID;
5477                 chan = NULL;
5478                 goto response_unlock;
5479         }
5480
5481         /* Check if we already have channel with that dcid */
5482         if (__l2cap_get_chan_by_dcid(conn, scid)) {
5483                 result = L2CAP_CR_SCID_IN_USE;
5484                 chan = NULL;
5485                 goto response_unlock;
5486         }
5487
5488         chan = pchan->ops->new_connection(pchan);
5489         if (!chan) {
5490                 result = L2CAP_CR_NO_MEM;
5491                 goto response_unlock;
5492         }
5493
5494         l2cap_le_flowctl_init(chan);
5495
5496         bacpy(&chan->src, &conn->hcon->src);
5497         bacpy(&chan->dst, &conn->hcon->dst);
5498         chan->src_type = bdaddr_src_type(conn->hcon);
5499         chan->dst_type = bdaddr_dst_type(conn->hcon);
5500         chan->psm  = psm;
5501         chan->dcid = scid;
5502         chan->omtu = mtu;
5503         chan->remote_mps = mps;
5504         chan->tx_credits = __le16_to_cpu(req->credits);
5505
5506         __l2cap_chan_add(conn, chan);
5507         dcid = chan->scid;
5508         credits = chan->rx_credits;
5509
5510         __set_chan_timer(chan, chan->ops->get_sndtimeo(chan));
5511
5512         chan->ident = cmd->ident;
5513
5514         if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
5515                 l2cap_state_change(chan, BT_CONNECT2);
5516                 /* The following result value is actually not defined
5517                  * for LE CoC but we use it to let the function know
5518                  * that it should bail out after doing its cleanup
5519                  * instead of sending a response.
5520                  */
5521                 result = L2CAP_CR_PEND;
5522                 chan->ops->defer(chan);
5523         } else {
5524                 l2cap_chan_ready(chan);
5525                 result = L2CAP_CR_SUCCESS;
5526         }
5527
5528 response_unlock:
5529         l2cap_chan_unlock(pchan);
5530         mutex_unlock(&conn->chan_lock);
5531         l2cap_chan_put(pchan);
5532
5533         if (result == L2CAP_CR_PEND)
5534                 return 0;
5535
5536 response:
5537         if (chan) {
5538                 rsp.mtu = cpu_to_le16(chan->imtu);
5539                 rsp.mps = cpu_to_le16(chan->mps);
5540         } else {
5541                 rsp.mtu = 0;
5542                 rsp.mps = 0;
5543         }
5544
5545         rsp.dcid    = cpu_to_le16(dcid);
5546         rsp.credits = cpu_to_le16(credits);
5547         rsp.result  = cpu_to_le16(result);
5548
5549         l2cap_send_cmd(conn, cmd->ident, L2CAP_LE_CONN_RSP, sizeof(rsp), &rsp);
5550
5551         return 0;
5552 }
5553
5554 static inline int l2cap_le_credits(struct l2cap_conn *conn,
5555                                    struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5556                                    u8 *data)
5557 {
5558         struct l2cap_le_credits *pkt;
5559         struct l2cap_chan *chan;
5560         u16 cid, credits, max_credits;
5561
5562         if (cmd_len != sizeof(*pkt))
5563                 return -EPROTO;
5564
5565         pkt = (struct l2cap_le_credits *) data;
5566         cid     = __le16_to_cpu(pkt->cid);
5567         credits = __le16_to_cpu(pkt->credits);
5568
5569         BT_DBG("cid 0x%4.4x credits 0x%4.4x", cid, credits);
5570
5571         chan = l2cap_get_chan_by_dcid(conn, cid);
5572         if (!chan)
5573                 return -EBADSLT;
5574
5575         max_credits = LE_FLOWCTL_MAX_CREDITS - chan->tx_credits;
5576         if (credits > max_credits) {
5577                 BT_ERR("LE credits overflow");
5578                 l2cap_send_disconn_req(chan, ECONNRESET);
5579                 l2cap_chan_unlock(chan);
5580
5581                 /* Return 0 so that we don't trigger an unnecessary
5582                  * command reject packet.
5583                  */
5584                 return 0;
5585         }
5586
5587         chan->tx_credits += credits;
5588
5589         /* Resume sending */
5590         l2cap_le_flowctl_send(chan);
5591
5592         if (chan->tx_credits)
5593                 chan->ops->resume(chan);
5594
5595         l2cap_chan_unlock(chan);
5596
5597         return 0;
5598 }
5599
5600 static inline int l2cap_le_command_rej(struct l2cap_conn *conn,
5601                                        struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5602                                        u8 *data)
5603 {
5604         struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data;
5605         struct l2cap_chan *chan;
5606
5607         if (cmd_len < sizeof(*rej))
5608                 return -EPROTO;
5609
5610         mutex_lock(&conn->chan_lock);
5611
5612         chan = __l2cap_get_chan_by_ident(conn, cmd->ident);
5613         if (!chan)
5614                 goto done;
5615
5616         l2cap_chan_lock(chan);
5617         l2cap_chan_del(chan, ECONNREFUSED);
5618         l2cap_chan_unlock(chan);
5619
5620 done:
5621         mutex_unlock(&conn->chan_lock);
5622         return 0;
5623 }
5624
5625 static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
5626                                    struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5627                                    u8 *data)
5628 {
5629         int err = 0;
5630
5631         switch (cmd->code) {
5632         case L2CAP_COMMAND_REJ:
5633                 l2cap_le_command_rej(conn, cmd, cmd_len, data);
5634                 break;
5635
5636         case L2CAP_CONN_PARAM_UPDATE_REQ:
5637                 err = l2cap_conn_param_update_req(conn, cmd, cmd_len, data);
5638                 break;
5639
5640         case L2CAP_CONN_PARAM_UPDATE_RSP:
5641                 break;
5642
5643         case L2CAP_LE_CONN_RSP:
5644                 l2cap_le_connect_rsp(conn, cmd, cmd_len, data);
5645                 break;
5646
5647         case L2CAP_LE_CONN_REQ:
5648                 err = l2cap_le_connect_req(conn, cmd, cmd_len, data);
5649                 break;
5650
5651         case L2CAP_LE_CREDITS:
5652                 err = l2cap_le_credits(conn, cmd, cmd_len, data);
5653                 break;
5654
5655         case L2CAP_DISCONN_REQ:
5656                 err = l2cap_disconnect_req(conn, cmd, cmd_len, data);
5657                 break;
5658
5659         case L2CAP_DISCONN_RSP:
5660                 l2cap_disconnect_rsp(conn, cmd, cmd_len, data);
5661                 break;
5662
5663         default:
5664                 BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code);
5665                 err = -EINVAL;
5666                 break;
5667         }
5668
5669         return err;
5670 }
5671
5672 static inline void l2cap_le_sig_channel(struct l2cap_conn *conn,
5673                                         struct sk_buff *skb)
5674 {
5675         struct hci_conn *hcon = conn->hcon;
5676         struct l2cap_cmd_hdr *cmd;
5677         u16 len;
5678         int err;
5679
5680         if (hcon->type != LE_LINK)
5681                 goto drop;
5682
5683         if (skb->len < L2CAP_CMD_HDR_SIZE)
5684                 goto drop;
5685
5686         cmd = (void *) skb->data;
5687         skb_pull(skb, L2CAP_CMD_HDR_SIZE);
5688
5689         len = le16_to_cpu(cmd->len);
5690
5691         BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd->code, len, cmd->ident);
5692
5693         if (len != skb->len || !cmd->ident) {
5694                 BT_DBG("corrupted command");
5695                 goto drop;
5696         }
5697
5698         err = l2cap_le_sig_cmd(conn, cmd, len, skb->data);
5699         if (err) {
5700                 struct l2cap_cmd_rej_unk rej;
5701
5702                 BT_ERR("Wrong link type (%d)", err);
5703
5704                 rej.reason = cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
5705                 l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
5706                                sizeof(rej), &rej);
5707         }
5708
5709 drop:
5710         kfree_skb(skb);
5711 }
5712
5713 static inline void l2cap_sig_channel(struct l2cap_conn *conn,
5714                                      struct sk_buff *skb)
5715 {
5716         struct hci_conn *hcon = conn->hcon;
5717         u8 *data = skb->data;
5718         int len = skb->len;
5719         struct l2cap_cmd_hdr cmd;
5720         int err;
5721
5722         l2cap_raw_recv(conn, skb);
5723
5724         if (hcon->type != ACL_LINK)
5725                 goto drop;
5726
5727         while (len >= L2CAP_CMD_HDR_SIZE) {
5728                 u16 cmd_len;
5729                 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
5730                 data += L2CAP_CMD_HDR_SIZE;
5731                 len  -= L2CAP_CMD_HDR_SIZE;
5732
5733                 cmd_len = le16_to_cpu(cmd.len);
5734
5735                 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len,
5736                        cmd.ident);
5737
5738                 if (cmd_len > len || !cmd.ident) {
5739                         BT_DBG("corrupted command");
5740                         break;
5741                 }
5742
5743                 err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data);
5744                 if (err) {
5745                         struct l2cap_cmd_rej_unk rej;
5746
5747                         BT_ERR("Wrong link type (%d)", err);
5748
5749                         rej.reason = cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
5750                         l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ,
5751                                        sizeof(rej), &rej);
5752                 }
5753
5754                 data += cmd_len;
5755                 len  -= cmd_len;
5756         }
5757
5758 drop:
5759         kfree_skb(skb);
5760 }
5761
5762 static int l2cap_check_fcs(struct l2cap_chan *chan,  struct sk_buff *skb)
5763 {
5764         u16 our_fcs, rcv_fcs;
5765         int hdr_size;
5766
5767         if (test_bit(FLAG_EXT_CTRL, &chan->flags))
5768                 hdr_size = L2CAP_EXT_HDR_SIZE;
5769         else
5770                 hdr_size = L2CAP_ENH_HDR_SIZE;
5771
5772         if (chan->fcs == L2CAP_FCS_CRC16) {
5773                 skb_trim(skb, skb->len - L2CAP_FCS_SIZE);
5774                 rcv_fcs = get_unaligned_le16(skb->data + skb->len);
5775                 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
5776
5777                 if (our_fcs != rcv_fcs)
5778                         return -EBADMSG;
5779         }
5780         return 0;
5781 }
5782
5783 static void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan)
5784 {
5785         struct l2cap_ctrl control;
5786
5787         BT_DBG("chan %p", chan);
5788
5789         memset(&control, 0, sizeof(control));
5790         control.sframe = 1;
5791         control.final = 1;
5792         control.reqseq = chan->buffer_seq;
5793         set_bit(CONN_SEND_FBIT, &chan->conn_state);
5794
5795         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
5796                 control.super = L2CAP_SUPER_RNR;
5797                 l2cap_send_sframe(chan, &control);
5798         }
5799
5800         if (test_and_clear_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
5801             chan->unacked_frames > 0)
5802                 __set_retrans_timer(chan);
5803
5804         /* Send pending iframes */
5805         l2cap_ertm_send(chan);
5806
5807         if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
5808             test_bit(CONN_SEND_FBIT, &chan->conn_state)) {
5809                 /* F-bit wasn't sent in an s-frame or i-frame yet, so
5810                  * send it now.
5811                  */
5812                 control.super = L2CAP_SUPER_RR;
5813                 l2cap_send_sframe(chan, &control);
5814         }
5815 }
5816
5817 static void append_skb_frag(struct sk_buff *skb, struct sk_buff *new_frag,
5818                             struct sk_buff **last_frag)
5819 {
5820         /* skb->len reflects data in skb as well as all fragments
5821          * skb->data_len reflects only data in fragments
5822          */
5823         if (!skb_has_frag_list(skb))
5824                 skb_shinfo(skb)->frag_list = new_frag;
5825
5826         new_frag->next = NULL;
5827
5828         (*last_frag)->next = new_frag;
5829         *last_frag = new_frag;
5830
5831         skb->len += new_frag->len;
5832         skb->data_len += new_frag->len;
5833         skb->truesize += new_frag->truesize;
5834 }
5835
5836 static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb,
5837                                 struct l2cap_ctrl *control)
5838 {
5839         int err = -EINVAL;
5840
5841         switch (control->sar) {
5842         case L2CAP_SAR_UNSEGMENTED:
5843                 if (chan->sdu)
5844                         break;
5845
5846                 err = chan->ops->recv(chan, skb);
5847                 break;
5848
5849         case L2CAP_SAR_START:
5850                 if (chan->sdu)
5851                         break;
5852
5853                 if (!pskb_may_pull(skb, L2CAP_SDULEN_SIZE))
5854                         break;
5855
5856                 chan->sdu_len = get_unaligned_le16(skb->data);
5857                 skb_pull(skb, L2CAP_SDULEN_SIZE);
5858
5859                 if (chan->sdu_len > chan->imtu) {
5860                         err = -EMSGSIZE;
5861                         break;
5862                 }
5863
5864                 if (skb->len >= chan->sdu_len)
5865                         break;
5866
5867                 chan->sdu = skb;
5868                 chan->sdu_last_frag = skb;
5869
5870                 skb = NULL;
5871                 err = 0;
5872                 break;
5873
5874         case L2CAP_SAR_CONTINUE:
5875                 if (!chan->sdu)
5876                         break;
5877
5878                 append_skb_frag(chan->sdu, skb,
5879                                 &chan->sdu_last_frag);
5880                 skb = NULL;
5881
5882                 if (chan->sdu->len >= chan->sdu_len)
5883                         break;
5884
5885                 err = 0;
5886                 break;
5887
5888         case L2CAP_SAR_END:
5889                 if (!chan->sdu)
5890                         break;
5891
5892                 append_skb_frag(chan->sdu, skb,
5893                                 &chan->sdu_last_frag);
5894                 skb = NULL;
5895
5896                 if (chan->sdu->len != chan->sdu_len)
5897                         break;
5898
5899                 err = chan->ops->recv(chan, chan->sdu);
5900
5901                 if (!err) {
5902                         /* Reassembly complete */
5903                         chan->sdu = NULL;
5904                         chan->sdu_last_frag = NULL;
5905                         chan->sdu_len = 0;
5906                 }
5907                 break;
5908         }
5909
5910         if (err) {
5911                 kfree_skb(skb);
5912                 kfree_skb(chan->sdu);
5913                 chan->sdu = NULL;
5914                 chan->sdu_last_frag = NULL;
5915                 chan->sdu_len = 0;
5916         }
5917
5918         return err;
5919 }
5920
5921 static int l2cap_resegment(struct l2cap_chan *chan)
5922 {
5923         /* Placeholder */
5924         return 0;
5925 }
5926
5927 void l2cap_chan_busy(struct l2cap_chan *chan, int busy)
5928 {
5929         u8 event;
5930
5931         if (chan->mode != L2CAP_MODE_ERTM)
5932                 return;
5933
5934         event = busy ? L2CAP_EV_LOCAL_BUSY_DETECTED : L2CAP_EV_LOCAL_BUSY_CLEAR;
5935         l2cap_tx(chan, NULL, NULL, event);
5936 }
5937
5938 static int l2cap_rx_queued_iframes(struct l2cap_chan *chan)
5939 {
5940         int err = 0;
5941         /* Pass sequential frames to l2cap_reassemble_sdu()
5942          * until a gap is encountered.
5943          */
5944
5945         BT_DBG("chan %p", chan);
5946
5947         while (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
5948                 struct sk_buff *skb;
5949                 BT_DBG("Searching for skb with txseq %d (queue len %d)",
5950                        chan->buffer_seq, skb_queue_len(&chan->srej_q));
5951
5952                 skb = l2cap_ertm_seq_in_queue(&chan->srej_q, chan->buffer_seq);
5953
5954                 if (!skb)
5955                         break;
5956
5957                 skb_unlink(skb, &chan->srej_q);
5958                 chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
5959                 err = l2cap_reassemble_sdu(chan, skb, &bt_cb(skb)->l2cap);
5960                 if (err)
5961                         break;
5962         }
5963
5964         if (skb_queue_empty(&chan->srej_q)) {
5965                 chan->rx_state = L2CAP_RX_STATE_RECV;
5966                 l2cap_send_ack(chan);
5967         }
5968
5969         return err;
5970 }
5971
5972 static void l2cap_handle_srej(struct l2cap_chan *chan,
5973                               struct l2cap_ctrl *control)
5974 {
5975         struct sk_buff *skb;
5976
5977         BT_DBG("chan %p, control %p", chan, control);
5978
5979         if (control->reqseq == chan->next_tx_seq) {
5980                 BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq);
5981                 l2cap_send_disconn_req(chan, ECONNRESET);
5982                 return;
5983         }
5984
5985         skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq);
5986
5987         if (skb == NULL) {
5988                 BT_DBG("Seq %d not available for retransmission",
5989                        control->reqseq);
5990                 return;
5991         }
5992
5993         if (chan->max_tx != 0 && bt_cb(skb)->l2cap.retries >= chan->max_tx) {
5994                 BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
5995                 l2cap_send_disconn_req(chan, ECONNRESET);
5996                 return;
5997         }
5998
5999         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6000
6001         if (control->poll) {
6002                 l2cap_pass_to_tx(chan, control);
6003
6004                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
6005                 l2cap_retransmit(chan, control);
6006                 l2cap_ertm_send(chan);
6007
6008                 if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) {
6009                         set_bit(CONN_SREJ_ACT, &chan->conn_state);
6010                         chan->srej_save_reqseq = control->reqseq;
6011                 }
6012         } else {
6013                 l2cap_pass_to_tx_fbit(chan, control);
6014
6015                 if (control->final) {
6016                         if (chan->srej_save_reqseq != control->reqseq ||
6017                             !test_and_clear_bit(CONN_SREJ_ACT,
6018                                                 &chan->conn_state))
6019                                 l2cap_retransmit(chan, control);
6020                 } else {
6021                         l2cap_retransmit(chan, control);
6022                         if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) {
6023                                 set_bit(CONN_SREJ_ACT, &chan->conn_state);
6024                                 chan->srej_save_reqseq = control->reqseq;
6025                         }
6026                 }
6027         }
6028 }
6029
6030 static void l2cap_handle_rej(struct l2cap_chan *chan,
6031                              struct l2cap_ctrl *control)
6032 {
6033         struct sk_buff *skb;
6034
6035         BT_DBG("chan %p, control %p", chan, control);
6036
6037         if (control->reqseq == chan->next_tx_seq) {
6038                 BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq);
6039                 l2cap_send_disconn_req(chan, ECONNRESET);
6040                 return;
6041         }
6042
6043         skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq);
6044
6045         if (chan->max_tx && skb &&
6046             bt_cb(skb)->l2cap.retries >= chan->max_tx) {
6047                 BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
6048                 l2cap_send_disconn_req(chan, ECONNRESET);
6049                 return;
6050         }
6051
6052         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6053
6054         l2cap_pass_to_tx(chan, control);
6055
6056         if (control->final) {
6057                 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
6058                         l2cap_retransmit_all(chan, control);
6059         } else {
6060                 l2cap_retransmit_all(chan, control);
6061                 l2cap_ertm_send(chan);
6062                 if (chan->tx_state == L2CAP_TX_STATE_WAIT_F)
6063                         set_bit(CONN_REJ_ACT, &chan->conn_state);
6064         }
6065 }
6066
6067 static u8 l2cap_classify_txseq(struct l2cap_chan *chan, u16 txseq)
6068 {
6069         BT_DBG("chan %p, txseq %d", chan, txseq);
6070
6071         BT_DBG("last_acked_seq %d, expected_tx_seq %d", chan->last_acked_seq,
6072                chan->expected_tx_seq);
6073
6074         if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
6075                 if (__seq_offset(chan, txseq, chan->last_acked_seq) >=
6076                     chan->tx_win) {
6077                         /* See notes below regarding "double poll" and
6078                          * invalid packets.
6079                          */
6080                         if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) {
6081                                 BT_DBG("Invalid/Ignore - after SREJ");
6082                                 return L2CAP_TXSEQ_INVALID_IGNORE;
6083                         } else {
6084                                 BT_DBG("Invalid - in window after SREJ sent");
6085                                 return L2CAP_TXSEQ_INVALID;
6086                         }
6087                 }
6088
6089                 if (chan->srej_list.head == txseq) {
6090                         BT_DBG("Expected SREJ");
6091                         return L2CAP_TXSEQ_EXPECTED_SREJ;
6092                 }
6093
6094                 if (l2cap_ertm_seq_in_queue(&chan->srej_q, txseq)) {
6095                         BT_DBG("Duplicate SREJ - txseq already stored");
6096                         return L2CAP_TXSEQ_DUPLICATE_SREJ;
6097                 }
6098
6099                 if (l2cap_seq_list_contains(&chan->srej_list, txseq)) {
6100                         BT_DBG("Unexpected SREJ - not requested");
6101                         return L2CAP_TXSEQ_UNEXPECTED_SREJ;
6102                 }
6103         }
6104
6105         if (chan->expected_tx_seq == txseq) {
6106                 if (__seq_offset(chan, txseq, chan->last_acked_seq) >=
6107                     chan->tx_win) {
6108                         BT_DBG("Invalid - txseq outside tx window");
6109                         return L2CAP_TXSEQ_INVALID;
6110                 } else {
6111                         BT_DBG("Expected");
6112                         return L2CAP_TXSEQ_EXPECTED;
6113                 }
6114         }
6115
6116         if (__seq_offset(chan, txseq, chan->last_acked_seq) <
6117             __seq_offset(chan, chan->expected_tx_seq, chan->last_acked_seq)) {
6118                 BT_DBG("Duplicate - expected_tx_seq later than txseq");
6119                 return L2CAP_TXSEQ_DUPLICATE;
6120         }
6121
6122         if (__seq_offset(chan, txseq, chan->last_acked_seq) >= chan->tx_win) {
6123                 /* A source of invalid packets is a "double poll" condition,
6124                  * where delays cause us to send multiple poll packets.  If
6125                  * the remote stack receives and processes both polls,
6126                  * sequence numbers can wrap around in such a way that a
6127                  * resent frame has a sequence number that looks like new data
6128                  * with a sequence gap.  This would trigger an erroneous SREJ
6129                  * request.
6130                  *
6131                  * Fortunately, this is impossible with a tx window that's
6132                  * less than half of the maximum sequence number, which allows
6133                  * invalid frames to be safely ignored.
6134                  *
6135                  * With tx window sizes greater than half of the tx window
6136                  * maximum, the frame is invalid and cannot be ignored.  This
6137                  * causes a disconnect.
6138                  */
6139
6140                 if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) {
6141                         BT_DBG("Invalid/Ignore - txseq outside tx window");
6142                         return L2CAP_TXSEQ_INVALID_IGNORE;
6143                 } else {
6144                         BT_DBG("Invalid - txseq outside tx window");
6145                         return L2CAP_TXSEQ_INVALID;
6146                 }
6147         } else {
6148                 BT_DBG("Unexpected - txseq indicates missing frames");
6149                 return L2CAP_TXSEQ_UNEXPECTED;
6150         }
6151 }
6152
6153 static int l2cap_rx_state_recv(struct l2cap_chan *chan,
6154                                struct l2cap_ctrl *control,
6155                                struct sk_buff *skb, u8 event)
6156 {
6157         int err = 0;
6158         bool skb_in_use = false;
6159
6160         BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
6161                event);
6162
6163         switch (event) {
6164         case L2CAP_EV_RECV_IFRAME:
6165                 switch (l2cap_classify_txseq(chan, control->txseq)) {
6166                 case L2CAP_TXSEQ_EXPECTED:
6167                         l2cap_pass_to_tx(chan, control);
6168
6169                         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
6170                                 BT_DBG("Busy, discarding expected seq %d",
6171                                        control->txseq);
6172                                 break;
6173                         }
6174
6175                         chan->expected_tx_seq = __next_seq(chan,
6176                                                            control->txseq);
6177
6178                         chan->buffer_seq = chan->expected_tx_seq;
6179                         skb_in_use = true;
6180
6181                         err = l2cap_reassemble_sdu(chan, skb, control);
6182                         if (err)
6183                                 break;
6184
6185                         if (control->final) {
6186                                 if (!test_and_clear_bit(CONN_REJ_ACT,
6187                                                         &chan->conn_state)) {
6188                                         control->final = 0;
6189                                         l2cap_retransmit_all(chan, control);
6190                                         l2cap_ertm_send(chan);
6191                                 }
6192                         }
6193
6194                         if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state))
6195                                 l2cap_send_ack(chan);
6196                         break;
6197                 case L2CAP_TXSEQ_UNEXPECTED:
6198                         l2cap_pass_to_tx(chan, control);
6199
6200                         /* Can't issue SREJ frames in the local busy state.
6201                          * Drop this frame, it will be seen as missing
6202                          * when local busy is exited.
6203                          */
6204                         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
6205                                 BT_DBG("Busy, discarding unexpected seq %d",
6206                                        control->txseq);
6207                                 break;
6208                         }
6209
6210                         /* There was a gap in the sequence, so an SREJ
6211                          * must be sent for each missing frame.  The
6212                          * current frame is stored for later use.
6213                          */
6214                         skb_queue_tail(&chan->srej_q, skb);
6215                         skb_in_use = true;
6216                         BT_DBG("Queued %p (queue len %d)", skb,
6217                                skb_queue_len(&chan->srej_q));
6218
6219                         clear_bit(CONN_SREJ_ACT, &chan->conn_state);
6220                         l2cap_seq_list_clear(&chan->srej_list);
6221                         l2cap_send_srej(chan, control->txseq);
6222
6223                         chan->rx_state = L2CAP_RX_STATE_SREJ_SENT;
6224                         break;
6225                 case L2CAP_TXSEQ_DUPLICATE:
6226                         l2cap_pass_to_tx(chan, control);
6227                         break;
6228                 case L2CAP_TXSEQ_INVALID_IGNORE:
6229                         break;
6230                 case L2CAP_TXSEQ_INVALID:
6231                 default:
6232                         l2cap_send_disconn_req(chan, ECONNRESET);
6233                         break;
6234                 }
6235                 break;
6236         case L2CAP_EV_RECV_RR:
6237                 l2cap_pass_to_tx(chan, control);
6238                 if (control->final) {
6239                         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6240
6241                         if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state) &&
6242                             !__chan_is_moving(chan)) {
6243                                 control->final = 0;
6244                                 l2cap_retransmit_all(chan, control);
6245                         }
6246
6247                         l2cap_ertm_send(chan);
6248                 } else if (control->poll) {
6249                         l2cap_send_i_or_rr_or_rnr(chan);
6250                 } else {
6251                         if (test_and_clear_bit(CONN_REMOTE_BUSY,
6252                                                &chan->conn_state) &&
6253                             chan->unacked_frames)
6254                                 __set_retrans_timer(chan);
6255
6256                         l2cap_ertm_send(chan);
6257                 }
6258                 break;
6259         case L2CAP_EV_RECV_RNR:
6260                 set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6261                 l2cap_pass_to_tx(chan, control);
6262                 if (control && control->poll) {
6263                         set_bit(CONN_SEND_FBIT, &chan->conn_state);
6264                         l2cap_send_rr_or_rnr(chan, 0);
6265                 }
6266                 __clear_retrans_timer(chan);
6267                 l2cap_seq_list_clear(&chan->retrans_list);
6268                 break;
6269         case L2CAP_EV_RECV_REJ:
6270                 l2cap_handle_rej(chan, control);
6271                 break;
6272         case L2CAP_EV_RECV_SREJ:
6273                 l2cap_handle_srej(chan, control);
6274                 break;
6275         default:
6276                 break;
6277         }
6278
6279         if (skb && !skb_in_use) {
6280                 BT_DBG("Freeing %p", skb);
6281                 kfree_skb(skb);
6282         }
6283
6284         return err;
6285 }
6286
6287 static int l2cap_rx_state_srej_sent(struct l2cap_chan *chan,
6288                                     struct l2cap_ctrl *control,
6289                                     struct sk_buff *skb, u8 event)
6290 {
6291         int err = 0;
6292         u16 txseq = control->txseq;
6293         bool skb_in_use = false;
6294
6295         BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
6296                event);
6297
6298         switch (event) {
6299         case L2CAP_EV_RECV_IFRAME:
6300                 switch (l2cap_classify_txseq(chan, txseq)) {
6301                 case L2CAP_TXSEQ_EXPECTED:
6302                         /* Keep frame for reassembly later */
6303                         l2cap_pass_to_tx(chan, control);
6304                         skb_queue_tail(&chan->srej_q, skb);
6305                         skb_in_use = true;
6306                         BT_DBG("Queued %p (queue len %d)", skb,
6307                                skb_queue_len(&chan->srej_q));
6308
6309                         chan->expected_tx_seq = __next_seq(chan, txseq);
6310                         break;
6311                 case L2CAP_TXSEQ_EXPECTED_SREJ:
6312                         l2cap_seq_list_pop(&chan->srej_list);
6313
6314                         l2cap_pass_to_tx(chan, control);
6315                         skb_queue_tail(&chan->srej_q, skb);
6316                         skb_in_use = true;
6317                         BT_DBG("Queued %p (queue len %d)", skb,
6318                                skb_queue_len(&chan->srej_q));
6319
6320                         err = l2cap_rx_queued_iframes(chan);
6321                         if (err)
6322                                 break;
6323
6324                         break;
6325                 case L2CAP_TXSEQ_UNEXPECTED:
6326                         /* Got a frame that can't be reassembled yet.
6327                          * Save it for later, and send SREJs to cover
6328                          * the missing frames.
6329                          */
6330                         skb_queue_tail(&chan->srej_q, skb);
6331                         skb_in_use = true;
6332                         BT_DBG("Queued %p (queue len %d)", skb,
6333                                skb_queue_len(&chan->srej_q));
6334
6335                         l2cap_pass_to_tx(chan, control);
6336                         l2cap_send_srej(chan, control->txseq);
6337                         break;
6338                 case L2CAP_TXSEQ_UNEXPECTED_SREJ:
6339                         /* This frame was requested with an SREJ, but
6340                          * some expected retransmitted frames are
6341                          * missing.  Request retransmission of missing
6342                          * SREJ'd frames.
6343                          */
6344                         skb_queue_tail(&chan->srej_q, skb);
6345                         skb_in_use = true;
6346                         BT_DBG("Queued %p (queue len %d)", skb,
6347                                skb_queue_len(&chan->srej_q));
6348
6349                         l2cap_pass_to_tx(chan, control);
6350                         l2cap_send_srej_list(chan, control->txseq);
6351                         break;
6352                 case L2CAP_TXSEQ_DUPLICATE_SREJ:
6353                         /* We've already queued this frame.  Drop this copy. */
6354                         l2cap_pass_to_tx(chan, control);
6355                         break;
6356                 case L2CAP_TXSEQ_DUPLICATE:
6357                         /* Expecting a later sequence number, so this frame
6358                          * was already received.  Ignore it completely.
6359                          */
6360                         break;
6361                 case L2CAP_TXSEQ_INVALID_IGNORE:
6362                         break;
6363                 case L2CAP_TXSEQ_INVALID:
6364                 default:
6365                         l2cap_send_disconn_req(chan, ECONNRESET);
6366                         break;
6367                 }
6368                 break;
6369         case L2CAP_EV_RECV_RR:
6370                 l2cap_pass_to_tx(chan, control);
6371                 if (control->final) {
6372                         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6373
6374                         if (!test_and_clear_bit(CONN_REJ_ACT,
6375                                                 &chan->conn_state)) {
6376                                 control->final = 0;
6377                                 l2cap_retransmit_all(chan, control);
6378                         }
6379
6380                         l2cap_ertm_send(chan);
6381                 } else if (control->poll) {
6382                         if (test_and_clear_bit(CONN_REMOTE_BUSY,
6383                                                &chan->conn_state) &&
6384                             chan->unacked_frames) {
6385                                 __set_retrans_timer(chan);
6386                         }
6387
6388                         set_bit(CONN_SEND_FBIT, &chan->conn_state);
6389                         l2cap_send_srej_tail(chan);
6390                 } else {
6391                         if (test_and_clear_bit(CONN_REMOTE_BUSY,
6392                                                &chan->conn_state) &&
6393                             chan->unacked_frames)
6394                                 __set_retrans_timer(chan);
6395
6396                         l2cap_send_ack(chan);
6397                 }
6398                 break;
6399         case L2CAP_EV_RECV_RNR:
6400                 set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6401                 l2cap_pass_to_tx(chan, control);
6402                 if (control->poll) {
6403                         l2cap_send_srej_tail(chan);
6404                 } else {
6405                         struct l2cap_ctrl rr_control;
6406                         memset(&rr_control, 0, sizeof(rr_control));
6407                         rr_control.sframe = 1;
6408                         rr_control.super = L2CAP_SUPER_RR;
6409                         rr_control.reqseq = chan->buffer_seq;
6410                         l2cap_send_sframe(chan, &rr_control);
6411                 }
6412
6413                 break;
6414         case L2CAP_EV_RECV_REJ:
6415                 l2cap_handle_rej(chan, control);
6416                 break;
6417         case L2CAP_EV_RECV_SREJ:
6418                 l2cap_handle_srej(chan, control);
6419                 break;
6420         }
6421
6422         if (skb && !skb_in_use) {
6423                 BT_DBG("Freeing %p", skb);
6424                 kfree_skb(skb);
6425         }
6426
6427         return err;
6428 }
6429
6430 static int l2cap_finish_move(struct l2cap_chan *chan)
6431 {
6432         BT_DBG("chan %p", chan);
6433
6434         chan->rx_state = L2CAP_RX_STATE_RECV;
6435
6436         if (chan->hs_hcon)
6437                 chan->conn->mtu = chan->hs_hcon->hdev->block_mtu;
6438         else
6439                 chan->conn->mtu = chan->conn->hcon->hdev->acl_mtu;
6440
6441         return l2cap_resegment(chan);
6442 }
6443
6444 static int l2cap_rx_state_wait_p(struct l2cap_chan *chan,
6445                                  struct l2cap_ctrl *control,
6446                                  struct sk_buff *skb, u8 event)
6447 {
6448         int err;
6449
6450         BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
6451                event);
6452
6453         if (!control->poll)
6454                 return -EPROTO;
6455
6456         l2cap_process_reqseq(chan, control->reqseq);
6457
6458         if (!skb_queue_empty(&chan->tx_q))
6459                 chan->tx_send_head = skb_peek(&chan->tx_q);
6460         else
6461                 chan->tx_send_head = NULL;
6462
6463         /* Rewind next_tx_seq to the point expected
6464          * by the receiver.
6465          */
6466         chan->next_tx_seq = control->reqseq;
6467         chan->unacked_frames = 0;
6468
6469         err = l2cap_finish_move(chan);
6470         if (err)
6471                 return err;
6472
6473         set_bit(CONN_SEND_FBIT, &chan->conn_state);
6474         l2cap_send_i_or_rr_or_rnr(chan);
6475
6476         if (event == L2CAP_EV_RECV_IFRAME)
6477                 return -EPROTO;
6478
6479         return l2cap_rx_state_recv(chan, control, NULL, event);
6480 }
6481
6482 static int l2cap_rx_state_wait_f(struct l2cap_chan *chan,
6483                                  struct l2cap_ctrl *control,
6484                                  struct sk_buff *skb, u8 event)
6485 {
6486         int err;
6487
6488         if (!control->final)
6489                 return -EPROTO;
6490
6491         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6492
6493         chan->rx_state = L2CAP_RX_STATE_RECV;
6494         l2cap_process_reqseq(chan, control->reqseq);
6495
6496         if (!skb_queue_empty(&chan->tx_q))
6497                 chan->tx_send_head = skb_peek(&chan->tx_q);
6498         else
6499                 chan->tx_send_head = NULL;
6500
6501         /* Rewind next_tx_seq to the point expected
6502          * by the receiver.
6503          */
6504         chan->next_tx_seq = control->reqseq;
6505         chan->unacked_frames = 0;
6506
6507         if (chan->hs_hcon)
6508                 chan->conn->mtu = chan->hs_hcon->hdev->block_mtu;
6509         else
6510                 chan->conn->mtu = chan->conn->hcon->hdev->acl_mtu;
6511
6512         err = l2cap_resegment(chan);
6513
6514         if (!err)
6515                 err = l2cap_rx_state_recv(chan, control, skb, event);
6516
6517         return err;
6518 }
6519
6520 static bool __valid_reqseq(struct l2cap_chan *chan, u16 reqseq)
6521 {
6522         /* Make sure reqseq is for a packet that has been sent but not acked */
6523         u16 unacked;
6524
6525         unacked = __seq_offset(chan, chan->next_tx_seq, chan->expected_ack_seq);
6526         return __seq_offset(chan, chan->next_tx_seq, reqseq) <= unacked;
6527 }
6528
6529 static int l2cap_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
6530                     struct sk_buff *skb, u8 event)
6531 {
6532         int err = 0;
6533
6534         BT_DBG("chan %p, control %p, skb %p, event %d, state %d", chan,
6535                control, skb, event, chan->rx_state);
6536
6537         if (__valid_reqseq(chan, control->reqseq)) {
6538                 switch (chan->rx_state) {
6539                 case L2CAP_RX_STATE_RECV:
6540                         err = l2cap_rx_state_recv(chan, control, skb, event);
6541                         break;
6542                 case L2CAP_RX_STATE_SREJ_SENT:
6543                         err = l2cap_rx_state_srej_sent(chan, control, skb,
6544                                                        event);
6545                         break;
6546                 case L2CAP_RX_STATE_WAIT_P:
6547                         err = l2cap_rx_state_wait_p(chan, control, skb, event);
6548                         break;
6549                 case L2CAP_RX_STATE_WAIT_F:
6550                         err = l2cap_rx_state_wait_f(chan, control, skb, event);
6551                         break;
6552                 default:
6553                         /* shut it down */
6554                         break;
6555                 }
6556         } else {
6557                 BT_DBG("Invalid reqseq %d (next_tx_seq %d, expected_ack_seq %d",
6558                        control->reqseq, chan->next_tx_seq,
6559                        chan->expected_ack_seq);
6560                 l2cap_send_disconn_req(chan, ECONNRESET);
6561         }
6562
6563         return err;
6564 }
6565
6566 static int l2cap_stream_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
6567                            struct sk_buff *skb)
6568 {
6569         BT_DBG("chan %p, control %p, skb %p, state %d", chan, control, skb,
6570                chan->rx_state);
6571
6572         if (l2cap_classify_txseq(chan, control->txseq) ==
6573             L2CAP_TXSEQ_EXPECTED) {
6574                 l2cap_pass_to_tx(chan, control);
6575
6576                 BT_DBG("buffer_seq %d->%d", chan->buffer_seq,
6577                        __next_seq(chan, chan->buffer_seq));
6578
6579                 chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
6580
6581                 l2cap_reassemble_sdu(chan, skb, control);
6582         } else {
6583                 if (chan->sdu) {
6584                         kfree_skb(chan->sdu);
6585                         chan->sdu = NULL;
6586                 }
6587                 chan->sdu_last_frag = NULL;
6588                 chan->sdu_len = 0;
6589
6590                 if (skb) {
6591                         BT_DBG("Freeing %p", skb);
6592                         kfree_skb(skb);
6593                 }
6594         }
6595
6596         chan->last_acked_seq = control->txseq;
6597         chan->expected_tx_seq = __next_seq(chan, control->txseq);
6598
6599         return 0;
6600 }
6601
6602 static int l2cap_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb)
6603 {
6604         struct l2cap_ctrl *control = &bt_cb(skb)->l2cap;
6605         u16 len;
6606         u8 event;
6607
6608         __unpack_control(chan, skb);
6609
6610         len = skb->len;
6611
6612         /*
6613          * We can just drop the corrupted I-frame here.
6614          * Receiver will miss it and start proper recovery
6615          * procedures and ask for retransmission.
6616          */
6617         if (l2cap_check_fcs(chan, skb))
6618                 goto drop;
6619
6620         if (!control->sframe && control->sar == L2CAP_SAR_START)
6621                 len -= L2CAP_SDULEN_SIZE;
6622
6623         if (chan->fcs == L2CAP_FCS_CRC16)
6624                 len -= L2CAP_FCS_SIZE;
6625
6626         if (len > chan->mps) {
6627                 l2cap_send_disconn_req(chan, ECONNRESET);
6628                 goto drop;
6629         }
6630
6631         if ((chan->mode == L2CAP_MODE_ERTM ||
6632              chan->mode == L2CAP_MODE_STREAMING) && sk_filter(chan->data, skb))
6633                 goto drop;
6634
6635         if (!control->sframe) {
6636                 int err;
6637
6638                 BT_DBG("iframe sar %d, reqseq %d, final %d, txseq %d",
6639                        control->sar, control->reqseq, control->final,
6640                        control->txseq);
6641
6642                 /* Validate F-bit - F=0 always valid, F=1 only
6643                  * valid in TX WAIT_F
6644                  */
6645                 if (control->final && chan->tx_state != L2CAP_TX_STATE_WAIT_F)
6646                         goto drop;
6647
6648                 if (chan->mode != L2CAP_MODE_STREAMING) {
6649                         event = L2CAP_EV_RECV_IFRAME;
6650                         err = l2cap_rx(chan, control, skb, event);
6651                 } else {
6652                         err = l2cap_stream_rx(chan, control, skb);
6653                 }
6654
6655                 if (err)
6656                         l2cap_send_disconn_req(chan, ECONNRESET);
6657         } else {
6658                 const u8 rx_func_to_event[4] = {
6659                         L2CAP_EV_RECV_RR, L2CAP_EV_RECV_REJ,
6660                         L2CAP_EV_RECV_RNR, L2CAP_EV_RECV_SREJ
6661                 };
6662
6663                 /* Only I-frames are expected in streaming mode */
6664                 if (chan->mode == L2CAP_MODE_STREAMING)
6665                         goto drop;
6666
6667                 BT_DBG("sframe reqseq %d, final %d, poll %d, super %d",
6668                        control->reqseq, control->final, control->poll,
6669                        control->super);
6670
6671                 if (len != 0) {
6672                         BT_ERR("Trailing bytes: %d in sframe", len);
6673                         l2cap_send_disconn_req(chan, ECONNRESET);
6674                         goto drop;
6675                 }
6676
6677                 /* Validate F and P bits */
6678                 if (control->final && (control->poll ||
6679                                        chan->tx_state != L2CAP_TX_STATE_WAIT_F))
6680                         goto drop;
6681
6682                 event = rx_func_to_event[control->super];
6683                 if (l2cap_rx(chan, control, skb, event))
6684                         l2cap_send_disconn_req(chan, ECONNRESET);
6685         }
6686
6687         return 0;
6688
6689 drop:
6690         kfree_skb(skb);
6691         return 0;
6692 }
6693
6694 static void l2cap_chan_le_send_credits(struct l2cap_chan *chan)
6695 {
6696         struct l2cap_conn *conn = chan->conn;
6697         struct l2cap_le_credits pkt;
6698         u16 return_credits;
6699
6700         /* We return more credits to the sender only after the amount of
6701          * credits falls below half of the initial amount.
6702          */
6703         if (chan->rx_credits >= (le_max_credits + 1) / 2)
6704                 return;
6705
6706         return_credits = le_max_credits - chan->rx_credits;
6707
6708         BT_DBG("chan %p returning %u credits to sender", chan, return_credits);
6709
6710         chan->rx_credits += return_credits;
6711
6712         pkt.cid     = cpu_to_le16(chan->scid);
6713         pkt.credits = cpu_to_le16(return_credits);
6714
6715         chan->ident = l2cap_get_ident(conn);
6716
6717         l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CREDITS, sizeof(pkt), &pkt);
6718 }
6719
6720 static int l2cap_le_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb)
6721 {
6722         int err;
6723
6724         if (!chan->rx_credits) {
6725                 BT_ERR("No credits to receive LE L2CAP data");
6726                 l2cap_send_disconn_req(chan, ECONNRESET);
6727                 return -ENOBUFS;
6728         }
6729
6730         if (chan->imtu < skb->len) {
6731                 BT_ERR("Too big LE L2CAP PDU");
6732                 return -ENOBUFS;
6733         }
6734
6735         chan->rx_credits--;
6736         BT_DBG("rx_credits %u -> %u", chan->rx_credits + 1, chan->rx_credits);
6737
6738         l2cap_chan_le_send_credits(chan);
6739
6740         err = 0;
6741
6742         if (!chan->sdu) {
6743                 u16 sdu_len;
6744
6745                 sdu_len = get_unaligned_le16(skb->data);
6746                 skb_pull(skb, L2CAP_SDULEN_SIZE);
6747
6748                 BT_DBG("Start of new SDU. sdu_len %u skb->len %u imtu %u",
6749                        sdu_len, skb->len, chan->imtu);
6750
6751                 if (sdu_len > chan->imtu) {
6752                         BT_ERR("Too big LE L2CAP SDU length received");
6753                         err = -EMSGSIZE;
6754                         goto failed;
6755                 }
6756
6757                 if (skb->len > sdu_len) {
6758                         BT_ERR("Too much LE L2CAP data received");
6759                         err = -EINVAL;
6760                         goto failed;
6761                 }
6762
6763                 if (skb->len == sdu_len)
6764                         return chan->ops->recv(chan, skb);
6765
6766                 chan->sdu = skb;
6767                 chan->sdu_len = sdu_len;
6768                 chan->sdu_last_frag = skb;
6769
6770                 return 0;
6771         }
6772
6773         BT_DBG("SDU fragment. chan->sdu->len %u skb->len %u chan->sdu_len %u",
6774                chan->sdu->len, skb->len, chan->sdu_len);
6775
6776         if (chan->sdu->len + skb->len > chan->sdu_len) {
6777                 BT_ERR("Too much LE L2CAP data received");
6778                 err = -EINVAL;
6779                 goto failed;
6780         }
6781
6782         append_skb_frag(chan->sdu, skb, &chan->sdu_last_frag);
6783         skb = NULL;
6784
6785         if (chan->sdu->len == chan->sdu_len) {
6786                 err = chan->ops->recv(chan, chan->sdu);
6787                 if (!err) {
6788                         chan->sdu = NULL;
6789                         chan->sdu_last_frag = NULL;
6790                         chan->sdu_len = 0;
6791                 }
6792         }
6793
6794 failed:
6795         if (err) {
6796                 kfree_skb(skb);
6797                 kfree_skb(chan->sdu);
6798                 chan->sdu = NULL;
6799                 chan->sdu_last_frag = NULL;
6800                 chan->sdu_len = 0;
6801         }
6802
6803         /* We can't return an error here since we took care of the skb
6804          * freeing internally. An error return would cause the caller to
6805          * do a double-free of the skb.
6806          */
6807         return 0;
6808 }
6809
6810 static void l2cap_data_channel(struct l2cap_conn *conn, u16 cid,
6811                                struct sk_buff *skb)
6812 {
6813         struct l2cap_chan *chan;
6814
6815         chan = l2cap_get_chan_by_scid(conn, cid);
6816         if (!chan) {
6817                 if (cid == L2CAP_CID_A2MP) {
6818                         chan = a2mp_channel_create(conn, skb);
6819                         if (!chan) {
6820                                 kfree_skb(skb);
6821                                 return;
6822                         }
6823
6824                         l2cap_chan_lock(chan);
6825                 } else {
6826                         BT_DBG("unknown cid 0x%4.4x", cid);
6827                         /* Drop packet and return */
6828                         kfree_skb(skb);
6829                         return;
6830                 }
6831         }
6832
6833         BT_DBG("chan %p, len %d", chan, skb->len);
6834
6835         /* If we receive data on a fixed channel before the info req/rsp
6836          * procdure is done simply assume that the channel is supported
6837          * and mark it as ready.
6838          */
6839         if (chan->chan_type == L2CAP_CHAN_FIXED)
6840                 l2cap_chan_ready(chan);
6841
6842         if (chan->state != BT_CONNECTED)
6843                 goto drop;
6844
6845         switch (chan->mode) {
6846         case L2CAP_MODE_LE_FLOWCTL:
6847                 if (l2cap_le_data_rcv(chan, skb) < 0)
6848                         goto drop;
6849
6850                 goto done;
6851
6852         case L2CAP_MODE_BASIC:
6853                 /* If socket recv buffers overflows we drop data here
6854                  * which is *bad* because L2CAP has to be reliable.
6855                  * But we don't have any other choice. L2CAP doesn't
6856                  * provide flow control mechanism. */
6857
6858                 if (chan->imtu < skb->len) {
6859                         BT_ERR("Dropping L2CAP data: receive buffer overflow");
6860                         goto drop;
6861                 }
6862
6863                 if (!chan->ops->recv(chan, skb))
6864                         goto done;
6865                 break;
6866
6867         case L2CAP_MODE_ERTM:
6868         case L2CAP_MODE_STREAMING:
6869                 l2cap_data_rcv(chan, skb);
6870                 goto done;
6871
6872         default:
6873                 BT_DBG("chan %p: bad mode 0x%2.2x", chan, chan->mode);
6874                 break;
6875         }
6876
6877 drop:
6878         kfree_skb(skb);
6879
6880 done:
6881         l2cap_chan_unlock(chan);
6882 }
6883
6884 static void l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm,
6885                                   struct sk_buff *skb)
6886 {
6887         struct hci_conn *hcon = conn->hcon;
6888         struct l2cap_chan *chan;
6889
6890         if (hcon->type != ACL_LINK)
6891                 goto free_skb;
6892
6893         chan = l2cap_global_chan_by_psm(0, psm, &hcon->src, &hcon->dst,
6894                                         ACL_LINK);
6895         if (!chan)
6896                 goto free_skb;
6897
6898         BT_DBG("chan %p, len %d", chan, skb->len);
6899
6900         if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
6901                 goto drop;
6902
6903         if (chan->imtu < skb->len)
6904                 goto drop;
6905
6906         /* Store remote BD_ADDR and PSM for msg_name */
6907         bacpy(&bt_cb(skb)->l2cap.bdaddr, &hcon->dst);
6908         bt_cb(skb)->l2cap.psm = psm;
6909
6910         if (!chan->ops->recv(chan, skb)) {
6911                 l2cap_chan_put(chan);
6912                 return;
6913         }
6914
6915 drop:
6916         l2cap_chan_put(chan);
6917 free_skb:
6918         kfree_skb(skb);
6919 }
6920
6921 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
6922 {
6923         struct l2cap_hdr *lh = (void *) skb->data;
6924         struct hci_conn *hcon = conn->hcon;
6925         u16 cid, len;
6926         __le16 psm;
6927
6928         if (hcon->state != BT_CONNECTED) {
6929                 BT_DBG("queueing pending rx skb");
6930                 skb_queue_tail(&conn->pending_rx, skb);
6931                 return;
6932         }
6933
6934         skb_pull(skb, L2CAP_HDR_SIZE);
6935         cid = __le16_to_cpu(lh->cid);
6936         len = __le16_to_cpu(lh->len);
6937
6938         if (len != skb->len) {
6939                 kfree_skb(skb);
6940                 return;
6941         }
6942
6943         /* Since we can't actively block incoming LE connections we must
6944          * at least ensure that we ignore incoming data from them.
6945          */
6946         if (hcon->type == LE_LINK &&
6947             hci_bdaddr_list_lookup(&hcon->hdev->blacklist, &hcon->dst,
6948                                    bdaddr_dst_type(hcon))) {
6949                 kfree_skb(skb);
6950                 return;
6951         }
6952
6953         BT_DBG("len %d, cid 0x%4.4x", len, cid);
6954
6955         switch (cid) {
6956         case L2CAP_CID_SIGNALING:
6957                 l2cap_sig_channel(conn, skb);
6958                 break;
6959
6960         case L2CAP_CID_CONN_LESS:
6961                 psm = get_unaligned((__le16 *) skb->data);
6962                 skb_pull(skb, L2CAP_PSMLEN_SIZE);
6963                 l2cap_conless_channel(conn, psm, skb);
6964                 break;
6965
6966         case L2CAP_CID_LE_SIGNALING:
6967                 l2cap_le_sig_channel(conn, skb);
6968                 break;
6969
6970         default:
6971                 l2cap_data_channel(conn, cid, skb);
6972                 break;
6973         }
6974 }
6975
6976 static void process_pending_rx(struct work_struct *work)
6977 {
6978         struct l2cap_conn *conn = container_of(work, struct l2cap_conn,
6979                                                pending_rx_work);
6980         struct sk_buff *skb;
6981
6982         BT_DBG("");
6983
6984         while ((skb = skb_dequeue(&conn->pending_rx)))
6985                 l2cap_recv_frame(conn, skb);
6986 }
6987
6988 static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon)
6989 {
6990         struct l2cap_conn *conn = hcon->l2cap_data;
6991         struct hci_chan *hchan;
6992
6993         if (conn)
6994                 return conn;
6995
6996         hchan = hci_chan_create(hcon);
6997         if (!hchan)
6998                 return NULL;
6999
7000         conn = kzalloc(sizeof(*conn), GFP_KERNEL);
7001         if (!conn) {
7002                 hci_chan_del(hchan);
7003                 return NULL;
7004         }
7005
7006         kref_init(&conn->ref);
7007         hcon->l2cap_data = conn;
7008         conn->hcon = hci_conn_get(hcon);
7009         conn->hchan = hchan;
7010
7011         BT_DBG("hcon %p conn %p hchan %p", hcon, conn, hchan);
7012
7013         switch (hcon->type) {
7014         case LE_LINK:
7015                 if (hcon->hdev->le_mtu) {
7016                         conn->mtu = hcon->hdev->le_mtu;
7017                         break;
7018                 }
7019                 /* fall through */
7020         default:
7021                 conn->mtu = hcon->hdev->acl_mtu;
7022                 break;
7023         }
7024
7025         conn->feat_mask = 0;
7026
7027         conn->local_fixed_chan = L2CAP_FC_SIG_BREDR | L2CAP_FC_CONNLESS;
7028
7029         if (hcon->type == ACL_LINK &&
7030             hci_dev_test_flag(hcon->hdev, HCI_HS_ENABLED))
7031                 conn->local_fixed_chan |= L2CAP_FC_A2MP;
7032
7033         if (hci_dev_test_flag(hcon->hdev, HCI_LE_ENABLED) &&
7034             (bredr_sc_enabled(hcon->hdev) ||
7035              hci_dev_test_flag(hcon->hdev, HCI_FORCE_BREDR_SMP)))
7036                 conn->local_fixed_chan |= L2CAP_FC_SMP_BREDR;
7037
7038         mutex_init(&conn->ident_lock);
7039         mutex_init(&conn->chan_lock);
7040
7041         INIT_LIST_HEAD(&conn->chan_l);
7042         INIT_LIST_HEAD(&conn->users);
7043
7044         INIT_DELAYED_WORK(&conn->info_timer, l2cap_info_timeout);
7045
7046         skb_queue_head_init(&conn->pending_rx);
7047         INIT_WORK(&conn->pending_rx_work, process_pending_rx);
7048         INIT_WORK(&conn->id_addr_update_work, l2cap_conn_update_id_addr);
7049
7050         conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM;
7051
7052         return conn;
7053 }
7054
7055 static bool is_valid_psm(u16 psm, u8 dst_type) {
7056         if (!psm)
7057                 return false;
7058
7059         if (bdaddr_type_is_le(dst_type))
7060                 return (psm <= 0x00ff);
7061
7062         /* PSM must be odd and lsb of upper byte must be 0 */
7063         return ((psm & 0x0101) == 0x0001);
7064 }
7065
7066 int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid,
7067                        bdaddr_t *dst, u8 dst_type)
7068 {
7069         struct l2cap_conn *conn;
7070         struct hci_conn *hcon;
7071         struct hci_dev *hdev;
7072         int err;
7073
7074         BT_DBG("%pMR -> %pMR (type %u) psm 0x%2.2x", &chan->src, dst,
7075                dst_type, __le16_to_cpu(psm));
7076
7077         hdev = hci_get_route(dst, &chan->src, chan->src_type);
7078         if (!hdev)
7079                 return -EHOSTUNREACH;
7080
7081         hci_dev_lock(hdev);
7082
7083         if (!is_valid_psm(__le16_to_cpu(psm), dst_type) && !cid &&
7084             chan->chan_type != L2CAP_CHAN_RAW) {
7085                 err = -EINVAL;
7086                 goto done;
7087         }
7088
7089         if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED && !psm) {
7090                 err = -EINVAL;
7091                 goto done;
7092         }
7093
7094         if (chan->chan_type == L2CAP_CHAN_FIXED && !cid) {
7095                 err = -EINVAL;
7096                 goto done;
7097         }
7098
7099         switch (chan->mode) {
7100         case L2CAP_MODE_BASIC:
7101                 break;
7102         case L2CAP_MODE_LE_FLOWCTL:
7103                 l2cap_le_flowctl_init(chan);
7104                 break;
7105         case L2CAP_MODE_ERTM:
7106         case L2CAP_MODE_STREAMING:
7107                 if (!disable_ertm)
7108                         break;
7109                 /* fall through */
7110         default:
7111                 err = -EOPNOTSUPP;
7112                 goto done;
7113         }
7114
7115         switch (chan->state) {
7116         case BT_CONNECT:
7117         case BT_CONNECT2:
7118         case BT_CONFIG:
7119                 /* Already connecting */
7120                 err = 0;
7121                 goto done;
7122
7123         case BT_CONNECTED:
7124                 /* Already connected */
7125                 err = -EISCONN;
7126                 goto done;
7127
7128         case BT_OPEN:
7129         case BT_BOUND:
7130                 /* Can connect */
7131                 break;
7132
7133         default:
7134                 err = -EBADFD;
7135                 goto done;
7136         }
7137
7138         /* Set destination address and psm */
7139         bacpy(&chan->dst, dst);
7140         chan->dst_type = dst_type;
7141
7142         chan->psm = psm;
7143         chan->dcid = cid;
7144
7145         if (bdaddr_type_is_le(dst_type)) {
7146                 /* Convert from L2CAP channel address type to HCI address type
7147                  */
7148                 if (dst_type == BDADDR_LE_PUBLIC)
7149                         dst_type = ADDR_LE_DEV_PUBLIC;
7150                 else
7151                         dst_type = ADDR_LE_DEV_RANDOM;
7152
7153                 if (hci_dev_test_flag(hdev, HCI_ADVERTISING))
7154                         hcon = hci_connect_le(hdev, dst, dst_type,
7155                                               chan->sec_level,
7156                                               HCI_LE_CONN_TIMEOUT,
7157                                               HCI_ROLE_SLAVE);
7158                 else
7159                         hcon = hci_connect_le_scan(hdev, dst, dst_type,
7160                                                    chan->sec_level,
7161                                                    HCI_LE_CONN_TIMEOUT);
7162
7163         } else {
7164                 u8 auth_type = l2cap_get_auth_type(chan);
7165                 hcon = hci_connect_acl(hdev, dst, chan->sec_level, auth_type);
7166         }
7167
7168         if (IS_ERR(hcon)) {
7169                 err = PTR_ERR(hcon);
7170                 goto done;
7171         }
7172
7173         conn = l2cap_conn_add(hcon);
7174         if (!conn) {
7175                 hci_conn_drop(hcon);
7176                 err = -ENOMEM;
7177                 goto done;
7178         }
7179
7180         mutex_lock(&conn->chan_lock);
7181         l2cap_chan_lock(chan);
7182
7183         if (cid && __l2cap_get_chan_by_dcid(conn, cid)) {
7184                 hci_conn_drop(hcon);
7185                 err = -EBUSY;
7186                 goto chan_unlock;
7187         }
7188
7189         /* Update source addr of the socket */
7190         bacpy(&chan->src, &hcon->src);
7191         chan->src_type = bdaddr_src_type(hcon);
7192
7193         __l2cap_chan_add(conn, chan);
7194
7195         /* l2cap_chan_add takes its own ref so we can drop this one */
7196         hci_conn_drop(hcon);
7197
7198         l2cap_state_change(chan, BT_CONNECT);
7199         __set_chan_timer(chan, chan->ops->get_sndtimeo(chan));
7200
7201         /* Release chan->sport so that it can be reused by other
7202          * sockets (as it's only used for listening sockets).
7203          */
7204         write_lock(&chan_list_lock);
7205         chan->sport = 0;
7206         write_unlock(&chan_list_lock);
7207
7208         if (hcon->state == BT_CONNECTED) {
7209                 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
7210                         __clear_chan_timer(chan);
7211                         if (l2cap_chan_check_security(chan, true))
7212                                 l2cap_state_change(chan, BT_CONNECTED);
7213                 } else
7214                         l2cap_do_start(chan);
7215         }
7216
7217         err = 0;
7218
7219 chan_unlock:
7220         l2cap_chan_unlock(chan);
7221         mutex_unlock(&conn->chan_lock);
7222 done:
7223         hci_dev_unlock(hdev);
7224         hci_dev_put(hdev);
7225         return err;
7226 }
7227 EXPORT_SYMBOL_GPL(l2cap_chan_connect);
7228
7229 /* ---- L2CAP interface with lower layer (HCI) ---- */
7230
7231 int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr)
7232 {
7233         int exact = 0, lm1 = 0, lm2 = 0;
7234         struct l2cap_chan *c;
7235
7236         BT_DBG("hdev %s, bdaddr %pMR", hdev->name, bdaddr);
7237
7238         /* Find listening sockets and check their link_mode */
7239         read_lock(&chan_list_lock);
7240         list_for_each_entry(c, &chan_list, global_l) {
7241                 if (c->state != BT_LISTEN)
7242                         continue;
7243
7244                 if (!bacmp(&c->src, &hdev->bdaddr)) {
7245                         lm1 |= HCI_LM_ACCEPT;
7246                         if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
7247                                 lm1 |= HCI_LM_MASTER;
7248                         exact++;
7249                 } else if (!bacmp(&c->src, BDADDR_ANY)) {
7250                         lm2 |= HCI_LM_ACCEPT;
7251                         if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
7252                                 lm2 |= HCI_LM_MASTER;
7253                 }
7254         }
7255         read_unlock(&chan_list_lock);
7256
7257         return exact ? lm1 : lm2;
7258 }
7259
7260 /* Find the next fixed channel in BT_LISTEN state, continue iteration
7261  * from an existing channel in the list or from the beginning of the
7262  * global list (by passing NULL as first parameter).
7263  */
7264 static struct l2cap_chan *l2cap_global_fixed_chan(struct l2cap_chan *c,
7265                                                   struct hci_conn *hcon)
7266 {
7267         u8 src_type = bdaddr_src_type(hcon);
7268
7269         read_lock(&chan_list_lock);
7270
7271         if (c)
7272                 c = list_next_entry(c, global_l);
7273         else
7274                 c = list_entry(chan_list.next, typeof(*c), global_l);
7275
7276         list_for_each_entry_from(c, &chan_list, global_l) {
7277                 if (c->chan_type != L2CAP_CHAN_FIXED)
7278                         continue;
7279                 if (c->state != BT_LISTEN)
7280                         continue;
7281                 if (bacmp(&c->src, &hcon->src) && bacmp(&c->src, BDADDR_ANY))
7282                         continue;
7283                 if (src_type != c->src_type)
7284                         continue;
7285
7286                 l2cap_chan_hold(c);
7287                 read_unlock(&chan_list_lock);
7288                 return c;
7289         }
7290
7291         read_unlock(&chan_list_lock);
7292
7293         return NULL;
7294 }
7295
7296 static void l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
7297 {
7298         struct hci_dev *hdev = hcon->hdev;
7299         struct l2cap_conn *conn;
7300         struct l2cap_chan *pchan;
7301         u8 dst_type;
7302
7303         if (hcon->type != ACL_LINK && hcon->type != LE_LINK)
7304                 return;
7305
7306         BT_DBG("hcon %p bdaddr %pMR status %d", hcon, &hcon->dst, status);
7307
7308         if (status) {
7309                 l2cap_conn_del(hcon, bt_to_errno(status));
7310                 return;
7311         }
7312
7313         conn = l2cap_conn_add(hcon);
7314         if (!conn)
7315                 return;
7316
7317         dst_type = bdaddr_dst_type(hcon);
7318
7319         /* If device is blocked, do not create channels for it */
7320         if (hci_bdaddr_list_lookup(&hdev->blacklist, &hcon->dst, dst_type))
7321                 return;
7322
7323         /* Find fixed channels and notify them of the new connection. We
7324          * use multiple individual lookups, continuing each time where
7325          * we left off, because the list lock would prevent calling the
7326          * potentially sleeping l2cap_chan_lock() function.
7327          */
7328         pchan = l2cap_global_fixed_chan(NULL, hcon);
7329         while (pchan) {
7330                 struct l2cap_chan *chan, *next;
7331
7332                 /* Client fixed channels should override server ones */
7333                 if (__l2cap_get_chan_by_dcid(conn, pchan->scid))
7334                         goto next;
7335
7336                 l2cap_chan_lock(pchan);
7337                 chan = pchan->ops->new_connection(pchan);
7338                 if (chan) {
7339                         bacpy(&chan->src, &hcon->src);
7340                         bacpy(&chan->dst, &hcon->dst);
7341                         chan->src_type = bdaddr_src_type(hcon);
7342                         chan->dst_type = dst_type;
7343
7344                         __l2cap_chan_add(conn, chan);
7345                 }
7346
7347                 l2cap_chan_unlock(pchan);
7348 next:
7349                 next = l2cap_global_fixed_chan(pchan, hcon);
7350                 l2cap_chan_put(pchan);
7351                 pchan = next;
7352         }
7353
7354         l2cap_conn_ready(conn);
7355 }
7356
7357 int l2cap_disconn_ind(struct hci_conn *hcon)
7358 {
7359         struct l2cap_conn *conn = hcon->l2cap_data;
7360
7361         BT_DBG("hcon %p", hcon);
7362
7363         if (!conn)
7364                 return HCI_ERROR_REMOTE_USER_TERM;
7365         return conn->disc_reason;
7366 }
7367
7368 static void l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
7369 {
7370         if (hcon->type != ACL_LINK && hcon->type != LE_LINK)
7371                 return;
7372
7373         BT_DBG("hcon %p reason %d", hcon, reason);
7374
7375         l2cap_conn_del(hcon, bt_to_errno(reason));
7376 }
7377
7378 static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt)
7379 {
7380         if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED)
7381                 return;
7382
7383         if (encrypt == 0x00) {
7384                 if (chan->sec_level == BT_SECURITY_MEDIUM) {
7385                         __set_chan_timer(chan, L2CAP_ENC_TIMEOUT);
7386                 } else if (chan->sec_level == BT_SECURITY_HIGH ||
7387                            chan->sec_level == BT_SECURITY_FIPS)
7388                         l2cap_chan_close(chan, ECONNREFUSED);
7389         } else {
7390                 if (chan->sec_level == BT_SECURITY_MEDIUM)
7391                         __clear_chan_timer(chan);
7392         }
7393 }
7394
7395 static void l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
7396 {
7397         struct l2cap_conn *conn = hcon->l2cap_data;
7398         struct l2cap_chan *chan;
7399
7400         if (!conn)
7401                 return;
7402
7403         BT_DBG("conn %p status 0x%2.2x encrypt %u", conn, status, encrypt);
7404
7405         mutex_lock(&conn->chan_lock);
7406
7407         list_for_each_entry(chan, &conn->chan_l, list) {
7408                 l2cap_chan_lock(chan);
7409
7410                 BT_DBG("chan %p scid 0x%4.4x state %s", chan, chan->scid,
7411                        state_to_string(chan->state));
7412
7413                 if (chan->scid == L2CAP_CID_A2MP) {
7414                         l2cap_chan_unlock(chan);
7415                         continue;
7416                 }
7417
7418                 if (!status && encrypt)
7419                         chan->sec_level = hcon->sec_level;
7420
7421                 if (!__l2cap_no_conn_pending(chan)) {
7422                         l2cap_chan_unlock(chan);
7423                         continue;
7424                 }
7425
7426                 if (!status && (chan->state == BT_CONNECTED ||
7427                                 chan->state == BT_CONFIG)) {
7428                         chan->ops->resume(chan);
7429                         l2cap_check_encryption(chan, encrypt);
7430                         l2cap_chan_unlock(chan);
7431                         continue;
7432                 }
7433
7434                 if (chan->state == BT_CONNECT) {
7435                         if (!status)
7436                                 l2cap_start_connection(chan);
7437                         else
7438                                 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
7439                 } else if (chan->state == BT_CONNECT2 &&
7440                            chan->mode != L2CAP_MODE_LE_FLOWCTL) {
7441                         struct l2cap_conn_rsp rsp;
7442                         __u16 res, stat;
7443
7444                         if (!status) {
7445                                 if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
7446                                         res = L2CAP_CR_PEND;
7447                                         stat = L2CAP_CS_AUTHOR_PEND;
7448                                         chan->ops->defer(chan);
7449                                 } else {
7450                                         l2cap_state_change(chan, BT_CONFIG);
7451                                         res = L2CAP_CR_SUCCESS;
7452                                         stat = L2CAP_CS_NO_INFO;
7453                                 }
7454                         } else {
7455                                 l2cap_state_change(chan, BT_DISCONN);
7456                                 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
7457                                 res = L2CAP_CR_SEC_BLOCK;
7458                                 stat = L2CAP_CS_NO_INFO;
7459                         }
7460
7461                         rsp.scid   = cpu_to_le16(chan->dcid);
7462                         rsp.dcid   = cpu_to_le16(chan->scid);
7463                         rsp.result = cpu_to_le16(res);
7464                         rsp.status = cpu_to_le16(stat);
7465                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
7466                                        sizeof(rsp), &rsp);
7467
7468                         if (!test_bit(CONF_REQ_SENT, &chan->conf_state) &&
7469                             res == L2CAP_CR_SUCCESS) {
7470                                 char buf[128];
7471                                 set_bit(CONF_REQ_SENT, &chan->conf_state);
7472                                 l2cap_send_cmd(conn, l2cap_get_ident(conn),
7473                                                L2CAP_CONF_REQ,
7474                                                l2cap_build_conf_req(chan, buf, sizeof(buf)),
7475                                                buf);
7476                                 chan->num_conf_req++;
7477                         }
7478                 }
7479
7480                 l2cap_chan_unlock(chan);
7481         }
7482
7483         mutex_unlock(&conn->chan_lock);
7484 }
7485
7486 void l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
7487 {
7488         struct l2cap_conn *conn = hcon->l2cap_data;
7489         struct l2cap_hdr *hdr;
7490         int len;
7491
7492         /* For AMP controller do not create l2cap conn */
7493         if (!conn && hcon->hdev->dev_type != HCI_PRIMARY)
7494                 goto drop;
7495
7496         if (!conn)
7497                 conn = l2cap_conn_add(hcon);
7498
7499         if (!conn)
7500                 goto drop;
7501
7502         BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
7503
7504         switch (flags) {
7505         case ACL_START:
7506         case ACL_START_NO_FLUSH:
7507         case ACL_COMPLETE:
7508                 if (conn->rx_len) {
7509                         BT_ERR("Unexpected start frame (len %d)", skb->len);
7510                         kfree_skb(conn->rx_skb);
7511                         conn->rx_skb = NULL;
7512                         conn->rx_len = 0;
7513                         l2cap_conn_unreliable(conn, ECOMM);
7514                 }
7515
7516                 /* Start fragment always begin with Basic L2CAP header */
7517                 if (skb->len < L2CAP_HDR_SIZE) {
7518                         BT_ERR("Frame is too short (len %d)", skb->len);
7519                         l2cap_conn_unreliable(conn, ECOMM);
7520                         goto drop;
7521                 }
7522
7523                 hdr = (struct l2cap_hdr *) skb->data;
7524                 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
7525
7526                 if (len == skb->len) {
7527                         /* Complete frame received */
7528                         l2cap_recv_frame(conn, skb);
7529                         return;
7530                 }
7531
7532                 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
7533
7534                 if (skb->len > len) {
7535                         BT_ERR("Frame is too long (len %d, expected len %d)",
7536                                skb->len, len);
7537                         l2cap_conn_unreliable(conn, ECOMM);
7538                         goto drop;
7539                 }
7540
7541                 /* Allocate skb for the complete frame (with header) */
7542                 conn->rx_skb = bt_skb_alloc(len, GFP_KERNEL);
7543                 if (!conn->rx_skb)
7544                         goto drop;
7545
7546                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
7547                                           skb->len);
7548                 conn->rx_len = len - skb->len;
7549                 break;
7550
7551         case ACL_CONT:
7552                 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
7553
7554                 if (!conn->rx_len) {
7555                         BT_ERR("Unexpected continuation frame (len %d)", skb->len);
7556                         l2cap_conn_unreliable(conn, ECOMM);
7557                         goto drop;
7558                 }
7559
7560                 if (skb->len > conn->rx_len) {
7561                         BT_ERR("Fragment is too long (len %d, expected %d)",
7562                                skb->len, conn->rx_len);
7563                         kfree_skb(conn->rx_skb);
7564                         conn->rx_skb = NULL;
7565                         conn->rx_len = 0;
7566                         l2cap_conn_unreliable(conn, ECOMM);
7567                         goto drop;
7568                 }
7569
7570                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
7571                                           skb->len);
7572                 conn->rx_len -= skb->len;
7573
7574                 if (!conn->rx_len) {
7575                         /* Complete frame received. l2cap_recv_frame
7576                          * takes ownership of the skb so set the global
7577                          * rx_skb pointer to NULL first.
7578                          */
7579                         struct sk_buff *rx_skb = conn->rx_skb;
7580                         conn->rx_skb = NULL;
7581                         l2cap_recv_frame(conn, rx_skb);
7582                 }
7583                 break;
7584         }
7585
7586 drop:
7587         kfree_skb(skb);
7588 }
7589
7590 static struct hci_cb l2cap_cb = {
7591         .name           = "L2CAP",
7592         .connect_cfm    = l2cap_connect_cfm,
7593         .disconn_cfm    = l2cap_disconn_cfm,
7594         .security_cfm   = l2cap_security_cfm,
7595 };
7596
7597 static int l2cap_debugfs_show(struct seq_file *f, void *p)
7598 {
7599         struct l2cap_chan *c;
7600
7601         read_lock(&chan_list_lock);
7602
7603         list_for_each_entry(c, &chan_list, global_l) {
7604                 seq_printf(f, "%pMR (%u) %pMR (%u) %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
7605                            &c->src, c->src_type, &c->dst, c->dst_type,
7606                            c->state, __le16_to_cpu(c->psm),
7607                            c->scid, c->dcid, c->imtu, c->omtu,
7608                            c->sec_level, c->mode);
7609         }
7610
7611         read_unlock(&chan_list_lock);
7612
7613         return 0;
7614 }
7615
7616 static int l2cap_debugfs_open(struct inode *inode, struct file *file)
7617 {
7618         return single_open(file, l2cap_debugfs_show, inode->i_private);
7619 }
7620
7621 static const struct file_operations l2cap_debugfs_fops = {
7622         .open           = l2cap_debugfs_open,
7623         .read           = seq_read,
7624         .llseek         = seq_lseek,
7625         .release        = single_release,
7626 };
7627
7628 static struct dentry *l2cap_debugfs;
7629
7630 int __init l2cap_init(void)
7631 {
7632         int err;
7633
7634         err = l2cap_init_sockets();
7635         if (err < 0)
7636                 return err;
7637
7638         hci_register_cb(&l2cap_cb);
7639
7640         if (IS_ERR_OR_NULL(bt_debugfs))
7641                 return 0;
7642
7643         l2cap_debugfs = debugfs_create_file("l2cap", 0444, bt_debugfs,
7644                                             NULL, &l2cap_debugfs_fops);
7645
7646         debugfs_create_u16("l2cap_le_max_credits", 0644, bt_debugfs,
7647                            &le_max_credits);
7648         debugfs_create_u16("l2cap_le_default_mps", 0644, bt_debugfs,
7649                            &le_default_mps);
7650
7651         return 0;
7652 }
7653
7654 void l2cap_exit(void)
7655 {
7656         debugfs_remove(l2cap_debugfs);
7657         hci_unregister_cb(&l2cap_cb);
7658         l2cap_cleanup_sockets();
7659 }
7660
7661 module_param(disable_ertm, bool, 0644);
7662 MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode");