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