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