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