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