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] = { 0x02, };
  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, HZ / 5);
 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         return chan;
 298 }
 299
 300 void l2cap_chan_destroy(struct l2cap_chan *chan)
 301 {
 302         write_lock_bh(&chan_list_lock);
 303         list_del(&chan->global_l);
 304         write_unlock_bh(&chan_list_lock);
 305
 306         chan_put(chan);
 307 }
 308
 309 static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 310 {
 311         BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn,
 312                         chan->psm, chan->dcid);
 313
 314         conn->disc_reason = 0x13;
 315
 316         chan->conn = conn;
 317
 318         if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED) {
 319                 if (conn->hcon->type == LE_LINK) {
 320                         /* LE connection */
 321                         chan->omtu = L2CAP_LE_DEFAULT_MTU;
 322                         chan->scid = L2CAP_CID_LE_DATA;
 323                         chan->dcid = L2CAP_CID_LE_DATA;
 324                 } else {
 325                         /* Alloc CID for connection-oriented socket */
 326                         chan->scid = l2cap_alloc_cid(conn);
 327                         chan->omtu = L2CAP_DEFAULT_MTU;
 328                 }
 329         } else if (chan->chan_type == L2CAP_CHAN_CONN_LESS) {
 330                 /* Connectionless socket */
 331                 chan->scid = L2CAP_CID_CONN_LESS;
 332                 chan->dcid = L2CAP_CID_CONN_LESS;
 333                 chan->omtu = L2CAP_DEFAULT_MTU;
 334         } else {
 335                 /* Raw socket can send/recv signalling messages only */
 336                 chan->scid = L2CAP_CID_SIGNALING;
 337                 chan->dcid = L2CAP_CID_SIGNALING;
 338                 chan->omtu = L2CAP_DEFAULT_MTU;
 339         }
 340
 341         chan->local_id          = L2CAP_BESTEFFORT_ID;
 342         chan->local_stype       = L2CAP_SERV_BESTEFFORT;
 343         chan->local_msdu        = L2CAP_DEFAULT_MAX_SDU_SIZE;
 344         chan->local_sdu_itime   = L2CAP_DEFAULT_SDU_ITIME;
 345         chan->local_acc_lat     = L2CAP_DEFAULT_ACC_LAT;
 346         chan->local_flush_to    = L2CAP_DEFAULT_FLUSH_TO;
 347
 348         chan_hold(chan);
 349
 350         list_add(&chan->list, &conn->chan_l);
 351 }
 352
 353 /* Delete channel.
 354  * Must be called on the locked socket. */
 355 static void l2cap_chan_del(struct l2cap_chan *chan, int err)
 356 {
 357         struct sock *sk = chan->sk;
 358         struct l2cap_conn *conn = chan->conn;
 359         struct sock *parent = bt_sk(sk)->parent;
 360
 361         __clear_chan_timer(chan);
 362
 363         BT_DBG("chan %p, conn %p, err %d", chan, conn, err);
 364
 365         if (conn) {
 366                 /* Delete from channel list */
 367                 write_lock_bh(&conn->chan_lock);
 368                 list_del(&chan->list);
 369                 write_unlock_bh(&conn->chan_lock);
 370                 chan_put(chan);
 371
 372                 chan->conn = NULL;
 373                 hci_conn_put(conn->hcon);
 374         }
 375
 376         l2cap_state_change(chan, BT_CLOSED);
 377         sock_set_flag(sk, SOCK_ZAPPED);
 378
 379         if (err)
 380                 sk->sk_err = err;
 381
 382         if (parent) {
 383                 bt_accept_unlink(sk);
 384                 parent->sk_data_ready(parent, 0);
 385         } else
 386                 sk->sk_state_change(sk);
 387
 388         if (!(test_bit(CONF_OUTPUT_DONE, &chan->conf_state) &&
 389                         test_bit(CONF_INPUT_DONE, &chan->conf_state)))
 390                 return;
 391
 392         skb_queue_purge(&chan->tx_q);
 393
 394         if (chan->mode == L2CAP_MODE_ERTM) {
 395                 struct srej_list *l, *tmp;
 396
 397                 __clear_retrans_timer(chan);
 398                 __clear_monitor_timer(chan);
 399                 __clear_ack_timer(chan);
 400
 401                 skb_queue_purge(&chan->srej_q);
 402
 403                 list_for_each_entry_safe(l, tmp, &chan->srej_l, list) {
 404                         list_del(&l->list);
 405                         kfree(l);
 406                 }
 407         }
 408 }
 409
 410 static void l2cap_chan_cleanup_listen(struct sock *parent)
 411 {
 412         struct sock *sk;
 413
 414         BT_DBG("parent %p", parent);
 415
 416         /* Close not yet accepted channels */
 417         while ((sk = bt_accept_dequeue(parent, NULL))) {
 418                 struct l2cap_chan *chan = l2cap_pi(sk)->chan;
 419                 __clear_chan_timer(chan);
 420                 lock_sock(sk);
 421                 l2cap_chan_close(chan, ECONNRESET);
 422                 release_sock(sk);
 423                 chan->ops->close(chan->data);
 424         }
 425 }
 426
 427 void l2cap_chan_close(struct l2cap_chan *chan, int reason)
 428 {
 429         struct l2cap_conn *conn = chan->conn;
 430         struct sock *sk = chan->sk;
 431
 432         BT_DBG("chan %p state %d socket %p", chan, chan->state, sk->sk_socket);
 433
 434         switch (chan->state) {
 435         case BT_LISTEN:
 436                 l2cap_chan_cleanup_listen(sk);
 437
 438                 l2cap_state_change(chan, BT_CLOSED);
 439                 sock_set_flag(sk, SOCK_ZAPPED);
 440                 break;
 441
 442         case BT_CONNECTED:
 443         case BT_CONFIG:
 444                 if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED &&
 445                                         conn->hcon->type == ACL_LINK) {
 446                         __clear_chan_timer(chan);
 447                         __set_chan_timer(chan, sk->sk_sndtimeo);
 448                         l2cap_send_disconn_req(conn, chan, reason);
 449                 } else
 450                         l2cap_chan_del(chan, reason);
 451                 break;
 452
 453         case BT_CONNECT2:
 454                 if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED &&
 455                                         conn->hcon->type == ACL_LINK) {
 456                         struct l2cap_conn_rsp rsp;
 457                         __u16 result;
 458
 459                         if (bt_sk(sk)->defer_setup)
 460                                 result = L2CAP_CR_SEC_BLOCK;
 461                         else
 462                                 result = L2CAP_CR_BAD_PSM;
 463                         l2cap_state_change(chan, BT_DISCONN);
 464
 465                         rsp.scid   = cpu_to_le16(chan->dcid);
 466                         rsp.dcid   = cpu_to_le16(chan->scid);
 467                         rsp.result = cpu_to_le16(result);
 468                         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
 469                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
 470                                                         sizeof(rsp), &rsp);
 471                 }
 472
 473                 l2cap_chan_del(chan, reason);
 474                 break;
 475
 476         case BT_CONNECT:
 477         case BT_DISCONN:
 478                 l2cap_chan_del(chan, reason);
 479                 break;
 480
 481         default:
 482                 sock_set_flag(sk, SOCK_ZAPPED);
 483                 break;
 484         }
 485 }
 486
 487 static inline u8 l2cap_get_auth_type(struct l2cap_chan *chan)
 488 {
 489         if (chan->chan_type == L2CAP_CHAN_RAW) {
 490                 switch (chan->sec_level) {
 491                 case BT_SECURITY_HIGH:
 492                         return HCI_AT_DEDICATED_BONDING_MITM;
 493                 case BT_SECURITY_MEDIUM:
 494                         return HCI_AT_DEDICATED_BONDING;
 495                 default:
 496                         return HCI_AT_NO_BONDING;
 497                 }
 498         } else if (chan->psm == cpu_to_le16(0x0001)) {
 499                 if (chan->sec_level == BT_SECURITY_LOW)
 500                         chan->sec_level = BT_SECURITY_SDP;
 501
 502                 if (chan->sec_level == BT_SECURITY_HIGH)
 503                         return HCI_AT_NO_BONDING_MITM;
 504                 else
 505                         return HCI_AT_NO_BONDING;
 506         } else {
 507                 switch (chan->sec_level) {
 508                 case BT_SECURITY_HIGH:
 509                         return HCI_AT_GENERAL_BONDING_MITM;
 510                 case BT_SECURITY_MEDIUM:
 511                         return HCI_AT_GENERAL_BONDING;
 512                 default:
 513                         return HCI_AT_NO_BONDING;
 514                 }
 515         }
 516 }
 517
 518 /* Service level security */
 519 static inline int l2cap_check_security(struct l2cap_chan *chan)
 520 {
 521         struct l2cap_conn *conn = chan->conn;
 522         __u8 auth_type;
 523
 524         auth_type = l2cap_get_auth_type(chan);
 525
 526         return hci_conn_security(conn->hcon, chan->sec_level, auth_type);
 527 }
 528
 529 static u8 l2cap_get_ident(struct l2cap_conn *conn)
 530 {
 531         u8 id;
 532
 533         /* Get next available identificator.
 534          *    1 - 128 are used by kernel.
 535          *  129 - 199 are reserved.
 536          *  200 - 254 are used by utilities like l2ping, etc.
 537          */
 538
 539         spin_lock_bh(&conn->lock);
 540
 541         if (++conn->tx_ident > 128)
 542                 conn->tx_ident = 1;
 543
 544         id = conn->tx_ident;
 545
 546         spin_unlock_bh(&conn->lock);
 547
 548         return id;
 549 }
 550
 551 static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, void *data)
 552 {
 553         struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data);
 554         u8 flags;
 555
 556         BT_DBG("code 0x%2.2x", code);
 557
 558         if (!skb)
 559                 return;
 560
 561         if (lmp_no_flush_capable(conn->hcon->hdev))
 562                 flags = ACL_START_NO_FLUSH;
 563         else
 564                 flags = ACL_START;
 565
 566         bt_cb(skb)->force_active = BT_POWER_FORCE_ACTIVE_ON;
 567         skb->priority = HCI_PRIO_MAX;
 568
 569         hci_send_acl(conn->hchan, skb, flags);
 570 }
 571
 572 static void l2cap_do_send(struct l2cap_chan *chan, struct sk_buff *skb)
 573 {
 574         struct hci_conn *hcon = chan->conn->hcon;
 575         u16 flags;
 576
 577         BT_DBG("chan %p, skb %p len %d priority %u", chan, skb, skb->len,
 578                                                         skb->priority);
 579
 580         if (!test_bit(FLAG_FLUSHABLE, &chan->flags) &&
 581                                         lmp_no_flush_capable(hcon->hdev))
 582                 flags = ACL_START_NO_FLUSH;
 583         else
 584                 flags = ACL_START;
 585
 586         bt_cb(skb)->force_active = test_bit(FLAG_FORCE_ACTIVE, &chan->flags);
 587         hci_send_acl(chan->conn->hchan, skb, flags);
 588 }
 589
 590 static inline void l2cap_send_sframe(struct l2cap_chan *chan, u32 control)
 591 {
 592         struct sk_buff *skb;
 593         struct l2cap_hdr *lh;
 594         struct l2cap_conn *conn = chan->conn;
 595         int count, hlen;
 596
 597         if (chan->state != BT_CONNECTED)
 598                 return;
 599
 600         if (test_bit(FLAG_EXT_CTRL, &chan->flags))
 601                 hlen = L2CAP_EXT_HDR_SIZE;
 602         else
 603                 hlen = L2CAP_ENH_HDR_SIZE;
 604
 605         if (chan->fcs == L2CAP_FCS_CRC16)
 606                 hlen += L2CAP_FCS_SIZE;
 607
 608         BT_DBG("chan %p, control 0x%8.8x", chan, control);
 609
 610         count = min_t(unsigned int, conn->mtu, hlen);
 611
 612         control |= __set_sframe(chan);
 613
 614         if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
 615                 control |= __set_ctrl_final(chan);
 616
 617         if (test_and_clear_bit(CONN_SEND_PBIT, &chan->conn_state))
 618                 control |= __set_ctrl_poll(chan);
 619
 620         skb = bt_skb_alloc(count, GFP_ATOMIC);
 621         if (!skb)
 622                 return;
 623
 624         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
 625         lh->len = cpu_to_le16(hlen - L2CAP_HDR_SIZE);
 626         lh->cid = cpu_to_le16(chan->dcid);
 627
 628         __put_control(chan, control, skb_put(skb, __ctrl_size(chan)));
 629
 630         if (chan->fcs == L2CAP_FCS_CRC16) {
 631                 u16 fcs = crc16(0, (u8 *)lh, count - L2CAP_FCS_SIZE);
 632                 put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE));
 633         }
 634
 635         skb->priority = HCI_PRIO_MAX;
 636         l2cap_do_send(chan, skb);
 637 }
 638
 639 static inline void l2cap_send_rr_or_rnr(struct l2cap_chan *chan, u32 control)
 640 {
 641         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
 642                 control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
 643                 set_bit(CONN_RNR_SENT, &chan->conn_state);
 644         } else
 645                 control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
 646
 647         control |= __set_reqseq(chan, chan->buffer_seq);
 648
 649         l2cap_send_sframe(chan, control);
 650 }
 651
 652 static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan)
 653 {
 654         return !test_bit(CONF_CONNECT_PEND, &chan->conf_state);
 655 }
 656
 657 static void l2cap_do_start(struct l2cap_chan *chan)
 658 {
 659         struct l2cap_conn *conn = chan->conn;
 660
 661         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) {
 662                 if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE))
 663                         return;
 664
 665                 if (l2cap_check_security(chan) &&
 666                                 __l2cap_no_conn_pending(chan)) {
 667                         struct l2cap_conn_req req;
 668                         req.scid = cpu_to_le16(chan->scid);
 669                         req.psm  = chan->psm;
 670
 671                         chan->ident = l2cap_get_ident(conn);
 672                         set_bit(CONF_CONNECT_PEND, &chan->conf_state);
 673
 674                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_REQ,
 675                                                         sizeof(req), &req);
 676                 }
 677         } else {
 678                 struct l2cap_info_req req;
 679                 req.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
 680
 681                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
 682                 conn->info_ident = l2cap_get_ident(conn);
 683
 684                 mod_timer(&conn->info_timer, jiffies +
 685                                         msecs_to_jiffies(L2CAP_INFO_TIMEOUT));
 686
 687                 l2cap_send_cmd(conn, conn->info_ident,
 688                                         L2CAP_INFO_REQ, sizeof(req), &req);
 689         }
 690 }
 691
 692 static inline int l2cap_mode_supported(__u8 mode, __u32 feat_mask)
 693 {
 694         u32 local_feat_mask = l2cap_feat_mask;
 695         if (!disable_ertm)
 696                 local_feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING;
 697
 698         switch (mode) {
 699         case L2CAP_MODE_ERTM:
 700                 return L2CAP_FEAT_ERTM & feat_mask & local_feat_mask;
 701         case L2CAP_MODE_STREAMING:
 702                 return L2CAP_FEAT_STREAMING & feat_mask & local_feat_mask;
 703         default:
 704                 return 0x00;
 705         }
 706 }
 707
 708 static void l2cap_send_disconn_req(struct l2cap_conn *conn, struct l2cap_chan *chan, int err)
 709 {
 710         struct sock *sk;
 711         struct l2cap_disconn_req req;
 712
 713         if (!conn)
 714                 return;
 715
 716         sk = chan->sk;
 717
 718         if (chan->mode == L2CAP_MODE_ERTM) {
 719                 __clear_retrans_timer(chan);
 720                 __clear_monitor_timer(chan);
 721                 __clear_ack_timer(chan);
 722         }
 723
 724         req.dcid = cpu_to_le16(chan->dcid);
 725         req.scid = cpu_to_le16(chan->scid);
 726         l2cap_send_cmd(conn, l2cap_get_ident(conn),
 727                         L2CAP_DISCONN_REQ, sizeof(req), &req);
 728
 729         l2cap_state_change(chan, BT_DISCONN);
 730         sk->sk_err = err;
 731 }
 732
 733 /* ---- L2CAP connections ---- */
 734 static void l2cap_conn_start(struct l2cap_conn *conn)
 735 {
 736         struct l2cap_chan *chan, *tmp;
 737
 738         BT_DBG("conn %p", conn);
 739
 740         read_lock(&conn->chan_lock);
 741
 742         list_for_each_entry_safe(chan, tmp, &conn->chan_l, list) {
 743                 struct sock *sk = chan->sk;
 744
 745                 bh_lock_sock(sk);
 746
 747                 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
 748                         bh_unlock_sock(sk);
 749                         continue;
 750                 }
 751
 752                 if (chan->state == BT_CONNECT) {
 753                         struct l2cap_conn_req req;
 754
 755                         if (!l2cap_check_security(chan) ||
 756                                         !__l2cap_no_conn_pending(chan)) {
 757                                 bh_unlock_sock(sk);
 758                                 continue;
 759                         }
 760
 761                         if (!l2cap_mode_supported(chan->mode, conn->feat_mask)
 762                                         && test_bit(CONF_STATE2_DEVICE,
 763                                         &chan->conf_state)) {
 764                                 /* l2cap_chan_close() calls list_del(chan)
 765                                  * so release the lock */
 766                                 read_unlock(&conn->chan_lock);
 767                                 l2cap_chan_close(chan, ECONNRESET);
 768                                 read_lock(&conn->chan_lock);
 769                                 bh_unlock_sock(sk);
 770                                 continue;
 771                         }
 772
 773                         req.scid = cpu_to_le16(chan->scid);
 774                         req.psm  = chan->psm;
 775
 776                         chan->ident = l2cap_get_ident(conn);
 777                         set_bit(CONF_CONNECT_PEND, &chan->conf_state);
 778
 779                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_REQ,
 780                                                         sizeof(req), &req);
 781
 782                 } else if (chan->state == BT_CONNECT2) {
 783                         struct l2cap_conn_rsp rsp;
 784                         char buf[128];
 785                         rsp.scid = cpu_to_le16(chan->dcid);
 786                         rsp.dcid = cpu_to_le16(chan->scid);
 787
 788                         if (l2cap_check_security(chan)) {
 789                                 if (bt_sk(sk)->defer_setup) {
 790                                         struct sock *parent = bt_sk(sk)->parent;
 791                                         rsp.result = cpu_to_le16(L2CAP_CR_PEND);
 792                                         rsp.status = cpu_to_le16(L2CAP_CS_AUTHOR_PEND);
 793                                         if (parent)
 794                                                 parent->sk_data_ready(parent, 0);
 795
 796                                 } else {
 797                                         l2cap_state_change(chan, BT_CONFIG);
 798                                         rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
 799                                         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
 800                                 }
 801                         } else {
 802                                 rsp.result = cpu_to_le16(L2CAP_CR_PEND);
 803                                 rsp.status = cpu_to_le16(L2CAP_CS_AUTHEN_PEND);
 804                         }
 805
 806                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
 807                                                         sizeof(rsp), &rsp);
 808
 809                         if (test_bit(CONF_REQ_SENT, &chan->conf_state) ||
 810                                         rsp.result != L2CAP_CR_SUCCESS) {
 811                                 bh_unlock_sock(sk);
 812                                 continue;
 813                         }
 814
 815                         set_bit(CONF_REQ_SENT, &chan->conf_state);
 816                         l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
 817                                                 l2cap_build_conf_req(chan, buf), buf);
 818                         chan->num_conf_req++;
 819                 }
 820
 821                 bh_unlock_sock(sk);
 822         }
 823
 824         read_unlock(&conn->chan_lock);
 825 }
 826
 827 /* Find socket with cid and source bdaddr.
 828  * Returns closest match, locked.
 829  */
 830 static struct l2cap_chan *l2cap_global_chan_by_scid(int state, __le16 cid, bdaddr_t *src)
 831 {
 832         struct l2cap_chan *c, *c1 = NULL;
 833
 834         read_lock(&chan_list_lock);
 835
 836         list_for_each_entry(c, &chan_list, global_l) {
 837                 struct sock *sk = c->sk;
 838
 839                 if (state && c->state != state)
 840                         continue;
 841
 842                 if (c->scid == cid) {
 843                         /* Exact match. */
 844                         if (!bacmp(&bt_sk(sk)->src, src)) {
 845                                 read_unlock(&chan_list_lock);
 846                                 return c;
 847                         }
 848
 849                         /* Closest match */
 850                         if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
 851                                 c1 = c;
 852                 }
 853         }
 854
 855         read_unlock(&chan_list_lock);
 856
 857         return c1;
 858 }
 859
 860 static void l2cap_le_conn_ready(struct l2cap_conn *conn)
 861 {
 862         struct sock *parent, *sk;
 863         struct l2cap_chan *chan, *pchan;
 864
 865         BT_DBG("");
 866
 867         /* Check if we have socket listening on cid */
 868         pchan = l2cap_global_chan_by_scid(BT_LISTEN, L2CAP_CID_LE_DATA,
 869                                                         conn->src);
 870         if (!pchan)
 871                 return;
 872
 873         parent = pchan->sk;
 874
 875         bh_lock_sock(parent);
 876
 877         /* Check for backlog size */
 878         if (sk_acceptq_is_full(parent)) {
 879                 BT_DBG("backlog full %d", parent->sk_ack_backlog);
 880                 goto clean;
 881         }
 882
 883         chan = pchan->ops->new_connection(pchan->data);
 884         if (!chan)
 885                 goto clean;
 886
 887         sk = chan->sk;
 888
 889         write_lock_bh(&conn->chan_lock);
 890
 891         hci_conn_hold(conn->hcon);
 892
 893         bacpy(&bt_sk(sk)->src, conn->src);
 894         bacpy(&bt_sk(sk)->dst, conn->dst);
 895
 896         bt_accept_enqueue(parent, sk);
 897
 898         __l2cap_chan_add(conn, chan);
 899
 900         __set_chan_timer(chan, sk->sk_sndtimeo);
 901
 902         l2cap_state_change(chan, BT_CONNECTED);
 903         parent->sk_data_ready(parent, 0);
 904
 905         write_unlock_bh(&conn->chan_lock);
 906
 907 clean:
 908         bh_unlock_sock(parent);
 909 }
 910
 911 static void l2cap_chan_ready(struct sock *sk)
 912 {
 913         struct l2cap_chan *chan = l2cap_pi(sk)->chan;
 914         struct sock *parent = bt_sk(sk)->parent;
 915
 916         BT_DBG("sk %p, parent %p", sk, parent);
 917
 918         chan->conf_state = 0;
 919         __clear_chan_timer(chan);
 920
 921         l2cap_state_change(chan, BT_CONNECTED);
 922         sk->sk_state_change(sk);
 923
 924         if (parent)
 925                 parent->sk_data_ready(parent, 0);
 926 }
 927
 928 static void l2cap_conn_ready(struct l2cap_conn *conn)
 929 {
 930         struct l2cap_chan *chan;
 931
 932         BT_DBG("conn %p", conn);
 933
 934         if (!conn->hcon->out && conn->hcon->type == LE_LINK)
 935                 l2cap_le_conn_ready(conn);
 936
 937         if (conn->hcon->out && conn->hcon->type == LE_LINK)
 938                 smp_conn_security(conn, conn->hcon->pending_sec_level);
 939
 940         read_lock(&conn->chan_lock);
 941
 942         list_for_each_entry(chan, &conn->chan_l, list) {
 943                 struct sock *sk = chan->sk;
 944
 945                 bh_lock_sock(sk);
 946
 947                 if (conn->hcon->type == LE_LINK) {
 948                         if (smp_conn_security(conn, chan->sec_level))
 949                                 l2cap_chan_ready(sk);
 950
 951                 } else if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
 952                         __clear_chan_timer(chan);
 953                         l2cap_state_change(chan, BT_CONNECTED);
 954                         sk->sk_state_change(sk);
 955
 956                 } else if (chan->state == BT_CONNECT)
 957                         l2cap_do_start(chan);
 958
 959                 bh_unlock_sock(sk);
 960         }
 961
 962         read_unlock(&conn->chan_lock);
 963 }
 964
 965 /* Notify sockets that we cannot guaranty reliability anymore */
 966 static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err)
 967 {
 968         struct l2cap_chan *chan;
 969
 970         BT_DBG("conn %p", conn);
 971
 972         read_lock(&conn->chan_lock);
 973
 974         list_for_each_entry(chan, &conn->chan_l, list) {
 975                 struct sock *sk = chan->sk;
 976
 977                 if (test_bit(FLAG_FORCE_RELIABLE, &chan->flags))
 978                         sk->sk_err = err;
 979         }
 980
 981         read_unlock(&conn->chan_lock);
 982 }
 983
 984 static void l2cap_info_timeout(unsigned long arg)
 985 {
 986         struct l2cap_conn *conn = (void *) arg;
 987
 988         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
 989         conn->info_ident = 0;
 990
 991         l2cap_conn_start(conn);
 992 }
 993
 994 static void l2cap_conn_del(struct hci_conn *hcon, int err)
 995 {
 996         struct l2cap_conn *conn = hcon->l2cap_data;
 997         struct l2cap_chan *chan, *l;
 998         struct sock *sk;
 999
1000         if (!conn)
1001                 return;
1002
1003         BT_DBG("hcon %p conn %p, err %d", hcon, conn, err);
1004
1005         kfree_skb(conn->rx_skb);
1006
1007         /* Kill channels */
1008         list_for_each_entry_safe(chan, l, &conn->chan_l, list) {
1009                 sk = chan->sk;
1010                 bh_lock_sock(sk);
1011                 l2cap_chan_del(chan, err);
1012                 bh_unlock_sock(sk);
1013                 chan->ops->close(chan->data);
1014         }
1015
1016         hci_chan_del(conn->hchan);
1017
1018         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT)
1019                 del_timer_sync(&conn->info_timer);
1020
1021         if (test_and_clear_bit(HCI_CONN_LE_SMP_PEND, &hcon->pend)) {
1022                 del_timer(&conn->security_timer);
1023                 smp_chan_destroy(conn);
1024         }
1025
1026         hcon->l2cap_data = NULL;
1027         kfree(conn);
1028 }
1029
1030 static void security_timeout(unsigned long arg)
1031 {
1032         struct l2cap_conn *conn = (void *) arg;
1033
1034         l2cap_conn_del(conn->hcon, ETIMEDOUT);
1035 }
1036
1037 static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status)
1038 {
1039         struct l2cap_conn *conn = hcon->l2cap_data;
1040         struct hci_chan *hchan;
1041
1042         if (conn || status)
1043                 return conn;
1044
1045         hchan = hci_chan_create(hcon);
1046         if (!hchan)
1047                 return NULL;
1048
1049         conn = kzalloc(sizeof(struct l2cap_conn), GFP_ATOMIC);
1050         if (!conn) {
1051                 hci_chan_del(hchan);
1052                 return NULL;
1053         }
1054
1055         hcon->l2cap_data = conn;
1056         conn->hcon = hcon;
1057         conn->hchan = hchan;
1058
1059         BT_DBG("hcon %p conn %p hchan %p", hcon, conn, hchan);
1060
1061         if (hcon->hdev->le_mtu && hcon->type == LE_LINK)
1062                 conn->mtu = hcon->hdev->le_mtu;
1063         else
1064                 conn->mtu = hcon->hdev->acl_mtu;
1065
1066         conn->src = &hcon->hdev->bdaddr;
1067         conn->dst = &hcon->dst;
1068
1069         conn->feat_mask = 0;
1070
1071         spin_lock_init(&conn->lock);
1072         rwlock_init(&conn->chan_lock);
1073
1074         INIT_LIST_HEAD(&conn->chan_l);
1075
1076         if (hcon->type == LE_LINK)
1077                 setup_timer(&conn->security_timer, security_timeout,
1078                                                 (unsigned long) conn);
1079         else
1080                 setup_timer(&conn->info_timer, l2cap_info_timeout,
1081                                                 (unsigned long) conn);
1082
1083         conn->disc_reason = 0x13;
1084
1085         return conn;
1086 }
1087
1088 static inline void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
1089 {
1090         write_lock_bh(&conn->chan_lock);
1091         __l2cap_chan_add(conn, chan);
1092         write_unlock_bh(&conn->chan_lock);
1093 }
1094
1095 /* ---- Socket interface ---- */
1096
1097 /* Find socket with psm and source bdaddr.
1098  * Returns closest match.
1099  */
1100 static struct l2cap_chan *l2cap_global_chan_by_psm(int state, __le16 psm, bdaddr_t *src)
1101 {
1102         struct l2cap_chan *c, *c1 = NULL;
1103
1104         read_lock(&chan_list_lock);
1105
1106         list_for_each_entry(c, &chan_list, global_l) {
1107                 struct sock *sk = c->sk;
1108
1109                 if (state && c->state != state)
1110                         continue;
1111
1112                 if (c->psm == psm) {
1113                         /* Exact match. */
1114                         if (!bacmp(&bt_sk(sk)->src, src)) {
1115                                 read_unlock(&chan_list_lock);
1116                                 return c;
1117                         }
1118
1119                         /* Closest match */
1120                         if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
1121                                 c1 = c;
1122                 }
1123         }
1124
1125         read_unlock(&chan_list_lock);
1126
1127         return c1;
1128 }
1129
1130 int l2cap_chan_connect(struct l2cap_chan *chan)
1131 {
1132         struct sock *sk = chan->sk;
1133         bdaddr_t *src = &bt_sk(sk)->src;
1134         bdaddr_t *dst = &bt_sk(sk)->dst;
1135         struct l2cap_conn *conn;
1136         struct hci_conn *hcon;
1137         struct hci_dev *hdev;
1138         __u8 auth_type;
1139         int err;
1140
1141         BT_DBG("%s -> %s psm 0x%2.2x", batostr(src), batostr(dst),
1142                                                         chan->psm);
1143
1144         hdev = hci_get_route(dst, src);
1145         if (!hdev)
1146                 return -EHOSTUNREACH;
1147
1148         hci_dev_lock_bh(hdev);
1149
1150         auth_type = l2cap_get_auth_type(chan);
1151
1152         if (chan->dcid == L2CAP_CID_LE_DATA)
1153                 hcon = hci_connect(hdev, LE_LINK, dst,
1154                                         chan->sec_level, auth_type);
1155         else
1156                 hcon = hci_connect(hdev, ACL_LINK, dst,
1157                                         chan->sec_level, auth_type);
1158
1159         if (IS_ERR(hcon)) {
1160                 err = PTR_ERR(hcon);
1161                 goto done;
1162         }
1163
1164         conn = l2cap_conn_add(hcon, 0);
1165         if (!conn) {
1166                 hci_conn_put(hcon);
1167                 err = -ENOMEM;
1168                 goto done;
1169         }
1170
1171         /* Update source addr of the socket */
1172         bacpy(src, conn->src);
1173
1174         l2cap_chan_add(conn, chan);
1175
1176         l2cap_state_change(chan, BT_CONNECT);
1177         __set_chan_timer(chan, sk->sk_sndtimeo);
1178
1179         if (hcon->state == BT_CONNECTED) {
1180                 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
1181                         __clear_chan_timer(chan);
1182                         if (l2cap_check_security(chan))
1183                                 l2cap_state_change(chan, BT_CONNECTED);
1184                 } else
1185                         l2cap_do_start(chan);
1186         }
1187
1188         err = 0;
1189
1190 done:
1191         hci_dev_unlock_bh(hdev);
1192         hci_dev_put(hdev);
1193         return err;
1194 }
1195
1196 int __l2cap_wait_ack(struct sock *sk)
1197 {
1198         struct l2cap_chan *chan = l2cap_pi(sk)->chan;
1199         DECLARE_WAITQUEUE(wait, current);
1200         int err = 0;
1201         int timeo = HZ/5;
1202
1203         add_wait_queue(sk_sleep(sk), &wait);
1204         set_current_state(TASK_INTERRUPTIBLE);
1205         while (chan->unacked_frames > 0 && chan->conn) {
1206                 if (!timeo)
1207                         timeo = HZ/5;
1208
1209                 if (signal_pending(current)) {
1210                         err = sock_intr_errno(timeo);
1211                         break;
1212                 }
1213
1214                 release_sock(sk);
1215                 timeo = schedule_timeout(timeo);
1216                 lock_sock(sk);
1217                 set_current_state(TASK_INTERRUPTIBLE);
1218
1219                 err = sock_error(sk);
1220                 if (err)
1221                         break;
1222         }
1223         set_current_state(TASK_RUNNING);
1224         remove_wait_queue(sk_sleep(sk), &wait);
1225         return err;
1226 }
1227
1228 static void l2cap_monitor_timeout(unsigned long arg)
1229 {
1230         struct l2cap_chan *chan = (void *) arg;
1231         struct sock *sk = chan->sk;
1232
1233         BT_DBG("chan %p", chan);
1234
1235         bh_lock_sock(sk);
1236         if (chan->retry_count >= chan->remote_max_tx) {
1237                 l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED);
1238                 bh_unlock_sock(sk);
1239                 return;
1240         }
1241
1242         chan->retry_count++;
1243         __set_monitor_timer(chan);
1244
1245         l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_POLL);
1246         bh_unlock_sock(sk);
1247 }
1248
1249 static void l2cap_retrans_timeout(unsigned long arg)
1250 {
1251         struct l2cap_chan *chan = (void *) arg;
1252         struct sock *sk = chan->sk;
1253
1254         BT_DBG("chan %p", chan);
1255
1256         bh_lock_sock(sk);
1257         chan->retry_count = 1;
1258         __set_monitor_timer(chan);
1259
1260         set_bit(CONN_WAIT_F, &chan->conn_state);
1261
1262         l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_POLL);
1263         bh_unlock_sock(sk);
1264 }
1265
1266 static void l2cap_drop_acked_frames(struct l2cap_chan *chan)
1267 {
1268         struct sk_buff *skb;
1269
1270         while ((skb = skb_peek(&chan->tx_q)) &&
1271                         chan->unacked_frames) {
1272                 if (bt_cb(skb)->tx_seq == chan->expected_ack_seq)
1273                         break;
1274
1275                 skb = skb_dequeue(&chan->tx_q);
1276                 kfree_skb(skb);
1277
1278                 chan->unacked_frames--;
1279         }
1280
1281         if (!chan->unacked_frames)
1282                 __clear_retrans_timer(chan);
1283 }
1284
1285 static void l2cap_streaming_send(struct l2cap_chan *chan)
1286 {
1287         struct sk_buff *skb;
1288         u32 control;
1289         u16 fcs;
1290
1291         while ((skb = skb_dequeue(&chan->tx_q))) {
1292                 control = __get_control(chan, skb->data + L2CAP_HDR_SIZE);
1293                 control |= __set_txseq(chan, chan->next_tx_seq);
1294                 __put_control(chan, control, skb->data + L2CAP_HDR_SIZE);
1295
1296                 if (chan->fcs == L2CAP_FCS_CRC16) {
1297                         fcs = crc16(0, (u8 *)skb->data,
1298                                                 skb->len - L2CAP_FCS_SIZE);
1299                         put_unaligned_le16(fcs,
1300                                         skb->data + skb->len - L2CAP_FCS_SIZE);
1301                 }
1302
1303                 l2cap_do_send(chan, skb);
1304
1305                 chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq);
1306         }
1307 }
1308
1309 static void l2cap_retransmit_one_frame(struct l2cap_chan *chan, u16 tx_seq)
1310 {
1311         struct sk_buff *skb, *tx_skb;
1312         u16 fcs;
1313         u32 control;
1314
1315         skb = skb_peek(&chan->tx_q);
1316         if (!skb)
1317                 return;
1318
1319         do {
1320                 if (bt_cb(skb)->tx_seq == tx_seq)
1321                         break;
1322
1323                 if (skb_queue_is_last(&chan->tx_q, skb))
1324                         return;
1325
1326         } while ((skb = skb_queue_next(&chan->tx_q, skb)));
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
2353         BT_DBG("chan %p, rsp %p, len %d, req %p", chan, rsp, len, data);
2354
2355         while (len >= L2CAP_CONF_OPT_SIZE) {
2356                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
2357
2358                 switch (type) {
2359                 case L2CAP_CONF_MTU:
2360                         if (val < L2CAP_DEFAULT_MIN_MTU) {
2361                                 *result = L2CAP_CONF_UNACCEPT;
2362                                 chan->imtu = L2CAP_DEFAULT_MIN_MTU;
2363                         } else
2364                                 chan->imtu = val;
2365                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu);
2366                         break;
2367
2368                 case L2CAP_CONF_FLUSH_TO:
2369                         chan->flush_to = val;
2370                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
2371                                                         2, chan->flush_to);
2372                         break;
2373
2374                 case L2CAP_CONF_RFC:
2375                         if (olen == sizeof(rfc))
2376                                 memcpy(&rfc, (void *)val, olen);
2377
2378                         if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state) &&
2379                                                         rfc.mode != chan->mode)
2380                                 return -ECONNREFUSED;
2381
2382                         chan->fcs = 0;
2383
2384                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2385                                         sizeof(rfc), (unsigned long) &rfc);
2386                         break;
2387
2388                 case L2CAP_CONF_EWS:
2389                         chan->tx_win = min_t(u16, val,
2390                                                 L2CAP_DEFAULT_EXT_WINDOW);
2391                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
2392                                                         chan->tx_win);
2393                         break;
2394                 }
2395         }
2396
2397         if (chan->mode == L2CAP_MODE_BASIC && chan->mode != rfc.mode)
2398                 return -ECONNREFUSED;
2399
2400         chan->mode = rfc.mode;
2401
2402         if (*result == L2CAP_CONF_SUCCESS || *result == L2CAP_CONF_PENDING) {
2403                 switch (rfc.mode) {
2404                 case L2CAP_MODE_ERTM:
2405                         chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
2406                         chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
2407                         chan->mps    = le16_to_cpu(rfc.max_pdu_size);
2408                         break;
2409                 case L2CAP_MODE_STREAMING:
2410                         chan->mps    = le16_to_cpu(rfc.max_pdu_size);
2411                 }
2412         }
2413
2414         req->dcid   = cpu_to_le16(chan->dcid);
2415         req->flags  = cpu_to_le16(0x0000);
2416
2417         return ptr - data;
2418 }
2419
2420 static int l2cap_build_conf_rsp(struct l2cap_chan *chan, void *data, u16 result, u16 flags)
2421 {
2422         struct l2cap_conf_rsp *rsp = data;
2423         void *ptr = rsp->data;
2424
2425         BT_DBG("chan %p", chan);
2426
2427         rsp->scid   = cpu_to_le16(chan->dcid);
2428         rsp->result = cpu_to_le16(result);
2429         rsp->flags  = cpu_to_le16(flags);
2430
2431         return ptr - data;
2432 }
2433
2434 void __l2cap_connect_rsp_defer(struct l2cap_chan *chan)
2435 {
2436         struct l2cap_conn_rsp rsp;
2437         struct l2cap_conn *conn = chan->conn;
2438         u8 buf[128];
2439
2440         rsp.scid   = cpu_to_le16(chan->dcid);
2441         rsp.dcid   = cpu_to_le16(chan->scid);
2442         rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
2443         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
2444         l2cap_send_cmd(conn, chan->ident,
2445                                 L2CAP_CONN_RSP, sizeof(rsp), &rsp);
2446
2447         if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
2448                 return;
2449
2450         l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2451                         l2cap_build_conf_req(chan, buf), buf);
2452         chan->num_conf_req++;
2453 }
2454
2455 static void l2cap_conf_rfc_get(struct l2cap_chan *chan, void *rsp, int len)
2456 {
2457         int type, olen;
2458         unsigned long val;
2459         struct l2cap_conf_rfc rfc;
2460
2461         BT_DBG("chan %p, rsp %p, len %d", chan, rsp, len);
2462
2463         if ((chan->mode != L2CAP_MODE_ERTM) && (chan->mode != L2CAP_MODE_STREAMING))
2464                 return;
2465
2466         while (len >= L2CAP_CONF_OPT_SIZE) {
2467                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
2468
2469                 switch (type) {
2470                 case L2CAP_CONF_RFC:
2471                         if (olen == sizeof(rfc))
2472                                 memcpy(&rfc, (void *)val, olen);
2473                         goto done;
2474                 }
2475         }
2476
2477 done:
2478         switch (rfc.mode) {
2479         case L2CAP_MODE_ERTM:
2480                 chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
2481                 chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
2482                 chan->mps    = le16_to_cpu(rfc.max_pdu_size);
2483                 break;
2484         case L2CAP_MODE_STREAMING:
2485                 chan->mps    = le16_to_cpu(rfc.max_pdu_size);
2486         }
2487 }
2488
2489 static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2490 {
2491         struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data;
2492
2493         if (rej->reason != L2CAP_REJ_NOT_UNDERSTOOD)
2494                 return 0;
2495
2496         if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
2497                                         cmd->ident == conn->info_ident) {
2498                 del_timer(&conn->info_timer);
2499
2500                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2501                 conn->info_ident = 0;
2502
2503                 l2cap_conn_start(conn);
2504         }
2505
2506         return 0;
2507 }
2508
2509 static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2510 {
2511         struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
2512         struct l2cap_conn_rsp rsp;
2513         struct l2cap_chan *chan = NULL, *pchan;
2514         struct sock *parent, *sk = NULL;
2515         int result, status = L2CAP_CS_NO_INFO;
2516
2517         u16 dcid = 0, scid = __le16_to_cpu(req->scid);
2518         __le16 psm = req->psm;
2519
2520         BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid);
2521
2522         /* Check if we have socket listening on psm */
2523         pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, conn->src);
2524         if (!pchan) {
2525                 result = L2CAP_CR_BAD_PSM;
2526                 goto sendresp;
2527         }
2528
2529         parent = pchan->sk;
2530
2531         bh_lock_sock(parent);
2532
2533         /* Check if the ACL is secure enough (if not SDP) */
2534         if (psm != cpu_to_le16(0x0001) &&
2535                                 !hci_conn_check_link_mode(conn->hcon)) {
2536                 conn->disc_reason = 0x05;
2537                 result = L2CAP_CR_SEC_BLOCK;
2538                 goto response;
2539         }
2540
2541         result = L2CAP_CR_NO_MEM;
2542
2543         /* Check for backlog size */
2544         if (sk_acceptq_is_full(parent)) {
2545                 BT_DBG("backlog full %d", parent->sk_ack_backlog);
2546                 goto response;
2547         }
2548
2549         chan = pchan->ops->new_connection(pchan->data);
2550         if (!chan)
2551                 goto response;
2552
2553         sk = chan->sk;
2554
2555         write_lock_bh(&conn->chan_lock);
2556
2557         /* Check if we already have channel with that dcid */
2558         if (__l2cap_get_chan_by_dcid(conn, scid)) {
2559                 write_unlock_bh(&conn->chan_lock);
2560                 sock_set_flag(sk, SOCK_ZAPPED);
2561                 chan->ops->close(chan->data);
2562                 goto response;
2563         }
2564
2565         hci_conn_hold(conn->hcon);
2566
2567         bacpy(&bt_sk(sk)->src, conn->src);
2568         bacpy(&bt_sk(sk)->dst, conn->dst);
2569         chan->psm  = psm;
2570         chan->dcid = scid;
2571
2572         bt_accept_enqueue(parent, sk);
2573
2574         __l2cap_chan_add(conn, chan);
2575
2576         dcid = chan->scid;
2577
2578         __set_chan_timer(chan, sk->sk_sndtimeo);
2579
2580         chan->ident = cmd->ident;
2581
2582         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
2583                 if (l2cap_check_security(chan)) {
2584                         if (bt_sk(sk)->defer_setup) {
2585                                 l2cap_state_change(chan, BT_CONNECT2);
2586                                 result = L2CAP_CR_PEND;
2587                                 status = L2CAP_CS_AUTHOR_PEND;
2588                                 parent->sk_data_ready(parent, 0);
2589                         } else {
2590                                 l2cap_state_change(chan, BT_CONFIG);
2591                                 result = L2CAP_CR_SUCCESS;
2592                                 status = L2CAP_CS_NO_INFO;
2593                         }
2594                 } else {
2595                         l2cap_state_change(chan, BT_CONNECT2);
2596                         result = L2CAP_CR_PEND;
2597                         status = L2CAP_CS_AUTHEN_PEND;
2598                 }
2599         } else {
2600                 l2cap_state_change(chan, BT_CONNECT2);
2601                 result = L2CAP_CR_PEND;
2602                 status = L2CAP_CS_NO_INFO;
2603         }
2604
2605         write_unlock_bh(&conn->chan_lock);
2606
2607 response:
2608         bh_unlock_sock(parent);
2609
2610 sendresp:
2611         rsp.scid   = cpu_to_le16(scid);
2612         rsp.dcid   = cpu_to_le16(dcid);
2613         rsp.result = cpu_to_le16(result);
2614         rsp.status = cpu_to_le16(status);
2615         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
2616
2617         if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
2618                 struct l2cap_info_req info;
2619                 info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
2620
2621                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
2622                 conn->info_ident = l2cap_get_ident(conn);
2623
2624                 mod_timer(&conn->info_timer, jiffies +
2625                                         msecs_to_jiffies(L2CAP_INFO_TIMEOUT));
2626
2627                 l2cap_send_cmd(conn, conn->info_ident,
2628                                         L2CAP_INFO_REQ, sizeof(info), &info);
2629         }
2630
2631         if (chan && !test_bit(CONF_REQ_SENT, &chan->conf_state) &&
2632                                 result == L2CAP_CR_SUCCESS) {
2633                 u8 buf[128];
2634                 set_bit(CONF_REQ_SENT, &chan->conf_state);
2635                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2636                                         l2cap_build_conf_req(chan, buf), buf);
2637                 chan->num_conf_req++;
2638         }
2639
2640         return 0;
2641 }
2642
2643 static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2644 {
2645         struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
2646         u16 scid, dcid, result, status;
2647         struct l2cap_chan *chan;
2648         struct sock *sk;
2649         u8 req[128];
2650
2651         scid   = __le16_to_cpu(rsp->scid);
2652         dcid   = __le16_to_cpu(rsp->dcid);
2653         result = __le16_to_cpu(rsp->result);
2654         status = __le16_to_cpu(rsp->status);
2655
2656         BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status);
2657
2658         if (scid) {
2659                 chan = l2cap_get_chan_by_scid(conn, scid);
2660                 if (!chan)
2661                         return -EFAULT;
2662         } else {
2663                 chan = l2cap_get_chan_by_ident(conn, cmd->ident);
2664                 if (!chan)
2665                         return -EFAULT;
2666         }
2667
2668         sk = chan->sk;
2669
2670         switch (result) {
2671         case L2CAP_CR_SUCCESS:
2672                 l2cap_state_change(chan, BT_CONFIG);
2673                 chan->ident = 0;
2674                 chan->dcid = dcid;
2675                 clear_bit(CONF_CONNECT_PEND, &chan->conf_state);
2676
2677                 if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
2678                         break;
2679
2680                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2681                                         l2cap_build_conf_req(chan, req), req);
2682                 chan->num_conf_req++;
2683                 break;
2684
2685         case L2CAP_CR_PEND:
2686                 set_bit(CONF_CONNECT_PEND, &chan->conf_state);
2687                 break;
2688
2689         default:
2690                 /* don't delete l2cap channel if sk is owned by user */
2691                 if (sock_owned_by_user(sk)) {
2692                         l2cap_state_change(chan, BT_DISCONN);
2693                         __clear_chan_timer(chan);
2694                         __set_chan_timer(chan, HZ / 5);
2695                         break;
2696                 }
2697
2698                 l2cap_chan_del(chan, ECONNREFUSED);
2699                 break;
2700         }
2701
2702         bh_unlock_sock(sk);
2703         return 0;
2704 }
2705
2706 static inline void set_default_fcs(struct l2cap_chan *chan)
2707 {
2708         /* FCS is enabled only in ERTM or streaming mode, if one or both
2709          * sides request it.
2710          */
2711         if (chan->mode != L2CAP_MODE_ERTM && chan->mode != L2CAP_MODE_STREAMING)
2712                 chan->fcs = L2CAP_FCS_NONE;
2713         else if (!test_bit(CONF_NO_FCS_RECV, &chan->conf_state))
2714                 chan->fcs = L2CAP_FCS_CRC16;
2715 }
2716
2717 static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
2718 {
2719         struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
2720         u16 dcid, flags;
2721         u8 rsp[64];
2722         struct l2cap_chan *chan;
2723         struct sock *sk;
2724         int len;
2725
2726         dcid  = __le16_to_cpu(req->dcid);
2727         flags = __le16_to_cpu(req->flags);
2728
2729         BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
2730
2731         chan = l2cap_get_chan_by_scid(conn, dcid);
2732         if (!chan)
2733                 return -ENOENT;
2734
2735         sk = chan->sk;
2736
2737         if (chan->state != BT_CONFIG && chan->state != BT_CONNECT2) {
2738                 struct l2cap_cmd_rej_cid rej;
2739
2740                 rej.reason = cpu_to_le16(L2CAP_REJ_INVALID_CID);
2741                 rej.scid = cpu_to_le16(chan->scid);
2742                 rej.dcid = cpu_to_le16(chan->dcid);
2743
2744                 l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
2745                                 sizeof(rej), &rej);
2746                 goto unlock;
2747         }
2748
2749         /* Reject if config buffer is too small. */
2750         len = cmd_len - sizeof(*req);
2751         if (len < 0 || chan->conf_len + len > sizeof(chan->conf_req)) {
2752                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2753                                 l2cap_build_conf_rsp(chan, rsp,
2754                                         L2CAP_CONF_REJECT, flags), rsp);
2755                 goto unlock;
2756         }
2757
2758         /* Store config. */
2759         memcpy(chan->conf_req + chan->conf_len, req->data, len);
2760         chan->conf_len += len;
2761
2762         if (flags & 0x0001) {
2763                 /* Incomplete config. Send empty response. */
2764                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2765                                 l2cap_build_conf_rsp(chan, rsp,
2766                                         L2CAP_CONF_SUCCESS, 0x0001), rsp);
2767                 goto unlock;
2768         }
2769
2770         /* Complete config. */
2771         len = l2cap_parse_conf_req(chan, rsp);
2772         if (len < 0) {
2773                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
2774                 goto unlock;
2775         }
2776
2777         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
2778         chan->num_conf_rsp++;
2779
2780         /* Reset config buffer. */
2781         chan->conf_len = 0;
2782
2783         if (!test_bit(CONF_OUTPUT_DONE, &chan->conf_state))
2784                 goto unlock;
2785
2786         if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
2787                 set_default_fcs(chan);
2788
2789                 l2cap_state_change(chan, BT_CONNECTED);
2790
2791                 chan->next_tx_seq = 0;
2792                 chan->expected_tx_seq = 0;
2793                 skb_queue_head_init(&chan->tx_q);
2794                 if (chan->mode == L2CAP_MODE_ERTM)
2795                         l2cap_ertm_init(chan);
2796
2797                 l2cap_chan_ready(sk);
2798                 goto unlock;
2799         }
2800
2801         if (!test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) {
2802                 u8 buf[64];
2803                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2804                                         l2cap_build_conf_req(chan, buf), buf);
2805                 chan->num_conf_req++;
2806         }
2807
2808         /* Got Conf Rsp PENDING from remote side and asume we sent
2809            Conf Rsp PENDING in the code above */
2810         if (test_bit(CONF_REM_CONF_PEND, &chan->conf_state) &&
2811                         test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
2812
2813                 /* check compatibility */
2814
2815                 clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
2816                 set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
2817
2818                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2819                                         l2cap_build_conf_rsp(chan, rsp,
2820                                         L2CAP_CONF_SUCCESS, 0x0000), rsp);
2821         }
2822
2823 unlock:
2824         bh_unlock_sock(sk);
2825         return 0;
2826 }
2827
2828 static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2829 {
2830         struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
2831         u16 scid, flags, result;
2832         struct l2cap_chan *chan;
2833         struct sock *sk;
2834         int len = cmd->len - sizeof(*rsp);
2835
2836         scid   = __le16_to_cpu(rsp->scid);
2837         flags  = __le16_to_cpu(rsp->flags);
2838         result = __le16_to_cpu(rsp->result);
2839
2840         BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x",
2841                         scid, flags, result);
2842
2843         chan = l2cap_get_chan_by_scid(conn, scid);
2844         if (!chan)
2845                 return 0;
2846
2847         sk = chan->sk;
2848
2849         switch (result) {
2850         case L2CAP_CONF_SUCCESS:
2851                 l2cap_conf_rfc_get(chan, rsp->data, len);
2852                 clear_bit(CONF_REM_CONF_PEND, &chan->conf_state);
2853                 break;
2854
2855         case L2CAP_CONF_PENDING:
2856                 set_bit(CONF_REM_CONF_PEND, &chan->conf_state);
2857
2858                 if (test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
2859                         char buf[64];
2860
2861                         len = l2cap_parse_conf_rsp(chan, rsp->data, len,
2862                                                                 buf, &result);
2863                         if (len < 0) {
2864                                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
2865                                 goto done;
2866                         }
2867
2868                         /* check compatibility */
2869
2870                         clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
2871                         set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
2872
2873                         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2874                                                 l2cap_build_conf_rsp(chan, buf,
2875                                                 L2CAP_CONF_SUCCESS, 0x0000), buf);
2876                 }
2877                 goto done;
2878
2879         case L2CAP_CONF_UNACCEPT:
2880                 if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
2881                         char req[64];
2882
2883                         if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
2884                                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
2885                                 goto done;
2886                         }
2887
2888                         /* throw out any old stored conf requests */
2889                         result = L2CAP_CONF_SUCCESS;
2890                         len = l2cap_parse_conf_rsp(chan, rsp->data, len,
2891                                                                 req, &result);
2892                         if (len < 0) {
2893                                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
2894                                 goto done;
2895                         }
2896
2897                         l2cap_send_cmd(conn, l2cap_get_ident(conn),
2898                                                 L2CAP_CONF_REQ, len, req);
2899                         chan->num_conf_req++;
2900                         if (result != L2CAP_CONF_SUCCESS)
2901                                 goto done;
2902                         break;
2903                 }
2904
2905         default:
2906                 sk->sk_err = ECONNRESET;
2907                 __set_chan_timer(chan, HZ * 5);
2908                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
2909                 goto done;
2910         }
2911
2912         if (flags & 0x01)
2913                 goto done;
2914
2915         set_bit(CONF_INPUT_DONE, &chan->conf_state);
2916
2917         if (test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) {
2918                 set_default_fcs(chan);
2919
2920                 l2cap_state_change(chan, BT_CONNECTED);
2921                 chan->next_tx_seq = 0;
2922                 chan->expected_tx_seq = 0;
2923                 skb_queue_head_init(&chan->tx_q);
2924                 if (chan->mode ==  L2CAP_MODE_ERTM)
2925                         l2cap_ertm_init(chan);
2926
2927                 l2cap_chan_ready(sk);
2928         }
2929
2930 done:
2931         bh_unlock_sock(sk);
2932         return 0;
2933 }
2934
2935 static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2936 {
2937         struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
2938         struct l2cap_disconn_rsp rsp;
2939         u16 dcid, scid;
2940         struct l2cap_chan *chan;
2941         struct sock *sk;
2942
2943         scid = __le16_to_cpu(req->scid);
2944         dcid = __le16_to_cpu(req->dcid);
2945
2946         BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
2947
2948         chan = l2cap_get_chan_by_scid(conn, dcid);
2949         if (!chan)
2950                 return 0;
2951
2952         sk = chan->sk;
2953
2954         rsp.dcid = cpu_to_le16(chan->scid);
2955         rsp.scid = cpu_to_le16(chan->dcid);
2956         l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
2957
2958         sk->sk_shutdown = SHUTDOWN_MASK;
2959
2960         /* don't delete l2cap channel if sk is owned by user */
2961         if (sock_owned_by_user(sk)) {
2962                 l2cap_state_change(chan, BT_DISCONN);
2963                 __clear_chan_timer(chan);
2964                 __set_chan_timer(chan, HZ / 5);
2965                 bh_unlock_sock(sk);
2966                 return 0;
2967         }
2968
2969         l2cap_chan_del(chan, ECONNRESET);
2970         bh_unlock_sock(sk);
2971
2972         chan->ops->close(chan->data);
2973         return 0;
2974 }
2975
2976 static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2977 {
2978         struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
2979         u16 dcid, scid;
2980         struct l2cap_chan *chan;
2981         struct sock *sk;
2982
2983         scid = __le16_to_cpu(rsp->scid);
2984         dcid = __le16_to_cpu(rsp->dcid);
2985
2986         BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
2987
2988         chan = l2cap_get_chan_by_scid(conn, scid);
2989         if (!chan)
2990                 return 0;
2991
2992         sk = chan->sk;
2993
2994         /* don't delete l2cap channel if sk is owned by user */
2995         if (sock_owned_by_user(sk)) {
2996                 l2cap_state_change(chan,BT_DISCONN);
2997                 __clear_chan_timer(chan);
2998                 __set_chan_timer(chan, HZ / 5);
2999                 bh_unlock_sock(sk);
3000                 return 0;
3001         }
3002
3003         l2cap_chan_del(chan, 0);
3004         bh_unlock_sock(sk);
3005
3006         chan->ops->close(chan->data);
3007         return 0;
3008 }
3009
3010 static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3011 {
3012         struct l2cap_info_req *req = (struct l2cap_info_req *) data;
3013         u16 type;
3014
3015         type = __le16_to_cpu(req->type);
3016
3017         BT_DBG("type 0x%4.4x", type);
3018
3019         if (type == L2CAP_IT_FEAT_MASK) {
3020                 u8 buf[8];
3021                 u32 feat_mask = l2cap_feat_mask;
3022                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
3023                 rsp->type   = cpu_to_le16(L2CAP_IT_FEAT_MASK);
3024                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
3025                 if (!disable_ertm)
3026                         feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
3027                                                          | L2CAP_FEAT_FCS;
3028                 if (enable_hs)
3029                         feat_mask |= L2CAP_FEAT_EXT_FLOW
3030                                                 | L2CAP_FEAT_EXT_WINDOW;
3031
3032                 put_unaligned_le32(feat_mask, rsp->data);
3033                 l2cap_send_cmd(conn, cmd->ident,
3034                                         L2CAP_INFO_RSP, sizeof(buf), buf);
3035         } else if (type == L2CAP_IT_FIXED_CHAN) {
3036                 u8 buf[12];
3037                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
3038                 rsp->type   = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
3039                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
3040                 memcpy(rsp->data, l2cap_fixed_chan, sizeof(l2cap_fixed_chan));
3041                 l2cap_send_cmd(conn, cmd->ident,
3042                                         L2CAP_INFO_RSP, sizeof(buf), buf);
3043         } else {
3044                 struct l2cap_info_rsp rsp;
3045                 rsp.type   = cpu_to_le16(type);
3046                 rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP);
3047                 l2cap_send_cmd(conn, cmd->ident,
3048                                         L2CAP_INFO_RSP, sizeof(rsp), &rsp);
3049         }
3050
3051         return 0;
3052 }
3053
3054 static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3055 {
3056         struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
3057         u16 type, result;
3058
3059         type   = __le16_to_cpu(rsp->type);
3060         result = __le16_to_cpu(rsp->result);
3061
3062         BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
3063
3064         /* L2CAP Info req/rsp are unbound to channels, add extra checks */
3065         if (cmd->ident != conn->info_ident ||
3066                         conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)
3067                 return 0;
3068
3069         del_timer(&conn->info_timer);
3070
3071         if (result != L2CAP_IR_SUCCESS) {
3072                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3073                 conn->info_ident = 0;
3074
3075                 l2cap_conn_start(conn);
3076
3077                 return 0;
3078         }
3079
3080         if (type == L2CAP_IT_FEAT_MASK) {
3081                 conn->feat_mask = get_unaligned_le32(rsp->data);
3082
3083                 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
3084                         struct l2cap_info_req req;
3085                         req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
3086
3087                         conn->info_ident = l2cap_get_ident(conn);
3088
3089                         l2cap_send_cmd(conn, conn->info_ident,
3090                                         L2CAP_INFO_REQ, sizeof(req), &req);
3091                 } else {
3092                         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3093                         conn->info_ident = 0;
3094
3095                         l2cap_conn_start(conn);
3096                 }
3097         } else if (type == L2CAP_IT_FIXED_CHAN) {
3098                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3099                 conn->info_ident = 0;
3100
3101                 l2cap_conn_start(conn);
3102         }
3103
3104         return 0;
3105 }
3106
3107 static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency,
3108                                                         u16 to_multiplier)
3109 {
3110         u16 max_latency;
3111
3112         if (min > max || min < 6 || max > 3200)
3113                 return -EINVAL;
3114
3115         if (to_multiplier < 10 || to_multiplier > 3200)
3116                 return -EINVAL;
3117
3118         if (max >= to_multiplier * 8)
3119                 return -EINVAL;
3120
3121         max_latency = (to_multiplier * 8 / max) - 1;
3122         if (latency > 499 || latency > max_latency)
3123                 return -EINVAL;
3124
3125         return 0;
3126 }
3127
3128 static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
3129                                         struct l2cap_cmd_hdr *cmd, u8 *data)
3130 {
3131         struct hci_conn *hcon = conn->hcon;
3132         struct l2cap_conn_param_update_req *req;
3133         struct l2cap_conn_param_update_rsp rsp;
3134         u16 min, max, latency, to_multiplier, cmd_len;
3135         int err;
3136
3137         if (!(hcon->link_mode & HCI_LM_MASTER))
3138                 return -EINVAL;
3139
3140         cmd_len = __le16_to_cpu(cmd->len);
3141         if (cmd_len != sizeof(struct l2cap_conn_param_update_req))
3142                 return -EPROTO;
3143
3144         req = (struct l2cap_conn_param_update_req *) data;
3145         min             = __le16_to_cpu(req->min);
3146         max             = __le16_to_cpu(req->max);
3147         latency         = __le16_to_cpu(req->latency);
3148         to_multiplier   = __le16_to_cpu(req->to_multiplier);
3149
3150         BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x",
3151                                                 min, max, latency, to_multiplier);
3152
3153         memset(&rsp, 0, sizeof(rsp));
3154
3155         err = l2cap_check_conn_param(min, max, latency, to_multiplier);
3156         if (err)
3157                 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_REJECTED);
3158         else
3159                 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED);
3160
3161         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP,
3162                                                         sizeof(rsp), &rsp);
3163
3164         if (!err)
3165                 hci_le_conn_update(hcon, min, max, latency, to_multiplier);
3166
3167         return 0;
3168 }
3169
3170 static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
3171                         struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
3172 {
3173         int err = 0;
3174
3175         switch (cmd->code) {
3176         case L2CAP_COMMAND_REJ:
3177                 l2cap_command_rej(conn, cmd, data);
3178                 break;
3179
3180         case L2CAP_CONN_REQ:
3181                 err = l2cap_connect_req(conn, cmd, data);
3182                 break;
3183
3184         case L2CAP_CONN_RSP:
3185                 err = l2cap_connect_rsp(conn, cmd, data);
3186                 break;
3187
3188         case L2CAP_CONF_REQ:
3189                 err = l2cap_config_req(conn, cmd, cmd_len, data);
3190                 break;
3191
3192         case L2CAP_CONF_RSP:
3193                 err = l2cap_config_rsp(conn, cmd, data);
3194                 break;
3195
3196         case L2CAP_DISCONN_REQ:
3197                 err = l2cap_disconnect_req(conn, cmd, data);
3198                 break;
3199
3200         case L2CAP_DISCONN_RSP:
3201                 err = l2cap_disconnect_rsp(conn, cmd, data);
3202                 break;
3203
3204         case L2CAP_ECHO_REQ:
3205                 l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data);
3206                 break;
3207
3208         case L2CAP_ECHO_RSP:
3209                 break;
3210
3211         case L2CAP_INFO_REQ:
3212                 err = l2cap_information_req(conn, cmd, data);
3213                 break;
3214
3215         case L2CAP_INFO_RSP:
3216                 err = l2cap_information_rsp(conn, cmd, data);
3217                 break;
3218
3219         default:
3220                 BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code);
3221                 err = -EINVAL;
3222                 break;
3223         }
3224
3225         return err;
3226 }
3227
3228 static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
3229                                         struct l2cap_cmd_hdr *cmd, u8 *data)
3230 {
3231         switch (cmd->code) {
3232         case L2CAP_COMMAND_REJ:
3233                 return 0;
3234
3235         case L2CAP_CONN_PARAM_UPDATE_REQ:
3236                 return l2cap_conn_param_update_req(conn, cmd, data);
3237
3238         case L2CAP_CONN_PARAM_UPDATE_RSP:
3239                 return 0;
3240
3241         default:
3242                 BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code);
3243                 return -EINVAL;
3244         }
3245 }
3246
3247 static inline void l2cap_sig_channel(struct l2cap_conn *conn,
3248                                                         struct sk_buff *skb)
3249 {
3250         u8 *data = skb->data;
3251         int len = skb->len;
3252         struct l2cap_cmd_hdr cmd;
3253         int err;
3254
3255         l2cap_raw_recv(conn, skb);
3256
3257         while (len >= L2CAP_CMD_HDR_SIZE) {
3258                 u16 cmd_len;
3259                 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
3260                 data += L2CAP_CMD_HDR_SIZE;
3261                 len  -= L2CAP_CMD_HDR_SIZE;
3262
3263                 cmd_len = le16_to_cpu(cmd.len);
3264
3265                 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, cmd.ident);
3266
3267                 if (cmd_len > len || !cmd.ident) {
3268                         BT_DBG("corrupted command");
3269                         break;
3270                 }
3271
3272                 if (conn->hcon->type == LE_LINK)
3273                         err = l2cap_le_sig_cmd(conn, &cmd, data);
3274                 else
3275                         err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data);
3276
3277                 if (err) {
3278                         struct l2cap_cmd_rej_unk rej;
3279
3280                         BT_ERR("Wrong link type (%d)", err);
3281
3282                         /* FIXME: Map err to a valid reason */
3283                         rej.reason = cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
3284                         l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
3285                 }
3286
3287                 data += cmd_len;
3288                 len  -= cmd_len;
3289         }
3290
3291         kfree_skb(skb);
3292 }
3293
3294 static int l2cap_check_fcs(struct l2cap_chan *chan,  struct sk_buff *skb)
3295 {
3296         u16 our_fcs, rcv_fcs;
3297         int hdr_size;
3298
3299         if (test_bit(FLAG_EXT_CTRL, &chan->flags))
3300                 hdr_size = L2CAP_EXT_HDR_SIZE;
3301         else
3302                 hdr_size = L2CAP_ENH_HDR_SIZE;
3303
3304         if (chan->fcs == L2CAP_FCS_CRC16) {
3305                 skb_trim(skb, skb->len - L2CAP_FCS_SIZE);
3306                 rcv_fcs = get_unaligned_le16(skb->data + skb->len);
3307                 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
3308
3309                 if (our_fcs != rcv_fcs)
3310                         return -EBADMSG;
3311         }
3312         return 0;
3313 }
3314
3315 static inline void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan)
3316 {
3317         u32 control = 0;
3318
3319         chan->frames_sent = 0;
3320
3321         control |= __set_reqseq(chan, chan->buffer_seq);
3322
3323         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
3324                 control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
3325                 l2cap_send_sframe(chan, control);
3326                 set_bit(CONN_RNR_SENT, &chan->conn_state);
3327         }
3328
3329         if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
3330                 l2cap_retransmit_frames(chan);
3331
3332         l2cap_ertm_send(chan);
3333
3334         if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
3335                         chan->frames_sent == 0) {
3336                 control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
3337                 l2cap_send_sframe(chan, control);
3338         }
3339 }
3340
3341 static int l2cap_add_to_srej_queue(struct l2cap_chan *chan, struct sk_buff *skb, u16 tx_seq, u8 sar)
3342 {
3343         struct sk_buff *next_skb;
3344         int tx_seq_offset, next_tx_seq_offset;
3345
3346         bt_cb(skb)->tx_seq = tx_seq;
3347         bt_cb(skb)->sar = sar;
3348
3349         next_skb = skb_peek(&chan->srej_q);
3350         if (!next_skb) {
3351                 __skb_queue_tail(&chan->srej_q, skb);
3352                 return 0;
3353         }
3354
3355         tx_seq_offset = __seq_offset(chan, tx_seq, chan->buffer_seq);
3356
3357         do {
3358                 if (bt_cb(next_skb)->tx_seq == tx_seq)
3359                         return -EINVAL;
3360
3361                 next_tx_seq_offset = __seq_offset(chan,
3362                                 bt_cb(next_skb)->tx_seq, chan->buffer_seq);
3363
3364                 if (next_tx_seq_offset > tx_seq_offset) {
3365                         __skb_queue_before(&chan->srej_q, next_skb, skb);
3366                         return 0;
3367                 }
3368
3369                 if (skb_queue_is_last(&chan->srej_q, next_skb))
3370                         break;
3371
3372         } while ((next_skb = skb_queue_next(&chan->srej_q, next_skb)));
3373
3374         __skb_queue_tail(&chan->srej_q, skb);
3375
3376         return 0;
3377 }
3378
3379 static void append_skb_frag(struct sk_buff *skb,
3380                         struct sk_buff *new_frag, struct sk_buff **last_frag)
3381 {
3382         /* skb->len reflects data in skb as well as all fragments
3383          * skb->data_len reflects only data in fragments
3384          */
3385         if (!skb_has_frag_list(skb))
3386                 skb_shinfo(skb)->frag_list = new_frag;
3387
3388         new_frag->next = NULL;
3389
3390         (*last_frag)->next = new_frag;
3391         *last_frag = new_frag;
3392
3393         skb->len += new_frag->len;
3394         skb->data_len += new_frag->len;
3395         skb->truesize += new_frag->truesize;
3396 }
3397
3398 static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u32 control)
3399 {
3400         int err = -EINVAL;
3401
3402         switch (__get_ctrl_sar(chan, control)) {
3403         case L2CAP_SAR_UNSEGMENTED:
3404                 if (chan->sdu)
3405                         break;
3406
3407                 err = chan->ops->recv(chan->data, skb);
3408                 break;
3409
3410         case L2CAP_SAR_START:
3411                 if (chan->sdu)
3412                         break;
3413
3414                 chan->sdu_len = get_unaligned_le16(skb->data);
3415                 skb_pull(skb, L2CAP_SDULEN_SIZE);
3416
3417                 if (chan->sdu_len > chan->imtu) {
3418                         err = -EMSGSIZE;
3419                         break;
3420                 }
3421
3422                 if (skb->len >= chan->sdu_len)
3423                         break;
3424
3425                 chan->sdu = skb;
3426                 chan->sdu_last_frag = skb;
3427
3428                 skb = NULL;
3429                 err = 0;
3430                 break;
3431
3432         case L2CAP_SAR_CONTINUE:
3433                 if (!chan->sdu)
3434                         break;
3435
3436                 append_skb_frag(chan->sdu, skb,
3437                                 &chan->sdu_last_frag);
3438                 skb = NULL;
3439
3440                 if (chan->sdu->len >= chan->sdu_len)
3441                         break;
3442
3443                 err = 0;
3444                 break;
3445
3446         case L2CAP_SAR_END:
3447                 if (!chan->sdu)
3448                         break;
3449
3450                 append_skb_frag(chan->sdu, skb,
3451                                 &chan->sdu_last_frag);
3452                 skb = NULL;
3453
3454                 if (chan->sdu->len != chan->sdu_len)
3455                         break;
3456
3457                 err = chan->ops->recv(chan->data, chan->sdu);
3458
3459                 if (!err) {
3460                         /* Reassembly complete */
3461                         chan->sdu = NULL;
3462                         chan->sdu_last_frag = NULL;
3463                         chan->sdu_len = 0;
3464                 }
3465                 break;
3466         }
3467
3468         if (err) {
3469                 kfree_skb(skb);
3470                 kfree_skb(chan->sdu);
3471                 chan->sdu = NULL;
3472                 chan->sdu_last_frag = NULL;
3473                 chan->sdu_len = 0;
3474         }
3475
3476         return err;
3477 }
3478
3479 static void l2cap_ertm_enter_local_busy(struct l2cap_chan *chan)
3480 {
3481         u32 control;
3482
3483         BT_DBG("chan %p, Enter local busy", chan);
3484
3485         set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
3486
3487         control = __set_reqseq(chan, chan->buffer_seq);
3488         control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
3489         l2cap_send_sframe(chan, control);
3490
3491         set_bit(CONN_RNR_SENT, &chan->conn_state);
3492
3493         __clear_ack_timer(chan);
3494 }
3495
3496 static void l2cap_ertm_exit_local_busy(struct l2cap_chan *chan)
3497 {
3498         u32 control;
3499
3500         if (!test_bit(CONN_RNR_SENT, &chan->conn_state))
3501                 goto done;
3502
3503         control = __set_reqseq(chan, chan->buffer_seq);
3504         control |= __set_ctrl_poll(chan);
3505         control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
3506         l2cap_send_sframe(chan, control);
3507         chan->retry_count = 1;
3508
3509         __clear_retrans_timer(chan);
3510         __set_monitor_timer(chan);
3511
3512         set_bit(CONN_WAIT_F, &chan->conn_state);
3513
3514 done:
3515         clear_bit(CONN_LOCAL_BUSY, &chan->conn_state);
3516         clear_bit(CONN_RNR_SENT, &chan->conn_state);
3517
3518         BT_DBG("chan %p, Exit local busy", chan);
3519 }
3520
3521 void l2cap_chan_busy(struct l2cap_chan *chan, int busy)
3522 {
3523         if (chan->mode == L2CAP_MODE_ERTM) {
3524                 if (busy)
3525                         l2cap_ertm_enter_local_busy(chan);
3526                 else
3527                         l2cap_ertm_exit_local_busy(chan);
3528         }
3529 }
3530
3531 static void l2cap_check_srej_gap(struct l2cap_chan *chan, u16 tx_seq)
3532 {
3533         struct sk_buff *skb;
3534         u32 control;
3535
3536         while ((skb = skb_peek(&chan->srej_q)) &&
3537                         !test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
3538                 int err;
3539
3540                 if (bt_cb(skb)->tx_seq != tx_seq)
3541                         break;
3542
3543                 skb = skb_dequeue(&chan->srej_q);
3544                 control = __set_ctrl_sar(chan, bt_cb(skb)->sar);
3545                 err = l2cap_reassemble_sdu(chan, skb, control);
3546
3547                 if (err < 0) {
3548                         l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3549                         break;
3550                 }
3551
3552                 chan->buffer_seq_srej = __next_seq(chan, chan->buffer_seq_srej);
3553                 tx_seq = __next_seq(chan, tx_seq);
3554         }
3555 }
3556
3557 static void l2cap_resend_srejframe(struct l2cap_chan *chan, u16 tx_seq)
3558 {
3559         struct srej_list *l, *tmp;
3560         u32 control;
3561
3562         list_for_each_entry_safe(l, tmp, &chan->srej_l, list) {
3563                 if (l->tx_seq == tx_seq) {
3564                         list_del(&l->list);
3565                         kfree(l);
3566                         return;
3567                 }
3568                 control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ);
3569                 control |= __set_reqseq(chan, l->tx_seq);
3570                 l2cap_send_sframe(chan, control);
3571                 list_del(&l->list);
3572                 list_add_tail(&l->list, &chan->srej_l);
3573         }
3574 }
3575
3576 static void l2cap_send_srejframe(struct l2cap_chan *chan, u16 tx_seq)
3577 {
3578         struct srej_list *new;
3579         u32 control;
3580
3581         while (tx_seq != chan->expected_tx_seq) {
3582                 control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ);
3583                 control |= __set_reqseq(chan, chan->expected_tx_seq);
3584                 l2cap_send_sframe(chan, control);
3585
3586                 new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC);
3587                 new->tx_seq = chan->expected_tx_seq;
3588
3589                 chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq);
3590
3591                 list_add_tail(&new->list, &chan->srej_l);
3592         }
3593
3594         chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq);
3595 }
3596
3597 static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u32 rx_control, struct sk_buff *skb)
3598 {
3599         u16 tx_seq = __get_txseq(chan, rx_control);
3600         u16 req_seq = __get_reqseq(chan, rx_control);
3601         u8 sar = __get_ctrl_sar(chan, rx_control);
3602         int tx_seq_offset, expected_tx_seq_offset;
3603         int num_to_ack = (chan->tx_win/6) + 1;
3604         int err = 0;
3605
3606         BT_DBG("chan %p len %d tx_seq %d rx_control 0x%8.8x", chan, skb->len,
3607                                                         tx_seq, rx_control);
3608
3609         if (__is_ctrl_final(chan, rx_control) &&
3610                         test_bit(CONN_WAIT_F, &chan->conn_state)) {
3611                 __clear_monitor_timer(chan);
3612                 if (chan->unacked_frames > 0)
3613                         __set_retrans_timer(chan);
3614                 clear_bit(CONN_WAIT_F, &chan->conn_state);
3615         }
3616
3617         chan->expected_ack_seq = req_seq;
3618         l2cap_drop_acked_frames(chan);
3619
3620         tx_seq_offset = __seq_offset(chan, tx_seq, chan->buffer_seq);
3621
3622         /* invalid tx_seq */
3623         if (tx_seq_offset >= chan->tx_win) {
3624                 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3625                 goto drop;
3626         }
3627
3628         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state))
3629                 goto drop;
3630
3631         if (tx_seq == chan->expected_tx_seq)
3632                 goto expected;
3633
3634         if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
3635                 struct srej_list *first;
3636
3637                 first = list_first_entry(&chan->srej_l,
3638                                 struct srej_list, list);
3639                 if (tx_seq == first->tx_seq) {
3640                         l2cap_add_to_srej_queue(chan, skb, tx_seq, sar);
3641                         l2cap_check_srej_gap(chan, tx_seq);
3642
3643                         list_del(&first->list);
3644                         kfree(first);
3645
3646                         if (list_empty(&chan->srej_l)) {
3647                                 chan->buffer_seq = chan->buffer_seq_srej;
3648                                 clear_bit(CONN_SREJ_SENT, &chan->conn_state);
3649                                 l2cap_send_ack(chan);
3650                                 BT_DBG("chan %p, Exit SREJ_SENT", chan);
3651                         }
3652                 } else {
3653                         struct srej_list *l;
3654
3655                         /* duplicated tx_seq */
3656                         if (l2cap_add_to_srej_queue(chan, skb, tx_seq, sar) < 0)
3657                                 goto drop;
3658
3659                         list_for_each_entry(l, &chan->srej_l, list) {
3660                                 if (l->tx_seq == tx_seq) {
3661                                         l2cap_resend_srejframe(chan, tx_seq);
3662                                         return 0;
3663                                 }
3664                         }
3665                         l2cap_send_srejframe(chan, tx_seq);
3666                 }
3667         } else {
3668                 expected_tx_seq_offset = __seq_offset(chan,
3669                                 chan->expected_tx_seq, chan->buffer_seq);
3670
3671                 /* duplicated tx_seq */
3672                 if (tx_seq_offset < expected_tx_seq_offset)
3673                         goto drop;
3674
3675                 set_bit(CONN_SREJ_SENT, &chan->conn_state);
3676
3677                 BT_DBG("chan %p, Enter SREJ", chan);
3678
3679                 INIT_LIST_HEAD(&chan->srej_l);
3680                 chan->buffer_seq_srej = chan->buffer_seq;
3681
3682                 __skb_queue_head_init(&chan->srej_q);
3683                 l2cap_add_to_srej_queue(chan, skb, tx_seq, sar);
3684
3685                 set_bit(CONN_SEND_PBIT, &chan->conn_state);
3686
3687                 l2cap_send_srejframe(chan, tx_seq);
3688
3689                 __clear_ack_timer(chan);
3690         }
3691         return 0;
3692
3693 expected:
3694         chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq);
3695
3696         if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
3697                 bt_cb(skb)->tx_seq = tx_seq;
3698                 bt_cb(skb)->sar = sar;
3699                 __skb_queue_tail(&chan->srej_q, skb);
3700                 return 0;
3701         }
3702
3703         err = l2cap_reassemble_sdu(chan, skb, rx_control);
3704         chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
3705
3706         if (err < 0) {
3707                 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3708                 return err;
3709         }
3710
3711         if (__is_ctrl_final(chan, rx_control)) {
3712                 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
3713                         l2cap_retransmit_frames(chan);
3714         }
3715
3716         __set_ack_timer(chan);
3717
3718         chan->num_acked = (chan->num_acked + 1) % num_to_ack;
3719         if (chan->num_acked == num_to_ack - 1)
3720                 l2cap_send_ack(chan);
3721
3722         return 0;
3723
3724 drop:
3725         kfree_skb(skb);
3726         return 0;
3727 }
3728
3729 static inline void l2cap_data_channel_rrframe(struct l2cap_chan *chan, u32 rx_control)
3730 {
3731         BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan,
3732                                 __get_reqseq(chan, rx_control), rx_control);
3733
3734         chan->expected_ack_seq = __get_reqseq(chan, rx_control);
3735         l2cap_drop_acked_frames(chan);
3736
3737         if (__is_ctrl_poll(chan, rx_control)) {
3738                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
3739                 if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
3740                         if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
3741                                         (chan->unacked_frames > 0))
3742                                 __set_retrans_timer(chan);
3743
3744                         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3745                         l2cap_send_srejtail(chan);
3746                 } else {
3747                         l2cap_send_i_or_rr_or_rnr(chan);
3748                 }
3749
3750         } else if (__is_ctrl_final(chan, rx_control)) {
3751                 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3752
3753                 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
3754                         l2cap_retransmit_frames(chan);
3755
3756         } else {
3757                 if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
3758                                 (chan->unacked_frames > 0))
3759                         __set_retrans_timer(chan);
3760
3761                 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3762                 if (test_bit(CONN_SREJ_SENT, &chan->conn_state))
3763                         l2cap_send_ack(chan);
3764                 else
3765                         l2cap_ertm_send(chan);
3766         }
3767 }
3768
3769 static inline void l2cap_data_channel_rejframe(struct l2cap_chan *chan, u32 rx_control)
3770 {
3771         u16 tx_seq = __get_reqseq(chan, rx_control);
3772
3773         BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control);
3774
3775         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3776
3777         chan->expected_ack_seq = tx_seq;
3778         l2cap_drop_acked_frames(chan);
3779
3780         if (__is_ctrl_final(chan, rx_control)) {
3781                 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
3782                         l2cap_retransmit_frames(chan);
3783         } else {
3784                 l2cap_retransmit_frames(chan);
3785
3786                 if (test_bit(CONN_WAIT_F, &chan->conn_state))
3787                         set_bit(CONN_REJ_ACT, &chan->conn_state);
3788         }
3789 }
3790 static inline void l2cap_data_channel_srejframe(struct l2cap_chan *chan, u32 rx_control)
3791 {
3792         u16 tx_seq = __get_reqseq(chan, rx_control);
3793
3794         BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control);
3795
3796         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3797
3798         if (__is_ctrl_poll(chan, rx_control)) {
3799                 chan->expected_ack_seq = tx_seq;
3800                 l2cap_drop_acked_frames(chan);
3801
3802                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
3803                 l2cap_retransmit_one_frame(chan, tx_seq);
3804
3805                 l2cap_ertm_send(chan);
3806
3807                 if (test_bit(CONN_WAIT_F, &chan->conn_state)) {
3808                         chan->srej_save_reqseq = tx_seq;
3809                         set_bit(CONN_SREJ_ACT, &chan->conn_state);
3810                 }
3811         } else if (__is_ctrl_final(chan, rx_control)) {
3812                 if (test_bit(CONN_SREJ_ACT, &chan->conn_state) &&
3813                                 chan->srej_save_reqseq == tx_seq)
3814                         clear_bit(CONN_SREJ_ACT, &chan->conn_state);
3815                 else
3816                         l2cap_retransmit_one_frame(chan, tx_seq);
3817         } else {
3818                 l2cap_retransmit_one_frame(chan, tx_seq);
3819                 if (test_bit(CONN_WAIT_F, &chan->conn_state)) {
3820                         chan->srej_save_reqseq = tx_seq;
3821                         set_bit(CONN_SREJ_ACT, &chan->conn_state);
3822                 }
3823         }
3824 }
3825
3826 static inline void l2cap_data_channel_rnrframe(struct l2cap_chan *chan, u32 rx_control)
3827 {
3828         u16 tx_seq = __get_reqseq(chan, rx_control);
3829
3830         BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control);
3831
3832         set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3833         chan->expected_ack_seq = tx_seq;
3834         l2cap_drop_acked_frames(chan);
3835
3836         if (__is_ctrl_poll(chan, rx_control))
3837                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
3838
3839         if (!test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
3840                 __clear_retrans_timer(chan);
3841                 if (__is_ctrl_poll(chan, rx_control))
3842                         l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_FINAL);
3843                 return;
3844         }
3845
3846         if (__is_ctrl_poll(chan, rx_control)) {
3847                 l2cap_send_srejtail(chan);
3848         } else {
3849                 rx_control = __set_ctrl_super(chan, L2CAP_SUPER_RR);
3850                 l2cap_send_sframe(chan, rx_control);
3851         }
3852 }
3853
3854 static inline int l2cap_data_channel_sframe(struct l2cap_chan *chan, u32 rx_control, struct sk_buff *skb)
3855 {
3856         BT_DBG("chan %p rx_control 0x%8.8x len %d", chan, rx_control, skb->len);
3857
3858         if (__is_ctrl_final(chan, rx_control) &&
3859                         test_bit(CONN_WAIT_F, &chan->conn_state)) {
3860                 __clear_monitor_timer(chan);
3861                 if (chan->unacked_frames > 0)
3862                         __set_retrans_timer(chan);
3863                 clear_bit(CONN_WAIT_F, &chan->conn_state);
3864         }
3865
3866         switch (__get_ctrl_super(chan, rx_control)) {
3867         case L2CAP_SUPER_RR:
3868                 l2cap_data_channel_rrframe(chan, rx_control);
3869                 break;
3870
3871         case L2CAP_SUPER_REJ:
3872                 l2cap_data_channel_rejframe(chan, rx_control);
3873                 break;
3874
3875         case L2CAP_SUPER_SREJ:
3876                 l2cap_data_channel_srejframe(chan, rx_control);
3877                 break;
3878
3879         case L2CAP_SUPER_RNR:
3880                 l2cap_data_channel_rnrframe(chan, rx_control);
3881                 break;
3882         }
3883
3884         kfree_skb(skb);
3885         return 0;
3886 }
3887
3888 static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb)
3889 {
3890         struct l2cap_chan *chan = l2cap_pi(sk)->chan;
3891         u32 control;
3892         u16 req_seq;
3893         int len, next_tx_seq_offset, req_seq_offset;
3894
3895         control = __get_control(chan, skb->data);
3896         skb_pull(skb, __ctrl_size(chan));
3897         len = skb->len;
3898
3899         /*
3900          * We can just drop the corrupted I-frame here.
3901          * Receiver will miss it and start proper recovery
3902          * procedures and ask retransmission.
3903          */
3904         if (l2cap_check_fcs(chan, skb))
3905                 goto drop;
3906
3907         if (__is_sar_start(chan, control) && !__is_sframe(chan, control))
3908                 len -= L2CAP_SDULEN_SIZE;
3909
3910         if (chan->fcs == L2CAP_FCS_CRC16)
3911                 len -= L2CAP_FCS_SIZE;
3912
3913         if (len > chan->mps) {
3914                 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3915                 goto drop;
3916         }
3917
3918         req_seq = __get_reqseq(chan, control);
3919
3920         req_seq_offset = __seq_offset(chan, req_seq, chan->expected_ack_seq);
3921
3922         next_tx_seq_offset = __seq_offset(chan, chan->next_tx_seq,
3923                                                 chan->expected_ack_seq);
3924
3925         /* check for invalid req-seq */
3926         if (req_seq_offset > next_tx_seq_offset) {
3927                 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3928                 goto drop;
3929         }
3930
3931         if (!__is_sframe(chan, control)) {
3932                 if (len < 0) {
3933                         l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3934                         goto drop;
3935                 }
3936
3937                 l2cap_data_channel_iframe(chan, control, skb);
3938         } else {
3939                 if (len != 0) {
3940                         BT_ERR("%d", len);
3941                         l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3942                         goto drop;
3943                 }
3944
3945                 l2cap_data_channel_sframe(chan, control, skb);
3946         }
3947
3948         return 0;
3949
3950 drop:
3951         kfree_skb(skb);
3952         return 0;
3953 }
3954
3955 static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
3956 {
3957         struct l2cap_chan *chan;
3958         struct sock *sk = NULL;
3959         u32 control;
3960         u16 tx_seq;
3961         int len;
3962
3963         chan = l2cap_get_chan_by_scid(conn, cid);
3964         if (!chan) {
3965                 BT_DBG("unknown cid 0x%4.4x", cid);
3966                 goto drop;
3967         }
3968
3969         sk = chan->sk;
3970
3971         BT_DBG("chan %p, len %d", chan, skb->len);
3972
3973         if (chan->state != BT_CONNECTED)
3974                 goto drop;
3975
3976         switch (chan->mode) {
3977         case L2CAP_MODE_BASIC:
3978                 /* If socket recv buffers overflows we drop data here
3979                  * which is *bad* because L2CAP has to be reliable.
3980                  * But we don't have any other choice. L2CAP doesn't
3981                  * provide flow control mechanism. */
3982
3983                 if (chan->imtu < skb->len)
3984                         goto drop;
3985
3986                 if (!chan->ops->recv(chan->data, skb))
3987                         goto done;
3988                 break;
3989
3990         case L2CAP_MODE_ERTM:
3991                 if (!sock_owned_by_user(sk)) {
3992                         l2cap_ertm_data_rcv(sk, skb);
3993                 } else {
3994                         if (sk_add_backlog(sk, skb))
3995                                 goto drop;
3996                 }
3997
3998                 goto done;
3999
4000         case L2CAP_MODE_STREAMING:
4001                 control = __get_control(chan, skb->data);
4002                 skb_pull(skb, __ctrl_size(chan));
4003                 len = skb->len;
4004
4005                 if (l2cap_check_fcs(chan, skb))
4006                         goto drop;
4007
4008                 if (__is_sar_start(chan, control))
4009                         len -= L2CAP_SDULEN_SIZE;
4010
4011                 if (chan->fcs == L2CAP_FCS_CRC16)
4012                         len -= L2CAP_FCS_SIZE;
4013
4014                 if (len > chan->mps || len < 0 || __is_sframe(chan, control))
4015                         goto drop;
4016
4017                 tx_seq = __get_txseq(chan, control);
4018
4019                 if (chan->expected_tx_seq != tx_seq) {
4020                         /* Frame(s) missing - must discard partial SDU */
4021                         kfree_skb(chan->sdu);
4022                         chan->sdu = NULL;
4023                         chan->sdu_last_frag = NULL;
4024                         chan->sdu_len = 0;
4025
4026                         /* TODO: Notify userland of missing data */
4027                 }
4028
4029                 chan->expected_tx_seq = __next_seq(chan, tx_seq);
4030
4031                 if (l2cap_reassemble_sdu(chan, skb, control) == -EMSGSIZE)
4032                         l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
4033
4034                 goto done;
4035
4036         default:
4037                 BT_DBG("chan %p: bad mode 0x%2.2x", chan, chan->mode);
4038                 break;
4039         }
4040
4041 drop:
4042         kfree_skb(skb);
4043
4044 done:
4045         if (sk)
4046                 bh_unlock_sock(sk);
4047
4048         return 0;
4049 }
4050
4051 static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb)
4052 {
4053         struct sock *sk = NULL;
4054         struct l2cap_chan *chan;
4055
4056         chan = l2cap_global_chan_by_psm(0, psm, conn->src);
4057         if (!chan)
4058                 goto drop;
4059
4060         sk = chan->sk;
4061
4062         bh_lock_sock(sk);
4063
4064         BT_DBG("sk %p, len %d", sk, skb->len);
4065
4066         if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
4067                 goto drop;
4068
4069         if (chan->imtu < skb->len)
4070                 goto drop;
4071
4072         if (!chan->ops->recv(chan->data, skb))
4073                 goto done;
4074
4075 drop:
4076         kfree_skb(skb);
4077
4078 done:
4079         if (sk)
4080                 bh_unlock_sock(sk);
4081         return 0;
4082 }
4083
4084 static inline int l2cap_att_channel(struct l2cap_conn *conn, __le16 cid, struct sk_buff *skb)
4085 {
4086         struct sock *sk = NULL;
4087         struct l2cap_chan *chan;
4088
4089         chan = l2cap_global_chan_by_scid(0, cid, conn->src);
4090         if (!chan)
4091                 goto drop;
4092
4093         sk = chan->sk;
4094
4095         bh_lock_sock(sk);
4096
4097         BT_DBG("sk %p, len %d", sk, skb->len);
4098
4099         if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
4100                 goto drop;
4101
4102         if (chan->imtu < skb->len)
4103                 goto drop;
4104
4105         if (!chan->ops->recv(chan->data, skb))
4106                 goto done;
4107
4108 drop:
4109         kfree_skb(skb);
4110
4111 done:
4112         if (sk)
4113                 bh_unlock_sock(sk);
4114         return 0;
4115 }
4116
4117 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
4118 {
4119         struct l2cap_hdr *lh = (void *) skb->data;
4120         u16 cid, len;
4121         __le16 psm;
4122
4123         skb_pull(skb, L2CAP_HDR_SIZE);
4124         cid = __le16_to_cpu(lh->cid);
4125         len = __le16_to_cpu(lh->len);
4126
4127         if (len != skb->len) {
4128                 kfree_skb(skb);
4129                 return;
4130         }
4131
4132         BT_DBG("len %d, cid 0x%4.4x", len, cid);
4133
4134         switch (cid) {
4135         case L2CAP_CID_LE_SIGNALING:
4136         case L2CAP_CID_SIGNALING:
4137                 l2cap_sig_channel(conn, skb);
4138                 break;
4139
4140         case L2CAP_CID_CONN_LESS:
4141                 psm = get_unaligned_le16(skb->data);
4142                 skb_pull(skb, 2);
4143                 l2cap_conless_channel(conn, psm, skb);
4144                 break;
4145
4146         case L2CAP_CID_LE_DATA:
4147                 l2cap_att_channel(conn, cid, skb);
4148                 break;
4149
4150         case L2CAP_CID_SMP:
4151                 if (smp_sig_channel(conn, skb))
4152                         l2cap_conn_del(conn->hcon, EACCES);
4153                 break;
4154
4155         default:
4156                 l2cap_data_channel(conn, cid, skb);
4157                 break;
4158         }
4159 }
4160
4161 /* ---- L2CAP interface with lower layer (HCI) ---- */
4162
4163 static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
4164 {
4165         int exact = 0, lm1 = 0, lm2 = 0;
4166         struct l2cap_chan *c;
4167
4168         if (type != ACL_LINK)
4169                 return -EINVAL;
4170
4171         BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
4172
4173         /* Find listening sockets and check their link_mode */
4174         read_lock(&chan_list_lock);
4175         list_for_each_entry(c, &chan_list, global_l) {
4176                 struct sock *sk = c->sk;
4177
4178                 if (c->state != BT_LISTEN)
4179                         continue;
4180
4181                 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
4182                         lm1 |= HCI_LM_ACCEPT;
4183                         if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
4184                                 lm1 |= HCI_LM_MASTER;
4185                         exact++;
4186                 } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
4187                         lm2 |= HCI_LM_ACCEPT;
4188                         if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
4189                                 lm2 |= HCI_LM_MASTER;
4190                 }
4191         }
4192         read_unlock(&chan_list_lock);
4193
4194         return exact ? lm1 : lm2;
4195 }
4196
4197 static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
4198 {
4199         struct l2cap_conn *conn;
4200
4201         BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
4202
4203         if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK))
4204                 return -EINVAL;
4205
4206         if (!status) {
4207                 conn = l2cap_conn_add(hcon, status);
4208                 if (conn)
4209                         l2cap_conn_ready(conn);
4210         } else
4211                 l2cap_conn_del(hcon, bt_to_errno(status));
4212
4213         return 0;
4214 }
4215
4216 static int l2cap_disconn_ind(struct hci_conn *hcon)
4217 {
4218         struct l2cap_conn *conn = hcon->l2cap_data;
4219
4220         BT_DBG("hcon %p", hcon);
4221
4222         if ((hcon->type != ACL_LINK && hcon->type != LE_LINK) || !conn)
4223                 return 0x13;
4224
4225         return conn->disc_reason;
4226 }
4227
4228 static int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
4229 {
4230         BT_DBG("hcon %p reason %d", hcon, reason);
4231
4232         if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK))
4233                 return -EINVAL;
4234
4235         l2cap_conn_del(hcon, bt_to_errno(reason));
4236
4237         return 0;
4238 }
4239
4240 static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt)
4241 {
4242         if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED)
4243                 return;
4244
4245         if (encrypt == 0x00) {
4246                 if (chan->sec_level == BT_SECURITY_MEDIUM) {
4247                         __clear_chan_timer(chan);
4248                         __set_chan_timer(chan, HZ * 5);
4249                 } else if (chan->sec_level == BT_SECURITY_HIGH)
4250                         l2cap_chan_close(chan, ECONNREFUSED);
4251         } else {
4252                 if (chan->sec_level == BT_SECURITY_MEDIUM)
4253                         __clear_chan_timer(chan);
4254         }
4255 }
4256
4257 static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
4258 {
4259         struct l2cap_conn *conn = hcon->l2cap_data;
4260         struct l2cap_chan *chan;
4261
4262         if (!conn)
4263                 return 0;
4264
4265         BT_DBG("conn %p", conn);
4266
4267         if (hcon->type == LE_LINK) {
4268                 smp_distribute_keys(conn, 0);
4269                 del_timer(&conn->security_timer);
4270         }
4271
4272         read_lock(&conn->chan_lock);
4273
4274         list_for_each_entry(chan, &conn->chan_l, list) {
4275                 struct sock *sk = chan->sk;
4276
4277                 bh_lock_sock(sk);
4278
4279                 BT_DBG("chan->scid %d", chan->scid);
4280
4281                 if (chan->scid == L2CAP_CID_LE_DATA) {
4282                         if (!status && encrypt) {
4283                                 chan->sec_level = hcon->sec_level;
4284                                 l2cap_chan_ready(sk);
4285                         }
4286
4287                         bh_unlock_sock(sk);
4288                         continue;
4289                 }
4290
4291                 if (test_bit(CONF_CONNECT_PEND, &chan->conf_state)) {
4292                         bh_unlock_sock(sk);
4293                         continue;
4294                 }
4295
4296                 if (!status && (chan->state == BT_CONNECTED ||
4297                                                 chan->state == BT_CONFIG)) {
4298                         l2cap_check_encryption(chan, encrypt);
4299                         bh_unlock_sock(sk);
4300                         continue;
4301                 }
4302
4303                 if (chan->state == BT_CONNECT) {
4304                         if (!status) {
4305                                 struct l2cap_conn_req req;
4306                                 req.scid = cpu_to_le16(chan->scid);
4307                                 req.psm  = chan->psm;
4308
4309                                 chan->ident = l2cap_get_ident(conn);
4310                                 set_bit(CONF_CONNECT_PEND, &chan->conf_state);
4311
4312                                 l2cap_send_cmd(conn, chan->ident,
4313                                         L2CAP_CONN_REQ, sizeof(req), &req);
4314                         } else {
4315                                 __clear_chan_timer(chan);
4316                                 __set_chan_timer(chan, HZ / 10);
4317                         }
4318                 } else if (chan->state == BT_CONNECT2) {
4319                         struct l2cap_conn_rsp rsp;
4320                         __u16 res, stat;
4321
4322                         if (!status) {
4323                                 if (bt_sk(sk)->defer_setup) {
4324                                         struct sock *parent = bt_sk(sk)->parent;
4325                                         res = L2CAP_CR_PEND;
4326                                         stat = L2CAP_CS_AUTHOR_PEND;
4327                                         if (parent)
4328                                                 parent->sk_data_ready(parent, 0);
4329                                 } else {
4330                                         l2cap_state_change(chan, BT_CONFIG);
4331                                         res = L2CAP_CR_SUCCESS;
4332                                         stat = L2CAP_CS_NO_INFO;
4333                                 }
4334                         } else {
4335                                 l2cap_state_change(chan, BT_DISCONN);
4336                                 __set_chan_timer(chan, HZ / 10);
4337                                 res = L2CAP_CR_SEC_BLOCK;
4338                                 stat = L2CAP_CS_NO_INFO;
4339                         }
4340
4341                         rsp.scid   = cpu_to_le16(chan->dcid);
4342                         rsp.dcid   = cpu_to_le16(chan->scid);
4343                         rsp.result = cpu_to_le16(res);
4344                         rsp.status = cpu_to_le16(stat);
4345                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
4346                                                         sizeof(rsp), &rsp);
4347                 }
4348
4349                 bh_unlock_sock(sk);
4350         }
4351
4352         read_unlock(&conn->chan_lock);
4353
4354         return 0;
4355 }
4356
4357 static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
4358 {
4359         struct l2cap_conn *conn = hcon->l2cap_data;
4360
4361         if (!conn)
4362                 conn = l2cap_conn_add(hcon, 0);
4363
4364         if (!conn)
4365                 goto drop;
4366
4367         BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
4368
4369         if (!(flags & ACL_CONT)) {
4370                 struct l2cap_hdr *hdr;
4371                 struct l2cap_chan *chan;
4372                 u16 cid;
4373                 int len;
4374
4375                 if (conn->rx_len) {
4376                         BT_ERR("Unexpected start frame (len %d)", skb->len);
4377                         kfree_skb(conn->rx_skb);
4378                         conn->rx_skb = NULL;
4379                         conn->rx_len = 0;
4380                         l2cap_conn_unreliable(conn, ECOMM);
4381                 }
4382
4383                 /* Start fragment always begin with Basic L2CAP header */
4384                 if (skb->len < L2CAP_HDR_SIZE) {
4385                         BT_ERR("Frame is too short (len %d)", skb->len);
4386                         l2cap_conn_unreliable(conn, ECOMM);
4387                         goto drop;
4388                 }
4389
4390                 hdr = (struct l2cap_hdr *) skb->data;
4391                 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
4392                 cid = __le16_to_cpu(hdr->cid);
4393
4394                 if (len == skb->len) {
4395                         /* Complete frame received */
4396                         l2cap_recv_frame(conn, skb);
4397                         return 0;
4398                 }
4399
4400                 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
4401
4402                 if (skb->len > len) {
4403                         BT_ERR("Frame is too long (len %d, expected len %d)",
4404                                 skb->len, len);
4405                         l2cap_conn_unreliable(conn, ECOMM);
4406                         goto drop;
4407                 }
4408
4409                 chan = l2cap_get_chan_by_scid(conn, cid);
4410
4411                 if (chan && chan->sk) {
4412                         struct sock *sk = chan->sk;
4413
4414                         if (chan->imtu < len - L2CAP_HDR_SIZE) {
4415                                 BT_ERR("Frame exceeding recv MTU (len %d, "
4416                                                         "MTU %d)", len,
4417                                                         chan->imtu);
4418                                 bh_unlock_sock(sk);
4419                                 l2cap_conn_unreliable(conn, ECOMM);
4420                                 goto drop;
4421                         }
4422                         bh_unlock_sock(sk);
4423                 }
4424
4425                 /* Allocate skb for the complete frame (with header) */
4426                 conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC);
4427                 if (!conn->rx_skb)
4428                         goto drop;
4429
4430                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
4431                                                                 skb->len);
4432                 conn->rx_len = len - skb->len;
4433         } else {
4434                 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
4435
4436                 if (!conn->rx_len) {
4437                         BT_ERR("Unexpected continuation frame (len %d)", skb->len);
4438                         l2cap_conn_unreliable(conn, ECOMM);
4439                         goto drop;
4440                 }
4441
4442                 if (skb->len > conn->rx_len) {
4443                         BT_ERR("Fragment is too long (len %d, expected %d)",
4444                                         skb->len, conn->rx_len);
4445                         kfree_skb(conn->rx_skb);
4446                         conn->rx_skb = NULL;
4447                         conn->rx_len = 0;
4448                         l2cap_conn_unreliable(conn, ECOMM);
4449                         goto drop;
4450                 }
4451
4452                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
4453                                                                 skb->len);
4454                 conn->rx_len -= skb->len;
4455
4456                 if (!conn->rx_len) {
4457                         /* Complete frame received */
4458                         l2cap_recv_frame(conn, conn->rx_skb);
4459                         conn->rx_skb = NULL;
4460                 }
4461         }
4462
4463 drop:
4464         kfree_skb(skb);
4465         return 0;
4466 }
4467
4468 static int l2cap_debugfs_show(struct seq_file *f, void *p)
4469 {
4470         struct l2cap_chan *c;
4471
4472         read_lock_bh(&chan_list_lock);
4473
4474         list_for_each_entry(c, &chan_list, global_l) {
4475                 struct sock *sk = c->sk;
4476
4477                 seq_printf(f, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
4478                                         batostr(&bt_sk(sk)->src),
4479                                         batostr(&bt_sk(sk)->dst),
4480                                         c->state, __le16_to_cpu(c->psm),
4481                                         c->scid, c->dcid, c->imtu, c->omtu,
4482                                         c->sec_level, c->mode);
4483 }
4484
4485         read_unlock_bh(&chan_list_lock);
4486
4487         return 0;
4488 }
4489
4490 static int l2cap_debugfs_open(struct inode *inode, struct file *file)
4491 {
4492         return single_open(file, l2cap_debugfs_show, inode->i_private);
4493 }
4494
4495 static const struct file_operations l2cap_debugfs_fops = {
4496         .open           = l2cap_debugfs_open,
4497         .read           = seq_read,
4498         .llseek         = seq_lseek,
4499         .release        = single_release,
4500 };
4501
4502 static struct dentry *l2cap_debugfs;
4503
4504 static struct hci_proto l2cap_hci_proto = {
4505         .name           = "L2CAP",
4506         .id             = HCI_PROTO_L2CAP,
4507         .connect_ind    = l2cap_connect_ind,
4508         .connect_cfm    = l2cap_connect_cfm,
4509         .disconn_ind    = l2cap_disconn_ind,
4510         .disconn_cfm    = l2cap_disconn_cfm,
4511         .security_cfm   = l2cap_security_cfm,
4512         .recv_acldata   = l2cap_recv_acldata
4513 };
4514
4515 int __init l2cap_init(void)
4516 {
4517         int err;
4518
4519         err = l2cap_init_sockets();
4520         if (err < 0)
4521                 return err;
4522
4523         err = hci_register_proto(&l2cap_hci_proto);
4524         if (err < 0) {
4525                 BT_ERR("L2CAP protocol registration failed");
4526                 bt_sock_unregister(BTPROTO_L2CAP);
4527                 goto error;
4528         }
4529
4530         if (bt_debugfs) {
4531                 l2cap_debugfs = debugfs_create_file("l2cap", 0444,
4532                                         bt_debugfs, NULL, &l2cap_debugfs_fops);
4533                 if (!l2cap_debugfs)
4534                         BT_ERR("Failed to create L2CAP debug file");
4535         }
4536
4537         return 0;
4538
4539 error:
4540         l2cap_cleanup_sockets();
4541         return err;
4542 }
4543
4544 void l2cap_exit(void)
4545 {
4546         debugfs_remove(l2cap_debugfs);
4547
4548         if (hci_unregister_proto(&l2cap_hci_proto) < 0)
4549                 BT_ERR("L2CAP protocol unregistration failed");
4550
4551         l2cap_cleanup_sockets();
4552 }
4553
4554 module_param(disable_ertm, bool, 0644);
4555 MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode");
4556
4557 module_param(enable_hs, bool, 0644);
4558 MODULE_PARM_DESC(enable_hs, "Enable High Speed");