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