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

   1 /*
   2    BlueZ - Bluetooth protocol stack for Linux
   3    Copyright (C) 2000-2001 Qualcomm Incorporated
   4    Copyright (C) 2009-2010 Gustavo F. Padovan <gustavo@padovan.org>
   5    Copyright (C) 2010 Google Inc.
   6    Copyright (C) 2011 ProFUSION Embedded Systems
   7    Copyright (c) 2012 Code Aurora Forum.  All rights reserved.
   8
   9    Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.com>
  10
  11    This program is free software; you can redistribute it and/or modify
  12    it under the terms of the GNU General Public License version 2 as
  13    published by the Free Software Foundation;
  14
  15    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
  16    OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  17    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
  18    IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY
  19    CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES
  20    WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
  21    ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
  22    OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  23
  24    ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS,
  25    COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS
  26    SOFTWARE IS DISCLAIMED.
  27 */
  28
  29 /* Bluetooth L2CAP core. */
  30
  31 #include <linux/module.h>
  32
  33 #include <linux/debugfs.h>
  34 #include <linux/crc16.h>
  35
  36 #include <net/bluetooth/bluetooth.h>
  37 #include <net/bluetooth/hci_core.h>
  38 #include <net/bluetooth/l2cap.h>
  39
  40 #include "smp.h"
  41 #include "a2mp.h"
  42 #include "amp.h"
  43 #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         pdu_len = chan->conn->mtu - L2CAP_HDR_SIZE;
