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