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