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