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