net/bluetooth/l2cap_core.c

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