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