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