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