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                 if (type != L2CAP_CONF_RFC)
2919                         continue;
2920
2921                 if (olen != sizeof(rfc))
2922                         break;
2923
2924                 memcpy(&rfc, (void *)val, olen);
2925                 goto done;
2926         }
2927
2928         /* Use sane default values in case a misbehaving remote device
2929          * did not send an RFC option.
2930          */
2931         rfc.mode = chan->mode;
2932         rfc.retrans_timeout = cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO);
2933         rfc.monitor_timeout = cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO);
2934         rfc.max_pdu_size = cpu_to_le16(chan->imtu);
2935
2936         BT_ERR("Expected RFC option was not found, using defaults");
2937
2938 done:
2939         switch (rfc.mode) {
2940         case L2CAP_MODE_ERTM:
2941                 chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
2942                 chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
2943                 chan->mps    = le16_to_cpu(rfc.max_pdu_size);
2944                 break;
2945         case L2CAP_MODE_STREAMING:
2946                 chan->mps    = le16_to_cpu(rfc.max_pdu_size);
2947         }
2948 }
2949
2950 static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2951 {
2952         struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data;
2953
2954         if (rej->reason != L2CAP_REJ_NOT_UNDERSTOOD)
2955                 return 0;
2956
2957         if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
2958                                         cmd->ident == conn->info_ident) {
2959                 cancel_delayed_work(&conn->info_timer);
2960
2961                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2962                 conn->info_ident = 0;
2963
2964                 l2cap_conn_start(conn);
2965         }
2966
2967         return 0;
2968 }
2969
2970 static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2971 {
2972         struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
2973         struct l2cap_conn_rsp rsp;
2974         struct l2cap_chan *chan = NULL, *pchan;
2975         struct sock *parent, *sk = NULL;
2976         int result, status = L2CAP_CS_NO_INFO;
2977
2978         u16 dcid = 0, scid = __le16_to_cpu(req->scid);
2979         __le16 psm = req->psm;
2980
2981         BT_DBG("psm 0x%2.2x scid 0x%4.4x", __le16_to_cpu(psm), scid);
2982
2983         /* Check if we have socket listening on psm */
2984         pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, conn->src, conn->dst);
2985         if (!pchan) {
2986                 result = L2CAP_CR_BAD_PSM;
2987                 goto sendresp;
2988         }
2989
2990         parent = pchan->sk;
2991
2992         mutex_lock(&conn->chan_lock);
2993         lock_sock(parent);
2994
2995         /* Check if the ACL is secure enough (if not SDP) */
2996         if (psm != cpu_to_le16(0x0001) &&
2997                                 !hci_conn_check_link_mode(conn->hcon)) {
2998                 conn->disc_reason = HCI_ERROR_AUTH_FAILURE;
2999                 result = L2CAP_CR_SEC_BLOCK;
3000                 goto response;
3001         }
3002
3003         result = L2CAP_CR_NO_MEM;
3004
3005         /* Check for backlog size */
3006         if (sk_acceptq_is_full(parent)) {
3007                 BT_DBG("backlog full %d", parent->sk_ack_backlog);
3008                 goto response;
3009         }
3010
3011         chan = pchan->ops->new_connection(pchan->data);
3012         if (!chan)
3013                 goto response;
3014
3015         sk = chan->sk;
3016
3017         /* Check if we already have channel with that dcid */
3018         if (__l2cap_get_chan_by_dcid(conn, scid)) {
3019                 sock_set_flag(sk, SOCK_ZAPPED);
3020                 chan->ops->close(chan->data);
3021                 goto response;
3022         }
3023
3024         hci_conn_hold(conn->hcon);
3025
3026         bacpy(&bt_sk(sk)->src, conn->src);
3027         bacpy(&bt_sk(sk)->dst, conn->dst);
3028         chan->psm  = psm;
3029         chan->dcid = scid;
3030
3031         bt_accept_enqueue(parent, sk);
3032
3033         __l2cap_chan_add(conn, chan);
3034
3035         dcid = chan->scid;
3036
3037         __set_chan_timer(chan, sk->sk_sndtimeo);
3038
3039         chan->ident = cmd->ident;
3040
3041         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
3042                 if (l2cap_chan_check_security(chan)) {
3043                         if (test_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags)) {
3044                                 __l2cap_state_change(chan, BT_CONNECT2);
3045                                 result = L2CAP_CR_PEND;
3046                                 status = L2CAP_CS_AUTHOR_PEND;
3047                                 parent->sk_data_ready(parent, 0);
3048                         } else {
3049                                 __l2cap_state_change(chan, BT_CONFIG);
3050                                 result = L2CAP_CR_SUCCESS;
3051                                 status = L2CAP_CS_NO_INFO;
3052                         }
3053                 } else {
3054                         __l2cap_state_change(chan, BT_CONNECT2);
3055                         result = L2CAP_CR_PEND;
3056                         status = L2CAP_CS_AUTHEN_PEND;
3057                 }
3058         } else {
3059                 __l2cap_state_change(chan, BT_CONNECT2);
3060                 result = L2CAP_CR_PEND;
3061                 status = L2CAP_CS_NO_INFO;
3062         }
3063
3064 response:
3065         release_sock(parent);
3066         mutex_unlock(&conn->chan_lock);
3067
3068 sendresp:
3069         rsp.scid   = cpu_to_le16(scid);
3070         rsp.dcid   = cpu_to_le16(dcid);
3071         rsp.result = cpu_to_le16(result);
3072         rsp.status = cpu_to_le16(status);
3073         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
3074
3075         if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
3076                 struct l2cap_info_req info;
3077                 info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
3078
3079                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
3080                 conn->info_ident = l2cap_get_ident(conn);
3081
3082                 schedule_delayed_work(&conn->info_timer, L2CAP_INFO_TIMEOUT);
3083
3084                 l2cap_send_cmd(conn, conn->info_ident,
3085                                         L2CAP_INFO_REQ, sizeof(info), &info);
3086         }
3087
3088         if (chan && !test_bit(CONF_REQ_SENT, &chan->conf_state) &&
3089                                 result == L2CAP_CR_SUCCESS) {
3090                 u8 buf[128];
3091                 set_bit(CONF_REQ_SENT, &chan->conf_state);
3092                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3093                                         l2cap_build_conf_req(chan, buf), buf);
3094                 chan->num_conf_req++;
3095         }
3096
3097         return 0;
3098 }
3099
3100 static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3101 {
3102         struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
3103         u16 scid, dcid, result, status;
3104         struct l2cap_chan *chan;
3105         u8 req[128];
3106         int err;
3107
3108         scid   = __le16_to_cpu(rsp->scid);
3109         dcid   = __le16_to_cpu(rsp->dcid);
3110         result = __le16_to_cpu(rsp->result);
3111         status = __le16_to_cpu(rsp->status);
3112
3113         BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x",
3114                                                 dcid, scid, result, status);
3115
3116         mutex_lock(&conn->chan_lock);
3117
3118         if (scid) {
3119                 chan = __l2cap_get_chan_by_scid(conn, scid);
3120                 if (!chan) {
3121                         err = -EFAULT;
3122                         goto unlock;
3123                 }
3124         } else {
3125                 chan = __l2cap_get_chan_by_ident(conn, cmd->ident);
3126                 if (!chan) {
3127                         err = -EFAULT;
3128                         goto unlock;
3129                 }
3130         }
3131
3132         err = 0;
3133
3134         l2cap_chan_lock(chan);
3135
3136         switch (result) {
3137         case L2CAP_CR_SUCCESS:
3138                 l2cap_state_change(chan, BT_CONFIG);
3139                 chan->ident = 0;
3140                 chan->dcid = dcid;
3141                 clear_bit(CONF_CONNECT_PEND, &chan->conf_state);
3142
3143                 if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
3144                         break;
3145
3146                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3147                                         l2cap_build_conf_req(chan, req), req);
3148                 chan->num_conf_req++;
3149                 break;
3150
3151         case L2CAP_CR_PEND:
3152                 set_bit(CONF_CONNECT_PEND, &chan->conf_state);
3153                 break;
3154
3155         default:
3156                 l2cap_chan_del(chan, ECONNREFUSED);
3157                 break;
3158         }
3159
3160         l2cap_chan_unlock(chan);
3161
3162 unlock:
3163         mutex_unlock(&conn->chan_lock);
3164
3165         return err;
3166 }
3167
3168 static inline void set_default_fcs(struct l2cap_chan *chan)
3169 {
3170         /* FCS is enabled only in ERTM or streaming mode, if one or both
3171          * sides request it.
3172          */
3173         if (chan->mode != L2CAP_MODE_ERTM && chan->mode != L2CAP_MODE_STREAMING)
3174                 chan->fcs = L2CAP_FCS_NONE;
3175         else if (!test_bit(CONF_NO_FCS_RECV, &chan->conf_state))
3176                 chan->fcs = L2CAP_FCS_CRC16;
3177 }
3178
3179 static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
3180 {
3181         struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
3182         u16 dcid, flags;
3183         u8 rsp[64];
3184         struct l2cap_chan *chan;
3185         int len, err = 0;
3186
3187         dcid  = __le16_to_cpu(req->dcid);
3188         flags = __le16_to_cpu(req->flags);
3189
3190         BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
3191
3192         chan = l2cap_get_chan_by_scid(conn, dcid);
3193         if (!chan)
3194                 return -ENOENT;
3195
3196         if (chan->state != BT_CONFIG && chan->state != BT_CONNECT2) {
3197                 struct l2cap_cmd_rej_cid rej;
3198
3199                 rej.reason = cpu_to_le16(L2CAP_REJ_INVALID_CID);
3200                 rej.scid = cpu_to_le16(chan->scid);
3201                 rej.dcid = cpu_to_le16(chan->dcid);
3202
3203                 l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
3204                                 sizeof(rej), &rej);
3205                 goto unlock;
3206         }
3207
3208         /* Reject if config buffer is too small. */
3209         len = cmd_len - sizeof(*req);
3210         if (len < 0 || chan->conf_len + len > sizeof(chan->conf_req)) {
3211                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
3212                                 l2cap_build_conf_rsp(chan, rsp,
3213                                         L2CAP_CONF_REJECT, flags), rsp);
3214                 goto unlock;
3215         }
3216
3217         /* Store config. */
3218         memcpy(chan->conf_req + chan->conf_len, req->data, len);
3219         chan->conf_len += len;
3220
3221         if (flags & 0x0001) {
3222                 /* Incomplete config. Send empty response. */
3223                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
3224                                 l2cap_build_conf_rsp(chan, rsp,
3225                                         L2CAP_CONF_SUCCESS, 0x0001), rsp);
3226                 goto unlock;
3227         }
3228
3229         /* Complete config. */
3230         len = l2cap_parse_conf_req(chan, rsp);
3231         if (len < 0) {
3232                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
3233                 goto unlock;
3234         }
3235
3236         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
3237         chan->num_conf_rsp++;
3238
3239         /* Reset config buffer. */
3240         chan->conf_len = 0;
3241
3242         if (!test_bit(CONF_OUTPUT_DONE, &chan->conf_state))
3243                 goto unlock;
3244
3245         if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
3246                 set_default_fcs(chan);
3247
3248                 l2cap_state_change(chan, BT_CONNECTED);
3249
3250                 if (chan->mode == L2CAP_MODE_ERTM ||
3251                     chan->mode == L2CAP_MODE_STREAMING)
3252                         err = l2cap_ertm_init(chan);
3253
3254                 if (err < 0)
3255                         l2cap_send_disconn_req(chan->conn, chan, -err);
3256                 else
3257                         l2cap_chan_ready(chan);
3258
3259                 goto unlock;
3260         }
3261
3262         if (!test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) {
3263                 u8 buf[64];
3264                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3265                                         l2cap_build_conf_req(chan, buf), buf);
3266                 chan->num_conf_req++;
3267         }
3268
3269         /* Got Conf Rsp PENDING from remote side and asume we sent
3270            Conf Rsp PENDING in the code above */
3271         if (test_bit(CONF_REM_CONF_PEND, &chan->conf_state) &&
3272                         test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
3273
3274                 /* check compatibility */
3275
3276                 clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
3277                 set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
3278
3279                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
3280                                         l2cap_build_conf_rsp(chan, rsp,
3281                                         L2CAP_CONF_SUCCESS, 0x0000), rsp);
3282         }
3283
3284 unlock:
3285         l2cap_chan_unlock(chan);
3286         return err;
3287 }
3288
3289 static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3290 {
3291         struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
3292         u16 scid, flags, result;
3293         struct l2cap_chan *chan;
3294         int len = le16_to_cpu(cmd->len) - sizeof(*rsp);
3295         int err = 0;
3296
3297         scid   = __le16_to_cpu(rsp->scid);
3298         flags  = __le16_to_cpu(rsp->flags);
3299         result = __le16_to_cpu(rsp->result);
3300
3301         BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x len %d", scid, flags,
3302                result, len);
3303
3304         chan = l2cap_get_chan_by_scid(conn, scid);
3305         if (!chan)
3306                 return 0;
3307
3308         switch (result) {
3309         case L2CAP_CONF_SUCCESS:
3310                 l2cap_conf_rfc_get(chan, rsp->data, len);
3311                 clear_bit(CONF_REM_CONF_PEND, &chan->conf_state);
3312                 break;
3313
3314         case L2CAP_CONF_PENDING:
3315                 set_bit(CONF_REM_CONF_PEND, &chan->conf_state);
3316
3317                 if (test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
3318                         char buf[64];
3319
3320                         len = l2cap_parse_conf_rsp(chan, rsp->data, len,
3321                                                                 buf, &result);
3322                         if (len < 0) {
3323                                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
3324                                 goto done;
3325                         }
3326
3327                         /* check compatibility */
3328
3329                         clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
3330                         set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
3331
3332                         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
3333                                                 l2cap_build_conf_rsp(chan, buf,
3334                                                 L2CAP_CONF_SUCCESS, 0x0000), buf);
3335                 }
3336                 goto done;
3337
3338         case L2CAP_CONF_UNACCEPT:
3339                 if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
3340                         char req[64];
3341
3342                         if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
3343                                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
3344                                 goto done;
3345                         }
3346
3347                         /* throw out any old stored conf requests */
3348                         result = L2CAP_CONF_SUCCESS;
3349                         len = l2cap_parse_conf_rsp(chan, rsp->data, len,
3350                                                                 req, &result);
3351                         if (len < 0) {
3352                                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
3353                                 goto done;
3354                         }
3355
3356                         l2cap_send_cmd(conn, l2cap_get_ident(conn),
3357                                                 L2CAP_CONF_REQ, len, req);
3358                         chan->num_conf_req++;
3359                         if (result != L2CAP_CONF_SUCCESS)
3360                                 goto done;
3361                         break;
3362                 }
3363
3364         default:
3365                 l2cap_chan_set_err(chan, ECONNRESET);
3366
3367                 __set_chan_timer(chan, L2CAP_DISC_REJ_TIMEOUT);
3368                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
3369                 goto done;
3370         }
3371
3372         if (flags & 0x01)
3373                 goto done;
3374
3375         set_bit(CONF_INPUT_DONE, &chan->conf_state);
3376
3377         if (test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) {
3378                 set_default_fcs(chan);
3379
3380                 l2cap_state_change(chan, BT_CONNECTED);
3381                 if (chan->mode == L2CAP_MODE_ERTM ||
3382                     chan->mode == L2CAP_MODE_STREAMING)
3383                         err = l2cap_ertm_init(chan);
3384
3385                 if (err < 0)
3386                         l2cap_send_disconn_req(chan->conn, chan, -err);
3387                 else
3388                         l2cap_chan_ready(chan);
3389         }
3390
3391 done:
3392         l2cap_chan_unlock(chan);
3393         return err;
3394 }
3395
3396 static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3397 {
3398         struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
3399         struct l2cap_disconn_rsp rsp;
3400         u16 dcid, scid;
3401         struct l2cap_chan *chan;
3402         struct sock *sk;
3403
3404         scid = __le16_to_cpu(req->scid);
3405         dcid = __le16_to_cpu(req->dcid);
3406
3407         BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
3408
3409         mutex_lock(&conn->chan_lock);
3410
3411         chan = __l2cap_get_chan_by_scid(conn, dcid);
3412         if (!chan) {
3413                 mutex_unlock(&conn->chan_lock);
3414                 return 0;
3415         }
3416
3417         l2cap_chan_lock(chan);
3418
3419         sk = chan->sk;
3420
3421         rsp.dcid = cpu_to_le16(chan->scid);
3422         rsp.scid = cpu_to_le16(chan->dcid);
3423         l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
3424
3425         lock_sock(sk);
3426         sk->sk_shutdown = SHUTDOWN_MASK;
3427         release_sock(sk);
3428
3429         l2cap_chan_hold(chan);
3430         l2cap_chan_del(chan, ECONNRESET);
3431
3432         l2cap_chan_unlock(chan);
3433
3434         chan->ops->close(chan->data);
3435         l2cap_chan_put(chan);
3436
3437         mutex_unlock(&conn->chan_lock);
3438
3439         return 0;
3440 }
3441
3442 static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3443 {
3444         struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
3445         u16 dcid, scid;
3446         struct l2cap_chan *chan;
3447
3448         scid = __le16_to_cpu(rsp->scid);
3449         dcid = __le16_to_cpu(rsp->dcid);
3450
3451         BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
3452
3453         mutex_lock(&conn->chan_lock);
3454
3455         chan = __l2cap_get_chan_by_scid(conn, scid);
3456         if (!chan) {
3457                 mutex_unlock(&conn->chan_lock);
3458                 return 0;
3459         }
3460
3461         l2cap_chan_lock(chan);
3462
3463         l2cap_chan_hold(chan);
3464         l2cap_chan_del(chan, 0);
3465
3466         l2cap_chan_unlock(chan);
3467
3468         chan->ops->close(chan->data);
3469         l2cap_chan_put(chan);
3470
3471         mutex_unlock(&conn->chan_lock);
3472
3473         return 0;
3474 }
3475
3476 static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3477 {
3478         struct l2cap_info_req *req = (struct l2cap_info_req *) data;
3479         u16 type;
3480
3481         type = __le16_to_cpu(req->type);
3482
3483         BT_DBG("type 0x%4.4x", type);
3484
3485         if (type == L2CAP_IT_FEAT_MASK) {
3486                 u8 buf[8];
3487                 u32 feat_mask = l2cap_feat_mask;
3488                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
3489                 rsp->type   = cpu_to_le16(L2CAP_IT_FEAT_MASK);
3490                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
3491                 if (!disable_ertm)
3492                         feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
3493                                                          | L2CAP_FEAT_FCS;
3494                 if (enable_hs)
3495                         feat_mask |= L2CAP_FEAT_EXT_FLOW
3496                                                 | L2CAP_FEAT_EXT_WINDOW;
3497
3498                 put_unaligned_le32(feat_mask, rsp->data);
3499                 l2cap_send_cmd(conn, cmd->ident,
3500                                         L2CAP_INFO_RSP, sizeof(buf), buf);
3501         } else if (type == L2CAP_IT_FIXED_CHAN) {
3502                 u8 buf[12];
3503                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
3504
3505                 if (enable_hs)
3506                         l2cap_fixed_chan[0] |= L2CAP_FC_A2MP;
3507                 else
3508                         l2cap_fixed_chan[0] &= ~L2CAP_FC_A2MP;
3509
3510                 rsp->type   = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
3511                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
3512                 memcpy(rsp->data, l2cap_fixed_chan, sizeof(l2cap_fixed_chan));
3513                 l2cap_send_cmd(conn, cmd->ident,
3514                                         L2CAP_INFO_RSP, sizeof(buf), buf);
3515         } else {
3516                 struct l2cap_info_rsp rsp;
3517                 rsp.type   = cpu_to_le16(type);
3518                 rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP);
3519                 l2cap_send_cmd(conn, cmd->ident,
3520                                         L2CAP_INFO_RSP, sizeof(rsp), &rsp);
3521         }
3522
3523         return 0;
3524 }
3525
3526 static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3527 {
3528         struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
3529         u16 type, result;
3530
3531         type   = __le16_to_cpu(rsp->type);
3532         result = __le16_to_cpu(rsp->result);
3533
3534         BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
3535
3536         /* L2CAP Info req/rsp are unbound to channels, add extra checks */
3537         if (cmd->ident != conn->info_ident ||
3538                         conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)
3539                 return 0;
3540
3541         cancel_delayed_work(&conn->info_timer);
3542
3543         if (result != L2CAP_IR_SUCCESS) {
3544                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3545                 conn->info_ident = 0;
3546
3547                 l2cap_conn_start(conn);
3548
3549                 return 0;
3550         }
3551
3552         switch (type) {
3553         case L2CAP_IT_FEAT_MASK:
3554                 conn->feat_mask = get_unaligned_le32(rsp->data);
3555
3556                 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
3557                         struct l2cap_info_req req;
3558                         req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
3559
3560                         conn->info_ident = l2cap_get_ident(conn);
3561
3562                         l2cap_send_cmd(conn, conn->info_ident,
3563                                         L2CAP_INFO_REQ, sizeof(req), &req);
3564                 } else {
3565                         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3566                         conn->info_ident = 0;
3567
3568                         l2cap_conn_start(conn);
3569                 }
3570                 break;
3571
3572         case L2CAP_IT_FIXED_CHAN:
3573                 conn->fixed_chan_mask = rsp->data[0];
3574                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3575                 conn->info_ident = 0;
3576
3577                 l2cap_conn_start(conn);
3578                 break;
3579         }
3580
3581         return 0;
3582 }
3583
3584 static inline int l2cap_create_channel_req(struct l2cap_conn *conn,
3585                                         struct l2cap_cmd_hdr *cmd, u16 cmd_len,
3586                                         void *data)
3587 {
3588         struct l2cap_create_chan_req *req = data;
3589         struct l2cap_create_chan_rsp rsp;
3590         u16 psm, scid;
3591
3592         if (cmd_len != sizeof(*req))
3593                 return -EPROTO;
3594
3595         if (!enable_hs)
3596                 return -EINVAL;
3597
3598         psm = le16_to_cpu(req->psm);
3599         scid = le16_to_cpu(req->scid);
3600
3601         BT_DBG("psm %d, scid %d, amp_id %d", psm, scid, req->amp_id);
3602
3603         /* Placeholder: Always reject */
3604         rsp.dcid = 0;
3605         rsp.scid = cpu_to_le16(scid);
3606         rsp.result = __constant_cpu_to_le16(L2CAP_CR_NO_MEM);
3607         rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
3608
3609         l2cap_send_cmd(conn, cmd->ident, L2CAP_CREATE_CHAN_RSP,
3610                        sizeof(rsp), &rsp);
3611
3612         return 0;
3613 }
3614
3615 static inline int l2cap_create_channel_rsp(struct l2cap_conn *conn,
3616                                         struct l2cap_cmd_hdr *cmd, void *data)
3617 {
3618         BT_DBG("conn %p", conn);
3619
3620         return l2cap_connect_rsp(conn, cmd, data);
3621 }
3622
3623 static void l2cap_send_move_chan_rsp(struct l2cap_conn *conn, u8 ident,
3624                                                         u16 icid, u16 result)
3625 {
3626         struct l2cap_move_chan_rsp rsp;
3627
3628         BT_DBG("icid %d, result %d", icid, result);
3629
3630         rsp.icid = cpu_to_le16(icid);
3631         rsp.result = cpu_to_le16(result);
3632
3633         l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_RSP, sizeof(rsp), &rsp);
3634 }
3635
3636 static void l2cap_send_move_chan_cfm(struct l2cap_conn *conn,
3637                                 struct l2cap_chan *chan, u16 icid, u16 result)
3638 {
3639         struct l2cap_move_chan_cfm cfm;
3640         u8 ident;
3641
3642         BT_DBG("icid %d, result %d", icid, result);
3643
3644         ident = l2cap_get_ident(conn);
3645         if (chan)
3646                 chan->ident = ident;
3647
3648         cfm.icid = cpu_to_le16(icid);
3649         cfm.result = cpu_to_le16(result);
3650
3651         l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM, sizeof(cfm), &cfm);
3652 }
3653
3654 static void l2cap_send_move_chan_cfm_rsp(struct l2cap_conn *conn, u8 ident,
3655                                                                 u16 icid)
3656 {
3657         struct l2cap_move_chan_cfm_rsp rsp;
3658
3659         BT_DBG("icid %d", icid);
3660
3661         rsp.icid = cpu_to_le16(icid);
3662         l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM_RSP, sizeof(rsp), &rsp);
3663 }
3664
3665 static inline int l2cap_move_channel_req(struct l2cap_conn *conn,
3666                         struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data)
3667 {
3668         struct l2cap_move_chan_req *req = data;
3669         u16 icid = 0;
3670         u16 result = L2CAP_MR_NOT_ALLOWED;
3671
3672         if (cmd_len != sizeof(*req))
3673                 return -EPROTO;
3674
3675         icid = le16_to_cpu(req->icid);
3676
3677         BT_DBG("icid %d, dest_amp_id %d", icid, req->dest_amp_id);
3678
3679         if (!enable_hs)
3680                 return -EINVAL;
3681
3682         /* Placeholder: Always refuse */
3683         l2cap_send_move_chan_rsp(conn, cmd->ident, icid, result);
3684
3685         return 0;
3686 }
3687
3688 static inline int l2cap_move_channel_rsp(struct l2cap_conn *conn,
3689                         struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data)
3690 {
3691         struct l2cap_move_chan_rsp *rsp = data;
3692         u16 icid, result;
3693
3694         if (cmd_len != sizeof(*rsp))
3695                 return -EPROTO;
3696
3697         icid = le16_to_cpu(rsp->icid);
3698         result = le16_to_cpu(rsp->result);
3699
3700         BT_DBG("icid %d, result %d", icid, result);
3701
3702         /* Placeholder: Always unconfirmed */
3703         l2cap_send_move_chan_cfm(conn, NULL, icid, L2CAP_MC_UNCONFIRMED);
3704
3705         return 0;
3706 }
3707
3708 static inline int l2cap_move_channel_confirm(struct l2cap_conn *conn,
3709                         struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data)
3710 {
3711         struct l2cap_move_chan_cfm *cfm = data;
3712         u16 icid, result;
3713
3714         if (cmd_len != sizeof(*cfm))
3715                 return -EPROTO;
3716
3717         icid = le16_to_cpu(cfm->icid);
3718         result = le16_to_cpu(cfm->result);
3719
3720         BT_DBG("icid %d, result %d", icid, result);
3721
3722         l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid);
3723
3724         return 0;
3725 }
3726
3727 static inline int l2cap_move_channel_confirm_rsp(struct l2cap_conn *conn,
3728                         struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data)
3729 {
3730         struct l2cap_move_chan_cfm_rsp *rsp = data;
3731         u16 icid;
3732
3733         if (cmd_len != sizeof(*rsp))
3734                 return -EPROTO;
3735
3736         icid = le16_to_cpu(rsp->icid);
3737
3738         BT_DBG("icid %d", icid);
3739
3740         return 0;
3741 }
3742
3743 static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency,
3744                                                         u16 to_multiplier)
3745 {
3746         u16 max_latency;
3747
3748         if (min > max || min < 6 || max > 3200)
3749                 return -EINVAL;
3750
3751         if (to_multiplier < 10 || to_multiplier > 3200)
3752                 return -EINVAL;
3753
3754         if (max >= to_multiplier * 8)
3755                 return -EINVAL;
3756
3757         max_latency = (to_multiplier * 8 / max) - 1;
3758         if (latency > 499 || latency > max_latency)
3759                 return -EINVAL;
3760
3761         return 0;
3762 }
3763
3764 static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
3765                                         struct l2cap_cmd_hdr *cmd, u8 *data)
3766 {
3767         struct hci_conn *hcon = conn->hcon;
3768         struct l2cap_conn_param_update_req *req;
3769         struct l2cap_conn_param_update_rsp rsp;
3770         u16 min, max, latency, to_multiplier, cmd_len;
3771         int err;
3772
3773         if (!(hcon->link_mode & HCI_LM_MASTER))
3774                 return -EINVAL;
3775
3776         cmd_len = __le16_to_cpu(cmd->len);
3777         if (cmd_len != sizeof(struct l2cap_conn_param_update_req))
3778                 return -EPROTO;
3779
3780         req = (struct l2cap_conn_param_update_req *) data;
3781         min             = __le16_to_cpu(req->min);
3782         max             = __le16_to_cpu(req->max);
3783         latency         = __le16_to_cpu(req->latency);
3784         to_multiplier   = __le16_to_cpu(req->to_multiplier);
3785
3786         BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x",
3787                                                 min, max, latency, to_multiplier);
3788
3789         memset(&rsp, 0, sizeof(rsp));
3790
3791         err = l2cap_check_conn_param(min, max, latency, to_multiplier);
3792         if (err)
3793                 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_REJECTED);
3794         else
3795                 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED);
3796
3797         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP,
3798                                                         sizeof(rsp), &rsp);
3799
3800         if (!err)
3801                 hci_le_conn_update(hcon, min, max, latency, to_multiplier);
3802
3803         return 0;
3804 }
3805
3806 static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
3807                         struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
3808 {
3809         int err = 0;
3810
3811         switch (cmd->code) {
3812         case L2CAP_COMMAND_REJ:
3813                 l2cap_command_rej(conn, cmd, data);
3814                 break;
3815
3816         case L2CAP_CONN_REQ:
3817                 err = l2cap_connect_req(conn, cmd, data);
3818                 break;
3819
3820         case L2CAP_CONN_RSP:
3821                 err = l2cap_connect_rsp(conn, cmd, data);
3822                 break;
3823
3824         case L2CAP_CONF_REQ:
3825                 err = l2cap_config_req(conn, cmd, cmd_len, data);
3826                 break;
3827
3828         case L2CAP_CONF_RSP:
3829                 err = l2cap_config_rsp(conn, cmd, data);
3830                 break;
3831
3832         case L2CAP_DISCONN_REQ:
3833                 err = l2cap_disconnect_req(conn, cmd, data);
3834                 break;
3835
3836         case L2CAP_DISCONN_RSP:
3837                 err = l2cap_disconnect_rsp(conn, cmd, data);
3838                 break;
3839
3840         case L2CAP_ECHO_REQ:
3841                 l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data);
3842                 break;
3843
3844         case L2CAP_ECHO_RSP:
3845                 break;
3846
3847         case L2CAP_INFO_REQ:
3848                 err = l2cap_information_req(conn, cmd, data);
3849                 break;
3850
3851         case L2CAP_INFO_RSP:
3852                 err = l2cap_information_rsp(conn, cmd, data);
3853                 break;
3854
3855         case L2CAP_CREATE_CHAN_REQ:
3856                 err = l2cap_create_channel_req(conn, cmd, cmd_len, data);
3857                 break;
3858
3859         case L2CAP_CREATE_CHAN_RSP:
3860                 err = l2cap_create_channel_rsp(conn, cmd, data);
3861                 break;
3862
3863         case L2CAP_MOVE_CHAN_REQ:
3864                 err = l2cap_move_channel_req(conn, cmd, cmd_len, data);
3865                 break;
3866
3867         case L2CAP_MOVE_CHAN_RSP:
3868                 err = l2cap_move_channel_rsp(conn, cmd, cmd_len, data);
3869                 break;
3870
3871         case L2CAP_MOVE_CHAN_CFM:
3872                 err = l2cap_move_channel_confirm(conn, cmd, cmd_len, data);
3873                 break;
3874
3875         case L2CAP_MOVE_CHAN_CFM_RSP:
3876                 err = l2cap_move_channel_confirm_rsp(conn, cmd, cmd_len, data);
3877                 break;
3878
3879         default:
3880                 BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code);
3881                 err = -EINVAL;
3882                 break;
3883         }
3884
3885         return err;
3886 }
3887
3888 static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
3889                                         struct l2cap_cmd_hdr *cmd, u8 *data)
3890 {
3891         switch (cmd->code) {
3892         case L2CAP_COMMAND_REJ:
3893                 return 0;
3894
3895         case L2CAP_CONN_PARAM_UPDATE_REQ:
3896                 return l2cap_conn_param_update_req(conn, cmd, data);
3897
3898         case L2CAP_CONN_PARAM_UPDATE_RSP:
3899                 return 0;
3900
3901         default:
3902                 BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code);
3903                 return -EINVAL;
3904         }
3905 }
3906
3907 static inline void l2cap_sig_channel(struct l2cap_conn *conn,
3908                                                         struct sk_buff *skb)
3909 {
3910         u8 *data = skb->data;
3911         int len = skb->len;
3912         struct l2cap_cmd_hdr cmd;
3913         int err;
3914
3915         l2cap_raw_recv(conn, skb);
3916
3917         while (len >= L2CAP_CMD_HDR_SIZE) {
3918                 u16 cmd_len;
3919                 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
3920                 data += L2CAP_CMD_HDR_SIZE;
3921                 len  -= L2CAP_CMD_HDR_SIZE;
3922
3923                 cmd_len = le16_to_cpu(cmd.len);
3924
3925                 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, cmd.ident);
3926
3927                 if (cmd_len > len || !cmd.ident) {
3928                         BT_DBG("corrupted command");
3929                         break;
3930                 }
3931
3932                 if (conn->hcon->type == LE_LINK)
3933                         err = l2cap_le_sig_cmd(conn, &cmd, data);
3934                 else
3935                         err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data);
3936
3937                 if (err) {
3938                         struct l2cap_cmd_rej_unk rej;
3939
3940                         BT_ERR("Wrong link type (%d)", err);
3941
3942                         /* FIXME: Map err to a valid reason */
3943                         rej.reason = cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
3944                         l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
3945                 }
3946
3947                 data += cmd_len;
3948                 len  -= cmd_len;
3949         }
3950
3951         kfree_skb(skb);
3952 }
3953
3954 static int l2cap_check_fcs(struct l2cap_chan *chan,  struct sk_buff *skb)
3955 {
3956         u16 our_fcs, rcv_fcs;
3957         int hdr_size;
3958
3959         if (test_bit(FLAG_EXT_CTRL, &chan->flags))
3960                 hdr_size = L2CAP_EXT_HDR_SIZE;
3961         else
3962                 hdr_size = L2CAP_ENH_HDR_SIZE;
3963
3964         if (chan->fcs == L2CAP_FCS_CRC16) {
3965                 skb_trim(skb, skb->len - L2CAP_FCS_SIZE);
3966                 rcv_fcs = get_unaligned_le16(skb->data + skb->len);
3967                 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
3968
3969                 if (our_fcs != rcv_fcs)
3970                         return -EBADMSG;
3971         }
3972         return 0;
3973 }
3974
3975 static inline void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan)
3976 {
3977         u32 control = 0;
3978
3979         chan->frames_sent = 0;
3980
3981         control |= __set_reqseq(chan, chan->buffer_seq);
3982
3983         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
3984                 control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
3985                 l2cap_send_sframe(chan, control);
3986                 set_bit(CONN_RNR_SENT, &chan->conn_state);
3987         }
3988
3989         if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
3990                 l2cap_retransmit_frames(chan);
3991
3992         l2cap_ertm_send(chan);
3993
3994         if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
3995                         chan->frames_sent == 0) {
3996                 control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
3997                 l2cap_send_sframe(chan, control);
3998         }
3999 }
4000
4001 static int l2cap_add_to_srej_queue(struct l2cap_chan *chan, struct sk_buff *skb, u16 tx_seq, u8 sar)
4002 {
4003         struct sk_buff *next_skb;
4004         int tx_seq_offset, next_tx_seq_offset;
4005
4006         bt_cb(skb)->control.txseq = tx_seq;
4007         bt_cb(skb)->control.sar = sar;
4008
4009         next_skb = skb_peek(&chan->srej_q);
4010
4011         tx_seq_offset = __seq_offset(chan, tx_seq, chan->buffer_seq);
4012
4013         while (next_skb) {
4014                 if (bt_cb(next_skb)->control.txseq == tx_seq)
4015                         return -EINVAL;
4016
4017                 next_tx_seq_offset = __seq_offset(chan,
4018                         bt_cb(next_skb)->control.txseq, chan->buffer_seq);
4019
4020                 if (next_tx_seq_offset > tx_seq_offset) {
4021                         __skb_queue_before(&chan->srej_q, next_skb, skb);
4022                         return 0;
4023                 }
4024
4025                 if (skb_queue_is_last(&chan->srej_q, next_skb))
4026                         next_skb = NULL;
4027                 else
4028                         next_skb = skb_queue_next(&chan->srej_q, next_skb);
4029         }
4030
4031         __skb_queue_tail(&chan->srej_q, skb);
4032
4033         return 0;
4034 }
4035
4036 static void append_skb_frag(struct sk_buff *skb,
4037                         struct sk_buff *new_frag, struct sk_buff **last_frag)
4038 {
4039         /* skb->len reflects data in skb as well as all fragments
4040          * skb->data_len reflects only data in fragments
4041          */
4042         if (!skb_has_frag_list(skb))
4043                 skb_shinfo(skb)->frag_list = new_frag;
4044
4045         new_frag->next = NULL;
4046
4047         (*last_frag)->next = new_frag;
4048         *last_frag = new_frag;
4049
4050         skb->len += new_frag->len;
4051         skb->data_len += new_frag->len;
4052         skb->truesize += new_frag->truesize;
4053 }
4054
4055 static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u32 control)
4056 {
4057         int err = -EINVAL;
4058
4059         switch (__get_ctrl_sar(chan, control)) {
4060         case L2CAP_SAR_UNSEGMENTED:
4061                 if (chan->sdu)
4062                         break;
4063
4064                 err = chan->ops->recv(chan->data, skb);
4065                 break;
4066
4067         case L2CAP_SAR_START:
4068                 if (chan->sdu)
4069                         break;
4070
4071                 chan->sdu_len = get_unaligned_le16(skb->data);
4072                 skb_pull(skb, L2CAP_SDULEN_SIZE);
4073
4074                 if (chan->sdu_len > chan->imtu) {
4075                         err = -EMSGSIZE;
4076                         break;
4077                 }
4078
4079                 if (skb->len >= chan->sdu_len)
4080                         break;
4081
4082                 chan->sdu = skb;
4083                 chan->sdu_last_frag = skb;
4084
4085                 skb = NULL;
4086                 err = 0;
4087                 break;
4088
4089         case L2CAP_SAR_CONTINUE:
4090                 if (!chan->sdu)
4091                         break;
4092
4093                 append_skb_frag(chan->sdu, skb,
4094                                 &chan->sdu_last_frag);
4095                 skb = NULL;
4096
4097                 if (chan->sdu->len >= chan->sdu_len)
4098                         break;
4099
4100                 err = 0;
4101                 break;
4102
4103         case L2CAP_SAR_END:
4104                 if (!chan->sdu)
4105                         break;
4106
4107                 append_skb_frag(chan->sdu, skb,
4108                                 &chan->sdu_last_frag);
4109                 skb = NULL;
4110
4111                 if (chan->sdu->len != chan->sdu_len)
4112                         break;
4113
4114                 err = chan->ops->recv(chan->data, chan->sdu);
4115
4116                 if (!err) {
4117                         /* Reassembly complete */
4118                         chan->sdu = NULL;
4119                         chan->sdu_last_frag = NULL;
4120                         chan->sdu_len = 0;
4121                 }
4122                 break;
4123         }
4124
4125         if (err) {
4126                 kfree_skb(skb);
4127                 kfree_skb(chan->sdu);
4128                 chan->sdu = NULL;
4129                 chan->sdu_last_frag = NULL;
4130                 chan->sdu_len = 0;
4131         }
4132
4133         return err;
4134 }
4135
4136 static void l2cap_ertm_enter_local_busy(struct l2cap_chan *chan)
4137 {
4138         BT_DBG("chan %p, Enter local busy", chan);
4139
4140         set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
4141         l2cap_seq_list_clear(&chan->srej_list);
4142
4143         __set_ack_timer(chan);
4144 }
4145
4146 static void l2cap_ertm_exit_local_busy(struct l2cap_chan *chan)
4147 {
4148         u32 control;
4149
4150         if (!test_bit(CONN_RNR_SENT, &chan->conn_state))
4151                 goto done;
4152
4153         control = __set_reqseq(chan, chan->buffer_seq);
4154         control |= __set_ctrl_poll(chan);
4155         control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
4156         l2cap_send_sframe(chan, control);
4157         chan->retry_count = 1;
4158
4159         __clear_retrans_timer(chan);
4160         __set_monitor_timer(chan);
4161
4162         set_bit(CONN_WAIT_F, &chan->conn_state);
4163
4164 done:
4165         clear_bit(CONN_LOCAL_BUSY, &chan->conn_state);
4166         clear_bit(CONN_RNR_SENT, &chan->conn_state);
4167
4168         BT_DBG("chan %p, Exit local busy", chan);
4169 }
4170
4171 void l2cap_chan_busy(struct l2cap_chan *chan, int busy)
4172 {
4173         if (chan->mode == L2CAP_MODE_ERTM) {
4174                 if (busy)
4175                         l2cap_ertm_enter_local_busy(chan);
4176                 else
4177                         l2cap_ertm_exit_local_busy(chan);
4178         }
4179 }
4180
4181 static void l2cap_check_srej_gap(struct l2cap_chan *chan, u16 tx_seq)
4182 {
4183         struct sk_buff *skb;
4184         u32 control;
4185
4186         while ((skb = skb_peek(&chan->srej_q)) &&
4187                         !test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
4188                 int err;
4189
4190                 if (bt_cb(skb)->control.txseq != tx_seq)
4191                         break;
4192
4193                 skb = skb_dequeue(&chan->srej_q);
4194                 control = __set_ctrl_sar(chan, bt_cb(skb)->control.sar);
4195                 err = l2cap_reassemble_sdu(chan, skb, control);
4196
4197                 if (err < 0) {
4198                         l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
4199                         break;
4200                 }
4201
4202                 chan->buffer_seq_srej = __next_seq(chan, chan->buffer_seq_srej);
4203                 tx_seq = __next_seq(chan, tx_seq);
4204         }
4205 }
4206
4207 static void l2cap_resend_srejframe(struct l2cap_chan *chan, u16 tx_seq)
4208 {
4209         struct srej_list *l, *tmp;
4210         u32 control;
4211
4212         list_for_each_entry_safe(l, tmp, &chan->srej_l, list) {
4213                 if (l->tx_seq == tx_seq) {
4214                         list_del(&l->list);
4215                         kfree(l);
4216                         return;
4217                 }
4218                 control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ);
4219                 control |= __set_reqseq(chan, l->tx_seq);
4220                 l2cap_send_sframe(chan, control);
4221                 list_del(&l->list);
4222                 list_add_tail(&l->list, &chan->srej_l);
4223         }
4224 }
4225
4226 static int l2cap_send_srejframe(struct l2cap_chan *chan, u16 tx_seq)
4227 {
4228         struct srej_list *new;
4229         u32 control;
4230
4231         while (tx_seq != chan->expected_tx_seq) {
4232                 control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ);
4233                 control |= __set_reqseq(chan, chan->expected_tx_seq);
4234                 l2cap_seq_list_append(&chan->srej_list, chan->expected_tx_seq);
4235                 l2cap_send_sframe(chan, control);
4236
4237                 new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC);
4238                 if (!new)
4239                         return -ENOMEM;
4240
4241                 new->tx_seq = chan->expected_tx_seq;
4242
4243                 chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq);
4244
4245                 list_add_tail(&new->list, &chan->srej_l);
4246         }
4247
4248         chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq);
4249
4250         return 0;
4251 }
4252
4253 static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u32 rx_control, struct sk_buff *skb)
4254 {
4255         u16 tx_seq = __get_txseq(chan, rx_control);
4256         u16 req_seq = __get_reqseq(chan, rx_control);
4257         u8 sar = __get_ctrl_sar(chan, rx_control);
4258         int tx_seq_offset, expected_tx_seq_offset;
4259         int num_to_ack = (chan->tx_win/6) + 1;
4260         int err = 0;
4261
4262         BT_DBG("chan %p len %d tx_seq %d rx_control 0x%8.8x", chan, skb->len,
4263                                                         tx_seq, rx_control);
4264
4265         if (__is_ctrl_final(chan, rx_control) &&
4266                         test_bit(CONN_WAIT_F, &chan->conn_state)) {
4267                 __clear_monitor_timer(chan);
4268                 if (chan->unacked_frames > 0)
4269                         __set_retrans_timer(chan);
4270                 clear_bit(CONN_WAIT_F, &chan->conn_state);
4271         }
4272
4273         chan->expected_ack_seq = req_seq;
4274         l2cap_drop_acked_frames(chan);
4275
4276         tx_seq_offset = __seq_offset(chan, tx_seq, chan->buffer_seq);
4277
4278         /* invalid tx_seq */
4279         if (tx_seq_offset >= chan->tx_win) {
4280                 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
4281                 goto drop;
4282         }
4283
4284         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
4285                 if (!test_bit(CONN_RNR_SENT, &chan->conn_state))
4286                         l2cap_send_ack(chan);
4287                 goto drop;
4288         }
4289
4290         if (tx_seq == chan->expected_tx_seq)
4291                 goto expected;
4292
4293         if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
4294                 struct srej_list *first;
4295
4296                 first = list_first_entry(&chan->srej_l,
4297                                 struct srej_list, list);
4298                 if (tx_seq == first->tx_seq) {
4299                         l2cap_add_to_srej_queue(chan, skb, tx_seq, sar);
4300                         l2cap_check_srej_gap(chan, tx_seq);
4301
4302                         list_del(&first->list);
4303                         kfree(first);
4304
4305                         if (list_empty(&chan->srej_l)) {
4306                                 chan->buffer_seq = chan->buffer_seq_srej;
4307                                 clear_bit(CONN_SREJ_SENT, &chan->conn_state);
4308                                 l2cap_send_ack(chan);
4309                                 BT_DBG("chan %p, Exit SREJ_SENT", chan);
4310                         }
4311                 } else {
4312                         struct srej_list *l;
4313
4314                         /* duplicated tx_seq */
4315                         if (l2cap_add_to_srej_queue(chan, skb, tx_seq, sar) < 0)
4316                                 goto drop;
4317
4318                         list_for_each_entry(l, &chan->srej_l, list) {
4319                                 if (l->tx_seq == tx_seq) {
4320                                         l2cap_resend_srejframe(chan, tx_seq);
4321                                         return 0;
4322                                 }
4323                         }
4324
4325                         err = l2cap_send_srejframe(chan, tx_seq);
4326                         if (err < 0) {
4327                                 l2cap_send_disconn_req(chan->conn, chan, -err);
4328                                 return err;
4329                         }
4330                 }
4331         } else {
4332                 expected_tx_seq_offset = __seq_offset(chan,
4333                                 chan->expected_tx_seq, chan->buffer_seq);
4334
4335                 /* duplicated tx_seq */
4336                 if (tx_seq_offset < expected_tx_seq_offset)
4337                         goto drop;
4338
4339                 set_bit(CONN_SREJ_SENT, &chan->conn_state);
4340
4341                 BT_DBG("chan %p, Enter SREJ", chan);
4342
4343                 INIT_LIST_HEAD(&chan->srej_l);
4344                 chan->buffer_seq_srej = chan->buffer_seq;
4345
4346                 __skb_queue_head_init(&chan->srej_q);
4347                 l2cap_add_to_srej_queue(chan, skb, tx_seq, sar);
4348
4349                 /* Set P-bit only if there are some I-frames to ack. */
4350                 if (__clear_ack_timer(chan))
4351                         set_bit(CONN_SEND_PBIT, &chan->conn_state);
4352
4353                 err = l2cap_send_srejframe(chan, tx_seq);
4354                 if (err < 0) {
4355                         l2cap_send_disconn_req(chan->conn, chan, -err);
4356                         return err;
4357                 }
4358         }
4359         return 0;
4360
4361 expected:
4362         chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq);
4363
4364         if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
4365                 bt_cb(skb)->control.txseq = tx_seq;
4366                 bt_cb(skb)->control.sar = sar;
4367                 __skb_queue_tail(&chan->srej_q, skb);
4368                 return 0;
4369         }
4370
4371         err = l2cap_reassemble_sdu(chan, skb, rx_control);
4372         chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
4373
4374         if (err < 0) {
4375                 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
4376                 return err;
4377         }
4378
4379         if (__is_ctrl_final(chan, rx_control)) {
4380                 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
4381                         l2cap_retransmit_frames(chan);
4382         }
4383
4384
4385         chan->num_acked = (chan->num_acked + 1) % num_to_ack;
4386         if (chan->num_acked == num_to_ack - 1)
4387                 l2cap_send_ack(chan);
4388         else
4389                 __set_ack_timer(chan);
4390
4391         return 0;
4392
4393 drop:
4394         kfree_skb(skb);
4395         return 0;
4396 }
4397
4398 static inline void l2cap_data_channel_rrframe(struct l2cap_chan *chan, u32 rx_control)
4399 {
4400         BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan,
4401                                 __get_reqseq(chan, rx_control), rx_control);
4402
4403         chan->expected_ack_seq = __get_reqseq(chan, rx_control);
4404         l2cap_drop_acked_frames(chan);
4405
4406         if (__is_ctrl_poll(chan, rx_control)) {
4407                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
4408                 if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
4409                         if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
4410                                         (chan->unacked_frames > 0))
4411                                 __set_retrans_timer(chan);
4412
4413                         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
4414                         l2cap_send_srejtail(chan);
4415                 } else {
4416                         l2cap_send_i_or_rr_or_rnr(chan);
4417                 }
4418
4419         } else if (__is_ctrl_final(chan, rx_control)) {
4420                 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
4421
4422                 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
4423                         l2cap_retransmit_frames(chan);
4424
4425         } else {
4426                 if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
4427                                 (chan->unacked_frames > 0))
4428                         __set_retrans_timer(chan);
4429
4430                 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
4431                 if (test_bit(CONN_SREJ_SENT, &chan->conn_state))
4432                         l2cap_send_ack(chan);
4433                 else
4434                         l2cap_ertm_send(chan);
4435         }
4436 }
4437
4438 static inline void l2cap_data_channel_rejframe(struct l2cap_chan *chan, u32 rx_control)
4439 {
4440         u16 tx_seq = __get_reqseq(chan, rx_control);
4441
4442         BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control);
4443
4444         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
4445
4446         chan->expected_ack_seq = tx_seq;
4447         l2cap_drop_acked_frames(chan);
4448
4449         if (__is_ctrl_final(chan, rx_control)) {
4450                 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
4451                         l2cap_retransmit_frames(chan);
4452         } else {
4453                 l2cap_retransmit_frames(chan);
4454
4455                 if (test_bit(CONN_WAIT_F, &chan->conn_state))
4456                         set_bit(CONN_REJ_ACT, &chan->conn_state);
4457         }
4458 }
4459 static inline void l2cap_data_channel_srejframe(struct l2cap_chan *chan, u32 rx_control)
4460 {
4461         u16 tx_seq = __get_reqseq(chan, rx_control);
4462
4463         BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control);
4464
4465         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
4466
4467         if (__is_ctrl_poll(chan, rx_control)) {
4468                 chan->expected_ack_seq = tx_seq;
4469                 l2cap_drop_acked_frames(chan);
4470
4471                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
4472                 l2cap_retransmit_one_frame(chan, tx_seq);
4473
4474                 l2cap_ertm_send(chan);
4475
4476                 if (test_bit(CONN_WAIT_F, &chan->conn_state)) {
4477                         chan->srej_save_reqseq = tx_seq;
4478                         set_bit(CONN_SREJ_ACT, &chan->conn_state);
4479                 }
4480         } else if (__is_ctrl_final(chan, rx_control)) {
4481                 if (test_bit(CONN_SREJ_ACT, &chan->conn_state) &&
4482                                 chan->srej_save_reqseq == tx_seq)
4483                         clear_bit(CONN_SREJ_ACT, &chan->conn_state);
4484                 else
4485                         l2cap_retransmit_one_frame(chan, tx_seq);
4486         } else {
4487                 l2cap_retransmit_one_frame(chan, tx_seq);
4488                 if (test_bit(CONN_WAIT_F, &chan->conn_state)) {
4489                         chan->srej_save_reqseq = tx_seq;
4490                         set_bit(CONN_SREJ_ACT, &chan->conn_state);
4491                 }
4492         }
4493 }
4494
4495 static inline void l2cap_data_channel_rnrframe(struct l2cap_chan *chan, u32 rx_control)
4496 {
4497         u16 tx_seq = __get_reqseq(chan, rx_control);
4498
4499         BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control);
4500
4501         set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
4502         chan->expected_ack_seq = tx_seq;
4503         l2cap_drop_acked_frames(chan);
4504
4505         if (__is_ctrl_poll(chan, rx_control))
4506                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
4507
4508         if (!test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
4509                 __clear_retrans_timer(chan);
4510                 if (__is_ctrl_poll(chan, rx_control))
4511                         l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_FINAL);
4512                 return;
4513         }
4514
4515         if (__is_ctrl_poll(chan, rx_control)) {
4516                 l2cap_send_srejtail(chan);
4517         } else {
4518                 rx_control = __set_ctrl_super(chan, L2CAP_SUPER_RR);
4519                 l2cap_send_sframe(chan, rx_control);
4520         }
4521 }
4522
4523 static inline int l2cap_data_channel_sframe(struct l2cap_chan *chan, u32 rx_control, struct sk_buff *skb)
4524 {
4525         BT_DBG("chan %p rx_control 0x%8.8x len %d", chan, rx_control, skb->len);
4526
4527         if (__is_ctrl_final(chan, rx_control) &&
4528                         test_bit(CONN_WAIT_F, &chan->conn_state)) {
4529                 __clear_monitor_timer(chan);
4530                 if (chan->unacked_frames > 0)
4531                         __set_retrans_timer(chan);
4532                 clear_bit(CONN_WAIT_F, &chan->conn_state);
4533         }
4534
4535         switch (__get_ctrl_super(chan, rx_control)) {
4536         case L2CAP_SUPER_RR:
4537                 l2cap_data_channel_rrframe(chan, rx_control);
4538                 break;
4539
4540         case L2CAP_SUPER_REJ:
4541                 l2cap_data_channel_rejframe(chan, rx_control);
4542                 break;
4543
4544         case L2CAP_SUPER_SREJ:
4545                 l2cap_data_channel_srejframe(chan, rx_control);
4546                 break;
4547
4548         case L2CAP_SUPER_RNR:
4549                 l2cap_data_channel_rnrframe(chan, rx_control);
4550                 break;
4551         }
4552
4553         kfree_skb(skb);
4554         return 0;
4555 }
4556
4557 static int l2cap_ertm_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb)
4558 {
4559         u32 control;
4560         u16 req_seq;
4561         int len, next_tx_seq_offset, req_seq_offset;
4562
4563         __unpack_control(chan, skb);
4564
4565         control = __get_control(chan, skb->data);
4566         skb_pull(skb, __ctrl_size(chan));
4567         len = skb->len;
4568
4569         /*
4570          * We can just drop the corrupted I-frame here.
4571          * Receiver will miss it and start proper recovery
4572          * procedures and ask retransmission.
4573          */
4574         if (l2cap_check_fcs(chan, skb))
4575                 goto drop;
4576
4577         if (__is_sar_start(chan, control) && !__is_sframe(chan, control))
4578                 len -= L2CAP_SDULEN_SIZE;
4579
4580         if (chan->fcs == L2CAP_FCS_CRC16)
4581                 len -= L2CAP_FCS_SIZE;
4582
4583         if (len > chan->mps) {
4584                 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
4585                 goto drop;
4586         }
4587
4588         req_seq = __get_reqseq(chan, control);
4589
4590         req_seq_offset = __seq_offset(chan, req_seq, chan->expected_ack_seq);
4591
4592         next_tx_seq_offset = __seq_offset(chan, chan->next_tx_seq,
4593                                                 chan->expected_ack_seq);
4594
4595         /* check for invalid req-seq */
4596         if (req_seq_offset > next_tx_seq_offset) {
4597                 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
4598                 goto drop;
4599         }
4600
4601         if (!__is_sframe(chan, control)) {
4602                 if (len < 0) {
4603                         l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
4604                         goto drop;
4605                 }
4606
4607                 l2cap_data_channel_iframe(chan, control, skb);
4608         } else {
4609                 if (len != 0) {
4610                         BT_ERR("%d", len);
4611                         l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
4612                         goto drop;
4613                 }
4614
4615                 l2cap_data_channel_sframe(chan, control, skb);
4616         }
4617
4618         return 0;
4619
4620 drop:
4621         kfree_skb(skb);
4622         return 0;
4623 }
4624
4625 static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
4626 {
4627         struct l2cap_chan *chan;
4628         u32 control;
4629         u16 tx_seq;
4630         int len;
4631
4632         chan = l2cap_get_chan_by_scid(conn, cid);
4633         if (!chan) {
4634                 BT_DBG("unknown cid 0x%4.4x", cid);
4635                 /* Drop packet and return */
4636                 kfree_skb(skb);
4637                 return 0;
4638         }
4639
4640         BT_DBG("chan %p, len %d", chan, skb->len);
4641
4642         if (chan->state != BT_CONNECTED)
4643                 goto drop;
4644
4645         switch (chan->mode) {
4646         case L2CAP_MODE_BASIC:
4647                 /* If socket recv buffers overflows we drop data here
4648                  * which is *bad* because L2CAP has to be reliable.
4649                  * But we don't have any other choice. L2CAP doesn't
4650                  * provide flow control mechanism. */
4651
4652                 if (chan->imtu < skb->len)
4653                         goto drop;
4654
4655                 if (!chan->ops->recv(chan->data, skb))
4656                         goto done;
4657                 break;
4658
4659         case L2CAP_MODE_ERTM:
4660                 l2cap_ertm_data_rcv(chan, skb);
4661
4662                 goto done;
4663
4664         case L2CAP_MODE_STREAMING:
4665                 control = __get_control(chan, skb->data);
4666                 skb_pull(skb, __ctrl_size(chan));
4667                 len = skb->len;
4668
4669                 if (l2cap_check_fcs(chan, skb))
4670                         goto drop;
4671
4672                 if (__is_sar_start(chan, control))
4673                         len -= L2CAP_SDULEN_SIZE;
4674
4675                 if (chan->fcs == L2CAP_FCS_CRC16)
4676                         len -= L2CAP_FCS_SIZE;
4677
4678                 if (len > chan->mps || len < 0 || __is_sframe(chan, control))
4679                         goto drop;
4680
4681                 tx_seq = __get_txseq(chan, control);
4682
4683                 if (chan->expected_tx_seq != tx_seq) {
4684                         /* Frame(s) missing - must discard partial SDU */
4685                         kfree_skb(chan->sdu);
4686                         chan->sdu = NULL;
4687                         chan->sdu_last_frag = NULL;
4688                         chan->sdu_len = 0;
4689
4690                         /* TODO: Notify userland of missing data */
4691                 }
4692
4693                 chan->expected_tx_seq = __next_seq(chan, tx_seq);
4694
4695                 if (l2cap_reassemble_sdu(chan, skb, control) == -EMSGSIZE)
4696                         l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
4697
4698                 goto done;
4699
4700         default:
4701                 BT_DBG("chan %p: bad mode 0x%2.2x", chan, chan->mode);
4702                 break;
4703         }
4704
4705 drop:
4706         kfree_skb(skb);
4707
4708 done:
4709         l2cap_chan_unlock(chan);
4710
4711         return 0;
4712 }
4713
4714 static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb)
4715 {
4716         struct l2cap_chan *chan;
4717
4718         chan = l2cap_global_chan_by_psm(0, psm, conn->src, conn->dst);
4719         if (!chan)
4720                 goto drop;
4721
4722         BT_DBG("chan %p, len %d", chan, skb->len);
4723
4724         if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
4725                 goto drop;
4726
4727         if (chan->imtu < skb->len)
4728                 goto drop;
4729
4730         if (!chan->ops->recv(chan->data, skb))
4731                 return 0;
4732
4733 drop:
4734         kfree_skb(skb);
4735
4736         return 0;
4737 }
4738
4739 static inline int l2cap_att_channel(struct l2cap_conn *conn, u16 cid,
4740                                     struct sk_buff *skb)
4741 {
4742         struct l2cap_chan *chan;
4743
4744         chan = l2cap_global_chan_by_scid(0, cid, conn->src, conn->dst);
4745         if (!chan)
4746                 goto drop;
4747
4748         BT_DBG("chan %p, len %d", chan, skb->len);
4749
4750         if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
4751                 goto drop;
4752
4753         if (chan->imtu < skb->len)
4754                 goto drop;
4755
4756         if (!chan->ops->recv(chan->data, skb))
4757                 return 0;
4758
4759 drop:
4760         kfree_skb(skb);
4761
4762         return 0;
4763 }
4764
4765 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
4766 {
4767         struct l2cap_hdr *lh = (void *) skb->data;
4768         u16 cid, len;
4769         __le16 psm;
4770
4771         skb_pull(skb, L2CAP_HDR_SIZE);
4772         cid = __le16_to_cpu(lh->cid);
4773         len = __le16_to_cpu(lh->len);
4774
4775         if (len != skb->len) {
4776                 kfree_skb(skb);
4777                 return;
4778         }
4779
4780         BT_DBG("len %d, cid 0x%4.4x", len, cid);
4781
4782         switch (cid) {
4783         case L2CAP_CID_LE_SIGNALING:
4784         case L2CAP_CID_SIGNALING:
4785                 l2cap_sig_channel(conn, skb);
4786                 break;
4787
4788         case L2CAP_CID_CONN_LESS:
4789                 psm = get_unaligned((__le16 *) skb->data);
4790                 skb_pull(skb, 2);
4791                 l2cap_conless_channel(conn, psm, skb);
4792                 break;
4793
4794         case L2CAP_CID_LE_DATA:
4795                 l2cap_att_channel(conn, cid, skb);
4796                 break;
4797
4798         case L2CAP_CID_SMP:
4799                 if (smp_sig_channel(conn, skb))
4800                         l2cap_conn_del(conn->hcon, EACCES);
4801                 break;
4802
4803         default:
4804                 l2cap_data_channel(conn, cid, skb);
4805                 break;
4806         }
4807 }
4808
4809 /* ---- L2CAP interface with lower layer (HCI) ---- */
4810
4811 int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr)
4812 {
4813         int exact = 0, lm1 = 0, lm2 = 0;
4814         struct l2cap_chan *c;
4815
4816         BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
4817
4818         /* Find listening sockets and check their link_mode */
4819         read_lock(&chan_list_lock);
4820         list_for_each_entry(c, &chan_list, global_l) {
4821                 struct sock *sk = c->sk;
4822
4823                 if (c->state != BT_LISTEN)
4824                         continue;
4825
4826                 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
4827                         lm1 |= HCI_LM_ACCEPT;
4828                         if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
4829                                 lm1 |= HCI_LM_MASTER;
4830                         exact++;
4831                 } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
4832                         lm2 |= HCI_LM_ACCEPT;
4833                         if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
4834                                 lm2 |= HCI_LM_MASTER;
4835                 }
4836         }
4837         read_unlock(&chan_list_lock);
4838
4839         return exact ? lm1 : lm2;
4840 }
4841
4842 int l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
4843 {
4844         struct l2cap_conn *conn;
4845
4846         BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
4847
4848         if (!status) {
4849                 conn = l2cap_conn_add(hcon, status);
4850                 if (conn)
4851                         l2cap_conn_ready(conn);
4852         } else
4853                 l2cap_conn_del(hcon, bt_to_errno(status));
4854
4855         return 0;
4856 }
4857
4858 int l2cap_disconn_ind(struct hci_conn *hcon)
4859 {
4860         struct l2cap_conn *conn = hcon->l2cap_data;
4861
4862         BT_DBG("hcon %p", hcon);
4863
4864         if (!conn)
4865                 return HCI_ERROR_REMOTE_USER_TERM;
4866         return conn->disc_reason;
4867 }
4868
4869 int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
4870 {
4871         BT_DBG("hcon %p reason %d", hcon, reason);
4872
4873         l2cap_conn_del(hcon, bt_to_errno(reason));
4874         return 0;
4875 }
4876
4877 static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt)
4878 {
4879         if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED)
4880                 return;
4881
4882         if (encrypt == 0x00) {
4883                 if (chan->sec_level == BT_SECURITY_MEDIUM) {
4884                         __set_chan_timer(chan, L2CAP_ENC_TIMEOUT);
4885                 } else if (chan->sec_level == BT_SECURITY_HIGH)
4886                         l2cap_chan_close(chan, ECONNREFUSED);
4887         } else {
4888                 if (chan->sec_level == BT_SECURITY_MEDIUM)
4889                         __clear_chan_timer(chan);
4890         }
4891 }
4892
4893 int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
4894 {
4895         struct l2cap_conn *conn = hcon->l2cap_data;
4896         struct l2cap_chan *chan;
4897
4898         if (!conn)
4899                 return 0;
4900
4901         BT_DBG("conn %p", conn);
4902
4903         if (hcon->type == LE_LINK) {
4904                 if (!status && encrypt)
4905                         smp_distribute_keys(conn, 0);
4906                 cancel_delayed_work(&conn->security_timer);
4907         }
4908
4909         mutex_lock(&conn->chan_lock);
4910
4911         list_for_each_entry(chan, &conn->chan_l, list) {
4912                 l2cap_chan_lock(chan);
4913
4914                 BT_DBG("chan->scid %d", chan->scid);
4915
4916                 if (chan->scid == L2CAP_CID_LE_DATA) {
4917                         if (!status && encrypt) {
4918                                 chan->sec_level = hcon->sec_level;
4919                                 l2cap_chan_ready(chan);
4920                         }
4921
4922                         l2cap_chan_unlock(chan);
4923                         continue;
4924                 }
4925
4926                 if (test_bit(CONF_CONNECT_PEND, &chan->conf_state)) {
4927                         l2cap_chan_unlock(chan);
4928                         continue;
4929                 }
4930
4931                 if (!status && (chan->state == BT_CONNECTED ||
4932                                                 chan->state == BT_CONFIG)) {
4933                         struct sock *sk = chan->sk;
4934
4935                         clear_bit(BT_SK_SUSPEND, &bt_sk(sk)->flags);
4936                         sk->sk_state_change(sk);
4937
4938                         l2cap_check_encryption(chan, encrypt);
4939                         l2cap_chan_unlock(chan);
4940                         continue;
4941                 }
4942
4943                 if (chan->state == BT_CONNECT) {
4944                         if (!status) {
4945                                 l2cap_send_conn_req(chan);
4946                         } else {
4947                                 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
4948                         }
4949                 } else if (chan->state == BT_CONNECT2) {
4950                         struct sock *sk = chan->sk;
4951                         struct l2cap_conn_rsp rsp;
4952                         __u16 res, stat;
4953
4954                         lock_sock(sk);
4955
4956                         if (!status) {
4957                                 if (test_bit(BT_SK_DEFER_SETUP,
4958                                              &bt_sk(sk)->flags)) {
4959                                         struct sock *parent = bt_sk(sk)->parent;
4960                                         res = L2CAP_CR_PEND;
4961                                         stat = L2CAP_CS_AUTHOR_PEND;
4962                                         if (parent)
4963                                                 parent->sk_data_ready(parent, 0);
4964                                 } else {
4965                                         __l2cap_state_change(chan, BT_CONFIG);
4966                                         res = L2CAP_CR_SUCCESS;
4967                                         stat = L2CAP_CS_NO_INFO;
4968                                 }
4969                         } else {
4970                                 __l2cap_state_change(chan, BT_DISCONN);
4971                                 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
4972                                 res = L2CAP_CR_SEC_BLOCK;
4973                                 stat = L2CAP_CS_NO_INFO;
4974                         }
4975
4976                         release_sock(sk);
4977
4978                         rsp.scid   = cpu_to_le16(chan->dcid);
4979                         rsp.dcid   = cpu_to_le16(chan->scid);
4980                         rsp.result = cpu_to_le16(res);
4981                         rsp.status = cpu_to_le16(stat);
4982                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
4983                                                         sizeof(rsp), &rsp);
4984                 }
4985
4986                 l2cap_chan_unlock(chan);
4987         }
4988
4989         mutex_unlock(&conn->chan_lock);
4990
4991         return 0;
4992 }
4993
4994 int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
4995 {
4996         struct l2cap_conn *conn = hcon->l2cap_data;
4997
4998         if (!conn)
4999                 conn = l2cap_conn_add(hcon, 0);
5000
5001         if (!conn)
5002                 goto drop;
5003
5004         BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
5005
5006         if (!(flags & ACL_CONT)) {
5007                 struct l2cap_hdr *hdr;
5008                 int len;
5009
5010                 if (conn->rx_len) {
5011                         BT_ERR("Unexpected start frame (len %d)", skb->len);
5012                         kfree_skb(conn->rx_skb);
5013                         conn->rx_skb = NULL;
5014                         conn->rx_len = 0;
5015                         l2cap_conn_unreliable(conn, ECOMM);
5016                 }
5017
5018                 /* Start fragment always begin with Basic L2CAP header */
5019                 if (skb->len < L2CAP_HDR_SIZE) {
5020                         BT_ERR("Frame is too short (len %d)", skb->len);
5021                         l2cap_conn_unreliable(conn, ECOMM);
5022                         goto drop;
5023                 }
5024
5025                 hdr = (struct l2cap_hdr *) skb->data;
5026                 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
5027
5028                 if (len == skb->len) {
5029                         /* Complete frame received */
5030                         l2cap_recv_frame(conn, skb);
5031                         return 0;
5032                 }
5033
5034                 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
5035
5036                 if (skb->len > len) {
5037                         BT_ERR("Frame is too long (len %d, expected len %d)",
5038                                 skb->len, len);
5039                         l2cap_conn_unreliable(conn, ECOMM);
5040                         goto drop;
5041                 }
5042
5043                 /* Allocate skb for the complete frame (with header) */
5044                 conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC);
5045                 if (!conn->rx_skb)
5046                         goto drop;
5047
5048                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
5049                                                                 skb->len);
5050                 conn->rx_len = len - skb->len;
5051         } else {
5052                 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
5053
5054                 if (!conn->rx_len) {
5055                         BT_ERR("Unexpected continuation frame (len %d)", skb->len);
5056                         l2cap_conn_unreliable(conn, ECOMM);
5057                         goto drop;
5058                 }
5059
5060                 if (skb->len > conn->rx_len) {
5061                         BT_ERR("Fragment is too long (len %d, expected %d)",
5062                                         skb->len, conn->rx_len);
5063                         kfree_skb(conn->rx_skb);
5064                         conn->rx_skb = NULL;
5065                         conn->rx_len = 0;
5066                         l2cap_conn_unreliable(conn, ECOMM);
5067                         goto drop;
5068                 }
5069
5070                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
5071                                                                 skb->len);
5072                 conn->rx_len -= skb->len;
5073
5074                 if (!conn->rx_len) {
5075                         /* Complete frame received */
5076                         l2cap_recv_frame(conn, conn->rx_skb);
5077                         conn->rx_skb = NULL;
5078                 }
5079         }
5080
5081 drop:
5082         kfree_skb(skb);
5083         return 0;
5084 }
5085
5086 static int l2cap_debugfs_show(struct seq_file *f, void *p)
5087 {
5088         struct l2cap_chan *c;
5089
5090         read_lock(&chan_list_lock);
5091
5092         list_for_each_entry(c, &chan_list, global_l) {
5093                 struct sock *sk = c->sk;
5094
5095                 seq_printf(f, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
5096                                         batostr(&bt_sk(sk)->src),
5097                                         batostr(&bt_sk(sk)->dst),
5098                                         c->state, __le16_to_cpu(c->psm),
5099                                         c->scid, c->dcid, c->imtu, c->omtu,
5100                                         c->sec_level, c->mode);
5101         }
5102
5103         read_unlock(&chan_list_lock);
5104
5105         return 0;
5106 }
5107
5108 static int l2cap_debugfs_open(struct inode *inode, struct file *file)
5109 {
5110         return single_open(file, l2cap_debugfs_show, inode->i_private);
5111 }
5112
5113 static const struct file_operations l2cap_debugfs_fops = {
5114         .open           = l2cap_debugfs_open,
5115         .read           = seq_read,
5116         .llseek         = seq_lseek,
5117         .release        = single_release,
5118 };
5119
5120 static struct dentry *l2cap_debugfs;
5121
5122 int __init l2cap_init(void)
5123 {
5124         int err;
5125
5126         err = l2cap_init_sockets();
5127         if (err < 0)
5128                 return err;
5129
5130         if (bt_debugfs) {
5131                 l2cap_debugfs = debugfs_create_file("l2cap", 0444,
5132                                         bt_debugfs, NULL, &l2cap_debugfs_fops);
5133                 if (!l2cap_debugfs)
5134                         BT_ERR("Failed to create L2CAP debug file");
5135         }
5136
5137         return 0;
5138 }
5139
5140 void l2cap_exit(void)
5141 {
5142         debugfs_remove(l2cap_debugfs);
5143         l2cap_cleanup_sockets();
5144 }
5145
5146 module_param(disable_ertm, bool, 0644);
5147 MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode");