net/bluetooth/l2cap_core.c

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