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