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