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