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