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