net/bluetooth/l2cap_core.c

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