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 static bool is_valid_psm(u16 psm, u8 dst_type)
1865 {
1866         if (!psm)
1867                 return false;
1868
1869         if (bdaddr_type_is_le(dst_type))
1870                 return (psm < 0x00ff);
1871
1872         /* PSM must be odd and lsb of upper byte must be 0 */
1873         return ((psm & 0x0101) == 0x0001);
1874 }
1875
1876 int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid,
1877                        bdaddr_t *dst, u8 dst_type)
1878 {
1879         struct l2cap_conn *conn;
1880         struct hci_conn *hcon;
1881         struct hci_dev *hdev;
1882         __u8 auth_type;
1883         int err;
1884
1885         BT_DBG("%pMR -> %pMR (type %u) psm 0x%2.2x", &chan->src, dst,
1886                dst_type, __le16_to_cpu(psm));
1887
1888         hdev = hci_get_route(dst, &chan->src);
1889         if (!hdev)
1890                 return -EHOSTUNREACH;
1891
1892         hci_dev_lock(hdev);
1893
1894         l2cap_chan_lock(chan);
1895
1896         if (!is_valid_psm(__le16_to_cpu(psm), dst_type) && !cid &&
1897             chan->chan_type != L2CAP_CHAN_RAW) {
1898                 err = -EINVAL;
1899                 goto done;
1900         }
1901
1902         if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED && !(psm || cid)) {
1903                 err = -EINVAL;
1904                 goto done;
1905         }
1906
1907         switch (chan->mode) {
1908         case L2CAP_MODE_BASIC:
1909         case L2CAP_MODE_LE_FLOWCTL:
1910                 break;
1911         case L2CAP_MODE_ERTM:
1912         case L2CAP_MODE_STREAMING:
1913                 if (!disable_ertm)
1914                         break;
1915                 /* fall through */
1916         default:
1917                 err = -ENOTSUPP;
1918                 goto done;
1919         }
1920
1921         switch (chan->state) {
1922         case BT_CONNECT:
1923         case BT_CONNECT2:
1924         case BT_CONFIG:
1925                 /* Already connecting */
1926                 err = 0;
1927                 goto done;
1928
1929         case BT_CONNECTED:
1930                 /* Already connected */
1931                 err = -EISCONN;
1932                 goto done;
1933
1934         case BT_OPEN:
1935         case BT_BOUND:
1936                 /* Can connect */
1937                 break;
1938
1939         default:
1940                 err = -EBADFD;
1941                 goto done;
1942         }
1943
1944         /* Set destination address and psm */
1945         bacpy(&chan->dst, dst);
1946         chan->dst_type = dst_type;
1947
1948         chan->psm = psm;
1949         chan->dcid = cid;
1950
1951         auth_type = l2cap_get_auth_type(chan);
1952
1953         if (bdaddr_type_is_le(dst_type))
1954                 hcon = hci_connect(hdev, LE_LINK, dst, dst_type,
1955                                    chan->sec_level, auth_type);
1956         else
1957                 hcon = hci_connect(hdev, ACL_LINK, dst, dst_type,
1958                                    chan->sec_level, auth_type);
1959
1960         if (IS_ERR(hcon)) {
1961                 err = PTR_ERR(hcon);
1962                 goto done;
1963         }
1964
1965         conn = l2cap_conn_add(hcon);
1966         if (!conn) {
1967                 hci_conn_drop(hcon);
1968                 err = -ENOMEM;
1969                 goto done;
1970         }
1971
1972         if (cid && __l2cap_get_chan_by_dcid(conn, cid)) {
1973                 hci_conn_drop(hcon);
1974                 err = -EBUSY;
1975                 goto done;
1976         }
1977
1978         /* Update source addr of the socket */
1979         bacpy(&chan->src, &hcon->src);
1980         chan->src_type = bdaddr_type(hcon, hcon->src_type);
1981
1982         l2cap_chan_unlock(chan);
1983         l2cap_chan_add(conn, chan);
1984         l2cap_chan_lock(chan);
1985
1986         /* l2cap_chan_add takes its own ref so we can drop this one */
1987         hci_conn_drop(hcon);
1988
1989         l2cap_state_change(chan, BT_CONNECT);
1990         __set_chan_timer(chan, chan->ops->get_sndtimeo(chan));
1991
1992         if (hcon->state == BT_CONNECTED) {
1993                 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
1994                         __clear_chan_timer(chan);
1995                         if (l2cap_chan_check_security(chan))
1996                                 l2cap_state_change(chan, BT_CONNECTED);
1997                 } else
1998                         l2cap_do_start(chan);
1999         }
2000
2001         err = 0;
2002
2003 done:
2004         l2cap_chan_unlock(chan);
2005         hci_dev_unlock(hdev);
2006         hci_dev_put(hdev);
2007         return err;
2008 }
2009
2010 static void l2cap_monitor_timeout(struct work_struct *work)
2011 {
2012         struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
2013                                                monitor_timer.work);
2014
2015         BT_DBG("chan %p", chan);
2016
2017         l2cap_chan_lock(chan);
2018
2019         if (!chan->conn) {
2020                 l2cap_chan_unlock(chan);
2021                 l2cap_chan_put(chan);
2022                 return;
2023         }
2024
2025         l2cap_tx(chan, NULL, NULL, L2CAP_EV_MONITOR_TO);
2026
2027         l2cap_chan_unlock(chan);
2028         l2cap_chan_put(chan);
2029 }
2030
2031 static void l2cap_retrans_timeout(struct work_struct *work)
2032 {
2033         struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
2034                                                retrans_timer.work);
2035
2036         BT_DBG("chan %p", chan);
2037
2038         l2cap_chan_lock(chan);
2039
2040         if (!chan->conn) {
2041                 l2cap_chan_unlock(chan);
2042                 l2cap_chan_put(chan);
2043                 return;
2044         }
2045
2046         l2cap_tx(chan, NULL, NULL, L2CAP_EV_RETRANS_TO);
2047         l2cap_chan_unlock(chan);
2048         l2cap_chan_put(chan);
2049 }
2050
2051 static void l2cap_streaming_send(struct l2cap_chan *chan,
2052                                  struct sk_buff_head *skbs)
2053 {
2054         struct sk_buff *skb;
2055         struct l2cap_ctrl *control;
2056
2057         BT_DBG("chan %p, skbs %p", chan, skbs);
2058
2059         if (__chan_is_moving(chan))
2060                 return;
2061
2062         skb_queue_splice_tail_init(skbs, &chan->tx_q);
2063
2064         while (!skb_queue_empty(&chan->tx_q)) {
2065
2066                 skb = skb_dequeue(&chan->tx_q);
2067
2068                 bt_cb(skb)->control.retries = 1;
2069                 control = &bt_cb(skb)->control;
2070
2071                 control->reqseq = 0;
2072                 control->txseq = chan->next_tx_seq;
2073
2074                 __pack_control(chan, control, skb);
2075
2076                 if (chan->fcs == L2CAP_FCS_CRC16) {
2077                         u16 fcs = crc16(0, (u8 *) skb->data, skb->len);
2078                         put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE));
2079                 }
2080
2081                 l2cap_do_send(chan, skb);
2082
2083                 BT_DBG("Sent txseq %u", control->txseq);
2084
2085                 chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq);
2086                 chan->frames_sent++;
2087         }
2088 }
2089
2090 static int l2cap_ertm_send(struct l2cap_chan *chan)
2091 {
2092         struct sk_buff *skb, *tx_skb;
2093         struct l2cap_ctrl *control;
2094         int sent = 0;
2095
2096         BT_DBG("chan %p", chan);
2097
2098         if (chan->state != BT_CONNECTED)
2099                 return -ENOTCONN;
2100
2101         if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
2102                 return 0;
2103
2104         if (__chan_is_moving(chan))
2105                 return 0;
2106
2107         while (chan->tx_send_head &&
2108                chan->unacked_frames < chan->remote_tx_win &&
2109                chan->tx_state == L2CAP_TX_STATE_XMIT) {
2110
2111                 skb = chan->tx_send_head;
2112
2113                 bt_cb(skb)->control.retries = 1;
2114                 control = &bt_cb(skb)->control;
2115
2116                 if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
2117                         control->final = 1;
2118
2119                 control->reqseq = chan->buffer_seq;
2120                 chan->last_acked_seq = chan->buffer_seq;
2121                 control->txseq = chan->next_tx_seq;
2122
2123                 __pack_control(chan, control, skb);
2124
2125                 if (chan->fcs == L2CAP_FCS_CRC16) {
2126                         u16 fcs = crc16(0, (u8 *) skb->data, skb->len);
2127                         put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE));
2128                 }
2129
2130                 /* Clone after data has been modified. Data is assumed to be
2131                    read-only (for locking purposes) on cloned sk_buffs.
2132                  */
2133                 tx_skb = skb_clone(skb, GFP_KERNEL);
2134
2135                 if (!tx_skb)
2136                         break;
2137
2138                 __set_retrans_timer(chan);
2139
2140                 chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq);
2141                 chan->unacked_frames++;
2142                 chan->frames_sent++;
2143                 sent++;
2144
2145                 if (skb_queue_is_last(&chan->tx_q, skb))
2146                         chan->tx_send_head = NULL;
2147                 else
2148                         chan->tx_send_head = skb_queue_next(&chan->tx_q, skb);
2149
2150                 l2cap_do_send(chan, tx_skb);
2151                 BT_DBG("Sent txseq %u", control->txseq);
2152         }
2153
2154         BT_DBG("Sent %d, %u unacked, %u in ERTM queue", sent,
2155                chan->unacked_frames, skb_queue_len(&chan->tx_q));
2156
2157         return sent;
2158 }
2159
2160 static void l2cap_ertm_resend(struct l2cap_chan *chan)
2161 {
2162         struct l2cap_ctrl control;
2163         struct sk_buff *skb;
2164         struct sk_buff *tx_skb;
2165         u16 seq;
2166
2167         BT_DBG("chan %p", chan);
2168
2169         if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
2170                 return;
2171
2172         if (__chan_is_moving(chan))
2173                 return;
2174
2175         while (chan->retrans_list.head != L2CAP_SEQ_LIST_CLEAR) {
2176                 seq = l2cap_seq_list_pop(&chan->retrans_list);
2177
2178                 skb = l2cap_ertm_seq_in_queue(&chan->tx_q, seq);
2179                 if (!skb) {
2180                         BT_DBG("Error: Can't retransmit seq %d, frame missing",
2181                                seq);
2182                         continue;
2183                 }
2184
2185                 bt_cb(skb)->control.retries++;
2186                 control = bt_cb(skb)->control;
2187
2188                 if (chan->max_tx != 0 &&
2189                     bt_cb(skb)->control.retries > chan->max_tx) {
2190                         BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
2191                         l2cap_send_disconn_req(chan, ECONNRESET);
2192                         l2cap_seq_list_clear(&chan->retrans_list);
2193                         break;
2194                 }
2195
2196                 control.reqseq = chan->buffer_seq;
2197                 if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
2198                         control.final = 1;
2199                 else
2200                         control.final = 0;
2201
2202                 if (skb_cloned(skb)) {
2203                         /* Cloned sk_buffs are read-only, so we need a
2204                          * writeable copy
2205                          */
2206                         tx_skb = skb_copy(skb, GFP_KERNEL);
2207                 } else {
2208                         tx_skb = skb_clone(skb, GFP_KERNEL);
2209                 }
2210
2211                 if (!tx_skb) {
2212                         l2cap_seq_list_clear(&chan->retrans_list);
2213                         break;
2214                 }
2215
2216                 /* Update skb contents */
2217                 if (test_bit(FLAG_EXT_CTRL, &chan->flags)) {
2218                         put_unaligned_le32(__pack_extended_control(&control),
2219                                            tx_skb->data + L2CAP_HDR_SIZE);
2220                 } else {
2221                         put_unaligned_le16(__pack_enhanced_control(&control),
2222                                            tx_skb->data + L2CAP_HDR_SIZE);
2223                 }
2224
2225                 if (chan->fcs == L2CAP_FCS_CRC16) {
2226                         u16 fcs = crc16(0, (u8 *) tx_skb->data, tx_skb->len);
2227                         put_unaligned_le16(fcs, skb_put(tx_skb,
2228                                                         L2CAP_FCS_SIZE));
2229                 }
2230
2231                 l2cap_do_send(chan, tx_skb);
2232
2233                 BT_DBG("Resent txseq %d", control.txseq);
2234
2235                 chan->last_acked_seq = chan->buffer_seq;
2236         }
2237 }
2238
2239 static void l2cap_retransmit(struct l2cap_chan *chan,
2240                              struct l2cap_ctrl *control)
2241 {
2242         BT_DBG("chan %p, control %p", chan, control);
2243
2244         l2cap_seq_list_append(&chan->retrans_list, control->reqseq);
2245         l2cap_ertm_resend(chan);
2246 }
2247
2248 static void l2cap_retransmit_all(struct l2cap_chan *chan,
2249                                  struct l2cap_ctrl *control)
2250 {
2251         struct sk_buff *skb;
2252
2253         BT_DBG("chan %p, control %p", chan, control);
2254
2255         if (control->poll)
2256                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
2257
2258         l2cap_seq_list_clear(&chan->retrans_list);
2259
2260         if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
2261                 return;
2262
2263         if (chan->unacked_frames) {
2264                 skb_queue_walk(&chan->tx_q, skb) {
2265                         if (bt_cb(skb)->control.txseq == control->reqseq ||
2266                             skb == chan->tx_send_head)
2267                                 break;
2268                 }
2269
2270                 skb_queue_walk_from(&chan->tx_q, skb) {
2271                         if (skb == chan->tx_send_head)
2272                                 break;
2273
2274                         l2cap_seq_list_append(&chan->retrans_list,
2275                                               bt_cb(skb)->control.txseq);
2276                 }
2277
2278                 l2cap_ertm_resend(chan);
2279         }
2280 }
2281
2282 static void l2cap_send_ack(struct l2cap_chan *chan)
2283 {
2284         struct l2cap_ctrl control;
2285         u16 frames_to_ack = __seq_offset(chan, chan->buffer_seq,
2286                                          chan->last_acked_seq);
2287         int threshold;
2288
2289         BT_DBG("chan %p last_acked_seq %d buffer_seq %d",
2290                chan, chan->last_acked_seq, chan->buffer_seq);
2291
2292         memset(&control, 0, sizeof(control));
2293         control.sframe = 1;
2294
2295         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
2296             chan->rx_state == L2CAP_RX_STATE_RECV) {
2297                 __clear_ack_timer(chan);
2298                 control.super = L2CAP_SUPER_RNR;
2299                 control.reqseq = chan->buffer_seq;
2300                 l2cap_send_sframe(chan, &control);
2301         } else {
2302                 if (!test_bit(CONN_REMOTE_BUSY, &chan->conn_state)) {
2303                         l2cap_ertm_send(chan);
2304                         /* If any i-frames were sent, they included an ack */
2305                         if (chan->buffer_seq == chan->last_acked_seq)
2306                                 frames_to_ack = 0;
2307                 }
2308
2309                 /* Ack now if the window is 3/4ths full.
2310                  * Calculate without mul or div
2311                  */
2312                 threshold = chan->ack_win;
2313                 threshold += threshold << 1;
2314                 threshold >>= 2;
2315
2316                 BT_DBG("frames_to_ack %u, threshold %d", frames_to_ack,
2317                        threshold);
2318
2319                 if (frames_to_ack >= threshold) {
2320                         __clear_ack_timer(chan);
2321                         control.super = L2CAP_SUPER_RR;
2322                         control.reqseq = chan->buffer_seq;
2323                         l2cap_send_sframe(chan, &control);
2324                         frames_to_ack = 0;
2325                 }
2326
2327                 if (frames_to_ack)
2328                         __set_ack_timer(chan);
2329         }
2330 }
2331
2332 static inline int l2cap_skbuff_fromiovec(struct l2cap_chan *chan,
2333                                          struct msghdr *msg, int len,
2334                                          int count, struct sk_buff *skb)
2335 {
2336         struct l2cap_conn *conn = chan->conn;
2337         struct sk_buff **frag;
2338         int sent = 0;
2339
2340         if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count))
2341                 return -EFAULT;
2342
2343         sent += count;
2344         len  -= count;
2345
2346         /* Continuation fragments (no L2CAP header) */
2347         frag = &skb_shinfo(skb)->frag_list;
2348         while (len) {
2349                 struct sk_buff *tmp;
2350
2351                 count = min_t(unsigned int, conn->mtu, len);
2352
2353                 tmp = chan->ops->alloc_skb(chan, count,
2354                                            msg->msg_flags & MSG_DONTWAIT);
2355                 if (IS_ERR(tmp))
2356                         return PTR_ERR(tmp);
2357
2358                 *frag = tmp;
2359
2360                 if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count))
2361                         return -EFAULT;
2362
2363                 (*frag)->priority = skb->priority;
2364
2365                 sent += count;
2366                 len  -= count;
2367
2368                 skb->len += (*frag)->len;
2369                 skb->data_len += (*frag)->len;
2370
2371                 frag = &(*frag)->next;
2372         }
2373
2374         return sent;
2375 }
2376
2377 static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan,
2378                                                  struct msghdr *msg, size_t len,
2379                                                  u32 priority)
2380 {
2381         struct l2cap_conn *conn = chan->conn;
2382         struct sk_buff *skb;
2383         int err, count, hlen = L2CAP_HDR_SIZE + L2CAP_PSMLEN_SIZE;
2384         struct l2cap_hdr *lh;
2385
2386         BT_DBG("chan %p psm 0x%2.2x len %zu priority %u", chan,
2387                __le16_to_cpu(chan->psm), len, priority);
2388
2389         count = min_t(unsigned int, (conn->mtu - hlen), len);
2390
2391         skb = chan->ops->alloc_skb(chan, count + hlen,
2392                                    msg->msg_flags & MSG_DONTWAIT);
2393         if (IS_ERR(skb))
2394                 return skb;
2395
2396         skb->priority = priority;
2397
2398         /* Create L2CAP header */
2399         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
2400         lh->cid = cpu_to_le16(chan->dcid);
2401         lh->len = cpu_to_le16(len + L2CAP_PSMLEN_SIZE);
2402         put_unaligned(chan->psm, (__le16 *) skb_put(skb, L2CAP_PSMLEN_SIZE));
2403
2404         err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb);
2405         if (unlikely(err < 0)) {
2406                 kfree_skb(skb);
2407                 return ERR_PTR(err);
2408         }
2409         return skb;
2410 }
2411
2412 static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan,
2413                                               struct msghdr *msg, size_t len,
2414                                               u32 priority)
2415 {
2416         struct l2cap_conn *conn = chan->conn;
2417         struct sk_buff *skb;
2418         int err, count;
2419         struct l2cap_hdr *lh;
2420
2421         BT_DBG("chan %p len %zu", chan, len);
2422
2423         count = min_t(unsigned int, (conn->mtu - L2CAP_HDR_SIZE), len);
2424
2425         skb = chan->ops->alloc_skb(chan, count + L2CAP_HDR_SIZE,
2426                                    msg->msg_flags & MSG_DONTWAIT);
2427         if (IS_ERR(skb))
2428                 return skb;
2429
2430         skb->priority = priority;
2431
2432         /* Create L2CAP header */
2433         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
2434         lh->cid = cpu_to_le16(chan->dcid);
2435         lh->len = cpu_to_le16(len);
2436
2437         err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb);
2438         if (unlikely(err < 0)) {
2439                 kfree_skb(skb);
2440                 return ERR_PTR(err);
2441         }
2442         return skb;
2443 }
2444
2445 static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan,
2446                                                struct msghdr *msg, size_t len,
2447                                                u16 sdulen)
2448 {
2449         struct l2cap_conn *conn = chan->conn;
2450         struct sk_buff *skb;
2451         int err, count, hlen;
2452         struct l2cap_hdr *lh;
2453
2454         BT_DBG("chan %p len %zu", chan, len);
2455
2456         if (!conn)
2457                 return ERR_PTR(-ENOTCONN);
2458
2459         hlen = __ertm_hdr_size(chan);
2460
2461         if (sdulen)
2462                 hlen += L2CAP_SDULEN_SIZE;
2463
2464         if (chan->fcs == L2CAP_FCS_CRC16)
2465                 hlen += L2CAP_FCS_SIZE;
2466
2467         count = min_t(unsigned int, (conn->mtu - hlen), len);
2468
2469         skb = chan->ops->alloc_skb(chan, count + hlen,
2470                                    msg->msg_flags & MSG_DONTWAIT);
2471         if (IS_ERR(skb))
2472                 return skb;
2473
2474         /* Create L2CAP header */
2475         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
2476         lh->cid = cpu_to_le16(chan->dcid);
2477         lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
2478
2479         /* Control header is populated later */
2480         if (test_bit(FLAG_EXT_CTRL, &chan->flags))
2481                 put_unaligned_le32(0, skb_put(skb, L2CAP_EXT_CTRL_SIZE));
2482         else
2483                 put_unaligned_le16(0, skb_put(skb, L2CAP_ENH_CTRL_SIZE));
2484
2485         if (sdulen)
2486                 put_unaligned_le16(sdulen, skb_put(skb, L2CAP_SDULEN_SIZE));
2487
2488         err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb);
2489         if (unlikely(err < 0)) {
2490                 kfree_skb(skb);
2491                 return ERR_PTR(err);
2492         }
2493
2494         bt_cb(skb)->control.fcs = chan->fcs;
2495         bt_cb(skb)->control.retries = 0;
2496         return skb;
2497 }
2498
2499 static int l2cap_segment_sdu(struct l2cap_chan *chan,
2500                              struct sk_buff_head *seg_queue,
2501                              struct msghdr *msg, size_t len)
2502 {
2503         struct sk_buff *skb;
2504         u16 sdu_len;
2505         size_t pdu_len;
2506         u8 sar;
2507
2508         BT_DBG("chan %p, msg %p, len %zu", chan, msg, len);
2509
2510         /* It is critical that ERTM PDUs fit in a single HCI fragment,
2511          * so fragmented skbs are not used.  The HCI layer's handling
2512          * of fragmented skbs is not compatible with ERTM's queueing.
2513          */
2514
2515         /* PDU size is derived from the HCI MTU */
2516         pdu_len = chan->conn->mtu;
2517
2518         /* Constrain PDU size for BR/EDR connections */
2519         if (!chan->hs_hcon)
2520                 pdu_len = min_t(size_t, pdu_len, L2CAP_BREDR_MAX_PAYLOAD);
2521
2522         /* Adjust for largest possible L2CAP overhead. */
2523         if (chan->fcs)
2524                 pdu_len -= L2CAP_FCS_SIZE;
2525
2526         pdu_len -= __ertm_hdr_size(chan);
2527
2528         /* Remote device may have requested smaller PDUs */
2529         pdu_len = min_t(size_t, pdu_len, chan->remote_mps);
2530
2531         if (len <= pdu_len) {
2532                 sar = L2CAP_SAR_UNSEGMENTED;
2533                 sdu_len = 0;
2534                 pdu_len = len;
2535         } else {
2536                 sar = L2CAP_SAR_START;
2537                 sdu_len = len;
2538                 pdu_len -= L2CAP_SDULEN_SIZE;
2539         }
2540
2541         while (len > 0) {
2542                 skb = l2cap_create_iframe_pdu(chan, msg, pdu_len, sdu_len);
2543
2544                 if (IS_ERR(skb)) {
2545                         __skb_queue_purge(seg_queue);
2546                         return PTR_ERR(skb);
2547                 }
2548
2549                 bt_cb(skb)->control.sar = sar;
2550                 __skb_queue_tail(seg_queue, skb);
2551
2552                 len -= pdu_len;
2553                 if (sdu_len) {
2554                         sdu_len = 0;
2555                         pdu_len += L2CAP_SDULEN_SIZE;
2556                 }
2557
2558                 if (len <= pdu_len) {
2559                         sar = L2CAP_SAR_END;
2560                         pdu_len = len;
2561                 } else {
2562                         sar = L2CAP_SAR_CONTINUE;
2563                 }
2564         }
2565
2566         return 0;
2567 }
2568
2569 static struct sk_buff *l2cap_create_le_flowctl_pdu(struct l2cap_chan *chan,
2570                                                    struct msghdr *msg,
2571                                                    size_t len, u16 sdulen)
2572 {
2573         struct l2cap_conn *conn = chan->conn;
2574         struct sk_buff *skb;
2575         int err, count, hlen;
2576         struct l2cap_hdr *lh;
2577
2578         BT_DBG("chan %p len %zu", chan, len);
2579
2580         if (!conn)
2581                 return ERR_PTR(-ENOTCONN);
2582
2583         hlen = L2CAP_HDR_SIZE;
2584
2585         if (sdulen)
2586                 hlen += L2CAP_SDULEN_SIZE;
2587
2588         count = min_t(unsigned int, (conn->mtu - hlen), len);
2589
2590         skb = chan->ops->alloc_skb(chan, count + hlen,
2591                                    msg->msg_flags & MSG_DONTWAIT);
2592         if (IS_ERR(skb))
2593                 return skb;
2594
2595         /* Create L2CAP header */
2596         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
2597         lh->cid = cpu_to_le16(chan->dcid);
2598         lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
2599
2600         if (sdulen)
2601                 put_unaligned_le16(sdulen, skb_put(skb, L2CAP_SDULEN_SIZE));
2602
2603         err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb);
2604         if (unlikely(err < 0)) {
2605                 kfree_skb(skb);
2606                 return ERR_PTR(err);
2607         }
2608
2609         return skb;
2610 }
2611
2612 static int l2cap_segment_le_sdu(struct l2cap_chan *chan,
2613                                 struct sk_buff_head *seg_queue,
2614                                 struct msghdr *msg, size_t len)
2615 {
2616         struct sk_buff *skb;
2617         size_t pdu_len;
2618         u16 sdu_len;
2619
2620         BT_DBG("chan %p, msg %p, len %zu", chan, msg, len);
2621
2622         pdu_len = chan->conn->mtu - L2CAP_HDR_SIZE;
2623
2624         pdu_len = min_t(size_t, pdu_len, chan->remote_mps);
2625
2626         sdu_len = len;
2627         pdu_len -= L2CAP_SDULEN_SIZE;
2628
2629         while (len > 0) {
2630                 if (len <= pdu_len)
2631                         pdu_len = len;
2632
2633                 skb = l2cap_create_le_flowctl_pdu(chan, msg, pdu_len, sdu_len);
2634                 if (IS_ERR(skb)) {
2635                         __skb_queue_purge(seg_queue);
2636                         return PTR_ERR(skb);
2637                 }
2638
2639                 __skb_queue_tail(seg_queue, skb);
2640
2641                 len -= pdu_len;
2642
2643                 if (sdu_len) {
2644                         sdu_len = 0;
2645                         pdu_len += L2CAP_SDULEN_SIZE;
2646                 }
2647         }
2648
2649         return 0;
2650 }
2651
2652 int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
2653                     u32 priority)
2654 {
2655         struct sk_buff *skb;
2656         int err;
2657         struct sk_buff_head seg_queue;
2658
2659         if (!chan->conn)
2660                 return -ENOTCONN;
2661
2662         /* Connectionless channel */
2663         if (chan->chan_type == L2CAP_CHAN_CONN_LESS) {
2664                 skb = l2cap_create_connless_pdu(chan, msg, len, priority);
2665                 if (IS_ERR(skb))
2666                         return PTR_ERR(skb);
2667
2668                 l2cap_do_send(chan, skb);
2669                 return len;
2670         }
2671
2672         switch (chan->mode) {
2673         case L2CAP_MODE_LE_FLOWCTL:
2674                 /* Check outgoing MTU */
2675                 if (len > chan->omtu)
2676                         return -EMSGSIZE;
2677
2678                 if (!chan->tx_credits)
2679                         return -EAGAIN;
2680
2681                 __skb_queue_head_init(&seg_queue);
2682
2683                 err = l2cap_segment_le_sdu(chan, &seg_queue, msg, len);
2684
2685                 if (chan->state != BT_CONNECTED) {
2686                         __skb_queue_purge(&seg_queue);
2687                         err = -ENOTCONN;
2688                 }
2689
2690                 if (err)
2691                         return err;
2692
2693                 skb_queue_splice_tail_init(&seg_queue, &chan->tx_q);
2694
2695                 while (chan->tx_credits && !skb_queue_empty(&chan->tx_q)) {
2696                         l2cap_do_send(chan, skb_dequeue(&chan->tx_q));
2697                         chan->tx_credits--;
2698                 }
2699
2700                 if (!chan->tx_credits)
2701                         chan->ops->suspend(chan);
2702
2703                 err = len;
2704
2705                 break;
2706
2707         case L2CAP_MODE_BASIC:
2708                 /* Check outgoing MTU */
2709                 if (len > chan->omtu)
2710                         return -EMSGSIZE;
2711
2712                 /* Create a basic PDU */
2713                 skb = l2cap_create_basic_pdu(chan, msg, len, priority);
2714                 if (IS_ERR(skb))
2715                         return PTR_ERR(skb);
2716
2717                 l2cap_do_send(chan, skb);
2718                 err = len;
2719                 break;
2720
2721         case L2CAP_MODE_ERTM:
2722         case L2CAP_MODE_STREAMING:
2723                 /* Check outgoing MTU */
2724                 if (len > chan->omtu) {
2725                         err = -EMSGSIZE;
2726                         break;
2727                 }
2728
2729                 __skb_queue_head_init(&seg_queue);
2730
2731                 /* Do segmentation before calling in to the state machine,
2732                  * since it's possible to block while waiting for memory
2733                  * allocation.
2734                  */
2735                 err = l2cap_segment_sdu(chan, &seg_queue, msg, len);
2736
2737                 /* The channel could have been closed while segmenting,
2738                  * check that it is still connected.
2739                  */
2740                 if (chan->state != BT_CONNECTED) {
2741                         __skb_queue_purge(&seg_queue);
2742                         err = -ENOTCONN;
2743                 }
2744
2745                 if (err)
2746                         break;
2747
2748                 if (chan->mode == L2CAP_MODE_ERTM)
2749                         l2cap_tx(chan, NULL, &seg_queue, L2CAP_EV_DATA_REQUEST);
2750                 else
2751                         l2cap_streaming_send(chan, &seg_queue);
2752
2753                 err = len;
2754
2755                 /* If the skbs were not queued for sending, they'll still be in
2756                  * seg_queue and need to be purged.
2757                  */
2758                 __skb_queue_purge(&seg_queue);
2759                 break;
2760
2761         default:
2762                 BT_DBG("bad state %1.1x", chan->mode);
2763                 err = -EBADFD;
2764         }
2765
2766         return err;
2767 }
2768
2769 static void l2cap_send_srej(struct l2cap_chan *chan, u16 txseq)
2770 {
2771         struct l2cap_ctrl control;
2772         u16 seq;
2773
2774         BT_DBG("chan %p, txseq %u", chan, txseq);
2775
2776         memset(&control, 0, sizeof(control));
2777         control.sframe = 1;
2778         control.super = L2CAP_SUPER_SREJ;
2779
2780         for (seq = chan->expected_tx_seq; seq != txseq;
2781              seq = __next_seq(chan, seq)) {
2782                 if (!l2cap_ertm_seq_in_queue(&chan->srej_q, seq)) {
2783                         control.reqseq = seq;
2784                         l2cap_send_sframe(chan, &control);
2785                         l2cap_seq_list_append(&chan->srej_list, seq);
2786                 }
2787         }
2788
2789         chan->expected_tx_seq = __next_seq(chan, txseq);
2790 }
2791
2792 static void l2cap_send_srej_tail(struct l2cap_chan *chan)
2793 {
2794         struct l2cap_ctrl control;
2795
2796         BT_DBG("chan %p", chan);
2797
2798         if (chan->srej_list.tail == L2CAP_SEQ_LIST_CLEAR)
2799                 return;
2800
2801         memset(&control, 0, sizeof(control));
2802         control.sframe = 1;
2803         control.super = L2CAP_SUPER_SREJ;
2804         control.reqseq = chan->srej_list.tail;
2805         l2cap_send_sframe(chan, &control);
2806 }
2807
2808 static void l2cap_send_srej_list(struct l2cap_chan *chan, u16 txseq)
2809 {
2810         struct l2cap_ctrl control;
2811         u16 initial_head;
2812         u16 seq;
2813
2814         BT_DBG("chan %p, txseq %u", chan, txseq);
2815
2816         memset(&control, 0, sizeof(control));
2817         control.sframe = 1;
2818         control.super = L2CAP_SUPER_SREJ;
2819
2820         /* Capture initial list head to allow only one pass through the list. */
2821         initial_head = chan->srej_list.head;
2822
2823         do {
2824                 seq = l2cap_seq_list_pop(&chan->srej_list);
2825                 if (seq == txseq || seq == L2CAP_SEQ_LIST_CLEAR)
2826                         break;
2827
2828                 control.reqseq = seq;
2829                 l2cap_send_sframe(chan, &control);
2830                 l2cap_seq_list_append(&chan->srej_list, seq);
2831         } while (chan->srej_list.head != initial_head);
2832 }
2833
2834 static void l2cap_process_reqseq(struct l2cap_chan *chan, u16 reqseq)
2835 {
2836         struct sk_buff *acked_skb;
2837         u16 ackseq;
2838
2839         BT_DBG("chan %p, reqseq %u", chan, reqseq);
2840
2841         if (chan->unacked_frames == 0 || reqseq == chan->expected_ack_seq)
2842                 return;
2843
2844         BT_DBG("expected_ack_seq %u, unacked_frames %u",
2845                chan->expected_ack_seq, chan->unacked_frames);
2846
2847         for (ackseq = chan->expected_ack_seq; ackseq != reqseq;
2848              ackseq = __next_seq(chan, ackseq)) {
2849
2850                 acked_skb = l2cap_ertm_seq_in_queue(&chan->tx_q, ackseq);
2851                 if (acked_skb) {
2852                         skb_unlink(acked_skb, &chan->tx_q);
2853                         kfree_skb(acked_skb);
2854                         chan->unacked_frames--;
2855                 }
2856         }
2857
2858         chan->expected_ack_seq = reqseq;
2859
2860         if (chan->unacked_frames == 0)
2861                 __clear_retrans_timer(chan);
2862
2863         BT_DBG("unacked_frames %u", chan->unacked_frames);
2864 }
2865
2866 static void l2cap_abort_rx_srej_sent(struct l2cap_chan *chan)
2867 {
2868         BT_DBG("chan %p", chan);
2869
2870         chan->expected_tx_seq = chan->buffer_seq;
2871         l2cap_seq_list_clear(&chan->srej_list);
2872         skb_queue_purge(&chan->srej_q);
2873         chan->rx_state = L2CAP_RX_STATE_RECV;
2874 }
2875
2876 static void l2cap_tx_state_xmit(struct l2cap_chan *chan,
2877                                 struct l2cap_ctrl *control,
2878                                 struct sk_buff_head *skbs, u8 event)
2879 {
2880         BT_DBG("chan %p, control %p, skbs %p, event %d", chan, control, skbs,
2881                event);
2882
2883         switch (event) {
2884         case L2CAP_EV_DATA_REQUEST:
2885                 if (chan->tx_send_head == NULL)
2886                         chan->tx_send_head = skb_peek(skbs);
2887
2888                 skb_queue_splice_tail_init(skbs, &chan->tx_q);
2889                 l2cap_ertm_send(chan);
2890                 break;
2891         case L2CAP_EV_LOCAL_BUSY_DETECTED:
2892                 BT_DBG("Enter LOCAL_BUSY");
2893                 set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2894
2895                 if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
2896                         /* The SREJ_SENT state must be aborted if we are to
2897                          * enter the LOCAL_BUSY state.
2898                          */
2899                         l2cap_abort_rx_srej_sent(chan);
2900                 }
2901
2902                 l2cap_send_ack(chan);
2903
2904                 break;
2905         case L2CAP_EV_LOCAL_BUSY_CLEAR:
2906                 BT_DBG("Exit LOCAL_BUSY");
2907                 clear_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2908
2909                 if (test_bit(CONN_RNR_SENT, &chan->conn_state)) {
2910                         struct l2cap_ctrl local_control;
2911
2912                         memset(&local_control, 0, sizeof(local_control));
2913                         local_control.sframe = 1;
2914                         local_control.super = L2CAP_SUPER_RR;
2915                         local_control.poll = 1;
2916                         local_control.reqseq = chan->buffer_seq;
2917                         l2cap_send_sframe(chan, &local_control);
2918
2919                         chan->retry_count = 1;
2920                         __set_monitor_timer(chan);
2921                         chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2922                 }
2923                 break;
2924         case L2CAP_EV_RECV_REQSEQ_AND_FBIT:
2925                 l2cap_process_reqseq(chan, control->reqseq);
2926                 break;
2927         case L2CAP_EV_EXPLICIT_POLL:
2928                 l2cap_send_rr_or_rnr(chan, 1);
2929                 chan->retry_count = 1;
2930                 __set_monitor_timer(chan);
2931                 __clear_ack_timer(chan);
2932                 chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2933                 break;
2934         case L2CAP_EV_RETRANS_TO:
2935                 l2cap_send_rr_or_rnr(chan, 1);
2936                 chan->retry_count = 1;
2937                 __set_monitor_timer(chan);
2938                 chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2939                 break;
2940         case L2CAP_EV_RECV_FBIT:
2941                 /* Nothing to process */
2942                 break;
2943         default:
2944                 break;
2945         }
2946 }
2947
2948 static void l2cap_tx_state_wait_f(struct l2cap_chan *chan,
2949                                   struct l2cap_ctrl *control,
2950                                   struct sk_buff_head *skbs, u8 event)
2951 {
2952         BT_DBG("chan %p, control %p, skbs %p, event %d", chan, control, skbs,
2953                event);
2954
2955         switch (event) {
2956         case L2CAP_EV_DATA_REQUEST:
2957                 if (chan->tx_send_head == NULL)
2958                         chan->tx_send_head = skb_peek(skbs);
2959                 /* Queue data, but don't send. */
2960                 skb_queue_splice_tail_init(skbs, &chan->tx_q);
2961                 break;
2962         case L2CAP_EV_LOCAL_BUSY_DETECTED:
2963                 BT_DBG("Enter LOCAL_BUSY");
2964                 set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2965
2966                 if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
2967                         /* The SREJ_SENT state must be aborted if we are to
2968                          * enter the LOCAL_BUSY state.
2969                          */
2970                         l2cap_abort_rx_srej_sent(chan);
2971                 }
2972
2973                 l2cap_send_ack(chan);
2974
2975                 break;
2976         case L2CAP_EV_LOCAL_BUSY_CLEAR:
2977                 BT_DBG("Exit LOCAL_BUSY");
2978                 clear_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2979
2980                 if (test_bit(CONN_RNR_SENT, &chan->conn_state)) {
2981                         struct l2cap_ctrl local_control;
2982                         memset(&local_control, 0, sizeof(local_control));
2983                         local_control.sframe = 1;
2984                         local_control.super = L2CAP_SUPER_RR;
2985                         local_control.poll = 1;
2986                         local_control.reqseq = chan->buffer_seq;
2987                         l2cap_send_sframe(chan, &local_control);
2988
2989                         chan->retry_count = 1;
2990                         __set_monitor_timer(chan);
2991                         chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2992                 }
2993                 break;
2994         case L2CAP_EV_RECV_REQSEQ_AND_FBIT:
2995                 l2cap_process_reqseq(chan, control->reqseq);
2996
2997                 /* Fall through */
2998
2999         case L2CAP_EV_RECV_FBIT:
3000                 if (control && control->final) {
3001                         __clear_monitor_timer(chan);
3002                         if (chan->unacked_frames > 0)
3003                                 __set_retrans_timer(chan);
3004                         chan->retry_count = 0;
3005                         chan->tx_state = L2CAP_TX_STATE_XMIT;
3006                         BT_DBG("recv fbit tx_state 0x2.2%x", chan->tx_state);
3007                 }
3008                 break;
3009         case L2CAP_EV_EXPLICIT_POLL:
3010                 /* Ignore */
3011                 break;
3012         case L2CAP_EV_MONITOR_TO:
3013                 if (chan->max_tx == 0 || chan->retry_count < chan->max_tx) {
3014                         l2cap_send_rr_or_rnr(chan, 1);
3015                         __set_monitor_timer(chan);
3016                         chan->retry_count++;
3017                 } else {
3018                         l2cap_send_disconn_req(chan, ECONNABORTED);
3019                 }
3020                 break;
3021         default:
3022                 break;
3023         }
3024 }
3025
3026 static void l2cap_tx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
3027                      struct sk_buff_head *skbs, u8 event)
3028 {
3029         BT_DBG("chan %p, control %p, skbs %p, event %d, state %d",
3030                chan, control, skbs, event, chan->tx_state);
3031
3032         switch (chan->tx_state) {
3033         case L2CAP_TX_STATE_XMIT:
3034                 l2cap_tx_state_xmit(chan, control, skbs, event);
3035                 break;
3036         case L2CAP_TX_STATE_WAIT_F:
3037                 l2cap_tx_state_wait_f(chan, control, skbs, event);
3038                 break;
3039         default:
3040                 /* Ignore event */
3041                 break;
3042         }
3043 }
3044
3045 static void l2cap_pass_to_tx(struct l2cap_chan *chan,
3046                              struct l2cap_ctrl *control)
3047 {
3048         BT_DBG("chan %p, control %p", chan, control);
3049         l2cap_tx(chan, control, NULL, L2CAP_EV_RECV_REQSEQ_AND_FBIT);
3050 }
3051
3052 static void l2cap_pass_to_tx_fbit(struct l2cap_chan *chan,
3053                                   struct l2cap_ctrl *control)
3054 {
3055         BT_DBG("chan %p, control %p", chan, control);
3056         l2cap_tx(chan, control, NULL, L2CAP_EV_RECV_FBIT);
3057 }
3058
3059 /* Copy frame to all raw sockets on that connection */
3060 static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
3061 {
3062         struct sk_buff *nskb;
3063         struct l2cap_chan *chan;
3064
3065         BT_DBG("conn %p", conn);
3066
3067         mutex_lock(&conn->chan_lock);
3068
3069         list_for_each_entry(chan, &conn->chan_l, list) {
3070                 if (chan->chan_type != L2CAP_CHAN_RAW)
3071                         continue;
3072
3073                 /* Don't send frame to the channel it came from */
3074                 if (bt_cb(skb)->chan == chan)
3075                         continue;
3076
3077                 nskb = skb_clone(skb, GFP_KERNEL);
3078                 if (!nskb)
3079                         continue;
3080                 if (chan->ops->recv(chan, nskb))
3081                         kfree_skb(nskb);
3082         }
3083
3084         mutex_unlock(&conn->chan_lock);
3085 }
3086
3087 /* ---- L2CAP signalling commands ---- */
3088 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn, u8 code,
3089                                        u8 ident, u16 dlen, void *data)
3090 {
3091         struct sk_buff *skb, **frag;
3092         struct l2cap_cmd_hdr *cmd;
3093         struct l2cap_hdr *lh;
3094         int len, count;
3095
3096         BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %u",
3097                conn, code, ident, dlen);
3098
3099         if (conn->mtu < L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE)
3100                 return NULL;
3101
3102         len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen;
3103         count = min_t(unsigned int, conn->mtu, len);
3104
3105         skb = bt_skb_alloc(count, GFP_KERNEL);
3106         if (!skb)
3107                 return NULL;
3108
3109         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
3110         lh->len = cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen);
3111
3112         if (conn->hcon->type == LE_LINK)
3113                 lh->cid = __constant_cpu_to_le16(L2CAP_CID_LE_SIGNALING);
3114         else
3115                 lh->cid = __constant_cpu_to_le16(L2CAP_CID_SIGNALING);
3116
3117         cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE);
3118         cmd->code  = code;
3119         cmd->ident = ident;
3120         cmd->len   = cpu_to_le16(dlen);
3121
3122         if (dlen) {
3123                 count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE;
3124                 memcpy(skb_put(skb, count), data, count);
3125                 data += count;
3126         }
3127
3128         len -= skb->len;
3129
3130         /* Continuation fragments (no L2CAP header) */
3131         frag = &skb_shinfo(skb)->frag_list;
3132         while (len) {
3133                 count = min_t(unsigned int, conn->mtu, len);
3134
3135                 *frag = bt_skb_alloc(count, GFP_KERNEL);
3136                 if (!*frag)
3137                         goto fail;
3138
3139                 memcpy(skb_put(*frag, count), data, count);
3140
3141                 len  -= count;
3142                 data += count;
3143
3144                 frag = &(*frag)->next;
3145         }
3146
3147         return skb;
3148
3149 fail:
3150         kfree_skb(skb);
3151         return NULL;
3152 }
3153
3154 static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen,
3155                                      unsigned long *val)
3156 {
3157         struct l2cap_conf_opt *opt = *ptr;
3158         int len;
3159
3160         len = L2CAP_CONF_OPT_SIZE + opt->len;
3161         *ptr += len;
3162
3163         *type = opt->type;
3164         *olen = opt->len;
3165
3166         switch (opt->len) {
3167         case 1:
3168                 *val = *((u8 *) opt->val);
3169                 break;
3170
3171         case 2:
3172                 *val = get_unaligned_le16(opt->val);
3173                 break;
3174
3175         case 4:
3176                 *val = get_unaligned_le32(opt->val);
3177                 break;
3178
3179         default:
3180                 *val = (unsigned long) opt->val;
3181                 break;
3182         }
3183
3184         BT_DBG("type 0x%2.2x len %u val 0x%lx", *type, opt->len, *val);
3185         return len;
3186 }
3187
3188 static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val)
3189 {
3190         struct l2cap_conf_opt *opt = *ptr;
3191
3192         BT_DBG("type 0x%2.2x len %u val 0x%lx", type, len, val);
3193
3194         opt->type = type;
3195         opt->len  = len;
3196
3197         switch (len) {
3198         case 1:
3199                 *((u8 *) opt->val)  = val;
3200                 break;
3201
3202         case 2:
3203                 put_unaligned_le16(val, opt->val);
3204                 break;
3205
3206         case 4:
3207                 put_unaligned_le32(val, opt->val);
3208                 break;
3209
3210         default:
3211                 memcpy(opt->val, (void *) val, len);
3212                 break;
3213         }
3214
3215         *ptr += L2CAP_CONF_OPT_SIZE + len;
3216 }
3217
3218 static void l2cap_add_opt_efs(void **ptr, struct l2cap_chan *chan)
3219 {
3220         struct l2cap_conf_efs efs;
3221
3222         switch (chan->mode) {
3223         case L2CAP_MODE_ERTM:
3224                 efs.id          = chan->local_id;
3225                 efs.stype       = chan->local_stype;
3226                 efs.msdu        = cpu_to_le16(chan->local_msdu);
3227                 efs.sdu_itime   = cpu_to_le32(chan->local_sdu_itime);
3228                 efs.acc_lat     = __constant_cpu_to_le32(L2CAP_DEFAULT_ACC_LAT);
3229                 efs.flush_to    = __constant_cpu_to_le32(L2CAP_EFS_DEFAULT_FLUSH_TO);
3230                 break;
3231
3232         case L2CAP_MODE_STREAMING:
3233                 efs.id          = 1;
3234                 efs.stype       = L2CAP_SERV_BESTEFFORT;
3235                 efs.msdu        = cpu_to_le16(chan->local_msdu);
3236                 efs.sdu_itime   = cpu_to_le32(chan->local_sdu_itime);
3237                 efs.acc_lat     = 0;
3238                 efs.flush_to    = 0;
3239                 break;
3240
3241         default:
3242                 return;
3243         }
3244
3245         l2cap_add_conf_opt(ptr, L2CAP_CONF_EFS, sizeof(efs),
3246                            (unsigned long) &efs);
3247 }
3248
3249 static void l2cap_ack_timeout(struct work_struct *work)
3250 {
3251         struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
3252                                                ack_timer.work);
3253         u16 frames_to_ack;
3254
3255         BT_DBG("chan %p", chan);
3256
3257         l2cap_chan_lock(chan);
3258
3259         frames_to_ack = __seq_offset(chan, chan->buffer_seq,
3260                                      chan->last_acked_seq);
3261
3262         if (frames_to_ack)
3263                 l2cap_send_rr_or_rnr(chan, 0);
3264
3265         l2cap_chan_unlock(chan);
3266         l2cap_chan_put(chan);
3267 }
3268
3269 int l2cap_ertm_init(struct l2cap_chan *chan)
3270 {
3271         int err;
3272
3273         chan->next_tx_seq = 0;
3274         chan->expected_tx_seq = 0;
3275         chan->expected_ack_seq = 0;
3276         chan->unacked_frames = 0;
3277         chan->buffer_seq = 0;
3278         chan->frames_sent = 0;
3279         chan->last_acked_seq = 0;
3280         chan->sdu = NULL;
3281         chan->sdu_last_frag = NULL;
3282         chan->sdu_len = 0;
3283
3284         skb_queue_head_init(&chan->tx_q);
3285
3286         chan->local_amp_id = AMP_ID_BREDR;
3287         chan->move_id = AMP_ID_BREDR;
3288         chan->move_state = L2CAP_MOVE_STABLE;
3289         chan->move_role = L2CAP_MOVE_ROLE_NONE;
3290
3291         if (chan->mode != L2CAP_MODE_ERTM)
3292                 return 0;
3293
3294         chan->rx_state = L2CAP_RX_STATE_RECV;
3295         chan->tx_state = L2CAP_TX_STATE_XMIT;
3296
3297         INIT_DELAYED_WORK(&chan->retrans_timer, l2cap_retrans_timeout);
3298         INIT_DELAYED_WORK(&chan->monitor_timer, l2cap_monitor_timeout);
3299         INIT_DELAYED_WORK(&chan->ack_timer, l2cap_ack_timeout);
3300
3301         skb_queue_head_init(&chan->srej_q);
3302
3303         err = l2cap_seq_list_init(&chan->srej_list, chan->tx_win);
3304         if (err < 0)
3305                 return err;
3306
3307         err = l2cap_seq_list_init(&chan->retrans_list, chan->remote_tx_win);
3308         if (err < 0)
3309                 l2cap_seq_list_free(&chan->srej_list);
3310
3311         return err;
3312 }
3313
3314 static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
3315 {
3316         switch (mode) {
3317         case L2CAP_MODE_STREAMING:
3318         case L2CAP_MODE_ERTM:
3319                 if (l2cap_mode_supported(mode, remote_feat_mask))
3320                         return mode;
3321                 /* fall through */
3322         default:
3323                 return L2CAP_MODE_BASIC;
3324         }
3325 }
3326
3327 static inline bool __l2cap_ews_supported(struct l2cap_conn *conn)
3328 {
3329         return conn->hs_enabled && conn->feat_mask & L2CAP_FEAT_EXT_WINDOW;
3330 }
3331
3332 static inline bool __l2cap_efs_supported(struct l2cap_conn *conn)
3333 {
3334         return conn->hs_enabled && conn->feat_mask & L2CAP_FEAT_EXT_FLOW;
3335 }
3336
3337 static void __l2cap_set_ertm_timeouts(struct l2cap_chan *chan,
3338                                       struct l2cap_conf_rfc *rfc)
3339 {
3340         if (chan->local_amp_id != AMP_ID_BREDR && chan->hs_hcon) {
3341                 u64 ertm_to = chan->hs_hcon->hdev->amp_be_flush_to;
3342
3343                 /* Class 1 devices have must have ERTM timeouts
3344                  * exceeding the Link Supervision Timeout.  The
3345                  * default Link Supervision Timeout for AMP
3346                  * controllers is 10 seconds.
3347                  *
3348                  * Class 1 devices use 0xffffffff for their
3349                  * best-effort flush timeout, so the clamping logic
3350                  * will result in a timeout that meets the above
3351                  * requirement.  ERTM timeouts are 16-bit values, so
3352                  * the maximum timeout is 65.535 seconds.
3353                  */
3354
3355                 /* Convert timeout to milliseconds and round */
3356                 ertm_to = DIV_ROUND_UP_ULL(ertm_to, 1000);
3357
3358                 /* This is the recommended formula for class 2 devices
3359                  * that start ERTM timers when packets are sent to the
3360                  * controller.
3361                  */
3362                 ertm_to = 3 * ertm_to + 500;
3363
3364                 if (ertm_to > 0xffff)
3365                         ertm_to = 0xffff;
3366
3367                 rfc->retrans_timeout = cpu_to_le16((u16) ertm_to);
3368                 rfc->monitor_timeout = rfc->retrans_timeout;
3369         } else {
3370                 rfc->retrans_timeout = __constant_cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO);
3371                 rfc->monitor_timeout = __constant_cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO);
3372         }
3373 }
3374
3375 static inline void l2cap_txwin_setup(struct l2cap_chan *chan)
3376 {
3377         if (chan->tx_win > L2CAP_DEFAULT_TX_WINDOW &&
3378             __l2cap_ews_supported(chan->conn)) {
3379                 /* use extended control field */
3380                 set_bit(FLAG_EXT_CTRL, &chan->flags);
3381                 chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
3382         } else {
3383                 chan->tx_win = min_t(u16, chan->tx_win,
3384                                      L2CAP_DEFAULT_TX_WINDOW);
3385                 chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
3386         }
3387         chan->ack_win = chan->tx_win;
3388 }
3389
3390 static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data)
3391 {
3392         struct l2cap_conf_req *req = data;
3393         struct l2cap_conf_rfc rfc = { .mode = chan->mode };
3394         void *ptr = req->data;
3395         u16 size;
3396
3397         BT_DBG("chan %p", chan);
3398
3399         if (chan->num_conf_req || chan->num_conf_rsp)
3400                 goto done;
3401
3402         switch (chan->mode) {
3403         case L2CAP_MODE_STREAMING:
3404         case L2CAP_MODE_ERTM:
3405                 if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state))
3406                         break;
3407
3408                 if (__l2cap_efs_supported(chan->conn))
3409                         set_bit(FLAG_EFS_ENABLE, &chan->flags);
3410
3411                 /* fall through */
3412         default:
3413                 chan->mode = l2cap_select_mode(rfc.mode, chan->conn->feat_mask);
3414                 break;
3415         }
3416
3417 done:
3418         if (chan->imtu != L2CAP_DEFAULT_MTU)
3419                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu);
3420
3421         switch (chan->mode) {
3422         case L2CAP_MODE_BASIC:
3423                 if (!(chan->conn->feat_mask & L2CAP_FEAT_ERTM) &&
3424                     !(chan->conn->feat_mask & L2CAP_FEAT_STREAMING))
3425                         break;
3426
3427                 rfc.mode            = L2CAP_MODE_BASIC;
3428                 rfc.txwin_size      = 0;
3429                 rfc.max_transmit    = 0;
3430                 rfc.retrans_timeout = 0;
3431                 rfc.monitor_timeout = 0;
3432                 rfc.max_pdu_size    = 0;
3433
3434                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3435                                    (unsigned long) &rfc);
3436                 break;
3437
3438         case L2CAP_MODE_ERTM:
3439                 rfc.mode            = L2CAP_MODE_ERTM;
3440                 rfc.max_transmit    = chan->max_tx;
3441
3442                 __l2cap_set_ertm_timeouts(chan, &rfc);
3443
3444                 size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
3445                              L2CAP_EXT_HDR_SIZE - L2CAP_SDULEN_SIZE -
3446                              L2CAP_FCS_SIZE);
3447                 rfc.max_pdu_size = cpu_to_le16(size);
3448
3449                 l2cap_txwin_setup(chan);
3450
3451                 rfc.txwin_size = min_t(u16, chan->tx_win,
3452                                        L2CAP_DEFAULT_TX_WINDOW);
3453
3454                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3455                                    (unsigned long) &rfc);
3456
3457                 if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
3458                         l2cap_add_opt_efs(&ptr, chan);
3459
3460                 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
3461                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
3462                                            chan->tx_win);
3463
3464                 if (chan->conn->feat_mask & L2CAP_FEAT_FCS)
3465                         if (chan->fcs == L2CAP_FCS_NONE ||
3466                             test_bit(CONF_RECV_NO_FCS, &chan->conf_state)) {
3467                                 chan->fcs = L2CAP_FCS_NONE;
3468                                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1,
3469                                                    chan->fcs);
3470                         }
3471                 break;
3472
3473         case L2CAP_MODE_STREAMING:
3474                 l2cap_txwin_setup(chan);
3475                 rfc.mode            = L2CAP_MODE_STREAMING;
3476                 rfc.txwin_size      = 0;
3477                 rfc.max_transmit    = 0;
3478                 rfc.retrans_timeout = 0;
3479                 rfc.monitor_timeout = 0;
3480
3481                 size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
3482                              L2CAP_EXT_HDR_SIZE - L2CAP_SDULEN_SIZE -
3483                              L2CAP_FCS_SIZE);
3484                 rfc.max_pdu_size = cpu_to_le16(size);
3485
3486                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3487                                    (unsigned long) &rfc);
3488
3489                 if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
3490                         l2cap_add_opt_efs(&ptr, chan);
3491
3492                 if (chan->conn->feat_mask & L2CAP_FEAT_FCS)
3493                         if (chan->fcs == L2CAP_FCS_NONE ||
3494                             test_bit(CONF_RECV_NO_FCS, &chan->conf_state)) {
3495                                 chan->fcs = L2CAP_FCS_NONE;
3496                                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1,
3497                                                    chan->fcs);
3498                         }
3499                 break;
3500         }
3501
3502         req->dcid  = cpu_to_le16(chan->dcid);
3503         req->flags = __constant_cpu_to_le16(0);
3504
3505         return ptr - data;
3506 }
3507
3508 static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data)
3509 {
3510         struct l2cap_conf_rsp *rsp = data;
3511         void *ptr = rsp->data;
3512         void *req = chan->conf_req;
3513         int len = chan->conf_len;
3514         int type, hint, olen;
3515         unsigned long val;
3516         struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
3517         struct l2cap_conf_efs efs;
3518         u8 remote_efs = 0;
3519         u16 mtu = L2CAP_DEFAULT_MTU;
3520         u16 result = L2CAP_CONF_SUCCESS;
3521         u16 size;
3522
3523         BT_DBG("chan %p", chan);
3524
3525         while (len >= L2CAP_CONF_OPT_SIZE) {
3526                 len -= l2cap_get_conf_opt(&req, &type, &olen, &val);
3527
3528                 hint  = type & L2CAP_CONF_HINT;
3529                 type &= L2CAP_CONF_MASK;
3530
3531                 switch (type) {
3532                 case L2CAP_CONF_MTU:
3533                         mtu = val;
3534                         break;
3535
3536                 case L2CAP_CONF_FLUSH_TO:
3537                         chan->flush_to = val;
3538                         break;
3539
3540                 case L2CAP_CONF_QOS:
3541                         break;
3542
3543                 case L2CAP_CONF_RFC:
3544                         if (olen == sizeof(rfc))
3545                                 memcpy(&rfc, (void *) val, olen);
3546                         break;
3547
3548                 case L2CAP_CONF_FCS:
3549                         if (val == L2CAP_FCS_NONE)
3550                                 set_bit(CONF_RECV_NO_FCS, &chan->conf_state);
3551                         break;
3552
3553                 case L2CAP_CONF_EFS:
3554                         remote_efs = 1;
3555                         if (olen == sizeof(efs))
3556                                 memcpy(&efs, (void *) val, olen);
3557                         break;
3558
3559                 case L2CAP_CONF_EWS:
3560                         if (!chan->conn->hs_enabled)
3561                                 return -ECONNREFUSED;
3562
3563                         set_bit(FLAG_EXT_CTRL, &chan->flags);
3564                         set_bit(CONF_EWS_RECV, &chan->conf_state);
3565                         chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
3566                         chan->remote_tx_win = val;
3567                         break;
3568
3569                 default:
3570                         if (hint)
3571                                 break;
3572
3573                         result = L2CAP_CONF_UNKNOWN;
3574                         *((u8 *) ptr++) = type;
3575                         break;
3576                 }
3577         }
3578
3579         if (chan->num_conf_rsp || chan->num_conf_req > 1)
3580                 goto done;
3581
3582         switch (chan->mode) {
3583         case L2CAP_MODE_STREAMING:
3584         case L2CAP_MODE_ERTM:
3585                 if (!test_bit(CONF_STATE2_DEVICE, &chan->conf_state)) {
3586                         chan->mode = l2cap_select_mode(rfc.mode,
3587                                                        chan->conn->feat_mask);
3588                         break;
3589                 }
3590
3591                 if (remote_efs) {
3592                         if (__l2cap_efs_supported(chan->conn))
3593                                 set_bit(FLAG_EFS_ENABLE, &chan->flags);
3594                         else
3595                                 return -ECONNREFUSED;
3596                 }
3597
3598                 if (chan->mode != rfc.mode)
3599                         return -ECONNREFUSED;
3600
3601                 break;
3602         }
3603
3604 done:
3605         if (chan->mode != rfc.mode) {
3606                 result = L2CAP_CONF_UNACCEPT;
3607                 rfc.mode = chan->mode;
3608
3609                 if (chan->num_conf_rsp == 1)
3610                         return -ECONNREFUSED;
3611
3612                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3613                                    (unsigned long) &rfc);
3614         }
3615
3616         if (result == L2CAP_CONF_SUCCESS) {
3617                 /* Configure output options and let the other side know
3618                  * which ones we don't like. */
3619
3620                 if (mtu < L2CAP_DEFAULT_MIN_MTU)
3621                         result = L2CAP_CONF_UNACCEPT;
3622                 else {
3623                         chan->omtu = mtu;
3624                         set_bit(CONF_MTU_DONE, &chan->conf_state);
3625                 }
3626                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->omtu);
3627
3628                 if (remote_efs) {
3629                         if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
3630                             efs.stype != L2CAP_SERV_NOTRAFIC &&
3631                             efs.stype != chan->local_stype) {
3632
3633                                 result = L2CAP_CONF_UNACCEPT;
3634
3635                                 if (chan->num_conf_req >= 1)
3636                                         return -ECONNREFUSED;
3637
3638                                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
3639                                                    sizeof(efs),
3640                                                    (unsigned long) &efs);
3641                         } else {
3642                                 /* Send PENDING Conf Rsp */
3643                                 result = L2CAP_CONF_PENDING;
3644                                 set_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
3645                         }
3646                 }
3647
3648                 switch (rfc.mode) {
3649                 case L2CAP_MODE_BASIC:
3650                         chan->fcs = L2CAP_FCS_NONE;
3651                         set_bit(CONF_MODE_DONE, &chan->conf_state);
3652                         break;
3653
3654                 case L2CAP_MODE_ERTM:
3655                         if (!test_bit(CONF_EWS_RECV, &chan->conf_state))
3656                                 chan->remote_tx_win = rfc.txwin_size;
3657                         else
3658                                 rfc.txwin_size = L2CAP_DEFAULT_TX_WINDOW;
3659
3660                         chan->remote_max_tx = rfc.max_transmit;
3661
3662                         size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
3663                                      chan->conn->mtu - L2CAP_EXT_HDR_SIZE -
3664                                      L2CAP_SDULEN_SIZE - L2CAP_FCS_SIZE);
3665                         rfc.max_pdu_size = cpu_to_le16(size);
3666                         chan->remote_mps = size;
3667
3668                         __l2cap_set_ertm_timeouts(chan, &rfc);
3669
3670                         set_bit(CONF_MODE_DONE, &chan->conf_state);
3671
3672                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
3673                                            sizeof(rfc), (unsigned long) &rfc);
3674
3675                         if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
3676                                 chan->remote_id = efs.id;
3677                                 chan->remote_stype = efs.stype;
3678                                 chan->remote_msdu = le16_to_cpu(efs.msdu);
3679                                 chan->remote_flush_to =
3680                                         le32_to_cpu(efs.flush_to);
3681                                 chan->remote_acc_lat =
3682                                         le32_to_cpu(efs.acc_lat);
3683                                 chan->remote_sdu_itime =
3684                                         le32_to_cpu(efs.sdu_itime);
3685                                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
3686                                                    sizeof(efs),
3687                                                    (unsigned long) &efs);
3688                         }
3689                         break;
3690
3691                 case L2CAP_MODE_STREAMING:
3692                         size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
3693                                      chan->conn->mtu - L2CAP_EXT_HDR_SIZE -
3694                                      L2CAP_SDULEN_SIZE - L2CAP_FCS_SIZE);
3695                         rfc.max_pdu_size = cpu_to_le16(size);
3696                         chan->remote_mps = size;
3697
3698                         set_bit(CONF_MODE_DONE, &chan->conf_state);
3699
3700                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3701                                            (unsigned long) &rfc);
3702
3703                         break;
3704
3705                 default:
3706                         result = L2CAP_CONF_UNACCEPT;
3707
3708                         memset(&rfc, 0, sizeof(rfc));
3709                         rfc.mode = chan->mode;
3710                 }
3711
3712                 if (result == L2CAP_CONF_SUCCESS)
3713                         set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
3714         }
3715         rsp->scid   = cpu_to_le16(chan->dcid);
3716         rsp->result = cpu_to_le16(result);
3717         rsp->flags  = __constant_cpu_to_le16(0);
3718
3719         return ptr - data;
3720 }
3721
3722 static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len,
3723                                 void *data, u16 *result)
3724 {
3725         struct l2cap_conf_req *req = data;
3726         void *ptr = req->data;
3727         int type, olen;
3728         unsigned long val;
3729         struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
3730         struct l2cap_conf_efs efs;
3731
3732         BT_DBG("chan %p, rsp %p, len %d, req %p", chan, rsp, len, data);
3733
3734         while (len >= L2CAP_CONF_OPT_SIZE) {
3735                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
3736
3737                 switch (type) {
3738                 case L2CAP_CONF_MTU:
3739                         if (val < L2CAP_DEFAULT_MIN_MTU) {
3740                                 *result = L2CAP_CONF_UNACCEPT;
3741                                 chan->imtu = L2CAP_DEFAULT_MIN_MTU;
3742                         } else
3743                                 chan->imtu = val;
3744                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu);
3745                         break;
3746
3747                 case L2CAP_CONF_FLUSH_TO:
3748                         chan->flush_to = val;
3749                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
3750                                            2, chan->flush_to);
3751                         break;
3752
3753                 case L2CAP_CONF_RFC:
3754                         if (olen == sizeof(rfc))
3755                                 memcpy(&rfc, (void *)val, olen);
3756
3757                         if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state) &&
3758                             rfc.mode != chan->mode)
3759                                 return -ECONNREFUSED;
3760
3761                         chan->fcs = 0;
3762
3763                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
3764                                            sizeof(rfc), (unsigned long) &rfc);
3765                         break;
3766
3767                 case L2CAP_CONF_EWS:
3768                         chan->ack_win = min_t(u16, val, chan->ack_win);
3769                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
3770                                            chan->tx_win);
3771                         break;
3772
3773                 case L2CAP_CONF_EFS:
3774                         if (olen == sizeof(efs))
3775                                 memcpy(&efs, (void *)val, olen);
3776
3777                         if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
3778                             efs.stype != L2CAP_SERV_NOTRAFIC &&
3779                             efs.stype != chan->local_stype)
3780                                 return -ECONNREFUSED;
3781
3782                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS, sizeof(efs),
3783                                            (unsigned long) &efs);
3784                         break;
3785
3786                 case L2CAP_CONF_FCS:
3787                         if (*result == L2CAP_CONF_PENDING)
3788                                 if (val == L2CAP_FCS_NONE)
3789                                         set_bit(CONF_RECV_NO_FCS,
3790                                                 &chan->conf_state);
3791                         break;
3792                 }
3793         }
3794
3795         if (chan->mode == L2CAP_MODE_BASIC && chan->mode != rfc.mode)
3796                 return -ECONNREFUSED;
3797
3798         chan->mode = rfc.mode;
3799
3800         if (*result == L2CAP_CONF_SUCCESS || *result == L2CAP_CONF_PENDING) {
3801                 switch (rfc.mode) {
3802                 case L2CAP_MODE_ERTM:
3803                         chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
3804                         chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
3805                         chan->mps    = le16_to_cpu(rfc.max_pdu_size);
3806                         if (!test_bit(FLAG_EXT_CTRL, &chan->flags))
3807                                 chan->ack_win = min_t(u16, chan->ack_win,
3808                                                       rfc.txwin_size);
3809
3810                         if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
3811                                 chan->local_msdu = le16_to_cpu(efs.msdu);
3812                                 chan->local_sdu_itime =
3813                                         le32_to_cpu(efs.sdu_itime);
3814                                 chan->local_acc_lat = le32_to_cpu(efs.acc_lat);
3815                                 chan->local_flush_to =
3816                                         le32_to_cpu(efs.flush_to);
3817                         }
3818                         break;
3819
3820                 case L2CAP_MODE_STREAMING:
3821                         chan->mps    = le16_to_cpu(rfc.max_pdu_size);
3822                 }
3823         }
3824
3825         req->dcid   = cpu_to_le16(chan->dcid);
3826         req->flags  = __constant_cpu_to_le16(0);
3827
3828         return ptr - data;
3829 }
3830
3831 static int l2cap_build_conf_rsp(struct l2cap_chan *chan, void *data,
3832                                 u16 result, u16 flags)
3833 {
3834         struct l2cap_conf_rsp *rsp = data;
3835         void *ptr = rsp->data;
3836
3837         BT_DBG("chan %p", chan);
3838
3839         rsp->scid   = cpu_to_le16(chan->dcid);
3840         rsp->result = cpu_to_le16(result);
3841         rsp->flags  = cpu_to_le16(flags);
3842
3843         return ptr - data;
3844 }
3845
3846 void __l2cap_le_connect_rsp_defer(struct l2cap_chan *chan)
3847 {
3848         struct l2cap_le_conn_rsp rsp;
3849         struct l2cap_conn *conn = chan->conn;
3850
3851         BT_DBG("chan %p", chan);
3852
3853         rsp.dcid    = cpu_to_le16(chan->scid);
3854         rsp.mtu     = cpu_to_le16(chan->imtu);
3855         rsp.mps     = cpu_to_le16(chan->mps);
3856         rsp.credits = cpu_to_le16(chan->rx_credits);
3857         rsp.result  = __constant_cpu_to_le16(L2CAP_CR_SUCCESS);
3858
3859         l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CONN_RSP, sizeof(rsp),
3860                        &rsp);
3861 }
3862
3863 void __l2cap_connect_rsp_defer(struct l2cap_chan *chan)
3864 {
3865         struct l2cap_conn_rsp rsp;
3866         struct l2cap_conn *conn = chan->conn;
3867         u8 buf[128];
3868         u8 rsp_code;
3869
3870         rsp.scid   = cpu_to_le16(chan->dcid);
3871         rsp.dcid   = cpu_to_le16(chan->scid);
3872         rsp.result = __constant_cpu_to_le16(L2CAP_CR_SUCCESS);
3873         rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
3874
3875         if (chan->hs_hcon)
3876                 rsp_code = L2CAP_CREATE_CHAN_RSP;
3877         else
3878                 rsp_code = L2CAP_CONN_RSP;
3879
3880         BT_DBG("chan %p rsp_code %u", chan, rsp_code);
3881
3882         l2cap_send_cmd(conn, chan->ident, rsp_code, sizeof(rsp), &rsp);
3883
3884         if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
3885                 return;
3886
3887         l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3888                        l2cap_build_conf_req(chan, buf), buf);
3889         chan->num_conf_req++;
3890 }
3891
3892 static void l2cap_conf_rfc_get(struct l2cap_chan *chan, void *rsp, int len)
3893 {
3894         int type, olen;
3895         unsigned long val;
3896         /* Use sane default values in case a misbehaving remote device
3897          * did not send an RFC or extended window size option.
3898          */
3899         u16 txwin_ext = chan->ack_win;
3900         struct l2cap_conf_rfc rfc = {
3901                 .mode = chan->mode,
3902                 .retrans_timeout = __constant_cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO),
3903                 .monitor_timeout = __constant_cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO),
3904                 .max_pdu_size = cpu_to_le16(chan->imtu),
3905                 .txwin_size = min_t(u16, chan->ack_win, L2CAP_DEFAULT_TX_WINDOW),
3906         };
3907
3908         BT_DBG("chan %p, rsp %p, len %d", chan, rsp, len);
3909
3910         if ((chan->mode != L2CAP_MODE_ERTM) && (chan->mode != L2CAP_MODE_STREAMING))
3911                 return;
3912
3913         while (len >= L2CAP_CONF_OPT_SIZE) {
3914                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
3915
3916                 switch (type) {
3917                 case L2CAP_CONF_RFC:
3918                         if (olen == sizeof(rfc))
3919                                 memcpy(&rfc, (void *)val, olen);
3920                         break;
3921                 case L2CAP_CONF_EWS:
3922                         txwin_ext = val;
3923                         break;
3924                 }
3925         }
3926
3927         switch (rfc.mode) {
3928         case L2CAP_MODE_ERTM:
3929                 chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
3930                 chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
3931                 chan->mps = le16_to_cpu(rfc.max_pdu_size);
3932                 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
3933                         chan->ack_win = min_t(u16, chan->ack_win, txwin_ext);
3934                 else
3935                         chan->ack_win = min_t(u16, chan->ack_win,
3936                                               rfc.txwin_size);
3937                 break;
3938         case L2CAP_MODE_STREAMING:
3939                 chan->mps    = le16_to_cpu(rfc.max_pdu_size);
3940         }
3941 }
3942
3943 static inline int l2cap_command_rej(struct l2cap_conn *conn,
3944                                     struct l2cap_cmd_hdr *cmd, u16 cmd_len,
3945                                     u8 *data)
3946 {
3947         struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data;
3948
3949         if (cmd_len < sizeof(*rej))
3950                 return -EPROTO;
3951
3952         if (rej->reason != L2CAP_REJ_NOT_UNDERSTOOD)
3953                 return 0;
3954
3955         if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
3956             cmd->ident == conn->info_ident) {
3957                 cancel_delayed_work(&conn->info_timer);
3958
3959                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3960                 conn->info_ident = 0;
3961
3962                 l2cap_conn_start(conn);
3963         }
3964
3965         return 0;
3966 }
3967
3968 static struct l2cap_chan *l2cap_connect(struct l2cap_conn *conn,
3969                                         struct l2cap_cmd_hdr *cmd,
3970                                         u8 *data, u8 rsp_code, u8 amp_id)
3971 {
3972         struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
3973         struct l2cap_conn_rsp rsp;
3974         struct l2cap_chan *chan = NULL, *pchan;
3975         int result, status = L2CAP_CS_NO_INFO;
3976
3977         u16 dcid = 0, scid = __le16_to_cpu(req->scid);
3978         __le16 psm = req->psm;
3979
3980         BT_DBG("psm 0x%2.2x scid 0x%4.4x", __le16_to_cpu(psm), scid);
3981
3982         /* Check if we have socket listening on psm */
3983         pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, &conn->hcon->src,
3984                                          &conn->hcon->dst, ACL_LINK);
3985         if (!pchan) {
3986                 result = L2CAP_CR_BAD_PSM;
3987                 goto sendresp;
3988         }
3989
3990         mutex_lock(&conn->chan_lock);
3991         l2cap_chan_lock(pchan);
3992
3993         /* Check if the ACL is secure enough (if not SDP) */
3994         if (psm != __constant_cpu_to_le16(L2CAP_PSM_SDP) &&
3995             !hci_conn_check_link_mode(conn->hcon)) {
3996                 conn->disc_reason = HCI_ERROR_AUTH_FAILURE;
3997                 result = L2CAP_CR_SEC_BLOCK;
3998                 goto response;
3999         }
4000
4001         result = L2CAP_CR_NO_MEM;
4002
4003         /* Check if we already have channel with that dcid */
4004         if (__l2cap_get_chan_by_dcid(conn, scid))
4005                 goto response;
4006
4007         chan = pchan->ops->new_connection(pchan);
4008         if (!chan)
4009                 goto response;
4010
4011         /* For certain devices (ex: HID mouse), support for authentication,
4012          * pairing and bonding is optional. For such devices, inorder to avoid
4013          * the ACL alive for too long after L2CAP disconnection, reset the ACL
4014          * disc_timeout back to HCI_DISCONN_TIMEOUT during L2CAP connect.
4015          */
4016         conn->hcon->disc_timeout = HCI_DISCONN_TIMEOUT;
4017
4018         bacpy(&chan->src, &conn->hcon->src);
4019         bacpy(&chan->dst, &conn->hcon->dst);
4020         chan->src_type = bdaddr_type(conn->hcon, conn->hcon->src_type);
4021         chan->dst_type = bdaddr_type(conn->hcon, conn->hcon->dst_type);
4022         chan->psm  = psm;
4023         chan->dcid = scid;
4024         chan->local_amp_id = amp_id;
4025
4026         __l2cap_chan_add(conn, chan);
4027
4028         dcid = chan->scid;
4029
4030         __set_chan_timer(chan, chan->ops->get_sndtimeo(chan));
4031
4032         chan->ident = cmd->ident;
4033
4034         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
4035                 if (l2cap_chan_check_security(chan)) {
4036                         if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
4037                                 l2cap_state_change(chan, BT_CONNECT2);
4038                                 result = L2CAP_CR_PEND;
4039                                 status = L2CAP_CS_AUTHOR_PEND;
4040                                 chan->ops->defer(chan);
4041                         } else {
4042                                 /* Force pending result for AMP controllers.
4043                                  * The connection will succeed after the
4044                                  * physical link is up.
4045                                  */
4046                                 if (amp_id == AMP_ID_BREDR) {
4047                                         l2cap_state_change(chan, BT_CONFIG);
4048                                         result = L2CAP_CR_SUCCESS;
4049                                 } else {
4050                                         l2cap_state_change(chan, BT_CONNECT2);
4051                                         result = L2CAP_CR_PEND;
4052                                 }
4053                                 status = L2CAP_CS_NO_INFO;
4054                         }
4055                 } else {
4056                         l2cap_state_change(chan, BT_CONNECT2);
4057                         result = L2CAP_CR_PEND;
4058                         status = L2CAP_CS_AUTHEN_PEND;
4059                 }
4060         } else {
4061                 l2cap_state_change(chan, BT_CONNECT2);
4062                 result = L2CAP_CR_PEND;
4063                 status = L2CAP_CS_NO_INFO;
4064         }
4065
4066 response:
4067         l2cap_chan_unlock(pchan);
4068         mutex_unlock(&conn->chan_lock);
4069
4070 sendresp:
4071         rsp.scid   = cpu_to_le16(scid);
4072         rsp.dcid   = cpu_to_le16(dcid);
4073         rsp.result = cpu_to_le16(result);
4074         rsp.status = cpu_to_le16(status);
4075         l2cap_send_cmd(conn, cmd->ident, rsp_code, sizeof(rsp), &rsp);
4076
4077         if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
4078                 struct l2cap_info_req info;
4079                 info.type = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
4080
4081                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
4082                 conn->info_ident = l2cap_get_ident(conn);
4083
4084                 schedule_delayed_work(&conn->info_timer, L2CAP_INFO_TIMEOUT);
4085
4086                 l2cap_send_cmd(conn, conn->info_ident, L2CAP_INFO_REQ,
4087                                sizeof(info), &info);
4088         }
4089
4090         if (chan && !test_bit(CONF_REQ_SENT, &chan->conf_state) &&
4091             result == L2CAP_CR_SUCCESS) {
4092                 u8 buf[128];
4093                 set_bit(CONF_REQ_SENT, &chan->conf_state);
4094                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
4095                                l2cap_build_conf_req(chan, buf), buf);
4096                 chan->num_conf_req++;
4097         }
4098
4099         return chan;
4100 }
4101
4102 static int l2cap_connect_req(struct l2cap_conn *conn,
4103                              struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
4104 {
4105         struct hci_dev *hdev = conn->hcon->hdev;
4106         struct hci_conn *hcon = conn->hcon;
4107
4108         if (cmd_len < sizeof(struct l2cap_conn_req))
4109                 return -EPROTO;
4110
4111         hci_dev_lock(hdev);
4112         if (test_bit(HCI_MGMT, &hdev->dev_flags) &&
4113             !test_and_set_bit(HCI_CONN_MGMT_CONNECTED, &hcon->flags))
4114                 mgmt_device_connected(hdev, &hcon->dst, hcon->type,
4115                                       hcon->dst_type, 0, NULL, 0,
4116                                       hcon->dev_class);
4117         hci_dev_unlock(hdev);
4118
4119         l2cap_connect(conn, cmd, data, L2CAP_CONN_RSP, 0);
4120         return 0;
4121 }
4122
4123 static int l2cap_connect_create_rsp(struct l2cap_conn *conn,
4124                                     struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4125                                     u8 *data)
4126 {
4127         struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
4128         u16 scid, dcid, result, status;
4129         struct l2cap_chan *chan;
4130         u8 req[128];
4131         int err;
4132
4133         if (cmd_len < sizeof(*rsp))
4134                 return -EPROTO;
4135
4136         scid   = __le16_to_cpu(rsp->scid);
4137         dcid   = __le16_to_cpu(rsp->dcid);
4138         result = __le16_to_cpu(rsp->result);
4139         status = __le16_to_cpu(rsp->status);
4140
4141         BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x",
4142                dcid, scid, result, status);
4143
4144         mutex_lock(&conn->chan_lock);
4145
4146         if (scid) {
4147                 chan = __l2cap_get_chan_by_scid(conn, scid);
4148                 if (!chan) {
4149                         err = -EBADSLT;
4150                         goto unlock;
4151                 }
4152         } else {
4153                 chan = __l2cap_get_chan_by_ident(conn, cmd->ident);
4154                 if (!chan) {
4155                         err = -EBADSLT;
4156                         goto unlock;
4157                 }
4158         }
4159
4160         err = 0;
4161
4162         l2cap_chan_lock(chan);
4163
4164         switch (result) {
4165         case L2CAP_CR_SUCCESS:
4166                 l2cap_state_change(chan, BT_CONFIG);
4167                 chan->ident = 0;
4168                 chan->dcid = dcid;
4169                 clear_bit(CONF_CONNECT_PEND, &chan->conf_state);
4170
4171                 if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
4172                         break;
4173
4174                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
4175                                l2cap_build_conf_req(chan, req), req);
4176                 chan->num_conf_req++;
4177                 break;
4178
4179         case L2CAP_CR_PEND:
4180                 set_bit(CONF_CONNECT_PEND, &chan->conf_state);
4181                 break;
4182
4183         default:
4184                 l2cap_chan_del(chan, ECONNREFUSED);
4185                 break;
4186         }
4187
4188         l2cap_chan_unlock(chan);
4189
4190 unlock:
4191         mutex_unlock(&conn->chan_lock);
4192
4193         return err;
4194 }
4195
4196 static inline void set_default_fcs(struct l2cap_chan *chan)
4197 {
4198         /* FCS is enabled only in ERTM or streaming mode, if one or both
4199          * sides request it.
4200          */
4201         if (chan->mode != L2CAP_MODE_ERTM && chan->mode != L2CAP_MODE_STREAMING)
4202                 chan->fcs = L2CAP_FCS_NONE;
4203         else if (!test_bit(CONF_RECV_NO_FCS, &chan->conf_state))
4204                 chan->fcs = L2CAP_FCS_CRC16;
4205 }
4206
4207 static void l2cap_send_efs_conf_rsp(struct l2cap_chan *chan, void *data,
4208                                     u8 ident, u16 flags)
4209 {
4210         struct l2cap_conn *conn = chan->conn;
4211
4212         BT_DBG("conn %p chan %p ident %d flags 0x%4.4x", conn, chan, ident,
4213                flags);
4214
4215         clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
4216         set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
4217
4218         l2cap_send_cmd(conn, ident, L2CAP_CONF_RSP,
4219                        l2cap_build_conf_rsp(chan, data,
4220                                             L2CAP_CONF_SUCCESS, flags), data);
4221 }
4222
4223 static void cmd_reject_invalid_cid(struct l2cap_conn *conn, u8 ident,
4224                                    u16 scid, u16 dcid)
4225 {
4226         struct l2cap_cmd_rej_cid rej;
4227
4228         rej.reason = __constant_cpu_to_le16(L2CAP_REJ_INVALID_CID);
4229         rej.scid = __cpu_to_le16(scid);
4230         rej.dcid = __cpu_to_le16(dcid);
4231
4232         l2cap_send_cmd(conn, ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
4233 }
4234
4235 static inline int l2cap_config_req(struct l2cap_conn *conn,
4236                                    struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4237                                    u8 *data)
4238 {
4239         struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
4240         u16 dcid, flags;
4241         u8 rsp[64];
4242         struct l2cap_chan *chan;
4243         int len, err = 0;
4244
4245         if (cmd_len < sizeof(*req))
4246                 return -EPROTO;
4247
4248         dcid  = __le16_to_cpu(req->dcid);
4249         flags = __le16_to_cpu(req->flags);
4250
4251         BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
4252
4253         chan = l2cap_get_chan_by_scid(conn, dcid);
4254         if (!chan) {
4255                 cmd_reject_invalid_cid(conn, cmd->ident, dcid, 0);
4256                 return 0;
4257         }
4258
4259         if (chan->state != BT_CONFIG && chan->state != BT_CONNECT2) {
4260                 cmd_reject_invalid_cid(conn, cmd->ident, chan->scid,
4261                                        chan->dcid);
4262                 goto unlock;
4263         }
4264
4265         /* Reject if config buffer is too small. */
4266         len = cmd_len - sizeof(*req);
4267         if (chan->conf_len + len > sizeof(chan->conf_req)) {
4268                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
4269                                l2cap_build_conf_rsp(chan, rsp,
4270                                L2CAP_CONF_REJECT, flags), rsp);
4271                 goto unlock;
4272         }
4273
4274         /* Store config. */
4275         memcpy(chan->conf_req + chan->conf_len, req->data, len);
4276         chan->conf_len += len;
4277
4278         if (flags & L2CAP_CONF_FLAG_CONTINUATION) {
4279                 /* Incomplete config. Send empty response. */
4280                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
4281                                l2cap_build_conf_rsp(chan, rsp,
4282                                L2CAP_CONF_SUCCESS, flags), rsp);
4283                 goto unlock;
4284         }
4285
4286         /* Complete config. */
4287         len = l2cap_parse_conf_req(chan, rsp);
4288         if (len < 0) {
4289                 l2cap_send_disconn_req(chan, ECONNRESET);
4290                 goto unlock;
4291         }
4292
4293         chan->ident = cmd->ident;
4294         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
4295         chan->num_conf_rsp++;
4296
4297         /* Reset config buffer. */
4298         chan->conf_len = 0;
4299
4300         if (!test_bit(CONF_OUTPUT_DONE, &chan->conf_state))
4301                 goto unlock;
4302
4303         if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
4304                 set_default_fcs(chan);
4305
4306                 if (chan->mode == L2CAP_MODE_ERTM ||
4307                     chan->mode == L2CAP_MODE_STREAMING)
4308                         err = l2cap_ertm_init(chan);
4309
4310                 if (err < 0)
4311                         l2cap_send_disconn_req(chan, -err);
4312                 else
4313                         l2cap_chan_ready(chan);
4314
4315                 goto unlock;
4316         }
4317
4318         if (!test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) {
4319                 u8 buf[64];
4320                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
4321                                l2cap_build_conf_req(chan, buf), buf);
4322                 chan->num_conf_req++;
4323         }
4324
4325         /* Got Conf Rsp PENDING from remote side and asume we sent
4326            Conf Rsp PENDING in the code above */
4327         if (test_bit(CONF_REM_CONF_PEND, &chan->conf_state) &&
4328             test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
4329
4330                 /* check compatibility */
4331
4332                 /* Send rsp for BR/EDR channel */
4333                 if (!chan->hs_hcon)
4334                         l2cap_send_efs_conf_rsp(chan, rsp, cmd->ident, flags);
4335                 else
4336                         chan->ident = cmd->ident;
4337         }
4338
4339 unlock:
4340         l2cap_chan_unlock(chan);
4341         return err;
4342 }
4343
4344 static inline int l2cap_config_rsp(struct l2cap_conn *conn,
4345                                    struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4346                                    u8 *data)
4347 {
4348         struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
4349         u16 scid, flags, result;
4350         struct l2cap_chan *chan;
4351         int len = cmd_len - sizeof(*rsp);
4352         int err = 0;
4353
4354         if (cmd_len < sizeof(*rsp))
4355                 return -EPROTO;
4356
4357         scid   = __le16_to_cpu(rsp->scid);
4358         flags  = __le16_to_cpu(rsp->flags);
4359         result = __le16_to_cpu(rsp->result);
4360
4361         BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x len %d", scid, flags,
4362                result, len);
4363
4364         chan = l2cap_get_chan_by_scid(conn, scid);
4365         if (!chan)
4366                 return 0;
4367
4368         switch (result) {
4369         case L2CAP_CONF_SUCCESS:
4370                 l2cap_conf_rfc_get(chan, rsp->data, len);
4371                 clear_bit(CONF_REM_CONF_PEND, &chan->conf_state);
4372                 break;
4373
4374         case L2CAP_CONF_PENDING:
4375                 set_bit(CONF_REM_CONF_PEND, &chan->conf_state);
4376
4377                 if (test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
4378                         char buf[64];
4379
4380                         len = l2cap_parse_conf_rsp(chan, rsp->data, len,
4381                                                    buf, &result);
4382                         if (len < 0) {
4383                                 l2cap_send_disconn_req(chan, ECONNRESET);
4384                                 goto done;
4385                         }
4386
4387                         if (!chan->hs_hcon) {
4388                                 l2cap_send_efs_conf_rsp(chan, buf, cmd->ident,
4389                                                         0);
4390                         } else {
4391                                 if (l2cap_check_efs(chan)) {
4392                                         amp_create_logical_link(chan);
4393                                         chan->ident = cmd->ident;
4394                                 }
4395                         }
4396                 }
4397                 goto done;
4398
4399         case L2CAP_CONF_UNACCEPT:
4400                 if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
4401                         char req[64];
4402
4403                         if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
4404                                 l2cap_send_disconn_req(chan, ECONNRESET);
4405                                 goto done;
4406                         }
4407
4408                         /* throw out any old stored conf requests */
4409                         result = L2CAP_CONF_SUCCESS;
4410                         len = l2cap_parse_conf_rsp(chan, rsp->data, len,
4411                                                    req, &result);
4412                         if (len < 0) {
4413                                 l2cap_send_disconn_req(chan, ECONNRESET);
4414                                 goto done;
4415                         }
4416
4417                         l2cap_send_cmd(conn, l2cap_get_ident(conn),
4418                                        L2CAP_CONF_REQ, len, req);
4419                         chan->num_conf_req++;
4420                         if (result != L2CAP_CONF_SUCCESS)
4421                                 goto done;
4422                         break;
4423                 }
4424
4425         default:
4426                 l2cap_chan_set_err(chan, ECONNRESET);
4427
4428                 __set_chan_timer(chan, L2CAP_DISC_REJ_TIMEOUT);
4429                 l2cap_send_disconn_req(chan, ECONNRESET);
4430                 goto done;
4431         }
4432
4433         if (flags & L2CAP_CONF_FLAG_CONTINUATION)
4434                 goto done;
4435
4436         set_bit(CONF_INPUT_DONE, &chan->conf_state);
4437
4438         if (test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) {
4439                 set_default_fcs(chan);
4440
4441                 if (chan->mode == L2CAP_MODE_ERTM ||
4442                     chan->mode == L2CAP_MODE_STREAMING)
4443                         err = l2cap_ertm_init(chan);
4444
4445                 if (err < 0)
4446                         l2cap_send_disconn_req(chan, -err);
4447                 else
4448                         l2cap_chan_ready(chan);
4449         }
4450
4451 done:
4452         l2cap_chan_unlock(chan);
4453         return err;
4454 }
4455
4456 static inline int l2cap_disconnect_req(struct l2cap_conn *conn,
4457                                        struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4458                                        u8 *data)
4459 {
4460         struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
4461         struct l2cap_disconn_rsp rsp;
4462         u16 dcid, scid;
4463         struct l2cap_chan *chan;
4464
4465         if (cmd_len != sizeof(*req))
4466                 return -EPROTO;
4467
4468         scid = __le16_to_cpu(req->scid);
4469         dcid = __le16_to_cpu(req->dcid);
4470
4471         BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
4472
4473         mutex_lock(&conn->chan_lock);
4474
4475         chan = __l2cap_get_chan_by_scid(conn, dcid);
4476         if (!chan) {
4477                 mutex_unlock(&conn->chan_lock);
4478                 cmd_reject_invalid_cid(conn, cmd->ident, dcid, scid);
4479                 return 0;
4480         }
4481
4482         l2cap_chan_lock(chan);
4483
4484         rsp.dcid = cpu_to_le16(chan->scid);
4485         rsp.scid = cpu_to_le16(chan->dcid);
4486         l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
4487
4488         chan->ops->set_shutdown(chan);
4489
4490         l2cap_chan_hold(chan);
4491         l2cap_chan_del(chan, ECONNRESET);
4492
4493         l2cap_chan_unlock(chan);
4494
4495         chan->ops->close(chan);
4496         l2cap_chan_put(chan);
4497
4498         mutex_unlock(&conn->chan_lock);
4499
4500         return 0;
4501 }
4502
4503 static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn,
4504                                        struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4505                                        u8 *data)
4506 {
4507         struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
4508         u16 dcid, scid;
4509         struct l2cap_chan *chan;
4510
4511         if (cmd_len != sizeof(*rsp))
4512                 return -EPROTO;
4513
4514         scid = __le16_to_cpu(rsp->scid);
4515         dcid = __le16_to_cpu(rsp->dcid);
4516
4517         BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
4518
4519         mutex_lock(&conn->chan_lock);
4520
4521         chan = __l2cap_get_chan_by_scid(conn, scid);
4522         if (!chan) {
4523                 mutex_unlock(&conn->chan_lock);
4524                 return 0;
4525         }
4526
4527         l2cap_chan_lock(chan);
4528
4529         l2cap_chan_hold(chan);
4530         l2cap_chan_del(chan, 0);
4531
4532         l2cap_chan_unlock(chan);
4533
4534         chan->ops->close(chan);
4535         l2cap_chan_put(chan);
4536
4537         mutex_unlock(&conn->chan_lock);
4538
4539         return 0;
4540 }
4541
4542 static inline int l2cap_information_req(struct l2cap_conn *conn,
4543                                         struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4544                                         u8 *data)
4545 {
4546         struct l2cap_info_req *req = (struct l2cap_info_req *) data;
4547         u16 type;
4548
4549         if (cmd_len != sizeof(*req))
4550                 return -EPROTO;
4551
4552         type = __le16_to_cpu(req->type);
4553
4554         BT_DBG("type 0x%4.4x", type);
4555
4556         if (type == L2CAP_IT_FEAT_MASK) {
4557                 u8 buf[8];
4558                 u32 feat_mask = l2cap_feat_mask;
4559                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
4560                 rsp->type   = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
4561                 rsp->result = __constant_cpu_to_le16(L2CAP_IR_SUCCESS);
4562                 if (!disable_ertm)
4563                         feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
4564                                 | L2CAP_FEAT_FCS;
4565                 if (conn->hs_enabled)
4566                         feat_mask |= L2CAP_FEAT_EXT_FLOW
4567                                 | L2CAP_FEAT_EXT_WINDOW;
4568
4569                 put_unaligned_le32(feat_mask, rsp->data);
4570                 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf),
4571                                buf);
4572         } else if (type == L2CAP_IT_FIXED_CHAN) {
4573                 u8 buf[12];
4574                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
4575
4576                 if (conn->hs_enabled)
4577                         l2cap_fixed_chan[0] |= L2CAP_FC_A2MP;
4578                 else
4579                         l2cap_fixed_chan[0] &= ~L2CAP_FC_A2MP;
4580
4581                 rsp->type   = __constant_cpu_to_le16(L2CAP_IT_FIXED_CHAN);
4582                 rsp->result = __constant_cpu_to_le16(L2CAP_IR_SUCCESS);
4583                 memcpy(rsp->data, l2cap_fixed_chan, sizeof(l2cap_fixed_chan));
4584                 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf),
4585                                buf);
4586         } else {
4587                 struct l2cap_info_rsp rsp;
4588                 rsp.type   = cpu_to_le16(type);
4589                 rsp.result = __constant_cpu_to_le16(L2CAP_IR_NOTSUPP);
4590                 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(rsp),
4591                                &rsp);
4592         }
4593
4594         return 0;
4595 }
4596
4597 static inline int l2cap_information_rsp(struct l2cap_conn *conn,
4598                                         struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4599                                         u8 *data)
4600 {
4601         struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
4602         u16 type, result;
4603
4604         if (cmd_len < sizeof(*rsp))
4605                 return -EPROTO;
4606
4607         type   = __le16_to_cpu(rsp->type);
4608         result = __le16_to_cpu(rsp->result);
4609
4610         BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
4611
4612         /* L2CAP Info req/rsp are unbound to channels, add extra checks */
4613         if (cmd->ident != conn->info_ident ||
4614             conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)
4615                 return 0;
4616
4617         cancel_delayed_work(&conn->info_timer);
4618
4619         if (result != L2CAP_IR_SUCCESS) {
4620                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4621                 conn->info_ident = 0;
4622
4623                 l2cap_conn_start(conn);
4624
4625                 return 0;
4626         }
4627
4628         switch (type) {
4629         case L2CAP_IT_FEAT_MASK:
4630                 conn->feat_mask = get_unaligned_le32(rsp->data);
4631
4632                 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
4633                         struct l2cap_info_req req;
4634                         req.type = __constant_cpu_to_le16(L2CAP_IT_FIXED_CHAN);
4635
4636                         conn->info_ident = l2cap_get_ident(conn);
4637
4638                         l2cap_send_cmd(conn, conn->info_ident,
4639                                        L2CAP_INFO_REQ, sizeof(req), &req);
4640                 } else {
4641                         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4642                         conn->info_ident = 0;
4643
4644                         l2cap_conn_start(conn);
4645                 }
4646                 break;
4647
4648         case L2CAP_IT_FIXED_CHAN:
4649                 conn->fixed_chan_mask = rsp->data[0];
4650                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4651                 conn->info_ident = 0;
4652
4653                 l2cap_conn_start(conn);
4654                 break;
4655         }
4656
4657         return 0;
4658 }
4659
4660 static int l2cap_create_channel_req(struct l2cap_conn *conn,
4661                                     struct l2cap_cmd_hdr *cmd,
4662                                     u16 cmd_len, void *data)
4663 {
4664         struct l2cap_create_chan_req *req = data;
4665         struct l2cap_create_chan_rsp rsp;
4666         struct l2cap_chan *chan;
4667         struct hci_dev *hdev;
4668         u16 psm, scid;
4669
4670         if (cmd_len != sizeof(*req))
4671                 return -EPROTO;
4672
4673         if (!conn->hs_enabled)
4674                 return -EINVAL;
4675
4676         psm = le16_to_cpu(req->psm);
4677         scid = le16_to_cpu(req->scid);
4678
4679         BT_DBG("psm 0x%2.2x, scid 0x%4.4x, amp_id %d", psm, scid, req->amp_id);
4680
4681         /* For controller id 0 make BR/EDR connection */
4682         if (req->amp_id == AMP_ID_BREDR) {
4683                 l2cap_connect(conn, cmd, data, L2CAP_CREATE_CHAN_RSP,
4684                               req->amp_id);
4685                 return 0;
4686         }
4687
4688         /* Validate AMP controller id */
4689         hdev = hci_dev_get(req->amp_id);
4690         if (!hdev)
4691                 goto error;
4692
4693         if (hdev->dev_type != HCI_AMP || !test_bit(HCI_UP, &hdev->flags)) {
4694                 hci_dev_put(hdev);
4695                 goto error;
4696         }
4697
4698         chan = l2cap_connect(conn, cmd, data, L2CAP_CREATE_CHAN_RSP,
4699                              req->amp_id);
4700         if (chan) {
4701                 struct amp_mgr *mgr = conn->hcon->amp_mgr;
4702                 struct hci_conn *hs_hcon;
4703
4704                 hs_hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK,
4705                                                   &conn->hcon->dst);
4706                 if (!hs_hcon) {
4707                         hci_dev_put(hdev);
4708                         cmd_reject_invalid_cid(conn, cmd->ident, chan->scid,
4709                                                chan->dcid);
4710                         return 0;
4711                 }
4712
4713                 BT_DBG("mgr %p bredr_chan %p hs_hcon %p", mgr, chan, hs_hcon);
4714
4715                 mgr->bredr_chan = chan;
4716                 chan->hs_hcon = hs_hcon;
4717                 chan->fcs = L2CAP_FCS_NONE;
4718                 conn->mtu = hdev->block_mtu;
4719         }
4720
4721         hci_dev_put(hdev);
4722
4723         return 0;
4724
4725 error:
4726         rsp.dcid = 0;
4727         rsp.scid = cpu_to_le16(scid);
4728         rsp.result = __constant_cpu_to_le16(L2CAP_CR_BAD_AMP);
4729         rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
4730
4731         l2cap_send_cmd(conn, cmd->ident, L2CAP_CREATE_CHAN_RSP,
4732                        sizeof(rsp), &rsp);
4733
4734         return 0;
4735 }
4736
4737 static void l2cap_send_move_chan_req(struct l2cap_chan *chan, u8 dest_amp_id)
4738 {
4739         struct l2cap_move_chan_req req;
4740         u8 ident;
4741
4742         BT_DBG("chan %p, dest_amp_id %d", chan, dest_amp_id);
4743
4744         ident = l2cap_get_ident(chan->conn);
4745         chan->ident = ident;
4746
4747         req.icid = cpu_to_le16(chan->scid);
4748         req.dest_amp_id = dest_amp_id;
4749
4750         l2cap_send_cmd(chan->conn, ident, L2CAP_MOVE_CHAN_REQ, sizeof(req),
4751                        &req);
4752
4753         __set_chan_timer(chan, L2CAP_MOVE_TIMEOUT);
4754 }
4755
4756 static void l2cap_send_move_chan_rsp(struct l2cap_chan *chan, u16 result)
4757 {
4758         struct l2cap_move_chan_rsp rsp;
4759
4760         BT_DBG("chan %p, result 0x%4.4x", chan, result);
4761
4762         rsp.icid = cpu_to_le16(chan->dcid);
4763         rsp.result = cpu_to_le16(result);
4764
4765         l2cap_send_cmd(chan->conn, chan->ident, L2CAP_MOVE_CHAN_RSP,
4766                        sizeof(rsp), &rsp);
4767 }
4768
4769 static void l2cap_send_move_chan_cfm(struct l2cap_chan *chan, u16 result)
4770 {
4771         struct l2cap_move_chan_cfm cfm;
4772
4773         BT_DBG("chan %p, result 0x%4.4x", chan, result);
4774
4775         chan->ident = l2cap_get_ident(chan->conn);
4776
4777         cfm.icid = cpu_to_le16(chan->scid);
4778         cfm.result = cpu_to_le16(result);
4779
4780         l2cap_send_cmd(chan->conn, chan->ident, L2CAP_MOVE_CHAN_CFM,
4781                        sizeof(cfm), &cfm);
4782
4783         __set_chan_timer(chan, L2CAP_MOVE_TIMEOUT);
4784 }
4785
4786 static void l2cap_send_move_chan_cfm_icid(struct l2cap_conn *conn, u16 icid)
4787 {
4788         struct l2cap_move_chan_cfm cfm;
4789
4790         BT_DBG("conn %p, icid 0x%4.4x", conn, icid);
4791
4792         cfm.icid = cpu_to_le16(icid);
4793         cfm.result = __constant_cpu_to_le16(L2CAP_MC_UNCONFIRMED);
4794
4795         l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_MOVE_CHAN_CFM,
4796                        sizeof(cfm), &cfm);
4797 }
4798
4799 static void l2cap_send_move_chan_cfm_rsp(struct l2cap_conn *conn, u8 ident,
4800                                          u16 icid)
4801 {
4802         struct l2cap_move_chan_cfm_rsp rsp;
4803
4804         BT_DBG("icid 0x%4.4x", icid);
4805
4806         rsp.icid = cpu_to_le16(icid);
4807         l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM_RSP, sizeof(rsp), &rsp);
4808 }
4809
4810 static void __release_logical_link(struct l2cap_chan *chan)
4811 {
4812         chan->hs_hchan = NULL;
4813         chan->hs_hcon = NULL;
4814
4815         /* Placeholder - release the logical link */
4816 }
4817
4818 static void l2cap_logical_fail(struct l2cap_chan *chan)
4819 {
4820         /* Logical link setup failed */
4821         if (chan->state != BT_CONNECTED) {
4822                 /* Create channel failure, disconnect */
4823                 l2cap_send_disconn_req(chan, ECONNRESET);
4824                 return;
4825         }
4826
4827         switch (chan->move_role) {
4828         case L2CAP_MOVE_ROLE_RESPONDER:
4829                 l2cap_move_done(chan);
4830                 l2cap_send_move_chan_rsp(chan, L2CAP_MR_NOT_SUPP);
4831                 break;
4832         case L2CAP_MOVE_ROLE_INITIATOR:
4833                 if (chan->move_state == L2CAP_MOVE_WAIT_LOGICAL_COMP ||
4834                     chan->move_state == L2CAP_MOVE_WAIT_LOGICAL_CFM) {
4835                         /* Remote has only sent pending or
4836                          * success responses, clean up
4837                          */
4838                         l2cap_move_done(chan);
4839                 }
4840
4841                 /* Other amp move states imply that the move
4842                  * has already aborted
4843                  */
4844                 l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
4845                 break;
4846         }
4847 }
4848
4849 static void l2cap_logical_finish_create(struct l2cap_chan *chan,
4850                                         struct hci_chan *hchan)
4851 {
4852         struct l2cap_conf_rsp rsp;
4853
4854         chan->hs_hchan = hchan;
4855         chan->hs_hcon->l2cap_data = chan->conn;
4856
4857         l2cap_send_efs_conf_rsp(chan, &rsp, chan->ident, 0);
4858
4859         if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
4860                 int err;
4861
4862                 set_default_fcs(chan);
4863
4864                 err = l2cap_ertm_init(chan);
4865                 if (err < 0)
4866                         l2cap_send_disconn_req(chan, -err);
4867                 else
4868                         l2cap_chan_ready(chan);
4869         }
4870 }
4871
4872 static void l2cap_logical_finish_move(struct l2cap_chan *chan,
4873                                       struct hci_chan *hchan)
4874 {
4875         chan->hs_hcon = hchan->conn;
4876         chan->hs_hcon->l2cap_data = chan->conn;
4877
4878         BT_DBG("move_state %d", chan->move_state);
4879
4880         switch (chan->move_state) {
4881         case L2CAP_MOVE_WAIT_LOGICAL_COMP:
4882                 /* Move confirm will be sent after a success
4883                  * response is received
4884                  */
4885                 chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
4886                 break;
4887         case L2CAP_MOVE_WAIT_LOGICAL_CFM:
4888                 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
4889                         chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
4890                 } else if (chan->move_role == L2CAP_MOVE_ROLE_INITIATOR) {
4891                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM_RSP;
4892                         l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
4893                 } else if (chan->move_role == L2CAP_MOVE_ROLE_RESPONDER) {
4894                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
4895                         l2cap_send_move_chan_rsp(chan, L2CAP_MR_SUCCESS);
4896                 }
4897                 break;
4898         default:
4899                 /* Move was not in expected state, free the channel */
4900                 __release_logical_link(chan);
4901
4902                 chan->move_state = L2CAP_MOVE_STABLE;
4903         }
4904 }
4905
4906 /* Call with chan locked */
4907 void l2cap_logical_cfm(struct l2cap_chan *chan, struct hci_chan *hchan,
4908                        u8 status)
4909 {
4910         BT_DBG("chan %p, hchan %p, status %d", chan, hchan, status);
4911
4912         if (status) {
4913                 l2cap_logical_fail(chan);
4914                 __release_logical_link(chan);
4915                 return;
4916         }
4917
4918         if (chan->state != BT_CONNECTED) {
4919                 /* Ignore logical link if channel is on BR/EDR */
4920                 if (chan->local_amp_id != AMP_ID_BREDR)
4921                         l2cap_logical_finish_create(chan, hchan);
4922         } else {
4923                 l2cap_logical_finish_move(chan, hchan);
4924         }
4925 }
4926
4927 void l2cap_move_start(struct l2cap_chan *chan)
4928 {
4929         BT_DBG("chan %p", chan);
4930
4931         if (chan->local_amp_id == AMP_ID_BREDR) {
4932                 if (chan->chan_policy != BT_CHANNEL_POLICY_AMP_PREFERRED)
4933                         return;
4934                 chan->move_role = L2CAP_MOVE_ROLE_INITIATOR;
4935                 chan->move_state = L2CAP_MOVE_WAIT_PREPARE;
4936                 /* Placeholder - start physical link setup */
4937         } else {
4938                 chan->move_role = L2CAP_MOVE_ROLE_INITIATOR;
4939                 chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
4940                 chan->move_id = 0;
4941                 l2cap_move_setup(chan);
4942                 l2cap_send_move_chan_req(chan, 0);
4943         }
4944 }
4945
4946 static void l2cap_do_create(struct l2cap_chan *chan, int result,
4947                             u8 local_amp_id, u8 remote_amp_id)
4948 {
4949         BT_DBG("chan %p state %s %u -> %u", chan, state_to_string(chan->state),
4950                local_amp_id, remote_amp_id);
4951
4952         chan->fcs = L2CAP_FCS_NONE;
4953
4954         /* Outgoing channel on AMP */
4955         if (chan->state == BT_CONNECT) {
4956                 if (result == L2CAP_CR_SUCCESS) {
4957                         chan->local_amp_id = local_amp_id;
4958                         l2cap_send_create_chan_req(chan, remote_amp_id);
4959                 } else {
4960                         /* Revert to BR/EDR connect */
4961                         l2cap_send_conn_req(chan);
4962                 }
4963
4964                 return;
4965         }
4966
4967         /* Incoming channel on AMP */
4968         if (__l2cap_no_conn_pending(chan)) {
4969                 struct l2cap_conn_rsp rsp;
4970                 char buf[128];
4971                 rsp.scid = cpu_to_le16(chan->dcid);
4972                 rsp.dcid = cpu_to_le16(chan->scid);
4973
4974                 if (result == L2CAP_CR_SUCCESS) {
4975                         /* Send successful response */
4976                         rsp.result = __constant_cpu_to_le16(L2CAP_CR_SUCCESS);
4977                         rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
4978                 } else {
4979                         /* Send negative response */
4980                         rsp.result = __constant_cpu_to_le16(L2CAP_CR_NO_MEM);
4981                         rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
4982                 }
4983
4984                 l2cap_send_cmd(chan->conn, chan->ident, L2CAP_CREATE_CHAN_RSP,
4985                                sizeof(rsp), &rsp);
4986
4987                 if (result == L2CAP_CR_SUCCESS) {
4988                         l2cap_state_change(chan, BT_CONFIG);
4989                         set_bit(CONF_REQ_SENT, &chan->conf_state);
4990                         l2cap_send_cmd(chan->conn, l2cap_get_ident(chan->conn),
4991                                        L2CAP_CONF_REQ,
4992                                        l2cap_build_conf_req(chan, buf), buf);
4993                         chan->num_conf_req++;
4994                 }
4995         }
4996 }
4997
4998 static void l2cap_do_move_initiate(struct l2cap_chan *chan, u8 local_amp_id,
4999                                    u8 remote_amp_id)
5000 {
5001         l2cap_move_setup(chan);
5002         chan->move_id = local_amp_id;
5003         chan->move_state = L2CAP_MOVE_WAIT_RSP;
5004
5005         l2cap_send_move_chan_req(chan, remote_amp_id);
5006 }
5007
5008 static void l2cap_do_move_respond(struct l2cap_chan *chan, int result)
5009 {
5010         struct hci_chan *hchan = NULL;
5011
5012         /* Placeholder - get hci_chan for logical link */
5013
5014         if (hchan) {
5015                 if (hchan->state == BT_CONNECTED) {
5016                         /* Logical link is ready to go */
5017                         chan->hs_hcon = hchan->conn;
5018                         chan->hs_hcon->l2cap_data = chan->conn;
5019                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
5020                         l2cap_send_move_chan_rsp(chan, L2CAP_MR_SUCCESS);
5021
5022                         l2cap_logical_cfm(chan, hchan, L2CAP_MR_SUCCESS);
5023                 } else {
5024                         /* Wait for logical link to be ready */
5025                         chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
5026                 }
5027         } else {
5028                 /* Logical link not available */
5029                 l2cap_send_move_chan_rsp(chan, L2CAP_MR_NOT_ALLOWED);
5030         }
5031 }
5032
5033 static void l2cap_do_move_cancel(struct l2cap_chan *chan, int result)
5034 {
5035         if (chan->move_role == L2CAP_MOVE_ROLE_RESPONDER) {
5036                 u8 rsp_result;
5037                 if (result == -EINVAL)
5038                         rsp_result = L2CAP_MR_BAD_ID;
5039                 else
5040                         rsp_result = L2CAP_MR_NOT_ALLOWED;
5041
5042                 l2cap_send_move_chan_rsp(chan, rsp_result);
5043         }
5044
5045         chan->move_role = L2CAP_MOVE_ROLE_NONE;
5046         chan->move_state = L2CAP_MOVE_STABLE;
5047
5048         /* Restart data transmission */
5049         l2cap_ertm_send(chan);
5050 }
5051
5052 /* Invoke with locked chan */
5053 void __l2cap_physical_cfm(struct l2cap_chan *chan, int result)
5054 {
5055         u8 local_amp_id = chan->local_amp_id;
5056         u8 remote_amp_id = chan->remote_amp_id;
5057
5058         BT_DBG("chan %p, result %d, local_amp_id %d, remote_amp_id %d",
5059                chan, result, local_amp_id, remote_amp_id);
5060
5061         if (chan->state == BT_DISCONN || chan->state == BT_CLOSED) {
5062                 l2cap_chan_unlock(chan);
5063                 return;
5064         }
5065
5066         if (chan->state != BT_CONNECTED) {
5067                 l2cap_do_create(chan, result, local_amp_id, remote_amp_id);
5068         } else if (result != L2CAP_MR_SUCCESS) {
5069                 l2cap_do_move_cancel(chan, result);
5070         } else {
5071                 switch (chan->move_role) {
5072                 case L2CAP_MOVE_ROLE_INITIATOR:
5073                         l2cap_do_move_initiate(chan, local_amp_id,
5074                                                remote_amp_id);
5075                         break;
5076                 case L2CAP_MOVE_ROLE_RESPONDER:
5077                         l2cap_do_move_respond(chan, result);
5078                         break;
5079                 default:
5080                         l2cap_do_move_cancel(chan, result);
5081                         break;
5082                 }
5083         }
5084 }
5085
5086 static inline int l2cap_move_channel_req(struct l2cap_conn *conn,
5087                                          struct l2cap_cmd_hdr *cmd,
5088                                          u16 cmd_len, void *data)
5089 {
5090         struct l2cap_move_chan_req *req = data;
5091         struct l2cap_move_chan_rsp rsp;
5092         struct l2cap_chan *chan;
5093         u16 icid = 0;
5094         u16 result = L2CAP_MR_NOT_ALLOWED;
5095
5096         if (cmd_len != sizeof(*req))
5097                 return -EPROTO;
5098
5099         icid = le16_to_cpu(req->icid);
5100
5101         BT_DBG("icid 0x%4.4x, dest_amp_id %d", icid, req->dest_amp_id);
5102
5103         if (!conn->hs_enabled)
5104                 return -EINVAL;
5105
5106         chan = l2cap_get_chan_by_dcid(conn, icid);
5107         if (!chan) {
5108                 rsp.icid = cpu_to_le16(icid);
5109                 rsp.result = __constant_cpu_to_le16(L2CAP_MR_NOT_ALLOWED);
5110                 l2cap_send_cmd(conn, cmd->ident, L2CAP_MOVE_CHAN_RSP,
5111                                sizeof(rsp), &rsp);
5112                 return 0;
5113         }
5114
5115         chan->ident = cmd->ident;
5116
5117         if (chan->scid < L2CAP_CID_DYN_START ||
5118             chan->chan_policy == BT_CHANNEL_POLICY_BREDR_ONLY ||
5119             (chan->mode != L2CAP_MODE_ERTM &&
5120              chan->mode != L2CAP_MODE_STREAMING)) {
5121                 result = L2CAP_MR_NOT_ALLOWED;
5122                 goto send_move_response;
5123         }
5124
5125         if (chan->local_amp_id == req->dest_amp_id) {
5126                 result = L2CAP_MR_SAME_ID;
5127                 goto send_move_response;
5128         }
5129
5130         if (req->dest_amp_id != AMP_ID_BREDR) {
5131                 struct hci_dev *hdev;
5132                 hdev = hci_dev_get(req->dest_amp_id);
5133                 if (!hdev || hdev->dev_type != HCI_AMP ||
5134                     !test_bit(HCI_UP, &hdev->flags)) {
5135                         if (hdev)
5136                                 hci_dev_put(hdev);
5137
5138                         result = L2CAP_MR_BAD_ID;
5139                         goto send_move_response;
5140                 }
5141                 hci_dev_put(hdev);
5142         }
5143
5144         /* Detect a move collision.  Only send a collision response
5145          * if this side has "lost", otherwise proceed with the move.
5146          * The winner has the larger bd_addr.
5147          */
5148         if ((__chan_is_moving(chan) ||
5149              chan->move_role != L2CAP_MOVE_ROLE_NONE) &&
5150             bacmp(&conn->hcon->src, &conn->hcon->dst) > 0) {
5151                 result = L2CAP_MR_COLLISION;
5152                 goto send_move_response;
5153         }
5154
5155         chan->move_role = L2CAP_MOVE_ROLE_RESPONDER;
5156         l2cap_move_setup(chan);
5157         chan->move_id = req->dest_amp_id;
5158         icid = chan->dcid;
5159
5160         if (req->dest_amp_id == AMP_ID_BREDR) {
5161                 /* Moving to BR/EDR */
5162                 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
5163                         chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
5164                         result = L2CAP_MR_PEND;
5165                 } else {
5166                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
5167                         result = L2CAP_MR_SUCCESS;
5168                 }
5169         } else {
5170                 chan->move_state = L2CAP_MOVE_WAIT_PREPARE;
5171                 /* Placeholder - uncomment when amp functions are available */
5172                 /*amp_accept_physical(chan, req->dest_amp_id);*/
5173                 result = L2CAP_MR_PEND;
5174         }
5175
5176 send_move_response:
5177         l2cap_send_move_chan_rsp(chan, result);
5178
5179         l2cap_chan_unlock(chan);
5180
5181         return 0;
5182 }
5183
5184 static void l2cap_move_continue(struct l2cap_conn *conn, u16 icid, u16 result)
5185 {
5186         struct l2cap_chan *chan;
5187         struct hci_chan *hchan = NULL;
5188
5189         chan = l2cap_get_chan_by_scid(conn, icid);
5190         if (!chan) {
5191                 l2cap_send_move_chan_cfm_icid(conn, icid);
5192                 return;
5193         }
5194
5195         __clear_chan_timer(chan);
5196         if (result == L2CAP_MR_PEND)
5197                 __set_chan_timer(chan, L2CAP_MOVE_ERTX_TIMEOUT);
5198
5199         switch (chan->move_state) {
5200         case L2CAP_MOVE_WAIT_LOGICAL_COMP:
5201                 /* Move confirm will be sent when logical link
5202                  * is complete.
5203                  */
5204                 chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
5205                 break;
5206         case L2CAP_MOVE_WAIT_RSP_SUCCESS:
5207                 if (result == L2CAP_MR_PEND) {
5208                         break;
5209                 } else if (test_bit(CONN_LOCAL_BUSY,
5210                                     &chan->conn_state)) {
5211                         chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
5212                 } else {
5213                         /* Logical link is up or moving to BR/EDR,
5214                          * proceed with move
5215                          */
5216                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM_RSP;
5217                         l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
5218                 }
5219                 break;
5220         case L2CAP_MOVE_WAIT_RSP:
5221                 /* Moving to AMP */
5222                 if (result == L2CAP_MR_SUCCESS) {
5223                         /* Remote is ready, send confirm immediately
5224                          * after logical link is ready
5225                          */
5226                         chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
5227                 } else {
5228                         /* Both logical link and move success
5229                          * are required to confirm
5230                          */
5231                         chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_COMP;
5232                 }
5233
5234                 /* Placeholder - get hci_chan for logical link */
5235                 if (!hchan) {
5236                         /* Logical link not available */
5237                         l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
5238                         break;
5239                 }
5240
5241                 /* If the logical link is not yet connected, do not
5242                  * send confirmation.
5243                  */
5244                 if (hchan->state != BT_CONNECTED)
5245                         break;
5246
5247                 /* Logical link is already ready to go */
5248
5249                 chan->hs_hcon = hchan->conn;
5250                 chan->hs_hcon->l2cap_data = chan->conn;
5251
5252                 if (result == L2CAP_MR_SUCCESS) {
5253                         /* Can confirm now */
5254                         l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
5255                 } else {
5256                         /* Now only need move success
5257                          * to confirm
5258                          */
5259                         chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
5260                 }
5261
5262                 l2cap_logical_cfm(chan, hchan, L2CAP_MR_SUCCESS);
5263                 break;
5264         default:
5265                 /* Any other amp move state means the move failed. */
5266                 chan->move_id = chan->local_amp_id;
5267                 l2cap_move_done(chan);
5268                 l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
5269         }
5270
5271         l2cap_chan_unlock(chan);
5272 }
5273
5274 static void l2cap_move_fail(struct l2cap_conn *conn, u8 ident, u16 icid,
5275                             u16 result)
5276 {
5277         struct l2cap_chan *chan;
5278
5279         chan = l2cap_get_chan_by_ident(conn, ident);
5280         if (!chan) {
5281                 /* Could not locate channel, icid is best guess */
5282                 l2cap_send_move_chan_cfm_icid(conn, icid);
5283                 return;
5284         }
5285
5286         __clear_chan_timer(chan);
5287
5288         if (chan->move_role == L2CAP_MOVE_ROLE_INITIATOR) {
5289                 if (result == L2CAP_MR_COLLISION) {
5290                         chan->move_role = L2CAP_MOVE_ROLE_RESPONDER;
5291                 } else {
5292                         /* Cleanup - cancel move */
5293                         chan->move_id = chan->local_amp_id;
5294                         l2cap_move_done(chan);
5295                 }
5296         }
5297
5298         l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
5299
5300         l2cap_chan_unlock(chan);
5301 }
5302
5303 static int l2cap_move_channel_rsp(struct l2cap_conn *conn,
5304                                   struct l2cap_cmd_hdr *cmd,
5305                                   u16 cmd_len, void *data)
5306 {
5307         struct l2cap_move_chan_rsp *rsp = data;
5308         u16 icid, result;
5309
5310         if (cmd_len != sizeof(*rsp))
5311                 return -EPROTO;
5312
5313         icid = le16_to_cpu(rsp->icid);
5314         result = le16_to_cpu(rsp->result);
5315
5316         BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result);
5317
5318         if (result == L2CAP_MR_SUCCESS || result == L2CAP_MR_PEND)
5319                 l2cap_move_continue(conn, icid, result);
5320         else
5321                 l2cap_move_fail(conn, cmd->ident, icid, result);
5322
5323         return 0;
5324 }
5325
5326 static int l2cap_move_channel_confirm(struct l2cap_conn *conn,
5327                                       struct l2cap_cmd_hdr *cmd,
5328                                       u16 cmd_len, void *data)
5329 {
5330         struct l2cap_move_chan_cfm *cfm = data;
5331         struct l2cap_chan *chan;
5332         u16 icid, result;
5333
5334         if (cmd_len != sizeof(*cfm))
5335                 return -EPROTO;
5336
5337         icid = le16_to_cpu(cfm->icid);
5338         result = le16_to_cpu(cfm->result);
5339
5340         BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result);
5341
5342         chan = l2cap_get_chan_by_dcid(conn, icid);
5343         if (!chan) {
5344                 /* Spec requires a response even if the icid was not found */
5345                 l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid);
5346                 return 0;
5347         }
5348
5349         if (chan->move_state == L2CAP_MOVE_WAIT_CONFIRM) {
5350                 if (result == L2CAP_MC_CONFIRMED) {
5351                         chan->local_amp_id = chan->move_id;
5352                         if (chan->local_amp_id == AMP_ID_BREDR)
5353                                 __release_logical_link(chan);
5354                 } else {
5355                         chan->move_id = chan->local_amp_id;
5356                 }
5357
5358                 l2cap_move_done(chan);
5359         }
5360
5361         l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid);
5362
5363         l2cap_chan_unlock(chan);
5364
5365         return 0;
5366 }
5367
5368 static inline int l2cap_move_channel_confirm_rsp(struct l2cap_conn *conn,
5369                                                  struct l2cap_cmd_hdr *cmd,
5370                                                  u16 cmd_len, void *data)
5371 {
5372         struct l2cap_move_chan_cfm_rsp *rsp = data;
5373         struct l2cap_chan *chan;
5374         u16 icid;
5375
5376         if (cmd_len != sizeof(*rsp))
5377                 return -EPROTO;
5378
5379         icid = le16_to_cpu(rsp->icid);
5380
5381         BT_DBG("icid 0x%4.4x", icid);
5382
5383         chan = l2cap_get_chan_by_scid(conn, icid);
5384         if (!chan)
5385                 return 0;
5386
5387         __clear_chan_timer(chan);
5388
5389         if (chan->move_state == L2CAP_MOVE_WAIT_CONFIRM_RSP) {
5390                 chan->local_amp_id = chan->move_id;
5391
5392                 if (chan->local_amp_id == AMP_ID_BREDR && chan->hs_hchan)
5393                         __release_logical_link(chan);
5394
5395                 l2cap_move_done(chan);
5396         }
5397
5398         l2cap_chan_unlock(chan);
5399
5400         return 0;
5401 }
5402
5403 static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency,
5404                                          u16 to_multiplier)
5405 {
5406         u16 max_latency;
5407
5408         if (min > max || min < 6 || max > 3200)
5409                 return -EINVAL;
5410
5411         if (to_multiplier < 10 || to_multiplier > 3200)
5412                 return -EINVAL;
5413
5414         if (max >= to_multiplier * 8)
5415                 return -EINVAL;
5416
5417         max_latency = (to_multiplier * 8 / max) - 1;
5418         if (latency > 499 || latency > max_latency)
5419                 return -EINVAL;
5420
5421         return 0;
5422 }
5423
5424 static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
5425                                               struct l2cap_cmd_hdr *cmd,
5426                                               u16 cmd_len, u8 *data)
5427 {
5428         struct hci_conn *hcon = conn->hcon;
5429         struct l2cap_conn_param_update_req *req;
5430         struct l2cap_conn_param_update_rsp rsp;
5431         u16 min, max, latency, to_multiplier;
5432         int err;
5433
5434         if (!(hcon->link_mode & HCI_LM_MASTER))
5435                 return -EINVAL;
5436
5437         if (cmd_len != sizeof(struct l2cap_conn_param_update_req))
5438                 return -EPROTO;
5439
5440         req = (struct l2cap_conn_param_update_req *) data;
5441         min             = __le16_to_cpu(req->min);
5442         max             = __le16_to_cpu(req->max);
5443         latency         = __le16_to_cpu(req->latency);
5444         to_multiplier   = __le16_to_cpu(req->to_multiplier);
5445
5446         BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x",
5447                min, max, latency, to_multiplier);
5448
5449         memset(&rsp, 0, sizeof(rsp));
5450
5451         err = l2cap_check_conn_param(min, max, latency, to_multiplier);
5452         if (err)
5453                 rsp.result = __constant_cpu_to_le16(L2CAP_CONN_PARAM_REJECTED);
5454         else
5455                 rsp.result = __constant_cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED);
5456
5457         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP,
5458                        sizeof(rsp), &rsp);
5459
5460         if (!err)
5461                 hci_le_conn_update(hcon, min, max, latency, to_multiplier);
5462
5463         return 0;
5464 }
5465
5466 static int l2cap_le_connect_rsp(struct l2cap_conn *conn,
5467                                 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5468                                 u8 *data)
5469 {
5470         struct l2cap_le_conn_rsp *rsp = (struct l2cap_le_conn_rsp *) data;
5471         u16 dcid, mtu, mps, credits, result;
5472         struct l2cap_chan *chan;
5473         int err;
5474
5475         if (cmd_len < sizeof(*rsp))
5476                 return -EPROTO;
5477
5478         dcid    = __le16_to_cpu(rsp->dcid);
5479         mtu     = __le16_to_cpu(rsp->mtu);
5480         mps     = __le16_to_cpu(rsp->mps);
5481         credits = __le16_to_cpu(rsp->credits);
5482         result  = __le16_to_cpu(rsp->result);
5483
5484         if (result == L2CAP_CR_SUCCESS && (mtu < 23 || mps < 23))
5485                 return -EPROTO;
5486
5487         BT_DBG("dcid 0x%4.4x mtu %u mps %u credits %u result 0x%2.2x",
5488                dcid, mtu, mps, credits, result);
5489
5490         mutex_lock(&conn->chan_lock);
5491
5492         chan = __l2cap_get_chan_by_ident(conn, cmd->ident);
5493         if (!chan) {
5494                 err = -EBADSLT;
5495                 goto unlock;
5496         }
5497
5498         err = 0;
5499
5500         l2cap_chan_lock(chan);
5501
5502         switch (result) {
5503         case L2CAP_CR_SUCCESS:
5504                 chan->ident = 0;
5505                 chan->dcid = dcid;
5506                 chan->omtu = mtu;
5507                 chan->remote_mps = mps;
5508                 chan->tx_credits = credits;
5509                 l2cap_chan_ready(chan);
5510                 break;
5511
5512         default:
5513                 l2cap_chan_del(chan, ECONNREFUSED);
5514                 break;
5515         }
5516
5517         l2cap_chan_unlock(chan);
5518
5519 unlock:
5520         mutex_unlock(&conn->chan_lock);
5521
5522         return err;
5523 }
5524
5525 static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
5526                                       struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5527                                       u8 *data)
5528 {
5529         int err = 0;
5530
5531         switch (cmd->code) {
5532         case L2CAP_COMMAND_REJ:
5533                 l2cap_command_rej(conn, cmd, cmd_len, data);
5534                 break;
5535
5536         case L2CAP_CONN_REQ:
5537                 err = l2cap_connect_req(conn, cmd, cmd_len, data);
5538                 break;
5539
5540         case L2CAP_CONN_RSP:
5541         case L2CAP_CREATE_CHAN_RSP:
5542                 l2cap_connect_create_rsp(conn, cmd, cmd_len, data);
5543                 break;
5544
5545         case L2CAP_CONF_REQ:
5546                 err = l2cap_config_req(conn, cmd, cmd_len, data);
5547                 break;
5548
5549         case L2CAP_CONF_RSP:
5550                 l2cap_config_rsp(conn, cmd, cmd_len, data);
5551                 break;
5552
5553         case L2CAP_DISCONN_REQ:
5554                 err = l2cap_disconnect_req(conn, cmd, cmd_len, data);
5555                 break;
5556
5557         case L2CAP_DISCONN_RSP:
5558                 l2cap_disconnect_rsp(conn, cmd, cmd_len, data);
5559                 break;
5560
5561         case L2CAP_ECHO_REQ:
5562                 l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data);
5563                 break;
5564
5565         case L2CAP_ECHO_RSP:
5566                 break;
5567
5568         case L2CAP_INFO_REQ:
5569                 err = l2cap_information_req(conn, cmd, cmd_len, data);
5570                 break;
5571
5572         case L2CAP_INFO_RSP:
5573                 l2cap_information_rsp(conn, cmd, cmd_len, data);
5574                 break;
5575
5576         case L2CAP_CREATE_CHAN_REQ:
5577                 err = l2cap_create_channel_req(conn, cmd, cmd_len, data);
5578                 break;
5579
5580         case L2CAP_MOVE_CHAN_REQ:
5581                 err = l2cap_move_channel_req(conn, cmd, cmd_len, data);
5582                 break;
5583
5584         case L2CAP_MOVE_CHAN_RSP:
5585                 l2cap_move_channel_rsp(conn, cmd, cmd_len, data);
5586                 break;
5587
5588         case L2CAP_MOVE_CHAN_CFM:
5589                 err = l2cap_move_channel_confirm(conn, cmd, cmd_len, data);
5590                 break;
5591
5592         case L2CAP_MOVE_CHAN_CFM_RSP:
5593                 l2cap_move_channel_confirm_rsp(conn, cmd, cmd_len, data);
5594                 break;
5595
5596         default:
5597                 BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code);
5598                 err = -EINVAL;
5599                 break;
5600         }
5601
5602         return err;
5603 }
5604
5605 static int l2cap_le_connect_req(struct l2cap_conn *conn,
5606                                 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5607                                 u8 *data)
5608 {
5609         struct l2cap_le_conn_req *req = (struct l2cap_le_conn_req *) data;
5610         struct l2cap_le_conn_rsp rsp;
5611         struct l2cap_chan *chan, *pchan;
5612         u16 dcid, scid, credits, mtu, mps;
5613         __le16 psm;
5614         u8 result;
5615
5616         if (cmd_len != sizeof(*req))
5617                 return -EPROTO;
5618
5619         scid = __le16_to_cpu(req->scid);
5620         mtu  = __le16_to_cpu(req->mtu);
5621         mps  = __le16_to_cpu(req->mps);
5622         psm  = req->psm;
5623         dcid = 0;
5624         credits = 0;
5625
5626         if (mtu < 23 || mps < 23)
5627                 return -EPROTO;
5628
5629         BT_DBG("psm 0x%2.2x scid 0x%4.4x mtu %u mps %u", __le16_to_cpu(psm),
5630                scid, mtu, mps);
5631
5632         /* Check if we have socket listening on psm */
5633         pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, &conn->hcon->src,
5634                                          &conn->hcon->dst, LE_LINK);
5635         if (!pchan) {
5636                 result = L2CAP_CR_BAD_PSM;
5637                 chan = NULL;
5638                 goto response;
5639         }
5640
5641         mutex_lock(&conn->chan_lock);
5642         l2cap_chan_lock(pchan);
5643
5644         if (!smp_sufficient_security(conn->hcon, pchan->sec_level)) {
5645                 result = L2CAP_CR_AUTHENTICATION;
5646                 chan = NULL;
5647                 goto response_unlock;
5648         }
5649
5650         /* Check if we already have channel with that dcid */
5651         if (__l2cap_get_chan_by_dcid(conn, scid)) {
5652                 result = L2CAP_CR_NO_MEM;
5653                 chan = NULL;
5654                 goto response_unlock;
5655         }
5656
5657         chan = pchan->ops->new_connection(pchan);
5658         if (!chan) {
5659                 result = L2CAP_CR_NO_MEM;
5660                 goto response_unlock;
5661         }
5662
5663         bacpy(&chan->src, &conn->hcon->src);
5664         bacpy(&chan->dst, &conn->hcon->dst);
5665         chan->src_type = bdaddr_type(conn->hcon, conn->hcon->src_type);
5666         chan->dst_type = bdaddr_type(conn->hcon, conn->hcon->dst_type);
5667         chan->psm  = psm;
5668         chan->dcid = scid;
5669         chan->omtu = mtu;
5670         chan->remote_mps = mps;
5671         chan->tx_credits = __le16_to_cpu(req->credits);
5672
5673         __l2cap_chan_add(conn, chan);
5674         dcid = chan->scid;
5675         credits = chan->rx_credits;
5676
5677         __set_chan_timer(chan, chan->ops->get_sndtimeo(chan));
5678
5679         chan->ident = cmd->ident;
5680
5681         if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
5682                 l2cap_state_change(chan, BT_CONNECT2);
5683                 result = L2CAP_CR_PEND;
5684                 chan->ops->defer(chan);
5685         } else {
5686                 l2cap_chan_ready(chan);
5687                 result = L2CAP_CR_SUCCESS;
5688         }
5689
5690 response_unlock:
5691         l2cap_chan_unlock(pchan);
5692         mutex_unlock(&conn->chan_lock);
5693
5694         if (result == L2CAP_CR_PEND)
5695                 return 0;
5696
5697 response:
5698         if (chan) {
5699                 rsp.mtu = cpu_to_le16(chan->imtu);
5700                 rsp.mps = cpu_to_le16(chan->mps);
5701         } else {
5702                 rsp.mtu = 0;
5703                 rsp.mps = 0;
5704         }
5705
5706         rsp.dcid    = cpu_to_le16(dcid);
5707         rsp.credits = cpu_to_le16(credits);
5708         rsp.result  = cpu_to_le16(result);
5709
5710         l2cap_send_cmd(conn, cmd->ident, L2CAP_LE_CONN_RSP, sizeof(rsp), &rsp);
5711
5712         return 0;
5713 }
5714
5715 static inline int l2cap_le_credits(struct l2cap_conn *conn,
5716                                    struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5717                                    u8 *data)
5718 {
5719         struct l2cap_le_credits *pkt;
5720         struct l2cap_chan *chan;
5721         u16 cid, credits;
5722
5723         if (cmd_len != sizeof(*pkt))
5724                 return -EPROTO;
5725
5726         pkt = (struct l2cap_le_credits *) data;
5727         cid     = __le16_to_cpu(pkt->cid);
5728         credits = __le16_to_cpu(pkt->credits);
5729
5730         BT_DBG("cid 0x%4.4x credits 0x%4.4x", cid, credits);
5731
5732         chan = l2cap_get_chan_by_dcid(conn, cid);
5733         if (!chan)
5734                 return -EBADSLT;
5735
5736         chan->tx_credits += credits;
5737
5738         while (chan->tx_credits && !skb_queue_empty(&chan->tx_q)) {
5739                 l2cap_do_send(chan, skb_dequeue(&chan->tx_q));
5740                 chan->tx_credits--;
5741         }
5742
5743         if (chan->tx_credits)
5744                 chan->ops->resume(chan);
5745
5746         l2cap_chan_unlock(chan);
5747
5748         return 0;
5749 }
5750
5751 static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
5752                                    struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5753                                    u8 *data)
5754 {
5755         int err = 0;
5756
5757         if (!enable_lecoc) {
5758                 switch (cmd->code) {
5759                 case L2CAP_LE_CONN_REQ:
5760                 case L2CAP_LE_CONN_RSP:
5761                 case L2CAP_LE_CREDITS:
5762                 case L2CAP_DISCONN_REQ:
5763                 case L2CAP_DISCONN_RSP:
5764                         return -EINVAL;
5765                 }
5766         }
5767
5768         switch (cmd->code) {
5769         case L2CAP_COMMAND_REJ:
5770                 break;
5771
5772         case L2CAP_CONN_PARAM_UPDATE_REQ:
5773                 err = l2cap_conn_param_update_req(conn, cmd, cmd_len, data);
5774                 break;
5775
5776         case L2CAP_CONN_PARAM_UPDATE_RSP:
5777                 break;
5778
5779         case L2CAP_LE_CONN_RSP:
5780                 l2cap_le_connect_rsp(conn, cmd, cmd_len, data);
5781                 break;
5782
5783         case L2CAP_LE_CONN_REQ:
5784                 err = l2cap_le_connect_req(conn, cmd, cmd_len, data);
5785                 break;
5786
5787         case L2CAP_LE_CREDITS:
5788                 err = l2cap_le_credits(conn, cmd, cmd_len, data);
5789                 break;
5790
5791         case L2CAP_DISCONN_REQ:
5792                 err = l2cap_disconnect_req(conn, cmd, cmd_len, data);
5793                 break;
5794
5795         case L2CAP_DISCONN_RSP:
5796                 l2cap_disconnect_rsp(conn, cmd, cmd_len, data);
5797                 break;
5798
5799         default:
5800                 BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code);
5801                 err = -EINVAL;
5802                 break;
5803         }
5804
5805         return err;
5806 }
5807
5808 static inline void l2cap_le_sig_channel(struct l2cap_conn *conn,
5809                                         struct sk_buff *skb)
5810 {
5811         struct hci_conn *hcon = conn->hcon;
5812         struct l2cap_cmd_hdr *cmd;
5813         u16 len;
5814         int err;
5815
5816         if (hcon->type != LE_LINK)
5817                 goto drop;
5818
5819         if (skb->len < L2CAP_CMD_HDR_SIZE)
5820                 goto drop;
5821
5822         cmd = (void *) skb->data;
5823         skb_pull(skb, L2CAP_CMD_HDR_SIZE);
5824
5825         len = le16_to_cpu(cmd->len);
5826
5827         BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd->code, len, cmd->ident);
5828
5829         if (len != skb->len || !cmd->ident) {
5830                 BT_DBG("corrupted command");
5831                 goto drop;
5832         }
5833
5834         err = l2cap_le_sig_cmd(conn, cmd, len, skb->data);
5835         if (err) {
5836                 struct l2cap_cmd_rej_unk rej;
5837
5838                 BT_ERR("Wrong link type (%d)", err);
5839
5840                 rej.reason = __constant_cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
5841                 l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
5842                                sizeof(rej), &rej);
5843         }
5844
5845 drop:
5846         kfree_skb(skb);
5847 }
5848
5849 static inline void l2cap_sig_channel(struct l2cap_conn *conn,
5850                                      struct sk_buff *skb)
5851 {
5852         struct hci_conn *hcon = conn->hcon;
5853         u8 *data = skb->data;
5854         int len = skb->len;
5855         struct l2cap_cmd_hdr cmd;
5856         int err;
5857
5858         l2cap_raw_recv(conn, skb);
5859
5860         if (hcon->type != ACL_LINK)
5861                 goto drop;
5862
5863         while (len >= L2CAP_CMD_HDR_SIZE) {
5864                 u16 cmd_len;
5865                 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
5866                 data += L2CAP_CMD_HDR_SIZE;
5867                 len  -= L2CAP_CMD_HDR_SIZE;
5868
5869                 cmd_len = le16_to_cpu(cmd.len);
5870
5871                 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len,
5872                        cmd.ident);
5873
5874                 if (cmd_len > len || !cmd.ident) {
5875                         BT_DBG("corrupted command");
5876                         break;
5877                 }
5878
5879                 err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data);
5880                 if (err) {
5881                         struct l2cap_cmd_rej_unk rej;
5882
5883                         BT_ERR("Wrong link type (%d)", err);
5884
5885                         rej.reason = __constant_cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
5886                         l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ,
5887                                        sizeof(rej), &rej);
5888                 }
5889
5890                 data += cmd_len;
5891                 len  -= cmd_len;
5892         }
5893
5894 drop:
5895         kfree_skb(skb);
5896 }
5897
5898 static int l2cap_check_fcs(struct l2cap_chan *chan,  struct sk_buff *skb)
5899 {
5900         u16 our_fcs, rcv_fcs;
5901         int hdr_size;
5902
5903         if (test_bit(FLAG_EXT_CTRL, &chan->flags))
5904                 hdr_size = L2CAP_EXT_HDR_SIZE;
5905         else
5906                 hdr_size = L2CAP_ENH_HDR_SIZE;
5907
5908         if (chan->fcs == L2CAP_FCS_CRC16) {
5909                 skb_trim(skb, skb->len - L2CAP_FCS_SIZE);
5910                 rcv_fcs = get_unaligned_le16(skb->data + skb->len);
5911                 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
5912
5913                 if (our_fcs != rcv_fcs)
5914                         return -EBADMSG;
5915         }
5916         return 0;
5917 }
5918
5919 static void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan)
5920 {
5921         struct l2cap_ctrl control;
5922
5923         BT_DBG("chan %p", chan);
5924
5925         memset(&control, 0, sizeof(control));
5926         control.sframe = 1;
5927         control.final = 1;
5928         control.reqseq = chan->buffer_seq;
5929         set_bit(CONN_SEND_FBIT, &chan->conn_state);
5930
5931         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
5932                 control.super = L2CAP_SUPER_RNR;
5933                 l2cap_send_sframe(chan, &control);
5934         }
5935
5936         if (test_and_clear_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
5937             chan->unacked_frames > 0)
5938                 __set_retrans_timer(chan);
5939
5940         /* Send pending iframes */
5941         l2cap_ertm_send(chan);
5942
5943         if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
5944             test_bit(CONN_SEND_FBIT, &chan->conn_state)) {
5945                 /* F-bit wasn't sent in an s-frame or i-frame yet, so
5946                  * send it now.
5947                  */
5948                 control.super = L2CAP_SUPER_RR;
5949                 l2cap_send_sframe(chan, &control);
5950         }
5951 }
5952
5953 static void append_skb_frag(struct sk_buff *skb, struct sk_buff *new_frag,
5954                             struct sk_buff **last_frag)
5955 {
5956         /* skb->len reflects data in skb as well as all fragments
5957          * skb->data_len reflects only data in fragments
5958          */
5959         if (!skb_has_frag_list(skb))
5960                 skb_shinfo(skb)->frag_list = new_frag;
5961
5962         new_frag->next = NULL;
5963
5964         (*last_frag)->next = new_frag;
5965         *last_frag = new_frag;
5966
5967         skb->len += new_frag->len;
5968         skb->data_len += new_frag->len;
5969         skb->truesize += new_frag->truesize;
5970 }
5971
5972 static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb,
5973                                 struct l2cap_ctrl *control)
5974 {
5975         int err = -EINVAL;
5976
5977         switch (control->sar) {
5978         case L2CAP_SAR_UNSEGMENTED:
5979                 if (chan->sdu)
5980                         break;
5981
5982                 err = chan->ops->recv(chan, skb);
5983                 break;
5984
5985         case L2CAP_SAR_START:
5986                 if (chan->sdu)
5987                         break;
5988
5989                 chan->sdu_len = get_unaligned_le16(skb->data);
5990                 skb_pull(skb, L2CAP_SDULEN_SIZE);
5991
5992                 if (chan->sdu_len > chan->imtu) {
5993                         err = -EMSGSIZE;
5994                         break;
5995                 }
5996
5997                 if (skb->len >= chan->sdu_len)
5998                         break;
5999
6000                 chan->sdu = skb;
6001                 chan->sdu_last_frag = skb;
6002
6003                 skb = NULL;
6004                 err = 0;
6005                 break;
6006
6007         case L2CAP_SAR_CONTINUE:
6008                 if (!chan->sdu)
6009                         break;
6010
6011                 append_skb_frag(chan->sdu, skb,
6012                                 &chan->sdu_last_frag);
6013                 skb = NULL;
6014
6015                 if (chan->sdu->len >= chan->sdu_len)
6016                         break;
6017
6018                 err = 0;
6019                 break;
6020
6021         case L2CAP_SAR_END:
6022                 if (!chan->sdu)
6023                         break;
6024
6025                 append_skb_frag(chan->sdu, skb,
6026                                 &chan->sdu_last_frag);
6027                 skb = NULL;
6028
6029                 if (chan->sdu->len != chan->sdu_len)
6030                         break;
6031
6032                 err = chan->ops->recv(chan, chan->sdu);
6033
6034                 if (!err) {
6035                         /* Reassembly complete */
6036                         chan->sdu = NULL;
6037                         chan->sdu_last_frag = NULL;
6038                         chan->sdu_len = 0;
6039                 }
6040                 break;
6041         }
6042
6043         if (err) {
6044                 kfree_skb(skb);
6045                 kfree_skb(chan->sdu);
6046                 chan->sdu = NULL;
6047                 chan->sdu_last_frag = NULL;
6048                 chan->sdu_len = 0;
6049         }
6050
6051         return err;
6052 }
6053
6054 static int l2cap_resegment(struct l2cap_chan *chan)
6055 {
6056         /* Placeholder */
6057         return 0;
6058 }
6059
6060 void l2cap_chan_busy(struct l2cap_chan *chan, int busy)
6061 {
6062         u8 event;
6063
6064         if (chan->mode != L2CAP_MODE_ERTM)
6065                 return;
6066
6067         event = busy ? L2CAP_EV_LOCAL_BUSY_DETECTED : L2CAP_EV_LOCAL_BUSY_CLEAR;
6068         l2cap_tx(chan, NULL, NULL, event);
6069 }
6070
6071 static int l2cap_rx_queued_iframes(struct l2cap_chan *chan)
6072 {
6073         int err = 0;
6074         /* Pass sequential frames to l2cap_reassemble_sdu()
6075          * until a gap is encountered.
6076          */
6077
6078         BT_DBG("chan %p", chan);
6079
6080         while (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
6081                 struct sk_buff *skb;
6082                 BT_DBG("Searching for skb with txseq %d (queue len %d)",
6083                        chan->buffer_seq, skb_queue_len(&chan->srej_q));
6084
6085                 skb = l2cap_ertm_seq_in_queue(&chan->srej_q, chan->buffer_seq);
6086
6087                 if (!skb)
6088                         break;
6089
6090                 skb_unlink(skb, &chan->srej_q);
6091                 chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
6092                 err = l2cap_reassemble_sdu(chan, skb, &bt_cb(skb)->control);
6093                 if (err)
6094                         break;
6095         }
6096
6097         if (skb_queue_empty(&chan->srej_q)) {
6098                 chan->rx_state = L2CAP_RX_STATE_RECV;
6099                 l2cap_send_ack(chan);
6100         }
6101
6102         return err;
6103 }
6104
6105 static void l2cap_handle_srej(struct l2cap_chan *chan,
6106                               struct l2cap_ctrl *control)
6107 {
6108         struct sk_buff *skb;
6109
6110         BT_DBG("chan %p, control %p", chan, control);
6111
6112         if (control->reqseq == chan->next_tx_seq) {
6113                 BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq);
6114                 l2cap_send_disconn_req(chan, ECONNRESET);
6115                 return;
6116         }
6117
6118         skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq);
6119
6120         if (skb == NULL) {
6121                 BT_DBG("Seq %d not available for retransmission",
6122                        control->reqseq);
6123                 return;
6124         }
6125
6126         if (chan->max_tx != 0 && bt_cb(skb)->control.retries >= chan->max_tx) {
6127                 BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
6128                 l2cap_send_disconn_req(chan, ECONNRESET);
6129                 return;
6130         }
6131
6132         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6133
6134         if (control->poll) {
6135                 l2cap_pass_to_tx(chan, control);
6136
6137                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
6138                 l2cap_retransmit(chan, control);
6139                 l2cap_ertm_send(chan);
6140
6141                 if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) {
6142                         set_bit(CONN_SREJ_ACT, &chan->conn_state);
6143                         chan->srej_save_reqseq = control->reqseq;
6144                 }
6145         } else {
6146                 l2cap_pass_to_tx_fbit(chan, control);
6147
6148                 if (control->final) {
6149                         if (chan->srej_save_reqseq != control->reqseq ||
6150                             !test_and_clear_bit(CONN_SREJ_ACT,
6151                                                 &chan->conn_state))
6152                                 l2cap_retransmit(chan, control);
6153                 } else {
6154                         l2cap_retransmit(chan, control);
6155                         if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) {
6156                                 set_bit(CONN_SREJ_ACT, &chan->conn_state);
6157                                 chan->srej_save_reqseq = control->reqseq;
6158                         }
6159                 }
6160         }
6161 }
6162
6163 static void l2cap_handle_rej(struct l2cap_chan *chan,
6164                              struct l2cap_ctrl *control)
6165 {
6166         struct sk_buff *skb;
6167
6168         BT_DBG("chan %p, control %p", chan, control);
6169
6170         if (control->reqseq == chan->next_tx_seq) {
6171                 BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq);
6172                 l2cap_send_disconn_req(chan, ECONNRESET);
6173                 return;
6174         }
6175
6176         skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq);
6177
6178         if (chan->max_tx && skb &&
6179             bt_cb(skb)->control.retries >= chan->max_tx) {
6180                 BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
6181                 l2cap_send_disconn_req(chan, ECONNRESET);
6182                 return;
6183         }
6184
6185         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6186
6187         l2cap_pass_to_tx(chan, control);
6188
6189         if (control->final) {
6190                 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
6191                         l2cap_retransmit_all(chan, control);
6192         } else {
6193                 l2cap_retransmit_all(chan, control);
6194                 l2cap_ertm_send(chan);
6195                 if (chan->tx_state == L2CAP_TX_STATE_WAIT_F)
6196                         set_bit(CONN_REJ_ACT, &chan->conn_state);
6197         }
6198 }
6199
6200 static u8 l2cap_classify_txseq(struct l2cap_chan *chan, u16 txseq)
6201 {
6202         BT_DBG("chan %p, txseq %d", chan, txseq);
6203
6204         BT_DBG("last_acked_seq %d, expected_tx_seq %d", chan->last_acked_seq,
6205                chan->expected_tx_seq);
6206
6207         if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
6208                 if (__seq_offset(chan, txseq, chan->last_acked_seq) >=
6209                     chan->tx_win) {
6210                         /* See notes below regarding "double poll" and
6211                          * invalid packets.
6212                          */
6213                         if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) {
6214                                 BT_DBG("Invalid/Ignore - after SREJ");
6215                                 return L2CAP_TXSEQ_INVALID_IGNORE;
6216                         } else {
6217                                 BT_DBG("Invalid - in window after SREJ sent");
6218                                 return L2CAP_TXSEQ_INVALID;
6219                         }
6220                 }
6221
6222                 if (chan->srej_list.head == txseq) {
6223                         BT_DBG("Expected SREJ");
6224                         return L2CAP_TXSEQ_EXPECTED_SREJ;
6225                 }
6226
6227                 if (l2cap_ertm_seq_in_queue(&chan->srej_q, txseq)) {
6228                         BT_DBG("Duplicate SREJ - txseq already stored");
6229                         return L2CAP_TXSEQ_DUPLICATE_SREJ;
6230                 }
6231
6232                 if (l2cap_seq_list_contains(&chan->srej_list, txseq)) {
6233                         BT_DBG("Unexpected SREJ - not requested");
6234                         return L2CAP_TXSEQ_UNEXPECTED_SREJ;
6235                 }
6236         }
6237
6238         if (chan->expected_tx_seq == txseq) {
6239                 if (__seq_offset(chan, txseq, chan->last_acked_seq) >=
6240                     chan->tx_win) {
6241                         BT_DBG("Invalid - txseq outside tx window");
6242                         return L2CAP_TXSEQ_INVALID;
6243                 } else {
6244                         BT_DBG("Expected");
6245                         return L2CAP_TXSEQ_EXPECTED;
6246                 }
6247         }
6248
6249         if (__seq_offset(chan, txseq, chan->last_acked_seq) <
6250             __seq_offset(chan, chan->expected_tx_seq, chan->last_acked_seq)) {
6251                 BT_DBG("Duplicate - expected_tx_seq later than txseq");
6252                 return L2CAP_TXSEQ_DUPLICATE;
6253         }
6254
6255         if (__seq_offset(chan, txseq, chan->last_acked_seq) >= chan->tx_win) {
6256                 /* A source of invalid packets is a "double poll" condition,
6257                  * where delays cause us to send multiple poll packets.  If
6258                  * the remote stack receives and processes both polls,
6259                  * sequence numbers can wrap around in such a way that a
6260                  * resent frame has a sequence number that looks like new data
6261                  * with a sequence gap.  This would trigger an erroneous SREJ
6262                  * request.
6263                  *
6264                  * Fortunately, this is impossible with a tx window that's
6265                  * less than half of the maximum sequence number, which allows
6266                  * invalid frames to be safely ignored.
6267                  *
6268                  * With tx window sizes greater than half of the tx window
6269                  * maximum, the frame is invalid and cannot be ignored.  This
6270                  * causes a disconnect.
6271                  */
6272
6273                 if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) {
6274                         BT_DBG("Invalid/Ignore - txseq outside tx window");
6275                         return L2CAP_TXSEQ_INVALID_IGNORE;
6276                 } else {
6277                         BT_DBG("Invalid - txseq outside tx window");
6278                         return L2CAP_TXSEQ_INVALID;
6279                 }
6280         } else {
6281                 BT_DBG("Unexpected - txseq indicates missing frames");
6282                 return L2CAP_TXSEQ_UNEXPECTED;
6283         }
6284 }
6285
6286 static int l2cap_rx_state_recv(struct l2cap_chan *chan,
6287                                struct l2cap_ctrl *control,
6288                                struct sk_buff *skb, u8 event)
6289 {
6290         int err = 0;
6291         bool skb_in_use = false;
6292
6293         BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
6294                event);
6295
6296         switch (event) {
6297         case L2CAP_EV_RECV_IFRAME:
6298                 switch (l2cap_classify_txseq(chan, control->txseq)) {
6299                 case L2CAP_TXSEQ_EXPECTED:
6300                         l2cap_pass_to_tx(chan, control);
6301
6302                         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
6303                                 BT_DBG("Busy, discarding expected seq %d",
6304                                        control->txseq);
6305                                 break;
6306                         }
6307
6308                         chan->expected_tx_seq = __next_seq(chan,
6309                                                            control->txseq);
6310
6311                         chan->buffer_seq = chan->expected_tx_seq;
6312                         skb_in_use = true;
6313
6314                         err = l2cap_reassemble_sdu(chan, skb, control);
6315                         if (err)
6316                                 break;
6317
6318                         if (control->final) {
6319                                 if (!test_and_clear_bit(CONN_REJ_ACT,
6320                                                         &chan->conn_state)) {
6321                                         control->final = 0;
6322                                         l2cap_retransmit_all(chan, control);
6323                                         l2cap_ertm_send(chan);
6324                                 }
6325                         }
6326
6327                         if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state))
6328                                 l2cap_send_ack(chan);
6329                         break;
6330                 case L2CAP_TXSEQ_UNEXPECTED:
6331                         l2cap_pass_to_tx(chan, control);
6332
6333                         /* Can't issue SREJ frames in the local busy state.
6334                          * Drop this frame, it will be seen as missing
6335                          * when local busy is exited.
6336                          */
6337                         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
6338                                 BT_DBG("Busy, discarding unexpected seq %d",
6339                                        control->txseq);
6340                                 break;
6341                         }
6342
6343                         /* There was a gap in the sequence, so an SREJ
6344                          * must be sent for each missing frame.  The
6345                          * current frame is stored for later use.
6346                          */
6347                         skb_queue_tail(&chan->srej_q, skb);
6348                         skb_in_use = true;
6349                         BT_DBG("Queued %p (queue len %d)", skb,
6350                                skb_queue_len(&chan->srej_q));
6351
6352                         clear_bit(CONN_SREJ_ACT, &chan->conn_state);
6353                         l2cap_seq_list_clear(&chan->srej_list);
6354                         l2cap_send_srej(chan, control->txseq);
6355
6356                         chan->rx_state = L2CAP_RX_STATE_SREJ_SENT;
6357                         break;
6358                 case L2CAP_TXSEQ_DUPLICATE:
6359                         l2cap_pass_to_tx(chan, control);
6360                         break;
6361                 case L2CAP_TXSEQ_INVALID_IGNORE:
6362                         break;
6363                 case L2CAP_TXSEQ_INVALID:
6364                 default:
6365                         l2cap_send_disconn_req(chan, ECONNRESET);
6366                         break;
6367                 }
6368                 break;
6369         case L2CAP_EV_RECV_RR:
6370                 l2cap_pass_to_tx(chan, control);
6371                 if (control->final) {
6372                         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6373
6374                         if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state) &&
6375                             !__chan_is_moving(chan)) {
6376                                 control->final = 0;
6377                                 l2cap_retransmit_all(chan, control);
6378                         }
6379
6380                         l2cap_ertm_send(chan);
6381                 } else if (control->poll) {
6382                         l2cap_send_i_or_rr_or_rnr(chan);
6383                 } else {
6384                         if (test_and_clear_bit(CONN_REMOTE_BUSY,
6385                                                &chan->conn_state) &&
6386                             chan->unacked_frames)
6387                                 __set_retrans_timer(chan);
6388
6389                         l2cap_ertm_send(chan);
6390                 }
6391                 break;
6392         case L2CAP_EV_RECV_RNR:
6393                 set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6394                 l2cap_pass_to_tx(chan, control);
6395                 if (control && control->poll) {
6396                         set_bit(CONN_SEND_FBIT, &chan->conn_state);
6397                         l2cap_send_rr_or_rnr(chan, 0);
6398                 }
6399                 __clear_retrans_timer(chan);
6400                 l2cap_seq_list_clear(&chan->retrans_list);
6401                 break;
6402         case L2CAP_EV_RECV_REJ:
6403                 l2cap_handle_rej(chan, control);
6404                 break;
6405         case L2CAP_EV_RECV_SREJ:
6406                 l2cap_handle_srej(chan, control);
6407                 break;
6408         default:
6409                 break;
6410         }
6411
6412         if (skb && !skb_in_use) {
6413                 BT_DBG("Freeing %p", skb);
6414                 kfree_skb(skb);
6415         }
6416
6417         return err;
6418 }
6419
6420 static int l2cap_rx_state_srej_sent(struct l2cap_chan *chan,
6421                                     struct l2cap_ctrl *control,
6422                                     struct sk_buff *skb, u8 event)
6423 {
6424         int err = 0;
6425         u16 txseq = control->txseq;
6426         bool skb_in_use = false;
6427
6428         BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
6429                event);
6430
6431         switch (event) {
6432         case L2CAP_EV_RECV_IFRAME:
6433                 switch (l2cap_classify_txseq(chan, txseq)) {
6434                 case L2CAP_TXSEQ_EXPECTED:
6435                         /* Keep frame for reassembly later */
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                         chan->expected_tx_seq = __next_seq(chan, txseq);
6443                         break;
6444                 case L2CAP_TXSEQ_EXPECTED_SREJ:
6445                         l2cap_seq_list_pop(&chan->srej_list);
6446
6447                         l2cap_pass_to_tx(chan, control);
6448                         skb_queue_tail(&chan->srej_q, skb);
6449                         skb_in_use = true;
6450                         BT_DBG("Queued %p (queue len %d)", skb,
6451                                skb_queue_len(&chan->srej_q));
6452
6453                         err = l2cap_rx_queued_iframes(chan);
6454                         if (err)
6455                                 break;
6456
6457                         break;
6458                 case L2CAP_TXSEQ_UNEXPECTED:
6459                         /* Got a frame that can't be reassembled yet.
6460                          * Save it for later, and send SREJs to cover
6461                          * the missing frames.
6462                          */
6463                         skb_queue_tail(&chan->srej_q, skb);
6464                         skb_in_use = true;
6465                         BT_DBG("Queued %p (queue len %d)", skb,
6466                                skb_queue_len(&chan->srej_q));
6467
6468                         l2cap_pass_to_tx(chan, control);
6469                         l2cap_send_srej(chan, control->txseq);
6470                         break;
6471                 case L2CAP_TXSEQ_UNEXPECTED_SREJ:
6472                         /* This frame was requested with an SREJ, but
6473                          * some expected retransmitted frames are
6474                          * missing.  Request retransmission of missing
6475                          * SREJ'd frames.
6476                          */
6477                         skb_queue_tail(&chan->srej_q, skb);
6478                         skb_in_use = true;
6479                         BT_DBG("Queued %p (queue len %d)", skb,
6480                                skb_queue_len(&chan->srej_q));
6481
6482                         l2cap_pass_to_tx(chan, control);
6483                         l2cap_send_srej_list(chan, control->txseq);
6484                         break;
6485                 case L2CAP_TXSEQ_DUPLICATE_SREJ:
6486                         /* We've already queued this frame.  Drop this copy. */
6487                         l2cap_pass_to_tx(chan, control);
6488                         break;
6489                 case L2CAP_TXSEQ_DUPLICATE:
6490                         /* Expecting a later sequence number, so this frame
6491                          * was already received.  Ignore it completely.
6492                          */
6493                         break;
6494                 case L2CAP_TXSEQ_INVALID_IGNORE:
6495                         break;
6496                 case L2CAP_TXSEQ_INVALID:
6497                 default:
6498                         l2cap_send_disconn_req(chan, ECONNRESET);
6499                         break;
6500                 }
6501                 break;
6502         case L2CAP_EV_RECV_RR:
6503                 l2cap_pass_to_tx(chan, control);
6504                 if (control->final) {
6505                         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6506
6507                         if (!test_and_clear_bit(CONN_REJ_ACT,
6508                                                 &chan->conn_state)) {
6509                                 control->final = 0;
6510                                 l2cap_retransmit_all(chan, control);
6511                         }
6512
6513                         l2cap_ertm_send(chan);
6514                 } else if (control->poll) {
6515                         if (test_and_clear_bit(CONN_REMOTE_BUSY,
6516                                                &chan->conn_state) &&
6517                             chan->unacked_frames) {
6518                                 __set_retrans_timer(chan);
6519                         }
6520
6521                         set_bit(CONN_SEND_FBIT, &chan->conn_state);
6522                         l2cap_send_srej_tail(chan);
6523                 } else {
6524                         if (test_and_clear_bit(CONN_REMOTE_BUSY,
6525                                                &chan->conn_state) &&
6526                             chan->unacked_frames)
6527                                 __set_retrans_timer(chan);
6528
6529                         l2cap_send_ack(chan);
6530                 }
6531                 break;
6532         case L2CAP_EV_RECV_RNR:
6533                 set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6534                 l2cap_pass_to_tx(chan, control);
6535                 if (control->poll) {
6536                         l2cap_send_srej_tail(chan);
6537                 } else {
6538                         struct l2cap_ctrl rr_control;
6539                         memset(&rr_control, 0, sizeof(rr_control));
6540                         rr_control.sframe = 1;
6541                         rr_control.super = L2CAP_SUPER_RR;
6542                         rr_control.reqseq = chan->buffer_seq;
6543                         l2cap_send_sframe(chan, &rr_control);
6544                 }
6545
6546                 break;
6547         case L2CAP_EV_RECV_REJ:
6548                 l2cap_handle_rej(chan, control);
6549                 break;
6550         case L2CAP_EV_RECV_SREJ:
6551                 l2cap_handle_srej(chan, control);
6552                 break;
6553         }
6554
6555         if (skb && !skb_in_use) {
6556                 BT_DBG("Freeing %p", skb);
6557                 kfree_skb(skb);
6558         }
6559
6560         return err;
6561 }
6562
6563 static int l2cap_finish_move(struct l2cap_chan *chan)
6564 {
6565         BT_DBG("chan %p", chan);
6566
6567         chan->rx_state = L2CAP_RX_STATE_RECV;
6568
6569         if (chan->hs_hcon)
6570                 chan->conn->mtu = chan->hs_hcon->hdev->block_mtu;
6571         else
6572                 chan->conn->mtu = chan->conn->hcon->hdev->acl_mtu;
6573
6574         return l2cap_resegment(chan);
6575 }
6576
6577 static int l2cap_rx_state_wait_p(struct l2cap_chan *chan,
6578                                  struct l2cap_ctrl *control,
6579                                  struct sk_buff *skb, u8 event)
6580 {
6581         int err;
6582
6583         BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
6584                event);
6585
6586         if (!control->poll)
6587                 return -EPROTO;
6588
6589         l2cap_process_reqseq(chan, control->reqseq);
6590
6591         if (!skb_queue_empty(&chan->tx_q))
6592                 chan->tx_send_head = skb_peek(&chan->tx_q);
6593         else
6594                 chan->tx_send_head = NULL;
6595
6596         /* Rewind next_tx_seq to the point expected
6597          * by the receiver.
6598          */
6599         chan->next_tx_seq = control->reqseq;
6600         chan->unacked_frames = 0;
6601
6602         err = l2cap_finish_move(chan);
6603         if (err)
6604                 return err;
6605
6606         set_bit(CONN_SEND_FBIT, &chan->conn_state);
6607         l2cap_send_i_or_rr_or_rnr(chan);
6608
6609         if (event == L2CAP_EV_RECV_IFRAME)
6610                 return -EPROTO;
6611
6612         return l2cap_rx_state_recv(chan, control, NULL, event);
6613 }
6614
6615 static int l2cap_rx_state_wait_f(struct l2cap_chan *chan,
6616                                  struct l2cap_ctrl *control,
6617                                  struct sk_buff *skb, u8 event)
6618 {
6619         int err;
6620
6621         if (!control->final)
6622                 return -EPROTO;
6623
6624         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6625
6626         chan->rx_state = L2CAP_RX_STATE_RECV;
6627         l2cap_process_reqseq(chan, control->reqseq);
6628
6629         if (!skb_queue_empty(&chan->tx_q))
6630                 chan->tx_send_head = skb_peek(&chan->tx_q);
6631         else
6632                 chan->tx_send_head = NULL;
6633
6634         /* Rewind next_tx_seq to the point expected
6635          * by the receiver.
6636          */
6637         chan->next_tx_seq = control->reqseq;
6638         chan->unacked_frames = 0;
6639
6640         if (chan->hs_hcon)
6641                 chan->conn->mtu = chan->hs_hcon->hdev->block_mtu;
6642         else
6643                 chan->conn->mtu = chan->conn->hcon->hdev->acl_mtu;
6644
6645         err = l2cap_resegment(chan);
6646
6647         if (!err)
6648                 err = l2cap_rx_state_recv(chan, control, skb, event);
6649
6650         return err;
6651 }
6652
6653 static bool __valid_reqseq(struct l2cap_chan *chan, u16 reqseq)
6654 {
6655         /* Make sure reqseq is for a packet that has been sent but not acked */
6656         u16 unacked;
6657
6658         unacked = __seq_offset(chan, chan->next_tx_seq, chan->expected_ack_seq);
6659         return __seq_offset(chan, chan->next_tx_seq, reqseq) <= unacked;
6660 }
6661
6662 static int l2cap_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
6663                     struct sk_buff *skb, u8 event)
6664 {
6665         int err = 0;
6666
6667         BT_DBG("chan %p, control %p, skb %p, event %d, state %d", chan,
6668                control, skb, event, chan->rx_state);
6669
6670         if (__valid_reqseq(chan, control->reqseq)) {
6671                 switch (chan->rx_state) {
6672                 case L2CAP_RX_STATE_RECV:
6673                         err = l2cap_rx_state_recv(chan, control, skb, event);
6674                         break;
6675                 case L2CAP_RX_STATE_SREJ_SENT:
6676                         err = l2cap_rx_state_srej_sent(chan, control, skb,
6677                                                        event);
6678                         break;
6679                 case L2CAP_RX_STATE_WAIT_P:
6680                         err = l2cap_rx_state_wait_p(chan, control, skb, event);
6681                         break;
6682                 case L2CAP_RX_STATE_WAIT_F:
6683                         err = l2cap_rx_state_wait_f(chan, control, skb, event);
6684                         break;
6685                 default:
6686                         /* shut it down */
6687                         break;
6688                 }
6689         } else {
6690                 BT_DBG("Invalid reqseq %d (next_tx_seq %d, expected_ack_seq %d",
6691                        control->reqseq, chan->next_tx_seq,
6692                        chan->expected_ack_seq);
6693                 l2cap_send_disconn_req(chan, ECONNRESET);
6694         }
6695
6696         return err;
6697 }
6698
6699 static int l2cap_stream_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
6700                            struct sk_buff *skb)
6701 {
6702         int err = 0;
6703
6704         BT_DBG("chan %p, control %p, skb %p, state %d", chan, control, skb,
6705                chan->rx_state);
6706
6707         if (l2cap_classify_txseq(chan, control->txseq) ==
6708             L2CAP_TXSEQ_EXPECTED) {
6709                 l2cap_pass_to_tx(chan, control);
6710
6711                 BT_DBG("buffer_seq %d->%d", chan->buffer_seq,
6712                        __next_seq(chan, chan->buffer_seq));
6713
6714                 chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
6715
6716                 l2cap_reassemble_sdu(chan, skb, control);
6717         } else {
6718                 if (chan->sdu) {
6719                         kfree_skb(chan->sdu);
6720                         chan->sdu = NULL;
6721                 }
6722                 chan->sdu_last_frag = NULL;
6723                 chan->sdu_len = 0;
6724
6725                 if (skb) {
6726                         BT_DBG("Freeing %p", skb);
6727                         kfree_skb(skb);
6728                 }
6729         }
6730
6731         chan->last_acked_seq = control->txseq;
6732         chan->expected_tx_seq = __next_seq(chan, control->txseq);
6733
6734         return err;
6735 }
6736
6737 static int l2cap_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb)
6738 {
6739         struct l2cap_ctrl *control = &bt_cb(skb)->control;
6740         u16 len;
6741         u8 event;
6742
6743         __unpack_control(chan, skb);
6744
6745         len = skb->len;
6746
6747         /*
6748          * We can just drop the corrupted I-frame here.
6749          * Receiver will miss it and start proper recovery
6750          * procedures and ask for retransmission.
6751          */
6752         if (l2cap_check_fcs(chan, skb))
6753                 goto drop;
6754
6755         if (!control->sframe && control->sar == L2CAP_SAR_START)
6756                 len -= L2CAP_SDULEN_SIZE;
6757
6758         if (chan->fcs == L2CAP_FCS_CRC16)
6759                 len -= L2CAP_FCS_SIZE;
6760
6761         if (len > chan->mps) {
6762                 l2cap_send_disconn_req(chan, ECONNRESET);
6763                 goto drop;
6764         }
6765
6766         if (!control->sframe) {
6767                 int err;
6768
6769                 BT_DBG("iframe sar %d, reqseq %d, final %d, txseq %d",
6770                        control->sar, control->reqseq, control->final,
6771                        control->txseq);
6772
6773                 /* Validate F-bit - F=0 always valid, F=1 only
6774                  * valid in TX WAIT_F
6775                  */
6776                 if (control->final && chan->tx_state != L2CAP_TX_STATE_WAIT_F)
6777                         goto drop;
6778
6779                 if (chan->mode != L2CAP_MODE_STREAMING) {
6780                         event = L2CAP_EV_RECV_IFRAME;
6781                         err = l2cap_rx(chan, control, skb, event);
6782                 } else {
6783                         err = l2cap_stream_rx(chan, control, skb);
6784                 }
6785
6786                 if (err)
6787                         l2cap_send_disconn_req(chan, ECONNRESET);
6788         } else {
6789                 const u8 rx_func_to_event[4] = {
6790                         L2CAP_EV_RECV_RR, L2CAP_EV_RECV_REJ,
6791                         L2CAP_EV_RECV_RNR, L2CAP_EV_RECV_SREJ
6792                 };
6793
6794                 /* Only I-frames are expected in streaming mode */
6795                 if (chan->mode == L2CAP_MODE_STREAMING)
6796                         goto drop;
6797
6798                 BT_DBG("sframe reqseq %d, final %d, poll %d, super %d",
6799                        control->reqseq, control->final, control->poll,
6800                        control->super);
6801
6802                 if (len != 0) {
6803                         BT_ERR("Trailing bytes: %d in sframe", len);
6804                         l2cap_send_disconn_req(chan, ECONNRESET);
6805                         goto drop;
6806                 }
6807
6808                 /* Validate F and P bits */
6809                 if (control->final && (control->poll ||
6810                                        chan->tx_state != L2CAP_TX_STATE_WAIT_F))
6811                         goto drop;
6812
6813                 event = rx_func_to_event[control->super];
6814                 if (l2cap_rx(chan, control, skb, event))
6815                         l2cap_send_disconn_req(chan, ECONNRESET);
6816         }
6817
6818         return 0;
6819
6820 drop:
6821         kfree_skb(skb);
6822         return 0;
6823 }
6824
6825 static void l2cap_chan_le_send_credits(struct l2cap_chan *chan)
6826 {
6827         struct l2cap_conn *conn = chan->conn;
6828         struct l2cap_le_credits pkt;
6829         u16 return_credits;
6830
6831         /* We return more credits to the sender only after the amount of
6832          * credits falls below half of the initial amount.
6833          */
6834         if (chan->rx_credits >= (L2CAP_LE_MAX_CREDITS + 1) / 2)
6835                 return;
6836
6837         return_credits = L2CAP_LE_MAX_CREDITS - chan->rx_credits;
6838
6839         BT_DBG("chan %p returning %u credits to sender", chan, return_credits);
6840
6841         chan->rx_credits += return_credits;
6842
6843         pkt.cid     = cpu_to_le16(chan->scid);
6844         pkt.credits = cpu_to_le16(return_credits);
6845
6846         chan->ident = l2cap_get_ident(conn);
6847
6848         l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CREDITS, sizeof(pkt), &pkt);
6849 }
6850
6851 static int l2cap_le_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb)
6852 {
6853         int err;
6854
6855         if (!chan->rx_credits) {
6856                 BT_ERR("No credits to receive LE L2CAP data");
6857                 return -ENOBUFS;
6858         }
6859
6860         if (chan->imtu < skb->len) {
6861                 BT_ERR("Too big LE L2CAP PDU");
6862                 return -ENOBUFS;
6863         }
6864
6865         chan->rx_credits--;
6866         BT_DBG("rx_credits %u -> %u", chan->rx_credits + 1, chan->rx_credits);
6867
6868         l2cap_chan_le_send_credits(chan);
6869
6870         err = 0;
6871
6872         if (!chan->sdu) {
6873                 u16 sdu_len;
6874
6875                 sdu_len = get_unaligned_le16(skb->data);
6876                 skb_pull(skb, L2CAP_SDULEN_SIZE);
6877
6878                 BT_DBG("Start of new SDU. sdu_len %u skb->len %u imtu %u",
6879                        sdu_len, skb->len, chan->imtu);
6880
6881                 if (sdu_len > chan->imtu) {
6882                         BT_ERR("Too big LE L2CAP SDU length received");
6883                         err = -EMSGSIZE;
6884                         goto failed;
6885                 }
6886
6887                 if (skb->len > sdu_len) {
6888                         BT_ERR("Too much LE L2CAP data received");
6889                         err = -EINVAL;
6890                         goto failed;
6891                 }
6892
6893                 if (skb->len == sdu_len)
6894                         return chan->ops->recv(chan, skb);
6895
6896                 chan->sdu = skb;
6897                 chan->sdu_len = sdu_len;
6898                 chan->sdu_last_frag = skb;
6899
6900                 return 0;
6901         }
6902
6903         BT_DBG("SDU fragment. chan->sdu->len %u skb->len %u chan->sdu_len %u",
6904                chan->sdu->len, skb->len, chan->sdu_len);
6905
6906         if (chan->sdu->len + skb->len > chan->sdu_len) {
6907                 BT_ERR("Too much LE L2CAP data received");
6908                 err = -EINVAL;
6909                 goto failed;
6910         }
6911
6912         append_skb_frag(chan->sdu, skb, &chan->sdu_last_frag);
6913         skb = NULL;
6914
6915         if (chan->sdu->len == chan->sdu_len) {
6916                 err = chan->ops->recv(chan, chan->sdu);
6917                 if (!err) {
6918                         chan->sdu = NULL;
6919                         chan->sdu_last_frag = NULL;
6920                         chan->sdu_len = 0;
6921                 }
6922         }
6923
6924 failed:
6925         if (err) {
6926                 kfree_skb(skb);
6927                 kfree_skb(chan->sdu);
6928                 chan->sdu = NULL;
6929                 chan->sdu_last_frag = NULL;
6930                 chan->sdu_len = 0;
6931         }
6932
6933         /* We can't return an error here since we took care of the skb
6934          * freeing internally. An error return would cause the caller to
6935          * do a double-free of the skb.
6936          */
6937         return 0;
6938 }
6939
6940 static void l2cap_data_channel(struct l2cap_conn *conn, u16 cid,
6941                                struct sk_buff *skb)
6942 {
6943         struct l2cap_chan *chan;
6944
6945         chan = l2cap_get_chan_by_scid(conn, cid);
6946         if (!chan) {
6947                 if (cid == L2CAP_CID_A2MP) {
6948                         chan = a2mp_channel_create(conn, skb);
6949                         if (!chan) {
6950                                 kfree_skb(skb);
6951                                 return;
6952                         }
6953
6954                         l2cap_chan_lock(chan);
6955                 } else {
6956                         BT_DBG("unknown cid 0x%4.4x", cid);
6957                         /* Drop packet and return */
6958                         kfree_skb(skb);
6959                         return;
6960                 }
6961         }
6962
6963         BT_DBG("chan %p, len %d", chan, skb->len);
6964
6965         if (chan->state != BT_CONNECTED)
6966                 goto drop;
6967
6968         switch (chan->mode) {
6969         case L2CAP_MODE_LE_FLOWCTL:
6970                 if (l2cap_le_data_rcv(chan, skb) < 0)
6971                         goto drop;
6972
6973                 goto done;
6974
6975         case L2CAP_MODE_BASIC:
6976                 /* If socket recv buffers overflows we drop data here
6977                  * which is *bad* because L2CAP has to be reliable.
6978                  * But we don't have any other choice. L2CAP doesn't
6979                  * provide flow control mechanism. */
6980
6981                 if (chan->imtu < skb->len)
6982                         goto drop;
6983
6984                 if (!chan->ops->recv(chan, skb))
6985                         goto done;
6986                 break;
6987
6988         case L2CAP_MODE_ERTM:
6989         case L2CAP_MODE_STREAMING:
6990                 l2cap_data_rcv(chan, skb);
6991                 goto done;
6992
6993         default:
6994                 BT_DBG("chan %p: bad mode 0x%2.2x", chan, chan->mode);
6995                 break;
6996         }
6997
6998 drop:
6999         kfree_skb(skb);
7000
7001 done:
7002         l2cap_chan_unlock(chan);
7003 }
7004
7005 static void l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm,
7006                                   struct sk_buff *skb)
7007 {
7008         struct hci_conn *hcon = conn->hcon;
7009         struct l2cap_chan *chan;
7010
7011         if (hcon->type != ACL_LINK)
7012                 goto drop;
7013
7014         chan = l2cap_global_chan_by_psm(0, psm, &hcon->src, &hcon->dst,
7015                                         ACL_LINK);
7016         if (!chan)
7017                 goto drop;
7018
7019         BT_DBG("chan %p, len %d", chan, skb->len);
7020
7021         if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
7022                 goto drop;
7023
7024         if (chan->imtu < skb->len)
7025                 goto drop;
7026
7027         /* Store remote BD_ADDR and PSM for msg_name */
7028         bacpy(&bt_cb(skb)->bdaddr, &hcon->dst);
7029         bt_cb(skb)->psm = psm;
7030
7031         if (!chan->ops->recv(chan, skb))
7032                 return;
7033
7034 drop:
7035         kfree_skb(skb);
7036 }
7037
7038 static void l2cap_att_channel(struct l2cap_conn *conn,
7039                               struct sk_buff *skb)
7040 {
7041         struct hci_conn *hcon = conn->hcon;
7042         struct l2cap_chan *chan;
7043
7044         if (hcon->type != LE_LINK)
7045                 goto drop;
7046
7047         chan = l2cap_global_chan_by_scid(BT_CONNECTED, L2CAP_CID_ATT,
7048                                          &hcon->src, &hcon->dst);
7049         if (!chan)
7050                 goto drop;
7051
7052         BT_DBG("chan %p, len %d", chan, skb->len);
7053
7054         if (hci_blacklist_lookup(hcon->hdev, &hcon->dst, hcon->dst_type))
7055                 goto drop;
7056
7057         if (chan->imtu < skb->len)
7058                 goto drop;
7059
7060         if (!chan->ops->recv(chan, skb))
7061                 return;
7062
7063 drop:
7064         kfree_skb(skb);
7065 }
7066
7067 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
7068 {
7069         struct l2cap_hdr *lh = (void *) skb->data;
7070         u16 cid, len;
7071         __le16 psm;
7072
7073         skb_pull(skb, L2CAP_HDR_SIZE);
7074         cid = __le16_to_cpu(lh->cid);
7075         len = __le16_to_cpu(lh->len);
7076
7077         if (len != skb->len) {
7078                 kfree_skb(skb);
7079                 return;
7080         }
7081
7082         BT_DBG("len %d, cid 0x%4.4x", len, cid);
7083
7084         switch (cid) {
7085         case L2CAP_CID_SIGNALING:
7086                 l2cap_sig_channel(conn, skb);
7087                 break;
7088
7089         case L2CAP_CID_CONN_LESS:
7090                 psm = get_unaligned((__le16 *) skb->data);
7091                 skb_pull(skb, L2CAP_PSMLEN_SIZE);
7092                 l2cap_conless_channel(conn, psm, skb);
7093                 break;
7094
7095         case L2CAP_CID_ATT:
7096                 l2cap_att_channel(conn, skb);
7097                 break;
7098
7099         case L2CAP_CID_LE_SIGNALING:
7100                 l2cap_le_sig_channel(conn, skb);
7101                 break;
7102
7103         case L2CAP_CID_SMP:
7104                 if (smp_sig_channel(conn, skb))
7105                         l2cap_conn_del(conn->hcon, EACCES);
7106                 break;
7107
7108         default:
7109                 l2cap_data_channel(conn, cid, skb);
7110                 break;
7111         }
7112 }
7113
7114 /* ---- L2CAP interface with lower layer (HCI) ---- */
7115
7116 int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr)
7117 {
7118         int exact = 0, lm1 = 0, lm2 = 0;
7119         struct l2cap_chan *c;
7120
7121         BT_DBG("hdev %s, bdaddr %pMR", hdev->name, bdaddr);
7122
7123         /* Find listening sockets and check their link_mode */
7124         read_lock(&chan_list_lock);
7125         list_for_each_entry(c, &chan_list, global_l) {
7126                 if (c->state != BT_LISTEN)
7127                         continue;
7128
7129                 if (!bacmp(&c->src, &hdev->bdaddr)) {
7130                         lm1 |= HCI_LM_ACCEPT;
7131                         if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
7132                                 lm1 |= HCI_LM_MASTER;
7133                         exact++;
7134                 } else if (!bacmp(&c->src, BDADDR_ANY)) {
7135                         lm2 |= HCI_LM_ACCEPT;
7136                         if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
7137                                 lm2 |= HCI_LM_MASTER;
7138                 }
7139         }
7140         read_unlock(&chan_list_lock);
7141
7142         return exact ? lm1 : lm2;
7143 }
7144
7145 void l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
7146 {
7147         struct l2cap_conn *conn;
7148
7149         BT_DBG("hcon %p bdaddr %pMR status %d", hcon, &hcon->dst, status);
7150
7151         if (!status) {
7152                 conn = l2cap_conn_add(hcon);
7153                 if (conn)
7154                         l2cap_conn_ready(conn);
7155         } else {
7156                 l2cap_conn_del(hcon, bt_to_errno(status));
7157         }
7158 }
7159
7160 int l2cap_disconn_ind(struct hci_conn *hcon)
7161 {
7162         struct l2cap_conn *conn = hcon->l2cap_data;
7163
7164         BT_DBG("hcon %p", hcon);
7165
7166         if (!conn)
7167                 return HCI_ERROR_REMOTE_USER_TERM;
7168         return conn->disc_reason;
7169 }
7170
7171 void l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
7172 {
7173         BT_DBG("hcon %p reason %d", hcon, reason);
7174
7175         l2cap_conn_del(hcon, bt_to_errno(reason));
7176 }
7177
7178 static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt)
7179 {
7180         if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED)
7181                 return;
7182
7183         if (encrypt == 0x00) {
7184                 if (chan->sec_level == BT_SECURITY_MEDIUM) {
7185                         __set_chan_timer(chan, L2CAP_ENC_TIMEOUT);
7186                 } else if (chan->sec_level == BT_SECURITY_HIGH)
7187                         l2cap_chan_close(chan, ECONNREFUSED);
7188         } else {
7189                 if (chan->sec_level == BT_SECURITY_MEDIUM)
7190                         __clear_chan_timer(chan);
7191         }
7192 }
7193
7194 int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
7195 {
7196         struct l2cap_conn *conn = hcon->l2cap_data;
7197         struct l2cap_chan *chan;
7198
7199         if (!conn)
7200                 return 0;
7201
7202         BT_DBG("conn %p status 0x%2.2x encrypt %u", conn, status, encrypt);
7203
7204         if (hcon->type == LE_LINK) {
7205                 if (!status && encrypt)
7206                         smp_distribute_keys(conn, 0);
7207                 cancel_delayed_work(&conn->security_timer);
7208         }
7209
7210         mutex_lock(&conn->chan_lock);
7211
7212         list_for_each_entry(chan, &conn->chan_l, list) {
7213                 l2cap_chan_lock(chan);
7214
7215                 BT_DBG("chan %p scid 0x%4.4x state %s", chan, chan->scid,
7216                        state_to_string(chan->state));
7217
7218                 if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP) {
7219                         l2cap_chan_unlock(chan);
7220                         continue;
7221                 }
7222
7223                 if (chan->scid == L2CAP_CID_ATT) {
7224                         if (!status && encrypt) {
7225                                 chan->sec_level = hcon->sec_level;
7226                                 l2cap_chan_ready(chan);
7227                         }
7228
7229                         l2cap_chan_unlock(chan);
7230                         continue;
7231                 }
7232
7233                 if (!__l2cap_no_conn_pending(chan)) {
7234                         l2cap_chan_unlock(chan);
7235                         continue;
7236                 }
7237
7238                 if (!status && (chan->state == BT_CONNECTED ||
7239                                 chan->state == BT_CONFIG)) {
7240                         chan->ops->resume(chan);
7241                         l2cap_check_encryption(chan, encrypt);
7242                         l2cap_chan_unlock(chan);
7243                         continue;
7244                 }
7245
7246                 if (chan->state == BT_CONNECT) {
7247                         if (!status)
7248                                 l2cap_start_connection(chan);
7249                         else
7250                                 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
7251                 } else if (chan->state == BT_CONNECT2) {
7252                         struct l2cap_conn_rsp rsp;
7253                         __u16 res, stat;
7254
7255                         if (!status) {
7256                                 if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
7257                                         res = L2CAP_CR_PEND;
7258                                         stat = L2CAP_CS_AUTHOR_PEND;
7259                                         chan->ops->defer(chan);
7260                                 } else {
7261                                         l2cap_state_change(chan, BT_CONFIG);
7262                                         res = L2CAP_CR_SUCCESS;
7263                                         stat = L2CAP_CS_NO_INFO;
7264                                 }
7265                         } else {
7266                                 l2cap_state_change(chan, BT_DISCONN);
7267                                 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
7268                                 res = L2CAP_CR_SEC_BLOCK;
7269                                 stat = L2CAP_CS_NO_INFO;
7270                         }
7271
7272                         rsp.scid   = cpu_to_le16(chan->dcid);
7273                         rsp.dcid   = cpu_to_le16(chan->scid);
7274                         rsp.result = cpu_to_le16(res);
7275                         rsp.status = cpu_to_le16(stat);
7276                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
7277                                        sizeof(rsp), &rsp);
7278
7279                         if (!test_bit(CONF_REQ_SENT, &chan->conf_state) &&
7280                             res == L2CAP_CR_SUCCESS) {
7281                                 char buf[128];
7282                                 set_bit(CONF_REQ_SENT, &chan->conf_state);
7283                                 l2cap_send_cmd(conn, l2cap_get_ident(conn),
7284                                                L2CAP_CONF_REQ,
7285                                                l2cap_build_conf_req(chan, buf),
7286                                                buf);
7287                                 chan->num_conf_req++;
7288                         }
7289                 }
7290
7291                 l2cap_chan_unlock(chan);
7292         }
7293
7294         mutex_unlock(&conn->chan_lock);
7295
7296         return 0;
7297 }
7298
7299 int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
7300 {
7301         struct l2cap_conn *conn = hcon->l2cap_data;
7302         struct l2cap_hdr *hdr;
7303         int len;
7304
7305         /* For AMP controller do not create l2cap conn */
7306         if (!conn && hcon->hdev->dev_type != HCI_BREDR)
7307                 goto drop;
7308
7309         if (!conn)
7310                 conn = l2cap_conn_add(hcon);
7311
7312         if (!conn)
7313                 goto drop;
7314
7315         BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
7316
7317         switch (flags) {
7318         case ACL_START:
7319         case ACL_START_NO_FLUSH:
7320         case ACL_COMPLETE:
7321                 if (conn->rx_len) {
7322                         BT_ERR("Unexpected start frame (len %d)", skb->len);
7323                         kfree_skb(conn->rx_skb);
7324                         conn->rx_skb = NULL;
7325                         conn->rx_len = 0;
7326                         l2cap_conn_unreliable(conn, ECOMM);
7327                 }
7328
7329                 /* Start fragment always begin with Basic L2CAP header */
7330                 if (skb->len < L2CAP_HDR_SIZE) {
7331                         BT_ERR("Frame is too short (len %d)", skb->len);
7332                         l2cap_conn_unreliable(conn, ECOMM);
7333                         goto drop;
7334                 }
7335
7336                 hdr = (struct l2cap_hdr *) skb->data;
7337                 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
7338
7339                 if (len == skb->len) {
7340                         /* Complete frame received */
7341                         l2cap_recv_frame(conn, skb);
7342                         return 0;
7343                 }
7344
7345                 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
7346
7347                 if (skb->len > len) {
7348                         BT_ERR("Frame is too long (len %d, expected len %d)",
7349                                skb->len, len);
7350                         l2cap_conn_unreliable(conn, ECOMM);
7351                         goto drop;
7352                 }
7353
7354                 /* Allocate skb for the complete frame (with header) */
7355                 conn->rx_skb = bt_skb_alloc(len, GFP_KERNEL);
7356                 if (!conn->rx_skb)
7357                         goto drop;
7358
7359                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
7360                                           skb->len);
7361                 conn->rx_len = len - skb->len;
7362                 break;
7363
7364         case ACL_CONT:
7365                 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
7366
7367                 if (!conn->rx_len) {
7368                         BT_ERR("Unexpected continuation frame (len %d)", skb->len);
7369                         l2cap_conn_unreliable(conn, ECOMM);
7370                         goto drop;
7371                 }
7372
7373                 if (skb->len > conn->rx_len) {
7374                         BT_ERR("Fragment is too long (len %d, expected %d)",
7375                                skb->len, conn->rx_len);
7376                         kfree_skb(conn->rx_skb);
7377                         conn->rx_skb = NULL;
7378                         conn->rx_len = 0;
7379                         l2cap_conn_unreliable(conn, ECOMM);
7380                         goto drop;
7381                 }
7382
7383                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
7384                                           skb->len);
7385                 conn->rx_len -= skb->len;
7386
7387                 if (!conn->rx_len) {
7388                         /* Complete frame received. l2cap_recv_frame
7389                          * takes ownership of the skb so set the global
7390                          * rx_skb pointer to NULL first.
7391                          */
7392                         struct sk_buff *rx_skb = conn->rx_skb;
7393                         conn->rx_skb = NULL;
7394                         l2cap_recv_frame(conn, rx_skb);
7395                 }
7396                 break;
7397         }
7398
7399 drop:
7400         kfree_skb(skb);
7401         return 0;
7402 }
7403
7404 static int l2cap_debugfs_show(struct seq_file *f, void *p)
7405 {
7406         struct l2cap_chan *c;
7407
7408         read_lock(&chan_list_lock);
7409
7410         list_for_each_entry(c, &chan_list, global_l) {
7411                 seq_printf(f, "%pMR %pMR %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
7412                            &c->src, &c->dst,
7413                            c->state, __le16_to_cpu(c->psm),
7414                            c->scid, c->dcid, c->imtu, c->omtu,
7415                            c->sec_level, c->mode);
7416         }
7417
7418         read_unlock(&chan_list_lock);
7419
7420         return 0;
7421 }
7422
7423 static int l2cap_debugfs_open(struct inode *inode, struct file *file)
7424 {
7425         return single_open(file, l2cap_debugfs_show, inode->i_private);
7426 }
7427
7428 static const struct file_operations l2cap_debugfs_fops = {
7429         .open           = l2cap_debugfs_open,
7430         .read           = seq_read,
7431         .llseek         = seq_lseek,
7432         .release        = single_release,
7433 };
7434
7435 static struct dentry *l2cap_debugfs;
7436
7437 int __init l2cap_init(void)
7438 {
7439         int err;
7440
7441         err = l2cap_init_sockets();
7442         if (err < 0)
7443                 return err;
7444
7445         if (IS_ERR_OR_NULL(bt_debugfs))
7446                 return 0;
7447
7448         l2cap_debugfs = debugfs_create_file("l2cap", 0444, bt_debugfs,
7449                                             NULL, &l2cap_debugfs_fops);
7450
7451         return 0;
7452 }
7453
7454 void l2cap_exit(void)
7455 {
7456         debugfs_remove(l2cap_debugfs);
7457         l2cap_cleanup_sockets();
7458 }
7459
7460 module_param(disable_ertm, bool, 0644);
7461 MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode");