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