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