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