net/bluetooth/l2cap_core.c

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