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 = __constant_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 == __constant_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 == __constant_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 = __constant_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 = __constant_cpu_to_le16(L2CAP_CR_PEND);
1374                                         rsp.status = __constant_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 = __constant_cpu_to_le16(L2CAP_CR_SUCCESS);
1380                                         rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
1381                                 }
1382                         } else {
1383                                 rsp.result = __constant_cpu_to_le16(L2CAP_CR_PEND);
1384                                 rsp.status = __constant_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                 l2cap_do_send(chan, skb);
2438                 return len;
2439         }
2440
2441         switch (chan->mode) {
2442         case L2CAP_MODE_LE_FLOWCTL:
2443                 /* Check outgoing MTU */
2444                 if (len > chan->omtu)
2445                         return -EMSGSIZE;
2446
2447                 if (!chan->tx_credits)
2448                         return -EAGAIN;
2449
2450                 __skb_queue_head_init(&seg_queue);
2451
2452                 err = l2cap_segment_le_sdu(chan, &seg_queue, msg, len);
2453
2454                 if (chan->state != BT_CONNECTED) {
2455                         __skb_queue_purge(&seg_queue);
2456                         err = -ENOTCONN;
2457                 }
2458
2459                 if (err)
2460                         return err;
2461
2462                 skb_queue_splice_tail_init(&seg_queue, &chan->tx_q);
2463
2464                 while (chan->tx_credits && !skb_queue_empty(&chan->tx_q)) {
2465                         l2cap_do_send(chan, skb_dequeue(&chan->tx_q));
2466                         chan->tx_credits--;
2467                 }
2468
2469                 if (!chan->tx_credits)
2470                         chan->ops->suspend(chan);
2471
2472                 err = len;
2473
2474                 break;
2475
2476         case L2CAP_MODE_BASIC:
2477                 /* Check outgoing MTU */
2478                 if (len > chan->omtu)
2479                         return -EMSGSIZE;
2480
2481                 /* Create a basic PDU */
2482                 skb = l2cap_create_basic_pdu(chan, msg, len, priority);
2483                 if (IS_ERR(skb))
2484                         return PTR_ERR(skb);
2485
2486                 l2cap_do_send(chan, skb);
2487                 err = len;
2488                 break;
2489
2490         case L2CAP_MODE_ERTM:
2491         case L2CAP_MODE_STREAMING:
2492                 /* Check outgoing MTU */
2493                 if (len > chan->omtu) {
2494                         err = -EMSGSIZE;
2495                         break;
2496                 }
2497
2498                 __skb_queue_head_init(&seg_queue);
2499
2500                 /* Do segmentation before calling in to the state machine,
2501                  * since it's possible to block while waiting for memory
2502                  * allocation.
2503                  */
2504                 err = l2cap_segment_sdu(chan, &seg_queue, msg, len);
2505
2506                 /* The channel could have been closed while segmenting,
2507                  * check that it is still connected.
2508                  */
2509                 if (chan->state != BT_CONNECTED) {
2510                         __skb_queue_purge(&seg_queue);
2511                         err = -ENOTCONN;
2512                 }
2513
2514                 if (err)
2515                         break;
2516
2517                 if (chan->mode == L2CAP_MODE_ERTM)
2518                         l2cap_tx(chan, NULL, &seg_queue, L2CAP_EV_DATA_REQUEST);
2519                 else
2520                         l2cap_streaming_send(chan, &seg_queue);
2521
2522                 err = len;
2523
2524                 /* If the skbs were not queued for sending, they'll still be in
2525                  * seg_queue and need to be purged.
2526                  */
2527                 __skb_queue_purge(&seg_queue);
2528                 break;
2529
2530         default:
2531                 BT_DBG("bad state %1.1x", chan->mode);
2532                 err = -EBADFD;
2533         }
2534
2535         return err;
2536 }
2537
2538 static void l2cap_send_srej(struct l2cap_chan *chan, u16 txseq)
2539 {
2540         struct l2cap_ctrl control;
2541         u16 seq;
2542
2543         BT_DBG("chan %p, txseq %u", chan, txseq);
2544
2545         memset(&control, 0, sizeof(control));
2546         control.sframe = 1;
2547         control.super = L2CAP_SUPER_SREJ;
2548
2549         for (seq = chan->expected_tx_seq; seq != txseq;
2550              seq = __next_seq(chan, seq)) {
2551                 if (!l2cap_ertm_seq_in_queue(&chan->srej_q, seq)) {
2552                         control.reqseq = seq;
2553                         l2cap_send_sframe(chan, &control);
2554                         l2cap_seq_list_append(&chan->srej_list, seq);
2555                 }
2556         }
2557
2558         chan->expected_tx_seq = __next_seq(chan, txseq);
2559 }
2560
2561 static void l2cap_send_srej_tail(struct l2cap_chan *chan)
2562 {
2563         struct l2cap_ctrl control;
2564
2565         BT_DBG("chan %p", chan);
2566
2567         if (chan->srej_list.tail == L2CAP_SEQ_LIST_CLEAR)
2568                 return;
2569
2570         memset(&control, 0, sizeof(control));
2571         control.sframe = 1;
2572         control.super = L2CAP_SUPER_SREJ;
2573         control.reqseq = chan->srej_list.tail;
2574         l2cap_send_sframe(chan, &control);
2575 }
2576
2577 static void l2cap_send_srej_list(struct l2cap_chan *chan, u16 txseq)
2578 {
2579         struct l2cap_ctrl control;
2580         u16 initial_head;
2581         u16 seq;
2582
2583         BT_DBG("chan %p, txseq %u", chan, txseq);
2584
2585         memset(&control, 0, sizeof(control));
2586         control.sframe = 1;
2587         control.super = L2CAP_SUPER_SREJ;
2588
2589         /* Capture initial list head to allow only one pass through the list. */
2590         initial_head = chan->srej_list.head;
2591
2592         do {
2593                 seq = l2cap_seq_list_pop(&chan->srej_list);
2594                 if (seq == txseq || seq == L2CAP_SEQ_LIST_CLEAR)
2595                         break;
2596
2597                 control.reqseq = seq;
2598                 l2cap_send_sframe(chan, &control);
2599                 l2cap_seq_list_append(&chan->srej_list, seq);
2600         } while (chan->srej_list.head != initial_head);
2601 }
2602
2603 static void l2cap_process_reqseq(struct l2cap_chan *chan, u16 reqseq)
2604 {
2605         struct sk_buff *acked_skb;
2606         u16 ackseq;
2607
2608         BT_DBG("chan %p, reqseq %u", chan, reqseq);
2609
2610         if (chan->unacked_frames == 0 || reqseq == chan->expected_ack_seq)
2611                 return;
2612
2613         BT_DBG("expected_ack_seq %u, unacked_frames %u",
2614                chan->expected_ack_seq, chan->unacked_frames);
2615
2616         for (ackseq = chan->expected_ack_seq; ackseq != reqseq;
2617              ackseq = __next_seq(chan, ackseq)) {
2618
2619                 acked_skb = l2cap_ertm_seq_in_queue(&chan->tx_q, ackseq);
2620                 if (acked_skb) {
2621                         skb_unlink(acked_skb, &chan->tx_q);
2622                         kfree_skb(acked_skb);
2623                         chan->unacked_frames--;
2624                 }
2625         }
2626
2627         chan->expected_ack_seq = reqseq;
2628
2629         if (chan->unacked_frames == 0)
2630                 __clear_retrans_timer(chan);
2631
2632         BT_DBG("unacked_frames %u", chan->unacked_frames);
2633 }
2634
2635 static void l2cap_abort_rx_srej_sent(struct l2cap_chan *chan)
2636 {
2637         BT_DBG("chan %p", chan);
2638
2639         chan->expected_tx_seq = chan->buffer_seq;
2640         l2cap_seq_list_clear(&chan->srej_list);
2641         skb_queue_purge(&chan->srej_q);
2642         chan->rx_state = L2CAP_RX_STATE_RECV;
2643 }
2644
2645 static void l2cap_tx_state_xmit(struct l2cap_chan *chan,
2646                                 struct l2cap_ctrl *control,
2647                                 struct sk_buff_head *skbs, u8 event)
2648 {
2649         BT_DBG("chan %p, control %p, skbs %p, event %d", chan, control, skbs,
2650                event);
2651
2652         switch (event) {
2653         case L2CAP_EV_DATA_REQUEST:
2654                 if (chan->tx_send_head == NULL)
2655                         chan->tx_send_head = skb_peek(skbs);
2656
2657                 skb_queue_splice_tail_init(skbs, &chan->tx_q);
2658                 l2cap_ertm_send(chan);
2659                 break;
2660         case L2CAP_EV_LOCAL_BUSY_DETECTED:
2661                 BT_DBG("Enter LOCAL_BUSY");
2662                 set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2663
2664                 if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
2665                         /* The SREJ_SENT state must be aborted if we are to
2666                          * enter the LOCAL_BUSY state.
2667                          */
2668                         l2cap_abort_rx_srej_sent(chan);
2669                 }
2670
2671                 l2cap_send_ack(chan);
2672
2673                 break;
2674         case L2CAP_EV_LOCAL_BUSY_CLEAR:
2675                 BT_DBG("Exit LOCAL_BUSY");
2676                 clear_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2677
2678                 if (test_bit(CONN_RNR_SENT, &chan->conn_state)) {
2679                         struct l2cap_ctrl local_control;
2680
2681                         memset(&local_control, 0, sizeof(local_control));
2682                         local_control.sframe = 1;
2683                         local_control.super = L2CAP_SUPER_RR;
2684                         local_control.poll = 1;
2685                         local_control.reqseq = chan->buffer_seq;
2686                         l2cap_send_sframe(chan, &local_control);
2687
2688                         chan->retry_count = 1;
2689                         __set_monitor_timer(chan);
2690                         chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2691                 }
2692                 break;
2693         case L2CAP_EV_RECV_REQSEQ_AND_FBIT:
2694                 l2cap_process_reqseq(chan, control->reqseq);
2695                 break;
2696         case L2CAP_EV_EXPLICIT_POLL:
2697                 l2cap_send_rr_or_rnr(chan, 1);
2698                 chan->retry_count = 1;
2699                 __set_monitor_timer(chan);
2700                 __clear_ack_timer(chan);
2701                 chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2702                 break;
2703         case L2CAP_EV_RETRANS_TO:
2704                 l2cap_send_rr_or_rnr(chan, 1);
2705                 chan->retry_count = 1;
2706                 __set_monitor_timer(chan);
2707                 chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2708                 break;
2709         case L2CAP_EV_RECV_FBIT:
2710                 /* Nothing to process */
2711                 break;
2712         default:
2713                 break;
2714         }
2715 }
2716
2717 static void l2cap_tx_state_wait_f(struct l2cap_chan *chan,
2718                                   struct l2cap_ctrl *control,
2719                                   struct sk_buff_head *skbs, u8 event)
2720 {
2721         BT_DBG("chan %p, control %p, skbs %p, event %d", chan, control, skbs,
2722                event);
2723
2724         switch (event) {
2725         case L2CAP_EV_DATA_REQUEST:
2726                 if (chan->tx_send_head == NULL)
2727                         chan->tx_send_head = skb_peek(skbs);
2728                 /* Queue data, but don't send. */
2729                 skb_queue_splice_tail_init(skbs, &chan->tx_q);
2730                 break;
2731         case L2CAP_EV_LOCAL_BUSY_DETECTED:
2732                 BT_DBG("Enter LOCAL_BUSY");
2733                 set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2734
2735                 if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
2736                         /* The SREJ_SENT state must be aborted if we are to
2737                          * enter the LOCAL_BUSY state.
2738                          */
2739                         l2cap_abort_rx_srej_sent(chan);
2740                 }
2741
2742                 l2cap_send_ack(chan);
2743
2744                 break;
2745         case L2CAP_EV_LOCAL_BUSY_CLEAR:
2746                 BT_DBG("Exit LOCAL_BUSY");
2747                 clear_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2748
2749                 if (test_bit(CONN_RNR_SENT, &chan->conn_state)) {
2750                         struct l2cap_ctrl local_control;
2751                         memset(&local_control, 0, sizeof(local_control));
2752                         local_control.sframe = 1;
2753                         local_control.super = L2CAP_SUPER_RR;
2754                         local_control.poll = 1;
2755                         local_control.reqseq = chan->buffer_seq;
2756                         l2cap_send_sframe(chan, &local_control);
2757
2758                         chan->retry_count = 1;
2759                         __set_monitor_timer(chan);
2760                         chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2761                 }
2762                 break;
2763         case L2CAP_EV_RECV_REQSEQ_AND_FBIT:
2764                 l2cap_process_reqseq(chan, control->reqseq);
2765
2766                 /* Fall through */
2767
2768         case L2CAP_EV_RECV_FBIT:
2769                 if (control && control->final) {
2770                         __clear_monitor_timer(chan);
2771                         if (chan->unacked_frames > 0)
2772                                 __set_retrans_timer(chan);
2773                         chan->retry_count = 0;
2774                         chan->tx_state = L2CAP_TX_STATE_XMIT;
2775                         BT_DBG("recv fbit tx_state 0x2.2%x", chan->tx_state);
2776                 }
2777                 break;
2778         case L2CAP_EV_EXPLICIT_POLL:
2779                 /* Ignore */
2780                 break;
2781         case L2CAP_EV_MONITOR_TO:
2782                 if (chan->max_tx == 0 || chan->retry_count < chan->max_tx) {
2783                         l2cap_send_rr_or_rnr(chan, 1);
2784                         __set_monitor_timer(chan);
2785                         chan->retry_count++;
2786                 } else {
2787                         l2cap_send_disconn_req(chan, ECONNABORTED);
2788                 }
2789                 break;
2790         default:
2791                 break;
2792         }
2793 }
2794
2795 static void l2cap_tx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
2796                      struct sk_buff_head *skbs, u8 event)
2797 {
2798         BT_DBG("chan %p, control %p, skbs %p, event %d, state %d",
2799                chan, control, skbs, event, chan->tx_state);
2800
2801         switch (chan->tx_state) {
2802         case L2CAP_TX_STATE_XMIT:
2803                 l2cap_tx_state_xmit(chan, control, skbs, event);
2804                 break;
2805         case L2CAP_TX_STATE_WAIT_F:
2806                 l2cap_tx_state_wait_f(chan, control, skbs, event);
2807                 break;
2808         default:
2809                 /* Ignore event */
2810                 break;
2811         }
2812 }
2813
2814 static void l2cap_pass_to_tx(struct l2cap_chan *chan,
2815                              struct l2cap_ctrl *control)
2816 {
2817         BT_DBG("chan %p, control %p", chan, control);
2818         l2cap_tx(chan, control, NULL, L2CAP_EV_RECV_REQSEQ_AND_FBIT);
2819 }
2820
2821 static void l2cap_pass_to_tx_fbit(struct l2cap_chan *chan,
2822                                   struct l2cap_ctrl *control)
2823 {
2824         BT_DBG("chan %p, control %p", chan, control);
2825         l2cap_tx(chan, control, NULL, L2CAP_EV_RECV_FBIT);
2826 }
2827
2828 /* Copy frame to all raw sockets on that connection */
2829 static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
2830 {
2831         struct sk_buff *nskb;
2832         struct l2cap_chan *chan;
2833
2834         BT_DBG("conn %p", conn);
2835
2836         mutex_lock(&conn->chan_lock);
2837
2838         list_for_each_entry(chan, &conn->chan_l, list) {
2839                 if (chan->chan_type != L2CAP_CHAN_RAW)
2840                         continue;
2841
2842                 /* Don't send frame to the channel it came from */
2843                 if (bt_cb(skb)->chan == chan)
2844                         continue;
2845
2846                 nskb = skb_clone(skb, GFP_KERNEL);
2847                 if (!nskb)
2848                         continue;
2849                 if (chan->ops->recv(chan, nskb))
2850                         kfree_skb(nskb);
2851         }
2852
2853         mutex_unlock(&conn->chan_lock);
2854 }
2855
2856 /* ---- L2CAP signalling commands ---- */
2857 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn, u8 code,
2858                                        u8 ident, u16 dlen, void *data)
2859 {
2860         struct sk_buff *skb, **frag;
2861         struct l2cap_cmd_hdr *cmd;
2862         struct l2cap_hdr *lh;
2863         int len, count;
2864
2865         BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %u",
2866                conn, code, ident, dlen);
2867
2868         if (conn->mtu < L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE)
2869                 return NULL;
2870
2871         len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen;
2872         count = min_t(unsigned int, conn->mtu, len);
2873
2874         skb = bt_skb_alloc(count, GFP_KERNEL);
2875         if (!skb)
2876                 return NULL;
2877
2878         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
2879         lh->len = cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen);
2880
2881         if (conn->hcon->type == LE_LINK)
2882                 lh->cid = __constant_cpu_to_le16(L2CAP_CID_LE_SIGNALING);
2883         else
2884                 lh->cid = __constant_cpu_to_le16(L2CAP_CID_SIGNALING);
2885
2886         cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE);
2887         cmd->code  = code;
2888         cmd->ident = ident;
2889         cmd->len   = cpu_to_le16(dlen);
2890
2891         if (dlen) {
2892                 count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE;
2893                 memcpy(skb_put(skb, count), data, count);
2894                 data += count;
2895         }
2896
2897         len -= skb->len;
2898
2899         /* Continuation fragments (no L2CAP header) */
2900         frag = &skb_shinfo(skb)->frag_list;
2901         while (len) {
2902                 count = min_t(unsigned int, conn->mtu, len);
2903
2904                 *frag = bt_skb_alloc(count, GFP_KERNEL);
2905                 if (!*frag)
2906                         goto fail;
2907
2908                 memcpy(skb_put(*frag, count), data, count);
2909
2910                 len  -= count;
2911                 data += count;
2912
2913                 frag = &(*frag)->next;
2914         }
2915
2916         return skb;
2917
2918 fail:
2919         kfree_skb(skb);
2920         return NULL;
2921 }
2922
2923 static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen,
2924                                      unsigned long *val)
2925 {
2926         struct l2cap_conf_opt *opt = *ptr;
2927         int len;
2928
2929         len = L2CAP_CONF_OPT_SIZE + opt->len;
2930         *ptr += len;
2931
2932         *type = opt->type;
2933         *olen = opt->len;
2934
2935         switch (opt->len) {
2936         case 1:
2937                 *val = *((u8 *) opt->val);
2938                 break;
2939
2940         case 2:
2941                 *val = get_unaligned_le16(opt->val);
2942                 break;
2943
2944         case 4:
2945                 *val = get_unaligned_le32(opt->val);
2946                 break;
2947
2948         default:
2949                 *val = (unsigned long) opt->val;
2950                 break;
2951         }
2952
2953         BT_DBG("type 0x%2.2x len %u val 0x%lx", *type, opt->len, *val);
2954         return len;
2955 }
2956
2957 static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val)
2958 {
2959         struct l2cap_conf_opt *opt = *ptr;
2960
2961         BT_DBG("type 0x%2.2x len %u val 0x%lx", type, len, val);
2962
2963         opt->type = type;
2964         opt->len  = len;
2965
2966         switch (len) {
2967         case 1:
2968                 *((u8 *) opt->val)  = val;
2969                 break;
2970
2971         case 2:
2972                 put_unaligned_le16(val, opt->val);
2973                 break;
2974
2975         case 4:
2976                 put_unaligned_le32(val, opt->val);
2977                 break;
2978
2979         default:
2980                 memcpy(opt->val, (void *) val, len);
2981                 break;
2982         }
2983
2984         *ptr += L2CAP_CONF_OPT_SIZE + len;
2985 }
2986
2987 static void l2cap_add_opt_efs(void **ptr, struct l2cap_chan *chan)
2988 {
2989         struct l2cap_conf_efs efs;
2990
2991         switch (chan->mode) {
2992         case L2CAP_MODE_ERTM:
2993                 efs.id          = chan->local_id;
2994                 efs.stype       = chan->local_stype;
2995                 efs.msdu        = cpu_to_le16(chan->local_msdu);
2996                 efs.sdu_itime   = cpu_to_le32(chan->local_sdu_itime);
2997                 efs.acc_lat     = __constant_cpu_to_le32(L2CAP_DEFAULT_ACC_LAT);
2998                 efs.flush_to    = __constant_cpu_to_le32(L2CAP_EFS_DEFAULT_FLUSH_TO);
2999                 break;
3000
3001         case L2CAP_MODE_STREAMING:
3002                 efs.id          = 1;
3003                 efs.stype       = L2CAP_SERV_BESTEFFORT;
3004                 efs.msdu        = cpu_to_le16(chan->local_msdu);
3005                 efs.sdu_itime   = cpu_to_le32(chan->local_sdu_itime);
3006                 efs.acc_lat     = 0;
3007                 efs.flush_to    = 0;
3008                 break;
3009
3010         default:
3011                 return;
3012         }
3013
3014         l2cap_add_conf_opt(ptr, L2CAP_CONF_EFS, sizeof(efs),
3015                            (unsigned long) &efs);
3016 }
3017
3018 static void l2cap_ack_timeout(struct work_struct *work)
3019 {
3020         struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
3021                                                ack_timer.work);
3022         u16 frames_to_ack;
3023
3024         BT_DBG("chan %p", chan);
3025
3026         l2cap_chan_lock(chan);
3027
3028         frames_to_ack = __seq_offset(chan, chan->buffer_seq,
3029                                      chan->last_acked_seq);
3030
3031         if (frames_to_ack)
3032                 l2cap_send_rr_or_rnr(chan, 0);
3033
3034         l2cap_chan_unlock(chan);
3035         l2cap_chan_put(chan);
3036 }
3037
3038 int l2cap_ertm_init(struct l2cap_chan *chan)
3039 {
3040         int err;
3041
3042         chan->next_tx_seq = 0;
3043         chan->expected_tx_seq = 0;
3044         chan->expected_ack_seq = 0;
3045         chan->unacked_frames = 0;
3046         chan->buffer_seq = 0;
3047         chan->frames_sent = 0;
3048         chan->last_acked_seq = 0;
3049         chan->sdu = NULL;
3050         chan->sdu_last_frag = NULL;
3051         chan->sdu_len = 0;
3052
3053         skb_queue_head_init(&chan->tx_q);
3054
3055         chan->local_amp_id = AMP_ID_BREDR;
3056         chan->move_id = AMP_ID_BREDR;
3057         chan->move_state = L2CAP_MOVE_STABLE;
3058         chan->move_role = L2CAP_MOVE_ROLE_NONE;
3059
3060         if (chan->mode != L2CAP_MODE_ERTM)
3061                 return 0;
3062
3063         chan->rx_state = L2CAP_RX_STATE_RECV;
3064         chan->tx_state = L2CAP_TX_STATE_XMIT;
3065
3066         INIT_DELAYED_WORK(&chan->retrans_timer, l2cap_retrans_timeout);
3067         INIT_DELAYED_WORK(&chan->monitor_timer, l2cap_monitor_timeout);
3068         INIT_DELAYED_WORK(&chan->ack_timer, l2cap_ack_timeout);
3069
3070         skb_queue_head_init(&chan->srej_q);
3071
3072         err = l2cap_seq_list_init(&chan->srej_list, chan->tx_win);
3073         if (err < 0)
3074                 return err;
3075
3076         err = l2cap_seq_list_init(&chan->retrans_list, chan->remote_tx_win);
3077         if (err < 0)
3078                 l2cap_seq_list_free(&chan->srej_list);
3079
3080         return err;
3081 }
3082
3083 static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
3084 {
3085         switch (mode) {
3086         case L2CAP_MODE_STREAMING:
3087         case L2CAP_MODE_ERTM:
3088                 if (l2cap_mode_supported(mode, remote_feat_mask))
3089                         return mode;
3090                 /* fall through */
3091         default:
3092                 return L2CAP_MODE_BASIC;
3093         }
3094 }
3095
3096 static inline bool __l2cap_ews_supported(struct l2cap_conn *conn)
3097 {
3098         return conn->hs_enabled && conn->feat_mask & L2CAP_FEAT_EXT_WINDOW;
3099 }
3100
3101 static inline bool __l2cap_efs_supported(struct l2cap_conn *conn)
3102 {
3103         return conn->hs_enabled && conn->feat_mask & L2CAP_FEAT_EXT_FLOW;
3104 }
3105
3106 static void __l2cap_set_ertm_timeouts(struct l2cap_chan *chan,
3107                                       struct l2cap_conf_rfc *rfc)
3108 {
3109         if (chan->local_amp_id != AMP_ID_BREDR && chan->hs_hcon) {
3110                 u64 ertm_to = chan->hs_hcon->hdev->amp_be_flush_to;
3111
3112                 /* Class 1 devices have must have ERTM timeouts
3113                  * exceeding the Link Supervision Timeout.  The
3114                  * default Link Supervision Timeout for AMP
3115                  * controllers is 10 seconds.
3116                  *
3117                  * Class 1 devices use 0xffffffff for their
3118                  * best-effort flush timeout, so the clamping logic
3119                  * will result in a timeout that meets the above
3120                  * requirement.  ERTM timeouts are 16-bit values, so
3121                  * the maximum timeout is 65.535 seconds.
3122                  */
3123
3124                 /* Convert timeout to milliseconds and round */
3125                 ertm_to = DIV_ROUND_UP_ULL(ertm_to, 1000);
3126
3127                 /* This is the recommended formula for class 2 devices
3128                  * that start ERTM timers when packets are sent to the
3129                  * controller.
3130                  */
3131                 ertm_to = 3 * ertm_to + 500;
3132
3133                 if (ertm_to > 0xffff)
3134                         ertm_to = 0xffff;
3135
3136                 rfc->retrans_timeout = cpu_to_le16((u16) ertm_to);
3137                 rfc->monitor_timeout = rfc->retrans_timeout;
3138         } else {
3139                 rfc->retrans_timeout = __constant_cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO);
3140                 rfc->monitor_timeout = __constant_cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO);
3141         }
3142 }
3143
3144 static inline void l2cap_txwin_setup(struct l2cap_chan *chan)
3145 {
3146         if (chan->tx_win > L2CAP_DEFAULT_TX_WINDOW &&
3147             __l2cap_ews_supported(chan->conn)) {
3148                 /* use extended control field */
3149                 set_bit(FLAG_EXT_CTRL, &chan->flags);
3150                 chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
3151         } else {
3152                 chan->tx_win = min_t(u16, chan->tx_win,
3153                                      L2CAP_DEFAULT_TX_WINDOW);
3154                 chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
3155         }
3156         chan->ack_win = chan->tx_win;
3157 }
3158
3159 static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data)
3160 {
3161         struct l2cap_conf_req *req = data;
3162         struct l2cap_conf_rfc rfc = { .mode = chan->mode };
3163         void *ptr = req->data;
3164         u16 size;
3165
3166         BT_DBG("chan %p", chan);
3167
3168         if (chan->num_conf_req || chan->num_conf_rsp)
3169                 goto done;
3170
3171         switch (chan->mode) {
3172         case L2CAP_MODE_STREAMING:
3173         case L2CAP_MODE_ERTM:
3174                 if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state))
3175                         break;
3176
3177                 if (__l2cap_efs_supported(chan->conn))
3178                         set_bit(FLAG_EFS_ENABLE, &chan->flags);
3179
3180                 /* fall through */
3181         default:
3182                 chan->mode = l2cap_select_mode(rfc.mode, chan->conn->feat_mask);
3183                 break;
3184         }
3185
3186 done:
3187         if (chan->imtu != L2CAP_DEFAULT_MTU)
3188                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu);
3189
3190         switch (chan->mode) {
3191         case L2CAP_MODE_BASIC:
3192                 if (!(chan->conn->feat_mask & L2CAP_FEAT_ERTM) &&
3193                     !(chan->conn->feat_mask & L2CAP_FEAT_STREAMING))
3194                         break;
3195
3196                 rfc.mode            = L2CAP_MODE_BASIC;
3197                 rfc.txwin_size      = 0;
3198                 rfc.max_transmit    = 0;
3199                 rfc.retrans_timeout = 0;
3200                 rfc.monitor_timeout = 0;
3201                 rfc.max_pdu_size    = 0;
3202
3203                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3204                                    (unsigned long) &rfc);
3205                 break;
3206
3207         case L2CAP_MODE_ERTM:
3208                 rfc.mode            = L2CAP_MODE_ERTM;
3209                 rfc.max_transmit    = chan->max_tx;
3210
3211                 __l2cap_set_ertm_timeouts(chan, &rfc);
3212
3213                 size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
3214                              L2CAP_EXT_HDR_SIZE - L2CAP_SDULEN_SIZE -
3215                              L2CAP_FCS_SIZE);
3216                 rfc.max_pdu_size = cpu_to_le16(size);
3217
3218                 l2cap_txwin_setup(chan);
3219
3220                 rfc.txwin_size = min_t(u16, chan->tx_win,
3221                                        L2CAP_DEFAULT_TX_WINDOW);
3222
3223                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3224                                    (unsigned long) &rfc);
3225
3226                 if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
3227                         l2cap_add_opt_efs(&ptr, chan);
3228
3229                 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
3230                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
3231                                            chan->tx_win);
3232
3233                 if (chan->conn->feat_mask & L2CAP_FEAT_FCS)
3234                         if (chan->fcs == L2CAP_FCS_NONE ||
3235                             test_bit(CONF_RECV_NO_FCS, &chan->conf_state)) {
3236                                 chan->fcs = L2CAP_FCS_NONE;
3237                                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1,
3238                                                    chan->fcs);
3239                         }
3240                 break;
3241
3242         case L2CAP_MODE_STREAMING:
3243                 l2cap_txwin_setup(chan);
3244                 rfc.mode            = L2CAP_MODE_STREAMING;
3245                 rfc.txwin_size      = 0;
3246                 rfc.max_transmit    = 0;
3247                 rfc.retrans_timeout = 0;
3248                 rfc.monitor_timeout = 0;
3249
3250                 size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
3251                              L2CAP_EXT_HDR_SIZE - L2CAP_SDULEN_SIZE -
3252                              L2CAP_FCS_SIZE);
3253                 rfc.max_pdu_size = cpu_to_le16(size);
3254
3255                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3256                                    (unsigned long) &rfc);
3257
3258                 if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
3259                         l2cap_add_opt_efs(&ptr, chan);
3260
3261                 if (chan->conn->feat_mask & L2CAP_FEAT_FCS)
3262                         if (chan->fcs == L2CAP_FCS_NONE ||
3263                             test_bit(CONF_RECV_NO_FCS, &chan->conf_state)) {
3264                                 chan->fcs = L2CAP_FCS_NONE;
3265                                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1,
3266                                                    chan->fcs);
3267                         }
3268                 break;
3269         }
3270
3271         req->dcid  = cpu_to_le16(chan->dcid);
3272         req->flags = __constant_cpu_to_le16(0);
3273
3274         return ptr - data;
3275 }
3276
3277 static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data)
3278 {
3279         struct l2cap_conf_rsp *rsp = data;
3280         void *ptr = rsp->data;
3281         void *req = chan->conf_req;
3282         int len = chan->conf_len;
3283         int type, hint, olen;
3284         unsigned long val;
3285         struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
3286         struct l2cap_conf_efs efs;
3287         u8 remote_efs = 0;
3288         u16 mtu = L2CAP_DEFAULT_MTU;
3289         u16 result = L2CAP_CONF_SUCCESS;
3290         u16 size;
3291
3292         BT_DBG("chan %p", chan);
3293
3294         while (len >= L2CAP_CONF_OPT_SIZE) {
3295                 len -= l2cap_get_conf_opt(&req, &type, &olen, &val);
3296
3297                 hint  = type & L2CAP_CONF_HINT;
3298                 type &= L2CAP_CONF_MASK;
3299
3300                 switch (type) {
3301                 case L2CAP_CONF_MTU:
3302                         mtu = val;
3303                         break;
3304
3305                 case L2CAP_CONF_FLUSH_TO:
3306                         chan->flush_to = val;
3307                         break;
3308
3309                 case L2CAP_CONF_QOS:
3310                         break;
3311
3312                 case L2CAP_CONF_RFC:
3313                         if (olen == sizeof(rfc))
3314                                 memcpy(&rfc, (void *) val, olen);
3315                         break;
3316
3317                 case L2CAP_CONF_FCS:
3318                         if (val == L2CAP_FCS_NONE)
3319                                 set_bit(CONF_RECV_NO_FCS, &chan->conf_state);
3320                         break;
3321
3322                 case L2CAP_CONF_EFS:
3323                         remote_efs = 1;
3324                         if (olen == sizeof(efs))
3325                                 memcpy(&efs, (void *) val, olen);
3326                         break;
3327
3328                 case L2CAP_CONF_EWS:
3329                         if (!chan->conn->hs_enabled)
3330                                 return -ECONNREFUSED;
3331
3332                         set_bit(FLAG_EXT_CTRL, &chan->flags);
3333                         set_bit(CONF_EWS_RECV, &chan->conf_state);
3334                         chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
3335                         chan->remote_tx_win = val;
3336                         break;
3337
3338                 default:
3339                         if (hint)
3340                                 break;
3341
3342                         result = L2CAP_CONF_UNKNOWN;
3343                         *((u8 *) ptr++) = type;
3344                         break;
3345                 }
3346         }
3347
3348         if (chan->num_conf_rsp || chan->num_conf_req > 1)
3349                 goto done;
3350
3351         switch (chan->mode) {
3352         case L2CAP_MODE_STREAMING:
3353         case L2CAP_MODE_ERTM:
3354                 if (!test_bit(CONF_STATE2_DEVICE, &chan->conf_state)) {
3355                         chan->mode = l2cap_select_mode(rfc.mode,
3356                                                        chan->conn->feat_mask);
3357                         break;
3358                 }
3359
3360                 if (remote_efs) {
3361                         if (__l2cap_efs_supported(chan->conn))
3362                                 set_bit(FLAG_EFS_ENABLE, &chan->flags);
3363                         else
3364                                 return -ECONNREFUSED;
3365                 }
3366
3367                 if (chan->mode != rfc.mode)
3368                         return -ECONNREFUSED;
3369
3370                 break;
3371         }
3372
3373 done:
3374         if (chan->mode != rfc.mode) {
3375                 result = L2CAP_CONF_UNACCEPT;
3376                 rfc.mode = chan->mode;
3377
3378                 if (chan->num_conf_rsp == 1)
3379                         return -ECONNREFUSED;
3380
3381                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3382                                    (unsigned long) &rfc);
3383         }
3384
3385         if (result == L2CAP_CONF_SUCCESS) {
3386                 /* Configure output options and let the other side know
3387                  * which ones we don't like. */
3388
3389                 if (mtu < L2CAP_DEFAULT_MIN_MTU)
3390                         result = L2CAP_CONF_UNACCEPT;
3391                 else {
3392                         chan->omtu = mtu;
3393                         set_bit(CONF_MTU_DONE, &chan->conf_state);
3394                 }
3395                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->omtu);
3396
3397                 if (remote_efs) {
3398                         if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
3399                             efs.stype != L2CAP_SERV_NOTRAFIC &&
3400                             efs.stype != chan->local_stype) {
3401
3402                                 result = L2CAP_CONF_UNACCEPT;
3403
3404                                 if (chan->num_conf_req >= 1)
3405                                         return -ECONNREFUSED;
3406
3407                                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
3408                                                    sizeof(efs),
3409                                                    (unsigned long) &efs);
3410                         } else {
3411                                 /* Send PENDING Conf Rsp */
3412                                 result = L2CAP_CONF_PENDING;
3413                                 set_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
3414                         }
3415                 }
3416
3417                 switch (rfc.mode) {
3418                 case L2CAP_MODE_BASIC:
3419                         chan->fcs = L2CAP_FCS_NONE;
3420                         set_bit(CONF_MODE_DONE, &chan->conf_state);
3421                         break;
3422
3423                 case L2CAP_MODE_ERTM:
3424                         if (!test_bit(CONF_EWS_RECV, &chan->conf_state))
3425                                 chan->remote_tx_win = rfc.txwin_size;
3426                         else
3427                                 rfc.txwin_size = L2CAP_DEFAULT_TX_WINDOW;
3428
3429                         chan->remote_max_tx = rfc.max_transmit;
3430
3431                         size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
3432                                      chan->conn->mtu - L2CAP_EXT_HDR_SIZE -
3433                                      L2CAP_SDULEN_SIZE - L2CAP_FCS_SIZE);
3434                         rfc.max_pdu_size = cpu_to_le16(size);
3435                         chan->remote_mps = size;
3436
3437                         __l2cap_set_ertm_timeouts(chan, &rfc);
3438
3439                         set_bit(CONF_MODE_DONE, &chan->conf_state);
3440
3441                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
3442                                            sizeof(rfc), (unsigned long) &rfc);
3443
3444                         if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
3445                                 chan->remote_id = efs.id;
3446                                 chan->remote_stype = efs.stype;
3447                                 chan->remote_msdu = le16_to_cpu(efs.msdu);
3448                                 chan->remote_flush_to =
3449                                         le32_to_cpu(efs.flush_to);
3450                                 chan->remote_acc_lat =
3451                                         le32_to_cpu(efs.acc_lat);
3452                                 chan->remote_sdu_itime =
3453                                         le32_to_cpu(efs.sdu_itime);
3454                                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
3455                                                    sizeof(efs),
3456                                                    (unsigned long) &efs);
3457                         }
3458                         break;
3459
3460                 case L2CAP_MODE_STREAMING:
3461                         size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
3462                                      chan->conn->mtu - L2CAP_EXT_HDR_SIZE -
3463                                      L2CAP_SDULEN_SIZE - L2CAP_FCS_SIZE);
3464                         rfc.max_pdu_size = cpu_to_le16(size);
3465                         chan->remote_mps = size;
3466
3467                         set_bit(CONF_MODE_DONE, &chan->conf_state);
3468
3469                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3470                                            (unsigned long) &rfc);
3471
3472                         break;
3473
3474                 default:
3475                         result = L2CAP_CONF_UNACCEPT;
3476
3477                         memset(&rfc, 0, sizeof(rfc));
3478                         rfc.mode = chan->mode;
3479                 }
3480
3481                 if (result == L2CAP_CONF_SUCCESS)
3482                         set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
3483         }
3484         rsp->scid   = cpu_to_le16(chan->dcid);
3485         rsp->result = cpu_to_le16(result);
3486         rsp->flags  = __constant_cpu_to_le16(0);
3487
3488         return ptr - data;
3489 }
3490
3491 static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len,
3492                                 void *data, u16 *result)
3493 {
3494         struct l2cap_conf_req *req = data;
3495         void *ptr = req->data;
3496         int type, olen;
3497         unsigned long val;
3498         struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
3499         struct l2cap_conf_efs efs;
3500
3501         BT_DBG("chan %p, rsp %p, len %d, req %p", chan, rsp, len, data);
3502
3503         while (len >= L2CAP_CONF_OPT_SIZE) {
3504                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
3505
3506                 switch (type) {
3507                 case L2CAP_CONF_MTU:
3508                         if (val < L2CAP_DEFAULT_MIN_MTU) {
3509                                 *result = L2CAP_CONF_UNACCEPT;
3510                                 chan->imtu = L2CAP_DEFAULT_MIN_MTU;
3511                         } else
3512                                 chan->imtu = val;
3513                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu);
3514                         break;
3515
3516                 case L2CAP_CONF_FLUSH_TO:
3517                         chan->flush_to = val;
3518                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
3519                                            2, chan->flush_to);
3520                         break;
3521
3522                 case L2CAP_CONF_RFC:
3523                         if (olen == sizeof(rfc))
3524                                 memcpy(&rfc, (void *)val, olen);
3525
3526                         if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state) &&
3527                             rfc.mode != chan->mode)
3528                                 return -ECONNREFUSED;
3529
3530                         chan->fcs = 0;
3531
3532                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
3533                                            sizeof(rfc), (unsigned long) &rfc);
3534                         break;
3535
3536                 case L2CAP_CONF_EWS:
3537                         chan->ack_win = min_t(u16, val, chan->ack_win);
3538                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
3539                                            chan->tx_win);
3540                         break;
3541
3542                 case L2CAP_CONF_EFS:
3543                         if (olen == sizeof(efs))
3544                                 memcpy(&efs, (void *)val, olen);
3545
3546                         if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
3547                             efs.stype != L2CAP_SERV_NOTRAFIC &&
3548                             efs.stype != chan->local_stype)
3549                                 return -ECONNREFUSED;
3550
3551                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS, sizeof(efs),
3552                                            (unsigned long) &efs);
3553                         break;
3554
3555                 case L2CAP_CONF_FCS:
3556                         if (*result == L2CAP_CONF_PENDING)
3557                                 if (val == L2CAP_FCS_NONE)
3558                                         set_bit(CONF_RECV_NO_FCS,
3559                                                 &chan->conf_state);
3560                         break;
3561                 }
3562         }
3563
3564         if (chan->mode == L2CAP_MODE_BASIC && chan->mode != rfc.mode)
3565                 return -ECONNREFUSED;
3566
3567         chan->mode = rfc.mode;
3568
3569         if (*result == L2CAP_CONF_SUCCESS || *result == L2CAP_CONF_PENDING) {
3570                 switch (rfc.mode) {
3571                 case L2CAP_MODE_ERTM:
3572                         chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
3573                         chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
3574                         chan->mps    = le16_to_cpu(rfc.max_pdu_size);
3575                         if (!test_bit(FLAG_EXT_CTRL, &chan->flags))
3576                                 chan->ack_win = min_t(u16, chan->ack_win,
3577                                                       rfc.txwin_size);
3578
3579                         if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
3580                                 chan->local_msdu = le16_to_cpu(efs.msdu);
3581                                 chan->local_sdu_itime =
3582                                         le32_to_cpu(efs.sdu_itime);
3583                                 chan->local_acc_lat = le32_to_cpu(efs.acc_lat);
3584                                 chan->local_flush_to =
3585                                         le32_to_cpu(efs.flush_to);
3586                         }
3587                         break;
3588
3589                 case L2CAP_MODE_STREAMING:
3590                         chan->mps    = le16_to_cpu(rfc.max_pdu_size);
3591                 }
3592         }
3593
3594         req->dcid   = cpu_to_le16(chan->dcid);
3595         req->flags  = __constant_cpu_to_le16(0);
3596
3597         return ptr - data;
3598 }
3599
3600 static int l2cap_build_conf_rsp(struct l2cap_chan *chan, void *data,
3601                                 u16 result, u16 flags)
3602 {
3603         struct l2cap_conf_rsp *rsp = data;
3604         void *ptr = rsp->data;
3605
3606         BT_DBG("chan %p", chan);
3607
3608         rsp->scid   = cpu_to_le16(chan->dcid);
3609         rsp->result = cpu_to_le16(result);
3610         rsp->flags  = cpu_to_le16(flags);
3611
3612         return ptr - data;
3613 }
3614
3615 void __l2cap_le_connect_rsp_defer(struct l2cap_chan *chan)
3616 {
3617         struct l2cap_le_conn_rsp rsp;
3618         struct l2cap_conn *conn = chan->conn;
3619
3620         BT_DBG("chan %p", chan);
3621
3622         rsp.dcid    = cpu_to_le16(chan->scid);
3623         rsp.mtu     = cpu_to_le16(chan->imtu);
3624         rsp.mps     = cpu_to_le16(chan->mps);
3625         rsp.credits = cpu_to_le16(chan->rx_credits);
3626         rsp.result  = __constant_cpu_to_le16(L2CAP_CR_SUCCESS);
3627
3628         l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CONN_RSP, sizeof(rsp),
3629                        &rsp);
3630 }
3631
3632 void __l2cap_connect_rsp_defer(struct l2cap_chan *chan)
3633 {
3634         struct l2cap_conn_rsp rsp;
3635         struct l2cap_conn *conn = chan->conn;
3636         u8 buf[128];
3637         u8 rsp_code;
3638
3639         rsp.scid   = cpu_to_le16(chan->dcid);
3640         rsp.dcid   = cpu_to_le16(chan->scid);
3641         rsp.result = __constant_cpu_to_le16(L2CAP_CR_SUCCESS);
3642         rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
3643
3644         if (chan->hs_hcon)
3645                 rsp_code = L2CAP_CREATE_CHAN_RSP;
3646         else
3647                 rsp_code = L2CAP_CONN_RSP;
3648
3649         BT_DBG("chan %p rsp_code %u", chan, rsp_code);
3650
3651         l2cap_send_cmd(conn, chan->ident, rsp_code, sizeof(rsp), &rsp);
3652
3653         if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
3654                 return;
3655
3656         l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3657                        l2cap_build_conf_req(chan, buf), buf);
3658         chan->num_conf_req++;
3659 }
3660
3661 static void l2cap_conf_rfc_get(struct l2cap_chan *chan, void *rsp, int len)
3662 {
3663         int type, olen;
3664         unsigned long val;
3665         /* Use sane default values in case a misbehaving remote device
3666          * did not send an RFC or extended window size option.
3667          */
3668         u16 txwin_ext = chan->ack_win;
3669         struct l2cap_conf_rfc rfc = {
3670                 .mode = chan->mode,
3671                 .retrans_timeout = __constant_cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO),
3672                 .monitor_timeout = __constant_cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO),
3673                 .max_pdu_size = cpu_to_le16(chan->imtu),
3674                 .txwin_size = min_t(u16, chan->ack_win, L2CAP_DEFAULT_TX_WINDOW),
3675         };
3676
3677         BT_DBG("chan %p, rsp %p, len %d", chan, rsp, len);
3678
3679         if ((chan->mode != L2CAP_MODE_ERTM) && (chan->mode != L2CAP_MODE_STREAMING))
3680                 return;
3681
3682         while (len >= L2CAP_CONF_OPT_SIZE) {
3683                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
3684
3685                 switch (type) {
3686                 case L2CAP_CONF_RFC:
3687                         if (olen == sizeof(rfc))
3688                                 memcpy(&rfc, (void *)val, olen);
3689                         break;
3690                 case L2CAP_CONF_EWS:
3691                         txwin_ext = val;
3692                         break;
3693                 }
3694         }
3695
3696         switch (rfc.mode) {
3697         case L2CAP_MODE_ERTM:
3698                 chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
3699                 chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
3700                 chan->mps = le16_to_cpu(rfc.max_pdu_size);
3701                 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
3702                         chan->ack_win = min_t(u16, chan->ack_win, txwin_ext);
3703                 else
3704                         chan->ack_win = min_t(u16, chan->ack_win,
3705                                               rfc.txwin_size);
3706                 break;
3707         case L2CAP_MODE_STREAMING:
3708                 chan->mps    = le16_to_cpu(rfc.max_pdu_size);
3709         }
3710 }
3711
3712 static inline int l2cap_command_rej(struct l2cap_conn *conn,
3713                                     struct l2cap_cmd_hdr *cmd, u16 cmd_len,
3714                                     u8 *data)
3715 {
3716         struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data;
3717
3718         if (cmd_len < sizeof(*rej))
3719                 return -EPROTO;
3720
3721         if (rej->reason != L2CAP_REJ_NOT_UNDERSTOOD)
3722                 return 0;
3723
3724         if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
3725             cmd->ident == conn->info_ident) {
3726                 cancel_delayed_work(&conn->info_timer);
3727
3728                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3729                 conn->info_ident = 0;
3730
3731                 l2cap_conn_start(conn);
3732         }
3733
3734         return 0;
3735 }
3736
3737 static struct l2cap_chan *l2cap_connect(struct l2cap_conn *conn,
3738                                         struct l2cap_cmd_hdr *cmd,
3739                                         u8 *data, u8 rsp_code, u8 amp_id)
3740 {
3741         struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
3742         struct l2cap_conn_rsp rsp;
3743         struct l2cap_chan *chan = NULL, *pchan;
3744         int result, status = L2CAP_CS_NO_INFO;
3745
3746         u16 dcid = 0, scid = __le16_to_cpu(req->scid);
3747         __le16 psm = req->psm;
3748
3749         BT_DBG("psm 0x%2.2x scid 0x%4.4x", __le16_to_cpu(psm), scid);
3750
3751         /* Check if we have socket listening on psm */
3752         pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, &conn->hcon->src,
3753                                          &conn->hcon->dst, ACL_LINK);
3754         if (!pchan) {
3755                 result = L2CAP_CR_BAD_PSM;
3756                 goto sendresp;
3757         }
3758
3759         mutex_lock(&conn->chan_lock);
3760         l2cap_chan_lock(pchan);
3761
3762         /* Check if the ACL is secure enough (if not SDP) */
3763         if (psm != __constant_cpu_to_le16(L2CAP_PSM_SDP) &&
3764             !hci_conn_check_link_mode(conn->hcon)) {
3765                 conn->disc_reason = HCI_ERROR_AUTH_FAILURE;
3766                 result = L2CAP_CR_SEC_BLOCK;
3767                 goto response;
3768         }
3769
3770         result = L2CAP_CR_NO_MEM;
3771
3772         /* Check if we already have channel with that dcid */
3773         if (__l2cap_get_chan_by_dcid(conn, scid))
3774                 goto response;
3775
3776         chan = pchan->ops->new_connection(pchan);
3777         if (!chan)
3778                 goto response;
3779
3780         /* For certain devices (ex: HID mouse), support for authentication,
3781          * pairing and bonding is optional. For such devices, inorder to avoid
3782          * the ACL alive for too long after L2CAP disconnection, reset the ACL
3783          * disc_timeout back to HCI_DISCONN_TIMEOUT during L2CAP connect.
3784          */
3785         conn->hcon->disc_timeout = HCI_DISCONN_TIMEOUT;
3786
3787         bacpy(&chan->src, &conn->hcon->src);
3788         bacpy(&chan->dst, &conn->hcon->dst);
3789         chan->src_type = bdaddr_type(conn->hcon, conn->hcon->src_type);
3790         chan->dst_type = bdaddr_type(conn->hcon, conn->hcon->dst_type);
3791         chan->psm  = psm;
3792         chan->dcid = scid;
3793         chan->local_amp_id = amp_id;
3794
3795         __l2cap_chan_add(conn, chan);
3796
3797         dcid = chan->scid;
3798
3799         __set_chan_timer(chan, chan->ops->get_sndtimeo(chan));
3800
3801         chan->ident = cmd->ident;
3802
3803         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
3804                 if (l2cap_chan_check_security(chan)) {
3805                         if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
3806                                 l2cap_state_change(chan, BT_CONNECT2);
3807                                 result = L2CAP_CR_PEND;
3808                                 status = L2CAP_CS_AUTHOR_PEND;
3809                                 chan->ops->defer(chan);
3810                         } else {
3811                                 /* Force pending result for AMP controllers.
3812                                  * The connection will succeed after the
3813                                  * physical link is up.
3814                                  */
3815                                 if (amp_id == AMP_ID_BREDR) {
3816                                         l2cap_state_change(chan, BT_CONFIG);
3817                                         result = L2CAP_CR_SUCCESS;
3818                                 } else {
3819                                         l2cap_state_change(chan, BT_CONNECT2);
3820                                         result = L2CAP_CR_PEND;
3821                                 }
3822                                 status = L2CAP_CS_NO_INFO;
3823                         }
3824                 } else {
3825                         l2cap_state_change(chan, BT_CONNECT2);
3826                         result = L2CAP_CR_PEND;
3827                         status = L2CAP_CS_AUTHEN_PEND;
3828                 }
3829         } else {
3830                 l2cap_state_change(chan, BT_CONNECT2);
3831                 result = L2CAP_CR_PEND;
3832                 status = L2CAP_CS_NO_INFO;
3833         }
3834
3835 response:
3836         l2cap_chan_unlock(pchan);
3837         mutex_unlock(&conn->chan_lock);
3838
3839 sendresp:
3840         rsp.scid   = cpu_to_le16(scid);
3841         rsp.dcid   = cpu_to_le16(dcid);
3842         rsp.result = cpu_to_le16(result);
3843         rsp.status = cpu_to_le16(status);
3844         l2cap_send_cmd(conn, cmd->ident, rsp_code, sizeof(rsp), &rsp);
3845
3846         if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
3847                 struct l2cap_info_req info;
3848                 info.type = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
3849
3850                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
3851                 conn->info_ident = l2cap_get_ident(conn);
3852
3853                 schedule_delayed_work(&conn->info_timer, L2CAP_INFO_TIMEOUT);
3854
3855                 l2cap_send_cmd(conn, conn->info_ident, L2CAP_INFO_REQ,
3856                                sizeof(info), &info);
3857         }
3858
3859         if (chan && !test_bit(CONF_REQ_SENT, &chan->conf_state) &&
3860             result == L2CAP_CR_SUCCESS) {
3861                 u8 buf[128];
3862                 set_bit(CONF_REQ_SENT, &chan->conf_state);
3863                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3864                                l2cap_build_conf_req(chan, buf), buf);
3865                 chan->num_conf_req++;
3866         }
3867
3868         return chan;
3869 }
3870
3871 static int l2cap_connect_req(struct l2cap_conn *conn,
3872                              struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
3873 {
3874         struct hci_dev *hdev = conn->hcon->hdev;
3875         struct hci_conn *hcon = conn->hcon;
3876
3877         if (cmd_len < sizeof(struct l2cap_conn_req))
3878                 return -EPROTO;
3879
3880         hci_dev_lock(hdev);
3881         if (test_bit(HCI_MGMT, &hdev->dev_flags) &&
3882             !test_and_set_bit(HCI_CONN_MGMT_CONNECTED, &hcon->flags))
3883                 mgmt_device_connected(hdev, &hcon->dst, hcon->type,
3884                                       hcon->dst_type, 0, NULL, 0,
3885                                       hcon->dev_class);
3886         hci_dev_unlock(hdev);
3887
3888         l2cap_connect(conn, cmd, data, L2CAP_CONN_RSP, 0);
3889         return 0;
3890 }
3891
3892 static int l2cap_connect_create_rsp(struct l2cap_conn *conn,
3893                                     struct l2cap_cmd_hdr *cmd, u16 cmd_len,
3894                                     u8 *data)
3895 {
3896         struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
3897         u16 scid, dcid, result, status;
3898         struct l2cap_chan *chan;
3899         u8 req[128];
3900         int err;
3901
3902         if (cmd_len < sizeof(*rsp))
3903                 return -EPROTO;
3904
3905         scid   = __le16_to_cpu(rsp->scid);
3906         dcid   = __le16_to_cpu(rsp->dcid);
3907         result = __le16_to_cpu(rsp->result);
3908         status = __le16_to_cpu(rsp->status);
3909
3910         BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x",
3911                dcid, scid, result, status);
3912
3913         mutex_lock(&conn->chan_lock);
3914
3915         if (scid) {
3916                 chan = __l2cap_get_chan_by_scid(conn, scid);
3917                 if (!chan) {
3918                         err = -EBADSLT;
3919                         goto unlock;
3920                 }
3921         } else {
3922                 chan = __l2cap_get_chan_by_ident(conn, cmd->ident);
3923                 if (!chan) {
3924                         err = -EBADSLT;
3925                         goto unlock;
3926                 }
3927         }
3928
3929         err = 0;
3930
3931         l2cap_chan_lock(chan);
3932
3933         switch (result) {
3934         case L2CAP_CR_SUCCESS:
3935                 l2cap_state_change(chan, BT_CONFIG);
3936                 chan->ident = 0;
3937                 chan->dcid = dcid;
3938                 clear_bit(CONF_CONNECT_PEND, &chan->conf_state);
3939
3940                 if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
3941                         break;
3942
3943                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3944                                l2cap_build_conf_req(chan, req), req);
3945                 chan->num_conf_req++;
3946                 break;
3947
3948         case L2CAP_CR_PEND:
3949                 set_bit(CONF_CONNECT_PEND, &chan->conf_state);
3950                 break;
3951
3952         default:
3953                 l2cap_chan_del(chan, ECONNREFUSED);
3954                 break;
3955         }
3956
3957         l2cap_chan_unlock(chan);
3958
3959 unlock:
3960         mutex_unlock(&conn->chan_lock);
3961
3962         return err;
3963 }
3964
3965 static inline void set_default_fcs(struct l2cap_chan *chan)
3966 {
3967         /* FCS is enabled only in ERTM or streaming mode, if one or both
3968          * sides request it.
3969          */
3970         if (chan->mode != L2CAP_MODE_ERTM && chan->mode != L2CAP_MODE_STREAMING)
3971                 chan->fcs = L2CAP_FCS_NONE;
3972         else if (!test_bit(CONF_RECV_NO_FCS, &chan->conf_state))
3973                 chan->fcs = L2CAP_FCS_CRC16;
3974 }
3975
3976 static void l2cap_send_efs_conf_rsp(struct l2cap_chan *chan, void *data,
3977                                     u8 ident, u16 flags)
3978 {
3979         struct l2cap_conn *conn = chan->conn;
3980
3981         BT_DBG("conn %p chan %p ident %d flags 0x%4.4x", conn, chan, ident,
3982                flags);
3983
3984         clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
3985         set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
3986
3987         l2cap_send_cmd(conn, ident, L2CAP_CONF_RSP,
3988                        l2cap_build_conf_rsp(chan, data,
3989                                             L2CAP_CONF_SUCCESS, flags), data);
3990 }
3991
3992 static void cmd_reject_invalid_cid(struct l2cap_conn *conn, u8 ident,
3993                                    u16 scid, u16 dcid)
3994 {
3995         struct l2cap_cmd_rej_cid rej;
3996
3997         rej.reason = __constant_cpu_to_le16(L2CAP_REJ_INVALID_CID);
3998         rej.scid = __cpu_to_le16(scid);
3999         rej.dcid = __cpu_to_le16(dcid);
4000
4001         l2cap_send_cmd(conn, ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
4002 }
4003
4004 static inline int l2cap_config_req(struct l2cap_conn *conn,
4005                                    struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4006                                    u8 *data)
4007 {
4008         struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
4009         u16 dcid, flags;
4010         u8 rsp[64];
4011         struct l2cap_chan *chan;
4012         int len, err = 0;
4013
4014         if (cmd_len < sizeof(*req))
4015                 return -EPROTO;
4016
4017         dcid  = __le16_to_cpu(req->dcid);
4018         flags = __le16_to_cpu(req->flags);
4019
4020         BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
4021
4022         chan = l2cap_get_chan_by_scid(conn, dcid);
4023         if (!chan) {
4024                 cmd_reject_invalid_cid(conn, cmd->ident, dcid, 0);
4025                 return 0;
4026         }
4027
4028         if (chan->state != BT_CONFIG && chan->state != BT_CONNECT2) {
4029                 cmd_reject_invalid_cid(conn, cmd->ident, chan->scid,
4030                                        chan->dcid);
4031                 goto unlock;
4032         }
4033
4034         /* Reject if config buffer is too small. */
4035         len = cmd_len - sizeof(*req);
4036         if (chan->conf_len + len > sizeof(chan->conf_req)) {
4037                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
4038                                l2cap_build_conf_rsp(chan, rsp,
4039                                L2CAP_CONF_REJECT, flags), rsp);
4040                 goto unlock;
4041         }
4042
4043         /* Store config. */
4044         memcpy(chan->conf_req + chan->conf_len, req->data, len);
4045         chan->conf_len += len;
4046
4047         if (flags & L2CAP_CONF_FLAG_CONTINUATION) {
4048                 /* Incomplete config. Send empty response. */
4049                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
4050                                l2cap_build_conf_rsp(chan, rsp,
4051                                L2CAP_CONF_SUCCESS, flags), rsp);
4052                 goto unlock;
4053         }
4054
4055         /* Complete config. */
4056         len = l2cap_parse_conf_req(chan, rsp);
4057         if (len < 0) {
4058                 l2cap_send_disconn_req(chan, ECONNRESET);
4059                 goto unlock;
4060         }
4061
4062         chan->ident = cmd->ident;
4063         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
4064         chan->num_conf_rsp++;
4065
4066         /* Reset config buffer. */
4067         chan->conf_len = 0;
4068
4069         if (!test_bit(CONF_OUTPUT_DONE, &chan->conf_state))
4070                 goto unlock;
4071
4072         if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
4073                 set_default_fcs(chan);
4074
4075                 if (chan->mode == L2CAP_MODE_ERTM ||
4076                     chan->mode == L2CAP_MODE_STREAMING)
4077                         err = l2cap_ertm_init(chan);
4078
4079                 if (err < 0)
4080                         l2cap_send_disconn_req(chan, -err);
4081                 else
4082                         l2cap_chan_ready(chan);
4083
4084                 goto unlock;
4085         }
4086
4087         if (!test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) {
4088                 u8 buf[64];
4089                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
4090                                l2cap_build_conf_req(chan, buf), buf);
4091                 chan->num_conf_req++;
4092         }
4093
4094         /* Got Conf Rsp PENDING from remote side and asume we sent
4095            Conf Rsp PENDING in the code above */
4096         if (test_bit(CONF_REM_CONF_PEND, &chan->conf_state) &&
4097             test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
4098
4099                 /* check compatibility */
4100
4101                 /* Send rsp for BR/EDR channel */
4102                 if (!chan->hs_hcon)
4103                         l2cap_send_efs_conf_rsp(chan, rsp, cmd->ident, flags);
4104                 else
4105                         chan->ident = cmd->ident;
4106         }
4107
4108 unlock:
4109         l2cap_chan_unlock(chan);
4110         return err;
4111 }
4112
4113 static inline int l2cap_config_rsp(struct l2cap_conn *conn,
4114                                    struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4115                                    u8 *data)
4116 {
4117         struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
4118         u16 scid, flags, result;
4119         struct l2cap_chan *chan;
4120         int len = cmd_len - sizeof(*rsp);
4121         int err = 0;
4122
4123         if (cmd_len < sizeof(*rsp))
4124                 return -EPROTO;
4125
4126         scid   = __le16_to_cpu(rsp->scid);
4127         flags  = __le16_to_cpu(rsp->flags);
4128         result = __le16_to_cpu(rsp->result);
4129
4130         BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x len %d", scid, flags,
4131                result, len);
4132
4133         chan = l2cap_get_chan_by_scid(conn, scid);
4134         if (!chan)
4135                 return 0;
4136
4137         switch (result) {
4138         case L2CAP_CONF_SUCCESS:
4139                 l2cap_conf_rfc_get(chan, rsp->data, len);
4140                 clear_bit(CONF_REM_CONF_PEND, &chan->conf_state);
4141                 break;
4142
4143         case L2CAP_CONF_PENDING:
4144                 set_bit(CONF_REM_CONF_PEND, &chan->conf_state);
4145
4146                 if (test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
4147                         char buf[64];
4148
4149                         len = l2cap_parse_conf_rsp(chan, rsp->data, len,
4150                                                    buf, &result);
4151                         if (len < 0) {
4152                                 l2cap_send_disconn_req(chan, ECONNRESET);
4153                                 goto done;
4154                         }
4155
4156                         if (!chan->hs_hcon) {
4157                                 l2cap_send_efs_conf_rsp(chan, buf, cmd->ident,
4158                                                         0);
4159                         } else {
4160                                 if (l2cap_check_efs(chan)) {
4161                                         amp_create_logical_link(chan);
4162                                         chan->ident = cmd->ident;
4163                                 }
4164                         }
4165                 }
4166                 goto done;
4167
4168         case L2CAP_CONF_UNACCEPT:
4169                 if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
4170                         char req[64];
4171
4172                         if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
4173                                 l2cap_send_disconn_req(chan, ECONNRESET);
4174                                 goto done;
4175                         }
4176
4177                         /* throw out any old stored conf requests */
4178                         result = L2CAP_CONF_SUCCESS;
4179                         len = l2cap_parse_conf_rsp(chan, rsp->data, len,
4180                                                    req, &result);
4181                         if (len < 0) {
4182                                 l2cap_send_disconn_req(chan, ECONNRESET);
4183                                 goto done;
4184                         }
4185
4186                         l2cap_send_cmd(conn, l2cap_get_ident(conn),
4187                                        L2CAP_CONF_REQ, len, req);
4188                         chan->num_conf_req++;
4189                         if (result != L2CAP_CONF_SUCCESS)
4190                                 goto done;
4191                         break;
4192                 }
4193
4194         default:
4195                 l2cap_chan_set_err(chan, ECONNRESET);
4196
4197                 __set_chan_timer(chan, L2CAP_DISC_REJ_TIMEOUT);
4198                 l2cap_send_disconn_req(chan, ECONNRESET);
4199                 goto done;
4200         }
4201
4202         if (flags & L2CAP_CONF_FLAG_CONTINUATION)
4203                 goto done;
4204
4205         set_bit(CONF_INPUT_DONE, &chan->conf_state);
4206
4207         if (test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) {
4208                 set_default_fcs(chan);
4209
4210                 if (chan->mode == L2CAP_MODE_ERTM ||
4211                     chan->mode == L2CAP_MODE_STREAMING)
4212                         err = l2cap_ertm_init(chan);
4213
4214                 if (err < 0)
4215                         l2cap_send_disconn_req(chan, -err);
4216                 else
4217                         l2cap_chan_ready(chan);
4218         }
4219
4220 done:
4221         l2cap_chan_unlock(chan);
4222         return err;
4223 }
4224
4225 static inline int l2cap_disconnect_req(struct l2cap_conn *conn,
4226                                        struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4227                                        u8 *data)
4228 {
4229         struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
4230         struct l2cap_disconn_rsp rsp;
4231         u16 dcid, scid;
4232         struct l2cap_chan *chan;
4233
4234         if (cmd_len != sizeof(*req))
4235                 return -EPROTO;
4236
4237         scid = __le16_to_cpu(req->scid);
4238         dcid = __le16_to_cpu(req->dcid);
4239
4240         BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
4241
4242         mutex_lock(&conn->chan_lock);
4243
4244         chan = __l2cap_get_chan_by_scid(conn, dcid);
4245         if (!chan) {
4246                 mutex_unlock(&conn->chan_lock);
4247                 cmd_reject_invalid_cid(conn, cmd->ident, dcid, scid);
4248                 return 0;
4249         }
4250
4251         l2cap_chan_lock(chan);
4252
4253         rsp.dcid = cpu_to_le16(chan->scid);
4254         rsp.scid = cpu_to_le16(chan->dcid);
4255         l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
4256
4257         chan->ops->set_shutdown(chan);
4258
4259         l2cap_chan_hold(chan);
4260         l2cap_chan_del(chan, ECONNRESET);
4261
4262         l2cap_chan_unlock(chan);
4263
4264         chan->ops->close(chan);
4265         l2cap_chan_put(chan);
4266
4267         mutex_unlock(&conn->chan_lock);
4268
4269         return 0;
4270 }
4271
4272 static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn,
4273                                        struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4274                                        u8 *data)
4275 {
4276         struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
4277         u16 dcid, scid;
4278         struct l2cap_chan *chan;
4279
4280         if (cmd_len != sizeof(*rsp))
4281                 return -EPROTO;
4282
4283         scid = __le16_to_cpu(rsp->scid);
4284         dcid = __le16_to_cpu(rsp->dcid);
4285
4286         BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
4287
4288         mutex_lock(&conn->chan_lock);
4289
4290         chan = __l2cap_get_chan_by_scid(conn, scid);
4291         if (!chan) {
4292                 mutex_unlock(&conn->chan_lock);
4293                 return 0;
4294         }
4295
4296         l2cap_chan_lock(chan);
4297
4298         l2cap_chan_hold(chan);
4299         l2cap_chan_del(chan, 0);
4300
4301         l2cap_chan_unlock(chan);
4302
4303         chan->ops->close(chan);
4304         l2cap_chan_put(chan);
4305
4306         mutex_unlock(&conn->chan_lock);
4307
4308         return 0;
4309 }
4310
4311 static inline int l2cap_information_req(struct l2cap_conn *conn,
4312                                         struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4313                                         u8 *data)
4314 {
4315         struct l2cap_info_req *req = (struct l2cap_info_req *) data;
4316         u16 type;
4317
4318         if (cmd_len != sizeof(*req))
4319                 return -EPROTO;
4320
4321         type = __le16_to_cpu(req->type);
4322
4323         BT_DBG("type 0x%4.4x", type);
4324
4325         if (type == L2CAP_IT_FEAT_MASK) {
4326                 u8 buf[8];
4327                 u32 feat_mask = l2cap_feat_mask;
4328                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
4329                 rsp->type   = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
4330                 rsp->result = __constant_cpu_to_le16(L2CAP_IR_SUCCESS);
4331                 if (!disable_ertm)
4332                         feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
4333                                 | L2CAP_FEAT_FCS;
4334                 if (conn->hs_enabled)
4335                         feat_mask |= L2CAP_FEAT_EXT_FLOW
4336                                 | L2CAP_FEAT_EXT_WINDOW;
4337
4338                 put_unaligned_le32(feat_mask, rsp->data);
4339                 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf),
4340                                buf);
4341         } else if (type == L2CAP_IT_FIXED_CHAN) {
4342                 u8 buf[12];
4343                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
4344
4345                 if (conn->hs_enabled)
4346                         l2cap_fixed_chan[0] |= L2CAP_FC_A2MP;
4347                 else
4348                         l2cap_fixed_chan[0] &= ~L2CAP_FC_A2MP;
4349
4350                 rsp->type   = __constant_cpu_to_le16(L2CAP_IT_FIXED_CHAN);
4351                 rsp->result = __constant_cpu_to_le16(L2CAP_IR_SUCCESS);
4352                 memcpy(rsp->data, l2cap_fixed_chan, sizeof(l2cap_fixed_chan));
4353                 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf),
4354                                buf);
4355         } else {
4356                 struct l2cap_info_rsp rsp;
4357                 rsp.type   = cpu_to_le16(type);
4358                 rsp.result = __constant_cpu_to_le16(L2CAP_IR_NOTSUPP);
4359                 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(rsp),
4360                                &rsp);
4361         }
4362
4363         return 0;
4364 }
4365
4366 static inline int l2cap_information_rsp(struct l2cap_conn *conn,
4367                                         struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4368                                         u8 *data)
4369 {
4370         struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
4371         u16 type, result;
4372
4373         if (cmd_len < sizeof(*rsp))
4374                 return -EPROTO;
4375
4376         type   = __le16_to_cpu(rsp->type);
4377         result = __le16_to_cpu(rsp->result);
4378
4379         BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
4380
4381         /* L2CAP Info req/rsp are unbound to channels, add extra checks */
4382         if (cmd->ident != conn->info_ident ||
4383             conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)
4384                 return 0;
4385
4386         cancel_delayed_work(&conn->info_timer);
4387
4388         if (result != L2CAP_IR_SUCCESS) {
4389                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4390                 conn->info_ident = 0;
4391
4392                 l2cap_conn_start(conn);
4393
4394                 return 0;
4395         }
4396
4397         switch (type) {
4398         case L2CAP_IT_FEAT_MASK:
4399                 conn->feat_mask = get_unaligned_le32(rsp->data);
4400
4401                 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
4402                         struct l2cap_info_req req;
4403                         req.type = __constant_cpu_to_le16(L2CAP_IT_FIXED_CHAN);
4404
4405                         conn->info_ident = l2cap_get_ident(conn);
4406
4407                         l2cap_send_cmd(conn, conn->info_ident,
4408                                        L2CAP_INFO_REQ, sizeof(req), &req);
4409                 } else {
4410                         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4411                         conn->info_ident = 0;
4412
4413                         l2cap_conn_start(conn);
4414                 }
4415                 break;
4416
4417         case L2CAP_IT_FIXED_CHAN:
4418                 conn->fixed_chan_mask = rsp->data[0];
4419                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4420                 conn->info_ident = 0;
4421
4422                 l2cap_conn_start(conn);
4423                 break;
4424         }
4425
4426         return 0;
4427 }
4428
4429 static int l2cap_create_channel_req(struct l2cap_conn *conn,
4430                                     struct l2cap_cmd_hdr *cmd,
4431                                     u16 cmd_len, void *data)
4432 {
4433         struct l2cap_create_chan_req *req = data;
4434         struct l2cap_create_chan_rsp rsp;
4435         struct l2cap_chan *chan;
4436         struct hci_dev *hdev;
4437         u16 psm, scid;
4438
4439         if (cmd_len != sizeof(*req))
4440                 return -EPROTO;
4441
4442         if (!conn->hs_enabled)
4443                 return -EINVAL;
4444
4445         psm = le16_to_cpu(req->psm);
4446         scid = le16_to_cpu(req->scid);
4447
4448         BT_DBG("psm 0x%2.2x, scid 0x%4.4x, amp_id %d", psm, scid, req->amp_id);
4449
4450         /* For controller id 0 make BR/EDR connection */
4451         if (req->amp_id == AMP_ID_BREDR) {
4452                 l2cap_connect(conn, cmd, data, L2CAP_CREATE_CHAN_RSP,
4453                               req->amp_id);
4454                 return 0;
4455         }
4456
4457         /* Validate AMP controller id */
4458         hdev = hci_dev_get(req->amp_id);
4459         if (!hdev)
4460                 goto error;
4461
4462         if (hdev->dev_type != HCI_AMP || !test_bit(HCI_UP, &hdev->flags)) {
4463                 hci_dev_put(hdev);
4464                 goto error;
4465         }
4466
4467         chan = l2cap_connect(conn, cmd, data, L2CAP_CREATE_CHAN_RSP,
4468                              req->amp_id);
4469         if (chan) {
4470                 struct amp_mgr *mgr = conn->hcon->amp_mgr;
4471                 struct hci_conn *hs_hcon;
4472
4473                 hs_hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK,
4474                                                   &conn->hcon->dst);
4475                 if (!hs_hcon) {
4476                         hci_dev_put(hdev);
4477                         cmd_reject_invalid_cid(conn, cmd->ident, chan->scid,
4478                                                chan->dcid);
4479                         return 0;
4480                 }
4481
4482                 BT_DBG("mgr %p bredr_chan %p hs_hcon %p", mgr, chan, hs_hcon);
4483
4484                 mgr->bredr_chan = chan;
4485                 chan->hs_hcon = hs_hcon;
4486                 chan->fcs = L2CAP_FCS_NONE;
4487                 conn->mtu = hdev->block_mtu;
4488         }
4489
4490         hci_dev_put(hdev);
4491
4492         return 0;
4493
4494 error:
4495         rsp.dcid = 0;
4496         rsp.scid = cpu_to_le16(scid);
4497         rsp.result = __constant_cpu_to_le16(L2CAP_CR_BAD_AMP);
4498         rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
4499
4500         l2cap_send_cmd(conn, cmd->ident, L2CAP_CREATE_CHAN_RSP,
4501                        sizeof(rsp), &rsp);
4502
4503         return 0;
4504 }
4505
4506 static void l2cap_send_move_chan_req(struct l2cap_chan *chan, u8 dest_amp_id)
4507 {
4508         struct l2cap_move_chan_req req;
4509         u8 ident;
4510
4511         BT_DBG("chan %p, dest_amp_id %d", chan, dest_amp_id);
4512
4513         ident = l2cap_get_ident(chan->conn);
4514         chan->ident = ident;
4515
4516         req.icid = cpu_to_le16(chan->scid);
4517         req.dest_amp_id = dest_amp_id;
4518
4519         l2cap_send_cmd(chan->conn, ident, L2CAP_MOVE_CHAN_REQ, sizeof(req),
4520                        &req);
4521
4522         __set_chan_timer(chan, L2CAP_MOVE_TIMEOUT);
4523 }
4524
4525 static void l2cap_send_move_chan_rsp(struct l2cap_chan *chan, u16 result)
4526 {
4527         struct l2cap_move_chan_rsp rsp;
4528
4529         BT_DBG("chan %p, result 0x%4.4x", chan, result);
4530
4531         rsp.icid = cpu_to_le16(chan->dcid);
4532         rsp.result = cpu_to_le16(result);
4533
4534         l2cap_send_cmd(chan->conn, chan->ident, L2CAP_MOVE_CHAN_RSP,
4535                        sizeof(rsp), &rsp);
4536 }
4537
4538 static void l2cap_send_move_chan_cfm(struct l2cap_chan *chan, u16 result)
4539 {
4540         struct l2cap_move_chan_cfm cfm;
4541
4542         BT_DBG("chan %p, result 0x%4.4x", chan, result);
4543
4544         chan->ident = l2cap_get_ident(chan->conn);
4545
4546         cfm.icid = cpu_to_le16(chan->scid);
4547         cfm.result = cpu_to_le16(result);
4548
4549         l2cap_send_cmd(chan->conn, chan->ident, L2CAP_MOVE_CHAN_CFM,
4550                        sizeof(cfm), &cfm);
4551
4552         __set_chan_timer(chan, L2CAP_MOVE_TIMEOUT);
4553 }
4554
4555 static void l2cap_send_move_chan_cfm_icid(struct l2cap_conn *conn, u16 icid)
4556 {
4557         struct l2cap_move_chan_cfm cfm;
4558
4559         BT_DBG("conn %p, icid 0x%4.4x", conn, icid);
4560
4561         cfm.icid = cpu_to_le16(icid);
4562         cfm.result = __constant_cpu_to_le16(L2CAP_MC_UNCONFIRMED);
4563
4564         l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_MOVE_CHAN_CFM,
4565                        sizeof(cfm), &cfm);
4566 }
4567
4568 static void l2cap_send_move_chan_cfm_rsp(struct l2cap_conn *conn, u8 ident,
4569                                          u16 icid)
4570 {
4571         struct l2cap_move_chan_cfm_rsp rsp;
4572
4573         BT_DBG("icid 0x%4.4x", icid);
4574
4575         rsp.icid = cpu_to_le16(icid);
4576         l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM_RSP, sizeof(rsp), &rsp);
4577 }
4578
4579 static void __release_logical_link(struct l2cap_chan *chan)
4580 {
4581         chan->hs_hchan = NULL;
4582         chan->hs_hcon = NULL;
4583
4584         /* Placeholder - release the logical link */
4585 }
4586
4587 static void l2cap_logical_fail(struct l2cap_chan *chan)
4588 {
4589         /* Logical link setup failed */
4590         if (chan->state != BT_CONNECTED) {
4591                 /* Create channel failure, disconnect */
4592                 l2cap_send_disconn_req(chan, ECONNRESET);
4593                 return;
4594         }
4595
4596         switch (chan->move_role) {
4597         case L2CAP_MOVE_ROLE_RESPONDER:
4598                 l2cap_move_done(chan);
4599                 l2cap_send_move_chan_rsp(chan, L2CAP_MR_NOT_SUPP);
4600                 break;
4601         case L2CAP_MOVE_ROLE_INITIATOR:
4602                 if (chan->move_state == L2CAP_MOVE_WAIT_LOGICAL_COMP ||
4603                     chan->move_state == L2CAP_MOVE_WAIT_LOGICAL_CFM) {
4604                         /* Remote has only sent pending or
4605                          * success responses, clean up
4606                          */
4607                         l2cap_move_done(chan);
4608                 }
4609
4610                 /* Other amp move states imply that the move
4611                  * has already aborted
4612                  */
4613                 l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
4614                 break;
4615         }
4616 }
4617
4618 static void l2cap_logical_finish_create(struct l2cap_chan *chan,
4619                                         struct hci_chan *hchan)
4620 {
4621         struct l2cap_conf_rsp rsp;
4622
4623         chan->hs_hchan = hchan;
4624         chan->hs_hcon->l2cap_data = chan->conn;
4625
4626         l2cap_send_efs_conf_rsp(chan, &rsp, chan->ident, 0);
4627
4628         if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
4629                 int err;
4630
4631                 set_default_fcs(chan);
4632
4633                 err = l2cap_ertm_init(chan);
4634                 if (err < 0)
4635                         l2cap_send_disconn_req(chan, -err);
4636                 else
4637                         l2cap_chan_ready(chan);
4638         }
4639 }
4640
4641 static void l2cap_logical_finish_move(struct l2cap_chan *chan,
4642                                       struct hci_chan *hchan)
4643 {
4644         chan->hs_hcon = hchan->conn;
4645         chan->hs_hcon->l2cap_data = chan->conn;
4646
4647         BT_DBG("move_state %d", chan->move_state);
4648
4649         switch (chan->move_state) {
4650         case L2CAP_MOVE_WAIT_LOGICAL_COMP:
4651                 /* Move confirm will be sent after a success
4652                  * response is received
4653                  */
4654                 chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
4655                 break;
4656         case L2CAP_MOVE_WAIT_LOGICAL_CFM:
4657                 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
4658                         chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
4659                 } else if (chan->move_role == L2CAP_MOVE_ROLE_INITIATOR) {
4660                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM_RSP;
4661                         l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
4662                 } else if (chan->move_role == L2CAP_MOVE_ROLE_RESPONDER) {
4663                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
4664                         l2cap_send_move_chan_rsp(chan, L2CAP_MR_SUCCESS);
4665                 }
4666                 break;
4667         default:
4668                 /* Move was not in expected state, free the channel */
4669                 __release_logical_link(chan);
4670
4671                 chan->move_state = L2CAP_MOVE_STABLE;
4672         }
4673 }
4674
4675 /* Call with chan locked */
4676 void l2cap_logical_cfm(struct l2cap_chan *chan, struct hci_chan *hchan,
4677                        u8 status)
4678 {
4679         BT_DBG("chan %p, hchan %p, status %d", chan, hchan, status);
4680
4681         if (status) {
4682                 l2cap_logical_fail(chan);
4683                 __release_logical_link(chan);
4684                 return;
4685         }
4686
4687         if (chan->state != BT_CONNECTED) {
4688                 /* Ignore logical link if channel is on BR/EDR */
4689                 if (chan->local_amp_id != AMP_ID_BREDR)
4690                         l2cap_logical_finish_create(chan, hchan);
4691         } else {
4692                 l2cap_logical_finish_move(chan, hchan);
4693         }
4694 }
4695
4696 void l2cap_move_start(struct l2cap_chan *chan)
4697 {
4698         BT_DBG("chan %p", chan);
4699
4700         if (chan->local_amp_id == AMP_ID_BREDR) {
4701                 if (chan->chan_policy != BT_CHANNEL_POLICY_AMP_PREFERRED)
4702                         return;
4703                 chan->move_role = L2CAP_MOVE_ROLE_INITIATOR;
4704                 chan->move_state = L2CAP_MOVE_WAIT_PREPARE;
4705                 /* Placeholder - start physical link setup */
4706         } else {
4707                 chan->move_role = L2CAP_MOVE_ROLE_INITIATOR;
4708                 chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
4709                 chan->move_id = 0;
4710                 l2cap_move_setup(chan);
4711                 l2cap_send_move_chan_req(chan, 0);
4712         }
4713 }
4714
4715 static void l2cap_do_create(struct l2cap_chan *chan, int result,
4716                             u8 local_amp_id, u8 remote_amp_id)
4717 {
4718         BT_DBG("chan %p state %s %u -> %u", chan, state_to_string(chan->state),
4719                local_amp_id, remote_amp_id);
4720
4721         chan->fcs = L2CAP_FCS_NONE;
4722
4723         /* Outgoing channel on AMP */
4724         if (chan->state == BT_CONNECT) {
4725                 if (result == L2CAP_CR_SUCCESS) {
4726                         chan->local_amp_id = local_amp_id;
4727                         l2cap_send_create_chan_req(chan, remote_amp_id);
4728                 } else {
4729                         /* Revert to BR/EDR connect */
4730                         l2cap_send_conn_req(chan);
4731                 }
4732
4733                 return;
4734         }
4735
4736         /* Incoming channel on AMP */
4737         if (__l2cap_no_conn_pending(chan)) {
4738                 struct l2cap_conn_rsp rsp;
4739                 char buf[128];
4740                 rsp.scid = cpu_to_le16(chan->dcid);
4741                 rsp.dcid = cpu_to_le16(chan->scid);
4742
4743                 if (result == L2CAP_CR_SUCCESS) {
4744                         /* Send successful response */
4745                         rsp.result = __constant_cpu_to_le16(L2CAP_CR_SUCCESS);
4746                         rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
4747                 } else {
4748                         /* Send negative response */
4749                         rsp.result = __constant_cpu_to_le16(L2CAP_CR_NO_MEM);
4750                         rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
4751                 }
4752
4753                 l2cap_send_cmd(chan->conn, chan->ident, L2CAP_CREATE_CHAN_RSP,
4754                                sizeof(rsp), &rsp);
4755
4756                 if (result == L2CAP_CR_SUCCESS) {
4757                         l2cap_state_change(chan, BT_CONFIG);
4758                         set_bit(CONF_REQ_SENT, &chan->conf_state);
4759                         l2cap_send_cmd(chan->conn, l2cap_get_ident(chan->conn),
4760                                        L2CAP_CONF_REQ,
4761                                        l2cap_build_conf_req(chan, buf), buf);
4762                         chan->num_conf_req++;
4763                 }
4764         }
4765 }
4766
4767 static void l2cap_do_move_initiate(struct l2cap_chan *chan, u8 local_amp_id,
4768                                    u8 remote_amp_id)
4769 {
4770         l2cap_move_setup(chan);
4771         chan->move_id = local_amp_id;
4772         chan->move_state = L2CAP_MOVE_WAIT_RSP;
4773
4774         l2cap_send_move_chan_req(chan, remote_amp_id);
4775 }
4776
4777 static void l2cap_do_move_respond(struct l2cap_chan *chan, int result)
4778 {
4779         struct hci_chan *hchan = NULL;
4780
4781         /* Placeholder - get hci_chan for logical link */
4782
4783         if (hchan) {
4784                 if (hchan->state == BT_CONNECTED) {
4785                         /* Logical link is ready to go */
4786                         chan->hs_hcon = hchan->conn;
4787                         chan->hs_hcon->l2cap_data = chan->conn;
4788                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
4789                         l2cap_send_move_chan_rsp(chan, L2CAP_MR_SUCCESS);
4790
4791                         l2cap_logical_cfm(chan, hchan, L2CAP_MR_SUCCESS);
4792                 } else {
4793                         /* Wait for logical link to be ready */
4794                         chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
4795                 }
4796         } else {
4797                 /* Logical link not available */
4798                 l2cap_send_move_chan_rsp(chan, L2CAP_MR_NOT_ALLOWED);
4799         }
4800 }
4801
4802 static void l2cap_do_move_cancel(struct l2cap_chan *chan, int result)
4803 {
4804         if (chan->move_role == L2CAP_MOVE_ROLE_RESPONDER) {
4805                 u8 rsp_result;
4806                 if (result == -EINVAL)
4807                         rsp_result = L2CAP_MR_BAD_ID;
4808                 else
4809                         rsp_result = L2CAP_MR_NOT_ALLOWED;
4810
4811                 l2cap_send_move_chan_rsp(chan, rsp_result);
4812         }
4813
4814         chan->move_role = L2CAP_MOVE_ROLE_NONE;
4815         chan->move_state = L2CAP_MOVE_STABLE;
4816
4817         /* Restart data transmission */
4818         l2cap_ertm_send(chan);
4819 }
4820
4821 /* Invoke with locked chan */
4822 void __l2cap_physical_cfm(struct l2cap_chan *chan, int result)
4823 {
4824         u8 local_amp_id = chan->local_amp_id;
4825         u8 remote_amp_id = chan->remote_amp_id;
4826
4827         BT_DBG("chan %p, result %d, local_amp_id %d, remote_amp_id %d",
4828                chan, result, local_amp_id, remote_amp_id);
4829
4830         if (chan->state == BT_DISCONN || chan->state == BT_CLOSED) {
4831                 l2cap_chan_unlock(chan);
4832                 return;
4833         }
4834
4835         if (chan->state != BT_CONNECTED) {
4836                 l2cap_do_create(chan, result, local_amp_id, remote_amp_id);
4837         } else if (result != L2CAP_MR_SUCCESS) {
4838                 l2cap_do_move_cancel(chan, result);
4839         } else {
4840                 switch (chan->move_role) {
4841                 case L2CAP_MOVE_ROLE_INITIATOR:
4842                         l2cap_do_move_initiate(chan, local_amp_id,
4843                                                remote_amp_id);
4844                         break;
4845                 case L2CAP_MOVE_ROLE_RESPONDER:
4846                         l2cap_do_move_respond(chan, result);
4847                         break;
4848                 default:
4849                         l2cap_do_move_cancel(chan, result);
4850                         break;
4851                 }
4852         }
4853 }
4854
4855 static inline int l2cap_move_channel_req(struct l2cap_conn *conn,
4856                                          struct l2cap_cmd_hdr *cmd,
4857                                          u16 cmd_len, void *data)
4858 {
4859         struct l2cap_move_chan_req *req = data;
4860         struct l2cap_move_chan_rsp rsp;
4861         struct l2cap_chan *chan;
4862         u16 icid = 0;
4863         u16 result = L2CAP_MR_NOT_ALLOWED;
4864
4865         if (cmd_len != sizeof(*req))
4866                 return -EPROTO;
4867
4868         icid = le16_to_cpu(req->icid);
4869
4870         BT_DBG("icid 0x%4.4x, dest_amp_id %d", icid, req->dest_amp_id);
4871
4872         if (!conn->hs_enabled)
4873                 return -EINVAL;
4874
4875         chan = l2cap_get_chan_by_dcid(conn, icid);
4876         if (!chan) {
4877                 rsp.icid = cpu_to_le16(icid);
4878                 rsp.result = __constant_cpu_to_le16(L2CAP_MR_NOT_ALLOWED);
4879                 l2cap_send_cmd(conn, cmd->ident, L2CAP_MOVE_CHAN_RSP,
4880                                sizeof(rsp), &rsp);
4881                 return 0;
4882         }
4883
4884         chan->ident = cmd->ident;
4885
4886         if (chan->scid < L2CAP_CID_DYN_START ||
4887             chan->chan_policy == BT_CHANNEL_POLICY_BREDR_ONLY ||
4888             (chan->mode != L2CAP_MODE_ERTM &&
4889              chan->mode != L2CAP_MODE_STREAMING)) {
4890                 result = L2CAP_MR_NOT_ALLOWED;
4891                 goto send_move_response;
4892         }
4893
4894         if (chan->local_amp_id == req->dest_amp_id) {
4895                 result = L2CAP_MR_SAME_ID;
4896                 goto send_move_response;
4897         }
4898
4899         if (req->dest_amp_id != AMP_ID_BREDR) {
4900                 struct hci_dev *hdev;
4901                 hdev = hci_dev_get(req->dest_amp_id);
4902                 if (!hdev || hdev->dev_type != HCI_AMP ||
4903                     !test_bit(HCI_UP, &hdev->flags)) {
4904                         if (hdev)
4905                                 hci_dev_put(hdev);
4906
4907                         result = L2CAP_MR_BAD_ID;
4908                         goto send_move_response;
4909                 }
4910                 hci_dev_put(hdev);
4911         }
4912
4913         /* Detect a move collision.  Only send a collision response
4914          * if this side has "lost", otherwise proceed with the move.
4915          * The winner has the larger bd_addr.
4916          */
4917         if ((__chan_is_moving(chan) ||
4918              chan->move_role != L2CAP_MOVE_ROLE_NONE) &&
4919             bacmp(&conn->hcon->src, &conn->hcon->dst) > 0) {
4920                 result = L2CAP_MR_COLLISION;
4921                 goto send_move_response;
4922         }
4923
4924         chan->move_role = L2CAP_MOVE_ROLE_RESPONDER;
4925         l2cap_move_setup(chan);
4926         chan->move_id = req->dest_amp_id;
4927         icid = chan->dcid;
4928
4929         if (req->dest_amp_id == AMP_ID_BREDR) {
4930                 /* Moving to BR/EDR */
4931                 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
4932                         chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
4933                         result = L2CAP_MR_PEND;
4934                 } else {
4935                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
4936                         result = L2CAP_MR_SUCCESS;
4937                 }
4938         } else {
4939                 chan->move_state = L2CAP_MOVE_WAIT_PREPARE;
4940                 /* Placeholder - uncomment when amp functions are available */
4941                 /*amp_accept_physical(chan, req->dest_amp_id);*/
4942                 result = L2CAP_MR_PEND;
4943         }
4944
4945 send_move_response:
4946         l2cap_send_move_chan_rsp(chan, result);
4947
4948         l2cap_chan_unlock(chan);
4949
4950         return 0;
4951 }
4952
4953 static void l2cap_move_continue(struct l2cap_conn *conn, u16 icid, u16 result)
4954 {
4955         struct l2cap_chan *chan;
4956         struct hci_chan *hchan = NULL;
4957
4958         chan = l2cap_get_chan_by_scid(conn, icid);
4959         if (!chan) {
4960                 l2cap_send_move_chan_cfm_icid(conn, icid);
4961                 return;
4962         }
4963
4964         __clear_chan_timer(chan);
4965         if (result == L2CAP_MR_PEND)
4966                 __set_chan_timer(chan, L2CAP_MOVE_ERTX_TIMEOUT);
4967
4968         switch (chan->move_state) {
4969         case L2CAP_MOVE_WAIT_LOGICAL_COMP:
4970                 /* Move confirm will be sent when logical link
4971                  * is complete.
4972                  */
4973                 chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
4974                 break;
4975         case L2CAP_MOVE_WAIT_RSP_SUCCESS:
4976                 if (result == L2CAP_MR_PEND) {
4977                         break;
4978                 } else if (test_bit(CONN_LOCAL_BUSY,
4979                                     &chan->conn_state)) {
4980                         chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
4981                 } else {
4982                         /* Logical link is up or moving to BR/EDR,
4983                          * proceed with move
4984                          */
4985                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM_RSP;
4986                         l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
4987                 }
4988                 break;
4989         case L2CAP_MOVE_WAIT_RSP:
4990                 /* Moving to AMP */
4991                 if (result == L2CAP_MR_SUCCESS) {
4992                         /* Remote is ready, send confirm immediately
4993                          * after logical link is ready
4994                          */
4995                         chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
4996                 } else {
4997                         /* Both logical link and move success
4998                          * are required to confirm
4999                          */
5000                         chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_COMP;
5001                 }
5002
5003                 /* Placeholder - get hci_chan for logical link */
5004                 if (!hchan) {
5005                         /* Logical link not available */
5006                         l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
5007                         break;
5008                 }
5009
5010                 /* If the logical link is not yet connected, do not
5011                  * send confirmation.
5012                  */
5013                 if (hchan->state != BT_CONNECTED)
5014                         break;
5015
5016                 /* Logical link is already ready to go */
5017
5018                 chan->hs_hcon = hchan->conn;
5019                 chan->hs_hcon->l2cap_data = chan->conn;
5020
5021                 if (result == L2CAP_MR_SUCCESS) {
5022                         /* Can confirm now */
5023                         l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
5024                 } else {
5025                         /* Now only need move success
5026                          * to confirm
5027                          */
5028                         chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
5029                 }
5030
5031                 l2cap_logical_cfm(chan, hchan, L2CAP_MR_SUCCESS);
5032                 break;
5033         default:
5034                 /* Any other amp move state means the move failed. */
5035                 chan->move_id = chan->local_amp_id;
5036                 l2cap_move_done(chan);
5037                 l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
5038         }
5039
5040         l2cap_chan_unlock(chan);
5041 }
5042
5043 static void l2cap_move_fail(struct l2cap_conn *conn, u8 ident, u16 icid,
5044                             u16 result)
5045 {
5046         struct l2cap_chan *chan;
5047
5048         chan = l2cap_get_chan_by_ident(conn, ident);
5049         if (!chan) {
5050                 /* Could not locate channel, icid is best guess */
5051                 l2cap_send_move_chan_cfm_icid(conn, icid);
5052                 return;
5053         }
5054
5055         __clear_chan_timer(chan);
5056
5057         if (chan->move_role == L2CAP_MOVE_ROLE_INITIATOR) {
5058                 if (result == L2CAP_MR_COLLISION) {
5059                         chan->move_role = L2CAP_MOVE_ROLE_RESPONDER;
5060                 } else {
5061                         /* Cleanup - cancel move */
5062                         chan->move_id = chan->local_amp_id;
5063                         l2cap_move_done(chan);
5064                 }
5065         }
5066
5067         l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
5068
5069         l2cap_chan_unlock(chan);
5070 }
5071
5072 static int l2cap_move_channel_rsp(struct l2cap_conn *conn,
5073                                   struct l2cap_cmd_hdr *cmd,
5074                                   u16 cmd_len, void *data)
5075 {
5076         struct l2cap_move_chan_rsp *rsp = data;
5077         u16 icid, result;
5078
5079         if (cmd_len != sizeof(*rsp))
5080                 return -EPROTO;
5081
5082         icid = le16_to_cpu(rsp->icid);
5083         result = le16_to_cpu(rsp->result);
5084
5085         BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result);
5086
5087         if (result == L2CAP_MR_SUCCESS || result == L2CAP_MR_PEND)
5088                 l2cap_move_continue(conn, icid, result);
5089         else
5090                 l2cap_move_fail(conn, cmd->ident, icid, result);
5091
5092         return 0;
5093 }
5094
5095 static int l2cap_move_channel_confirm(struct l2cap_conn *conn,
5096                                       struct l2cap_cmd_hdr *cmd,
5097                                       u16 cmd_len, void *data)
5098 {
5099         struct l2cap_move_chan_cfm *cfm = data;
5100         struct l2cap_chan *chan;
5101         u16 icid, result;
5102
5103         if (cmd_len != sizeof(*cfm))
5104                 return -EPROTO;
5105
5106         icid = le16_to_cpu(cfm->icid);
5107         result = le16_to_cpu(cfm->result);
5108
5109         BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result);
5110
5111         chan = l2cap_get_chan_by_dcid(conn, icid);
5112         if (!chan) {
5113                 /* Spec requires a response even if the icid was not found */
5114                 l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid);
5115                 return 0;
5116         }
5117
5118         if (chan->move_state == L2CAP_MOVE_WAIT_CONFIRM) {
5119                 if (result == L2CAP_MC_CONFIRMED) {
5120                         chan->local_amp_id = chan->move_id;
5121                         if (chan->local_amp_id == AMP_ID_BREDR)
5122                                 __release_logical_link(chan);
5123                 } else {
5124                         chan->move_id = chan->local_amp_id;
5125                 }
5126
5127                 l2cap_move_done(chan);
5128         }
5129
5130         l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid);
5131
5132         l2cap_chan_unlock(chan);
5133
5134         return 0;
5135 }
5136
5137 static inline int l2cap_move_channel_confirm_rsp(struct l2cap_conn *conn,
5138                                                  struct l2cap_cmd_hdr *cmd,
5139                                                  u16 cmd_len, void *data)
5140 {
5141         struct l2cap_move_chan_cfm_rsp *rsp = data;
5142         struct l2cap_chan *chan;
5143         u16 icid;
5144
5145         if (cmd_len != sizeof(*rsp))
5146                 return -EPROTO;
5147
5148         icid = le16_to_cpu(rsp->icid);
5149
5150         BT_DBG("icid 0x%4.4x", icid);
5151
5152         chan = l2cap_get_chan_by_scid(conn, icid);
5153         if (!chan)
5154                 return 0;
5155
5156         __clear_chan_timer(chan);
5157
5158         if (chan->move_state == L2CAP_MOVE_WAIT_CONFIRM_RSP) {
5159                 chan->local_amp_id = chan->move_id;
5160
5161                 if (chan->local_amp_id == AMP_ID_BREDR && chan->hs_hchan)
5162                         __release_logical_link(chan);
5163
5164                 l2cap_move_done(chan);
5165         }
5166
5167         l2cap_chan_unlock(chan);
5168
5169         return 0;
5170 }
5171
5172 static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency,
5173                                          u16 to_multiplier)
5174 {
5175         u16 max_latency;
5176
5177         if (min > max || min < 6 || max > 3200)
5178                 return -EINVAL;
5179
5180         if (to_multiplier < 10 || to_multiplier > 3200)
5181                 return -EINVAL;
5182
5183         if (max >= to_multiplier * 8)
5184                 return -EINVAL;
5185
5186         max_latency = (to_multiplier * 8 / max) - 1;
5187         if (latency > 499 || latency > max_latency)
5188                 return -EINVAL;
5189
5190         return 0;
5191 }
5192
5193 static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
5194                                               struct l2cap_cmd_hdr *cmd,
5195                                               u16 cmd_len, u8 *data)
5196 {
5197         struct hci_conn *hcon = conn->hcon;
5198         struct l2cap_conn_param_update_req *req;
5199         struct l2cap_conn_param_update_rsp rsp;
5200         u16 min, max, latency, to_multiplier;
5201         int err;
5202
5203         if (!(hcon->link_mode & HCI_LM_MASTER))
5204                 return -EINVAL;
5205
5206         if (cmd_len != sizeof(struct l2cap_conn_param_update_req))
5207                 return -EPROTO;
5208
5209         req = (struct l2cap_conn_param_update_req *) data;
5210         min             = __le16_to_cpu(req->min);
5211         max             = __le16_to_cpu(req->max);
5212         latency         = __le16_to_cpu(req->latency);
5213         to_multiplier   = __le16_to_cpu(req->to_multiplier);
5214
5215         BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x",
5216                min, max, latency, to_multiplier);
5217
5218         memset(&rsp, 0, sizeof(rsp));
5219
5220         err = l2cap_check_conn_param(min, max, latency, to_multiplier);
5221         if (err)
5222                 rsp.result = __constant_cpu_to_le16(L2CAP_CONN_PARAM_REJECTED);
5223         else
5224                 rsp.result = __constant_cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED);
5225
5226         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP,
5227                        sizeof(rsp), &rsp);
5228
5229         if (!err)
5230                 hci_le_conn_update(hcon, min, max, latency, to_multiplier);
5231
5232         return 0;
5233 }
5234
5235 static int l2cap_le_connect_rsp(struct l2cap_conn *conn,
5236                                 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5237                                 u8 *data)
5238 {
5239         struct l2cap_le_conn_rsp *rsp = (struct l2cap_le_conn_rsp *) data;
5240         u16 dcid, mtu, mps, credits, result;
5241         struct l2cap_chan *chan;
5242         int err;
5243
5244         if (cmd_len < sizeof(*rsp))
5245                 return -EPROTO;
5246
5247         dcid    = __le16_to_cpu(rsp->dcid);
5248         mtu     = __le16_to_cpu(rsp->mtu);
5249         mps     = __le16_to_cpu(rsp->mps);
5250         credits = __le16_to_cpu(rsp->credits);
5251         result  = __le16_to_cpu(rsp->result);
5252
5253         if (result == L2CAP_CR_SUCCESS && (mtu < 23 || mps < 23))
5254                 return -EPROTO;
5255
5256         BT_DBG("dcid 0x%4.4x mtu %u mps %u credits %u result 0x%2.2x",
5257                dcid, mtu, mps, credits, result);
5258
5259         mutex_lock(&conn->chan_lock);
5260
5261         chan = __l2cap_get_chan_by_ident(conn, cmd->ident);
5262         if (!chan) {
5263                 err = -EBADSLT;
5264                 goto unlock;
5265         }
5266
5267         err = 0;
5268
5269         l2cap_chan_lock(chan);
5270
5271         switch (result) {
5272         case L2CAP_CR_SUCCESS:
5273                 chan->ident = 0;
5274                 chan->dcid = dcid;
5275                 chan->omtu = mtu;
5276                 chan->remote_mps = mps;
5277                 chan->tx_credits = credits;
5278                 l2cap_chan_ready(chan);
5279                 break;
5280
5281         default:
5282                 l2cap_chan_del(chan, ECONNREFUSED);
5283                 break;
5284         }
5285
5286         l2cap_chan_unlock(chan);
5287
5288 unlock:
5289         mutex_unlock(&conn->chan_lock);
5290
5291         return err;
5292 }
5293
5294 static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
5295                                       struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5296                                       u8 *data)
5297 {
5298         int err = 0;
5299
5300         switch (cmd->code) {
5301         case L2CAP_COMMAND_REJ:
5302                 l2cap_command_rej(conn, cmd, cmd_len, data);
5303                 break;
5304
5305         case L2CAP_CONN_REQ:
5306                 err = l2cap_connect_req(conn, cmd, cmd_len, data);
5307                 break;
5308
5309         case L2CAP_CONN_RSP:
5310         case L2CAP_CREATE_CHAN_RSP:
5311                 l2cap_connect_create_rsp(conn, cmd, cmd_len, data);
5312                 break;
5313
5314         case L2CAP_CONF_REQ:
5315                 err = l2cap_config_req(conn, cmd, cmd_len, data);
5316                 break;
5317
5318         case L2CAP_CONF_RSP:
5319                 l2cap_config_rsp(conn, cmd, cmd_len, data);
5320                 break;
5321
5322         case L2CAP_DISCONN_REQ:
5323                 err = l2cap_disconnect_req(conn, cmd, cmd_len, data);
5324                 break;
5325
5326         case L2CAP_DISCONN_RSP:
5327                 l2cap_disconnect_rsp(conn, cmd, cmd_len, data);
5328                 break;
5329
5330         case L2CAP_ECHO_REQ:
5331                 l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data);
5332                 break;
5333
5334         case L2CAP_ECHO_RSP:
5335                 break;
5336
5337         case L2CAP_INFO_REQ:
5338                 err = l2cap_information_req(conn, cmd, cmd_len, data);
5339                 break;
5340
5341         case L2CAP_INFO_RSP:
5342                 l2cap_information_rsp(conn, cmd, cmd_len, data);
5343                 break;
5344
5345         case L2CAP_CREATE_CHAN_REQ:
5346                 err = l2cap_create_channel_req(conn, cmd, cmd_len, data);
5347                 break;
5348
5349         case L2CAP_MOVE_CHAN_REQ:
5350                 err = l2cap_move_channel_req(conn, cmd, cmd_len, data);
5351                 break;
5352
5353         case L2CAP_MOVE_CHAN_RSP:
5354                 l2cap_move_channel_rsp(conn, cmd, cmd_len, data);
5355                 break;
5356
5357         case L2CAP_MOVE_CHAN_CFM:
5358                 err = l2cap_move_channel_confirm(conn, cmd, cmd_len, data);
5359                 break;
5360
5361         case L2CAP_MOVE_CHAN_CFM_RSP:
5362                 l2cap_move_channel_confirm_rsp(conn, cmd, cmd_len, data);
5363                 break;
5364
5365         default:
5366                 BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code);
5367                 err = -EINVAL;
5368                 break;
5369         }
5370
5371         return err;
5372 }
5373
5374 static int l2cap_le_connect_req(struct l2cap_conn *conn,
5375                                 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5376                                 u8 *data)
5377 {
5378         struct l2cap_le_conn_req *req = (struct l2cap_le_conn_req *) data;
5379         struct l2cap_le_conn_rsp rsp;
5380         struct l2cap_chan *chan, *pchan;
5381         u16 dcid, scid, credits, mtu, mps;
5382         __le16 psm;
5383         u8 result;
5384
5385         if (cmd_len != sizeof(*req))
5386                 return -EPROTO;
5387
5388         scid = __le16_to_cpu(req->scid);
5389         mtu  = __le16_to_cpu(req->mtu);
5390         mps  = __le16_to_cpu(req->mps);
5391         psm  = req->psm;
5392         dcid = 0;
5393         credits = 0;
5394
5395         if (mtu < 23 || mps < 23)
5396                 return -EPROTO;
5397
5398         BT_DBG("psm 0x%2.2x scid 0x%4.4x mtu %u mps %u", __le16_to_cpu(psm),
5399                scid, mtu, mps);
5400
5401         /* Check if we have socket listening on psm */
5402         pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, &conn->hcon->src,
5403                                          &conn->hcon->dst, LE_LINK);
5404         if (!pchan) {
5405                 result = L2CAP_CR_BAD_PSM;
5406                 chan = NULL;
5407                 goto response;
5408         }
5409
5410         mutex_lock(&conn->chan_lock);
5411         l2cap_chan_lock(pchan);
5412
5413         if (!smp_sufficient_security(conn->hcon, pchan->sec_level)) {
5414                 result = L2CAP_CR_AUTHENTICATION;
5415                 chan = NULL;
5416                 goto response_unlock;
5417         }
5418
5419         /* Check if we already have channel with that dcid */
5420         if (__l2cap_get_chan_by_dcid(conn, scid)) {
5421                 result = L2CAP_CR_NO_MEM;
5422                 chan = NULL;
5423                 goto response_unlock;
5424         }
5425
5426         chan = pchan->ops->new_connection(pchan);
5427         if (!chan) {
5428                 result = L2CAP_CR_NO_MEM;
5429                 goto response_unlock;
5430         }
5431
5432         l2cap_le_flowctl_init(chan);
5433
5434         bacpy(&chan->src, &conn->hcon->src);
5435         bacpy(&chan->dst, &conn->hcon->dst);
5436         chan->src_type = bdaddr_type(conn->hcon, conn->hcon->src_type);
5437         chan->dst_type = bdaddr_type(conn->hcon, conn->hcon->dst_type);
5438         chan->psm  = psm;
5439         chan->dcid = scid;
5440         chan->omtu = mtu;
5441         chan->remote_mps = mps;
5442         chan->tx_credits = __le16_to_cpu(req->credits);
5443
5444         __l2cap_chan_add(conn, chan);
5445         dcid = chan->scid;
5446         credits = chan->rx_credits;
5447
5448         __set_chan_timer(chan, chan->ops->get_sndtimeo(chan));
5449
5450         chan->ident = cmd->ident;
5451
5452         if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
5453                 l2cap_state_change(chan, BT_CONNECT2);
5454                 result = L2CAP_CR_PEND;
5455                 chan->ops->defer(chan);
5456         } else {
5457                 l2cap_chan_ready(chan);
5458                 result = L2CAP_CR_SUCCESS;
5459         }
5460
5461 response_unlock:
5462         l2cap_chan_unlock(pchan);
5463         mutex_unlock(&conn->chan_lock);
5464
5465         if (result == L2CAP_CR_PEND)
5466                 return 0;
5467
5468 response:
5469         if (chan) {
5470                 rsp.mtu = cpu_to_le16(chan->imtu);
5471                 rsp.mps = cpu_to_le16(chan->mps);
5472         } else {
5473                 rsp.mtu = 0;
5474                 rsp.mps = 0;
5475         }
5476
5477         rsp.dcid    = cpu_to_le16(dcid);
5478         rsp.credits = cpu_to_le16(credits);
5479         rsp.result  = cpu_to_le16(result);
5480
5481         l2cap_send_cmd(conn, cmd->ident, L2CAP_LE_CONN_RSP, sizeof(rsp), &rsp);
5482
5483         return 0;
5484 }
5485
5486 static inline int l2cap_le_credits(struct l2cap_conn *conn,
5487                                    struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5488                                    u8 *data)
5489 {
5490         struct l2cap_le_credits *pkt;
5491         struct l2cap_chan *chan;
5492         u16 cid, credits, max_credits;
5493
5494         if (cmd_len != sizeof(*pkt))
5495                 return -EPROTO;
5496
5497         pkt = (struct l2cap_le_credits *) data;
5498         cid     = __le16_to_cpu(pkt->cid);
5499         credits = __le16_to_cpu(pkt->credits);
5500
5501         BT_DBG("cid 0x%4.4x credits 0x%4.4x", cid, credits);
5502
5503         chan = l2cap_get_chan_by_dcid(conn, cid);
5504         if (!chan)
5505                 return -EBADSLT;
5506
5507         max_credits = LE_FLOWCTL_MAX_CREDITS - chan->tx_credits;
5508         if (credits > max_credits) {
5509                 BT_ERR("LE credits overflow");
5510                 l2cap_send_disconn_req(chan, ECONNRESET);
5511
5512                 /* Return 0 so that we don't trigger an unnecessary
5513                  * command reject packet.
5514                  */
5515                 return 0;
5516         }
5517
5518         chan->tx_credits += credits;
5519
5520         while (chan->tx_credits && !skb_queue_empty(&chan->tx_q)) {
5521                 l2cap_do_send(chan, skb_dequeue(&chan->tx_q));
5522                 chan->tx_credits--;
5523         }
5524
5525         if (chan->tx_credits)
5526                 chan->ops->resume(chan);
5527
5528         l2cap_chan_unlock(chan);
5529
5530         return 0;
5531 }
5532
5533 static inline int l2cap_le_command_rej(struct l2cap_conn *conn,
5534                                        struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5535                                        u8 *data)
5536 {
5537         struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data;
5538         struct l2cap_chan *chan;
5539
5540         if (cmd_len < sizeof(*rej))
5541                 return -EPROTO;
5542
5543         mutex_lock(&conn->chan_lock);
5544
5545         chan = __l2cap_get_chan_by_ident(conn, cmd->ident);
5546         if (!chan)
5547                 goto done;
5548
5549         l2cap_chan_lock(chan);
5550         l2cap_chan_del(chan, ECONNREFUSED);
5551         l2cap_chan_unlock(chan);
5552
5553 done:
5554         mutex_unlock(&conn->chan_lock);
5555         return 0;
5556 }
5557
5558 static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
5559                                    struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5560                                    u8 *data)
5561 {
5562         int err = 0;
5563
5564         switch (cmd->code) {
5565         case L2CAP_COMMAND_REJ:
5566                 l2cap_le_command_rej(conn, cmd, cmd_len, data);
5567                 break;
5568
5569         case L2CAP_CONN_PARAM_UPDATE_REQ:
5570                 err = l2cap_conn_param_update_req(conn, cmd, cmd_len, data);
5571                 break;
5572
5573         case L2CAP_CONN_PARAM_UPDATE_RSP:
5574                 break;
5575
5576         case L2CAP_LE_CONN_RSP:
5577                 l2cap_le_connect_rsp(conn, cmd, cmd_len, data);
5578                 break;
5579
5580         case L2CAP_LE_CONN_REQ:
5581                 err = l2cap_le_connect_req(conn, cmd, cmd_len, data);
5582                 break;
5583
5584         case L2CAP_LE_CREDITS:
5585                 err = l2cap_le_credits(conn, cmd, cmd_len, data);
5586                 break;
5587
5588         case L2CAP_DISCONN_REQ:
5589                 err = l2cap_disconnect_req(conn, cmd, cmd_len, data);
5590                 break;
5591
5592         case L2CAP_DISCONN_RSP:
5593                 l2cap_disconnect_rsp(conn, cmd, cmd_len, data);
5594                 break;
5595
5596         default:
5597                 BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code);
5598                 err = -EINVAL;
5599                 break;
5600         }
5601
5602         return err;
5603 }
5604
5605 static inline void l2cap_le_sig_channel(struct l2cap_conn *conn,
5606                                         struct sk_buff *skb)
5607 {
5608         struct hci_conn *hcon = conn->hcon;
5609         struct l2cap_cmd_hdr *cmd;
5610         u16 len;
5611         int err;
5612
5613         if (hcon->type != LE_LINK)
5614                 goto drop;
5615
5616         if (skb->len < L2CAP_CMD_HDR_SIZE)
5617                 goto drop;
5618
5619         cmd = (void *) skb->data;
5620         skb_pull(skb, L2CAP_CMD_HDR_SIZE);
5621
5622         len = le16_to_cpu(cmd->len);
5623
5624         BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd->code, len, cmd->ident);
5625
5626         if (len != skb->len || !cmd->ident) {
5627                 BT_DBG("corrupted command");
5628                 goto drop;
5629         }
5630
5631         err = l2cap_le_sig_cmd(conn, cmd, len, skb->data);
5632         if (err) {
5633                 struct l2cap_cmd_rej_unk rej;
5634
5635                 BT_ERR("Wrong link type (%d)", err);
5636
5637                 rej.reason = __constant_cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
5638                 l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
5639                                sizeof(rej), &rej);
5640         }
5641
5642 drop:
5643         kfree_skb(skb);
5644 }
5645
5646 static inline void l2cap_sig_channel(struct l2cap_conn *conn,
5647                                      struct sk_buff *skb)
5648 {
5649         struct hci_conn *hcon = conn->hcon;
5650         u8 *data = skb->data;
5651         int len = skb->len;
5652         struct l2cap_cmd_hdr cmd;
5653         int err;
5654
5655         l2cap_raw_recv(conn, skb);
5656
5657         if (hcon->type != ACL_LINK)
5658                 goto drop;
5659
5660         while (len >= L2CAP_CMD_HDR_SIZE) {
5661                 u16 cmd_len;
5662                 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
5663                 data += L2CAP_CMD_HDR_SIZE;
5664                 len  -= L2CAP_CMD_HDR_SIZE;
5665
5666                 cmd_len = le16_to_cpu(cmd.len);
5667
5668                 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len,
5669                        cmd.ident);
5670
5671                 if (cmd_len > len || !cmd.ident) {
5672                         BT_DBG("corrupted command");
5673                         break;
5674                 }
5675
5676                 err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data);
5677                 if (err) {
5678                         struct l2cap_cmd_rej_unk rej;
5679
5680                         BT_ERR("Wrong link type (%d)", err);
5681
5682                         rej.reason = __constant_cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
5683                         l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ,
5684                                        sizeof(rej), &rej);
5685                 }
5686
5687                 data += cmd_len;
5688                 len  -= cmd_len;
5689         }
5690
5691 drop:
5692         kfree_skb(skb);
5693 }
5694
5695 static int l2cap_check_fcs(struct l2cap_chan *chan,  struct sk_buff *skb)
5696 {
5697         u16 our_fcs, rcv_fcs;
5698         int hdr_size;
5699
5700         if (test_bit(FLAG_EXT_CTRL, &chan->flags))
5701                 hdr_size = L2CAP_EXT_HDR_SIZE;
5702         else
5703                 hdr_size = L2CAP_ENH_HDR_SIZE;
5704
5705         if (chan->fcs == L2CAP_FCS_CRC16) {
5706                 skb_trim(skb, skb->len - L2CAP_FCS_SIZE);
5707                 rcv_fcs = get_unaligned_le16(skb->data + skb->len);
5708                 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
5709
5710                 if (our_fcs != rcv_fcs)
5711                         return -EBADMSG;
5712         }
5713         return 0;
5714 }
5715
5716 static void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan)
5717 {
5718         struct l2cap_ctrl control;
5719
5720         BT_DBG("chan %p", chan);
5721
5722         memset(&control, 0, sizeof(control));
5723         control.sframe = 1;
5724         control.final = 1;
5725         control.reqseq = chan->buffer_seq;
5726         set_bit(CONN_SEND_FBIT, &chan->conn_state);
5727
5728         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
5729                 control.super = L2CAP_SUPER_RNR;
5730                 l2cap_send_sframe(chan, &control);
5731         }
5732
5733         if (test_and_clear_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
5734             chan->unacked_frames > 0)
5735                 __set_retrans_timer(chan);
5736
5737         /* Send pending iframes */
5738         l2cap_ertm_send(chan);
5739
5740         if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
5741             test_bit(CONN_SEND_FBIT, &chan->conn_state)) {
5742                 /* F-bit wasn't sent in an s-frame or i-frame yet, so
5743                  * send it now.
5744                  */
5745                 control.super = L2CAP_SUPER_RR;
5746                 l2cap_send_sframe(chan, &control);
5747         }
5748 }
5749
5750 static void append_skb_frag(struct sk_buff *skb, struct sk_buff *new_frag,
5751                             struct sk_buff **last_frag)
5752 {
5753         /* skb->len reflects data in skb as well as all fragments
5754          * skb->data_len reflects only data in fragments
5755          */
5756         if (!skb_has_frag_list(skb))
5757                 skb_shinfo(skb)->frag_list = new_frag;
5758
5759         new_frag->next = NULL;
5760
5761         (*last_frag)->next = new_frag;
5762         *last_frag = new_frag;
5763
5764         skb->len += new_frag->len;
5765         skb->data_len += new_frag->len;
5766         skb->truesize += new_frag->truesize;
5767 }
5768
5769 static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb,
5770                                 struct l2cap_ctrl *control)
5771 {
5772         int err = -EINVAL;
5773
5774         switch (control->sar) {
5775         case L2CAP_SAR_UNSEGMENTED:
5776                 if (chan->sdu)
5777                         break;
5778
5779                 err = chan->ops->recv(chan, skb);
5780                 break;
5781
5782         case L2CAP_SAR_START:
5783                 if (chan->sdu)
5784                         break;
5785
5786                 chan->sdu_len = get_unaligned_le16(skb->data);
5787                 skb_pull(skb, L2CAP_SDULEN_SIZE);
5788
5789                 if (chan->sdu_len > chan->imtu) {
5790                         err = -EMSGSIZE;
5791                         break;
5792                 }
5793
5794                 if (skb->len >= chan->sdu_len)
5795                         break;
5796
5797                 chan->sdu = skb;
5798                 chan->sdu_last_frag = skb;
5799
5800                 skb = NULL;
5801                 err = 0;
5802                 break;
5803
5804         case L2CAP_SAR_CONTINUE:
5805                 if (!chan->sdu)
5806                         break;
5807
5808                 append_skb_frag(chan->sdu, skb,
5809                                 &chan->sdu_last_frag);
5810                 skb = NULL;
5811
5812                 if (chan->sdu->len >= chan->sdu_len)
5813                         break;
5814
5815                 err = 0;
5816                 break;
5817
5818         case L2CAP_SAR_END:
5819                 if (!chan->sdu)
5820                         break;
5821
5822                 append_skb_frag(chan->sdu, skb,
5823                                 &chan->sdu_last_frag);
5824                 skb = NULL;
5825
5826                 if (chan->sdu->len != chan->sdu_len)
5827                         break;
5828
5829                 err = chan->ops->recv(chan, chan->sdu);
5830
5831                 if (!err) {
5832                         /* Reassembly complete */
5833                         chan->sdu = NULL;
5834                         chan->sdu_last_frag = NULL;
5835                         chan->sdu_len = 0;
5836                 }
5837                 break;
5838         }
5839
5840         if (err) {
5841                 kfree_skb(skb);
5842                 kfree_skb(chan->sdu);
5843                 chan->sdu = NULL;
5844                 chan->sdu_last_frag = NULL;
5845                 chan->sdu_len = 0;
5846         }
5847
5848         return err;
5849 }
5850
5851 static int l2cap_resegment(struct l2cap_chan *chan)
5852 {
5853         /* Placeholder */
5854         return 0;
5855 }
5856
5857 void l2cap_chan_busy(struct l2cap_chan *chan, int busy)
5858 {
5859         u8 event;
5860
5861         if (chan->mode != L2CAP_MODE_ERTM)
5862                 return;
5863
5864         event = busy ? L2CAP_EV_LOCAL_BUSY_DETECTED : L2CAP_EV_LOCAL_BUSY_CLEAR;
5865         l2cap_tx(chan, NULL, NULL, event);
5866 }
5867
5868 static int l2cap_rx_queued_iframes(struct l2cap_chan *chan)
5869 {
5870         int err = 0;
5871         /* Pass sequential frames to l2cap_reassemble_sdu()
5872          * until a gap is encountered.
5873          */
5874
5875         BT_DBG("chan %p", chan);
5876
5877         while (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
5878                 struct sk_buff *skb;
5879                 BT_DBG("Searching for skb with txseq %d (queue len %d)",
5880                        chan->buffer_seq, skb_queue_len(&chan->srej_q));
5881
5882                 skb = l2cap_ertm_seq_in_queue(&chan->srej_q, chan->buffer_seq);
5883
5884                 if (!skb)
5885                         break;
5886
5887                 skb_unlink(skb, &chan->srej_q);
5888                 chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
5889                 err = l2cap_reassemble_sdu(chan, skb, &bt_cb(skb)->control);
5890                 if (err)
5891                         break;
5892         }
5893
5894         if (skb_queue_empty(&chan->srej_q)) {
5895                 chan->rx_state = L2CAP_RX_STATE_RECV;
5896                 l2cap_send_ack(chan);
5897         }
5898
5899         return err;
5900 }
5901
5902 static void l2cap_handle_srej(struct l2cap_chan *chan,
5903                               struct l2cap_ctrl *control)
5904 {
5905         struct sk_buff *skb;
5906
5907         BT_DBG("chan %p, control %p", chan, control);
5908
5909         if (control->reqseq == chan->next_tx_seq) {
5910                 BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq);
5911                 l2cap_send_disconn_req(chan, ECONNRESET);
5912                 return;
5913         }
5914
5915         skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq);
5916
5917         if (skb == NULL) {
5918                 BT_DBG("Seq %d not available for retransmission",
5919                        control->reqseq);
5920                 return;
5921         }
5922
5923         if (chan->max_tx != 0 && bt_cb(skb)->control.retries >= chan->max_tx) {
5924                 BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
5925                 l2cap_send_disconn_req(chan, ECONNRESET);
5926                 return;
5927         }
5928
5929         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
5930
5931         if (control->poll) {
5932                 l2cap_pass_to_tx(chan, control);
5933
5934                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
5935                 l2cap_retransmit(chan, control);
5936                 l2cap_ertm_send(chan);
5937
5938                 if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) {
5939                         set_bit(CONN_SREJ_ACT, &chan->conn_state);
5940                         chan->srej_save_reqseq = control->reqseq;
5941                 }
5942         } else {
5943                 l2cap_pass_to_tx_fbit(chan, control);
5944
5945                 if (control->final) {
5946                         if (chan->srej_save_reqseq != control->reqseq ||
5947                             !test_and_clear_bit(CONN_SREJ_ACT,
5948                                                 &chan->conn_state))
5949                                 l2cap_retransmit(chan, control);
5950                 } else {
5951                         l2cap_retransmit(chan, control);
5952                         if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) {
5953                                 set_bit(CONN_SREJ_ACT, &chan->conn_state);
5954                                 chan->srej_save_reqseq = control->reqseq;
5955                         }
5956                 }
5957         }
5958 }
5959
5960 static void l2cap_handle_rej(struct l2cap_chan *chan,
5961                              struct l2cap_ctrl *control)
5962 {
5963         struct sk_buff *skb;
5964
5965         BT_DBG("chan %p, control %p", chan, control);
5966
5967         if (control->reqseq == chan->next_tx_seq) {
5968                 BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq);
5969                 l2cap_send_disconn_req(chan, ECONNRESET);
5970                 return;
5971         }
5972
5973         skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq);
5974
5975         if (chan->max_tx && skb &&
5976             bt_cb(skb)->control.retries >= chan->max_tx) {
5977                 BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
5978                 l2cap_send_disconn_req(chan, ECONNRESET);
5979                 return;
5980         }
5981
5982         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
5983
5984         l2cap_pass_to_tx(chan, control);
5985
5986         if (control->final) {
5987                 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
5988                         l2cap_retransmit_all(chan, control);
5989         } else {
5990                 l2cap_retransmit_all(chan, control);
5991                 l2cap_ertm_send(chan);
5992                 if (chan->tx_state == L2CAP_TX_STATE_WAIT_F)
5993                         set_bit(CONN_REJ_ACT, &chan->conn_state);
5994         }
5995 }
5996
5997 static u8 l2cap_classify_txseq(struct l2cap_chan *chan, u16 txseq)
5998 {
5999         BT_DBG("chan %p, txseq %d", chan, txseq);
6000
6001         BT_DBG("last_acked_seq %d, expected_tx_seq %d", chan->last_acked_seq,
6002                chan->expected_tx_seq);
6003
6004         if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
6005                 if (__seq_offset(chan, txseq, chan->last_acked_seq) >=
6006                     chan->tx_win) {
6007                         /* See notes below regarding "double poll" and
6008                          * invalid packets.
6009                          */
6010                         if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) {
6011                                 BT_DBG("Invalid/Ignore - after SREJ");
6012                                 return L2CAP_TXSEQ_INVALID_IGNORE;
6013                         } else {
6014                                 BT_DBG("Invalid - in window after SREJ sent");
6015                                 return L2CAP_TXSEQ_INVALID;
6016                         }
6017                 }
6018
6019                 if (chan->srej_list.head == txseq) {
6020                         BT_DBG("Expected SREJ");
6021                         return L2CAP_TXSEQ_EXPECTED_SREJ;
6022                 }
6023
6024                 if (l2cap_ertm_seq_in_queue(&chan->srej_q, txseq)) {
6025                         BT_DBG("Duplicate SREJ - txseq already stored");
6026                         return L2CAP_TXSEQ_DUPLICATE_SREJ;
6027                 }
6028
6029                 if (l2cap_seq_list_contains(&chan->srej_list, txseq)) {
6030                         BT_DBG("Unexpected SREJ - not requested");
6031                         return L2CAP_TXSEQ_UNEXPECTED_SREJ;
6032                 }
6033         }
6034
6035         if (chan->expected_tx_seq == txseq) {
6036                 if (__seq_offset(chan, txseq, chan->last_acked_seq) >=
6037                     chan->tx_win) {
6038                         BT_DBG("Invalid - txseq outside tx window");
6039                         return L2CAP_TXSEQ_INVALID;
6040                 } else {
6041                         BT_DBG("Expected");
6042                         return L2CAP_TXSEQ_EXPECTED;
6043                 }
6044         }
6045
6046         if (__seq_offset(chan, txseq, chan->last_acked_seq) <
6047             __seq_offset(chan, chan->expected_tx_seq, chan->last_acked_seq)) {
6048                 BT_DBG("Duplicate - expected_tx_seq later than txseq");
6049                 return L2CAP_TXSEQ_DUPLICATE;
6050         }
6051
6052         if (__seq_offset(chan, txseq, chan->last_acked_seq) >= chan->tx_win) {
6053                 /* A source of invalid packets is a "double poll" condition,
6054                  * where delays cause us to send multiple poll packets.  If
6055                  * the remote stack receives and processes both polls,
6056                  * sequence numbers can wrap around in such a way that a
6057                  * resent frame has a sequence number that looks like new data
6058                  * with a sequence gap.  This would trigger an erroneous SREJ
6059                  * request.
6060                  *
6061                  * Fortunately, this is impossible with a tx window that's
6062                  * less than half of the maximum sequence number, which allows
6063                  * invalid frames to be safely ignored.
6064                  *
6065                  * With tx window sizes greater than half of the tx window
6066                  * maximum, the frame is invalid and cannot be ignored.  This
6067                  * causes a disconnect.
6068                  */
6069
6070                 if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) {
6071                         BT_DBG("Invalid/Ignore - txseq outside tx window");
6072                         return L2CAP_TXSEQ_INVALID_IGNORE;
6073                 } else {
6074                         BT_DBG("Invalid - txseq outside tx window");
6075                         return L2CAP_TXSEQ_INVALID;
6076                 }
6077         } else {
6078                 BT_DBG("Unexpected - txseq indicates missing frames");
6079                 return L2CAP_TXSEQ_UNEXPECTED;
6080         }
6081 }
6082
6083 static int l2cap_rx_state_recv(struct l2cap_chan *chan,
6084                                struct l2cap_ctrl *control,
6085                                struct sk_buff *skb, u8 event)
6086 {
6087         int err = 0;
6088         bool skb_in_use = false;
6089
6090         BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
6091                event);
6092
6093         switch (event) {
6094         case L2CAP_EV_RECV_IFRAME:
6095                 switch (l2cap_classify_txseq(chan, control->txseq)) {
6096                 case L2CAP_TXSEQ_EXPECTED:
6097                         l2cap_pass_to_tx(chan, control);
6098
6099                         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
6100                                 BT_DBG("Busy, discarding expected seq %d",
6101                                        control->txseq);
6102                                 break;
6103                         }
6104
6105                         chan->expected_tx_seq = __next_seq(chan,
6106                                                            control->txseq);
6107
6108                         chan->buffer_seq = chan->expected_tx_seq;
6109                         skb_in_use = true;
6110
6111                         err = l2cap_reassemble_sdu(chan, skb, control);
6112                         if (err)
6113                                 break;
6114
6115                         if (control->final) {
6116                                 if (!test_and_clear_bit(CONN_REJ_ACT,
6117                                                         &chan->conn_state)) {
6118                                         control->final = 0;
6119                                         l2cap_retransmit_all(chan, control);
6120                                         l2cap_ertm_send(chan);
6121                                 }
6122                         }
6123
6124                         if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state))
6125                                 l2cap_send_ack(chan);
6126                         break;
6127                 case L2CAP_TXSEQ_UNEXPECTED:
6128                         l2cap_pass_to_tx(chan, control);
6129
6130                         /* Can't issue SREJ frames in the local busy state.
6131                          * Drop this frame, it will be seen as missing
6132                          * when local busy is exited.
6133                          */
6134                         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
6135                                 BT_DBG("Busy, discarding unexpected seq %d",
6136                                        control->txseq);
6137                                 break;
6138                         }
6139
6140                         /* There was a gap in the sequence, so an SREJ
6141                          * must be sent for each missing frame.  The
6142                          * current frame is stored for later use.
6143                          */
6144                         skb_queue_tail(&chan->srej_q, skb);
6145                         skb_in_use = true;
6146                         BT_DBG("Queued %p (queue len %d)", skb,
6147                                skb_queue_len(&chan->srej_q));
6148
6149                         clear_bit(CONN_SREJ_ACT, &chan->conn_state);
6150                         l2cap_seq_list_clear(&chan->srej_list);
6151                         l2cap_send_srej(chan, control->txseq);
6152
6153                         chan->rx_state = L2CAP_RX_STATE_SREJ_SENT;
6154                         break;
6155                 case L2CAP_TXSEQ_DUPLICATE:
6156                         l2cap_pass_to_tx(chan, control);
6157                         break;
6158                 case L2CAP_TXSEQ_INVALID_IGNORE:
6159                         break;
6160                 case L2CAP_TXSEQ_INVALID:
6161                 default:
6162                         l2cap_send_disconn_req(chan, ECONNRESET);
6163                         break;
6164                 }
6165                 break;
6166         case L2CAP_EV_RECV_RR:
6167                 l2cap_pass_to_tx(chan, control);
6168                 if (control->final) {
6169                         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6170
6171                         if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state) &&
6172                             !__chan_is_moving(chan)) {
6173                                 control->final = 0;
6174                                 l2cap_retransmit_all(chan, control);
6175                         }
6176
6177                         l2cap_ertm_send(chan);
6178                 } else if (control->poll) {
6179                         l2cap_send_i_or_rr_or_rnr(chan);
6180                 } else {
6181                         if (test_and_clear_bit(CONN_REMOTE_BUSY,
6182                                                &chan->conn_state) &&
6183                             chan->unacked_frames)
6184                                 __set_retrans_timer(chan);
6185
6186                         l2cap_ertm_send(chan);
6187                 }
6188                 break;
6189         case L2CAP_EV_RECV_RNR:
6190                 set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6191                 l2cap_pass_to_tx(chan, control);
6192                 if (control && control->poll) {
6193                         set_bit(CONN_SEND_FBIT, &chan->conn_state);
6194                         l2cap_send_rr_or_rnr(chan, 0);
6195                 }
6196                 __clear_retrans_timer(chan);
6197                 l2cap_seq_list_clear(&chan->retrans_list);
6198                 break;
6199         case L2CAP_EV_RECV_REJ:
6200                 l2cap_handle_rej(chan, control);
6201                 break;
6202         case L2CAP_EV_RECV_SREJ:
6203                 l2cap_handle_srej(chan, control);
6204                 break;
6205         default:
6206                 break;
6207         }
6208
6209         if (skb && !skb_in_use) {
6210                 BT_DBG("Freeing %p", skb);
6211                 kfree_skb(skb);
6212         }
6213
6214         return err;
6215 }
6216
6217 static int l2cap_rx_state_srej_sent(struct l2cap_chan *chan,
6218                                     struct l2cap_ctrl *control,
6219                                     struct sk_buff *skb, u8 event)
6220 {
6221         int err = 0;
6222         u16 txseq = control->txseq;
6223         bool skb_in_use = false;
6224
6225         BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
6226                event);
6227
6228         switch (event) {
6229         case L2CAP_EV_RECV_IFRAME:
6230                 switch (l2cap_classify_txseq(chan, txseq)) {
6231                 case L2CAP_TXSEQ_EXPECTED:
6232                         /* Keep frame for reassembly later */
6233                         l2cap_pass_to_tx(chan, control);
6234                         skb_queue_tail(&chan->srej_q, skb);
6235                         skb_in_use = true;
6236                         BT_DBG("Queued %p (queue len %d)", skb,
6237                                skb_queue_len(&chan->srej_q));
6238
6239                         chan->expected_tx_seq = __next_seq(chan, txseq);
6240                         break;
6241                 case L2CAP_TXSEQ_EXPECTED_SREJ:
6242                         l2cap_seq_list_pop(&chan->srej_list);
6243
6244                         l2cap_pass_to_tx(chan, control);
6245                         skb_queue_tail(&chan->srej_q, skb);
6246                         skb_in_use = true;
6247                         BT_DBG("Queued %p (queue len %d)", skb,
6248                                skb_queue_len(&chan->srej_q));
6249
6250                         err = l2cap_rx_queued_iframes(chan);
6251                         if (err)
6252                                 break;
6253
6254                         break;
6255                 case L2CAP_TXSEQ_UNEXPECTED:
6256                         /* Got a frame that can't be reassembled yet.
6257                          * Save it for later, and send SREJs to cover
6258                          * the missing frames.
6259                          */
6260                         skb_queue_tail(&chan->srej_q, skb);
6261                         skb_in_use = true;
6262                         BT_DBG("Queued %p (queue len %d)", skb,
6263                                skb_queue_len(&chan->srej_q));
6264
6265                         l2cap_pass_to_tx(chan, control);
6266                         l2cap_send_srej(chan, control->txseq);
6267                         break;
6268                 case L2CAP_TXSEQ_UNEXPECTED_SREJ:
6269                         /* This frame was requested with an SREJ, but
6270                          * some expected retransmitted frames are
6271                          * missing.  Request retransmission of missing
6272                          * SREJ'd frames.
6273                          */
6274                         skb_queue_tail(&chan->srej_q, skb);
6275                         skb_in_use = true;
6276                         BT_DBG("Queued %p (queue len %d)", skb,
6277                                skb_queue_len(&chan->srej_q));
6278
6279                         l2cap_pass_to_tx(chan, control);
6280                         l2cap_send_srej_list(chan, control->txseq);
6281                         break;
6282                 case L2CAP_TXSEQ_DUPLICATE_SREJ:
6283                         /* We've already queued this frame.  Drop this copy. */
6284                         l2cap_pass_to_tx(chan, control);
6285                         break;
6286                 case L2CAP_TXSEQ_DUPLICATE:
6287                         /* Expecting a later sequence number, so this frame
6288                          * was already received.  Ignore it completely.
6289                          */
6290                         break;
6291                 case L2CAP_TXSEQ_INVALID_IGNORE:
6292                         break;
6293                 case L2CAP_TXSEQ_INVALID:
6294                 default:
6295                         l2cap_send_disconn_req(chan, ECONNRESET);
6296                         break;
6297                 }
6298                 break;
6299         case L2CAP_EV_RECV_RR:
6300                 l2cap_pass_to_tx(chan, control);
6301                 if (control->final) {
6302                         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6303
6304                         if (!test_and_clear_bit(CONN_REJ_ACT,
6305                                                 &chan->conn_state)) {
6306                                 control->final = 0;
6307                                 l2cap_retransmit_all(chan, control);
6308                         }
6309
6310                         l2cap_ertm_send(chan);
6311                 } else if (control->poll) {
6312                         if (test_and_clear_bit(CONN_REMOTE_BUSY,
6313                                                &chan->conn_state) &&
6314                             chan->unacked_frames) {
6315                                 __set_retrans_timer(chan);
6316                         }
6317
6318                         set_bit(CONN_SEND_FBIT, &chan->conn_state);
6319                         l2cap_send_srej_tail(chan);
6320                 } else {
6321                         if (test_and_clear_bit(CONN_REMOTE_BUSY,
6322                                                &chan->conn_state) &&
6323                             chan->unacked_frames)
6324                                 __set_retrans_timer(chan);
6325
6326                         l2cap_send_ack(chan);
6327                 }
6328                 break;
6329         case L2CAP_EV_RECV_RNR:
6330                 set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6331                 l2cap_pass_to_tx(chan, control);
6332                 if (control->poll) {
6333                         l2cap_send_srej_tail(chan);
6334                 } else {
6335                         struct l2cap_ctrl rr_control;
6336                         memset(&rr_control, 0, sizeof(rr_control));
6337                         rr_control.sframe = 1;
6338                         rr_control.super = L2CAP_SUPER_RR;
6339                         rr_control.reqseq = chan->buffer_seq;
6340                         l2cap_send_sframe(chan, &rr_control);
6341                 }
6342
6343                 break;
6344         case L2CAP_EV_RECV_REJ:
6345                 l2cap_handle_rej(chan, control);
6346                 break;
6347         case L2CAP_EV_RECV_SREJ:
6348                 l2cap_handle_srej(chan, control);
6349                 break;
6350         }
6351
6352         if (skb && !skb_in_use) {
6353                 BT_DBG("Freeing %p", skb);
6354                 kfree_skb(skb);
6355         }
6356
6357         return err;
6358 }
6359
6360 static int l2cap_finish_move(struct l2cap_chan *chan)
6361 {
6362         BT_DBG("chan %p", chan);
6363
6364         chan->rx_state = L2CAP_RX_STATE_RECV;
6365
6366         if (chan->hs_hcon)
6367                 chan->conn->mtu = chan->hs_hcon->hdev->block_mtu;
6368         else
6369                 chan->conn->mtu = chan->conn->hcon->hdev->acl_mtu;
6370
6371         return l2cap_resegment(chan);
6372 }
6373
6374 static int l2cap_rx_state_wait_p(struct l2cap_chan *chan,
6375                                  struct l2cap_ctrl *control,
6376                                  struct sk_buff *skb, u8 event)
6377 {
6378         int err;
6379
6380         BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
6381                event);
6382
6383         if (!control->poll)
6384                 return -EPROTO;
6385
6386         l2cap_process_reqseq(chan, control->reqseq);
6387
6388         if (!skb_queue_empty(&chan->tx_q))
6389                 chan->tx_send_head = skb_peek(&chan->tx_q);
6390         else
6391                 chan->tx_send_head = NULL;
6392
6393         /* Rewind next_tx_seq to the point expected
6394          * by the receiver.
6395          */
6396         chan->next_tx_seq = control->reqseq;
6397         chan->unacked_frames = 0;
6398
6399         err = l2cap_finish_move(chan);
6400         if (err)
6401                 return err;
6402
6403         set_bit(CONN_SEND_FBIT, &chan->conn_state);
6404         l2cap_send_i_or_rr_or_rnr(chan);
6405
6406         if (event == L2CAP_EV_RECV_IFRAME)
6407                 return -EPROTO;
6408
6409         return l2cap_rx_state_recv(chan, control, NULL, event);
6410 }
6411
6412 static int l2cap_rx_state_wait_f(struct l2cap_chan *chan,
6413                                  struct l2cap_ctrl *control,
6414                                  struct sk_buff *skb, u8 event)
6415 {
6416         int err;
6417
6418         if (!control->final)
6419                 return -EPROTO;
6420
6421         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6422
6423         chan->rx_state = L2CAP_RX_STATE_RECV;
6424         l2cap_process_reqseq(chan, control->reqseq);
6425
6426         if (!skb_queue_empty(&chan->tx_q))
6427                 chan->tx_send_head = skb_peek(&chan->tx_q);
6428         else
6429                 chan->tx_send_head = NULL;
6430
6431         /* Rewind next_tx_seq to the point expected
6432          * by the receiver.
6433          */
6434         chan->next_tx_seq = control->reqseq;
6435         chan->unacked_frames = 0;
6436
6437         if (chan->hs_hcon)
6438                 chan->conn->mtu = chan->hs_hcon->hdev->block_mtu;
6439         else
6440                 chan->conn->mtu = chan->conn->hcon->hdev->acl_mtu;
6441
6442         err = l2cap_resegment(chan);
6443
6444         if (!err)
6445                 err = l2cap_rx_state_recv(chan, control, skb, event);
6446
6447         return err;
6448 }
6449
6450 static bool __valid_reqseq(struct l2cap_chan *chan, u16 reqseq)
6451 {
6452         /* Make sure reqseq is for a packet that has been sent but not acked */
6453         u16 unacked;
6454
6455         unacked = __seq_offset(chan, chan->next_tx_seq, chan->expected_ack_seq);
6456         return __seq_offset(chan, chan->next_tx_seq, reqseq) <= unacked;
6457 }
6458
6459 static int l2cap_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
6460                     struct sk_buff *skb, u8 event)
6461 {
6462         int err = 0;
6463
6464         BT_DBG("chan %p, control %p, skb %p, event %d, state %d", chan,
6465                control, skb, event, chan->rx_state);
6466
6467         if (__valid_reqseq(chan, control->reqseq)) {
6468                 switch (chan->rx_state) {
6469                 case L2CAP_RX_STATE_RECV:
6470                         err = l2cap_rx_state_recv(chan, control, skb, event);
6471                         break;
6472                 case L2CAP_RX_STATE_SREJ_SENT:
6473                         err = l2cap_rx_state_srej_sent(chan, control, skb,
6474                                                        event);
6475                         break;
6476                 case L2CAP_RX_STATE_WAIT_P:
6477                         err = l2cap_rx_state_wait_p(chan, control, skb, event);
6478                         break;
6479                 case L2CAP_RX_STATE_WAIT_F:
6480                         err = l2cap_rx_state_wait_f(chan, control, skb, event);
6481                         break;
6482                 default:
6483                         /* shut it down */
6484                         break;
6485                 }
6486         } else {
6487                 BT_DBG("Invalid reqseq %d (next_tx_seq %d, expected_ack_seq %d",
6488                        control->reqseq, chan->next_tx_seq,
6489                        chan->expected_ack_seq);
6490                 l2cap_send_disconn_req(chan, ECONNRESET);
6491         }
6492
6493         return err;
6494 }
6495
6496 static int l2cap_stream_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
6497                            struct sk_buff *skb)
6498 {
6499         int err = 0;
6500
6501         BT_DBG("chan %p, control %p, skb %p, state %d", chan, control, skb,
6502                chan->rx_state);
6503
6504         if (l2cap_classify_txseq(chan, control->txseq) ==
6505             L2CAP_TXSEQ_EXPECTED) {
6506                 l2cap_pass_to_tx(chan, control);
6507
6508                 BT_DBG("buffer_seq %d->%d", chan->buffer_seq,
6509                        __next_seq(chan, chan->buffer_seq));
6510
6511                 chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
6512
6513                 l2cap_reassemble_sdu(chan, skb, control);
6514         } else {
6515                 if (chan->sdu) {
6516                         kfree_skb(chan->sdu);
6517                         chan->sdu = NULL;
6518                 }
6519                 chan->sdu_last_frag = NULL;
6520                 chan->sdu_len = 0;
6521
6522                 if (skb) {
6523                         BT_DBG("Freeing %p", skb);
6524                         kfree_skb(skb);
6525                 }
6526         }
6527
6528         chan->last_acked_seq = control->txseq;
6529         chan->expected_tx_seq = __next_seq(chan, control->txseq);
6530
6531         return err;
6532 }
6533
6534 static int l2cap_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb)
6535 {
6536         struct l2cap_ctrl *control = &bt_cb(skb)->control;
6537         u16 len;
6538         u8 event;
6539
6540         __unpack_control(chan, skb);
6541
6542         len = skb->len;
6543
6544         /*
6545          * We can just drop the corrupted I-frame here.
6546          * Receiver will miss it and start proper recovery
6547          * procedures and ask for retransmission.
6548          */
6549         if (l2cap_check_fcs(chan, skb))
6550                 goto drop;
6551
6552         if (!control->sframe && control->sar == L2CAP_SAR_START)
6553                 len -= L2CAP_SDULEN_SIZE;
6554
6555         if (chan->fcs == L2CAP_FCS_CRC16)
6556                 len -= L2CAP_FCS_SIZE;
6557
6558         if (len > chan->mps) {
6559                 l2cap_send_disconn_req(chan, ECONNRESET);
6560                 goto drop;
6561         }
6562
6563         if (!control->sframe) {
6564                 int err;
6565
6566                 BT_DBG("iframe sar %d, reqseq %d, final %d, txseq %d",
6567                        control->sar, control->reqseq, control->final,
6568                        control->txseq);
6569
6570                 /* Validate F-bit - F=0 always valid, F=1 only
6571                  * valid in TX WAIT_F
6572                  */
6573                 if (control->final && chan->tx_state != L2CAP_TX_STATE_WAIT_F)
6574                         goto drop;
6575
6576                 if (chan->mode != L2CAP_MODE_STREAMING) {
6577                         event = L2CAP_EV_RECV_IFRAME;
6578                         err = l2cap_rx(chan, control, skb, event);
6579                 } else {
6580                         err = l2cap_stream_rx(chan, control, skb);
6581                 }
6582
6583                 if (err)
6584                         l2cap_send_disconn_req(chan, ECONNRESET);
6585         } else {
6586                 const u8 rx_func_to_event[4] = {
6587                         L2CAP_EV_RECV_RR, L2CAP_EV_RECV_REJ,
6588                         L2CAP_EV_RECV_RNR, L2CAP_EV_RECV_SREJ
6589                 };
6590
6591                 /* Only I-frames are expected in streaming mode */
6592                 if (chan->mode == L2CAP_MODE_STREAMING)
6593                         goto drop;
6594
6595                 BT_DBG("sframe reqseq %d, final %d, poll %d, super %d",
6596                        control->reqseq, control->final, control->poll,
6597                        control->super);
6598
6599                 if (len != 0) {
6600                         BT_ERR("Trailing bytes: %d in sframe", len);
6601                         l2cap_send_disconn_req(chan, ECONNRESET);
6602                         goto drop;
6603                 }
6604
6605                 /* Validate F and P bits */
6606                 if (control->final && (control->poll ||
6607                                        chan->tx_state != L2CAP_TX_STATE_WAIT_F))
6608                         goto drop;
6609
6610                 event = rx_func_to_event[control->super];
6611                 if (l2cap_rx(chan, control, skb, event))
6612                         l2cap_send_disconn_req(chan, ECONNRESET);
6613         }
6614
6615         return 0;
6616
6617 drop:
6618         kfree_skb(skb);
6619         return 0;
6620 }
6621
6622 static void l2cap_chan_le_send_credits(struct l2cap_chan *chan)
6623 {
6624         struct l2cap_conn *conn = chan->conn;
6625         struct l2cap_le_credits pkt;
6626         u16 return_credits;
6627
6628         /* We return more credits to the sender only after the amount of
6629          * credits falls below half of the initial amount.
6630          */
6631         if (chan->rx_credits >= (le_max_credits + 1) / 2)
6632                 return;
6633
6634         return_credits = le_max_credits - chan->rx_credits;
6635
6636         BT_DBG("chan %p returning %u credits to sender", chan, return_credits);
6637
6638         chan->rx_credits += return_credits;
6639
6640         pkt.cid     = cpu_to_le16(chan->scid);
6641         pkt.credits = cpu_to_le16(return_credits);
6642
6643         chan->ident = l2cap_get_ident(conn);
6644
6645         l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CREDITS, sizeof(pkt), &pkt);
6646 }
6647
6648 static int l2cap_le_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb)
6649 {
6650         int err;
6651
6652         if (!chan->rx_credits) {
6653                 BT_ERR("No credits to receive LE L2CAP data");
6654                 l2cap_send_disconn_req(chan, ECONNRESET);
6655                 return -ENOBUFS;
6656         }
6657
6658         if (chan->imtu < skb->len) {
6659                 BT_ERR("Too big LE L2CAP PDU");
6660                 return -ENOBUFS;
6661         }
6662
6663         chan->rx_credits--;
6664         BT_DBG("rx_credits %u -> %u", chan->rx_credits + 1, chan->rx_credits);
6665
6666         l2cap_chan_le_send_credits(chan);
6667
6668         err = 0;
6669
6670         if (!chan->sdu) {
6671                 u16 sdu_len;
6672
6673                 sdu_len = get_unaligned_le16(skb->data);
6674                 skb_pull(skb, L2CAP_SDULEN_SIZE);
6675
6676                 BT_DBG("Start of new SDU. sdu_len %u skb->len %u imtu %u",
6677                        sdu_len, skb->len, chan->imtu);
6678
6679                 if (sdu_len > chan->imtu) {
6680                         BT_ERR("Too big LE L2CAP SDU length received");
6681                         err = -EMSGSIZE;
6682                         goto failed;
6683                 }
6684
6685                 if (skb->len > sdu_len) {
6686                         BT_ERR("Too much LE L2CAP data received");
6687                         err = -EINVAL;
6688                         goto failed;
6689                 }
6690
6691                 if (skb->len == sdu_len)
6692                         return chan->ops->recv(chan, skb);
6693
6694                 chan->sdu = skb;
6695                 chan->sdu_len = sdu_len;
6696                 chan->sdu_last_frag = skb;
6697
6698                 return 0;
6699         }
6700
6701         BT_DBG("SDU fragment. chan->sdu->len %u skb->len %u chan->sdu_len %u",
6702                chan->sdu->len, skb->len, chan->sdu_len);
6703
6704         if (chan->sdu->len + skb->len > chan->sdu_len) {
6705                 BT_ERR("Too much LE L2CAP data received");
6706                 err = -EINVAL;
6707                 goto failed;
6708         }
6709
6710         append_skb_frag(chan->sdu, skb, &chan->sdu_last_frag);
6711         skb = NULL;
6712
6713         if (chan->sdu->len == chan->sdu_len) {
6714                 err = chan->ops->recv(chan, chan->sdu);
6715                 if (!err) {
6716                         chan->sdu = NULL;
6717                         chan->sdu_last_frag = NULL;
6718                         chan->sdu_len = 0;
6719                 }
6720         }
6721
6722 failed:
6723         if (err) {
6724                 kfree_skb(skb);
6725                 kfree_skb(chan->sdu);
6726                 chan->sdu = NULL;
6727                 chan->sdu_last_frag = NULL;
6728                 chan->sdu_len = 0;
6729         }
6730
6731         /* We can't return an error here since we took care of the skb
6732          * freeing internally. An error return would cause the caller to
6733          * do a double-free of the skb.
6734          */
6735         return 0;
6736 }
6737
6738 static void l2cap_data_channel(struct l2cap_conn *conn, u16 cid,
6739                                struct sk_buff *skb)
6740 {
6741         struct l2cap_chan *chan;
6742
6743         chan = l2cap_get_chan_by_scid(conn, cid);
6744         if (!chan) {
6745                 if (cid == L2CAP_CID_A2MP) {
6746                         chan = a2mp_channel_create(conn, skb);
6747                         if (!chan) {
6748                                 kfree_skb(skb);
6749                                 return;
6750                         }
6751
6752                         l2cap_chan_lock(chan);
6753                 } else {
6754                         BT_DBG("unknown cid 0x%4.4x", cid);
6755                         /* Drop packet and return */
6756                         kfree_skb(skb);
6757                         return;
6758                 }
6759         }
6760
6761         BT_DBG("chan %p, len %d", chan, skb->len);
6762
6763         if (chan->state != BT_CONNECTED)
6764                 goto drop;
6765
6766         switch (chan->mode) {
6767         case L2CAP_MODE_LE_FLOWCTL:
6768                 if (l2cap_le_data_rcv(chan, skb) < 0)
6769                         goto drop;
6770
6771                 goto done;
6772
6773         case L2CAP_MODE_BASIC:
6774                 /* If socket recv buffers overflows we drop data here
6775                  * which is *bad* because L2CAP has to be reliable.
6776                  * But we don't have any other choice. L2CAP doesn't
6777                  * provide flow control mechanism. */
6778
6779                 if (chan->imtu < skb->len) {
6780                         BT_ERR("Dropping L2CAP data: receive buffer overflow");
6781                         goto drop;
6782                 }
6783
6784                 if (!chan->ops->recv(chan, skb))
6785                         goto done;
6786                 break;
6787
6788         case L2CAP_MODE_ERTM:
6789         case L2CAP_MODE_STREAMING:
6790                 l2cap_data_rcv(chan, skb);
6791                 goto done;
6792
6793         default:
6794                 BT_DBG("chan %p: bad mode 0x%2.2x", chan, chan->mode);
6795                 break;
6796         }
6797
6798 drop:
6799         kfree_skb(skb);
6800
6801 done:
6802         l2cap_chan_unlock(chan);
6803 }
6804
6805 static void l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm,
6806                                   struct sk_buff *skb)
6807 {
6808         struct hci_conn *hcon = conn->hcon;
6809         struct l2cap_chan *chan;
6810
6811         if (hcon->type != ACL_LINK)
6812                 goto drop;
6813
6814         chan = l2cap_global_chan_by_psm(0, psm, &hcon->src, &hcon->dst,
6815                                         ACL_LINK);
6816         if (!chan)
6817                 goto drop;
6818
6819         BT_DBG("chan %p, len %d", chan, skb->len);
6820
6821         if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
6822                 goto drop;
6823
6824         if (chan->imtu < skb->len)
6825                 goto drop;
6826
6827         /* Store remote BD_ADDR and PSM for msg_name */
6828         bacpy(&bt_cb(skb)->bdaddr, &hcon->dst);
6829         bt_cb(skb)->psm = psm;
6830
6831         if (!chan->ops->recv(chan, skb))
6832                 return;
6833
6834 drop:
6835         kfree_skb(skb);
6836 }
6837
6838 static void l2cap_att_channel(struct l2cap_conn *conn,
6839                               struct sk_buff *skb)
6840 {
6841         struct hci_conn *hcon = conn->hcon;
6842         struct l2cap_chan *chan;
6843
6844         if (hcon->type != LE_LINK)
6845                 goto drop;
6846
6847         chan = l2cap_global_chan_by_scid(BT_CONNECTED, L2CAP_CID_ATT,
6848                                          &hcon->src, &hcon->dst);
6849         if (!chan)
6850                 goto drop;
6851
6852         BT_DBG("chan %p, len %d", chan, skb->len);
6853
6854         if (hci_blacklist_lookup(hcon->hdev, &hcon->dst, hcon->dst_type))
6855                 goto drop;
6856
6857         if (chan->imtu < skb->len)
6858                 goto drop;
6859
6860         if (!chan->ops->recv(chan, skb))
6861                 return;
6862
6863 drop:
6864         kfree_skb(skb);
6865 }
6866
6867 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
6868 {
6869         struct l2cap_hdr *lh = (void *) skb->data;
6870         struct hci_conn *hcon = conn->hcon;
6871         u16 cid, len;
6872         __le16 psm;
6873
6874         if (hcon->state != BT_CONNECTED) {
6875                 BT_DBG("queueing pending rx skb");
6876                 skb_queue_tail(&conn->pending_rx, skb);
6877                 return;
6878         }
6879
6880         skb_pull(skb, L2CAP_HDR_SIZE);
6881         cid = __le16_to_cpu(lh->cid);
6882         len = __le16_to_cpu(lh->len);
6883
6884         if (len != skb->len) {
6885                 kfree_skb(skb);
6886                 return;
6887         }
6888
6889         BT_DBG("len %d, cid 0x%4.4x", len, cid);
6890
6891         switch (cid) {
6892         case L2CAP_CID_SIGNALING:
6893                 l2cap_sig_channel(conn, skb);
6894                 break;
6895
6896         case L2CAP_CID_CONN_LESS:
6897                 psm = get_unaligned((__le16 *) skb->data);
6898                 skb_pull(skb, L2CAP_PSMLEN_SIZE);
6899                 l2cap_conless_channel(conn, psm, skb);
6900                 break;
6901
6902         case L2CAP_CID_ATT:
6903                 l2cap_att_channel(conn, skb);
6904                 break;
6905
6906         case L2CAP_CID_LE_SIGNALING:
6907                 l2cap_le_sig_channel(conn, skb);
6908                 break;
6909
6910         case L2CAP_CID_SMP:
6911                 if (smp_sig_channel(conn, skb))
6912                         l2cap_conn_del(conn->hcon, EACCES);
6913                 break;
6914
6915         case L2CAP_FC_6LOWPAN:
6916                 bt_6lowpan_recv(conn, skb);
6917                 break;
6918
6919         default:
6920                 l2cap_data_channel(conn, cid, skb);
6921                 break;
6922         }
6923 }
6924
6925 static void process_pending_rx(struct work_struct *work)
6926 {
6927         struct l2cap_conn *conn = container_of(work, struct l2cap_conn,
6928                                                pending_rx_work);
6929         struct sk_buff *skb;
6930
6931         BT_DBG("");
6932
6933         while ((skb = skb_dequeue(&conn->pending_rx)))
6934                 l2cap_recv_frame(conn, skb);
6935 }
6936
6937 static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon)
6938 {
6939         struct l2cap_conn *conn = hcon->l2cap_data;
6940         struct hci_chan *hchan;
6941
6942         if (conn)
6943                 return conn;
6944
6945         hchan = hci_chan_create(hcon);
6946         if (!hchan)
6947                 return NULL;
6948
6949         conn = kzalloc(sizeof(struct l2cap_conn), GFP_KERNEL);
6950         if (!conn) {
6951                 hci_chan_del(hchan);
6952                 return NULL;
6953         }
6954
6955         kref_init(&conn->ref);
6956         hcon->l2cap_data = conn;
6957         conn->hcon = hcon;
6958         hci_conn_get(conn->hcon);
6959         conn->hchan = hchan;
6960
6961         BT_DBG("hcon %p conn %p hchan %p", hcon, conn, hchan);
6962
6963         switch (hcon->type) {
6964         case LE_LINK:
6965                 if (hcon->hdev->le_mtu) {
6966                         conn->mtu = hcon->hdev->le_mtu;
6967                         break;
6968                 }
6969                 /* fall through */
6970         default:
6971                 conn->mtu = hcon->hdev->acl_mtu;
6972                 break;
6973         }
6974
6975         conn->feat_mask = 0;
6976
6977         if (hcon->type == ACL_LINK)
6978                 conn->hs_enabled = test_bit(HCI_HS_ENABLED,
6979                                             &hcon->hdev->dev_flags);
6980
6981         spin_lock_init(&conn->lock);
6982         mutex_init(&conn->chan_lock);
6983
6984         INIT_LIST_HEAD(&conn->chan_l);
6985         INIT_LIST_HEAD(&conn->users);
6986
6987         if (hcon->type == LE_LINK)
6988                 INIT_DELAYED_WORK(&conn->security_timer, security_timeout);
6989         else
6990                 INIT_DELAYED_WORK(&conn->info_timer, l2cap_info_timeout);
6991
6992         skb_queue_head_init(&conn->pending_rx);
6993         INIT_WORK(&conn->pending_rx_work, process_pending_rx);
6994
6995         conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM;
6996
6997         return conn;
6998 }
6999
7000 static bool is_valid_psm(u16 psm, u8 dst_type) {
7001         if (!psm)
7002                 return false;
7003
7004         if (bdaddr_type_is_le(dst_type))
7005                 return (psm <= 0x00ff);
7006
7007         /* PSM must be odd and lsb of upper byte must be 0 */
7008         return ((psm & 0x0101) == 0x0001);
7009 }
7010
7011 int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid,
7012                        bdaddr_t *dst, u8 dst_type)
7013 {
7014         struct l2cap_conn *conn;
7015         struct hci_conn *hcon;
7016         struct hci_dev *hdev;
7017         __u8 auth_type;
7018         int err;
7019
7020         BT_DBG("%pMR -> %pMR (type %u) psm 0x%2.2x", &chan->src, dst,
7021                dst_type, __le16_to_cpu(psm));
7022
7023         hdev = hci_get_route(dst, &chan->src);
7024         if (!hdev)
7025                 return -EHOSTUNREACH;
7026
7027         hci_dev_lock(hdev);
7028
7029         l2cap_chan_lock(chan);
7030
7031         if (!is_valid_psm(__le16_to_cpu(psm), dst_type) && !cid &&
7032             chan->chan_type != L2CAP_CHAN_RAW) {
7033                 err = -EINVAL;
7034                 goto done;
7035         }
7036
7037         if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED && !psm) {
7038                 err = -EINVAL;
7039                 goto done;
7040         }
7041
7042         if (chan->chan_type == L2CAP_CHAN_FIXED && !cid) {
7043                 err = -EINVAL;
7044                 goto done;
7045         }
7046
7047         switch (chan->mode) {
7048         case L2CAP_MODE_BASIC:
7049                 break;
7050         case L2CAP_MODE_LE_FLOWCTL:
7051                 l2cap_le_flowctl_init(chan);
7052                 break;
7053         case L2CAP_MODE_ERTM:
7054         case L2CAP_MODE_STREAMING:
7055                 if (!disable_ertm)
7056                         break;
7057                 /* fall through */
7058         default:
7059                 err = -ENOTSUPP;
7060                 goto done;
7061         }
7062
7063         switch (chan->state) {
7064         case BT_CONNECT:
7065         case BT_CONNECT2:
7066         case BT_CONFIG:
7067                 /* Already connecting */
7068                 err = 0;
7069                 goto done;
7070
7071         case BT_CONNECTED:
7072                 /* Already connected */
7073                 err = -EISCONN;
7074                 goto done;
7075
7076         case BT_OPEN:
7077         case BT_BOUND:
7078                 /* Can connect */
7079                 break;
7080
7081         default:
7082                 err = -EBADFD;
7083                 goto done;
7084         }
7085
7086         /* Set destination address and psm */
7087         bacpy(&chan->dst, dst);
7088         chan->dst_type = dst_type;
7089
7090         chan->psm = psm;
7091         chan->dcid = cid;
7092
7093         auth_type = l2cap_get_auth_type(chan);
7094
7095         if (bdaddr_type_is_le(dst_type))
7096                 hcon = hci_connect(hdev, LE_LINK, dst, dst_type,
7097                                    chan->sec_level, auth_type);
7098         else
7099                 hcon = hci_connect(hdev, ACL_LINK, dst, dst_type,
7100                                    chan->sec_level, auth_type);
7101
7102         if (IS_ERR(hcon)) {
7103                 err = PTR_ERR(hcon);
7104                 goto done;
7105         }
7106
7107         conn = l2cap_conn_add(hcon);
7108         if (!conn) {
7109                 hci_conn_drop(hcon);
7110                 err = -ENOMEM;
7111                 goto done;
7112         }
7113
7114         if (cid && __l2cap_get_chan_by_dcid(conn, cid)) {
7115                 hci_conn_drop(hcon);
7116                 err = -EBUSY;
7117                 goto done;
7118         }
7119
7120         /* Update source addr of the socket */
7121         bacpy(&chan->src, &hcon->src);
7122         chan->src_type = bdaddr_type(hcon, hcon->src_type);
7123
7124         l2cap_chan_unlock(chan);
7125         l2cap_chan_add(conn, chan);
7126         l2cap_chan_lock(chan);
7127
7128         /* l2cap_chan_add takes its own ref so we can drop this one */
7129         hci_conn_drop(hcon);
7130
7131         l2cap_state_change(chan, BT_CONNECT);
7132         __set_chan_timer(chan, chan->ops->get_sndtimeo(chan));
7133
7134         /* Release chan->sport so that it can be reused by other
7135          * sockets (as it's only used for listening sockets).
7136          */
7137         write_lock(&chan_list_lock);
7138         chan->sport = 0;
7139         write_unlock(&chan_list_lock);
7140
7141         if (hcon->state == BT_CONNECTED) {
7142                 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
7143                         __clear_chan_timer(chan);
7144                         if (l2cap_chan_check_security(chan))
7145                                 l2cap_state_change(chan, BT_CONNECTED);
7146                 } else
7147                         l2cap_do_start(chan);
7148         }
7149
7150         err = 0;
7151
7152 done:
7153         l2cap_chan_unlock(chan);
7154         hci_dev_unlock(hdev);
7155         hci_dev_put(hdev);
7156         return err;
7157 }
7158
7159 /* ---- L2CAP interface with lower layer (HCI) ---- */
7160
7161 int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr)
7162 {
7163         int exact = 0, lm1 = 0, lm2 = 0;
7164         struct l2cap_chan *c;
7165
7166         BT_DBG("hdev %s, bdaddr %pMR", hdev->name, bdaddr);
7167
7168         /* Find listening sockets and check their link_mode */
7169         read_lock(&chan_list_lock);
7170         list_for_each_entry(c, &chan_list, global_l) {
7171                 if (c->state != BT_LISTEN)
7172                         continue;
7173
7174                 if (!bacmp(&c->src, &hdev->bdaddr)) {
7175                         lm1 |= HCI_LM_ACCEPT;
7176                         if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
7177                                 lm1 |= HCI_LM_MASTER;
7178                         exact++;
7179                 } else if (!bacmp(&c->src, BDADDR_ANY)) {
7180                         lm2 |= HCI_LM_ACCEPT;
7181                         if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
7182                                 lm2 |= HCI_LM_MASTER;
7183                 }
7184         }
7185         read_unlock(&chan_list_lock);
7186
7187         return exact ? lm1 : lm2;
7188 }
7189
7190 void l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
7191 {
7192         struct l2cap_conn *conn;
7193
7194         BT_DBG("hcon %p bdaddr %pMR status %d", hcon, &hcon->dst, status);
7195
7196         if (!status) {
7197                 conn = l2cap_conn_add(hcon);
7198                 if (conn)
7199                         l2cap_conn_ready(conn);
7200         } else {
7201                 l2cap_conn_del(hcon, bt_to_errno(status));
7202         }
7203 }
7204
7205 int l2cap_disconn_ind(struct hci_conn *hcon)
7206 {
7207         struct l2cap_conn *conn = hcon->l2cap_data;
7208
7209         BT_DBG("hcon %p", hcon);
7210
7211         if (!conn)
7212                 return HCI_ERROR_REMOTE_USER_TERM;
7213         return conn->disc_reason;
7214 }
7215
7216 void l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
7217 {
7218         BT_DBG("hcon %p reason %d", hcon, reason);
7219
7220         bt_6lowpan_del_conn(hcon->l2cap_data);
7221
7222         l2cap_conn_del(hcon, bt_to_errno(reason));
7223 }
7224
7225 static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt)
7226 {
7227         if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED)
7228                 return;
7229
7230         if (encrypt == 0x00) {
7231                 if (chan->sec_level == BT_SECURITY_MEDIUM) {
7232                         __set_chan_timer(chan, L2CAP_ENC_TIMEOUT);
7233                 } else if (chan->sec_level == BT_SECURITY_HIGH ||
7234                            chan->sec_level == BT_SECURITY_FIPS)
7235                         l2cap_chan_close(chan, ECONNREFUSED);
7236         } else {
7237                 if (chan->sec_level == BT_SECURITY_MEDIUM)
7238                         __clear_chan_timer(chan);
7239         }
7240 }
7241
7242 int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
7243 {
7244         struct l2cap_conn *conn = hcon->l2cap_data;
7245         struct l2cap_chan *chan;
7246
7247         if (!conn)
7248                 return 0;
7249
7250         BT_DBG("conn %p status 0x%2.2x encrypt %u", conn, status, encrypt);
7251
7252         if (hcon->type == LE_LINK) {
7253                 if (!status && encrypt)
7254                         smp_distribute_keys(conn, 0);
7255                 cancel_delayed_work(&conn->security_timer);
7256         }
7257
7258         mutex_lock(&conn->chan_lock);
7259
7260         list_for_each_entry(chan, &conn->chan_l, list) {
7261                 l2cap_chan_lock(chan);
7262
7263                 BT_DBG("chan %p scid 0x%4.4x state %s", chan, chan->scid,
7264                        state_to_string(chan->state));
7265
7266                 if (chan->scid == L2CAP_CID_A2MP) {
7267                         l2cap_chan_unlock(chan);
7268                         continue;
7269                 }
7270
7271                 if (chan->scid == L2CAP_CID_ATT) {
7272                         if (!status && encrypt) {
7273                                 chan->sec_level = hcon->sec_level;
7274                                 l2cap_chan_ready(chan);
7275                         }
7276
7277                         l2cap_chan_unlock(chan);
7278                         continue;
7279                 }
7280
7281                 if (!__l2cap_no_conn_pending(chan)) {
7282                         l2cap_chan_unlock(chan);
7283                         continue;
7284                 }
7285
7286                 if (!status && (chan->state == BT_CONNECTED ||
7287                                 chan->state == BT_CONFIG)) {
7288                         chan->ops->resume(chan);
7289                         l2cap_check_encryption(chan, encrypt);
7290                         l2cap_chan_unlock(chan);
7291                         continue;
7292                 }
7293
7294                 if (chan->state == BT_CONNECT) {
7295                         if (!status)
7296                                 l2cap_start_connection(chan);
7297                         else
7298                                 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
7299                 } else if (chan->state == BT_CONNECT2) {
7300                         struct l2cap_conn_rsp rsp;
7301                         __u16 res, stat;
7302
7303                         if (!status) {
7304                                 if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
7305                                         res = L2CAP_CR_PEND;
7306                                         stat = L2CAP_CS_AUTHOR_PEND;
7307                                         chan->ops->defer(chan);
7308                                 } else {
7309                                         l2cap_state_change(chan, BT_CONFIG);
7310                                         res = L2CAP_CR_SUCCESS;
7311                                         stat = L2CAP_CS_NO_INFO;
7312                                 }
7313                         } else {
7314                                 l2cap_state_change(chan, BT_DISCONN);
7315                                 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
7316                                 res = L2CAP_CR_SEC_BLOCK;
7317                                 stat = L2CAP_CS_NO_INFO;
7318                         }
7319
7320                         rsp.scid   = cpu_to_le16(chan->dcid);
7321                         rsp.dcid   = cpu_to_le16(chan->scid);
7322                         rsp.result = cpu_to_le16(res);
7323                         rsp.status = cpu_to_le16(stat);
7324                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
7325                                        sizeof(rsp), &rsp);
7326
7327                         if (!test_bit(CONF_REQ_SENT, &chan->conf_state) &&
7328                             res == L2CAP_CR_SUCCESS) {
7329                                 char buf[128];
7330                                 set_bit(CONF_REQ_SENT, &chan->conf_state);
7331                                 l2cap_send_cmd(conn, l2cap_get_ident(conn),
7332                                                L2CAP_CONF_REQ,
7333                                                l2cap_build_conf_req(chan, buf),
7334                                                buf);
7335                                 chan->num_conf_req++;
7336                         }
7337                 }
7338
7339                 l2cap_chan_unlock(chan);
7340         }
7341
7342         mutex_unlock(&conn->chan_lock);
7343
7344         return 0;
7345 }
7346
7347 int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
7348 {
7349         struct l2cap_conn *conn = hcon->l2cap_data;
7350         struct l2cap_hdr *hdr;
7351         int len;
7352
7353         /* For AMP controller do not create l2cap conn */
7354         if (!conn && hcon->hdev->dev_type != HCI_BREDR)
7355                 goto drop;
7356
7357         if (!conn)
7358                 conn = l2cap_conn_add(hcon);
7359
7360         if (!conn)
7361                 goto drop;
7362
7363         BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
7364
7365         switch (flags) {
7366         case ACL_START:
7367         case ACL_START_NO_FLUSH:
7368         case ACL_COMPLETE:
7369                 if (conn->rx_len) {
7370                         BT_ERR("Unexpected start frame (len %d)", skb->len);
7371                         kfree_skb(conn->rx_skb);
7372                         conn->rx_skb = NULL;
7373                         conn->rx_len = 0;
7374                         l2cap_conn_unreliable(conn, ECOMM);
7375                 }
7376
7377                 /* Start fragment always begin with Basic L2CAP header */
7378                 if (skb->len < L2CAP_HDR_SIZE) {
7379                         BT_ERR("Frame is too short (len %d)", skb->len);
7380                         l2cap_conn_unreliable(conn, ECOMM);
7381                         goto drop;
7382                 }
7383
7384                 hdr = (struct l2cap_hdr *) skb->data;
7385                 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
7386
7387                 if (len == skb->len) {
7388                         /* Complete frame received */
7389                         l2cap_recv_frame(conn, skb);
7390                         return 0;
7391                 }
7392
7393                 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
7394
7395                 if (skb->len > len) {
7396                         BT_ERR("Frame is too long (len %d, expected len %d)",
7397                                skb->len, len);
7398                         l2cap_conn_unreliable(conn, ECOMM);
7399                         goto drop;
7400                 }
7401
7402                 /* Allocate skb for the complete frame (with header) */
7403                 conn->rx_skb = bt_skb_alloc(len, GFP_KERNEL);
7404                 if (!conn->rx_skb)
7405                         goto drop;
7406
7407                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
7408                                           skb->len);
7409                 conn->rx_len = len - skb->len;
7410                 break;
7411
7412         case ACL_CONT:
7413                 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
7414
7415                 if (!conn->rx_len) {
7416                         BT_ERR("Unexpected continuation frame (len %d)", skb->len);
7417                         l2cap_conn_unreliable(conn, ECOMM);
7418                         goto drop;
7419                 }
7420
7421                 if (skb->len > conn->rx_len) {
7422                         BT_ERR("Fragment is too long (len %d, expected %d)",
7423                                skb->len, conn->rx_len);
7424                         kfree_skb(conn->rx_skb);
7425                         conn->rx_skb = NULL;
7426                         conn->rx_len = 0;
7427                         l2cap_conn_unreliable(conn, ECOMM);
7428                         goto drop;
7429                 }
7430
7431                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
7432                                           skb->len);
7433                 conn->rx_len -= skb->len;
7434
7435                 if (!conn->rx_len) {
7436                         /* Complete frame received. l2cap_recv_frame
7437                          * takes ownership of the skb so set the global
7438                          * rx_skb pointer to NULL first.
7439                          */
7440                         struct sk_buff *rx_skb = conn->rx_skb;
7441                         conn->rx_skb = NULL;
7442                         l2cap_recv_frame(conn, rx_skb);
7443                 }
7444                 break;
7445         }
7446
7447 drop:
7448         kfree_skb(skb);
7449         return 0;
7450 }
7451
7452 static int l2cap_debugfs_show(struct seq_file *f, void *p)
7453 {
7454         struct l2cap_chan *c;
7455
7456         read_lock(&chan_list_lock);
7457
7458         list_for_each_entry(c, &chan_list, global_l) {
7459                 seq_printf(f, "%pMR %pMR %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
7460                            &c->src, &c->dst,
7461                            c->state, __le16_to_cpu(c->psm),
7462                            c->scid, c->dcid, c->imtu, c->omtu,
7463                            c->sec_level, c->mode);
7464         }
7465
7466         read_unlock(&chan_list_lock);
7467
7468         return 0;
7469 }
7470
7471 static int l2cap_debugfs_open(struct inode *inode, struct file *file)
7472 {
7473         return single_open(file, l2cap_debugfs_show, inode->i_private);
7474 }
7475
7476 static const struct file_operations l2cap_debugfs_fops = {
7477         .open           = l2cap_debugfs_open,
7478         .read           = seq_read,
7479         .llseek         = seq_lseek,
7480         .release        = single_release,
7481 };
7482
7483 static struct dentry *l2cap_debugfs;
7484
7485 int __init l2cap_init(void)
7486 {
7487         int err;
7488
7489         err = l2cap_init_sockets();
7490         if (err < 0)
7491                 return err;
7492
7493         if (IS_ERR_OR_NULL(bt_debugfs))
7494                 return 0;
7495
7496         l2cap_debugfs = debugfs_create_file("l2cap", 0444, bt_debugfs,
7497                                             NULL, &l2cap_debugfs_fops);
7498
7499         debugfs_create_u16("l2cap_le_max_credits", 0466, bt_debugfs,
7500                            &le_max_credits);
7501         debugfs_create_u16("l2cap_le_default_mps", 0466, bt_debugfs,
7502                            &le_default_mps);
7503
7504         bt_6lowpan_init();
7505
7506         return 0;
7507 }
7508
7509 void l2cap_exit(void)
7510 {
7511         bt_6lowpan_cleanup();
7512         debugfs_remove(l2cap_debugfs);
7513         l2cap_cleanup_sockets();
7514 }
7515
7516 module_param(disable_ertm, bool, 0644);
7517 MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode");