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