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