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