net/bluetooth/l2cap_core.c

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