Pileus Git - ~andy/linux/blob - net/bluetooth/l2cap_core.c

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