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         while (bt_cb(skb)->tx_seq != tx_seq) {
1322                 if (skb_queue_is_last(&chan->tx_q, skb))
1323                         return;
1324
1325                 skb = skb_queue_next(&chan->tx_q, skb);
1326         }
1327
1328         if (chan->remote_max_tx &&
1329                         bt_cb(skb)->retries == chan->remote_max_tx) {
1330                 l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED);
1331                 return;
1332         }
1333
1334         tx_skb = skb_clone(skb, GFP_ATOMIC);
1335         bt_cb(skb)->retries++;
1336
1337         control = __get_control(chan, tx_skb->data + L2CAP_HDR_SIZE);
1338         control &= __get_sar_mask(chan);
1339
1340         if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
1341                 control |= __set_ctrl_final(chan);
1342
1343         control |= __set_reqseq(chan, chan->buffer_seq);
1344         control |= __set_txseq(chan, tx_seq);
1345
1346         __put_control(chan, control, tx_skb->data + L2CAP_HDR_SIZE);
1347
1348         if (chan->fcs == L2CAP_FCS_CRC16) {
1349                 fcs = crc16(0, (u8 *)tx_skb->data,
1350                                                 tx_skb->len - L2CAP_FCS_SIZE);
1351                 put_unaligned_le16(fcs,
1352                                 tx_skb->data + tx_skb->len - L2CAP_FCS_SIZE);
1353         }
1354
1355         l2cap_do_send(chan, tx_skb);
1356 }
1357
1358 static int l2cap_ertm_send(struct l2cap_chan *chan)
1359 {
1360         struct sk_buff *skb, *tx_skb;
1361         u16 fcs;
1362         u32 control;
1363         int nsent = 0;
1364
1365         if (chan->state != BT_CONNECTED)
1366                 return -ENOTCONN;
1367
1368         while ((skb = chan->tx_send_head) && (!l2cap_tx_window_full(chan))) {
1369
1370                 if (chan->remote_max_tx &&
1371                                 bt_cb(skb)->retries == chan->remote_max_tx) {
1372                         l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED);
1373                         break;
1374                 }
1375
1376                 tx_skb = skb_clone(skb, GFP_ATOMIC);
1377
1378                 bt_cb(skb)->retries++;
1379
1380                 control = __get_control(chan, tx_skb->data + L2CAP_HDR_SIZE);
1381                 control &= __get_sar_mask(chan);
1382
1383                 if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
1384                         control |= __set_ctrl_final(chan);
1385
1386                 control |= __set_reqseq(chan, chan->buffer_seq);
1387                 control |= __set_txseq(chan, chan->next_tx_seq);
1388
1389                 __put_control(chan, control, tx_skb->data + L2CAP_HDR_SIZE);
1390
1391                 if (chan->fcs == L2CAP_FCS_CRC16) {
1392                         fcs = crc16(0, (u8 *)skb->data,
1393                                                 tx_skb->len - L2CAP_FCS_SIZE);
1394                         put_unaligned_le16(fcs, skb->data +
1395                                                 tx_skb->len - L2CAP_FCS_SIZE);
1396                 }
1397
1398                 l2cap_do_send(chan, tx_skb);
1399
1400                 __set_retrans_timer(chan);
1401
1402                 bt_cb(skb)->tx_seq = chan->next_tx_seq;
1403
1404                 chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq);
1405
1406                 if (bt_cb(skb)->retries == 1)
1407                         chan->unacked_frames++;
1408
1409                 chan->frames_sent++;
1410
1411                 if (skb_queue_is_last(&chan->tx_q, skb))
1412                         chan->tx_send_head = NULL;
1413                 else
1414                         chan->tx_send_head = skb_queue_next(&chan->tx_q, skb);
1415
1416                 nsent++;
1417         }
1418
1419         return nsent;
1420 }
1421
1422 static int l2cap_retransmit_frames(struct l2cap_chan *chan)
1423 {
1424         int ret;
1425
1426         if (!skb_queue_empty(&chan->tx_q))
1427                 chan->tx_send_head = chan->tx_q.next;
1428
1429         chan->next_tx_seq = chan->expected_ack_seq;
1430         ret = l2cap_ertm_send(chan);
1431         return ret;
1432 }
1433
1434 static void l2cap_send_ack(struct l2cap_chan *chan)
1435 {
1436         u32 control = 0;
1437
1438         control |= __set_reqseq(chan, chan->buffer_seq);
1439
1440         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
1441                 control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
1442                 set_bit(CONN_RNR_SENT, &chan->conn_state);
1443                 l2cap_send_sframe(chan, control);
1444                 return;
1445         }
1446
1447         if (l2cap_ertm_send(chan) > 0)
1448                 return;
1449
1450         control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
1451         l2cap_send_sframe(chan, control);
1452 }
1453
1454 static void l2cap_send_srejtail(struct l2cap_chan *chan)
1455 {
1456         struct srej_list *tail;
1457         u32 control;
1458
1459         control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ);
1460         control |= __set_ctrl_final(chan);
1461
1462         tail = list_entry((&chan->srej_l)->prev, struct srej_list, list);
1463         control |= __set_reqseq(chan, tail->tx_seq);
1464
1465         l2cap_send_sframe(chan, control);
1466 }
1467
1468 static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, int len, int count, struct sk_buff *skb)
1469 {
1470         struct l2cap_conn *conn = l2cap_pi(sk)->chan->conn;
1471         struct sk_buff **frag;
1472         int err, sent = 0;
1473
1474         if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count))
1475                 return -EFAULT;
1476
1477         sent += count;
1478         len  -= count;
1479
1480         /* Continuation fragments (no L2CAP header) */
1481         frag = &skb_shinfo(skb)->frag_list;
1482         while (len) {
1483                 count = min_t(unsigned int, conn->mtu, len);
1484
1485                 *frag = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err);
1486                 if (!*frag)
1487                         return err;
1488                 if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count))
1489                         return -EFAULT;
1490
1491                 (*frag)->priority = skb->priority;
1492
1493                 sent += count;
1494                 len  -= count;
1495
1496                 frag = &(*frag)->next;
1497         }
1498
1499         return sent;
1500 }
1501
1502 static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan,
1503                                                 struct msghdr *msg, size_t len,
1504                                                 u32 priority)
1505 {
1506         struct sock *sk = chan->sk;
1507         struct l2cap_conn *conn = chan->conn;
1508         struct sk_buff *skb;
1509         int err, count, hlen = L2CAP_HDR_SIZE + L2CAP_PSMLEN_SIZE;
1510         struct l2cap_hdr *lh;
1511
1512         BT_DBG("sk %p len %d priority %u", sk, (int)len, priority);
1513
1514         count = min_t(unsigned int, (conn->mtu - hlen), len);
1515         skb = bt_skb_send_alloc(sk, count + hlen,
1516                         msg->msg_flags & MSG_DONTWAIT, &err);
1517         if (!skb)
1518                 return ERR_PTR(err);
1519
1520         skb->priority = priority;
1521
1522         /* Create L2CAP header */
1523         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1524         lh->cid = cpu_to_le16(chan->dcid);
1525         lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1526         put_unaligned_le16(chan->psm, skb_put(skb, 2));
1527
1528         err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1529         if (unlikely(err < 0)) {
1530                 kfree_skb(skb);
1531                 return ERR_PTR(err);
1532         }
1533         return skb;
1534 }
1535
1536 static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan,
1537                                                 struct msghdr *msg, size_t len,
1538                                                 u32 priority)
1539 {
1540         struct sock *sk = chan->sk;
1541         struct l2cap_conn *conn = chan->conn;
1542         struct sk_buff *skb;
1543         int err, count, hlen = L2CAP_HDR_SIZE;
1544         struct l2cap_hdr *lh;
1545
1546         BT_DBG("sk %p len %d", sk, (int)len);
1547
1548         count = min_t(unsigned int, (conn->mtu - hlen), len);
1549         skb = bt_skb_send_alloc(sk, count + hlen,
1550                         msg->msg_flags & MSG_DONTWAIT, &err);
1551         if (!skb)
1552                 return ERR_PTR(err);
1553
1554         skb->priority = priority;
1555
1556         /* Create L2CAP header */
1557         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1558         lh->cid = cpu_to_le16(chan->dcid);
1559         lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1560
1561         err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1562         if (unlikely(err < 0)) {
1563                 kfree_skb(skb);
1564                 return ERR_PTR(err);
1565         }
1566         return skb;
1567 }
1568
1569 static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan,
1570                                                 struct msghdr *msg, size_t len,
1571                                                 u32 control, u16 sdulen)
1572 {
1573         struct sock *sk = chan->sk;
1574         struct l2cap_conn *conn = chan->conn;
1575         struct sk_buff *skb;
1576         int err, count, hlen;
1577         struct l2cap_hdr *lh;
1578
1579         BT_DBG("sk %p len %d", sk, (int)len);
1580
1581         if (!conn)
1582                 return ERR_PTR(-ENOTCONN);
1583
1584         if (test_bit(FLAG_EXT_CTRL, &chan->flags))
1585                 hlen = L2CAP_EXT_HDR_SIZE;
1586         else
1587                 hlen = L2CAP_ENH_HDR_SIZE;
1588
1589         if (sdulen)
1590                 hlen += L2CAP_SDULEN_SIZE;
1591
1592         if (chan->fcs == L2CAP_FCS_CRC16)
1593                 hlen += L2CAP_FCS_SIZE;
1594
1595         count = min_t(unsigned int, (conn->mtu - hlen), len);
1596         skb = bt_skb_send_alloc(sk, count + hlen,
1597                         msg->msg_flags & MSG_DONTWAIT, &err);
1598         if (!skb)
1599                 return ERR_PTR(err);
1600
1601         /* Create L2CAP header */
1602         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1603         lh->cid = cpu_to_le16(chan->dcid);
1604         lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1605
1606         __put_control(chan, control, skb_put(skb, __ctrl_size(chan)));
1607
1608         if (sdulen)
1609                 put_unaligned_le16(sdulen, skb_put(skb, L2CAP_SDULEN_SIZE));
1610
1611         err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1612         if (unlikely(err < 0)) {
1613                 kfree_skb(skb);
1614                 return ERR_PTR(err);
1615         }
1616
1617         if (chan->fcs == L2CAP_FCS_CRC16)
1618                 put_unaligned_le16(0, skb_put(skb, L2CAP_FCS_SIZE));
1619
1620         bt_cb(skb)->retries = 0;
1621         return skb;
1622 }
1623
1624 static int l2cap_sar_segment_sdu(struct l2cap_chan *chan, struct msghdr *msg, size_t len)
1625 {
1626         struct sk_buff *skb;
1627         struct sk_buff_head sar_queue;
1628         u32 control;
1629         size_t size = 0;
1630
1631         skb_queue_head_init(&sar_queue);
1632         control = __set_ctrl_sar(chan, L2CAP_SAR_START);
1633         skb = l2cap_create_iframe_pdu(chan, msg, chan->remote_mps, control, len);
1634         if (IS_ERR(skb))
1635                 return PTR_ERR(skb);
1636
1637         __skb_queue_tail(&sar_queue, skb);
1638         len -= chan->remote_mps;
1639         size += chan->remote_mps;
1640
1641         while (len > 0) {
1642                 size_t buflen;
1643
1644                 if (len > chan->remote_mps) {
1645                         control = __set_ctrl_sar(chan, L2CAP_SAR_CONTINUE);
1646                         buflen = chan->remote_mps;
1647                 } else {
1648                         control = __set_ctrl_sar(chan, L2CAP_SAR_END);
1649                         buflen = len;
1650                 }
1651
1652                 skb = l2cap_create_iframe_pdu(chan, msg, buflen, control, 0);
1653                 if (IS_ERR(skb)) {
1654                         skb_queue_purge(&sar_queue);
1655                         return PTR_ERR(skb);
1656                 }
1657
1658                 __skb_queue_tail(&sar_queue, skb);
1659                 len -= buflen;
1660                 size += buflen;
1661         }
1662         skb_queue_splice_tail(&sar_queue, &chan->tx_q);
1663         if (chan->tx_send_head == NULL)
1664                 chan->tx_send_head = sar_queue.next;
1665
1666         return size;
1667 }
1668
1669 int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
1670                                                                 u32 priority)
1671 {
1672         struct sk_buff *skb;
1673         u32 control;
1674         int err;
1675
1676         /* Connectionless channel */
1677         if (chan->chan_type == L2CAP_CHAN_CONN_LESS) {
1678                 skb = l2cap_create_connless_pdu(chan, msg, len, priority);
1679                 if (IS_ERR(skb))
1680                         return PTR_ERR(skb);
1681
1682                 l2cap_do_send(chan, skb);
1683                 return len;
1684         }
1685
1686         switch (chan->mode) {
1687         case L2CAP_MODE_BASIC:
1688                 /* Check outgoing MTU */
1689                 if (len > chan->omtu)
1690                         return -EMSGSIZE;
1691
1692                 /* Create a basic PDU */
1693                 skb = l2cap_create_basic_pdu(chan, msg, len, priority);
1694                 if (IS_ERR(skb))
1695                         return PTR_ERR(skb);
1696
1697                 l2cap_do_send(chan, skb);
1698                 err = len;
1699                 break;
1700
1701         case L2CAP_MODE_ERTM:
1702         case L2CAP_MODE_STREAMING:
1703                 /* Entire SDU fits into one PDU */
1704                 if (len <= chan->remote_mps) {
1705                         control = __set_ctrl_sar(chan, L2CAP_SAR_UNSEGMENTED);
1706                         skb = l2cap_create_iframe_pdu(chan, msg, len, control,
1707                                                                         0);
1708                         if (IS_ERR(skb))
1709                                 return PTR_ERR(skb);
1710
1711                         __skb_queue_tail(&chan->tx_q, skb);
1712
1713                         if (chan->tx_send_head == NULL)
1714                                 chan->tx_send_head = skb;
1715
1716                 } else {
1717                         /* Segment SDU into multiples PDUs */
1718                         err = l2cap_sar_segment_sdu(chan, msg, len);
1719                         if (err < 0)
1720                                 return err;
1721                 }
1722
1723                 if (chan->mode == L2CAP_MODE_STREAMING) {
1724                         l2cap_streaming_send(chan);
1725                         err = len;
1726                         break;
1727                 }
1728
1729                 if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
1730                                 test_bit(CONN_WAIT_F, &chan->conn_state)) {
1731                         err = len;
1732                         break;
1733                 }
1734
1735                 err = l2cap_ertm_send(chan);
1736                 if (err >= 0)
1737                         err = len;
1738
1739                 break;
1740
1741         default:
1742                 BT_DBG("bad state %1.1x", chan->mode);
1743                 err = -EBADFD;
1744         }
1745
1746         return err;
1747 }
1748
1749 /* Copy frame to all raw sockets on that connection */
1750 static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
1751 {
1752         struct sk_buff *nskb;
1753         struct l2cap_chan *chan;
1754
1755         BT_DBG("conn %p", conn);
1756
1757         read_lock(&conn->chan_lock);
1758         list_for_each_entry(chan, &conn->chan_l, list) {
1759                 struct sock *sk = chan->sk;
1760                 if (chan->chan_type != L2CAP_CHAN_RAW)
1761                         continue;
1762
1763                 /* Don't send frame to the socket it came from */
1764                 if (skb->sk == sk)
1765                         continue;
1766                 nskb = skb_clone(skb, GFP_ATOMIC);
1767                 if (!nskb)
1768                         continue;
1769
1770                 if (chan->ops->recv(chan->data, nskb))
1771                         kfree_skb(nskb);
1772         }
1773         read_unlock(&conn->chan_lock);
1774 }
1775
1776 /* ---- L2CAP signalling commands ---- */
1777 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
1778                                 u8 code, u8 ident, u16 dlen, void *data)
1779 {
1780         struct sk_buff *skb, **frag;
1781         struct l2cap_cmd_hdr *cmd;
1782         struct l2cap_hdr *lh;
1783         int len, count;
1784
1785         BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %d",
1786                         conn, code, ident, dlen);
1787
1788         len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen;
1789         count = min_t(unsigned int, conn->mtu, len);
1790
1791         skb = bt_skb_alloc(count, GFP_ATOMIC);
1792         if (!skb)
1793                 return NULL;
1794
1795         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1796         lh->len = cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen);
1797
1798         if (conn->hcon->type == LE_LINK)
1799                 lh->cid = cpu_to_le16(L2CAP_CID_LE_SIGNALING);
1800         else
1801                 lh->cid = cpu_to_le16(L2CAP_CID_SIGNALING);
1802
1803         cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE);
1804         cmd->code  = code;
1805         cmd->ident = ident;
1806         cmd->len   = cpu_to_le16(dlen);
1807
1808         if (dlen) {
1809                 count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE;
1810                 memcpy(skb_put(skb, count), data, count);
1811                 data += count;
1812         }
1813
1814         len -= skb->len;
1815
1816         /* Continuation fragments (no L2CAP header) */
1817         frag = &skb_shinfo(skb)->frag_list;
1818         while (len) {
1819                 count = min_t(unsigned int, conn->mtu, len);
1820
1821                 *frag = bt_skb_alloc(count, GFP_ATOMIC);
1822                 if (!*frag)
1823                         goto fail;
1824
1825                 memcpy(skb_put(*frag, count), data, count);
1826
1827                 len  -= count;
1828                 data += count;
1829
1830                 frag = &(*frag)->next;
1831         }
1832
1833         return skb;
1834
1835 fail:
1836         kfree_skb(skb);
1837         return NULL;
1838 }
1839
1840 static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val)
1841 {
1842         struct l2cap_conf_opt *opt = *ptr;
1843         int len;
1844
1845         len = L2CAP_CONF_OPT_SIZE + opt->len;
1846         *ptr += len;
1847
1848         *type = opt->type;
1849         *olen = opt->len;
1850
1851         switch (opt->len) {
1852         case 1:
1853                 *val = *((u8 *) opt->val);
1854                 break;
1855
1856         case 2:
1857                 *val = get_unaligned_le16(opt->val);
1858                 break;
1859
1860         case 4:
1861                 *val = get_unaligned_le32(opt->val);
1862                 break;
1863
1864         default:
1865                 *val = (unsigned long) opt->val;
1866                 break;
1867         }
1868
1869         BT_DBG("type 0x%2.2x len %d val 0x%lx", *type, opt->len, *val);
1870         return len;
1871 }
1872
1873 static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val)
1874 {
1875         struct l2cap_conf_opt *opt = *ptr;
1876
1877         BT_DBG("type 0x%2.2x len %d val 0x%lx", type, len, val);
1878
1879         opt->type = type;
1880         opt->len  = len;
1881
1882         switch (len) {
1883         case 1:
1884                 *((u8 *) opt->val)  = val;
1885                 break;
1886
1887         case 2:
1888                 put_unaligned_le16(val, opt->val);
1889                 break;
1890
1891         case 4:
1892                 put_unaligned_le32(val, opt->val);
1893                 break;
1894
1895         default:
1896                 memcpy(opt->val, (void *) val, len);
1897                 break;
1898         }
1899
1900         *ptr += L2CAP_CONF_OPT_SIZE + len;
1901 }
1902
1903 static void l2cap_add_opt_efs(void **ptr, struct l2cap_chan *chan)
1904 {
1905         struct l2cap_conf_efs efs;
1906
1907         switch(chan->mode) {
1908         case L2CAP_MODE_ERTM:
1909                 efs.id          = chan->local_id;
1910                 efs.stype       = chan->local_stype;
1911                 efs.msdu        = cpu_to_le16(chan->local_msdu);
1912                 efs.sdu_itime   = cpu_to_le32(chan->local_sdu_itime);
1913                 efs.acc_lat     = cpu_to_le32(L2CAP_DEFAULT_ACC_LAT);
1914                 efs.flush_to    = cpu_to_le32(L2CAP_DEFAULT_FLUSH_TO);
1915                 break;
1916
1917         case L2CAP_MODE_STREAMING:
1918                 efs.id          = 1;
1919                 efs.stype       = L2CAP_SERV_BESTEFFORT;
1920                 efs.msdu        = cpu_to_le16(chan->local_msdu);
1921                 efs.sdu_itime   = cpu_to_le32(chan->local_sdu_itime);
1922                 efs.acc_lat     = 0;
1923                 efs.flush_to    = 0;
1924                 break;
1925
1926         default:
1927                 return;
1928         }
1929
1930         l2cap_add_conf_opt(ptr, L2CAP_CONF_EFS, sizeof(efs),
1931                                                         (unsigned long) &efs);
1932 }
1933
1934 static void l2cap_ack_timeout(unsigned long arg)
1935 {
1936         struct l2cap_chan *chan = (void *) arg;
1937
1938         bh_lock_sock(chan->sk);
1939         l2cap_send_ack(chan);
1940         bh_unlock_sock(chan->sk);
1941 }
1942
1943 static inline void l2cap_ertm_init(struct l2cap_chan *chan)
1944 {
1945         struct sock *sk = chan->sk;
1946
1947         chan->expected_ack_seq = 0;
1948         chan->unacked_frames = 0;
1949         chan->buffer_seq = 0;
1950         chan->num_acked = 0;
1951         chan->frames_sent = 0;
1952
1953         setup_timer(&chan->retrans_timer, l2cap_retrans_timeout,
1954                                                         (unsigned long) chan);
1955         setup_timer(&chan->monitor_timer, l2cap_monitor_timeout,
1956                                                         (unsigned long) chan);
1957         setup_timer(&chan->ack_timer, l2cap_ack_timeout, (unsigned long) chan);
1958
1959         skb_queue_head_init(&chan->srej_q);
1960
1961         INIT_LIST_HEAD(&chan->srej_l);
1962
1963
1964         sk->sk_backlog_rcv = l2cap_ertm_data_rcv;
1965 }
1966
1967 static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
1968 {
1969         switch (mode) {
1970         case L2CAP_MODE_STREAMING:
1971         case L2CAP_MODE_ERTM:
1972                 if (l2cap_mode_supported(mode, remote_feat_mask))
1973                         return mode;
1974                 /* fall through */
1975         default:
1976                 return L2CAP_MODE_BASIC;
1977         }
1978 }
1979
1980 static inline bool __l2cap_ews_supported(struct l2cap_chan *chan)
1981 {
1982         return enable_hs && chan->conn->feat_mask & L2CAP_FEAT_EXT_WINDOW;
1983 }
1984
1985 static inline bool __l2cap_efs_supported(struct l2cap_chan *chan)
1986 {
1987         return enable_hs && chan->conn->feat_mask & L2CAP_FEAT_EXT_FLOW;
1988 }
1989
1990 static inline void l2cap_txwin_setup(struct l2cap_chan *chan)
1991 {
1992         if (chan->tx_win > L2CAP_DEFAULT_TX_WINDOW &&
1993                                                 __l2cap_ews_supported(chan)) {
1994                 /* use extended control field */
1995                 set_bit(FLAG_EXT_CTRL, &chan->flags);
1996                 chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
1997         } else {
1998                 chan->tx_win = min_t(u16, chan->tx_win,
1999                                                 L2CAP_DEFAULT_TX_WINDOW);
2000                 chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
2001         }
2002 }
2003
2004 static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data)
2005 {
2006         struct l2cap_conf_req *req = data;
2007         struct l2cap_conf_rfc rfc = { .mode = chan->mode };
2008         void *ptr = req->data;
2009         u16 size;
2010
2011         BT_DBG("chan %p", chan);
2012
2013         if (chan->num_conf_req || chan->num_conf_rsp)
2014                 goto done;
2015
2016         switch (chan->mode) {
2017         case L2CAP_MODE_STREAMING:
2018         case L2CAP_MODE_ERTM:
2019                 if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state))
2020                         break;
2021
2022                 if (__l2cap_efs_supported(chan))
2023                         set_bit(FLAG_EFS_ENABLE, &chan->flags);
2024
2025                 /* fall through */
2026         default:
2027                 chan->mode = l2cap_select_mode(rfc.mode, chan->conn->feat_mask);
2028                 break;
2029         }
2030
2031 done:
2032         if (chan->imtu != L2CAP_DEFAULT_MTU)
2033                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu);
2034
2035         switch (chan->mode) {
2036         case L2CAP_MODE_BASIC:
2037                 if (!(chan->conn->feat_mask & L2CAP_FEAT_ERTM) &&
2038                                 !(chan->conn->feat_mask & L2CAP_FEAT_STREAMING))
2039                         break;
2040
2041                 rfc.mode            = L2CAP_MODE_BASIC;
2042                 rfc.txwin_size      = 0;
2043                 rfc.max_transmit    = 0;
2044                 rfc.retrans_timeout = 0;
2045                 rfc.monitor_timeout = 0;
2046                 rfc.max_pdu_size    = 0;
2047
2048                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
2049                                                         (unsigned long) &rfc);
2050                 break;
2051
2052         case L2CAP_MODE_ERTM:
2053                 rfc.mode            = L2CAP_MODE_ERTM;
2054                 rfc.max_transmit    = chan->max_tx;
2055                 rfc.retrans_timeout = 0;
2056                 rfc.monitor_timeout = 0;
2057
2058                 size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
2059                                                 L2CAP_EXT_HDR_SIZE -
2060                                                 L2CAP_SDULEN_SIZE -
2061                                                 L2CAP_FCS_SIZE);
2062                 rfc.max_pdu_size = cpu_to_le16(size);
2063
2064                 l2cap_txwin_setup(chan);
2065
2066                 rfc.txwin_size = min_t(u16, chan->tx_win,
2067                                                 L2CAP_DEFAULT_TX_WINDOW);
2068
2069                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
2070                                                         (unsigned long) &rfc);
2071
2072                 if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
2073                         l2cap_add_opt_efs(&ptr, chan);
2074
2075                 if (!(chan->conn->feat_mask & L2CAP_FEAT_FCS))
2076                         break;
2077
2078                 if (chan->fcs == L2CAP_FCS_NONE ||
2079                                 test_bit(CONF_NO_FCS_RECV, &chan->conf_state)) {
2080                         chan->fcs = L2CAP_FCS_NONE;
2081                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, chan->fcs);
2082                 }
2083
2084                 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
2085                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
2086                                                                 chan->tx_win);
2087                 break;
2088
2089         case L2CAP_MODE_STREAMING:
2090                 rfc.mode            = L2CAP_MODE_STREAMING;
2091                 rfc.txwin_size      = 0;
2092                 rfc.max_transmit    = 0;
2093                 rfc.retrans_timeout = 0;
2094                 rfc.monitor_timeout = 0;
2095
2096                 size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
2097                                                 L2CAP_EXT_HDR_SIZE -
2098                                                 L2CAP_SDULEN_SIZE -
2099                                                 L2CAP_FCS_SIZE);
2100                 rfc.max_pdu_size = cpu_to_le16(size);
2101
2102                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
2103                                                         (unsigned long) &rfc);
2104
2105                 if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
2106                         l2cap_add_opt_efs(&ptr, chan);
2107
2108                 if (!(chan->conn->feat_mask & L2CAP_FEAT_FCS))
2109                         break;
2110
2111                 if (chan->fcs == L2CAP_FCS_NONE ||
2112                                 test_bit(CONF_NO_FCS_RECV, &chan->conf_state)) {
2113                         chan->fcs = L2CAP_FCS_NONE;
2114                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, chan->fcs);
2115                 }
2116                 break;
2117         }
2118
2119         req->dcid  = cpu_to_le16(chan->dcid);
2120         req->flags = cpu_to_le16(0);
2121
2122         return ptr - data;
2123 }
2124
2125 static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data)
2126 {
2127         struct l2cap_conf_rsp *rsp = data;
2128         void *ptr = rsp->data;
2129         void *req = chan->conf_req;
2130         int len = chan->conf_len;
2131         int type, hint, olen;
2132         unsigned long val;
2133         struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
2134         struct l2cap_conf_efs efs;
2135         u8 remote_efs = 0;
2136         u16 mtu = L2CAP_DEFAULT_MTU;
2137         u16 result = L2CAP_CONF_SUCCESS;
2138         u16 size;
2139
2140         BT_DBG("chan %p", chan);
2141
2142         while (len >= L2CAP_CONF_OPT_SIZE) {
2143                 len -= l2cap_get_conf_opt(&req, &type, &olen, &val);
2144
2145                 hint  = type & L2CAP_CONF_HINT;
2146                 type &= L2CAP_CONF_MASK;
2147
2148                 switch (type) {
2149                 case L2CAP_CONF_MTU:
2150                         mtu = val;
2151                         break;
2152
2153                 case L2CAP_CONF_FLUSH_TO:
2154                         chan->flush_to = val;
2155                         break;
2156
2157                 case L2CAP_CONF_QOS:
2158                         break;
2159
2160                 case L2CAP_CONF_RFC:
2161                         if (olen == sizeof(rfc))
2162                                 memcpy(&rfc, (void *) val, olen);
2163                         break;
2164
2165                 case L2CAP_CONF_FCS:
2166                         if (val == L2CAP_FCS_NONE)
2167                                 set_bit(CONF_NO_FCS_RECV, &chan->conf_state);
2168                         break;
2169
2170                 case L2CAP_CONF_EFS:
2171                         remote_efs = 1;
2172                         if (olen == sizeof(efs))
2173                                 memcpy(&efs, (void *) val, olen);
2174                         break;
2175
2176                 case L2CAP_CONF_EWS:
2177                         if (!enable_hs)
2178                                 return -ECONNREFUSED;
2179
2180                         set_bit(FLAG_EXT_CTRL, &chan->flags);
2181                         set_bit(CONF_EWS_RECV, &chan->conf_state);
2182                         chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
2183                         chan->remote_tx_win = val;
2184                         break;
2185
2186                 default:
2187                         if (hint)
2188                                 break;
2189
2190                         result = L2CAP_CONF_UNKNOWN;
2191                         *((u8 *) ptr++) = type;
2192                         break;
2193                 }
2194         }
2195
2196         if (chan->num_conf_rsp || chan->num_conf_req > 1)
2197                 goto done;
2198
2199         switch (chan->mode) {
2200         case L2CAP_MODE_STREAMING:
2201         case L2CAP_MODE_ERTM:
2202                 if (!test_bit(CONF_STATE2_DEVICE, &chan->conf_state)) {
2203                         chan->mode = l2cap_select_mode(rfc.mode,
2204                                         chan->conn->feat_mask);
2205                         break;
2206                 }
2207
2208                 if (remote_efs) {
2209                         if (__l2cap_efs_supported(chan))
2210                                 set_bit(FLAG_EFS_ENABLE, &chan->flags);
2211                         else
2212                                 return -ECONNREFUSED;
2213                 }
2214
2215                 if (chan->mode != rfc.mode)
2216                         return -ECONNREFUSED;
2217
2218                 break;
2219         }
2220
2221 done:
2222         if (chan->mode != rfc.mode) {
2223                 result = L2CAP_CONF_UNACCEPT;
2224                 rfc.mode = chan->mode;
2225
2226                 if (chan->num_conf_rsp == 1)
2227                         return -ECONNREFUSED;
2228
2229                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2230                                         sizeof(rfc), (unsigned long) &rfc);
2231         }
2232
2233         if (result == L2CAP_CONF_SUCCESS) {
2234                 /* Configure output options and let the other side know
2235                  * which ones we don't like. */
2236
2237                 if (mtu < L2CAP_DEFAULT_MIN_MTU)
2238                         result = L2CAP_CONF_UNACCEPT;
2239                 else {
2240                         chan->omtu = mtu;
2241                         set_bit(CONF_MTU_DONE, &chan->conf_state);
2242                 }
2243                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->omtu);
2244
2245                 if (remote_efs) {
2246                         if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
2247                                         efs.stype != L2CAP_SERV_NOTRAFIC &&
2248                                         efs.stype != chan->local_stype) {
2249
2250                                 result = L2CAP_CONF_UNACCEPT;
2251
2252                                 if (chan->num_conf_req >= 1)
2253                                         return -ECONNREFUSED;
2254
2255                                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
2256                                                         sizeof(efs),
2257                                                         (unsigned long) &efs);
2258                         } else {
2259                                 /* Send PENDING Conf Rsp */
2260                                 result = L2CAP_CONF_PENDING;
2261                                 set_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
2262                         }
2263                 }
2264
2265                 switch (rfc.mode) {
2266                 case L2CAP_MODE_BASIC:
2267                         chan->fcs = L2CAP_FCS_NONE;
2268                         set_bit(CONF_MODE_DONE, &chan->conf_state);
2269                         break;
2270
2271                 case L2CAP_MODE_ERTM:
2272                         if (!test_bit(CONF_EWS_RECV, &chan->conf_state))
2273                                 chan->remote_tx_win = rfc.txwin_size;
2274                         else
2275                                 rfc.txwin_size = L2CAP_DEFAULT_TX_WINDOW;
2276
2277                         chan->remote_max_tx = rfc.max_transmit;
2278
2279                         size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
2280                                                 chan->conn->mtu -
2281                                                 L2CAP_EXT_HDR_SIZE -
2282                                                 L2CAP_SDULEN_SIZE -
2283                                                 L2CAP_FCS_SIZE);
2284                         rfc.max_pdu_size = cpu_to_le16(size);
2285                         chan->remote_mps = size;
2286
2287                         rfc.retrans_timeout =
2288                                 le16_to_cpu(L2CAP_DEFAULT_RETRANS_TO);
2289                         rfc.monitor_timeout =
2290                                 le16_to_cpu(L2CAP_DEFAULT_MONITOR_TO);
2291
2292                         set_bit(CONF_MODE_DONE, &chan->conf_state);
2293
2294                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2295                                         sizeof(rfc), (unsigned long) &rfc);
2296
2297                         if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
2298                                 chan->remote_id = efs.id;
2299                                 chan->remote_stype = efs.stype;
2300                                 chan->remote_msdu = le16_to_cpu(efs.msdu);
2301                                 chan->remote_flush_to =
2302                                                 le32_to_cpu(efs.flush_to);
2303                                 chan->remote_acc_lat =
2304                                                 le32_to_cpu(efs.acc_lat);
2305                                 chan->remote_sdu_itime =
2306                                         le32_to_cpu(efs.sdu_itime);
2307                                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
2308                                         sizeof(efs), (unsigned long) &efs);
2309                         }
2310                         break;
2311
2312                 case L2CAP_MODE_STREAMING:
2313                         size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
2314                                                 chan->conn->mtu -
2315                                                 L2CAP_EXT_HDR_SIZE -
2316                                                 L2CAP_SDULEN_SIZE -
2317                                                 L2CAP_FCS_SIZE);
2318                         rfc.max_pdu_size = cpu_to_le16(size);
2319                         chan->remote_mps = size;
2320
2321                         set_bit(CONF_MODE_DONE, &chan->conf_state);
2322
2323                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2324                                         sizeof(rfc), (unsigned long) &rfc);
2325
2326                         break;
2327
2328                 default:
2329                         result = L2CAP_CONF_UNACCEPT;
2330
2331                         memset(&rfc, 0, sizeof(rfc));
2332                         rfc.mode = chan->mode;
2333                 }
2334
2335                 if (result == L2CAP_CONF_SUCCESS)
2336                         set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
2337         }
2338         rsp->scid   = cpu_to_le16(chan->dcid);
2339         rsp->result = cpu_to_le16(result);
2340         rsp->flags  = cpu_to_le16(0x0000);
2341
2342         return ptr - data;
2343 }
2344
2345 static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, void *data, u16 *result)
2346 {
2347         struct l2cap_conf_req *req = data;
2348         void *ptr = req->data;
2349         int type, olen;
2350         unsigned long val;
2351         struct l2cap_conf_rfc rfc;
2352         struct l2cap_conf_efs efs;
2353
2354         BT_DBG("chan %p, rsp %p, len %d, req %p", chan, rsp, len, data);
2355
2356         while (len >= L2CAP_CONF_OPT_SIZE) {
2357                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
2358
2359                 switch (type) {
2360                 case L2CAP_CONF_MTU:
2361                         if (val < L2CAP_DEFAULT_MIN_MTU) {
2362                                 *result = L2CAP_CONF_UNACCEPT;
2363                                 chan->imtu = L2CAP_DEFAULT_MIN_MTU;
2364                         } else
2365                                 chan->imtu = val;
2366                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu);
2367                         break;
2368
2369                 case L2CAP_CONF_FLUSH_TO:
2370                         chan->flush_to = val;
2371                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
2372                                                         2, chan->flush_to);
2373                         break;
2374
2375                 case L2CAP_CONF_RFC:
2376                         if (olen == sizeof(rfc))
2377                                 memcpy(&rfc, (void *)val, olen);
2378
2379                         if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state) &&
2380                                                         rfc.mode != chan->mode)
2381                                 return -ECONNREFUSED;
2382
2383                         chan->fcs = 0;
2384
2385                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2386                                         sizeof(rfc), (unsigned long) &rfc);
2387                         break;
2388
2389                 case L2CAP_CONF_EWS:
2390                         chan->tx_win = min_t(u16, val,
2391                                                 L2CAP_DEFAULT_EXT_WINDOW);
2392                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
2393                                                         chan->tx_win);
2394                         break;
2395
2396                 case L2CAP_CONF_EFS:
2397                         if (olen == sizeof(efs))
2398                                 memcpy(&efs, (void *)val, olen);
2399
2400                         if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
2401                                         efs.stype != L2CAP_SERV_NOTRAFIC &&
2402                                         efs.stype != chan->local_stype)
2403                                 return -ECONNREFUSED;
2404
2405                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
2406                                         sizeof(efs), (unsigned long) &efs);
2407                         break;
2408                 }
2409         }
2410
2411         if (chan->mode == L2CAP_MODE_BASIC && chan->mode != rfc.mode)
2412                 return -ECONNREFUSED;
2413
2414         chan->mode = rfc.mode;
2415
2416         if (*result == L2CAP_CONF_SUCCESS || *result == L2CAP_CONF_PENDING) {
2417                 switch (rfc.mode) {
2418                 case L2CAP_MODE_ERTM:
2419                         chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
2420                         chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
2421                         chan->mps    = le16_to_cpu(rfc.max_pdu_size);
2422
2423                         if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
2424                                 chan->local_msdu = le16_to_cpu(efs.msdu);
2425                                 chan->local_sdu_itime =
2426                                                 le32_to_cpu(efs.sdu_itime);
2427                                 chan->local_acc_lat = le32_to_cpu(efs.acc_lat);
2428                                 chan->local_flush_to =
2429                                                 le32_to_cpu(efs.flush_to);
2430                         }
2431                         break;
2432
2433                 case L2CAP_MODE_STREAMING:
2434                         chan->mps    = le16_to_cpu(rfc.max_pdu_size);
2435                 }
2436         }
2437
2438         req->dcid   = cpu_to_le16(chan->dcid);
2439         req->flags  = cpu_to_le16(0x0000);
2440
2441         return ptr - data;
2442 }
2443
2444 static int l2cap_build_conf_rsp(struct l2cap_chan *chan, void *data, u16 result, u16 flags)
2445 {
2446         struct l2cap_conf_rsp *rsp = data;
2447         void *ptr = rsp->data;
2448
2449         BT_DBG("chan %p", chan);
2450
2451         rsp->scid   = cpu_to_le16(chan->dcid);
2452         rsp->result = cpu_to_le16(result);
2453         rsp->flags  = cpu_to_le16(flags);
2454
2455         return ptr - data;
2456 }
2457
2458 void __l2cap_connect_rsp_defer(struct l2cap_chan *chan)
2459 {
2460         struct l2cap_conn_rsp rsp;
2461         struct l2cap_conn *conn = chan->conn;
2462         u8 buf[128];
2463
2464         rsp.scid   = cpu_to_le16(chan->dcid);
2465         rsp.dcid   = cpu_to_le16(chan->scid);
2466         rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
2467         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
2468         l2cap_send_cmd(conn, chan->ident,
2469                                 L2CAP_CONN_RSP, sizeof(rsp), &rsp);
2470
2471         if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
2472                 return;
2473
2474         l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2475                         l2cap_build_conf_req(chan, buf), buf);
2476         chan->num_conf_req++;
2477 }
2478
2479 static void l2cap_conf_rfc_get(struct l2cap_chan *chan, void *rsp, int len)
2480 {
2481         int type, olen;
2482         unsigned long val;
2483         struct l2cap_conf_rfc rfc;
2484
2485         BT_DBG("chan %p, rsp %p, len %d", chan, rsp, len);
2486
2487         if ((chan->mode != L2CAP_MODE_ERTM) && (chan->mode != L2CAP_MODE_STREAMING))
2488                 return;
2489
2490         while (len >= L2CAP_CONF_OPT_SIZE) {
2491                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
2492
2493                 switch (type) {
2494                 case L2CAP_CONF_RFC:
2495                         if (olen == sizeof(rfc))
2496                                 memcpy(&rfc, (void *)val, olen);
2497                         goto done;
2498                 }
2499         }
2500
2501 done:
2502         switch (rfc.mode) {
2503         case L2CAP_MODE_ERTM:
2504                 chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
2505                 chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
2506                 chan->mps    = le16_to_cpu(rfc.max_pdu_size);
2507                 break;
2508         case L2CAP_MODE_STREAMING:
2509                 chan->mps    = le16_to_cpu(rfc.max_pdu_size);
2510         }
2511 }
2512
2513 static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2514 {
2515         struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data;
2516
2517         if (rej->reason != L2CAP_REJ_NOT_UNDERSTOOD)
2518                 return 0;
2519
2520         if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
2521                                         cmd->ident == conn->info_ident) {
2522                 del_timer(&conn->info_timer);
2523
2524                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2525                 conn->info_ident = 0;
2526
2527                 l2cap_conn_start(conn);
2528         }
2529
2530         return 0;
2531 }
2532
2533 static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2534 {
2535         struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
2536         struct l2cap_conn_rsp rsp;
2537         struct l2cap_chan *chan = NULL, *pchan;
2538         struct sock *parent, *sk = NULL;
2539         int result, status = L2CAP_CS_NO_INFO;
2540
2541         u16 dcid = 0, scid = __le16_to_cpu(req->scid);
2542         __le16 psm = req->psm;
2543
2544         BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid);
2545
2546         /* Check if we have socket listening on psm */
2547         pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, conn->src);
2548         if (!pchan) {
2549                 result = L2CAP_CR_BAD_PSM;
2550                 goto sendresp;
2551         }
2552
2553         parent = pchan->sk;
2554
2555         bh_lock_sock(parent);
2556
2557         /* Check if the ACL is secure enough (if not SDP) */
2558         if (psm != cpu_to_le16(0x0001) &&
2559                                 !hci_conn_check_link_mode(conn->hcon)) {
2560                 conn->disc_reason = HCI_ERROR_AUTH_FAILURE;
2561                 result = L2CAP_CR_SEC_BLOCK;
2562                 goto response;
2563         }
2564
2565         result = L2CAP_CR_NO_MEM;
2566
2567         /* Check for backlog size */
2568         if (sk_acceptq_is_full(parent)) {
2569                 BT_DBG("backlog full %d", parent->sk_ack_backlog);
2570                 goto response;
2571         }
2572
2573         chan = pchan->ops->new_connection(pchan->data);
2574         if (!chan)
2575                 goto response;
2576
2577         sk = chan->sk;
2578
2579         write_lock_bh(&conn->chan_lock);
2580
2581         /* Check if we already have channel with that dcid */
2582         if (__l2cap_get_chan_by_dcid(conn, scid)) {
2583                 write_unlock_bh(&conn->chan_lock);
2584                 sock_set_flag(sk, SOCK_ZAPPED);
2585                 chan->ops->close(chan->data);
2586                 goto response;
2587         }
2588
2589         hci_conn_hold(conn->hcon);
2590
2591         bacpy(&bt_sk(sk)->src, conn->src);
2592         bacpy(&bt_sk(sk)->dst, conn->dst);
2593         chan->psm  = psm;
2594         chan->dcid = scid;
2595
2596         bt_accept_enqueue(parent, sk);
2597
2598         __l2cap_chan_add(conn, chan);
2599
2600         dcid = chan->scid;
2601
2602         __set_chan_timer(chan, sk->sk_sndtimeo);
2603
2604         chan->ident = cmd->ident;
2605
2606         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
2607                 if (l2cap_chan_check_security(chan)) {
2608                         if (bt_sk(sk)->defer_setup) {
2609                                 l2cap_state_change(chan, BT_CONNECT2);
2610                                 result = L2CAP_CR_PEND;
2611                                 status = L2CAP_CS_AUTHOR_PEND;
2612                                 parent->sk_data_ready(parent, 0);
2613                         } else {
2614                                 l2cap_state_change(chan, BT_CONFIG);
2615                                 result = L2CAP_CR_SUCCESS;
2616                                 status = L2CAP_CS_NO_INFO;
2617                         }
2618                 } else {
2619                         l2cap_state_change(chan, BT_CONNECT2);
2620                         result = L2CAP_CR_PEND;
2621                         status = L2CAP_CS_AUTHEN_PEND;
2622                 }
2623         } else {
2624                 l2cap_state_change(chan, BT_CONNECT2);
2625                 result = L2CAP_CR_PEND;
2626                 status = L2CAP_CS_NO_INFO;
2627         }
2628
2629         write_unlock_bh(&conn->chan_lock);
2630
2631 response:
2632         bh_unlock_sock(parent);
2633
2634 sendresp:
2635         rsp.scid   = cpu_to_le16(scid);
2636         rsp.dcid   = cpu_to_le16(dcid);
2637         rsp.result = cpu_to_le16(result);
2638         rsp.status = cpu_to_le16(status);
2639         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
2640
2641         if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
2642                 struct l2cap_info_req info;
2643                 info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
2644
2645                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
2646                 conn->info_ident = l2cap_get_ident(conn);
2647
2648                 mod_timer(&conn->info_timer, jiffies +
2649                                         msecs_to_jiffies(L2CAP_INFO_TIMEOUT));
2650
2651                 l2cap_send_cmd(conn, conn->info_ident,
2652                                         L2CAP_INFO_REQ, sizeof(info), &info);
2653         }
2654
2655         if (chan && !test_bit(CONF_REQ_SENT, &chan->conf_state) &&
2656                                 result == L2CAP_CR_SUCCESS) {
2657                 u8 buf[128];
2658                 set_bit(CONF_REQ_SENT, &chan->conf_state);
2659                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2660                                         l2cap_build_conf_req(chan, buf), buf);
2661                 chan->num_conf_req++;
2662         }
2663
2664         return 0;
2665 }
2666
2667 static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2668 {
2669         struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
2670         u16 scid, dcid, result, status;
2671         struct l2cap_chan *chan;
2672         struct sock *sk;
2673         u8 req[128];
2674
2675         scid   = __le16_to_cpu(rsp->scid);
2676         dcid   = __le16_to_cpu(rsp->dcid);
2677         result = __le16_to_cpu(rsp->result);
2678         status = __le16_to_cpu(rsp->status);
2679
2680         BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status);
2681
2682         if (scid) {
2683                 chan = l2cap_get_chan_by_scid(conn, scid);
2684                 if (!chan)
2685                         return -EFAULT;
2686         } else {
2687                 chan = l2cap_get_chan_by_ident(conn, cmd->ident);
2688                 if (!chan)
2689                         return -EFAULT;
2690         }
2691
2692         sk = chan->sk;
2693
2694         switch (result) {
2695         case L2CAP_CR_SUCCESS:
2696                 l2cap_state_change(chan, BT_CONFIG);
2697                 chan->ident = 0;
2698                 chan->dcid = dcid;
2699                 clear_bit(CONF_CONNECT_PEND, &chan->conf_state);
2700
2701                 if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
2702                         break;
2703
2704                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2705                                         l2cap_build_conf_req(chan, req), req);
2706                 chan->num_conf_req++;
2707                 break;
2708
2709         case L2CAP_CR_PEND:
2710                 set_bit(CONF_CONNECT_PEND, &chan->conf_state);
2711                 break;
2712
2713         default:
2714                 /* don't delete l2cap channel if sk is owned by user */
2715                 if (sock_owned_by_user(sk)) {
2716                         l2cap_state_change(chan, BT_DISCONN);
2717                         __clear_chan_timer(chan);
2718                         __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
2719                         break;
2720                 }
2721
2722                 l2cap_chan_del(chan, ECONNREFUSED);
2723                 break;
2724         }
2725
2726         bh_unlock_sock(sk);
2727         return 0;
2728 }
2729
2730 static inline void set_default_fcs(struct l2cap_chan *chan)
2731 {
2732         /* FCS is enabled only in ERTM or streaming mode, if one or both
2733          * sides request it.
2734          */
2735         if (chan->mode != L2CAP_MODE_ERTM && chan->mode != L2CAP_MODE_STREAMING)
2736                 chan->fcs = L2CAP_FCS_NONE;
2737         else if (!test_bit(CONF_NO_FCS_RECV, &chan->conf_state))
2738                 chan->fcs = L2CAP_FCS_CRC16;
2739 }
2740
2741 static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
2742 {
2743         struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
2744         u16 dcid, flags;
2745         u8 rsp[64];
2746         struct l2cap_chan *chan;
2747         struct sock *sk;
2748         int len;
2749
2750         dcid  = __le16_to_cpu(req->dcid);
2751         flags = __le16_to_cpu(req->flags);
2752
2753         BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
2754
2755         chan = l2cap_get_chan_by_scid(conn, dcid);
2756         if (!chan)
2757                 return -ENOENT;
2758
2759         sk = chan->sk;
2760
2761         if (chan->state != BT_CONFIG && chan->state != BT_CONNECT2) {
2762                 struct l2cap_cmd_rej_cid rej;
2763
2764                 rej.reason = cpu_to_le16(L2CAP_REJ_INVALID_CID);
2765                 rej.scid = cpu_to_le16(chan->scid);
2766                 rej.dcid = cpu_to_le16(chan->dcid);
2767
2768                 l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
2769                                 sizeof(rej), &rej);
2770                 goto unlock;
2771         }
2772
2773         /* Reject if config buffer is too small. */
2774         len = cmd_len - sizeof(*req);
2775         if (len < 0 || chan->conf_len + len > sizeof(chan->conf_req)) {
2776                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2777                                 l2cap_build_conf_rsp(chan, rsp,
2778                                         L2CAP_CONF_REJECT, flags), rsp);
2779                 goto unlock;
2780         }
2781
2782         /* Store config. */
2783         memcpy(chan->conf_req + chan->conf_len, req->data, len);
2784         chan->conf_len += len;
2785
2786         if (flags & 0x0001) {
2787                 /* Incomplete config. Send empty response. */
2788                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2789                                 l2cap_build_conf_rsp(chan, rsp,
2790                                         L2CAP_CONF_SUCCESS, 0x0001), rsp);
2791                 goto unlock;
2792         }
2793
2794         /* Complete config. */
2795         len = l2cap_parse_conf_req(chan, rsp);
2796         if (len < 0) {
2797                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
2798                 goto unlock;
2799         }
2800
2801         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
2802         chan->num_conf_rsp++;
2803
2804         /* Reset config buffer. */
2805         chan->conf_len = 0;
2806
2807         if (!test_bit(CONF_OUTPUT_DONE, &chan->conf_state))
2808                 goto unlock;
2809
2810         if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
2811                 set_default_fcs(chan);
2812
2813                 l2cap_state_change(chan, BT_CONNECTED);
2814
2815                 chan->next_tx_seq = 0;
2816                 chan->expected_tx_seq = 0;
2817                 skb_queue_head_init(&chan->tx_q);
2818                 if (chan->mode == L2CAP_MODE_ERTM)
2819                         l2cap_ertm_init(chan);
2820
2821                 l2cap_chan_ready(sk);
2822                 goto unlock;
2823         }
2824
2825         if (!test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) {
2826                 u8 buf[64];
2827                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2828                                         l2cap_build_conf_req(chan, buf), buf);
2829                 chan->num_conf_req++;
2830         }
2831
2832         /* Got Conf Rsp PENDING from remote side and asume we sent
2833            Conf Rsp PENDING in the code above */
2834         if (test_bit(CONF_REM_CONF_PEND, &chan->conf_state) &&
2835                         test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
2836
2837                 /* check compatibility */
2838
2839                 clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
2840                 set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
2841
2842                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2843                                         l2cap_build_conf_rsp(chan, rsp,
2844                                         L2CAP_CONF_SUCCESS, 0x0000), rsp);
2845         }
2846
2847 unlock:
2848         bh_unlock_sock(sk);
2849         return 0;
2850 }
2851
2852 static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2853 {
2854         struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
2855         u16 scid, flags, result;
2856         struct l2cap_chan *chan;
2857         struct sock *sk;
2858         int len = cmd->len - sizeof(*rsp);
2859
2860         scid   = __le16_to_cpu(rsp->scid);
2861         flags  = __le16_to_cpu(rsp->flags);
2862         result = __le16_to_cpu(rsp->result);
2863
2864         BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x",
2865                         scid, flags, result);
2866
2867         chan = l2cap_get_chan_by_scid(conn, scid);
2868         if (!chan)
2869                 return 0;
2870
2871         sk = chan->sk;
2872
2873         switch (result) {
2874         case L2CAP_CONF_SUCCESS:
2875                 l2cap_conf_rfc_get(chan, rsp->data, len);
2876                 clear_bit(CONF_REM_CONF_PEND, &chan->conf_state);
2877                 break;
2878
2879         case L2CAP_CONF_PENDING:
2880                 set_bit(CONF_REM_CONF_PEND, &chan->conf_state);
2881
2882                 if (test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
2883                         char buf[64];
2884
2885                         len = l2cap_parse_conf_rsp(chan, rsp->data, len,
2886                                                                 buf, &result);
2887                         if (len < 0) {
2888                                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
2889                                 goto done;
2890                         }
2891
2892                         /* check compatibility */
2893
2894                         clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
2895                         set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
2896
2897                         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2898                                                 l2cap_build_conf_rsp(chan, buf,
2899                                                 L2CAP_CONF_SUCCESS, 0x0000), buf);
2900                 }
2901                 goto done;
2902
2903         case L2CAP_CONF_UNACCEPT:
2904                 if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
2905                         char req[64];
2906
2907                         if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
2908                                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
2909                                 goto done;
2910                         }
2911
2912                         /* throw out any old stored conf requests */
2913                         result = L2CAP_CONF_SUCCESS;
2914                         len = l2cap_parse_conf_rsp(chan, rsp->data, len,
2915                                                                 req, &result);
2916                         if (len < 0) {
2917                                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
2918                                 goto done;
2919                         }
2920
2921                         l2cap_send_cmd(conn, l2cap_get_ident(conn),
2922                                                 L2CAP_CONF_REQ, len, req);
2923                         chan->num_conf_req++;
2924                         if (result != L2CAP_CONF_SUCCESS)
2925                                 goto done;
2926                         break;
2927                 }
2928
2929         default:
2930                 sk->sk_err = ECONNRESET;
2931                 __set_chan_timer(chan, L2CAP_DISC_REJ_TIMEOUT);
2932                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
2933                 goto done;
2934         }
2935
2936         if (flags & 0x01)
2937                 goto done;
2938
2939         set_bit(CONF_INPUT_DONE, &chan->conf_state);
2940
2941         if (test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) {
2942                 set_default_fcs(chan);
2943
2944                 l2cap_state_change(chan, BT_CONNECTED);
2945                 chan->next_tx_seq = 0;
2946                 chan->expected_tx_seq = 0;
2947                 skb_queue_head_init(&chan->tx_q);
2948                 if (chan->mode ==  L2CAP_MODE_ERTM)
2949                         l2cap_ertm_init(chan);
2950
2951                 l2cap_chan_ready(sk);
2952         }
2953
2954 done:
2955         bh_unlock_sock(sk);
2956         return 0;
2957 }
2958
2959 static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2960 {
2961         struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
2962         struct l2cap_disconn_rsp rsp;
2963         u16 dcid, scid;
2964         struct l2cap_chan *chan;
2965         struct sock *sk;
2966
2967         scid = __le16_to_cpu(req->scid);
2968         dcid = __le16_to_cpu(req->dcid);
2969
2970         BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
2971
2972         chan = l2cap_get_chan_by_scid(conn, dcid);
2973         if (!chan)
2974                 return 0;
2975
2976         sk = chan->sk;
2977
2978         rsp.dcid = cpu_to_le16(chan->scid);
2979         rsp.scid = cpu_to_le16(chan->dcid);
2980         l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
2981
2982         sk->sk_shutdown = SHUTDOWN_MASK;
2983
2984         /* don't delete l2cap channel if sk is owned by user */
2985         if (sock_owned_by_user(sk)) {
2986                 l2cap_state_change(chan, BT_DISCONN);
2987                 __clear_chan_timer(chan);
2988                 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
2989                 bh_unlock_sock(sk);
2990                 return 0;
2991         }
2992
2993         l2cap_chan_del(chan, ECONNRESET);
2994         bh_unlock_sock(sk);
2995
2996         chan->ops->close(chan->data);
2997         return 0;
2998 }
2999
3000 static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3001 {
3002         struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
3003         u16 dcid, scid;
3004         struct l2cap_chan *chan;
3005         struct sock *sk;
3006
3007         scid = __le16_to_cpu(rsp->scid);
3008         dcid = __le16_to_cpu(rsp->dcid);
3009
3010         BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
3011
3012         chan = l2cap_get_chan_by_scid(conn, scid);
3013         if (!chan)
3014                 return 0;
3015
3016         sk = chan->sk;
3017
3018         /* don't delete l2cap channel if sk is owned by user */
3019         if (sock_owned_by_user(sk)) {
3020                 l2cap_state_change(chan,BT_DISCONN);
3021                 __clear_chan_timer(chan);
3022                 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
3023                 bh_unlock_sock(sk);
3024                 return 0;
3025         }
3026
3027         l2cap_chan_del(chan, 0);
3028         bh_unlock_sock(sk);
3029
3030         chan->ops->close(chan->data);
3031         return 0;
3032 }
3033
3034 static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3035 {
3036         struct l2cap_info_req *req = (struct l2cap_info_req *) data;
3037         u16 type;
3038
3039         type = __le16_to_cpu(req->type);
3040
3041         BT_DBG("type 0x%4.4x", type);
3042
3043         if (type == L2CAP_IT_FEAT_MASK) {
3044                 u8 buf[8];
3045                 u32 feat_mask = l2cap_feat_mask;
3046                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
3047                 rsp->type   = cpu_to_le16(L2CAP_IT_FEAT_MASK);
3048                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
3049                 if (!disable_ertm)
3050                         feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
3051                                                          | L2CAP_FEAT_FCS;
3052                 if (enable_hs)
3053                         feat_mask |= L2CAP_FEAT_EXT_FLOW
3054                                                 | L2CAP_FEAT_EXT_WINDOW;
3055
3056                 put_unaligned_le32(feat_mask, rsp->data);
3057                 l2cap_send_cmd(conn, cmd->ident,
3058                                         L2CAP_INFO_RSP, sizeof(buf), buf);
3059         } else if (type == L2CAP_IT_FIXED_CHAN) {
3060                 u8 buf[12];
3061                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
3062
3063                 if (enable_hs)
3064                         l2cap_fixed_chan[0] |= L2CAP_FC_A2MP;
3065                 else
3066                         l2cap_fixed_chan[0] &= ~L2CAP_FC_A2MP;
3067
3068                 rsp->type   = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
3069                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
3070                 memcpy(rsp->data, l2cap_fixed_chan, sizeof(l2cap_fixed_chan));
3071                 l2cap_send_cmd(conn, cmd->ident,
3072                                         L2CAP_INFO_RSP, sizeof(buf), buf);
3073         } else {
3074                 struct l2cap_info_rsp rsp;
3075                 rsp.type   = cpu_to_le16(type);
3076                 rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP);
3077                 l2cap_send_cmd(conn, cmd->ident,
3078                                         L2CAP_INFO_RSP, sizeof(rsp), &rsp);
3079         }
3080
3081         return 0;
3082 }
3083
3084 static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3085 {
3086         struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
3087         u16 type, result;
3088
3089         type   = __le16_to_cpu(rsp->type);
3090         result = __le16_to_cpu(rsp->result);
3091
3092         BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
3093
3094         /* L2CAP Info req/rsp are unbound to channels, add extra checks */
3095         if (cmd->ident != conn->info_ident ||
3096                         conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)
3097                 return 0;
3098
3099         del_timer(&conn->info_timer);
3100
3101         if (result != L2CAP_IR_SUCCESS) {
3102                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3103                 conn->info_ident = 0;
3104
3105                 l2cap_conn_start(conn);
3106
3107                 return 0;
3108         }
3109
3110         if (type == L2CAP_IT_FEAT_MASK) {
3111                 conn->feat_mask = get_unaligned_le32(rsp->data);
3112
3113                 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
3114                         struct l2cap_info_req req;
3115                         req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
3116
3117                         conn->info_ident = l2cap_get_ident(conn);
3118
3119                         l2cap_send_cmd(conn, conn->info_ident,
3120                                         L2CAP_INFO_REQ, sizeof(req), &req);
3121                 } else {
3122                         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3123                         conn->info_ident = 0;
3124
3125                         l2cap_conn_start(conn);
3126                 }
3127         } else if (type == L2CAP_IT_FIXED_CHAN) {
3128                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3129                 conn->info_ident = 0;
3130
3131                 l2cap_conn_start(conn);
3132         }
3133
3134         return 0;
3135 }
3136
3137 static inline int l2cap_create_channel_req(struct l2cap_conn *conn,
3138                                         struct l2cap_cmd_hdr *cmd, u16 cmd_len,
3139                                         void *data)
3140 {
3141         struct l2cap_create_chan_req *req = data;
3142         struct l2cap_create_chan_rsp rsp;
3143         u16 psm, scid;
3144
3145         if (cmd_len != sizeof(*req))
3146                 return -EPROTO;
3147
3148         if (!enable_hs)
3149                 return -EINVAL;
3150
3151         psm = le16_to_cpu(req->psm);
3152         scid = le16_to_cpu(req->scid);
3153
3154         BT_DBG("psm %d, scid %d, amp_id %d", psm, scid, req->amp_id);
3155
3156         /* Placeholder: Always reject */
3157         rsp.dcid = 0;
3158         rsp.scid = cpu_to_le16(scid);
3159         rsp.result = L2CAP_CR_NO_MEM;
3160         rsp.status = L2CAP_CS_NO_INFO;
3161
3162         l2cap_send_cmd(conn, cmd->ident, L2CAP_CREATE_CHAN_RSP,
3163                        sizeof(rsp), &rsp);
3164
3165         return 0;
3166 }
3167
3168 static inline int l2cap_create_channel_rsp(struct l2cap_conn *conn,
3169                                         struct l2cap_cmd_hdr *cmd, void *data)
3170 {
3171         BT_DBG("conn %p", conn);
3172
3173         return l2cap_connect_rsp(conn, cmd, data);
3174 }
3175
3176 static void l2cap_send_move_chan_rsp(struct l2cap_conn *conn, u8 ident,
3177                                                         u16 icid, u16 result)
3178 {
3179         struct l2cap_move_chan_rsp rsp;
3180
3181         BT_DBG("icid %d, result %d", icid, result);
3182
3183         rsp.icid = cpu_to_le16(icid);
3184         rsp.result = cpu_to_le16(result);
3185
3186         l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_RSP, sizeof(rsp), &rsp);
3187 }
3188
3189 static void l2cap_send_move_chan_cfm(struct l2cap_conn *conn,
3190                                 struct l2cap_chan *chan, u16 icid, u16 result)
3191 {
3192         struct l2cap_move_chan_cfm cfm;
3193         u8 ident;
3194
3195         BT_DBG("icid %d, result %d", icid, result);
3196
3197         ident = l2cap_get_ident(conn);
3198         if (chan)
3199                 chan->ident = ident;
3200
3201         cfm.icid = cpu_to_le16(icid);
3202         cfm.result = cpu_to_le16(result);
3203
3204         l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM, sizeof(cfm), &cfm);
3205 }
3206
3207 static void l2cap_send_move_chan_cfm_rsp(struct l2cap_conn *conn, u8 ident,
3208                                                                 u16 icid)
3209 {
3210         struct l2cap_move_chan_cfm_rsp rsp;
3211
3212         BT_DBG("icid %d", icid);
3213
3214         rsp.icid = cpu_to_le16(icid);
3215         l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM_RSP, sizeof(rsp), &rsp);
3216 }
3217
3218 static inline int l2cap_move_channel_req(struct l2cap_conn *conn,
3219                         struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data)
3220 {
3221         struct l2cap_move_chan_req *req = data;
3222         u16 icid = 0;
3223         u16 result = L2CAP_MR_NOT_ALLOWED;
3224
3225         if (cmd_len != sizeof(*req))
3226                 return -EPROTO;
3227
3228         icid = le16_to_cpu(req->icid);
3229
3230         BT_DBG("icid %d, dest_amp_id %d", icid, req->dest_amp_id);
3231
3232         if (!enable_hs)
3233                 return -EINVAL;
3234
3235         /* Placeholder: Always refuse */
3236         l2cap_send_move_chan_rsp(conn, cmd->ident, icid, result);
3237
3238         return 0;
3239 }
3240
3241 static inline int l2cap_move_channel_rsp(struct l2cap_conn *conn,
3242                         struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data)
3243 {
3244         struct l2cap_move_chan_rsp *rsp = data;
3245         u16 icid, result;
3246
3247         if (cmd_len != sizeof(*rsp))
3248                 return -EPROTO;
3249
3250         icid = le16_to_cpu(rsp->icid);
3251         result = le16_to_cpu(rsp->result);
3252
3253         BT_DBG("icid %d, result %d", icid, result);
3254
3255         /* Placeholder: Always unconfirmed */
3256         l2cap_send_move_chan_cfm(conn, NULL, icid, L2CAP_MC_UNCONFIRMED);
3257
3258         return 0;
3259 }
3260
3261 static inline int l2cap_move_channel_confirm(struct l2cap_conn *conn,
3262                         struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data)
3263 {
3264         struct l2cap_move_chan_cfm *cfm = data;
3265         u16 icid, result;
3266
3267         if (cmd_len != sizeof(*cfm))
3268                 return -EPROTO;
3269
3270         icid = le16_to_cpu(cfm->icid);
3271         result = le16_to_cpu(cfm->result);
3272
3273         BT_DBG("icid %d, result %d", icid, result);
3274
3275         l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid);
3276
3277         return 0;
3278 }
3279
3280 static inline int l2cap_move_channel_confirm_rsp(struct l2cap_conn *conn,
3281                         struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data)
3282 {
3283         struct l2cap_move_chan_cfm_rsp *rsp = data;
3284         u16 icid;
3285
3286         if (cmd_len != sizeof(*rsp))
3287                 return -EPROTO;
3288
3289         icid = le16_to_cpu(rsp->icid);
3290
3291         BT_DBG("icid %d", icid);
3292
3293         return 0;
3294 }
3295
3296 static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency,
3297                                                         u16 to_multiplier)
3298 {
3299         u16 max_latency;
3300
3301         if (min > max || min < 6 || max > 3200)
3302                 return -EINVAL;
3303
3304         if (to_multiplier < 10 || to_multiplier > 3200)
3305                 return -EINVAL;
3306
3307         if (max >= to_multiplier * 8)
3308                 return -EINVAL;
3309
3310         max_latency = (to_multiplier * 8 / max) - 1;
3311         if (latency > 499 || latency > max_latency)
3312                 return -EINVAL;
3313
3314         return 0;
3315 }
3316
3317 static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
3318                                         struct l2cap_cmd_hdr *cmd, u8 *data)
3319 {
3320         struct hci_conn *hcon = conn->hcon;
3321         struct l2cap_conn_param_update_req *req;
3322         struct l2cap_conn_param_update_rsp rsp;
3323         u16 min, max, latency, to_multiplier, cmd_len;
3324         int err;
3325
3326         if (!(hcon->link_mode & HCI_LM_MASTER))
3327                 return -EINVAL;
3328
3329         cmd_len = __le16_to_cpu(cmd->len);
3330         if (cmd_len != sizeof(struct l2cap_conn_param_update_req))
3331                 return -EPROTO;
3332
3333         req = (struct l2cap_conn_param_update_req *) data;
3334         min             = __le16_to_cpu(req->min);
3335         max             = __le16_to_cpu(req->max);
3336         latency         = __le16_to_cpu(req->latency);
3337         to_multiplier   = __le16_to_cpu(req->to_multiplier);
3338
3339         BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x",
3340                                                 min, max, latency, to_multiplier);
3341
3342         memset(&rsp, 0, sizeof(rsp));
3343
3344         err = l2cap_check_conn_param(min, max, latency, to_multiplier);
3345         if (err)
3346                 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_REJECTED);
3347         else
3348                 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED);
3349
3350         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP,
3351                                                         sizeof(rsp), &rsp);
3352
3353         if (!err)
3354                 hci_le_conn_update(hcon, min, max, latency, to_multiplier);
3355
3356         return 0;
3357 }
3358
3359 static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
3360                         struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
3361 {
3362         int err = 0;
3363
3364         switch (cmd->code) {
3365         case L2CAP_COMMAND_REJ:
3366                 l2cap_command_rej(conn, cmd, data);
3367                 break;
3368
3369         case L2CAP_CONN_REQ:
3370                 err = l2cap_connect_req(conn, cmd, data);
3371                 break;
3372
3373         case L2CAP_CONN_RSP:
3374                 err = l2cap_connect_rsp(conn, cmd, data);
3375                 break;
3376
3377         case L2CAP_CONF_REQ:
3378                 err = l2cap_config_req(conn, cmd, cmd_len, data);
3379                 break;
3380
3381         case L2CAP_CONF_RSP:
3382                 err = l2cap_config_rsp(conn, cmd, data);
3383                 break;
3384
3385         case L2CAP_DISCONN_REQ:
3386                 err = l2cap_disconnect_req(conn, cmd, data);
3387                 break;
3388
3389         case L2CAP_DISCONN_RSP:
3390                 err = l2cap_disconnect_rsp(conn, cmd, data);
3391                 break;
3392
3393         case L2CAP_ECHO_REQ:
3394                 l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data);
3395                 break;
3396
3397         case L2CAP_ECHO_RSP:
3398                 break;
3399
3400         case L2CAP_INFO_REQ:
3401                 err = l2cap_information_req(conn, cmd, data);
3402                 break;
3403
3404         case L2CAP_INFO_RSP:
3405                 err = l2cap_information_rsp(conn, cmd, data);
3406                 break;
3407
3408         case L2CAP_CREATE_CHAN_REQ:
3409                 err = l2cap_create_channel_req(conn, cmd, cmd_len, data);
3410                 break;
3411
3412         case L2CAP_CREATE_CHAN_RSP:
3413                 err = l2cap_create_channel_rsp(conn, cmd, data);
3414                 break;
3415
3416         case L2CAP_MOVE_CHAN_REQ:
3417                 err = l2cap_move_channel_req(conn, cmd, cmd_len, data);
3418                 break;
3419
3420         case L2CAP_MOVE_CHAN_RSP:
3421                 err = l2cap_move_channel_rsp(conn, cmd, cmd_len, data);
3422                 break;
3423
3424         case L2CAP_MOVE_CHAN_CFM:
3425                 err = l2cap_move_channel_confirm(conn, cmd, cmd_len, data);
3426                 break;
3427
3428         case L2CAP_MOVE_CHAN_CFM_RSP:
3429                 err = l2cap_move_channel_confirm_rsp(conn, cmd, cmd_len, data);
3430                 break;
3431
3432         default:
3433                 BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code);
3434                 err = -EINVAL;
3435                 break;
3436         }
3437
3438         return err;
3439 }
3440
3441 static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
3442                                         struct l2cap_cmd_hdr *cmd, u8 *data)
3443 {
3444         switch (cmd->code) {
3445         case L2CAP_COMMAND_REJ:
3446                 return 0;
3447
3448         case L2CAP_CONN_PARAM_UPDATE_REQ:
3449                 return l2cap_conn_param_update_req(conn, cmd, data);
3450
3451         case L2CAP_CONN_PARAM_UPDATE_RSP:
3452                 return 0;
3453
3454         default:
3455                 BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code);
3456                 return -EINVAL;
3457         }
3458 }
3459
3460 static inline void l2cap_sig_channel(struct l2cap_conn *conn,
3461                                                         struct sk_buff *skb)
3462 {
3463         u8 *data = skb->data;
3464         int len = skb->len;
3465         struct l2cap_cmd_hdr cmd;
3466         int err;
3467
3468         l2cap_raw_recv(conn, skb);
3469
3470         while (len >= L2CAP_CMD_HDR_SIZE) {
3471                 u16 cmd_len;
3472                 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
3473                 data += L2CAP_CMD_HDR_SIZE;
3474                 len  -= L2CAP_CMD_HDR_SIZE;
3475
3476                 cmd_len = le16_to_cpu(cmd.len);
3477
3478                 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, cmd.ident);
3479
3480                 if (cmd_len > len || !cmd.ident) {
3481                         BT_DBG("corrupted command");
3482                         break;
3483                 }
3484
3485                 if (conn->hcon->type == LE_LINK)
3486                         err = l2cap_le_sig_cmd(conn, &cmd, data);
3487                 else
3488                         err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data);
3489
3490                 if (err) {
3491                         struct l2cap_cmd_rej_unk rej;
3492
3493                         BT_ERR("Wrong link type (%d)", err);
3494
3495                         /* FIXME: Map err to a valid reason */
3496                         rej.reason = cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
3497                         l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
3498                 }
3499
3500                 data += cmd_len;
3501                 len  -= cmd_len;
3502         }
3503
3504         kfree_skb(skb);
3505 }
3506
3507 static int l2cap_check_fcs(struct l2cap_chan *chan,  struct sk_buff *skb)
3508 {
3509         u16 our_fcs, rcv_fcs;
3510         int hdr_size;
3511
3512         if (test_bit(FLAG_EXT_CTRL, &chan->flags))
3513                 hdr_size = L2CAP_EXT_HDR_SIZE;
3514         else
3515                 hdr_size = L2CAP_ENH_HDR_SIZE;
3516
3517         if (chan->fcs == L2CAP_FCS_CRC16) {
3518                 skb_trim(skb, skb->len - L2CAP_FCS_SIZE);
3519                 rcv_fcs = get_unaligned_le16(skb->data + skb->len);
3520                 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
3521
3522                 if (our_fcs != rcv_fcs)
3523                         return -EBADMSG;
3524         }
3525         return 0;
3526 }
3527
3528 static inline void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan)
3529 {
3530         u32 control = 0;
3531
3532         chan->frames_sent = 0;
3533
3534         control |= __set_reqseq(chan, chan->buffer_seq);
3535
3536         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
3537                 control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
3538                 l2cap_send_sframe(chan, control);
3539                 set_bit(CONN_RNR_SENT, &chan->conn_state);
3540         }
3541
3542         if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
3543                 l2cap_retransmit_frames(chan);
3544
3545         l2cap_ertm_send(chan);
3546
3547         if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
3548                         chan->frames_sent == 0) {
3549                 control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
3550                 l2cap_send_sframe(chan, control);
3551         }
3552 }
3553
3554 static int l2cap_add_to_srej_queue(struct l2cap_chan *chan, struct sk_buff *skb, u16 tx_seq, u8 sar)
3555 {
3556         struct sk_buff *next_skb;
3557         int tx_seq_offset, next_tx_seq_offset;
3558
3559         bt_cb(skb)->tx_seq = tx_seq;
3560         bt_cb(skb)->sar = sar;
3561
3562         next_skb = skb_peek(&chan->srej_q);
3563
3564         tx_seq_offset = __seq_offset(chan, tx_seq, chan->buffer_seq);
3565
3566         while (next_skb) {
3567                 if (bt_cb(next_skb)->tx_seq == tx_seq)
3568                         return -EINVAL;
3569
3570                 next_tx_seq_offset = __seq_offset(chan,
3571                                 bt_cb(next_skb)->tx_seq, chan->buffer_seq);
3572
3573                 if (next_tx_seq_offset > tx_seq_offset) {
3574                         __skb_queue_before(&chan->srej_q, next_skb, skb);
3575                         return 0;
3576                 }
3577
3578                 if (skb_queue_is_last(&chan->srej_q, next_skb))
3579                         next_skb = NULL;
3580                 else
3581                         next_skb = skb_queue_next(&chan->srej_q, next_skb);
3582         }
3583
3584         __skb_queue_tail(&chan->srej_q, skb);
3585
3586         return 0;
3587 }
3588
3589 static void append_skb_frag(struct sk_buff *skb,
3590                         struct sk_buff *new_frag, struct sk_buff **last_frag)
3591 {
3592         /* skb->len reflects data in skb as well as all fragments
3593          * skb->data_len reflects only data in fragments
3594          */
3595         if (!skb_has_frag_list(skb))
3596                 skb_shinfo(skb)->frag_list = new_frag;
3597
3598         new_frag->next = NULL;
3599
3600         (*last_frag)->next = new_frag;
3601         *last_frag = new_frag;
3602
3603         skb->len += new_frag->len;
3604         skb->data_len += new_frag->len;
3605         skb->truesize += new_frag->truesize;
3606 }
3607
3608 static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u32 control)
3609 {
3610         int err = -EINVAL;
3611
3612         switch (__get_ctrl_sar(chan, control)) {
3613         case L2CAP_SAR_UNSEGMENTED:
3614                 if (chan->sdu)
3615                         break;
3616
3617                 err = chan->ops->recv(chan->data, skb);
3618                 break;
3619
3620         case L2CAP_SAR_START:
3621                 if (chan->sdu)
3622                         break;
3623
3624                 chan->sdu_len = get_unaligned_le16(skb->data);
3625                 skb_pull(skb, L2CAP_SDULEN_SIZE);
3626
3627                 if (chan->sdu_len > chan->imtu) {
3628                         err = -EMSGSIZE;
3629                         break;
3630                 }
3631
3632                 if (skb->len >= chan->sdu_len)
3633                         break;
3634
3635                 chan->sdu = skb;
3636                 chan->sdu_last_frag = skb;
3637
3638                 skb = NULL;
3639                 err = 0;
3640                 break;
3641
3642         case L2CAP_SAR_CONTINUE:
3643                 if (!chan->sdu)
3644                         break;
3645
3646                 append_skb_frag(chan->sdu, skb,
3647                                 &chan->sdu_last_frag);
3648                 skb = NULL;
3649
3650                 if (chan->sdu->len >= chan->sdu_len)
3651                         break;
3652
3653                 err = 0;
3654                 break;
3655
3656         case L2CAP_SAR_END:
3657                 if (!chan->sdu)
3658                         break;
3659
3660                 append_skb_frag(chan->sdu, skb,
3661                                 &chan->sdu_last_frag);
3662                 skb = NULL;
3663
3664                 if (chan->sdu->len != chan->sdu_len)
3665                         break;
3666
3667                 err = chan->ops->recv(chan->data, chan->sdu);
3668
3669                 if (!err) {
3670                         /* Reassembly complete */
3671                         chan->sdu = NULL;
3672                         chan->sdu_last_frag = NULL;
3673                         chan->sdu_len = 0;
3674                 }
3675                 break;
3676         }
3677
3678         if (err) {
3679                 kfree_skb(skb);
3680                 kfree_skb(chan->sdu);
3681                 chan->sdu = NULL;
3682                 chan->sdu_last_frag = NULL;
3683                 chan->sdu_len = 0;
3684         }
3685
3686         return err;
3687 }
3688
3689 static void l2cap_ertm_enter_local_busy(struct l2cap_chan *chan)
3690 {
3691         u32 control;
3692
3693         BT_DBG("chan %p, Enter local busy", chan);
3694
3695         set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
3696
3697         control = __set_reqseq(chan, chan->buffer_seq);
3698         control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
3699         l2cap_send_sframe(chan, control);
3700
3701         set_bit(CONN_RNR_SENT, &chan->conn_state);
3702
3703         __clear_ack_timer(chan);
3704 }
3705
3706 static void l2cap_ertm_exit_local_busy(struct l2cap_chan *chan)
3707 {
3708         u32 control;
3709
3710         if (!test_bit(CONN_RNR_SENT, &chan->conn_state))
3711                 goto done;
3712
3713         control = __set_reqseq(chan, chan->buffer_seq);
3714         control |= __set_ctrl_poll(chan);
3715         control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
3716         l2cap_send_sframe(chan, control);
3717         chan->retry_count = 1;
3718
3719         __clear_retrans_timer(chan);
3720         __set_monitor_timer(chan);
3721
3722         set_bit(CONN_WAIT_F, &chan->conn_state);
3723
3724 done:
3725         clear_bit(CONN_LOCAL_BUSY, &chan->conn_state);
3726         clear_bit(CONN_RNR_SENT, &chan->conn_state);
3727
3728         BT_DBG("chan %p, Exit local busy", chan);
3729 }
3730
3731 void l2cap_chan_busy(struct l2cap_chan *chan, int busy)
3732 {
3733         if (chan->mode == L2CAP_MODE_ERTM) {
3734                 if (busy)
3735                         l2cap_ertm_enter_local_busy(chan);
3736                 else
3737                         l2cap_ertm_exit_local_busy(chan);
3738         }
3739 }
3740
3741 static void l2cap_check_srej_gap(struct l2cap_chan *chan, u16 tx_seq)
3742 {
3743         struct sk_buff *skb;
3744         u32 control;
3745
3746         while ((skb = skb_peek(&chan->srej_q)) &&
3747                         !test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
3748                 int err;
3749
3750                 if (bt_cb(skb)->tx_seq != tx_seq)
3751                         break;
3752
3753                 skb = skb_dequeue(&chan->srej_q);
3754                 control = __set_ctrl_sar(chan, bt_cb(skb)->sar);
3755                 err = l2cap_reassemble_sdu(chan, skb, control);
3756
3757                 if (err < 0) {
3758                         l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3759                         break;
3760                 }
3761
3762                 chan->buffer_seq_srej = __next_seq(chan, chan->buffer_seq_srej);
3763                 tx_seq = __next_seq(chan, tx_seq);
3764         }
3765 }
3766
3767 static void l2cap_resend_srejframe(struct l2cap_chan *chan, u16 tx_seq)
3768 {
3769         struct srej_list *l, *tmp;
3770         u32 control;
3771
3772         list_for_each_entry_safe(l, tmp, &chan->srej_l, list) {
3773                 if (l->tx_seq == tx_seq) {
3774                         list_del(&l->list);
3775                         kfree(l);
3776                         return;
3777                 }
3778                 control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ);
3779                 control |= __set_reqseq(chan, l->tx_seq);
3780                 l2cap_send_sframe(chan, control);
3781                 list_del(&l->list);
3782                 list_add_tail(&l->list, &chan->srej_l);
3783         }
3784 }
3785
3786 static int l2cap_send_srejframe(struct l2cap_chan *chan, u16 tx_seq)
3787 {
3788         struct srej_list *new;
3789         u32 control;
3790
3791         while (tx_seq != chan->expected_tx_seq) {
3792                 control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ);
3793                 control |= __set_reqseq(chan, chan->expected_tx_seq);
3794                 l2cap_send_sframe(chan, control);
3795
3796                 new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC);
3797                 if (!new)
3798                         return -ENOMEM;
3799
3800                 new->tx_seq = chan->expected_tx_seq;
3801
3802                 chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq);
3803
3804                 list_add_tail(&new->list, &chan->srej_l);
3805         }
3806
3807         chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq);
3808
3809         return 0;
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
3881                         err = l2cap_send_srejframe(chan, tx_seq);
3882                         if (err < 0) {
3883                                 l2cap_send_disconn_req(chan->conn, chan, -err);
3884                                 return err;
3885                         }
3886                 }
3887         } else {
3888                 expected_tx_seq_offset = __seq_offset(chan,
3889                                 chan->expected_tx_seq, chan->buffer_seq);
3890
3891                 /* duplicated tx_seq */
3892                 if (tx_seq_offset < expected_tx_seq_offset)
3893                         goto drop;
3894
3895                 set_bit(CONN_SREJ_SENT, &chan->conn_state);
3896
3897                 BT_DBG("chan %p, Enter SREJ", chan);
3898
3899                 INIT_LIST_HEAD(&chan->srej_l);
3900                 chan->buffer_seq_srej = chan->buffer_seq;
3901
3902                 __skb_queue_head_init(&chan->srej_q);
3903                 l2cap_add_to_srej_queue(chan, skb, tx_seq, sar);
3904
3905                 set_bit(CONN_SEND_PBIT, &chan->conn_state);
3906
3907                 err = l2cap_send_srejframe(chan, tx_seq);
3908                 if (err < 0) {
3909                         l2cap_send_disconn_req(chan->conn, chan, -err);
3910                         return err;
3911                 }
3912
3913                 __clear_ack_timer(chan);
3914         }
3915         return 0;
3916
3917 expected:
3918         chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq);
3919
3920         if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
3921                 bt_cb(skb)->tx_seq = tx_seq;
3922                 bt_cb(skb)->sar = sar;
3923                 __skb_queue_tail(&chan->srej_q, skb);
3924                 return 0;
3925         }
3926
3927         err = l2cap_reassemble_sdu(chan, skb, rx_control);
3928         chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
3929
3930         if (err < 0) {
3931                 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3932                 return err;
3933         }
3934
3935         if (__is_ctrl_final(chan, rx_control)) {
3936                 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
3937                         l2cap_retransmit_frames(chan);
3938         }
3939
3940
3941         chan->num_acked = (chan->num_acked + 1) % num_to_ack;
3942         if (chan->num_acked == num_to_ack - 1)
3943                 l2cap_send_ack(chan);
3944         else
3945                 __set_ack_timer(chan);
3946
3947         return 0;
3948
3949 drop:
3950         kfree_skb(skb);
3951         return 0;
3952 }
3953
3954 static inline void l2cap_data_channel_rrframe(struct l2cap_chan *chan, u32 rx_control)
3955 {
3956         BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan,
3957                                 __get_reqseq(chan, rx_control), rx_control);
3958
3959         chan->expected_ack_seq = __get_reqseq(chan, rx_control);
3960         l2cap_drop_acked_frames(chan);
3961
3962         if (__is_ctrl_poll(chan, rx_control)) {
3963                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
3964                 if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
3965                         if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
3966                                         (chan->unacked_frames > 0))
3967                                 __set_retrans_timer(chan);
3968
3969                         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3970                         l2cap_send_srejtail(chan);
3971                 } else {
3972                         l2cap_send_i_or_rr_or_rnr(chan);
3973                 }
3974
3975         } else if (__is_ctrl_final(chan, rx_control)) {
3976                 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3977
3978                 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
3979                         l2cap_retransmit_frames(chan);
3980
3981         } else {
3982                 if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
3983                                 (chan->unacked_frames > 0))
3984                         __set_retrans_timer(chan);
3985
3986                 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3987                 if (test_bit(CONN_SREJ_SENT, &chan->conn_state))
3988                         l2cap_send_ack(chan);
3989                 else
3990                         l2cap_ertm_send(chan);
3991         }
3992 }
3993
3994 static inline void l2cap_data_channel_rejframe(struct l2cap_chan *chan, u32 rx_control)
3995 {
3996         u16 tx_seq = __get_reqseq(chan, rx_control);
3997
3998         BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control);
3999
4000         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
4001
4002         chan->expected_ack_seq = tx_seq;
4003         l2cap_drop_acked_frames(chan);
4004
4005         if (__is_ctrl_final(chan, rx_control)) {
4006                 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
4007                         l2cap_retransmit_frames(chan);
4008         } else {
4009                 l2cap_retransmit_frames(chan);
4010
4011                 if (test_bit(CONN_WAIT_F, &chan->conn_state))
4012                         set_bit(CONN_REJ_ACT, &chan->conn_state);
4013         }
4014 }
4015 static inline void l2cap_data_channel_srejframe(struct l2cap_chan *chan, u32 rx_control)
4016 {
4017         u16 tx_seq = __get_reqseq(chan, rx_control);
4018
4019         BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control);
4020
4021         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
4022
4023         if (__is_ctrl_poll(chan, rx_control)) {
4024                 chan->expected_ack_seq = tx_seq;
4025                 l2cap_drop_acked_frames(chan);
4026
4027                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
4028                 l2cap_retransmit_one_frame(chan, tx_seq);
4029
4030                 l2cap_ertm_send(chan);
4031
4032                 if (test_bit(CONN_WAIT_F, &chan->conn_state)) {
4033                         chan->srej_save_reqseq = tx_seq;
4034                         set_bit(CONN_SREJ_ACT, &chan->conn_state);
4035                 }
4036         } else if (__is_ctrl_final(chan, rx_control)) {
4037                 if (test_bit(CONN_SREJ_ACT, &chan->conn_state) &&
4038                                 chan->srej_save_reqseq == tx_seq)
4039                         clear_bit(CONN_SREJ_ACT, &chan->conn_state);
4040                 else
4041                         l2cap_retransmit_one_frame(chan, tx_seq);
4042         } else {
4043                 l2cap_retransmit_one_frame(chan, tx_seq);
4044                 if (test_bit(CONN_WAIT_F, &chan->conn_state)) {
4045                         chan->srej_save_reqseq = tx_seq;
4046                         set_bit(CONN_SREJ_ACT, &chan->conn_state);
4047                 }
4048         }
4049 }
4050
4051 static inline void l2cap_data_channel_rnrframe(struct l2cap_chan *chan, u32 rx_control)
4052 {
4053         u16 tx_seq = __get_reqseq(chan, rx_control);
4054
4055         BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control);
4056
4057         set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
4058         chan->expected_ack_seq = tx_seq;
4059         l2cap_drop_acked_frames(chan);
4060
4061         if (__is_ctrl_poll(chan, rx_control))
4062                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
4063
4064         if (!test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
4065                 __clear_retrans_timer(chan);
4066                 if (__is_ctrl_poll(chan, rx_control))
4067                         l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_FINAL);
4068                 return;
4069         }
4070
4071         if (__is_ctrl_poll(chan, rx_control)) {
4072                 l2cap_send_srejtail(chan);
4073         } else {
4074                 rx_control = __set_ctrl_super(chan, L2CAP_SUPER_RR);
4075                 l2cap_send_sframe(chan, rx_control);
4076         }
4077 }
4078
4079 static inline int l2cap_data_channel_sframe(struct l2cap_chan *chan, u32 rx_control, struct sk_buff *skb)
4080 {
4081         BT_DBG("chan %p rx_control 0x%8.8x len %d", chan, rx_control, skb->len);
4082
4083         if (__is_ctrl_final(chan, rx_control) &&
4084                         test_bit(CONN_WAIT_F, &chan->conn_state)) {
4085                 __clear_monitor_timer(chan);
4086                 if (chan->unacked_frames > 0)
4087                         __set_retrans_timer(chan);
4088                 clear_bit(CONN_WAIT_F, &chan->conn_state);
4089         }
4090
4091         switch (__get_ctrl_super(chan, rx_control)) {
4092         case L2CAP_SUPER_RR:
4093                 l2cap_data_channel_rrframe(chan, rx_control);
4094                 break;
4095
4096         case L2CAP_SUPER_REJ:
4097                 l2cap_data_channel_rejframe(chan, rx_control);
4098                 break;
4099
4100         case L2CAP_SUPER_SREJ:
4101                 l2cap_data_channel_srejframe(chan, rx_control);
4102                 break;
4103
4104         case L2CAP_SUPER_RNR:
4105                 l2cap_data_channel_rnrframe(chan, rx_control);
4106                 break;
4107         }
4108
4109         kfree_skb(skb);
4110         return 0;
4111 }
4112
4113 static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb)
4114 {
4115         struct l2cap_chan *chan = l2cap_pi(sk)->chan;
4116         u32 control;
4117         u16 req_seq;
4118         int len, next_tx_seq_offset, req_seq_offset;
4119
4120         control = __get_control(chan, skb->data);
4121         skb_pull(skb, __ctrl_size(chan));
4122         len = skb->len;
4123
4124         /*
4125          * We can just drop the corrupted I-frame here.
4126          * Receiver will miss it and start proper recovery
4127          * procedures and ask retransmission.
4128          */
4129         if (l2cap_check_fcs(chan, skb))
4130                 goto drop;
4131
4132         if (__is_sar_start(chan, control) && !__is_sframe(chan, control))
4133                 len -= L2CAP_SDULEN_SIZE;
4134
4135         if (chan->fcs == L2CAP_FCS_CRC16)
4136                 len -= L2CAP_FCS_SIZE;
4137
4138         if (len > chan->mps) {
4139                 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
4140                 goto drop;
4141         }
4142
4143         req_seq = __get_reqseq(chan, control);
4144
4145         req_seq_offset = __seq_offset(chan, req_seq, chan->expected_ack_seq);
4146
4147         next_tx_seq_offset = __seq_offset(chan, chan->next_tx_seq,
4148                                                 chan->expected_ack_seq);
4149
4150         /* check for invalid req-seq */
4151         if (req_seq_offset > next_tx_seq_offset) {
4152                 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
4153                 goto drop;
4154         }
4155
4156         if (!__is_sframe(chan, control)) {
4157                 if (len < 0) {
4158                         l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
4159                         goto drop;
4160                 }
4161
4162                 l2cap_data_channel_iframe(chan, control, skb);
4163         } else {
4164                 if (len != 0) {
4165                         BT_ERR("%d", len);
4166                         l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
4167                         goto drop;
4168                 }
4169
4170                 l2cap_data_channel_sframe(chan, control, skb);
4171         }
4172
4173         return 0;
4174
4175 drop:
4176         kfree_skb(skb);
4177         return 0;
4178 }
4179
4180 static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
4181 {
4182         struct l2cap_chan *chan;
4183         struct sock *sk = NULL;
4184         u32 control;
4185         u16 tx_seq;
4186         int len;
4187
4188         chan = l2cap_get_chan_by_scid(conn, cid);
4189         if (!chan) {
4190                 BT_DBG("unknown cid 0x%4.4x", cid);
4191                 goto drop;
4192         }
4193
4194         sk = chan->sk;
4195
4196         BT_DBG("chan %p, len %d", chan, skb->len);
4197
4198         if (chan->state != BT_CONNECTED)
4199                 goto drop;
4200
4201         switch (chan->mode) {
4202         case L2CAP_MODE_BASIC:
4203                 /* If socket recv buffers overflows we drop data here
4204                  * which is *bad* because L2CAP has to be reliable.
4205                  * But we don't have any other choice. L2CAP doesn't
4206                  * provide flow control mechanism. */
4207
4208                 if (chan->imtu < skb->len)
4209                         goto drop;
4210
4211                 if (!chan->ops->recv(chan->data, skb))
4212                         goto done;
4213                 break;
4214
4215         case L2CAP_MODE_ERTM:
4216                 if (!sock_owned_by_user(sk)) {
4217                         l2cap_ertm_data_rcv(sk, skb);
4218                 } else {
4219                         if (sk_add_backlog(sk, skb))
4220                                 goto drop;
4221                 }
4222
4223                 goto done;
4224
4225         case L2CAP_MODE_STREAMING:
4226                 control = __get_control(chan, skb->data);
4227                 skb_pull(skb, __ctrl_size(chan));
4228                 len = skb->len;
4229
4230                 if (l2cap_check_fcs(chan, skb))
4231                         goto drop;
4232
4233                 if (__is_sar_start(chan, control))
4234                         len -= L2CAP_SDULEN_SIZE;
4235
4236                 if (chan->fcs == L2CAP_FCS_CRC16)
4237                         len -= L2CAP_FCS_SIZE;
4238
4239                 if (len > chan->mps || len < 0 || __is_sframe(chan, control))
4240                         goto drop;
4241
4242                 tx_seq = __get_txseq(chan, control);
4243
4244                 if (chan->expected_tx_seq != tx_seq) {
4245                         /* Frame(s) missing - must discard partial SDU */
4246                         kfree_skb(chan->sdu);
4247                         chan->sdu = NULL;
4248                         chan->sdu_last_frag = NULL;
4249                         chan->sdu_len = 0;
4250
4251                         /* TODO: Notify userland of missing data */
4252                 }
4253
4254                 chan->expected_tx_seq = __next_seq(chan, tx_seq);
4255
4256                 if (l2cap_reassemble_sdu(chan, skb, control) == -EMSGSIZE)
4257                         l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
4258
4259                 goto done;
4260
4261         default:
4262                 BT_DBG("chan %p: bad mode 0x%2.2x", chan, chan->mode);
4263                 break;
4264         }
4265
4266 drop:
4267         kfree_skb(skb);
4268
4269 done:
4270         if (sk)
4271                 bh_unlock_sock(sk);
4272
4273         return 0;
4274 }
4275
4276 static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb)
4277 {
4278         struct sock *sk = NULL;
4279         struct l2cap_chan *chan;
4280
4281         chan = l2cap_global_chan_by_psm(0, psm, conn->src);
4282         if (!chan)
4283                 goto drop;
4284
4285         sk = chan->sk;
4286
4287         bh_lock_sock(sk);
4288
4289         BT_DBG("sk %p, len %d", sk, skb->len);
4290
4291         if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
4292                 goto drop;
4293
4294         if (chan->imtu < skb->len)
4295                 goto drop;
4296
4297         if (!chan->ops->recv(chan->data, skb))
4298                 goto done;
4299
4300 drop:
4301         kfree_skb(skb);
4302
4303 done:
4304         if (sk)
4305                 bh_unlock_sock(sk);
4306         return 0;
4307 }
4308
4309 static inline int l2cap_att_channel(struct l2cap_conn *conn, __le16 cid, struct sk_buff *skb)
4310 {
4311         struct sock *sk = NULL;
4312         struct l2cap_chan *chan;
4313
4314         chan = l2cap_global_chan_by_scid(0, cid, conn->src);
4315         if (!chan)
4316                 goto drop;
4317
4318         sk = chan->sk;
4319
4320         bh_lock_sock(sk);
4321
4322         BT_DBG("sk %p, len %d", sk, skb->len);
4323
4324         if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
4325                 goto drop;
4326
4327         if (chan->imtu < skb->len)
4328                 goto drop;
4329
4330         if (!chan->ops->recv(chan->data, skb))
4331                 goto done;
4332
4333 drop:
4334         kfree_skb(skb);
4335
4336 done:
4337         if (sk)
4338                 bh_unlock_sock(sk);
4339         return 0;
4340 }
4341
4342 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
4343 {
4344         struct l2cap_hdr *lh = (void *) skb->data;
4345         u16 cid, len;
4346         __le16 psm;
4347
4348         skb_pull(skb, L2CAP_HDR_SIZE);
4349         cid = __le16_to_cpu(lh->cid);
4350         len = __le16_to_cpu(lh->len);
4351
4352         if (len != skb->len) {
4353                 kfree_skb(skb);
4354                 return;
4355         }
4356
4357         BT_DBG("len %d, cid 0x%4.4x", len, cid);
4358
4359         switch (cid) {
4360         case L2CAP_CID_LE_SIGNALING:
4361         case L2CAP_CID_SIGNALING:
4362                 l2cap_sig_channel(conn, skb);
4363                 break;
4364
4365         case L2CAP_CID_CONN_LESS:
4366                 psm = get_unaligned_le16(skb->data);
4367                 skb_pull(skb, 2);
4368                 l2cap_conless_channel(conn, psm, skb);
4369                 break;
4370
4371         case L2CAP_CID_LE_DATA:
4372                 l2cap_att_channel(conn, cid, skb);
4373                 break;
4374
4375         case L2CAP_CID_SMP:
4376                 if (smp_sig_channel(conn, skb))
4377                         l2cap_conn_del(conn->hcon, EACCES);
4378                 break;
4379
4380         default:
4381                 l2cap_data_channel(conn, cid, skb);
4382                 break;
4383         }
4384 }
4385
4386 /* ---- L2CAP interface with lower layer (HCI) ---- */
4387
4388 static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
4389 {
4390         int exact = 0, lm1 = 0, lm2 = 0;
4391         struct l2cap_chan *c;
4392
4393         if (type != ACL_LINK)
4394                 return -EINVAL;
4395
4396         BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
4397
4398         /* Find listening sockets and check their link_mode */
4399         read_lock(&chan_list_lock);
4400         list_for_each_entry(c, &chan_list, global_l) {
4401                 struct sock *sk = c->sk;
4402
4403                 if (c->state != BT_LISTEN)
4404                         continue;
4405
4406                 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
4407                         lm1 |= HCI_LM_ACCEPT;
4408                         if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
4409                                 lm1 |= HCI_LM_MASTER;
4410                         exact++;
4411                 } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
4412                         lm2 |= HCI_LM_ACCEPT;
4413                         if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
4414                                 lm2 |= HCI_LM_MASTER;
4415                 }
4416         }
4417         read_unlock(&chan_list_lock);
4418
4419         return exact ? lm1 : lm2;
4420 }
4421
4422 static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
4423 {
4424         struct l2cap_conn *conn;
4425
4426         BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
4427
4428         if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK))
4429                 return -EINVAL;
4430
4431         if (!status) {
4432                 conn = l2cap_conn_add(hcon, status);
4433                 if (conn)
4434                         l2cap_conn_ready(conn);
4435         } else
4436                 l2cap_conn_del(hcon, bt_to_errno(status));
4437
4438         return 0;
4439 }
4440
4441 static int l2cap_disconn_ind(struct hci_conn *hcon)
4442 {
4443         struct l2cap_conn *conn = hcon->l2cap_data;
4444
4445         BT_DBG("hcon %p", hcon);
4446
4447         if ((hcon->type != ACL_LINK && hcon->type != LE_LINK) || !conn)
4448                 return HCI_ERROR_REMOTE_USER_TERM;
4449
4450         return conn->disc_reason;
4451 }
4452
4453 static int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
4454 {
4455         BT_DBG("hcon %p reason %d", hcon, reason);
4456
4457         if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK))
4458                 return -EINVAL;
4459
4460         l2cap_conn_del(hcon, bt_to_errno(reason));
4461
4462         return 0;
4463 }
4464
4465 static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt)
4466 {
4467         if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED)
4468                 return;
4469
4470         if (encrypt == 0x00) {
4471                 if (chan->sec_level == BT_SECURITY_MEDIUM) {
4472                         __clear_chan_timer(chan);
4473                         __set_chan_timer(chan, L2CAP_ENC_TIMEOUT);
4474                 } else if (chan->sec_level == BT_SECURITY_HIGH)
4475                         l2cap_chan_close(chan, ECONNREFUSED);
4476         } else {
4477                 if (chan->sec_level == BT_SECURITY_MEDIUM)
4478                         __clear_chan_timer(chan);
4479         }
4480 }
4481
4482 static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
4483 {
4484         struct l2cap_conn *conn = hcon->l2cap_data;
4485         struct l2cap_chan *chan;
4486
4487         if (!conn)
4488                 return 0;
4489
4490         BT_DBG("conn %p", conn);
4491
4492         if (hcon->type == LE_LINK) {
4493                 smp_distribute_keys(conn, 0);
4494                 del_timer(&conn->security_timer);
4495         }
4496
4497         read_lock(&conn->chan_lock);
4498
4499         list_for_each_entry(chan, &conn->chan_l, list) {
4500                 struct sock *sk = chan->sk;
4501
4502                 bh_lock_sock(sk);
4503
4504                 BT_DBG("chan->scid %d", chan->scid);
4505
4506                 if (chan->scid == L2CAP_CID_LE_DATA) {
4507                         if (!status && encrypt) {
4508                                 chan->sec_level = hcon->sec_level;
4509                                 l2cap_chan_ready(sk);
4510                         }
4511
4512                         bh_unlock_sock(sk);
4513                         continue;
4514                 }
4515
4516                 if (test_bit(CONF_CONNECT_PEND, &chan->conf_state)) {
4517                         bh_unlock_sock(sk);
4518                         continue;
4519                 }
4520
4521                 if (!status && (chan->state == BT_CONNECTED ||
4522                                                 chan->state == BT_CONFIG)) {
4523                         l2cap_check_encryption(chan, encrypt);
4524                         bh_unlock_sock(sk);
4525                         continue;
4526                 }
4527
4528                 if (chan->state == BT_CONNECT) {
4529                         if (!status) {
4530                                 struct l2cap_conn_req req;
4531                                 req.scid = cpu_to_le16(chan->scid);
4532                                 req.psm  = chan->psm;
4533
4534                                 chan->ident = l2cap_get_ident(conn);
4535                                 set_bit(CONF_CONNECT_PEND, &chan->conf_state);
4536
4537                                 l2cap_send_cmd(conn, chan->ident,
4538                                         L2CAP_CONN_REQ, sizeof(req), &req);
4539                         } else {
4540                                 __clear_chan_timer(chan);
4541                                 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
4542                         }
4543                 } else if (chan->state == BT_CONNECT2) {
4544                         struct l2cap_conn_rsp rsp;
4545                         __u16 res, stat;
4546
4547                         if (!status) {
4548                                 if (bt_sk(sk)->defer_setup) {
4549                                         struct sock *parent = bt_sk(sk)->parent;
4550                                         res = L2CAP_CR_PEND;
4551                                         stat = L2CAP_CS_AUTHOR_PEND;
4552                                         if (parent)
4553                                                 parent->sk_data_ready(parent, 0);
4554                                 } else {
4555                                         l2cap_state_change(chan, BT_CONFIG);
4556                                         res = L2CAP_CR_SUCCESS;
4557                                         stat = L2CAP_CS_NO_INFO;
4558                                 }
4559                         } else {
4560                                 l2cap_state_change(chan, BT_DISCONN);
4561                                 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
4562                                 res = L2CAP_CR_SEC_BLOCK;
4563                                 stat = L2CAP_CS_NO_INFO;
4564                         }
4565
4566                         rsp.scid   = cpu_to_le16(chan->dcid);
4567                         rsp.dcid   = cpu_to_le16(chan->scid);
4568                         rsp.result = cpu_to_le16(res);
4569                         rsp.status = cpu_to_le16(stat);
4570                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
4571                                                         sizeof(rsp), &rsp);
4572                 }
4573
4574                 bh_unlock_sock(sk);
4575         }
4576
4577         read_unlock(&conn->chan_lock);
4578
4579         return 0;
4580 }
4581
4582 static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
4583 {
4584         struct l2cap_conn *conn = hcon->l2cap_data;
4585
4586         if (!conn)
4587                 conn = l2cap_conn_add(hcon, 0);
4588
4589         if (!conn)
4590                 goto drop;
4591
4592         BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
4593
4594         if (!(flags & ACL_CONT)) {
4595                 struct l2cap_hdr *hdr;
4596                 struct l2cap_chan *chan;
4597                 u16 cid;
4598                 int len;
4599
4600                 if (conn->rx_len) {
4601                         BT_ERR("Unexpected start frame (len %d)", skb->len);
4602                         kfree_skb(conn->rx_skb);
4603                         conn->rx_skb = NULL;
4604                         conn->rx_len = 0;
4605                         l2cap_conn_unreliable(conn, ECOMM);
4606                 }
4607
4608                 /* Start fragment always begin with Basic L2CAP header */
4609                 if (skb->len < L2CAP_HDR_SIZE) {
4610                         BT_ERR("Frame is too short (len %d)", skb->len);
4611                         l2cap_conn_unreliable(conn, ECOMM);
4612                         goto drop;
4613                 }
4614
4615                 hdr = (struct l2cap_hdr *) skb->data;
4616                 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
4617                 cid = __le16_to_cpu(hdr->cid);
4618
4619                 if (len == skb->len) {
4620                         /* Complete frame received */
4621                         l2cap_recv_frame(conn, skb);
4622                         return 0;
4623                 }
4624
4625                 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
4626
4627                 if (skb->len > len) {
4628                         BT_ERR("Frame is too long (len %d, expected len %d)",
4629                                 skb->len, len);
4630                         l2cap_conn_unreliable(conn, ECOMM);
4631                         goto drop;
4632                 }
4633
4634                 chan = l2cap_get_chan_by_scid(conn, cid);
4635
4636                 if (chan && chan->sk) {
4637                         struct sock *sk = chan->sk;
4638
4639                         if (chan->imtu < len - L2CAP_HDR_SIZE) {
4640                                 BT_ERR("Frame exceeding recv MTU (len %d, "
4641                                                         "MTU %d)", len,
4642                                                         chan->imtu);
4643                                 bh_unlock_sock(sk);
4644                                 l2cap_conn_unreliable(conn, ECOMM);
4645                                 goto drop;
4646                         }
4647                         bh_unlock_sock(sk);
4648                 }
4649
4650                 /* Allocate skb for the complete frame (with header) */
4651                 conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC);
4652                 if (!conn->rx_skb)
4653                         goto drop;
4654
4655                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
4656                                                                 skb->len);
4657                 conn->rx_len = len - skb->len;
4658         } else {
4659                 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
4660
4661                 if (!conn->rx_len) {
4662                         BT_ERR("Unexpected continuation frame (len %d)", skb->len);
4663                         l2cap_conn_unreliable(conn, ECOMM);
4664                         goto drop;
4665                 }
4666
4667                 if (skb->len > conn->rx_len) {
4668                         BT_ERR("Fragment is too long (len %d, expected %d)",
4669                                         skb->len, conn->rx_len);
4670                         kfree_skb(conn->rx_skb);
4671                         conn->rx_skb = NULL;
4672                         conn->rx_len = 0;
4673                         l2cap_conn_unreliable(conn, ECOMM);
4674                         goto drop;
4675                 }
4676
4677                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
4678                                                                 skb->len);
4679                 conn->rx_len -= skb->len;
4680
4681                 if (!conn->rx_len) {
4682                         /* Complete frame received */
4683                         l2cap_recv_frame(conn, conn->rx_skb);
4684                         conn->rx_skb = NULL;
4685                 }
4686         }
4687
4688 drop:
4689         kfree_skb(skb);
4690         return 0;
4691 }
4692
4693 static int l2cap_debugfs_show(struct seq_file *f, void *p)
4694 {
4695         struct l2cap_chan *c;
4696
4697         read_lock_bh(&chan_list_lock);
4698
4699         list_for_each_entry(c, &chan_list, global_l) {
4700                 struct sock *sk = c->sk;
4701
4702                 seq_printf(f, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
4703                                         batostr(&bt_sk(sk)->src),
4704                                         batostr(&bt_sk(sk)->dst),
4705                                         c->state, __le16_to_cpu(c->psm),
4706                                         c->scid, c->dcid, c->imtu, c->omtu,
4707                                         c->sec_level, c->mode);
4708 }
4709
4710         read_unlock_bh(&chan_list_lock);
4711
4712         return 0;
4713 }
4714
4715 static int l2cap_debugfs_open(struct inode *inode, struct file *file)
4716 {
4717         return single_open(file, l2cap_debugfs_show, inode->i_private);
4718 }
4719
4720 static const struct file_operations l2cap_debugfs_fops = {
4721         .open           = l2cap_debugfs_open,
4722         .read           = seq_read,
4723         .llseek         = seq_lseek,
4724         .release        = single_release,
4725 };
4726
4727 static struct dentry *l2cap_debugfs;
4728
4729 static struct hci_proto l2cap_hci_proto = {
4730         .name           = "L2CAP",
4731         .id             = HCI_PROTO_L2CAP,
4732         .connect_ind    = l2cap_connect_ind,
4733         .connect_cfm    = l2cap_connect_cfm,
4734         .disconn_ind    = l2cap_disconn_ind,
4735         .disconn_cfm    = l2cap_disconn_cfm,
4736         .security_cfm   = l2cap_security_cfm,
4737         .recv_acldata   = l2cap_recv_acldata
4738 };
4739
4740 int __init l2cap_init(void)
4741 {
4742         int err;
4743
4744         err = l2cap_init_sockets();
4745         if (err < 0)
4746                 return err;
4747
4748         err = hci_register_proto(&l2cap_hci_proto);
4749         if (err < 0) {
4750                 BT_ERR("L2CAP protocol registration failed");
4751                 bt_sock_unregister(BTPROTO_L2CAP);
4752                 goto error;
4753         }
4754
4755         if (bt_debugfs) {
4756                 l2cap_debugfs = debugfs_create_file("l2cap", 0444,
4757                                         bt_debugfs, NULL, &l2cap_debugfs_fops);
4758                 if (!l2cap_debugfs)
4759                         BT_ERR("Failed to create L2CAP debug file");
4760         }
4761
4762         return 0;
4763
4764 error:
4765         l2cap_cleanup_sockets();
4766         return err;
4767 }
4768
4769 void l2cap_exit(void)
4770 {
4771         debugfs_remove(l2cap_debugfs);
4772
4773         if (hci_unregister_proto(&l2cap_hci_proto) < 0)
4774                 BT_ERR("L2CAP protocol unregistration failed");
4775
4776         l2cap_cleanup_sockets();
4777 }
4778
4779 module_param(disable_ertm, bool, 0644);
4780 MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode");
4781
4782 module_param(enable_hs, bool, 0644);
4783 MODULE_PARM_DESC(enable_hs, "Enable High Speed");