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