net/bluetooth/l2cap_core.c

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