net/bluetooth/l2cap_core.c

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