2612
2613         pdu_len = min_t(size_t, pdu_len, chan->remote_mps);
2614
2615         sdu_len = len;
2616         pdu_len -= L2CAP_SDULEN_SIZE;
2617
2618         while (len > 0) {
2619                 if (len <= pdu_len)
2620                         pdu_len = len;
2621
2622                 skb = l2cap_create_le_flowctl_pdu(chan, msg, pdu_len, sdu_len);
2623                 if (IS_ERR(skb)) {
2624                         __skb_queue_purge(seg_queue);
2625                         return PTR_ERR(skb);
2626                 }
2627
2628                 __skb_queue_tail(seg_queue, skb);
2629
2630                 len -= pdu_len;
2631
2632                 if (sdu_len) {
2633                         sdu_len = 0;
2634                         pdu_len += L2CAP_SDULEN_SIZE;
2635                 }
2636         }
2637
2638         return 0;
2639 }
2640
2641 int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
2642                     u32 priority)
2643 {
2644         struct sk_buff *skb;
2645         int err;
2646         struct sk_buff_head seg_queue;
2647
2648         if (!chan->conn)
2649                 return -ENOTCONN;
2650
2651         /* Connectionless channel */
2652         if (chan->chan_type == L2CAP_CHAN_CONN_LESS) {
2653                 skb = l2cap_create_connless_pdu(chan, msg, len, priority);
2654                 if (IS_ERR(skb))
2655                         return PTR_ERR(skb);
2656
2657                 l2cap_do_send(chan, skb);
2658                 return len;
2659         }
2660
2661         switch (chan->mode) {
2662         case L2CAP_MODE_LE_FLOWCTL:
2663                 /* Check outgoing MTU */
2664                 if (len > chan->omtu)
2665                         return -EMSGSIZE;
2666
2667                 if (!chan->tx_credits)
2668                         return -EAGAIN;
2669
2670                 __skb_queue_head_init(&seg_queue);
2671
2672                 err = l2cap_segment_le_sdu(chan, &seg_queue, msg, len);
2673
2674                 if (chan->state != BT_CONNECTED) {
2675                         __skb_queue_purge(&seg_queue);
2676                         err = -ENOTCONN;
2677                 }
2678
2679                 if (err)
2680                         return err;
2681
2682                 skb_queue_splice_tail_init(&seg_queue, &chan->tx_q);
2683
2684                 while (chan->tx_credits && !skb_queue_empty(&chan->tx_q)) {
2685                         l2cap_do_send(chan, skb_dequeue(&chan->tx_q));
2686                         chan->tx_credits--;
2687                 }
2688
2689                 if (!chan->tx_credits)
2690                         chan->ops->suspend(chan);
2691
2692                 err = len;
2693
2694                 break;
2695
2696         case L2CAP_MODE_BASIC:
2697                 /* Check outgoing MTU */
2698                 if (len > chan->omtu)
2699                         return -EMSGSIZE;
2700
2701                 /* Create a basic PDU */
2702                 skb = l2cap_create_basic_pdu(chan, msg, len, priority);
2703                 if (IS_ERR(skb))
2704                         return PTR_ERR(skb);
2705
2706                 l2cap_do_send(chan, skb);
2707                 err = len;
2708                 break;
2709
2710         case L2CAP_MODE_ERTM:
2711         case L2CAP_MODE_STREAMING:
2712                 /* Check outgoing MTU */
2713                 if (len > chan->omtu) {
2714                         err = -EMSGSIZE;
2715                         break;
2716                 }
2717
2718                 __skb_queue_head_init(&seg_queue);
2719
2720                 /* Do segmentation before calling in to the state machine,
2721                  * since it's possible to block while waiting for memory
2722                  * allocation.
2723                  */
2724                 err = l2cap_segment_sdu(chan, &seg_queue, msg, len);
2725
2726                 /* The channel could have been closed while segmenting,
2727                  * check that it is still connected.
2728                  */
2729                 if (chan->state != BT_CONNECTED) {
2730                         __skb_queue_purge(&seg_queue);
2731                         err = -ENOTCONN;
2732                 }
2733
2734                 if (err)
2735                         break;
2736
2737                 if (chan->mode == L2CAP_MODE_ERTM)
2738                         l2cap_tx(chan, NULL, &seg_queue, L2CAP_EV_DATA_REQUEST);
2739                 else
2740                         l2cap_streaming_send(chan, &seg_queue);
2741
2742                 err = len;
2743
2744                 /* If the skbs were not queued for sending, they'll still be in
2745                  * seg_queue and need to be purged.
2746                  */
2747                 __skb_queue_purge(&seg_queue);
2748                 break;
2749
2750         default:
2751                 BT_DBG("bad state %1.1x", chan->mode);
2752                 err = -EBADFD;
2753         }
2754
2755         return err;
2756 }
2757
2758 static void l2cap_send_srej(struct l2cap_chan *chan, u16 txseq)
2759 {
2760         struct l2cap_ctrl control;
2761         u16 seq;
2762
2763         BT_DBG("chan %p, txseq %u", chan, txseq);
2764
2765         memset(&control, 0, sizeof(control));
2766         control.sframe = 1;
2767         control.super = L2CAP_SUPER_SREJ;
2768
2769         for (seq = chan->expected_tx_seq; seq != txseq;
2770              seq = __next_seq(chan, seq)) {
2771                 if (!l2cap_ertm_seq_in_queue(&chan->srej_q, seq)) {
2772                         control.reqseq = seq;
2773                         l2cap_send_sframe(chan, &control);
2774                         l2cap_seq_list_append(&chan->srej_list, seq);
2775                 }
2776         }
2777
2778         chan->expected_tx_seq = __next_seq(chan, txseq);
2779 }
2780
2781 static void l2cap_send_srej_tail(struct l2cap_chan *chan)
2782 {
2783         struct l2cap_ctrl control;
2784
2785         BT_DBG("chan %p", chan);
2786
2787         if (chan->srej_list.tail == L2CAP_SEQ_LIST_CLEAR)
2788                 return;
2789
2790         memset(&control, 0, sizeof(control));
2791         control.sframe = 1;
2792         control.super = L2CAP_SUPER_SREJ;
2793         control.reqseq = chan->srej_list.tail;
2794         l2cap_send_sframe(chan, &control);
2795 }
2796
2797 static void l2cap_send_srej_list(struct l2cap_chan *chan, u16 txseq)
2798 {
2799         struct l2cap_ctrl control;
2800         u16 initial_head;
2801         u16 seq;
2802
2803         BT_DBG("chan %p, txseq %u", chan, txseq);
2804
2805         memset(&control, 0, sizeof(control));
2806         control.sframe = 1;
2807         control.super = L2CAP_SUPER_SREJ;
2808
2809         /* Capture initial list head to allow only one pass through the list. */
2810         initial_head = chan->srej_list.head;
2811
2812         do {
2813                 seq = l2cap_seq_list_pop(&chan->srej_list);
2814                 if (seq == txseq || seq == L2CAP_SEQ_LIST_CLEAR)
2815                         break;
2816
2817                 control.reqseq = seq;
2818                 l2cap_send_sframe(chan, &control);
2819                 l2cap_seq_list_append(&chan->srej_list, seq);
2820         } while (chan->srej_list.head != initial_head);
2821 }
2822
2823 static void l2cap_process_reqseq(struct l2cap_chan *chan, u16 reqseq)
2824 {
2825         struct sk_buff *acked_skb;
2826         u16 ackseq;
2827
2828         BT_DBG("chan %p, reqseq %u", chan, reqseq);
2829
2830         if (chan->unacked_frames == 0 || reqseq == chan->expected_ack_seq)
2831                 return;
2832
2833         BT_DBG("expected_ack_seq %u, unacked_frames %u",
2834                chan->expected_ack_seq, chan->unacked_frames);
2835
2836         for (ackseq = chan->expected_ack_seq; ackseq != reqseq;
2837              ackseq = __next_seq(chan, ackseq)) {
2838
2839                 acked_skb = l2cap_ertm_seq_in_queue(&chan->tx_q, ackseq);
2840                 if (acked_skb) {
2841                         skb_unlink(acked_skb, &chan->tx_q);
2842                         kfree_skb(acked_skb);
2843                         chan->unacked_frames--;
2844                 }
2845         }
2846
2847         chan->expected_ack_seq = reqseq;
2848
2849         if (chan->unacked_frames == 0)
2850                 __clear_retrans_timer(chan);
2851
2852         BT_DBG("unacked_frames %u", chan->unacked_frames);
2853 }
2854
2855 static void l2cap_abort_rx_srej_sent(struct l2cap_chan *chan)
2856 {
2857         BT_DBG("chan %p", chan);
2858
2859         chan->expected_tx_seq = chan->buffer_seq;
2860         l2cap_seq_list_clear(&chan->srej_list);
2861         skb_queue_purge(&chan->srej_q);
2862         chan->rx_state = L2CAP_RX_STATE_RECV;
2863 }
2864
2865 static void l2cap_tx_state_xmit(struct l2cap_chan *chan,
2866                                 struct l2cap_ctrl *control,
2867                                 struct sk_buff_head *skbs, u8 event)
2868 {
2869         BT_DBG("chan %p, control %p, skbs %p, event %d", chan, control, skbs,
2870                event);
2871
2872         switch (event) {
2873         case L2CAP_EV_DATA_REQUEST:
2874                 if (chan->tx_send_head == NULL)
2875                         chan->tx_send_head = skb_peek(skbs);
2876
2877                 skb_queue_splice_tail_init(skbs, &chan->tx_q);
2878                 l2cap_ertm_send(chan);
2879                 break;
2880         case L2CAP_EV_LOCAL_BUSY_DETECTED:
2881                 BT_DBG("Enter LOCAL_BUSY");
2882                 set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2883
2884                 if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
2885                         /* The SREJ_SENT state must be aborted if we are to
2886                          * enter the LOCAL_BUSY state.
2887                          */
2888                         l2cap_abort_rx_srej_sent(chan);
2889                 }
2890
2891                 l2cap_send_ack(chan);
2892
2893                 break;
2894         case L2CAP_EV_LOCAL_BUSY_CLEAR:
2895                 BT_DBG("Exit LOCAL_BUSY");
2896                 clear_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2897
2898                 if (test_bit(CONN_RNR_SENT, &chan->conn_state)) {
2899                         struct l2cap_ctrl local_control;
2900
2901                         memset(&local_control, 0, sizeof(local_control));
2902                         local_control.sframe = 1;
2903                         local_control.super = L2CAP_SUPER_RR;
2904                         local_control.poll = 1;
2905                         local_control.reqseq = chan->buffer_seq;
2906                         l2cap_send_sframe(chan, &local_control);
2907
2908                         chan->retry_count = 1;
2909                         __set_monitor_timer(chan);
2910                         chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2911                 }
2912                 break;
2913         case L2CAP_EV_RECV_REQSEQ_AND_FBIT:
2914                 l2cap_process_reqseq(chan, control->reqseq);
2915                 break;
2916         case L2CAP_EV_EXPLICIT_POLL:
2917                 l2cap_send_rr_or_rnr(chan, 1);
2918                 chan->retry_count = 1;
2919                 __set_monitor_timer(chan);
2920                 __clear_ack_timer(chan);
2921                 chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2922                 break;
2923         case L2CAP_EV_RETRANS_TO:
2924                 l2cap_send_rr_or_rnr(chan, 1);
2925                 chan->retry_count = 1;
2926                 __set_monitor_timer(chan);
2927                 chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2928                 break;
2929         case L2CAP_EV_RECV_FBIT:
2930                 /* Nothing to process */
2931                 break;
2932         default:
2933                 break;
2934         }
2935 }
2936
2937 static void l2cap_tx_state_wait_f(struct l2cap_chan *chan,
2938                                   struct l2cap_ctrl *control,
2939                                   struct sk_buff_head *skbs, u8 event)
2940 {
2941         BT_DBG("chan %p, control %p, skbs %p, event %d", chan, control, skbs,
2942                event);
2943
2944         switch (event) {
2945         case L2CAP_EV_DATA_REQUEST:
2946                 if (chan->tx_send_head == NULL)
2947                         chan->tx_send_head = skb_peek(skbs);
2948                 /* Queue data, but don't send. */
2949                 skb_queue_splice_tail_init(skbs, &chan->tx_q);
2950                 break;
2951         case L2CAP_EV_LOCAL_BUSY_DETECTED:
2952                 BT_DBG("Enter LOCAL_BUSY");
2953                 set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2954
2955                 if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
2956                         /* The SREJ_SENT state must be aborted if we are to
2957                          * enter the LOCAL_BUSY state.
2958                          */
2959                         l2cap_abort_rx_srej_sent(chan);
2960                 }
2961
2962                 l2cap_send_ack(chan);
2963
2964                 break;
2965         case L2CAP_EV_LOCAL_BUSY_CLEAR:
2966                 BT_DBG("Exit LOCAL_BUSY");
2967                 clear_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2968
2969                 if (test_bit(CONN_RNR_SENT, &chan->conn_state)) {
2970                         struct l2cap_ctrl local_control;
2971                         memset(&local_control, 0, sizeof(local_control));
2972                         local_control.sframe = 1;
2973                         local_control.super = L2CAP_SUPER_RR;
2974                         local_control.poll = 1;
2975                         local_control.reqseq = chan->buffer_seq;
2976                         l2cap_send_sframe(chan, &local_control);
2977
2978                         chan->retry_count = 1;
2979                         __set_monitor_timer(chan);
2980                         chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2981                 }
2982                 break;
2983         case L2CAP_EV_RECV_REQSEQ_AND_FBIT:
2984                 l2cap_process_reqseq(chan, control->reqseq);
2985
2986                 /* Fall through */
2987
2988         case L2CAP_EV_RECV_FBIT:
2989                 if (control && control->final) {
2990                         __clear_monitor_timer(chan);
2991                         if (chan->unacked_frames > 0)
2992                                 __set_retrans_timer(chan);
2993                         chan->retry_count = 0;
2994                         chan->tx_state = L2CAP_TX_STATE_XMIT;
2995                         BT_DBG("recv fbit tx_state 0x2.2%x", chan->tx_state);
2996                 }
2997                 break;
2998         case L2CAP_EV_EXPLICIT_POLL:
2999                 /* Ignore */
3000                 break;
3001         case L2CAP_EV_MONITOR_TO:
3002                 if (chan->max_tx == 0 || chan->retry_count < chan->max_tx) {
3003                         l2cap_send_rr_or_rnr(chan, 1);
3004                         __set_monitor_timer(chan);
3005                         chan->retry_count++;
3006                 } else {
3007                         l2cap_send_disconn_req(chan, ECONNABORTED);
3008                 }
3009                 break;
3010         default:
3011                 break;
3012         }
3013 }
3014
3015 static void l2cap_tx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
3016                      struct sk_buff_head *skbs, u8 event)
3017 {
3018         BT_DBG("chan %p, control %p, skbs %p, event %d, state %d",
3019                chan, control, skbs, event, chan->tx_state);
3020
3021         switch (chan->tx_state) {
3022         case L2CAP_TX_STATE_XMIT:
3023                 l2cap_tx_state_xmit(chan, control, skbs, event);
3024                 break;
3025         case L2CAP_TX_STATE_WAIT_F:
3026                 l2cap_tx_state_wait_f(chan, control, skbs, event);
3027                 break;
3028         default:
3029                 /* Ignore event */
3030                 break;
3031         }
3032 }
3033
3034 static void l2cap_pass_to_tx(struct l2cap_chan *chan,
3035                              struct l2cap_ctrl *control)
3036 {
3037         BT_DBG("chan %p, control %p", chan, control);
3038         l2cap_tx(chan, control, NULL, L2CAP_EV_RECV_REQSEQ_AND_FBIT);
3039 }
3040
3041 static void l2cap_pass_to_tx_fbit(struct l2cap_chan *chan,
3042                                   struct l2cap_ctrl *control)
3043 {
3044         BT_DBG("chan %p, control %p", chan, control);
3045         l2cap_tx(chan, control, NULL, L2CAP_EV_RECV_FBIT);
3046 }
3047
3048 /* Copy frame to all raw sockets on that connection */
3049 static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
3050 {
3051         struct sk_buff *nskb;
3052         struct l2cap_chan *chan;
3053
3054         BT_DBG("conn %p", conn);
3055
3056         mutex_lock(&conn->chan_lock);
3057
3058         list_for_each_entry(chan, &conn->chan_l, list) {
3059                 if (chan->chan_type != L2CAP_CHAN_RAW)
3060                         continue;
3061
3062                 /* Don't send frame to the channel it came from */
3063                 if (bt_cb(skb)->chan == chan)
3064                         continue;
3065
3066                 nskb = skb_clone(skb, GFP_KERNEL);
3067                 if (!nskb)
3068                         continue;
3069                 if (chan->ops->recv(chan, nskb))
3070                         kfree_skb(nskb);
3071         }
3072
3073         mutex_unlock(&conn->chan_lock);
3074 }
3075
3076 /* ---- L2CAP signalling commands ---- */
3077 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn, u8 code,
3078                                        u8 ident, u16 dlen, void *data)
3079 {
3080         struct sk_buff *skb, **frag;
3081         struct l2cap_cmd_hdr *cmd;
3082         struct l2cap_hdr *lh;
3083         int len, count;
3084
3085         BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %u",
3086                conn, code, ident, dlen);
3087
3088         if (conn->mtu < L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE)
3089                 return NULL;
3090
3091         len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen;
3092         count = min_t(unsigned int, conn->mtu, len);
3093
3094         skb = bt_skb_alloc(count, GFP_KERNEL);
3095         if (!skb)
3096                 return NULL;
3097
3098         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
3099         lh->len = cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen);
3100
3101         if (conn->hcon->type == LE_LINK)
3102                 lh->cid = __constant_cpu_to_le16(L2CAP_CID_LE_SIGNALING);
3103         else
3104                 lh->cid = __constant_cpu_to_le16(L2CAP_CID_SIGNALING);
3105
3106         cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE);
3107         cmd->code  = code;
3108         cmd->ident = ident;
3109         cmd->len   = cpu_to_le16(dlen);
3110
3111         if (dlen) {
3112                 count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE;
3113                 memcpy(skb_put(skb, count), data, count);
3114                 data += count;
3115         }
3116
3117         len -= skb->len;
3118
3119         /* Continuation fragments (no L2CAP header) */
3120         frag = &skb_shinfo(skb)->frag_list;
3121         while (len) {
3122                 count = min_t(unsigned int, conn->mtu, len);
3123
3124                 *frag = bt_skb_alloc(count, GFP_KERNEL);
3125                 if (!*frag)
3126                         goto fail;
3127
3128                 memcpy(skb_put(*frag, count), data, count);
3129
3130                 len  -= count;
3131                 data += count;
3132
3133                 frag = &(*frag)->next;
3134         }
3135
3136         return skb;
3137
3138 fail:
3139         kfree_skb(skb);
3140         return NULL;
3141 }
3142
3143 static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen,
3144                                      unsigned long *val)
3145 {
3146         struct l2cap_conf_opt *opt = *ptr;
3147         int len;
3148
3149         len = L2CAP_CONF_OPT_SIZE + opt->len;
3150         *ptr += len;
3151
3152         *type = opt->type;
3153         *olen = opt->len;
3154
3155         switch (opt->len) {
3156         case 1:
3157                 *val = *((u8 *) opt->val);
3158                 break;
3159
3160         case 2:
3161                 *val = get_unaligned_le16(opt->val);
3162                 break;
3163
3164         case 4:
3165                 *val = get_unaligned_le32(opt->val);
3166                 break;
3167
3168         default:
3169                 *val = (unsigned long) opt->val;
3170                 break;
3171         }
3172
3173         BT_DBG("type 0x%2.2x len %u val 0x%lx", *type, opt->len, *val);
3174         return len;
3175 }
3176
3177 static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val)
3178 {
3179         struct l2cap_conf_opt *opt = *ptr;
3180
3181         BT_DBG("type 0x%2.2x len %u val 0x%lx", type, len, val);
3182
3183         opt->type = type;
3184         opt->len  = len;
3185
3186         switch (len) {
3187         case 1:
3188                 *((u8 *) opt->val)  = val;
3189                 break;
3190
3191         case 2:
3192                 put_unaligned_le16(val, opt->val);
3193                 break;
3194
3195         case 4:
3196                 put_unaligned_le32(val, opt->val);
3197                 break;
3198
3199         default:
3200                 memcpy(opt->val, (void *) val, len);
3201                 break;
3202         }
3203
3204         *ptr += L2CAP_CONF_OPT_SIZE + len;
3205 }
3206
3207 static void l2cap_add_opt_efs(void **ptr, struct l2cap_chan *chan)
3208 {
3209         struct l2cap_conf_efs efs;
3210
3211         switch (chan->mode) {
3212         case L2CAP_MODE_ERTM:
3213                 efs.id          = chan->local_id;
3214                 efs.stype       = chan->local_stype;
3215                 efs.msdu        = cpu_to_le16(chan->local_msdu);
3216                 efs.sdu_itime   = cpu_to_le32(chan->local_sdu_itime);
3217                 efs.acc_lat     = __constant_cpu_to_le32(L2CAP_DEFAULT_ACC_LAT);
3218                 efs.flush_to    = __constant_cpu_to_le32(L2CAP_EFS_DEFAULT_FLUSH_TO);
3219                 break;
3220
3221         case L2CAP_MODE_STREAMING:
3222                 efs.id          = 1;
3223                 efs.stype       = L2CAP_SERV_BESTEFFORT;
3224                 efs.msdu        = cpu_to_le16(chan->local_msdu);
3225                 efs.sdu_itime   = cpu_to_le32(chan->local_sdu_itime);
3226                 efs.acc_lat     = 0;
3227                 efs.flush_to    = 0;
3228                 break;
3229
3230         default:
3231                 return;
3232         }
3233
3234         l2cap_add_conf_opt(ptr, L2CAP_CONF_EFS, sizeof(efs),
3235                            (unsigned long) &efs);
3236 }
3237
3238 static void l2cap_ack_timeout(struct work_struct *work)
3239 {
3240         struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
3241                                                ack_timer.work);
3242         u16 frames_to_ack;
3243
3244         BT_DBG("chan %p", chan);
3245
3246         l2cap_chan_lock(chan);
3247
3248         frames_to_ack = __seq_offset(chan, chan->buffer_seq,
3249                                      chan->last_acked_seq);
3250
3251         if (frames_to_ack)
3252                 l2cap_send_rr_or_rnr(chan, 0);
3253
3254         l2cap_chan_unlock(chan);
3255         l2cap_chan_put(chan);
3256 }
3257
3258 int l2cap_ertm_init(struct l2cap_chan *chan)
3259 {
3260         int err;
3261
3262         chan->next_tx_seq = 0;
3263         chan->expected_tx_seq = 0;
3264         chan->expected_ack_seq = 0;
3265         chan->unacked_frames = 0;
3266         chan->buffer_seq = 0;
3267         chan->frames_sent = 0;
3268         chan->last_acked_seq = 0;
3269         chan->sdu = NULL;
3270         chan->sdu_last_frag = NULL;
3271         chan->sdu_len = 0;
3272
3273         skb_queue_head_init(&chan->tx_q);
3274
3275         chan->local_amp_id = AMP_ID_BREDR;
3276         chan->move_id = AMP_ID_BREDR;
3277         chan->move_state = L2CAP_MOVE_STABLE;
3278         chan->move_role = L2CAP_MOVE_ROLE_NONE;
3279
3280         if (chan->mode != L2CAP_MODE_ERTM)
3281                 return 0;
3282
3283         chan->rx_state = L2CAP_RX_STATE_RECV;
3284         chan->tx_state = L2CAP_TX_STATE_XMIT;
3285
3286         INIT_DELAYED_WORK(&chan->retrans_timer, l2cap_retrans_timeout);
3287         INIT_DELAYED_WORK(&chan->monitor_timer, l2cap_monitor_timeout);
3288         INIT_DELAYED_WORK(&chan->ack_timer, l2cap_ack_timeout);
3289
3290         skb_queue_head_init(&chan->srej_q);
3291
3292         err = l2cap_seq_list_init(&chan->srej_list, chan->tx_win);
3293         if (err < 0)
3294                 return err;
3295
3296         err = l2cap_seq_list_init(&chan->retrans_list, chan->remote_tx_win);
3297         if (err < 0)
3298                 l2cap_seq_list_free(&chan->srej_list);
3299
3300         return err;
3301 }
3302
3303 static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
3304 {
3305         switch (mode) {
3306         case L2CAP_MODE_STREAMING:
3307         case L2CAP_MODE_ERTM:
3308                 if (l2cap_mode_supported(mode, remote_feat_mask))
3309                         return mode;
3310                 /* fall through */
3311         default:
3312                 return L2CAP_MODE_BASIC;
3313         }
3314 }
3315
3316 static inline bool __l2cap_ews_supported(struct l2cap_conn *conn)
3317 {
3318         return conn->hs_enabled && conn->feat_mask & L2CAP_FEAT_EXT_WINDOW;
3319 }
3320
3321 static inline bool __l2cap_efs_supported(struct l2cap_conn *conn)
3322 {
3323         return conn->hs_enabled && conn->feat_mask & L2CAP_FEAT_EXT_FLOW;
3324 }
3325
3326 static void __l2cap_set_ertm_timeouts(struct l2cap_chan *chan,
3327                                       struct l2cap_conf_rfc *rfc)
3328 {
3329         if (chan->local_amp_id != AMP_ID_BREDR && chan->hs_hcon) {
3330                 u64 ertm_to = chan->hs_hcon->hdev->amp_be_flush_to;
3331
3332                 /* Class 1 devices have must have ERTM timeouts
3333                  * exceeding the Link Supervision Timeout.  The
3334                  * default Link Supervision Timeout for AMP
3335                  * controllers is 10 seconds.
3336                  *
3337                  * Class 1 devices use 0xffffffff for their
3338                  * best-effort flush timeout, so the clamping logic
3339                  * will result in a timeout that meets the above
3340                  * requirement.  ERTM timeouts are 16-bit values, so
3341                  * the maximum timeout is 65.535 seconds.
3342                  */
3343
3344                 /* Convert timeout to milliseconds and round */
3345                 ertm_to = DIV_ROUND_UP_ULL(ertm_to, 1000);
3346
3347                 /* This is the recommended formula for class 2 devices
3348                  * that start ERTM timers when packets are sent to the
3349                  * controller.
3350                  */
3351                 ertm_to = 3 * ertm_to + 500;
3352
3353                 if (ertm_to > 0xffff)
3354                         ertm_to = 0xffff;
3355
3356                 rfc->retrans_timeout = cpu_to_le16((u16) ertm_to);
3357                 rfc->monitor_timeout = rfc->retrans_timeout;
3358         } else {
3359                 rfc->retrans_timeout = __constant_cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO);
3360                 rfc->monitor_timeout = __constant_cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO);
3361         }
3362 }
3363
3364 static inline void l2cap_txwin_setup(struct l2cap_chan *chan)
3365 {
3366         if (chan->tx_win > L2CAP_DEFAULT_TX_WINDOW &&
3367             __l2cap_ews_supported(chan->conn)) {
3368                 /* use extended control field */
3369                 set_bit(FLAG_EXT_CTRL, &chan->flags);
3370                 chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
3371         } else {
3372                 chan->tx_win = min_t(u16, chan->tx_win,
3373                                      L2CAP_DEFAULT_TX_WINDOW);
3374                 chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
3375         }
3376         chan->ack_win = chan->tx_win;
3377 }
3378
3379 static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data)
3380 {
3381         struct l2cap_conf_req *req = data;
3382         struct l2cap_conf_rfc rfc = { .mode = chan->mode };
3383         void *ptr = req->data;
3384         u16 size;
3385
3386         BT_DBG("chan %p", chan);
3387
3388         if (chan->num_conf_req || chan->num_conf_rsp)
3389                 goto done;
3390
3391         switch (chan->mode) {
3392         case L2CAP_MODE_STREAMING:
3393         case L2CAP_MODE_ERTM:
3394                 if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state))
3395                         break;
3396
3397                 if (__l2cap_efs_supported(chan->conn))
3398                         set_bit(FLAG_EFS_ENABLE, &chan->flags);
3399
3400                 /* fall through */
3401         default:
3402                 chan->mode = l2cap_select_mode(rfc.mode, chan->conn->feat_mask);
3403                 break;
3404         }
3405
3406 done:
3407         if (chan->imtu != L2CAP_DEFAULT_MTU)
3408                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu);
3409
3410         switch (chan->mode) {
3411         case L2CAP_MODE_BASIC:
3412                 if (!(chan->conn->feat_mask & L2CAP_FEAT_ERTM) &&
3413                     !(chan->conn->feat_mask & L2CAP_FEAT_STREAMING))
3414                         break;
3415
3416                 rfc.mode            = L2CAP_MODE_BASIC;
3417                 rfc.txwin_size      = 0;
3418                 rfc.max_transmit    = 0;
3419                 rfc.retrans_timeout = 0;
3420                 rfc.monitor_timeout = 0;
3421                 rfc.max_pdu_size    = 0;
3422
3423                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3424                                    (unsigned long) &rfc);
3425                 break;
3426
3427         case L2CAP_MODE_ERTM:
3428                 rfc.mode            = L2CAP_MODE_ERTM;
3429                 rfc.max_transmit    = chan->max_tx;
3430
3431                 __l2cap_set_ertm_timeouts(chan, &rfc);
3432
3433                 size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
3434                              L2CAP_EXT_HDR_SIZE - L2CAP_SDULEN_SIZE -
3435                              L2CAP_FCS_SIZE);
3436                 rfc.max_pdu_size = cpu_to_le16(size);
3437
3438                 l2cap_txwin_setup(chan);
3439
3440                 rfc.txwin_size = min_t(u16, chan->tx_win,
3441                                        L2CAP_DEFAULT_TX_WINDOW);
3442
3443                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3444                                    (unsigned long) &rfc);
3445
3446                 if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
3447                         l2cap_add_opt_efs(&ptr, chan);
3448
3449                 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
3450                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
3451                                            chan->tx_win);
3452
3453                 if (chan->conn->feat_mask & L2CAP_FEAT_FCS)
3454                         if (chan->fcs == L2CAP_FCS_NONE ||
3455                             test_bit(CONF_RECV_NO_FCS, &chan->conf_state)) {
3456                                 chan->fcs = L2CAP_FCS_NONE;
3457                                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1,
3458                                                    chan->fcs);
3459                         }
3460                 break;
3461
3462         case L2CAP_MODE_STREAMING:
3463                 l2cap_txwin_setup(chan);
3464                 rfc.mode            = L2CAP_MODE_STREAMING;
3465                 rfc.txwin_size      = 0;
3466                 rfc.max_transmit    = 0;
3467                 rfc.retrans_timeout = 0;
3468                 rfc.monitor_timeout = 0;
3469
3470                 size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
3471                              L2CAP_EXT_HDR_SIZE - L2CAP_SDULEN_SIZE -
3472                              L2CAP_FCS_SIZE);
3473                 rfc.max_pdu_size = cpu_to_le16(size);
3474
3475                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3476                                    (unsigned long) &rfc);
3477
3478                 if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
3479                         l2cap_add_opt_efs(&ptr, chan);
3480
3481                 if (chan->conn->feat_mask & L2CAP_FEAT_FCS)
3482                         if (chan->fcs == L2CAP_FCS_NONE ||
3483                             test_bit(CONF_RECV_NO_FCS, &chan->conf_state)) {
3484                                 chan->fcs = L2CAP_FCS_NONE;
3485                                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1,
3486                                                    chan->fcs);
3487                         }
3488                 break;
3489         }
3490
3491         req->dcid  = cpu_to_le16(chan->dcid);
3492         req->flags = __constant_cpu_to_le16(0);
3493
3494         return ptr - data;
3495 }
3496
3497 static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data)
3498 {
3499         struct l2cap_conf_rsp *rsp = data;
3500         void *ptr = rsp->data;
3501         void *req = chan->conf_req;
3502         int len = chan->conf_len;
3503         int type, hint, olen;
3504         unsigned long val;
3505         struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
3506         struct l2cap_conf_efs efs;
3507         u8 remote_efs = 0;
3508         u16 mtu = L2CAP_DEFAULT_MTU;
3509         u16 result = L2CAP_CONF_SUCCESS;
3510         u16 size;
3511
3512         BT_DBG("chan %p", chan);
3513
3514         while (len >= L2CAP_CONF_OPT_SIZE) {
3515                 len -= l2cap_get_conf_opt(&req, &type, &olen, &val);
3516
3517                 hint  = type & L2CAP_CONF_HINT;
3518                 type &= L2CAP_CONF_MASK;
3519
3520                 switch (type) {
3521                 case L2CAP_CONF_MTU:
3522                         mtu = val;
3523                         break;
3524
3525                 case L2CAP_CONF_FLUSH_TO:
3526                         chan->flush_to = val;
3527                         break;
3528
3529                 case L2CAP_CONF_QOS:
3530                         break;
3531
3532                 case L2CAP_CONF_RFC:
3533                         if (olen == sizeof(rfc))
3534                                 memcpy(&rfc, (void *) val, olen);
3535                         break;
3536
3537                 case L2CAP_CONF_FCS:
3538                         if (val == L2CAP_FCS_NONE)
3539                                 set_bit(CONF_RECV_NO_FCS, &chan->conf_state);
3540                         break;
3541
3542                 case L2CAP_CONF_EFS:
3543                         remote_efs = 1;
3544                         if (olen == sizeof(efs))
3545                                 memcpy(&efs, (void *) val, olen);
3546                         break;
3547
3548                 case L2CAP_CONF_EWS:
3549                         if (!chan->conn->hs_enabled)
3550                                 return -ECONNREFUSED;
3551
3552                         set_bit(FLAG_EXT_CTRL, &chan->flags);
3553                         set_bit(CONF_EWS_RECV, &chan->conf_state);
3554                         chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
3555                         chan->remote_tx_win = val;
3556                         break;
3557
3558                 default:
3559                         if (hint)
3560                                 break;
3561
3562                         result = L2CAP_CONF_UNKNOWN;
3563                         *((u8 *) ptr++) = type;
3564                         break;
3565                 }
3566         }
3567
3568         if (chan->num_conf_rsp || chan->num_conf_req > 1)
3569                 goto done;
3570
3571         switch (chan->mode) {
3572         case L2CAP_MODE_STREAMING:
3573         case L2CAP_MODE_ERTM:
3574                 if (!test_bit(CONF_STATE2_DEVICE, &chan->conf_state)) {
3575                         chan->mode = l2cap_select_mode(rfc.mode,
3576                                                        chan->conn->feat_mask);
3577                         break;
3578                 }
3579
3580                 if (remote_efs) {
3581                         if (__l2cap_efs_supported(chan->conn))
3582                                 set_bit(FLAG_EFS_ENABLE, &chan->flags);
3583                         else
3584                                 return -ECONNREFUSED;
3585                 }
3586
3587                 if (chan->mode != rfc.mode)
3588                         return -ECONNREFUSED;
3589
3590                 break;
3591         }
3592
3593 done:
3594         if (chan->mode != rfc.mode) {
3595                 result = L2CAP_CONF_UNACCEPT;
3596                 rfc.mode = chan->mode;
3597
3598                 if (chan->num_conf_rsp == 1)
3599                         return -ECONNREFUSED;
3600
3601                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3602                                    (unsigned long) &rfc);
3603         }
3604
3605         if (result == L2CAP_CONF_SUCCESS) {
3606                 /* Configure output options and let the other side know
3607                  * which ones we don't like. */
3608
3609                 if (mtu < L2CAP_DEFAULT_MIN_MTU)
3610                         result = L2CAP_CONF_UNACCEPT;
3611                 else {
3612                         chan->omtu = mtu;
3613                         set_bit(CONF_MTU_DONE, &chan->conf_state);
3614                 }
3615                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->omtu);
3616
3617                 if (remote_efs) {
3618                         if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
3619                             efs.stype != L2CAP_SERV_NOTRAFIC &&
3620                             efs.stype != chan->local_stype) {
3621
3622                                 result = L2CAP_CONF_UNACCEPT;
3623
3624                                 if (chan->num_conf_req >= 1)
3625                                         return -ECONNREFUSED;
3626
3627                                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
3628                                                    sizeof(efs),
3629                                                    (unsigned long) &efs);
3630                         } else {
3631                                 /* Send PENDING Conf Rsp */
3632                                 result = L2CAP_CONF_PENDING;
3633                                 set_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
3634                         }
3635                 }
3636
3637                 switch (rfc.mode) {
3638                 case L2CAP_MODE_BASIC:
3639                         chan->fcs = L2CAP_FCS_NONE;
3640                         set_bit(CONF_MODE_DONE, &chan->conf_state);
3641                         break;
3642
3643                 case L2CAP_MODE_ERTM:
3644                         if (!test_bit(CONF_EWS_RECV, &chan->conf_state))
3645                                 chan->remote_tx_win = rfc.txwin_size;
3646                         else
3647                                 rfc.txwin_size = L2CAP_DEFAULT_TX_WINDOW;
3648
3649                         chan->remote_max_tx = rfc.max_transmit;
3650
3651                         size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
3652                                      chan->conn->mtu - L2CAP_EXT_HDR_SIZE -
3653                                      L2CAP_SDULEN_SIZE - L2CAP_FCS_SIZE);
3654                         rfc.max_pdu_size = cpu_to_le16(size);
3655                         chan->remote_mps = size;
3656
3657                         __l2cap_set_ertm_timeouts(chan, &rfc);
3658
3659                         set_bit(CONF_MODE_DONE, &chan->conf_state);
3660
3661                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
3662                                            sizeof(rfc), (unsigned long) &rfc);
3663
3664                         if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
3665                                 chan->remote_id = efs.id;
3666                                 chan->remote_stype = efs.stype;
3667                                 chan->remote_msdu = le16_to_cpu(efs.msdu);
3668                                 chan->remote_flush_to =
3669                                         le32_to_cpu(efs.flush_to);
3670                                 chan->remote_acc_lat =
3671                                         le32_to_cpu(efs.acc_lat);
3672                                 chan->remote_sdu_itime =
3673                                         le32_to_cpu(efs.sdu_itime);
3674                                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
3675                                                    sizeof(efs),
3676                                                    (unsigned long) &efs);
3677                         }
3678                         break;
3679
3680                 case L2CAP_MODE_STREAMING:
3681                         size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
3682                                      chan->conn->mtu - L2CAP_EXT_HDR_SIZE -
3683                                      L2CAP_SDULEN_SIZE - L2CAP_FCS_SIZE);
3684                         rfc.max_pdu_size = cpu_to_le16(size);
3685                         chan->remote_mps = size;
3686
3687                         set_bit(CONF_MODE_DONE, &chan->conf_state);
3688
3689                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3690                                            (unsigned long) &rfc);
3691
3692                         break;
3693
3694                 default:
3695                         result = L2CAP_CONF_UNACCEPT;
3696
3697                         memset(&rfc, 0, sizeof(rfc));
3698                         rfc.mode = chan->mode;
3699                 }
3700
3701                 if (result == L2CAP_CONF_SUCCESS)
3702                         set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
3703         }
3704         rsp->scid   = cpu_to_le16(chan->dcid);
3705         rsp->result = cpu_to_le16(result);
3706         rsp->flags  = __constant_cpu_to_le16(0);
3707
3708         return ptr - data;
3709 }
3710
3711 static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len,
3712                                 void *data, u16 *result)
3713 {
3714         struct l2cap_conf_req *req = data;
3715         void *ptr = req->data;
3716         int type, olen;
3717         unsigned long val;
3718         struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
3719         struct l2cap_conf_efs efs;
3720
3721         BT_DBG("chan %p, rsp %p, len %d, req %p", chan, rsp, len, data);
3722
3723         while (len >= L2CAP_CONF_OPT_SIZE) {
3724                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
3725
3726                 switch (type) {
3727                 case L2CAP_CONF_MTU:
3728                         if (val < L2CAP_DEFAULT_MIN_MTU) {
3729                                 *result = L2CAP_CONF_UNACCEPT;
3730                                 chan->imtu = L2CAP_DEFAULT_MIN_MTU;
3731                         } else
3732                                 chan->imtu = val;
3733                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu);
3734                         break;
3735
3736                 case L2CAP_CONF_FLUSH_TO:
3737                         chan->flush_to = val;
3738                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
3739                                            2, chan->flush_to);
3740                         break;
3741
3742                 case L2CAP_CONF_RFC:
3743                         if (olen == sizeof(rfc))
3744                                 memcpy(&rfc, (void *)val, olen);
3745
3746                         if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state) &&
3747                             rfc.mode != chan->mode)
3748                                 return -ECONNREFUSED;
3749
3750                         chan->fcs = 0;
3751
3752                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
3753                                            sizeof(rfc), (unsigned long) &rfc);
3754                         break;
3755
3756                 case L2CAP_CONF_EWS:
3757                         chan->ack_win = min_t(u16, val, chan->ack_win);
3758                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
3759                                            chan->tx_win);
3760                         break;
3761
3762                 case L2CAP_CONF_EFS:
3763                         if (olen == sizeof(efs))
3764                                 memcpy(&efs, (void *)val, olen);
3765
3766                         if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
3767                             efs.stype != L2CAP_SERV_NOTRAFIC &&
3768                             efs.stype != chan->local_stype)
3769                                 return -ECONNREFUSED;
3770
3771                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS, sizeof(efs),
3772                                            (unsigned long) &efs);
3773                         break;
3774
3775                 case L2CAP_CONF_FCS:
3776                         if (*result == L2CAP_CONF_PENDING)
3777                                 if (val == L2CAP_FCS_NONE)
3778                                         set_bit(CONF_RECV_NO_FCS,
3779                                                 &chan->conf_state);
3780                         break;
3781                 }
3782         }
3783
3784         if (chan->mode == L2CAP_MODE_BASIC && chan->mode != rfc.mode)
3785                 return -ECONNREFUSED;
3786
3787         chan->mode = rfc.mode;
3788
3789         if (*result == L2CAP_CONF_SUCCESS || *result == L2CAP_CONF_PENDING) {
3790                 switch (rfc.mode) {
3791                 case L2CAP_MODE_ERTM:
3792                         chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
3793                         chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
3794                         chan->mps    = le16_to_cpu(rfc.max_pdu_size);
3795                         if (!test_bit(FLAG_EXT_CTRL, &chan->flags))
3796                                 chan->ack_win = min_t(u16, chan->ack_win,
3797                                                       rfc.txwin_size);
3798
3799                         if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
3800                                 chan->local_msdu = le16_to_cpu(efs.msdu);
3801                                 chan->local_sdu_itime =
3802                                         le32_to_cpu(efs.sdu_itime);
3803                                 chan->local_acc_lat = le32_to_cpu(efs.acc_lat);
3804                                 chan->local_flush_to =
3805                                         le32_to_cpu(efs.flush_to);
3806                         }
3807                         break;
3808
3809                 case L2CAP_MODE_STREAMING:
3810                         chan->mps    = le16_to_cpu(rfc.max_pdu_size);
3811                 }
3812         }
3813
3814         req->dcid   = cpu_to_le16(chan->dcid);
3815         req->flags  = __constant_cpu_to_le16(0);
3816
3817         return ptr - data;
3818 }
3819
3820 static int l2cap_build_conf_rsp(struct l2cap_chan *chan, void *data,
3821                                 u16 result, u16 flags)
3822 {
3823         struct l2cap_conf_rsp *rsp = data;
3824         void *ptr = rsp->data;
3825
3826         BT_DBG("chan %p", chan);
3827
3828         rsp->scid   = cpu_to_le16(chan->dcid);
3829         rsp->result = cpu_to_le16(result);
3830         rsp->flags  = cpu_to_le16(flags);
3831
3832         return ptr - data;
3833 }
3834
3835 void __l2cap_le_connect_rsp_defer(struct l2cap_chan *chan)
3836 {
3837         struct l2cap_le_conn_rsp rsp;
3838         struct l2cap_conn *conn = chan->conn;
3839
3840         BT_DBG("chan %p", chan);
3841
3842         rsp.dcid    = cpu_to_le16(chan->scid);
3843         rsp.mtu     = cpu_to_le16(chan->imtu);
3844         rsp.mps     = cpu_to_le16(chan->mps);
3845         rsp.credits = cpu_to_le16(chan->rx_credits);
3846         rsp.result  = __constant_cpu_to_le16(L2CAP_CR_SUCCESS);
3847
3848         l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CONN_RSP, sizeof(rsp),
3849                        &rsp);
3850 }
3851
3852 void __l2cap_connect_rsp_defer(struct l2cap_chan *chan)
3853 {
3854         struct l2cap_conn_rsp rsp;
3855         struct l2cap_conn *conn = chan->conn;
3856         u8 buf[128];
3857         u8 rsp_code;
3858
3859         rsp.scid   = cpu_to_le16(chan->dcid);
3860         rsp.dcid   = cpu_to_le16(chan->scid);
3861         rsp.result = __constant_cpu_to_le16(L2CAP_CR_SUCCESS);
3862         rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
3863
3864         if (chan->hs_hcon)
3865                 rsp_code = L2CAP_CREATE_CHAN_RSP;
3866         else
3867                 rsp_code = L2CAP_CONN_RSP;
3868
3869         BT_DBG("chan %p rsp_code %u", chan, rsp_code);
3870
3871         l2cap_send_cmd(conn, chan->ident, rsp_code, sizeof(rsp), &rsp);
3872
3873         if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
3874                 return;
3875
3876         l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3877                        l2cap_build_conf_req(chan, buf), buf);
3878         chan->num_conf_req++;
3879 }
3880
3881 static void l2cap_conf_rfc_get(struct l2cap_chan *chan, void *rsp, int len)
3882 {
3883         int type, olen;
3884         unsigned long val;
3885         /* Use sane default values in case a misbehaving remote device
3886          * did not send an RFC or extended window size option.
3887          */
3888         u16 txwin_ext = chan->ack_win;
3889         struct l2cap_conf_rfc rfc = {
3890                 .mode = chan->mode,
3891                 .retrans_timeout = __constant_cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO),
3892                 .monitor_timeout = __constant_cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO),
3893                 .max_pdu_size = cpu_to_le16(chan->imtu),
3894                 .txwin_size = min_t(u16, chan->ack_win, L2CAP_DEFAULT_TX_WINDOW),
3895         };
3896
3897         BT_DBG("chan %p, rsp %p, len %d", chan, rsp, len);
3898
3899         if ((chan->mode != L2CAP_MODE_ERTM) && (chan->mode != L2CAP_MODE_STREAMING))
3900                 return;
3901
3902         while (len >= L2CAP_CONF_OPT_SIZE) {
3903                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
3904
3905                 switch (type) {
3906                 case L2CAP_CONF_RFC:
3907                         if (olen == sizeof(rfc))
3908                                 memcpy(&rfc, (void *)val, olen);
3909                         break;
3910                 case L2CAP_CONF_EWS:
3911                         txwin_ext = val;
3912                         break;
3913                 }
3914         }
3915
3916         switch (rfc.mode) {
3917         case L2CAP_MODE_ERTM:
3918                 chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
3919                 chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
3920                 chan->mps = le16_to_cpu(rfc.max_pdu_size);
3921                 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
3922                         chan->ack_win = min_t(u16, chan->ack_win, txwin_ext);
3923                 else
3924                         chan->ack_win = min_t(u16, chan->ack_win,
3925                                               rfc.txwin_size);
3926                 break;
3927         case L2CAP_MODE_STREAMING:
3928                 chan->mps    = le16_to_cpu(rfc.max_pdu_size);
3929         }
3930 }
3931
3932 static inline int l2cap_command_rej(struct l2cap_conn *conn,
3933                                     struct l2cap_cmd_hdr *cmd, u16 cmd_len,
3934                                     u8 *data)
3935 {
3936         struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data;
3937
3938         if (cmd_len < sizeof(*rej))
3939                 return -EPROTO;
3940
3941         if (rej->reason != L2CAP_REJ_NOT_UNDERSTOOD)
3942                 return 0;
3943
3944         if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
3945             cmd->ident == conn->info_ident) {
3946                 cancel_delayed_work(&conn->info_timer);
3947
3948                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3949                 conn->info_ident = 0;
3950
3951                 l2cap_conn_start(conn);
3952         }
3953
3954         return 0;
3955 }
3956
3957 static struct l2cap_chan *l2cap_connect(struct l2cap_conn *conn,
3958                                         struct l2cap_cmd_hdr *cmd,
3959                                         u8 *data, u8 rsp_code, u8 amp_id)
3960 {
3961         struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
3962         struct l2cap_conn_rsp rsp;
3963         struct l2cap_chan *chan = NULL, *pchan;
3964         int result, status = L2CAP_CS_NO_INFO;
3965
3966         u16 dcid = 0, scid = __le16_to_cpu(req->scid);
3967         __le16 psm = req->psm;
3968
3969         BT_DBG("psm 0x%2.2x scid 0x%4.4x", __le16_to_cpu(psm), scid);
3970
3971         /* Check if we have socket listening on psm */
3972         pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, &conn->hcon->src,
3973                                          &conn->hcon->dst, ACL_LINK);
3974         if (!pchan) {
3975                 result = L2CAP_CR_BAD_PSM;
3976                 goto sendresp;
3977         }
3978
3979         mutex_lock(&conn->chan_lock);
3980         l2cap_chan_lock(pchan);
3981
3982         /* Check if the ACL is secure enough (if not SDP) */
3983         if (psm != __constant_cpu_to_le16(L2CAP_PSM_SDP) &&
3984             !hci_conn_check_link_mode(conn->hcon)) {
3985                 conn->disc_reason = HCI_ERROR_AUTH_FAILURE;
3986                 result = L2CAP_CR_SEC_BLOCK;
3987                 goto response;
3988         }
3989
3990         result = L2CAP_CR_NO_MEM;
3991
3992         /* Check if we already have channel with that dcid */
3993         if (__l2cap_get_chan_by_dcid(conn, scid))
3994                 goto response;
3995
3996         chan = pchan->ops->new_connection(pchan);
3997         if (!chan)
3998                 goto response;
3999
4000         /* For certain devices (ex: HID mouse), support for authentication,
4001          * pairing and bonding is optional. For such devices, inorder to avoid
4002          * the ACL alive for too long after L2CAP disconnection, reset the ACL
4003          * disc_timeout back to HCI_DISCONN_TIMEOUT during L2CAP connect.
4004          */
4005         conn->hcon->disc_timeout = HCI_DISCONN_TIMEOUT;
4006
4007         bacpy(&chan->src, &conn->hcon->src);
4008         bacpy(&chan->dst, &conn->hcon->dst);
4009         chan->src_type = bdaddr_type(conn->hcon, conn->hcon->src_type);
4010         chan->dst_type = bdaddr_type(conn->hcon, conn->hcon->dst_type);
4011         chan->psm  = psm;
4012         chan->dcid = scid;
4013         chan->local_amp_id = amp_id;
4014
4015         __l2cap_chan_add(conn, chan);
4016
4017         dcid = chan->scid;
4018
4019         __set_chan_timer(chan, chan->ops->get_sndtimeo(chan));
4020
4021         chan->ident = cmd->ident;
4022
4023         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
4024                 if (l2cap_chan_check_security(chan)) {
4025                         if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
4026                                 l2cap_state_change(chan, BT_CONNECT2);
4027                                 result = L2CAP_CR_PEND;
4028                                 status = L2CAP_CS_AUTHOR_PEND;
4029                                 chan->ops->defer(chan);
4030                         } else {
4031                                 /* Force pending result for AMP controllers.
4032                                  * The connection will succeed after the
4033                                  * physical link is up.
4034                                  */
4035                                 if (amp_id == AMP_ID_BREDR) {
4036                                         l2cap_state_change(chan, BT_CONFIG);
4037                                         result = L2CAP_CR_SUCCESS;
4038                                 } else {
4039                                         l2cap_state_change(chan, BT_CONNECT2);
4040                                         result = L2CAP_CR_PEND;
4041                                 }
4042                                 status = L2CAP_CS_NO_INFO;
4043                         }
4044                 } else {
4045                         l2cap_state_change(chan, BT_CONNECT2);
4046                         result = L2CAP_CR_PEND;
4047                         status = L2CAP_CS_AUTHEN_PEND;
4048                 }
4049         } else {
4050                 l2cap_state_change(chan, BT_CONNECT2);
4051                 result = L2CAP_CR_PEND;
4052                 status = L2CAP_CS_NO_INFO;
4053         }
4054
4055 response:
4056         l2cap_chan_unlock(pchan);
4057         mutex_unlock(&conn->chan_lock);
4058
4059 sendresp:
4060         rsp.scid   = cpu_to_le16(scid);
4061         rsp.dcid   = cpu_to_le16(dcid);
4062         rsp.result = cpu_to_le16(result);
4063         rsp.status = cpu_to_le16(status);
4064         l2cap_send_cmd(conn, cmd->ident, rsp_code, sizeof(rsp), &rsp);
4065
4066         if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
4067                 struct l2cap_info_req info;
4068                 info.type = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
4069
4070                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
4071                 conn->info_ident = l2cap_get_ident(conn);
4072
4073                 schedule_delayed_work(&conn->info_timer, L2CAP_INFO_TIMEOUT);
4074
4075                 l2cap_send_cmd(conn, conn->info_ident, L2CAP_INFO_REQ,
4076                                sizeof(info), &info);
4077         }
4078
4079         if (chan && !test_bit(CONF_REQ_SENT, &chan->conf_state) &&
4080             result == L2CAP_CR_SUCCESS) {
4081                 u8 buf[128];
4082                 set_bit(CONF_REQ_SENT, &chan->conf_state);
4083                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
4084                                l2cap_build_conf_req(chan, buf), buf);
4085                 chan->num_conf_req++;
4086         }
4087
4088         return chan;
4089 }
4090
4091 static int l2cap_connect_req(struct l2cap_conn *conn,
4092                              struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
4093 {
4094         struct hci_dev *hdev = conn->hcon->hdev;
4095         struct hci_conn *hcon = conn->hcon;
4096
4097         if (cmd_len < sizeof(struct l2cap_conn_req))
4098                 return -EPROTO;
4099
4100         hci_dev_lock(hdev);
4101         if (test_bit(HCI_MGMT, &hdev->dev_flags) &&
4102             !test_and_set_bit(HCI_CONN_MGMT_CONNECTED, &hcon->flags))
4103                 mgmt_device_connected(hdev, &hcon->dst, hcon->type,
4104                                       hcon->dst_type, 0, NULL, 0,
4105                                       hcon->dev_class);
4106         hci_dev_unlock(hdev);
4107
4108         l2cap_connect(conn, cmd, data, L2CAP_CONN_RSP, 0);
4109         return 0;
4110 }
4111
4112 static int l2cap_connect_create_rsp(struct l2cap_conn *conn,
4113                                     struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4114                                     u8 *data)
4115 {
4116         struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
4117         u16 scid, dcid, result, status;
4118         struct l2cap_chan *chan;
4119         u8 req[128];
4120         int err;
4121
4122         if (cmd_len < sizeof(*rsp))
4123                 return -EPROTO;
4124
4125         scid   = __le16_to_cpu(rsp->scid);
4126         dcid   = __le16_to_cpu(rsp->dcid);
4127         result = __le16_to_cpu(rsp->result);
4128         status = __le16_to_cpu(rsp->status);
4129
4130         BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x",
4131                dcid, scid, result, status);
4132
4133         mutex_lock(&conn->chan_lock);
4134
4135         if (scid) {
4136                 chan = __l2cap_get_chan_by_scid(conn, scid);
4137                 if (!chan) {
4138                         err = -EBADSLT;
4139                         goto unlock;
4140                 }
4141         } else {
4142                 chan = __l2cap_get_chan_by_ident(conn, cmd->ident);
4143                 if (!chan) {
4144                         err = -EBADSLT;
4145                         goto unlock;
4146                 }
4147         }
4148
4149         err = 0;
4150
4151         l2cap_chan_lock(chan);
4152
4153         switch (result) {
4154         case L2CAP_CR_SUCCESS:
4155                 l2cap_state_change(chan, BT_CONFIG);
4156                 chan->ident = 0;
4157                 chan->dcid = dcid;
4158                 clear_bit(CONF_CONNECT_PEND, &chan->conf_state);
4159
4160                 if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
4161                         break;
4162
4163                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
4164                                l2cap_build_conf_req(chan, req), req);
4165                 chan->num_conf_req++;
4166                 break;
4167
4168         case L2CAP_CR_PEND:
4169                 set_bit(CONF_CONNECT_PEND, &chan->conf_state);
4170                 break;
4171
4172         default:
4173                 l2cap_chan_del(chan, ECONNREFUSED);
4174                 break;
4175         }
4176
4177         l2cap_chan_unlock(chan);
4178
4179 unlock:
4180         mutex_unlock(&conn->chan_lock);
4181
4182         return err;
4183 }
4184
4185 static inline void set_default_fcs(struct l2cap_chan *chan)
4186 {
4187         /* FCS is enabled only in ERTM or streaming mode, if one or both
4188          * sides request it.
4189          */
4190         if (chan->mode != L2CAP_MODE_ERTM && chan->mode != L2CAP_MODE_STREAMING)
4191                 chan->fcs = L2CAP_FCS_NONE;
4192         else if (!test_bit(CONF_RECV_NO_FCS, &chan->conf_state))
4193                 chan->fcs = L2CAP_FCS_CRC16;
4194 }
4195
4196 static void l2cap_send_efs_conf_rsp(struct l2cap_chan *chan, void *data,
4197                                     u8 ident, u16 flags)
4198 {
4199         struct l2cap_conn *conn = chan->conn;
4200
4201         BT_DBG("conn %p chan %p ident %d flags 0x%4.4x", conn, chan, ident,
4202                flags);
4203
4204         clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
4205         set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
4206
4207         l2cap_send_cmd(conn, ident, L2CAP_CONF_RSP,
4208                        l2cap_build_conf_rsp(chan, data,
4209                                             L2CAP_CONF_SUCCESS, flags), data);
4210 }
4211
4212 static void cmd_reject_invalid_cid(struct l2cap_conn *conn, u8 ident,
4213                                    u16 scid, u16 dcid)
4214 {
4215         struct l2cap_cmd_rej_cid rej;
4216
4217         rej.reason = __constant_cpu_to_le16(L2CAP_REJ_INVALID_CID);
4218         rej.scid = __cpu_to_le16(scid);
4219         rej.dcid = __cpu_to_le16(dcid);
4220
4221         l2cap_send_cmd(conn, ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
4222 }
4223
4224 static inline int l2cap_config_req(struct l2cap_conn *conn,
4225                                    struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4226                                    u8 *data)
4227 {
4228         struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
4229         u16 dcid, flags;
4230         u8 rsp[64];
4231         struct l2cap_chan *chan;
4232         int len, err = 0;
4233
4234         if (cmd_len < sizeof(*req))
4235                 return -EPROTO;
4236
4237         dcid  = __le16_to_cpu(req->dcid);
4238         flags = __le16_to_cpu(req->flags);
4239
4240         BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
4241
4242         chan = l2cap_get_chan_by_scid(conn, dcid);
4243         if (!chan) {
4244                 cmd_reject_invalid_cid(conn, cmd->ident, dcid, 0);
4245                 return 0;
4246         }
4247
4248         if (chan->state != BT_CONFIG && chan->state != BT_CONNECT2) {
4249                 cmd_reject_invalid_cid(conn, cmd->ident, chan->scid,
4250                                        chan->dcid);
4251                 goto unlock;
4252         }
4253
4254         /* Reject if config buffer is too small. */
4255         len = cmd_len - sizeof(*req);
4256         if (chan->conf_len + len > sizeof(chan->conf_req)) {
4257                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
4258                                l2cap_build_conf_rsp(chan, rsp,
4259                                L2CAP_CONF_REJECT, flags), rsp);
4260                 goto unlock;
4261         }
4262
4263         /* Store config. */
4264         memcpy(chan->conf_req + chan->conf_len, req->data, len);
4265         chan->conf_len += len;
4266
4267         if (flags & L2CAP_CONF_FLAG_CONTINUATION) {
4268                 /* Incomplete config. Send empty response. */
4269                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
4270                                l2cap_build_conf_rsp(chan, rsp,
4271                                L2CAP_CONF_SUCCESS, flags), rsp);
4272                 goto unlock;
4273         }
4274
4275         /* Complete config. */
4276         len = l2cap_parse_conf_req(chan, rsp);
4277         if (len < 0) {
4278                 l2cap_send_disconn_req(chan, ECONNRESET);
4279                 goto unlock;
4280         }
4281
4282         chan->ident = cmd->ident;
4283         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
4284         chan->num_conf_rsp++;
4285
4286         /* Reset config buffer. */
4287         chan->conf_len = 0;
4288
4289         if (!test_bit(CONF_OUTPUT_DONE, &chan->conf_state))
4290                 goto unlock;
4291
4292         if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
4293                 set_default_fcs(chan);
4294
4295                 if (chan->mode == L2CAP_MODE_ERTM ||
4296                     chan->mode == L2CAP_MODE_STREAMING)
4297                         err = l2cap_ertm_init(chan);
4298
4299                 if (err < 0)
4300                         l2cap_send_disconn_req(chan, -err);
4301                 else
4302                         l2cap_chan_ready(chan);
4303
4304                 goto unlock;
4305         }
4306
4307         if (!test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) {
4308                 u8 buf[64];
4309                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
4310                                l2cap_build_conf_req(chan, buf), buf);
4311                 chan->num_conf_req++;
4312         }
4313
4314         /* Got Conf Rsp PENDING from remote side and asume we sent
4315            Conf Rsp PENDING in the code above */
4316         if (test_bit(CONF_REM_CONF_PEND, &chan->conf_state) &&
4317             test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
4318
4319                 /* check compatibility */
4320
4321                 /* Send rsp for BR/EDR channel */
4322                 if (!chan->hs_hcon)
4323                         l2cap_send_efs_conf_rsp(chan, rsp, cmd->ident, flags);
4324                 else
4325                         chan->ident = cmd->ident;
4326         }
4327
4328 unlock:
4329         l2cap_chan_unlock(chan);
4330         return err;
4331 }
4332
4333 static inline int l2cap_config_rsp(struct l2cap_conn *conn,
4334                                    struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4335                                    u8 *data)
4336 {
4337         struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
4338         u16 scid, flags, result;
4339         struct l2cap_chan *chan;
4340         int len = cmd_len - sizeof(*rsp);
4341         int err = 0;
4342
4343         if (cmd_len < sizeof(*rsp))
4344                 return -EPROTO;
4345
4346         scid   = __le16_to_cpu(rsp->scid);
4347         flags  = __le16_to_cpu(rsp->flags);
4348         result = __le16_to_cpu(rsp->result);
4349
4350         BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x len %d", scid, flags,
4351                result, len);
4352
4353         chan = l2cap_get_chan_by_scid(conn, scid);
4354         if (!chan)
4355                 return 0;
4356
4357         switch (result) {
4358         case L2CAP_CONF_SUCCESS:
4359                 l2cap_conf_rfc_get(chan, rsp->data, len);
4360                 clear_bit(CONF_REM_CONF_PEND, &chan->conf_state);
4361                 break;
4362
4363         case L2CAP_CONF_PENDING:
4364                 set_bit(CONF_REM_CONF_PEND, &chan->conf_state);
4365
4366                 if (test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
4367                         char buf[64];
4368
4369                         len = l2cap_parse_conf_rsp(chan, rsp->data, len,
4370                                                    buf, &result);
4371                         if (len < 0) {
4372                                 l2cap_send_disconn_req(chan, ECONNRESET);
4373                                 goto done;
4374                         }
4375
4376                         if (!chan->hs_hcon) {
4377                                 l2cap_send_efs_conf_rsp(chan, buf, cmd->ident,
4378                                                         0);
4379                         } else {
4380                                 if (l2cap_check_efs(chan)) {
4381                                         amp_create_logical_link(chan);
4382                                         chan->ident = cmd->ident;
4383                                 }
4384                         }
4385                 }
4386                 goto done;
4387
4388         case L2CAP_CONF_UNACCEPT:
4389                 if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
4390                         char req[64];
4391
4392                         if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
4393                                 l2cap_send_disconn_req(chan, ECONNRESET);
4394                                 goto done;
4395                         }
4396
4397                         /* throw out any old stored conf requests */
4398                         result = L2CAP_CONF_SUCCESS;
4399                         len = l2cap_parse_conf_rsp(chan, rsp->data, len,
4400                                                    req, &result);
4401                         if (len < 0) {
4402                                 l2cap_send_disconn_req(chan, ECONNRESET);
4403                                 goto done;
4404                         }
4405
4406                         l2cap_send_cmd(conn, l2cap_get_ident(conn),
4407                                        L2CAP_CONF_REQ, len, req);
4408                         chan->num_conf_req++;
4409                         if (result != L2CAP_CONF_SUCCESS)
4410                                 goto done;
4411                         break;
4412                 }
4413
4414         default:
4415                 l2cap_chan_set_err(chan, ECONNRESET);
4416
4417                 __set_chan_timer(chan, L2CAP_DISC_REJ_TIMEOUT);
4418                 l2cap_send_disconn_req(chan, ECONNRESET);
4419                 goto done;
4420         }
4421
4422         if (flags & L2CAP_CONF_FLAG_CONTINUATION)
4423                 goto done;
4424
4425         set_bit(CONF_INPUT_DONE, &chan->conf_state);
4426
4427         if (test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) {
4428                 set_default_fcs(chan);
4429
4430                 if (chan->mode == L2CAP_MODE_ERTM ||
4431                     chan->mode == L2CAP_MODE_STREAMING)
4432                         err = l2cap_ertm_init(chan);
4433
4434                 if (err < 0)
4435                         l2cap_send_disconn_req(chan, -err);
4436                 else
4437                         l2cap_chan_ready(chan);
4438         }
4439
4440 done:
4441         l2cap_chan_unlock(chan);
4442         return err;
4443 }
4444
4445 static inline int l2cap_disconnect_req(struct l2cap_conn *conn,
4446                                        struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4447                                        u8 *data)
4448 {
4449         struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
4450         struct l2cap_disconn_rsp rsp;
4451         u16 dcid, scid;
4452         struct l2cap_chan *chan;
4453
4454         if (cmd_len != sizeof(*req))
4455                 return -EPROTO;
4456
4457         scid = __le16_to_cpu(req->scid);
4458         dcid = __le16_to_cpu(req->dcid);
4459
4460         BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
4461
4462         mutex_lock(&conn->chan_lock);
4463
4464         chan = __l2cap_get_chan_by_scid(conn, dcid);
4465         if (!chan) {
4466                 mutex_unlock(&conn->chan_lock);
4467                 cmd_reject_invalid_cid(conn, cmd->ident, dcid, scid);
4468                 return 0;
4469         }
4470
4471         l2cap_chan_lock(chan);
4472
4473         rsp.dcid = cpu_to_le16(chan->scid);
4474         rsp.scid = cpu_to_le16(chan->dcid);
4475         l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
4476
4477         chan->ops->set_shutdown(chan);
4478
4479         l2cap_chan_hold(chan);
4480         l2cap_chan_del(chan, ECONNRESET);
4481
4482         l2cap_chan_unlock(chan);
4483
4484         chan->ops->close(chan);
4485         l2cap_chan_put(chan);
4486
4487         mutex_unlock(&conn->chan_lock);
4488
4489         return 0;
4490 }
4491
4492 static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn,
4493                                        struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4494                                        u8 *data)
4495 {
4496         struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
4497         u16 dcid, scid;
4498         struct l2cap_chan *chan;
4499
4500         if (cmd_len != sizeof(*rsp))
4501                 return -EPROTO;
4502
4503         scid = __le16_to_cpu(rsp->scid);
4504         dcid = __le16_to_cpu(rsp->dcid);
4505
4506         BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
4507
4508         mutex_lock(&conn->chan_lock);
4509
4510         chan = __l2cap_get_chan_by_scid(conn, scid);
4511         if (!chan) {
4512                 mutex_unlock(&conn->chan_lock);
4513                 return 0;
4514         }
4515
4516         l2cap_chan_lock(chan);
4517
4518         l2cap_chan_hold(chan);
4519         l2cap_chan_del(chan, 0);
4520
4521         l2cap_chan_unlock(chan);
4522
4523         chan->ops->close(chan);
4524         l2cap_chan_put(chan);
4525
4526         mutex_unlock(&conn->chan_lock);
4527
4528         return 0;
4529 }
4530
4531 static inline int l2cap_information_req(struct l2cap_conn *conn,
4532                                         struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4533                                         u8 *data)
4534 {
4535         struct l2cap_info_req *req = (struct l2cap_info_req *) data;
4536         u16 type;
4537
4538         if (cmd_len != sizeof(*req))
4539                 return -EPROTO;
4540
4541         type = __le16_to_cpu(req->type);
4542
4543         BT_DBG("type 0x%4.4x", type);
4544
4545         if (type == L2CAP_IT_FEAT_MASK) {
4546                 u8 buf[8];
4547                 u32 feat_mask = l2cap_feat_mask;
4548                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
4549                 rsp->type   = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
4550                 rsp->result = __constant_cpu_to_le16(L2CAP_IR_SUCCESS);
4551                 if (!disable_ertm)
4552                         feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
4553                                 | L2CAP_FEAT_FCS;
4554                 if (conn->hs_enabled)
4555                         feat_mask |= L2CAP_FEAT_EXT_FLOW
4556                                 | L2CAP_FEAT_EXT_WINDOW;
4557
4558                 put_unaligned_le32(feat_mask, rsp->data);
4559                 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf),
4560                                buf);
4561         } else if (type == L2CAP_IT_FIXED_CHAN) {
4562                 u8 buf[12];
4563                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
4564
4565                 if (conn->hs_enabled)
4566                         l2cap_fixed_chan[0] |= L2CAP_FC_A2MP;
4567                 else
4568                         l2cap_fixed_chan[0] &= ~L2CAP_FC_A2MP;
4569
4570                 rsp->type   = __constant_cpu_to_le16(L2CAP_IT_FIXED_CHAN);
4571                 rsp->result = __constant_cpu_to_le16(L2CAP_IR_SUCCESS);
4572                 memcpy(rsp->data, l2cap_fixed_chan, sizeof(l2cap_fixed_chan));
4573                 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf),
4574                                buf);
4575         } else {
4576                 struct l2cap_info_rsp rsp;
4577                 rsp.type   = cpu_to_le16(type);
4578                 rsp.result = __constant_cpu_to_le16(L2CAP_IR_NOTSUPP);
4579                 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(rsp),
4580                                &rsp);
4581         }
4582
4583         return 0;
4584 }
4585
4586 static inline int l2cap_information_rsp(struct l2cap_conn *conn,
4587                                         struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4588                                         u8 *data)
4589 {
4590         struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
4591         u16 type, result;
4592
4593         if (cmd_len < sizeof(*rsp))
4594                 return -EPROTO;
4595
4596         type   = __le16_to_cpu(rsp->type);
4597         result = __le16_to_cpu(rsp->result);
4598
4599         BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
4600
4601         /* L2CAP Info req/rsp are unbound to channels, add extra checks */
4602         if (cmd->ident != conn->info_ident ||
4603             conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)
4604                 return 0;
4605
4606         cancel_delayed_work(&conn->info_timer);
4607
4608         if (result != L2CAP_IR_SUCCESS) {
4609                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4610                 conn->info_ident = 0;
4611
4612                 l2cap_conn_start(conn);
4613
4614                 return 0;
4615         }
4616
4617         switch (type) {
4618         case L2CAP_IT_FEAT_MASK:
4619                 conn->feat_mask = get_unaligned_le32(rsp->data);
4620
4621                 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
4622                         struct l2cap_info_req req;
4623                         req.type = __constant_cpu_to_le16(L2CAP_IT_FIXED_CHAN);
4624
4625                         conn->info_ident = l2cap_get_ident(conn);
4626
4627                         l2cap_send_cmd(conn, conn->info_ident,
4628                                        L2CAP_INFO_REQ, sizeof(req), &req);
4629                 } else {
4630                         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4631                         conn->info_ident = 0;
4632
4633                         l2cap_conn_start(conn);
4634                 }
4635                 break;
4636
4637         case L2CAP_IT_FIXED_CHAN:
4638                 conn->fixed_chan_mask = rsp->data[0];
4639                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4640                 conn->info_ident = 0;
4641
4642                 l2cap_conn_start(conn);
4643                 break;
4644         }
4645
4646         return 0;
4647 }
4648
4649 static int l2cap_create_channel_req(struct l2cap_conn *conn,
4650                                     struct l2cap_cmd_hdr *cmd,
4651                                     u16 cmd_len, void *data)
4652 {
4653         struct l2cap_create_chan_req *req = data;
4654         struct l2cap_create_chan_rsp rsp;
4655         struct l2cap_chan *chan;
4656         struct hci_dev *hdev;
4657         u16 psm, scid;
4658
4659         if (cmd_len != sizeof(*req))
4660                 return -EPROTO;
4661
4662         if (!conn->hs_enabled)
4663                 return -EINVAL;
4664
4665         psm = le16_to_cpu(req->psm);
4666         scid = le16_to_cpu(req->scid);
4667
4668         BT_DBG("psm 0x%2.2x, scid 0x%4.4x, amp_id %d", psm, scid, req->amp_id);
4669
4670         /* For controller id 0 make BR/EDR connection */
4671         if (req->amp_id == AMP_ID_BREDR) {
4672                 l2cap_connect(conn, cmd, data, L2CAP_CREATE_CHAN_RSP,
4673                               req->amp_id);
4674                 return 0;
4675         }
4676
4677         /* Validate AMP controller id */
4678         hdev = hci_dev_get(req->amp_id);
4679         if (!hdev)
4680                 goto error;
4681
4682         if (hdev->dev_type != HCI_AMP || !test_bit(HCI_UP, &hdev->flags)) {
4683                 hci_dev_put(hdev);
4684                 goto error;
4685         }
4686
4687         chan = l2cap_connect(conn, cmd, data, L2CAP_CREATE_CHAN_RSP,
4688                              req->amp_id);
4689         if (chan) {
4690                 struct amp_mgr *mgr = conn->hcon->amp_mgr;
4691                 struct hci_conn *hs_hcon;
4692
4693                 hs_hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK,
4694                                                   &conn->hcon->dst);
4695                 if (!hs_hcon) {
4696                         hci_dev_put(hdev);
4697                         cmd_reject_invalid_cid(conn, cmd->ident, chan->scid,
4698                                                chan->dcid);
4699                         return 0;
4700                 }
4701
4702                 BT_DBG("mgr %p bredr_chan %p hs_hcon %p", mgr, chan, hs_hcon);
4703
4704                 mgr->bredr_chan = chan;
4705                 chan->hs_hcon = hs_hcon;
4706                 chan->fcs = L2CAP_FCS_NONE;
4707                 conn->mtu = hdev->block_mtu;
4708         }
4709
4710         hci_dev_put(hdev);
4711
4712         return 0;
4713
4714 error:
4715         rsp.dcid = 0;
4716         rsp.scid = cpu_to_le16(scid);
4717         rsp.result = __constant_cpu_to_le16(L2CAP_CR_BAD_AMP);
4718         rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
4719
4720         l2cap_send_cmd(conn, cmd->ident, L2CAP_CREATE_CHAN_RSP,
4721                        sizeof(rsp), &rsp);
4722
4723         return 0;
4724 }
4725
4726 static void l2cap_send_move_chan_req(struct l2cap_chan *chan, u8 dest_amp_id)
4727 {
4728         struct l2cap_move_chan_req req;
4729         u8 ident;
4730
4731         BT_DBG("chan %p, dest_amp_id %d", chan, dest_amp_id);
4732
4733         ident = l2cap_get_ident(chan->conn);
4734         chan->ident = ident;
4735
4736         req.icid = cpu_to_le16(chan->scid);
4737         req.dest_amp_id = dest_amp_id;
4738
4739         l2cap_send_cmd(chan->conn, ident, L2CAP_MOVE_CHAN_REQ, sizeof(req),
4740                        &req);
4741
4742         __set_chan_timer(chan, L2CAP_MOVE_TIMEOUT);
4743 }
4744
4745 static void l2cap_send_move_chan_rsp(struct l2cap_chan *chan, u16 result)
4746 {
4747         struct l2cap_move_chan_rsp rsp;
4748
4749         BT_DBG("chan %p, result 0x%4.4x", chan, result);
4750
4751         rsp.icid = cpu_to_le16(chan->dcid);
4752         rsp.result = cpu_to_le16(result);
4753
4754         l2cap_send_cmd(chan->conn, chan->ident, L2CAP_MOVE_CHAN_RSP,
4755                        sizeof(rsp), &rsp);
4756 }
4757
4758 static void l2cap_send_move_chan_cfm(struct l2cap_chan *chan, u16 result)
4759 {
4760         struct l2cap_move_chan_cfm cfm;
4761
4762         BT_DBG("chan %p, result 0x%4.4x", chan, result);
4763
4764         chan->ident = l2cap_get_ident(chan->conn);
4765
4766         cfm.icid = cpu_to_le16(chan->scid);
4767         cfm.result = cpu_to_le16(result);
4768
4769         l2cap_send_cmd(chan->conn, chan->ident, L2CAP_MOVE_CHAN_CFM,
4770                        sizeof(cfm), &cfm);
4771
4772         __set_chan_timer(chan, L2CAP_MOVE_TIMEOUT);
4773 }
4774
4775 static void l2cap_send_move_chan_cfm_icid(struct l2cap_conn *conn, u16 icid)
4776 {
4777         struct l2cap_move_chan_cfm cfm;
4778
4779         BT_DBG("conn %p, icid 0x%4.4x", conn, icid);
4780
4781         cfm.icid = cpu_to_le16(icid);
4782         cfm.result = __constant_cpu_to_le16(L2CAP_MC_UNCONFIRMED);
4783
4784         l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_MOVE_CHAN_CFM,
4785                        sizeof(cfm), &cfm);
4786 }
4787
4788 static void l2cap_send_move_chan_cfm_rsp(struct l2cap_conn *conn, u8 ident,
4789                                          u16 icid)
4790 {
4791         struct l2cap_move_chan_cfm_rsp rsp;
4792
4793         BT_DBG("icid 0x%4.4x", icid);
4794
4795         rsp.icid = cpu_to_le16(icid);
4796         l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM_RSP, sizeof(rsp), &rsp);
4797 }
4798
4799 static void __release_logical_link(struct l2cap_chan *chan)
4800 {
4801         chan->hs_hchan = NULL;
4802         chan->hs_hcon = NULL;
4803
4804         /* Placeholder - release the logical link */
4805 }
4806
4807 static void l2cap_logical_fail(struct l2cap_chan *chan)
4808 {
4809         /* Logical link setup failed */
4810         if (chan->state != BT_CONNECTED) {
4811                 /* Create channel failure, disconnect */
4812                 l2cap_send_disconn_req(chan, ECONNRESET);
4813                 return;
4814         }
4815
4816         switch (chan->move_role) {
4817         case L2CAP_MOVE_ROLE_RESPONDER:
4818                 l2cap_move_done(chan);
4819                 l2cap_send_move_chan_rsp(chan, L2CAP_MR_NOT_SUPP);
4820                 break;
4821         case L2CAP_MOVE_ROLE_INITIATOR:
4822                 if (chan->move_state == L2CAP_MOVE_WAIT_LOGICAL_COMP ||
4823                     chan->move_state == L2CAP_MOVE_WAIT_LOGICAL_CFM) {
4824                         /* Remote has only sent pending or
4825                          * success responses, clean up
4826                          */
4827                         l2cap_move_done(chan);
4828                 }
4829
4830                 /* Other amp move states imply that the move
4831                  * has already aborted
4832                  */
4833                 l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
4834                 break;
4835         }
4836 }
4837
4838 static void l2cap_logical_finish_create(struct l2cap_chan *chan,
4839                                         struct hci_chan *hchan)
4840 {
4841         struct l2cap_conf_rsp rsp;
4842
4843         chan->hs_hchan = hchan;
4844         chan->hs_hcon->l2cap_data = chan->conn;
4845
4846         l2cap_send_efs_conf_rsp(chan, &rsp, chan->ident, 0);
4847
4848         if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
4849                 int err;
4850
4851                 set_default_fcs(chan);
4852
4853                 err = l2cap_ertm_init(chan);
4854                 if (err < 0)
4855                         l2cap_send_disconn_req(chan, -err);
4856                 else
4857                         l2cap_chan_ready(chan);
4858         }
4859 }
4860
4861 static void l2cap_logical_finish_move(struct l2cap_chan *chan,
4862                                       struct hci_chan *hchan)
4863 {
4864         chan->hs_hcon = hchan->conn;
4865         chan->hs_hcon->l2cap_data = chan->conn;
4866
4867         BT_DBG("move_state %d", chan->move_state);
4868
4869         switch (chan->move_state) {
4870         case L2CAP_MOVE_WAIT_LOGICAL_COMP:
4871                 /* Move confirm will be sent after a success
4872                  * response is received
4873                  */
4874                 chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
4875                 break;
4876         case L2CAP_MOVE_WAIT_LOGICAL_CFM:
4877                 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
4878                         chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
4879                 } else if (chan->move_role == L2CAP_MOVE_ROLE_INITIATOR) {
4880                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM_RSP;
4881                         l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
4882                 } else if (chan->move_role == L2CAP_MOVE_ROLE_RESPONDER) {
4883                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
4884                         l2cap_send_move_chan_rsp(chan, L2CAP_MR_SUCCESS);
4885                 }
4886                 break;
4887         default:
4888                 /* Move was not in expected state, free the channel */
4889                 __release_logical_link(chan);
4890
4891                 chan->move_state = L2CAP_MOVE_STABLE;
4892         }
4893 }
4894
4895 /* Call with chan locked */
4896 void l2cap_logical_cfm(struct l2cap_chan *chan, struct hci_chan *hchan,
4897                        u8 status)
4898 {
4899         BT_DBG("chan %p, hchan %p, status %d", chan, hchan, status);
4900
4901         if (status) {
4902                 l2cap_logical_fail(chan);
4903                 __release_logical_link(chan);
4904                 return;
4905         }
4906
4907         if (chan->state != BT_CONNECTED) {
4908                 /* Ignore logical link if channel is on BR/EDR */
4909                 if (chan->local_amp_id != AMP_ID_BREDR)
4910                         l2cap_logical_finish_create(chan, hchan);
4911         } else {
4912                 l2cap_logical_finish_move(chan, hchan);
4913         }
4914 }
4915
4916 void l2cap_move_start(struct l2cap_chan *chan)
4917 {
4918         BT_DBG("chan %p", chan);
4919
4920         if (chan->local_amp_id == AMP_ID_BREDR) {
4921                 if (chan->chan_policy != BT_CHANNEL_POLICY_AMP_PREFERRED)
4922                         return;
4923                 chan->move_role = L2CAP_MOVE_ROLE_INITIATOR;
4924                 chan->move_state = L2CAP_MOVE_WAIT_PREPARE;
4925                 /* Placeholder - start physical link setup */
4926         } else {
4927                 chan->move_role = L2CAP_MOVE_ROLE_INITIATOR;
4928                 chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
4929                 chan->move_id = 0;
4930                 l2cap_move_setup(chan);
4931                 l2cap_send_move_chan_req(chan, 0);
4932         }
4933 }
4934
4935 static void l2cap_do_create(struct l2cap_chan *chan, int result,
4936                             u8 local_amp_id, u8 remote_amp_id)
4937 {
4938         BT_DBG("chan %p state %s %u -> %u", chan, state_to_string(chan->state),
4939                local_amp_id, remote_amp_id);
4940
4941         chan->fcs = L2CAP_FCS_NONE;
4942
4943         /* Outgoing channel on AMP */
4944         if (chan->state == BT_CONNECT) {
4945                 if (result == L2CAP_CR_SUCCESS) {
4946                         chan->local_amp_id = local_amp_id;
4947                         l2cap_send_create_chan_req(chan, remote_amp_id);
4948                 } else {
4949                         /* Revert to BR/EDR connect */
4950                         l2cap_send_conn_req(chan);
4951                 }
4952
4953                 return;
4954         }
4955
4956         /* Incoming channel on AMP */
4957         if (__l2cap_no_conn_pending(chan)) {
4958                 struct l2cap_conn_rsp rsp;
4959                 char buf[128];
4960                 rsp.scid = cpu_to_le16(chan->dcid);
4961                 rsp.dcid = cpu_to_le16(chan->scid);
4962
4963                 if (result == L2CAP_CR_SUCCESS) {
4964                         /* Send successful response */
4965                         rsp.result = __constant_cpu_to_le16(L2CAP_CR_SUCCESS);
4966                         rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
4967                 } else {
4968                         /* Send negative response */
4969                         rsp.result = __constant_cpu_to_le16(L2CAP_CR_NO_MEM);
4970                         rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
4971                 }
4972
4973                 l2cap_send_cmd(chan->conn, chan->ident, L2CAP_CREATE_CHAN_RSP,
4974                                sizeof(rsp), &rsp);
4975
4976                 if (result == L2CAP_CR_SUCCESS) {
4977                         l2cap_state_change(chan, BT_CONFIG);
4978                         set_bit(CONF_REQ_SENT, &chan->conf_state);
4979                         l2cap_send_cmd(chan->conn, l2cap_get_ident(chan->conn),
4980                                        L2CAP_CONF_REQ,
4981                                        l2cap_build_conf_req(chan, buf), buf);
4982                         chan->num_conf_req++;
4983                 }
4984         }
4985 }
4986
4987 static void l2cap_do_move_initiate(struct l2cap_chan *chan, u8 local_amp_id,
4988                                    u8 remote_amp_id)
4989 {
4990         l2cap_move_setup(chan);
4991         chan->move_id = local_amp_id;
4992         chan->move_state = L2CAP_MOVE_WAIT_RSP;
4993
4994         l2cap_send_move_chan_req(chan, remote_amp_id);
4995 }
4996
4997 static void l2cap_do_move_respond(struct l2cap_chan *chan, int result)
4998 {
4999         struct hci_chan *hchan = NULL;
5000
5001         /* Placeholder - get hci_chan for logical link */
5002
5003         if (hchan) {
5004                 if (hchan->state == BT_CONNECTED) {
5005                         /* Logical link is ready to go */
5006                         chan->hs_hcon = hchan->conn;
5007                         chan->hs_hcon->l2cap_data = chan->conn;
5008                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
5009                         l2cap_send_move_chan_rsp(chan, L2CAP_MR_SUCCESS);
5010
5011                         l2cap_logical_cfm(chan, hchan, L2CAP_MR_SUCCESS);
5012                 } else {
5013                         /* Wait for logical link to be ready */
5014                         chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
5015                 }
5016         } else {
5017                 /* Logical link not available */
5018                 l2cap_send_move_chan_rsp(chan, L2CAP_MR_NOT_ALLOWED);
5019         }
5020 }
5021
5022 static void l2cap_do_move_cancel(struct l2cap_chan *chan, int result)
5023 {
5024         if (chan->move_role == L2CAP_MOVE_ROLE_RESPONDER) {
5025                 u8 rsp_result;
5026                 if (result == -EINVAL)
5027                         rsp_result = L2CAP_MR_BAD_ID;
5028                 else
5029                         rsp_result = L2CAP_MR_NOT_ALLOWED;
5030
5031                 l2cap_send_move_chan_rsp(chan, rsp_result);
5032         }
5033
5034         chan->move_role = L2CAP_MOVE_ROLE_NONE;
5035         chan->move_state = L2CAP_MOVE_STABLE;
5036
5037         /* Restart data transmission */
5038         l2cap_ertm_send(chan);
5039 }
5040
5041 /* Invoke with locked chan */
5042 void __l2cap_physical_cfm(struct l2cap_chan *chan, int result)
5043 {
5044         u8 local_amp_id = chan->local_amp_id;
5045         u8 remote_amp_id = chan->remote_amp_id;
5046
5047         BT_DBG("chan %p, result %d, local_amp_id %d, remote_amp_id %d",
5048                chan, result, local_amp_id, remote_amp_id);
5049
5050         if (chan->state == BT_DISCONN || chan->state == BT_CLOSED) {
5051                 l2cap_chan_unlock(chan);
5052                 return;
5053         }
5054
5055         if (chan->state != BT_CONNECTED) {
5056                 l2cap_do_create(chan, result, local_amp_id, remote_amp_id);
5057         } else if (result != L2CAP_MR_SUCCESS) {
5058                 l2cap_do_move_cancel(chan, result);
5059         } else {
5060                 switch (chan->move_role) {
5061                 case L2CAP_MOVE_ROLE_INITIATOR:
5062                         l2cap_do_move_initiate(chan, local_amp_id,
5063                                                remote_amp_id);
5064                         break;
5065                 case L2CAP_MOVE_ROLE_RESPONDER:
5066                         l2cap_do_move_respond(chan, result);
5067                         break;
5068                 default:
5069                         l2cap_do_move_cancel(chan, result);
5070                         break;
5071                 }
5072         }
5073 }
5074
5075 static inline int l2cap_move_channel_req(struct l2cap_conn *conn,
5076                                          struct l2cap_cmd_hdr *cmd,
5077                                          u16 cmd_len, void *data)
5078 {
5079         struct l2cap_move_chan_req *req = data;
5080         struct l2cap_move_chan_rsp rsp;
5081         struct l2cap_chan *chan;
5082         u16 icid = 0;
5083         u16 result = L2CAP_MR_NOT_ALLOWED;
5084
5085         if (cmd_len != sizeof(*req))
5086                 return -EPROTO;
5087
5088         icid = le16_to_cpu(req->icid);
5089
5090         BT_DBG("icid 0x%4.4x, dest_amp_id %d", icid, req->dest_amp_id);
5091
5092         if (!conn->hs_enabled)
5093                 return -EINVAL;
5094
5095         chan = l2cap_get_chan_by_dcid(conn, icid);
5096         if (!chan) {
5097                 rsp.icid = cpu_to_le16(icid);
5098                 rsp.result = __constant_cpu_to_le16(L2CAP_MR_NOT_ALLOWED);
5099                 l2cap_send_cmd(conn, cmd->ident, L2CAP_MOVE_CHAN_RSP,
5100                                sizeof(rsp), &rsp);
5101                 return 0;
5102         }
5103
5104         chan->ident = cmd->ident;
5105
5106         if (chan->scid < L2CAP_CID_DYN_START ||
5107             chan->chan_policy == BT_CHANNEL_POLICY_BREDR_ONLY ||
5108             (chan->mode != L2CAP_MODE_ERTM &&
5109              chan->mode != L2CAP_MODE_STREAMING)) {
5110                 result = L2CAP_MR_NOT_ALLOWED;
5111                 goto send_move_response;
5112         }
5113
5114         if (chan->local_amp_id == req->dest_amp_id) {
5115                 result = L2CAP_MR_SAME_ID;
5116                 goto send_move_response;
5117         }
5118
5119         if (req->dest_amp_id != AMP_ID_BREDR) {
5120                 struct hci_dev *hdev;
5121                 hdev = hci_dev_get(req->dest_amp_id);
5122                 if (!hdev || hdev->dev_type != HCI_AMP ||
5123                     !test_bit(HCI_UP, &hdev->flags)) {
5124                         if (hdev)
5125                                 hci_dev_put(hdev);
5126
5127                         result = L2CAP_MR_BAD_ID;
5128                         goto send_move_response;
5129                 }
5130                 hci_dev_put(hdev);
5131         }
5132
5133         /* Detect a move collision.  Only send a collision response
5134          * if this side has "lost", otherwise proceed with the move.
5135          * The winner has the larger bd_addr.
5136          */
5137         if ((__chan_is_moving(chan) ||
5138              chan->move_role != L2CAP_MOVE_ROLE_NONE) &&
5139             bacmp(&conn->hcon->src, &conn->hcon->dst) > 0) {
5140                 result = L2CAP_MR_COLLISION;
5141                 goto send_move_response;
5142         }
5143
5144         chan->move_role = L2CAP_MOVE_ROLE_RESPONDER;
5145         l2cap_move_setup(chan);
5146         chan->move_id = req->dest_amp_id;
5147         icid = chan->dcid;
5148
5149         if (req->dest_amp_id == AMP_ID_BREDR) {
5150                 /* Moving to BR/EDR */
5151                 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
5152                         chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
5153                         result = L2CAP_MR_PEND;
5154                 } else {
5155                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
5156                         result = L2CAP_MR_SUCCESS;
5157                 }
5158         } else {
5159                 chan->move_state = L2CAP_MOVE_WAIT_PREPARE;
5160                 /* Placeholder - uncomment when amp functions are available */
5161                 /*amp_accept_physical(chan, req->dest_amp_id);*/
5162                 result = L2CAP_MR_PEND;
5163         }
5164
5165 send_move_response:
5166         l2cap_send_move_chan_rsp(chan, result);
5167
5168         l2cap_chan_unlock(chan);
5169
5170         return 0;
5171 }
5172
5173 static void l2cap_move_continue(struct l2cap_conn *conn, u16 icid, u16 result)
5174 {
5175         struct l2cap_chan *chan;
5176         struct hci_chan *hchan = NULL;
5177
5178         chan = l2cap_get_chan_by_scid(conn, icid);
5179         if (!chan) {
5180                 l2cap_send_move_chan_cfm_icid(conn, icid);
5181                 return;
5182         }
5183
5184         __clear_chan_timer(chan);
5185         if (result == L2CAP_MR_PEND)
5186                 __set_chan_timer(chan, L2CAP_MOVE_ERTX_TIMEOUT);
5187
5188         switch (chan->move_state) {
5189         case L2CAP_MOVE_WAIT_LOGICAL_COMP:
5190                 /* Move confirm will be sent when logical link
5191                  * is complete.
5192                  */
5193                 chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
5194                 break;
5195         case L2CAP_MOVE_WAIT_RSP_SUCCESS:
5196                 if (result == L2CAP_MR_PEND) {
5197                         break;
5198                 } else if (test_bit(CONN_LOCAL_BUSY,
5199                                     &chan->conn_state)) {
5200                         chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
5201                 } else {
5202                         /* Logical link is up or moving to BR/EDR,
5203                          * proceed with move
5204                          */
5205                         chan->move_state = L2CAP_MOVE_WAIT_CONFIRM_RSP;
5206                         l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
5207                 }
5208                 break;
5209         case L2CAP_MOVE_WAIT_RSP:
5210                 /* Moving to AMP */
5211                 if (result == L2CAP_MR_SUCCESS) {
5212                         /* Remote is ready, send confirm immediately
5213                          * after logical link is ready
5214                          */
5215                         chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
5216                 } else {
5217                         /* Both logical link and move success
5218                          * are required to confirm
5219                          */
5220                         chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_COMP;
5221                 }
5222
5223                 /* Placeholder - get hci_chan for logical link */
5224                 if (!hchan) {
5225                         /* Logical link not available */
5226                         l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
5227                         break;
5228                 }
5229
5230                 /* If the logical link is not yet connected, do not
5231                  * send confirmation.
5232                  */
5233                 if (hchan->state != BT_CONNECTED)
5234                         break;
5235
5236                 /* Logical link is already ready to go */
5237
5238                 chan->hs_hcon = hchan->conn;
5239                 chan->hs_hcon->l2cap_data = chan->conn;
5240
5241                 if (result == L2CAP_MR_SUCCESS) {
5242                         /* Can confirm now */
5243                         l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
5244                 } else {
5245                         /* Now only need move success
5246                          * to confirm
5247                          */
5248                         chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
5249                 }
5250
5251                 l2cap_logical_cfm(chan, hchan, L2CAP_MR_SUCCESS);
5252                 break;
5253         default:
5254                 /* Any other amp move state means the move failed. */
5255                 chan->move_id = chan->local_amp_id;
5256                 l2cap_move_done(chan);
5257                 l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
5258         }
5259
5260         l2cap_chan_unlock(chan);
5261 }
5262
5263 static void l2cap_move_fail(struct l2cap_conn *conn, u8 ident, u16 icid,
5264                             u16 result)
5265 {
5266         struct l2cap_chan *chan;
5267
5268         chan = l2cap_get_chan_by_ident(conn, ident);
5269         if (!chan) {
5270                 /* Could not locate channel, icid is best guess */
5271                 l2cap_send_move_chan_cfm_icid(conn, icid);
5272                 return;
5273         }
5274
5275         __clear_chan_timer(chan);
5276
5277         if (chan->move_role == L2CAP_MOVE_ROLE_INITIATOR) {
5278                 if (result == L2CAP_MR_COLLISION) {
5279                         chan->move_role = L2CAP_MOVE_ROLE_RESPONDER;
5280                 } else {
5281                         /* Cleanup - cancel move */
5282                         chan->move_id = chan->local_amp_id;
5283                         l2cap_move_done(chan);
5284                 }
5285         }
5286
5287         l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
5288
5289         l2cap_chan_unlock(chan);
5290 }
5291
5292 static int l2cap_move_channel_rsp(struct l2cap_conn *conn,
5293                                   struct l2cap_cmd_hdr *cmd,
5294                                   u16 cmd_len, void *data)
5295 {
5296         struct l2cap_move_chan_rsp *rsp = data;
5297         u16 icid, result;
5298
5299         if (cmd_len != sizeof(*rsp))
5300                 return -EPROTO;
5301
5302         icid = le16_to_cpu(rsp->icid);
5303         result = le16_to_cpu(rsp->result);
5304
5305         BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result);
5306
5307         if (result == L2CAP_MR_SUCCESS || result == L2CAP_MR_PEND)
5308                 l2cap_move_continue(conn, icid, result);
5309         else
5310                 l2cap_move_fail(conn, cmd->ident, icid, result);
5311
5312         return 0;
5313 }
5314
5315 static int l2cap_move_channel_confirm(struct l2cap_conn *conn,
5316                                       struct l2cap_cmd_hdr *cmd,
5317                                       u16 cmd_len, void *data)
5318 {
5319         struct l2cap_move_chan_cfm *cfm = data;
5320         struct l2cap_chan *chan;
5321         u16 icid, result;
5322
5323         if (cmd_len != sizeof(*cfm))
5324                 return -EPROTO;
5325
5326         icid = le16_to_cpu(cfm->icid);
5327         result = le16_to_cpu(cfm->result);
5328
5329         BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result);
5330
5331         chan = l2cap_get_chan_by_dcid(conn, icid);
5332         if (!chan) {
5333                 /* Spec requires a response even if the icid was not found */
5334                 l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid);
5335                 return 0;
5336         }
5337
5338         if (chan->move_state == L2CAP_MOVE_WAIT_CONFIRM) {
5339                 if (result == L2CAP_MC_CONFIRMED) {
5340                         chan->local_amp_id = chan->move_id;
5341                         if (chan->local_amp_id == AMP_ID_BREDR)
5342                                 __release_logical_link(chan);
5343                 } else {
5344                         chan->move_id = chan->local_amp_id;
5345                 }
5346
5347                 l2cap_move_done(chan);
5348         }
5349
5350         l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid);
5351
5352         l2cap_chan_unlock(chan);
5353
5354         return 0;
5355 }
5356
5357 static inline int l2cap_move_channel_confirm_rsp(struct l2cap_conn *conn,
5358                                                  struct l2cap_cmd_hdr *cmd,
5359                                                  u16 cmd_len, void *data)
5360 {
5361         struct l2cap_move_chan_cfm_rsp *rsp = data;
5362         struct l2cap_chan *chan;
5363         u16 icid;
5364
5365         if (cmd_len != sizeof(*rsp))
5366                 return -EPROTO;
5367
5368         icid = le16_to_cpu(rsp->icid);
5369
5370         BT_DBG("icid 0x%4.4x", icid);
5371
5372         chan = l2cap_get_chan_by_scid(conn, icid);
5373         if (!chan)
5374                 return 0;
5375
5376         __clear_chan_timer(chan);
5377
5378         if (chan->move_state == L2CAP_MOVE_WAIT_CONFIRM_RSP) {
5379                 chan->local_amp_id = chan->move_id;
5380
5381                 if (chan->local_amp_id == AMP_ID_BREDR && chan->hs_hchan)
5382                         __release_logical_link(chan);
5383
5384                 l2cap_move_done(chan);
5385         }
5386
5387         l2cap_chan_unlock(chan);
5388
5389         return 0;
5390 }
5391
5392 static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency,
5393                                          u16 to_multiplier)
5394 {
5395         u16 max_latency;
5396
5397         if (min > max || min < 6 || max > 3200)
5398                 return -EINVAL;
5399
5400         if (to_multiplier < 10 || to_multiplier > 3200)
5401                 return -EINVAL;
5402
5403         if (max >= to_multiplier * 8)
5404                 return -EINVAL;
5405
5406         max_latency = (to_multiplier * 8 / max) - 1;
5407         if (latency > 499 || latency > max_latency)
5408                 return -EINVAL;
5409
5410         return 0;
5411 }
5412
5413 static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
5414                                               struct l2cap_cmd_hdr *cmd,
5415                                               u16 cmd_len, u8 *data)
5416 {
5417         struct hci_conn *hcon = conn->hcon;
5418         struct l2cap_conn_param_update_req *req;
5419         struct l2cap_conn_param_update_rsp rsp;
5420         u16 min, max, latency, to_multiplier;
5421         int err;
5422
5423         if (!(hcon->link_mode & HCI_LM_MASTER))
5424                 return -EINVAL;
5425
5426         if (cmd_len != sizeof(struct l2cap_conn_param_update_req))
5427                 return -EPROTO;
5428
5429         req = (struct l2cap_conn_param_update_req *) data;
5430         min             = __le16_to_cpu(req->min);
5431         max             = __le16_to_cpu(req->max);
5432         latency         = __le16_to_cpu(req->latency);
5433         to_multiplier   = __le16_to_cpu(req->to_multiplier);
5434
5435         BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x",
5436                min, max, latency, to_multiplier);
5437
5438         memset(&rsp, 0, sizeof(rsp));
5439
5440         err = l2cap_check_conn_param(min, max, latency, to_multiplier);
5441         if (err)
5442                 rsp.result = __constant_cpu_to_le16(L2CAP_CONN_PARAM_REJECTED);
5443         else
5444                 rsp.result = __constant_cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED);
5445
5446         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP,
5447                        sizeof(rsp), &rsp);
5448
5449         if (!err)
5450                 hci_le_conn_update(hcon, min, max, latency, to_multiplier);
5451
5452         return 0;
5453 }
5454
5455 static int l2cap_le_connect_rsp(struct l2cap_conn *conn,
5456                                 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5457                                 u8 *data)
5458 {
5459         struct l2cap_le_conn_rsp *rsp = (struct l2cap_le_conn_rsp *) data;
5460         u16 dcid, mtu, mps, credits, result;
5461         struct l2cap_chan *chan;
5462         int err;
5463
5464         if (cmd_len < sizeof(*rsp))
5465                 return -EPROTO;
5466
5467         dcid    = __le16_to_cpu(rsp->dcid);
5468         mtu     = __le16_to_cpu(rsp->mtu);
5469         mps     = __le16_to_cpu(rsp->mps);
5470         credits = __le16_to_cpu(rsp->credits);
5471         result  = __le16_to_cpu(rsp->result);
5472
5473         if (result == L2CAP_CR_SUCCESS && (mtu < 23 || mps < 23))
5474                 return -EPROTO;
5475
5476         BT_DBG("dcid 0x%4.4x mtu %u mps %u credits %u result 0x%2.2x",
5477                dcid, mtu, mps, credits, result);
5478
5479         mutex_lock(&conn->chan_lock);
5480
5481         chan = __l2cap_get_chan_by_ident(conn, cmd->ident);
5482         if (!chan) {
5483                 err = -EBADSLT;
5484                 goto unlock;
5485         }
5486
5487         err = 0;
5488
5489         l2cap_chan_lock(chan);
5490
5491         switch (result) {
5492         case L2CAP_CR_SUCCESS:
5493                 chan->ident = 0;
5494                 chan->dcid = dcid;
5495                 chan->omtu = mtu;
5496                 chan->remote_mps = mps;
5497                 chan->tx_credits = credits;
5498                 l2cap_chan_ready(chan);
5499                 break;
5500
5501         default:
5502                 l2cap_chan_del(chan, ECONNREFUSED);
5503                 break;
5504         }
5505
5506         l2cap_chan_unlock(chan);
5507
5508 unlock:
5509         mutex_unlock(&conn->chan_lock);
5510
5511         return err;
5512 }
5513
5514 static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
5515                                       struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5516                                       u8 *data)
5517 {
5518         int err = 0;
5519
5520         switch (cmd->code) {
5521         case L2CAP_COMMAND_REJ:
5522                 l2cap_command_rej(conn, cmd, cmd_len, data);
5523                 break;
5524
5525         case L2CAP_CONN_REQ:
5526                 err = l2cap_connect_req(conn, cmd, cmd_len, data);
5527                 break;
5528
5529         case L2CAP_CONN_RSP:
5530         case L2CAP_CREATE_CHAN_RSP:
5531                 l2cap_connect_create_rsp(conn, cmd, cmd_len, data);
5532                 break;
5533
5534         case L2CAP_CONF_REQ:
5535                 err = l2cap_config_req(conn, cmd, cmd_len, data);
5536                 break;
5537
5538         case L2CAP_CONF_RSP:
5539                 l2cap_config_rsp(conn, cmd, cmd_len, data);
5540                 break;
5541
5542         case L2CAP_DISCONN_REQ:
5543                 err = l2cap_disconnect_req(conn, cmd, cmd_len, data);
5544                 break;
5545
5546         case L2CAP_DISCONN_RSP:
5547                 l2cap_disconnect_rsp(conn, cmd, cmd_len, data);
5548                 break;
5549
5550         case L2CAP_ECHO_REQ:
5551                 l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data);
5552                 break;
5553
5554         case L2CAP_ECHO_RSP:
5555                 break;
5556
5557         case L2CAP_INFO_REQ:
5558                 err = l2cap_information_req(conn, cmd, cmd_len, data);
5559                 break;
5560
5561         case L2CAP_INFO_RSP:
5562                 l2cap_information_rsp(conn, cmd, cmd_len, data);
5563                 break;
5564
5565         case L2CAP_CREATE_CHAN_REQ:
5566                 err = l2cap_create_channel_req(conn, cmd, cmd_len, data);
5567                 break;
5568
5569         case L2CAP_MOVE_CHAN_REQ:
5570                 err = l2cap_move_channel_req(conn, cmd, cmd_len, data);
5571                 break;
5572
5573         case L2CAP_MOVE_CHAN_RSP:
5574                 l2cap_move_channel_rsp(conn, cmd, cmd_len, data);
5575                 break;
5576
5577         case L2CAP_MOVE_CHAN_CFM:
5578                 err = l2cap_move_channel_confirm(conn, cmd, cmd_len, data);
5579                 break;
5580
5581         case L2CAP_MOVE_CHAN_CFM_RSP:
5582                 l2cap_move_channel_confirm_rsp(conn, cmd, cmd_len, data);
5583                 break;
5584
5585         default:
5586                 BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code);
5587                 err = -EINVAL;
5588                 break;
5589         }
5590
5591         return err;
5592 }
5593
5594 static int l2cap_le_connect_req(struct l2cap_conn *conn,
5595                                 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5596                                 u8 *data)
5597 {
5598         struct l2cap_le_conn_req *req = (struct l2cap_le_conn_req *) data;
5599         struct l2cap_le_conn_rsp rsp;
5600         struct l2cap_chan *chan, *pchan;
5601         u16 dcid, scid, credits, mtu, mps;
5602         __le16 psm;
5603         u8 result;
5604
5605         if (cmd_len != sizeof(*req))
5606                 return -EPROTO;
5607
5608         scid = __le16_to_cpu(req->scid);
5609         mtu  = __le16_to_cpu(req->mtu);
5610         mps  = __le16_to_cpu(req->mps);
5611         psm  = req->psm;
5612         dcid = 0;
5613         credits = 0;
5614
5615         if (mtu < 23 || mps < 23)
5616                 return -EPROTO;
5617
5618         BT_DBG("psm 0x%2.2x scid 0x%4.4x mtu %u mps %u", __le16_to_cpu(psm),
5619                scid, mtu, mps);
5620
5621         /* Check if we have socket listening on psm */
5622         pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, &conn->hcon->src,
5623                                          &conn->hcon->dst, LE_LINK);
5624         if (!pchan) {
5625                 result = L2CAP_CR_BAD_PSM;
5626                 chan = NULL;
5627                 goto response;
5628         }
5629
5630         mutex_lock(&conn->chan_lock);
5631         l2cap_chan_lock(pchan);
5632
5633         if (!smp_sufficient_security(conn->hcon, pchan->sec_level)) {
5634                 result = L2CAP_CR_AUTHENTICATION;
5635                 chan = NULL;
5636                 goto response_unlock;
5637         }
5638
5639         /* Check if we already have channel with that dcid */
5640         if (__l2cap_get_chan_by_dcid(conn, scid)) {
5641                 result = L2CAP_CR_NO_MEM;
5642                 chan = NULL;
5643                 goto response_unlock;
5644         }
5645
5646         chan = pchan->ops->new_connection(pchan);
5647         if (!chan) {
5648                 result = L2CAP_CR_NO_MEM;
5649                 goto response_unlock;
5650         }
5651
5652         l2cap_le_flowctl_init(chan);
5653
5654         bacpy(&chan->src, &conn->hcon->src);
5655         bacpy(&chan->dst, &conn->hcon->dst);
5656         chan->src_type = bdaddr_type(conn->hcon, conn->hcon->src_type);
5657         chan->dst_type = bdaddr_type(conn->hcon, conn->hcon->dst_type);
5658         chan->psm  = psm;
5659         chan->dcid = scid;
5660         chan->omtu = mtu;
5661         chan->remote_mps = mps;
5662         chan->tx_credits = __le16_to_cpu(req->credits);
5663
5664         __l2cap_chan_add(conn, chan);
5665         dcid = chan->scid;
5666         credits = chan->rx_credits;
5667
5668         __set_chan_timer(chan, chan->ops->get_sndtimeo(chan));
5669
5670         chan->ident = cmd->ident;
5671
5672         if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
5673                 l2cap_state_change(chan, BT_CONNECT2);
5674                 result = L2CAP_CR_PEND;
5675                 chan->ops->defer(chan);
5676         } else {
5677                 l2cap_chan_ready(chan);
5678                 result = L2CAP_CR_SUCCESS;
5679         }
5680
5681 response_unlock:
5682         l2cap_chan_unlock(pchan);
5683         mutex_unlock(&conn->chan_lock);
5684
5685         if (result == L2CAP_CR_PEND)
5686                 return 0;
5687
5688 response:
5689         if (chan) {
5690                 rsp.mtu = cpu_to_le16(chan->imtu);
5691                 rsp.mps = cpu_to_le16(chan->mps);
5692         } else {
5693                 rsp.mtu = 0;
5694                 rsp.mps = 0;
5695         }
5696
5697         rsp.dcid    = cpu_to_le16(dcid);
5698         rsp.credits = cpu_to_le16(credits);
5699         rsp.result  = cpu_to_le16(result);
5700
5701         l2cap_send_cmd(conn, cmd->ident, L2CAP_LE_CONN_RSP, sizeof(rsp), &rsp);
5702
5703         return 0;
5704 }
5705
5706 static inline int l2cap_le_credits(struct l2cap_conn *conn,
5707                                    struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5708                                    u8 *data)
5709 {
5710         struct l2cap_le_credits *pkt;
5711         struct l2cap_chan *chan;
5712         u16 cid, credits;
5713
5714         if (cmd_len != sizeof(*pkt))
5715                 return -EPROTO;
5716
5717         pkt = (struct l2cap_le_credits *) data;
5718         cid     = __le16_to_cpu(pkt->cid);
5719         credits = __le16_to_cpu(pkt->credits);
5720
5721         BT_DBG("cid 0x%4.4x credits 0x%4.4x", cid, credits);
5722
5723         chan = l2cap_get_chan_by_dcid(conn, cid);
5724         if (!chan)
5725                 return -EBADSLT;
5726
5727         chan->tx_credits += credits;
5728
5729         while (chan->tx_credits && !skb_queue_empty(&chan->tx_q)) {
5730                 l2cap_do_send(chan, skb_dequeue(&chan->tx_q));
5731                 chan->tx_credits--;
5732         }
5733
5734         if (chan->tx_credits)
5735                 chan->ops->resume(chan);
5736
5737         l2cap_chan_unlock(chan);
5738
5739         return 0;
5740 }
5741
5742 static inline int l2cap_le_command_rej(struct l2cap_conn *conn,
5743                                        struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5744                                        u8 *data)
5745 {
5746         struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data;
5747         struct l2cap_chan *chan;
5748
5749         if (cmd_len < sizeof(*rej))
5750                 return -EPROTO;
5751
5752         mutex_lock(&conn->chan_lock);
5753
5754         chan = __l2cap_get_chan_by_ident(conn, cmd->ident);
5755         if (!chan)
5756                 goto done;
5757
5758         l2cap_chan_lock(chan);
5759         l2cap_chan_del(chan, ECONNREFUSED);
5760         l2cap_chan_unlock(chan);
5761
5762 done:
5763         mutex_unlock(&conn->chan_lock);
5764         return 0;
5765 }
5766
5767 static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
5768                                    struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5769                                    u8 *data)
5770 {
5771         int err = 0;
5772
5773         if (!enable_lecoc) {
5774                 switch (cmd->code) {
5775                 case L2CAP_LE_CONN_REQ:
5776                 case L2CAP_LE_CONN_RSP:
5777                 case L2CAP_LE_CREDITS:
5778                 case L2CAP_DISCONN_REQ:
5779                 case L2CAP_DISCONN_RSP:
5780                         return -EINVAL;
5781                 }
5782         }
5783
5784         switch (cmd->code) {
5785         case L2CAP_COMMAND_REJ:
5786                 l2cap_le_command_rej(conn, cmd, cmd_len, data);
5787                 break;
5788
5789         case L2CAP_CONN_PARAM_UPDATE_REQ:
5790                 err = l2cap_conn_param_update_req(conn, cmd, cmd_len, data);
5791                 break;
5792
5793         case L2CAP_CONN_PARAM_UPDATE_RSP:
5794                 break;
5795
5796         case L2CAP_LE_CONN_RSP:
5797                 l2cap_le_connect_rsp(conn, cmd, cmd_len, data);
5798                 break;
5799
5800         case L2CAP_LE_CONN_REQ:
5801                 err = l2cap_le_connect_req(conn, cmd, cmd_len, data);
5802                 break;
5803
5804         case L2CAP_LE_CREDITS:
5805                 err = l2cap_le_credits(conn, cmd, cmd_len, data);
5806                 break;
5807
5808         case L2CAP_DISCONN_REQ:
5809                 err = l2cap_disconnect_req(conn, cmd, cmd_len, data);
5810                 break;
5811
5812         case L2CAP_DISCONN_RSP:
5813                 l2cap_disconnect_rsp(conn, cmd, cmd_len, data);
5814                 break;
5815
5816         default:
5817                 BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code);
5818                 err = -EINVAL;
5819                 break;
5820         }
5821
5822         return err;
5823 }
5824
5825 static inline void l2cap_le_sig_channel(struct l2cap_conn *conn,
5826                                         struct sk_buff *skb)
5827 {
5828         struct hci_conn *hcon = conn->hcon;
5829         struct l2cap_cmd_hdr *cmd;
5830         u16 len;
5831         int err;
5832
5833         if (hcon->type != LE_LINK)
5834                 goto drop;
5835
5836         if (skb->len < L2CAP_CMD_HDR_SIZE)
5837                 goto drop;
5838
5839         cmd = (void *) skb->data;
5840         skb_pull(skb, L2CAP_CMD_HDR_SIZE);
5841
5842         len = le16_to_cpu(cmd->len);
5843
5844         BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd->code, len, cmd->ident);
5845
5846         if (len != skb->len || !cmd->ident) {
5847                 BT_DBG("corrupted command");
5848                 goto drop;
5849         }
5850
5851         err = l2cap_le_sig_cmd(conn, cmd, len, skb->data);
5852         if (err) {
5853                 struct l2cap_cmd_rej_unk rej;
5854
5855                 BT_ERR("Wrong link type (%d)", err);
5856
5857                 rej.reason = __constant_cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
5858                 l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
5859                                sizeof(rej), &rej);
5860         }
5861
5862 drop:
5863         kfree_skb(skb);
5864 }
5865
5866 static inline void l2cap_sig_channel(struct l2cap_conn *conn,
5867                                      struct sk_buff *skb)
5868 {
5869         struct hci_conn *hcon = conn->hcon;
5870         u8 *data = skb->data;
5871         int len = skb->len;
5872         struct l2cap_cmd_hdr cmd;
5873         int err;
5874
5875         l2cap_raw_recv(conn, skb);
5876
5877         if (hcon->type != ACL_LINK)
5878                 goto drop;
5879
5880         while (len >= L2CAP_CMD_HDR_SIZE) {
5881                 u16 cmd_len;
5882                 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
5883                 data += L2CAP_CMD_HDR_SIZE;
5884                 len  -= L2CAP_CMD_HDR_SIZE;
5885
5886                 cmd_len = le16_to_cpu(cmd.len);
5887
5888                 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len,
5889                        cmd.ident);
5890
5891                 if (cmd_len > len || !cmd.ident) {
5892                         BT_DBG("corrupted command");
5893                         break;
5894                 }
5895
5896                 err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data);
5897                 if (err) {
5898                         struct l2cap_cmd_rej_unk rej;
5899
5900                         BT_ERR("Wrong link type (%d)", err);
5901
5902                         rej.reason = __constant_cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
5903                         l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ,
5904                                        sizeof(rej), &rej);
5905                 }
5906
5907                 data += cmd_len;
5908                 len  -= cmd_len;
5909         }
5910
5911 drop:
5912         kfree_skb(skb);
5913 }
5914
5915 static int l2cap_check_fcs(struct l2cap_chan *chan,  struct sk_buff *skb)
5916 {
5917         u16 our_fcs, rcv_fcs;
5918         int hdr_size;
5919
5920         if (test_bit(FLAG_EXT_CTRL, &chan->flags))
5921                 hdr_size = L2CAP_EXT_HDR_SIZE;
5922         else
5923                 hdr_size = L2CAP_ENH_HDR_SIZE;
5924
5925         if (chan->fcs == L2CAP_FCS_CRC16) {
5926                 skb_trim(skb, skb->len - L2CAP_FCS_SIZE);
5927                 rcv_fcs = get_unaligned_le16(skb->data + skb->len);
5928                 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
5929
5930                 if (our_fcs != rcv_fcs)
5931                         return -EBADMSG;
5932         }
5933         return 0;
5934 }
5935
5936 static void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan)
5937 {
5938         struct l2cap_ctrl control;
5939
5940         BT_DBG("chan %p", chan);
5941
5942         memset(&control, 0, sizeof(control));
5943         control.sframe = 1;
5944         control.final = 1;
5945         control.reqseq = chan->buffer_seq;
5946         set_bit(CONN_SEND_FBIT, &chan->conn_state);
5947
5948         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
5949                 control.super = L2CAP_SUPER_RNR;
5950                 l2cap_send_sframe(chan, &control);
5951         }
5952
5953         if (test_and_clear_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
5954             chan->unacked_frames > 0)
5955                 __set_retrans_timer(chan);
5956
5957         /* Send pending iframes */
5958         l2cap_ertm_send(chan);
5959
5960         if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
5961             test_bit(CONN_SEND_FBIT, &chan->conn_state)) {
5962                 /* F-bit wasn't sent in an s-frame or i-frame yet, so
5963                  * send it now.
5964                  */
5965                 control.super = L2CAP_SUPER_RR;
5966                 l2cap_send_sframe(chan, &control);
5967         }
5968 }
5969
5970 static void append_skb_frag(struct sk_buff *skb, struct sk_buff *new_frag,
5971                             struct sk_buff **last_frag)
5972 {
5973         /* skb->len reflects data in skb as well as all fragments
5974          * skb->data_len reflects only data in fragments
5975          */
5976         if (!skb_has_frag_list(skb))
5977                 skb_shinfo(skb)->frag_list = new_frag;
5978
5979         new_frag->next = NULL;
5980
5981         (*last_frag)->next = new_frag;
5982         *last_frag = new_frag;
5983
5984         skb->len += new_frag->len;
5985         skb->data_len += new_frag->len;
5986         skb->truesize += new_frag->truesize;
5987 }
5988
5989 static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb,
5990                                 struct l2cap_ctrl *control)
5991 {
5992         int err = -EINVAL;
5993
5994         switch (control->sar) {
5995         case L2CAP_SAR_UNSEGMENTED:
5996                 if (chan->sdu)
5997                         break;
5998
5999                 err = chan->ops->recv(chan, skb);
6000                 break;
6001
6002         case L2CAP_SAR_START:
6003                 if (chan->sdu)
6004                         break;
6005
6006                 chan->sdu_len = get_unaligned_le16(skb->data);
6007                 skb_pull(skb, L2CAP_SDULEN_SIZE);
6008
6009                 if (chan->sdu_len > chan->imtu) {
6010                         err = -EMSGSIZE;
6011                         break;
6012                 }
6013
6014                 if (skb->len >= chan->sdu_len)
6015                         break;
6016
6017                 chan->sdu = skb;
6018                 chan->sdu_last_frag = skb;
6019
6020                 skb = NULL;
6021                 err = 0;
6022                 break;
6023
6024         case L2CAP_SAR_CONTINUE:
6025                 if (!chan->sdu)
6026                         break;
6027
6028                 append_skb_frag(chan->sdu, skb,
6029                                 &chan->sdu_last_frag);
6030                 skb = NULL;
6031
6032                 if (chan->sdu->len >= chan->sdu_len)
6033                         break;
6034
6035                 err = 0;
6036                 break;
6037
6038         case L2CAP_SAR_END:
6039                 if (!chan->sdu)
6040                         break;
6041
6042                 append_skb_frag(chan->sdu, skb,
6043                                 &chan->sdu_last_frag);
6044                 skb = NULL;
6045
6046                 if (chan->sdu->len != chan->sdu_len)
6047                         break;
6048
6049                 err = chan->ops->recv(chan, chan->sdu);
6050
6051                 if (!err) {
6052                         /* Reassembly complete */
6053                         chan->sdu = NULL;
6054                         chan->sdu_last_frag = NULL;
6055                         chan->sdu_len = 0;
6056                 }
6057                 break;
6058         }
6059
6060         if (err) {
6061                 kfree_skb(skb);
6062                 kfree_skb(chan->sdu);
6063                 chan->sdu = NULL;
6064                 chan->sdu_last_frag = NULL;
6065                 chan->sdu_len = 0;
6066         }
6067
6068         return err;
6069 }
6070
6071 static int l2cap_resegment(struct l2cap_chan *chan)
6072 {
6073         /* Placeholder */
6074         return 0;
6075 }
6076
6077 void l2cap_chan_busy(struct l2cap_chan *chan, int busy)
6078 {
6079         u8 event;
6080
6081         if (chan->mode != L2CAP_MODE_ERTM)
6082                 return;
6083
6084         event = busy ? L2CAP_EV_LOCAL_BUSY_DETECTED : L2CAP_EV_LOCAL_BUSY_CLEAR;
6085         l2cap_tx(chan, NULL, NULL, event);
6086 }
6087
6088 static int l2cap_rx_queued_iframes(struct l2cap_chan *chan)
6089 {
6090         int err = 0;
6091         /* Pass sequential frames to l2cap_reassemble_sdu()
6092          * until a gap is encountered.
6093          */
6094
6095         BT_DBG("chan %p", chan);
6096
6097         while (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
6098                 struct sk_buff *skb;
6099                 BT_DBG("Searching for skb with txseq %d (queue len %d)",
6100                        chan->buffer_seq, skb_queue_len(&chan->srej_q));
6101
6102                 skb = l2cap_ertm_seq_in_queue(&chan->srej_q, chan->buffer_seq);
6103
6104                 if (!skb)
6105                         break;
6106
6107                 skb_unlink(skb, &chan->srej_q);
6108                 chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
6109                 err = l2cap_reassemble_sdu(chan, skb, &bt_cb(skb)->control);
6110                 if (err)
6111                         break;
6112         }
6113
6114         if (skb_queue_empty(&chan->srej_q)) {
6115                 chan->rx_state = L2CAP_RX_STATE_RECV;
6116                 l2cap_send_ack(chan);
6117         }
6118
6119         return err;
6120 }
6121
6122 static void l2cap_handle_srej(struct l2cap_chan *chan,
6123                               struct l2cap_ctrl *control)
6124 {
6125         struct sk_buff *skb;
6126
6127         BT_DBG("chan %p, control %p", chan, control);
6128
6129         if (control->reqseq == chan->next_tx_seq) {
6130                 BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq);
6131                 l2cap_send_disconn_req(chan, ECONNRESET);
6132                 return;
6133         }
6134
6135         skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq);
6136
6137         if (skb == NULL) {
6138                 BT_DBG("Seq %d not available for retransmission",
6139                        control->reqseq);
6140                 return;
6141         }
6142
6143         if (chan->max_tx != 0 && bt_cb(skb)->control.retries >= chan->max_tx) {
6144                 BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
6145                 l2cap_send_disconn_req(chan, ECONNRESET);
6146                 return;
6147         }
6148
6149         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6150
6151         if (control->poll) {
6152                 l2cap_pass_to_tx(chan, control);
6153
6154                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
6155                 l2cap_retransmit(chan, control);
6156                 l2cap_ertm_send(chan);
6157
6158                 if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) {
6159                         set_bit(CONN_SREJ_ACT, &chan->conn_state);
6160                         chan->srej_save_reqseq = control->reqseq;
6161                 }
6162         } else {
6163                 l2cap_pass_to_tx_fbit(chan, control);
6164
6165                 if (control->final) {
6166                         if (chan->srej_save_reqseq != control->reqseq ||
6167                             !test_and_clear_bit(CONN_SREJ_ACT,
6168                                                 &chan->conn_state))
6169                                 l2cap_retransmit(chan, control);
6170                 } else {
6171                         l2cap_retransmit(chan, control);
6172                         if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) {
6173                                 set_bit(CONN_SREJ_ACT, &chan->conn_state);
6174                                 chan->srej_save_reqseq = control->reqseq;
6175                         }
6176                 }
6177         }
6178 }
6179
6180 static void l2cap_handle_rej(struct l2cap_chan *chan,
6181                              struct l2cap_ctrl *control)
6182 {
6183         struct sk_buff *skb;
6184
6185         BT_DBG("chan %p, control %p", chan, control);
6186
6187         if (control->reqseq == chan->next_tx_seq) {
6188                 BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq);
6189                 l2cap_send_disconn_req(chan, ECONNRESET);
6190                 return;
6191         }
6192
6193         skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq);
6194
6195         if (chan->max_tx && skb &&
6196             bt_cb(skb)->control.retries >= chan->max_tx) {
6197                 BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
6198                 l2cap_send_disconn_req(chan, ECONNRESET);
6199                 return;
6200         }
6201
6202         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6203
6204         l2cap_pass_to_tx(chan, control);
6205
6206         if (control->final) {
6207                 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
6208                         l2cap_retransmit_all(chan, control);
6209         } else {
6210                 l2cap_retransmit_all(chan, control);
6211                 l2cap_ertm_send(chan);
6212                 if (chan->tx_state == L2CAP_TX_STATE_WAIT_F)
6213                         set_bit(CONN_REJ_ACT, &chan->conn_state);
6214         }
6215 }
6216
6217 static u8 l2cap_classify_txseq(struct l2cap_chan *chan, u16 txseq)
6218 {
6219         BT_DBG("chan %p, txseq %d", chan, txseq);
6220
6221         BT_DBG("last_acked_seq %d, expected_tx_seq %d", chan->last_acked_seq,
6222                chan->expected_tx_seq);
6223
6224         if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
6225                 if (__seq_offset(chan, txseq, chan->last_acked_seq) >=
6226                     chan->tx_win) {
6227                         /* See notes below regarding "double poll" and
6228                          * invalid packets.
6229                          */
6230                         if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) {
6231                                 BT_DBG("Invalid/Ignore - after SREJ");
6232                                 return L2CAP_TXSEQ_INVALID_IGNORE;
6233                         } else {
6234                                 BT_DBG("Invalid - in window after SREJ sent");
6235                                 return L2CAP_TXSEQ_INVALID;
6236                         }
6237                 }
6238
6239                 if (chan->srej_list.head == txseq) {
6240                         BT_DBG("Expected SREJ");
6241                         return L2CAP_TXSEQ_EXPECTED_SREJ;
6242                 }
6243
6244                 if (l2cap_ertm_seq_in_queue(&chan->srej_q, txseq)) {
6245                         BT_DBG("Duplicate SREJ - txseq already stored");
6246                         return L2CAP_TXSEQ_DUPLICATE_SREJ;
6247                 }
6248
6249                 if (l2cap_seq_list_contains(&chan->srej_list, txseq)) {
6250                         BT_DBG("Unexpected SREJ - not requested");
6251                         return L2CAP_TXSEQ_UNEXPECTED_SREJ;
6252                 }
6253         }
6254
6255         if (chan->expected_tx_seq == txseq) {
6256                 if (__seq_offset(chan, txseq, chan->last_acked_seq) >=
6257                     chan->tx_win) {
6258                         BT_DBG("Invalid - txseq outside tx window");
6259                         return L2CAP_TXSEQ_INVALID;
6260                 } else {
6261                         BT_DBG("Expected");
6262                         return L2CAP_TXSEQ_EXPECTED;
6263                 }
6264         }
6265
6266         if (__seq_offset(chan, txseq, chan->last_acked_seq) <
6267             __seq_offset(chan, chan->expected_tx_seq, chan->last_acked_seq)) {
6268                 BT_DBG("Duplicate - expected_tx_seq later than txseq");
6269                 return L2CAP_TXSEQ_DUPLICATE;
6270         }
6271
6272         if (__seq_offset(chan, txseq, chan->last_acked_seq) >= chan->tx_win) {
6273                 /* A source of invalid packets is a "double poll" condition,
6274                  * where delays cause us to send multiple poll packets.  If
6275                  * the remote stack receives and processes both polls,
6276                  * sequence numbers can wrap around in such a way that a
6277                  * resent frame has a sequence number that looks like new data
6278                  * with a sequence gap.  This would trigger an erroneous SREJ
6279                  * request.
6280                  *
6281                  * Fortunately, this is impossible with a tx window that's
6282                  * less than half of the maximum sequence number, which allows
6283                  * invalid frames to be safely ignored.
6284                  *
6285                  * With tx window sizes greater than half of the tx window
6286                  * maximum, the frame is invalid and cannot be ignored.  This
6287                  * causes a disconnect.
6288                  */
6289
6290                 if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) {
6291                         BT_DBG("Invalid/Ignore - txseq outside tx window");
6292                         return L2CAP_TXSEQ_INVALID_IGNORE;
6293                 } else {
6294                         BT_DBG("Invalid - txseq outside tx window");
6295                         return L2CAP_TXSEQ_INVALID;
6296                 }
6297         } else {
6298                 BT_DBG("Unexpected - txseq indicates missing frames");
6299                 return L2CAP_TXSEQ_UNEXPECTED;
6300         }
6301 }
6302
6303 static int l2cap_rx_state_recv(struct l2cap_chan *chan,
6304                                struct l2cap_ctrl *control,
6305                                struct sk_buff *skb, u8 event)
6306 {
6307         int err = 0;
6308         bool skb_in_use = false;
6309
6310         BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
6311                event);
6312
6313         switch (event) {
6314         case L2CAP_EV_RECV_IFRAME:
6315                 switch (l2cap_classify_txseq(chan, control->txseq)) {
6316                 case L2CAP_TXSEQ_EXPECTED:
6317                         l2cap_pass_to_tx(chan, control);
6318
6319                         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
6320                                 BT_DBG("Busy, discarding expected seq %d",
6321                                        control->txseq);
6322                                 break;
6323                         }
6324
6325                         chan->expected_tx_seq = __next_seq(chan,
6326                                                            control->txseq);
6327
6328                         chan->buffer_seq = chan->expected_tx_seq;
6329                         skb_in_use = true;
6330
6331                         err = l2cap_reassemble_sdu(chan, skb, control);
6332                         if (err)
6333                                 break;
6334
6335                         if (control->final) {
6336                                 if (!test_and_clear_bit(CONN_REJ_ACT,
6337                                                         &chan->conn_state)) {
6338                                         control->final = 0;
6339                                         l2cap_retransmit_all(chan, control);
6340                                         l2cap_ertm_send(chan);
6341                                 }
6342                         }
6343
6344                         if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state))
6345                                 l2cap_send_ack(chan);
6346                         break;
6347                 case L2CAP_TXSEQ_UNEXPECTED:
6348                         l2cap_pass_to_tx(chan, control);
6349
6350                         /* Can't issue SREJ frames in the local busy state.
6351                          * Drop this frame, it will be seen as missing
6352                          * when local busy is exited.
6353                          */
6354                         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
6355                                 BT_DBG("Busy, discarding unexpected seq %d",
6356                                        control->txseq);
6357                                 break;
6358                         }
6359
6360                         /* There was a gap in the sequence, so an SREJ
6361                          * must be sent for each missing frame.  The
6362                          * current frame is stored for later use.
6363                          */
6364                         skb_queue_tail(&chan->srej_q, skb);
6365                         skb_in_use = true;
6366                         BT_DBG("Queued %p (queue len %d)", skb,
6367                                skb_queue_len(&chan->srej_q));
6368
6369                         clear_bit(CONN_SREJ_ACT, &chan->conn_state);
6370                         l2cap_seq_list_clear(&chan->srej_list);
6371                         l2cap_send_srej(chan, control->txseq);
6372
6373                         chan->rx_state = L2CAP_RX_STATE_SREJ_SENT;
6374                         break;
6375                 case L2CAP_TXSEQ_DUPLICATE:
6376                         l2cap_pass_to_tx(chan, control);
6377                         break;
6378                 case L2CAP_TXSEQ_INVALID_IGNORE:
6379                         break;
6380                 case L2CAP_TXSEQ_INVALID:
6381                 default:
6382                         l2cap_send_disconn_req(chan, ECONNRESET);
6383                         break;
6384                 }
6385                 break;
6386         case L2CAP_EV_RECV_RR:
6387                 l2cap_pass_to_tx(chan, control);
6388                 if (control->final) {
6389                         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6390
6391                         if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state) &&
6392                             !__chan_is_moving(chan)) {
6393                                 control->final = 0;
6394                                 l2cap_retransmit_all(chan, control);
6395                         }
6396
6397                         l2cap_ertm_send(chan);
6398                 } else if (control->poll) {
6399                         l2cap_send_i_or_rr_or_rnr(chan);
6400                 } else {
6401                         if (test_and_clear_bit(CONN_REMOTE_BUSY,
6402                                                &chan->conn_state) &&
6403                             chan->unacked_frames)
6404                                 __set_retrans_timer(chan);
6405
6406                         l2cap_ertm_send(chan);
6407                 }
6408                 break;
6409         case L2CAP_EV_RECV_RNR:
6410                 set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6411                 l2cap_pass_to_tx(chan, control);
6412                 if (control && control->poll) {
6413                         set_bit(CONN_SEND_FBIT, &chan->conn_state);
6414                         l2cap_send_rr_or_rnr(chan, 0);
6415                 }
6416                 __clear_retrans_timer(chan);
6417                 l2cap_seq_list_clear(&chan->retrans_list);
6418                 break;
6419         case L2CAP_EV_RECV_REJ:
6420                 l2cap_handle_rej(chan, control);
6421                 break;
6422         case L2CAP_EV_RECV_SREJ:
6423                 l2cap_handle_srej(chan, control);
6424                 break;
6425         default:
6426                 break;
6427         }
6428
6429         if (skb && !skb_in_use) {
6430                 BT_DBG("Freeing %p", skb);
6431                 kfree_skb(skb);
6432         }
6433
6434         return err;
6435 }
6436
6437 static int l2cap_rx_state_srej_sent(struct l2cap_chan *chan,
6438                                     struct l2cap_ctrl *control,
6439                                     struct sk_buff *skb, u8 event)
6440 {
6441         int err = 0;
6442         u16 txseq = control->txseq;
6443         bool skb_in_use = false;
6444
6445         BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
6446                event);
6447
6448         switch (event) {
6449         case L2CAP_EV_RECV_IFRAME:
6450                 switch (l2cap_classify_txseq(chan, txseq)) {
6451                 case L2CAP_TXSEQ_EXPECTED:
6452                         /* Keep frame for reassembly later */
6453                         l2cap_pass_to_tx(chan, control);
6454                         skb_queue_tail(&chan->srej_q, skb);
6455                         skb_in_use = true;
6456                         BT_DBG("Queued %p (queue len %d)", skb,
6457                                skb_queue_len(&chan->srej_q));
6458
6459                         chan->expected_tx_seq = __next_seq(chan, txseq);
6460                         break;
6461                 case L2CAP_TXSEQ_EXPECTED_SREJ:
6462                         l2cap_seq_list_pop(&chan->srej_list);
6463
6464                         l2cap_pass_to_tx(chan, control);
6465                         skb_queue_tail(&chan->srej_q, skb);
6466                         skb_in_use = true;
6467                         BT_DBG("Queued %p (queue len %d)", skb,
6468                                skb_queue_len(&chan->srej_q));
6469
6470                         err = l2cap_rx_queued_iframes(chan);
6471                         if (err)
6472                                 break;
6473
6474                         break;
6475                 case L2CAP_TXSEQ_UNEXPECTED:
6476                         /* Got a frame that can't be reassembled yet.
6477                          * Save it for later, and send SREJs to cover
6478                          * the missing frames.
6479                          */
6480                         skb_queue_tail(&chan->srej_q, skb);
6481                         skb_in_use = true;
6482                         BT_DBG("Queued %p (queue len %d)", skb,
6483                                skb_queue_len(&chan->srej_q));
6484
6485                         l2cap_pass_to_tx(chan, control);
6486                         l2cap_send_srej(chan, control->txseq);
6487                         break;
6488                 case L2CAP_TXSEQ_UNEXPECTED_SREJ:
6489                         /* This frame was requested with an SREJ, but
6490                          * some expected retransmitted frames are
6491                          * missing.  Request retransmission of missing
6492                          * SREJ'd frames.
6493                          */
6494                         skb_queue_tail(&chan->srej_q, skb);
6495                         skb_in_use = true;
6496                         BT_DBG("Queued %p (queue len %d)", skb,
6497                                skb_queue_len(&chan->srej_q));
6498
6499                         l2cap_pass_to_tx(chan, control);
6500                         l2cap_send_srej_list(chan, control->txseq);
6501                         break;
6502                 case L2CAP_TXSEQ_DUPLICATE_SREJ:
6503                         /* We've already queued this frame.  Drop this copy. */
6504                         l2cap_pass_to_tx(chan, control);
6505                         break;
6506                 case L2CAP_TXSEQ_DUPLICATE:
6507                         /* Expecting a later sequence number, so this frame
6508                          * was already received.  Ignore it completely.
6509                          */
6510                         break;
6511                 case L2CAP_TXSEQ_INVALID_IGNORE:
6512                         break;
6513                 case L2CAP_TXSEQ_INVALID:
6514                 default:
6515                         l2cap_send_disconn_req(chan, ECONNRESET);
6516                         break;
6517                 }
6518                 break;
6519         case L2CAP_EV_RECV_RR:
6520                 l2cap_pass_to_tx(chan, control);
6521                 if (control->final) {
6522                         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6523
6524                         if (!test_and_clear_bit(CONN_REJ_ACT,
6525                                                 &chan->conn_state)) {
6526                                 control->final = 0;
6527                                 l2cap_retransmit_all(chan, control);
6528                         }
6529
6530                         l2cap_ertm_send(chan);
6531                 } else if (control->poll) {
6532                         if (test_and_clear_bit(CONN_REMOTE_BUSY,
6533                                                &chan->conn_state) &&
6534                             chan->unacked_frames) {
6535                                 __set_retrans_timer(chan);
6536                         }
6537
6538                         set_bit(CONN_SEND_FBIT, &chan->conn_state);
6539                         l2cap_send_srej_tail(chan);
6540                 } else {
6541                         if (test_and_clear_bit(CONN_REMOTE_BUSY,
6542                                                &chan->conn_state) &&
6543                             chan->unacked_frames)
6544                                 __set_retrans_timer(chan);
6545
6546                         l2cap_send_ack(chan);
6547                 }
6548                 break;
6549         case L2CAP_EV_RECV_RNR:
6550                 set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6551                 l2cap_pass_to_tx(chan, control);
6552                 if (control->poll) {
6553                         l2cap_send_srej_tail(chan);
6554                 } else {
6555                         struct l2cap_ctrl rr_control;
6556                         memset(&rr_control, 0, sizeof(rr_control));
6557                         rr_control.sframe = 1;
6558                         rr_control.super = L2CAP_SUPER_RR;
6559                         rr_control.reqseq = chan->buffer_seq;
6560                         l2cap_send_sframe(chan, &rr_control);
6561                 }
6562
6563                 break;
6564         case L2CAP_EV_RECV_REJ:
6565                 l2cap_handle_rej(chan, control);
6566                 break;
6567         case L2CAP_EV_RECV_SREJ:
6568                 l2cap_handle_srej(chan, control);
6569                 break;
6570         }
6571
6572         if (skb && !skb_in_use) {
6573                 BT_DBG("Freeing %p", skb);
6574                 kfree_skb(skb);
6575         }
6576
6577         return err;
6578 }
6579
6580 static int l2cap_finish_move(struct l2cap_chan *chan)
6581 {
6582         BT_DBG("chan %p", chan);
6583
6584         chan->rx_state = L2CAP_RX_STATE_RECV;
6585
6586         if (chan->hs_hcon)
6587                 chan->conn->mtu = chan->hs_hcon->hdev->block_mtu;
6588         else
6589                 chan->conn->mtu = chan->conn->hcon->hdev->acl_mtu;
6590
6591         return l2cap_resegment(chan);
6592 }
6593
6594 static int l2cap_rx_state_wait_p(struct l2cap_chan *chan,
6595                                  struct l2cap_ctrl *control,
6596                                  struct sk_buff *skb, u8 event)
6597 {
6598         int err;
6599
6600         BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
6601                event);
6602
6603         if (!control->poll)
6604                 return -EPROTO;
6605
6606         l2cap_process_reqseq(chan, control->reqseq);
6607
6608         if (!skb_queue_empty(&chan->tx_q))
6609                 chan->tx_send_head = skb_peek(&chan->tx_q);
6610         else
6611                 chan->tx_send_head = NULL;
6612
6613         /* Rewind next_tx_seq to the point expected
6614          * by the receiver.
6615          */
6616         chan->next_tx_seq = control->reqseq;
6617         chan->unacked_frames = 0;
6618
6619         err = l2cap_finish_move(chan);
6620         if (err)
6621                 return err;
6622
6623         set_bit(CONN_SEND_FBIT, &chan->conn_state);
6624         l2cap_send_i_or_rr_or_rnr(chan);
6625
6626         if (event == L2CAP_EV_RECV_IFRAME)
6627                 return -EPROTO;
6628
6629         return l2cap_rx_state_recv(chan, control, NULL, event);
6630 }
6631
6632 static int l2cap_rx_state_wait_f(struct l2cap_chan *chan,
6633                                  struct l2cap_ctrl *control,
6634                                  struct sk_buff *skb, u8 event)
6635 {
6636         int err;
6637
6638         if (!control->final)
6639                 return -EPROTO;
6640
6641         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6642
6643         chan->rx_state = L2CAP_RX_STATE_RECV;
6644         l2cap_process_reqseq(chan, control->reqseq);
6645
6646         if (!skb_queue_empty(&chan->tx_q))
6647                 chan->tx_send_head = skb_peek(&chan->tx_q);
6648         else
6649                 chan->tx_send_head = NULL;
6650
6651         /* Rewind next_tx_seq to the point expected
6652          * by the receiver.
6653          */
6654         chan->next_tx_seq = control->reqseq;
6655         chan->unacked_frames = 0;
6656
6657         if (chan->hs_hcon)
6658                 chan->conn->mtu = chan->hs_hcon->hdev->block_mtu;
6659         else
6660                 chan->conn->mtu = chan->conn->hcon->hdev->acl_mtu;
6661
6662         err = l2cap_resegment(chan);
6663
6664         if (!err)
6665                 err = l2cap_rx_state_recv(chan, control, skb, event);
6666
6667         return err;
6668 }
6669
6670 static bool __valid_reqseq(struct l2cap_chan *chan, u16 reqseq)
6671 {
6672         /* Make sure reqseq is for a packet that has been sent but not acked */
6673         u16 unacked;
6674
6675         unacked = __seq_offset(chan, chan->next_tx_seq, chan->expected_ack_seq);
6676         return __seq_offset(chan, chan->next_tx_seq, reqseq) <= unacked;
6677 }
6678
6679 static int l2cap_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
6680                     struct sk_buff *skb, u8 event)
6681 {
6682         int err = 0;
6683
6684         BT_DBG("chan %p, control %p, skb %p, event %d, state %d", chan,
6685                control, skb, event, chan->rx_state);
6686
6687         if (__valid_reqseq(chan, control->reqseq)) {
6688                 switch (chan->rx_state) {
6689                 case L2CAP_RX_STATE_RECV:
6690                         err = l2cap_rx_state_recv(chan, control, skb, event);
6691                         break;
6692                 case L2CAP_RX_STATE_SREJ_SENT:
6693                         err = l2cap_rx_state_srej_sent(chan, control, skb,
6694                                                        event);
6695                         break;
6696                 case L2CAP_RX_STATE_WAIT_P:
6697                         err = l2cap_rx_state_wait_p(chan, control, skb, event);
6698                         break;
6699                 case L2CAP_RX_STATE_WAIT_F:
6700                         err = l2cap_rx_state_wait_f(chan, control, skb, event);
6701                         break;
6702                 default:
6703                         /* shut it down */
6704                         break;
6705                 }
6706         } else {
6707                 BT_DBG("Invalid reqseq %d (next_tx_seq %d, expected_ack_seq %d",
6708                        control->reqseq, chan->next_tx_seq,
6709                        chan->expected_ack_seq);
6710                 l2cap_send_disconn_req(chan, ECONNRESET);
6711         }
6712
6713         return err;
6714 }
6715
6716 static int l2cap_stream_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
6717                            struct sk_buff *skb)
6718 {
6719         int err = 0;
6720
6721         BT_DBG("chan %p, control %p, skb %p, state %d", chan, control, skb,
6722                chan->rx_state);
6723
6724         if (l2cap_classify_txseq(chan, control->txseq) ==
6725             L2CAP_TXSEQ_EXPECTED) {
6726                 l2cap_pass_to_tx(chan, control);
6727
6728                 BT_DBG("buffer_seq %d->%d", chan->buffer_seq,
6729                        __next_seq(chan, chan->buffer_seq));
6730
6731                 chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
6732
6733                 l2cap_reassemble_sdu(chan, skb, control);
6734         } else {
6735                 if (chan->sdu) {
6736                         kfree_skb(chan->sdu);
6737                         chan->sdu = NULL;
6738                 }
6739                 chan->sdu_last_frag = NULL;
6740                 chan->sdu_len = 0;
6741
6742                 if (skb) {
6743                         BT_DBG("Freeing %p", skb);
6744                         kfree_skb(skb);
6745                 }
6746         }
6747
6748         chan->last_acked_seq = control->txseq;
6749         chan->expected_tx_seq = __next_seq(chan, control->txseq);
6750
6751         return err;
6752 }
6753
6754 static int l2cap_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb)
6755 {
6756         struct l2cap_ctrl *control = &bt_cb(skb)->control;
6757         u16 len;
6758         u8 event;
6759
6760         __unpack_control(chan, skb);
6761
6762         len = skb->len;
6763
6764         /*
6765          * We can just drop the corrupted I-frame here.
6766          * Receiver will miss it and start proper recovery
6767          * procedures and ask for retransmission.
6768          */
6769         if (l2cap_check_fcs(chan, skb))
6770                 goto drop;
6771
6772         if (!control->sframe && control->sar == L2CAP_SAR_START)
6773                 len -= L2CAP_SDULEN_SIZE;
6774
6775         if (chan->fcs == L2CAP_FCS_CRC16)
6776                 len -= L2CAP_FCS_SIZE;
6777
6778         if (len > chan->mps) {
6779                 l2cap_send_disconn_req(chan, ECONNRESET);
6780                 goto drop;
6781         }
6782
6783         if (!control->sframe) {
6784                 int err;
6785
6786                 BT_DBG("iframe sar %d, reqseq %d, final %d, txseq %d",
6787                        control->sar, control->reqseq, control->final,
6788                        control->txseq);
6789
6790                 /* Validate F-bit - F=0 always valid, F=1 only
6791                  * valid in TX WAIT_F
6792                  */
6793                 if (control->final && chan->tx_state != L2CAP_TX_STATE_WAIT_F)
6794                         goto drop;
6795
6796                 if (chan->mode != L2CAP_MODE_STREAMING) {
6797                         event = L2CAP_EV_RECV_IFRAME;
6798                         err = l2cap_rx(chan, control, skb, event);
6799                 } else {
6800                         err = l2cap_stream_rx(chan, control, skb);
6801                 }
6802
6803                 if (err)
6804                         l2cap_send_disconn_req(chan, ECONNRESET);
6805         } else {
6806                 const u8 rx_func_to_event[4] = {
6807                         L2CAP_EV_RECV_RR, L2CAP_EV_RECV_REJ,
6808                         L2CAP_EV_RECV_RNR, L2CAP_EV_RECV_SREJ
6809                 };
6810
6811                 /* Only I-frames are expected in streaming mode */
6812                 if (chan->mode == L2CAP_MODE_STREAMING)
6813                         goto drop;
6814
6815                 BT_DBG("sframe reqseq %d, final %d, poll %d, super %d",
6816                        control->reqseq, control->final, control->poll,
6817                        control->super);
6818
6819                 if (len != 0) {
6820                         BT_ERR("Trailing bytes: %d in sframe", len);
6821                         l2cap_send_disconn_req(chan, ECONNRESET);
6822                         goto drop;
6823                 }
6824
6825                 /* Validate F and P bits */
6826                 if (control->final && (control->poll ||
6827                                        chan->tx_state != L2CAP_TX_STATE_WAIT_F))
6828                         goto drop;
6829
6830                 event = rx_func_to_event[control->super];
6831                 if (l2cap_rx(chan, control, skb, event))
6832                         l2cap_send_disconn_req(chan, ECONNRESET);
6833         }
6834
6835         return 0;
6836
6837 drop:
6838         kfree_skb(skb);
6839         return 0;
6840 }
6841
6842 static void l2cap_chan_le_send_credits(struct l2cap_chan *chan)
6843 {
6844         struct l2cap_conn *conn = chan->conn;
6845         struct l2cap_le_credits pkt;
6846         u16 return_credits;
6847
6848         /* We return more credits to the sender only after the amount of
6849          * credits falls below half of the initial amount.
6850          */
6851         if (chan->rx_credits >= (le_max_credits + 1) / 2)
6852                 return;
6853
6854         return_credits = le_max_credits - chan->rx_credits;
6855
6856         BT_DBG("chan %p returning %u credits to sender", chan, return_credits);
6857
6858         chan->rx_credits += return_credits;
6859
6860         pkt.cid     = cpu_to_le16(chan->scid);
6861         pkt.credits = cpu_to_le16(return_credits);
6862
6863         chan->ident = l2cap_get_ident(conn);
6864
6865         l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CREDITS, sizeof(pkt), &pkt);
6866 }
6867
6868 static int l2cap_le_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb)
6869 {
6870         int err;
6871
6872         if (!chan->rx_credits) {
6873                 BT_ERR("No credits to receive LE L2CAP data");
6874                 return -ENOBUFS;
6875         }
6876
6877         if (chan->imtu < skb->len) {
6878                 BT_ERR("Too big LE L2CAP PDU");
6879                 return -ENOBUFS;
6880         }
6881
6882         chan->rx_credits--;
6883         BT_DBG("rx_credits %u -> %u", chan->rx_credits + 1, chan->rx_credits);
6884
6885         l2cap_chan_le_send_credits(chan);
6886
6887         err = 0;
6888
6889         if (!chan->sdu) {
6890                 u16 sdu_len;
6891
6892                 sdu_len = get_unaligned_le16(skb->data);
6893                 skb_pull(skb, L2CAP_SDULEN_SIZE);
6894
6895                 BT_DBG("Start of new SDU. sdu_len %u skb->len %u imtu %u",
6896                        sdu_len, skb->len, chan->imtu);
6897
6898                 if (sdu_len > chan->imtu) {
6899                         BT_ERR("Too big LE L2CAP SDU length received");
6900                         err = -EMSGSIZE;
6901                         goto failed;
6902                 }
6903
6904                 if (skb->len > sdu_len) {
6905                         BT_ERR("Too much LE L2CAP data received");
6906                         err = -EINVAL;
6907                         goto failed;
6908                 }
6909
6910                 if (skb->len == sdu_len)
6911                         return chan->ops->recv(chan, skb);
6912
6913                 chan->sdu = skb;
6914                 chan->sdu_len = sdu_len;
6915                 chan->sdu_last_frag = skb;
6916
6917                 return 0;
6918         }
6919
6920         BT_DBG("SDU fragment. chan->sdu->len %u skb->len %u chan->sdu_len %u",
6921                chan->sdu->len, skb->len, chan->sdu_len);
6922
6923         if (chan->sdu->len + skb->len > chan->sdu_len) {
6924                 BT_ERR("Too much LE L2CAP data received");
6925                 err = -EINVAL;
6926                 goto failed;
6927         }
6928
6929         append_skb_frag(chan->sdu, skb, &chan->sdu_last_frag);
6930         skb = NULL;
6931
6932         if (chan->sdu->len == chan->sdu_len) {
6933                 err = chan->ops->recv(chan, chan->sdu);
6934                 if (!err) {
6935                         chan->sdu = NULL;
6936                         chan->sdu_last_frag = NULL;
6937                         chan->sdu_len = 0;
6938                 }
6939         }
6940
6941 failed:
6942         if (err) {
6943                 kfree_skb(skb);
6944                 kfree_skb(chan->sdu);
6945                 chan->sdu = NULL;
6946                 chan->sdu_last_frag = NULL;
6947                 chan->sdu_len = 0;
6948         }
6949
6950         /* We can't return an error here since we took care of the skb
6951          * freeing internally. An error return would cause the caller to
6952          * do a double-free of the skb.
6953          */
6954         return 0;
6955 }
6956
6957 static void l2cap_data_channel(struct l2cap_conn *conn, u16 cid,
6958                                struct sk_buff *skb)
6959 {
6960         struct l2cap_chan *chan;
6961
6962         chan = l2cap_get_chan_by_scid(conn, cid);
6963         if (!chan) {
6964                 if (cid == L2CAP_CID_A2MP) {
6965                         chan = a2mp_channel_create(conn, skb);
6966                         if (!chan) {
6967                                 kfree_skb(skb);
6968                                 return;
6969                         }
6970
6971                         l2cap_chan_lock(chan);
6972                 } else {
6973                         BT_DBG("unknown cid 0x%4.4x", cid);
6974                         /* Drop packet and return */
6975                         kfree_skb(skb);
6976                         return;
6977                 }
6978         }
6979
6980         BT_DBG("chan %p, len %d", chan, skb->len);
6981
6982         if (chan->state != BT_CONNECTED)
6983                 goto drop;
6984
6985         switch (chan->mode) {
6986         case L2CAP_MODE_LE_FLOWCTL:
6987                 if (l2cap_le_data_rcv(chan, skb) < 0)
6988                         goto drop;
6989
6990                 goto done;
6991
6992         case L2CAP_MODE_BASIC:
6993                 /* If socket recv buffers overflows we drop data here
6994                  * which is *bad* because L2CAP has to be reliable.
6995                  * But we don't have any other choice. L2CAP doesn't
6996                  * provide flow control mechanism. */
6997
6998                 if (chan->imtu < skb->len)
6999                         goto drop;
7000
7001                 if (!chan->ops->recv(chan, skb))
7002                         goto done;
7003                 break;
7004
7005         case L2CAP_MODE_ERTM:
7006         case L2CAP_MODE_STREAMING:
7007                 l2cap_data_rcv(chan, skb);
7008                 goto done;
7009
7010         default:
7011                 BT_DBG("chan %p: bad mode 0x%2.2x", chan, chan->mode);
7012                 break;
7013         }
7014
7015 drop:
7016         kfree_skb(skb);
7017
7018 done:
7019         l2cap_chan_unlock(chan);
7020 }
7021
7022 static void l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm,
7023                                   struct sk_buff *skb)
7024 {
7025         struct hci_conn *hcon = conn->hcon;
7026         struct l2cap_chan *chan;
7027
7028         if (hcon->type != ACL_LINK)
7029                 goto drop;
7030
7031         chan = l2cap_global_chan_by_psm(0, psm, &hcon->src, &hcon->dst,
7032                                         ACL_LINK);
7033         if (!chan)
7034                 goto drop;
7035
7036         BT_DBG("chan %p, len %d", chan, skb->len);
7037
7038         if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
7039                 goto drop;
7040
7041         if (chan->imtu < skb->len)
7042                 goto drop;
7043
7044         /* Store remote BD_ADDR and PSM for msg_name */
7045         bacpy(&bt_cb(skb)->bdaddr, &hcon->dst);
7046         bt_cb(skb)->psm = psm;
7047
7048         if (!chan->ops->recv(chan, skb))
7049                 return;
7050
7051 drop:
7052         kfree_skb(skb);
7053 }
7054
7055 static void l2cap_att_channel(struct l2cap_conn *conn,
7056                               struct sk_buff *skb)
7057 {
7058         struct hci_conn *hcon = conn->hcon;
7059         struct l2cap_chan *chan;
7060
7061         if (hcon->type != LE_LINK)
7062                 goto drop;
7063
7064         chan = l2cap_global_chan_by_scid(BT_CONNECTED, L2CAP_CID_ATT,
7065                                          &hcon->src, &hcon->dst);
7066         if (!chan)
7067                 goto drop;
7068
7069         BT_DBG("chan %p, len %d", chan, skb->len);
7070
7071         if (hci_blacklist_lookup(hcon->hdev, &hcon->dst, hcon->dst_type))
7072                 goto drop;
7073
7074         if (chan->imtu < skb->len)
7075                 goto drop;
7076
7077         if (!chan->ops->recv(chan, skb))
7078                 return;
7079
7080 drop:
7081         kfree_skb(skb);
7082 }
7083
7084 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
7085 {
7086         struct l2cap_hdr *lh = (void *) skb->data;
7087         u16 cid, len;
7088         __le16 psm;
7089
7090         skb_pull(skb, L2CAP_HDR_SIZE);
7091         cid = __le16_to_cpu(lh->cid);
7092         len = __le16_to_cpu(lh->len);
7093
7094         if (len != skb->len) {
7095                 kfree_skb(skb);
7096                 return;
7097         }
7098
7099         BT_DBG("len %d, cid 0x%4.4x", len, cid);
7100
7101         switch (cid) {
7102         case L2CAP_CID_SIGNALING:
7103                 l2cap_sig_channel(conn, skb);
7104                 break;
7105
7106         case L2CAP_CID_CONN_LESS:
7107                 psm = get_unaligned((__le16 *) skb->data);
7108                 skb_pull(skb, L2CAP_PSMLEN_SIZE);
7109                 l2cap_conless_channel(conn, psm, skb);
7110                 break;
7111
7112         case L2CAP_CID_ATT:
7113                 l2cap_att_channel(conn, skb);
7114                 break;
7115
7116         case L2CAP_CID_LE_SIGNALING:
7117                 l2cap_le_sig_channel(conn, skb);
7118                 break;
7119
7120         case L2CAP_CID_SMP:
7121                 if (smp_sig_channel(conn, skb))
7122                         l2cap_conn_del(conn->hcon, EACCES);
7123                 break;
7124
7125         case L2CAP_FC_6LOWPAN:
7126                 bt_6lowpan_recv(conn, skb);
7127                 break;
7128
7129         default:
7130                 l2cap_data_channel(conn, cid, skb);
7131                 break;
7132         }
7133 }
7134
7135 /* ---- L2CAP interface with lower layer (HCI) ---- */
7136
7137 int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr)
7138 {
7139         int exact = 0, lm1 = 0, lm2 = 0;
7140         struct l2cap_chan *c;
7141
7142         BT_DBG("hdev %s, bdaddr %pMR", hdev->name, bdaddr);
7143
7144         /* Find listening sockets and check their link_mode */
7145         read_lock(&chan_list_lock);
7146         list_for_each_entry(c, &chan_list, global_l) {
7147                 if (c->state != BT_LISTEN)
7148                         continue;
7149
7150                 if (!bacmp(&c->src, &hdev->bdaddr)) {
7151                         lm1 |= HCI_LM_ACCEPT;
7152                         if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
7153                                 lm1 |= HCI_LM_MASTER;
7154                         exact++;
7155                 } else if (!bacmp(&c->src, BDADDR_ANY)) {
7156                         lm2 |= HCI_LM_ACCEPT;
7157                         if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
7158                                 lm2 |= HCI_LM_MASTER;
7159                 }
7160         }
7161         read_unlock(&chan_list_lock);
7162
7163         return exact ? lm1 : lm2;
7164 }
7165
7166 void l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
7167 {
7168         struct l2cap_conn *conn;
7169
7170         BT_DBG("hcon %p bdaddr %pMR status %d", hcon, &hcon->dst, status);
7171
7172         if (!status) {
7173                 conn = l2cap_conn_add(hcon);
7174                 if (conn)
7175                         l2cap_conn_ready(conn);
7176         } else {
7177                 l2cap_conn_del(hcon, bt_to_errno(status));
7178         }
7179 }
7180
7181 int l2cap_disconn_ind(struct hci_conn *hcon)
7182 {
7183         struct l2cap_conn *conn = hcon->l2cap_data;
7184
7185         BT_DBG("hcon %p", hcon);
7186
7187         if (!conn)
7188                 return HCI_ERROR_REMOTE_USER_TERM;
7189         return conn->disc_reason;
7190 }
7191
7192 void l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
7193 {
7194         BT_DBG("hcon %p reason %d", hcon, reason);
7195
7196         bt_6lowpan_del_conn(hcon->l2cap_data);
7197
7198         l2cap_conn_del(hcon, bt_to_errno(reason));
7199 }
7200
7201 static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt)
7202 {
7203         if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED)
7204                 return;
7205
7206         if (encrypt == 0x00) {
7207                 if (chan->sec_level == BT_SECURITY_MEDIUM) {
7208                         __set_chan_timer(chan, L2CAP_ENC_TIMEOUT);
7209                 } else if (chan->sec_level == BT_SECURITY_HIGH)
7210                         l2cap_chan_close(chan, ECONNREFUSED);
7211         } else {
7212                 if (chan->sec_level == BT_SECURITY_MEDIUM)
7213                         __clear_chan_timer(chan);
7214         }
7215 }
7216
7217 int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
7218 {
7219         struct l2cap_conn *conn = hcon->l2cap_data;
7220         struct l2cap_chan *chan;
7221
7222         if (!conn)
7223                 return 0;
7224
7225         BT_DBG("conn %p status 0x%2.2x encrypt %u", conn, status, encrypt);
7226
7227         if (hcon->type == LE_LINK) {
7228                 if (!status && encrypt)
7229                         smp_distribute_keys(conn, 0);
7230                 cancel_delayed_work(&conn->security_timer);
7231         }
7232
7233         mutex_lock(&conn->chan_lock);
7234
7235         list_for_each_entry(chan, &conn->chan_l, list) {
7236                 l2cap_chan_lock(chan);
7237
7238                 BT_DBG("chan %p scid 0x%4.4x state %s", chan, chan->scid,
7239                        state_to_string(chan->state));
7240
7241                 if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP) {
7242                         l2cap_chan_unlock(chan);
7243                         continue;
7244                 }
7245
7246                 if (chan->scid == L2CAP_CID_ATT) {
7247                         if (!status && encrypt) {
7248                                 chan->sec_level = hcon->sec_level;
7249                                 l2cap_chan_ready(chan);
7250                         }
7251
7252                         l2cap_chan_unlock(chan);
7253                         continue;
7254                 }
7255
7256                 if (!__l2cap_no_conn_pending(chan)) {
7257                         l2cap_chan_unlock(chan);
7258                         continue;
7259                 }
7260
7261                 if (!status && (chan->state == BT_CONNECTED ||
7262                                 chan->state == BT_CONFIG)) {
7263                         chan->ops->resume(chan);
7264                         l2cap_check_encryption(chan, encrypt);
7265                         l2cap_chan_unlock(chan);
7266                         continue;
7267                 }
7268
7269                 if (chan->state == BT_CONNECT) {
7270                         if (!status)
7271                                 l2cap_start_connection(chan);
7272                         else
7273                                 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
7274                 } else if (chan->state == BT_CONNECT2) {
7275                         struct l2cap_conn_rsp rsp;
7276                         __u16 res, stat;
7277
7278                         if (!status) {
7279                                 if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
7280                                         res = L2CAP_CR_PEND;
7281                                         stat = L2CAP_CS_AUTHOR_PEND;
7282                                         chan->ops->defer(chan);
7283                                 } else {
7284                                         l2cap_state_change(chan, BT_CONFIG);
7285                                         res = L2CAP_CR_SUCCESS;
7286                                         stat = L2CAP_CS_NO_INFO;
7287                                 }
7288                         } else {
7289                                 l2cap_state_change(chan, BT_DISCONN);
7290                                 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
7291                                 res = L2CAP_CR_SEC_BLOCK;
7292                                 stat = L2CAP_CS_NO_INFO;
7293                         }
7294
7295                         rsp.scid   = cpu_to_le16(chan->dcid);
7296                         rsp.dcid   = cpu_to_le16(chan->scid);
7297                         rsp.result = cpu_to_le16(res);
7298                         rsp.status = cpu_to_le16(stat);
7299                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
7300                                        sizeof(rsp), &rsp);
7301
7302                         if (!test_bit(CONF_REQ_SENT, &chan->conf_state) &&
7303                             res == L2CAP_CR_SUCCESS) {
7304                                 char buf[128];
7305                                 set_bit(CONF_REQ_SENT, &chan->conf_state);
7306                                 l2cap_send_cmd(conn, l2cap_get_ident(conn),
7307                                                L2CAP_CONF_REQ,
7308                                                l2cap_build_conf_req(chan, buf),
7309                                                buf);
7310                                 chan->num_conf_req++;
7311                         }
7312                 }
7313
7314                 l2cap_chan_unlock(chan);
7315         }
7316
7317         mutex_unlock(&conn->chan_lock);
7318
7319         return 0;
7320 }
7321
7322 int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
7323 {
7324         struct l2cap_conn *conn = hcon->l2cap_data;
7325         struct l2cap_hdr *hdr;
7326         int len;
7327
7328         /* For AMP controller do not create l2cap conn */
7329         if (!conn && hcon->hdev->dev_type != HCI_BREDR)
7330                 goto drop;
7331
7332         if (!conn)
7333                 conn = l2cap_conn_add(hcon);
7334
7335         if (!conn)
7336                 goto drop;
7337
7338         BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
7339
7340         switch (flags) {
7341         case ACL_START:
7342         case ACL_START_NO_FLUSH:
7343         case ACL_COMPLETE:
7344                 if (conn->rx_len) {
7345                         BT_ERR("Unexpected start frame (len %d)", skb->len);
7346                         kfree_skb(conn->rx_skb);
7347                         conn->rx_skb = NULL;
7348                         conn->rx_len = 0;
7349                         l2cap_conn_unreliable(conn, ECOMM);
7350                 }
7351
7352                 /* Start fragment always begin with Basic L2CAP header */
7353                 if (skb->len < L2CAP_HDR_SIZE) {
7354                         BT_ERR("Frame is too short (len %d)", skb->len);
7355                         l2cap_conn_unreliable(conn, ECOMM);
7356                         goto drop;
7357                 }
7358
7359                 hdr = (struct l2cap_hdr *) skb->data;
7360                 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
7361
7362                 if (len == skb->len) {
7363                         /* Complete frame received */
7364                         l2cap_recv_frame(conn, skb);
7365                         return 0;
7366                 }
7367
7368                 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
7369
7370                 if (skb->len > len) {
7371                         BT_ERR("Frame is too long (len %d, expected len %d)",
7372                                skb->len, len);
7373                         l2cap_conn_unreliable(conn, ECOMM);
7374                         goto drop;
7375                 }
7376
7377                 /* Allocate skb for the complete frame (with header) */
7378                 conn->rx_skb = bt_skb_alloc(len, GFP_KERNEL);
7379                 if (!conn->rx_skb)
7380                         goto drop;
7381
7382                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
7383                                           skb->len);
7384                 conn->rx_len = len - skb->len;
7385                 break;
7386
7387         case ACL_CONT:
7388                 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
7389
7390                 if (!conn->rx_len) {
7391                         BT_ERR("Unexpected continuation frame (len %d)", skb->len);
7392                         l2cap_conn_unreliable(conn, ECOMM);
7393                         goto drop;
7394                 }
7395
7396                 if (skb->len > conn->rx_len) {
7397                         BT_ERR("Fragment is too long (len %d, expected %d)",
7398                                skb->len, conn->rx_len);
7399                         kfree_skb(conn->rx_skb);
7400                         conn->rx_skb = NULL;
7401                         conn->rx_len = 0;
7402                         l2cap_conn_unreliable(conn, ECOMM);
7403                         goto drop;
7404                 }
7405
7406                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
7407                                           skb->len);
7408                 conn->rx_len -= skb->len;
7409
7410                 if (!conn->rx_len) {
7411                         /* Complete frame received. l2cap_recv_frame
7412                          * takes ownership of the skb so set the global
7413                          * rx_skb pointer to NULL first.
7414                          */
7415                         struct sk_buff *rx_skb = conn->rx_skb;
7416                         conn->rx_skb = NULL;
7417                         l2cap_recv_frame(conn, rx_skb);
7418                 }
7419                 break;
7420         }
7421
7422 drop:
7423         kfree_skb(skb);
7424         return 0;
7425 }
7426
7427 static int l2cap_debugfs_show(struct seq_file *f, void *p)
7428 {
7429         struct l2cap_chan *c;
7430
7431         read_lock(&chan_list_lock);
7432
7433         list_for_each_entry(c, &chan_list, global_l) {
7434                 seq_printf(f, "%pMR %pMR %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
7435                            &c->src, &c->dst,
7436                            c->state, __le16_to_cpu(c->psm),
7437                            c->scid, c->dcid, c->imtu, c->omtu,
7438                            c->sec_level, c->mode);
7439         }
7440
7441         read_unlock(&chan_list_lock);
7442
7443         return 0;
7444 }
7445
7446 static int l2cap_debugfs_open(struct inode *inode, struct file *file)
7447 {
7448         return single_open(file, l2cap_debugfs_show, inode->i_private);
7449 }
7450
7451 static const struct file_operations l2cap_debugfs_fops = {
7452         .open           = l2cap_debugfs_open,
7453         .read           = seq_read,
7454         .llseek         = seq_lseek,
7455         .release        = single_release,
7456 };
7457
7458 static struct dentry *l2cap_debugfs;
7459
7460 int __init l2cap_init(void)
7461 {
7462         int err;
7463
7464         err = l2cap_init_sockets();
7465         if (err < 0)
7466                 return err;
7467
7468         if (IS_ERR_OR_NULL(bt_debugfs))
7469                 return 0;
7470
7471         l2cap_debugfs = debugfs_create_file("l2cap", 0444, bt_debugfs,
7472                                             NULL, &l2cap_debugfs_fops);
7473
7474         debugfs_create_u16("l2cap_le_max_credits", 0466, bt_debugfs,
7475                            &le_max_credits);
7476         debugfs_create_u16("l2cap_le_default_mps", 0466, bt_debugfs,
7477                            &le_default_mps);
7478
7479         bt_6lowpan_init();
7480
7481         return 0;
7482 }
7483
7484 void l2cap_exit(void)
7485 {
7486         bt_6lowpan_cleanup();
7487         debugfs_remove(l2cap_debugfs);
7488         l2cap_cleanup_sockets();
7489 }
7490
7491 module_param(disable_ertm, bool, 0644);
7492 MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode");