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, 0, NULL, 0);
3877         hci_dev_unlock(hdev);
3878
3879         l2cap_connect(conn, cmd, data, L2CAP_CONN_RSP, 0);
3880         return 0;
3881 }
3882
3883 static int l2cap_connect_create_rsp(struct l2cap_conn *conn,
3884                                     struct l2cap_cmd_hdr *cmd, u16 cmd_len,
3885                                     u8 *data)
3886 {
3887         struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
3888         u16 scid, dcid, result, status;
3889         struct l2cap_chan *chan;
3890         u8 req[128];
3891         int err;
3892
3893         if (cmd_len < sizeof(*rsp))
3894                 return -EPROTO;
3895
3896         scid   = __le16_to_cpu(rsp->scid);
3897         dcid   = __le16_to_cpu(rsp->dcid);
3898         result = __le16_to_cpu(rsp->result);
3899         status = __le16_to_cpu(rsp->status);
3900
3901         BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x",
3902                dcid, scid, result, status);
3903
3904         mutex_lock(&conn->chan_lock);
3905
3906         if (scid) {
3907                 chan = __l2cap_get_chan_by_scid(conn, scid);
3908                 if (!chan) {
3909                         err = -EBADSLT;
3910                         goto unlock;
3911                 }
3912         } else {
3913                 chan = __l2cap_get_chan_by_ident(conn, cmd->ident);
3914                 if (!chan) {
3915                         err = -EBADSLT;
3916                         goto unlock;
3917                 }
3918         }
3919
3920         err = 0;
3921
3922         l2cap_chan_lock(chan);
3923
3924         switch (result) {
3925         case L2CAP_CR_SUCCESS:
3926                 l2cap_state_change(chan, BT_CONFIG);
3927                 chan->ident = 0;
3928                 chan->dcid = dcid;
3929                 clear_bit(CONF_CONNECT_PEND, &chan->conf_state);
3930
3931                 if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
3932                         break;
3933
3934                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3935                                l2cap_build_conf_req(chan, req), req);
3936                 chan->num_conf_req++;
3937                 break;
3938
3939         case L2CAP_CR_PEND:
3940                 set_bit(CONF_CONNECT_PEND, &chan->conf_state);
3941                 break;
3942
3943         default:
3944                 l2cap_chan_del(chan, ECONNREFUSED);
3945                 break;
3946         }
3947
3948         l2cap_chan_unlock(chan);
3949
3950 unlock:
3951         mutex_unlock(&conn->chan_lock);
3952
3953         return err;
3954 }
3955
3956 static inline void set_default_fcs(struct l2cap_chan *chan)
3957 {
3958         /* FCS is enabled only in ERTM or streaming mode, if one or both
3959          * sides request it.
3960          */
3961         if (chan->mode != L2CAP_MODE_ERTM && chan->mode != L2CAP_MODE_STREAMING)
3962                 chan->fcs = L2CAP_FCS_NONE;
3963         else if (!test_bit(CONF_RECV_NO_FCS, &chan->conf_state))
3964                 chan->fcs = L2CAP_FCS_CRC16;
3965 }
3966
3967 static void l2cap_send_efs_conf_rsp(struct l2cap_chan *chan, void *data,
3968                                     u8 ident, u16 flags)
3969 {
3970         struct l2cap_conn *conn = chan->conn;
3971
3972         BT_DBG("conn %p chan %p ident %d flags 0x%4.4x", conn, chan, ident,
3973                flags);
3974
3975         clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
3976         set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
3977
3978         l2cap_send_cmd(conn, ident, L2CAP_CONF_RSP,
3979                        l2cap_build_conf_rsp(chan, data,
3980                                             L2CAP_CONF_SUCCESS, flags), data);
3981 }
3982
3983 static void cmd_reject_invalid_cid(struct l2cap_conn *conn, u8 ident,
3984                                    u16 scid, u16 dcid)
3985 {
3986         struct l2cap_cmd_rej_cid rej;
3987
3988         rej.reason = cpu_to_le16(L2CAP_REJ_INVALID_CID);
3989         rej.scid = __cpu_to_le16(scid);
3990         rej.dcid = __cpu_to_le16(dcid);
3991
3992         l2cap_send_cmd(conn, ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
3993 }
3994
3995 static inline int l2cap_config_req(struct l2cap_conn *conn,
3996                                    struct l2cap_cmd_hdr *cmd, u16 cmd_len,
3997                                    u8 *data)
3998 {
3999         struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
4000         u16 dcid, flags;
4001         u8 rsp[64];
4002         struct l2cap_chan *chan;
4003         int len, err = 0;
4004
4005         if (cmd_len < sizeof(*req))
4006                 return -EPROTO;
4007
4008         dcid  = __le16_to_cpu(req->dcid);
4009         flags = __le16_to_cpu(req->flags);
4010
4011         BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
4012
4013         chan = l2cap_get_chan_by_scid(conn, dcid);
4014         if (!chan) {
4015                 cmd_reject_invalid_cid(conn, cmd->ident, dcid, 0);
4016                 return 0;
4017         }
4018
4019         if (chan->state != BT_CONFIG && chan->state != BT_CONNECT2) {
4020                 cmd_reject_invalid_cid(conn, cmd->ident, chan->scid,
4021                                        chan->dcid);
4022                 goto unlock;
4023         }
4024
4025         /* Reject if config buffer is too small. */
4026         len = cmd_len - sizeof(*req);
4027         if (chan->conf_len + len > sizeof(chan->conf_req)) {
4028                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
4029                                l2cap_build_conf_rsp(chan, rsp,
4030                                L2CAP_CONF_REJECT, flags), rsp);
4031                 goto unlock;
4032         }
4033
4034         /* Store config. */
4035         memcpy(chan->conf_req + chan->conf_len, req->data, len);
4036         chan->conf_len += len;
4037
4038         if (flags & L2CAP_CONF_FLAG_CONTINUATION) {
4039                 /* Incomplete config. Send empty response. */
4040                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
4041                                l2cap_build_conf_rsp(chan, rsp,
4042                                L2CAP_CONF_SUCCESS, flags), rsp);
4043                 goto unlock;
4044         }
4045
4046         /* Complete config. */
4047         len = l2cap_parse_conf_req(chan, rsp);
4048         if (len < 0) {
4049                 l2cap_send_disconn_req(chan, ECONNRESET);
4050                 goto unlock;
4051         }
4052
4053         chan->ident = cmd->ident;
4054         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
4055         chan->num_conf_rsp++;
4056
4057         /* Reset config buffer. */
4058         chan->conf_len = 0;
4059
4060         if (!test_bit(CONF_OUTPUT_DONE, &chan->conf_state))
4061                 goto unlock;
4062
4063         if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
4064                 set_default_fcs(chan);
4065
4066                 if (chan->mode == L2CAP_MODE_ERTM ||
4067                     chan->mode == L2CAP_MODE_STREAMING)
4068                         err = l2cap_ertm_init(chan);
4069
4070                 if (err < 0)
4071                         l2cap_send_disconn_req(chan, -err);
4072                 else
4073                         l2cap_chan_ready(chan);
4074
4075                 goto unlock;
4076         }
4077
4078         if (!test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) {
4079                 u8 buf[64];
4080                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
4081                                l2cap_build_conf_req(chan, buf), buf);
4082                 chan->num_conf_req++;
4083         }
4084
4085         /* Got Conf Rsp PENDING from remote side and assume we sent
4086            Conf Rsp PENDING in the code above */
4087         if (test_bit(CONF_REM_CONF_PEND, &chan->conf_state) &&
4088             test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
4089
4090                 /* check compatibility */
4091
4092                 /* Send rsp for BR/EDR channel */
4093                 if (!chan->hs_hcon)
4094                         l2cap_send_efs_conf_rsp(chan, rsp, cmd->ident, flags);
4095                 else
4096                         chan->ident = cmd->ident;
4097         }
4098
4099 unlock:
4100         l2cap_chan_unlock(chan);
4101         return err;
4102 }
4103
4104 static inline int l2cap_config_rsp(struct l2cap_conn *conn,
4105                                    struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4106                                    u8 *data)
4107 {
4108         struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
4109         u16 scid, flags, result;
4110         struct l2cap_chan *chan;
4111         int len = cmd_len - sizeof(*rsp);
4112         int err = 0;
4113
4114         if (cmd_len < sizeof(*rsp))
4115                 return -EPROTO;
4116
4117         scid   = __le16_to_cpu(rsp->scid);
4118         flags  = __le16_to_cpu(rsp->flags);
4119         result = __le16_to_cpu(rsp->result);
4120
4121         BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x len %d", scid, flags,
4122                result, len);
4123
4124         chan = l2cap_get_chan_by_scid(conn, scid);
4125         if (!chan)
4126                 return 0;
4127
4128         switch (result) {
4129         case L2CAP_CONF_SUCCESS:
4130                 l2cap_conf_rfc_get(chan, rsp->data, len);
4131                 clear_bit(CONF_REM_CONF_PEND, &chan->conf_state);
4132                 break;
4133
4134         case L2CAP_CONF_PENDING:
4135                 set_bit(CONF_REM_CONF_PEND, &chan->conf_state);
4136
4137                 if (test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
4138                         char buf[64];
4139
4140                         len = l2cap_parse_conf_rsp(chan, rsp->data, len,
4141                                                    buf, &result);
4142                         if (len < 0) {
4143                                 l2cap_send_disconn_req(chan, ECONNRESET);
4144                                 goto done;
4145                         }
4146
4147                         if (!chan->hs_hcon) {
4148                                 l2cap_send_efs_conf_rsp(chan, buf, cmd->ident,
4149                                                         0);
4150                         } else {
4151                                 if (l2cap_check_efs(chan)) {
4152                                         amp_create_logical_link(chan);
4153                                         chan->ident = cmd->ident;
4154                                 }
4155                         }
4156                 }
4157                 goto done;
4158
4159         case L2CAP_CONF_UNACCEPT:
4160                 if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
4161                         char req[64];
4162
4163                         if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
4164                                 l2cap_send_disconn_req(chan, ECONNRESET);
4165                                 goto done;
4166                         }
4167
4168                         /* throw out any old stored conf requests */
4169                         result = L2CAP_CONF_SUCCESS;
4170                         len = l2cap_parse_conf_rsp(chan, rsp->data, len,
4171                                                    req, &result);
4172                         if (len < 0) {
4173                                 l2cap_send_disconn_req(chan, ECONNRESET);
4174                                 goto done;
4175                         }
4176
4177                         l2cap_send_cmd(conn, l2cap_get_ident(conn),
4178                                        L2CAP_CONF_REQ, len, req);
4179                         chan->num_conf_req++;
4180                         if (result != L2CAP_CONF_SUCCESS)
4181                                 goto done;
4182                         break;
4183                 }
4184
4185         default:
4186                 l2cap_chan_set_err(chan, ECONNRESET);
4187
4188                 __set_chan_timer(chan, L2CAP_DISC_REJ_TIMEOUT);
4189                 l2cap_send_disconn_req(chan, ECONNRESET);
4190                 goto done;
4191         }
4192
4193         if (flags & L2CAP_CONF_FLAG_CONTINUATION)
4194                 goto done;
4195
4196         set_bit(CONF_INPUT_DONE, &chan->conf_state);
4197
4198         if (test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) {
4199                 set_default_fcs(chan);
4200
4201                 if (chan->mode == L2CAP_MODE_ERTM ||
4202                     chan->mode == L2CAP_MODE_STREAMING)
4203                         err = l2cap_ertm_init(chan);
4204
4205                 if (err < 0)
4206                         l2cap_send_disconn_req(chan, -err);
4207                 else
4208                         l2cap_chan_ready(chan);
4209         }
4210
4211 done:
4212         l2cap_chan_unlock(chan);
4213         return err;
4214 }
4215
4216 static inline int l2cap_disconnect_req(struct l2cap_conn *conn,
4217                                        struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4218                                        u8 *data)
4219 {
4220         struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
4221         struct l2cap_disconn_rsp rsp;
4222         u16 dcid, scid;
4223         struct l2cap_chan *chan;
4224
4225         if (cmd_len != sizeof(*req))
4226                 return -EPROTO;
4227
4228         scid = __le16_to_cpu(req->scid);
4229         dcid = __le16_to_cpu(req->dcid);
4230
4231         BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
4232
4233         mutex_lock(&conn->chan_lock);
4234
4235         chan = __l2cap_get_chan_by_scid(conn, dcid);
4236         if (!chan) {
4237                 mutex_unlock(&conn->chan_lock);
4238                 cmd_reject_invalid_cid(conn, cmd->ident, dcid, scid);
4239                 return 0;
4240         }
4241
4242         l2cap_chan_lock(chan);
4243
4244         rsp.dcid = cpu_to_le16(chan->scid);
4245         rsp.scid = cpu_to_le16(chan->dcid);
4246         l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
4247
4248         chan->ops->set_shutdown(chan);
4249
4250         l2cap_chan_hold(chan);
4251         l2cap_chan_del(chan, ECONNRESET);
4252
4253         l2cap_chan_unlock(chan);
4254
4255         chan->ops->close(chan);
4256         l2cap_chan_put(chan);
4257
4258         mutex_unlock(&conn->chan_lock);
4259
4260         return 0;
4261 }
4262
4263 static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn,
4264                                        struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4265                                        u8 *data)
4266 {
4267         struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
4268         u16 dcid, scid;
4269         struct l2cap_chan *chan;
4270
4271         if (cmd_len != sizeof(*rsp))
4272                 return -EPROTO;
4273
4274         scid = __le16_to_cpu(rsp->scid);
4275         dcid = __le16_to_cpu(rsp->dcid);
4276
4277         BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
4278
4279         mutex_lock(&conn->chan_lock);
4280
4281         chan = __l2cap_get_chan_by_scid(conn, scid);
4282         if (!chan) {
4283                 mutex_unlock(&conn->chan_lock);
4284                 return 0;
4285         }
4286
4287         l2cap_chan_lock(chan);
4288
4289         l2cap_chan_hold(chan);
4290         l2cap_chan_del(chan, 0);
4291
4292         l2cap_chan_unlock(chan);
4293
4294         chan->ops->close(chan);
4295         l2cap_chan_put(chan);
4296
4297         mutex_unlock(&conn->chan_lock);
4298
4299         return 0;
4300 }
4301
4302 static inline int l2cap_information_req(struct l2cap_conn *conn,
4303                                         struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4304                                         u8 *data)
4305 {
4306         struct l2cap_info_req *req = (struct l2cap_info_req *) data;
4307         u16 type;
4308
4309         if (cmd_len != sizeof(*req))
4310                 return -EPROTO;
4311
4312         type = __le16_to_cpu(req->type);
4313
4314         BT_DBG("type 0x%4.4x", type);
4315
4316         if (type == L2CAP_IT_FEAT_MASK) {
4317                 u8 buf[8];
4318                 u32 feat_mask = l2cap_feat_mask;
4319                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
4320                 rsp->type   = cpu_to_le16(L2CAP_IT_FEAT_MASK);
4321                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
4322                 if (!disable_ertm)
4323                         feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
4324                                 | L2CAP_FEAT_FCS;
4325                 if (conn->hs_enabled)
4326                         feat_mask |= L2CAP_FEAT_EXT_FLOW
4327                                 | L2CAP_FEAT_EXT_WINDOW;
4328
4329                 put_unaligned_le32(feat_mask, rsp->data);
4330                 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf),
4331                                buf);
4332         } else if (type == L2CAP_IT_FIXED_CHAN) {
4333                 u8 buf[12];
4334                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
4335
4336                 if (conn->hs_enabled)
4337                         l2cap_fixed_chan[0] |= L2CAP_FC_A2MP;
4338                 else
4339                         l2cap_fixed_chan[0] &= ~L2CAP_FC_A2MP;
4340
4341                 rsp->type   = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
4342                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
4343                 memcpy(rsp->data, l2cap_fixed_chan, sizeof(l2cap_fixed_chan));
4344                 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf),
4345                                buf);
4346         } else {
4347                 struct l2cap_info_rsp rsp;
4348                 rsp.type   = cpu_to_le16(type);
4349                 rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP);
4350                 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(rsp),
4351                                &rsp);
4352         }
4353
4354         return 0;
4355 }
4356
4357 static inline int l2cap_information_rsp(struct l2cap_conn *conn,
4358                                         struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4359                                         u8 *data)
4360 {
4361         struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
4362         u16 type, result;
4363
4364         if (cmd_len < sizeof(*rsp))
4365                 return -EPROTO;
4366
4367         type   = __le16_to_cpu(rsp->type);
4368         result = __le16_to_cpu(rsp->result);
4369
4370         BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
4371
4372         /* L2CAP Info req/rsp are unbound to channels, add extra checks */
4373         if (cmd->ident != conn->info_ident ||
4374             conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)
4375                 return 0;
4376
4377         cancel_delayed_work(&conn->info_timer);
4378
4379         if (result != L2CAP_IR_SUCCESS) {
4380                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4381                 conn->info_ident = 0;
4382
4383                 l2cap_conn_start(conn);
4384
4385                 return 0;
4386         }
4387
4388         switch (type) {
4389         case L2CAP_IT_FEAT_MASK:
4390                 conn->feat_mask = get_unaligned_le32(rsp->data);
4391
4392                 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
4393                         struct l2cap_info_req req;
4394                         req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
4395
4396                         conn->info_ident = l2cap_get_ident(conn);
4397
4398                         l2cap_send_cmd(conn, conn->info_ident,
4399                                        L2CAP_INFO_REQ, sizeof(req), &req);
4400                 } else {
4401                         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4402                         conn->info_ident = 0;
4403
4404                         l2cap_conn_start(conn);
4405                 }
4406                 break;
4407
4408         case L2CAP_IT_FIXED_CHAN:
4409                 conn->fixed_chan_mask = rsp->data[0];
4410                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4411                 conn->info_ident = 0;
4412
4413                 l2cap_conn_start(conn);
4414                 break;
4415         }
4416
4417         return 0;
4418 }
4419
4420 static int l2cap_create_channel_req(struct l2cap_conn *conn,
4421                                     struct l2cap_cmd_hdr *cmd,
4422                                     u16 cmd_len, void *data)
4423 {
4424         struct l2cap_create_chan_req *req = data;
4425         struct l2cap_create_chan_rsp rsp;
4426         struct l2cap_chan *chan;
4427         struct hci_dev *hdev;
4428         u16 psm, scid;
4429
4430         if (cmd_len != sizeof(*req))
4431                 return -EPROTO;
4432
4433         if (!conn->hs_enabled)
4434                 return -EINVAL;
4435
4436         psm = le16_to_cpu(req->psm);
4437         scid = le16_to_cpu(req->scid);
4438
4439         BT_DBG("psm 0x%2.2x, scid 0x%4.4x, amp_id %d", psm, scid, req->amp_id);
4440
4441         /* For controller id 0 make BR/EDR connection */
4442         if (req->amp_id == AMP_ID_BREDR) {
4443                 l2cap_connect(conn, cmd, data, L2CAP_CREATE_CHAN_RSP,
4444                               req->amp_id);
4445                 return 0;
4446         }
4447
4448         /* Validate AMP controller id */
4449         hdev = hci_dev_get(req->amp_id);
4450         if (!hdev)
4451                 goto error;
4452
4453         if (hdev->dev_type != HCI_AMP || !test_bit(HCI_UP, &hdev->flags)) {
4454                 hci_dev_put(hdev);
4455                 goto error;
4456         }
4457
4458         chan = l2cap_connect(conn, cmd, data, L2CAP_CREATE_CHAN_RSP,
4459                              req->amp_id);
4460         if (chan) {
4461                 struct amp_mgr *mgr = conn->hcon->amp_mgr;
4462                 struct hci_conn *hs_hcon;
4463
4464                 hs_hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK,
4465                                                   &conn->hcon->dst);
4466                 if (!hs_hcon) {
4467                         hci_dev_put(hdev);
4468                         cmd_reject_invalid_cid(conn, cmd->ident, chan->scid,
4469                                                chan->dcid);
4470                         return 0;
4471                 }
4472
4473                 BT_DBG("mgr %p bredr_chan %p hs_hcon %p", mgr, chan, hs_hcon);
4474
4475                 mgr->bredr_chan = chan;
4476                 chan->hs_hcon = hs_hcon;
4477                 chan->fcs = L2CAP_FCS_NONE;
4478                 conn->mtu = hdev->block_mtu;
4479         }
4480
4481         hci_dev_put(hdev);
4482
4483         return 0;
4484
4485 error:
4486         rsp.dcid = 0;
4487         rsp.scid = cpu_to_le16(scid);
4488         rsp.result = cpu_to_le16(L2CAP_CR_BAD_AMP);
4489         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
4490
4491         l2cap_send_cmd(conn, cmd->ident, L2CAP_CREATE_CHAN_RSP,
4492                        sizeof(rsp), &rsp);
4493
4494         return 0;
4495 }
4496
4497 static void l2cap_send_move_chan_req(struct l2cap_chan *chan, u8 dest_amp_id)
4498 {
4499         struct l2cap_move_chan_req req;
4500         u8 ident;
4501
4502         BT_DBG("chan %p, dest_amp_id %d", chan, dest_amp_id);
4503
4504         ident = l2cap_get_ident(chan->conn);
4505         chan->ident = ident;
4506
4507         req.icid = cpu_to_le16(chan->scid);
4508         req.dest_amp_id = dest_amp_id;
4509
4510         l2cap_send_cmd(chan->conn, ident, L2CAP_MOVE_CHAN_REQ, sizeof(req),
4511                        &req);
4512
4513         __set_chan_timer(chan, L2CAP_MOVE_TIMEOUT);
4514 }
4515
4516 static void l2cap_send_move_chan_rsp(struct l2cap_chan *chan, u16 result)
4517 {
4518         struct l2cap_move_chan_rsp rsp;
4519
4520         BT_DBG("chan %p, result 0x%4.4x", chan, result);
4521
4522         rsp.icid = cpu_to_le16(chan->dcid);
4523         rsp.result = cpu_to_le16(result);
4524
4525         l2cap_send_cmd(chan->conn, chan->ident, L2CAP_MOVE_CHAN_RSP,
4526                        sizeof(rsp), &rsp);
4527 }
4528
4529 static void l2cap_send_move_chan_cfm(struct l2cap_chan *chan, u16 result)
4530 {
4531         struct l2cap_move_chan_cfm cfm;
4532
4533         BT_DBG("chan %p, result 0x%4.4x", chan, result);
4534
4535         chan->ident = l2cap_get_ident(chan->conn);
4536
4537         cfm.icid = cpu_to_le16(chan->scid);
4538         cfm.result = cpu_to_le16(result);
4539
4540         l2cap_send_cmd(chan->conn, chan->ident, L2CAP_MOVE_CHAN_CFM,
4541                        sizeof(cfm), &cfm);
4542
4543         __set_chan_timer(chan, L2CAP_MOVE_TIMEOUT);
4544 }
4545
4546 static void l2cap_send_move_chan_cfm_icid(struct l2cap_conn *conn, u16 icid)
4547 {
4548         struct l2cap_move_chan_cfm cfm;
4549
4550         BT_DBG("conn %p, icid 0x%4.4x", conn, icid);
4551
4552         cfm.icid = cpu_to_le16(icid);
4553         cfm.result = cpu_to_le16(L2CAP_MC_UNCONFIRMED);
4554
4555         l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_MOVE_CHAN_CFM,
4556                        sizeof(cfm), &cfm);
4557 }
4558
4559 static void l2cap_send_move_chan_cfm_rsp(struct l2cap_conn *conn, u8 ident,
4560                                          u16 icid)
4561 {
4562         struct l2cap_move_chan_cfm_rsp rsp;
4563
4564         BT_DBG("icid 0x%4.4x", icid);
4565
4566         rsp.icid = cpu_to_le16(icid);
4567         l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM_RSP, sizeof(rsp), &rsp);
4568 }
4569
4570 static void __release_logical_link(struct l2cap_chan *chan)
4571 {
4572         chan->hs_hchan = NULL;
4573         chan->hs_hcon = NULL;
4574
4575         /* Placeholder - release the logical link */
4576 }
4577
4578 static void l2cap_logical_fail(struct l2cap_chan *chan)
4579 {
4580         /* Logical link setup failed */
4581         if (chan->state != BT_CONNECTED) {
4582                 /* Create channel failure, disconnect */
4583                 l2cap_send_disconn_req(chan, ECONNRESET);
4584                 return;
4585         }
4586
4587         switch (chan->move_role) {
4588         case L2CAP_MOVE_ROLE_RESPONDER:
4589                 l2cap_move_done(chan);
4590                 l2cap_send_move_chan_rsp(chan, L2CAP_MR_NOT_SUPP);
4591                 break;
4592         case L2CAP_MOVE_ROLE_INITIATOR:
4593                 if (chan->move_state == L2CAP_MOVE_WAIT_LOGICAL_COMP ||
4594                     chan->move_state == L2CAP_MOVE_WAIT_LOGICAL_CFM) {
4595                         /* Remote has only sent pending or
4596                          * success responses, clean up
4597                          */
4598                         l2cap_move_done(chan);
4599                 }
4600
4601                 /* Other amp move states imply that the move
4602                  * has already aborted
4603                  */
4604                 l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
4605                 break;
4606         }
4607 }
4608
4609 static void l2cap_logical_finish_create(struct l2cap_chan *chan,
4610                                         struct hci_chan *hchan)
4611 {
4612         struct l2cap_conf_rsp rsp;
4613
4614         chan->hs_hchan = hchan;
4615         chan->hs_hcon->l2cap_data = chan->conn;
4616
4617         l2cap_send_efs_conf_rsp(chan, &rsp, chan->ident, 0);
4618
4619         if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
4620                 int err;
4621
4622                 set_default_fcs(chan);
4623
4624                 err = l2cap_ertm_init(chan);
4625                 if (err < 0)
4626                         l2cap_send_disconn_req(chan, -err);
4627                 else
4628                         l2cap_chan_ready(chan);
4629         }
4630 }
4631
4632 static void l2cap_logical_finish_move(struct l2cap_chan *chan,
4633                                       struct hci_chan *hchan)
4634 {
4635         chan->hs_hcon = hchan->conn;
4636         chan->hs_hcon->l2cap_data = chan->conn;
4637
4638         BT_DBG("move_state %d", chan->move_state);
4639
4640         switch (chan->move_state) {
4641         case L2CAP_MOVE_WAIT_LOGICAL_COMP:
4642                 /* Move confirm will be sent after a success
4643                  * response is received
4644                  */
4645                 chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
4646                 break;
4647         case L2CAP_MOVE_WAIT_LOGICAL_CFM:
4648                 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
4649                         chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
4650                 } else if (chan->move_role == L2CAP_MOVE_ROLE_INITIATOR) {
4651                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM_RSP;
4652                         l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
4653                 } else if (chan->move_role == L2CAP_MOVE_ROLE_RESPONDER) {
4654                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
4655                         l2cap_send_move_chan_rsp(chan, L2CAP_MR_SUCCESS);
4656                 }
4657                 break;
4658         default:
4659                 /* Move was not in expected state, free the channel */
4660                 __release_logical_link(chan);
4661
4662                 chan->move_state = L2CAP_MOVE_STABLE;
4663         }
4664 }
4665
4666 /* Call with chan locked */
4667 void l2cap_logical_cfm(struct l2cap_chan *chan, struct hci_chan *hchan,
4668                        u8 status)
4669 {
4670         BT_DBG("chan %p, hchan %p, status %d", chan, hchan, status);
4671
4672         if (status) {
4673                 l2cap_logical_fail(chan);
4674                 __release_logical_link(chan);
4675                 return;
4676         }
4677
4678         if (chan->state != BT_CONNECTED) {
4679                 /* Ignore logical link if channel is on BR/EDR */
4680                 if (chan->local_amp_id != AMP_ID_BREDR)
4681                         l2cap_logical_finish_create(chan, hchan);
4682         } else {
4683                 l2cap_logical_finish_move(chan, hchan);
4684         }
4685 }
4686
4687 void l2cap_move_start(struct l2cap_chan *chan)
4688 {
4689         BT_DBG("chan %p", chan);
4690
4691         if (chan->local_amp_id == AMP_ID_BREDR) {
4692                 if (chan->chan_policy != BT_CHANNEL_POLICY_AMP_PREFERRED)
4693                         return;
4694                 chan->move_role = L2CAP_MOVE_ROLE_INITIATOR;
4695                 chan->move_state = L2CAP_MOVE_WAIT_PREPARE;
4696                 /* Placeholder - start physical link setup */
4697         } else {
4698                 chan->move_role = L2CAP_MOVE_ROLE_INITIATOR;
4699                 chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
4700                 chan->move_id = 0;
4701                 l2cap_move_setup(chan);
4702                 l2cap_send_move_chan_req(chan, 0);
4703         }
4704 }
4705
4706 static void l2cap_do_create(struct l2cap_chan *chan, int result,
4707                             u8 local_amp_id, u8 remote_amp_id)
4708 {
4709         BT_DBG("chan %p state %s %u -> %u", chan, state_to_string(chan->state),
4710                local_amp_id, remote_amp_id);
4711
4712         chan->fcs = L2CAP_FCS_NONE;
4713
4714         /* Outgoing channel on AMP */
4715         if (chan->state == BT_CONNECT) {
4716                 if (result == L2CAP_CR_SUCCESS) {
4717                         chan->local_amp_id = local_amp_id;
4718                         l2cap_send_create_chan_req(chan, remote_amp_id);
4719                 } else {
4720                         /* Revert to BR/EDR connect */
4721                         l2cap_send_conn_req(chan);
4722                 }
4723
4724                 return;
4725         }
4726
4727         /* Incoming channel on AMP */
4728         if (__l2cap_no_conn_pending(chan)) {
4729                 struct l2cap_conn_rsp rsp;
4730                 char buf[128];
4731                 rsp.scid = cpu_to_le16(chan->dcid);
4732                 rsp.dcid = cpu_to_le16(chan->scid);
4733
4734                 if (result == L2CAP_CR_SUCCESS) {
4735                         /* Send successful response */
4736                         rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
4737                         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
4738                 } else {
4739                         /* Send negative response */
4740                         rsp.result = cpu_to_le16(L2CAP_CR_NO_MEM);
4741                         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
4742                 }
4743
4744                 l2cap_send_cmd(chan->conn, chan->ident, L2CAP_CREATE_CHAN_RSP,
4745                                sizeof(rsp), &rsp);
4746
4747                 if (result == L2CAP_CR_SUCCESS) {
4748                         l2cap_state_change(chan, BT_CONFIG);
4749                         set_bit(CONF_REQ_SENT, &chan->conf_state);
4750                         l2cap_send_cmd(chan->conn, l2cap_get_ident(chan->conn),
4751                                        L2CAP_CONF_REQ,
4752                                        l2cap_build_conf_req(chan, buf), buf);
4753                         chan->num_conf_req++;
4754                 }
4755         }
4756 }
4757
4758 static void l2cap_do_move_initiate(struct l2cap_chan *chan, u8 local_amp_id,
4759                                    u8 remote_amp_id)
4760 {
4761         l2cap_move_setup(chan);
4762         chan->move_id = local_amp_id;
4763         chan->move_state = L2CAP_MOVE_WAIT_RSP;
4764
4765         l2cap_send_move_chan_req(chan, remote_amp_id);
4766 }
4767
4768 static void l2cap_do_move_respond(struct l2cap_chan *chan, int result)
4769 {
4770         struct hci_chan *hchan = NULL;
4771
4772         /* Placeholder - get hci_chan for logical link */
4773
4774         if (hchan) {
4775                 if (hchan->state == BT_CONNECTED) {
4776                         /* Logical link is ready to go */
4777                         chan->hs_hcon = hchan->conn;
4778                         chan->hs_hcon->l2cap_data = chan->conn;
4779                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
4780                         l2cap_send_move_chan_rsp(chan, L2CAP_MR_SUCCESS);
4781
4782                         l2cap_logical_cfm(chan, hchan, L2CAP_MR_SUCCESS);
4783                 } else {
4784                         /* Wait for logical link to be ready */
4785                         chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
4786                 }
4787         } else {
4788                 /* Logical link not available */
4789                 l2cap_send_move_chan_rsp(chan, L2CAP_MR_NOT_ALLOWED);
4790         }
4791 }
4792
4793 static void l2cap_do_move_cancel(struct l2cap_chan *chan, int result)
4794 {
4795         if (chan->move_role == L2CAP_MOVE_ROLE_RESPONDER) {
4796                 u8 rsp_result;
4797                 if (result == -EINVAL)
4798                         rsp_result = L2CAP_MR_BAD_ID;
4799                 else
4800                         rsp_result = L2CAP_MR_NOT_ALLOWED;
4801
4802                 l2cap_send_move_chan_rsp(chan, rsp_result);
4803         }
4804
4805         chan->move_role = L2CAP_MOVE_ROLE_NONE;
4806         chan->move_state = L2CAP_MOVE_STABLE;
4807
4808         /* Restart data transmission */
4809         l2cap_ertm_send(chan);
4810 }
4811
4812 /* Invoke with locked chan */
4813 void __l2cap_physical_cfm(struct l2cap_chan *chan, int result)
4814 {
4815         u8 local_amp_id = chan->local_amp_id;
4816         u8 remote_amp_id = chan->remote_amp_id;
4817
4818         BT_DBG("chan %p, result %d, local_amp_id %d, remote_amp_id %d",
4819                chan, result, local_amp_id, remote_amp_id);
4820
4821         if (chan->state == BT_DISCONN || chan->state == BT_CLOSED) {
4822                 l2cap_chan_unlock(chan);
4823                 return;
4824         }
4825
4826         if (chan->state != BT_CONNECTED) {
4827                 l2cap_do_create(chan, result, local_amp_id, remote_amp_id);
4828         } else if (result != L2CAP_MR_SUCCESS) {
4829                 l2cap_do_move_cancel(chan, result);
4830         } else {
4831                 switch (chan->move_role) {
4832                 case L2CAP_MOVE_ROLE_INITIATOR:
4833                         l2cap_do_move_initiate(chan, local_amp_id,
4834                                                remote_amp_id);
4835                         break;
4836                 case L2CAP_MOVE_ROLE_RESPONDER:
4837                         l2cap_do_move_respond(chan, result);
4838                         break;
4839                 default:
4840                         l2cap_do_move_cancel(chan, result);
4841                         break;
4842                 }
4843         }
4844 }
4845
4846 static inline int l2cap_move_channel_req(struct l2cap_conn *conn,
4847                                          struct l2cap_cmd_hdr *cmd,
4848                                          u16 cmd_len, void *data)
4849 {
4850         struct l2cap_move_chan_req *req = data;
4851         struct l2cap_move_chan_rsp rsp;
4852         struct l2cap_chan *chan;
4853         u16 icid = 0;
4854         u16 result = L2CAP_MR_NOT_ALLOWED;
4855
4856         if (cmd_len != sizeof(*req))
4857                 return -EPROTO;
4858
4859         icid = le16_to_cpu(req->icid);
4860
4861         BT_DBG("icid 0x%4.4x, dest_amp_id %d", icid, req->dest_amp_id);
4862
4863         if (!conn->hs_enabled)
4864                 return -EINVAL;
4865
4866         chan = l2cap_get_chan_by_dcid(conn, icid);
4867         if (!chan) {
4868                 rsp.icid = cpu_to_le16(icid);
4869                 rsp.result = cpu_to_le16(L2CAP_MR_NOT_ALLOWED);
4870                 l2cap_send_cmd(conn, cmd->ident, L2CAP_MOVE_CHAN_RSP,
4871                                sizeof(rsp), &rsp);
4872                 return 0;
4873         }
4874
4875         chan->ident = cmd->ident;
4876
4877         if (chan->scid < L2CAP_CID_DYN_START ||
4878             chan->chan_policy == BT_CHANNEL_POLICY_BREDR_ONLY ||
4879             (chan->mode != L2CAP_MODE_ERTM &&
4880              chan->mode != L2CAP_MODE_STREAMING)) {
4881                 result = L2CAP_MR_NOT_ALLOWED;
4882                 goto send_move_response;
4883         }
4884
4885         if (chan->local_amp_id == req->dest_amp_id) {
4886                 result = L2CAP_MR_SAME_ID;
4887                 goto send_move_response;
4888         }
4889
4890         if (req->dest_amp_id != AMP_ID_BREDR) {
4891                 struct hci_dev *hdev;
4892                 hdev = hci_dev_get(req->dest_amp_id);
4893                 if (!hdev || hdev->dev_type != HCI_AMP ||
4894                     !test_bit(HCI_UP, &hdev->flags)) {
4895                         if (hdev)
4896                                 hci_dev_put(hdev);
4897
4898                         result = L2CAP_MR_BAD_ID;
4899                         goto send_move_response;
4900                 }
4901                 hci_dev_put(hdev);
4902         }
4903
4904         /* Detect a move collision.  Only send a collision response
4905          * if this side has "lost", otherwise proceed with the move.
4906          * The winner has the larger bd_addr.
4907          */
4908         if ((__chan_is_moving(chan) ||
4909              chan->move_role != L2CAP_MOVE_ROLE_NONE) &&
4910             bacmp(&conn->hcon->src, &conn->hcon->dst) > 0) {
4911                 result = L2CAP_MR_COLLISION;
4912                 goto send_move_response;
4913         }
4914
4915         chan->move_role = L2CAP_MOVE_ROLE_RESPONDER;
4916         l2cap_move_setup(chan);
4917         chan->move_id = req->dest_amp_id;
4918         icid = chan->dcid;
4919
4920         if (req->dest_amp_id == AMP_ID_BREDR) {
4921                 /* Moving to BR/EDR */
4922                 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
4923                         chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
4924                         result = L2CAP_MR_PEND;
4925                 } else {
4926                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
4927                         result = L2CAP_MR_SUCCESS;
4928                 }
4929         } else {
4930                 chan->move_state = L2CAP_MOVE_WAIT_PREPARE;
4931                 /* Placeholder - uncomment when amp functions are available */
4932                 /*amp_accept_physical(chan, req->dest_amp_id);*/
4933                 result = L2CAP_MR_PEND;
4934         }
4935
4936 send_move_response:
4937         l2cap_send_move_chan_rsp(chan, result);
4938
4939         l2cap_chan_unlock(chan);
4940
4941         return 0;
4942 }
4943
4944 static void l2cap_move_continue(struct l2cap_conn *conn, u16 icid, u16 result)
4945 {
4946         struct l2cap_chan *chan;
4947         struct hci_chan *hchan = NULL;
4948
4949         chan = l2cap_get_chan_by_scid(conn, icid);
4950         if (!chan) {
4951                 l2cap_send_move_chan_cfm_icid(conn, icid);
4952                 return;
4953         }
4954
4955         __clear_chan_timer(chan);
4956         if (result == L2CAP_MR_PEND)
4957                 __set_chan_timer(chan, L2CAP_MOVE_ERTX_TIMEOUT);
4958
4959         switch (chan->move_state) {
4960         case L2CAP_MOVE_WAIT_LOGICAL_COMP:
4961                 /* Move confirm will be sent when logical link
4962                  * is complete.
4963                  */
4964                 chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
4965                 break;
4966         case L2CAP_MOVE_WAIT_RSP_SUCCESS:
4967                 if (result == L2CAP_MR_PEND) {
4968                         break;
4969                 } else if (test_bit(CONN_LOCAL_BUSY,
4970                                     &chan->conn_state)) {
4971                         chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
4972                 } else {
4973                         /* Logical link is up or moving to BR/EDR,
4974                          * proceed with move
4975                          */
4976                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM_RSP;
4977                         l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
4978                 }
4979                 break;
4980         case L2CAP_MOVE_WAIT_RSP:
4981                 /* Moving to AMP */
4982                 if (result == L2CAP_MR_SUCCESS) {
4983                         /* Remote is ready, send confirm immediately
4984                          * after logical link is ready
4985                          */
4986                         chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
4987                 } else {
4988                         /* Both logical link and move success
4989                          * are required to confirm
4990                          */
4991                         chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_COMP;
4992                 }
4993
4994                 /* Placeholder - get hci_chan for logical link */
4995                 if (!hchan) {
4996                         /* Logical link not available */
4997                         l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
4998                         break;
4999                 }
5000
5001                 /* If the logical link is not yet connected, do not
5002                  * send confirmation.
5003                  */
5004                 if (hchan->state != BT_CONNECTED)
5005                         break;
5006
5007                 /* Logical link is already ready to go */
5008
5009                 chan->hs_hcon = hchan->conn;
5010                 chan->hs_hcon->l2cap_data = chan->conn;
5011
5012                 if (result == L2CAP_MR_SUCCESS) {
5013                         /* Can confirm now */
5014                         l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
5015                 } else {
5016                         /* Now only need move success
5017                          * to confirm
5018                          */
5019                         chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
5020                 }
5021
5022                 l2cap_logical_cfm(chan, hchan, L2CAP_MR_SUCCESS);
5023                 break;
5024         default:
5025                 /* Any other amp move state means the move failed. */
5026                 chan->move_id = chan->local_amp_id;
5027                 l2cap_move_done(chan);
5028                 l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
5029         }
5030
5031         l2cap_chan_unlock(chan);
5032 }
5033
5034 static void l2cap_move_fail(struct l2cap_conn *conn, u8 ident, u16 icid,
5035                             u16 result)
5036 {
5037         struct l2cap_chan *chan;
5038
5039         chan = l2cap_get_chan_by_ident(conn, ident);
5040         if (!chan) {
5041                 /* Could not locate channel, icid is best guess */
5042                 l2cap_send_move_chan_cfm_icid(conn, icid);
5043                 return;
5044         }
5045
5046         __clear_chan_timer(chan);
5047
5048         if (chan->move_role == L2CAP_MOVE_ROLE_INITIATOR) {
5049                 if (result == L2CAP_MR_COLLISION) {
5050                         chan->move_role = L2CAP_MOVE_ROLE_RESPONDER;
5051                 } else {
5052                         /* Cleanup - cancel move */
5053                         chan->move_id = chan->local_amp_id;
5054                         l2cap_move_done(chan);
5055                 }
5056         }
5057
5058         l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
5059
5060         l2cap_chan_unlock(chan);
5061 }
5062
5063 static int l2cap_move_channel_rsp(struct l2cap_conn *conn,
5064                                   struct l2cap_cmd_hdr *cmd,
5065                                   u16 cmd_len, void *data)
5066 {
5067         struct l2cap_move_chan_rsp *rsp = data;
5068         u16 icid, result;
5069
5070         if (cmd_len != sizeof(*rsp))
5071                 return -EPROTO;
5072
5073         icid = le16_to_cpu(rsp->icid);
5074         result = le16_to_cpu(rsp->result);
5075
5076         BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result);
5077
5078         if (result == L2CAP_MR_SUCCESS || result == L2CAP_MR_PEND)
5079                 l2cap_move_continue(conn, icid, result);
5080         else
5081                 l2cap_move_fail(conn, cmd->ident, icid, result);
5082
5083         return 0;
5084 }
5085
5086 static int l2cap_move_channel_confirm(struct l2cap_conn *conn,
5087                                       struct l2cap_cmd_hdr *cmd,
5088                                       u16 cmd_len, void *data)
5089 {
5090         struct l2cap_move_chan_cfm *cfm = data;
5091         struct l2cap_chan *chan;
5092         u16 icid, result;
5093
5094         if (cmd_len != sizeof(*cfm))
5095                 return -EPROTO;
5096
5097         icid = le16_to_cpu(cfm->icid);
5098         result = le16_to_cpu(cfm->result);
5099
5100         BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result);
5101
5102         chan = l2cap_get_chan_by_dcid(conn, icid);
5103         if (!chan) {
5104                 /* Spec requires a response even if the icid was not found */
5105                 l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid);
5106                 return 0;
5107         }
5108
5109         if (chan->move_state == L2CAP_MOVE_WAIT_CONFIRM) {
5110                 if (result == L2CAP_MC_CONFIRMED) {
5111                         chan->local_amp_id = chan->move_id;
5112                         if (chan->local_amp_id == AMP_ID_BREDR)
5113                                 __release_logical_link(chan);
5114                 } else {
5115                         chan->move_id = chan->local_amp_id;
5116                 }
5117
5118                 l2cap_move_done(chan);
5119         }
5120
5121         l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid);
5122
5123         l2cap_chan_unlock(chan);
5124
5125         return 0;
5126 }
5127
5128 static inline int l2cap_move_channel_confirm_rsp(struct l2cap_conn *conn,
5129                                                  struct l2cap_cmd_hdr *cmd,
5130                                                  u16 cmd_len, void *data)
5131 {
5132         struct l2cap_move_chan_cfm_rsp *rsp = data;
5133         struct l2cap_chan *chan;
5134         u16 icid;
5135
5136         if (cmd_len != sizeof(*rsp))
5137                 return -EPROTO;
5138
5139         icid = le16_to_cpu(rsp->icid);
5140
5141         BT_DBG("icid 0x%4.4x", icid);
5142
5143         chan = l2cap_get_chan_by_scid(conn, icid);
5144         if (!chan)
5145                 return 0;
5146
5147         __clear_chan_timer(chan);
5148
5149         if (chan->move_state == L2CAP_MOVE_WAIT_CONFIRM_RSP) {
5150                 chan->local_amp_id = chan->move_id;
5151
5152                 if (chan->local_amp_id == AMP_ID_BREDR && chan->hs_hchan)
5153                         __release_logical_link(chan);
5154
5155                 l2cap_move_done(chan);
5156         }
5157
5158         l2cap_chan_unlock(chan);
5159
5160         return 0;
5161 }
5162
5163 static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
5164                                               struct l2cap_cmd_hdr *cmd,
5165                                               u16 cmd_len, u8 *data)
5166 {
5167         struct hci_conn *hcon = conn->hcon;
5168         struct l2cap_conn_param_update_req *req;
5169         struct l2cap_conn_param_update_rsp rsp;
5170         u16 min, max, latency, to_multiplier;
5171         int err;
5172
5173         if (hcon->role != HCI_ROLE_MASTER)
5174                 return -EINVAL;
5175
5176         if (cmd_len != sizeof(struct l2cap_conn_param_update_req))
5177                 return -EPROTO;
5178
5179         req = (struct l2cap_conn_param_update_req *) data;
5180         min             = __le16_to_cpu(req->min);
5181         max             = __le16_to_cpu(req->max);
5182         latency         = __le16_to_cpu(req->latency);
5183         to_multiplier   = __le16_to_cpu(req->to_multiplier);
5184
5185         BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x",
5186                min, max, latency, to_multiplier);
5187
5188         memset(&rsp, 0, sizeof(rsp));
5189
5190         err = hci_check_conn_params(min, max, latency, to_multiplier);
5191         if (err)
5192                 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_REJECTED);
5193         else
5194                 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED);
5195
5196         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP,
5197                        sizeof(rsp), &rsp);
5198
5199         if (!err) {
5200                 u8 store_hint;
5201
5202                 store_hint = hci_le_conn_update(hcon, min, max, latency,
5203                                                 to_multiplier);
5204                 mgmt_new_conn_param(hcon->hdev, &hcon->dst, hcon->dst_type,
5205                                     store_hint, min, max, latency,
5206                                     to_multiplier);
5207
5208         }
5209
5210         return 0;
5211 }
5212
5213 static int l2cap_le_connect_rsp(struct l2cap_conn *conn,
5214                                 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5215                                 u8 *data)
5216 {
5217         struct l2cap_le_conn_rsp *rsp = (struct l2cap_le_conn_rsp *) data;
5218         u16 dcid, mtu, mps, credits, result;
5219         struct l2cap_chan *chan;
5220         int err;
5221
5222         if (cmd_len < sizeof(*rsp))
5223                 return -EPROTO;
5224
5225         dcid    = __le16_to_cpu(rsp->dcid);
5226         mtu     = __le16_to_cpu(rsp->mtu);
5227         mps     = __le16_to_cpu(rsp->mps);
5228         credits = __le16_to_cpu(rsp->credits);
5229         result  = __le16_to_cpu(rsp->result);
5230
5231         if (result == L2CAP_CR_SUCCESS && (mtu < 23 || mps < 23))
5232                 return -EPROTO;
5233
5234         BT_DBG("dcid 0x%4.4x mtu %u mps %u credits %u result 0x%2.2x",
5235                dcid, mtu, mps, credits, result);
5236
5237         mutex_lock(&conn->chan_lock);
5238
5239         chan = __l2cap_get_chan_by_ident(conn, cmd->ident);
5240         if (!chan) {
5241                 err = -EBADSLT;
5242                 goto unlock;
5243         }
5244
5245         err = 0;
5246
5247         l2cap_chan_lock(chan);
5248
5249         switch (result) {
5250         case L2CAP_CR_SUCCESS:
5251                 chan->ident = 0;
5252                 chan->dcid = dcid;
5253                 chan->omtu = mtu;
5254                 chan->remote_mps = mps;
5255                 chan->tx_credits = credits;
5256                 l2cap_chan_ready(chan);
5257                 break;
5258
5259         default:
5260                 l2cap_chan_del(chan, ECONNREFUSED);
5261                 break;
5262         }
5263
5264         l2cap_chan_unlock(chan);
5265
5266 unlock:
5267         mutex_unlock(&conn->chan_lock);
5268
5269         return err;
5270 }
5271
5272 static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
5273                                       struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5274                                       u8 *data)
5275 {
5276         int err = 0;
5277
5278         switch (cmd->code) {
5279         case L2CAP_COMMAND_REJ:
5280                 l2cap_command_rej(conn, cmd, cmd_len, data);
5281                 break;
5282
5283         case L2CAP_CONN_REQ:
5284                 err = l2cap_connect_req(conn, cmd, cmd_len, data);
5285                 break;
5286
5287         case L2CAP_CONN_RSP:
5288         case L2CAP_CREATE_CHAN_RSP:
5289                 l2cap_connect_create_rsp(conn, cmd, cmd_len, data);
5290                 break;
5291
5292         case L2CAP_CONF_REQ:
5293                 err = l2cap_config_req(conn, cmd, cmd_len, data);
5294                 break;
5295
5296         case L2CAP_CONF_RSP:
5297                 l2cap_config_rsp(conn, cmd, cmd_len, data);
5298                 break;
5299
5300         case L2CAP_DISCONN_REQ:
5301                 err = l2cap_disconnect_req(conn, cmd, cmd_len, data);
5302                 break;
5303
5304         case L2CAP_DISCONN_RSP:
5305                 l2cap_disconnect_rsp(conn, cmd, cmd_len, data);
5306                 break;
5307
5308         case L2CAP_ECHO_REQ:
5309                 l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data);
5310                 break;
5311
5312         case L2CAP_ECHO_RSP:
5313                 break;
5314
5315         case L2CAP_INFO_REQ:
5316                 err = l2cap_information_req(conn, cmd, cmd_len, data);
5317                 break;
5318
5319         case L2CAP_INFO_RSP:
5320                 l2cap_information_rsp(conn, cmd, cmd_len, data);
5321                 break;
5322
5323         case L2CAP_CREATE_CHAN_REQ:
5324                 err = l2cap_create_channel_req(conn, cmd, cmd_len, data);
5325                 break;
5326
5327         case L2CAP_MOVE_CHAN_REQ:
5328                 err = l2cap_move_channel_req(conn, cmd, cmd_len, data);
5329                 break;
5330
5331         case L2CAP_MOVE_CHAN_RSP:
5332                 l2cap_move_channel_rsp(conn, cmd, cmd_len, data);
5333                 break;
5334
5335         case L2CAP_MOVE_CHAN_CFM:
5336                 err = l2cap_move_channel_confirm(conn, cmd, cmd_len, data);
5337                 break;
5338
5339         case L2CAP_MOVE_CHAN_CFM_RSP:
5340                 l2cap_move_channel_confirm_rsp(conn, cmd, cmd_len, data);
5341                 break;
5342
5343         default:
5344                 BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code);
5345                 err = -EINVAL;
5346                 break;
5347         }
5348
5349         return err;
5350 }
5351
5352 static int l2cap_le_connect_req(struct l2cap_conn *conn,
5353                                 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5354                                 u8 *data)
5355 {
5356         struct l2cap_le_conn_req *req = (struct l2cap_le_conn_req *) data;
5357         struct l2cap_le_conn_rsp rsp;
5358         struct l2cap_chan *chan, *pchan;
5359         u16 dcid, scid, credits, mtu, mps;
5360         __le16 psm;
5361         u8 result;
5362
5363         if (cmd_len != sizeof(*req))
5364                 return -EPROTO;
5365
5366         scid = __le16_to_cpu(req->scid);
5367         mtu  = __le16_to_cpu(req->mtu);
5368         mps  = __le16_to_cpu(req->mps);
5369         psm  = req->psm;
5370         dcid = 0;
5371         credits = 0;
5372
5373         if (mtu < 23 || mps < 23)
5374                 return -EPROTO;
5375
5376         BT_DBG("psm 0x%2.2x scid 0x%4.4x mtu %u mps %u", __le16_to_cpu(psm),
5377                scid, mtu, mps);
5378
5379         /* Check if we have socket listening on psm */
5380         pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, &conn->hcon->src,
5381                                          &conn->hcon->dst, LE_LINK);
5382         if (!pchan) {
5383                 result = L2CAP_CR_BAD_PSM;
5384                 chan = NULL;
5385                 goto response;
5386         }
5387
5388         mutex_lock(&conn->chan_lock);
5389         l2cap_chan_lock(pchan);
5390
5391         if (!smp_sufficient_security(conn->hcon, pchan->sec_level)) {
5392                 result = L2CAP_CR_AUTHENTICATION;
5393                 chan = NULL;
5394                 goto response_unlock;
5395         }
5396
5397         /* Check if we already have channel with that dcid */
5398         if (__l2cap_get_chan_by_dcid(conn, scid)) {
5399                 result = L2CAP_CR_NO_MEM;
5400                 chan = NULL;
5401                 goto response_unlock;
5402         }
5403
5404         chan = pchan->ops->new_connection(pchan);
5405         if (!chan) {
5406                 result = L2CAP_CR_NO_MEM;
5407                 goto response_unlock;
5408         }
5409
5410         l2cap_le_flowctl_init(chan);
5411
5412         bacpy(&chan->src, &conn->hcon->src);
5413         bacpy(&chan->dst, &conn->hcon->dst);
5414         chan->src_type = bdaddr_type(conn->hcon, conn->hcon->src_type);
5415         chan->dst_type = bdaddr_type(conn->hcon, conn->hcon->dst_type);
5416         chan->psm  = psm;
5417         chan->dcid = scid;
5418         chan->omtu = mtu;
5419         chan->remote_mps = mps;
5420         chan->tx_credits = __le16_to_cpu(req->credits);
5421
5422         __l2cap_chan_add(conn, chan);
5423         dcid = chan->scid;
5424         credits = chan->rx_credits;
5425
5426         __set_chan_timer(chan, chan->ops->get_sndtimeo(chan));
5427
5428         chan->ident = cmd->ident;
5429
5430         if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
5431                 l2cap_state_change(chan, BT_CONNECT2);
5432                 /* The following result value is actually not defined
5433                  * for LE CoC but we use it to let the function know
5434                  * that it should bail out after doing its cleanup
5435                  * instead of sending a response.
5436                  */
5437                 result = L2CAP_CR_PEND;
5438                 chan->ops->defer(chan);
5439         } else {
5440                 l2cap_chan_ready(chan);
5441                 result = L2CAP_CR_SUCCESS;
5442         }
5443
5444 response_unlock:
5445         l2cap_chan_unlock(pchan);
5446         mutex_unlock(&conn->chan_lock);
5447         l2cap_chan_put(pchan);
5448
5449         if (result == L2CAP_CR_PEND)
5450                 return 0;
5451
5452 response:
5453         if (chan) {
5454                 rsp.mtu = cpu_to_le16(chan->imtu);
5455                 rsp.mps = cpu_to_le16(chan->mps);
5456         } else {
5457                 rsp.mtu = 0;
5458                 rsp.mps = 0;
5459         }
5460
5461         rsp.dcid    = cpu_to_le16(dcid);
5462         rsp.credits = cpu_to_le16(credits);
5463         rsp.result  = cpu_to_le16(result);
5464
5465         l2cap_send_cmd(conn, cmd->ident, L2CAP_LE_CONN_RSP, sizeof(rsp), &rsp);
5466
5467         return 0;
5468 }
5469
5470 static inline int l2cap_le_credits(struct l2cap_conn *conn,
5471                                    struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5472                                    u8 *data)
5473 {
5474         struct l2cap_le_credits *pkt;
5475         struct l2cap_chan *chan;
5476         u16 cid, credits, max_credits;
5477
5478         if (cmd_len != sizeof(*pkt))
5479                 return -EPROTO;
5480
5481         pkt = (struct l2cap_le_credits *) data;
5482         cid     = __le16_to_cpu(pkt->cid);
5483         credits = __le16_to_cpu(pkt->credits);
5484
5485         BT_DBG("cid 0x%4.4x credits 0x%4.4x", cid, credits);
5486
5487         chan = l2cap_get_chan_by_dcid(conn, cid);
5488         if (!chan)
5489                 return -EBADSLT;
5490
5491         max_credits = LE_FLOWCTL_MAX_CREDITS - chan->tx_credits;
5492         if (credits > max_credits) {
5493                 BT_ERR("LE credits overflow");
5494                 l2cap_send_disconn_req(chan, ECONNRESET);
5495                 l2cap_chan_unlock(chan);
5496
5497                 /* Return 0 so that we don't trigger an unnecessary
5498                  * command reject packet.
5499                  */
5500                 return 0;
5501         }
5502
5503         chan->tx_credits += credits;
5504
5505         while (chan->tx_credits && !skb_queue_empty(&chan->tx_q)) {
5506                 l2cap_do_send(chan, skb_dequeue(&chan->tx_q));
5507                 chan->tx_credits--;
5508         }
5509
5510         if (chan->tx_credits)
5511                 chan->ops->resume(chan);
5512
5513         l2cap_chan_unlock(chan);
5514
5515         return 0;
5516 }
5517
5518 static inline int l2cap_le_command_rej(struct l2cap_conn *conn,
5519                                        struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5520                                        u8 *data)
5521 {
5522         struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data;
5523         struct l2cap_chan *chan;
5524
5525         if (cmd_len < sizeof(*rej))
5526                 return -EPROTO;
5527
5528         mutex_lock(&conn->chan_lock);
5529
5530         chan = __l2cap_get_chan_by_ident(conn, cmd->ident);
5531         if (!chan)
5532                 goto done;
5533
5534         l2cap_chan_lock(chan);
5535         l2cap_chan_del(chan, ECONNREFUSED);
5536         l2cap_chan_unlock(chan);
5537
5538 done:
5539         mutex_unlock(&conn->chan_lock);
5540         return 0;
5541 }
5542
5543 static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
5544                                    struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5545                                    u8 *data)
5546 {
5547         int err = 0;
5548
5549         switch (cmd->code) {
5550         case L2CAP_COMMAND_REJ:
5551                 l2cap_le_command_rej(conn, cmd, cmd_len, data);
5552                 break;
5553
5554         case L2CAP_CONN_PARAM_UPDATE_REQ:
5555                 err = l2cap_conn_param_update_req(conn, cmd, cmd_len, data);
5556                 break;
5557
5558         case L2CAP_CONN_PARAM_UPDATE_RSP:
5559                 break;
5560
5561         case L2CAP_LE_CONN_RSP:
5562                 l2cap_le_connect_rsp(conn, cmd, cmd_len, data);
5563                 break;
5564
5565         case L2CAP_LE_CONN_REQ:
5566                 err = l2cap_le_connect_req(conn, cmd, cmd_len, data);
5567                 break;
5568
5569         case L2CAP_LE_CREDITS:
5570                 err = l2cap_le_credits(conn, cmd, cmd_len, data);
5571                 break;
5572
5573         case L2CAP_DISCONN_REQ:
5574                 err = l2cap_disconnect_req(conn, cmd, cmd_len, data);
5575                 break;
5576
5577         case L2CAP_DISCONN_RSP:
5578                 l2cap_disconnect_rsp(conn, cmd, cmd_len, data);
5579                 break;
5580
5581         default:
5582                 BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code);
5583                 err = -EINVAL;
5584                 break;
5585         }
5586
5587         return err;
5588 }
5589
5590 static inline void l2cap_le_sig_channel(struct l2cap_conn *conn,
5591                                         struct sk_buff *skb)
5592 {
5593         struct hci_conn *hcon = conn->hcon;
5594         struct l2cap_cmd_hdr *cmd;
5595         u16 len;
5596         int err;
5597
5598         if (hcon->type != LE_LINK)
5599                 goto drop;
5600
5601         if (skb->len < L2CAP_CMD_HDR_SIZE)
5602                 goto drop;
5603
5604         cmd = (void *) skb->data;
5605         skb_pull(skb, L2CAP_CMD_HDR_SIZE);
5606
5607         len = le16_to_cpu(cmd->len);
5608
5609         BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd->code, len, cmd->ident);
5610
5611         if (len != skb->len || !cmd->ident) {
5612                 BT_DBG("corrupted command");
5613                 goto drop;
5614         }
5615
5616         err = l2cap_le_sig_cmd(conn, cmd, len, skb->data);
5617         if (err) {
5618                 struct l2cap_cmd_rej_unk rej;
5619
5620                 BT_ERR("Wrong link type (%d)", err);
5621
5622                 rej.reason = cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
5623                 l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
5624                                sizeof(rej), &rej);
5625         }
5626
5627 drop:
5628         kfree_skb(skb);
5629 }
5630
5631 static inline void l2cap_sig_channel(struct l2cap_conn *conn,
5632                                      struct sk_buff *skb)
5633 {
5634         struct hci_conn *hcon = conn->hcon;
5635         u8 *data = skb->data;
5636         int len = skb->len;
5637         struct l2cap_cmd_hdr cmd;
5638         int err;
5639
5640         l2cap_raw_recv(conn, skb);
5641
5642         if (hcon->type != ACL_LINK)
5643                 goto drop;
5644
5645         while (len >= L2CAP_CMD_HDR_SIZE) {
5646                 u16 cmd_len;
5647                 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
5648                 data += L2CAP_CMD_HDR_SIZE;
5649                 len  -= L2CAP_CMD_HDR_SIZE;
5650
5651                 cmd_len = le16_to_cpu(cmd.len);
5652
5653                 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len,
5654                        cmd.ident);
5655
5656                 if (cmd_len > len || !cmd.ident) {
5657                         BT_DBG("corrupted command");
5658                         break;
5659                 }
5660
5661                 err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data);
5662                 if (err) {
5663                         struct l2cap_cmd_rej_unk rej;
5664
5665                         BT_ERR("Wrong link type (%d)", err);
5666
5667                         rej.reason = cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
5668                         l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ,
5669                                        sizeof(rej), &rej);
5670                 }
5671
5672                 data += cmd_len;
5673                 len  -= cmd_len;
5674         }
5675
5676 drop:
5677         kfree_skb(skb);
5678 }
5679
5680 static int l2cap_check_fcs(struct l2cap_chan *chan,  struct sk_buff *skb)
5681 {
5682         u16 our_fcs, rcv_fcs;
5683         int hdr_size;
5684
5685         if (test_bit(FLAG_EXT_CTRL, &chan->flags))
5686                 hdr_size = L2CAP_EXT_HDR_SIZE;
5687         else
5688                 hdr_size = L2CAP_ENH_HDR_SIZE;
5689
5690         if (chan->fcs == L2CAP_FCS_CRC16) {
5691                 skb_trim(skb, skb->len - L2CAP_FCS_SIZE);
5692                 rcv_fcs = get_unaligned_le16(skb->data + skb->len);
5693                 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
5694
5695                 if (our_fcs != rcv_fcs)
5696                         return -EBADMSG;
5697         }
5698         return 0;
5699 }
5700
5701 static void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan)
5702 {
5703         struct l2cap_ctrl control;
5704
5705         BT_DBG("chan %p", chan);
5706
5707         memset(&control, 0, sizeof(control));
5708         control.sframe = 1;
5709         control.final = 1;
5710         control.reqseq = chan->buffer_seq;
5711         set_bit(CONN_SEND_FBIT, &chan->conn_state);
5712
5713         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
5714                 control.super = L2CAP_SUPER_RNR;
5715                 l2cap_send_sframe(chan, &control);
5716         }
5717
5718         if (test_and_clear_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
5719             chan->unacked_frames > 0)
5720                 __set_retrans_timer(chan);
5721
5722         /* Send pending iframes */
5723         l2cap_ertm_send(chan);
5724
5725         if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
5726             test_bit(CONN_SEND_FBIT, &chan->conn_state)) {
5727                 /* F-bit wasn't sent in an s-frame or i-frame yet, so
5728                  * send it now.
5729                  */
5730                 control.super = L2CAP_SUPER_RR;
5731                 l2cap_send_sframe(chan, &control);
5732         }
5733 }
5734
5735 static void append_skb_frag(struct sk_buff *skb, struct sk_buff *new_frag,
5736                             struct sk_buff **last_frag)
5737 {
5738         /* skb->len reflects data in skb as well as all fragments
5739          * skb->data_len reflects only data in fragments
5740          */
5741         if (!skb_has_frag_list(skb))
5742                 skb_shinfo(skb)->frag_list = new_frag;
5743
5744         new_frag->next = NULL;
5745
5746         (*last_frag)->next = new_frag;
5747         *last_frag = new_frag;
5748
5749         skb->len += new_frag->len;
5750         skb->data_len += new_frag->len;
5751         skb->truesize += new_frag->truesize;
5752 }
5753
5754 static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb,
5755                                 struct l2cap_ctrl *control)
5756 {
5757         int err = -EINVAL;
5758
5759         switch (control->sar) {
5760         case L2CAP_SAR_UNSEGMENTED:
5761                 if (chan->sdu)
5762                         break;
5763
5764                 err = chan->ops->recv(chan, skb);
5765                 break;
5766
5767         case L2CAP_SAR_START:
5768                 if (chan->sdu)
5769                         break;
5770
5771                 chan->sdu_len = get_unaligned_le16(skb->data);
5772                 skb_pull(skb, L2CAP_SDULEN_SIZE);
5773
5774                 if (chan->sdu_len > chan->imtu) {
5775                         err = -EMSGSIZE;
5776                         break;
5777                 }
5778
5779                 if (skb->len >= chan->sdu_len)
5780                         break;
5781
5782                 chan->sdu = skb;
5783                 chan->sdu_last_frag = skb;
5784
5785                 skb = NULL;
5786                 err = 0;
5787                 break;
5788
5789         case L2CAP_SAR_CONTINUE:
5790                 if (!chan->sdu)
5791                         break;
5792
5793                 append_skb_frag(chan->sdu, skb,
5794                                 &chan->sdu_last_frag);
5795                 skb = NULL;
5796
5797                 if (chan->sdu->len >= chan->sdu_len)
5798                         break;
5799
5800                 err = 0;
5801                 break;
5802
5803         case L2CAP_SAR_END:
5804                 if (!chan->sdu)
5805                         break;
5806
5807                 append_skb_frag(chan->sdu, skb,
5808                                 &chan->sdu_last_frag);
5809                 skb = NULL;
5810
5811                 if (chan->sdu->len != chan->sdu_len)
5812                         break;
5813
5814                 err = chan->ops->recv(chan, chan->sdu);
5815
5816                 if (!err) {
5817                         /* Reassembly complete */
5818                         chan->sdu = NULL;
5819                         chan->sdu_last_frag = NULL;
5820                         chan->sdu_len = 0;
5821                 }
5822                 break;
5823         }
5824
5825         if (err) {
5826                 kfree_skb(skb);
5827                 kfree_skb(chan->sdu);
5828                 chan->sdu = NULL;
5829                 chan->sdu_last_frag = NULL;
5830                 chan->sdu_len = 0;
5831         }
5832
5833         return err;
5834 }
5835
5836 static int l2cap_resegment(struct l2cap_chan *chan)
5837 {
5838         /* Placeholder */
5839         return 0;
5840 }
5841
5842 void l2cap_chan_busy(struct l2cap_chan *chan, int busy)
5843 {
5844         u8 event;
5845
5846         if (chan->mode != L2CAP_MODE_ERTM)
5847                 return;
5848
5849         event = busy ? L2CAP_EV_LOCAL_BUSY_DETECTED : L2CAP_EV_LOCAL_BUSY_CLEAR;
5850         l2cap_tx(chan, NULL, NULL, event);
5851 }
5852
5853 static int l2cap_rx_queued_iframes(struct l2cap_chan *chan)
5854 {
5855         int err = 0;
5856         /* Pass sequential frames to l2cap_reassemble_sdu()
5857          * until a gap is encountered.
5858          */
5859
5860         BT_DBG("chan %p", chan);
5861
5862         while (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
5863                 struct sk_buff *skb;
5864                 BT_DBG("Searching for skb with txseq %d (queue len %d)",
5865                        chan->buffer_seq, skb_queue_len(&chan->srej_q));
5866
5867                 skb = l2cap_ertm_seq_in_queue(&chan->srej_q, chan->buffer_seq);
5868
5869                 if (!skb)
5870                         break;
5871
5872                 skb_unlink(skb, &chan->srej_q);
5873                 chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
5874                 err = l2cap_reassemble_sdu(chan, skb, &bt_cb(skb)->control);
5875                 if (err)
5876                         break;
5877         }
5878
5879         if (skb_queue_empty(&chan->srej_q)) {
5880                 chan->rx_state = L2CAP_RX_STATE_RECV;
5881                 l2cap_send_ack(chan);
5882         }
5883
5884         return err;
5885 }
5886
5887 static void l2cap_handle_srej(struct l2cap_chan *chan,
5888                               struct l2cap_ctrl *control)
5889 {
5890         struct sk_buff *skb;
5891
5892         BT_DBG("chan %p, control %p", chan, control);
5893
5894         if (control->reqseq == chan->next_tx_seq) {
5895                 BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq);
5896                 l2cap_send_disconn_req(chan, ECONNRESET);
5897                 return;
5898         }
5899
5900         skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq);
5901
5902         if (skb == NULL) {
5903                 BT_DBG("Seq %d not available for retransmission",
5904                        control->reqseq);
5905                 return;
5906         }
5907
5908         if (chan->max_tx != 0 && bt_cb(skb)->control.retries >= chan->max_tx) {
5909                 BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
5910                 l2cap_send_disconn_req(chan, ECONNRESET);
5911                 return;
5912         }
5913
5914         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
5915
5916         if (control->poll) {
5917                 l2cap_pass_to_tx(chan, control);
5918
5919                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
5920                 l2cap_retransmit(chan, control);
5921                 l2cap_ertm_send(chan);
5922
5923                 if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) {
5924                         set_bit(CONN_SREJ_ACT, &chan->conn_state);
5925                         chan->srej_save_reqseq = control->reqseq;
5926                 }
5927         } else {
5928                 l2cap_pass_to_tx_fbit(chan, control);
5929
5930                 if (control->final) {
5931                         if (chan->srej_save_reqseq != control->reqseq ||
5932                             !test_and_clear_bit(CONN_SREJ_ACT,
5933                                                 &chan->conn_state))
5934                                 l2cap_retransmit(chan, control);
5935                 } else {
5936                         l2cap_retransmit(chan, control);
5937                         if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) {
5938                                 set_bit(CONN_SREJ_ACT, &chan->conn_state);
5939                                 chan->srej_save_reqseq = control->reqseq;
5940                         }
5941                 }
5942         }
5943 }
5944
5945 static void l2cap_handle_rej(struct l2cap_chan *chan,
5946                              struct l2cap_ctrl *control)
5947 {
5948         struct sk_buff *skb;
5949
5950         BT_DBG("chan %p, control %p", chan, control);
5951
5952         if (control->reqseq == chan->next_tx_seq) {
5953                 BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq);
5954                 l2cap_send_disconn_req(chan, ECONNRESET);
5955                 return;
5956         }
5957
5958         skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq);
5959
5960         if (chan->max_tx && skb &&
5961             bt_cb(skb)->control.retries >= chan->max_tx) {
5962                 BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
5963                 l2cap_send_disconn_req(chan, ECONNRESET);
5964                 return;
5965         }
5966
5967         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
5968
5969         l2cap_pass_to_tx(chan, control);
5970
5971         if (control->final) {
5972                 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
5973                         l2cap_retransmit_all(chan, control);
5974         } else {
5975                 l2cap_retransmit_all(chan, control);
5976                 l2cap_ertm_send(chan);
5977                 if (chan->tx_state == L2CAP_TX_STATE_WAIT_F)
5978                         set_bit(CONN_REJ_ACT, &chan->conn_state);
5979         }
5980 }
5981
5982 static u8 l2cap_classify_txseq(struct l2cap_chan *chan, u16 txseq)
5983 {
5984         BT_DBG("chan %p, txseq %d", chan, txseq);
5985
5986         BT_DBG("last_acked_seq %d, expected_tx_seq %d", chan->last_acked_seq,
5987                chan->expected_tx_seq);
5988
5989         if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
5990                 if (__seq_offset(chan, txseq, chan->last_acked_seq) >=
5991                     chan->tx_win) {
5992                         /* See notes below regarding "double poll" and
5993                          * invalid packets.
5994                          */
5995                         if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) {
5996                                 BT_DBG("Invalid/Ignore - after SREJ");
5997                                 return L2CAP_TXSEQ_INVALID_IGNORE;
5998                         } else {
5999                                 BT_DBG("Invalid - in window after SREJ sent");
6000                                 return L2CAP_TXSEQ_INVALID;
6001                         }
6002                 }
6003
6004                 if (chan->srej_list.head == txseq) {
6005                         BT_DBG("Expected SREJ");
6006                         return L2CAP_TXSEQ_EXPECTED_SREJ;
6007                 }
6008
6009                 if (l2cap_ertm_seq_in_queue(&chan->srej_q, txseq)) {
6010                         BT_DBG("Duplicate SREJ - txseq already stored");
6011                         return L2CAP_TXSEQ_DUPLICATE_SREJ;
6012                 }
6013
6014                 if (l2cap_seq_list_contains(&chan->srej_list, txseq)) {
6015                         BT_DBG("Unexpected SREJ - not requested");
6016                         return L2CAP_TXSEQ_UNEXPECTED_SREJ;
6017                 }
6018         }
6019
6020         if (chan->expected_tx_seq == txseq) {
6021                 if (__seq_offset(chan, txseq, chan->last_acked_seq) >=
6022                     chan->tx_win) {
6023                         BT_DBG("Invalid - txseq outside tx window");
6024                         return L2CAP_TXSEQ_INVALID;
6025                 } else {
6026                         BT_DBG("Expected");
6027                         return L2CAP_TXSEQ_EXPECTED;
6028                 }
6029         }
6030
6031         if (__seq_offset(chan, txseq, chan->last_acked_seq) <
6032             __seq_offset(chan, chan->expected_tx_seq, chan->last_acked_seq)) {
6033                 BT_DBG("Duplicate - expected_tx_seq later than txseq");
6034                 return L2CAP_TXSEQ_DUPLICATE;
6035         }
6036
6037         if (__seq_offset(chan, txseq, chan->last_acked_seq) >= chan->tx_win) {
6038                 /* A source of invalid packets is a "double poll" condition,
6039                  * where delays cause us to send multiple poll packets.  If
6040                  * the remote stack receives and processes both polls,
6041                  * sequence numbers can wrap around in such a way that a
6042                  * resent frame has a sequence number that looks like new data
6043                  * with a sequence gap.  This would trigger an erroneous SREJ
6044                  * request.
6045                  *
6046                  * Fortunately, this is impossible with a tx window that's
6047                  * less than half of the maximum sequence number, which allows
6048                  * invalid frames to be safely ignored.
6049                  *
6050                  * With tx window sizes greater than half of the tx window
6051                  * maximum, the frame is invalid and cannot be ignored.  This
6052                  * causes a disconnect.
6053                  */
6054
6055                 if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) {
6056                         BT_DBG("Invalid/Ignore - txseq outside tx window");
6057                         return L2CAP_TXSEQ_INVALID_IGNORE;
6058                 } else {
6059                         BT_DBG("Invalid - txseq outside tx window");
6060                         return L2CAP_TXSEQ_INVALID;
6061                 }
6062         } else {
6063                 BT_DBG("Unexpected - txseq indicates missing frames");
6064                 return L2CAP_TXSEQ_UNEXPECTED;
6065         }
6066 }
6067
6068 static int l2cap_rx_state_recv(struct l2cap_chan *chan,
6069                                struct l2cap_ctrl *control,
6070                                struct sk_buff *skb, u8 event)
6071 {
6072         int err = 0;
6073         bool skb_in_use = false;
6074
6075         BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
6076                event);
6077
6078         switch (event) {
6079         case L2CAP_EV_RECV_IFRAME:
6080                 switch (l2cap_classify_txseq(chan, control->txseq)) {
6081                 case L2CAP_TXSEQ_EXPECTED:
6082                         l2cap_pass_to_tx(chan, control);
6083
6084                         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
6085                                 BT_DBG("Busy, discarding expected seq %d",
6086                                        control->txseq);
6087                                 break;
6088                         }
6089
6090                         chan->expected_tx_seq = __next_seq(chan,
6091                                                            control->txseq);
6092
6093                         chan->buffer_seq = chan->expected_tx_seq;
6094                         skb_in_use = true;
6095
6096                         err = l2cap_reassemble_sdu(chan, skb, control);
6097                         if (err)
6098                                 break;
6099
6100                         if (control->final) {
6101                                 if (!test_and_clear_bit(CONN_REJ_ACT,
6102                                                         &chan->conn_state)) {
6103                                         control->final = 0;
6104                                         l2cap_retransmit_all(chan, control);
6105                                         l2cap_ertm_send(chan);
6106                                 }
6107                         }
6108
6109                         if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state))
6110                                 l2cap_send_ack(chan);
6111                         break;
6112                 case L2CAP_TXSEQ_UNEXPECTED:
6113                         l2cap_pass_to_tx(chan, control);
6114
6115                         /* Can't issue SREJ frames in the local busy state.
6116                          * Drop this frame, it will be seen as missing
6117                          * when local busy is exited.
6118                          */
6119                         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
6120                                 BT_DBG("Busy, discarding unexpected seq %d",
6121                                        control->txseq);
6122                                 break;
6123                         }
6124
6125                         /* There was a gap in the sequence, so an SREJ
6126                          * must be sent for each missing frame.  The
6127                          * current frame is stored for later use.
6128                          */
6129                         skb_queue_tail(&chan->srej_q, skb);
6130                         skb_in_use = true;
6131                         BT_DBG("Queued %p (queue len %d)", skb,
6132                                skb_queue_len(&chan->srej_q));
6133
6134                         clear_bit(CONN_SREJ_ACT, &chan->conn_state);
6135                         l2cap_seq_list_clear(&chan->srej_list);
6136                         l2cap_send_srej(chan, control->txseq);
6137
6138                         chan->rx_state = L2CAP_RX_STATE_SREJ_SENT;
6139                         break;
6140                 case L2CAP_TXSEQ_DUPLICATE:
6141                         l2cap_pass_to_tx(chan, control);
6142                         break;
6143                 case L2CAP_TXSEQ_INVALID_IGNORE:
6144                         break;
6145                 case L2CAP_TXSEQ_INVALID:
6146                 default:
6147                         l2cap_send_disconn_req(chan, ECONNRESET);
6148                         break;
6149                 }
6150                 break;
6151         case L2CAP_EV_RECV_RR:
6152                 l2cap_pass_to_tx(chan, control);
6153                 if (control->final) {
6154                         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6155
6156                         if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state) &&
6157                             !__chan_is_moving(chan)) {
6158                                 control->final = 0;
6159                                 l2cap_retransmit_all(chan, control);
6160                         }
6161
6162                         l2cap_ertm_send(chan);
6163                 } else if (control->poll) {
6164                         l2cap_send_i_or_rr_or_rnr(chan);
6165                 } else {
6166                         if (test_and_clear_bit(CONN_REMOTE_BUSY,
6167                                                &chan->conn_state) &&
6168                             chan->unacked_frames)
6169                                 __set_retrans_timer(chan);
6170
6171                         l2cap_ertm_send(chan);
6172                 }
6173                 break;
6174         case L2CAP_EV_RECV_RNR:
6175                 set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6176                 l2cap_pass_to_tx(chan, control);
6177                 if (control && control->poll) {
6178                         set_bit(CONN_SEND_FBIT, &chan->conn_state);
6179                         l2cap_send_rr_or_rnr(chan, 0);
6180                 }
6181                 __clear_retrans_timer(chan);
6182                 l2cap_seq_list_clear(&chan->retrans_list);
6183                 break;
6184         case L2CAP_EV_RECV_REJ:
6185                 l2cap_handle_rej(chan, control);
6186                 break;
6187         case L2CAP_EV_RECV_SREJ:
6188                 l2cap_handle_srej(chan, control);
6189                 break;
6190         default:
6191                 break;
6192         }
6193
6194         if (skb && !skb_in_use) {
6195                 BT_DBG("Freeing %p", skb);
6196                 kfree_skb(skb);
6197         }
6198
6199         return err;
6200 }
6201
6202 static int l2cap_rx_state_srej_sent(struct l2cap_chan *chan,
6203                                     struct l2cap_ctrl *control,
6204                                     struct sk_buff *skb, u8 event)
6205 {
6206         int err = 0;
6207         u16 txseq = control->txseq;
6208         bool skb_in_use = false;
6209
6210         BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
6211                event);
6212
6213         switch (event) {
6214         case L2CAP_EV_RECV_IFRAME:
6215                 switch (l2cap_classify_txseq(chan, txseq)) {
6216                 case L2CAP_TXSEQ_EXPECTED:
6217                         /* Keep frame for reassembly later */
6218                         l2cap_pass_to_tx(chan, control);
6219                         skb_queue_tail(&chan->srej_q, skb);
6220                         skb_in_use = true;
6221                         BT_DBG("Queued %p (queue len %d)", skb,
6222                                skb_queue_len(&chan->srej_q));
6223
6224                         chan->expected_tx_seq = __next_seq(chan, txseq);
6225                         break;
6226                 case L2CAP_TXSEQ_EXPECTED_SREJ:
6227                         l2cap_seq_list_pop(&chan->srej_list);
6228
6229                         l2cap_pass_to_tx(chan, control);
6230                         skb_queue_tail(&chan->srej_q, skb);
6231                         skb_in_use = true;
6232                         BT_DBG("Queued %p (queue len %d)", skb,
6233                                skb_queue_len(&chan->srej_q));
6234
6235                         err = l2cap_rx_queued_iframes(chan);
6236                         if (err)
6237                                 break;
6238
6239                         break;
6240                 case L2CAP_TXSEQ_UNEXPECTED:
6241                         /* Got a frame that can't be reassembled yet.
6242                          * Save it for later, and send SREJs to cover
6243                          * the missing frames.
6244                          */
6245                         skb_queue_tail(&chan->srej_q, skb);
6246                         skb_in_use = true;
6247                         BT_DBG("Queued %p (queue len %d)", skb,
6248                                skb_queue_len(&chan->srej_q));
6249
6250                         l2cap_pass_to_tx(chan, control);
6251                         l2cap_send_srej(chan, control->txseq);
6252                         break;
6253                 case L2CAP_TXSEQ_UNEXPECTED_SREJ:
6254                         /* This frame was requested with an SREJ, but
6255                          * some expected retransmitted frames are
6256                          * missing.  Request retransmission of missing
6257                          * SREJ'd frames.
6258                          */
6259                         skb_queue_tail(&chan->srej_q, skb);
6260                         skb_in_use = true;
6261                         BT_DBG("Queued %p (queue len %d)", skb,
6262                                skb_queue_len(&chan->srej_q));
6263
6264                         l2cap_pass_to_tx(chan, control);
6265                         l2cap_send_srej_list(chan, control->txseq);
6266                         break;
6267                 case L2CAP_TXSEQ_DUPLICATE_SREJ:
6268                         /* We've already queued this frame.  Drop this copy. */
6269                         l2cap_pass_to_tx(chan, control);
6270                         break;
6271                 case L2CAP_TXSEQ_DUPLICATE:
6272                         /* Expecting a later sequence number, so this frame
6273                          * was already received.  Ignore it completely.
6274                          */
6275                         break;
6276                 case L2CAP_TXSEQ_INVALID_IGNORE:
6277                         break;
6278                 case L2CAP_TXSEQ_INVALID:
6279                 default:
6280                         l2cap_send_disconn_req(chan, ECONNRESET);
6281                         break;
6282                 }
6283                 break;
6284         case L2CAP_EV_RECV_RR:
6285                 l2cap_pass_to_tx(chan, control);
6286                 if (control->final) {
6287                         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6288
6289                         if (!test_and_clear_bit(CONN_REJ_ACT,
6290                                                 &chan->conn_state)) {
6291                                 control->final = 0;
6292                                 l2cap_retransmit_all(chan, control);
6293                         }
6294
6295                         l2cap_ertm_send(chan);
6296                 } else if (control->poll) {
6297                         if (test_and_clear_bit(CONN_REMOTE_BUSY,
6298                                                &chan->conn_state) &&
6299                             chan->unacked_frames) {
6300                                 __set_retrans_timer(chan);
6301                         }
6302
6303                         set_bit(CONN_SEND_FBIT, &chan->conn_state);
6304                         l2cap_send_srej_tail(chan);
6305                 } else {
6306                         if (test_and_clear_bit(CONN_REMOTE_BUSY,
6307                                                &chan->conn_state) &&
6308                             chan->unacked_frames)
6309                                 __set_retrans_timer(chan);
6310
6311                         l2cap_send_ack(chan);
6312                 }
6313                 break;
6314         case L2CAP_EV_RECV_RNR:
6315                 set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6316                 l2cap_pass_to_tx(chan, control);
6317                 if (control->poll) {
6318                         l2cap_send_srej_tail(chan);
6319                 } else {
6320                         struct l2cap_ctrl rr_control;
6321                         memset(&rr_control, 0, sizeof(rr_control));
6322                         rr_control.sframe = 1;
6323                         rr_control.super = L2CAP_SUPER_RR;
6324                         rr_control.reqseq = chan->buffer_seq;
6325                         l2cap_send_sframe(chan, &rr_control);
6326                 }
6327
6328                 break;
6329         case L2CAP_EV_RECV_REJ:
6330                 l2cap_handle_rej(chan, control);
6331                 break;
6332         case L2CAP_EV_RECV_SREJ:
6333                 l2cap_handle_srej(chan, control);
6334                 break;
6335         }
6336
6337         if (skb && !skb_in_use) {
6338                 BT_DBG("Freeing %p", skb);
6339                 kfree_skb(skb);
6340         }
6341
6342         return err;
6343 }
6344
6345 static int l2cap_finish_move(struct l2cap_chan *chan)
6346 {
6347         BT_DBG("chan %p", chan);
6348
6349         chan->rx_state = L2CAP_RX_STATE_RECV;
6350
6351         if (chan->hs_hcon)
6352                 chan->conn->mtu = chan->hs_hcon->hdev->block_mtu;
6353         else
6354                 chan->conn->mtu = chan->conn->hcon->hdev->acl_mtu;
6355
6356         return l2cap_resegment(chan);
6357 }
6358
6359 static int l2cap_rx_state_wait_p(struct l2cap_chan *chan,
6360                                  struct l2cap_ctrl *control,
6361                                  struct sk_buff *skb, u8 event)
6362 {
6363         int err;
6364
6365         BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
6366                event);
6367
6368         if (!control->poll)
6369                 return -EPROTO;
6370
6371         l2cap_process_reqseq(chan, control->reqseq);
6372
6373         if (!skb_queue_empty(&chan->tx_q))
6374                 chan->tx_send_head = skb_peek(&chan->tx_q);
6375         else
6376                 chan->tx_send_head = NULL;
6377
6378         /* Rewind next_tx_seq to the point expected
6379          * by the receiver.
6380          */
6381         chan->next_tx_seq = control->reqseq;
6382         chan->unacked_frames = 0;
6383
6384         err = l2cap_finish_move(chan);
6385         if (err)
6386                 return err;
6387
6388         set_bit(CONN_SEND_FBIT, &chan->conn_state);
6389         l2cap_send_i_or_rr_or_rnr(chan);
6390
6391         if (event == L2CAP_EV_RECV_IFRAME)
6392                 return -EPROTO;
6393
6394         return l2cap_rx_state_recv(chan, control, NULL, event);
6395 }
6396
6397 static int l2cap_rx_state_wait_f(struct l2cap_chan *chan,
6398                                  struct l2cap_ctrl *control,
6399                                  struct sk_buff *skb, u8 event)
6400 {
6401         int err;
6402
6403         if (!control->final)
6404                 return -EPROTO;
6405
6406         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6407
6408         chan->rx_state = L2CAP_RX_STATE_RECV;
6409         l2cap_process_reqseq(chan, control->reqseq);
6410
6411         if (!skb_queue_empty(&chan->tx_q))
6412                 chan->tx_send_head = skb_peek(&chan->tx_q);
6413         else
6414                 chan->tx_send_head = NULL;
6415
6416         /* Rewind next_tx_seq to the point expected
6417          * by the receiver.
6418          */
6419         chan->next_tx_seq = control->reqseq;
6420         chan->unacked_frames = 0;
6421
6422         if (chan->hs_hcon)
6423                 chan->conn->mtu = chan->hs_hcon->hdev->block_mtu;
6424         else
6425                 chan->conn->mtu = chan->conn->hcon->hdev->acl_mtu;
6426
6427         err = l2cap_resegment(chan);
6428
6429         if (!err)
6430                 err = l2cap_rx_state_recv(chan, control, skb, event);
6431
6432         return err;
6433 }
6434
6435 static bool __valid_reqseq(struct l2cap_chan *chan, u16 reqseq)
6436 {
6437         /* Make sure reqseq is for a packet that has been sent but not acked */
6438         u16 unacked;
6439
6440         unacked = __seq_offset(chan, chan->next_tx_seq, chan->expected_ack_seq);
6441         return __seq_offset(chan, chan->next_tx_seq, reqseq) <= unacked;
6442 }
6443
6444 static int l2cap_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
6445                     struct sk_buff *skb, u8 event)
6446 {
6447         int err = 0;
6448
6449         BT_DBG("chan %p, control %p, skb %p, event %d, state %d", chan,
6450                control, skb, event, chan->rx_state);
6451
6452         if (__valid_reqseq(chan, control->reqseq)) {
6453                 switch (chan->rx_state) {
6454                 case L2CAP_RX_STATE_RECV:
6455                         err = l2cap_rx_state_recv(chan, control, skb, event);
6456                         break;
6457                 case L2CAP_RX_STATE_SREJ_SENT:
6458                         err = l2cap_rx_state_srej_sent(chan, control, skb,
6459                                                        event);
6460                         break;
6461                 case L2CAP_RX_STATE_WAIT_P:
6462                         err = l2cap_rx_state_wait_p(chan, control, skb, event);
6463                         break;
6464                 case L2CAP_RX_STATE_WAIT_F:
6465                         err = l2cap_rx_state_wait_f(chan, control, skb, event);
6466                         break;
6467                 default:
6468                         /* shut it down */
6469                         break;
6470                 }
6471         } else {
6472                 BT_DBG("Invalid reqseq %d (next_tx_seq %d, expected_ack_seq %d",
6473                        control->reqseq, chan->next_tx_seq,
6474                        chan->expected_ack_seq);
6475                 l2cap_send_disconn_req(chan, ECONNRESET);
6476         }
6477
6478         return err;
6479 }
6480
6481 static int l2cap_stream_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
6482                            struct sk_buff *skb)
6483 {
6484         int err = 0;
6485
6486         BT_DBG("chan %p, control %p, skb %p, state %d", chan, control, skb,
6487                chan->rx_state);
6488
6489         if (l2cap_classify_txseq(chan, control->txseq) ==
6490             L2CAP_TXSEQ_EXPECTED) {
6491                 l2cap_pass_to_tx(chan, control);
6492
6493                 BT_DBG("buffer_seq %d->%d", chan->buffer_seq,
6494                        __next_seq(chan, chan->buffer_seq));
6495
6496                 chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
6497
6498                 l2cap_reassemble_sdu(chan, skb, control);
6499         } else {
6500                 if (chan->sdu) {
6501                         kfree_skb(chan->sdu);
6502                         chan->sdu = NULL;
6503                 }
6504                 chan->sdu_last_frag = NULL;
6505                 chan->sdu_len = 0;
6506
6507                 if (skb) {
6508                         BT_DBG("Freeing %p", skb);
6509                         kfree_skb(skb);
6510                 }
6511         }
6512
6513         chan->last_acked_seq = control->txseq;
6514         chan->expected_tx_seq = __next_seq(chan, control->txseq);
6515
6516         return err;
6517 }
6518
6519 static int l2cap_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb)
6520 {
6521         struct l2cap_ctrl *control = &bt_cb(skb)->control;
6522         u16 len;
6523         u8 event;
6524
6525         __unpack_control(chan, skb);
6526
6527         len = skb->len;
6528
6529         /*
6530          * We can just drop the corrupted I-frame here.
6531          * Receiver will miss it and start proper recovery
6532          * procedures and ask for retransmission.
6533          */
6534         if (l2cap_check_fcs(chan, skb))
6535                 goto drop;
6536
6537         if (!control->sframe && control->sar == L2CAP_SAR_START)
6538                 len -= L2CAP_SDULEN_SIZE;
6539
6540         if (chan->fcs == L2CAP_FCS_CRC16)
6541                 len -= L2CAP_FCS_SIZE;
6542
6543         if (len > chan->mps) {
6544                 l2cap_send_disconn_req(chan, ECONNRESET);
6545                 goto drop;
6546         }
6547
6548         if (!control->sframe) {
6549                 int err;
6550
6551                 BT_DBG("iframe sar %d, reqseq %d, final %d, txseq %d",
6552                        control->sar, control->reqseq, control->final,
6553                        control->txseq);
6554
6555                 /* Validate F-bit - F=0 always valid, F=1 only
6556                  * valid in TX WAIT_F
6557                  */
6558                 if (control->final && chan->tx_state != L2CAP_TX_STATE_WAIT_F)
6559                         goto drop;
6560
6561                 if (chan->mode != L2CAP_MODE_STREAMING) {
6562                         event = L2CAP_EV_RECV_IFRAME;
6563                         err = l2cap_rx(chan, control, skb, event);
6564                 } else {
6565                         err = l2cap_stream_rx(chan, control, skb);
6566                 }
6567
6568                 if (err)
6569                         l2cap_send_disconn_req(chan, ECONNRESET);
6570         } else {
6571                 const u8 rx_func_to_event[4] = {
6572                         L2CAP_EV_RECV_RR, L2CAP_EV_RECV_REJ,
6573                         L2CAP_EV_RECV_RNR, L2CAP_EV_RECV_SREJ
6574                 };
6575
6576                 /* Only I-frames are expected in streaming mode */
6577                 if (chan->mode == L2CAP_MODE_STREAMING)
6578                         goto drop;
6579
6580                 BT_DBG("sframe reqseq %d, final %d, poll %d, super %d",
6581                        control->reqseq, control->final, control->poll,
6582                        control->super);
6583
6584                 if (len != 0) {
6585                         BT_ERR("Trailing bytes: %d in sframe", len);
6586                         l2cap_send_disconn_req(chan, ECONNRESET);
6587                         goto drop;
6588                 }
6589
6590                 /* Validate F and P bits */
6591                 if (control->final && (control->poll ||
6592                                        chan->tx_state != L2CAP_TX_STATE_WAIT_F))
6593                         goto drop;
6594
6595                 event = rx_func_to_event[control->super];
6596                 if (l2cap_rx(chan, control, skb, event))
6597                         l2cap_send_disconn_req(chan, ECONNRESET);
6598         }
6599
6600         return 0;
6601
6602 drop:
6603         kfree_skb(skb);
6604         return 0;
6605 }
6606
6607 static void l2cap_chan_le_send_credits(struct l2cap_chan *chan)
6608 {
6609         struct l2cap_conn *conn = chan->conn;
6610         struct l2cap_le_credits pkt;
6611         u16 return_credits;
6612
6613         /* We return more credits to the sender only after the amount of
6614          * credits falls below half of the initial amount.
6615          */
6616         if (chan->rx_credits >= (le_max_credits + 1) / 2)
6617                 return;
6618
6619         return_credits = le_max_credits - chan->rx_credits;
6620
6621         BT_DBG("chan %p returning %u credits to sender", chan, return_credits);
6622
6623         chan->rx_credits += return_credits;
6624
6625         pkt.cid     = cpu_to_le16(chan->scid);
6626         pkt.credits = cpu_to_le16(return_credits);
6627
6628         chan->ident = l2cap_get_ident(conn);
6629
6630         l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CREDITS, sizeof(pkt), &pkt);
6631 }
6632
6633 static int l2cap_le_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb)
6634 {
6635         int err;
6636
6637         if (!chan->rx_credits) {
6638                 BT_ERR("No credits to receive LE L2CAP data");
6639                 l2cap_send_disconn_req(chan, ECONNRESET);
6640                 return -ENOBUFS;
6641         }
6642
6643         if (chan->imtu < skb->len) {
6644                 BT_ERR("Too big LE L2CAP PDU");
6645                 return -ENOBUFS;
6646         }
6647
6648         chan->rx_credits--;
6649         BT_DBG("rx_credits %u -> %u", chan->rx_credits + 1, chan->rx_credits);
6650
6651         l2cap_chan_le_send_credits(chan);
6652
6653         err = 0;
6654
6655         if (!chan->sdu) {
6656                 u16 sdu_len;
6657
6658                 sdu_len = get_unaligned_le16(skb->data);
6659                 skb_pull(skb, L2CAP_SDULEN_SIZE);
6660
6661                 BT_DBG("Start of new SDU. sdu_len %u skb->len %u imtu %u",
6662                        sdu_len, skb->len, chan->imtu);
6663
6664                 if (sdu_len > chan->imtu) {
6665                         BT_ERR("Too big LE L2CAP SDU length received");
6666                         err = -EMSGSIZE;
6667                         goto failed;
6668                 }
6669
6670                 if (skb->len > sdu_len) {
6671                         BT_ERR("Too much LE L2CAP data received");
6672                         err = -EINVAL;
6673                         goto failed;
6674                 }
6675
6676                 if (skb->len == sdu_len)
6677                         return chan->ops->recv(chan, skb);
6678
6679                 chan->sdu = skb;
6680                 chan->sdu_len = sdu_len;
6681                 chan->sdu_last_frag = skb;
6682
6683                 return 0;
6684         }
6685
6686         BT_DBG("SDU fragment. chan->sdu->len %u skb->len %u chan->sdu_len %u",
6687                chan->sdu->len, skb->len, chan->sdu_len);
6688
6689         if (chan->sdu->len + skb->len > chan->sdu_len) {
6690                 BT_ERR("Too much LE L2CAP data received");
6691                 err = -EINVAL;
6692                 goto failed;
6693         }
6694
6695         append_skb_frag(chan->sdu, skb, &chan->sdu_last_frag);
6696         skb = NULL;
6697
6698         if (chan->sdu->len == chan->sdu_len) {
6699                 err = chan->ops->recv(chan, chan->sdu);
6700                 if (!err) {
6701                         chan->sdu = NULL;
6702                         chan->sdu_last_frag = NULL;
6703                         chan->sdu_len = 0;
6704                 }
6705         }
6706
6707 failed:
6708         if (err) {
6709                 kfree_skb(skb);
6710                 kfree_skb(chan->sdu);
6711                 chan->sdu = NULL;
6712                 chan->sdu_last_frag = NULL;
6713                 chan->sdu_len = 0;
6714         }
6715
6716         /* We can't return an error here since we took care of the skb
6717          * freeing internally. An error return would cause the caller to
6718          * do a double-free of the skb.
6719          */
6720         return 0;
6721 }
6722
6723 static void l2cap_data_channel(struct l2cap_conn *conn, u16 cid,
6724                                struct sk_buff *skb)
6725 {
6726         struct l2cap_chan *chan;
6727
6728         chan = l2cap_get_chan_by_scid(conn, cid);
6729         if (!chan) {
6730                 if (cid == L2CAP_CID_A2MP) {
6731                         chan = a2mp_channel_create(conn, skb);
6732                         if (!chan) {
6733                                 kfree_skb(skb);
6734                                 return;
6735                         }
6736
6737                         l2cap_chan_lock(chan);
6738                 } else {
6739                         BT_DBG("unknown cid 0x%4.4x", cid);
6740                         /* Drop packet and return */
6741                         kfree_skb(skb);
6742                         return;
6743                 }
6744         }
6745
6746         BT_DBG("chan %p, len %d", chan, skb->len);
6747
6748         if (chan->state != BT_CONNECTED)
6749                 goto drop;
6750
6751         switch (chan->mode) {
6752         case L2CAP_MODE_LE_FLOWCTL:
6753                 if (l2cap_le_data_rcv(chan, skb) < 0)
6754                         goto drop;
6755
6756                 goto done;
6757
6758         case L2CAP_MODE_BASIC:
6759                 /* If socket recv buffers overflows we drop data here
6760                  * which is *bad* because L2CAP has to be reliable.
6761                  * But we don't have any other choice. L2CAP doesn't
6762                  * provide flow control mechanism. */
6763
6764                 if (chan->imtu < skb->len) {
6765                         BT_ERR("Dropping L2CAP data: receive buffer overflow");
6766                         goto drop;
6767                 }
6768
6769                 if (!chan->ops->recv(chan, skb))
6770                         goto done;
6771                 break;
6772
6773         case L2CAP_MODE_ERTM:
6774         case L2CAP_MODE_STREAMING:
6775                 l2cap_data_rcv(chan, skb);
6776                 goto done;
6777
6778         default:
6779                 BT_DBG("chan %p: bad mode 0x%2.2x", chan, chan->mode);
6780                 break;
6781         }
6782
6783 drop:
6784         kfree_skb(skb);
6785
6786 done:
6787         l2cap_chan_unlock(chan);
6788 }
6789
6790 static void l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm,
6791                                   struct sk_buff *skb)
6792 {
6793         struct hci_conn *hcon = conn->hcon;
6794         struct l2cap_chan *chan;
6795
6796         if (hcon->type != ACL_LINK)
6797                 goto free_skb;
6798
6799         chan = l2cap_global_chan_by_psm(0, psm, &hcon->src, &hcon->dst,
6800                                         ACL_LINK);
6801         if (!chan)
6802                 goto free_skb;
6803
6804         BT_DBG("chan %p, len %d", chan, skb->len);
6805
6806         if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
6807                 goto drop;
6808
6809         if (chan->imtu < skb->len)
6810                 goto drop;
6811
6812         /* Store remote BD_ADDR and PSM for msg_name */
6813         bacpy(&bt_cb(skb)->bdaddr, &hcon->dst);
6814         bt_cb(skb)->psm = psm;
6815
6816         if (!chan->ops->recv(chan, skb)) {
6817                 l2cap_chan_put(chan);
6818                 return;
6819         }
6820
6821 drop:
6822         l2cap_chan_put(chan);
6823 free_skb:
6824         kfree_skb(skb);
6825 }
6826
6827 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
6828 {
6829         struct l2cap_hdr *lh = (void *) skb->data;
6830         struct hci_conn *hcon = conn->hcon;
6831         u16 cid, len;
6832         __le16 psm;
6833
6834         if (hcon->state != BT_CONNECTED) {
6835                 BT_DBG("queueing pending rx skb");
6836                 skb_queue_tail(&conn->pending_rx, skb);
6837                 return;
6838         }
6839
6840         skb_pull(skb, L2CAP_HDR_SIZE);
6841         cid = __le16_to_cpu(lh->cid);
6842         len = __le16_to_cpu(lh->len);
6843
6844         if (len != skb->len) {
6845                 kfree_skb(skb);
6846                 return;
6847         }
6848
6849         /* Since we can't actively block incoming LE connections we must
6850          * at least ensure that we ignore incoming data from them.
6851          */
6852         if (hcon->type == LE_LINK &&
6853             hci_bdaddr_list_lookup(&hcon->hdev->blacklist, &hcon->dst,
6854                                    bdaddr_type(hcon, hcon->dst_type))) {
6855                 kfree_skb(skb);
6856                 return;
6857         }
6858
6859         BT_DBG("len %d, cid 0x%4.4x", len, cid);
6860
6861         switch (cid) {
6862         case L2CAP_CID_SIGNALING:
6863                 l2cap_sig_channel(conn, skb);
6864                 break;
6865
6866         case L2CAP_CID_CONN_LESS:
6867                 psm = get_unaligned((__le16 *) skb->data);
6868                 skb_pull(skb, L2CAP_PSMLEN_SIZE);
6869                 l2cap_conless_channel(conn, psm, skb);
6870                 break;
6871
6872         case L2CAP_CID_LE_SIGNALING:
6873                 l2cap_le_sig_channel(conn, skb);
6874                 break;
6875
6876         default:
6877                 l2cap_data_channel(conn, cid, skb);
6878                 break;
6879         }
6880 }
6881
6882 static void process_pending_rx(struct work_struct *work)
6883 {
6884         struct l2cap_conn *conn = container_of(work, struct l2cap_conn,
6885                                                pending_rx_work);
6886         struct sk_buff *skb;
6887
6888         BT_DBG("");
6889
6890         while ((skb = skb_dequeue(&conn->pending_rx)))
6891                 l2cap_recv_frame(conn, skb);
6892 }
6893
6894 static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon)
6895 {
6896         struct l2cap_conn *conn = hcon->l2cap_data;
6897         struct hci_chan *hchan;
6898
6899         if (conn)
6900                 return conn;
6901
6902         hchan = hci_chan_create(hcon);
6903         if (!hchan)
6904                 return NULL;
6905
6906         conn = kzalloc(sizeof(*conn), GFP_KERNEL);
6907         if (!conn) {
6908                 hci_chan_del(hchan);
6909                 return NULL;
6910         }
6911
6912         kref_init(&conn->ref);
6913         hcon->l2cap_data = conn;
6914         conn->hcon = hci_conn_get(hcon);
6915         conn->hchan = hchan;
6916
6917         BT_DBG("hcon %p conn %p hchan %p", hcon, conn, hchan);
6918
6919         switch (hcon->type) {
6920         case LE_LINK:
6921                 if (hcon->hdev->le_mtu) {
6922                         conn->mtu = hcon->hdev->le_mtu;
6923                         break;
6924                 }
6925                 /* fall through */
6926         default:
6927                 conn->mtu = hcon->hdev->acl_mtu;
6928                 break;
6929         }
6930
6931         conn->feat_mask = 0;
6932
6933         if (hcon->type == ACL_LINK)
6934                 conn->hs_enabled = test_bit(HCI_HS_ENABLED,
6935                                             &hcon->hdev->dev_flags);
6936
6937         mutex_init(&conn->ident_lock);
6938         mutex_init(&conn->chan_lock);
6939
6940         INIT_LIST_HEAD(&conn->chan_l);
6941         INIT_LIST_HEAD(&conn->users);
6942
6943         INIT_DELAYED_WORK(&conn->info_timer, l2cap_info_timeout);
6944
6945         skb_queue_head_init(&conn->pending_rx);
6946         INIT_WORK(&conn->pending_rx_work, process_pending_rx);
6947         INIT_WORK(&conn->id_addr_update_work, l2cap_conn_update_id_addr);
6948
6949         conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM;
6950
6951         return conn;
6952 }
6953
6954 static bool is_valid_psm(u16 psm, u8 dst_type) {
6955         if (!psm)
6956                 return false;
6957
6958         if (bdaddr_type_is_le(dst_type))
6959                 return (psm <= 0x00ff);
6960
6961         /* PSM must be odd and lsb of upper byte must be 0 */
6962         return ((psm & 0x0101) == 0x0001);
6963 }
6964
6965 int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid,
6966                        bdaddr_t *dst, u8 dst_type)
6967 {
6968         struct l2cap_conn *conn;
6969         struct hci_conn *hcon;
6970         struct hci_dev *hdev;
6971         int err;
6972
6973         BT_DBG("%pMR -> %pMR (type %u) psm 0x%2.2x", &chan->src, dst,
6974                dst_type, __le16_to_cpu(psm));
6975
6976         hdev = hci_get_route(dst, &chan->src);
6977         if (!hdev)
6978                 return -EHOSTUNREACH;
6979
6980         hci_dev_lock(hdev);
6981
6982         if (!is_valid_psm(__le16_to_cpu(psm), dst_type) && !cid &&
6983             chan->chan_type != L2CAP_CHAN_RAW) {
6984                 err = -EINVAL;
6985                 goto done;
6986         }
6987
6988         if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED && !psm) {
6989                 err = -EINVAL;
6990                 goto done;
6991         }
6992
6993         if (chan->chan_type == L2CAP_CHAN_FIXED && !cid) {
6994                 err = -EINVAL;
6995                 goto done;
6996         }
6997
6998         switch (chan->mode) {
6999         case L2CAP_MODE_BASIC:
7000                 break;
7001         case L2CAP_MODE_LE_FLOWCTL:
7002                 l2cap_le_flowctl_init(chan);
7003                 break;
7004         case L2CAP_MODE_ERTM:
7005         case L2CAP_MODE_STREAMING:
7006                 if (!disable_ertm)
7007                         break;
7008                 /* fall through */
7009         default:
7010                 err = -EOPNOTSUPP;
7011                 goto done;
7012         }
7013
7014         switch (chan->state) {
7015         case BT_CONNECT:
7016         case BT_CONNECT2:
7017         case BT_CONFIG:
7018                 /* Already connecting */
7019                 err = 0;
7020                 goto done;
7021
7022         case BT_CONNECTED:
7023                 /* Already connected */
7024                 err = -EISCONN;
7025                 goto done;
7026
7027         case BT_OPEN:
7028         case BT_BOUND:
7029                 /* Can connect */
7030                 break;
7031
7032         default:
7033                 err = -EBADFD;
7034                 goto done;
7035         }
7036
7037         /* Set destination address and psm */
7038         bacpy(&chan->dst, dst);
7039         chan->dst_type = dst_type;
7040
7041         chan->psm = psm;
7042         chan->dcid = cid;
7043
7044         if (bdaddr_type_is_le(dst_type)) {
7045                 u8 role;
7046
7047                 /* Convert from L2CAP channel address type to HCI address type
7048                  */
7049                 if (dst_type == BDADDR_LE_PUBLIC)
7050                         dst_type = ADDR_LE_DEV_PUBLIC;
7051                 else
7052                         dst_type = ADDR_LE_DEV_RANDOM;
7053
7054                 if (test_bit(HCI_ADVERTISING, &hdev->dev_flags))
7055                         role = HCI_ROLE_SLAVE;
7056                 else
7057                         role = HCI_ROLE_MASTER;
7058
7059                 hcon = hci_connect_le(hdev, dst, dst_type, chan->sec_level,
7060                                       HCI_LE_CONN_TIMEOUT, role);
7061         } else {
7062                 u8 auth_type = l2cap_get_auth_type(chan);
7063                 hcon = hci_connect_acl(hdev, dst, chan->sec_level, auth_type);
7064         }
7065
7066         if (IS_ERR(hcon)) {
7067                 err = PTR_ERR(hcon);
7068                 goto done;
7069         }
7070
7071         conn = l2cap_conn_add(hcon);
7072         if (!conn) {
7073                 hci_conn_drop(hcon);
7074                 err = -ENOMEM;
7075                 goto done;
7076         }
7077
7078         mutex_lock(&conn->chan_lock);
7079         l2cap_chan_lock(chan);
7080
7081         if (cid && __l2cap_get_chan_by_dcid(conn, cid)) {
7082                 hci_conn_drop(hcon);
7083                 err = -EBUSY;
7084                 goto chan_unlock;
7085         }
7086
7087         /* Update source addr of the socket */
7088         bacpy(&chan->src, &hcon->src);
7089         chan->src_type = bdaddr_type(hcon, hcon->src_type);
7090
7091         __l2cap_chan_add(conn, chan);
7092
7093         /* l2cap_chan_add takes its own ref so we can drop this one */
7094         hci_conn_drop(hcon);
7095
7096         l2cap_state_change(chan, BT_CONNECT);
7097         __set_chan_timer(chan, chan->ops->get_sndtimeo(chan));
7098
7099         /* Release chan->sport so that it can be reused by other
7100          * sockets (as it's only used for listening sockets).
7101          */
7102         write_lock(&chan_list_lock);
7103         chan->sport = 0;
7104         write_unlock(&chan_list_lock);
7105
7106         if (hcon->state == BT_CONNECTED) {
7107                 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
7108                         __clear_chan_timer(chan);
7109                         if (l2cap_chan_check_security(chan, true))
7110                                 l2cap_state_change(chan, BT_CONNECTED);
7111                 } else
7112                         l2cap_do_start(chan);
7113         }
7114
7115         err = 0;
7116
7117 chan_unlock:
7118         l2cap_chan_unlock(chan);
7119         mutex_unlock(&conn->chan_lock);
7120 done:
7121         hci_dev_unlock(hdev);
7122         hci_dev_put(hdev);
7123         return err;
7124 }
7125 EXPORT_SYMBOL_GPL(l2cap_chan_connect);
7126
7127 /* ---- L2CAP interface with lower layer (HCI) ---- */
7128
7129 int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr)
7130 {
7131         int exact = 0, lm1 = 0, lm2 = 0;
7132         struct l2cap_chan *c;
7133
7134         BT_DBG("hdev %s, bdaddr %pMR", hdev->name, bdaddr);
7135
7136         /* Find listening sockets and check their link_mode */
7137         read_lock(&chan_list_lock);
7138         list_for_each_entry(c, &chan_list, global_l) {
7139                 if (c->state != BT_LISTEN)
7140                         continue;
7141
7142                 if (!bacmp(&c->src, &hdev->bdaddr)) {
7143                         lm1 |= HCI_LM_ACCEPT;
7144                         if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
7145                                 lm1 |= HCI_LM_MASTER;
7146                         exact++;
7147                 } else if (!bacmp(&c->src, BDADDR_ANY)) {
7148                         lm2 |= HCI_LM_ACCEPT;
7149                         if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
7150                                 lm2 |= HCI_LM_MASTER;
7151                 }
7152         }
7153         read_unlock(&chan_list_lock);
7154
7155         return exact ? lm1 : lm2;
7156 }
7157
7158 /* Find the next fixed channel in BT_LISTEN state, continue iteration
7159  * from an existing channel in the list or from the beginning of the
7160  * global list (by passing NULL as first parameter).
7161  */
7162 static struct l2cap_chan *l2cap_global_fixed_chan(struct l2cap_chan *c,
7163                                                   bdaddr_t *src, u8 link_type)
7164 {
7165         read_lock(&chan_list_lock);
7166
7167         if (c)
7168                 c = list_next_entry(c, global_l);
7169         else
7170                 c = list_entry(chan_list.next, typeof(*c), global_l);
7171
7172         list_for_each_entry_from(c, &chan_list, global_l) {
7173                 if (c->chan_type != L2CAP_CHAN_FIXED)
7174                         continue;
7175                 if (c->state != BT_LISTEN)
7176                         continue;
7177                 if (bacmp(&c->src, src) && bacmp(&c->src, BDADDR_ANY))
7178                         continue;
7179                 if (link_type == ACL_LINK && c->src_type != BDADDR_BREDR)
7180                         continue;
7181                 if (link_type == LE_LINK && c->src_type == BDADDR_BREDR)
7182                         continue;
7183
7184                 l2cap_chan_hold(c);
7185                 read_unlock(&chan_list_lock);
7186                 return c;
7187         }
7188
7189         read_unlock(&chan_list_lock);
7190
7191         return NULL;
7192 }
7193
7194 void l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
7195 {
7196         struct hci_dev *hdev = hcon->hdev;
7197         struct l2cap_conn *conn;
7198         struct l2cap_chan *pchan;
7199         u8 dst_type;
7200
7201         BT_DBG("hcon %p bdaddr %pMR status %d", hcon, &hcon->dst, status);
7202
7203         if (status) {
7204                 l2cap_conn_del(hcon, bt_to_errno(status));
7205                 return;
7206         }
7207
7208         conn = l2cap_conn_add(hcon);
7209         if (!conn)
7210                 return;
7211
7212         dst_type = bdaddr_type(hcon, hcon->dst_type);
7213
7214         /* If device is blocked, do not create channels for it */
7215         if (hci_bdaddr_list_lookup(&hdev->blacklist, &hcon->dst, dst_type))
7216                 return;
7217
7218         /* Find fixed channels and notify them of the new connection. We
7219          * use multiple individual lookups, continuing each time where
7220          * we left off, because the list lock would prevent calling the
7221          * potentially sleeping l2cap_chan_lock() function.
7222          */
7223         pchan = l2cap_global_fixed_chan(NULL, &hdev->bdaddr, hcon->type);
7224         while (pchan) {
7225                 struct l2cap_chan *chan, *next;
7226
7227                 /* Client fixed channels should override server ones */
7228                 if (__l2cap_get_chan_by_dcid(conn, pchan->scid))
7229                         goto next;
7230
7231                 l2cap_chan_lock(pchan);
7232                 chan = pchan->ops->new_connection(pchan);
7233                 if (chan) {
7234                         bacpy(&chan->src, &hcon->src);
7235                         bacpy(&chan->dst, &hcon->dst);
7236                         chan->src_type = bdaddr_type(hcon, hcon->src_type);
7237                         chan->dst_type = dst_type;
7238
7239                         __l2cap_chan_add(conn, chan);
7240                 }
7241
7242                 l2cap_chan_unlock(pchan);
7243 next:
7244                 next = l2cap_global_fixed_chan(pchan, &hdev->bdaddr,
7245                                                hcon->type);
7246                 l2cap_chan_put(pchan);
7247                 pchan = next;
7248         }
7249
7250         l2cap_conn_ready(conn);
7251 }
7252
7253 int l2cap_disconn_ind(struct hci_conn *hcon)
7254 {
7255         struct l2cap_conn *conn = hcon->l2cap_data;
7256
7257         BT_DBG("hcon %p", hcon);
7258
7259         if (!conn)
7260                 return HCI_ERROR_REMOTE_USER_TERM;
7261         return conn->disc_reason;
7262 }
7263
7264 void l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
7265 {
7266         BT_DBG("hcon %p reason %d", hcon, reason);
7267
7268         l2cap_conn_del(hcon, bt_to_errno(reason));
7269 }
7270
7271 static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt)
7272 {
7273         if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED)
7274                 return;
7275
7276         if (encrypt == 0x00) {
7277                 if (chan->sec_level == BT_SECURITY_MEDIUM) {
7278                         __set_chan_timer(chan, L2CAP_ENC_TIMEOUT);
7279                 } else if (chan->sec_level == BT_SECURITY_HIGH ||
7280                            chan->sec_level == BT_SECURITY_FIPS)
7281                         l2cap_chan_close(chan, ECONNREFUSED);
7282         } else {
7283                 if (chan->sec_level == BT_SECURITY_MEDIUM)
7284                         __clear_chan_timer(chan);
7285         }
7286 }
7287
7288 int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
7289 {
7290         struct l2cap_conn *conn = hcon->l2cap_data;
7291         struct l2cap_chan *chan;
7292
7293         if (!conn)
7294                 return 0;
7295
7296         BT_DBG("conn %p status 0x%2.2x encrypt %u", conn, status, encrypt);
7297
7298         mutex_lock(&conn->chan_lock);
7299
7300         list_for_each_entry(chan, &conn->chan_l, list) {
7301                 l2cap_chan_lock(chan);
7302
7303                 BT_DBG("chan %p scid 0x%4.4x state %s", chan, chan->scid,
7304                        state_to_string(chan->state));
7305
7306                 if (chan->scid == L2CAP_CID_A2MP) {
7307                         l2cap_chan_unlock(chan);
7308                         continue;
7309                 }
7310
7311                 if (!status && encrypt)
7312                         chan->sec_level = hcon->sec_level;
7313
7314                 if (!__l2cap_no_conn_pending(chan)) {
7315                         l2cap_chan_unlock(chan);
7316                         continue;
7317                 }
7318
7319                 if (!status && (chan->state == BT_CONNECTED ||
7320                                 chan->state == BT_CONFIG)) {
7321                         chan->ops->resume(chan);
7322                         l2cap_check_encryption(chan, encrypt);
7323                         l2cap_chan_unlock(chan);
7324                         continue;
7325                 }
7326
7327                 if (chan->state == BT_CONNECT) {
7328                         if (!status)
7329                                 l2cap_start_connection(chan);
7330                         else
7331                                 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
7332                 } else if (chan->state == BT_CONNECT2) {
7333                         struct l2cap_conn_rsp rsp;
7334                         __u16 res, stat;
7335
7336                         if (!status) {
7337                                 if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
7338                                         res = L2CAP_CR_PEND;
7339                                         stat = L2CAP_CS_AUTHOR_PEND;
7340                                         chan->ops->defer(chan);
7341                                 } else {
7342                                         l2cap_state_change(chan, BT_CONFIG);
7343                                         res = L2CAP_CR_SUCCESS;
7344                                         stat = L2CAP_CS_NO_INFO;
7345                                 }
7346                         } else {
7347                                 l2cap_state_change(chan, BT_DISCONN);
7348                                 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
7349                                 res = L2CAP_CR_SEC_BLOCK;
7350                                 stat = L2CAP_CS_NO_INFO;
7351                         }
7352
7353                         rsp.scid   = cpu_to_le16(chan->dcid);
7354                         rsp.dcid   = cpu_to_le16(chan->scid);
7355                         rsp.result = cpu_to_le16(res);
7356                         rsp.status = cpu_to_le16(stat);
7357                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
7358                                        sizeof(rsp), &rsp);
7359
7360                         if (!test_bit(CONF_REQ_SENT, &chan->conf_state) &&
7361                             res == L2CAP_CR_SUCCESS) {
7362                                 char buf[128];
7363                                 set_bit(CONF_REQ_SENT, &chan->conf_state);
7364                                 l2cap_send_cmd(conn, l2cap_get_ident(conn),
7365                                                L2CAP_CONF_REQ,
7366                                                l2cap_build_conf_req(chan, buf),
7367                                                buf);
7368                                 chan->num_conf_req++;
7369                         }
7370                 }
7371
7372                 l2cap_chan_unlock(chan);
7373         }
7374
7375         mutex_unlock(&conn->chan_lock);
7376
7377         return 0;
7378 }
7379
7380 int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
7381 {
7382         struct l2cap_conn *conn = hcon->l2cap_data;
7383         struct l2cap_hdr *hdr;
7384         int len;
7385
7386         /* For AMP controller do not create l2cap conn */
7387         if (!conn && hcon->hdev->dev_type != HCI_BREDR)
7388                 goto drop;
7389
7390         if (!conn)
7391                 conn = l2cap_conn_add(hcon);
7392
7393         if (!conn)
7394                 goto drop;
7395
7396         BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
7397
7398         switch (flags) {
7399         case ACL_START:
7400         case ACL_START_NO_FLUSH:
7401         case ACL_COMPLETE:
7402                 if (conn->rx_len) {
7403                         BT_ERR("Unexpected start frame (len %d)", skb->len);
7404                         kfree_skb(conn->rx_skb);
7405                         conn->rx_skb = NULL;
7406                         conn->rx_len = 0;
7407                         l2cap_conn_unreliable(conn, ECOMM);
7408                 }
7409
7410                 /* Start fragment always begin with Basic L2CAP header */
7411                 if (skb->len < L2CAP_HDR_SIZE) {
7412                         BT_ERR("Frame is too short (len %d)", skb->len);
7413                         l2cap_conn_unreliable(conn, ECOMM);
7414                         goto drop;
7415                 }
7416
7417                 hdr = (struct l2cap_hdr *) skb->data;
7418                 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
7419
7420                 if (len == skb->len) {
7421                         /* Complete frame received */
7422                         l2cap_recv_frame(conn, skb);
7423                         return 0;
7424                 }
7425
7426                 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
7427
7428                 if (skb->len > len) {
7429                         BT_ERR("Frame is too long (len %d, expected len %d)",
7430                                skb->len, len);
7431                         l2cap_conn_unreliable(conn, ECOMM);
7432                         goto drop;
7433                 }
7434
7435                 /* Allocate skb for the complete frame (with header) */
7436                 conn->rx_skb = bt_skb_alloc(len, GFP_KERNEL);
7437                 if (!conn->rx_skb)
7438                         goto drop;
7439
7440                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
7441                                           skb->len);
7442                 conn->rx_len = len - skb->len;
7443                 break;
7444
7445         case ACL_CONT:
7446                 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
7447
7448                 if (!conn->rx_len) {
7449                         BT_ERR("Unexpected continuation frame (len %d)", skb->len);
7450                         l2cap_conn_unreliable(conn, ECOMM);
7451                         goto drop;
7452                 }
7453
7454                 if (skb->len > conn->rx_len) {
7455                         BT_ERR("Fragment is too long (len %d, expected %d)",
7456                                skb->len, conn->rx_len);
7457                         kfree_skb(conn->rx_skb);
7458                         conn->rx_skb = NULL;
7459                         conn->rx_len = 0;
7460                         l2cap_conn_unreliable(conn, ECOMM);
7461                         goto drop;
7462                 }
7463
7464                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
7465                                           skb->len);
7466                 conn->rx_len -= skb->len;
7467
7468                 if (!conn->rx_len) {
7469                         /* Complete frame received. l2cap_recv_frame
7470                          * takes ownership of the skb so set the global
7471                          * rx_skb pointer to NULL first.
7472                          */
7473                         struct sk_buff *rx_skb = conn->rx_skb;
7474                         conn->rx_skb = NULL;
7475                         l2cap_recv_frame(conn, rx_skb);
7476                 }
7477                 break;
7478         }
7479
7480 drop:
7481         kfree_skb(skb);
7482         return 0;
7483 }
7484
7485 static int l2cap_debugfs_show(struct seq_file *f, void *p)
7486 {
7487         struct l2cap_chan *c;
7488
7489         read_lock(&chan_list_lock);
7490
7491         list_for_each_entry(c, &chan_list, global_l) {
7492                 seq_printf(f, "%pMR %pMR %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
7493                            &c->src, &c->dst,
7494                            c->state, __le16_to_cpu(c->psm),
7495                            c->scid, c->dcid, c->imtu, c->omtu,
7496                            c->sec_level, c->mode);
7497         }
7498
7499         read_unlock(&chan_list_lock);
7500
7501         return 0;
7502 }
7503
7504 static int l2cap_debugfs_open(struct inode *inode, struct file *file)
7505 {
7506         return single_open(file, l2cap_debugfs_show, inode->i_private);
7507 }
7508
7509 static const struct file_operations l2cap_debugfs_fops = {
7510         .open           = l2cap_debugfs_open,
7511         .read           = seq_read,
7512         .llseek         = seq_lseek,
7513         .release        = single_release,
7514 };
7515
7516 static struct dentry *l2cap_debugfs;
7517
7518 int __init l2cap_init(void)
7519 {
7520         int err;
7521
7522         err = l2cap_init_sockets();
7523         if (err < 0)
7524                 return err;
7525
7526         if (IS_ERR_OR_NULL(bt_debugfs))
7527                 return 0;
7528
7529         l2cap_debugfs = debugfs_create_file("l2cap", 0444, bt_debugfs,
7530                                             NULL, &l2cap_debugfs_fops);
7531
7532         debugfs_create_u16("l2cap_le_max_credits", 0644, bt_debugfs,
7533                            &le_max_credits);
7534         debugfs_create_u16("l2cap_le_default_mps", 0644, bt_debugfs,
7535                            &le_default_mps);
7536
7537         return 0;
7538 }
7539
7540 void l2cap_exit(void)
7541 {
7542         debugfs_remove(l2cap_debugfs);
7543         l2cap_cleanup_sockets();
7544 }
7545
7546 module_param(disable_ertm, bool, 0644);
7547 MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode");