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