net/bluetooth/l2cap_core.c

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