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 <[email protected]>
   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 <[email protected]>
  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                 return 0;
4697         }
4698
4699         if (chan->state != BT_DISCONN) {
4700                 l2cap_chan_unlock(chan);
4701                 l2cap_chan_put(chan);
4702                 return 0;
4703         }
4704
4705         mutex_lock(&conn->chan_lock);
4706         l2cap_chan_del(chan, 0);
4707         mutex_unlock(&conn->chan_lock);
4708
4709         chan->ops->close(chan);
4710
4711         l2cap_chan_unlock(chan);
4712         l2cap_chan_put(chan);
4713
4714         return 0;
4715 }
4716
4717 static inline int l2cap_information_req(struct l2cap_conn *conn,
4718                                         struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4719                                         u8 *data)
4720 {
4721         struct l2cap_info_req *req = (struct l2cap_info_req *) data;
4722         u16 type;
4723
4724         if (cmd_len != sizeof(*req))
4725                 return -EPROTO;
4726
4727         type = __le16_to_cpu(req->type);
4728
4729         BT_DBG("type 0x%4.4x", type);
4730
4731         if (type == L2CAP_IT_FEAT_MASK) {
4732                 u8 buf[8];
4733                 u32 feat_mask = l2cap_feat_mask;
4734                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
4735                 rsp->type   = cpu_to_le16(L2CAP_IT_FEAT_MASK);
4736                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
4737                 if (!disable_ertm)
4738                         feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
4739                                 | L2CAP_FEAT_FCS;
4740                 if (conn->local_fixed_chan & L2CAP_FC_A2MP)
4741                         feat_mask |= L2CAP_FEAT_EXT_FLOW
4742                                 | L2CAP_FEAT_EXT_WINDOW;
4743
4744                 put_unaligned_le32(feat_mask, rsp->data);
4745                 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf),
4746                                buf);
4747         } else if (type == L2CAP_IT_FIXED_CHAN) {
4748                 u8 buf[12];
4749                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
4750
4751                 rsp->type   = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
4752                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
4753                 rsp->data[0] = conn->local_fixed_chan;
4754                 memset(rsp->data + 1, 0, 7);
4755                 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf),
4756                                buf);
4757         } else {
4758                 struct l2cap_info_rsp rsp;
4759                 rsp.type   = cpu_to_le16(type);
4760                 rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP);
4761                 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(rsp),
4762                                &rsp);
4763         }
4764
4765         return 0;
4766 }
4767
4768 static inline int l2cap_information_rsp(struct l2cap_conn *conn,
4769                                         struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4770                                         u8 *data)
4771 {
4772         struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
4773         u16 type, result;
4774
4775         if (cmd_len < sizeof(*rsp))
4776                 return -EPROTO;
4777
4778         type   = __le16_to_cpu(rsp->type);
4779         result = __le16_to_cpu(rsp->result);
4780
4781         BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
4782
4783         /* L2CAP Info req/rsp are unbound to channels, add extra checks */
4784         if (cmd->ident != conn->info_ident ||
4785             conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)
4786                 return 0;
4787
4788         cancel_delayed_work(&conn->info_timer);
4789
4790         if (result != L2CAP_IR_SUCCESS) {
4791                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4792                 conn->info_ident = 0;
4793
4794                 l2cap_conn_start(conn);
4795
4796                 return 0;
4797         }
4798
4799         switch (type) {
4800         case L2CAP_IT_FEAT_MASK:
4801                 conn->feat_mask = get_unaligned_le32(rsp->data);
4802
4803                 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
4804                         struct l2cap_info_req req;
4805                         req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
4806
4807                         conn->info_ident = l2cap_get_ident(conn);
4808
4809                         l2cap_send_cmd(conn, conn->info_ident,
4810                                        L2CAP_INFO_REQ, sizeof(req), &req);
4811                 } else {
4812                         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4813                         conn->info_ident = 0;
4814
4815                         l2cap_conn_start(conn);
4816                 }
4817                 break;
4818
4819         case L2CAP_IT_FIXED_CHAN:
4820                 conn->remote_fixed_chan = rsp->data[0];
4821                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4822                 conn->info_ident = 0;
4823
4824                 l2cap_conn_start(conn);
4825                 break;
4826         }
4827
4828         return 0;
4829 }
4830
4831 static int l2cap_create_channel_req(struct l2cap_conn *conn,
4832                                     struct l2cap_cmd_hdr *cmd,
4833                                     u16 cmd_len, void *data)
4834 {
4835         struct l2cap_create_chan_req *req = data;
4836         struct l2cap_create_chan_rsp rsp;
4837         struct l2cap_chan *chan;
4838         struct hci_dev *hdev;
4839         u16 psm, scid;
4840
4841         if (cmd_len != sizeof(*req))
4842                 return -EPROTO;
4843
4844         if (!(conn->local_fixed_chan & L2CAP_FC_A2MP))
4845                 return -EINVAL;
4846
4847         psm = le16_to_cpu(req->psm);
4848         scid = le16_to_cpu(req->scid);
4849
4850         BT_DBG("psm 0x%2.2x, scid 0x%4.4x, amp_id %d", psm, scid, req->amp_id);
4851
4852         /* For controller id 0 make BR/EDR connection */
4853         if (req->amp_id == AMP_ID_BREDR) {
4854                 l2cap_connect(conn, cmd, data, L2CAP_CREATE_CHAN_RSP,
4855                               req->amp_id);
4856                 return 0;
4857         }
4858
4859         /* Validate AMP controller id */
4860         hdev = hci_dev_get(req->amp_id);
4861         if (!hdev)
4862                 goto error;
4863
4864         if (hdev->dev_type != HCI_AMP || !test_bit(HCI_UP, &hdev->flags)) {
4865                 hci_dev_put(hdev);
4866                 goto error;
4867         }
4868
4869         chan = l2cap_connect(conn, cmd, data, L2CAP_CREATE_CHAN_RSP,
4870                              req->amp_id);
4871         if (chan) {
4872                 struct amp_mgr *mgr = conn->hcon->amp_mgr;
4873                 struct hci_conn *hs_hcon;
4874
4875                 hs_hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK,
4876                                                   &conn->hcon->dst);
4877                 if (!hs_hcon) {
4878                         hci_dev_put(hdev);
4879                         cmd_reject_invalid_cid(conn, cmd->ident, chan->scid,
4880                                                chan->dcid);
4881                         return 0;
4882                 }
4883
4884                 BT_DBG("mgr %p bredr_chan %p hs_hcon %p", mgr, chan, hs_hcon);
4885
4886                 mgr->bredr_chan = chan;
4887                 chan->hs_hcon = hs_hcon;
4888                 chan->fcs = L2CAP_FCS_NONE;
4889                 conn->mtu = hdev->block_mtu;
4890         }
4891
4892         hci_dev_put(hdev);
4893
4894         return 0;
4895
4896 error:
4897         rsp.dcid = 0;
4898         rsp.scid = cpu_to_le16(scid);
4899         rsp.result = cpu_to_le16(L2CAP_CR_BAD_AMP);
4900         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
4901
4902         l2cap_send_cmd(conn, cmd->ident, L2CAP_CREATE_CHAN_RSP,
4903                        sizeof(rsp), &rsp);
4904
4905         return 0;
4906 }
4907
4908 static void l2cap_send_move_chan_req(struct l2cap_chan *chan, u8 dest_amp_id)
4909 {
4910         struct l2cap_move_chan_req req;
4911         u8 ident;
4912
4913         BT_DBG("chan %p, dest_amp_id %d", chan, dest_amp_id);
4914
4915         ident = l2cap_get_ident(chan->conn);
4916         chan->ident = ident;
4917
4918         req.icid = cpu_to_le16(chan->scid);
4919         req.dest_amp_id = dest_amp_id;
4920
4921         l2cap_send_cmd(chan->conn, ident, L2CAP_MOVE_CHAN_REQ, sizeof(req),
4922                        &req);
4923
4924         __set_chan_timer(chan, L2CAP_MOVE_TIMEOUT);
4925 }
4926
4927 static void l2cap_send_move_chan_rsp(struct l2cap_chan *chan, u16 result)
4928 {
4929         struct l2cap_move_chan_rsp rsp;
4930
4931         BT_DBG("chan %p, result 0x%4.4x", chan, result);
4932
4933         rsp.icid = cpu_to_le16(chan->dcid);
4934         rsp.result = cpu_to_le16(result);
4935
4936         l2cap_send_cmd(chan->conn, chan->ident, L2CAP_MOVE_CHAN_RSP,
4937                        sizeof(rsp), &rsp);
4938 }
4939
4940 static void l2cap_send_move_chan_cfm(struct l2cap_chan *chan, u16 result)
4941 {
4942         struct l2cap_move_chan_cfm cfm;
4943
4944         BT_DBG("chan %p, result 0x%4.4x", chan, result);
4945
4946         chan->ident = l2cap_get_ident(chan->conn);
4947
4948         cfm.icid = cpu_to_le16(chan->scid);
4949         cfm.result = cpu_to_le16(result);
4950
4951         l2cap_send_cmd(chan->conn, chan->ident, L2CAP_MOVE_CHAN_CFM,
4952                        sizeof(cfm), &cfm);
4953
4954         __set_chan_timer(chan, L2CAP_MOVE_TIMEOUT);
4955 }
4956
4957 static void l2cap_send_move_chan_cfm_icid(struct l2cap_conn *conn, u16 icid)
4958 {
4959         struct l2cap_move_chan_cfm cfm;
4960
4961         BT_DBG("conn %p, icid 0x%4.4x", conn, icid);
4962
4963         cfm.icid = cpu_to_le16(icid);
4964         cfm.result = cpu_to_le16(L2CAP_MC_UNCONFIRMED);
4965
4966         l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_MOVE_CHAN_CFM,
4967                        sizeof(cfm), &cfm);
4968 }
4969
4970 static void l2cap_send_move_chan_cfm_rsp(struct l2cap_conn *conn, u8 ident,
4971                                          u16 icid)
4972 {
4973         struct l2cap_move_chan_cfm_rsp rsp;
4974
4975         BT_DBG("icid 0x%4.4x", icid);
4976
4977         rsp.icid = cpu_to_le16(icid);
4978         l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM_RSP, sizeof(rsp), &rsp);
4979 }
4980
4981 static void __release_logical_link(struct l2cap_chan *chan)
4982 {
4983         chan->hs_hchan = NULL;
4984         chan->hs_hcon = NULL;
4985
4986         /* Placeholder - release the logical link */
4987 }
4988
4989 static void l2cap_logical_fail(struct l2cap_chan *chan)
4990 {
4991         /* Logical link setup failed */
4992         if (chan->state != BT_CONNECTED) {
4993                 /* Create channel failure, disconnect */
4994                 l2cap_send_disconn_req(chan, ECONNRESET);
4995                 return;
4996         }
4997
4998         switch (chan->move_role) {
4999         case L2CAP_MOVE_ROLE_RESPONDER:
5000                 l2cap_move_done(chan);
5001                 l2cap_send_move_chan_rsp(chan, L2CAP_MR_NOT_SUPP);
5002                 break;
5003         case L2CAP_MOVE_ROLE_INITIATOR:
5004                 if (chan->move_state == L2CAP_MOVE_WAIT_LOGICAL_COMP ||
5005                     chan->move_state == L2CAP_MOVE_WAIT_LOGICAL_CFM) {
5006                         /* Remote has only sent pending or
5007                          * success responses, clean up
5008                          */
5009                         l2cap_move_done(chan);
5010                 }
5011
5012                 /* Other amp move states imply that the move
5013                  * has already aborted
5014                  */
5015                 l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
5016                 break;
5017         }
5018 }
5019
5020 static void l2cap_logical_finish_create(struct l2cap_chan *chan,
5021                                         struct hci_chan *hchan)
5022 {
5023         struct l2cap_conf_rsp rsp;
5024
5025         chan->hs_hchan = hchan;
5026         chan->hs_hcon->l2cap_data = chan->conn;
5027
5028         l2cap_send_efs_conf_rsp(chan, &rsp, chan->ident, 0);
5029
5030         if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
5031                 int err;
5032
5033                 set_default_fcs(chan);
5034
5035                 err = l2cap_ertm_init(chan);
5036                 if (err < 0)
5037                         l2cap_send_disconn_req(chan, -err);
5038                 else
5039                         l2cap_chan_ready(chan);
5040         }
5041 }
5042
5043 static void l2cap_logical_finish_move(struct l2cap_chan *chan,
5044                                       struct hci_chan *hchan)
5045 {
5046         chan->hs_hcon = hchan->conn;
5047         chan->hs_hcon->l2cap_data = chan->conn;
5048
5049         BT_DBG("move_state %d", chan->move_state);
5050
5051         switch (chan->move_state) {
5052         case L2CAP_MOVE_WAIT_LOGICAL_COMP:
5053                 /* Move confirm will be sent after a success
5054                  * response is received
5055                  */
5056                 chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
5057                 break;
5058         case L2CAP_MOVE_WAIT_LOGICAL_CFM:
5059                 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
5060                         chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
5061                 } else if (chan->move_role == L2CAP_MOVE_ROLE_INITIATOR) {
5062                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM_RSP;
5063                         l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
5064                 } else if (chan->move_role == L2CAP_MOVE_ROLE_RESPONDER) {
5065                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
5066                         l2cap_send_move_chan_rsp(chan, L2CAP_MR_SUCCESS);
5067                 }
5068                 break;
5069         default:
5070                 /* Move was not in expected state, free the channel */
5071                 __release_logical_link(chan);
5072
5073                 chan->move_state = L2CAP_MOVE_STABLE;
5074         }
5075 }
5076
5077 /* Call with chan locked */
5078 void l2cap_logical_cfm(struct l2cap_chan *chan, struct hci_chan *hchan,
5079                        u8 status)
5080 {
5081         BT_DBG("chan %p, hchan %p, status %d", chan, hchan, status);
5082
5083         if (status) {
5084                 l2cap_logical_fail(chan);
5085                 __release_logical_link(chan);
5086                 return;
5087         }
5088
5089         if (chan->state != BT_CONNECTED) {
5090                 /* Ignore logical link if channel is on BR/EDR */
5091                 if (chan->local_amp_id != AMP_ID_BREDR)
5092                         l2cap_logical_finish_create(chan, hchan);
5093         } else {
5094                 l2cap_logical_finish_move(chan, hchan);
5095         }
5096 }
5097
5098 void l2cap_move_start(struct l2cap_chan *chan)
5099 {
5100         BT_DBG("chan %p", chan);
5101
5102         if (chan->local_amp_id == AMP_ID_BREDR) {
5103                 if (chan->chan_policy != BT_CHANNEL_POLICY_AMP_PREFERRED)
5104                         return;
5105                 chan->move_role = L2CAP_MOVE_ROLE_INITIATOR;
5106                 chan->move_state = L2CAP_MOVE_WAIT_PREPARE;
5107                 /* Placeholder - start physical link setup */
5108         } else {
5109                 chan->move_role = L2CAP_MOVE_ROLE_INITIATOR;
5110                 chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
5111                 chan->move_id = 0;
5112                 l2cap_move_setup(chan);
5113                 l2cap_send_move_chan_req(chan, 0);
5114         }
5115 }
5116
5117 static void l2cap_do_create(struct l2cap_chan *chan, int result,
5118                             u8 local_amp_id, u8 remote_amp_id)
5119 {
5120         BT_DBG("chan %p state %s %u -> %u", chan, state_to_string(chan->state),
5121                local_amp_id, remote_amp_id);
5122
5123         chan->fcs = L2CAP_FCS_NONE;
5124
5125         /* Outgoing channel on AMP */
5126         if (chan->state == BT_CONNECT) {
5127                 if (result == L2CAP_CR_SUCCESS) {
5128                         chan->local_amp_id = local_amp_id;
5129                         l2cap_send_create_chan_req(chan, remote_amp_id);
5130                 } else {
5131                         /* Revert to BR/EDR connect */
5132                         l2cap_send_conn_req(chan);
5133                 }
5134
5135                 return;
5136         }
5137
5138         /* Incoming channel on AMP */
5139         if (__l2cap_no_conn_pending(chan)) {
5140                 struct l2cap_conn_rsp rsp;
5141                 char buf[128];
5142                 rsp.scid = cpu_to_le16(chan->dcid);
5143                 rsp.dcid = cpu_to_le16(chan->scid);
5144
5145                 if (result == L2CAP_CR_SUCCESS) {
5146                         /* Send successful response */
5147                         rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
5148                         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
5149                 } else {
5150                         /* Send negative response */
5151                         rsp.result = cpu_to_le16(L2CAP_CR_NO_MEM);
5152                         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
5153                 }
5154
5155                 l2cap_send_cmd(chan->conn, chan->ident, L2CAP_CREATE_CHAN_RSP,
5156                                sizeof(rsp), &rsp);
5157
5158                 if (result == L2CAP_CR_SUCCESS) {
5159                         l2cap_state_change(chan, BT_CONFIG);
5160                         set_bit(CONF_REQ_SENT, &chan->conf_state);
5161                         l2cap_send_cmd(chan->conn, l2cap_get_ident(chan->conn),
5162                                        L2CAP_CONF_REQ,
5163                                        l2cap_build_conf_req(chan, buf, sizeof(buf)), buf);
5164                         chan->num_conf_req++;
5165                 }
5166         }
5167 }
5168
5169 static void l2cap_do_move_initiate(struct l2cap_chan *chan, u8 local_amp_id,
5170                                    u8 remote_amp_id)
5171 {
5172         l2cap_move_setup(chan);
5173         chan->move_id = local_amp_id;
5174         chan->move_state = L2CAP_MOVE_WAIT_RSP;
5175
5176         l2cap_send_move_chan_req(chan, remote_amp_id);
5177 }
5178
5179 static void l2cap_do_move_respond(struct l2cap_chan *chan, int result)
5180 {
5181         struct hci_chan *hchan = NULL;
5182
5183         /* Placeholder - get hci_chan for logical link */
5184
5185         if (hchan) {
5186                 if (hchan->state == BT_CONNECTED) {
5187                         /* Logical link is ready to go */
5188                         chan->hs_hcon = hchan->conn;
5189                         chan->hs_hcon->l2cap_data = chan->conn;
5190                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
5191                         l2cap_send_move_chan_rsp(chan, L2CAP_MR_SUCCESS);
5192
5193                         l2cap_logical_cfm(chan, hchan, L2CAP_MR_SUCCESS);
5194                 } else {
5195                         /* Wait for logical link to be ready */
5196                         chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
5197                 }
5198         } else {
5199                 /* Logical link not available */
5200                 l2cap_send_move_chan_rsp(chan, L2CAP_MR_NOT_ALLOWED);
5201         }
5202 }
5203
5204 static void l2cap_do_move_cancel(struct l2cap_chan *chan, int result)
5205 {
5206         if (chan->move_role == L2CAP_MOVE_ROLE_RESPONDER) {
5207                 u8 rsp_result;
5208                 if (result == -EINVAL)
5209                         rsp_result = L2CAP_MR_BAD_ID;
5210                 else
5211                         rsp_result = L2CAP_MR_NOT_ALLOWED;
5212
5213                 l2cap_send_move_chan_rsp(chan, rsp_result);
5214         }
5215
5216         chan->move_role = L2CAP_MOVE_ROLE_NONE;
5217         chan->move_state = L2CAP_MOVE_STABLE;
5218
5219         /* Restart data transmission */
5220         l2cap_ertm_send(chan);
5221 }
5222
5223 /* Invoke with locked chan */
5224 void __l2cap_physical_cfm(struct l2cap_chan *chan, int result)
5225 {
5226         u8 local_amp_id = chan->local_amp_id;
5227         u8 remote_amp_id = chan->remote_amp_id;
5228
5229         BT_DBG("chan %p, result %d, local_amp_id %d, remote_amp_id %d",
5230                chan, result, local_amp_id, remote_amp_id);
5231
5232         if (chan->state == BT_DISCONN || chan->state == BT_CLOSED)
5233                 return;
5234
5235         if (chan->state != BT_CONNECTED) {
5236                 l2cap_do_create(chan, result, local_amp_id, remote_amp_id);
5237         } else if (result != L2CAP_MR_SUCCESS) {
5238                 l2cap_do_move_cancel(chan, result);
5239         } else {
5240                 switch (chan->move_role) {
5241                 case L2CAP_MOVE_ROLE_INITIATOR:
5242                         l2cap_do_move_initiate(chan, local_amp_id,
5243                                                remote_amp_id);
5244                         break;
5245                 case L2CAP_MOVE_ROLE_RESPONDER:
5246                         l2cap_do_move_respond(chan, result);
5247                         break;
5248                 default:
5249                         l2cap_do_move_cancel(chan, result);
5250                         break;
5251                 }
5252         }
5253 }
5254
5255 static inline int l2cap_move_channel_req(struct l2cap_conn *conn,
5256                                          struct l2cap_cmd_hdr *cmd,
5257                                          u16 cmd_len, void *data)
5258 {
5259         struct l2cap_move_chan_req *req = data;
5260         struct l2cap_move_chan_rsp rsp;
5261         struct l2cap_chan *chan;
5262         u16 icid = 0;
5263         u16 result = L2CAP_MR_NOT_ALLOWED;
5264
5265         if (cmd_len != sizeof(*req))
5266                 return -EPROTO;
5267
5268         icid = le16_to_cpu(req->icid);
5269
5270         BT_DBG("icid 0x%4.4x, dest_amp_id %d", icid, req->dest_amp_id);
5271
5272         if (!(conn->local_fixed_chan & L2CAP_FC_A2MP))
5273                 return -EINVAL;
5274
5275         chan = l2cap_get_chan_by_dcid(conn, icid);
5276         if (!chan) {
5277                 rsp.icid = cpu_to_le16(icid);
5278                 rsp.result = cpu_to_le16(L2CAP_MR_NOT_ALLOWED);
5279                 l2cap_send_cmd(conn, cmd->ident, L2CAP_MOVE_CHAN_RSP,
5280                                sizeof(rsp), &rsp);
5281                 return 0;
5282         }
5283
5284         chan->ident = cmd->ident;
5285
5286         if (chan->scid < L2CAP_CID_DYN_START ||
5287             chan->chan_policy == BT_CHANNEL_POLICY_BREDR_ONLY ||
5288             (chan->mode != L2CAP_MODE_ERTM &&
5289              chan->mode != L2CAP_MODE_STREAMING)) {
5290                 result = L2CAP_MR_NOT_ALLOWED;
5291                 goto send_move_response;
5292         }
5293
5294         if (chan->local_amp_id == req->dest_amp_id) {
5295                 result = L2CAP_MR_SAME_ID;
5296                 goto send_move_response;
5297         }
5298
5299         if (req->dest_amp_id != AMP_ID_BREDR) {
5300                 struct hci_dev *hdev;
5301                 hdev = hci_dev_get(req->dest_amp_id);
5302                 if (!hdev || hdev->dev_type != HCI_AMP ||
5303                     !test_bit(HCI_UP, &hdev->flags)) {
5304                         if (hdev)
5305                                 hci_dev_put(hdev);
5306
5307                         result = L2CAP_MR_BAD_ID;
5308                         goto send_move_response;
5309                 }
5310                 hci_dev_put(hdev);
5311         }
5312
5313         /* Detect a move collision.  Only send a collision response
5314          * if this side has "lost", otherwise proceed with the move.
5315          * The winner has the larger bd_addr.
5316          */
5317         if ((__chan_is_moving(chan) ||
5318              chan->move_role != L2CAP_MOVE_ROLE_NONE) &&
5319             bacmp(&conn->hcon->src, &conn->hcon->dst) > 0) {
5320                 result = L2CAP_MR_COLLISION;
5321                 goto send_move_response;
5322         }
5323
5324         chan->move_role = L2CAP_MOVE_ROLE_RESPONDER;
5325         l2cap_move_setup(chan);
5326         chan->move_id = req->dest_amp_id;
5327
5328         if (req->dest_amp_id == AMP_ID_BREDR) {
5329                 /* Moving to BR/EDR */
5330                 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
5331                         chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
5332                         result = L2CAP_MR_PEND;
5333                 } else {
5334                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
5335                         result = L2CAP_MR_SUCCESS;
5336                 }
5337         } else {
5338                 chan->move_state = L2CAP_MOVE_WAIT_PREPARE;
5339                 /* Placeholder - uncomment when amp functions are available */
5340                 /*amp_accept_physical(chan, req->dest_amp_id);*/
5341                 result = L2CAP_MR_PEND;
5342         }
5343
5344 send_move_response:
5345         l2cap_send_move_chan_rsp(chan, result);
5346
5347         l2cap_chan_unlock(chan);
5348         l2cap_chan_put(chan);
5349
5350         return 0;
5351 }
5352
5353 static void l2cap_move_continue(struct l2cap_conn *conn, u16 icid, u16 result)
5354 {
5355         struct l2cap_chan *chan;
5356         struct hci_chan *hchan = NULL;
5357
5358         chan = l2cap_get_chan_by_scid(conn, icid);
5359         if (!chan) {
5360                 l2cap_send_move_chan_cfm_icid(conn, icid);
5361                 return;
5362         }
5363
5364         __clear_chan_timer(chan);
5365         if (result == L2CAP_MR_PEND)
5366                 __set_chan_timer(chan, L2CAP_MOVE_ERTX_TIMEOUT);
5367
5368         switch (chan->move_state) {
5369         case L2CAP_MOVE_WAIT_LOGICAL_COMP:
5370                 /* Move confirm will be sent when logical link
5371                  * is complete.
5372                  */
5373                 chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
5374                 break;
5375         case L2CAP_MOVE_WAIT_RSP_SUCCESS:
5376                 if (result == L2CAP_MR_PEND) {
5377                         break;
5378                 } else if (test_bit(CONN_LOCAL_BUSY,
5379                                     &chan->conn_state)) {
5380                         chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
5381                 } else {
5382                         /* Logical link is up or moving to BR/EDR,
5383                          * proceed with move
5384                          */
5385                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM_RSP;
5386                         l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
5387                 }
5388                 break;
5389         case L2CAP_MOVE_WAIT_RSP:
5390                 /* Moving to AMP */
5391                 if (result == L2CAP_MR_SUCCESS) {
5392                         /* Remote is ready, send confirm immediately
5393                          * after logical link is ready
5394                          */
5395                         chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
5396                 } else {
5397                         /* Both logical link and move success
5398                          * are required to confirm
5399                          */
5400                         chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_COMP;
5401                 }
5402
5403                 /* Placeholder - get hci_chan for logical link */
5404                 if (!hchan) {
5405                         /* Logical link not available */
5406                         l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
5407                         break;
5408                 }
5409
5410                 /* If the logical link is not yet connected, do not
5411                  * send confirmation.
5412                  */
5413                 if (hchan->state != BT_CONNECTED)
5414                         break;
5415
5416                 /* Logical link is already ready to go */
5417
5418                 chan->hs_hcon = hchan->conn;
5419                 chan->hs_hcon->l2cap_data = chan->conn;
5420
5421                 if (result == L2CAP_MR_SUCCESS) {
5422                         /* Can confirm now */
5423                         l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
5424                 } else {
5425                         /* Now only need move success
5426                          * to confirm
5427                          */
5428                         chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
5429                 }
5430
5431                 l2cap_logical_cfm(chan, hchan, L2CAP_MR_SUCCESS);
5432                 break;
5433         default:
5434                 /* Any other amp move state means the move failed. */
5435                 chan->move_id = chan->local_amp_id;
5436                 l2cap_move_done(chan);
5437                 l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
5438         }
5439
5440         l2cap_chan_unlock(chan);
5441         l2cap_chan_put(chan);
5442 }
5443
5444 static void l2cap_move_fail(struct l2cap_conn *conn, u8 ident, u16 icid,
5445                             u16 result)
5446 {
5447         struct l2cap_chan *chan;
5448
5449         chan = l2cap_get_chan_by_ident(conn, ident);
5450         if (!chan) {
5451                 /* Could not locate channel, icid is best guess */
5452                 l2cap_send_move_chan_cfm_icid(conn, icid);
5453                 return;
5454         }
5455
5456         __clear_chan_timer(chan);
5457
5458         if (chan->move_role == L2CAP_MOVE_ROLE_INITIATOR) {
5459                 if (result == L2CAP_MR_COLLISION) {
5460                         chan->move_role = L2CAP_MOVE_ROLE_RESPONDER;
5461                 } else {
5462                         /* Cleanup - cancel move */
5463                         chan->move_id = chan->local_amp_id;
5464                         l2cap_move_done(chan);
5465                 }
5466         }
5467
5468         l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
5469
5470         l2cap_chan_unlock(chan);
5471         l2cap_chan_put(chan);
5472 }
5473
5474 static int l2cap_move_channel_rsp(struct l2cap_conn *conn,
5475                                   struct l2cap_cmd_hdr *cmd,
5476                                   u16 cmd_len, void *data)
5477 {
5478         struct l2cap_move_chan_rsp *rsp = data;
5479         u16 icid, result;
5480
5481         if (cmd_len != sizeof(*rsp))
5482                 return -EPROTO;
5483
5484         icid = le16_to_cpu(rsp->icid);
5485         result = le16_to_cpu(rsp->result);
5486
5487         BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result);
5488
5489         if (result == L2CAP_MR_SUCCESS || result == L2CAP_MR_PEND)
5490                 l2cap_move_continue(conn, icid, result);
5491         else
5492                 l2cap_move_fail(conn, cmd->ident, icid, result);
5493
5494         return 0;
5495 }
5496
5497 static int l2cap_move_channel_confirm(struct l2cap_conn *conn,
5498                                       struct l2cap_cmd_hdr *cmd,
5499                                       u16 cmd_len, void *data)
5500 {
5501         struct l2cap_move_chan_cfm *cfm = data;
5502         struct l2cap_chan *chan;
5503         u16 icid, result;
5504
5505         if (cmd_len != sizeof(*cfm))
5506                 return -EPROTO;
5507
5508         icid = le16_to_cpu(cfm->icid);
5509         result = le16_to_cpu(cfm->result);
5510
5511         BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result);
5512
5513         chan = l2cap_get_chan_by_dcid(conn, icid);
5514         if (!chan) {
5515                 /* Spec requires a response even if the icid was not found */
5516                 l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid);
5517                 return 0;
5518         }
5519
5520         if (chan->move_state == L2CAP_MOVE_WAIT_CONFIRM) {
5521                 if (result == L2CAP_MC_CONFIRMED) {
5522                         chan->local_amp_id = chan->move_id;
5523                         if (chan->local_amp_id == AMP_ID_BREDR)
5524                                 __release_logical_link(chan);
5525                 } else {
5526                         chan->move_id = chan->local_amp_id;
5527                 }
5528
5529                 l2cap_move_done(chan);
5530         }
5531
5532         l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid);
5533
5534         l2cap_chan_unlock(chan);
5535         l2cap_chan_put(chan);
5536
5537         return 0;
5538 }
5539
5540 static inline int l2cap_move_channel_confirm_rsp(struct l2cap_conn *conn,
5541                                                  struct l2cap_cmd_hdr *cmd,
5542                                                  u16 cmd_len, void *data)
5543 {
5544         struct l2cap_move_chan_cfm_rsp *rsp = data;
5545         struct l2cap_chan *chan;
5546         u16 icid;
5547
5548         if (cmd_len != sizeof(*rsp))
5549                 return -EPROTO;
5550
5551         icid = le16_to_cpu(rsp->icid);
5552
5553         BT_DBG("icid 0x%4.4x", icid);
5554
5555         chan = l2cap_get_chan_by_scid(conn, icid);
5556         if (!chan)
5557                 return 0;
5558
5559         __clear_chan_timer(chan);
5560
5561         if (chan->move_state == L2CAP_MOVE_WAIT_CONFIRM_RSP) {
5562                 chan->local_amp_id = chan->move_id;
5563
5564                 if (chan->local_amp_id == AMP_ID_BREDR && chan->hs_hchan)
5565                         __release_logical_link(chan);
5566
5567                 l2cap_move_done(chan);
5568         }
5569
5570         l2cap_chan_unlock(chan);
5571         l2cap_chan_put(chan);
5572
5573         return 0;
5574 }
5575
5576 static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
5577                                               struct l2cap_cmd_hdr *cmd,
5578                                               u16 cmd_len, u8 *data)
5579 {
5580         struct hci_conn *hcon = conn->hcon;
5581         struct l2cap_conn_param_update_req *req;
5582         struct l2cap_conn_param_update_rsp rsp;
5583         u16 min, max, latency, to_multiplier;
5584         int err;
5585
5586         if (hcon->role != HCI_ROLE_MASTER)
5587                 return -EINVAL;
5588
5589         if (cmd_len != sizeof(struct l2cap_conn_param_update_req))
5590                 return -EPROTO;
5591
5592         req = (struct l2cap_conn_param_update_req *) data;
5593         min             = __le16_to_cpu(req->min);
5594         max             = __le16_to_cpu(req->max);
5595         latency         = __le16_to_cpu(req->latency);
5596         to_multiplier   = __le16_to_cpu(req->to_multiplier);
5597
5598         BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x",
5599                min, max, latency, to_multiplier);
5600
5601         memset(&rsp, 0, sizeof(rsp));
5602
5603         err = hci_check_conn_params(min, max, latency, to_multiplier);
5604         if (err)
5605                 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_REJECTED);
5606         else
5607                 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED);
5608
5609         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP,
5610                        sizeof(rsp), &rsp);
5611
5612         if (!err) {
5613                 u8 store_hint;
5614
5615                 store_hint = hci_le_conn_update(hcon, min, max, latency,
5616                                                 to_multiplier);
5617                 mgmt_new_conn_param(hcon->hdev, &hcon->dst, hcon->dst_type,
5618                                     store_hint, min, max, latency,
5619                                     to_multiplier);
5620
5621         }
5622
5623         return 0;
5624 }
5625
5626 static int l2cap_le_connect_rsp(struct l2cap_conn *conn,
5627                                 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5628                                 u8 *data)
5629 {
5630         struct l2cap_le_conn_rsp *rsp = (struct l2cap_le_conn_rsp *) data;
5631         struct hci_conn *hcon = conn->hcon;
5632         u16 dcid, mtu, mps, credits, result;
5633         struct l2cap_chan *chan;
5634         int err, sec_level;
5635
5636         if (cmd_len < sizeof(*rsp))
5637                 return -EPROTO;
5638
5639         dcid    = __le16_to_cpu(rsp->dcid);
5640         mtu     = __le16_to_cpu(rsp->mtu);
5641         mps     = __le16_to_cpu(rsp->mps);
5642         credits = __le16_to_cpu(rsp->credits);
5643         result  = __le16_to_cpu(rsp->result);
5644
5645         if (result == L2CAP_CR_LE_SUCCESS && (mtu < 23 || mps < 23 ||
5646                                            dcid < L2CAP_CID_DYN_START ||
5647                                            dcid > L2CAP_CID_LE_DYN_END))
5648                 return -EPROTO;
5649
5650         BT_DBG("dcid 0x%4.4x mtu %u mps %u credits %u result 0x%2.2x",
5651                dcid, mtu, mps, credits, result);
5652
5653         mutex_lock(&conn->chan_lock);
5654
5655         chan = __l2cap_get_chan_by_ident(conn, cmd->ident);
5656         if (!chan) {
5657                 err = -EBADSLT;
5658                 goto unlock;
5659         }
5660
5661         err = 0;
5662
5663         l2cap_chan_lock(chan);
5664
5665         switch (result) {
5666         case L2CAP_CR_LE_SUCCESS:
5667                 if (__l2cap_get_chan_by_dcid(conn, dcid)) {
5668                         err = -EBADSLT;
5669                         break;
5670                 }
5671
5672                 chan->ident = 0;
5673                 chan->dcid = dcid;
5674                 chan->omtu = mtu;
5675                 chan->remote_mps = mps;
5676                 chan->tx_credits = credits;
5677                 l2cap_chan_ready(chan);
5678                 break;
5679
5680         case L2CAP_CR_LE_AUTHENTICATION:
5681         case L2CAP_CR_LE_ENCRYPTION:
5682                 /* If we already have MITM protection we can't do
5683                  * anything.
5684                  */
5685                 if (hcon->sec_level > BT_SECURITY_MEDIUM) {
5686                         l2cap_chan_del(chan, ECONNREFUSED);
5687                         break;
5688                 }
5689
5690                 sec_level = hcon->sec_level + 1;
5691                 if (chan->sec_level < sec_level)
5692                         chan->sec_level = sec_level;
5693
5694                 /* We'll need to send a new Connect Request */
5695                 clear_bit(FLAG_LE_CONN_REQ_SENT, &chan->flags);
5696
5697                 smp_conn_security(hcon, chan->sec_level);
5698                 break;
5699
5700         default:
5701                 l2cap_chan_del(chan, ECONNREFUSED);
5702                 break;
5703         }
5704
5705         l2cap_chan_unlock(chan);
5706
5707 unlock:
5708         mutex_unlock(&conn->chan_lock);
5709
5710         return err;
5711 }
5712
5713 static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
5714                                       struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5715                                       u8 *data)
5716 {
5717         int err = 0;
5718
5719         switch (cmd->code) {
5720         case L2CAP_COMMAND_REJ:
5721                 l2cap_command_rej(conn, cmd, cmd_len, data);
5722                 break;
5723
5724         case L2CAP_CONN_REQ:
5725                 err = l2cap_connect_req(conn, cmd, cmd_len, data);
5726                 break;
5727
5728         case L2CAP_CONN_RSP:
5729         case L2CAP_CREATE_CHAN_RSP:
5730                 l2cap_connect_create_rsp(conn, cmd, cmd_len, data);
5731                 break;
5732
5733         case L2CAP_CONF_REQ:
5734                 err = l2cap_config_req(conn, cmd, cmd_len, data);
5735                 break;
5736
5737         case L2CAP_CONF_RSP:
5738                 l2cap_config_rsp(conn, cmd, cmd_len, data);
5739                 break;
5740
5741         case L2CAP_DISCONN_REQ:
5742                 err = l2cap_disconnect_req(conn, cmd, cmd_len, data);
5743                 break;
5744
5745         case L2CAP_DISCONN_RSP:
5746                 l2cap_disconnect_rsp(conn, cmd, cmd_len, data);
5747                 break;
5748
5749         case L2CAP_ECHO_REQ:
5750                 l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data);
5751                 break;
5752
5753         case L2CAP_ECHO_RSP:
5754                 break;
5755
5756         case L2CAP_INFO_REQ:
5757                 err = l2cap_information_req(conn, cmd, cmd_len, data);
5758                 break;
5759
5760         case L2CAP_INFO_RSP:
5761                 l2cap_information_rsp(conn, cmd, cmd_len, data);
5762                 break;
5763
5764         case L2CAP_CREATE_CHAN_REQ:
5765                 err = l2cap_create_channel_req(conn, cmd, cmd_len, data);
5766                 break;
5767
5768         case L2CAP_MOVE_CHAN_REQ:
5769                 err = l2cap_move_channel_req(conn, cmd, cmd_len, data);
5770                 break;
5771
5772         case L2CAP_MOVE_CHAN_RSP:
5773                 l2cap_move_channel_rsp(conn, cmd, cmd_len, data);
5774                 break;
5775
5776         case L2CAP_MOVE_CHAN_CFM:
5777                 err = l2cap_move_channel_confirm(conn, cmd, cmd_len, data);
5778                 break;
5779
5780         case L2CAP_MOVE_CHAN_CFM_RSP:
5781                 l2cap_move_channel_confirm_rsp(conn, cmd, cmd_len, data);
5782                 break;
5783
5784         default:
5785                 BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code);
5786                 err = -EINVAL;
5787                 break;
5788         }
5789
5790         return err;
5791 }
5792
5793 static int l2cap_le_connect_req(struct l2cap_conn *conn,
5794                                 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5795                                 u8 *data)
5796 {
5797         struct l2cap_le_conn_req *req = (struct l2cap_le_conn_req *) data;
5798         struct l2cap_le_conn_rsp rsp;
5799         struct l2cap_chan *chan, *pchan;
5800         u16 dcid, scid, credits, mtu, mps;
5801         __le16 psm;
5802         u8 result;
5803
5804         if (cmd_len != sizeof(*req))
5805                 return -EPROTO;
5806
5807         scid = __le16_to_cpu(req->scid);
5808         mtu  = __le16_to_cpu(req->mtu);
5809         mps  = __le16_to_cpu(req->mps);
5810         psm  = req->psm;
5811         dcid = 0;
5812         credits = 0;
5813
5814         if (mtu < 23 || mps < 23)
5815                 return -EPROTO;
5816
5817         BT_DBG("psm 0x%2.2x scid 0x%4.4x mtu %u mps %u", __le16_to_cpu(psm),
5818                scid, mtu, mps);
5819
5820         /* BLUETOOTH CORE SPECIFICATION Version 5.3 | Vol 3, Part A
5821          * page 1059:
5822          *
5823          * Valid range: 0x0001-0x00ff
5824          *
5825          * Table 4.15: L2CAP_LE_CREDIT_BASED_CONNECTION_REQ SPSM ranges
5826          */
5827         if (!psm || __le16_to_cpu(psm) > L2CAP_PSM_LE_DYN_END) {
5828                 result = L2CAP_CR_LE_BAD_PSM;
5829                 chan = NULL;
5830                 goto response;
5831         }
5832
5833         /* Check if we have socket listening on psm */
5834         pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, &conn->hcon->src,
5835                                          &conn->hcon->dst, LE_LINK);
5836         if (!pchan) {
5837                 result = L2CAP_CR_LE_BAD_PSM;
5838                 chan = NULL;
5839                 goto response;
5840         }
5841
5842         mutex_lock(&conn->chan_lock);
5843         l2cap_chan_lock(pchan);
5844
5845         if (!smp_sufficient_security(conn->hcon, pchan->sec_level,
5846                                      SMP_ALLOW_STK)) {
5847                 result = L2CAP_CR_LE_AUTHENTICATION;
5848                 chan = NULL;
5849                 goto response_unlock;
5850         }
5851
5852         /* Check for valid dynamic CID range */
5853         if (scid < L2CAP_CID_DYN_START || scid > L2CAP_CID_LE_DYN_END) {
5854                 result = L2CAP_CR_LE_INVALID_SCID;
5855                 chan = NULL;
5856                 goto response_unlock;
5857         }
5858
5859         /* Check if we already have channel with that dcid */
5860         if (__l2cap_get_chan_by_dcid(conn, scid)) {
5861                 result = L2CAP_CR_LE_SCID_IN_USE;
5862                 chan = NULL;
5863                 goto response_unlock;
5864         }
5865
5866         chan = pchan->ops->new_connection(pchan);
5867         if (!chan) {
5868                 result = L2CAP_CR_LE_NO_MEM;
5869                 goto response_unlock;
5870         }
5871
5872         bacpy(&chan->src, &conn->hcon->src);
5873         bacpy(&chan->dst, &conn->hcon->dst);
5874         chan->src_type = bdaddr_src_type(conn->hcon);
5875         chan->dst_type = bdaddr_dst_type(conn->hcon);
5876         chan->psm  = psm;
5877         chan->dcid = scid;
5878         chan->omtu = mtu;
5879         chan->remote_mps = mps;
5880
5881         __l2cap_chan_add(conn, chan);
5882
5883         l2cap_le_flowctl_init(chan, __le16_to_cpu(req->credits));
5884
5885         dcid = chan->scid;
5886         credits = chan->rx_credits;
5887
5888         __set_chan_timer(chan, chan->ops->get_sndtimeo(chan));
5889
5890         chan->ident = cmd->ident;
5891
5892         if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
5893                 l2cap_state_change(chan, BT_CONNECT2);
5894                 /* The following result value is actually not defined
5895                  * for LE CoC but we use it to let the function know
5896                  * that it should bail out after doing its cleanup
5897                  * instead of sending a response.
5898                  */
5899                 result = L2CAP_CR_PEND;
5900                 chan->ops->defer(chan);
5901         } else {
5902                 l2cap_chan_ready(chan);
5903                 result = L2CAP_CR_LE_SUCCESS;
5904         }
5905
5906 response_unlock:
5907         l2cap_chan_unlock(pchan);
5908         mutex_unlock(&conn->chan_lock);
5909         l2cap_chan_put(pchan);
5910
5911         if (result == L2CAP_CR_PEND)
5912                 return 0;
5913
5914 response:
5915         if (chan) {
5916                 rsp.mtu = cpu_to_le16(chan->imtu);
5917                 rsp.mps = cpu_to_le16(chan->mps);
5918         } else {
5919                 rsp.mtu = 0;
5920                 rsp.mps = 0;
5921         }
5922
5923         rsp.dcid    = cpu_to_le16(dcid);
5924         rsp.credits = cpu_to_le16(credits);
5925         rsp.result  = cpu_to_le16(result);
5926
5927         l2cap_send_cmd(conn, cmd->ident, L2CAP_LE_CONN_RSP, sizeof(rsp), &rsp);
5928
5929         return 0;
5930 }
5931
5932 static inline int l2cap_le_credits(struct l2cap_conn *conn,
5933                                    struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5934                                    u8 *data)
5935 {
5936         struct l2cap_le_credits *pkt;
5937         struct l2cap_chan *chan;
5938         u16 cid, credits, max_credits;
5939
5940         if (cmd_len != sizeof(*pkt))
5941                 return -EPROTO;
5942
5943         pkt = (struct l2cap_le_credits *) data;
5944         cid     = __le16_to_cpu(pkt->cid);
5945         credits = __le16_to_cpu(pkt->credits);
5946
5947         BT_DBG("cid 0x%4.4x credits 0x%4.4x", cid, credits);
5948
5949         chan = l2cap_get_chan_by_dcid(conn, cid);
5950         if (!chan)
5951                 return -EBADSLT;
5952
5953         max_credits = LE_FLOWCTL_MAX_CREDITS - chan->tx_credits;
5954         if (credits > max_credits) {
5955                 BT_ERR("LE credits overflow");
5956                 l2cap_send_disconn_req(chan, ECONNRESET);
5957
5958                 /* Return 0 so that we don't trigger an unnecessary
5959                  * command reject packet.
5960                  */
5961                 goto unlock;
5962         }
5963
5964         chan->tx_credits += credits;
5965
5966         /* Resume sending */
5967         l2cap_le_flowctl_send(chan);
5968
5969         if (chan->tx_credits)
5970                 chan->ops->resume(chan);
5971
5972 unlock:
5973         l2cap_chan_unlock(chan);
5974         l2cap_chan_put(chan);
5975
5976         return 0;
5977 }
5978
5979 static inline int l2cap_ecred_conn_req(struct l2cap_conn *conn,
5980                                        struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5981                                        u8 *data)
5982 {
5983         struct l2cap_ecred_conn_req *req = (void *) data;
5984         struct {
5985                 struct l2cap_ecred_conn_rsp rsp;
5986                 __le16 dcid[L2CAP_ECRED_MAX_CID];
5987         } __packed pdu;
5988         struct l2cap_chan *chan, *pchan;
5989         u16 mtu, mps;
5990         __le16 psm;
5991         u8 result, len = 0;
5992         int i, num_scid;
5993         bool defer = false;
5994
5995         if (!enable_ecred)
5996                 return -EINVAL;
5997
5998         if (cmd_len < sizeof(*req) || (cmd_len - sizeof(*req)) % sizeof(u16)) {
5999                 result = L2CAP_CR_LE_INVALID_PARAMS;
6000                 goto response;
6001         }
6002
6003         cmd_len -= sizeof(*req);
6004         num_scid = cmd_len / sizeof(u16);
6005
6006         if (num_scid > ARRAY_SIZE(pdu.dcid)) {
6007                 result = L2CAP_CR_LE_INVALID_PARAMS;
6008                 goto response;
6009         }
6010
6011         mtu  = __le16_to_cpu(req->mtu);
6012         mps  = __le16_to_cpu(req->mps);
6013
6014         if (mtu < L2CAP_ECRED_MIN_MTU || mps < L2CAP_ECRED_MIN_MPS) {
6015                 result = L2CAP_CR_LE_UNACCEPT_PARAMS;
6016                 goto response;
6017         }
6018
6019         psm  = req->psm;
6020
6021         /* BLUETOOTH CORE SPECIFICATION Version 5.3 | Vol 3, Part A
6022          * page 1059:
6023          *
6024          * Valid range: 0x0001-0x00ff
6025          *
6026          * Table 4.15: L2CAP_LE_CREDIT_BASED_CONNECTION_REQ SPSM ranges
6027          */
6028         if (!psm || __le16_to_cpu(psm) > L2CAP_PSM_LE_DYN_END) {
6029                 result = L2CAP_CR_LE_BAD_PSM;
6030                 goto response;
6031         }
6032
6033         BT_DBG("psm 0x%2.2x mtu %u mps %u", __le16_to_cpu(psm), mtu, mps);
6034
6035         memset(&pdu, 0, sizeof(pdu));
6036
6037         /* Check if we have socket listening on psm */
6038         pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, &conn->hcon->src,
6039                                          &conn->hcon->dst, LE_LINK);
6040         if (!pchan) {
6041                 result = L2CAP_CR_LE_BAD_PSM;
6042                 goto response;
6043         }
6044
6045         mutex_lock(&conn->chan_lock);
6046         l2cap_chan_lock(pchan);
6047
6048         if (!smp_sufficient_security(conn->hcon, pchan->sec_level,
6049                                      SMP_ALLOW_STK)) {
6050                 result = L2CAP_CR_LE_AUTHENTICATION;
6051                 goto unlock;
6052         }
6053
6054         result = L2CAP_CR_LE_SUCCESS;
6055
6056         for (i = 0; i < num_scid; i++) {
6057                 u16 scid = __le16_to_cpu(req->scid[i]);
6058
6059                 BT_DBG("scid[%d] 0x%4.4x", i, scid);
6060
6061                 pdu.dcid[i] = 0x0000;
6062                 len += sizeof(*pdu.dcid);
6063
6064                 /* Check for valid dynamic CID range */
6065                 if (scid < L2CAP_CID_DYN_START || scid > L2CAP_CID_LE_DYN_END) {
6066                         result = L2CAP_CR_LE_INVALID_SCID;
6067                         continue;
6068                 }
6069
6070                 /* Check if we already have channel with that dcid */
6071                 if (__l2cap_get_chan_by_dcid(conn, scid)) {
6072                         result = L2CAP_CR_LE_SCID_IN_USE;
6073                         continue;
6074                 }
6075
6076                 chan = pchan->ops->new_connection(pchan);
6077                 if (!chan) {
6078                         result = L2CAP_CR_LE_NO_MEM;
6079                         continue;
6080                 }
6081
6082                 bacpy(&chan->src, &conn->hcon->src);
6083                 bacpy(&chan->dst, &conn->hcon->dst);
6084                 chan->src_type = bdaddr_src_type(conn->hcon);
6085                 chan->dst_type = bdaddr_dst_type(conn->hcon);
6086                 chan->psm  = psm;
6087                 chan->dcid = scid;
6088                 chan->omtu = mtu;
6089                 chan->remote_mps = mps;
6090
6091                 __l2cap_chan_add(conn, chan);
6092
6093                 l2cap_ecred_init(chan, __le16_to_cpu(req->credits));
6094
6095                 /* Init response */
6096                 if (!pdu.rsp.credits) {
6097                         pdu.rsp.mtu = cpu_to_le16(chan->imtu);
6098                         pdu.rsp.mps = cpu_to_le16(chan->mps);
6099                         pdu.rsp.credits = cpu_to_le16(chan->rx_credits);
6100                 }
6101
6102                 pdu.dcid[i] = cpu_to_le16(chan->scid);
6103
6104                 __set_chan_timer(chan, chan->ops->get_sndtimeo(chan));
6105
6106                 chan->ident = cmd->ident;
6107                 chan->mode = L2CAP_MODE_EXT_FLOWCTL;
6108
6109                 if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
6110                         l2cap_state_change(chan, BT_CONNECT2);
6111                         defer = true;
6112                         chan->ops->defer(chan);
6113                 } else {
6114                         l2cap_chan_ready(chan);
6115                 }
6116         }
6117
6118 unlock:
6119         l2cap_chan_unlock(pchan);
6120         mutex_unlock(&conn->chan_lock);
6121         l2cap_chan_put(pchan);
6122
6123 response:
6124         pdu.rsp.result = cpu_to_le16(result);
6125
6126         if (defer)
6127                 return 0;
6128
6129         l2cap_send_cmd(conn, cmd->ident, L2CAP_ECRED_CONN_RSP,
6130                        sizeof(pdu.rsp) + len, &pdu);
6131
6132         return 0;
6133 }
6134
6135 static inline int l2cap_ecred_conn_rsp(struct l2cap_conn *conn,
6136                                        struct l2cap_cmd_hdr *cmd, u16 cmd_len,
6137                                        u8 *data)
6138 {
6139         struct l2cap_ecred_conn_rsp *rsp = (void *) data;
6140         struct hci_conn *hcon = conn->hcon;
6141         u16 mtu, mps, credits, result;
6142         struct l2cap_chan *chan, *tmp;
6143         int err = 0, sec_level;
6144         int i = 0;
6145
6146         if (cmd_len < sizeof(*rsp))
6147                 return -EPROTO;
6148
6149         mtu     = __le16_to_cpu(rsp->mtu);
6150         mps     = __le16_to_cpu(rsp->mps);
6151         credits = __le16_to_cpu(rsp->credits);
6152         result  = __le16_to_cpu(rsp->result);
6153
6154         BT_DBG("mtu %u mps %u credits %u result 0x%4.4x", mtu, mps, credits,
6155                result);
6156
6157         mutex_lock(&conn->chan_lock);
6158
6159         cmd_len -= sizeof(*rsp);
6160
6161         list_for_each_entry_safe(chan, tmp, &conn->chan_l, list) {
6162                 u16 dcid;
6163
6164                 if (chan->ident != cmd->ident ||
6165                     chan->mode != L2CAP_MODE_EXT_FLOWCTL ||
6166                     chan->state == BT_CONNECTED)
6167                         continue;
6168
6169                 l2cap_chan_lock(chan);
6170
6171                 /* Check that there is a dcid for each pending channel */
6172                 if (cmd_len < sizeof(dcid)) {
6173                         l2cap_chan_del(chan, ECONNREFUSED);
6174                         l2cap_chan_unlock(chan);
6175                         continue;
6176                 }
6177
6178                 dcid = __le16_to_cpu(rsp->dcid[i++]);
6179                 cmd_len -= sizeof(u16);
6180
6181                 BT_DBG("dcid[%d] 0x%4.4x", i, dcid);
6182
6183                 /* Check if dcid is already in use */
6184                 if (dcid && __l2cap_get_chan_by_dcid(conn, dcid)) {
6185                         /* If a device receives a
6186                          * L2CAP_CREDIT_BASED_CONNECTION_RSP packet with an
6187                          * already-assigned Destination CID, then both the
6188                          * original channel and the new channel shall be
6189                          * immediately discarded and not used.
6190                          */
6191                         l2cap_chan_del(chan, ECONNREFUSED);
6192                         l2cap_chan_unlock(chan);
6193                         chan = __l2cap_get_chan_by_dcid(conn, dcid);
6194                         l2cap_chan_lock(chan);
6195                         l2cap_chan_del(chan, ECONNRESET);
6196                         l2cap_chan_unlock(chan);
6197                         continue;
6198                 }
6199
6200                 switch (result) {
6201                 case L2CAP_CR_LE_AUTHENTICATION:
6202                 case L2CAP_CR_LE_ENCRYPTION:
6203                         /* If we already have MITM protection we can't do
6204                          * anything.
6205                          */
6206                         if (hcon->sec_level > BT_SECURITY_MEDIUM) {
6207                                 l2cap_chan_del(chan, ECONNREFUSED);
6208                                 break;
6209                         }
6210
6211                         sec_level = hcon->sec_level + 1;
6212                         if (chan->sec_level < sec_level)
6213                                 chan->sec_level = sec_level;
6214
6215                         /* We'll need to send a new Connect Request */
6216                         clear_bit(FLAG_ECRED_CONN_REQ_SENT, &chan->flags);
6217
6218                         smp_conn_security(hcon, chan->sec_level);
6219                         break;
6220
6221                 case L2CAP_CR_LE_BAD_PSM:
6222                         l2cap_chan_del(chan, ECONNREFUSED);
6223                         break;
6224
6225                 default:
6226                         /* If dcid was not set it means channels was refused */
6227                         if (!dcid) {
6228                                 l2cap_chan_del(chan, ECONNREFUSED);
6229                                 break;
6230                         }
6231
6232                         chan->ident = 0;
6233                         chan->dcid = dcid;
6234                         chan->omtu = mtu;
6235                         chan->remote_mps = mps;
6236                         chan->tx_credits = credits;
6237                         l2cap_chan_ready(chan);
6238                         break;
6239                 }
6240
6241                 l2cap_chan_unlock(chan);
6242         }
6243
6244         mutex_unlock(&conn->chan_lock);
6245
6246         return err;
6247 }
6248
6249 static inline int l2cap_ecred_reconf_req(struct l2cap_conn *conn,
6250                                          struct l2cap_cmd_hdr *cmd, u16 cmd_len,
6251                                          u8 *data)
6252 {
6253         struct l2cap_ecred_reconf_req *req = (void *) data;
6254         struct l2cap_ecred_reconf_rsp rsp;
6255         u16 mtu, mps, result;
6256         struct l2cap_chan *chan;
6257         int i, num_scid;
6258
6259         if (!enable_ecred)
6260                 return -EINVAL;
6261
6262         if (cmd_len < sizeof(*req) || cmd_len - sizeof(*req) % sizeof(u16)) {
6263                 result = L2CAP_CR_LE_INVALID_PARAMS;
6264                 goto respond;
6265         }
6266
6267         mtu = __le16_to_cpu(req->mtu);
6268         mps = __le16_to_cpu(req->mps);
6269
6270         BT_DBG("mtu %u mps %u", mtu, mps);
6271
6272         if (mtu < L2CAP_ECRED_MIN_MTU) {
6273                 result = L2CAP_RECONF_INVALID_MTU;
6274                 goto respond;
6275         }
6276
6277         if (mps < L2CAP_ECRED_MIN_MPS) {
6278                 result = L2CAP_RECONF_INVALID_MPS;
6279                 goto respond;
6280         }
6281
6282         cmd_len -= sizeof(*req);
6283         num_scid = cmd_len / sizeof(u16);
6284         result = L2CAP_RECONF_SUCCESS;
6285
6286         for (i = 0; i < num_scid; i++) {
6287                 u16 scid;
6288
6289                 scid = __le16_to_cpu(req->scid[i]);
6290                 if (!scid)
6291                         return -EPROTO;
6292
6293                 chan = __l2cap_get_chan_by_dcid(conn, scid);
6294                 if (!chan)
6295                         continue;
6296
6297                 /* If the MTU value is decreased for any of the included
6298                  * channels, then the receiver shall disconnect all
6299                  * included channels.
6300                  */
6301                 if (chan->omtu > mtu) {
6302                         BT_ERR("chan %p decreased MTU %u -> %u", chan,
6303                                chan->omtu, mtu);
6304                         result = L2CAP_RECONF_INVALID_MTU;
6305                 }
6306
6307                 chan->omtu = mtu;
6308                 chan->remote_mps = mps;
6309         }
6310
6311 respond:
6312         rsp.result = cpu_to_le16(result);
6313
6314         l2cap_send_cmd(conn, cmd->ident, L2CAP_ECRED_RECONF_RSP, sizeof(rsp),
6315                        &rsp);
6316
6317         return 0;
6318 }
6319
6320 static inline int l2cap_ecred_reconf_rsp(struct l2cap_conn *conn,
6321                                          struct l2cap_cmd_hdr *cmd, u16 cmd_len,
6322                                          u8 *data)
6323 {
6324         struct l2cap_chan *chan, *tmp;
6325         struct l2cap_ecred_conn_rsp *rsp = (void *) data;
6326         u16 result;
6327
6328         if (cmd_len < sizeof(*rsp))
6329                 return -EPROTO;
6330
6331         result = __le16_to_cpu(rsp->result);
6332
6333         BT_DBG("result 0x%4.4x", rsp->result);
6334
6335         if (!result)
6336                 return 0;
6337
6338         list_for_each_entry_safe(chan, tmp, &conn->chan_l, list) {
6339                 if (chan->ident != cmd->ident)
6340                         continue;
6341
6342                 l2cap_chan_del(chan, ECONNRESET);
6343         }
6344
6345         return 0;
6346 }
6347
6348 static inline int l2cap_le_command_rej(struct l2cap_conn *conn,
6349                                        struct l2cap_cmd_hdr *cmd, u16 cmd_len,
6350                                        u8 *data)
6351 {
6352         struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data;
6353         struct l2cap_chan *chan;
6354
6355         if (cmd_len < sizeof(*rej))
6356                 return -EPROTO;
6357
6358         mutex_lock(&conn->chan_lock);
6359
6360         chan = __l2cap_get_chan_by_ident(conn, cmd->ident);
6361         if (!chan)
6362                 goto done;
6363
6364         l2cap_chan_lock(chan);
6365         l2cap_chan_del(chan, ECONNREFUSED);
6366         l2cap_chan_unlock(chan);
6367
6368 done:
6369         mutex_unlock(&conn->chan_lock);
6370         return 0;
6371 }
6372
6373 static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
6374                                    struct l2cap_cmd_hdr *cmd, u16 cmd_len,
6375                                    u8 *data)
6376 {
6377         int err = 0;
6378
6379         switch (cmd->code) {
6380         case L2CAP_COMMAND_REJ:
6381                 l2cap_le_command_rej(conn, cmd, cmd_len, data);
6382                 break;
6383
6384         case L2CAP_CONN_PARAM_UPDATE_REQ:
6385                 err = l2cap_conn_param_update_req(conn, cmd, cmd_len, data);
6386                 break;
6387
6388         case L2CAP_CONN_PARAM_UPDATE_RSP:
6389                 break;
6390
6391         case L2CAP_LE_CONN_RSP:
6392                 l2cap_le_connect_rsp(conn, cmd, cmd_len, data);
6393                 break;
6394
6395         case L2CAP_LE_CONN_REQ:
6396                 err = l2cap_le_connect_req(conn, cmd, cmd_len, data);
6397                 break;
6398
6399         case L2CAP_LE_CREDITS:
6400                 err = l2cap_le_credits(conn, cmd, cmd_len, data);
6401                 break;
6402
6403         case L2CAP_ECRED_CONN_REQ:
6404                 err = l2cap_ecred_conn_req(conn, cmd, cmd_len, data);
6405                 break;
6406
6407         case L2CAP_ECRED_CONN_RSP:
6408                 err = l2cap_ecred_conn_rsp(conn, cmd, cmd_len, data);
6409                 break;
6410
6411         case L2CAP_ECRED_RECONF_REQ:
6412                 err = l2cap_ecred_reconf_req(conn, cmd, cmd_len, data);
6413                 break;
6414
6415         case L2CAP_ECRED_RECONF_RSP:
6416                 err = l2cap_ecred_reconf_rsp(conn, cmd, cmd_len, data);
6417                 break;
6418
6419         case L2CAP_DISCONN_REQ:
6420                 err = l2cap_disconnect_req(conn, cmd, cmd_len, data);
6421                 break;
6422
6423         case L2CAP_DISCONN_RSP:
6424                 l2cap_disconnect_rsp(conn, cmd, cmd_len, data);
6425                 break;
6426
6427         default:
6428                 BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code);
6429                 err = -EINVAL;
6430                 break;
6431         }
6432
6433         return err;
6434 }
6435
6436 static inline void l2cap_le_sig_channel(struct l2cap_conn *conn,
6437                                         struct sk_buff *skb)
6438 {
6439         struct hci_conn *hcon = conn->hcon;
6440         struct l2cap_cmd_hdr *cmd;
6441         u16 len;
6442         int err;
6443
6444         if (hcon->type != LE_LINK)
6445                 goto drop;
6446
6447         if (skb->len < L2CAP_CMD_HDR_SIZE)
6448                 goto drop;
6449
6450         cmd = (void *) skb->data;
6451         skb_pull(skb, L2CAP_CMD_HDR_SIZE);
6452
6453         len = le16_to_cpu(cmd->len);
6454
6455         BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd->code, len, cmd->ident);
6456
6457         if (len != skb->len || !cmd->ident) {
6458                 BT_DBG("corrupted command");
6459                 goto drop;
6460         }
6461
6462         err = l2cap_le_sig_cmd(conn, cmd, len, skb->data);
6463         if (err) {
6464                 struct l2cap_cmd_rej_unk rej;
6465
6466                 BT_ERR("Wrong link type (%d)", err);
6467
6468                 rej.reason = cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
6469                 l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
6470                                sizeof(rej), &rej);
6471         }
6472
6473 drop:
6474         kfree_skb(skb);
6475 }
6476
6477 static inline void l2cap_sig_channel(struct l2cap_conn *conn,
6478                                      struct sk_buff *skb)
6479 {
6480         struct hci_conn *hcon = conn->hcon;
6481         struct l2cap_cmd_hdr *cmd;
6482         int err;
6483
6484         l2cap_raw_recv(conn, skb);
6485
6486         if (hcon->type != ACL_LINK)
6487                 goto drop;
6488
6489         while (skb->len >= L2CAP_CMD_HDR_SIZE) {
6490                 u16 len;
6491
6492                 cmd = (void *) skb->data;
6493                 skb_pull(skb, L2CAP_CMD_HDR_SIZE);
6494
6495                 len = le16_to_cpu(cmd->len);
6496
6497                 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd->code, len,
6498                        cmd->ident);
6499
6500                 if (len > skb->len || !cmd->ident) {
6501                         BT_DBG("corrupted command");
6502                         break;
6503                 }
6504
6505                 err = l2cap_bredr_sig_cmd(conn, cmd, len, skb->data);
6506                 if (err) {
6507                         struct l2cap_cmd_rej_unk rej;
6508
6509                         BT_ERR("Wrong link type (%d)", err);
6510
6511                         rej.reason = cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
6512                         l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
6513                                        sizeof(rej), &rej);
6514                 }
6515
6516                 skb_pull(skb, len);
6517         }
6518
6519 drop:
6520         kfree_skb(skb);
6521 }
6522
6523 static int l2cap_check_fcs(struct l2cap_chan *chan,  struct sk_buff *skb)
6524 {
6525         u16 our_fcs, rcv_fcs;
6526         int hdr_size;
6527
6528         if (test_bit(FLAG_EXT_CTRL, &chan->flags))
6529                 hdr_size = L2CAP_EXT_HDR_SIZE;
6530         else
6531                 hdr_size = L2CAP_ENH_HDR_SIZE;
6532
6533         if (chan->fcs == L2CAP_FCS_CRC16) {
6534                 skb_trim(skb, skb->len - L2CAP_FCS_SIZE);
6535                 rcv_fcs = get_unaligned_le16(skb->data + skb->len);
6536                 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
6537
6538                 if (our_fcs != rcv_fcs)
6539                         return -EBADMSG;
6540         }
6541         return 0;
6542 }
6543
6544 static void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan)
6545 {
6546         struct l2cap_ctrl control;
6547
6548         BT_DBG("chan %p", chan);
6549
6550         memset(&control, 0, sizeof(control));
6551         control.sframe = 1;
6552         control.final = 1;
6553         control.reqseq = chan->buffer_seq;
6554         set_bit(CONN_SEND_FBIT, &chan->conn_state);
6555
6556         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
6557                 control.super = L2CAP_SUPER_RNR;
6558                 l2cap_send_sframe(chan, &control);
6559         }
6560
6561         if (test_and_clear_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
6562             chan->unacked_frames > 0)
6563                 __set_retrans_timer(chan);
6564
6565         /* Send pending iframes */
6566         l2cap_ertm_send(chan);
6567
6568         if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
6569             test_bit(CONN_SEND_FBIT, &chan->conn_state)) {
6570                 /* F-bit wasn't sent in an s-frame or i-frame yet, so
6571                  * send it now.
6572                  */
6573                 control.super = L2CAP_SUPER_RR;
6574                 l2cap_send_sframe(chan, &control);
6575         }
6576 }
6577
6578 static void append_skb_frag(struct sk_buff *skb, struct sk_buff *new_frag,
6579                             struct sk_buff **last_frag)
6580 {
6581         /* skb->len reflects data in skb as well as all fragments
6582          * skb->data_len reflects only data in fragments
6583          */
6584         if (!skb_has_frag_list(skb))
6585                 skb_shinfo(skb)->frag_list = new_frag;
6586
6587         new_frag->next = NULL;
6588
6589         (*last_frag)->next = new_frag;
6590         *last_frag = new_frag;
6591
6592         skb->len += new_frag->len;
6593         skb->data_len += new_frag->len;
6594         skb->truesize += new_frag->truesize;
6595 }
6596
6597 static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb,
6598                                 struct l2cap_ctrl *control)
6599 {
6600         int err = -EINVAL;
6601
6602         switch (control->sar) {
6603         case L2CAP_SAR_UNSEGMENTED:
6604                 if (chan->sdu)
6605                         break;
6606
6607                 err = chan->ops->recv(chan, skb);
6608                 break;
6609
6610         case L2CAP_SAR_START:
6611                 if (chan->sdu)
6612                         break;
6613
6614                 if (!pskb_may_pull(skb, L2CAP_SDULEN_SIZE))
6615                         break;
6616
6617                 chan->sdu_len = get_unaligned_le16(skb->data);
6618                 skb_pull(skb, L2CAP_SDULEN_SIZE);
6619
6620                 if (chan->sdu_len > chan->imtu) {
6621                         err = -EMSGSIZE;
6622                         break;
6623                 }
6624
6625                 if (skb->len >= chan->sdu_len)
6626                         break;
6627
6628                 chan->sdu = skb;
6629                 chan->sdu_last_frag = skb;
6630
6631                 skb = NULL;
6632                 err = 0;
6633                 break;
6634
6635         case L2CAP_SAR_CONTINUE:
6636                 if (!chan->sdu)
6637                         break;
6638
6639                 append_skb_frag(chan->sdu, skb,
6640                                 &chan->sdu_last_frag);
6641                 skb = NULL;
6642
6643                 if (chan->sdu->len >= chan->sdu_len)
6644                         break;
6645
6646                 err = 0;
6647                 break;
6648
6649         case L2CAP_SAR_END:
6650                 if (!chan->sdu)
6651                         break;
6652
6653                 append_skb_frag(chan->sdu, skb,
6654                                 &chan->sdu_last_frag);
6655                 skb = NULL;
6656
6657                 if (chan->sdu->len != chan->sdu_len)
6658                         break;
6659
6660                 err = chan->ops->recv(chan, chan->sdu);
6661
6662                 if (!err) {
6663                         /* Reassembly complete */
6664                         chan->sdu = NULL;
6665                         chan->sdu_last_frag = NULL;
6666                         chan->sdu_len = 0;
6667                 }
6668                 break;
6669         }
6670
6671         if (err) {
6672                 kfree_skb(skb);
6673                 kfree_skb(chan->sdu);
6674                 chan->sdu = NULL;
6675                 chan->sdu_last_frag = NULL;
6676                 chan->sdu_len = 0;
6677         }
6678
6679         return err;
6680 }
6681
6682 static int l2cap_resegment(struct l2cap_chan *chan)
6683 {
6684         /* Placeholder */
6685         return 0;
6686 }
6687
6688 void l2cap_chan_busy(struct l2cap_chan *chan, int busy)
6689 {
6690         u8 event;
6691
6692         if (chan->mode != L2CAP_MODE_ERTM)
6693                 return;
6694
6695         event = busy ? L2CAP_EV_LOCAL_BUSY_DETECTED : L2CAP_EV_LOCAL_BUSY_CLEAR;
6696         l2cap_tx(chan, NULL, NULL, event);
6697 }
6698
6699 static int l2cap_rx_queued_iframes(struct l2cap_chan *chan)
6700 {
6701         int err = 0;
6702         /* Pass sequential frames to l2cap_reassemble_sdu()
6703          * until a gap is encountered.
6704          */
6705
6706         BT_DBG("chan %p", chan);
6707
6708         while (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
6709                 struct sk_buff *skb;
6710                 BT_DBG("Searching for skb with txseq %d (queue len %d)",
6711                        chan->buffer_seq, skb_queue_len(&chan->srej_q));
6712
6713                 skb = l2cap_ertm_seq_in_queue(&chan->srej_q, chan->buffer_seq);
6714
6715                 if (!skb)
6716                         break;
6717
6718                 skb_unlink(skb, &chan->srej_q);
6719                 chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
6720                 err = l2cap_reassemble_sdu(chan, skb, &bt_cb(skb)->l2cap);
6721                 if (err)
6722                         break;
6723         }
6724
6725         if (skb_queue_empty(&chan->srej_q)) {
6726                 chan->rx_state = L2CAP_RX_STATE_RECV;
6727                 l2cap_send_ack(chan);
6728         }
6729
6730         return err;
6731 }
6732
6733 static void l2cap_handle_srej(struct l2cap_chan *chan,
6734                               struct l2cap_ctrl *control)
6735 {
6736         struct sk_buff *skb;
6737
6738         BT_DBG("chan %p, control %p", chan, control);
6739
6740         if (control->reqseq == chan->next_tx_seq) {
6741                 BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq);
6742                 l2cap_send_disconn_req(chan, ECONNRESET);
6743                 return;
6744         }
6745
6746         skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq);
6747
6748         if (skb == NULL) {
6749                 BT_DBG("Seq %d not available for retransmission",
6750                        control->reqseq);
6751                 return;
6752         }
6753
6754         if (chan->max_tx != 0 && bt_cb(skb)->l2cap.retries >= chan->max_tx) {
6755                 BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
6756                 l2cap_send_disconn_req(chan, ECONNRESET);
6757                 return;
6758         }
6759
6760         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6761
6762         if (control->poll) {
6763                 l2cap_pass_to_tx(chan, control);
6764
6765                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
6766                 l2cap_retransmit(chan, control);
6767                 l2cap_ertm_send(chan);
6768
6769                 if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) {
6770                         set_bit(CONN_SREJ_ACT, &chan->conn_state);
6771                         chan->srej_save_reqseq = control->reqseq;
6772                 }
6773         } else {
6774                 l2cap_pass_to_tx_fbit(chan, control);
6775
6776                 if (control->final) {
6777                         if (chan->srej_save_reqseq != control->reqseq ||
6778                             !test_and_clear_bit(CONN_SREJ_ACT,
6779                                                 &chan->conn_state))
6780                                 l2cap_retransmit(chan, control);
6781                 } else {
6782                         l2cap_retransmit(chan, control);
6783                         if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) {
6784                                 set_bit(CONN_SREJ_ACT, &chan->conn_state);
6785                                 chan->srej_save_reqseq = control->reqseq;
6786                         }
6787                 }
6788         }
6789 }
6790
6791 static void l2cap_handle_rej(struct l2cap_chan *chan,
6792                              struct l2cap_ctrl *control)
6793 {
6794         struct sk_buff *skb;
6795
6796         BT_DBG("chan %p, control %p", chan, control);
6797
6798         if (control->reqseq == chan->next_tx_seq) {
6799                 BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq);
6800                 l2cap_send_disconn_req(chan, ECONNRESET);
6801                 return;
6802         }
6803
6804         skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq);
6805
6806         if (chan->max_tx && skb &&
6807             bt_cb(skb)->l2cap.retries >= chan->max_tx) {
6808                 BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
6809                 l2cap_send_disconn_req(chan, ECONNRESET);
6810                 return;
6811         }
6812
6813         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6814
6815         l2cap_pass_to_tx(chan, control);
6816
6817         if (control->final) {
6818                 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
6819                         l2cap_retransmit_all(chan, control);
6820         } else {
6821                 l2cap_retransmit_all(chan, control);
6822                 l2cap_ertm_send(chan);
6823                 if (chan->tx_state == L2CAP_TX_STATE_WAIT_F)
6824                         set_bit(CONN_REJ_ACT, &chan->conn_state);
6825         }
6826 }
6827
6828 static u8 l2cap_classify_txseq(struct l2cap_chan *chan, u16 txseq)
6829 {
6830         BT_DBG("chan %p, txseq %d", chan, txseq);
6831
6832         BT_DBG("last_acked_seq %d, expected_tx_seq %d", chan->last_acked_seq,
6833                chan->expected_tx_seq);
6834
6835         if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
6836                 if (__seq_offset(chan, txseq, chan->last_acked_seq) >=
6837                     chan->tx_win) {
6838                         /* See notes below regarding "double poll" and
6839                          * invalid packets.
6840                          */
6841                         if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) {
6842                                 BT_DBG("Invalid/Ignore - after SREJ");
6843                                 return L2CAP_TXSEQ_INVALID_IGNORE;
6844                         } else {
6845                                 BT_DBG("Invalid - in window after SREJ sent");
6846                                 return L2CAP_TXSEQ_INVALID;
6847                         }
6848                 }
6849
6850                 if (chan->srej_list.head == txseq) {
6851                         BT_DBG("Expected SREJ");
6852                         return L2CAP_TXSEQ_EXPECTED_SREJ;
6853                 }
6854
6855                 if (l2cap_ertm_seq_in_queue(&chan->srej_q, txseq)) {
6856                         BT_DBG("Duplicate SREJ - txseq already stored");
6857                         return L2CAP_TXSEQ_DUPLICATE_SREJ;
6858                 }
6859
6860                 if (l2cap_seq_list_contains(&chan->srej_list, txseq)) {
6861                         BT_DBG("Unexpected SREJ - not requested");
6862                         return L2CAP_TXSEQ_UNEXPECTED_SREJ;
6863                 }
6864         }
6865
6866         if (chan->expected_tx_seq == txseq) {
6867                 if (__seq_offset(chan, txseq, chan->last_acked_seq) >=
6868                     chan->tx_win) {
6869                         BT_DBG("Invalid - txseq outside tx window");
6870                         return L2CAP_TXSEQ_INVALID;
6871                 } else {
6872                         BT_DBG("Expected");
6873                         return L2CAP_TXSEQ_EXPECTED;
6874                 }
6875         }
6876
6877         if (__seq_offset(chan, txseq, chan->last_acked_seq) <
6878             __seq_offset(chan, chan->expected_tx_seq, chan->last_acked_seq)) {
6879                 BT_DBG("Duplicate - expected_tx_seq later than txseq");
6880                 return L2CAP_TXSEQ_DUPLICATE;
6881         }
6882
6883         if (__seq_offset(chan, txseq, chan->last_acked_seq) >= chan->tx_win) {
6884                 /* A source of invalid packets is a "double poll" condition,
6885                  * where delays cause us to send multiple poll packets.  If
6886                  * the remote stack receives and processes both polls,
6887                  * sequence numbers can wrap around in such a way that a
6888                  * resent frame has a sequence number that looks like new data
6889                  * with a sequence gap.  This would trigger an erroneous SREJ
6890                  * request.
6891                  *
6892                  * Fortunately, this is impossible with a tx window that's
6893                  * less than half of the maximum sequence number, which allows
6894                  * invalid frames to be safely ignored.
6895                  *
6896                  * With tx window sizes greater than half of the tx window
6897                  * maximum, the frame is invalid and cannot be ignored.  This
6898                  * causes a disconnect.
6899                  */
6900
6901                 if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) {
6902                         BT_DBG("Invalid/Ignore - txseq outside tx window");
6903                         return L2CAP_TXSEQ_INVALID_IGNORE;
6904                 } else {
6905                         BT_DBG("Invalid - txseq outside tx window");
6906                         return L2CAP_TXSEQ_INVALID;
6907                 }
6908         } else {
6909                 BT_DBG("Unexpected - txseq indicates missing frames");
6910                 return L2CAP_TXSEQ_UNEXPECTED;
6911         }
6912 }
6913
6914 static int l2cap_rx_state_recv(struct l2cap_chan *chan,
6915                                struct l2cap_ctrl *control,
6916                                struct sk_buff *skb, u8 event)
6917 {
6918         struct l2cap_ctrl local_control;
6919         int err = 0;
6920         bool skb_in_use = false;
6921
6922         BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
6923                event);
6924
6925         switch (event) {
6926         case L2CAP_EV_RECV_IFRAME:
6927                 switch (l2cap_classify_txseq(chan, control->txseq)) {
6928                 case L2CAP_TXSEQ_EXPECTED:
6929                         l2cap_pass_to_tx(chan, control);
6930
6931                         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
6932                                 BT_DBG("Busy, discarding expected seq %d",
6933                                        control->txseq);
6934                                 break;
6935                         }
6936
6937                         chan->expected_tx_seq = __next_seq(chan,
6938                                                            control->txseq);
6939
6940                         chan->buffer_seq = chan->expected_tx_seq;
6941                         skb_in_use = true;
6942
6943                         /* l2cap_reassemble_sdu may free skb, hence invalidate
6944                          * control, so make a copy in advance to use it after
6945                          * l2cap_reassemble_sdu returns and to avoid the race
6946                          * condition, for example:
6947                          *
6948                          * The current thread calls:
6949                          *   l2cap_reassemble_sdu
6950                          *     chan->ops->recv == l2cap_sock_recv_cb
6951                          *       __sock_queue_rcv_skb
6952                          * Another thread calls:
6953                          *   bt_sock_recvmsg
6954                          *     skb_recv_datagram
6955                          *     skb_free_datagram
6956                          * Then the current thread tries to access control, but
6957                          * it was freed by skb_free_datagram.
6958                          */
6959                         local_control = *control;
6960                         err = l2cap_reassemble_sdu(chan, skb, control);
6961                         if (err)
6962                                 break;
6963
6964                         if (local_control.final) {
6965                                 if (!test_and_clear_bit(CONN_REJ_ACT,
6966                                                         &chan->conn_state)) {
6967                                         local_control.final = 0;
6968                                         l2cap_retransmit_all(chan, &local_control);
6969                                         l2cap_ertm_send(chan);
6970                                 }
6971                         }
6972
6973                         if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state))
6974                                 l2cap_send_ack(chan);
6975                         break;
6976                 case L2CAP_TXSEQ_UNEXPECTED:
6977                         l2cap_pass_to_tx(chan, control);
6978
6979                         /* Can't issue SREJ frames in the local busy state.
6980                          * Drop this frame, it will be seen as missing
6981                          * when local busy is exited.
6982                          */
6983                         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
6984                                 BT_DBG("Busy, discarding unexpected seq %d",
6985                                        control->txseq);
6986                                 break;
6987                         }
6988
6989                         /* There was a gap in the sequence, so an SREJ
6990                          * must be sent for each missing frame.  The
6991                          * current frame is stored for later use.
6992                          */
6993                         skb_queue_tail(&chan->srej_q, skb);
6994                         skb_in_use = true;
6995                         BT_DBG("Queued %p (queue len %d)", skb,
6996                                skb_queue_len(&chan->srej_q));
6997
6998                         clear_bit(CONN_SREJ_ACT, &chan->conn_state);
6999                         l2cap_seq_list_clear(&chan->srej_list);
7000                         l2cap_send_srej(chan, control->txseq);
7001
7002                         chan->rx_state = L2CAP_RX_STATE_SREJ_SENT;
7003                         break;
7004                 case L2CAP_TXSEQ_DUPLICATE:
7005                         l2cap_pass_to_tx(chan, control);
7006                         break;
7007                 case L2CAP_TXSEQ_INVALID_IGNORE:
7008                         break;
7009                 case L2CAP_TXSEQ_INVALID:
7010                 default:
7011                         l2cap_send_disconn_req(chan, ECONNRESET);
7012                         break;
7013                 }
7014                 break;
7015         case L2CAP_EV_RECV_RR:
7016                 l2cap_pass_to_tx(chan, control);
7017                 if (control->final) {
7018                         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
7019
7020                         if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state) &&
7021                             !__chan_is_moving(chan)) {
7022                                 control->final = 0;
7023                                 l2cap_retransmit_all(chan, control);
7024                         }
7025
7026                         l2cap_ertm_send(chan);
7027                 } else if (control->poll) {
7028                         l2cap_send_i_or_rr_or_rnr(chan);
7029                 } else {
7030                         if (test_and_clear_bit(CONN_REMOTE_BUSY,
7031                                                &chan->conn_state) &&
7032                             chan->unacked_frames)
7033                                 __set_retrans_timer(chan);
7034
7035                         l2cap_ertm_send(chan);
7036                 }
7037                 break;
7038         case L2CAP_EV_RECV_RNR:
7039                 set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
7040                 l2cap_pass_to_tx(chan, control);
7041                 if (control && control->poll) {
7042                         set_bit(CONN_SEND_FBIT, &chan->conn_state);
7043                         l2cap_send_rr_or_rnr(chan, 0);
7044                 }
7045                 __clear_retrans_timer(chan);
7046                 l2cap_seq_list_clear(&chan->retrans_list);
7047                 break;
7048         case L2CAP_EV_RECV_REJ:
7049                 l2cap_handle_rej(chan, control);
7050                 break;
7051         case L2CAP_EV_RECV_SREJ:
7052                 l2cap_handle_srej(chan, control);
7053                 break;
7054         default:
7055                 break;
7056         }
7057
7058         if (skb && !skb_in_use) {
7059                 BT_DBG("Freeing %p", skb);
7060                 kfree_skb(skb);
7061         }
7062
7063         return err;
7064 }
7065
7066 static int l2cap_rx_state_srej_sent(struct l2cap_chan *chan,
7067                                     struct l2cap_ctrl *control,
7068                                     struct sk_buff *skb, u8 event)
7069 {
7070         int err = 0;
7071         u16 txseq = control->txseq;
7072         bool skb_in_use = false;
7073
7074         BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
7075                event);
7076
7077         switch (event) {
7078         case L2CAP_EV_RECV_IFRAME:
7079                 switch (l2cap_classify_txseq(chan, txseq)) {
7080                 case L2CAP_TXSEQ_EXPECTED:
7081                         /* Keep frame for reassembly later */
7082                         l2cap_pass_to_tx(chan, control);
7083                         skb_queue_tail(&chan->srej_q, skb);
7084                         skb_in_use = true;
7085                         BT_DBG("Queued %p (queue len %d)", skb,
7086                                skb_queue_len(&chan->srej_q));
7087
7088                         chan->expected_tx_seq = __next_seq(chan, txseq);
7089                         break;
7090                 case L2CAP_TXSEQ_EXPECTED_SREJ:
7091                         l2cap_seq_list_pop(&chan->srej_list);
7092
7093                         l2cap_pass_to_tx(chan, control);
7094                         skb_queue_tail(&chan->srej_q, skb);
7095                         skb_in_use = true;
7096                         BT_DBG("Queued %p (queue len %d)", skb,
7097                                skb_queue_len(&chan->srej_q));
7098
7099                         err = l2cap_rx_queued_iframes(chan);
7100                         if (err)
7101                                 break;
7102
7103                         break;
7104                 case L2CAP_TXSEQ_UNEXPECTED:
7105                         /* Got a frame that can't be reassembled yet.
7106                          * Save it for later, and send SREJs to cover
7107                          * the missing frames.
7108                          */
7109                         skb_queue_tail(&chan->srej_q, skb);
7110                         skb_in_use = true;
7111                         BT_DBG("Queued %p (queue len %d)", skb,
7112                                skb_queue_len(&chan->srej_q));
7113
7114                         l2cap_pass_to_tx(chan, control);
7115                         l2cap_send_srej(chan, control->txseq);
7116                         break;
7117                 case L2CAP_TXSEQ_UNEXPECTED_SREJ:
7118                         /* This frame was requested with an SREJ, but
7119                          * some expected retransmitted frames are
7120                          * missing.  Request retransmission of missing
7121                          * SREJ'd frames.
7122                          */
7123                         skb_queue_tail(&chan->srej_q, skb);
7124                         skb_in_use = true;
7125                         BT_DBG("Queued %p (queue len %d)", skb,
7126                                skb_queue_len(&chan->srej_q));
7127
7128                         l2cap_pass_to_tx(chan, control);
7129                         l2cap_send_srej_list(chan, control->txseq);
7130                         break;
7131                 case L2CAP_TXSEQ_DUPLICATE_SREJ:
7132                         /* We've already queued this frame.  Drop this copy. */
7133                         l2cap_pass_to_tx(chan, control);
7134                         break;
7135                 case L2CAP_TXSEQ_DUPLICATE:
7136                         /* Expecting a later sequence number, so this frame
7137                          * was already received.  Ignore it completely.
7138                          */
7139                         break;
7140                 case L2CAP_TXSEQ_INVALID_IGNORE:
7141                         break;
7142                 case L2CAP_TXSEQ_INVALID:
7143                 default:
7144                         l2cap_send_disconn_req(chan, ECONNRESET);
7145                         break;
7146                 }
7147                 break;
7148         case L2CAP_EV_RECV_RR:
7149                 l2cap_pass_to_tx(chan, control);
7150                 if (control->final) {
7151                         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
7152
7153                         if (!test_and_clear_bit(CONN_REJ_ACT,
7154                                                 &chan->conn_state)) {
7155                                 control->final = 0;
7156                                 l2cap_retransmit_all(chan, control);
7157                         }
7158
7159                         l2cap_ertm_send(chan);
7160                 } else if (control->poll) {
7161                         if (test_and_clear_bit(CONN_REMOTE_BUSY,
7162                                                &chan->conn_state) &&
7163                             chan->unacked_frames) {
7164                                 __set_retrans_timer(chan);
7165                         }
7166
7167                         set_bit(CONN_SEND_FBIT, &chan->conn_state);
7168                         l2cap_send_srej_tail(chan);
7169                 } else {
7170                         if (test_and_clear_bit(CONN_REMOTE_BUSY,
7171                                                &chan->conn_state) &&
7172                             chan->unacked_frames)
7173                                 __set_retrans_timer(chan);
7174
7175                         l2cap_send_ack(chan);
7176                 }
7177                 break;
7178         case L2CAP_EV_RECV_RNR:
7179                 set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
7180                 l2cap_pass_to_tx(chan, control);
7181                 if (control->poll) {
7182                         l2cap_send_srej_tail(chan);
7183                 } else {
7184                         struct l2cap_ctrl rr_control;
7185                         memset(&rr_control, 0, sizeof(rr_control));
7186                         rr_control.sframe = 1;
7187                         rr_control.super = L2CAP_SUPER_RR;
7188                         rr_control.reqseq = chan->buffer_seq;
7189                         l2cap_send_sframe(chan, &rr_control);
7190                 }
7191
7192                 break;
7193         case L2CAP_EV_RECV_REJ:
7194                 l2cap_handle_rej(chan, control);
7195                 break;
7196         case L2CAP_EV_RECV_SREJ:
7197                 l2cap_handle_srej(chan, control);
7198                 break;
7199         }
7200
7201         if (skb && !skb_in_use) {
7202                 BT_DBG("Freeing %p", skb);
7203                 kfree_skb(skb);
7204         }
7205
7206         return err;
7207 }
7208
7209 static int l2cap_finish_move(struct l2cap_chan *chan)
7210 {
7211         BT_DBG("chan %p", chan);
7212
7213         chan->rx_state = L2CAP_RX_STATE_RECV;
7214
7215         if (chan->hs_hcon)
7216                 chan->conn->mtu = chan->hs_hcon->hdev->block_mtu;
7217         else
7218                 chan->conn->mtu = chan->conn->hcon->hdev->acl_mtu;
7219
7220         return l2cap_resegment(chan);
7221 }
7222
7223 static int l2cap_rx_state_wait_p(struct l2cap_chan *chan,
7224                                  struct l2cap_ctrl *control,
7225                                  struct sk_buff *skb, u8 event)
7226 {
7227         int err;
7228
7229         BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
7230                event);
7231
7232         if (!control->poll)
7233                 return -EPROTO;
7234
7235         l2cap_process_reqseq(chan, control->reqseq);
7236
7237         if (!skb_queue_empty(&chan->tx_q))
7238                 chan->tx_send_head = skb_peek(&chan->tx_q);
7239         else
7240                 chan->tx_send_head = NULL;
7241
7242         /* Rewind next_tx_seq to the point expected
7243          * by the receiver.
7244          */
7245         chan->next_tx_seq = control->reqseq;
7246         chan->unacked_frames = 0;
7247
7248         err = l2cap_finish_move(chan);
7249         if (err)
7250                 return err;
7251
7252         set_bit(CONN_SEND_FBIT, &chan->conn_state);
7253         l2cap_send_i_or_rr_or_rnr(chan);
7254
7255         if (event == L2CAP_EV_RECV_IFRAME)
7256                 return -EPROTO;
7257
7258         return l2cap_rx_state_recv(chan, control, NULL, event);
7259 }
7260
7261 static int l2cap_rx_state_wait_f(struct l2cap_chan *chan,
7262                                  struct l2cap_ctrl *control,
7263                                  struct sk_buff *skb, u8 event)
7264 {
7265         int err;
7266
7267         if (!control->final)
7268                 return -EPROTO;
7269
7270         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
7271
7272         chan->rx_state = L2CAP_RX_STATE_RECV;
7273         l2cap_process_reqseq(chan, control->reqseq);
7274
7275         if (!skb_queue_empty(&chan->tx_q))
7276                 chan->tx_send_head = skb_peek(&chan->tx_q);
7277         else
7278                 chan->tx_send_head = NULL;
7279
7280         /* Rewind next_tx_seq to the point expected
7281          * by the receiver.
7282          */
7283         chan->next_tx_seq = control->reqseq;
7284         chan->unacked_frames = 0;
7285
7286         if (chan->hs_hcon)
7287                 chan->conn->mtu = chan->hs_hcon->hdev->block_mtu;
7288         else
7289                 chan->conn->mtu = chan->conn->hcon->hdev->acl_mtu;
7290
7291         err = l2cap_resegment(chan);
7292
7293         if (!err)
7294                 err = l2cap_rx_state_recv(chan, control, skb, event);
7295
7296         return err;
7297 }
7298
7299 static bool __valid_reqseq(struct l2cap_chan *chan, u16 reqseq)
7300 {
7301         /* Make sure reqseq is for a packet that has been sent but not acked */
7302         u16 unacked;
7303
7304         unacked = __seq_offset(chan, chan->next_tx_seq, chan->expected_ack_seq);
7305         return __seq_offset(chan, chan->next_tx_seq, reqseq) <= unacked;
7306 }
7307
7308 static int l2cap_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
7309                     struct sk_buff *skb, u8 event)
7310 {
7311         int err = 0;
7312
7313         BT_DBG("chan %p, control %p, skb %p, event %d, state %d", chan,
7314                control, skb, event, chan->rx_state);
7315
7316         if (__valid_reqseq(chan, control->reqseq)) {
7317                 switch (chan->rx_state) {
7318                 case L2CAP_RX_STATE_RECV:
7319                         err = l2cap_rx_state_recv(chan, control, skb, event);
7320                         break;
7321                 case L2CAP_RX_STATE_SREJ_SENT:
7322                         err = l2cap_rx_state_srej_sent(chan, control, skb,
7323                                                        event);
7324                         break;
7325                 case L2CAP_RX_STATE_WAIT_P:
7326                         err = l2cap_rx_state_wait_p(chan, control, skb, event);
7327                         break;
7328                 case L2CAP_RX_STATE_WAIT_F:
7329                         err = l2cap_rx_state_wait_f(chan, control, skb, event);
7330                         break;
7331                 default:
7332                         /* shut it down */
7333                         break;
7334                 }
7335         } else {
7336                 BT_DBG("Invalid reqseq %d (next_tx_seq %d, expected_ack_seq %d",
7337                        control->reqseq, chan->next_tx_seq,
7338                        chan->expected_ack_seq);
7339                 l2cap_send_disconn_req(chan, ECONNRESET);
7340         }
7341
7342         return err;
7343 }
7344
7345 static int l2cap_stream_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
7346                            struct sk_buff *skb)
7347 {
7348         /* l2cap_reassemble_sdu may free skb, hence invalidate control, so store
7349          * the txseq field in advance to use it after l2cap_reassemble_sdu
7350          * returns and to avoid the race condition, for example:
7351          *
7352          * The current thread calls:
7353          *   l2cap_reassemble_sdu
7354          *     chan->ops->recv == l2cap_sock_recv_cb
7355          *       __sock_queue_rcv_skb
7356          * Another thread calls:
7357          *   bt_sock_recvmsg
7358          *     skb_recv_datagram
7359          *     skb_free_datagram
7360          * Then the current thread tries to access control, but it was freed by
7361          * skb_free_datagram.
7362          */
7363         u16 txseq = control->txseq;
7364
7365         BT_DBG("chan %p, control %p, skb %p, state %d", chan, control, skb,
7366                chan->rx_state);
7367
7368         if (l2cap_classify_txseq(chan, txseq) == L2CAP_TXSEQ_EXPECTED) {
7369                 l2cap_pass_to_tx(chan, control);
7370
7371                 BT_DBG("buffer_seq %u->%u", chan->buffer_seq,
7372                        __next_seq(chan, chan->buffer_seq));
7373
7374                 chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
7375
7376                 l2cap_reassemble_sdu(chan, skb, control);
7377         } else {
7378                 if (chan->sdu) {
7379                         kfree_skb(chan->sdu);
7380                         chan->sdu = NULL;
7381                 }
7382                 chan->sdu_last_frag = NULL;
7383                 chan->sdu_len = 0;
7384
7385                 if (skb) {
7386                         BT_DBG("Freeing %p", skb);
7387                         kfree_skb(skb);
7388                 }
7389         }
7390
7391         chan->last_acked_seq = txseq;
7392         chan->expected_tx_seq = __next_seq(chan, txseq);
7393
7394         return 0;
7395 }
7396
7397 static int l2cap_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb)
7398 {
7399         struct l2cap_ctrl *control = &bt_cb(skb)->l2cap;
7400         u16 len;
7401         u8 event;
7402
7403         __unpack_control(chan, skb);
7404
7405         len = skb->len;
7406
7407         /*
7408          * We can just drop the corrupted I-frame here.
7409          * Receiver will miss it and start proper recovery
7410          * procedures and ask for retransmission.
7411          */
7412         if (l2cap_check_fcs(chan, skb))
7413                 goto drop;
7414
7415         if (!control->sframe && control->sar == L2CAP_SAR_START)
7416                 len -= L2CAP_SDULEN_SIZE;
7417
7418         if (chan->fcs == L2CAP_FCS_CRC16)
7419                 len -= L2CAP_FCS_SIZE;
7420
7421         if (len > chan->mps) {
7422                 l2cap_send_disconn_req(chan, ECONNRESET);
7423                 goto drop;
7424         }
7425
7426         if (chan->ops->filter) {
7427                 if (chan->ops->filter(chan, skb))
7428                         goto drop;
7429         }
7430
7431         if (!control->sframe) {
7432                 int err;
7433
7434                 BT_DBG("iframe sar %d, reqseq %d, final %d, txseq %d",
7435                        control->sar, control->reqseq, control->final,
7436                        control->txseq);
7437
7438                 /* Validate F-bit - F=0 always valid, F=1 only
7439                  * valid in TX WAIT_F
7440                  */
7441                 if (control->final && chan->tx_state != L2CAP_TX_STATE_WAIT_F)
7442                         goto drop;
7443
7444                 if (chan->mode != L2CAP_MODE_STREAMING) {
7445                         event = L2CAP_EV_RECV_IFRAME;
7446                         err = l2cap_rx(chan, control, skb, event);
7447                 } else {
7448                         err = l2cap_stream_rx(chan, control, skb);
7449                 }
7450
7451                 if (err)
7452                         l2cap_send_disconn_req(chan, ECONNRESET);
7453         } else {
7454                 const u8 rx_func_to_event[4] = {
7455                         L2CAP_EV_RECV_RR, L2CAP_EV_RECV_REJ,
7456                         L2CAP_EV_RECV_RNR, L2CAP_EV_RECV_SREJ
7457                 };
7458
7459                 /* Only I-frames are expected in streaming mode */
7460                 if (chan->mode == L2CAP_MODE_STREAMING)
7461                         goto drop;
7462
7463                 BT_DBG("sframe reqseq %d, final %d, poll %d, super %d",
7464                        control->reqseq, control->final, control->poll,
7465                        control->super);
7466
7467                 if (len != 0) {
7468                         BT_ERR("Trailing bytes: %d in sframe", len);
7469                         l2cap_send_disconn_req(chan, ECONNRESET);
7470                         goto drop;
7471                 }
7472
7473                 /* Validate F and P bits */
7474                 if (control->final && (control->poll ||
7475                                        chan->tx_state != L2CAP_TX_STATE_WAIT_F))
7476                         goto drop;
7477
7478                 event = rx_func_to_event[control->super];
7479                 if (l2cap_rx(chan, control, skb, event))
7480                         l2cap_send_disconn_req(chan, ECONNRESET);
7481         }
7482
7483         return 0;
7484
7485 drop:
7486         kfree_skb(skb);
7487         return 0;
7488 }
7489
7490 static void l2cap_chan_le_send_credits(struct l2cap_chan *chan)
7491 {
7492         struct l2cap_conn *conn = chan->conn;
7493         struct l2cap_le_credits pkt;
7494         u16 return_credits;
7495
7496         return_credits = (chan->imtu / chan->mps) + 1;
7497
7498         if (chan->rx_credits >= return_credits)
7499                 return;
7500
7501         return_credits -= chan->rx_credits;
7502
7503         BT_DBG("chan %p returning %u credits to sender", chan, return_credits);
7504
7505         chan->rx_credits += return_credits;
7506
7507         pkt.cid     = cpu_to_le16(chan->scid);
7508         pkt.credits = cpu_to_le16(return_credits);
7509
7510         chan->ident = l2cap_get_ident(conn);
7511
7512         l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CREDITS, sizeof(pkt), &pkt);
7513 }
7514
7515 static int l2cap_ecred_recv(struct l2cap_chan *chan, struct sk_buff *skb)
7516 {
7517         int err;
7518
7519         BT_DBG("SDU reassemble complete: chan %p skb->len %u", chan, skb->len);
7520
7521         /* Wait recv to confirm reception before updating the credits */
7522         err = chan->ops->recv(chan, skb);
7523
7524         /* Update credits whenever an SDU is received */
7525         l2cap_chan_le_send_credits(chan);
7526
7527         return err;
7528 }
7529
7530 static int l2cap_ecred_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb)
7531 {
7532         int err;
7533
7534         if (!chan->rx_credits) {
7535                 BT_ERR("No credits to receive LE L2CAP data");
7536                 l2cap_send_disconn_req(chan, ECONNRESET);
7537                 return -ENOBUFS;
7538         }
7539
7540         if (chan->imtu < skb->len) {
7541                 BT_ERR("Too big LE L2CAP PDU");
7542                 return -ENOBUFS;
7543         }
7544
7545         chan->rx_credits--;
7546         BT_DBG("rx_credits %u -> %u", chan->rx_credits + 1, chan->rx_credits);
7547
7548         /* Update if remote had run out of credits, this should only happens
7549          * if the remote is not using the entire MPS.
7550          */
7551         if (!chan->rx_credits)
7552                 l2cap_chan_le_send_credits(chan);
7553
7554         err = 0;
7555
7556         if (!chan->sdu) {
7557                 u16 sdu_len;
7558
7559                 sdu_len = get_unaligned_le16(skb->data);
7560                 skb_pull(skb, L2CAP_SDULEN_SIZE);
7561
7562                 BT_DBG("Start of new SDU. sdu_len %u skb->len %u imtu %u",
7563                        sdu_len, skb->len, chan->imtu);
7564
7565                 if (sdu_len > chan->imtu) {
7566                         BT_ERR("Too big LE L2CAP SDU length received");
7567                         err = -EMSGSIZE;
7568                         goto failed;
7569                 }
7570
7571                 if (skb->len > sdu_len) {
7572                         BT_ERR("Too much LE L2CAP data received");
7573                         err = -EINVAL;
7574                         goto failed;
7575                 }
7576
7577                 if (skb->len == sdu_len)
7578                         return l2cap_ecred_recv(chan, skb);
7579
7580                 chan->sdu = skb;
7581                 chan->sdu_len = sdu_len;
7582                 chan->sdu_last_frag = skb;
7583
7584                 /* Detect if remote is not able to use the selected MPS */
7585                 if (skb->len + L2CAP_SDULEN_SIZE < chan->mps) {
7586                         u16 mps_len = skb->len + L2CAP_SDULEN_SIZE;
7587
7588                         /* Adjust the number of credits */
7589                         BT_DBG("chan->mps %u -> %u", chan->mps, mps_len);
7590                         chan->mps = mps_len;
7591                         l2cap_chan_le_send_credits(chan);
7592                 }
7593
7594                 return 0;
7595         }
7596
7597         BT_DBG("SDU fragment. chan->sdu->len %u skb->len %u chan->sdu_len %u",
7598                chan->sdu->len, skb->len, chan->sdu_len);
7599
7600         if (chan->sdu->len + skb->len > chan->sdu_len) {
7601                 BT_ERR("Too much LE L2CAP data received");
7602                 err = -EINVAL;
7603                 goto failed;
7604         }
7605
7606         append_skb_frag(chan->sdu, skb, &chan->sdu_last_frag);
7607         skb = NULL;
7608
7609         if (chan->sdu->len == chan->sdu_len) {
7610                 err = l2cap_ecred_recv(chan, chan->sdu);
7611                 if (!err) {
7612                         chan->sdu = NULL;
7613                         chan->sdu_last_frag = NULL;
7614                         chan->sdu_len = 0;
7615                 }
7616         }
7617
7618 failed:
7619         if (err) {
7620                 kfree_skb(skb);
7621                 kfree_skb(chan->sdu);
7622                 chan->sdu = NULL;
7623                 chan->sdu_last_frag = NULL;
7624                 chan->sdu_len = 0;
7625         }
7626
7627         /* We can't return an error here since we took care of the skb
7628          * freeing internally. An error return would cause the caller to
7629          * do a double-free of the skb.
7630          */
7631         return 0;
7632 }
7633
7634 static void l2cap_data_channel(struct l2cap_conn *conn, u16 cid,
7635                                struct sk_buff *skb)
7636 {
7637         struct l2cap_chan *chan;
7638
7639         chan = l2cap_get_chan_by_scid(conn, cid);
7640         if (!chan) {
7641                 if (cid == L2CAP_CID_A2MP) {
7642                         chan = a2mp_channel_create(conn, skb);
7643                         if (!chan) {
7644                                 kfree_skb(skb);
7645                                 return;
7646                         }
7647
7648                         l2cap_chan_hold(chan);
7649                         l2cap_chan_lock(chan);
7650                 } else {
7651                         BT_DBG("unknown cid 0x%4.4x", cid);
7652                         /* Drop packet and return */
7653                         kfree_skb(skb);
7654                         return;
7655                 }
7656         }
7657
7658         BT_DBG("chan %p, len %d", chan, skb->len);
7659
7660         /* If we receive data on a fixed channel before the info req/rsp
7661          * procedure is done simply assume that the channel is supported
7662          * and mark it as ready.
7663          */
7664         if (chan->chan_type == L2CAP_CHAN_FIXED)
7665                 l2cap_chan_ready(chan);
7666
7667         if (chan->state != BT_CONNECTED)
7668                 goto drop;
7669
7670         switch (chan->mode) {
7671         case L2CAP_MODE_LE_FLOWCTL:
7672         case L2CAP_MODE_EXT_FLOWCTL:
7673                 if (l2cap_ecred_data_rcv(chan, skb) < 0)
7674                         goto drop;
7675
7676                 goto done;
7677
7678         case L2CAP_MODE_BASIC:
7679                 /* If socket recv buffers overflows we drop data here
7680                  * which is *bad* because L2CAP has to be reliable.
7681                  * But we don't have any other choice. L2CAP doesn't
7682                  * provide flow control mechanism. */
7683
7684                 if (chan->imtu < skb->len) {
7685                         BT_ERR("Dropping L2CAP data: receive buffer overflow");
7686                         goto drop;
7687                 }
7688
7689                 if (!chan->ops->recv(chan, skb))
7690                         goto done;
7691                 break;
7692
7693         case L2CAP_MODE_ERTM:
7694         case L2CAP_MODE_STREAMING:
7695                 l2cap_data_rcv(chan, skb);
7696                 goto done;
7697
7698         default:
7699                 BT_DBG("chan %p: bad mode 0x%2.2x", chan, chan->mode);
7700                 break;
7701         }
7702
7703 drop:
7704         kfree_skb(skb);
7705
7706 done:
7707         l2cap_chan_unlock(chan);
7708         l2cap_chan_put(chan);
7709 }
7710
7711 static void l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm,
7712                                   struct sk_buff *skb)
7713 {
7714         struct hci_conn *hcon = conn->hcon;
7715         struct l2cap_chan *chan;
7716
7717         if (hcon->type != ACL_LINK)
7718                 goto free_skb;
7719
7720         chan = l2cap_global_chan_by_psm(0, psm, &hcon->src, &hcon->dst,
7721                                         ACL_LINK);
7722         if (!chan)
7723                 goto free_skb;
7724
7725         BT_DBG("chan %p, len %d", chan, skb->len);
7726
7727         if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
7728                 goto drop;
7729
7730         if (chan->imtu < skb->len)
7731                 goto drop;
7732
7733         /* Store remote BD_ADDR and PSM for msg_name */
7734         bacpy(&bt_cb(skb)->l2cap.bdaddr, &hcon->dst);
7735         bt_cb(skb)->l2cap.psm = psm;
7736
7737         if (!chan->ops->recv(chan, skb)) {
7738                 l2cap_chan_put(chan);
7739                 return;
7740         }
7741
7742 drop:
7743         l2cap_chan_put(chan);
7744 free_skb:
7745         kfree_skb(skb);
7746 }
7747
7748 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
7749 {
7750         struct l2cap_hdr *lh = (void *) skb->data;
7751         struct hci_conn *hcon = conn->hcon;
7752         u16 cid, len;
7753         __le16 psm;
7754
7755         if (hcon->state != BT_CONNECTED) {
7756                 BT_DBG("queueing pending rx skb");
7757                 skb_queue_tail(&conn->pending_rx, skb);
7758                 return;
7759         }
7760
7761         skb_pull(skb, L2CAP_HDR_SIZE);
7762         cid = __le16_to_cpu(lh->cid);
7763         len = __le16_to_cpu(lh->len);
7764
7765         if (len != skb->len) {
7766                 kfree_skb(skb);
7767                 return;
7768         }
7769
7770         /* Since we can't actively block incoming LE connections we must
7771          * at least ensure that we ignore incoming data from them.
7772          */
7773         if (hcon->type == LE_LINK &&
7774             hci_bdaddr_list_lookup(&hcon->hdev->reject_list, &hcon->dst,
7775                                    bdaddr_dst_type(hcon))) {
7776                 kfree_skb(skb);
7777                 return;
7778         }
7779
7780         BT_DBG("len %d, cid 0x%4.4x", len, cid);
7781
7782         switch (cid) {
7783         case L2CAP_CID_SIGNALING:
7784                 l2cap_sig_channel(conn, skb);
7785                 break;
7786
7787         case L2CAP_CID_CONN_LESS:
7788                 psm = get_unaligned((__le16 *) skb->data);
7789                 skb_pull(skb, L2CAP_PSMLEN_SIZE);
7790                 l2cap_conless_channel(conn, psm, skb);
7791                 break;
7792
7793         case L2CAP_CID_LE_SIGNALING:
7794                 l2cap_le_sig_channel(conn, skb);
7795                 break;
7796
7797         default:
7798                 l2cap_data_channel(conn, cid, skb);
7799                 break;
7800         }
7801 }
7802
7803 static void process_pending_rx(struct work_struct *work)
7804 {
7805         struct l2cap_conn *conn = container_of(work, struct l2cap_conn,
7806                                                pending_rx_work);
7807         struct sk_buff *skb;
7808
7809         BT_DBG("");
7810
7811         while ((skb = skb_dequeue(&conn->pending_rx)))
7812                 l2cap_recv_frame(conn, skb);
7813 }
7814
7815 static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon)
7816 {
7817         struct l2cap_conn *conn = hcon->l2cap_data;
7818         struct hci_chan *hchan;
7819
7820         if (conn)
7821                 return conn;
7822
7823         hchan = hci_chan_create(hcon);
7824         if (!hchan)
7825                 return NULL;
7826
7827         conn = kzalloc(sizeof(*conn), GFP_KERNEL);
7828         if (!conn) {
7829                 hci_chan_del(hchan);
7830                 return NULL;
7831         }
7832
7833         kref_init(&conn->ref);
7834         hcon->l2cap_data = conn;
7835         conn->hcon = hci_conn_get(hcon);
7836         conn->hchan = hchan;
7837
7838         BT_DBG("hcon %p conn %p hchan %p", hcon, conn, hchan);
7839
7840         switch (hcon->type) {
7841         case LE_LINK:
7842                 if (hcon->hdev->le_mtu) {
7843                         conn->mtu = hcon->hdev->le_mtu;
7844                         break;
7845                 }
7846                 fallthrough;
7847         default:
7848                 conn->mtu = hcon->hdev->acl_mtu;
7849                 break;
7850         }
7851
7852         conn->feat_mask = 0;
7853
7854         conn->local_fixed_chan = L2CAP_FC_SIG_BREDR | L2CAP_FC_CONNLESS;
7855
7856         if (hcon->type == ACL_LINK &&
7857             hci_dev_test_flag(hcon->hdev, HCI_HS_ENABLED))
7858                 conn->local_fixed_chan |= L2CAP_FC_A2MP;
7859
7860         if (hci_dev_test_flag(hcon->hdev, HCI_LE_ENABLED) &&
7861             (bredr_sc_enabled(hcon->hdev) ||
7862              hci_dev_test_flag(hcon->hdev, HCI_FORCE_BREDR_SMP)))
7863                 conn->local_fixed_chan |= L2CAP_FC_SMP_BREDR;
7864
7865         mutex_init(&conn->ident_lock);
7866         mutex_init(&conn->chan_lock);
7867
7868         INIT_LIST_HEAD(&conn->chan_l);
7869         INIT_LIST_HEAD(&conn->users);
7870
7871         INIT_DELAYED_WORK(&conn->info_timer, l2cap_info_timeout);
7872
7873         skb_queue_head_init(&conn->pending_rx);
7874         INIT_WORK(&conn->pending_rx_work, process_pending_rx);
7875         INIT_DELAYED_WORK(&conn->id_addr_timer, l2cap_conn_update_id_addr);
7876
7877         conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM;
7878
7879         return conn;
7880 }
7881
7882 static bool is_valid_psm(u16 psm, u8 dst_type)
7883 {
7884         if (!psm)
7885                 return false;
7886
7887         if (bdaddr_type_is_le(dst_type))
7888                 return (psm <= 0x00ff);
7889
7890         /* PSM must be odd and lsb of upper byte must be 0 */
7891         return ((psm & 0x0101) == 0x0001);
7892 }
7893
7894 struct l2cap_chan_data {
7895         struct l2cap_chan *chan;
7896         struct pid *pid;
7897         int count;
7898 };
7899
7900 static void l2cap_chan_by_pid(struct l2cap_chan *chan, void *data)
7901 {
7902         struct l2cap_chan_data *d = data;
7903         struct pid *pid;
7904
7905         if (chan == d->chan)
7906                 return;
7907
7908         if (!test_bit(FLAG_DEFER_SETUP, &chan->flags))
7909                 return;
7910
7911         pid = chan->ops->get_peer_pid(chan);
7912
7913         /* Only count deferred channels with the same PID/PSM */
7914         if (d->pid != pid || chan->psm != d->chan->psm || chan->ident ||
7915             chan->mode != L2CAP_MODE_EXT_FLOWCTL || chan->state != BT_CONNECT)
7916                 return;
7917
7918         d->count++;
7919 }
7920
7921 int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid,
7922                        bdaddr_t *dst, u8 dst_type)
7923 {
7924         struct l2cap_conn *conn;
7925         struct hci_conn *hcon;
7926         struct hci_dev *hdev;
7927         int err;
7928
7929         BT_DBG("%pMR -> %pMR (type %u) psm 0x%4.4x mode 0x%2.2x", &chan->src,
7930                dst, dst_type, __le16_to_cpu(psm), chan->mode);
7931
7932         hdev = hci_get_route(dst, &chan->src, chan->src_type);
7933         if (!hdev)
7934                 return -EHOSTUNREACH;
7935
7936         hci_dev_lock(hdev);
7937
7938         if (!is_valid_psm(__le16_to_cpu(psm), dst_type) && !cid &&
7939             chan->chan_type != L2CAP_CHAN_RAW) {
7940                 err = -EINVAL;
7941                 goto done;
7942         }
7943
7944         if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED && !psm) {
7945                 err = -EINVAL;
7946                 goto done;
7947         }
7948
7949         if (chan->chan_type == L2CAP_CHAN_FIXED && !cid) {
7950                 err = -EINVAL;
7951                 goto done;
7952         }
7953
7954         switch (chan->mode) {
7955         case L2CAP_MODE_BASIC:
7956                 break;
7957         case L2CAP_MODE_LE_FLOWCTL:
7958                 break;
7959         case L2CAP_MODE_EXT_FLOWCTL:
7960                 if (!enable_ecred) {
7961                         err = -EOPNOTSUPP;
7962                         goto done;
7963                 }
7964                 break;
7965         case L2CAP_MODE_ERTM:
7966         case L2CAP_MODE_STREAMING:
7967                 if (!disable_ertm)
7968                         break;
7969                 fallthrough;
7970         default:
7971                 err = -EOPNOTSUPP;
7972                 goto done;
7973         }
7974
7975         switch (chan->state) {
7976         case BT_CONNECT:
7977         case BT_CONNECT2:
7978         case BT_CONFIG:
7979                 /* Already connecting */
7980                 err = 0;
7981                 goto done;
7982
7983         case BT_CONNECTED:
7984                 /* Already connected */
7985                 err = -EISCONN;
7986                 goto done;
7987
7988         case BT_OPEN:
7989         case BT_BOUND:
7990                 /* Can connect */
7991                 break;
7992
7993         default:
7994                 err = -EBADFD;
7995                 goto done;
7996         }
7997
7998         /* Set destination address and psm */
7999         bacpy(&chan->dst, dst);
8000         chan->dst_type = dst_type;
8001
8002         chan->psm = psm;
8003         chan->dcid = cid;
8004
8005         if (bdaddr_type_is_le(dst_type)) {
8006                 /* Convert from L2CAP channel address type to HCI address type
8007                  */
8008                 if (dst_type == BDADDR_LE_PUBLIC)
8009                         dst_type = ADDR_LE_DEV_PUBLIC;
8010                 else
8011                         dst_type = ADDR_LE_DEV_RANDOM;
8012
8013                 if (hci_dev_test_flag(hdev, HCI_ADVERTISING))
8014                         hcon = hci_connect_le(hdev, dst, dst_type, false,
8015                                               chan->sec_level,
8016                                               HCI_LE_CONN_TIMEOUT,
8017                                               HCI_ROLE_SLAVE);
8018                 else
8019                         hcon = hci_connect_le_scan(hdev, dst, dst_type,
8020                                                    chan->sec_level,
8021                                                    HCI_LE_CONN_TIMEOUT,
8022                                                    CONN_REASON_L2CAP_CHAN);
8023
8024         } else {
8025                 u8 auth_type = l2cap_get_auth_type(chan);
8026                 hcon = hci_connect_acl(hdev, dst, chan->sec_level, auth_type,
8027                                        CONN_REASON_L2CAP_CHAN);
8028         }
8029
8030         if (IS_ERR(hcon)) {
8031                 err = PTR_ERR(hcon);
8032                 goto done;
8033         }
8034
8035         conn = l2cap_conn_add(hcon);
8036         if (!conn) {
8037                 hci_conn_drop(hcon);
8038                 err = -ENOMEM;
8039                 goto done;
8040         }
8041
8042         if (chan->mode == L2CAP_MODE_EXT_FLOWCTL) {
8043                 struct l2cap_chan_data data;
8044
8045                 data.chan = chan;
8046                 data.pid = chan->ops->get_peer_pid(chan);
8047                 data.count = 1;
8048
8049                 l2cap_chan_list(conn, l2cap_chan_by_pid, &data);
8050
8051                 /* Check if there isn't too many channels being connected */
8052                 if (data.count > L2CAP_ECRED_CONN_SCID_MAX) {
8053                         hci_conn_drop(hcon);
8054                         err = -EPROTO;
8055                         goto done;
8056                 }
8057         }
8058
8059         mutex_lock(&conn->chan_lock);
8060         l2cap_chan_lock(chan);
8061
8062         if (cid && __l2cap_get_chan_by_dcid(conn, cid)) {
8063                 hci_conn_drop(hcon);
8064                 err = -EBUSY;
8065                 goto chan_unlock;
8066         }
8067
8068         /* Update source addr of the socket */
8069         bacpy(&chan->src, &hcon->src);
8070         chan->src_type = bdaddr_src_type(hcon);
8071
8072         __l2cap_chan_add(conn, chan);
8073
8074         /* l2cap_chan_add takes its own ref so we can drop this one */
8075         hci_conn_drop(hcon);
8076
8077         l2cap_state_change(chan, BT_CONNECT);
8078         __set_chan_timer(chan, chan->ops->get_sndtimeo(chan));
8079
8080         /* Release chan->sport so that it can be reused by other
8081          * sockets (as it's only used for listening sockets).
8082          */
8083         write_lock(&chan_list_lock);
8084         chan->sport = 0;
8085         write_unlock(&chan_list_lock);
8086
8087         if (hcon->state == BT_CONNECTED) {
8088                 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
8089                         __clear_chan_timer(chan);
8090                         if (l2cap_chan_check_security(chan, true))
8091                                 l2cap_state_change(chan, BT_CONNECTED);
8092                 } else
8093                         l2cap_do_start(chan);
8094         }
8095
8096         err = 0;
8097
8098 chan_unlock:
8099         l2cap_chan_unlock(chan);
8100         mutex_unlock(&conn->chan_lock);
8101 done:
8102         hci_dev_unlock(hdev);
8103         hci_dev_put(hdev);
8104         return err;
8105 }
8106 EXPORT_SYMBOL_GPL(l2cap_chan_connect);
8107
8108 static void l2cap_ecred_reconfigure(struct l2cap_chan *chan)
8109 {
8110         struct l2cap_conn *conn = chan->conn;
8111         struct {
8112                 struct l2cap_ecred_reconf_req req;
8113                 __le16 scid;
8114         } pdu;
8115
8116         pdu.req.mtu = cpu_to_le16(chan->imtu);
8117         pdu.req.mps = cpu_to_le16(chan->mps);
8118         pdu.scid    = cpu_to_le16(chan->scid);
8119
8120         chan->ident = l2cap_get_ident(conn);
8121
8122         l2cap_send_cmd(conn, chan->ident, L2CAP_ECRED_RECONF_REQ,
8123                        sizeof(pdu), &pdu);
8124 }
8125
8126 int l2cap_chan_reconfigure(struct l2cap_chan *chan, __u16 mtu)
8127 {
8128         if (chan->imtu > mtu)
8129                 return -EINVAL;
8130
8131         BT_DBG("chan %p mtu 0x%4.4x", chan, mtu);
8132
8133         chan->imtu = mtu;
8134
8135         l2cap_ecred_reconfigure(chan);
8136
8137         return 0;
8138 }
8139
8140 /* ---- L2CAP interface with lower layer (HCI) ---- */
8141
8142 int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr)
8143 {
8144         int exact = 0, lm1 = 0, lm2 = 0;
8145         struct l2cap_chan *c;
8146
8147         BT_DBG("hdev %s, bdaddr %pMR", hdev->name, bdaddr);
8148
8149         /* Find listening sockets and check their link_mode */
8150         read_lock(&chan_list_lock);
8151         list_for_each_entry(c, &chan_list, global_l) {
8152                 if (c->state != BT_LISTEN)
8153                         continue;
8154
8155                 if (!bacmp(&c->src, &hdev->bdaddr)) {
8156                         lm1 |= HCI_LM_ACCEPT;
8157                         if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
8158                                 lm1 |= HCI_LM_MASTER;
8159                         exact++;
8160                 } else if (!bacmp(&c->src, BDADDR_ANY)) {
8161                         lm2 |= HCI_LM_ACCEPT;
8162                         if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
8163                                 lm2 |= HCI_LM_MASTER;
8164                 }
8165         }
8166         read_unlock(&chan_list_lock);
8167
8168         return exact ? lm1 : lm2;
8169 }
8170
8171 /* Find the next fixed channel in BT_LISTEN state, continue iteration
8172  * from an existing channel in the list or from the beginning of the
8173  * global list (by passing NULL as first parameter).
8174  */
8175 static struct l2cap_chan *l2cap_global_fixed_chan(struct l2cap_chan *c,
8176                                                   struct hci_conn *hcon)
8177 {
8178         u8 src_type = bdaddr_src_type(hcon);
8179
8180         read_lock(&chan_list_lock);
8181
8182         if (c)
8183                 c = list_next_entry(c, global_l);
8184         else
8185                 c = list_entry(chan_list.next, typeof(*c), global_l);
8186
8187         list_for_each_entry_from(c, &chan_list, global_l) {
8188                 if (c->chan_type != L2CAP_CHAN_FIXED)
8189                         continue;
8190                 if (c->state != BT_LISTEN)
8191                         continue;
8192                 if (bacmp(&c->src, &hcon->src) && bacmp(&c->src, BDADDR_ANY))
8193                         continue;
8194                 if (src_type != c->src_type)
8195                         continue;
8196
8197                 c = l2cap_chan_hold_unless_zero(c);
8198                 read_unlock(&chan_list_lock);
8199                 return c;
8200         }
8201
8202         read_unlock(&chan_list_lock);
8203
8204         return NULL;
8205 }
8206
8207 static void l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
8208 {
8209         struct hci_dev *hdev = hcon->hdev;
8210         struct l2cap_conn *conn;
8211         struct l2cap_chan *pchan;
8212         u8 dst_type;
8213
8214         if (hcon->type != ACL_LINK && hcon->type != LE_LINK)
8215                 return;
8216
8217         BT_DBG("hcon %p bdaddr %pMR status %d", hcon, &hcon->dst, status);
8218
8219         if (status) {
8220                 l2cap_conn_del(hcon, bt_to_errno(status));
8221                 return;
8222         }
8223
8224         conn = l2cap_conn_add(hcon);
8225         if (!conn)
8226                 return;
8227
8228         dst_type = bdaddr_dst_type(hcon);
8229
8230         /* If device is blocked, do not create channels for it */
8231         if (hci_bdaddr_list_lookup(&hdev->reject_list, &hcon->dst, dst_type))
8232                 return;
8233
8234         /* Find fixed channels and notify them of the new connection. We
8235          * use multiple individual lookups, continuing each time where
8236          * we left off, because the list lock would prevent calling the
8237          * potentially sleeping l2cap_chan_lock() function.
8238          */
8239         pchan = l2cap_global_fixed_chan(NULL, hcon);
8240         while (pchan) {
8241                 struct l2cap_chan *chan, *next;
8242
8243                 /* Client fixed channels should override server ones */
8244                 if (__l2cap_get_chan_by_dcid(conn, pchan->scid))
8245                         goto next;
8246
8247                 l2cap_chan_lock(pchan);
8248                 chan = pchan->ops->new_connection(pchan);
8249                 if (chan) {
8250                         bacpy(&chan->src, &hcon->src);
8251                         bacpy(&chan->dst, &hcon->dst);
8252                         chan->src_type = bdaddr_src_type(hcon);
8253                         chan->dst_type = dst_type;
8254
8255                         __l2cap_chan_add(conn, chan);
8256                 }
8257
8258                 l2cap_chan_unlock(pchan);
8259 next:
8260                 next = l2cap_global_fixed_chan(pchan, hcon);
8261                 l2cap_chan_put(pchan);
8262                 pchan = next;
8263         }
8264
8265         l2cap_conn_ready(conn);
8266 }
8267
8268 int l2cap_disconn_ind(struct hci_conn *hcon)
8269 {
8270         struct l2cap_conn *conn = hcon->l2cap_data;
8271
8272         BT_DBG("hcon %p", hcon);
8273
8274         if (!conn)
8275                 return HCI_ERROR_REMOTE_USER_TERM;
8276         return conn->disc_reason;
8277 }
8278
8279 static void l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
8280 {
8281         if (hcon->type != ACL_LINK && hcon->type != LE_LINK)
8282                 return;
8283
8284         BT_DBG("hcon %p reason %d", hcon, reason);
8285
8286         l2cap_conn_del(hcon, bt_to_errno(reason));
8287 }
8288
8289 static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt)
8290 {
8291         if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED)
8292                 return;
8293
8294         if (encrypt == 0x00) {
8295                 if (chan->sec_level == BT_SECURITY_MEDIUM) {
8296                         __set_chan_timer(chan, L2CAP_ENC_TIMEOUT);
8297                 } else if (chan->sec_level == BT_SECURITY_HIGH ||
8298                            chan->sec_level == BT_SECURITY_FIPS)
8299                         l2cap_chan_close(chan, ECONNREFUSED);
8300         } else {
8301                 if (chan->sec_level == BT_SECURITY_MEDIUM)
8302                         __clear_chan_timer(chan);
8303         }
8304 }
8305
8306 static void l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
8307 {
8308         struct l2cap_conn *conn = hcon->l2cap_data;
8309         struct l2cap_chan *chan;
8310
8311         if (!conn)
8312                 return;
8313
8314         BT_DBG("conn %p status 0x%2.2x encrypt %u", conn, status, encrypt);
8315
8316         mutex_lock(&conn->chan_lock);
8317
8318         list_for_each_entry(chan, &conn->chan_l, list) {
8319                 l2cap_chan_lock(chan);
8320
8321                 BT_DBG("chan %p scid 0x%4.4x state %s", chan, chan->scid,
8322                        state_to_string(chan->state));
8323
8324                 if (chan->scid == L2CAP_CID_A2MP) {
8325                         l2cap_chan_unlock(chan);
8326                         continue;
8327                 }
8328
8329                 if (!status && encrypt)
8330                         chan->sec_level = hcon->sec_level;
8331
8332                 if (!__l2cap_no_conn_pending(chan)) {
8333                         l2cap_chan_unlock(chan);
8334                         continue;
8335                 }
8336
8337                 if (!status && (chan->state == BT_CONNECTED ||
8338                                 chan->state == BT_CONFIG)) {
8339                         chan->ops->resume(chan);
8340                         l2cap_check_encryption(chan, encrypt);
8341                         l2cap_chan_unlock(chan);
8342                         continue;
8343                 }
8344
8345                 if (chan->state == BT_CONNECT) {
8346                         if (!status && l2cap_check_enc_key_size(hcon))
8347                                 l2cap_start_connection(chan);
8348                         else
8349                                 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
8350                 } else if (chan->state == BT_CONNECT2 &&
8351                            !(chan->mode == L2CAP_MODE_EXT_FLOWCTL ||
8352                              chan->mode == L2CAP_MODE_LE_FLOWCTL)) {
8353                         struct l2cap_conn_rsp rsp;
8354                         __u16 res, stat;
8355
8356                         if (!status && l2cap_check_enc_key_size(hcon)) {
8357                                 if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
8358                                         res = L2CAP_CR_PEND;
8359                                         stat = L2CAP_CS_AUTHOR_PEND;
8360                                         chan->ops->defer(chan);
8361                                 } else {
8362                                         l2cap_state_change(chan, BT_CONFIG);
8363                                         res = L2CAP_CR_SUCCESS;
8364                                         stat = L2CAP_CS_NO_INFO;
8365                                 }
8366                         } else {
8367                                 l2cap_state_change(chan, BT_DISCONN);
8368                                 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
8369                                 res = L2CAP_CR_SEC_BLOCK;
8370                                 stat = L2CAP_CS_NO_INFO;
8371                         }
8372
8373                         rsp.scid   = cpu_to_le16(chan->dcid);
8374                         rsp.dcid   = cpu_to_le16(chan->scid);
8375                         rsp.result = cpu_to_le16(res);
8376                         rsp.status = cpu_to_le16(stat);
8377                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
8378                                        sizeof(rsp), &rsp);
8379
8380                         if (!test_bit(CONF_REQ_SENT, &chan->conf_state) &&
8381                             res == L2CAP_CR_SUCCESS) {
8382                                 char buf[128];
8383                                 set_bit(CONF_REQ_SENT, &chan->conf_state);
8384                                 l2cap_send_cmd(conn, l2cap_get_ident(conn),
8385                                                L2CAP_CONF_REQ,
8386                                                l2cap_build_conf_req(chan, buf, sizeof(buf)),
8387                                                buf);
8388                                 chan->num_conf_req++;
8389                         }
8390                 }
8391
8392                 l2cap_chan_unlock(chan);
8393         }
8394
8395         mutex_unlock(&conn->chan_lock);
8396 }
8397
8398 /* Append fragment into frame respecting the maximum len of rx_skb */
8399 static int l2cap_recv_frag(struct l2cap_conn *conn, struct sk_buff *skb,
8400                            u16 len)
8401 {
8402         if (!conn->rx_skb) {
8403                 /* Allocate skb for the complete frame (with header) */
8404                 conn->rx_skb = bt_skb_alloc(len, GFP_KERNEL);
8405                 if (!conn->rx_skb)
8406                         return -ENOMEM;
8407                 /* Init rx_len */
8408                 conn->rx_len = len;
8409         }
8410
8411         /* Copy as much as the rx_skb can hold */
8412         len = min_t(u16, len, skb->len);
8413         skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, len), len);
8414         skb_pull(skb, len);
8415         conn->rx_len -= len;
8416
8417         return len;
8418 }
8419
8420 static int l2cap_recv_len(struct l2cap_conn *conn, struct sk_buff *skb)
8421 {
8422         struct sk_buff *rx_skb;
8423         int len;
8424
8425         /* Append just enough to complete the header */
8426         len = l2cap_recv_frag(conn, skb, L2CAP_LEN_SIZE - conn->rx_skb->len);
8427
8428         /* If header could not be read just continue */
8429         if (len < 0 || conn->rx_skb->len < L2CAP_LEN_SIZE)
8430                 return len;
8431
8432         rx_skb = conn->rx_skb;
8433         len = get_unaligned_le16(rx_skb->data);
8434
8435         /* Check if rx_skb has enough space to received all fragments */
8436         if (len + (L2CAP_HDR_SIZE - L2CAP_LEN_SIZE) <= skb_tailroom(rx_skb)) {
8437                 /* Update expected len */
8438                 conn->rx_len = len + (L2CAP_HDR_SIZE - L2CAP_LEN_SIZE);
8439                 return L2CAP_LEN_SIZE;
8440         }
8441
8442         /* Reset conn->rx_skb since it will need to be reallocated in order to
8443          * fit all fragments.
8444          */
8445         conn->rx_skb = NULL;
8446
8447         /* Reallocates rx_skb using the exact expected length */
8448         len = l2cap_recv_frag(conn, rx_skb,
8449                               len + (L2CAP_HDR_SIZE - L2CAP_LEN_SIZE));
8450         kfree_skb(rx_skb);
8451
8452         return len;
8453 }
8454
8455 static void l2cap_recv_reset(struct l2cap_conn *conn)
8456 {
8457         kfree_skb(conn->rx_skb);
8458         conn->rx_skb = NULL;
8459         conn->rx_len = 0;
8460 }
8461
8462 void l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
8463 {
8464         struct l2cap_conn *conn = hcon->l2cap_data;
8465         int len;
8466
8467         /* For AMP controller do not create l2cap conn */
8468         if (!conn && hcon->hdev->dev_type != HCI_PRIMARY)
8469                 goto drop;
8470
8471         if (!conn)
8472                 conn = l2cap_conn_add(hcon);
8473
8474         if (!conn)
8475                 goto drop;
8476
8477         BT_DBG("conn %p len %u flags 0x%x", conn, skb->len, flags);
8478
8479         switch (flags) {
8480         case ACL_START:
8481         case ACL_START_NO_FLUSH:
8482         case ACL_COMPLETE:
8483                 if (conn->rx_skb) {
8484                         BT_ERR("Unexpected start frame (len %d)", skb->len);
8485                         l2cap_recv_reset(conn);
8486                         l2cap_conn_unreliable(conn, ECOMM);
8487                 }
8488
8489                 /* Start fragment may not contain the L2CAP length so just
8490                  * copy the initial byte when that happens and use conn->mtu as
8491                  * expected length.
8492                  */
8493                 if (skb->len < L2CAP_LEN_SIZE) {
8494                         l2cap_recv_frag(conn, skb, conn->mtu);
8495                         break;
8496                 }
8497
8498                 len = get_unaligned_le16(skb->data) + L2CAP_HDR_SIZE;
8499
8500                 if (len == skb->len) {
8501                         /* Complete frame received */
8502                         l2cap_recv_frame(conn, skb);
8503                         return;
8504                 }
8505
8506                 BT_DBG("Start: total len %d, frag len %u", len, skb->len);
8507
8508                 if (skb->len > len) {
8509                         BT_ERR("Frame is too long (len %u, expected len %d)",
8510                                skb->len, len);
8511                         l2cap_conn_unreliable(conn, ECOMM);
8512                         goto drop;
8513                 }
8514
8515                 /* Append fragment into frame (with header) */
8516                 if (l2cap_recv_frag(conn, skb, len) < 0)
8517                         goto drop;
8518
8519                 break;
8520
8521         case ACL_CONT:
8522                 BT_DBG("Cont: frag len %u (expecting %u)", skb->len, conn->rx_len);
8523
8524                 if (!conn->rx_skb) {
8525                         BT_ERR("Unexpected continuation frame (len %d)", skb->len);
8526                         l2cap_conn_unreliable(conn, ECOMM);
8527                         goto drop;
8528                 }
8529
8530                 /* Complete the L2CAP length if it has not been read */
8531                 if (conn->rx_skb->len < L2CAP_LEN_SIZE) {
8532                         if (l2cap_recv_len(conn, skb) < 0) {
8533                                 l2cap_conn_unreliable(conn, ECOMM);
8534                                 goto drop;
8535                         }
8536
8537                         /* Header still could not be read just continue */
8538                         if (conn->rx_skb->len < L2CAP_LEN_SIZE)
8539                                 break;
8540                 }
8541
8542                 if (skb->len > conn->rx_len) {
8543                         BT_ERR("Fragment is too long (len %u, expected %u)",
8544                                skb->len, conn->rx_len);
8545                         l2cap_recv_reset(conn);
8546                         l2cap_conn_unreliable(conn, ECOMM);
8547                         goto drop;
8548                 }
8549
8550                 /* Append fragment into frame (with header) */
8551                 l2cap_recv_frag(conn, skb, skb->len);
8552
8553                 if (!conn->rx_len) {
8554                         /* Complete frame received. l2cap_recv_frame
8555                          * takes ownership of the skb so set the global
8556                          * rx_skb pointer to NULL first.
8557                          */
8558                         struct sk_buff *rx_skb = conn->rx_skb;
8559                         conn->rx_skb = NULL;
8560                         l2cap_recv_frame(conn, rx_skb);
8561                 }
8562                 break;
8563         }
8564
8565 drop:
8566         kfree_skb(skb);
8567 }
8568
8569 static struct hci_cb l2cap_cb = {
8570         .name           = "L2CAP",
8571         .connect_cfm    = l2cap_connect_cfm,
8572         .disconn_cfm    = l2cap_disconn_cfm,
8573         .security_cfm   = l2cap_security_cfm,
8574 };
8575
8576 static int l2cap_debugfs_show(struct seq_file *f, void *p)
8577 {
8578         struct l2cap_chan *c;
8579
8580         read_lock(&chan_list_lock);
8581
8582         list_for_each_entry(c, &chan_list, global_l) {
8583                 seq_printf(f, "%pMR (%u) %pMR (%u) %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
8584                            &c->src, c->src_type, &c->dst, c->dst_type,
8585                            c->state, __le16_to_cpu(c->psm),
8586                            c->scid, c->dcid, c->imtu, c->omtu,
8587                            c->sec_level, c->mode);
8588         }
8589
8590         read_unlock(&chan_list_lock);
8591
8592         return 0;
8593 }
8594
8595 DEFINE_SHOW_ATTRIBUTE(l2cap_debugfs);
8596
8597 static struct dentry *l2cap_debugfs;
8598
8599 int __init l2cap_init(void)
8600 {
8601         int err;
8602
8603         err = l2cap_init_sockets();
8604         if (err < 0)
8605                 return err;
8606
8607         hci_register_cb(&l2cap_cb);
8608
8609         if (IS_ERR_OR_NULL(bt_debugfs))
8610                 return 0;
8611
8612         l2cap_debugfs = debugfs_create_file("l2cap", 0444, bt_debugfs,
8613                                             NULL, &l2cap_debugfs_fops);
8614
8615         return 0;
8616 }
8617
8618 void l2cap_exit(void)
8619 {
8620         debugfs_remove(l2cap_debugfs);
8621         hci_unregister_cb(&l2cap_cb);
8622         l2cap_cleanup_sockets();
8623 }
8624
8625 module_param(disable_ertm, bool, 0644);
8626 MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode");
8627
8628 module_param(enable_ecred, bool, 0644);
8629 MODULE_PARM_DESC(enable_ecred, "Enable enhanced credit flow control mode");