net/bluetooth/l2cap_core.c

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