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