net/bluetooth/l2cap_core.c

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