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