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