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