net/bluetooth/l2cap_core.c

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