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