net/bluetooth/l2cap_core.c

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