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