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