net/bluetooth/l2cap_core.c

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