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                         if (olen == sizeof(efs)) {
3367                                 remote_efs = 1;
3368                                 memcpy(&efs, (void *) val, olen);
3369                         }
3370                         break;
3371
3372                 case L2CAP_CONF_EWS:
3373                         if (!(chan->conn->local_fixed_chan & L2CAP_FC_A2MP))
3374                                 return -ECONNREFUSED;
3375
3376                         set_bit(FLAG_EXT_CTRL, &chan->flags);
3377                         set_bit(CONF_EWS_RECV, &chan->conf_state);
3378                         chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
3379                         chan->remote_tx_win = val;
3380                         break;
3381
3382                 default:
3383                         if (hint)
3384                                 break;
3385
3386                         result = L2CAP_CONF_UNKNOWN;
3387                         *((u8 *) ptr++) = type;
3388                         break;
3389                 }
3390         }
3391
3392         if (chan->num_conf_rsp || chan->num_conf_req > 1)
3393                 goto done;
3394
3395         switch (chan->mode) {
3396         case L2CAP_MODE_STREAMING:
3397         case L2CAP_MODE_ERTM:
3398                 if (!test_bit(CONF_STATE2_DEVICE, &chan->conf_state)) {
3399                         chan->mode = l2cap_select_mode(rfc.mode,
3400                                                        chan->conn->feat_mask);
3401                         break;
3402                 }
3403
3404                 if (remote_efs) {
3405                         if (__l2cap_efs_supported(chan->conn))
3406                                 set_bit(FLAG_EFS_ENABLE, &chan->flags);
3407                         else
3408                                 return -ECONNREFUSED;
3409                 }
3410
3411                 if (chan->mode != rfc.mode)
3412                         return -ECONNREFUSED;
3413
3414                 break;
3415         }
3416
3417 done:
3418         if (chan->mode != rfc.mode) {
3419                 result = L2CAP_CONF_UNACCEPT;
3420                 rfc.mode = chan->mode;
3421
3422                 if (chan->num_conf_rsp == 1)
3423                         return -ECONNREFUSED;
3424
3425                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3426                                    (unsigned long) &rfc, endptr - ptr);
3427         }
3428
3429         if (result == L2CAP_CONF_SUCCESS) {
3430                 /* Configure output options and let the other side know
3431                  * which ones we don't like. */
3432
3433                 if (mtu < L2CAP_DEFAULT_MIN_MTU)
3434                         result = L2CAP_CONF_UNACCEPT;
3435                 else {
3436                         chan->omtu = mtu;
3437                         set_bit(CONF_MTU_DONE, &chan->conf_state);
3438                 }
3439                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->omtu, endptr - ptr);
3440
3441                 if (remote_efs) {
3442                         if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
3443                             efs.stype != L2CAP_SERV_NOTRAFIC &&
3444                             efs.stype != chan->local_stype) {
3445
3446                                 result = L2CAP_CONF_UNACCEPT;
3447
3448                                 if (chan->num_conf_req >= 1)
3449                                         return -ECONNREFUSED;
3450
3451                                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
3452                                                    sizeof(efs),
3453                                                    (unsigned long) &efs, endptr - ptr);
3454                         } else {
3455                                 /* Send PENDING Conf Rsp */
3456                                 result = L2CAP_CONF_PENDING;
3457                                 set_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
3458                         }
3459                 }
3460
3461                 switch (rfc.mode) {
3462                 case L2CAP_MODE_BASIC:
3463                         chan->fcs = L2CAP_FCS_NONE;
3464                         set_bit(CONF_MODE_DONE, &chan->conf_state);
3465                         break;
3466
3467                 case L2CAP_MODE_ERTM:
3468                         if (!test_bit(CONF_EWS_RECV, &chan->conf_state))
3469                                 chan->remote_tx_win = rfc.txwin_size;
3470                         else
3471                                 rfc.txwin_size = L2CAP_DEFAULT_TX_WINDOW;
3472
3473                         chan->remote_max_tx = rfc.max_transmit;
3474
3475                         size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
3476                                      chan->conn->mtu - L2CAP_EXT_HDR_SIZE -
3477                                      L2CAP_SDULEN_SIZE - L2CAP_FCS_SIZE);
3478                         rfc.max_pdu_size = cpu_to_le16(size);
3479                         chan->remote_mps = size;
3480
3481                         __l2cap_set_ertm_timeouts(chan, &rfc);
3482
3483                         set_bit(CONF_MODE_DONE, &chan->conf_state);
3484
3485                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
3486                                            sizeof(rfc), (unsigned long) &rfc, endptr - ptr);
3487
3488                         if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
3489                                 chan->remote_id = efs.id;
3490                                 chan->remote_stype = efs.stype;
3491                                 chan->remote_msdu = le16_to_cpu(efs.msdu);
3492                                 chan->remote_flush_to =
3493                                         le32_to_cpu(efs.flush_to);
3494                                 chan->remote_acc_lat =
3495                                         le32_to_cpu(efs.acc_lat);
3496                                 chan->remote_sdu_itime =
3497                                         le32_to_cpu(efs.sdu_itime);
3498                                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
3499                                                    sizeof(efs),
3500                                                    (unsigned long) &efs, endptr - ptr);
3501                         }
3502                         break;
3503
3504                 case L2CAP_MODE_STREAMING:
3505                         size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
3506                                      chan->conn->mtu - L2CAP_EXT_HDR_SIZE -
3507                                      L2CAP_SDULEN_SIZE - L2CAP_FCS_SIZE);
3508                         rfc.max_pdu_size = cpu_to_le16(size);
3509                         chan->remote_mps = size;
3510
3511                         set_bit(CONF_MODE_DONE, &chan->conf_state);
3512
3513                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3514                                            (unsigned long) &rfc, endptr - ptr);
3515
3516                         break;
3517
3518                 default:
3519                         result = L2CAP_CONF_UNACCEPT;
3520
3521                         memset(&rfc, 0, sizeof(rfc));
3522                         rfc.mode = chan->mode;
3523                 }
3524
3525                 if (result == L2CAP_CONF_SUCCESS)
3526                         set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
3527         }
3528         rsp->scid   = cpu_to_le16(chan->dcid);
3529         rsp->result = cpu_to_le16(result);
3530         rsp->flags  = cpu_to_le16(0);
3531
3532         return ptr - data;
3533 }
3534
3535 static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len,
3536                                 void *data, size_t size, u16 *result)
3537 {
3538         struct l2cap_conf_req *req = data;
3539         void *ptr = req->data;
3540         void *endptr = data + size;
3541         int type, olen;
3542         unsigned long val;
3543         struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
3544         struct l2cap_conf_efs efs;
3545
3546         BT_DBG("chan %p, rsp %p, len %d, req %p", chan, rsp, len, data);
3547
3548         while (len >= L2CAP_CONF_OPT_SIZE) {
3549                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
3550
3551                 switch (type) {
3552                 case L2CAP_CONF_MTU:
3553                         if (val < L2CAP_DEFAULT_MIN_MTU) {
3554                                 *result = L2CAP_CONF_UNACCEPT;
3555                                 chan->imtu = L2CAP_DEFAULT_MIN_MTU;
3556                         } else
3557                                 chan->imtu = val;
3558                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu, endptr - ptr);
3559                         break;
3560
3561                 case L2CAP_CONF_FLUSH_TO:
3562                         chan->flush_to = val;
3563                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
3564                                            2, chan->flush_to, endptr - ptr);
3565                         break;
3566
3567                 case L2CAP_CONF_RFC:
3568                         if (olen == sizeof(rfc))
3569                                 memcpy(&rfc, (void *)val, olen);
3570
3571                         if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state) &&
3572                             rfc.mode != chan->mode)
3573                                 return -ECONNREFUSED;
3574
3575                         chan->fcs = 0;
3576
3577                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
3578                                            sizeof(rfc), (unsigned long) &rfc, endptr - ptr);
3579                         break;
3580
3581                 case L2CAP_CONF_EWS:
3582                         chan->ack_win = min_t(u16, val, chan->ack_win);
3583                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
3584                                            chan->tx_win, endptr - ptr);
3585                         break;
3586
3587                 case L2CAP_CONF_EFS:
3588                         if (olen == sizeof(efs)) {
3589                                 memcpy(&efs, (void *)val, olen);
3590
3591                                 if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
3592                                     efs.stype != L2CAP_SERV_NOTRAFIC &&
3593                                     efs.stype != chan->local_stype)
3594                                         return -ECONNREFUSED;
3595
3596                                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS, sizeof(efs),
3597                                                    (unsigned long) &efs, endptr - ptr);
3598                         }
3599                         break;
3600
3601                 case L2CAP_CONF_FCS:
3602                         if (*result == L2CAP_CONF_PENDING)
3603                                 if (val == L2CAP_FCS_NONE)
3604                                         set_bit(CONF_RECV_NO_FCS,
3605                                                 &chan->conf_state);
3606                         break;
3607                 }
3608         }
3609
3610         if (chan->mode == L2CAP_MODE_BASIC && chan->mode != rfc.mode)
3611                 return -ECONNREFUSED;
3612
3613         chan->mode = rfc.mode;
3614
3615         if (*result == L2CAP_CONF_SUCCESS || *result == L2CAP_CONF_PENDING) {
3616                 switch (rfc.mode) {
3617                 case L2CAP_MODE_ERTM:
3618                         chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
3619                         chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
3620                         chan->mps    = le16_to_cpu(rfc.max_pdu_size);
3621                         if (!test_bit(FLAG_EXT_CTRL, &chan->flags))
3622                                 chan->ack_win = min_t(u16, chan->ack_win,
3623                                                       rfc.txwin_size);
3624
3625                         if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
3626                                 chan->local_msdu = le16_to_cpu(efs.msdu);
3627                                 chan->local_sdu_itime =
3628                                         le32_to_cpu(efs.sdu_itime);
3629                                 chan->local_acc_lat = le32_to_cpu(efs.acc_lat);
3630                                 chan->local_flush_to =
3631                                         le32_to_cpu(efs.flush_to);
3632                         }
3633                         break;
3634
3635                 case L2CAP_MODE_STREAMING:
3636                         chan->mps    = le16_to_cpu(rfc.max_pdu_size);
3637                 }
3638         }
3639
3640         req->dcid   = cpu_to_le16(chan->dcid);
3641         req->flags  = cpu_to_le16(0);
3642
3643         return ptr - data;
3644 }
3645
3646 static int l2cap_build_conf_rsp(struct l2cap_chan *chan, void *data,
3647                                 u16 result, u16 flags)
3648 {
3649         struct l2cap_conf_rsp *rsp = data;
3650         void *ptr = rsp->data;
3651
3652         BT_DBG("chan %p", chan);
3653
3654         rsp->scid   = cpu_to_le16(chan->dcid);
3655         rsp->result = cpu_to_le16(result);
3656         rsp->flags  = cpu_to_le16(flags);
3657
3658         return ptr - data;
3659 }
3660
3661 void __l2cap_le_connect_rsp_defer(struct l2cap_chan *chan)
3662 {
3663         struct l2cap_le_conn_rsp rsp;
3664         struct l2cap_conn *conn = chan->conn;
3665
3666         BT_DBG("chan %p", chan);
3667
3668         rsp.dcid    = cpu_to_le16(chan->scid);
3669         rsp.mtu     = cpu_to_le16(chan->imtu);
3670         rsp.mps     = cpu_to_le16(chan->mps);
3671         rsp.credits = cpu_to_le16(chan->rx_credits);
3672         rsp.result  = cpu_to_le16(L2CAP_CR_SUCCESS);
3673
3674         l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CONN_RSP, sizeof(rsp),
3675                        &rsp);
3676 }
3677
3678 void __l2cap_connect_rsp_defer(struct l2cap_chan *chan)
3679 {
3680         struct l2cap_conn_rsp rsp;
3681         struct l2cap_conn *conn = chan->conn;
3682         u8 buf[128];
3683         u8 rsp_code;
3684
3685         rsp.scid   = cpu_to_le16(chan->dcid);
3686         rsp.dcid   = cpu_to_le16(chan->scid);
3687         rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
3688         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
3689
3690         if (chan->hs_hcon)
3691                 rsp_code = L2CAP_CREATE_CHAN_RSP;
3692         else
3693                 rsp_code = L2CAP_CONN_RSP;
3694
3695         BT_DBG("chan %p rsp_code %u", chan, rsp_code);
3696
3697         l2cap_send_cmd(conn, chan->ident, rsp_code, sizeof(rsp), &rsp);
3698
3699         if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
3700                 return;
3701
3702         l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3703                        l2cap_build_conf_req(chan, buf, sizeof(buf)), buf);
3704         chan->num_conf_req++;
3705 }
3706
3707 static void l2cap_conf_rfc_get(struct l2cap_chan *chan, void *rsp, int len)
3708 {
3709         int type, olen;
3710         unsigned long val;
3711         /* Use sane default values in case a misbehaving remote device
3712          * did not send an RFC or extended window size option.
3713          */
3714         u16 txwin_ext = chan->ack_win;
3715         struct l2cap_conf_rfc rfc = {
3716                 .mode = chan->mode,
3717                 .retrans_timeout = cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO),
3718                 .monitor_timeout = cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO),
3719                 .max_pdu_size = cpu_to_le16(chan->imtu),
3720                 .txwin_size = min_t(u16, chan->ack_win, L2CAP_DEFAULT_TX_WINDOW),
3721         };
3722
3723         BT_DBG("chan %p, rsp %p, len %d", chan, rsp, len);
3724
3725         if ((chan->mode != L2CAP_MODE_ERTM) && (chan->mode != L2CAP_MODE_STREAMING))
3726                 return;
3727
3728         while (len >= L2CAP_CONF_OPT_SIZE) {
3729                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
3730
3731                 switch (type) {
3732                 case L2CAP_CONF_RFC:
3733                         if (olen == sizeof(rfc))
3734                                 memcpy(&rfc, (void *)val, olen);
3735                         break;
3736                 case L2CAP_CONF_EWS:
3737                         txwin_ext = val;
3738                         break;
3739                 }
3740         }
3741
3742         switch (rfc.mode) {
3743         case L2CAP_MODE_ERTM:
3744                 chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
3745                 chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
3746                 chan->mps = le16_to_cpu(rfc.max_pdu_size);
3747                 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
3748                         chan->ack_win = min_t(u16, chan->ack_win, txwin_ext);
3749                 else
3750                         chan->ack_win = min_t(u16, chan->ack_win,
3751                                               rfc.txwin_size);
3752                 break;
3753         case L2CAP_MODE_STREAMING:
3754                 chan->mps    = le16_to_cpu(rfc.max_pdu_size);
3755         }
3756 }
3757
3758 static inline int l2cap_command_rej(struct l2cap_conn *conn,
3759                                     struct l2cap_cmd_hdr *cmd, u16 cmd_len,
3760                                     u8 *data)
3761 {
3762         struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data;
3763
3764         if (cmd_len < sizeof(*rej))
3765                 return -EPROTO;
3766
3767         if (rej->reason != L2CAP_REJ_NOT_UNDERSTOOD)
3768                 return 0;
3769
3770         if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
3771             cmd->ident == conn->info_ident) {
3772                 cancel_delayed_work(&conn->info_timer);
3773
3774                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3775                 conn->info_ident = 0;
3776
3777                 l2cap_conn_start(conn);
3778         }
3779
3780         return 0;
3781 }
3782
3783 static struct l2cap_chan *l2cap_connect(struct l2cap_conn *conn,
3784                                         struct l2cap_cmd_hdr *cmd,
3785                                         u8 *data, u8 rsp_code, u8 amp_id)
3786 {
3787         struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
3788         struct l2cap_conn_rsp rsp;
3789         struct l2cap_chan *chan = NULL, *pchan;
3790         int result, status = L2CAP_CS_NO_INFO;
3791
3792         u16 dcid = 0, scid = __le16_to_cpu(req->scid);
3793         __le16 psm = req->psm;
3794
3795         BT_DBG("psm 0x%2.2x scid 0x%4.4x", __le16_to_cpu(psm), scid);
3796
3797         /* Check if we have socket listening on psm */
3798         pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, &conn->hcon->src,
3799                                          &conn->hcon->dst, ACL_LINK);
3800         if (!pchan) {
3801                 result = L2CAP_CR_BAD_PSM;
3802                 goto sendresp;
3803         }
3804
3805         mutex_lock(&conn->chan_lock);
3806         l2cap_chan_lock(pchan);
3807
3808         /* Check if the ACL is secure enough (if not SDP) */
3809         if (psm != cpu_to_le16(L2CAP_PSM_SDP) &&
3810             !hci_conn_check_link_mode(conn->hcon)) {
3811                 conn->disc_reason = HCI_ERROR_AUTH_FAILURE;
3812                 result = L2CAP_CR_SEC_BLOCK;
3813                 goto response;
3814         }
3815
3816         result = L2CAP_CR_NO_MEM;
3817
3818         /* Check if we already have channel with that dcid */
3819         if (__l2cap_get_chan_by_dcid(conn, scid))
3820                 goto response;
3821
3822         chan = pchan->ops->new_connection(pchan);
3823         if (!chan)
3824                 goto response;
3825
3826         /* For certain devices (ex: HID mouse), support for authentication,
3827          * pairing and bonding is optional. For such devices, inorder to avoid
3828          * the ACL alive for too long after L2CAP disconnection, reset the ACL
3829          * disc_timeout back to HCI_DISCONN_TIMEOUT during L2CAP connect.
3830          */
3831         conn->hcon->disc_timeout = HCI_DISCONN_TIMEOUT;
3832
3833         bacpy(&chan->src, &conn->hcon->src);
3834         bacpy(&chan->dst, &conn->hcon->dst);
3835         chan->src_type = bdaddr_src_type(conn->hcon);
3836         chan->dst_type = bdaddr_dst_type(conn->hcon);
3837         chan->psm  = psm;
3838         chan->dcid = scid;
3839         chan->local_amp_id = amp_id;
3840
3841         __l2cap_chan_add(conn, chan);
3842
3843         dcid = chan->scid;
3844
3845         __set_chan_timer(chan, chan->ops->get_sndtimeo(chan));
3846
3847         chan->ident = cmd->ident;
3848
3849         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
3850                 if (l2cap_chan_check_security(chan, false)) {
3851                         if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
3852                                 l2cap_state_change(chan, BT_CONNECT2);
3853                                 result = L2CAP_CR_PEND;
3854                                 status = L2CAP_CS_AUTHOR_PEND;
3855                                 chan->ops->defer(chan);
3856                         } else {
3857                                 /* Force pending result for AMP controllers.
3858                                  * The connection will succeed after the
3859                                  * physical link is up.
3860                                  */
3861                                 if (amp_id == AMP_ID_BREDR) {
3862                                         l2cap_state_change(chan, BT_CONFIG);
3863                                         result = L2CAP_CR_SUCCESS;
3864                                 } else {
3865                                         l2cap_state_change(chan, BT_CONNECT2);
3866                                         result = L2CAP_CR_PEND;
3867                                 }
3868                                 status = L2CAP_CS_NO_INFO;
3869                         }
3870                 } else {
3871                         l2cap_state_change(chan, BT_CONNECT2);
3872                         result = L2CAP_CR_PEND;
3873                         status = L2CAP_CS_AUTHEN_PEND;
3874                 }
3875         } else {
3876                 l2cap_state_change(chan, BT_CONNECT2);
3877                 result = L2CAP_CR_PEND;
3878                 status = L2CAP_CS_NO_INFO;
3879         }
3880
3881 response:
3882         l2cap_chan_unlock(pchan);
3883         mutex_unlock(&conn->chan_lock);
3884         l2cap_chan_put(pchan);
3885
3886 sendresp:
3887         rsp.scid   = cpu_to_le16(scid);
3888         rsp.dcid   = cpu_to_le16(dcid);
3889         rsp.result = cpu_to_le16(result);
3890         rsp.status = cpu_to_le16(status);
3891         l2cap_send_cmd(conn, cmd->ident, rsp_code, sizeof(rsp), &rsp);
3892
3893         if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
3894                 struct l2cap_info_req info;
3895                 info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
3896
3897                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
3898                 conn->info_ident = l2cap_get_ident(conn);
3899
3900                 schedule_delayed_work(&conn->info_timer, L2CAP_INFO_TIMEOUT);
3901
3902                 l2cap_send_cmd(conn, conn->info_ident, L2CAP_INFO_REQ,
3903                                sizeof(info), &info);
3904         }
3905
3906         if (chan && !test_bit(CONF_REQ_SENT, &chan->conf_state) &&
3907             result == L2CAP_CR_SUCCESS) {
3908                 u8 buf[128];
3909                 set_bit(CONF_REQ_SENT, &chan->conf_state);
3910                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3911                                l2cap_build_conf_req(chan, buf, sizeof(buf)), buf);
3912                 chan->num_conf_req++;
3913         }
3914
3915         return chan;
3916 }
3917
3918 static int l2cap_connect_req(struct l2cap_conn *conn,
3919                              struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
3920 {
3921         struct hci_dev *hdev = conn->hcon->hdev;
3922         struct hci_conn *hcon = conn->hcon;
3923
3924         if (cmd_len < sizeof(struct l2cap_conn_req))
3925                 return -EPROTO;
3926
3927         hci_dev_lock(hdev);
3928         if (hci_dev_test_flag(hdev, HCI_MGMT) &&
3929             !test_and_set_bit(HCI_CONN_MGMT_CONNECTED, &hcon->flags))
3930                 mgmt_device_connected(hdev, hcon, 0, NULL, 0);
3931         hci_dev_unlock(hdev);
3932
3933         l2cap_connect(conn, cmd, data, L2CAP_CONN_RSP, 0);
3934         return 0;
3935 }
3936
3937 static int l2cap_connect_create_rsp(struct l2cap_conn *conn,
3938                                     struct l2cap_cmd_hdr *cmd, u16 cmd_len,
3939                                     u8 *data)
3940 {
3941         struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
3942         u16 scid, dcid, result, status;
3943         struct l2cap_chan *chan;
3944         u8 req[128];
3945         int err;
3946
3947         if (cmd_len < sizeof(*rsp))
3948                 return -EPROTO;
3949
3950         scid   = __le16_to_cpu(rsp->scid);
3951         dcid   = __le16_to_cpu(rsp->dcid);
3952         result = __le16_to_cpu(rsp->result);
3953         status = __le16_to_cpu(rsp->status);
3954
3955         BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x",
3956                dcid, scid, result, status);
3957
3958         mutex_lock(&conn->chan_lock);
3959
3960         if (scid) {
3961                 chan = __l2cap_get_chan_by_scid(conn, scid);
3962                 if (!chan) {
3963                         err = -EBADSLT;
3964                         goto unlock;
3965                 }
3966         } else {
3967                 chan = __l2cap_get_chan_by_ident(conn, cmd->ident);
3968                 if (!chan) {
3969                         err = -EBADSLT;
3970                         goto unlock;
3971                 }
3972         }
3973
3974         err = 0;
3975
3976         l2cap_chan_lock(chan);
3977
3978         switch (result) {
3979         case L2CAP_CR_SUCCESS:
3980                 l2cap_state_change(chan, BT_CONFIG);
3981                 chan->ident = 0;
3982                 chan->dcid = dcid;
3983                 clear_bit(CONF_CONNECT_PEND, &chan->conf_state);
3984
3985                 if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
3986                         break;
3987
3988                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3989                                l2cap_build_conf_req(chan, req, sizeof(req)), req);
3990                 chan->num_conf_req++;
3991                 break;
3992
3993         case L2CAP_CR_PEND:
3994                 set_bit(CONF_CONNECT_PEND, &chan->conf_state);
3995                 break;
3996
3997         default:
3998                 l2cap_chan_del(chan, ECONNREFUSED);
3999                 break;
4000         }
4001
4002         l2cap_chan_unlock(chan);
4003
4004 unlock:
4005         mutex_unlock(&conn->chan_lock);
4006
4007         return err;
4008 }
4009
4010 static inline void set_default_fcs(struct l2cap_chan *chan)
4011 {
4012         /* FCS is enabled only in ERTM or streaming mode, if one or both
4013          * sides request it.
4014          */
4015         if (chan->mode != L2CAP_MODE_ERTM && chan->mode != L2CAP_MODE_STREAMING)
4016                 chan->fcs = L2CAP_FCS_NONE;
4017         else if (!test_bit(CONF_RECV_NO_FCS, &chan->conf_state))
4018                 chan->fcs = L2CAP_FCS_CRC16;
4019 }
4020
4021 static void l2cap_send_efs_conf_rsp(struct l2cap_chan *chan, void *data,
4022                                     u8 ident, u16 flags)
4023 {
4024         struct l2cap_conn *conn = chan->conn;
4025
4026         BT_DBG("conn %p chan %p ident %d flags 0x%4.4x", conn, chan, ident,
4027                flags);
4028
4029         clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
4030         set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
4031
4032         l2cap_send_cmd(conn, ident, L2CAP_CONF_RSP,
4033                        l2cap_build_conf_rsp(chan, data,
4034                                             L2CAP_CONF_SUCCESS, flags), data);
4035 }
4036
4037 static void cmd_reject_invalid_cid(struct l2cap_conn *conn, u8 ident,
4038                                    u16 scid, u16 dcid)
4039 {
4040         struct l2cap_cmd_rej_cid rej;
4041
4042         rej.reason = cpu_to_le16(L2CAP_REJ_INVALID_CID);
4043         rej.scid = __cpu_to_le16(scid);
4044         rej.dcid = __cpu_to_le16(dcid);
4045
4046         l2cap_send_cmd(conn, ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
4047 }
4048
4049 static inline int l2cap_config_req(struct l2cap_conn *conn,
4050                                    struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4051                                    u8 *data)
4052 {
4053         struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
4054         u16 dcid, flags;
4055         u8 rsp[64];
4056         struct l2cap_chan *chan;
4057         int len, err = 0;
4058
4059         if (cmd_len < sizeof(*req))
4060                 return -EPROTO;
4061
4062         dcid  = __le16_to_cpu(req->dcid);
4063         flags = __le16_to_cpu(req->flags);
4064
4065         BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
4066
4067         chan = l2cap_get_chan_by_scid(conn, dcid);
4068         if (!chan) {
4069                 cmd_reject_invalid_cid(conn, cmd->ident, dcid, 0);
4070                 return 0;
4071         }
4072
4073         if (chan->state != BT_CONFIG && chan->state != BT_CONNECT2) {
4074                 cmd_reject_invalid_cid(conn, cmd->ident, chan->scid,
4075                                        chan->dcid);
4076                 goto unlock;
4077         }
4078
4079         /* Reject if config buffer is too small. */
4080         len = cmd_len - sizeof(*req);
4081         if (chan->conf_len + len > sizeof(chan->conf_req)) {
4082                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
4083                                l2cap_build_conf_rsp(chan, rsp,
4084                                L2CAP_CONF_REJECT, flags), rsp);
4085                 goto unlock;
4086         }
4087
4088         /* Store config. */
4089         memcpy(chan->conf_req + chan->conf_len, req->data, len);
4090         chan->conf_len += len;
4091
4092         if (flags & L2CAP_CONF_FLAG_CONTINUATION) {
4093                 /* Incomplete config. Send empty response. */
4094                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
4095                                l2cap_build_conf_rsp(chan, rsp,
4096                                L2CAP_CONF_SUCCESS, flags), rsp);
4097                 goto unlock;
4098         }
4099
4100         /* Complete config. */
4101         len = l2cap_parse_conf_req(chan, rsp, sizeof(rsp));
4102         if (len < 0) {
4103                 l2cap_send_disconn_req(chan, ECONNRESET);
4104                 goto unlock;
4105         }
4106
4107         chan->ident = cmd->ident;
4108         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
4109         chan->num_conf_rsp++;
4110
4111         /* Reset config buffer. */
4112         chan->conf_len = 0;
4113
4114         if (!test_bit(CONF_OUTPUT_DONE, &chan->conf_state))
4115                 goto unlock;
4116
4117         if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
4118                 set_default_fcs(chan);
4119
4120                 if (chan->mode == L2CAP_MODE_ERTM ||
4121                     chan->mode == L2CAP_MODE_STREAMING)
4122                         err = l2cap_ertm_init(chan);
4123
4124                 if (err < 0)
4125                         l2cap_send_disconn_req(chan, -err);
4126                 else
4127                         l2cap_chan_ready(chan);
4128
4129                 goto unlock;
4130         }
4131
4132         if (!test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) {
4133                 u8 buf[64];
4134                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
4135                                l2cap_build_conf_req(chan, buf, sizeof(buf)), buf);
4136                 chan->num_conf_req++;
4137         }
4138
4139         /* Got Conf Rsp PENDING from remote side and assume we sent
4140            Conf Rsp PENDING in the code above */
4141         if (test_bit(CONF_REM_CONF_PEND, &chan->conf_state) &&
4142             test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
4143
4144                 /* check compatibility */
4145
4146                 /* Send rsp for BR/EDR channel */
4147                 if (!chan->hs_hcon)
4148                         l2cap_send_efs_conf_rsp(chan, rsp, cmd->ident, flags);
4149                 else
4150                         chan->ident = cmd->ident;
4151         }
4152
4153 unlock:
4154         l2cap_chan_unlock(chan);
4155         return err;
4156 }
4157
4158 static inline int l2cap_config_rsp(struct l2cap_conn *conn,
4159                                    struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4160                                    u8 *data)
4161 {
4162         struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
4163         u16 scid, flags, result;
4164         struct l2cap_chan *chan;
4165         int len = cmd_len - sizeof(*rsp);
4166         int err = 0;
4167
4168         if (cmd_len < sizeof(*rsp))
4169                 return -EPROTO;
4170
4171         scid   = __le16_to_cpu(rsp->scid);
4172         flags  = __le16_to_cpu(rsp->flags);
4173         result = __le16_to_cpu(rsp->result);
4174
4175         BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x len %d", scid, flags,
4176                result, len);
4177
4178         chan = l2cap_get_chan_by_scid(conn, scid);
4179         if (!chan)
4180                 return 0;
4181
4182         switch (result) {
4183         case L2CAP_CONF_SUCCESS:
4184                 l2cap_conf_rfc_get(chan, rsp->data, len);
4185                 clear_bit(CONF_REM_CONF_PEND, &chan->conf_state);
4186                 break;
4187
4188         case L2CAP_CONF_PENDING:
4189                 set_bit(CONF_REM_CONF_PEND, &chan->conf_state);
4190
4191                 if (test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
4192                         char buf[64];
4193
4194                         len = l2cap_parse_conf_rsp(chan, rsp->data, len,
4195                                                    buf, sizeof(buf), &result);
4196                         if (len < 0) {
4197                                 l2cap_send_disconn_req(chan, ECONNRESET);
4198                                 goto done;
4199                         }
4200
4201                         if (!chan->hs_hcon) {
4202                                 l2cap_send_efs_conf_rsp(chan, buf, cmd->ident,
4203                                                         0);
4204                         } else {
4205                                 if (l2cap_check_efs(chan)) {
4206                                         amp_create_logical_link(chan);
4207                                         chan->ident = cmd->ident;
4208                                 }
4209                         }
4210                 }
4211                 goto done;
4212
4213         case L2CAP_CONF_UNACCEPT:
4214                 if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
4215                         char req[64];
4216
4217                         if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
4218                                 l2cap_send_disconn_req(chan, ECONNRESET);
4219                                 goto done;
4220                         }
4221
4222                         /* throw out any old stored conf requests */
4223                         result = L2CAP_CONF_SUCCESS;
4224                         len = l2cap_parse_conf_rsp(chan, rsp->data, len,
4225                                                    req, sizeof(req), &result);
4226                         if (len < 0) {
4227                                 l2cap_send_disconn_req(chan, ECONNRESET);
4228                                 goto done;
4229                         }
4230
4231                         l2cap_send_cmd(conn, l2cap_get_ident(conn),
4232                                        L2CAP_CONF_REQ, len, req);
4233                         chan->num_conf_req++;
4234                         if (result != L2CAP_CONF_SUCCESS)
4235                                 goto done;
4236                         break;
4237                 }
4238
4239         default:
4240                 l2cap_chan_set_err(chan, ECONNRESET);
4241
4242                 __set_chan_timer(chan, L2CAP_DISC_REJ_TIMEOUT);
4243                 l2cap_send_disconn_req(chan, ECONNRESET);
4244                 goto done;
4245         }
4246
4247         if (flags & L2CAP_CONF_FLAG_CONTINUATION)
4248                 goto done;
4249
4250         set_bit(CONF_INPUT_DONE, &chan->conf_state);
4251
4252         if (test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) {
4253                 set_default_fcs(chan);
4254
4255                 if (chan->mode == L2CAP_MODE_ERTM ||
4256                     chan->mode == L2CAP_MODE_STREAMING)
4257                         err = l2cap_ertm_init(chan);
4258
4259                 if (err < 0)
4260                         l2cap_send_disconn_req(chan, -err);
4261                 else
4262                         l2cap_chan_ready(chan);
4263         }
4264
4265 done:
4266         l2cap_chan_unlock(chan);
4267         return err;
4268 }
4269
4270 static inline int l2cap_disconnect_req(struct l2cap_conn *conn,
4271                                        struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4272                                        u8 *data)
4273 {
4274         struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
4275         struct l2cap_disconn_rsp rsp;
4276         u16 dcid, scid;
4277         struct l2cap_chan *chan;
4278
4279         if (cmd_len != sizeof(*req))
4280                 return -EPROTO;
4281
4282         scid = __le16_to_cpu(req->scid);
4283         dcid = __le16_to_cpu(req->dcid);
4284
4285         BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
4286
4287         mutex_lock(&conn->chan_lock);
4288
4289         chan = __l2cap_get_chan_by_scid(conn, dcid);
4290         if (!chan) {
4291                 mutex_unlock(&conn->chan_lock);
4292                 cmd_reject_invalid_cid(conn, cmd->ident, dcid, scid);
4293                 return 0;
4294         }
4295
4296         l2cap_chan_lock(chan);
4297
4298         rsp.dcid = cpu_to_le16(chan->scid);
4299         rsp.scid = cpu_to_le16(chan->dcid);
4300         l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
4301
4302         chan->ops->set_shutdown(chan);
4303
4304         l2cap_chan_hold(chan);
4305         l2cap_chan_del(chan, ECONNRESET);
4306
4307         l2cap_chan_unlock(chan);
4308
4309         chan->ops->close(chan);
4310         l2cap_chan_put(chan);
4311
4312         mutex_unlock(&conn->chan_lock);
4313
4314         return 0;
4315 }
4316
4317 static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn,
4318                                        struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4319                                        u8 *data)
4320 {
4321         struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
4322         u16 dcid, scid;
4323         struct l2cap_chan *chan;
4324
4325         if (cmd_len != sizeof(*rsp))
4326                 return -EPROTO;
4327
4328         scid = __le16_to_cpu(rsp->scid);
4329         dcid = __le16_to_cpu(rsp->dcid);
4330
4331         BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
4332
4333         mutex_lock(&conn->chan_lock);
4334
4335         chan = __l2cap_get_chan_by_scid(conn, scid);
4336         if (!chan) {
4337                 mutex_unlock(&conn->chan_lock);
4338                 return 0;
4339         }
4340
4341         l2cap_chan_lock(chan);
4342
4343         l2cap_chan_hold(chan);
4344         l2cap_chan_del(chan, 0);
4345
4346         l2cap_chan_unlock(chan);
4347
4348         chan->ops->close(chan);
4349         l2cap_chan_put(chan);
4350
4351         mutex_unlock(&conn->chan_lock);
4352
4353         return 0;
4354 }
4355
4356 static inline int l2cap_information_req(struct l2cap_conn *conn,
4357                                         struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4358                                         u8 *data)
4359 {
4360         struct l2cap_info_req *req = (struct l2cap_info_req *) data;
4361         u16 type;
4362
4363         if (cmd_len != sizeof(*req))
4364                 return -EPROTO;
4365
4366         type = __le16_to_cpu(req->type);
4367
4368         BT_DBG("type 0x%4.4x", type);
4369
4370         if (type == L2CAP_IT_FEAT_MASK) {
4371                 u8 buf[8];
4372                 u32 feat_mask = l2cap_feat_mask;
4373                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
4374                 rsp->type   = cpu_to_le16(L2CAP_IT_FEAT_MASK);
4375                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
4376                 if (!disable_ertm)
4377                         feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
4378                                 | L2CAP_FEAT_FCS;
4379                 if (conn->local_fixed_chan & L2CAP_FC_A2MP)
4380                         feat_mask |= L2CAP_FEAT_EXT_FLOW
4381                                 | L2CAP_FEAT_EXT_WINDOW;
4382
4383                 put_unaligned_le32(feat_mask, rsp->data);
4384                 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf),
4385                                buf);
4386         } else if (type == L2CAP_IT_FIXED_CHAN) {
4387                 u8 buf[12];
4388                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
4389
4390                 rsp->type   = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
4391                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
4392                 rsp->data[0] = conn->local_fixed_chan;
4393                 memset(rsp->data + 1, 0, 7);
4394                 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf),
4395                                buf);
4396         } else {
4397                 struct l2cap_info_rsp rsp;
4398                 rsp.type   = cpu_to_le16(type);
4399                 rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP);
4400                 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(rsp),
4401                                &rsp);
4402         }
4403
4404         return 0;
4405 }
4406
4407 static inline int l2cap_information_rsp(struct l2cap_conn *conn,
4408                                         struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4409                                         u8 *data)
4410 {
4411         struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
4412         u16 type, result;
4413
4414         if (cmd_len < sizeof(*rsp))
4415                 return -EPROTO;
4416
4417         type   = __le16_to_cpu(rsp->type);
4418         result = __le16_to_cpu(rsp->result);
4419
4420         BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
4421
4422         /* L2CAP Info req/rsp are unbound to channels, add extra checks */
4423         if (cmd->ident != conn->info_ident ||
4424             conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)
4425                 return 0;
4426
4427         cancel_delayed_work(&conn->info_timer);
4428
4429         if (result != L2CAP_IR_SUCCESS) {
4430                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4431                 conn->info_ident = 0;
4432
4433                 l2cap_conn_start(conn);
4434
4435                 return 0;
4436         }
4437
4438         switch (type) {
4439         case L2CAP_IT_FEAT_MASK:
4440                 conn->feat_mask = get_unaligned_le32(rsp->data);
4441
4442                 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
4443                         struct l2cap_info_req req;
4444                         req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
4445
4446                         conn->info_ident = l2cap_get_ident(conn);
4447
4448                         l2cap_send_cmd(conn, conn->info_ident,
4449                                        L2CAP_INFO_REQ, sizeof(req), &req);
4450                 } else {
4451                         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4452                         conn->info_ident = 0;
4453
4454                         l2cap_conn_start(conn);
4455                 }
4456                 break;
4457
4458         case L2CAP_IT_FIXED_CHAN:
4459                 conn->remote_fixed_chan = rsp->data[0];
4460                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4461                 conn->info_ident = 0;
4462
4463                 l2cap_conn_start(conn);
4464                 break;
4465         }
4466
4467         return 0;
4468 }
4469
4470 static int l2cap_create_channel_req(struct l2cap_conn *conn,
4471                                     struct l2cap_cmd_hdr *cmd,
4472                                     u16 cmd_len, void *data)
4473 {
4474         struct l2cap_create_chan_req *req = data;
4475         struct l2cap_create_chan_rsp rsp;
4476         struct l2cap_chan *chan;
4477         struct hci_dev *hdev;
4478         u16 psm, scid;
4479
4480         if (cmd_len != sizeof(*req))
4481                 return -EPROTO;
4482
4483         if (!(conn->local_fixed_chan & L2CAP_FC_A2MP))
4484                 return -EINVAL;
4485
4486         psm = le16_to_cpu(req->psm);
4487         scid = le16_to_cpu(req->scid);
4488
4489         BT_DBG("psm 0x%2.2x, scid 0x%4.4x, amp_id %d", psm, scid, req->amp_id);
4490
4491         /* For controller id 0 make BR/EDR connection */
4492         if (req->amp_id == AMP_ID_BREDR) {
4493                 l2cap_connect(conn, cmd, data, L2CAP_CREATE_CHAN_RSP,
4494                               req->amp_id);
4495                 return 0;
4496         }
4497
4498         /* Validate AMP controller id */
4499         hdev = hci_dev_get(req->amp_id);
4500         if (!hdev)
4501                 goto error;
4502
4503         if (hdev->dev_type != HCI_AMP || !test_bit(HCI_UP, &hdev->flags)) {
4504                 hci_dev_put(hdev);
4505                 goto error;
4506         }
4507
4508         chan = l2cap_connect(conn, cmd, data, L2CAP_CREATE_CHAN_RSP,
4509                              req->amp_id);
4510         if (chan) {
4511                 struct amp_mgr *mgr = conn->hcon->amp_mgr;
4512                 struct hci_conn *hs_hcon;
4513
4514                 hs_hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK,
4515                                                   &conn->hcon->dst);
4516                 if (!hs_hcon) {
4517                         hci_dev_put(hdev);
4518                         cmd_reject_invalid_cid(conn, cmd->ident, chan->scid,
4519                                                chan->dcid);
4520                         return 0;
4521                 }
4522
4523                 BT_DBG("mgr %p bredr_chan %p hs_hcon %p", mgr, chan, hs_hcon);
4524
4525                 mgr->bredr_chan = chan;
4526                 chan->hs_hcon = hs_hcon;
4527                 chan->fcs = L2CAP_FCS_NONE;
4528                 conn->mtu = hdev->block_mtu;
4529         }
4530
4531         hci_dev_put(hdev);
4532
4533         return 0;
4534
4535 error:
4536         rsp.dcid = 0;
4537         rsp.scid = cpu_to_le16(scid);
4538         rsp.result = cpu_to_le16(L2CAP_CR_BAD_AMP);
4539         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
4540
4541         l2cap_send_cmd(conn, cmd->ident, L2CAP_CREATE_CHAN_RSP,
4542                        sizeof(rsp), &rsp);
4543
4544         return 0;
4545 }
4546
4547 static void l2cap_send_move_chan_req(struct l2cap_chan *chan, u8 dest_amp_id)
4548 {
4549         struct l2cap_move_chan_req req;
4550         u8 ident;
4551
4552         BT_DBG("chan %p, dest_amp_id %d", chan, dest_amp_id);
4553
4554         ident = l2cap_get_ident(chan->conn);
4555         chan->ident = ident;
4556
4557         req.icid = cpu_to_le16(chan->scid);
4558         req.dest_amp_id = dest_amp_id;
4559
4560         l2cap_send_cmd(chan->conn, ident, L2CAP_MOVE_CHAN_REQ, sizeof(req),
4561                        &req);
4562
4563         __set_chan_timer(chan, L2CAP_MOVE_TIMEOUT);
4564 }
4565
4566 static void l2cap_send_move_chan_rsp(struct l2cap_chan *chan, u16 result)
4567 {
4568         struct l2cap_move_chan_rsp rsp;
4569
4570         BT_DBG("chan %p, result 0x%4.4x", chan, result);
4571
4572         rsp.icid = cpu_to_le16(chan->dcid);
4573         rsp.result = cpu_to_le16(result);
4574
4575         l2cap_send_cmd(chan->conn, chan->ident, L2CAP_MOVE_CHAN_RSP,
4576                        sizeof(rsp), &rsp);
4577 }
4578
4579 static void l2cap_send_move_chan_cfm(struct l2cap_chan *chan, u16 result)
4580 {
4581         struct l2cap_move_chan_cfm cfm;
4582
4583         BT_DBG("chan %p, result 0x%4.4x", chan, result);
4584
4585         chan->ident = l2cap_get_ident(chan->conn);
4586
4587         cfm.icid = cpu_to_le16(chan->scid);
4588         cfm.result = cpu_to_le16(result);
4589
4590         l2cap_send_cmd(chan->conn, chan->ident, L2CAP_MOVE_CHAN_CFM,
4591                        sizeof(cfm), &cfm);
4592
4593         __set_chan_timer(chan, L2CAP_MOVE_TIMEOUT);
4594 }
4595
4596 static void l2cap_send_move_chan_cfm_icid(struct l2cap_conn *conn, u16 icid)
4597 {
4598         struct l2cap_move_chan_cfm cfm;
4599
4600         BT_DBG("conn %p, icid 0x%4.4x", conn, icid);
4601
4602         cfm.icid = cpu_to_le16(icid);
4603         cfm.result = cpu_to_le16(L2CAP_MC_UNCONFIRMED);
4604
4605         l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_MOVE_CHAN_CFM,
4606                        sizeof(cfm), &cfm);
4607 }
4608
4609 static void l2cap_send_move_chan_cfm_rsp(struct l2cap_conn *conn, u8 ident,
4610                                          u16 icid)
4611 {
4612         struct l2cap_move_chan_cfm_rsp rsp;
4613
4614         BT_DBG("icid 0x%4.4x", icid);
4615
4616         rsp.icid = cpu_to_le16(icid);
4617         l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM_RSP, sizeof(rsp), &rsp);
4618 }
4619
4620 static void __release_logical_link(struct l2cap_chan *chan)
4621 {
4622         chan->hs_hchan = NULL;
4623         chan->hs_hcon = NULL;
4624
4625         /* Placeholder - release the logical link */
4626 }
4627
4628 static void l2cap_logical_fail(struct l2cap_chan *chan)
4629 {
4630         /* Logical link setup failed */
4631         if (chan->state != BT_CONNECTED) {
4632                 /* Create channel failure, disconnect */
4633                 l2cap_send_disconn_req(chan, ECONNRESET);
4634                 return;
4635         }
4636
4637         switch (chan->move_role) {
4638         case L2CAP_MOVE_ROLE_RESPONDER:
4639                 l2cap_move_done(chan);
4640                 l2cap_send_move_chan_rsp(chan, L2CAP_MR_NOT_SUPP);
4641                 break;
4642         case L2CAP_MOVE_ROLE_INITIATOR:
4643                 if (chan->move_state == L2CAP_MOVE_WAIT_LOGICAL_COMP ||
4644                     chan->move_state == L2CAP_MOVE_WAIT_LOGICAL_CFM) {
4645                         /* Remote has only sent pending or
4646                          * success responses, clean up
4647                          */
4648                         l2cap_move_done(chan);
4649                 }
4650
4651                 /* Other amp move states imply that the move
4652                  * has already aborted
4653                  */
4654                 l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
4655                 break;
4656         }
4657 }
4658
4659 static void l2cap_logical_finish_create(struct l2cap_chan *chan,
4660                                         struct hci_chan *hchan)
4661 {
4662         struct l2cap_conf_rsp rsp;
4663
4664         chan->hs_hchan = hchan;
4665         chan->hs_hcon->l2cap_data = chan->conn;
4666
4667         l2cap_send_efs_conf_rsp(chan, &rsp, chan->ident, 0);
4668
4669         if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
4670                 int err;
4671
4672                 set_default_fcs(chan);
4673
4674                 err = l2cap_ertm_init(chan);
4675                 if (err < 0)
4676                         l2cap_send_disconn_req(chan, -err);
4677                 else
4678                         l2cap_chan_ready(chan);
4679         }
4680 }
4681
4682 static void l2cap_logical_finish_move(struct l2cap_chan *chan,
4683                                       struct hci_chan *hchan)
4684 {
4685         chan->hs_hcon = hchan->conn;
4686         chan->hs_hcon->l2cap_data = chan->conn;
4687
4688         BT_DBG("move_state %d", chan->move_state);
4689
4690         switch (chan->move_state) {
4691         case L2CAP_MOVE_WAIT_LOGICAL_COMP:
4692                 /* Move confirm will be sent after a success
4693                  * response is received
4694                  */
4695                 chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
4696                 break;
4697         case L2CAP_MOVE_WAIT_LOGICAL_CFM:
4698                 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
4699                         chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
4700                 } else if (chan->move_role == L2CAP_MOVE_ROLE_INITIATOR) {
4701                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM_RSP;
4702                         l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
4703                 } else if (chan->move_role == L2CAP_MOVE_ROLE_RESPONDER) {
4704                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
4705                         l2cap_send_move_chan_rsp(chan, L2CAP_MR_SUCCESS);
4706                 }
4707                 break;
4708         default:
4709                 /* Move was not in expected state, free the channel */
4710                 __release_logical_link(chan);
4711
4712                 chan->move_state = L2CAP_MOVE_STABLE;
4713         }
4714 }
4715
4716 /* Call with chan locked */
4717 void l2cap_logical_cfm(struct l2cap_chan *chan, struct hci_chan *hchan,
4718                        u8 status)
4719 {
4720         BT_DBG("chan %p, hchan %p, status %d", chan, hchan, status);
4721
4722         if (status) {
4723                 l2cap_logical_fail(chan);
4724                 __release_logical_link(chan);
4725                 return;
4726         }
4727
4728         if (chan->state != BT_CONNECTED) {
4729                 /* Ignore logical link if channel is on BR/EDR */
4730                 if (chan->local_amp_id != AMP_ID_BREDR)
4731                         l2cap_logical_finish_create(chan, hchan);
4732         } else {
4733                 l2cap_logical_finish_move(chan, hchan);
4734         }
4735 }
4736
4737 void l2cap_move_start(struct l2cap_chan *chan)
4738 {
4739         BT_DBG("chan %p", chan);
4740
4741         if (chan->local_amp_id == AMP_ID_BREDR) {
4742                 if (chan->chan_policy != BT_CHANNEL_POLICY_AMP_PREFERRED)
4743                         return;
4744                 chan->move_role = L2CAP_MOVE_ROLE_INITIATOR;
4745                 chan->move_state = L2CAP_MOVE_WAIT_PREPARE;
4746                 /* Placeholder - start physical link setup */
4747         } else {
4748                 chan->move_role = L2CAP_MOVE_ROLE_INITIATOR;
4749                 chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
4750                 chan->move_id = 0;
4751                 l2cap_move_setup(chan);
4752                 l2cap_send_move_chan_req(chan, 0);
4753         }
4754 }
4755
4756 static void l2cap_do_create(struct l2cap_chan *chan, int result,
4757                             u8 local_amp_id, u8 remote_amp_id)
4758 {
4759         BT_DBG("chan %p state %s %u -> %u", chan, state_to_string(chan->state),
4760                local_amp_id, remote_amp_id);
4761
4762         chan->fcs = L2CAP_FCS_NONE;
4763
4764         /* Outgoing channel on AMP */
4765         if (chan->state == BT_CONNECT) {
4766                 if (result == L2CAP_CR_SUCCESS) {
4767                         chan->local_amp_id = local_amp_id;
4768                         l2cap_send_create_chan_req(chan, remote_amp_id);
4769                 } else {
4770                         /* Revert to BR/EDR connect */
4771                         l2cap_send_conn_req(chan);
4772                 }
4773
4774                 return;
4775         }
4776
4777         /* Incoming channel on AMP */
4778         if (__l2cap_no_conn_pending(chan)) {
4779                 struct l2cap_conn_rsp rsp;
4780                 char buf[128];
4781                 rsp.scid = cpu_to_le16(chan->dcid);
4782                 rsp.dcid = cpu_to_le16(chan->scid);
4783
4784                 if (result == L2CAP_CR_SUCCESS) {
4785                         /* Send successful response */
4786                         rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
4787                         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
4788                 } else {
4789                         /* Send negative response */
4790                         rsp.result = cpu_to_le16(L2CAP_CR_NO_MEM);
4791                         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
4792                 }
4793
4794                 l2cap_send_cmd(chan->conn, chan->ident, L2CAP_CREATE_CHAN_RSP,
4795                                sizeof(rsp), &rsp);
4796
4797                 if (result == L2CAP_CR_SUCCESS) {
4798                         l2cap_state_change(chan, BT_CONFIG);
4799                         set_bit(CONF_REQ_SENT, &chan->conf_state);
4800                         l2cap_send_cmd(chan->conn, l2cap_get_ident(chan->conn),
4801                                        L2CAP_CONF_REQ,
4802                                        l2cap_build_conf_req(chan, buf, sizeof(buf)), buf);
4803                         chan->num_conf_req++;
4804                 }
4805         }
4806 }
4807
4808 static void l2cap_do_move_initiate(struct l2cap_chan *chan, u8 local_amp_id,
4809                                    u8 remote_amp_id)
4810 {
4811         l2cap_move_setup(chan);
4812         chan->move_id = local_amp_id;
4813         chan->move_state = L2CAP_MOVE_WAIT_RSP;
4814
4815         l2cap_send_move_chan_req(chan, remote_amp_id);
4816 }
4817
4818 static void l2cap_do_move_respond(struct l2cap_chan *chan, int result)
4819 {
4820         struct hci_chan *hchan = NULL;
4821
4822         /* Placeholder - get hci_chan for logical link */
4823
4824         if (hchan) {
4825                 if (hchan->state == BT_CONNECTED) {
4826                         /* Logical link is ready to go */
4827                         chan->hs_hcon = hchan->conn;
4828                         chan->hs_hcon->l2cap_data = chan->conn;
4829                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
4830                         l2cap_send_move_chan_rsp(chan, L2CAP_MR_SUCCESS);
4831
4832                         l2cap_logical_cfm(chan, hchan, L2CAP_MR_SUCCESS);
4833                 } else {
4834                         /* Wait for logical link to be ready */
4835                         chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
4836                 }
4837         } else {
4838                 /* Logical link not available */
4839                 l2cap_send_move_chan_rsp(chan, L2CAP_MR_NOT_ALLOWED);
4840         }
4841 }
4842
4843 static void l2cap_do_move_cancel(struct l2cap_chan *chan, int result)
4844 {
4845         if (chan->move_role == L2CAP_MOVE_ROLE_RESPONDER) {
4846                 u8 rsp_result;
4847                 if (result == -EINVAL)
4848                         rsp_result = L2CAP_MR_BAD_ID;
4849                 else
4850                         rsp_result = L2CAP_MR_NOT_ALLOWED;
4851
4852                 l2cap_send_move_chan_rsp(chan, rsp_result);
4853         }
4854
4855         chan->move_role = L2CAP_MOVE_ROLE_NONE;
4856         chan->move_state = L2CAP_MOVE_STABLE;
4857
4858         /* Restart data transmission */
4859         l2cap_ertm_send(chan);
4860 }
4861
4862 /* Invoke with locked chan */
4863 void __l2cap_physical_cfm(struct l2cap_chan *chan, int result)
4864 {
4865         u8 local_amp_id = chan->local_amp_id;
4866         u8 remote_amp_id = chan->remote_amp_id;
4867
4868         BT_DBG("chan %p, result %d, local_amp_id %d, remote_amp_id %d",
4869                chan, result, local_amp_id, remote_amp_id);
4870
4871         if (chan->state == BT_DISCONN || chan->state == BT_CLOSED) {
4872                 l2cap_chan_unlock(chan);
4873                 return;
4874         }
4875
4876         if (chan->state != BT_CONNECTED) {
4877                 l2cap_do_create(chan, result, local_amp_id, remote_amp_id);
4878         } else if (result != L2CAP_MR_SUCCESS) {
4879                 l2cap_do_move_cancel(chan, result);
4880         } else {
4881                 switch (chan->move_role) {
4882                 case L2CAP_MOVE_ROLE_INITIATOR:
4883                         l2cap_do_move_initiate(chan, local_amp_id,
4884                                                remote_amp_id);
4885                         break;
4886                 case L2CAP_MOVE_ROLE_RESPONDER:
4887                         l2cap_do_move_respond(chan, result);
4888                         break;
4889                 default:
4890                         l2cap_do_move_cancel(chan, result);
4891                         break;
4892                 }
4893         }
4894 }
4895
4896 static inline int l2cap_move_channel_req(struct l2cap_conn *conn,
4897                                          struct l2cap_cmd_hdr *cmd,
4898                                          u16 cmd_len, void *data)
4899 {
4900         struct l2cap_move_chan_req *req = data;
4901         struct l2cap_move_chan_rsp rsp;
4902         struct l2cap_chan *chan;
4903         u16 icid = 0;
4904         u16 result = L2CAP_MR_NOT_ALLOWED;
4905
4906         if (cmd_len != sizeof(*req))
4907                 return -EPROTO;
4908
4909         icid = le16_to_cpu(req->icid);
4910
4911         BT_DBG("icid 0x%4.4x, dest_amp_id %d", icid, req->dest_amp_id);
4912
4913         if (!(conn->local_fixed_chan & L2CAP_FC_A2MP))
4914                 return -EINVAL;
4915
4916         chan = l2cap_get_chan_by_dcid(conn, icid);
4917         if (!chan) {
4918                 rsp.icid = cpu_to_le16(icid);
4919                 rsp.result = cpu_to_le16(L2CAP_MR_NOT_ALLOWED);
4920                 l2cap_send_cmd(conn, cmd->ident, L2CAP_MOVE_CHAN_RSP,
4921                                sizeof(rsp), &rsp);
4922                 return 0;
4923         }
4924
4925         chan->ident = cmd->ident;
4926
4927         if (chan->scid < L2CAP_CID_DYN_START ||
4928             chan->chan_policy == BT_CHANNEL_POLICY_BREDR_ONLY ||
4929             (chan->mode != L2CAP_MODE_ERTM &&
4930              chan->mode != L2CAP_MODE_STREAMING)) {
4931                 result = L2CAP_MR_NOT_ALLOWED;
4932                 goto send_move_response;
4933         }
4934
4935         if (chan->local_amp_id == req->dest_amp_id) {
4936                 result = L2CAP_MR_SAME_ID;
4937                 goto send_move_response;
4938         }
4939
4940         if (req->dest_amp_id != AMP_ID_BREDR) {
4941                 struct hci_dev *hdev;
4942                 hdev = hci_dev_get(req->dest_amp_id);
4943                 if (!hdev || hdev->dev_type != HCI_AMP ||
4944                     !test_bit(HCI_UP, &hdev->flags)) {
4945                         if (hdev)
4946                                 hci_dev_put(hdev);
4947
4948                         result = L2CAP_MR_BAD_ID;
4949                         goto send_move_response;
4950                 }
4951                 hci_dev_put(hdev);
4952         }
4953
4954         /* Detect a move collision.  Only send a collision response
4955          * if this side has "lost", otherwise proceed with the move.
4956          * The winner has the larger bd_addr.
4957          */
4958         if ((__chan_is_moving(chan) ||
4959              chan->move_role != L2CAP_MOVE_ROLE_NONE) &&
4960             bacmp(&conn->hcon->src, &conn->hcon->dst) > 0) {
4961                 result = L2CAP_MR_COLLISION;
4962                 goto send_move_response;
4963         }
4964
4965         chan->move_role = L2CAP_MOVE_ROLE_RESPONDER;
4966         l2cap_move_setup(chan);
4967         chan->move_id = req->dest_amp_id;
4968         icid = chan->dcid;
4969
4970         if (req->dest_amp_id == AMP_ID_BREDR) {
4971                 /* Moving to BR/EDR */
4972                 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
4973                         chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
4974                         result = L2CAP_MR_PEND;
4975                 } else {
4976                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
4977                         result = L2CAP_MR_SUCCESS;
4978                 }
4979         } else {
4980                 chan->move_state = L2CAP_MOVE_WAIT_PREPARE;
4981                 /* Placeholder - uncomment when amp functions are available */
4982                 /*amp_accept_physical(chan, req->dest_amp_id);*/
4983                 result = L2CAP_MR_PEND;
4984         }
4985
4986 send_move_response:
4987         l2cap_send_move_chan_rsp(chan, result);
4988
4989         l2cap_chan_unlock(chan);
4990
4991         return 0;
4992 }
4993
4994 static void l2cap_move_continue(struct l2cap_conn *conn, u16 icid, u16 result)
4995 {
4996         struct l2cap_chan *chan;
4997         struct hci_chan *hchan = NULL;
4998
4999         chan = l2cap_get_chan_by_scid(conn, icid);
5000         if (!chan) {
5001                 l2cap_send_move_chan_cfm_icid(conn, icid);
5002                 return;
5003         }
5004
5005         __clear_chan_timer(chan);
5006         if (result == L2CAP_MR_PEND)
5007                 __set_chan_timer(chan, L2CAP_MOVE_ERTX_TIMEOUT);
5008
5009         switch (chan->move_state) {
5010         case L2CAP_MOVE_WAIT_LOGICAL_COMP:
5011                 /* Move confirm will be sent when logical link
5012                  * is complete.
5013                  */
5014                 chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
5015                 break;
5016         case L2CAP_MOVE_WAIT_RSP_SUCCESS:
5017                 if (result == L2CAP_MR_PEND) {
5018                         break;
5019                 } else if (test_bit(CONN_LOCAL_BUSY,
5020                                     &chan->conn_state)) {
5021                         chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
5022                 } else {
5023                         /* Logical link is up or moving to BR/EDR,
5024                          * proceed with move
5025                          */
5026                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM_RSP;
5027                         l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
5028                 }
5029                 break;
5030         case L2CAP_MOVE_WAIT_RSP:
5031                 /* Moving to AMP */
5032                 if (result == L2CAP_MR_SUCCESS) {
5033                         /* Remote is ready, send confirm immediately
5034                          * after logical link is ready
5035                          */
5036                         chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
5037                 } else {
5038                         /* Both logical link and move success
5039                          * are required to confirm
5040                          */
5041                         chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_COMP;
5042                 }
5043
5044                 /* Placeholder - get hci_chan for logical link */
5045                 if (!hchan) {
5046                         /* Logical link not available */
5047                         l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
5048                         break;
5049                 }
5050
5051                 /* If the logical link is not yet connected, do not
5052                  * send confirmation.
5053                  */
5054                 if (hchan->state != BT_CONNECTED)
5055                         break;
5056
5057                 /* Logical link is already ready to go */
5058
5059                 chan->hs_hcon = hchan->conn;
5060                 chan->hs_hcon->l2cap_data = chan->conn;
5061
5062                 if (result == L2CAP_MR_SUCCESS) {
5063                         /* Can confirm now */
5064                         l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
5065                 } else {
5066                         /* Now only need move success
5067                          * to confirm
5068                          */
5069                         chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
5070                 }
5071
5072                 l2cap_logical_cfm(chan, hchan, L2CAP_MR_SUCCESS);
5073                 break;
5074         default:
5075                 /* Any other amp move state means the move failed. */
5076                 chan->move_id = chan->local_amp_id;
5077                 l2cap_move_done(chan);
5078                 l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
5079         }
5080
5081         l2cap_chan_unlock(chan);
5082 }
5083
5084 static void l2cap_move_fail(struct l2cap_conn *conn, u8 ident, u16 icid,
5085                             u16 result)
5086 {
5087         struct l2cap_chan *chan;
5088
5089         chan = l2cap_get_chan_by_ident(conn, ident);
5090         if (!chan) {
5091                 /* Could not locate channel, icid is best guess */
5092                 l2cap_send_move_chan_cfm_icid(conn, icid);
5093                 return;
5094         }
5095
5096         __clear_chan_timer(chan);
5097
5098         if (chan->move_role == L2CAP_MOVE_ROLE_INITIATOR) {
5099                 if (result == L2CAP_MR_COLLISION) {
5100                         chan->move_role = L2CAP_MOVE_ROLE_RESPONDER;
5101                 } else {
5102                         /* Cleanup - cancel move */
5103                         chan->move_id = chan->local_amp_id;
5104                         l2cap_move_done(chan);
5105                 }
5106         }
5107
5108         l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
5109
5110         l2cap_chan_unlock(chan);
5111 }
5112
5113 static int l2cap_move_channel_rsp(struct l2cap_conn *conn,
5114                                   struct l2cap_cmd_hdr *cmd,
5115                                   u16 cmd_len, void *data)
5116 {
5117         struct l2cap_move_chan_rsp *rsp = data;
5118         u16 icid, result;
5119
5120         if (cmd_len != sizeof(*rsp))
5121                 return -EPROTO;
5122
5123         icid = le16_to_cpu(rsp->icid);
5124         result = le16_to_cpu(rsp->result);
5125
5126         BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result);
5127
5128         if (result == L2CAP_MR_SUCCESS || result == L2CAP_MR_PEND)
5129                 l2cap_move_continue(conn, icid, result);
5130         else
5131                 l2cap_move_fail(conn, cmd->ident, icid, result);
5132
5133         return 0;
5134 }
5135
5136 static int l2cap_move_channel_confirm(struct l2cap_conn *conn,
5137                                       struct l2cap_cmd_hdr *cmd,
5138                                       u16 cmd_len, void *data)
5139 {
5140         struct l2cap_move_chan_cfm *cfm = data;
5141         struct l2cap_chan *chan;
5142         u16 icid, result;
5143
5144         if (cmd_len != sizeof(*cfm))
5145                 return -EPROTO;
5146
5147         icid = le16_to_cpu(cfm->icid);
5148         result = le16_to_cpu(cfm->result);
5149
5150         BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result);
5151
5152         chan = l2cap_get_chan_by_dcid(conn, icid);
5153         if (!chan) {
5154                 /* Spec requires a response even if the icid was not found */
5155                 l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid);
5156                 return 0;
5157         }
5158
5159         if (chan->move_state == L2CAP_MOVE_WAIT_CONFIRM) {
5160                 if (result == L2CAP_MC_CONFIRMED) {
5161                         chan->local_amp_id = chan->move_id;
5162                         if (chan->local_amp_id == AMP_ID_BREDR)
5163                                 __release_logical_link(chan);
5164                 } else {
5165                         chan->move_id = chan->local_amp_id;
5166                 }
5167
5168                 l2cap_move_done(chan);
5169         }
5170
5171         l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid);
5172
5173         l2cap_chan_unlock(chan);
5174
5175         return 0;
5176 }
5177
5178 static inline int l2cap_move_channel_confirm_rsp(struct l2cap_conn *conn,
5179                                                  struct l2cap_cmd_hdr *cmd,
5180                                                  u16 cmd_len, void *data)
5181 {
5182         struct l2cap_move_chan_cfm_rsp *rsp = data;
5183         struct l2cap_chan *chan;
5184         u16 icid;
5185
5186         if (cmd_len != sizeof(*rsp))
5187                 return -EPROTO;
5188
5189         icid = le16_to_cpu(rsp->icid);
5190
5191         BT_DBG("icid 0x%4.4x", icid);
5192
5193         chan = l2cap_get_chan_by_scid(conn, icid);
5194         if (!chan)
5195                 return 0;
5196
5197         __clear_chan_timer(chan);
5198
5199         if (chan->move_state == L2CAP_MOVE_WAIT_CONFIRM_RSP) {
5200                 chan->local_amp_id = chan->move_id;
5201
5202                 if (chan->local_amp_id == AMP_ID_BREDR && chan->hs_hchan)
5203                         __release_logical_link(chan);
5204
5205                 l2cap_move_done(chan);
5206         }
5207
5208         l2cap_chan_unlock(chan);
5209
5210         return 0;
5211 }
5212
5213 static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
5214                                               struct l2cap_cmd_hdr *cmd,
5215                                               u16 cmd_len, u8 *data)
5216 {
5217         struct hci_conn *hcon = conn->hcon;
5218         struct l2cap_conn_param_update_req *req;
5219         struct l2cap_conn_param_update_rsp rsp;
5220         u16 min, max, latency, to_multiplier;
5221         int err;
5222
5223         if (hcon->role != HCI_ROLE_MASTER)
5224                 return -EINVAL;
5225
5226         if (cmd_len != sizeof(struct l2cap_conn_param_update_req))
5227                 return -EPROTO;
5228
5229         req = (struct l2cap_conn_param_update_req *) data;
5230         min             = __le16_to_cpu(req->min);
5231         max             = __le16_to_cpu(req->max);
5232         latency         = __le16_to_cpu(req->latency);
5233         to_multiplier   = __le16_to_cpu(req->to_multiplier);
5234
5235         BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x",
5236                min, max, latency, to_multiplier);
5237
5238         memset(&rsp, 0, sizeof(rsp));
5239
5240         err = hci_check_conn_params(min, max, latency, to_multiplier);
5241         if (err)
5242                 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_REJECTED);
5243         else
5244                 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED);
5245
5246         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP,
5247                        sizeof(rsp), &rsp);
5248
5249         if (!err) {
5250                 u8 store_hint;
5251
5252                 store_hint = hci_le_conn_update(hcon, min, max, latency,
5253                                                 to_multiplier);
5254                 mgmt_new_conn_param(hcon->hdev, &hcon->dst, hcon->dst_type,
5255                                     store_hint, min, max, latency,
5256                                     to_multiplier);
5257
5258         }
5259
5260         return 0;
5261 }
5262
5263 static int l2cap_le_connect_rsp(struct l2cap_conn *conn,
5264                                 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5265                                 u8 *data)
5266 {
5267         struct l2cap_le_conn_rsp *rsp = (struct l2cap_le_conn_rsp *) data;
5268         struct hci_conn *hcon = conn->hcon;
5269         u16 dcid, mtu, mps, credits, result;
5270         struct l2cap_chan *chan;
5271         int err, sec_level;
5272
5273         if (cmd_len < sizeof(*rsp))
5274                 return -EPROTO;
5275
5276         dcid    = __le16_to_cpu(rsp->dcid);
5277         mtu     = __le16_to_cpu(rsp->mtu);
5278         mps     = __le16_to_cpu(rsp->mps);
5279         credits = __le16_to_cpu(rsp->credits);
5280         result  = __le16_to_cpu(rsp->result);
5281
5282         if (result == L2CAP_CR_SUCCESS && (mtu < 23 || mps < 23 ||
5283                                            dcid < L2CAP_CID_DYN_START ||
5284                                            dcid > L2CAP_CID_LE_DYN_END))
5285                 return -EPROTO;
5286
5287         BT_DBG("dcid 0x%4.4x mtu %u mps %u credits %u result 0x%2.2x",
5288                dcid, mtu, mps, credits, result);
5289
5290         mutex_lock(&conn->chan_lock);
5291
5292         chan = __l2cap_get_chan_by_ident(conn, cmd->ident);
5293         if (!chan) {
5294                 err = -EBADSLT;
5295                 goto unlock;
5296         }
5297
5298         err = 0;
5299
5300         l2cap_chan_lock(chan);
5301
5302         switch (result) {
5303         case L2CAP_CR_SUCCESS:
5304                 if (__l2cap_get_chan_by_dcid(conn, dcid)) {
5305                         err = -EBADSLT;
5306                         break;
5307                 }
5308
5309                 chan->ident = 0;
5310                 chan->dcid = dcid;
5311                 chan->omtu = mtu;
5312                 chan->remote_mps = mps;
5313                 chan->tx_credits = credits;
5314                 l2cap_chan_ready(chan);
5315                 break;
5316
5317         case L2CAP_CR_AUTHENTICATION:
5318         case L2CAP_CR_ENCRYPTION:
5319                 /* If we already have MITM protection we can't do
5320                  * anything.
5321                  */
5322                 if (hcon->sec_level > BT_SECURITY_MEDIUM) {
5323                         l2cap_chan_del(chan, ECONNREFUSED);
5324                         break;
5325                 }
5326
5327                 sec_level = hcon->sec_level + 1;
5328                 if (chan->sec_level < sec_level)
5329                         chan->sec_level = sec_level;
5330
5331                 /* We'll need to send a new Connect Request */
5332                 clear_bit(FLAG_LE_CONN_REQ_SENT, &chan->flags);
5333
5334                 smp_conn_security(hcon, chan->sec_level);
5335                 break;
5336
5337         default:
5338                 l2cap_chan_del(chan, ECONNREFUSED);
5339                 break;
5340         }
5341
5342         l2cap_chan_unlock(chan);
5343
5344 unlock:
5345         mutex_unlock(&conn->chan_lock);
5346
5347         return err;
5348 }
5349
5350 static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
5351                                       struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5352                                       u8 *data)
5353 {
5354         int err = 0;
5355
5356         switch (cmd->code) {
5357         case L2CAP_COMMAND_REJ:
5358                 l2cap_command_rej(conn, cmd, cmd_len, data);
5359                 break;
5360
5361         case L2CAP_CONN_REQ:
5362                 err = l2cap_connect_req(conn, cmd, cmd_len, data);
5363                 break;
5364
5365         case L2CAP_CONN_RSP:
5366         case L2CAP_CREATE_CHAN_RSP:
5367                 l2cap_connect_create_rsp(conn, cmd, cmd_len, data);
5368                 break;
5369
5370         case L2CAP_CONF_REQ:
5371                 err = l2cap_config_req(conn, cmd, cmd_len, data);
5372                 break;
5373
5374         case L2CAP_CONF_RSP:
5375                 l2cap_config_rsp(conn, cmd, cmd_len, data);
5376                 break;
5377
5378         case L2CAP_DISCONN_REQ:
5379                 err = l2cap_disconnect_req(conn, cmd, cmd_len, data);
5380                 break;
5381
5382         case L2CAP_DISCONN_RSP:
5383                 l2cap_disconnect_rsp(conn, cmd, cmd_len, data);
5384                 break;
5385
5386         case L2CAP_ECHO_REQ:
5387                 l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data);
5388                 break;
5389
5390         case L2CAP_ECHO_RSP:
5391                 break;
5392
5393         case L2CAP_INFO_REQ:
5394                 err = l2cap_information_req(conn, cmd, cmd_len, data);
5395                 break;
5396
5397         case L2CAP_INFO_RSP:
5398                 l2cap_information_rsp(conn, cmd, cmd_len, data);
5399                 break;
5400
5401         case L2CAP_CREATE_CHAN_REQ:
5402                 err = l2cap_create_channel_req(conn, cmd, cmd_len, data);
5403                 break;
5404
5405         case L2CAP_MOVE_CHAN_REQ:
5406                 err = l2cap_move_channel_req(conn, cmd, cmd_len, data);
5407                 break;
5408
5409         case L2CAP_MOVE_CHAN_RSP:
5410                 l2cap_move_channel_rsp(conn, cmd, cmd_len, data);
5411                 break;
5412
5413         case L2CAP_MOVE_CHAN_CFM:
5414                 err = l2cap_move_channel_confirm(conn, cmd, cmd_len, data);
5415                 break;
5416
5417         case L2CAP_MOVE_CHAN_CFM_RSP:
5418                 l2cap_move_channel_confirm_rsp(conn, cmd, cmd_len, data);
5419                 break;
5420
5421         default:
5422                 BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code);
5423                 err = -EINVAL;
5424                 break;
5425         }
5426
5427         return err;
5428 }
5429
5430 static int l2cap_le_connect_req(struct l2cap_conn *conn,
5431                                 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5432                                 u8 *data)
5433 {
5434         struct l2cap_le_conn_req *req = (struct l2cap_le_conn_req *) data;
5435         struct l2cap_le_conn_rsp rsp;
5436         struct l2cap_chan *chan, *pchan;
5437         u16 dcid, scid, credits, mtu, mps;
5438         __le16 psm;
5439         u8 result;
5440
5441         if (cmd_len != sizeof(*req))
5442                 return -EPROTO;
5443
5444         scid = __le16_to_cpu(req->scid);
5445         mtu  = __le16_to_cpu(req->mtu);
5446         mps  = __le16_to_cpu(req->mps);
5447         psm  = req->psm;
5448         dcid = 0;
5449         credits = 0;
5450
5451         if (mtu < 23 || mps < 23)
5452                 return -EPROTO;
5453
5454         BT_DBG("psm 0x%2.2x scid 0x%4.4x mtu %u mps %u", __le16_to_cpu(psm),
5455                scid, mtu, mps);
5456
5457         /* Check if we have socket listening on psm */
5458         pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, &conn->hcon->src,
5459                                          &conn->hcon->dst, LE_LINK);
5460         if (!pchan) {
5461                 result = L2CAP_CR_BAD_PSM;
5462                 chan = NULL;
5463                 goto response;
5464         }
5465
5466         mutex_lock(&conn->chan_lock);
5467         l2cap_chan_lock(pchan);
5468
5469         if (!smp_sufficient_security(conn->hcon, pchan->sec_level,
5470                                      SMP_ALLOW_STK)) {
5471                 result = L2CAP_CR_AUTHENTICATION;
5472                 chan = NULL;
5473                 goto response_unlock;
5474         }
5475
5476         /* Check for valid dynamic CID range */
5477         if (scid < L2CAP_CID_DYN_START || scid > L2CAP_CID_LE_DYN_END) {
5478                 result = L2CAP_CR_INVALID_SCID;
5479                 chan = NULL;
5480                 goto response_unlock;
5481         }
5482
5483         /* Check if we already have channel with that dcid */
5484         if (__l2cap_get_chan_by_dcid(conn, scid)) {
5485                 result = L2CAP_CR_SCID_IN_USE;
5486                 chan = NULL;
5487                 goto response_unlock;
5488         }
5489
5490         chan = pchan->ops->new_connection(pchan);
5491         if (!chan) {
5492                 result = L2CAP_CR_NO_MEM;
5493                 goto response_unlock;
5494         }
5495
5496         l2cap_le_flowctl_init(chan);
5497
5498         bacpy(&chan->src, &conn->hcon->src);
5499         bacpy(&chan->dst, &conn->hcon->dst);
5500         chan->src_type = bdaddr_src_type(conn->hcon);
5501         chan->dst_type = bdaddr_dst_type(conn->hcon);
5502         chan->psm  = psm;
5503         chan->dcid = scid;
5504         chan->omtu = mtu;
5505         chan->remote_mps = mps;
5506         chan->tx_credits = __le16_to_cpu(req->credits);
5507
5508         __l2cap_chan_add(conn, chan);
5509         dcid = chan->scid;
5510         credits = chan->rx_credits;
5511
5512         __set_chan_timer(chan, chan->ops->get_sndtimeo(chan));
5513
5514         chan->ident = cmd->ident;
5515
5516         if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
5517                 l2cap_state_change(chan, BT_CONNECT2);
5518                 /* The following result value is actually not defined
5519                  * for LE CoC but we use it to let the function know
5520                  * that it should bail out after doing its cleanup
5521                  * instead of sending a response.
5522                  */
5523                 result = L2CAP_CR_PEND;
5524                 chan->ops->defer(chan);
5525         } else {
5526                 l2cap_chan_ready(chan);
5527                 result = L2CAP_CR_SUCCESS;
5528         }
5529
5530 response_unlock:
5531         l2cap_chan_unlock(pchan);
5532         mutex_unlock(&conn->chan_lock);
5533         l2cap_chan_put(pchan);
5534
5535         if (result == L2CAP_CR_PEND)
5536                 return 0;
5537
5538 response:
5539         if (chan) {
5540                 rsp.mtu = cpu_to_le16(chan->imtu);
5541                 rsp.mps = cpu_to_le16(chan->mps);
5542         } else {
5543                 rsp.mtu = 0;
5544                 rsp.mps = 0;
5545         }
5546
5547         rsp.dcid    = cpu_to_le16(dcid);
5548         rsp.credits = cpu_to_le16(credits);
5549         rsp.result  = cpu_to_le16(result);
5550
5551         l2cap_send_cmd(conn, cmd->ident, L2CAP_LE_CONN_RSP, sizeof(rsp), &rsp);
5552
5553         return 0;
5554 }
5555
5556 static inline int l2cap_le_credits(struct l2cap_conn *conn,
5557                                    struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5558                                    u8 *data)
5559 {
5560         struct l2cap_le_credits *pkt;
5561         struct l2cap_chan *chan;
5562         u16 cid, credits, max_credits;
5563
5564         if (cmd_len != sizeof(*pkt))
5565                 return -EPROTO;
5566
5567         pkt = (struct l2cap_le_credits *) data;
5568         cid     = __le16_to_cpu(pkt->cid);
5569         credits = __le16_to_cpu(pkt->credits);
5570
5571         BT_DBG("cid 0x%4.4x credits 0x%4.4x", cid, credits);
5572
5573         chan = l2cap_get_chan_by_dcid(conn, cid);
5574         if (!chan)
5575                 return -EBADSLT;
5576
5577         max_credits = LE_FLOWCTL_MAX_CREDITS - chan->tx_credits;
5578         if (credits > max_credits) {
5579                 BT_ERR("LE credits overflow");
5580                 l2cap_send_disconn_req(chan, ECONNRESET);
5581                 l2cap_chan_unlock(chan);
5582
5583                 /* Return 0 so that we don't trigger an unnecessary
5584                  * command reject packet.
5585                  */
5586                 return 0;
5587         }
5588
5589         chan->tx_credits += credits;
5590
5591         /* Resume sending */
5592         l2cap_le_flowctl_send(chan);
5593
5594         if (chan->tx_credits)
5595                 chan->ops->resume(chan);
5596
5597         l2cap_chan_unlock(chan);
5598
5599         return 0;
5600 }
5601
5602 static inline int l2cap_le_command_rej(struct l2cap_conn *conn,
5603                                        struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5604                                        u8 *data)
5605 {
5606         struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data;
5607         struct l2cap_chan *chan;
5608
5609         if (cmd_len < sizeof(*rej))
5610                 return -EPROTO;
5611
5612         mutex_lock(&conn->chan_lock);
5613
5614         chan = __l2cap_get_chan_by_ident(conn, cmd->ident);
5615         if (!chan)
5616                 goto done;
5617
5618         l2cap_chan_lock(chan);
5619         l2cap_chan_del(chan, ECONNREFUSED);
5620         l2cap_chan_unlock(chan);
5621
5622 done:
5623         mutex_unlock(&conn->chan_lock);
5624         return 0;
5625 }
5626
5627 static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
5628                                    struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5629                                    u8 *data)
5630 {
5631         int err = 0;
5632
5633         switch (cmd->code) {
5634         case L2CAP_COMMAND_REJ:
5635                 l2cap_le_command_rej(conn, cmd, cmd_len, data);
5636                 break;
5637
5638         case L2CAP_CONN_PARAM_UPDATE_REQ:
5639                 err = l2cap_conn_param_update_req(conn, cmd, cmd_len, data);
5640                 break;
5641
5642         case L2CAP_CONN_PARAM_UPDATE_RSP:
5643                 break;
5644
5645         case L2CAP_LE_CONN_RSP:
5646                 l2cap_le_connect_rsp(conn, cmd, cmd_len, data);
5647                 break;
5648
5649         case L2CAP_LE_CONN_REQ:
5650                 err = l2cap_le_connect_req(conn, cmd, cmd_len, data);
5651                 break;
5652
5653         case L2CAP_LE_CREDITS:
5654                 err = l2cap_le_credits(conn, cmd, cmd_len, data);
5655                 break;
5656
5657         case L2CAP_DISCONN_REQ:
5658                 err = l2cap_disconnect_req(conn, cmd, cmd_len, data);
5659                 break;
5660
5661         case L2CAP_DISCONN_RSP:
5662                 l2cap_disconnect_rsp(conn, cmd, cmd_len, data);
5663                 break;
5664
5665         default:
5666                 BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code);
5667                 err = -EINVAL;
5668                 break;
5669         }
5670
5671         return err;
5672 }
5673
5674 static inline void l2cap_le_sig_channel(struct l2cap_conn *conn,
5675                                         struct sk_buff *skb)
5676 {
5677         struct hci_conn *hcon = conn->hcon;
5678         struct l2cap_cmd_hdr *cmd;
5679         u16 len;
5680         int err;
5681
5682         if (hcon->type != LE_LINK)
5683                 goto drop;
5684
5685         if (skb->len < L2CAP_CMD_HDR_SIZE)
5686                 goto drop;
5687
5688         cmd = (void *) skb->data;
5689         skb_pull(skb, L2CAP_CMD_HDR_SIZE);
5690
5691         len = le16_to_cpu(cmd->len);
5692
5693         BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd->code, len, cmd->ident);
5694
5695         if (len != skb->len || !cmd->ident) {
5696                 BT_DBG("corrupted command");
5697                 goto drop;
5698         }
5699
5700         err = l2cap_le_sig_cmd(conn, cmd, len, skb->data);
5701         if (err) {
5702                 struct l2cap_cmd_rej_unk rej;
5703
5704                 BT_ERR("Wrong link type (%d)", err);
5705
5706                 rej.reason = cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
5707                 l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
5708                                sizeof(rej), &rej);
5709         }
5710
5711 drop:
5712         kfree_skb(skb);
5713 }
5714
5715 static inline void l2cap_sig_channel(struct l2cap_conn *conn,
5716                                      struct sk_buff *skb)
5717 {
5718         struct hci_conn *hcon = conn->hcon;
5719         u8 *data = skb->data;
5720         int len = skb->len;
5721         struct l2cap_cmd_hdr cmd;
5722         int err;
5723
5724         l2cap_raw_recv(conn, skb);
5725
5726         if (hcon->type != ACL_LINK)
5727                 goto drop;
5728
5729         while (len >= L2CAP_CMD_HDR_SIZE) {
5730                 u16 cmd_len;
5731                 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
5732                 data += L2CAP_CMD_HDR_SIZE;
5733                 len  -= L2CAP_CMD_HDR_SIZE;
5734
5735                 cmd_len = le16_to_cpu(cmd.len);
5736
5737                 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len,
5738                        cmd.ident);
5739
5740                 if (cmd_len > len || !cmd.ident) {
5741                         BT_DBG("corrupted command");
5742                         break;
5743                 }
5744
5745                 err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data);
5746                 if (err) {
5747                         struct l2cap_cmd_rej_unk rej;
5748
5749                         BT_ERR("Wrong link type (%d)", err);
5750
5751                         rej.reason = cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
5752                         l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ,
5753                                        sizeof(rej), &rej);
5754                 }
5755
5756                 data += cmd_len;
5757                 len  -= cmd_len;
5758         }
5759
5760 drop:
5761         kfree_skb(skb);
5762 }
5763
5764 static int l2cap_check_fcs(struct l2cap_chan *chan,  struct sk_buff *skb)
5765 {
5766         u16 our_fcs, rcv_fcs;
5767         int hdr_size;
5768
5769         if (test_bit(FLAG_EXT_CTRL, &chan->flags))
5770                 hdr_size = L2CAP_EXT_HDR_SIZE;
5771         else
5772                 hdr_size = L2CAP_ENH_HDR_SIZE;
5773
5774         if (chan->fcs == L2CAP_FCS_CRC16) {
5775                 skb_trim(skb, skb->len - L2CAP_FCS_SIZE);
5776                 rcv_fcs = get_unaligned_le16(skb->data + skb->len);
5777                 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
5778
5779                 if (our_fcs != rcv_fcs)
5780                         return -EBADMSG;
5781         }
5782         return 0;
5783 }
5784
5785 static void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan)
5786 {
5787         struct l2cap_ctrl control;
5788
5789         BT_DBG("chan %p", chan);
5790
5791         memset(&control, 0, sizeof(control));
5792         control.sframe = 1;
5793         control.final = 1;
5794         control.reqseq = chan->buffer_seq;
5795         set_bit(CONN_SEND_FBIT, &chan->conn_state);
5796
5797         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
5798                 control.super = L2CAP_SUPER_RNR;
5799                 l2cap_send_sframe(chan, &control);
5800         }
5801
5802         if (test_and_clear_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
5803             chan->unacked_frames > 0)
5804                 __set_retrans_timer(chan);
5805
5806         /* Send pending iframes */
5807         l2cap_ertm_send(chan);
5808
5809         if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
5810             test_bit(CONN_SEND_FBIT, &chan->conn_state)) {
5811                 /* F-bit wasn't sent in an s-frame or i-frame yet, so
5812                  * send it now.
5813                  */
5814                 control.super = L2CAP_SUPER_RR;
5815                 l2cap_send_sframe(chan, &control);
5816         }
5817 }
5818
5819 static void append_skb_frag(struct sk_buff *skb, struct sk_buff *new_frag,
5820                             struct sk_buff **last_frag)
5821 {
5822         /* skb->len reflects data in skb as well as all fragments
5823          * skb->data_len reflects only data in fragments
5824          */
5825         if (!skb_has_frag_list(skb))
5826                 skb_shinfo(skb)->frag_list = new_frag;
5827
5828         new_frag->next = NULL;
5829
5830         (*last_frag)->next = new_frag;
5831         *last_frag = new_frag;
5832
5833         skb->len += new_frag->len;
5834         skb->data_len += new_frag->len;
5835         skb->truesize += new_frag->truesize;
5836 }
5837
5838 static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb,
5839                                 struct l2cap_ctrl *control)
5840 {
5841         int err = -EINVAL;
5842
5843         switch (control->sar) {
5844         case L2CAP_SAR_UNSEGMENTED:
5845                 if (chan->sdu)
5846                         break;
5847
5848                 err = chan->ops->recv(chan, skb);
5849                 break;
5850
5851         case L2CAP_SAR_START:
5852                 if (chan->sdu)
5853                         break;
5854
5855                 if (!pskb_may_pull(skb, L2CAP_SDULEN_SIZE))
5856                         break;
5857
5858                 chan->sdu_len = get_unaligned_le16(skb->data);
5859                 skb_pull(skb, L2CAP_SDULEN_SIZE);
5860
5861                 if (chan->sdu_len > chan->imtu) {
5862                         err = -EMSGSIZE;
5863                         break;
5864                 }
5865
5866                 if (skb->len >= chan->sdu_len)
5867                         break;
5868
5869                 chan->sdu = skb;
5870                 chan->sdu_last_frag = skb;
5871
5872                 skb = NULL;
5873                 err = 0;
5874                 break;
5875
5876         case L2CAP_SAR_CONTINUE:
5877                 if (!chan->sdu)
5878                         break;
5879
5880                 append_skb_frag(chan->sdu, skb,
5881                                 &chan->sdu_last_frag);
5882                 skb = NULL;
5883
5884                 if (chan->sdu->len >= chan->sdu_len)
5885                         break;
5886
5887                 err = 0;
5888                 break;
5889
5890         case L2CAP_SAR_END:
5891                 if (!chan->sdu)
5892                         break;
5893
5894                 append_skb_frag(chan->sdu, skb,
5895                                 &chan->sdu_last_frag);
5896                 skb = NULL;
5897
5898                 if (chan->sdu->len != chan->sdu_len)
5899                         break;
5900
5901                 err = chan->ops->recv(chan, chan->sdu);
5902
5903                 if (!err) {
5904                         /* Reassembly complete */
5905                         chan->sdu = NULL;
5906                         chan->sdu_last_frag = NULL;
5907                         chan->sdu_len = 0;
5908                 }
5909                 break;
5910         }
5911
5912         if (err) {
5913                 kfree_skb(skb);
5914                 kfree_skb(chan->sdu);
5915                 chan->sdu = NULL;
5916                 chan->sdu_last_frag = NULL;
5917                 chan->sdu_len = 0;
5918         }
5919
5920         return err;
5921 }
5922
5923 static int l2cap_resegment(struct l2cap_chan *chan)
5924 {
5925         /* Placeholder */
5926         return 0;
5927 }
5928
5929 void l2cap_chan_busy(struct l2cap_chan *chan, int busy)
5930 {
5931         u8 event;
5932
5933         if (chan->mode != L2CAP_MODE_ERTM)
5934                 return;
5935
5936         event = busy ? L2CAP_EV_LOCAL_BUSY_DETECTED : L2CAP_EV_LOCAL_BUSY_CLEAR;
5937         l2cap_tx(chan, NULL, NULL, event);
5938 }
5939
5940 static int l2cap_rx_queued_iframes(struct l2cap_chan *chan)
5941 {
5942         int err = 0;
5943         /* Pass sequential frames to l2cap_reassemble_sdu()
5944          * until a gap is encountered.
5945          */
5946
5947         BT_DBG("chan %p", chan);
5948
5949         while (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
5950                 struct sk_buff *skb;
5951                 BT_DBG("Searching for skb with txseq %d (queue len %d)",
5952                        chan->buffer_seq, skb_queue_len(&chan->srej_q));
5953
5954                 skb = l2cap_ertm_seq_in_queue(&chan->srej_q, chan->buffer_seq);
5955
5956                 if (!skb)
5957                         break;
5958
5959                 skb_unlink(skb, &chan->srej_q);
5960                 chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
5961                 err = l2cap_reassemble_sdu(chan, skb, &bt_cb(skb)->l2cap);
5962                 if (err)
5963                         break;
5964         }
5965
5966         if (skb_queue_empty(&chan->srej_q)) {
5967                 chan->rx_state = L2CAP_RX_STATE_RECV;
5968                 l2cap_send_ack(chan);
5969         }
5970
5971         return err;
5972 }
5973
5974 static void l2cap_handle_srej(struct l2cap_chan *chan,
5975                               struct l2cap_ctrl *control)
5976 {
5977         struct sk_buff *skb;
5978
5979         BT_DBG("chan %p, control %p", chan, control);
5980
5981         if (control->reqseq == chan->next_tx_seq) {
5982                 BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq);
5983                 l2cap_send_disconn_req(chan, ECONNRESET);
5984                 return;
5985         }
5986
5987         skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq);
5988
5989         if (skb == NULL) {
5990                 BT_DBG("Seq %d not available for retransmission",
5991                        control->reqseq);
5992                 return;
5993         }
5994
5995         if (chan->max_tx != 0 && bt_cb(skb)->l2cap.retries >= chan->max_tx) {
5996                 BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
5997                 l2cap_send_disconn_req(chan, ECONNRESET);
5998                 return;
5999         }
6000
6001         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6002
6003         if (control->poll) {
6004                 l2cap_pass_to_tx(chan, control);
6005
6006                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
6007                 l2cap_retransmit(chan, control);
6008                 l2cap_ertm_send(chan);
6009
6010                 if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) {
6011                         set_bit(CONN_SREJ_ACT, &chan->conn_state);
6012                         chan->srej_save_reqseq = control->reqseq;
6013                 }
6014         } else {
6015                 l2cap_pass_to_tx_fbit(chan, control);
6016
6017                 if (control->final) {
6018                         if (chan->srej_save_reqseq != control->reqseq ||
6019                             !test_and_clear_bit(CONN_SREJ_ACT,
6020                                                 &chan->conn_state))
6021                                 l2cap_retransmit(chan, control);
6022                 } else {
6023                         l2cap_retransmit(chan, control);
6024                         if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) {
6025                                 set_bit(CONN_SREJ_ACT, &chan->conn_state);
6026                                 chan->srej_save_reqseq = control->reqseq;
6027                         }
6028                 }
6029         }
6030 }
6031
6032 static void l2cap_handle_rej(struct l2cap_chan *chan,
6033                              struct l2cap_ctrl *control)
6034 {
6035         struct sk_buff *skb;
6036
6037         BT_DBG("chan %p, control %p", chan, control);
6038
6039         if (control->reqseq == chan->next_tx_seq) {
6040                 BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq);
6041                 l2cap_send_disconn_req(chan, ECONNRESET);
6042                 return;
6043         }
6044
6045         skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq);
6046
6047         if (chan->max_tx && skb &&
6048             bt_cb(skb)->l2cap.retries >= chan->max_tx) {
6049                 BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
6050                 l2cap_send_disconn_req(chan, ECONNRESET);
6051                 return;
6052         }
6053
6054         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6055
6056         l2cap_pass_to_tx(chan, control);
6057
6058         if (control->final) {
6059                 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
6060                         l2cap_retransmit_all(chan, control);
6061         } else {
6062                 l2cap_retransmit_all(chan, control);
6063                 l2cap_ertm_send(chan);
6064                 if (chan->tx_state == L2CAP_TX_STATE_WAIT_F)
6065                         set_bit(CONN_REJ_ACT, &chan->conn_state);
6066         }
6067 }
6068
6069 static u8 l2cap_classify_txseq(struct l2cap_chan *chan, u16 txseq)
6070 {
6071         BT_DBG("chan %p, txseq %d", chan, txseq);
6072
6073         BT_DBG("last_acked_seq %d, expected_tx_seq %d", chan->last_acked_seq,
6074                chan->expected_tx_seq);
6075
6076         if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
6077                 if (__seq_offset(chan, txseq, chan->last_acked_seq) >=
6078                     chan->tx_win) {
6079                         /* See notes below regarding "double poll" and
6080                          * invalid packets.
6081                          */
6082                         if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) {
6083                                 BT_DBG("Invalid/Ignore - after SREJ");
6084                                 return L2CAP_TXSEQ_INVALID_IGNORE;
6085                         } else {
6086                                 BT_DBG("Invalid - in window after SREJ sent");
6087                                 return L2CAP_TXSEQ_INVALID;
6088                         }
6089                 }
6090
6091                 if (chan->srej_list.head == txseq) {
6092                         BT_DBG("Expected SREJ");
6093                         return L2CAP_TXSEQ_EXPECTED_SREJ;
6094                 }
6095
6096                 if (l2cap_ertm_seq_in_queue(&chan->srej_q, txseq)) {
6097                         BT_DBG("Duplicate SREJ - txseq already stored");
6098                         return L2CAP_TXSEQ_DUPLICATE_SREJ;
6099                 }
6100
6101                 if (l2cap_seq_list_contains(&chan->srej_list, txseq)) {
6102                         BT_DBG("Unexpected SREJ - not requested");
6103                         return L2CAP_TXSEQ_UNEXPECTED_SREJ;
6104                 }
6105         }
6106
6107         if (chan->expected_tx_seq == txseq) {
6108                 if (__seq_offset(chan, txseq, chan->last_acked_seq) >=
6109                     chan->tx_win) {
6110                         BT_DBG("Invalid - txseq outside tx window");
6111                         return L2CAP_TXSEQ_INVALID;
6112                 } else {
6113                         BT_DBG("Expected");
6114                         return L2CAP_TXSEQ_EXPECTED;
6115                 }
6116         }
6117
6118         if (__seq_offset(chan, txseq, chan->last_acked_seq) <
6119             __seq_offset(chan, chan->expected_tx_seq, chan->last_acked_seq)) {
6120                 BT_DBG("Duplicate - expected_tx_seq later than txseq");
6121                 return L2CAP_TXSEQ_DUPLICATE;
6122         }
6123
6124         if (__seq_offset(chan, txseq, chan->last_acked_seq) >= chan->tx_win) {
6125                 /* A source of invalid packets is a "double poll" condition,
6126                  * where delays cause us to send multiple poll packets.  If
6127                  * the remote stack receives and processes both polls,
6128                  * sequence numbers can wrap around in such a way that a
6129                  * resent frame has a sequence number that looks like new data
6130                  * with a sequence gap.  This would trigger an erroneous SREJ
6131                  * request.
6132                  *
6133                  * Fortunately, this is impossible with a tx window that's
6134                  * less than half of the maximum sequence number, which allows
6135                  * invalid frames to be safely ignored.
6136                  *
6137                  * With tx window sizes greater than half of the tx window
6138                  * maximum, the frame is invalid and cannot be ignored.  This
6139                  * causes a disconnect.
6140                  */
6141
6142                 if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) {
6143                         BT_DBG("Invalid/Ignore - txseq outside tx window");
6144                         return L2CAP_TXSEQ_INVALID_IGNORE;
6145                 } else {
6146                         BT_DBG("Invalid - txseq outside tx window");
6147                         return L2CAP_TXSEQ_INVALID;
6148                 }
6149         } else {
6150                 BT_DBG("Unexpected - txseq indicates missing frames");
6151                 return L2CAP_TXSEQ_UNEXPECTED;
6152         }
6153 }
6154
6155 static int l2cap_rx_state_recv(struct l2cap_chan *chan,
6156                                struct l2cap_ctrl *control,
6157                                struct sk_buff *skb, u8 event)
6158 {
6159         int err = 0;
6160         bool skb_in_use = false;
6161
6162         BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
6163                event);
6164
6165         switch (event) {
6166         case L2CAP_EV_RECV_IFRAME:
6167                 switch (l2cap_classify_txseq(chan, control->txseq)) {
6168                 case L2CAP_TXSEQ_EXPECTED:
6169                         l2cap_pass_to_tx(chan, control);
6170
6171                         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
6172                                 BT_DBG("Busy, discarding expected seq %d",
6173                                        control->txseq);
6174                                 break;
6175                         }
6176
6177                         chan->expected_tx_seq = __next_seq(chan,
6178                                                            control->txseq);
6179
6180                         chan->buffer_seq = chan->expected_tx_seq;
6181                         skb_in_use = true;
6182
6183                         err = l2cap_reassemble_sdu(chan, skb, control);
6184                         if (err)
6185                                 break;
6186
6187                         if (control->final) {
6188                                 if (!test_and_clear_bit(CONN_REJ_ACT,
6189                                                         &chan->conn_state)) {
6190                                         control->final = 0;
6191                                         l2cap_retransmit_all(chan, control);
6192                                         l2cap_ertm_send(chan);
6193                                 }
6194                         }
6195
6196                         if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state))
6197                                 l2cap_send_ack(chan);
6198                         break;
6199                 case L2CAP_TXSEQ_UNEXPECTED:
6200                         l2cap_pass_to_tx(chan, control);
6201
6202                         /* Can't issue SREJ frames in the local busy state.
6203                          * Drop this frame, it will be seen as missing
6204                          * when local busy is exited.
6205                          */
6206                         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
6207                                 BT_DBG("Busy, discarding unexpected seq %d",
6208                                        control->txseq);
6209                                 break;
6210                         }
6211
6212                         /* There was a gap in the sequence, so an SREJ
6213                          * must be sent for each missing frame.  The
6214                          * current frame is stored for later use.
6215                          */
6216                         skb_queue_tail(&chan->srej_q, skb);
6217                         skb_in_use = true;
6218                         BT_DBG("Queued %p (queue len %d)", skb,
6219                                skb_queue_len(&chan->srej_q));
6220
6221                         clear_bit(CONN_SREJ_ACT, &chan->conn_state);
6222                         l2cap_seq_list_clear(&chan->srej_list);
6223                         l2cap_send_srej(chan, control->txseq);
6224
6225                         chan->rx_state = L2CAP_RX_STATE_SREJ_SENT;
6226                         break;
6227                 case L2CAP_TXSEQ_DUPLICATE:
6228                         l2cap_pass_to_tx(chan, control);
6229                         break;
6230                 case L2CAP_TXSEQ_INVALID_IGNORE:
6231                         break;
6232                 case L2CAP_TXSEQ_INVALID:
6233                 default:
6234                         l2cap_send_disconn_req(chan, ECONNRESET);
6235                         break;
6236                 }
6237                 break;
6238         case L2CAP_EV_RECV_RR:
6239                 l2cap_pass_to_tx(chan, control);
6240                 if (control->final) {
6241                         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6242
6243                         if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state) &&
6244                             !__chan_is_moving(chan)) {
6245                                 control->final = 0;
6246                                 l2cap_retransmit_all(chan, control);
6247                         }
6248
6249                         l2cap_ertm_send(chan);
6250                 } else if (control->poll) {
6251                         l2cap_send_i_or_rr_or_rnr(chan);
6252                 } else {
6253                         if (test_and_clear_bit(CONN_REMOTE_BUSY,
6254                                                &chan->conn_state) &&
6255                             chan->unacked_frames)
6256                                 __set_retrans_timer(chan);
6257
6258                         l2cap_ertm_send(chan);
6259                 }
6260                 break;
6261         case L2CAP_EV_RECV_RNR:
6262                 set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6263                 l2cap_pass_to_tx(chan, control);
6264                 if (control && control->poll) {
6265                         set_bit(CONN_SEND_FBIT, &chan->conn_state);
6266                         l2cap_send_rr_or_rnr(chan, 0);
6267                 }
6268                 __clear_retrans_timer(chan);
6269                 l2cap_seq_list_clear(&chan->retrans_list);
6270                 break;
6271         case L2CAP_EV_RECV_REJ:
6272                 l2cap_handle_rej(chan, control);
6273                 break;
6274         case L2CAP_EV_RECV_SREJ:
6275                 l2cap_handle_srej(chan, control);
6276                 break;
6277         default:
6278                 break;
6279         }
6280
6281         if (skb && !skb_in_use) {
6282                 BT_DBG("Freeing %p", skb);
6283                 kfree_skb(skb);
6284         }
6285
6286         return err;
6287 }
6288
6289 static int l2cap_rx_state_srej_sent(struct l2cap_chan *chan,
6290                                     struct l2cap_ctrl *control,
6291                                     struct sk_buff *skb, u8 event)
6292 {
6293         int err = 0;
6294         u16 txseq = control->txseq;
6295         bool skb_in_use = false;
6296
6297         BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
6298                event);
6299
6300         switch (event) {
6301         case L2CAP_EV_RECV_IFRAME:
6302                 switch (l2cap_classify_txseq(chan, txseq)) {
6303                 case L2CAP_TXSEQ_EXPECTED:
6304                         /* Keep frame for reassembly later */
6305                         l2cap_pass_to_tx(chan, control);
6306                         skb_queue_tail(&chan->srej_q, skb);
6307                         skb_in_use = true;
6308                         BT_DBG("Queued %p (queue len %d)", skb,
6309                                skb_queue_len(&chan->srej_q));
6310
6311                         chan->expected_tx_seq = __next_seq(chan, txseq);
6312                         break;
6313                 case L2CAP_TXSEQ_EXPECTED_SREJ:
6314                         l2cap_seq_list_pop(&chan->srej_list);
6315
6316                         l2cap_pass_to_tx(chan, control);
6317                         skb_queue_tail(&chan->srej_q, skb);
6318                         skb_in_use = true;
6319                         BT_DBG("Queued %p (queue len %d)", skb,
6320                                skb_queue_len(&chan->srej_q));
6321
6322                         err = l2cap_rx_queued_iframes(chan);
6323                         if (err)
6324                                 break;
6325
6326                         break;
6327                 case L2CAP_TXSEQ_UNEXPECTED:
6328                         /* Got a frame that can't be reassembled yet.
6329                          * Save it for later, and send SREJs to cover
6330                          * the missing frames.
6331                          */
6332                         skb_queue_tail(&chan->srej_q, skb);
6333                         skb_in_use = true;
6334                         BT_DBG("Queued %p (queue len %d)", skb,
6335                                skb_queue_len(&chan->srej_q));
6336
6337                         l2cap_pass_to_tx(chan, control);
6338                         l2cap_send_srej(chan, control->txseq);
6339                         break;
6340                 case L2CAP_TXSEQ_UNEXPECTED_SREJ:
6341                         /* This frame was requested with an SREJ, but
6342                          * some expected retransmitted frames are
6343                          * missing.  Request retransmission of missing
6344                          * SREJ'd frames.
6345                          */
6346                         skb_queue_tail(&chan->srej_q, skb);
6347                         skb_in_use = true;
6348                         BT_DBG("Queued %p (queue len %d)", skb,
6349                                skb_queue_len(&chan->srej_q));
6350
6351                         l2cap_pass_to_tx(chan, control);
6352                         l2cap_send_srej_list(chan, control->txseq);
6353                         break;
6354                 case L2CAP_TXSEQ_DUPLICATE_SREJ:
6355                         /* We've already queued this frame.  Drop this copy. */
6356                         l2cap_pass_to_tx(chan, control);
6357                         break;
6358                 case L2CAP_TXSEQ_DUPLICATE:
6359                         /* Expecting a later sequence number, so this frame
6360                          * was already received.  Ignore it completely.
6361                          */
6362                         break;
6363                 case L2CAP_TXSEQ_INVALID_IGNORE:
6364                         break;
6365                 case L2CAP_TXSEQ_INVALID:
6366                 default:
6367                         l2cap_send_disconn_req(chan, ECONNRESET);
6368                         break;
6369                 }
6370                 break;
6371         case L2CAP_EV_RECV_RR:
6372                 l2cap_pass_to_tx(chan, control);
6373                 if (control->final) {
6374                         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6375
6376                         if (!test_and_clear_bit(CONN_REJ_ACT,
6377                                                 &chan->conn_state)) {
6378                                 control->final = 0;
6379                                 l2cap_retransmit_all(chan, control);
6380                         }
6381
6382                         l2cap_ertm_send(chan);
6383                 } else if (control->poll) {
6384                         if (test_and_clear_bit(CONN_REMOTE_BUSY,
6385                                                &chan->conn_state) &&
6386                             chan->unacked_frames) {
6387                                 __set_retrans_timer(chan);
6388                         }
6389
6390                         set_bit(CONN_SEND_FBIT, &chan->conn_state);
6391                         l2cap_send_srej_tail(chan);
6392                 } else {
6393                         if (test_and_clear_bit(CONN_REMOTE_BUSY,
6394                                                &chan->conn_state) &&
6395                             chan->unacked_frames)
6396                                 __set_retrans_timer(chan);
6397
6398                         l2cap_send_ack(chan);
6399                 }
6400                 break;
6401         case L2CAP_EV_RECV_RNR:
6402                 set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6403                 l2cap_pass_to_tx(chan, control);
6404                 if (control->poll) {
6405                         l2cap_send_srej_tail(chan);
6406                 } else {
6407                         struct l2cap_ctrl rr_control;
6408                         memset(&rr_control, 0, sizeof(rr_control));
6409                         rr_control.sframe = 1;
6410                         rr_control.super = L2CAP_SUPER_RR;
6411                         rr_control.reqseq = chan->buffer_seq;
6412                         l2cap_send_sframe(chan, &rr_control);
6413                 }
6414
6415                 break;
6416         case L2CAP_EV_RECV_REJ:
6417                 l2cap_handle_rej(chan, control);
6418                 break;
6419         case L2CAP_EV_RECV_SREJ:
6420                 l2cap_handle_srej(chan, control);
6421                 break;
6422         }
6423
6424         if (skb && !skb_in_use) {
6425                 BT_DBG("Freeing %p", skb);
6426                 kfree_skb(skb);
6427         }
6428
6429         return err;
6430 }
6431
6432 static int l2cap_finish_move(struct l2cap_chan *chan)
6433 {
6434         BT_DBG("chan %p", chan);
6435
6436         chan->rx_state = L2CAP_RX_STATE_RECV;
6437
6438         if (chan->hs_hcon)
6439                 chan->conn->mtu = chan->hs_hcon->hdev->block_mtu;
6440         else
6441                 chan->conn->mtu = chan->conn->hcon->hdev->acl_mtu;
6442
6443         return l2cap_resegment(chan);
6444 }
6445
6446 static int l2cap_rx_state_wait_p(struct l2cap_chan *chan,
6447                                  struct l2cap_ctrl *control,
6448                                  struct sk_buff *skb, u8 event)
6449 {
6450         int err;
6451
6452         BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
6453                event);
6454
6455         if (!control->poll)
6456                 return -EPROTO;
6457
6458         l2cap_process_reqseq(chan, control->reqseq);
6459
6460         if (!skb_queue_empty(&chan->tx_q))
6461                 chan->tx_send_head = skb_peek(&chan->tx_q);
6462         else
6463                 chan->tx_send_head = NULL;
6464
6465         /* Rewind next_tx_seq to the point expected
6466          * by the receiver.
6467          */
6468         chan->next_tx_seq = control->reqseq;
6469         chan->unacked_frames = 0;
6470
6471         err = l2cap_finish_move(chan);
6472         if (err)
6473                 return err;
6474
6475         set_bit(CONN_SEND_FBIT, &chan->conn_state);
6476         l2cap_send_i_or_rr_or_rnr(chan);
6477
6478         if (event == L2CAP_EV_RECV_IFRAME)
6479                 return -EPROTO;
6480
6481         return l2cap_rx_state_recv(chan, control, NULL, event);
6482 }
6483
6484 static int l2cap_rx_state_wait_f(struct l2cap_chan *chan,
6485                                  struct l2cap_ctrl *control,
6486                                  struct sk_buff *skb, u8 event)
6487 {
6488         int err;
6489
6490         if (!control->final)
6491                 return -EPROTO;
6492
6493         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6494
6495         chan->rx_state = L2CAP_RX_STATE_RECV;
6496         l2cap_process_reqseq(chan, control->reqseq);
6497
6498         if (!skb_queue_empty(&chan->tx_q))
6499                 chan->tx_send_head = skb_peek(&chan->tx_q);
6500         else
6501                 chan->tx_send_head = NULL;
6502
6503         /* Rewind next_tx_seq to the point expected
6504          * by the receiver.
6505          */
6506         chan->next_tx_seq = control->reqseq;
6507         chan->unacked_frames = 0;
6508
6509         if (chan->hs_hcon)
6510                 chan->conn->mtu = chan->hs_hcon->hdev->block_mtu;
6511         else
6512                 chan->conn->mtu = chan->conn->hcon->hdev->acl_mtu;
6513
6514         err = l2cap_resegment(chan);
6515
6516         if (!err)
6517                 err = l2cap_rx_state_recv(chan, control, skb, event);
6518
6519         return err;
6520 }
6521
6522 static bool __valid_reqseq(struct l2cap_chan *chan, u16 reqseq)
6523 {
6524         /* Make sure reqseq is for a packet that has been sent but not acked */
6525         u16 unacked;
6526
6527         unacked = __seq_offset(chan, chan->next_tx_seq, chan->expected_ack_seq);
6528         return __seq_offset(chan, chan->next_tx_seq, reqseq) <= unacked;
6529 }
6530
6531 static int l2cap_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
6532                     struct sk_buff *skb, u8 event)
6533 {
6534         int err = 0;
6535
6536         BT_DBG("chan %p, control %p, skb %p, event %d, state %d", chan,
6537                control, skb, event, chan->rx_state);
6538
6539         if (__valid_reqseq(chan, control->reqseq)) {
6540                 switch (chan->rx_state) {
6541                 case L2CAP_RX_STATE_RECV:
6542                         err = l2cap_rx_state_recv(chan, control, skb, event);
6543                         break;
6544                 case L2CAP_RX_STATE_SREJ_SENT:
6545                         err = l2cap_rx_state_srej_sent(chan, control, skb,
6546                                                        event);
6547                         break;
6548                 case L2CAP_RX_STATE_WAIT_P:
6549                         err = l2cap_rx_state_wait_p(chan, control, skb, event);
6550                         break;
6551                 case L2CAP_RX_STATE_WAIT_F:
6552                         err = l2cap_rx_state_wait_f(chan, control, skb, event);
6553                         break;
6554                 default:
6555                         /* shut it down */
6556                         break;
6557                 }
6558         } else {
6559                 BT_DBG("Invalid reqseq %d (next_tx_seq %d, expected_ack_seq %d",
6560                        control->reqseq, chan->next_tx_seq,
6561                        chan->expected_ack_seq);
6562                 l2cap_send_disconn_req(chan, ECONNRESET);
6563         }
6564
6565         return err;
6566 }
6567
6568 static int l2cap_stream_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
6569                            struct sk_buff *skb)
6570 {
6571         BT_DBG("chan %p, control %p, skb %p, state %d", chan, control, skb,
6572                chan->rx_state);
6573
6574         if (l2cap_classify_txseq(chan, control->txseq) ==
6575             L2CAP_TXSEQ_EXPECTED) {
6576                 l2cap_pass_to_tx(chan, control);
6577
6578                 BT_DBG("buffer_seq %d->%d", chan->buffer_seq,
6579                        __next_seq(chan, chan->buffer_seq));
6580
6581                 chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
6582
6583                 l2cap_reassemble_sdu(chan, skb, control);
6584         } else {
6585                 if (chan->sdu) {
6586                         kfree_skb(chan->sdu);
6587                         chan->sdu = NULL;
6588                 }
6589                 chan->sdu_last_frag = NULL;
6590                 chan->sdu_len = 0;
6591
6592                 if (skb) {
6593                         BT_DBG("Freeing %p", skb);
6594                         kfree_skb(skb);
6595                 }
6596         }
6597
6598         chan->last_acked_seq = control->txseq;
6599         chan->expected_tx_seq = __next_seq(chan, control->txseq);
6600
6601         return 0;
6602 }
6603
6604 static int l2cap_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb)
6605 {
6606         struct l2cap_ctrl *control = &bt_cb(skb)->l2cap;
6607         u16 len;
6608         u8 event;
6609
6610         __unpack_control(chan, skb);
6611
6612         len = skb->len;
6613
6614         /*
6615          * We can just drop the corrupted I-frame here.
6616          * Receiver will miss it and start proper recovery
6617          * procedures and ask for retransmission.
6618          */
6619         if (l2cap_check_fcs(chan, skb))
6620                 goto drop;
6621
6622         if (!control->sframe && control->sar == L2CAP_SAR_START)
6623                 len -= L2CAP_SDULEN_SIZE;
6624
6625         if (chan->fcs == L2CAP_FCS_CRC16)
6626                 len -= L2CAP_FCS_SIZE;
6627
6628         if (len > chan->mps) {
6629                 l2cap_send_disconn_req(chan, ECONNRESET);
6630                 goto drop;
6631         }
6632
6633         if ((chan->mode == L2CAP_MODE_ERTM ||
6634              chan->mode == L2CAP_MODE_STREAMING) && sk_filter(chan->data, skb))
6635                 goto drop;
6636
6637         if (!control->sframe) {
6638                 int err;
6639
6640                 BT_DBG("iframe sar %d, reqseq %d, final %d, txseq %d",
6641                        control->sar, control->reqseq, control->final,
6642                        control->txseq);
6643
6644                 /* Validate F-bit - F=0 always valid, F=1 only
6645                  * valid in TX WAIT_F
6646                  */
6647                 if (control->final && chan->tx_state != L2CAP_TX_STATE_WAIT_F)
6648                         goto drop;
6649
6650                 if (chan->mode != L2CAP_MODE_STREAMING) {
6651                         event = L2CAP_EV_RECV_IFRAME;
6652                         err = l2cap_rx(chan, control, skb, event);
6653                 } else {
6654                         err = l2cap_stream_rx(chan, control, skb);
6655                 }
6656
6657                 if (err)
6658                         l2cap_send_disconn_req(chan, ECONNRESET);
6659         } else {
6660                 const u8 rx_func_to_event[4] = {
6661                         L2CAP_EV_RECV_RR, L2CAP_EV_RECV_REJ,
6662                         L2CAP_EV_RECV_RNR, L2CAP_EV_RECV_SREJ
6663                 };
6664
6665                 /* Only I-frames are expected in streaming mode */
6666                 if (chan->mode == L2CAP_MODE_STREAMING)
6667                         goto drop;
6668
6669                 BT_DBG("sframe reqseq %d, final %d, poll %d, super %d",
6670                        control->reqseq, control->final, control->poll,
6671                        control->super);
6672
6673                 if (len != 0) {
6674                         BT_ERR("Trailing bytes: %d in sframe", len);
6675                         l2cap_send_disconn_req(chan, ECONNRESET);
6676                         goto drop;
6677                 }
6678
6679                 /* Validate F and P bits */
6680                 if (control->final && (control->poll ||
6681                                        chan->tx_state != L2CAP_TX_STATE_WAIT_F))
6682                         goto drop;
6683
6684                 event = rx_func_to_event[control->super];
6685                 if (l2cap_rx(chan, control, skb, event))
6686                         l2cap_send_disconn_req(chan, ECONNRESET);
6687         }
6688
6689         return 0;
6690
6691 drop:
6692         kfree_skb(skb);
6693         return 0;
6694 }
6695
6696 static void l2cap_chan_le_send_credits(struct l2cap_chan *chan)
6697 {
6698         struct l2cap_conn *conn = chan->conn;
6699         struct l2cap_le_credits pkt;
6700         u16 return_credits;
6701
6702         /* We return more credits to the sender only after the amount of
6703          * credits falls below half of the initial amount.
6704          */
6705         if (chan->rx_credits >= (le_max_credits + 1) / 2)
6706                 return;
6707
6708         return_credits = le_max_credits - chan->rx_credits;
6709
6710         BT_DBG("chan %p returning %u credits to sender", chan, return_credits);
6711
6712         chan->rx_credits += return_credits;
6713
6714         pkt.cid     = cpu_to_le16(chan->scid);
6715         pkt.credits = cpu_to_le16(return_credits);
6716
6717         chan->ident = l2cap_get_ident(conn);
6718
6719         l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CREDITS, sizeof(pkt), &pkt);
6720 }
6721
6722 static int l2cap_le_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb)
6723 {
6724         int err;
6725
6726         if (!chan->rx_credits) {
6727                 BT_ERR("No credits to receive LE L2CAP data");
6728                 l2cap_send_disconn_req(chan, ECONNRESET);
6729                 return -ENOBUFS;
6730         }
6731
6732         if (chan->imtu < skb->len) {
6733                 BT_ERR("Too big LE L2CAP PDU");
6734                 return -ENOBUFS;
6735         }
6736
6737         chan->rx_credits--;
6738         BT_DBG("rx_credits %u -> %u", chan->rx_credits + 1, chan->rx_credits);
6739
6740         l2cap_chan_le_send_credits(chan);
6741
6742         err = 0;
6743
6744         if (!chan->sdu) {
6745                 u16 sdu_len;
6746
6747                 sdu_len = get_unaligned_le16(skb->data);
6748                 skb_pull(skb, L2CAP_SDULEN_SIZE);
6749
6750                 BT_DBG("Start of new SDU. sdu_len %u skb->len %u imtu %u",
6751                        sdu_len, skb->len, chan->imtu);
6752
6753                 if (sdu_len > chan->imtu) {
6754                         BT_ERR("Too big LE L2CAP SDU length received");
6755                         err = -EMSGSIZE;
6756                         goto failed;
6757                 }
6758
6759                 if (skb->len > sdu_len) {
6760                         BT_ERR("Too much LE L2CAP data received");
6761                         err = -EINVAL;
6762                         goto failed;
6763                 }
6764
6765                 if (skb->len == sdu_len)
6766                         return chan->ops->recv(chan, skb);
6767
6768                 chan->sdu = skb;
6769                 chan->sdu_len = sdu_len;
6770                 chan->sdu_last_frag = skb;
6771
6772                 return 0;
6773         }
6774
6775         BT_DBG("SDU fragment. chan->sdu->len %u skb->len %u chan->sdu_len %u",
6776                chan->sdu->len, skb->len, chan->sdu_len);
6777
6778         if (chan->sdu->len + skb->len > chan->sdu_len) {
6779                 BT_ERR("Too much LE L2CAP data received");
6780                 err = -EINVAL;
6781                 goto failed;
6782         }
6783
6784         append_skb_frag(chan->sdu, skb, &chan->sdu_last_frag);
6785         skb = NULL;
6786
6787         if (chan->sdu->len == chan->sdu_len) {
6788                 err = chan->ops->recv(chan, chan->sdu);
6789                 if (!err) {
6790                         chan->sdu = NULL;
6791                         chan->sdu_last_frag = NULL;
6792                         chan->sdu_len = 0;
6793                 }
6794         }
6795
6796 failed:
6797         if (err) {
6798                 kfree_skb(skb);
6799                 kfree_skb(chan->sdu);
6800                 chan->sdu = NULL;
6801                 chan->sdu_last_frag = NULL;
6802                 chan->sdu_len = 0;
6803         }
6804
6805         /* We can't return an error here since we took care of the skb
6806          * freeing internally. An error return would cause the caller to
6807          * do a double-free of the skb.
6808          */
6809         return 0;
6810 }
6811
6812 static void l2cap_data_channel(struct l2cap_conn *conn, u16 cid,
6813                                struct sk_buff *skb)
6814 {
6815         struct l2cap_chan *chan;
6816
6817         chan = l2cap_get_chan_by_scid(conn, cid);
6818         if (!chan) {
6819                 if (cid == L2CAP_CID_A2MP) {
6820                         chan = a2mp_channel_create(conn, skb);
6821                         if (!chan) {
6822                                 kfree_skb(skb);
6823                                 return;
6824                         }
6825
6826                         l2cap_chan_lock(chan);
6827                 } else {
6828                         BT_DBG("unknown cid 0x%4.4x", cid);
6829                         /* Drop packet and return */
6830                         kfree_skb(skb);
6831                         return;
6832                 }
6833         }
6834
6835         BT_DBG("chan %p, len %d", chan, skb->len);
6836
6837         /* If we receive data on a fixed channel before the info req/rsp
6838          * procdure is done simply assume that the channel is supported
6839          * and mark it as ready.
6840          */
6841         if (chan->chan_type == L2CAP_CHAN_FIXED)
6842                 l2cap_chan_ready(chan);
6843
6844         if (chan->state != BT_CONNECTED)
6845                 goto drop;
6846
6847         switch (chan->mode) {
6848         case L2CAP_MODE_LE_FLOWCTL:
6849                 if (l2cap_le_data_rcv(chan, skb) < 0)
6850                         goto drop;
6851
6852                 goto done;
6853
6854         case L2CAP_MODE_BASIC:
6855                 /* If socket recv buffers overflows we drop data here
6856                  * which is *bad* because L2CAP has to be reliable.
6857                  * But we don't have any other choice. L2CAP doesn't
6858                  * provide flow control mechanism. */
6859
6860                 if (chan->imtu < skb->len) {
6861                         BT_ERR("Dropping L2CAP data: receive buffer overflow");
6862                         goto drop;
6863                 }
6864
6865                 if (!chan->ops->recv(chan, skb))
6866                         goto done;
6867                 break;
6868
6869         case L2CAP_MODE_ERTM:
6870         case L2CAP_MODE_STREAMING:
6871                 l2cap_data_rcv(chan, skb);
6872                 goto done;
6873
6874         default:
6875                 BT_DBG("chan %p: bad mode 0x%2.2x", chan, chan->mode);
6876                 break;
6877         }
6878
6879 drop:
6880         kfree_skb(skb);
6881
6882 done:
6883         l2cap_chan_unlock(chan);
6884 }
6885
6886 static void l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm,
6887                                   struct sk_buff *skb)
6888 {
6889         struct hci_conn *hcon = conn->hcon;
6890         struct l2cap_chan *chan;
6891
6892         if (hcon->type != ACL_LINK)
6893                 goto free_skb;
6894
6895         chan = l2cap_global_chan_by_psm(0, psm, &hcon->src, &hcon->dst,
6896                                         ACL_LINK);
6897         if (!chan)
6898                 goto free_skb;
6899
6900         BT_DBG("chan %p, len %d", chan, skb->len);
6901
6902         if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
6903                 goto drop;
6904
6905         if (chan->imtu < skb->len)
6906                 goto drop;
6907
6908         /* Store remote BD_ADDR and PSM for msg_name */
6909         bacpy(&bt_cb(skb)->l2cap.bdaddr, &hcon->dst);
6910         bt_cb(skb)->l2cap.psm = psm;
6911
6912         if (!chan->ops->recv(chan, skb)) {
6913                 l2cap_chan_put(chan);
6914                 return;
6915         }
6916
6917 drop:
6918         l2cap_chan_put(chan);
6919 free_skb:
6920         kfree_skb(skb);
6921 }
6922
6923 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
6924 {
6925         struct l2cap_hdr *lh = (void *) skb->data;
6926         struct hci_conn *hcon = conn->hcon;
6927         u16 cid, len;
6928         __le16 psm;
6929
6930         if (hcon->state != BT_CONNECTED) {
6931                 BT_DBG("queueing pending rx skb");
6932                 skb_queue_tail(&conn->pending_rx, skb);
6933                 return;
6934         }
6935
6936         skb_pull(skb, L2CAP_HDR_SIZE);
6937         cid = __le16_to_cpu(lh->cid);
6938         len = __le16_to_cpu(lh->len);
6939
6940         if (len != skb->len) {
6941                 kfree_skb(skb);
6942                 return;
6943         }
6944
6945         /* Since we can't actively block incoming LE connections we must
6946          * at least ensure that we ignore incoming data from them.
6947          */
6948         if (hcon->type == LE_LINK &&
6949             hci_bdaddr_list_lookup(&hcon->hdev->blacklist, &hcon->dst,
6950                                    bdaddr_dst_type(hcon))) {
6951                 kfree_skb(skb);
6952                 return;
6953         }
6954
6955         BT_DBG("len %d, cid 0x%4.4x", len, cid);
6956
6957         switch (cid) {
6958         case L2CAP_CID_SIGNALING:
6959                 l2cap_sig_channel(conn, skb);
6960                 break;
6961
6962         case L2CAP_CID_CONN_LESS:
6963                 psm = get_unaligned((__le16 *) skb->data);
6964                 skb_pull(skb, L2CAP_PSMLEN_SIZE);
6965                 l2cap_conless_channel(conn, psm, skb);
6966                 break;
6967
6968         case L2CAP_CID_LE_SIGNALING:
6969                 l2cap_le_sig_channel(conn, skb);
6970                 break;
6971
6972         default:
6973                 l2cap_data_channel(conn, cid, skb);
6974                 break;
6975         }
6976 }
6977
6978 static void process_pending_rx(struct work_struct *work)
6979 {
6980         struct l2cap_conn *conn = container_of(work, struct l2cap_conn,
6981                                                pending_rx_work);
6982         struct sk_buff *skb;
6983
6984         BT_DBG("");
6985
6986         while ((skb = skb_dequeue(&conn->pending_rx)))
6987                 l2cap_recv_frame(conn, skb);
6988 }
6989
6990 static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon)
6991 {
6992         struct l2cap_conn *conn = hcon->l2cap_data;
6993         struct hci_chan *hchan;
6994
6995         if (conn)
6996                 return conn;
6997
6998         hchan = hci_chan_create(hcon);
6999         if (!hchan)
7000                 return NULL;
7001
7002         conn = kzalloc(sizeof(*conn), GFP_KERNEL);
7003         if (!conn) {
7004                 hci_chan_del(hchan);
7005                 return NULL;
7006         }
7007
7008         kref_init(&conn->ref);
7009         hcon->l2cap_data = conn;
7010         conn->hcon = hci_conn_get(hcon);
7011         conn->hchan = hchan;
7012
7013         BT_DBG("hcon %p conn %p hchan %p", hcon, conn, hchan);
7014
7015         switch (hcon->type) {
7016         case LE_LINK:
7017                 if (hcon->hdev->le_mtu) {
7018                         conn->mtu = hcon->hdev->le_mtu;
7019                         break;
7020                 }
7021                 /* fall through */
7022         default:
7023                 conn->mtu = hcon->hdev->acl_mtu;
7024                 break;
7025         }
7026
7027         conn->feat_mask = 0;
7028
7029         conn->local_fixed_chan = L2CAP_FC_SIG_BREDR | L2CAP_FC_CONNLESS;
7030
7031         if (hcon->type == ACL_LINK &&
7032             hci_dev_test_flag(hcon->hdev, HCI_HS_ENABLED))
7033                 conn->local_fixed_chan |= L2CAP_FC_A2MP;
7034
7035         if (hci_dev_test_flag(hcon->hdev, HCI_LE_ENABLED) &&
7036             (bredr_sc_enabled(hcon->hdev) ||
7037              hci_dev_test_flag(hcon->hdev, HCI_FORCE_BREDR_SMP)))
7038                 conn->local_fixed_chan |= L2CAP_FC_SMP_BREDR;
7039
7040         mutex_init(&conn->ident_lock);
7041         mutex_init(&conn->chan_lock);
7042
7043         INIT_LIST_HEAD(&conn->chan_l);
7044         INIT_LIST_HEAD(&conn->users);
7045
7046         INIT_DELAYED_WORK(&conn->info_timer, l2cap_info_timeout);
7047
7048         skb_queue_head_init(&conn->pending_rx);
7049         INIT_WORK(&conn->pending_rx_work, process_pending_rx);
7050         INIT_WORK(&conn->id_addr_update_work, l2cap_conn_update_id_addr);
7051
7052         conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM;
7053
7054         return conn;
7055 }
7056
7057 static bool is_valid_psm(u16 psm, u8 dst_type) {
7058         if (!psm)
7059                 return false;
7060
7061         if (bdaddr_type_is_le(dst_type))
7062                 return (psm <= 0x00ff);
7063
7064         /* PSM must be odd and lsb of upper byte must be 0 */
7065         return ((psm & 0x0101) == 0x0001);
7066 }
7067
7068 int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid,
7069                        bdaddr_t *dst, u8 dst_type)
7070 {
7071         struct l2cap_conn *conn;
7072         struct hci_conn *hcon;
7073         struct hci_dev *hdev;
7074         int err;
7075
7076         BT_DBG("%pMR -> %pMR (type %u) psm 0x%2.2x", &chan->src, dst,
7077                dst_type, __le16_to_cpu(psm));
7078
7079         hdev = hci_get_route(dst, &chan->src, chan->src_type);
7080         if (!hdev)
7081                 return -EHOSTUNREACH;
7082
7083         hci_dev_lock(hdev);
7084
7085         if (!is_valid_psm(__le16_to_cpu(psm), dst_type) && !cid &&
7086             chan->chan_type != L2CAP_CHAN_RAW) {
7087                 err = -EINVAL;
7088                 goto done;
7089         }
7090
7091         if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED && !psm) {
7092                 err = -EINVAL;
7093                 goto done;
7094         }
7095
7096         if (chan->chan_type == L2CAP_CHAN_FIXED && !cid) {
7097                 err = -EINVAL;
7098                 goto done;
7099         }
7100
7101         switch (chan->mode) {
7102         case L2CAP_MODE_BASIC:
7103                 break;
7104         case L2CAP_MODE_LE_FLOWCTL:
7105                 l2cap_le_flowctl_init(chan);
7106                 break;
7107         case L2CAP_MODE_ERTM:
7108         case L2CAP_MODE_STREAMING:
7109                 if (!disable_ertm)
7110                         break;
7111                 /* fall through */
7112         default:
7113                 err = -EOPNOTSUPP;
7114                 goto done;
7115         }
7116
7117         switch (chan->state) {
7118         case BT_CONNECT:
7119         case BT_CONNECT2:
7120         case BT_CONFIG:
7121                 /* Already connecting */
7122                 err = 0;
7123                 goto done;
7124
7125         case BT_CONNECTED:
7126                 /* Already connected */
7127                 err = -EISCONN;
7128                 goto done;
7129
7130         case BT_OPEN:
7131         case BT_BOUND:
7132                 /* Can connect */
7133                 break;
7134
7135         default:
7136                 err = -EBADFD;
7137                 goto done;
7138         }
7139
7140         /* Set destination address and psm */
7141         bacpy(&chan->dst, dst);
7142         chan->dst_type = dst_type;
7143
7144         chan->psm = psm;
7145         chan->dcid = cid;
7146
7147         if (bdaddr_type_is_le(dst_type)) {
7148                 /* Convert from L2CAP channel address type to HCI address type
7149                  */
7150                 if (dst_type == BDADDR_LE_PUBLIC)
7151                         dst_type = ADDR_LE_DEV_PUBLIC;
7152                 else
7153                         dst_type = ADDR_LE_DEV_RANDOM;
7154
7155                 if (hci_dev_test_flag(hdev, HCI_ADVERTISING))
7156                         hcon = hci_connect_le(hdev, dst, dst_type,
7157                                               chan->sec_level,
7158                                               HCI_LE_CONN_TIMEOUT,
7159                                               HCI_ROLE_SLAVE);
7160                 else
7161                         hcon = hci_connect_le_scan(hdev, dst, dst_type,
7162                                                    chan->sec_level,
7163                                                    HCI_LE_CONN_TIMEOUT);
7164
7165         } else {
7166                 u8 auth_type = l2cap_get_auth_type(chan);
7167                 hcon = hci_connect_acl(hdev, dst, chan->sec_level, auth_type);
7168         }
7169
7170         if (IS_ERR(hcon)) {
7171                 err = PTR_ERR(hcon);
7172                 goto done;
7173         }
7174
7175         conn = l2cap_conn_add(hcon);
7176         if (!conn) {
7177                 hci_conn_drop(hcon);
7178                 err = -ENOMEM;
7179                 goto done;
7180         }
7181
7182         mutex_lock(&conn->chan_lock);
7183         l2cap_chan_lock(chan);
7184
7185         if (cid && __l2cap_get_chan_by_dcid(conn, cid)) {
7186                 hci_conn_drop(hcon);
7187                 err = -EBUSY;
7188                 goto chan_unlock;
7189         }
7190
7191         /* Update source addr of the socket */
7192         bacpy(&chan->src, &hcon->src);
7193         chan->src_type = bdaddr_src_type(hcon);
7194
7195         __l2cap_chan_add(conn, chan);
7196
7197         /* l2cap_chan_add takes its own ref so we can drop this one */
7198         hci_conn_drop(hcon);
7199
7200         l2cap_state_change(chan, BT_CONNECT);
7201         __set_chan_timer(chan, chan->ops->get_sndtimeo(chan));
7202
7203         /* Release chan->sport so that it can be reused by other
7204          * sockets (as it's only used for listening sockets).
7205          */
7206         write_lock(&chan_list_lock);
7207         chan->sport = 0;
7208         write_unlock(&chan_list_lock);
7209
7210         if (hcon->state == BT_CONNECTED) {
7211                 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
7212                         __clear_chan_timer(chan);
7213                         if (l2cap_chan_check_security(chan, true))
7214                                 l2cap_state_change(chan, BT_CONNECTED);
7215                 } else
7216                         l2cap_do_start(chan);
7217         }
7218
7219         err = 0;
7220
7221 chan_unlock:
7222         l2cap_chan_unlock(chan);
7223         mutex_unlock(&conn->chan_lock);
7224 done:
7225         hci_dev_unlock(hdev);
7226         hci_dev_put(hdev);
7227         return err;
7228 }
7229 EXPORT_SYMBOL_GPL(l2cap_chan_connect);
7230
7231 /* ---- L2CAP interface with lower layer (HCI) ---- */
7232
7233 int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr)
7234 {
7235         int exact = 0, lm1 = 0, lm2 = 0;
7236         struct l2cap_chan *c;
7237
7238         BT_DBG("hdev %s, bdaddr %pMR", hdev->name, bdaddr);
7239
7240         /* Find listening sockets and check their link_mode */
7241         read_lock(&chan_list_lock);
7242         list_for_each_entry(c, &chan_list, global_l) {
7243                 if (c->state != BT_LISTEN)
7244                         continue;
7245
7246                 if (!bacmp(&c->src, &hdev->bdaddr)) {
7247                         lm1 |= HCI_LM_ACCEPT;
7248                         if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
7249                                 lm1 |= HCI_LM_MASTER;
7250                         exact++;
7251                 } else if (!bacmp(&c->src, BDADDR_ANY)) {
7252                         lm2 |= HCI_LM_ACCEPT;
7253                         if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
7254                                 lm2 |= HCI_LM_MASTER;
7255                 }
7256         }
7257         read_unlock(&chan_list_lock);
7258
7259         return exact ? lm1 : lm2;
7260 }
7261
7262 /* Find the next fixed channel in BT_LISTEN state, continue iteration
7263  * from an existing channel in the list or from the beginning of the
7264  * global list (by passing NULL as first parameter).
7265  */
7266 static struct l2cap_chan *l2cap_global_fixed_chan(struct l2cap_chan *c,
7267                                                   struct hci_conn *hcon)
7268 {
7269         u8 src_type = bdaddr_src_type(hcon);
7270
7271         read_lock(&chan_list_lock);
7272
7273         if (c)
7274                 c = list_next_entry(c, global_l);
7275         else
7276                 c = list_entry(chan_list.next, typeof(*c), global_l);
7277
7278         list_for_each_entry_from(c, &chan_list, global_l) {
7279                 if (c->chan_type != L2CAP_CHAN_FIXED)
7280                         continue;
7281                 if (c->state != BT_LISTEN)
7282                         continue;
7283                 if (bacmp(&c->src, &hcon->src) && bacmp(&c->src, BDADDR_ANY))
7284                         continue;
7285                 if (src_type != c->src_type)
7286                         continue;
7287
7288                 l2cap_chan_hold(c);
7289                 read_unlock(&chan_list_lock);
7290                 return c;
7291         }
7292
7293         read_unlock(&chan_list_lock);
7294
7295         return NULL;
7296 }
7297
7298 static void l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
7299 {
7300         struct hci_dev *hdev = hcon->hdev;
7301         struct l2cap_conn *conn;
7302         struct l2cap_chan *pchan;
7303         u8 dst_type;
7304
7305         if (hcon->type != ACL_LINK && hcon->type != LE_LINK)
7306                 return;
7307
7308         BT_DBG("hcon %p bdaddr %pMR status %d", hcon, &hcon->dst, status);
7309
7310         if (status) {
7311                 l2cap_conn_del(hcon, bt_to_errno(status));
7312                 return;
7313         }
7314
7315         conn = l2cap_conn_add(hcon);
7316         if (!conn)
7317                 return;
7318
7319         dst_type = bdaddr_dst_type(hcon);
7320
7321         /* If device is blocked, do not create channels for it */
7322         if (hci_bdaddr_list_lookup(&hdev->blacklist, &hcon->dst, dst_type))
7323                 return;
7324
7325         /* Find fixed channels and notify them of the new connection. We
7326          * use multiple individual lookups, continuing each time where
7327          * we left off, because the list lock would prevent calling the
7328          * potentially sleeping l2cap_chan_lock() function.
7329          */
7330         pchan = l2cap_global_fixed_chan(NULL, hcon);
7331         while (pchan) {
7332                 struct l2cap_chan *chan, *next;
7333
7334                 /* Client fixed channels should override server ones */
7335                 if (__l2cap_get_chan_by_dcid(conn, pchan->scid))
7336                         goto next;
7337
7338                 l2cap_chan_lock(pchan);
7339                 chan = pchan->ops->new_connection(pchan);
7340                 if (chan) {
7341                         bacpy(&chan->src, &hcon->src);
7342                         bacpy(&chan->dst, &hcon->dst);
7343                         chan->src_type = bdaddr_src_type(hcon);
7344                         chan->dst_type = dst_type;
7345
7346                         __l2cap_chan_add(conn, chan);
7347                 }
7348
7349                 l2cap_chan_unlock(pchan);
7350 next:
7351                 next = l2cap_global_fixed_chan(pchan, hcon);
7352                 l2cap_chan_put(pchan);
7353                 pchan = next;
7354         }
7355
7356         l2cap_conn_ready(conn);
7357 }
7358
7359 int l2cap_disconn_ind(struct hci_conn *hcon)
7360 {
7361         struct l2cap_conn *conn = hcon->l2cap_data;
7362
7363         BT_DBG("hcon %p", hcon);
7364
7365         if (!conn)
7366                 return HCI_ERROR_REMOTE_USER_TERM;
7367         return conn->disc_reason;
7368 }
7369
7370 static void l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
7371 {
7372         if (hcon->type != ACL_LINK && hcon->type != LE_LINK)
7373                 return;
7374
7375         BT_DBG("hcon %p reason %d", hcon, reason);
7376
7377         l2cap_conn_del(hcon, bt_to_errno(reason));
7378 }
7379
7380 static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt)
7381 {
7382         if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED)
7383                 return;
7384
7385         if (encrypt == 0x00) {
7386                 if (chan->sec_level == BT_SECURITY_MEDIUM) {
7387                         __set_chan_timer(chan, L2CAP_ENC_TIMEOUT);
7388                 } else if (chan->sec_level == BT_SECURITY_HIGH ||
7389                            chan->sec_level == BT_SECURITY_FIPS)
7390                         l2cap_chan_close(chan, ECONNREFUSED);
7391         } else {
7392                 if (chan->sec_level == BT_SECURITY_MEDIUM)
7393                         __clear_chan_timer(chan);
7394         }
7395 }
7396
7397 static void l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
7398 {
7399         struct l2cap_conn *conn = hcon->l2cap_data;
7400         struct l2cap_chan *chan;
7401
7402         if (!conn)
7403                 return;
7404
7405         BT_DBG("conn %p status 0x%2.2x encrypt %u", conn, status, encrypt);
7406
7407         mutex_lock(&conn->chan_lock);
7408
7409         list_for_each_entry(chan, &conn->chan_l, list) {
7410                 l2cap_chan_lock(chan);
7411
7412                 BT_DBG("chan %p scid 0x%4.4x state %s", chan, chan->scid,
7413                        state_to_string(chan->state));
7414
7415                 if (chan->scid == L2CAP_CID_A2MP) {
7416                         l2cap_chan_unlock(chan);
7417                         continue;
7418                 }
7419
7420                 if (!status && encrypt)
7421                         chan->sec_level = hcon->sec_level;
7422
7423                 if (!__l2cap_no_conn_pending(chan)) {
7424                         l2cap_chan_unlock(chan);
7425                         continue;
7426                 }
7427
7428                 if (!status && (chan->state == BT_CONNECTED ||
7429                                 chan->state == BT_CONFIG)) {
7430                         chan->ops->resume(chan);
7431                         l2cap_check_encryption(chan, encrypt);
7432                         l2cap_chan_unlock(chan);
7433                         continue;
7434                 }
7435
7436                 if (chan->state == BT_CONNECT) {
7437                         if (!status)
7438                                 l2cap_start_connection(chan);
7439                         else
7440                                 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
7441                 } else if (chan->state == BT_CONNECT2 &&
7442                            chan->mode != L2CAP_MODE_LE_FLOWCTL) {
7443                         struct l2cap_conn_rsp rsp;
7444                         __u16 res, stat;
7445
7446                         if (!status) {
7447                                 if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
7448                                         res = L2CAP_CR_PEND;
7449                                         stat = L2CAP_CS_AUTHOR_PEND;
7450                                         chan->ops->defer(chan);
7451                                 } else {
7452                                         l2cap_state_change(chan, BT_CONFIG);
7453                                         res = L2CAP_CR_SUCCESS;
7454                                         stat = L2CAP_CS_NO_INFO;
7455                                 }
7456                         } else {
7457                                 l2cap_state_change(chan, BT_DISCONN);
7458                                 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
7459                                 res = L2CAP_CR_SEC_BLOCK;
7460                                 stat = L2CAP_CS_NO_INFO;
7461                         }
7462
7463                         rsp.scid   = cpu_to_le16(chan->dcid);
7464                         rsp.dcid   = cpu_to_le16(chan->scid);
7465                         rsp.result = cpu_to_le16(res);
7466                         rsp.status = cpu_to_le16(stat);
7467                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
7468                                        sizeof(rsp), &rsp);
7469
7470                         if (!test_bit(CONF_REQ_SENT, &chan->conf_state) &&
7471                             res == L2CAP_CR_SUCCESS) {
7472                                 char buf[128];
7473                                 set_bit(CONF_REQ_SENT, &chan->conf_state);
7474                                 l2cap_send_cmd(conn, l2cap_get_ident(conn),
7475                                                L2CAP_CONF_REQ,
7476                                                l2cap_build_conf_req(chan, buf, sizeof(buf)),
7477                                                buf);
7478                                 chan->num_conf_req++;
7479                         }
7480                 }
7481
7482                 l2cap_chan_unlock(chan);
7483         }
7484
7485         mutex_unlock(&conn->chan_lock);
7486 }
7487
7488 void l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
7489 {
7490         struct l2cap_conn *conn = hcon->l2cap_data;
7491         struct l2cap_hdr *hdr;
7492         int len;
7493
7494         /* For AMP controller do not create l2cap conn */
7495         if (!conn && hcon->hdev->dev_type != HCI_PRIMARY)
7496                 goto drop;
7497
7498         if (!conn)
7499                 conn = l2cap_conn_add(hcon);
7500
7501         if (!conn)
7502                 goto drop;
7503
7504         BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
7505
7506         switch (flags) {
7507         case ACL_START:
7508         case ACL_START_NO_FLUSH:
7509         case ACL_COMPLETE:
7510                 if (conn->rx_len) {
7511                         BT_ERR("Unexpected start frame (len %d)", skb->len);
7512                         kfree_skb(conn->rx_skb);
7513                         conn->rx_skb = NULL;
7514                         conn->rx_len = 0;
7515                         l2cap_conn_unreliable(conn, ECOMM);
7516                 }
7517
7518                 /* Start fragment always begin with Basic L2CAP header */
7519                 if (skb->len < L2CAP_HDR_SIZE) {
7520                         BT_ERR("Frame is too short (len %d)", skb->len);
7521                         l2cap_conn_unreliable(conn, ECOMM);
7522                         goto drop;
7523                 }
7524
7525                 hdr = (struct l2cap_hdr *) skb->data;
7526                 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
7527
7528                 if (len == skb->len) {
7529                         /* Complete frame received */
7530                         l2cap_recv_frame(conn, skb);
7531                         return;
7532                 }
7533
7534                 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
7535
7536                 if (skb->len > len) {
7537                         BT_ERR("Frame is too long (len %d, expected len %d)",
7538                                skb->len, len);
7539                         l2cap_conn_unreliable(conn, ECOMM);
7540                         goto drop;
7541                 }
7542
7543                 /* Allocate skb for the complete frame (with header) */
7544                 conn->rx_skb = bt_skb_alloc(len, GFP_KERNEL);
7545                 if (!conn->rx_skb)
7546                         goto drop;
7547
7548                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
7549                                           skb->len);
7550                 conn->rx_len = len - skb->len;
7551                 break;
7552
7553         case ACL_CONT:
7554                 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
7555
7556                 if (!conn->rx_len) {
7557                         BT_ERR("Unexpected continuation frame (len %d)", skb->len);
7558                         l2cap_conn_unreliable(conn, ECOMM);
7559                         goto drop;
7560                 }
7561
7562                 if (skb->len > conn->rx_len) {
7563                         BT_ERR("Fragment is too long (len %d, expected %d)",
7564                                skb->len, conn->rx_len);
7565                         kfree_skb(conn->rx_skb);
7566                         conn->rx_skb = NULL;
7567                         conn->rx_len = 0;
7568                         l2cap_conn_unreliable(conn, ECOMM);
7569                         goto drop;
7570                 }
7571
7572                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
7573                                           skb->len);
7574                 conn->rx_len -= skb->len;
7575
7576                 if (!conn->rx_len) {
7577                         /* Complete frame received. l2cap_recv_frame
7578                          * takes ownership of the skb so set the global
7579                          * rx_skb pointer to NULL first.
7580                          */
7581                         struct sk_buff *rx_skb = conn->rx_skb;
7582                         conn->rx_skb = NULL;
7583                         l2cap_recv_frame(conn, rx_skb);
7584                 }
7585                 break;
7586         }
7587
7588 drop:
7589         kfree_skb(skb);
7590 }
7591
7592 static struct hci_cb l2cap_cb = {
7593         .name           = "L2CAP",
7594         .connect_cfm    = l2cap_connect_cfm,
7595         .disconn_cfm    = l2cap_disconn_cfm,
7596         .security_cfm   = l2cap_security_cfm,
7597 };
7598
7599 static int l2cap_debugfs_show(struct seq_file *f, void *p)
7600 {
7601         struct l2cap_chan *c;
7602
7603         read_lock(&chan_list_lock);
7604
7605         list_for_each_entry(c, &chan_list, global_l) {
7606                 seq_printf(f, "%pMR (%u) %pMR (%u) %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
7607                            &c->src, c->src_type, &c->dst, c->dst_type,
7608                            c->state, __le16_to_cpu(c->psm),
7609                            c->scid, c->dcid, c->imtu, c->omtu,
7610                            c->sec_level, c->mode);
7611         }
7612
7613         read_unlock(&chan_list_lock);
7614
7615         return 0;
7616 }
7617
7618 static int l2cap_debugfs_open(struct inode *inode, struct file *file)
7619 {
7620         return single_open(file, l2cap_debugfs_show, inode->i_private);
7621 }
7622
7623 static const struct file_operations l2cap_debugfs_fops = {
7624         .open           = l2cap_debugfs_open,
7625         .read           = seq_read,
7626         .llseek         = seq_lseek,
7627         .release        = single_release,
7628 };
7629
7630 static struct dentry *l2cap_debugfs;
7631
7632 int __init l2cap_init(void)
7633 {
7634         int err;
7635
7636         err = l2cap_init_sockets();
7637         if (err < 0)
7638                 return err;
7639
7640         hci_register_cb(&l2cap_cb);
7641
7642         if (IS_ERR_OR_NULL(bt_debugfs))
7643                 return 0;
7644
7645         l2cap_debugfs = debugfs_create_file("l2cap", 0444, bt_debugfs,
7646                                             NULL, &l2cap_debugfs_fops);
7647
7648         debugfs_create_u16("l2cap_le_max_credits", 0644, bt_debugfs,
7649                            &le_max_credits);
7650         debugfs_create_u16("l2cap_le_default_mps", 0644, bt_debugfs,
7651                            &le_default_mps);
7652
7653         return 0;
7654 }
7655
7656 void l2cap_exit(void)
7657 {
7658         debugfs_remove(l2cap_debugfs);
7659         hci_unregister_cb(&l2cap_cb);
7660         l2cap_cleanup_sockets();
7661 }
7662
7663 module_param(disable_ertm, bool, 0644);
7664 MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode");