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