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