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