linux/net/bluetooth/l2cap_core.c
<<
>>
Prefs
   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#include <net/bluetooth/smp.h>
  40#include <net/bluetooth/a2mp.h>
  41
  42bool disable_ertm;
  43
  44static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN;
  45static u8 l2cap_fixed_chan[8] = { L2CAP_FC_L2CAP, };
  46
  47static LIST_HEAD(chan_list);
  48static DEFINE_RWLOCK(chan_list_lock);
  49
  50static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
  51                                u8 code, u8 ident, u16 dlen, void *data);
  52static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len,
  53                                                                void *data);
  54static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data);
  55static void l2cap_send_disconn_req(struct l2cap_conn *conn,
  56                                   struct l2cap_chan *chan, int err);
  57
  58static void l2cap_tx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
  59                    struct sk_buff_head *skbs, u8 event);
  60
  61/* ---- L2CAP channels ---- */
  62
  63static struct l2cap_chan *__l2cap_get_chan_by_dcid(struct l2cap_conn *conn, u16 cid)
  64{
  65        struct l2cap_chan *c;
  66
  67        list_for_each_entry(c, &conn->chan_l, list) {
  68                if (c->dcid == cid)
  69                        return c;
  70        }
  71        return NULL;
  72}
  73
  74static struct l2cap_chan *__l2cap_get_chan_by_scid(struct l2cap_conn *conn, u16 cid)
  75{
  76        struct l2cap_chan *c;
  77
  78        list_for_each_entry(c, &conn->chan_l, list) {
  79                if (c->scid == cid)
  80                        return c;
  81        }
  82        return NULL;
  83}
  84
  85/* Find channel with given SCID.
  86 * Returns locked channel. */
  87static struct l2cap_chan *l2cap_get_chan_by_scid(struct l2cap_conn *conn, u16 cid)
  88{
  89        struct l2cap_chan *c;
  90
  91        mutex_lock(&conn->chan_lock);
  92        c = __l2cap_get_chan_by_scid(conn, cid);
  93        if (c)
  94                l2cap_chan_lock(c);
  95        mutex_unlock(&conn->chan_lock);
  96
  97        return c;
  98}
  99
 100static struct l2cap_chan *__l2cap_get_chan_by_ident(struct l2cap_conn *conn, u8 ident)
 101{
 102        struct l2cap_chan *c;
 103
 104        list_for_each_entry(c, &conn->chan_l, list) {
 105                if (c->ident == ident)
 106                        return c;
 107        }
 108        return NULL;
 109}
 110
 111static struct l2cap_chan *__l2cap_global_chan_by_addr(__le16 psm, bdaddr_t *src)
 112{
 113        struct l2cap_chan *c;
 114
 115        list_for_each_entry(c, &chan_list, global_l) {
 116                if (c->sport == psm && !bacmp(&bt_sk(c->sk)->src, src))
 117                        return c;
 118        }
 119        return NULL;
 120}
 121
 122int l2cap_add_psm(struct l2cap_chan *chan, bdaddr_t *src, __le16 psm)
 123{
 124        int err;
 125
 126        write_lock(&chan_list_lock);
 127
 128        if (psm && __l2cap_global_chan_by_addr(psm, src)) {
 129                err = -EADDRINUSE;
 130                goto done;
 131        }
 132
 133        if (psm) {
 134                chan->psm = psm;
 135                chan->sport = psm;
 136                err = 0;
 137        } else {
 138                u16 p;
 139
 140                err = -EINVAL;
 141                for (p = 0x1001; p < 0x1100; p += 2)
 142                        if (!__l2cap_global_chan_by_addr(cpu_to_le16(p), src)) {
 143                                chan->psm   = cpu_to_le16(p);
 144                                chan->sport = cpu_to_le16(p);
 145                                err = 0;
 146                                break;
 147                        }
 148        }
 149
 150done:
 151        write_unlock(&chan_list_lock);
 152        return err;
 153}
 154
 155int l2cap_add_scid(struct l2cap_chan *chan,  __u16 scid)
 156{
 157        write_lock(&chan_list_lock);
 158
 159        chan->scid = scid;
 160
 161        write_unlock(&chan_list_lock);
 162
 163        return 0;
 164}
 165
 166static u16 l2cap_alloc_cid(struct l2cap_conn *conn)
 167{
 168        u16 cid = L2CAP_CID_DYN_START;
 169
 170        for (; cid < L2CAP_CID_DYN_END; cid++) {
 171                if (!__l2cap_get_chan_by_scid(conn, cid))
 172                        return cid;
 173        }
 174
 175        return 0;
 176}
 177
 178static void __l2cap_state_change(struct l2cap_chan *chan, int state)
 179{
 180        BT_DBG("chan %p %s -> %s", chan, state_to_string(chan->state),
 181                                                state_to_string(state));
 182
 183        chan->state = state;
 184        chan->ops->state_change(chan, state);
 185}
 186
 187static void l2cap_state_change(struct l2cap_chan *chan, int state)
 188{
 189        struct sock *sk = chan->sk;
 190
 191        lock_sock(sk);
 192        __l2cap_state_change(chan, state);
 193        release_sock(sk);
 194}
 195
 196static inline void __l2cap_chan_set_err(struct l2cap_chan *chan, int err)
 197{
 198        struct sock *sk = chan->sk;
 199
 200        sk->sk_err = err;
 201}
 202
 203static inline void l2cap_chan_set_err(struct l2cap_chan *chan, int err)
 204{
 205        struct sock *sk = chan->sk;
 206
 207        lock_sock(sk);
 208        __l2cap_chan_set_err(chan, err);
 209        release_sock(sk);
 210}
 211
 212static void __set_retrans_timer(struct l2cap_chan *chan)
 213{
 214        if (!delayed_work_pending(&chan->monitor_timer) &&
 215            chan->retrans_timeout) {
 216                l2cap_set_timer(chan, &chan->retrans_timer,
 217                                msecs_to_jiffies(chan->retrans_timeout));
 218        }
 219}
 220
 221static void __set_monitor_timer(struct l2cap_chan *chan)
 222{
 223        __clear_retrans_timer(chan);
 224        if (chan->monitor_timeout) {
 225                l2cap_set_timer(chan, &chan->monitor_timer,
 226                                msecs_to_jiffies(chan->monitor_timeout));
 227        }
 228}
 229
 230static struct sk_buff *l2cap_ertm_seq_in_queue(struct sk_buff_head *head,
 231                                               u16 seq)
 232{
 233        struct sk_buff *skb;
 234
 235        skb_queue_walk(head, skb) {
 236                if (bt_cb(skb)->control.txseq == seq)
 237                        return skb;
 238        }
 239
 240        return NULL;
 241}
 242
 243/* ---- L2CAP sequence number lists ---- */
 244
 245/* For ERTM, ordered lists of sequence numbers must be tracked for
 246 * SREJ requests that are received and for frames that are to be
 247 * retransmitted. These seq_list functions implement a singly-linked
 248 * list in an array, where membership in the list can also be checked
 249 * in constant time. Items can also be added to the tail of the list
 250 * and removed from the head in constant time, without further memory
 251 * allocs or frees.
 252 */
 253
 254static int l2cap_seq_list_init(struct l2cap_seq_list *seq_list, u16 size)
 255{
 256        size_t alloc_size, i;
 257
 258        /* Allocated size is a power of 2 to map sequence numbers
 259         * (which may be up to 14 bits) in to a smaller array that is
 260         * sized for the negotiated ERTM transmit windows.
 261         */
 262        alloc_size = roundup_pow_of_two(size);
 263
 264        seq_list->list = kmalloc(sizeof(u16) * alloc_size, GFP_KERNEL);
 265        if (!seq_list->list)
 266                return -ENOMEM;
 267
 268        seq_list->mask = alloc_size - 1;
 269        seq_list->head = L2CAP_SEQ_LIST_CLEAR;
 270        seq_list->tail = L2CAP_SEQ_LIST_CLEAR;
 271        for (i = 0; i < alloc_size; i++)
 272                seq_list->list[i] = L2CAP_SEQ_LIST_CLEAR;
 273
 274        return 0;
 275}
 276
 277static inline void l2cap_seq_list_free(struct l2cap_seq_list *seq_list)
 278{
 279        kfree(seq_list->list);
 280}
 281
 282static inline bool l2cap_seq_list_contains(struct l2cap_seq_list *seq_list,
 283                                           u16 seq)
 284{
 285        /* Constant-time check for list membership */
 286        return seq_list->list[seq & seq_list->mask] != L2CAP_SEQ_LIST_CLEAR;
 287}
 288
 289static u16 l2cap_seq_list_remove(struct l2cap_seq_list *seq_list, u16 seq)
 290{
 291        u16 mask = seq_list->mask;
 292
 293        if (seq_list->head == L2CAP_SEQ_LIST_CLEAR) {
 294                /* In case someone tries to pop the head of an empty list */
 295                return L2CAP_SEQ_LIST_CLEAR;
 296        } else if (seq_list->head == seq) {
 297                /* Head can be removed in constant time */
 298                seq_list->head = seq_list->list[seq & mask];
 299                seq_list->list[seq & mask] = L2CAP_SEQ_LIST_CLEAR;
 300
 301                if (seq_list->head == L2CAP_SEQ_LIST_TAIL) {
 302                        seq_list->head = L2CAP_SEQ_LIST_CLEAR;
 303                        seq_list->tail = L2CAP_SEQ_LIST_CLEAR;
 304                }
 305        } else {
 306                /* Walk the list to find the sequence number */
 307                u16 prev = seq_list->head;
 308                while (seq_list->list[prev & mask] != seq) {
 309                        prev = seq_list->list[prev & mask];
 310                        if (prev == L2CAP_SEQ_LIST_TAIL)
 311                                return L2CAP_SEQ_LIST_CLEAR;
 312                }
 313
 314                /* Unlink the number from the list and clear it */
 315                seq_list->list[prev & mask] = seq_list->list[seq & mask];
 316                seq_list->list[seq & mask] = L2CAP_SEQ_LIST_CLEAR;
 317                if (seq_list->tail == seq)
 318                        seq_list->tail = prev;
 319        }
 320        return seq;
 321}
 322
 323static inline u16 l2cap_seq_list_pop(struct l2cap_seq_list *seq_list)
 324{
 325        /* Remove the head in constant time */
 326        return l2cap_seq_list_remove(seq_list, seq_list->head);
 327}
 328
 329static void l2cap_seq_list_clear(struct l2cap_seq_list *seq_list)
 330{
 331        u16 i;
 332
 333        if (seq_list->head == L2CAP_SEQ_LIST_CLEAR)
 334                return;
 335
 336        for (i = 0; i <= seq_list->mask; i++)
 337                seq_list->list[i] = L2CAP_SEQ_LIST_CLEAR;
 338
 339        seq_list->head = L2CAP_SEQ_LIST_CLEAR;
 340        seq_list->tail = L2CAP_SEQ_LIST_CLEAR;
 341}
 342
 343static void l2cap_seq_list_append(struct l2cap_seq_list *seq_list, u16 seq)
 344{
 345        u16 mask = seq_list->mask;
 346
 347        /* All appends happen in constant time */
 348
 349        if (seq_list->list[seq & mask] != L2CAP_SEQ_LIST_CLEAR)
 350                return;
 351
 352        if (seq_list->tail == L2CAP_SEQ_LIST_CLEAR)
 353                seq_list->head = seq;
 354        else
 355                seq_list->list[seq_list->tail & mask] = seq;
 356
 357        seq_list->tail = seq;
 358        seq_list->list[seq & mask] = L2CAP_SEQ_LIST_TAIL;
 359}
 360
 361static void l2cap_chan_timeout(struct work_struct *work)
 362{
 363        struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
 364                                                        chan_timer.work);
 365        struct l2cap_conn *conn = chan->conn;
 366        int reason;
 367
 368        BT_DBG("chan %p state %s", chan, state_to_string(chan->state));
 369
 370        mutex_lock(&conn->chan_lock);
 371        l2cap_chan_lock(chan);
 372
 373        if (chan->state == BT_CONNECTED || chan->state == BT_CONFIG)
 374                reason = ECONNREFUSED;
 375        else if (chan->state == BT_CONNECT &&
 376                                        chan->sec_level != BT_SECURITY_SDP)
 377                reason = ECONNREFUSED;
 378        else
 379                reason = ETIMEDOUT;
 380
 381        l2cap_chan_close(chan, reason);
 382
 383        l2cap_chan_unlock(chan);
 384
 385        chan->ops->close(chan);
 386        mutex_unlock(&conn->chan_lock);
 387
 388        l2cap_chan_put(chan);
 389}
 390
 391struct l2cap_chan *l2cap_chan_create(void)
 392{
 393        struct l2cap_chan *chan;
 394
 395        chan = kzalloc(sizeof(*chan), GFP_ATOMIC);
 396        if (!chan)
 397                return NULL;
 398
 399        mutex_init(&chan->lock);
 400
 401        write_lock(&chan_list_lock);
 402        list_add(&chan->global_l, &chan_list);
 403        write_unlock(&chan_list_lock);
 404
 405        INIT_DELAYED_WORK(&chan->chan_timer, l2cap_chan_timeout);
 406
 407        chan->state = BT_OPEN;
 408
 409        atomic_set(&chan->refcnt, 1);
 410
 411        /* This flag is cleared in l2cap_chan_ready() */
 412        set_bit(CONF_NOT_COMPLETE, &chan->conf_state);
 413
 414        BT_DBG("chan %p", chan);
 415
 416        return chan;
 417}
 418
 419void l2cap_chan_destroy(struct l2cap_chan *chan)
 420{
 421        write_lock(&chan_list_lock);
 422        list_del(&chan->global_l);
 423        write_unlock(&chan_list_lock);
 424
 425        l2cap_chan_put(chan);
 426}
 427
 428void l2cap_chan_set_defaults(struct l2cap_chan *chan)
 429{
 430        chan->fcs  = L2CAP_FCS_CRC16;
 431        chan->max_tx = L2CAP_DEFAULT_MAX_TX;
 432        chan->tx_win = L2CAP_DEFAULT_TX_WINDOW;
 433        chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
 434        chan->ack_win = L2CAP_DEFAULT_TX_WINDOW;
 435        chan->sec_level = BT_SECURITY_LOW;
 436
 437        set_bit(FLAG_FORCE_ACTIVE, &chan->flags);
 438}
 439
 440static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 441{
 442        BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn,
 443               __le16_to_cpu(chan->psm), chan->dcid);
 444
 445        conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM;
 446
 447        chan->conn = conn;
 448
 449        switch (chan->chan_type) {
 450        case L2CAP_CHAN_CONN_ORIENTED:
 451                if (conn->hcon->type == LE_LINK) {
 452                        /* LE connection */
 453                        chan->omtu = L2CAP_DEFAULT_MTU;
 454                        chan->scid = L2CAP_CID_LE_DATA;
 455                        chan->dcid = L2CAP_CID_LE_DATA;
 456                } else {
 457                        /* Alloc CID for connection-oriented socket */
 458                        chan->scid = l2cap_alloc_cid(conn);
 459                        chan->omtu = L2CAP_DEFAULT_MTU;
 460                }
 461                break;
 462
 463        case L2CAP_CHAN_CONN_LESS:
 464                /* Connectionless socket */
 465                chan->scid = L2CAP_CID_CONN_LESS;
 466                chan->dcid = L2CAP_CID_CONN_LESS;
 467                chan->omtu = L2CAP_DEFAULT_MTU;
 468                break;
 469
 470        case L2CAP_CHAN_CONN_FIX_A2MP:
 471                chan->scid = L2CAP_CID_A2MP;
 472                chan->dcid = L2CAP_CID_A2MP;
 473                chan->omtu = L2CAP_A2MP_DEFAULT_MTU;
 474                chan->imtu = L2CAP_A2MP_DEFAULT_MTU;
 475                break;
 476
 477        default:
 478                /* Raw socket can send/recv signalling messages only */
 479                chan->scid = L2CAP_CID_SIGNALING;
 480                chan->dcid = L2CAP_CID_SIGNALING;
 481                chan->omtu = L2CAP_DEFAULT_MTU;
 482        }
 483
 484        chan->local_id          = L2CAP_BESTEFFORT_ID;
 485        chan->local_stype       = L2CAP_SERV_BESTEFFORT;
 486        chan->local_msdu        = L2CAP_DEFAULT_MAX_SDU_SIZE;
 487        chan->local_sdu_itime   = L2CAP_DEFAULT_SDU_ITIME;
 488        chan->local_acc_lat     = L2CAP_DEFAULT_ACC_LAT;
 489        chan->local_flush_to    = L2CAP_DEFAULT_FLUSH_TO;
 490
 491        l2cap_chan_hold(chan);
 492
 493        list_add(&chan->list, &conn->chan_l);
 494}
 495
 496void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 497{
 498        mutex_lock(&conn->chan_lock);
 499        __l2cap_chan_add(conn, chan);
 500        mutex_unlock(&conn->chan_lock);
 501}
 502
 503void l2cap_chan_del(struct l2cap_chan *chan, int err)
 504{
 505        struct l2cap_conn *conn = chan->conn;
 506
 507        __clear_chan_timer(chan);
 508
 509        BT_DBG("chan %p, conn %p, err %d", chan, conn, err);
 510
 511        if (conn) {
 512                /* Delete from channel list */
 513                list_del(&chan->list);
 514
 515                l2cap_chan_put(chan);
 516
 517                chan->conn = NULL;
 518
 519                if (chan->chan_type != L2CAP_CHAN_CONN_FIX_A2MP)
 520                        hci_conn_put(conn->hcon);
 521        }
 522
 523        if (chan->ops->teardown)
 524                chan->ops->teardown(chan, err);
 525
 526        if (test_bit(CONF_NOT_COMPLETE, &chan->conf_state))
 527                return;
 528
 529        switch(chan->mode) {
 530        case L2CAP_MODE_BASIC:
 531                break;
 532
 533        case L2CAP_MODE_ERTM:
 534                __clear_retrans_timer(chan);
 535                __clear_monitor_timer(chan);
 536                __clear_ack_timer(chan);
 537
 538                skb_queue_purge(&chan->srej_q);
 539
 540                l2cap_seq_list_free(&chan->srej_list);
 541                l2cap_seq_list_free(&chan->retrans_list);
 542
 543                /* fall through */
 544
 545        case L2CAP_MODE_STREAMING:
 546                skb_queue_purge(&chan->tx_q);
 547                break;
 548        }
 549
 550        return;
 551}
 552
 553void l2cap_chan_close(struct l2cap_chan *chan, int reason)
 554{
 555        struct l2cap_conn *conn = chan->conn;
 556        struct sock *sk = chan->sk;
 557
 558        BT_DBG("chan %p state %s sk %p", chan,
 559                                        state_to_string(chan->state), sk);
 560
 561        switch (chan->state) {
 562        case BT_LISTEN:
 563                if (chan->ops->teardown)
 564                        chan->ops->teardown(chan, 0);
 565                break;
 566
 567        case BT_CONNECTED:
 568        case BT_CONFIG:
 569                if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED &&
 570                                        conn->hcon->type == ACL_LINK) {
 571                        __set_chan_timer(chan, sk->sk_sndtimeo);
 572                        l2cap_send_disconn_req(conn, chan, reason);
 573                } else
 574                        l2cap_chan_del(chan, reason);
 575                break;
 576
 577        case BT_CONNECT2:
 578                if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED &&
 579                                        conn->hcon->type == ACL_LINK) {
 580                        struct l2cap_conn_rsp rsp;
 581                        __u16 result;
 582
 583                        if (test_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags))
 584                                result = L2CAP_CR_SEC_BLOCK;
 585                        else
 586                                result = L2CAP_CR_BAD_PSM;
 587                        l2cap_state_change(chan, BT_DISCONN);
 588
 589                        rsp.scid   = cpu_to_le16(chan->dcid);
 590                        rsp.dcid   = cpu_to_le16(chan->scid);
 591                        rsp.result = cpu_to_le16(result);
 592                        rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
 593                        l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
 594                                                        sizeof(rsp), &rsp);
 595                }
 596
 597                l2cap_chan_del(chan, reason);
 598                break;
 599
 600        case BT_CONNECT:
 601        case BT_DISCONN:
 602                l2cap_chan_del(chan, reason);
 603                break;
 604
 605        default:
 606                if (chan->ops->teardown)
 607                        chan->ops->teardown(chan, 0);
 608                break;
 609        }
 610}
 611
 612static inline u8 l2cap_get_auth_type(struct l2cap_chan *chan)
 613{
 614        if (chan->chan_type == L2CAP_CHAN_RAW) {
 615                switch (chan->sec_level) {
 616                case BT_SECURITY_HIGH:
 617                        return HCI_AT_DEDICATED_BONDING_MITM;
 618                case BT_SECURITY_MEDIUM:
 619                        return HCI_AT_DEDICATED_BONDING;
 620                default:
 621                        return HCI_AT_NO_BONDING;
 622                }
 623        } else if (chan->psm == __constant_cpu_to_le16(L2CAP_PSM_SDP)) {
 624                if (chan->sec_level == BT_SECURITY_LOW)
 625                        chan->sec_level = BT_SECURITY_SDP;
 626
 627                if (chan->sec_level == BT_SECURITY_HIGH)
 628                        return HCI_AT_NO_BONDING_MITM;
 629                else
 630                        return HCI_AT_NO_BONDING;
 631        } else {
 632                switch (chan->sec_level) {
 633                case BT_SECURITY_HIGH:
 634                        return HCI_AT_GENERAL_BONDING_MITM;
 635                case BT_SECURITY_MEDIUM:
 636                        return HCI_AT_GENERAL_BONDING;
 637                default:
 638                        return HCI_AT_NO_BONDING;
 639                }
 640        }
 641}
 642
 643/* Service level security */
 644int l2cap_chan_check_security(struct l2cap_chan *chan)
 645{
 646        struct l2cap_conn *conn = chan->conn;
 647        __u8 auth_type;
 648
 649        auth_type = l2cap_get_auth_type(chan);
 650
 651        return hci_conn_security(conn->hcon, chan->sec_level, auth_type);
 652}
 653
 654static u8 l2cap_get_ident(struct l2cap_conn *conn)
 655{
 656        u8 id;
 657
 658        /* Get next available identificator.
 659         *    1 - 128 are used by kernel.
 660         *  129 - 199 are reserved.
 661         *  200 - 254 are used by utilities like l2ping, etc.
 662         */
 663
 664        spin_lock(&conn->lock);
 665
 666        if (++conn->tx_ident > 128)
 667                conn->tx_ident = 1;
 668
 669        id = conn->tx_ident;
 670
 671        spin_unlock(&conn->lock);
 672
 673        return id;
 674}
 675
 676static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, void *data)
 677{
 678        struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data);
 679        u8 flags;
 680
 681        BT_DBG("code 0x%2.2x", code);
 682
 683        if (!skb)
 684                return;
 685
 686        if (lmp_no_flush_capable(conn->hcon->hdev))
 687                flags = ACL_START_NO_FLUSH;
 688        else
 689                flags = ACL_START;
 690
 691        bt_cb(skb)->force_active = BT_POWER_FORCE_ACTIVE_ON;
 692        skb->priority = HCI_PRIO_MAX;
 693
 694        hci_send_acl(conn->hchan, skb, flags);
 695}
 696
 697static void l2cap_do_send(struct l2cap_chan *chan, struct sk_buff *skb)
 698{
 699        struct hci_conn *hcon = chan->conn->hcon;
 700        u16 flags;
 701
 702        BT_DBG("chan %p, skb %p len %d priority %u", chan, skb, skb->len,
 703                                                        skb->priority);
 704
 705        if (!test_bit(FLAG_FLUSHABLE, &chan->flags) &&
 706                                        lmp_no_flush_capable(hcon->hdev))
 707                flags = ACL_START_NO_FLUSH;
 708        else
 709                flags = ACL_START;
 710
 711        bt_cb(skb)->force_active = test_bit(FLAG_FORCE_ACTIVE, &chan->flags);
 712        hci_send_acl(chan->conn->hchan, skb, flags);
 713}
 714
 715static void __unpack_enhanced_control(u16 enh, struct l2cap_ctrl *control)
 716{
 717        control->reqseq = (enh & L2CAP_CTRL_REQSEQ) >> L2CAP_CTRL_REQSEQ_SHIFT;
 718        control->final = (enh & L2CAP_CTRL_FINAL) >> L2CAP_CTRL_FINAL_SHIFT;
 719
 720        if (enh & L2CAP_CTRL_FRAME_TYPE) {
 721                /* S-Frame */
 722                control->sframe = 1;
 723                control->poll = (enh & L2CAP_CTRL_POLL) >> L2CAP_CTRL_POLL_SHIFT;
 724                control->super = (enh & L2CAP_CTRL_SUPERVISE) >> L2CAP_CTRL_SUPER_SHIFT;
 725
 726                control->sar = 0;
 727                control->txseq = 0;
 728        } else {
 729                /* I-Frame */
 730                control->sframe = 0;
 731                control->sar = (enh & L2CAP_CTRL_SAR) >> L2CAP_CTRL_SAR_SHIFT;
 732                control->txseq = (enh & L2CAP_CTRL_TXSEQ) >> L2CAP_CTRL_TXSEQ_SHIFT;
 733
 734                control->poll = 0;
 735                control->super = 0;
 736        }
 737}
 738
 739static void __unpack_extended_control(u32 ext, struct l2cap_ctrl *control)
 740{
 741        control->reqseq = (ext & L2CAP_EXT_CTRL_REQSEQ) >> L2CAP_EXT_CTRL_REQSEQ_SHIFT;
 742        control->final = (ext & L2CAP_EXT_CTRL_FINAL) >> L2CAP_EXT_CTRL_FINAL_SHIFT;
 743
 744        if (ext & L2CAP_EXT_CTRL_FRAME_TYPE) {
 745                /* S-Frame */
 746                control->sframe = 1;
 747                control->poll = (ext & L2CAP_EXT_CTRL_POLL) >> L2CAP_EXT_CTRL_POLL_SHIFT;
 748                control->super = (ext & L2CAP_EXT_CTRL_SUPERVISE) >> L2CAP_EXT_CTRL_SUPER_SHIFT;
 749
 750                control->sar = 0;
 751                control->txseq = 0;
 752        } else {
 753                /* I-Frame */
 754                control->sframe = 0;
 755                control->sar = (ext & L2CAP_EXT_CTRL_SAR) >> L2CAP_EXT_CTRL_SAR_SHIFT;
 756                control->txseq = (ext & L2CAP_EXT_CTRL_TXSEQ) >> L2CAP_EXT_CTRL_TXSEQ_SHIFT;
 757
 758                control->poll = 0;
 759                control->super = 0;
 760        }
 761}
 762
 763static inline void __unpack_control(struct l2cap_chan *chan,
 764                                    struct sk_buff *skb)
 765{
 766        if (test_bit(FLAG_EXT_CTRL, &chan->flags)) {
 767                __unpack_extended_control(get_unaligned_le32(skb->data),
 768                                          &bt_cb(skb)->control);
 769                skb_pull(skb, L2CAP_EXT_CTRL_SIZE);
 770        } else {
 771                __unpack_enhanced_control(get_unaligned_le16(skb->data),
 772                                          &bt_cb(skb)->control);
 773                skb_pull(skb, L2CAP_ENH_CTRL_SIZE);
 774        }
 775}
 776
 777static u32 __pack_extended_control(struct l2cap_ctrl *control)
 778{
 779        u32 packed;
 780
 781        packed = control->reqseq << L2CAP_EXT_CTRL_REQSEQ_SHIFT;
 782        packed |= control->final << L2CAP_EXT_CTRL_FINAL_SHIFT;
 783
 784        if (control->sframe) {
 785                packed |= control->poll << L2CAP_EXT_CTRL_POLL_SHIFT;
 786                packed |= control->super << L2CAP_EXT_CTRL_SUPER_SHIFT;
 787                packed |= L2CAP_EXT_CTRL_FRAME_TYPE;
 788        } else {
 789                packed |= control->sar << L2CAP_EXT_CTRL_SAR_SHIFT;
 790                packed |= control->txseq << L2CAP_EXT_CTRL_TXSEQ_SHIFT;
 791        }
 792
 793        return packed;
 794}
 795
 796static u16 __pack_enhanced_control(struct l2cap_ctrl *control)
 797{
 798        u16 packed;
 799
 800        packed = control->reqseq << L2CAP_CTRL_REQSEQ_SHIFT;
 801        packed |= control->final << L2CAP_CTRL_FINAL_SHIFT;
 802
 803        if (control->sframe) {
 804                packed |= control->poll << L2CAP_CTRL_POLL_SHIFT;
 805                packed |= control->super << L2CAP_CTRL_SUPER_SHIFT;
 806                packed |= L2CAP_CTRL_FRAME_TYPE;
 807        } else {
 808                packed |= control->sar << L2CAP_CTRL_SAR_SHIFT;
 809                packed |= control->txseq << L2CAP_CTRL_TXSEQ_SHIFT;
 810        }
 811
 812        return packed;
 813}
 814
 815static inline void __pack_control(struct l2cap_chan *chan,
 816                                  struct l2cap_ctrl *control,
 817                                  struct sk_buff *skb)
 818{
 819        if (test_bit(FLAG_EXT_CTRL, &chan->flags)) {
 820                put_unaligned_le32(__pack_extended_control(control),
 821                                   skb->data + L2CAP_HDR_SIZE);
 822        } else {
 823                put_unaligned_le16(__pack_enhanced_control(control),
 824                                   skb->data + L2CAP_HDR_SIZE);
 825        }
 826}
 827
 828static inline unsigned int __ertm_hdr_size(struct l2cap_chan *chan)
 829{
 830        if (test_bit(FLAG_EXT_CTRL, &chan->flags))
 831                return L2CAP_EXT_HDR_SIZE;
 832        else
 833                return L2CAP_ENH_HDR_SIZE;
 834}
 835
 836static struct sk_buff *l2cap_create_sframe_pdu(struct l2cap_chan *chan,
 837                                               u32 control)
 838{
 839        struct sk_buff *skb;
 840        struct l2cap_hdr *lh;
 841        int hlen = __ertm_hdr_size(chan);
 842
 843        if (chan->fcs == L2CAP_FCS_CRC16)
 844                hlen += L2CAP_FCS_SIZE;
 845
 846        skb = bt_skb_alloc(hlen, GFP_KERNEL);
 847
 848        if (!skb)
 849                return ERR_PTR(-ENOMEM);
 850
 851        lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
 852        lh->len = cpu_to_le16(hlen - L2CAP_HDR_SIZE);
 853        lh->cid = cpu_to_le16(chan->dcid);
 854
 855        if (test_bit(FLAG_EXT_CTRL, &chan->flags))
 856                put_unaligned_le32(control, skb_put(skb, L2CAP_EXT_CTRL_SIZE));
 857        else
 858                put_unaligned_le16(control, skb_put(skb, L2CAP_ENH_CTRL_SIZE));
 859
 860        if (chan->fcs == L2CAP_FCS_CRC16) {
 861                u16 fcs = crc16(0, (u8 *)skb->data, skb->len);
 862                put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE));
 863        }
 864
 865        skb->priority = HCI_PRIO_MAX;
 866        return skb;
 867}
 868
 869static void l2cap_send_sframe(struct l2cap_chan *chan,
 870                              struct l2cap_ctrl *control)
 871{
 872        struct sk_buff *skb;
 873        u32 control_field;
 874
 875        BT_DBG("chan %p, control %p", chan, control);
 876
 877        if (!control->sframe)
 878                return;
 879
 880        if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state) &&
 881            !control->poll)
 882                control->final = 1;
 883
 884        if (control->super == L2CAP_SUPER_RR)
 885                clear_bit(CONN_RNR_SENT, &chan->conn_state);
 886        else if (control->super == L2CAP_SUPER_RNR)
 887                set_bit(CONN_RNR_SENT, &chan->conn_state);
 888
 889        if (control->super != L2CAP_SUPER_SREJ) {
 890                chan->last_acked_seq = control->reqseq;
 891                __clear_ack_timer(chan);
 892        }
 893
 894        BT_DBG("reqseq %d, final %d, poll %d, super %d", control->reqseq,
 895               control->final, control->poll, control->super);
 896
 897        if (test_bit(FLAG_EXT_CTRL, &chan->flags))
 898                control_field = __pack_extended_control(control);
 899        else
 900                control_field = __pack_enhanced_control(control);
 901
 902        skb = l2cap_create_sframe_pdu(chan, control_field);
 903        if (!IS_ERR(skb))
 904                l2cap_do_send(chan, skb);
 905}
 906
 907static void l2cap_send_rr_or_rnr(struct l2cap_chan *chan, bool poll)
 908{
 909        struct l2cap_ctrl control;
 910
 911        BT_DBG("chan %p, poll %d", chan, poll);
 912
 913        memset(&control, 0, sizeof(control));
 914        control.sframe = 1;
 915        control.poll = poll;
 916
 917        if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state))
 918                control.super = L2CAP_SUPER_RNR;
 919        else
 920                control.super = L2CAP_SUPER_RR;
 921
 922        control.reqseq = chan->buffer_seq;
 923        l2cap_send_sframe(chan, &control);
 924}
 925
 926static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan)
 927{
 928        return !test_bit(CONF_CONNECT_PEND, &chan->conf_state);
 929}
 930
 931static void l2cap_send_conn_req(struct l2cap_chan *chan)
 932{
 933        struct l2cap_conn *conn = chan->conn;
 934        struct l2cap_conn_req req;
 935
 936        req.scid = cpu_to_le16(chan->scid);
 937        req.psm  = chan->psm;
 938
 939        chan->ident = l2cap_get_ident(conn);
 940
 941        set_bit(CONF_CONNECT_PEND, &chan->conf_state);
 942
 943        l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_REQ, sizeof(req), &req);
 944}
 945
 946static void l2cap_chan_ready(struct l2cap_chan *chan)
 947{
 948        /* This clears all conf flags, including CONF_NOT_COMPLETE */
 949        chan->conf_state = 0;
 950        __clear_chan_timer(chan);
 951
 952        chan->state = BT_CONNECTED;
 953
 954        chan->ops->ready(chan);
 955}
 956
 957static void l2cap_do_start(struct l2cap_chan *chan)
 958{
 959        struct l2cap_conn *conn = chan->conn;
 960
 961        if (conn->hcon->type == LE_LINK) {
 962                l2cap_chan_ready(chan);
 963                return;
 964        }
 965
 966        if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) {
 967                if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE))
 968                        return;
 969
 970                if (l2cap_chan_check_security(chan) &&
 971                                __l2cap_no_conn_pending(chan))
 972                        l2cap_send_conn_req(chan);
 973        } else {
 974                struct l2cap_info_req req;
 975                req.type = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
 976
 977                conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
 978                conn->info_ident = l2cap_get_ident(conn);
 979
 980                schedule_delayed_work(&conn->info_timer, L2CAP_INFO_TIMEOUT);
 981
 982                l2cap_send_cmd(conn, conn->info_ident,
 983                                        L2CAP_INFO_REQ, sizeof(req), &req);
 984        }
 985}
 986
 987static inline int l2cap_mode_supported(__u8 mode, __u32 feat_mask)
 988{
 989        u32 local_feat_mask = l2cap_feat_mask;
 990        if (!disable_ertm)
 991                local_feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING;
 992
 993        switch (mode) {
 994        case L2CAP_MODE_ERTM:
 995                return L2CAP_FEAT_ERTM & feat_mask & local_feat_mask;
 996        case L2CAP_MODE_STREAMING:
 997                return L2CAP_FEAT_STREAMING & feat_mask & local_feat_mask;
 998        default:
 999                return 0x00;
1000        }
1001}
1002
1003static void l2cap_send_disconn_req(struct l2cap_conn *conn, struct l2cap_chan *chan, int err)
1004{
1005        struct sock *sk = chan->sk;
1006        struct l2cap_disconn_req req;
1007
1008        if (!conn)
1009                return;
1010
1011        if (chan->mode == L2CAP_MODE_ERTM && chan->state == BT_CONNECTED) {
1012                __clear_retrans_timer(chan);
1013                __clear_monitor_timer(chan);
1014                __clear_ack_timer(chan);
1015        }
1016
1017        if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP) {
1018                __l2cap_state_change(chan, BT_DISCONN);
1019                return;
1020        }
1021
1022        req.dcid = cpu_to_le16(chan->dcid);
1023        req.scid = cpu_to_le16(chan->scid);
1024        l2cap_send_cmd(conn, l2cap_get_ident(conn),
1025                        L2CAP_DISCONN_REQ, sizeof(req), &req);
1026
1027        lock_sock(sk);
1028        __l2cap_state_change(chan, BT_DISCONN);
1029        __l2cap_chan_set_err(chan, err);
1030        release_sock(sk);
1031}
1032
1033/* ---- L2CAP connections ---- */
1034static void l2cap_conn_start(struct l2cap_conn *conn)
1035{
1036        struct l2cap_chan *chan, *tmp;
1037
1038        BT_DBG("conn %p", conn);
1039
1040        mutex_lock(&conn->chan_lock);
1041
1042        list_for_each_entry_safe(chan, tmp, &conn->chan_l, list) {
1043                struct sock *sk = chan->sk;
1044
1045                l2cap_chan_lock(chan);
1046
1047                if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
1048                        l2cap_chan_unlock(chan);
1049                        continue;
1050                }
1051
1052                if (chan->state == BT_CONNECT) {
1053                        if (!l2cap_chan_check_security(chan) ||
1054                                        !__l2cap_no_conn_pending(chan)) {
1055                                l2cap_chan_unlock(chan);
1056                                continue;
1057                        }
1058
1059                        if (!l2cap_mode_supported(chan->mode, conn->feat_mask)
1060                                        && test_bit(CONF_STATE2_DEVICE,
1061                                        &chan->conf_state)) {
1062                                l2cap_chan_close(chan, ECONNRESET);
1063                                l2cap_chan_unlock(chan);
1064                                continue;
1065                        }
1066
1067                        l2cap_send_conn_req(chan);
1068
1069                } else if (chan->state == BT_CONNECT2) {
1070                        struct l2cap_conn_rsp rsp;
1071                        char buf[128];
1072                        rsp.scid = cpu_to_le16(chan->dcid);
1073                        rsp.dcid = cpu_to_le16(chan->scid);
1074
1075                        if (l2cap_chan_check_security(chan)) {
1076                                lock_sock(sk);
1077                                if (test_bit(BT_SK_DEFER_SETUP,
1078                                             &bt_sk(sk)->flags)) {
1079                                        struct sock *parent = bt_sk(sk)->parent;
1080                                        rsp.result = __constant_cpu_to_le16(L2CAP_CR_PEND);
1081                                        rsp.status = __constant_cpu_to_le16(L2CAP_CS_AUTHOR_PEND);
1082                                        if (parent)
1083                                                parent->sk_data_ready(parent, 0);
1084
1085                                } else {
1086                                        __l2cap_state_change(chan, BT_CONFIG);
1087                                        rsp.result = __constant_cpu_to_le16(L2CAP_CR_SUCCESS);
1088                                        rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
1089                                }
1090                                release_sock(sk);
1091                        } else {
1092                                rsp.result = __constant_cpu_to_le16(L2CAP_CR_PEND);
1093                                rsp.status = __constant_cpu_to_le16(L2CAP_CS_AUTHEN_PEND);
1094                        }
1095
1096                        l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
1097                                                        sizeof(rsp), &rsp);
1098
1099                        if (test_bit(CONF_REQ_SENT, &chan->conf_state) ||
1100                                        rsp.result != L2CAP_CR_SUCCESS) {
1101                                l2cap_chan_unlock(chan);
1102                                continue;
1103                        }
1104
1105                        set_bit(CONF_REQ_SENT, &chan->conf_state);
1106                        l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
1107                                                l2cap_build_conf_req(chan, buf), buf);
1108                        chan->num_conf_req++;
1109                }
1110
1111                l2cap_chan_unlock(chan);
1112        }
1113
1114        mutex_unlock(&conn->chan_lock);
1115}
1116
1117/* Find socket with cid and source/destination bdaddr.
1118 * Returns closest match, locked.
1119 */
1120static struct l2cap_chan *l2cap_global_chan_by_scid(int state, u16 cid,
1121                                                    bdaddr_t *src,
1122                                                    bdaddr_t *dst)
1123{
1124        struct l2cap_chan *c, *c1 = NULL;
1125
1126        read_lock(&chan_list_lock);
1127
1128        list_for_each_entry(c, &chan_list, global_l) {
1129                struct sock *sk = c->sk;
1130
1131                if (state && c->state != state)
1132                        continue;
1133
1134                if (c->scid == cid) {
1135                        int src_match, dst_match;
1136                        int src_any, dst_any;
1137
1138                        /* Exact match. */
1139                        src_match = !bacmp(&bt_sk(sk)->src, src);
1140                        dst_match = !bacmp(&bt_sk(sk)->dst, dst);
1141                        if (src_match && dst_match) {
1142                                read_unlock(&chan_list_lock);
1143                                return c;
1144                        }
1145
1146                        /* Closest match */
1147                        src_any = !bacmp(&bt_sk(sk)->src, BDADDR_ANY);
1148                        dst_any = !bacmp(&bt_sk(sk)->dst, BDADDR_ANY);
1149                        if ((src_match && dst_any) || (src_any && dst_match) ||
1150                            (src_any && dst_any))
1151                                c1 = c;
1152                }
1153        }
1154
1155        read_unlock(&chan_list_lock);
1156
1157        return c1;
1158}
1159
1160static void l2cap_le_conn_ready(struct l2cap_conn *conn)
1161{
1162        struct sock *parent, *sk;
1163        struct l2cap_chan *chan, *pchan;
1164
1165        BT_DBG("");
1166
1167        /* Check if we have socket listening on cid */
1168        pchan = l2cap_global_chan_by_scid(BT_LISTEN, L2CAP_CID_LE_DATA,
1169                                          conn->src, conn->dst);
1170        if (!pchan)
1171                return;
1172
1173        parent = pchan->sk;
1174
1175        lock_sock(parent);
1176
1177        chan = pchan->ops->new_connection(pchan);
1178        if (!chan)
1179                goto clean;
1180
1181        sk = chan->sk;
1182
1183        hci_conn_hold(conn->hcon);
1184        conn->hcon->disc_timeout = HCI_DISCONN_TIMEOUT;
1185
1186        bacpy(&bt_sk(sk)->src, conn->src);
1187        bacpy(&bt_sk(sk)->dst, conn->dst);
1188
1189        bt_accept_enqueue(parent, sk);
1190
1191        l2cap_chan_add(conn, chan);
1192
1193        l2cap_chan_ready(chan);
1194
1195clean:
1196        release_sock(parent);
1197}
1198
1199static void l2cap_conn_ready(struct l2cap_conn *conn)
1200{
1201        struct l2cap_chan *chan;
1202        struct hci_conn *hcon = conn->hcon;
1203
1204        BT_DBG("conn %p", conn);
1205
1206        if (!hcon->out && hcon->type == LE_LINK)
1207                l2cap_le_conn_ready(conn);
1208
1209        if (hcon->out && hcon->type == LE_LINK)
1210                smp_conn_security(hcon, hcon->pending_sec_level);
1211
1212        mutex_lock(&conn->chan_lock);
1213
1214        list_for_each_entry(chan, &conn->chan_l, list) {
1215
1216                l2cap_chan_lock(chan);
1217
1218                if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP) {
1219                        l2cap_chan_unlock(chan);
1220                        continue;
1221                }
1222
1223                if (hcon->type == LE_LINK) {
1224                        if (smp_conn_security(hcon, chan->sec_level))
1225                                l2cap_chan_ready(chan);
1226
1227                } else if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
1228                        struct sock *sk = chan->sk;
1229                        __clear_chan_timer(chan);
1230                        lock_sock(sk);
1231                        __l2cap_state_change(chan, BT_CONNECTED);
1232                        sk->sk_state_change(sk);
1233                        release_sock(sk);
1234
1235                } else if (chan->state == BT_CONNECT)
1236                        l2cap_do_start(chan);
1237
1238                l2cap_chan_unlock(chan);
1239        }
1240
1241        mutex_unlock(&conn->chan_lock);
1242}
1243
1244/* Notify sockets that we cannot guaranty reliability anymore */
1245static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err)
1246{
1247        struct l2cap_chan *chan;
1248
1249        BT_DBG("conn %p", conn);
1250
1251        mutex_lock(&conn->chan_lock);
1252
1253        list_for_each_entry(chan, &conn->chan_l, list) {
1254                if (test_bit(FLAG_FORCE_RELIABLE, &chan->flags))
1255                        __l2cap_chan_set_err(chan, err);
1256        }
1257
1258        mutex_unlock(&conn->chan_lock);
1259}
1260
1261static void l2cap_info_timeout(struct work_struct *work)
1262{
1263        struct l2cap_conn *conn = container_of(work, struct l2cap_conn,
1264                                                        info_timer.work);
1265
1266        conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
1267        conn->info_ident = 0;
1268
1269        l2cap_conn_start(conn);
1270}
1271
1272static void l2cap_conn_del(struct hci_conn *hcon, int err)
1273{
1274        struct l2cap_conn *conn = hcon->l2cap_data;
1275        struct l2cap_chan *chan, *l;
1276
1277        if (!conn)
1278                return;
1279
1280        BT_DBG("hcon %p conn %p, err %d", hcon, conn, err);
1281
1282        kfree_skb(conn->rx_skb);
1283
1284        mutex_lock(&conn->chan_lock);
1285
1286        /* Kill channels */
1287        list_for_each_entry_safe(chan, l, &conn->chan_l, list) {
1288                l2cap_chan_hold(chan);
1289                l2cap_chan_lock(chan);
1290
1291                l2cap_chan_del(chan, err);
1292
1293                l2cap_chan_unlock(chan);
1294
1295                chan->ops->close(chan);
1296                l2cap_chan_put(chan);
1297        }
1298
1299        mutex_unlock(&conn->chan_lock);
1300
1301        hci_chan_del(conn->hchan);
1302
1303        if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT)
1304                cancel_delayed_work_sync(&conn->info_timer);
1305
1306        if (test_and_clear_bit(HCI_CONN_LE_SMP_PEND, &hcon->flags)) {
1307                cancel_delayed_work_sync(&conn->security_timer);
1308                smp_chan_destroy(conn);
1309        }
1310
1311        hcon->l2cap_data = NULL;
1312        kfree(conn);
1313}
1314
1315static void security_timeout(struct work_struct *work)
1316{
1317        struct l2cap_conn *conn = container_of(work, struct l2cap_conn,
1318                                                security_timer.work);
1319
1320        BT_DBG("conn %p", conn);
1321
1322        if (test_and_clear_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->flags)) {
1323                smp_chan_destroy(conn);
1324                l2cap_conn_del(conn->hcon, ETIMEDOUT);
1325        }
1326}
1327
1328static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status)
1329{
1330        struct l2cap_conn *conn = hcon->l2cap_data;
1331        struct hci_chan *hchan;
1332
1333        if (conn || status)
1334                return conn;
1335
1336        hchan = hci_chan_create(hcon);
1337        if (!hchan)
1338                return NULL;
1339
1340        conn = kzalloc(sizeof(struct l2cap_conn), GFP_ATOMIC);
1341        if (!conn) {
1342                hci_chan_del(hchan);
1343                return NULL;
1344        }
1345
1346        hcon->l2cap_data = conn;
1347        conn->hcon = hcon;
1348        conn->hchan = hchan;
1349
1350        BT_DBG("hcon %p conn %p hchan %p", hcon, conn, hchan);
1351
1352        if (hcon->hdev->le_mtu && hcon->type == LE_LINK)
1353                conn->mtu = hcon->hdev->le_mtu;
1354        else
1355                conn->mtu = hcon->hdev->acl_mtu;
1356
1357        conn->src = &hcon->hdev->bdaddr;
1358        conn->dst = &hcon->dst;
1359
1360        conn->feat_mask = 0;
1361
1362        spin_lock_init(&conn->lock);
1363        mutex_init(&conn->chan_lock);
1364
1365        INIT_LIST_HEAD(&conn->chan_l);
1366
1367        if (hcon->type == LE_LINK)
1368                INIT_DELAYED_WORK(&conn->security_timer, security_timeout);
1369        else
1370                INIT_DELAYED_WORK(&conn->info_timer, l2cap_info_timeout);
1371
1372        conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM;
1373
1374        return conn;
1375}
1376
1377/* ---- Socket interface ---- */
1378
1379/* Find socket with psm and source / destination bdaddr.
1380 * Returns closest match.
1381 */
1382static struct l2cap_chan *l2cap_global_chan_by_psm(int state, __le16 psm,
1383                                                   bdaddr_t *src,
1384                                                   bdaddr_t *dst)
1385{
1386        struct l2cap_chan *c, *c1 = NULL;
1387
1388        read_lock(&chan_list_lock);
1389
1390        list_for_each_entry(c, &chan_list, global_l) {
1391                struct sock *sk = c->sk;
1392
1393                if (state && c->state != state)
1394                        continue;
1395
1396                if (c->psm == psm) {
1397                        int src_match, dst_match;
1398                        int src_any, dst_any;
1399
1400                        /* Exact match. */
1401                        src_match = !bacmp(&bt_sk(sk)->src, src);
1402                        dst_match = !bacmp(&bt_sk(sk)->dst, dst);
1403                        if (src_match && dst_match) {
1404                                read_unlock(&chan_list_lock);
1405                                return c;
1406                        }
1407
1408                        /* Closest match */
1409                        src_any = !bacmp(&bt_sk(sk)->src, BDADDR_ANY);
1410                        dst_any = !bacmp(&bt_sk(sk)->dst, BDADDR_ANY);
1411                        if ((src_match && dst_any) || (src_any && dst_match) ||
1412                            (src_any && dst_any))
1413                                c1 = c;
1414                }
1415        }
1416
1417        read_unlock(&chan_list_lock);
1418
1419        return c1;
1420}
1421
1422int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid,
1423                       bdaddr_t *dst, u8 dst_type)
1424{
1425        struct sock *sk = chan->sk;
1426        bdaddr_t *src = &bt_sk(sk)->src;
1427        struct l2cap_conn *conn;
1428        struct hci_conn *hcon;
1429        struct hci_dev *hdev;
1430        __u8 auth_type;
1431        int err;
1432
1433        BT_DBG("%s -> %s (type %u) psm 0x%2.2x", batostr(src), batostr(dst),
1434               dst_type, __le16_to_cpu(chan->psm));
1435
1436        hdev = hci_get_route(dst, src);
1437        if (!hdev)
1438                return -EHOSTUNREACH;
1439
1440        hci_dev_lock(hdev);
1441
1442        l2cap_chan_lock(chan);
1443
1444        /* PSM must be odd and lsb of upper byte must be 0 */
1445        if ((__le16_to_cpu(psm) & 0x0101) != 0x0001 && !cid &&
1446                                        chan->chan_type != L2CAP_CHAN_RAW) {
1447                err = -EINVAL;
1448                goto done;
1449        }
1450
1451        if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED && !(psm || cid)) {
1452                err = -EINVAL;
1453                goto done;
1454        }
1455
1456        switch (chan->mode) {
1457        case L2CAP_MODE_BASIC:
1458                break;
1459        case L2CAP_MODE_ERTM:
1460        case L2CAP_MODE_STREAMING:
1461                if (!disable_ertm)
1462                        break;
1463                /* fall through */
1464        default:
1465                err = -ENOTSUPP;
1466                goto done;
1467        }
1468
1469        switch (chan->state) {
1470        case BT_CONNECT:
1471        case BT_CONNECT2:
1472        case BT_CONFIG:
1473                /* Already connecting */
1474                err = 0;
1475                goto done;
1476
1477        case BT_CONNECTED:
1478                /* Already connected */
1479                err = -EISCONN;
1480                goto done;
1481
1482        case BT_OPEN:
1483        case BT_BOUND:
1484                /* Can connect */
1485                break;
1486
1487        default:
1488                err = -EBADFD;
1489                goto done;
1490        }
1491
1492        /* Set destination address and psm */
1493        lock_sock(sk);
1494        bacpy(&bt_sk(sk)->dst, dst);
1495        release_sock(sk);
1496
1497        chan->psm = psm;
1498        chan->dcid = cid;
1499
1500        auth_type = l2cap_get_auth_type(chan);
1501
1502        if (chan->dcid == L2CAP_CID_LE_DATA)
1503                hcon = hci_connect(hdev, LE_LINK, dst, dst_type,
1504                                   chan->sec_level, auth_type);
1505        else
1506                hcon = hci_connect(hdev, ACL_LINK, dst, dst_type,
1507                                   chan->sec_level, auth_type);
1508
1509        if (IS_ERR(hcon)) {
1510                err = PTR_ERR(hcon);
1511                goto done;
1512        }
1513
1514        conn = l2cap_conn_add(hcon, 0);
1515        if (!conn) {
1516                hci_conn_put(hcon);
1517                err = -ENOMEM;
1518                goto done;
1519        }
1520
1521        if (hcon->type == LE_LINK) {
1522                err = 0;
1523
1524                if (!list_empty(&conn->chan_l)) {
1525                        err = -EBUSY;
1526                        hci_conn_put(hcon);
1527                }
1528
1529                if (err)
1530                        goto done;
1531        }
1532
1533        /* Update source addr of the socket */
1534        bacpy(src, conn->src);
1535
1536        l2cap_chan_unlock(chan);
1537        l2cap_chan_add(conn, chan);
1538        l2cap_chan_lock(chan);
1539
1540        l2cap_state_change(chan, BT_CONNECT);
1541        __set_chan_timer(chan, sk->sk_sndtimeo);
1542
1543        if (hcon->state == BT_CONNECTED) {
1544                if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
1545                        __clear_chan_timer(chan);
1546                        if (l2cap_chan_check_security(chan))
1547                                l2cap_state_change(chan, BT_CONNECTED);
1548                } else
1549                        l2cap_do_start(chan);
1550        }
1551
1552        err = 0;
1553
1554done:
1555        l2cap_chan_unlock(chan);
1556        hci_dev_unlock(hdev);
1557        hci_dev_put(hdev);
1558        return err;
1559}
1560
1561int __l2cap_wait_ack(struct sock *sk)
1562{
1563        struct l2cap_chan *chan = l2cap_pi(sk)->chan;
1564        DECLARE_WAITQUEUE(wait, current);
1565        int err = 0;
1566        int timeo = HZ/5;
1567
1568        add_wait_queue(sk_sleep(sk), &wait);
1569        set_current_state(TASK_INTERRUPTIBLE);
1570        while (chan->unacked_frames > 0 && chan->conn) {
1571                if (!timeo)
1572                        timeo = HZ/5;
1573
1574                if (signal_pending(current)) {
1575                        err = sock_intr_errno(timeo);
1576                        break;
1577                }
1578
1579                release_sock(sk);
1580                timeo = schedule_timeout(timeo);
1581                lock_sock(sk);
1582                set_current_state(TASK_INTERRUPTIBLE);
1583
1584                err = sock_error(sk);
1585                if (err)
1586                        break;
1587        }
1588        set_current_state(TASK_RUNNING);
1589        remove_wait_queue(sk_sleep(sk), &wait);
1590        return err;
1591}
1592
1593static void l2cap_monitor_timeout(struct work_struct *work)
1594{
1595        struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
1596                                               monitor_timer.work);
1597
1598        BT_DBG("chan %p", chan);
1599
1600        l2cap_chan_lock(chan);
1601
1602        if (!chan->conn) {
1603                l2cap_chan_unlock(chan);
1604                l2cap_chan_put(chan);
1605                return;
1606        }
1607
1608        l2cap_tx(chan, NULL, NULL, L2CAP_EV_MONITOR_TO);
1609
1610        l2cap_chan_unlock(chan);
1611        l2cap_chan_put(chan);
1612}
1613
1614static void l2cap_retrans_timeout(struct work_struct *work)
1615{
1616        struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
1617                                               retrans_timer.work);
1618
1619        BT_DBG("chan %p", chan);
1620
1621        l2cap_chan_lock(chan);
1622
1623        if (!chan->conn) {
1624                l2cap_chan_unlock(chan);
1625                l2cap_chan_put(chan);
1626                return;
1627        }
1628
1629        l2cap_tx(chan, NULL, NULL, L2CAP_EV_RETRANS_TO);
1630        l2cap_chan_unlock(chan);
1631        l2cap_chan_put(chan);
1632}
1633
1634static void l2cap_streaming_send(struct l2cap_chan *chan,
1635                                 struct sk_buff_head *skbs)
1636{
1637        struct sk_buff *skb;
1638        struct l2cap_ctrl *control;
1639
1640        BT_DBG("chan %p, skbs %p", chan, skbs);
1641
1642        skb_queue_splice_tail_init(skbs, &chan->tx_q);
1643
1644        while (!skb_queue_empty(&chan->tx_q)) {
1645
1646                skb = skb_dequeue(&chan->tx_q);
1647
1648                bt_cb(skb)->control.retries = 1;
1649                control = &bt_cb(skb)->control;
1650
1651                control->reqseq = 0;
1652                control->txseq = chan->next_tx_seq;
1653
1654                __pack_control(chan, control, skb);
1655
1656                if (chan->fcs == L2CAP_FCS_CRC16) {
1657                        u16 fcs = crc16(0, (u8 *) skb->data, skb->len);
1658                        put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE));
1659                }
1660
1661                l2cap_do_send(chan, skb);
1662
1663                BT_DBG("Sent txseq %u", control->txseq);
1664
1665                chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq);
1666                chan->frames_sent++;
1667        }
1668}
1669
1670static int l2cap_ertm_send(struct l2cap_chan *chan)
1671{
1672        struct sk_buff *skb, *tx_skb;
1673        struct l2cap_ctrl *control;
1674        int sent = 0;
1675
1676        BT_DBG("chan %p", chan);
1677
1678        if (chan->state != BT_CONNECTED)
1679                return -ENOTCONN;
1680
1681        if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
1682                return 0;
1683
1684        while (chan->tx_send_head &&
1685               chan->unacked_frames < chan->remote_tx_win &&
1686               chan->tx_state == L2CAP_TX_STATE_XMIT) {
1687
1688                skb = chan->tx_send_head;
1689
1690                bt_cb(skb)->control.retries = 1;
1691                control = &bt_cb(skb)->control;
1692
1693                if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
1694                        control->final = 1;
1695
1696                control->reqseq = chan->buffer_seq;
1697                chan->last_acked_seq = chan->buffer_seq;
1698                control->txseq = chan->next_tx_seq;
1699
1700                __pack_control(chan, control, skb);
1701
1702                if (chan->fcs == L2CAP_FCS_CRC16) {
1703                        u16 fcs = crc16(0, (u8 *) skb->data, skb->len);
1704                        put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE));
1705                }
1706
1707                /* Clone after data has been modified. Data is assumed to be
1708                   read-only (for locking purposes) on cloned sk_buffs.
1709                 */
1710                tx_skb = skb_clone(skb, GFP_KERNEL);
1711
1712                if (!tx_skb)
1713                        break;
1714
1715                __set_retrans_timer(chan);
1716
1717                chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq);
1718                chan->unacked_frames++;
1719                chan->frames_sent++;
1720                sent++;
1721
1722                if (skb_queue_is_last(&chan->tx_q, skb))
1723                        chan->tx_send_head = NULL;
1724                else
1725                        chan->tx_send_head = skb_queue_next(&chan->tx_q, skb);
1726
1727                l2cap_do_send(chan, tx_skb);
1728                BT_DBG("Sent txseq %u", control->txseq);
1729        }
1730
1731        BT_DBG("Sent %d, %u unacked, %u in ERTM queue", sent,
1732               chan->unacked_frames, skb_queue_len(&chan->tx_q));
1733
1734        return sent;
1735}
1736
1737static void l2cap_ertm_resend(struct l2cap_chan *chan)
1738{
1739        struct l2cap_ctrl control;
1740        struct sk_buff *skb;
1741        struct sk_buff *tx_skb;
1742        u16 seq;
1743
1744        BT_DBG("chan %p", chan);
1745
1746        if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
1747                return;
1748
1749        while (chan->retrans_list.head != L2CAP_SEQ_LIST_CLEAR) {
1750                seq = l2cap_seq_list_pop(&chan->retrans_list);
1751
1752                skb = l2cap_ertm_seq_in_queue(&chan->tx_q, seq);
1753                if (!skb) {
1754                        BT_DBG("Error: Can't retransmit seq %d, frame missing",
1755                                seq);
1756                        continue;
1757                }
1758
1759                bt_cb(skb)->control.retries++;
1760                control = bt_cb(skb)->control;
1761
1762                if (chan->max_tx != 0 &&
1763                    bt_cb(skb)->control.retries > chan->max_tx) {
1764                        BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
1765                        l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
1766                        l2cap_seq_list_clear(&chan->retrans_list);
1767                        break;
1768                }
1769
1770                control.reqseq = chan->buffer_seq;
1771                if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
1772                        control.final = 1;
1773                else
1774                        control.final = 0;
1775
1776                if (skb_cloned(skb)) {
1777                        /* Cloned sk_buffs are read-only, so we need a
1778                         * writeable copy
1779                         */
1780                        tx_skb = skb_copy(skb, GFP_ATOMIC);
1781                } else {
1782                        tx_skb = skb_clone(skb, GFP_ATOMIC);
1783                }
1784
1785                if (!tx_skb) {
1786                        l2cap_seq_list_clear(&chan->retrans_list);
1787                        break;
1788                }
1789
1790                /* Update skb contents */
1791                if (test_bit(FLAG_EXT_CTRL, &chan->flags)) {
1792                        put_unaligned_le32(__pack_extended_control(&control),
1793                                           tx_skb->data + L2CAP_HDR_SIZE);
1794                } else {
1795                        put_unaligned_le16(__pack_enhanced_control(&control),
1796                                           tx_skb->data + L2CAP_HDR_SIZE);
1797                }
1798
1799                if (chan->fcs == L2CAP_FCS_CRC16) {
1800                        u16 fcs = crc16(0, (u8 *) tx_skb->data, tx_skb->len);
1801                        put_unaligned_le16(fcs, skb_put(tx_skb,
1802                                                        L2CAP_FCS_SIZE));
1803                }
1804
1805                l2cap_do_send(chan, tx_skb);
1806
1807                BT_DBG("Resent txseq %d", control.txseq);
1808
1809                chan->last_acked_seq = chan->buffer_seq;
1810        }
1811}
1812
1813static void l2cap_retransmit(struct l2cap_chan *chan,
1814                             struct l2cap_ctrl *control)
1815{
1816        BT_DBG("chan %p, control %p", chan, control);
1817
1818        l2cap_seq_list_append(&chan->retrans_list, control->reqseq);
1819        l2cap_ertm_resend(chan);
1820}
1821
1822static void l2cap_retransmit_all(struct l2cap_chan *chan,
1823                                 struct l2cap_ctrl *control)
1824{
1825        struct sk_buff *skb;
1826
1827        BT_DBG("chan %p, control %p", chan, control);
1828
1829        if (control->poll)
1830                set_bit(CONN_SEND_FBIT, &chan->conn_state);
1831
1832        l2cap_seq_list_clear(&chan->retrans_list);
1833
1834        if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
1835                return;
1836
1837        if (chan->unacked_frames) {
1838                skb_queue_walk(&chan->tx_q, skb) {
1839                        if (bt_cb(skb)->control.txseq == control->reqseq ||
1840                                skb == chan->tx_send_head)
1841                                break;
1842                }
1843
1844                skb_queue_walk_from(&chan->tx_q, skb) {
1845                        if (skb == chan->tx_send_head)
1846                                break;
1847
1848                        l2cap_seq_list_append(&chan->retrans_list,
1849                                              bt_cb(skb)->control.txseq);
1850                }
1851
1852                l2cap_ertm_resend(chan);
1853        }
1854}
1855
1856static void l2cap_send_ack(struct l2cap_chan *chan)
1857{
1858        struct l2cap_ctrl control;
1859        u16 frames_to_ack = __seq_offset(chan, chan->buffer_seq,
1860                                         chan->last_acked_seq);
1861        int threshold;
1862
1863        BT_DBG("chan %p last_acked_seq %d buffer_seq %d",
1864               chan, chan->last_acked_seq, chan->buffer_seq);
1865
1866        memset(&control, 0, sizeof(control));
1867        control.sframe = 1;
1868
1869        if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
1870            chan->rx_state == L2CAP_RX_STATE_RECV) {
1871                __clear_ack_timer(chan);
1872                control.super = L2CAP_SUPER_RNR;
1873                control.reqseq = chan->buffer_seq;
1874                l2cap_send_sframe(chan, &control);
1875        } else {
1876                if (!test_bit(CONN_REMOTE_BUSY, &chan->conn_state)) {
1877                        l2cap_ertm_send(chan);
1878                        /* If any i-frames were sent, they included an ack */
1879                        if (chan->buffer_seq == chan->last_acked_seq)
1880                                frames_to_ack = 0;
1881                }
1882
1883                /* Ack now if the window is 3/4ths full.
1884                 * Calculate without mul or div
1885                 */
1886                threshold = chan->ack_win;
1887                threshold += threshold << 1;
1888                threshold >>= 2;
1889
1890                BT_DBG("frames_to_ack %u, threshold %d", frames_to_ack,
1891                       threshold);
1892
1893                if (frames_to_ack >= threshold) {
1894                        __clear_ack_timer(chan);
1895                        control.super = L2CAP_SUPER_RR;
1896                        control.reqseq = chan->buffer_seq;
1897                        l2cap_send_sframe(chan, &control);
1898                        frames_to_ack = 0;
1899                }
1900
1901                if (frames_to_ack)
1902                        __set_ack_timer(chan);
1903        }
1904}
1905
1906static inline int l2cap_skbuff_fromiovec(struct l2cap_chan *chan,
1907                                         struct msghdr *msg, int len,
1908                                         int count, struct sk_buff *skb)
1909{
1910        struct l2cap_conn *conn = chan->conn;
1911        struct sk_buff **frag;
1912        int sent = 0;
1913
1914        if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count))
1915                return -EFAULT;
1916
1917        sent += count;
1918        len  -= count;
1919
1920        /* Continuation fragments (no L2CAP header) */
1921        frag = &skb_shinfo(skb)->frag_list;
1922        while (len) {
1923                struct sk_buff *tmp;
1924
1925                count = min_t(unsigned int, conn->mtu, len);
1926
1927                tmp = chan->ops->alloc_skb(chan, count,
1928                                           msg->msg_flags & MSG_DONTWAIT);
1929                if (IS_ERR(tmp))
1930                        return PTR_ERR(tmp);
1931
1932                *frag = tmp;
1933
1934                if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count))
1935                        return -EFAULT;
1936
1937                (*frag)->priority = skb->priority;
1938
1939                sent += count;
1940                len  -= count;
1941
1942                skb->len += (*frag)->len;
1943                skb->data_len += (*frag)->len;
1944
1945                frag = &(*frag)->next;
1946        }
1947
1948        return sent;
1949}
1950
1951static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan,
1952                                                 struct msghdr *msg, size_t len,
1953                                                 u32 priority)
1954{
1955        struct l2cap_conn *conn = chan->conn;
1956        struct sk_buff *skb;
1957        int err, count, hlen = L2CAP_HDR_SIZE + L2CAP_PSMLEN_SIZE;
1958        struct l2cap_hdr *lh;
1959
1960        BT_DBG("chan %p len %zu priority %u", chan, len, priority);
1961
1962        count = min_t(unsigned int, (conn->mtu - hlen), len);
1963
1964        skb = chan->ops->alloc_skb(chan, count + hlen,
1965                                   msg->msg_flags & MSG_DONTWAIT);
1966        if (IS_ERR(skb))
1967                return skb;
1968
1969        skb->priority = priority;
1970
1971        /* Create L2CAP header */
1972        lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1973        lh->cid = cpu_to_le16(chan->dcid);
1974        lh->len = cpu_to_le16(len + L2CAP_PSMLEN_SIZE);
1975        put_unaligned(chan->psm, skb_put(skb, L2CAP_PSMLEN_SIZE));
1976
1977        err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb);
1978        if (unlikely(err < 0)) {
1979                kfree_skb(skb);
1980                return ERR_PTR(err);
1981        }
1982        return skb;
1983}
1984
1985static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan,
1986                                              struct msghdr *msg, size_t len,
1987                                              u32 priority)
1988{
1989        struct l2cap_conn *conn = chan->conn;
1990        struct sk_buff *skb;
1991        int err, count;
1992        struct l2cap_hdr *lh;
1993
1994        BT_DBG("chan %p len %zu", chan, len);
1995
1996        count = min_t(unsigned int, (conn->mtu - L2CAP_HDR_SIZE), len);
1997
1998        skb = chan->ops->alloc_skb(chan, count + L2CAP_HDR_SIZE,
1999                                   msg->msg_flags & MSG_DONTWAIT);
2000        if (IS_ERR(skb))
2001                return skb;
2002
2003        skb->priority = priority;
2004
2005        /* Create L2CAP header */
2006        lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
2007        lh->cid = cpu_to_le16(chan->dcid);
2008        lh->len = cpu_to_le16(len);
2009
2010        err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb);
2011        if (unlikely(err < 0)) {
2012                kfree_skb(skb);
2013                return ERR_PTR(err);
2014        }
2015        return skb;
2016}
2017
2018static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan,
2019                                               struct msghdr *msg, size_t len,
2020                                               u16 sdulen)
2021{
2022        struct l2cap_conn *conn = chan->conn;
2023        struct sk_buff *skb;
2024        int err, count, hlen;
2025        struct l2cap_hdr *lh;
2026
2027        BT_DBG("chan %p len %zu", chan, len);
2028
2029        if (!conn)
2030                return ERR_PTR(-ENOTCONN);
2031
2032        hlen = __ertm_hdr_size(chan);
2033
2034        if (sdulen)
2035                hlen += L2CAP_SDULEN_SIZE;
2036
2037        if (chan->fcs == L2CAP_FCS_CRC16)
2038                hlen += L2CAP_FCS_SIZE;
2039
2040        count = min_t(unsigned int, (conn->mtu - hlen), len);
2041
2042        skb = chan->ops->alloc_skb(chan, count + hlen,
2043                                   msg->msg_flags & MSG_DONTWAIT);
2044        if (IS_ERR(skb))
2045                return skb;
2046
2047        /* Create L2CAP header */
2048        lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
2049        lh->cid = cpu_to_le16(chan->dcid);
2050        lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
2051
2052        /* Control header is populated later */
2053        if (test_bit(FLAG_EXT_CTRL, &chan->flags))
2054                put_unaligned_le32(0, skb_put(skb, L2CAP_EXT_CTRL_SIZE));
2055        else
2056                put_unaligned_le16(0, skb_put(skb, L2CAP_ENH_CTRL_SIZE));
2057
2058        if (sdulen)
2059                put_unaligned_le16(sdulen, skb_put(skb, L2CAP_SDULEN_SIZE));
2060
2061        err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb);
2062        if (unlikely(err < 0)) {
2063                kfree_skb(skb);
2064                return ERR_PTR(err);
2065        }
2066
2067        bt_cb(skb)->control.fcs = chan->fcs;
2068        bt_cb(skb)->control.retries = 0;
2069        return skb;
2070}
2071
2072static int l2cap_segment_sdu(struct l2cap_chan *chan,
2073                             struct sk_buff_head *seg_queue,
2074                             struct msghdr *msg, size_t len)
2075{
2076        struct sk_buff *skb;
2077        u16 sdu_len;
2078        size_t pdu_len;
2079        u8 sar;
2080
2081        BT_DBG("chan %p, msg %p, len %zu", chan, msg, len);
2082
2083        /* It is critical that ERTM PDUs fit in a single HCI fragment,
2084         * so fragmented skbs are not used.  The HCI layer's handling
2085         * of fragmented skbs is not compatible with ERTM's queueing.
2086         */
2087
2088        /* PDU size is derived from the HCI MTU */
2089        pdu_len = chan->conn->mtu;
2090
2091        pdu_len = min_t(size_t, pdu_len, L2CAP_BREDR_MAX_PAYLOAD);
2092
2093        /* Adjust for largest possible L2CAP overhead. */
2094        if (chan->fcs)
2095                pdu_len -= L2CAP_FCS_SIZE;
2096
2097        pdu_len -= __ertm_hdr_size(chan);
2098
2099        /* Remote device may have requested smaller PDUs */
2100        pdu_len = min_t(size_t, pdu_len, chan->remote_mps);
2101
2102        if (len <= pdu_len) {
2103                sar = L2CAP_SAR_UNSEGMENTED;
2104                sdu_len = 0;
2105                pdu_len = len;
2106        } else {
2107                sar = L2CAP_SAR_START;
2108                sdu_len = len;
2109                pdu_len -= L2CAP_SDULEN_SIZE;
2110        }
2111
2112        while (len > 0) {
2113                skb = l2cap_create_iframe_pdu(chan, msg, pdu_len, sdu_len);
2114
2115                if (IS_ERR(skb)) {
2116                        __skb_queue_purge(seg_queue);
2117                        return PTR_ERR(skb);
2118                }
2119
2120                bt_cb(skb)->control.sar = sar;
2121                __skb_queue_tail(seg_queue, skb);
2122
2123                len -= pdu_len;
2124                if (sdu_len) {
2125                        sdu_len = 0;
2126                        pdu_len += L2CAP_SDULEN_SIZE;
2127                }
2128
2129                if (len <= pdu_len) {
2130                        sar = L2CAP_SAR_END;
2131                        pdu_len = len;
2132                } else {
2133                        sar = L2CAP_SAR_CONTINUE;
2134                }
2135        }
2136
2137        return 0;
2138}
2139
2140int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
2141                                                                u32 priority)
2142{
2143        struct sk_buff *skb;
2144        int err;
2145        struct sk_buff_head seg_queue;
2146
2147        /* Connectionless channel */
2148        if (chan->chan_type == L2CAP_CHAN_CONN_LESS) {
2149                skb = l2cap_create_connless_pdu(chan, msg, len, priority);
2150                if (IS_ERR(skb))
2151                        return PTR_ERR(skb);
2152
2153                l2cap_do_send(chan, skb);
2154                return len;
2155        }
2156
2157        switch (chan->mode) {
2158        case L2CAP_MODE_BASIC:
2159                /* Check outgoing MTU */
2160                if (len > chan->omtu)
2161                        return -EMSGSIZE;
2162
2163                /* Create a basic PDU */
2164                skb = l2cap_create_basic_pdu(chan, msg, len, priority);
2165                if (IS_ERR(skb))
2166                        return PTR_ERR(skb);
2167
2168                l2cap_do_send(chan, skb);
2169                err = len;
2170                break;
2171
2172        case L2CAP_MODE_ERTM:
2173        case L2CAP_MODE_STREAMING:
2174                /* Check outgoing MTU */
2175                if (len > chan->omtu) {
2176                        err = -EMSGSIZE;
2177                        break;
2178                }
2179
2180                __skb_queue_head_init(&seg_queue);
2181
2182                /* Do segmentation before calling in to the state machine,
2183                 * since it's possible to block while waiting for memory
2184                 * allocation.
2185                 */
2186                err = l2cap_segment_sdu(chan, &seg_queue, msg, len);
2187
2188                /* The channel could have been closed while segmenting,
2189                 * check that it is still connected.
2190                 */
2191                if (chan->state != BT_CONNECTED) {
2192                        __skb_queue_purge(&seg_queue);
2193                        err = -ENOTCONN;
2194                }
2195
2196                if (err)
2197                        break;
2198
2199                if (chan->mode == L2CAP_MODE_ERTM)
2200                        l2cap_tx(chan, NULL, &seg_queue, L2CAP_EV_DATA_REQUEST);
2201                else
2202                        l2cap_streaming_send(chan, &seg_queue);
2203
2204                err = len;
2205
2206                /* If the skbs were not queued for sending, they'll still be in
2207                 * seg_queue and need to be purged.
2208                 */
2209                __skb_queue_purge(&seg_queue);
2210                break;
2211
2212        default:
2213                BT_DBG("bad state %1.1x", chan->mode);
2214                err = -EBADFD;
2215        }
2216
2217        return err;
2218}
2219
2220static void l2cap_send_srej(struct l2cap_chan *chan, u16 txseq)
2221{
2222        struct l2cap_ctrl control;
2223        u16 seq;
2224
2225        BT_DBG("chan %p, txseq %u", chan, txseq);
2226
2227        memset(&control, 0, sizeof(control));
2228        control.sframe = 1;
2229        control.super = L2CAP_SUPER_SREJ;
2230
2231        for (seq = chan->expected_tx_seq; seq != txseq;
2232             seq = __next_seq(chan, seq)) {
2233                if (!l2cap_ertm_seq_in_queue(&chan->srej_q, seq)) {
2234                        control.reqseq = seq;
2235                        l2cap_send_sframe(chan, &control);
2236                        l2cap_seq_list_append(&chan->srej_list, seq);
2237                }
2238        }
2239
2240        chan->expected_tx_seq = __next_seq(chan, txseq);
2241}
2242
2243static void l2cap_send_srej_tail(struct l2cap_chan *chan)
2244{
2245        struct l2cap_ctrl control;
2246
2247        BT_DBG("chan %p", chan);
2248
2249        if (chan->srej_list.tail == L2CAP_SEQ_LIST_CLEAR)
2250                return;
2251
2252        memset(&control, 0, sizeof(control));
2253        control.sframe = 1;
2254        control.super = L2CAP_SUPER_SREJ;
2255        control.reqseq = chan->srej_list.tail;
2256        l2cap_send_sframe(chan, &control);
2257}
2258
2259static void l2cap_send_srej_list(struct l2cap_chan *chan, u16 txseq)
2260{
2261        struct l2cap_ctrl control;
2262        u16 initial_head;
2263        u16 seq;
2264
2265        BT_DBG("chan %p, txseq %u", chan, txseq);
2266
2267        memset(&control, 0, sizeof(control));
2268        control.sframe = 1;
2269        control.super = L2CAP_SUPER_SREJ;
2270
2271        /* Capture initial list head to allow only one pass through the list. */
2272        initial_head = chan->srej_list.head;
2273
2274        do {
2275                seq = l2cap_seq_list_pop(&chan->srej_list);
2276                if (seq == txseq || seq == L2CAP_SEQ_LIST_CLEAR)
2277                        break;
2278
2279                control.reqseq = seq;
2280                l2cap_send_sframe(chan, &control);
2281                l2cap_seq_list_append(&chan->srej_list, seq);
2282        } while (chan->srej_list.head != initial_head);
2283}
2284
2285static void l2cap_process_reqseq(struct l2cap_chan *chan, u16 reqseq)
2286{
2287        struct sk_buff *acked_skb;
2288        u16 ackseq;
2289
2290        BT_DBG("chan %p, reqseq %u", chan, reqseq);
2291
2292        if (chan->unacked_frames == 0 || reqseq == chan->expected_ack_seq)
2293                return;
2294
2295        BT_DBG("expected_ack_seq %u, unacked_frames %u",
2296               chan->expected_ack_seq, chan->unacked_frames);
2297
2298        for (ackseq = chan->expected_ack_seq; ackseq != reqseq;
2299             ackseq = __next_seq(chan, ackseq)) {
2300
2301                acked_skb = l2cap_ertm_seq_in_queue(&chan->tx_q, ackseq);
2302                if (acked_skb) {
2303                        skb_unlink(acked_skb, &chan->tx_q);
2304                        kfree_skb(acked_skb);
2305                        chan->unacked_frames--;
2306                }
2307        }
2308
2309        chan->expected_ack_seq = reqseq;
2310
2311        if (chan->unacked_frames == 0)
2312                __clear_retrans_timer(chan);
2313
2314        BT_DBG("unacked_frames %u", chan->unacked_frames);
2315}
2316
2317static void l2cap_abort_rx_srej_sent(struct l2cap_chan *chan)
2318{
2319        BT_DBG("chan %p", chan);
2320
2321        chan->expected_tx_seq = chan->buffer_seq;
2322        l2cap_seq_list_clear(&chan->srej_list);
2323        skb_queue_purge(&chan->srej_q);
2324        chan->rx_state = L2CAP_RX_STATE_RECV;
2325}
2326
2327static void l2cap_tx_state_xmit(struct l2cap_chan *chan,
2328                                struct l2cap_ctrl *control,
2329                                struct sk_buff_head *skbs, u8 event)
2330{
2331        BT_DBG("chan %p, control %p, skbs %p, event %d", chan, control, skbs,
2332               event);
2333
2334        switch (event) {
2335        case L2CAP_EV_DATA_REQUEST:
2336                if (chan->tx_send_head == NULL)
2337                        chan->tx_send_head = skb_peek(skbs);
2338
2339                skb_queue_splice_tail_init(skbs, &chan->tx_q);
2340                l2cap_ertm_send(chan);
2341                break;
2342        case L2CAP_EV_LOCAL_BUSY_DETECTED:
2343                BT_DBG("Enter LOCAL_BUSY");
2344                set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2345
2346                if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
2347                        /* The SREJ_SENT state must be aborted if we are to
2348                         * enter the LOCAL_BUSY state.
2349                         */
2350                        l2cap_abort_rx_srej_sent(chan);
2351                }
2352
2353                l2cap_send_ack(chan);
2354
2355                break;
2356        case L2CAP_EV_LOCAL_BUSY_CLEAR:
2357                BT_DBG("Exit LOCAL_BUSY");
2358                clear_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2359
2360                if (test_bit(CONN_RNR_SENT, &chan->conn_state)) {
2361                        struct l2cap_ctrl local_control;
2362
2363                        memset(&local_control, 0, sizeof(local_control));
2364                        local_control.sframe = 1;
2365                        local_control.super = L2CAP_SUPER_RR;
2366                        local_control.poll = 1;
2367                        local_control.reqseq = chan->buffer_seq;
2368                        l2cap_send_sframe(chan, &local_control);
2369
2370                        chan->retry_count = 1;
2371                        __set_monitor_timer(chan);
2372                        chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2373                }
2374                break;
2375        case L2CAP_EV_RECV_REQSEQ_AND_FBIT:
2376                l2cap_process_reqseq(chan, control->reqseq);
2377                break;
2378        case L2CAP_EV_EXPLICIT_POLL:
2379                l2cap_send_rr_or_rnr(chan, 1);
2380                chan->retry_count = 1;
2381                __set_monitor_timer(chan);
2382                __clear_ack_timer(chan);
2383                chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2384                break;
2385        case L2CAP_EV_RETRANS_TO:
2386                l2cap_send_rr_or_rnr(chan, 1);
2387                chan->retry_count = 1;
2388                __set_monitor_timer(chan);
2389                chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2390                break;
2391        case L2CAP_EV_RECV_FBIT:
2392                /* Nothing to process */
2393                break;
2394        default:
2395                break;
2396        }
2397}
2398
2399static void l2cap_tx_state_wait_f(struct l2cap_chan *chan,
2400                                  struct l2cap_ctrl *control,
2401                                  struct sk_buff_head *skbs, u8 event)
2402{
2403        BT_DBG("chan %p, control %p, skbs %p, event %d", chan, control, skbs,
2404               event);
2405
2406        switch (event) {
2407        case L2CAP_EV_DATA_REQUEST:
2408                if (chan->tx_send_head == NULL)
2409                        chan->tx_send_head = skb_peek(skbs);
2410                /* Queue data, but don't send. */
2411                skb_queue_splice_tail_init(skbs, &chan->tx_q);
2412                break;
2413        case L2CAP_EV_LOCAL_BUSY_DETECTED:
2414                BT_DBG("Enter LOCAL_BUSY");
2415                set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2416
2417                if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
2418                        /* The SREJ_SENT state must be aborted if we are to
2419                         * enter the LOCAL_BUSY state.
2420                         */
2421                        l2cap_abort_rx_srej_sent(chan);
2422                }
2423
2424                l2cap_send_ack(chan);
2425
2426                break;
2427        case L2CAP_EV_LOCAL_BUSY_CLEAR:
2428                BT_DBG("Exit LOCAL_BUSY");
2429                clear_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2430
2431                if (test_bit(CONN_RNR_SENT, &chan->conn_state)) {
2432                        struct l2cap_ctrl local_control;
2433                        memset(&local_control, 0, sizeof(local_control));
2434                        local_control.sframe = 1;
2435                        local_control.super = L2CAP_SUPER_RR;
2436                        local_control.poll = 1;
2437                        local_control.reqseq = chan->buffer_seq;
2438                        l2cap_send_sframe(chan, &local_control);
2439
2440                        chan->retry_count = 1;
2441                        __set_monitor_timer(chan);
2442                        chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2443                }
2444                break;
2445        case L2CAP_EV_RECV_REQSEQ_AND_FBIT:
2446                l2cap_process_reqseq(chan, control->reqseq);
2447
2448                /* Fall through */
2449
2450        case L2CAP_EV_RECV_FBIT:
2451                if (control && control->final) {
2452                        __clear_monitor_timer(chan);
2453                        if (chan->unacked_frames > 0)
2454                                __set_retrans_timer(chan);
2455                        chan->retry_count = 0;
2456                        chan->tx_state = L2CAP_TX_STATE_XMIT;
2457                        BT_DBG("recv fbit tx_state 0x2.2%x", chan->tx_state);
2458                }
2459                break;
2460        case L2CAP_EV_EXPLICIT_POLL:
2461                /* Ignore */
2462                break;
2463        case L2CAP_EV_MONITOR_TO:
2464                if (chan->max_tx == 0 || chan->retry_count < chan->max_tx) {
2465                        l2cap_send_rr_or_rnr(chan, 1);
2466                        __set_monitor_timer(chan);
2467                        chan->retry_count++;
2468                } else {
2469                        l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED);
2470                }
2471                break;
2472        default:
2473                break;
2474        }
2475}
2476
2477static void l2cap_tx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
2478                     struct sk_buff_head *skbs, u8 event)
2479{
2480        BT_DBG("chan %p, control %p, skbs %p, event %d, state %d",
2481               chan, control, skbs, event, chan->tx_state);
2482
2483        switch (chan->tx_state) {
2484        case L2CAP_TX_STATE_XMIT:
2485                l2cap_tx_state_xmit(chan, control, skbs, event);
2486                break;
2487        case L2CAP_TX_STATE_WAIT_F:
2488                l2cap_tx_state_wait_f(chan, control, skbs, event);
2489                break;
2490        default:
2491                /* Ignore event */
2492                break;
2493        }
2494}
2495
2496static void l2cap_pass_to_tx(struct l2cap_chan *chan,
2497                             struct l2cap_ctrl *control)
2498{
2499        BT_DBG("chan %p, control %p", chan, control);
2500        l2cap_tx(chan, control, NULL, L2CAP_EV_RECV_REQSEQ_AND_FBIT);
2501}
2502
2503static void l2cap_pass_to_tx_fbit(struct l2cap_chan *chan,
2504                                  struct l2cap_ctrl *control)
2505{
2506        BT_DBG("chan %p, control %p", chan, control);
2507        l2cap_tx(chan, control, NULL, L2CAP_EV_RECV_FBIT);
2508}
2509
2510/* Copy frame to all raw sockets on that connection */
2511static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
2512{
2513        struct sk_buff *nskb;
2514        struct l2cap_chan *chan;
2515
2516        BT_DBG("conn %p", conn);
2517
2518        mutex_lock(&conn->chan_lock);
2519
2520        list_for_each_entry(chan, &conn->chan_l, list) {
2521                struct sock *sk = chan->sk;
2522                if (chan->chan_type != L2CAP_CHAN_RAW)
2523                        continue;
2524
2525                /* Don't send frame to the socket it came from */
2526                if (skb->sk == sk)
2527                        continue;
2528                nskb = skb_clone(skb, GFP_ATOMIC);
2529                if (!nskb)
2530                        continue;
2531
2532                if (chan->ops->recv(chan, nskb))
2533                        kfree_skb(nskb);
2534        }
2535
2536        mutex_unlock(&conn->chan_lock);
2537}
2538
2539/* ---- L2CAP signalling commands ---- */
2540static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn, u8 code,
2541                                       u8 ident, u16 dlen, void *data)
2542{
2543        struct sk_buff *skb, **frag;
2544        struct l2cap_cmd_hdr *cmd;
2545        struct l2cap_hdr *lh;
2546        int len, count;
2547
2548        BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %u",
2549               conn, code, ident, dlen);
2550
2551        len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen;
2552        count = min_t(unsigned int, conn->mtu, len);
2553
2554        skb = bt_skb_alloc(count, GFP_ATOMIC);
2555        if (!skb)
2556                return NULL;
2557
2558        lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
2559        lh->len = cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen);
2560
2561        if (conn->hcon->type == LE_LINK)
2562                lh->cid = __constant_cpu_to_le16(L2CAP_CID_LE_SIGNALING);
2563        else
2564                lh->cid = __constant_cpu_to_le16(L2CAP_CID_SIGNALING);
2565
2566        cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE);
2567        cmd->code  = code;
2568        cmd->ident = ident;
2569        cmd->len   = cpu_to_le16(dlen);
2570
2571        if (dlen) {
2572                count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE;
2573                memcpy(skb_put(skb, count), data, count);
2574                data += count;
2575        }
2576
2577        len -= skb->len;
2578
2579        /* Continuation fragments (no L2CAP header) */
2580        frag = &skb_shinfo(skb)->frag_list;
2581        while (len) {
2582                count = min_t(unsigned int, conn->mtu, len);
2583
2584                *frag = bt_skb_alloc(count, GFP_ATOMIC);
2585                if (!*frag)
2586                        goto fail;
2587
2588                memcpy(skb_put(*frag, count), data, count);
2589
2590                len  -= count;
2591                data += count;
2592
2593                frag = &(*frag)->next;
2594        }
2595
2596        return skb;
2597
2598fail:
2599        kfree_skb(skb);
2600        return NULL;
2601}
2602
2603static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val)
2604{
2605        struct l2cap_conf_opt *opt = *ptr;
2606        int len;
2607
2608        len = L2CAP_CONF_OPT_SIZE + opt->len;
2609        *ptr += len;
2610
2611        *type = opt->type;
2612        *olen = opt->len;
2613
2614        switch (opt->len) {
2615        case 1:
2616                *val = *((u8 *) opt->val);
2617                break;
2618
2619        case 2:
2620                *val = get_unaligned_le16(opt->val);
2621                break;
2622
2623        case 4:
2624                *val = get_unaligned_le32(opt->val);
2625                break;
2626
2627        default:
2628                *val = (unsigned long) opt->val;
2629                break;
2630        }
2631
2632        BT_DBG("type 0x%2.2x len %u val 0x%lx", *type, opt->len, *val);
2633        return len;
2634}
2635
2636static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val)
2637{
2638        struct l2cap_conf_opt *opt = *ptr;
2639
2640        BT_DBG("type 0x%2.2x len %u val 0x%lx", type, len, val);
2641
2642        opt->type = type;
2643        opt->len  = len;
2644
2645        switch (len) {
2646        case 1:
2647                *((u8 *) opt->val)  = val;
2648                break;
2649
2650        case 2:
2651                put_unaligned_le16(val, opt->val);
2652                break;
2653
2654        case 4:
2655                put_unaligned_le32(val, opt->val);
2656                break;
2657
2658        default:
2659                memcpy(opt->val, (void *) val, len);
2660                break;
2661        }
2662
2663        *ptr += L2CAP_CONF_OPT_SIZE + len;
2664}
2665
2666static void l2cap_add_opt_efs(void **ptr, struct l2cap_chan *chan)
2667{
2668        struct l2cap_conf_efs efs;
2669
2670        switch (chan->mode) {
2671        case L2CAP_MODE_ERTM:
2672                efs.id          = chan->local_id;
2673                efs.stype       = chan->local_stype;
2674                efs.msdu        = cpu_to_le16(chan->local_msdu);
2675                efs.sdu_itime   = cpu_to_le32(chan->local_sdu_itime);
2676                efs.acc_lat     = __constant_cpu_to_le32(L2CAP_DEFAULT_ACC_LAT);
2677                efs.flush_to    = __constant_cpu_to_le32(L2CAP_DEFAULT_FLUSH_TO);
2678                break;
2679
2680        case L2CAP_MODE_STREAMING:
2681                efs.id          = 1;
2682                efs.stype       = L2CAP_SERV_BESTEFFORT;
2683                efs.msdu        = cpu_to_le16(chan->local_msdu);
2684                efs.sdu_itime   = cpu_to_le32(chan->local_sdu_itime);
2685                efs.acc_lat     = 0;
2686                efs.flush_to    = 0;
2687                break;
2688
2689        default:
2690                return;
2691        }
2692
2693        l2cap_add_conf_opt(ptr, L2CAP_CONF_EFS, sizeof(efs),
2694                                                        (unsigned long) &efs);
2695}
2696
2697static void l2cap_ack_timeout(struct work_struct *work)
2698{
2699        struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
2700                                               ack_timer.work);
2701        u16 frames_to_ack;
2702
2703        BT_DBG("chan %p", chan);
2704
2705        l2cap_chan_lock(chan);
2706
2707        frames_to_ack = __seq_offset(chan, chan->buffer_seq,
2708                                     chan->last_acked_seq);
2709
2710        if (frames_to_ack)
2711                l2cap_send_rr_or_rnr(chan, 0);
2712
2713        l2cap_chan_unlock(chan);
2714        l2cap_chan_put(chan);
2715}
2716
2717int l2cap_ertm_init(struct l2cap_chan *chan)
2718{
2719        int err;
2720
2721        chan->next_tx_seq = 0;
2722        chan->expected_tx_seq = 0;
2723        chan->expected_ack_seq = 0;
2724        chan->unacked_frames = 0;
2725        chan->buffer_seq = 0;
2726        chan->frames_sent = 0;
2727        chan->last_acked_seq = 0;
2728        chan->sdu = NULL;
2729        chan->sdu_last_frag = NULL;
2730        chan->sdu_len = 0;
2731
2732        skb_queue_head_init(&chan->tx_q);
2733
2734        if (chan->mode != L2CAP_MODE_ERTM)
2735                return 0;
2736
2737        chan->rx_state = L2CAP_RX_STATE_RECV;
2738        chan->tx_state = L2CAP_TX_STATE_XMIT;
2739
2740        INIT_DELAYED_WORK(&chan->retrans_timer, l2cap_retrans_timeout);
2741        INIT_DELAYED_WORK(&chan->monitor_timer, l2cap_monitor_timeout);
2742        INIT_DELAYED_WORK(&chan->ack_timer, l2cap_ack_timeout);
2743
2744        skb_queue_head_init(&chan->srej_q);
2745
2746        err = l2cap_seq_list_init(&chan->srej_list, chan->tx_win);
2747        if (err < 0)
2748                return err;
2749
2750        err = l2cap_seq_list_init(&chan->retrans_list, chan->remote_tx_win);
2751        if (err < 0)
2752                l2cap_seq_list_free(&chan->srej_list);
2753
2754        return err;
2755}
2756
2757static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
2758{
2759        switch (mode) {
2760        case L2CAP_MODE_STREAMING:
2761        case L2CAP_MODE_ERTM:
2762                if (l2cap_mode_supported(mode, remote_feat_mask))
2763                        return mode;
2764                /* fall through */
2765        default:
2766                return L2CAP_MODE_BASIC;
2767        }
2768}
2769
2770static inline bool __l2cap_ews_supported(struct l2cap_chan *chan)
2771{
2772        return enable_hs && chan->conn->feat_mask & L2CAP_FEAT_EXT_WINDOW;
2773}
2774
2775static inline bool __l2cap_efs_supported(struct l2cap_chan *chan)
2776{
2777        return enable_hs && chan->conn->feat_mask & L2CAP_FEAT_EXT_FLOW;
2778}
2779
2780static inline void l2cap_txwin_setup(struct l2cap_chan *chan)
2781{
2782        if (chan->tx_win > L2CAP_DEFAULT_TX_WINDOW &&
2783                                                __l2cap_ews_supported(chan)) {
2784                /* use extended control field */
2785                set_bit(FLAG_EXT_CTRL, &chan->flags);
2786                chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
2787        } else {
2788                chan->tx_win = min_t(u16, chan->tx_win,
2789                                                L2CAP_DEFAULT_TX_WINDOW);
2790                chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
2791        }
2792        chan->ack_win = chan->tx_win;
2793}
2794
2795static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data)
2796{
2797        struct l2cap_conf_req *req = data;
2798        struct l2cap_conf_rfc rfc = { .mode = chan->mode };
2799        void *ptr = req->data;
2800        u16 size;
2801
2802        BT_DBG("chan %p", chan);
2803
2804        if (chan->num_conf_req || chan->num_conf_rsp)
2805                goto done;
2806
2807        switch (chan->mode) {
2808        case L2CAP_MODE_STREAMING:
2809        case L2CAP_MODE_ERTM:
2810                if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state))
2811                        break;
2812
2813                if (__l2cap_efs_supported(chan))
2814                        set_bit(FLAG_EFS_ENABLE, &chan->flags);
2815
2816                /* fall through */
2817        default:
2818                chan->mode = l2cap_select_mode(rfc.mode, chan->conn->feat_mask);
2819                break;
2820        }
2821
2822done:
2823        if (chan->imtu != L2CAP_DEFAULT_MTU)
2824                l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu);
2825
2826        switch (chan->mode) {
2827        case L2CAP_MODE_BASIC:
2828                if (!(chan->conn->feat_mask & L2CAP_FEAT_ERTM) &&
2829                                !(chan->conn->feat_mask & L2CAP_FEAT_STREAMING))
2830                        break;
2831
2832                rfc.mode            = L2CAP_MODE_BASIC;
2833                rfc.txwin_size      = 0;
2834                rfc.max_transmit    = 0;
2835                rfc.retrans_timeout = 0;
2836                rfc.monitor_timeout = 0;
2837                rfc.max_pdu_size    = 0;
2838
2839                l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
2840                                                        (unsigned long) &rfc);
2841                break;
2842
2843        case L2CAP_MODE_ERTM:
2844                rfc.mode            = L2CAP_MODE_ERTM;
2845                rfc.max_transmit    = chan->max_tx;
2846                rfc.retrans_timeout = 0;
2847                rfc.monitor_timeout = 0;
2848
2849                size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
2850                                                L2CAP_EXT_HDR_SIZE -
2851                                                L2CAP_SDULEN_SIZE -
2852                                                L2CAP_FCS_SIZE);
2853                rfc.max_pdu_size = cpu_to_le16(size);
2854
2855                l2cap_txwin_setup(chan);
2856
2857                rfc.txwin_size = min_t(u16, chan->tx_win,
2858                                                L2CAP_DEFAULT_TX_WINDOW);
2859
2860                l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
2861                                                        (unsigned long) &rfc);
2862
2863                if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
2864                        l2cap_add_opt_efs(&ptr, chan);
2865
2866                if (!(chan->conn->feat_mask & L2CAP_FEAT_FCS))
2867                        break;
2868
2869                if (chan->fcs == L2CAP_FCS_NONE ||
2870                                test_bit(CONF_NO_FCS_RECV, &chan->conf_state)) {
2871                        chan->fcs = L2CAP_FCS_NONE;
2872                        l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, chan->fcs);
2873                }
2874
2875                if (test_bit(FLAG_EXT_CTRL, &chan->flags))
2876                        l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
2877                                                                chan->tx_win);
2878                break;
2879
2880        case L2CAP_MODE_STREAMING:
2881                l2cap_txwin_setup(chan);
2882                rfc.mode            = L2CAP_MODE_STREAMING;
2883                rfc.txwin_size      = 0;
2884                rfc.max_transmit    = 0;
2885                rfc.retrans_timeout = 0;
2886                rfc.monitor_timeout = 0;
2887
2888                size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
2889                                                L2CAP_EXT_HDR_SIZE -
2890                                                L2CAP_SDULEN_SIZE -
2891                                                L2CAP_FCS_SIZE);
2892                rfc.max_pdu_size = cpu_to_le16(size);
2893
2894                l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
2895                                                        (unsigned long) &rfc);
2896
2897                if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
2898                        l2cap_add_opt_efs(&ptr, chan);
2899
2900                if (!(chan->conn->feat_mask & L2CAP_FEAT_FCS))
2901                        break;
2902
2903                if (chan->fcs == L2CAP_FCS_NONE ||
2904                                test_bit(CONF_NO_FCS_RECV, &chan->conf_state)) {
2905                        chan->fcs = L2CAP_FCS_NONE;
2906                        l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, chan->fcs);
2907                }
2908                break;
2909        }
2910
2911        req->dcid  = cpu_to_le16(chan->dcid);
2912        req->flags = __constant_cpu_to_le16(0);
2913
2914        return ptr - data;
2915}
2916
2917static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data)
2918{
2919        struct l2cap_conf_rsp *rsp = data;
2920        void *ptr = rsp->data;
2921        void *req = chan->conf_req;
2922        int len = chan->conf_len;
2923        int type, hint, olen;
2924        unsigned long val;
2925        struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
2926        struct l2cap_conf_efs efs;
2927        u8 remote_efs = 0;
2928        u16 mtu = L2CAP_DEFAULT_MTU;
2929        u16 result = L2CAP_CONF_SUCCESS;
2930        u16 size;
2931
2932        BT_DBG("chan %p", chan);
2933
2934        while (len >= L2CAP_CONF_OPT_SIZE) {
2935                len -= l2cap_get_conf_opt(&req, &type, &olen, &val);
2936
2937                hint  = type & L2CAP_CONF_HINT;
2938                type &= L2CAP_CONF_MASK;
2939
2940                switch (type) {
2941                case L2CAP_CONF_MTU:
2942                        mtu = val;
2943                        break;
2944
2945                case L2CAP_CONF_FLUSH_TO:
2946                        chan->flush_to = val;
2947                        break;
2948
2949                case L2CAP_CONF_QOS:
2950                        break;
2951
2952                case L2CAP_CONF_RFC:
2953                        if (olen == sizeof(rfc))
2954                                memcpy(&rfc, (void *) val, olen);
2955                        break;
2956
2957                case L2CAP_CONF_FCS:
2958                        if (val == L2CAP_FCS_NONE)
2959                                set_bit(CONF_NO_FCS_RECV, &chan->conf_state);
2960                        break;
2961
2962                case L2CAP_CONF_EFS:
2963                        remote_efs = 1;
2964                        if (olen == sizeof(efs))
2965                                memcpy(&efs, (void *) val, olen);
2966                        break;
2967
2968                case L2CAP_CONF_EWS:
2969                        if (!enable_hs)
2970                                return -ECONNREFUSED;
2971
2972                        set_bit(FLAG_EXT_CTRL, &chan->flags);
2973                        set_bit(CONF_EWS_RECV, &chan->conf_state);
2974                        chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
2975                        chan->remote_tx_win = val;
2976                        break;
2977
2978                default:
2979                        if (hint)
2980                                break;
2981
2982                        result = L2CAP_CONF_UNKNOWN;
2983                        *((u8 *) ptr++) = type;
2984                        break;
2985                }
2986        }
2987
2988        if (chan->num_conf_rsp || chan->num_conf_req > 1)
2989                goto done;
2990
2991        switch (chan->mode) {
2992        case L2CAP_MODE_STREAMING:
2993        case L2CAP_MODE_ERTM:
2994                if (!test_bit(CONF_STATE2_DEVICE, &chan->conf_state)) {
2995                        chan->mode = l2cap_select_mode(rfc.mode,
2996                                        chan->conn->feat_mask);
2997                        break;
2998                }
2999
3000                if (remote_efs) {
3001                        if (__l2cap_efs_supported(chan))
3002                                set_bit(FLAG_EFS_ENABLE, &chan->flags);
3003                        else
3004                                return -ECONNREFUSED;
3005                }
3006
3007                if (chan->mode != rfc.mode)
3008                        return -ECONNREFUSED;
3009
3010                break;
3011        }
3012
3013done:
3014        if (chan->mode != rfc.mode) {
3015                result = L2CAP_CONF_UNACCEPT;
3016                rfc.mode = chan->mode;
3017
3018                if (chan->num_conf_rsp == 1)
3019                        return -ECONNREFUSED;
3020
3021                l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
3022                                        sizeof(rfc), (unsigned long) &rfc);
3023        }
3024
3025        if (result == L2CAP_CONF_SUCCESS) {
3026                /* Configure output options and let the other side know
3027                 * which ones we don't like. */
3028
3029                if (mtu < L2CAP_DEFAULT_MIN_MTU)
3030                        result = L2CAP_CONF_UNACCEPT;
3031                else {
3032                        chan->omtu = mtu;
3033                        set_bit(CONF_MTU_DONE, &chan->conf_state);
3034                }
3035                l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->omtu);
3036
3037                if (remote_efs) {
3038                        if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
3039                                        efs.stype != L2CAP_SERV_NOTRAFIC &&
3040                                        efs.stype != chan->local_stype) {
3041
3042                                result = L2CAP_CONF_UNACCEPT;
3043
3044                                if (chan->num_conf_req >= 1)
3045                                        return -ECONNREFUSED;
3046
3047                                l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
3048                                                        sizeof(efs),
3049                                                        (unsigned long) &efs);
3050                        } else {
3051                                /* Send PENDING Conf Rsp */
3052                                result = L2CAP_CONF_PENDING;
3053                                set_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
3054                        }
3055                }
3056
3057                switch (rfc.mode) {
3058                case L2CAP_MODE_BASIC:
3059                        chan->fcs = L2CAP_FCS_NONE;
3060                        set_bit(CONF_MODE_DONE, &chan->conf_state);
3061                        break;
3062
3063                case L2CAP_MODE_ERTM:
3064                        if (!test_bit(CONF_EWS_RECV, &chan->conf_state))
3065                                chan->remote_tx_win = rfc.txwin_size;
3066                        else
3067                                rfc.txwin_size = L2CAP_DEFAULT_TX_WINDOW;
3068
3069                        chan->remote_max_tx = rfc.max_transmit;
3070
3071                        size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
3072                                                chan->conn->mtu -
3073                                                L2CAP_EXT_HDR_SIZE -
3074                                                L2CAP_SDULEN_SIZE -
3075                                                L2CAP_FCS_SIZE);
3076                        rfc.max_pdu_size = cpu_to_le16(size);
3077                        chan->remote_mps = size;
3078
3079                        rfc.retrans_timeout =
3080                                __constant_cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO);
3081                        rfc.monitor_timeout =
3082                                __constant_cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO);
3083
3084                        set_bit(CONF_MODE_DONE, &chan->conf_state);
3085
3086                        l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
3087                                        sizeof(rfc), (unsigned long) &rfc);
3088
3089                        if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
3090                                chan->remote_id = efs.id;
3091                                chan->remote_stype = efs.stype;
3092                                chan->remote_msdu = le16_to_cpu(efs.msdu);
3093                                chan->remote_flush_to =
3094                                                le32_to_cpu(efs.flush_to);
3095                                chan->remote_acc_lat =
3096                                                le32_to_cpu(efs.acc_lat);
3097                                chan->remote_sdu_itime =
3098                                        le32_to_cpu(efs.sdu_itime);
3099                                l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
3100                                        sizeof(efs), (unsigned long) &efs);
3101                        }
3102                        break;
3103
3104                case L2CAP_MODE_STREAMING:
3105                        size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
3106                                                chan->conn->mtu -
3107                                                L2CAP_EXT_HDR_SIZE -
3108                                                L2CAP_SDULEN_SIZE -
3109                                                L2CAP_FCS_SIZE);
3110                        rfc.max_pdu_size = cpu_to_le16(size);
3111                        chan->remote_mps = size;
3112
3113                        set_bit(CONF_MODE_DONE, &chan->conf_state);
3114
3115                        l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
3116                                        sizeof(rfc), (unsigned long) &rfc);
3117
3118                        break;
3119
3120                default:
3121                        result = L2CAP_CONF_UNACCEPT;
3122
3123                        memset(&rfc, 0, sizeof(rfc));
3124                        rfc.mode = chan->mode;
3125                }
3126
3127                if (result == L2CAP_CONF_SUCCESS)
3128                        set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
3129        }
3130        rsp->scid   = cpu_to_le16(chan->dcid);
3131        rsp->result = cpu_to_le16(result);
3132        rsp->flags  = __constant_cpu_to_le16(0);
3133
3134        return ptr - data;
3135}
3136
3137static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, void *data, u16 *result)
3138{
3139        struct l2cap_conf_req *req = data;
3140        void *ptr = req->data;
3141        int type, olen;
3142        unsigned long val;
3143        struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
3144        struct l2cap_conf_efs efs;
3145
3146        BT_DBG("chan %p, rsp %p, len %d, req %p", chan, rsp, len, data);
3147
3148        while (len >= L2CAP_CONF_OPT_SIZE) {
3149                len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
3150
3151                switch (type) {
3152                case L2CAP_CONF_MTU:
3153                        if (val < L2CAP_DEFAULT_MIN_MTU) {
3154                                *result = L2CAP_CONF_UNACCEPT;
3155                                chan->imtu = L2CAP_DEFAULT_MIN_MTU;
3156                        } else
3157                                chan->imtu = val;
3158                        l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu);
3159                        break;
3160
3161                case L2CAP_CONF_FLUSH_TO:
3162                        chan->flush_to = val;
3163                        l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
3164                                                        2, chan->flush_to);
3165                        break;
3166
3167                case L2CAP_CONF_RFC:
3168                        if (olen == sizeof(rfc))
3169                                memcpy(&rfc, (void *)val, olen);
3170
3171                        if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state) &&
3172                                                        rfc.mode != chan->mode)
3173                                return -ECONNREFUSED;
3174
3175                        chan->fcs = 0;
3176
3177                        l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
3178                                        sizeof(rfc), (unsigned long) &rfc);
3179                        break;
3180
3181                case L2CAP_CONF_EWS:
3182                        chan->ack_win = min_t(u16, val, chan->ack_win);
3183                        l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
3184                                           chan->tx_win);
3185                        break;
3186
3187                case L2CAP_CONF_EFS:
3188                        if (olen == sizeof(efs))
3189                                memcpy(&efs, (void *)val, olen);
3190
3191                        if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
3192                                        efs.stype != L2CAP_SERV_NOTRAFIC &&
3193                                        efs.stype != chan->local_stype)
3194                                return -ECONNREFUSED;
3195
3196                        l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
3197                                        sizeof(efs), (unsigned long) &efs);
3198                        break;
3199                }
3200        }
3201
3202        if (chan->mode == L2CAP_MODE_BASIC && chan->mode != rfc.mode)
3203                return -ECONNREFUSED;
3204
3205        chan->mode = rfc.mode;
3206
3207        if (*result == L2CAP_CONF_SUCCESS || *result == L2CAP_CONF_PENDING) {
3208                switch (rfc.mode) {
3209                case L2CAP_MODE_ERTM:
3210                        chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
3211                        chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
3212                        chan->mps    = le16_to_cpu(rfc.max_pdu_size);
3213                        if (!test_bit(FLAG_EXT_CTRL, &chan->flags))
3214                                chan->ack_win = min_t(u16, chan->ack_win,
3215                                                      rfc.txwin_size);
3216
3217                        if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
3218                                chan->local_msdu = le16_to_cpu(efs.msdu);
3219                                chan->local_sdu_itime =
3220                                                le32_to_cpu(efs.sdu_itime);
3221                                chan->local_acc_lat = le32_to_cpu(efs.acc_lat);
3222                                chan->local_flush_to =
3223                                                le32_to_cpu(efs.flush_to);
3224                        }
3225                        break;
3226
3227                case L2CAP_MODE_STREAMING:
3228                        chan->mps    = le16_to_cpu(rfc.max_pdu_size);
3229                }
3230        }
3231
3232        req->dcid   = cpu_to_le16(chan->dcid);
3233        req->flags  = __constant_cpu_to_le16(0);
3234
3235        return ptr - data;
3236}
3237
3238static int l2cap_build_conf_rsp(struct l2cap_chan *chan, void *data, u16 result, u16 flags)
3239{
3240        struct l2cap_conf_rsp *rsp = data;
3241        void *ptr = rsp->data;
3242
3243        BT_DBG("chan %p", chan);
3244
3245        rsp->scid   = cpu_to_le16(chan->dcid);
3246        rsp->result = cpu_to_le16(result);
3247        rsp->flags  = cpu_to_le16(flags);
3248
3249        return ptr - data;
3250}
3251
3252void __l2cap_connect_rsp_defer(struct l2cap_chan *chan)
3253{
3254        struct l2cap_conn_rsp rsp;
3255        struct l2cap_conn *conn = chan->conn;
3256        u8 buf[128];
3257
3258        rsp.scid   = cpu_to_le16(chan->dcid);
3259        rsp.dcid   = cpu_to_le16(chan->scid);
3260        rsp.result = __constant_cpu_to_le16(L2CAP_CR_SUCCESS);
3261        rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
3262        l2cap_send_cmd(conn, chan->ident,
3263                                L2CAP_CONN_RSP, sizeof(rsp), &rsp);
3264
3265        if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
3266                return;
3267
3268        l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3269                        l2cap_build_conf_req(chan, buf), buf);
3270        chan->num_conf_req++;
3271}
3272
3273static void l2cap_conf_rfc_get(struct l2cap_chan *chan, void *rsp, int len)
3274{
3275        int type, olen;
3276        unsigned long val;
3277        /* Use sane default values in case a misbehaving remote device
3278         * did not send an RFC or extended window size option.
3279         */
3280        u16 txwin_ext = chan->ack_win;
3281        struct l2cap_conf_rfc rfc = {
3282                .mode = chan->mode,
3283                .retrans_timeout = __constant_cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO),
3284                .monitor_timeout = __constant_cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO),
3285                .max_pdu_size = cpu_to_le16(chan->imtu),
3286                .txwin_size = min_t(u16, chan->ack_win, L2CAP_DEFAULT_TX_WINDOW),
3287        };
3288
3289        BT_DBG("chan %p, rsp %p, len %d", chan, rsp, len);
3290
3291        if ((chan->mode != L2CAP_MODE_ERTM) && (chan->mode != L2CAP_MODE_STREAMING))
3292                return;
3293
3294        while (len >= L2CAP_CONF_OPT_SIZE) {
3295                len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
3296
3297                switch (type) {
3298                case L2CAP_CONF_RFC:
3299                        if (olen == sizeof(rfc))
3300                                memcpy(&rfc, (void *)val, olen);
3301                        break;
3302                case L2CAP_CONF_EWS:
3303                        txwin_ext = val;
3304                        break;
3305                }
3306        }
3307
3308        switch (rfc.mode) {
3309        case L2CAP_MODE_ERTM:
3310                chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
3311                chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
3312                chan->mps = le16_to_cpu(rfc.max_pdu_size);
3313                if (test_bit(FLAG_EXT_CTRL, &chan->flags))
3314                        chan->ack_win = min_t(u16, chan->ack_win, txwin_ext);
3315                else
3316                        chan->ack_win = min_t(u16, chan->ack_win,
3317                                              rfc.txwin_size);
3318                break;
3319        case L2CAP_MODE_STREAMING:
3320                chan->mps    = le16_to_cpu(rfc.max_pdu_size);
3321        }
3322}
3323
3324static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3325{
3326        struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data;
3327
3328        if (rej->reason != L2CAP_REJ_NOT_UNDERSTOOD)
3329                return 0;
3330
3331        if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
3332                                        cmd->ident == conn->info_ident) {
3333                cancel_delayed_work(&conn->info_timer);
3334
3335                conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3336                conn->info_ident = 0;
3337
3338                l2cap_conn_start(conn);
3339        }
3340
3341        return 0;
3342}
3343
3344static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3345{
3346        struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
3347        struct l2cap_conn_rsp rsp;
3348        struct l2cap_chan *chan = NULL, *pchan;
3349        struct sock *parent, *sk = NULL;
3350        int result, status = L2CAP_CS_NO_INFO;
3351
3352        u16 dcid = 0, scid = __le16_to_cpu(req->scid);
3353        __le16 psm = req->psm;
3354
3355        BT_DBG("psm 0x%2.2x scid 0x%4.4x", __le16_to_cpu(psm), scid);
3356
3357        /* Check if we have socket listening on psm */
3358        pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, conn->src, conn->dst);
3359        if (!pchan) {
3360                result = L2CAP_CR_BAD_PSM;
3361                goto sendresp;
3362        }
3363
3364        parent = pchan->sk;
3365
3366        mutex_lock(&conn->chan_lock);
3367        lock_sock(parent);
3368
3369        /* Check if the ACL is secure enough (if not SDP) */
3370        if (psm != __constant_cpu_to_le16(L2CAP_PSM_SDP) &&
3371                                !hci_conn_check_link_mode(conn->hcon)) {
3372                conn->disc_reason = HCI_ERROR_AUTH_FAILURE;
3373                result = L2CAP_CR_SEC_BLOCK;
3374                goto response;
3375        }
3376
3377        result = L2CAP_CR_NO_MEM;
3378
3379        /* Check if we already have channel with that dcid */
3380        if (__l2cap_get_chan_by_dcid(conn, scid))
3381                goto response;
3382
3383        chan = pchan->ops->new_connection(pchan);
3384        if (!chan)
3385                goto response;
3386
3387        sk = chan->sk;
3388
3389        hci_conn_hold(conn->hcon);
3390
3391        bacpy(&bt_sk(sk)->src, conn->src);
3392        bacpy(&bt_sk(sk)->dst, conn->dst);
3393        chan->psm  = psm;
3394        chan->dcid = scid;
3395
3396        bt_accept_enqueue(parent, sk);
3397
3398        __l2cap_chan_add(conn, chan);
3399
3400        dcid = chan->scid;
3401
3402        __set_chan_timer(chan, sk->sk_sndtimeo);
3403
3404        chan->ident = cmd->ident;
3405
3406        if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
3407                if (l2cap_chan_check_security(chan)) {
3408                        if (test_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags)) {
3409                                __l2cap_state_change(chan, BT_CONNECT2);
3410                                result = L2CAP_CR_PEND;
3411                                status = L2CAP_CS_AUTHOR_PEND;
3412                                parent->sk_data_ready(parent, 0);
3413                        } else {
3414                                __l2cap_state_change(chan, BT_CONFIG);
3415                                result = L2CAP_CR_SUCCESS;
3416                                status = L2CAP_CS_NO_INFO;
3417                        }
3418                } else {
3419                        __l2cap_state_change(chan, BT_CONNECT2);
3420                        result = L2CAP_CR_PEND;
3421                        status = L2CAP_CS_AUTHEN_PEND;
3422                }
3423        } else {
3424                __l2cap_state_change(chan, BT_CONNECT2);
3425                result = L2CAP_CR_PEND;
3426                status = L2CAP_CS_NO_INFO;
3427        }
3428
3429response:
3430        release_sock(parent);
3431        mutex_unlock(&conn->chan_lock);
3432
3433sendresp:
3434        rsp.scid   = cpu_to_le16(scid);
3435        rsp.dcid   = cpu_to_le16(dcid);
3436        rsp.result = cpu_to_le16(result);
3437        rsp.status = cpu_to_le16(status);
3438        l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
3439
3440        if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
3441                struct l2cap_info_req info;
3442                info.type = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
3443
3444                conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
3445                conn->info_ident = l2cap_get_ident(conn);
3446
3447                schedule_delayed_work(&conn->info_timer, L2CAP_INFO_TIMEOUT);
3448
3449                l2cap_send_cmd(conn, conn->info_ident,
3450                                        L2CAP_INFO_REQ, sizeof(info), &info);
3451        }
3452
3453        if (chan && !test_bit(CONF_REQ_SENT, &chan->conf_state) &&
3454                                result == L2CAP_CR_SUCCESS) {
3455                u8 buf[128];
3456                set_bit(CONF_REQ_SENT, &chan->conf_state);
3457                l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3458                                        l2cap_build_conf_req(chan, buf), buf);
3459                chan->num_conf_req++;
3460        }
3461
3462        return 0;
3463}
3464
3465static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3466{
3467        struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
3468        u16 scid, dcid, result, status;
3469        struct l2cap_chan *chan;
3470        u8 req[128];
3471        int err;
3472
3473        scid   = __le16_to_cpu(rsp->scid);
3474        dcid   = __le16_to_cpu(rsp->dcid);
3475        result = __le16_to_cpu(rsp->result);
3476        status = __le16_to_cpu(rsp->status);
3477
3478        BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x",
3479                                                dcid, scid, result, status);
3480
3481        mutex_lock(&conn->chan_lock);
3482
3483        if (scid) {
3484                chan = __l2cap_get_chan_by_scid(conn, scid);
3485                if (!chan) {
3486                        err = -EFAULT;
3487                        goto unlock;
3488                }
3489        } else {
3490                chan = __l2cap_get_chan_by_ident(conn, cmd->ident);
3491                if (!chan) {
3492                        err = -EFAULT;
3493                        goto unlock;
3494                }
3495        }
3496
3497        err = 0;
3498
3499        l2cap_chan_lock(chan);
3500
3501        switch (result) {
3502        case L2CAP_CR_SUCCESS:
3503                l2cap_state_change(chan, BT_CONFIG);
3504                chan->ident = 0;
3505                chan->dcid = dcid;
3506                clear_bit(CONF_CONNECT_PEND, &chan->conf_state);
3507
3508                if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
3509                        break;
3510
3511                l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3512                                        l2cap_build_conf_req(chan, req), req);
3513                chan->num_conf_req++;
3514                break;
3515
3516        case L2CAP_CR_PEND:
3517                set_bit(CONF_CONNECT_PEND, &chan->conf_state);
3518                break;
3519
3520        default:
3521                l2cap_chan_del(chan, ECONNREFUSED);
3522                break;
3523        }
3524
3525        l2cap_chan_unlock(chan);
3526
3527unlock:
3528        mutex_unlock(&conn->chan_lock);
3529
3530        return err;
3531}
3532
3533static inline void set_default_fcs(struct l2cap_chan *chan)
3534{
3535        /* FCS is enabled only in ERTM or streaming mode, if one or both
3536         * sides request it.
3537         */
3538        if (chan->mode != L2CAP_MODE_ERTM && chan->mode != L2CAP_MODE_STREAMING)
3539                chan->fcs = L2CAP_FCS_NONE;
3540        else if (!test_bit(CONF_NO_FCS_RECV, &chan->conf_state))
3541                chan->fcs = L2CAP_FCS_CRC16;
3542}
3543
3544static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
3545{
3546        struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
3547        u16 dcid, flags;
3548        u8 rsp[64];
3549        struct l2cap_chan *chan;
3550        int len, err = 0;
3551
3552        dcid  = __le16_to_cpu(req->dcid);
3553        flags = __le16_to_cpu(req->flags);
3554
3555        BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
3556
3557        chan = l2cap_get_chan_by_scid(conn, dcid);
3558        if (!chan)
3559                return -ENOENT;
3560
3561        if (chan->state != BT_CONFIG && chan->state != BT_CONNECT2) {
3562                struct l2cap_cmd_rej_cid rej;
3563
3564                rej.reason = __constant_cpu_to_le16(L2CAP_REJ_INVALID_CID);
3565                rej.scid = cpu_to_le16(chan->scid);
3566                rej.dcid = cpu_to_le16(chan->dcid);
3567
3568                l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
3569                                sizeof(rej), &rej);
3570                goto unlock;
3571        }
3572
3573        /* Reject if config buffer is too small. */
3574        len = cmd_len - sizeof(*req);
3575        if (len < 0 || chan->conf_len + len > sizeof(chan->conf_req)) {
3576                l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
3577                                l2cap_build_conf_rsp(chan, rsp,
3578                                        L2CAP_CONF_REJECT, flags), rsp);
3579                goto unlock;
3580        }
3581
3582        /* Store config. */
3583        memcpy(chan->conf_req + chan->conf_len, req->data, len);
3584        chan->conf_len += len;
3585
3586        if (flags & L2CAP_CONF_FLAG_CONTINUATION) {
3587                /* Incomplete config. Send empty response. */
3588                l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
3589                                l2cap_build_conf_rsp(chan, rsp,
3590                                        L2CAP_CONF_SUCCESS, flags), rsp);
3591                goto unlock;
3592        }
3593
3594        /* Complete config. */
3595        len = l2cap_parse_conf_req(chan, rsp);
3596        if (len < 0) {
3597                l2cap_send_disconn_req(conn, chan, ECONNRESET);
3598                goto unlock;
3599        }
3600
3601        l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
3602        chan->num_conf_rsp++;
3603
3604        /* Reset config buffer. */
3605        chan->conf_len = 0;
3606
3607        if (!test_bit(CONF_OUTPUT_DONE, &chan->conf_state))
3608                goto unlock;
3609
3610        if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
3611                set_default_fcs(chan);
3612
3613                if (chan->mode == L2CAP_MODE_ERTM ||
3614                    chan->mode == L2CAP_MODE_STREAMING)
3615                        err = l2cap_ertm_init(chan);
3616
3617                if (err < 0)
3618                        l2cap_send_disconn_req(chan->conn, chan, -err);
3619                else
3620                        l2cap_chan_ready(chan);
3621
3622                goto unlock;
3623        }
3624
3625        if (!test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) {
3626                u8 buf[64];
3627                l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3628                                        l2cap_build_conf_req(chan, buf), buf);
3629                chan->num_conf_req++;
3630        }
3631
3632        /* Got Conf Rsp PENDING from remote side and asume we sent
3633           Conf Rsp PENDING in the code above */
3634        if (test_bit(CONF_REM_CONF_PEND, &chan->conf_state) &&
3635                        test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
3636
3637                /* check compatibility */
3638
3639                clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
3640                set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
3641
3642                l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
3643                                        l2cap_build_conf_rsp(chan, rsp,
3644                                        L2CAP_CONF_SUCCESS, flags), rsp);
3645        }
3646
3647unlock:
3648        l2cap_chan_unlock(chan);
3649        return err;
3650}
3651
3652static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3653{
3654        struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
3655        u16 scid, flags, result;
3656        struct l2cap_chan *chan;
3657        int len = le16_to_cpu(cmd->len) - sizeof(*rsp);
3658        int err = 0;
3659
3660        scid   = __le16_to_cpu(rsp->scid);
3661        flags  = __le16_to_cpu(rsp->flags);
3662        result = __le16_to_cpu(rsp->result);
3663
3664        BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x len %d", scid, flags,
3665               result, len);
3666
3667        chan = l2cap_get_chan_by_scid(conn, scid);
3668        if (!chan)
3669                return 0;
3670
3671        switch (result) {
3672        case L2CAP_CONF_SUCCESS:
3673                l2cap_conf_rfc_get(chan, rsp->data, len);
3674                clear_bit(CONF_REM_CONF_PEND, &chan->conf_state);
3675                break;
3676
3677        case L2CAP_CONF_PENDING:
3678                set_bit(CONF_REM_CONF_PEND, &chan->conf_state);
3679
3680                if (test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
3681                        char buf[64];
3682
3683                        len = l2cap_parse_conf_rsp(chan, rsp->data, len,
3684                                                                buf, &result);
3685                        if (len < 0) {
3686                                l2cap_send_disconn_req(conn, chan, ECONNRESET);
3687                                goto done;
3688                        }
3689
3690                        /* check compatibility */
3691
3692                        clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
3693                        set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
3694
3695                        l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
3696                                                l2cap_build_conf_rsp(chan, buf,
3697                                                L2CAP_CONF_SUCCESS, 0x0000), buf);
3698                }
3699                goto done;
3700
3701        case L2CAP_CONF_UNACCEPT:
3702                if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
3703                        char req[64];
3704
3705                        if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
3706                                l2cap_send_disconn_req(conn, chan, ECONNRESET);
3707                                goto done;
3708                        }
3709
3710                        /* throw out any old stored conf requests */
3711                        result = L2CAP_CONF_SUCCESS;
3712                        len = l2cap_parse_conf_rsp(chan, rsp->data, len,
3713                                                                req, &result);
3714                        if (len < 0) {
3715                                l2cap_send_disconn_req(conn, chan, ECONNRESET);
3716                                goto done;
3717                        }
3718
3719                        l2cap_send_cmd(conn, l2cap_get_ident(conn),
3720                                                L2CAP_CONF_REQ, len, req);
3721                        chan->num_conf_req++;
3722                        if (result != L2CAP_CONF_SUCCESS)
3723                                goto done;
3724                        break;
3725                }
3726
3727        default:
3728                l2cap_chan_set_err(chan, ECONNRESET);
3729
3730                __set_chan_timer(chan, L2CAP_DISC_REJ_TIMEOUT);
3731                l2cap_send_disconn_req(conn, chan, ECONNRESET);
3732                goto done;
3733        }
3734
3735        if (flags & L2CAP_CONF_FLAG_CONTINUATION)
3736                goto done;
3737
3738        set_bit(CONF_INPUT_DONE, &chan->conf_state);
3739
3740        if (test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) {
3741                set_default_fcs(chan);
3742
3743                if (chan->mode == L2CAP_MODE_ERTM ||
3744                    chan->mode == L2CAP_MODE_STREAMING)
3745                        err = l2cap_ertm_init(chan);
3746
3747                if (err < 0)
3748                        l2cap_send_disconn_req(chan->conn, chan, -err);
3749                else
3750                        l2cap_chan_ready(chan);
3751        }
3752
3753done:
3754        l2cap_chan_unlock(chan);
3755        return err;
3756}
3757
3758static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3759{
3760        struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
3761        struct l2cap_disconn_rsp rsp;
3762        u16 dcid, scid;
3763        struct l2cap_chan *chan;
3764        struct sock *sk;
3765
3766        scid = __le16_to_cpu(req->scid);
3767        dcid = __le16_to_cpu(req->dcid);
3768
3769        BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
3770
3771        mutex_lock(&conn->chan_lock);
3772
3773        chan = __l2cap_get_chan_by_scid(conn, dcid);
3774        if (!chan) {
3775                mutex_unlock(&conn->chan_lock);
3776                return 0;
3777        }
3778
3779        l2cap_chan_lock(chan);
3780
3781        sk = chan->sk;
3782
3783        rsp.dcid = cpu_to_le16(chan->scid);
3784        rsp.scid = cpu_to_le16(chan->dcid);
3785        l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
3786
3787        lock_sock(sk);
3788        sk->sk_shutdown = SHUTDOWN_MASK;
3789        release_sock(sk);
3790
3791        l2cap_chan_hold(chan);
3792        l2cap_chan_del(chan, ECONNRESET);
3793
3794        l2cap_chan_unlock(chan);
3795
3796        chan->ops->close(chan);
3797        l2cap_chan_put(chan);
3798
3799        mutex_unlock(&conn->chan_lock);
3800
3801        return 0;
3802}
3803
3804static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3805{
3806        struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
3807        u16 dcid, scid;
3808        struct l2cap_chan *chan;
3809
3810        scid = __le16_to_cpu(rsp->scid);
3811        dcid = __le16_to_cpu(rsp->dcid);
3812
3813        BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
3814
3815        mutex_lock(&conn->chan_lock);
3816
3817        chan = __l2cap_get_chan_by_scid(conn, scid);
3818        if (!chan) {
3819                mutex_unlock(&conn->chan_lock);
3820                return 0;
3821        }
3822
3823        l2cap_chan_lock(chan);
3824
3825        l2cap_chan_hold(chan);
3826        l2cap_chan_del(chan, 0);
3827
3828        l2cap_chan_unlock(chan);
3829
3830        chan->ops->close(chan);
3831        l2cap_chan_put(chan);
3832
3833        mutex_unlock(&conn->chan_lock);
3834
3835        return 0;
3836}
3837
3838static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3839{
3840        struct l2cap_info_req *req = (struct l2cap_info_req *) data;
3841        u16 type;
3842
3843        type = __le16_to_cpu(req->type);
3844
3845        BT_DBG("type 0x%4.4x", type);
3846
3847        if (type == L2CAP_IT_FEAT_MASK) {
3848                u8 buf[8];
3849                u32 feat_mask = l2cap_feat_mask;
3850                struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
3851                rsp->type   = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
3852                rsp->result = __constant_cpu_to_le16(L2CAP_IR_SUCCESS);
3853                if (!disable_ertm)
3854                        feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
3855                                                         | L2CAP_FEAT_FCS;
3856                if (enable_hs)
3857                        feat_mask |= L2CAP_FEAT_EXT_FLOW
3858                                                | L2CAP_FEAT_EXT_WINDOW;
3859
3860                put_unaligned_le32(feat_mask, rsp->data);
3861                l2cap_send_cmd(conn, cmd->ident,
3862                                        L2CAP_INFO_RSP, sizeof(buf), buf);
3863        } else if (type == L2CAP_IT_FIXED_CHAN) {
3864                u8 buf[12];
3865                struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
3866
3867                if (enable_hs)
3868                        l2cap_fixed_chan[0] |= L2CAP_FC_A2MP;
3869                else
3870                        l2cap_fixed_chan[0] &= ~L2CAP_FC_A2MP;
3871
3872                rsp->type   = __constant_cpu_to_le16(L2CAP_IT_FIXED_CHAN);
3873                rsp->result = __constant_cpu_to_le16(L2CAP_IR_SUCCESS);
3874                memcpy(rsp->data, l2cap_fixed_chan, sizeof(l2cap_fixed_chan));
3875                l2cap_send_cmd(conn, cmd->ident,
3876                                        L2CAP_INFO_RSP, sizeof(buf), buf);
3877        } else {
3878                struct l2cap_info_rsp rsp;
3879                rsp.type   = cpu_to_le16(type);
3880                rsp.result = __constant_cpu_to_le16(L2CAP_IR_NOTSUPP);
3881                l2cap_send_cmd(conn, cmd->ident,
3882                                        L2CAP_INFO_RSP, sizeof(rsp), &rsp);
3883        }
3884
3885        return 0;
3886}
3887
3888static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3889{
3890        struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
3891        u16 type, result;
3892
3893        type   = __le16_to_cpu(rsp->type);
3894        result = __le16_to_cpu(rsp->result);
3895
3896        BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
3897
3898        /* L2CAP Info req/rsp are unbound to channels, add extra checks */
3899        if (cmd->ident != conn->info_ident ||
3900                        conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)
3901                return 0;
3902
3903        cancel_delayed_work(&conn->info_timer);
3904
3905        if (result != L2CAP_IR_SUCCESS) {
3906                conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3907                conn->info_ident = 0;
3908
3909                l2cap_conn_start(conn);
3910
3911                return 0;
3912        }
3913
3914        switch (type) {
3915        case L2CAP_IT_FEAT_MASK:
3916                conn->feat_mask = get_unaligned_le32(rsp->data);
3917
3918                if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
3919                        struct l2cap_info_req req;
3920                        req.type = __constant_cpu_to_le16(L2CAP_IT_FIXED_CHAN);
3921
3922                        conn->info_ident = l2cap_get_ident(conn);
3923
3924                        l2cap_send_cmd(conn, conn->info_ident,
3925                                        L2CAP_INFO_REQ, sizeof(req), &req);
3926                } else {
3927                        conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3928                        conn->info_ident = 0;
3929
3930                        l2cap_conn_start(conn);
3931                }
3932                break;
3933
3934        case L2CAP_IT_FIXED_CHAN:
3935                conn->fixed_chan_mask = rsp->data[0];
3936                conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3937                conn->info_ident = 0;
3938
3939                l2cap_conn_start(conn);
3940                break;
3941        }
3942
3943        return 0;
3944}
3945
3946static inline int l2cap_create_channel_req(struct l2cap_conn *conn,
3947                                        struct l2cap_cmd_hdr *cmd, u16 cmd_len,
3948                                        void *data)
3949{
3950        struct l2cap_create_chan_req *req = data;
3951        struct l2cap_create_chan_rsp rsp;
3952        u16 psm, scid;
3953
3954        if (cmd_len != sizeof(*req))
3955                return -EPROTO;
3956
3957        if (!enable_hs)
3958                return -EINVAL;
3959
3960        psm = le16_to_cpu(req->psm);
3961        scid = le16_to_cpu(req->scid);
3962
3963        BT_DBG("psm 0x%2.2x, scid 0x%4.4x, amp_id %d", psm, scid, req->amp_id);
3964
3965        /* Placeholder: Always reject */
3966        rsp.dcid = 0;
3967        rsp.scid = cpu_to_le16(scid);
3968        rsp.result = __constant_cpu_to_le16(L2CAP_CR_NO_MEM);
3969        rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
3970
3971        l2cap_send_cmd(conn, cmd->ident, L2CAP_CREATE_CHAN_RSP,
3972                       sizeof(rsp), &rsp);
3973
3974        return 0;
3975}
3976
3977static inline int l2cap_create_channel_rsp(struct l2cap_conn *conn,
3978                                        struct l2cap_cmd_hdr *cmd, void *data)
3979{
3980        BT_DBG("conn %p", conn);
3981
3982        return l2cap_connect_rsp(conn, cmd, data);
3983}
3984
3985static void l2cap_send_move_chan_rsp(struct l2cap_conn *conn, u8 ident,
3986                                     u16 icid, u16 result)
3987{
3988        struct l2cap_move_chan_rsp rsp;
3989
3990        BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result);
3991
3992        rsp.icid = cpu_to_le16(icid);
3993        rsp.result = cpu_to_le16(result);
3994
3995        l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_RSP, sizeof(rsp), &rsp);
3996}
3997
3998static void l2cap_send_move_chan_cfm(struct l2cap_conn *conn,
3999                                     struct l2cap_chan *chan,
4000                                     u16 icid, u16 result)
4001{
4002        struct l2cap_move_chan_cfm cfm;
4003        u8 ident;
4004
4005        BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result);
4006
4007        ident = l2cap_get_ident(conn);
4008        if (chan)
4009                chan->ident = ident;
4010
4011        cfm.icid = cpu_to_le16(icid);
4012        cfm.result = cpu_to_le16(result);
4013
4014        l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM, sizeof(cfm), &cfm);
4015}
4016
4017static void l2cap_send_move_chan_cfm_rsp(struct l2cap_conn *conn, u8 ident,
4018                                         u16 icid)
4019{
4020        struct l2cap_move_chan_cfm_rsp rsp;
4021
4022        BT_DBG("icid 0x%4.4x", icid);
4023
4024        rsp.icid = cpu_to_le16(icid);
4025        l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM_RSP, sizeof(rsp), &rsp);
4026}
4027
4028static inline int l2cap_move_channel_req(struct l2cap_conn *conn,
4029                                         struct l2cap_cmd_hdr *cmd,
4030                                         u16 cmd_len, void *data)
4031{
4032        struct l2cap_move_chan_req *req = data;
4033        u16 icid = 0;
4034        u16 result = L2CAP_MR_NOT_ALLOWED;
4035
4036        if (cmd_len != sizeof(*req))
4037                return -EPROTO;
4038
4039        icid = le16_to_cpu(req->icid);
4040
4041        BT_DBG("icid 0x%4.4x, dest_amp_id %d", icid, req->dest_amp_id);
4042
4043        if (!enable_hs)
4044                return -EINVAL;
4045
4046        /* Placeholder: Always refuse */
4047        l2cap_send_move_chan_rsp(conn, cmd->ident, icid, result);
4048
4049        return 0;
4050}
4051
4052static inline int l2cap_move_channel_rsp(struct l2cap_conn *conn,
4053                                         struct l2cap_cmd_hdr *cmd,
4054                                         u16 cmd_len, void *data)
4055{
4056        struct l2cap_move_chan_rsp *rsp = data;
4057        u16 icid, result;
4058
4059        if (cmd_len != sizeof(*rsp))
4060                return -EPROTO;
4061
4062        icid = le16_to_cpu(rsp->icid);
4063        result = le16_to_cpu(rsp->result);
4064
4065        BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result);
4066
4067        /* Placeholder: Always unconfirmed */
4068        l2cap_send_move_chan_cfm(conn, NULL, icid, L2CAP_MC_UNCONFIRMED);
4069
4070        return 0;
4071}
4072
4073static inline int l2cap_move_channel_confirm(struct l2cap_conn *conn,
4074                                             struct l2cap_cmd_hdr *cmd,
4075                                             u16 cmd_len, void *data)
4076{
4077        struct l2cap_move_chan_cfm *cfm = data;
4078        u16 icid, result;
4079
4080        if (cmd_len != sizeof(*cfm))
4081                return -EPROTO;
4082
4083        icid = le16_to_cpu(cfm->icid);
4084        result = le16_to_cpu(cfm->result);
4085
4086        BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result);
4087
4088        l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid);
4089
4090        return 0;
4091}
4092
4093static inline int l2cap_move_channel_confirm_rsp(struct l2cap_conn *conn,
4094                                                 struct l2cap_cmd_hdr *cmd,
4095                                                 u16 cmd_len, void *data)
4096{
4097        struct l2cap_move_chan_cfm_rsp *rsp = data;
4098        u16 icid;
4099
4100        if (cmd_len != sizeof(*rsp))
4101                return -EPROTO;
4102
4103        icid = le16_to_cpu(rsp->icid);
4104
4105        BT_DBG("icid 0x%4.4x", icid);
4106
4107        return 0;
4108}
4109
4110static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency,
4111                                                        u16 to_multiplier)
4112{
4113        u16 max_latency;
4114
4115        if (min > max || min < 6 || max > 3200)
4116                return -EINVAL;
4117
4118        if (to_multiplier < 10 || to_multiplier > 3200)
4119                return -EINVAL;
4120
4121        if (max >= to_multiplier * 8)
4122                return -EINVAL;
4123
4124        max_latency = (to_multiplier * 8 / max) - 1;
4125        if (latency > 499 || latency > max_latency)
4126                return -EINVAL;
4127
4128        return 0;
4129}
4130
4131static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
4132                                        struct l2cap_cmd_hdr *cmd, u8 *data)
4133{
4134        struct hci_conn *hcon = conn->hcon;
4135        struct l2cap_conn_param_update_req *req;
4136        struct l2cap_conn_param_update_rsp rsp;
4137        u16 min, max, latency, to_multiplier, cmd_len;
4138        int err;
4139
4140        if (!(hcon->link_mode & HCI_LM_MASTER))
4141                return -EINVAL;
4142
4143        cmd_len = __le16_to_cpu(cmd->len);
4144        if (cmd_len != sizeof(struct l2cap_conn_param_update_req))
4145                return -EPROTO;
4146
4147        req = (struct l2cap_conn_param_update_req *) data;
4148        min             = __le16_to_cpu(req->min);
4149        max             = __le16_to_cpu(req->max);
4150        latency         = __le16_to_cpu(req->latency);
4151        to_multiplier   = __le16_to_cpu(req->to_multiplier);
4152
4153        BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x",
4154                                                min, max, latency, to_multiplier);
4155
4156        memset(&rsp, 0, sizeof(rsp));
4157
4158        err = l2cap_check_conn_param(min, max, latency, to_multiplier);
4159        if (err)
4160                rsp.result = __constant_cpu_to_le16(L2CAP_CONN_PARAM_REJECTED);
4161        else
4162                rsp.result = __constant_cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED);
4163
4164        l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP,
4165                                                        sizeof(rsp), &rsp);
4166
4167        if (!err)
4168                hci_le_conn_update(hcon, min, max, latency, to_multiplier);
4169
4170        return 0;
4171}
4172
4173static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
4174                        struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
4175{
4176        int err = 0;
4177
4178        switch (cmd->code) {
4179        case L2CAP_COMMAND_REJ:
4180                l2cap_command_rej(conn, cmd, data);
4181                break;
4182
4183        case L2CAP_CONN_REQ:
4184                err = l2cap_connect_req(conn, cmd, data);
4185                break;
4186
4187        case L2CAP_CONN_RSP:
4188                err = l2cap_connect_rsp(conn, cmd, data);
4189                break;
4190
4191        case L2CAP_CONF_REQ:
4192                err = l2cap_config_req(conn, cmd, cmd_len, data);
4193                break;
4194
4195        case L2CAP_CONF_RSP:
4196                err = l2cap_config_rsp(conn, cmd, data);
4197                break;
4198
4199        case L2CAP_DISCONN_REQ:
4200                err = l2cap_disconnect_req(conn, cmd, data);
4201                break;
4202
4203        case L2CAP_DISCONN_RSP:
4204                err = l2cap_disconnect_rsp(conn, cmd, data);
4205                break;
4206
4207        case L2CAP_ECHO_REQ:
4208                l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data);
4209                break;
4210
4211        case L2CAP_ECHO_RSP:
4212                break;
4213
4214        case L2CAP_INFO_REQ:
4215                err = l2cap_information_req(conn, cmd, data);
4216                break;
4217
4218        case L2CAP_INFO_RSP:
4219                err = l2cap_information_rsp(conn, cmd, data);
4220                break;
4221
4222        case L2CAP_CREATE_CHAN_REQ:
4223                err = l2cap_create_channel_req(conn, cmd, cmd_len, data);
4224                break;
4225
4226        case L2CAP_CREATE_CHAN_RSP:
4227                err = l2cap_create_channel_rsp(conn, cmd, data);
4228                break;
4229
4230        case L2CAP_MOVE_CHAN_REQ:
4231                err = l2cap_move_channel_req(conn, cmd, cmd_len, data);
4232                break;
4233
4234        case L2CAP_MOVE_CHAN_RSP:
4235                err = l2cap_move_channel_rsp(conn, cmd, cmd_len, data);
4236                break;
4237
4238        case L2CAP_MOVE_CHAN_CFM:
4239                err = l2cap_move_channel_confirm(conn, cmd, cmd_len, data);
4240                break;
4241
4242        case L2CAP_MOVE_CHAN_CFM_RSP:
4243                err = l2cap_move_channel_confirm_rsp(conn, cmd, cmd_len, data);
4244                break;
4245
4246        default:
4247                BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code);
4248                err = -EINVAL;
4249                break;
4250        }
4251
4252        return err;
4253}
4254
4255static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
4256                                        struct l2cap_cmd_hdr *cmd, u8 *data)
4257{
4258        switch (cmd->code) {
4259        case L2CAP_COMMAND_REJ:
4260                return 0;
4261
4262        case L2CAP_CONN_PARAM_UPDATE_REQ:
4263                return l2cap_conn_param_update_req(conn, cmd, data);
4264
4265        case L2CAP_CONN_PARAM_UPDATE_RSP:
4266                return 0;
4267
4268        default:
4269                BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code);
4270                return -EINVAL;
4271        }
4272}
4273
4274static inline void l2cap_sig_channel(struct l2cap_conn *conn,
4275                                                        struct sk_buff *skb)
4276{
4277        u8 *data = skb->data;
4278        int len = skb->len;
4279        struct l2cap_cmd_hdr cmd;
4280        int err;
4281
4282        l2cap_raw_recv(conn, skb);
4283
4284        while (len >= L2CAP_CMD_HDR_SIZE) {
4285                u16 cmd_len;
4286                memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
4287                data += L2CAP_CMD_HDR_SIZE;
4288                len  -= L2CAP_CMD_HDR_SIZE;
4289
4290                cmd_len = le16_to_cpu(cmd.len);
4291
4292                BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, cmd.ident);
4293
4294                if (cmd_len > len || !cmd.ident) {
4295                        BT_DBG("corrupted command");
4296                        break;
4297                }
4298
4299                if (conn->hcon->type == LE_LINK)
4300                        err = l2cap_le_sig_cmd(conn, &cmd, data);
4301                else
4302                        err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data);
4303
4304                if (err) {
4305                        struct l2cap_cmd_rej_unk rej;
4306
4307                        BT_ERR("Wrong link type (%d)", err);
4308
4309                        /* FIXME: Map err to a valid reason */
4310                        rej.reason = __constant_cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
4311                        l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
4312                }
4313
4314                data += cmd_len;
4315                len  -= cmd_len;
4316        }
4317
4318        kfree_skb(skb);
4319}
4320
4321static int l2cap_check_fcs(struct l2cap_chan *chan,  struct sk_buff *skb)
4322{
4323        u16 our_fcs, rcv_fcs;
4324        int hdr_size;
4325
4326        if (test_bit(FLAG_EXT_CTRL, &chan->flags))
4327                hdr_size = L2CAP_EXT_HDR_SIZE;
4328        else
4329                hdr_size = L2CAP_ENH_HDR_SIZE;
4330
4331        if (chan->fcs == L2CAP_FCS_CRC16) {
4332                skb_trim(skb, skb->len - L2CAP_FCS_SIZE);
4333                rcv_fcs = get_unaligned_le16(skb->data + skb->len);
4334                our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
4335
4336                if (our_fcs != rcv_fcs)
4337                        return -EBADMSG;
4338        }
4339        return 0;
4340}
4341
4342static void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan)
4343{
4344        struct l2cap_ctrl control;
4345
4346        BT_DBG("chan %p", chan);
4347
4348        memset(&control, 0, sizeof(control));
4349        control.sframe = 1;
4350        control.final = 1;
4351        control.reqseq = chan->buffer_seq;
4352        set_bit(CONN_SEND_FBIT, &chan->conn_state);
4353
4354        if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
4355                control.super = L2CAP_SUPER_RNR;
4356                l2cap_send_sframe(chan, &control);
4357        }
4358
4359        if (test_and_clear_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
4360            chan->unacked_frames > 0)
4361                __set_retrans_timer(chan);
4362
4363        /* Send pending iframes */
4364        l2cap_ertm_send(chan);
4365
4366        if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
4367            test_bit(CONN_SEND_FBIT, &chan->conn_state)) {
4368                /* F-bit wasn't sent in an s-frame or i-frame yet, so
4369                 * send it now.
4370                 */
4371                control.super = L2CAP_SUPER_RR;
4372                l2cap_send_sframe(chan, &control);
4373        }
4374}
4375
4376static void append_skb_frag(struct sk_buff *skb,
4377                        struct sk_buff *new_frag, struct sk_buff **last_frag)
4378{
4379        /* skb->len reflects data in skb as well as all fragments
4380         * skb->data_len reflects only data in fragments
4381         */
4382        if (!skb_has_frag_list(skb))
4383                skb_shinfo(skb)->frag_list = new_frag;
4384
4385        new_frag->next = NULL;
4386
4387        (*last_frag)->next = new_frag;
4388        *last_frag = new_frag;
4389
4390        skb->len += new_frag->len;
4391        skb->data_len += new_frag->len;
4392        skb->truesize += new_frag->truesize;
4393}
4394
4395static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb,
4396                                struct l2cap_ctrl *control)
4397{
4398        int err = -EINVAL;
4399
4400        switch (control->sar) {
4401        case L2CAP_SAR_UNSEGMENTED:
4402                if (chan->sdu)
4403                        break;
4404
4405                err = chan->ops->recv(chan, skb);
4406                break;
4407
4408        case L2CAP_SAR_START:
4409                if (chan->sdu)
4410                        break;
4411
4412                chan->sdu_len = get_unaligned_le16(skb->data);
4413                skb_pull(skb, L2CAP_SDULEN_SIZE);
4414
4415                if (chan->sdu_len > chan->imtu) {
4416                        err = -EMSGSIZE;
4417                        break;
4418                }
4419
4420                if (skb->len >= chan->sdu_len)
4421                        break;
4422
4423                chan->sdu = skb;
4424                chan->sdu_last_frag = skb;
4425
4426                skb = NULL;
4427                err = 0;
4428                break;
4429
4430        case L2CAP_SAR_CONTINUE:
4431                if (!chan->sdu)
4432                        break;
4433
4434                append_skb_frag(chan->sdu, skb,
4435                                &chan->sdu_last_frag);
4436                skb = NULL;
4437
4438                if (chan->sdu->len >= chan->sdu_len)
4439                        break;
4440
4441                err = 0;
4442                break;
4443
4444        case L2CAP_SAR_END:
4445                if (!chan->sdu)
4446                        break;
4447
4448                append_skb_frag(chan->sdu, skb,
4449                                &chan->sdu_last_frag);
4450                skb = NULL;
4451
4452                if (chan->sdu->len != chan->sdu_len)
4453                        break;
4454
4455                err = chan->ops->recv(chan, chan->sdu);
4456
4457                if (!err) {
4458                        /* Reassembly complete */
4459                        chan->sdu = NULL;
4460                        chan->sdu_last_frag = NULL;
4461                        chan->sdu_len = 0;
4462                }
4463                break;
4464        }
4465
4466        if (err) {
4467                kfree_skb(skb);
4468                kfree_skb(chan->sdu);
4469                chan->sdu = NULL;
4470                chan->sdu_last_frag = NULL;
4471                chan->sdu_len = 0;
4472        }
4473
4474        return err;
4475}
4476
4477void l2cap_chan_busy(struct l2cap_chan *chan, int busy)
4478{
4479        u8 event;
4480
4481        if (chan->mode != L2CAP_MODE_ERTM)
4482                return;
4483
4484        event = busy ? L2CAP_EV_LOCAL_BUSY_DETECTED : L2CAP_EV_LOCAL_BUSY_CLEAR;
4485        l2cap_tx(chan, NULL, NULL, event);
4486}
4487
4488static int l2cap_rx_queued_iframes(struct l2cap_chan *chan)
4489{
4490        int err = 0;
4491        /* Pass sequential frames to l2cap_reassemble_sdu()
4492         * until a gap is encountered.
4493         */
4494
4495        BT_DBG("chan %p", chan);
4496
4497        while (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
4498                struct sk_buff *skb;
4499                BT_DBG("Searching for skb with txseq %d (queue len %d)",
4500                       chan->buffer_seq, skb_queue_len(&chan->srej_q));
4501
4502                skb = l2cap_ertm_seq_in_queue(&chan->srej_q, chan->buffer_seq);
4503
4504                if (!skb)
4505                        break;
4506
4507                skb_unlink(skb, &chan->srej_q);
4508                chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
4509                err = l2cap_reassemble_sdu(chan, skb, &bt_cb(skb)->control);
4510                if (err)
4511                        break;
4512        }
4513
4514        if (skb_queue_empty(&chan->srej_q)) {
4515                chan->rx_state = L2CAP_RX_STATE_RECV;
4516                l2cap_send_ack(chan);
4517        }
4518
4519        return err;
4520}
4521
4522static void l2cap_handle_srej(struct l2cap_chan *chan,
4523                              struct l2cap_ctrl *control)
4524{
4525        struct sk_buff *skb;
4526
4527        BT_DBG("chan %p, control %p", chan, control);
4528
4529        if (control->reqseq == chan->next_tx_seq) {
4530                BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq);
4531                l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
4532                return;
4533        }
4534
4535        skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq);
4536
4537        if (skb == NULL) {
4538                BT_DBG("Seq %d not available for retransmission",
4539                       control->reqseq);
4540                return;
4541        }
4542
4543        if (chan->max_tx != 0 && bt_cb(skb)->control.retries >= chan->max_tx) {
4544                BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
4545                l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
4546                return;
4547        }
4548
4549        clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
4550
4551        if (control->poll) {
4552                l2cap_pass_to_tx(chan, control);
4553
4554                set_bit(CONN_SEND_FBIT, &chan->conn_state);
4555                l2cap_retransmit(chan, control);
4556                l2cap_ertm_send(chan);
4557
4558                if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) {
4559                        set_bit(CONN_SREJ_ACT, &chan->conn_state);
4560                        chan->srej_save_reqseq = control->reqseq;
4561                }
4562        } else {
4563                l2cap_pass_to_tx_fbit(chan, control);
4564
4565                if (control->final) {
4566                        if (chan->srej_save_reqseq != control->reqseq ||
4567                            !test_and_clear_bit(CONN_SREJ_ACT,
4568                                                &chan->conn_state))
4569                                l2cap_retransmit(chan, control);
4570                } else {
4571                        l2cap_retransmit(chan, control);
4572                        if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) {
4573                                set_bit(CONN_SREJ_ACT, &chan->conn_state);
4574                                chan->srej_save_reqseq = control->reqseq;
4575                        }
4576                }
4577        }
4578}
4579
4580static void l2cap_handle_rej(struct l2cap_chan *chan,
4581                             struct l2cap_ctrl *control)
4582{
4583        struct sk_buff *skb;
4584
4585        BT_DBG("chan %p, control %p", chan, control);
4586
4587        if (control->reqseq == chan->next_tx_seq) {
4588                BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq);
4589                l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
4590                return;
4591        }
4592
4593        skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq);
4594
4595        if (chan->max_tx && skb &&
4596            bt_cb(skb)->control.retries >= chan->max_tx) {
4597                BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
4598                l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
4599                return;
4600        }
4601
4602        clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
4603
4604        l2cap_pass_to_tx(chan, control);
4605
4606        if (control->final) {
4607                if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
4608                        l2cap_retransmit_all(chan, control);
4609        } else {
4610                l2cap_retransmit_all(chan, control);
4611                l2cap_ertm_send(chan);
4612                if (chan->tx_state == L2CAP_TX_STATE_WAIT_F)
4613                        set_bit(CONN_REJ_ACT, &chan->conn_state);
4614        }
4615}
4616
4617static u8 l2cap_classify_txseq(struct l2cap_chan *chan, u16 txseq)
4618{
4619        BT_DBG("chan %p, txseq %d", chan, txseq);
4620
4621        BT_DBG("last_acked_seq %d, expected_tx_seq %d", chan->last_acked_seq,
4622               chan->expected_tx_seq);
4623
4624        if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
4625                if (__seq_offset(chan, txseq, chan->last_acked_seq) >=
4626                                                                chan->tx_win) {
4627                        /* See notes below regarding "double poll" and
4628                         * invalid packets.
4629                         */
4630                        if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) {
4631                                BT_DBG("Invalid/Ignore - after SREJ");
4632                                return L2CAP_TXSEQ_INVALID_IGNORE;
4633                        } else {
4634                                BT_DBG("Invalid - in window after SREJ sent");
4635                                return L2CAP_TXSEQ_INVALID;
4636                        }
4637                }
4638
4639                if (chan->srej_list.head == txseq) {
4640                        BT_DBG("Expected SREJ");
4641                        return L2CAP_TXSEQ_EXPECTED_SREJ;
4642                }
4643
4644                if (l2cap_ertm_seq_in_queue(&chan->srej_q, txseq)) {
4645                        BT_DBG("Duplicate SREJ - txseq already stored");
4646                        return L2CAP_TXSEQ_DUPLICATE_SREJ;
4647                }
4648
4649                if (l2cap_seq_list_contains(&chan->srej_list, txseq)) {
4650                        BT_DBG("Unexpected SREJ - not requested");
4651                        return L2CAP_TXSEQ_UNEXPECTED_SREJ;
4652                }
4653        }
4654
4655        if (chan->expected_tx_seq == txseq) {
4656                if (__seq_offset(chan, txseq, chan->last_acked_seq) >=
4657                    chan->tx_win) {
4658                        BT_DBG("Invalid - txseq outside tx window");
4659                        return L2CAP_TXSEQ_INVALID;
4660                } else {
4661                        BT_DBG("Expected");
4662                        return L2CAP_TXSEQ_EXPECTED;
4663                }
4664        }
4665
4666        if (__seq_offset(chan, txseq, chan->last_acked_seq) <
4667                __seq_offset(chan, chan->expected_tx_seq,
4668                             chan->last_acked_seq)){
4669                BT_DBG("Duplicate - expected_tx_seq later than txseq");
4670                return L2CAP_TXSEQ_DUPLICATE;
4671        }
4672
4673        if (__seq_offset(chan, txseq, chan->last_acked_seq) >= chan->tx_win) {
4674                /* A source of invalid packets is a "double poll" condition,
4675                 * where delays cause us to send multiple poll packets.  If
4676                 * the remote stack receives and processes both polls,
4677                 * sequence numbers can wrap around in such a way that a
4678                 * resent frame has a sequence number that looks like new data
4679                 * with a sequence gap.  This would trigger an erroneous SREJ
4680                 * request.
4681                 *
4682                 * Fortunately, this is impossible with a tx window that's
4683                 * less than half of the maximum sequence number, which allows
4684                 * invalid frames to be safely ignored.
4685                 *
4686                 * With tx window sizes greater than half of the tx window
4687                 * maximum, the frame is invalid and cannot be ignored.  This
4688                 * causes a disconnect.
4689                 */
4690
4691                if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) {
4692                        BT_DBG("Invalid/Ignore - txseq outside tx window");
4693                        return L2CAP_TXSEQ_INVALID_IGNORE;
4694                } else {
4695                        BT_DBG("Invalid - txseq outside tx window");
4696                        return L2CAP_TXSEQ_INVALID;
4697                }
4698        } else {
4699                BT_DBG("Unexpected - txseq indicates missing frames");
4700                return L2CAP_TXSEQ_UNEXPECTED;
4701        }
4702}
4703
4704static int l2cap_rx_state_recv(struct l2cap_chan *chan,
4705                               struct l2cap_ctrl *control,
4706                               struct sk_buff *skb, u8 event)
4707{
4708        int err = 0;
4709        bool skb_in_use = 0;
4710
4711        BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
4712               event);
4713
4714        switch (event) {
4715        case L2CAP_EV_RECV_IFRAME:
4716                switch (l2cap_classify_txseq(chan, control->txseq)) {
4717                case L2CAP_TXSEQ_EXPECTED:
4718                        l2cap_pass_to_tx(chan, control);
4719
4720                        if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
4721                                BT_DBG("Busy, discarding expected seq %d",
4722                                       control->txseq);
4723                                break;
4724                        }
4725
4726                        chan->expected_tx_seq = __next_seq(chan,
4727                                                           control->txseq);
4728
4729                        chan->buffer_seq = chan->expected_tx_seq;
4730                        skb_in_use = 1;
4731
4732                        err = l2cap_reassemble_sdu(chan, skb, control);
4733                        if (err)
4734                                break;
4735
4736                        if (control->final) {
4737                                if (!test_and_clear_bit(CONN_REJ_ACT,
4738                                                        &chan->conn_state)) {
4739                                        control->final = 0;
4740                                        l2cap_retransmit_all(chan, control);
4741                                        l2cap_ertm_send(chan);
4742                                }
4743                        }
4744
4745                        if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state))
4746                                l2cap_send_ack(chan);
4747                        break;
4748                case L2CAP_TXSEQ_UNEXPECTED:
4749                        l2cap_pass_to_tx(chan, control);
4750
4751                        /* Can't issue SREJ frames in the local busy state.
4752                         * Drop this frame, it will be seen as missing
4753                         * when local busy is exited.
4754                         */
4755                        if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
4756                                BT_DBG("Busy, discarding unexpected seq %d",
4757                                       control->txseq);
4758                                break;
4759                        }
4760
4761                        /* There was a gap in the sequence, so an SREJ
4762                         * must be sent for each missing frame.  The
4763                         * current frame is stored for later use.
4764                         */
4765                        skb_queue_tail(&chan->srej_q, skb);
4766                        skb_in_use = 1;
4767                        BT_DBG("Queued %p (queue len %d)", skb,
4768                               skb_queue_len(&chan->srej_q));
4769
4770                        clear_bit(CONN_SREJ_ACT, &chan->conn_state);
4771                        l2cap_seq_list_clear(&chan->srej_list);
4772                        l2cap_send_srej(chan, control->txseq);
4773
4774                        chan->rx_state = L2CAP_RX_STATE_SREJ_SENT;
4775                        break;
4776                case L2CAP_TXSEQ_DUPLICATE:
4777                        l2cap_pass_to_tx(chan, control);
4778                        break;
4779                case L2CAP_TXSEQ_INVALID_IGNORE:
4780                        break;
4781                case L2CAP_TXSEQ_INVALID:
4782                default:
4783                        l2cap_send_disconn_req(chan->conn, chan,
4784                                               ECONNRESET);
4785                        break;
4786                }
4787                break;
4788        case L2CAP_EV_RECV_RR:
4789                l2cap_pass_to_tx(chan, control);
4790                if (control->final) {
4791                        clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
4792
4793                        if (!test_and_clear_bit(CONN_REJ_ACT,
4794                                                &chan->conn_state)) {
4795                                control->final = 0;
4796                                l2cap_retransmit_all(chan, control);
4797                        }
4798
4799                        l2cap_ertm_send(chan);
4800                } else if (control->poll) {
4801                        l2cap_send_i_or_rr_or_rnr(chan);
4802                } else {
4803                        if (test_and_clear_bit(CONN_REMOTE_BUSY,
4804                                               &chan->conn_state) &&
4805                            chan->unacked_frames)
4806                                __set_retrans_timer(chan);
4807
4808                        l2cap_ertm_send(chan);
4809                }
4810                break;
4811        case L2CAP_EV_RECV_RNR:
4812                set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
4813                l2cap_pass_to_tx(chan, control);
4814                if (control && control->poll) {
4815                        set_bit(CONN_SEND_FBIT, &chan->conn_state);
4816                        l2cap_send_rr_or_rnr(chan, 0);
4817                }
4818                __clear_retrans_timer(chan);
4819                l2cap_seq_list_clear(&chan->retrans_list);
4820                break;
4821        case L2CAP_EV_RECV_REJ:
4822                l2cap_handle_rej(chan, control);
4823                break;
4824        case L2CAP_EV_RECV_SREJ:
4825                l2cap_handle_srej(chan, control);
4826                break;
4827        default:
4828                break;
4829        }
4830
4831        if (skb && !skb_in_use) {
4832                BT_DBG("Freeing %p", skb);
4833                kfree_skb(skb);
4834        }
4835
4836        return err;
4837}
4838
4839static int l2cap_rx_state_srej_sent(struct l2cap_chan *chan,
4840                                    struct l2cap_ctrl *control,
4841                                    struct sk_buff *skb, u8 event)
4842{
4843        int err = 0;
4844        u16 txseq = control->txseq;
4845        bool skb_in_use = 0;
4846
4847        BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
4848               event);
4849
4850        switch (event) {
4851        case L2CAP_EV_RECV_IFRAME:
4852                switch (l2cap_classify_txseq(chan, txseq)) {
4853                case L2CAP_TXSEQ_EXPECTED:
4854                        /* Keep frame for reassembly later */
4855                        l2cap_pass_to_tx(chan, control);
4856                        skb_queue_tail(&chan->srej_q, skb);
4857                        skb_in_use = 1;
4858                        BT_DBG("Queued %p (queue len %d)", skb,
4859                               skb_queue_len(&chan->srej_q));
4860
4861                        chan->expected_tx_seq = __next_seq(chan, txseq);
4862                        break;
4863                case L2CAP_TXSEQ_EXPECTED_SREJ:
4864                        l2cap_seq_list_pop(&chan->srej_list);
4865
4866                        l2cap_pass_to_tx(chan, control);
4867                        skb_queue_tail(&chan->srej_q, skb);
4868                        skb_in_use = 1;
4869                        BT_DBG("Queued %p (queue len %d)", skb,
4870                               skb_queue_len(&chan->srej_q));
4871
4872                        err = l2cap_rx_queued_iframes(chan);
4873                        if (err)
4874                                break;
4875
4876                        break;
4877                case L2CAP_TXSEQ_UNEXPECTED:
4878                        /* Got a frame that can't be reassembled yet.
4879                         * Save it for later, and send SREJs to cover
4880                         * the missing frames.
4881                         */
4882                        skb_queue_tail(&chan->srej_q, skb);
4883                        skb_in_use = 1;
4884                        BT_DBG("Queued %p (queue len %d)", skb,
4885                               skb_queue_len(&chan->srej_q));
4886
4887                        l2cap_pass_to_tx(chan, control);
4888                        l2cap_send_srej(chan, control->txseq);
4889                        break;
4890                case L2CAP_TXSEQ_UNEXPECTED_SREJ:
4891                        /* This frame was requested with an SREJ, but
4892                         * some expected retransmitted frames are
4893                         * missing.  Request retransmission of missing
4894                         * SREJ'd frames.
4895                         */
4896                        skb_queue_tail(&chan->srej_q, skb);
4897                        skb_in_use = 1;
4898                        BT_DBG("Queued %p (queue len %d)", skb,
4899                               skb_queue_len(&chan->srej_q));
4900
4901                        l2cap_pass_to_tx(chan, control);
4902                        l2cap_send_srej_list(chan, control->txseq);
4903                        break;
4904                case L2CAP_TXSEQ_DUPLICATE_SREJ:
4905                        /* We've already queued this frame.  Drop this copy. */
4906                        l2cap_pass_to_tx(chan, control);
4907                        break;
4908                case L2CAP_TXSEQ_DUPLICATE:
4909                        /* Expecting a later sequence number, so this frame
4910                         * was already received.  Ignore it completely.
4911                         */
4912                        break;
4913                case L2CAP_TXSEQ_INVALID_IGNORE:
4914                        break;
4915                case L2CAP_TXSEQ_INVALID:
4916                default:
4917                        l2cap_send_disconn_req(chan->conn, chan,
4918                                               ECONNRESET);
4919                        break;
4920                }
4921                break;
4922        case L2CAP_EV_RECV_RR:
4923                l2cap_pass_to_tx(chan, control);
4924                if (control->final) {
4925                        clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
4926
4927                        if (!test_and_clear_bit(CONN_REJ_ACT,
4928                                                &chan->conn_state)) {
4929                                control->final = 0;
4930                                l2cap_retransmit_all(chan, control);
4931                        }
4932
4933                        l2cap_ertm_send(chan);
4934                } else if (control->poll) {
4935                        if (test_and_clear_bit(CONN_REMOTE_BUSY,
4936                                               &chan->conn_state) &&
4937                            chan->unacked_frames) {
4938                                __set_retrans_timer(chan);
4939                        }
4940
4941                        set_bit(CONN_SEND_FBIT, &chan->conn_state);
4942                        l2cap_send_srej_tail(chan);
4943                } else {
4944                        if (test_and_clear_bit(CONN_REMOTE_BUSY,
4945                                               &chan->conn_state) &&
4946                            chan->unacked_frames)
4947                                __set_retrans_timer(chan);
4948
4949                        l2cap_send_ack(chan);
4950                }
4951                break;
4952        case L2CAP_EV_RECV_RNR:
4953                set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
4954                l2cap_pass_to_tx(chan, control);
4955                if (control->poll) {
4956                        l2cap_send_srej_tail(chan);
4957                } else {
4958                        struct l2cap_ctrl rr_control;
4959                        memset(&rr_control, 0, sizeof(rr_control));
4960                        rr_control.sframe = 1;
4961                        rr_control.super = L2CAP_SUPER_RR;
4962                        rr_control.reqseq = chan->buffer_seq;
4963                        l2cap_send_sframe(chan, &rr_control);
4964                }
4965
4966                break;
4967        case L2CAP_EV_RECV_REJ:
4968                l2cap_handle_rej(chan, control);
4969                break;
4970        case L2CAP_EV_RECV_SREJ:
4971                l2cap_handle_srej(chan, control);
4972                break;
4973        }
4974
4975        if (skb && !skb_in_use) {
4976                BT_DBG("Freeing %p", skb);
4977                kfree_skb(skb);
4978        }
4979
4980        return err;
4981}
4982
4983static bool __valid_reqseq(struct l2cap_chan *chan, u16 reqseq)
4984{
4985        /* Make sure reqseq is for a packet that has been sent but not acked */
4986        u16 unacked;
4987
4988        unacked = __seq_offset(chan, chan->next_tx_seq, chan->expected_ack_seq);
4989        return __seq_offset(chan, chan->next_tx_seq, reqseq) <= unacked;
4990}
4991
4992static int l2cap_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
4993                    struct sk_buff *skb, u8 event)
4994{
4995        int err = 0;
4996
4997        BT_DBG("chan %p, control %p, skb %p, event %d, state %d", chan,
4998               control, skb, event, chan->rx_state);
4999
5000        if (__valid_reqseq(chan, control->reqseq)) {
5001                switch (chan->rx_state) {
5002                case L2CAP_RX_STATE_RECV:
5003                        err = l2cap_rx_state_recv(chan, control, skb, event);
5004                        break;
5005                case L2CAP_RX_STATE_SREJ_SENT:
5006                        err = l2cap_rx_state_srej_sent(chan, control, skb,
5007                                                       event);
5008                        break;
5009                default:
5010                        /* shut it down */
5011                        break;
5012                }
5013        } else {
5014                BT_DBG("Invalid reqseq %d (next_tx_seq %d, expected_ack_seq %d",
5015                       control->reqseq, chan->next_tx_seq,
5016                       chan->expected_ack_seq);
5017                l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
5018        }
5019
5020        return err;
5021}
5022
5023static int l2cap_stream_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
5024                           struct sk_buff *skb)
5025{
5026        int err = 0;
5027
5028        BT_DBG("chan %p, control %p, skb %p, state %d", chan, control, skb,
5029               chan->rx_state);
5030
5031        if (l2cap_classify_txseq(chan, control->txseq) ==
5032            L2CAP_TXSEQ_EXPECTED) {
5033                l2cap_pass_to_tx(chan, control);
5034
5035                BT_DBG("buffer_seq %d->%d", chan->buffer_seq,
5036                       __next_seq(chan, chan->buffer_seq));
5037
5038                chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
5039
5040                l2cap_reassemble_sdu(chan, skb, control);
5041        } else {
5042                if (