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