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