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 ((bt_sk(sk)->defer_setup && chan->state != BT_CONNECT2) ||
2534                  (!bt_sk(sk)->defer_setup && chan->state != BT_CONFIG)) {
2535                 struct l2cap_cmd_rej rej;
2536
2537                 rej.reason = cpu_to_le16(0x0002);
2538                 l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
2539                                 sizeof(rej), &rej);
2540                 goto unlock;
2541         }
2542
2543         /* Reject if config buffer is too small. */
2544         len = cmd_len - sizeof(*req);
2545         if (len < 0 || chan->conf_len + len > sizeof(chan->conf_req)) {
2546                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2547                                 l2cap_build_conf_rsp(chan, rsp,
2548                                         L2CAP_CONF_REJECT, flags), rsp);
2549                 goto unlock;
2550         }
2551
2552         /* Store config. */
2553         memcpy(chan->conf_req + chan->conf_len, req->data, len);
2554         chan->conf_len += len;
2555
2556         if (flags & 0x0001) {
2557                 /* Incomplete config. Send empty response. */
2558                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2559                                 l2cap_build_conf_rsp(chan, rsp,
2560                                         L2CAP_CONF_SUCCESS, 0x0001), rsp);
2561                 goto unlock;
2562         }
2563
2564         /* Complete config. */
2565         len = l2cap_parse_conf_req(chan, rsp);
2566         if (len < 0) {
2567                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
2568                 goto unlock;
2569         }
2570
2571         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
2572         chan->num_conf_rsp++;
2573
2574         /* Reset config buffer. */
2575         chan->conf_len = 0;
2576
2577         if (!test_bit(CONF_OUTPUT_DONE, &chan->conf_state))
2578                 goto unlock;
2579
2580         if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
2581                 set_default_fcs(chan);
2582
2583                 l2cap_state_change(chan, BT_CONNECTED);
2584
2585                 chan->next_tx_seq = 0;
2586                 chan->expected_tx_seq = 0;
2587                 skb_queue_head_init(&chan->tx_q);
2588                 if (chan->mode == L2CAP_MODE_ERTM)
2589                         l2cap_ertm_init(chan);
2590
2591                 l2cap_chan_ready(sk);
2592                 goto unlock;
2593         }
2594
2595         if (!test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) {
2596                 u8 buf[64];
2597                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2598                                         l2cap_build_conf_req(chan, buf), buf);
2599                 chan->num_conf_req++;
2600         }
2601
2602 unlock:
2603         bh_unlock_sock(sk);
2604         return 0;
2605 }
2606
2607 static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2608 {
2609         struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
2610         u16 scid, flags, result;
2611         struct l2cap_chan *chan;
2612         struct sock *sk;
2613         int len = cmd->len - sizeof(*rsp);
2614
2615         scid   = __le16_to_cpu(rsp->scid);
2616         flags  = __le16_to_cpu(rsp->flags);
2617         result = __le16_to_cpu(rsp->result);
2618
2619         BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x",
2620                         scid, flags, result);
2621
2622         chan = l2cap_get_chan_by_scid(conn, scid);
2623         if (!chan)
2624                 return 0;
2625
2626         sk = chan->sk;
2627
2628         switch (result) {
2629         case L2CAP_CONF_SUCCESS:
2630                 l2cap_conf_rfc_get(chan, rsp->data, len);
2631                 break;
2632
2633         case L2CAP_CONF_UNACCEPT:
2634                 if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
2635                         char req[64];
2636
2637                         if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
2638                                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
2639                                 goto done;
2640                         }
2641
2642                         /* throw out any old stored conf requests */
2643                         result = L2CAP_CONF_SUCCESS;
2644                         len = l2cap_parse_conf_rsp(chan, rsp->data, len,
2645                                                                 req, &result);
2646                         if (len < 0) {
2647                                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
2648                                 goto done;
2649                         }
2650
2651                         l2cap_send_cmd(conn, l2cap_get_ident(conn),
2652                                                 L2CAP_CONF_REQ, len, req);
2653                         chan->num_conf_req++;
2654                         if (result != L2CAP_CONF_SUCCESS)
2655                                 goto done;
2656                         break;
2657                 }
2658
2659         default:
2660                 sk->sk_err = ECONNRESET;
2661                 __set_chan_timer(chan, HZ * 5);
2662                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
2663                 goto done;
2664         }
2665
2666         if (flags & 0x01)
2667                 goto done;
2668
2669         set_bit(CONF_INPUT_DONE, &chan->conf_state);
2670
2671         if (test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) {
2672                 set_default_fcs(chan);
2673
2674                 l2cap_state_change(chan, BT_CONNECTED);
2675                 chan->next_tx_seq = 0;
2676                 chan->expected_tx_seq = 0;
2677                 skb_queue_head_init(&chan->tx_q);
2678                 if (chan->mode ==  L2CAP_MODE_ERTM)
2679                         l2cap_ertm_init(chan);
2680
2681                 l2cap_chan_ready(sk);
2682         }
2683
2684 done:
2685         bh_unlock_sock(sk);
2686         return 0;
2687 }
2688
2689 static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2690 {
2691         struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
2692         struct l2cap_disconn_rsp rsp;
2693         u16 dcid, scid;
2694         struct l2cap_chan *chan;
2695         struct sock *sk;
2696
2697         scid = __le16_to_cpu(req->scid);
2698         dcid = __le16_to_cpu(req->dcid);
2699
2700         BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
2701
2702         chan = l2cap_get_chan_by_scid(conn, dcid);
2703         if (!chan)
2704                 return 0;
2705
2706         sk = chan->sk;
2707
2708         rsp.dcid = cpu_to_le16(chan->scid);
2709         rsp.scid = cpu_to_le16(chan->dcid);
2710         l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
2711
2712         sk->sk_shutdown = SHUTDOWN_MASK;
2713
2714         /* don't delete l2cap channel if sk is owned by user */
2715         if (sock_owned_by_user(sk)) {
2716                 l2cap_state_change(chan, BT_DISCONN);
2717                 __clear_chan_timer(chan);
2718                 __set_chan_timer(chan, HZ / 5);
2719                 bh_unlock_sock(sk);
2720                 return 0;
2721         }
2722
2723         l2cap_chan_del(chan, ECONNRESET);
2724         bh_unlock_sock(sk);
2725
2726         chan->ops->close(chan->data);
2727         return 0;
2728 }
2729
2730 static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2731 {
2732         struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
2733         u16 dcid, scid;
2734         struct l2cap_chan *chan;
2735         struct sock *sk;
2736
2737         scid = __le16_to_cpu(rsp->scid);
2738         dcid = __le16_to_cpu(rsp->dcid);
2739
2740         BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
2741
2742         chan = l2cap_get_chan_by_scid(conn, scid);
2743         if (!chan)
2744                 return 0;
2745
2746         sk = chan->sk;
2747
2748         /* don't delete l2cap channel if sk is owned by user */
2749         if (sock_owned_by_user(sk)) {
2750                 l2cap_state_change(chan,BT_DISCONN);
2751                 __clear_chan_timer(chan);
2752                 __set_chan_timer(chan, HZ / 5);
2753                 bh_unlock_sock(sk);
2754                 return 0;
2755         }
2756
2757         l2cap_chan_del(chan, 0);
2758         bh_unlock_sock(sk);
2759
2760         chan->ops->close(chan->data);
2761         return 0;
2762 }
2763
2764 static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2765 {
2766         struct l2cap_info_req *req = (struct l2cap_info_req *) data;
2767         u16 type;
2768
2769         type = __le16_to_cpu(req->type);
2770
2771         BT_DBG("type 0x%4.4x", type);
2772
2773         if (type == L2CAP_IT_FEAT_MASK) {
2774                 u8 buf[8];
2775                 u32 feat_mask = l2cap_feat_mask;
2776                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
2777                 rsp->type   = cpu_to_le16(L2CAP_IT_FEAT_MASK);
2778                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
2779                 if (!disable_ertm)
2780                         feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
2781                                                          | L2CAP_FEAT_FCS;
2782                 put_unaligned_le32(feat_mask, rsp->data);
2783                 l2cap_send_cmd(conn, cmd->ident,
2784                                         L2CAP_INFO_RSP, sizeof(buf), buf);
2785         } else if (type == L2CAP_IT_FIXED_CHAN) {
2786                 u8 buf[12];
2787                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
2788                 rsp->type   = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
2789                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
2790                 memcpy(buf + 4, l2cap_fixed_chan, 8);
2791                 l2cap_send_cmd(conn, cmd->ident,
2792                                         L2CAP_INFO_RSP, sizeof(buf), buf);
2793         } else {
2794                 struct l2cap_info_rsp rsp;
2795                 rsp.type   = cpu_to_le16(type);
2796                 rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP);
2797                 l2cap_send_cmd(conn, cmd->ident,
2798                                         L2CAP_INFO_RSP, sizeof(rsp), &rsp);
2799         }
2800
2801         return 0;
2802 }
2803
2804 static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2805 {
2806         struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
2807         u16 type, result;
2808
2809         type   = __le16_to_cpu(rsp->type);
2810         result = __le16_to_cpu(rsp->result);
2811
2812         BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
2813
2814         /* L2CAP Info req/rsp are unbound to channels, add extra checks */
2815         if (cmd->ident != conn->info_ident ||
2816                         conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)
2817                 return 0;
2818
2819         del_timer(&conn->info_timer);
2820
2821         if (result != L2CAP_IR_SUCCESS) {
2822                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2823                 conn->info_ident = 0;
2824
2825                 l2cap_conn_start(conn);
2826
2827                 return 0;
2828         }
2829
2830         if (type == L2CAP_IT_FEAT_MASK) {
2831                 conn->feat_mask = get_unaligned_le32(rsp->data);
2832
2833                 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
2834                         struct l2cap_info_req req;
2835                         req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
2836
2837                         conn->info_ident = l2cap_get_ident(conn);
2838
2839                         l2cap_send_cmd(conn, conn->info_ident,
2840                                         L2CAP_INFO_REQ, sizeof(req), &req);
2841                 } else {
2842                         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2843                         conn->info_ident = 0;
2844
2845                         l2cap_conn_start(conn);
2846                 }
2847         } else if (type == L2CAP_IT_FIXED_CHAN) {
2848                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2849                 conn->info_ident = 0;
2850
2851                 l2cap_conn_start(conn);
2852         }
2853
2854         return 0;
2855 }
2856
2857 static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency,
2858                                                         u16 to_multiplier)
2859 {
2860         u16 max_latency;
2861
2862         if (min > max || min < 6 || max > 3200)
2863                 return -EINVAL;
2864
2865         if (to_multiplier < 10 || to_multiplier > 3200)
2866                 return -EINVAL;
2867
2868         if (max >= to_multiplier * 8)
2869                 return -EINVAL;
2870
2871         max_latency = (to_multiplier * 8 / max) - 1;
2872         if (latency > 499 || latency > max_latency)
2873                 return -EINVAL;
2874
2875         return 0;
2876 }
2877
2878 static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
2879                                         struct l2cap_cmd_hdr *cmd, u8 *data)
2880 {
2881         struct hci_conn *hcon = conn->hcon;
2882         struct l2cap_conn_param_update_req *req;
2883         struct l2cap_conn_param_update_rsp rsp;
2884         u16 min, max, latency, to_multiplier, cmd_len;
2885         int err;
2886
2887         if (!(hcon->link_mode & HCI_LM_MASTER))
2888                 return -EINVAL;
2889
2890         cmd_len = __le16_to_cpu(cmd->len);
2891         if (cmd_len != sizeof(struct l2cap_conn_param_update_req))
2892                 return -EPROTO;
2893
2894         req = (struct l2cap_conn_param_update_req *) data;
2895         min             = __le16_to_cpu(req->min);
2896         max             = __le16_to_cpu(req->max);
2897         latency         = __le16_to_cpu(req->latency);
2898         to_multiplier   = __le16_to_cpu(req->to_multiplier);
2899
2900         BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x",
2901                                                 min, max, latency, to_multiplier);
2902
2903         memset(&rsp, 0, sizeof(rsp));
2904
2905         err = l2cap_check_conn_param(min, max, latency, to_multiplier);
2906         if (err)
2907                 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_REJECTED);
2908         else
2909                 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED);
2910
2911         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP,
2912                                                         sizeof(rsp), &rsp);
2913
2914         if (!err)
2915                 hci_le_conn_update(hcon, min, max, latency, to_multiplier);
2916
2917         return 0;
2918 }
2919
2920 static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
2921                         struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
2922 {
2923         int err = 0;
2924
2925         switch (cmd->code) {
2926         case L2CAP_COMMAND_REJ:
2927                 l2cap_command_rej(conn, cmd, data);
2928                 break;
2929
2930         case L2CAP_CONN_REQ:
2931                 err = l2cap_connect_req(conn, cmd, data);
2932                 break;
2933
2934         case L2CAP_CONN_RSP:
2935                 err = l2cap_connect_rsp(conn, cmd, data);
2936                 break;
2937
2938         case L2CAP_CONF_REQ:
2939                 err = l2cap_config_req(conn, cmd, cmd_len, data);
2940                 break;
2941
2942         case L2CAP_CONF_RSP:
2943                 err = l2cap_config_rsp(conn, cmd, data);
2944                 break;
2945
2946         case L2CAP_DISCONN_REQ:
2947                 err = l2cap_disconnect_req(conn, cmd, data);
2948                 break;
2949
2950         case L2CAP_DISCONN_RSP:
2951                 err = l2cap_disconnect_rsp(conn, cmd, data);
2952                 break;
2953
2954         case L2CAP_ECHO_REQ:
2955                 l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data);
2956                 break;
2957
2958         case L2CAP_ECHO_RSP:
2959                 break;
2960
2961         case L2CAP_INFO_REQ:
2962                 err = l2cap_information_req(conn, cmd, data);
2963                 break;
2964
2965         case L2CAP_INFO_RSP:
2966                 err = l2cap_information_rsp(conn, cmd, data);
2967                 break;
2968
2969         default:
2970                 BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code);
2971                 err = -EINVAL;
2972                 break;
2973         }
2974
2975         return err;
2976 }
2977
2978 static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
2979                                         struct l2cap_cmd_hdr *cmd, u8 *data)
2980 {
2981         switch (cmd->code) {
2982         case L2CAP_COMMAND_REJ:
2983                 return 0;
2984
2985         case L2CAP_CONN_PARAM_UPDATE_REQ:
2986                 return l2cap_conn_param_update_req(conn, cmd, data);
2987
2988         case L2CAP_CONN_PARAM_UPDATE_RSP:
2989                 return 0;
2990
2991         default:
2992                 BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code);
2993                 return -EINVAL;
2994         }
2995 }
2996
2997 static inline void l2cap_sig_channel(struct l2cap_conn *conn,
2998                                                         struct sk_buff *skb)
2999 {
3000         u8 *data = skb->data;
3001         int len = skb->len;
3002         struct l2cap_cmd_hdr cmd;
3003         int err;
3004
3005         l2cap_raw_recv(conn, skb);
3006
3007         while (len >= L2CAP_CMD_HDR_SIZE) {
3008                 u16 cmd_len;
3009                 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
3010                 data += L2CAP_CMD_HDR_SIZE;
3011                 len  -= L2CAP_CMD_HDR_SIZE;
3012
3013                 cmd_len = le16_to_cpu(cmd.len);
3014
3015                 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, cmd.ident);
3016
3017                 if (cmd_len > len || !cmd.ident) {
3018                         BT_DBG("corrupted command");
3019                         break;
3020                 }
3021
3022                 if (conn->hcon->type == LE_LINK)
3023                         err = l2cap_le_sig_cmd(conn, &cmd, data);
3024                 else
3025                         err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data);
3026
3027                 if (err) {
3028                         struct l2cap_cmd_rej rej;
3029
3030                         BT_ERR("Wrong link type (%d)", err);
3031
3032                         /* FIXME: Map err to a valid reason */
3033                         rej.reason = cpu_to_le16(0);
3034                         l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
3035                 }
3036
3037                 data += cmd_len;
3038                 len  -= cmd_len;
3039         }
3040
3041         kfree_skb(skb);
3042 }
3043
3044 static int l2cap_check_fcs(struct l2cap_chan *chan,  struct sk_buff *skb)
3045 {
3046         u16 our_fcs, rcv_fcs;
3047         int hdr_size = L2CAP_HDR_SIZE + 2;
3048
3049         if (chan->fcs == L2CAP_FCS_CRC16) {
3050                 skb_trim(skb, skb->len - 2);
3051                 rcv_fcs = get_unaligned_le16(skb->data + skb->len);
3052                 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
3053
3054                 if (our_fcs != rcv_fcs)
3055                         return -EBADMSG;
3056         }
3057         return 0;
3058 }
3059
3060 static inline void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan)
3061 {
3062         u16 control = 0;
3063
3064         chan->frames_sent = 0;
3065
3066         control |= chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3067
3068         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
3069                 control |= L2CAP_SUPER_RCV_NOT_READY;
3070                 l2cap_send_sframe(chan, control);
3071                 set_bit(CONN_RNR_SENT, &chan->conn_state);
3072         }
3073
3074         if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
3075                 l2cap_retransmit_frames(chan);
3076
3077         l2cap_ertm_send(chan);
3078
3079         if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
3080                         chan->frames_sent == 0) {
3081                 control |= L2CAP_SUPER_RCV_READY;
3082                 l2cap_send_sframe(chan, control);
3083         }
3084 }
3085
3086 static int l2cap_add_to_srej_queue(struct l2cap_chan *chan, struct sk_buff *skb, u8 tx_seq, u8 sar)
3087 {
3088         struct sk_buff *next_skb;
3089         int tx_seq_offset, next_tx_seq_offset;
3090
3091         bt_cb(skb)->tx_seq = tx_seq;
3092         bt_cb(skb)->sar = sar;
3093
3094         next_skb = skb_peek(&chan->srej_q);
3095         if (!next_skb) {
3096                 __skb_queue_tail(&chan->srej_q, skb);
3097                 return 0;
3098         }
3099
3100         tx_seq_offset = (tx_seq - chan->buffer_seq) % 64;
3101         if (tx_seq_offset < 0)
3102                 tx_seq_offset += 64;
3103
3104         do {
3105                 if (bt_cb(next_skb)->tx_seq == tx_seq)
3106                         return -EINVAL;
3107
3108                 next_tx_seq_offset = (bt_cb(next_skb)->tx_seq -
3109                                                 chan->buffer_seq) % 64;
3110                 if (next_tx_seq_offset < 0)
3111                         next_tx_seq_offset += 64;
3112
3113                 if (next_tx_seq_offset > tx_seq_offset) {
3114                         __skb_queue_before(&chan->srej_q, next_skb, skb);
3115                         return 0;
3116                 }
3117
3118                 if (skb_queue_is_last(&chan->srej_q, next_skb))
3119                         break;
3120
3121         } while ((next_skb = skb_queue_next(&chan->srej_q, next_skb)));
3122
3123         __skb_queue_tail(&chan->srej_q, skb);
3124
3125         return 0;
3126 }
3127
3128 static int l2cap_ertm_reassembly_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u16 control)
3129 {
3130         struct sk_buff *_skb;
3131         int err;
3132
3133         switch (control & L2CAP_CTRL_SAR) {
3134         case L2CAP_SDU_UNSEGMENTED:
3135                 if (test_bit(CONN_SAR_SDU, &chan->conn_state))
3136                         goto drop;
3137
3138                 return chan->ops->recv(chan->data, skb);
3139
3140         case L2CAP_SDU_START:
3141                 if (test_bit(CONN_SAR_SDU, &chan->conn_state))
3142                         goto drop;
3143
3144                 chan->sdu_len = get_unaligned_le16(skb->data);
3145
3146                 if (chan->sdu_len > chan->imtu)
3147                         goto disconnect;
3148
3149                 chan->sdu = bt_skb_alloc(chan->sdu_len, GFP_ATOMIC);
3150                 if (!chan->sdu)
3151                         return -ENOMEM;
3152
3153                 /* pull sdu_len bytes only after alloc, because of Local Busy
3154                  * condition we have to be sure that this will be executed
3155                  * only once, i.e., when alloc does not fail */
3156                 skb_pull(skb, 2);
3157
3158                 memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len);
3159
3160                 set_bit(CONN_SAR_SDU, &chan->conn_state);
3161                 chan->partial_sdu_len = skb->len;
3162                 break;
3163
3164         case L2CAP_SDU_CONTINUE:
3165                 if (!test_bit(CONN_SAR_SDU, &chan->conn_state))
3166                         goto disconnect;
3167
3168                 if (!chan->sdu)
3169                         goto disconnect;
3170
3171                 chan->partial_sdu_len += skb->len;
3172                 if (chan->partial_sdu_len > chan->sdu_len)
3173                         goto drop;
3174
3175                 memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len);
3176
3177                 break;
3178
3179         case L2CAP_SDU_END:
3180                 if (!test_bit(CONN_SAR_SDU, &chan->conn_state))
3181                         goto disconnect;
3182
3183                 if (!chan->sdu)
3184                         goto disconnect;
3185
3186                 if (!test_bit(CONN_SAR_RETRY, &chan->conn_state)) {
3187                         chan->partial_sdu_len += skb->len;
3188
3189                         if (chan->partial_sdu_len > chan->imtu)
3190                                 goto drop;
3191
3192                         if (chan->partial_sdu_len != chan->sdu_len)
3193                                 goto drop;
3194
3195                         memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len);
3196                 }
3197
3198                 _skb = skb_clone(chan->sdu, GFP_ATOMIC);
3199                 if (!_skb) {
3200                         set_bit(CONN_SAR_RETRY, &chan->conn_state);
3201                         return -ENOMEM;
3202                 }
3203
3204                 err = chan->ops->recv(chan->data, _skb);
3205                 if (err < 0) {
3206                         kfree_skb(_skb);
3207                         set_bit(CONN_SAR_RETRY, &chan->conn_state);
3208                         return err;
3209                 }
3210
3211                 clear_bit(CONN_SAR_RETRY, &chan->conn_state);
3212                 clear_bit(CONN_SAR_SDU, &chan->conn_state);
3213
3214                 kfree_skb(chan->sdu);
3215                 break;
3216         }
3217
3218         kfree_skb(skb);
3219         return 0;
3220
3221 drop:
3222         kfree_skb(chan->sdu);
3223         chan->sdu = NULL;
3224
3225 disconnect:
3226         l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3227         kfree_skb(skb);
3228         return 0;
3229 }
3230
3231 static int l2cap_try_push_rx_skb(struct l2cap_chan *chan)
3232 {
3233         struct sk_buff *skb;
3234         u16 control;
3235         int err;
3236
3237         while ((skb = skb_dequeue(&chan->busy_q))) {
3238                 control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
3239                 err = l2cap_ertm_reassembly_sdu(chan, skb, control);
3240                 if (err < 0) {
3241                         skb_queue_head(&chan->busy_q, skb);
3242                         return -EBUSY;
3243                 }
3244
3245                 chan->buffer_seq = (chan->buffer_seq + 1) % 64;
3246         }
3247
3248         if (!test_bit(CONN_RNR_SENT, &chan->conn_state))
3249                 goto done;
3250
3251         control = chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3252         control |= L2CAP_SUPER_RCV_READY | L2CAP_CTRL_POLL;
3253         l2cap_send_sframe(chan, control);
3254         chan->retry_count = 1;
3255
3256         __clear_retrans_timer(chan);
3257         __set_monitor_timer(chan);
3258
3259         set_bit(CONN_WAIT_F, &chan->conn_state);
3260
3261 done:
3262         clear_bit(CONN_LOCAL_BUSY, &chan->conn_state);
3263         clear_bit(CONN_RNR_SENT, &chan->conn_state);
3264
3265         BT_DBG("chan %p, Exit local busy", chan);
3266
3267         return 0;
3268 }
3269
3270 static void l2cap_busy_work(struct work_struct *work)
3271 {
3272         DECLARE_WAITQUEUE(wait, current);
3273         struct l2cap_chan *chan =
3274                 container_of(work, struct l2cap_chan, busy_work);
3275         struct sock *sk = chan->sk;
3276         int n_tries = 0, timeo = HZ/5, err;
3277         struct sk_buff *skb;
3278
3279         lock_sock(sk);
3280
3281         add_wait_queue(sk_sleep(sk), &wait);
3282         while ((skb = skb_peek(&chan->busy_q))) {
3283                 set_current_state(TASK_INTERRUPTIBLE);
3284
3285                 if (n_tries++ > L2CAP_LOCAL_BUSY_TRIES) {
3286                         err = -EBUSY;
3287                         l2cap_send_disconn_req(chan->conn, chan, EBUSY);
3288                         break;
3289                 }
3290
3291                 if (!timeo)
3292                         timeo = HZ/5;
3293
3294                 if (signal_pending(current)) {
3295                         err = sock_intr_errno(timeo);
3296                         break;
3297                 }
3298
3299                 release_sock(sk);
3300                 timeo = schedule_timeout(timeo);
3301                 lock_sock(sk);
3302
3303                 err = sock_error(sk);
3304                 if (err)
3305                         break;
3306
3307                 if (l2cap_try_push_rx_skb(chan) == 0)
3308                         break;
3309         }
3310
3311         set_current_state(TASK_RUNNING);
3312         remove_wait_queue(sk_sleep(sk), &wait);
3313
3314         release_sock(sk);
3315 }
3316
3317 static int l2cap_push_rx_skb(struct l2cap_chan *chan, struct sk_buff *skb, u16 control)
3318 {
3319         int sctrl, err;
3320
3321         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
3322                 bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT;
3323                 __skb_queue_tail(&chan->busy_q, skb);
3324                 return l2cap_try_push_rx_skb(chan);
3325
3326
3327         }
3328
3329         err = l2cap_ertm_reassembly_sdu(chan, skb, control);
3330         if (err >= 0) {
3331                 chan->buffer_seq = (chan->buffer_seq + 1) % 64;
3332                 return err;
3333         }
3334
3335         /* Busy Condition */
3336         BT_DBG("chan %p, Enter local busy", chan);
3337
3338         set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
3339         bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT;
3340         __skb_queue_tail(&chan->busy_q, skb);
3341
3342         sctrl = chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3343         sctrl |= L2CAP_SUPER_RCV_NOT_READY;
3344         l2cap_send_sframe(chan, sctrl);
3345
3346         set_bit(CONN_RNR_SENT, &chan->conn_state);
3347
3348         __clear_ack_timer(chan);
3349
3350         queue_work(_busy_wq, &chan->busy_work);
3351
3352         return err;
3353 }
3354
3355 static int l2cap_streaming_reassembly_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u16 control)
3356 {
3357         struct sk_buff *_skb;
3358         int err = -EINVAL;
3359
3360         /*
3361          * TODO: We have to notify the userland if some data is lost with the
3362          * Streaming Mode.
3363          */
3364
3365         switch (control & L2CAP_CTRL_SAR) {
3366         case L2CAP_SDU_UNSEGMENTED:
3367                 if (test_bit(CONN_SAR_SDU, &chan->conn_state)) {
3368                         kfree_skb(chan->sdu);
3369                         break;
3370                 }
3371
3372                 err = chan->ops->recv(chan->data, skb);
3373                 if (!err)
3374                         return 0;
3375
3376                 break;
3377
3378         case L2CAP_SDU_START:
3379                 if (test_bit(CONN_SAR_SDU, &chan->conn_state)) {
3380                         kfree_skb(chan->sdu);
3381                         break;
3382                 }
3383
3384                 chan->sdu_len = get_unaligned_le16(skb->data);
3385                 skb_pull(skb, 2);
3386
3387                 if (chan->sdu_len > chan->imtu) {
3388                         err = -EMSGSIZE;
3389                         break;
3390                 }
3391
3392                 chan->sdu = bt_skb_alloc(chan->sdu_len, GFP_ATOMIC);
3393                 if (!chan->sdu) {
3394                         err = -ENOMEM;
3395                         break;
3396                 }
3397
3398                 memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len);
3399
3400                 set_bit(CONN_SAR_SDU, &chan->conn_state);
3401                 chan->partial_sdu_len = skb->len;
3402                 err = 0;
3403                 break;
3404
3405         case L2CAP_SDU_CONTINUE:
3406                 if (!test_bit(CONN_SAR_SDU, &chan->conn_state))
3407                         break;
3408
3409                 memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len);
3410
3411                 chan->partial_sdu_len += skb->len;
3412                 if (chan->partial_sdu_len > chan->sdu_len)
3413                         kfree_skb(chan->sdu);
3414                 else
3415                         err = 0;
3416
3417                 break;
3418
3419         case L2CAP_SDU_END:
3420                 if (!test_bit(CONN_SAR_SDU, &chan->conn_state))
3421                         break;
3422
3423                 memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len);
3424
3425                 clear_bit(CONN_SAR_SDU, &chan->conn_state);
3426                 chan->partial_sdu_len += skb->len;
3427
3428                 if (chan->partial_sdu_len > chan->imtu)
3429                         goto drop;
3430
3431                 if (chan->partial_sdu_len == chan->sdu_len) {
3432                         _skb = skb_clone(chan->sdu, GFP_ATOMIC);
3433                         err = chan->ops->recv(chan->data, _skb);
3434                         if (err < 0)
3435                                 kfree_skb(_skb);
3436                 }
3437                 err = 0;
3438
3439 drop:
3440                 kfree_skb(chan->sdu);
3441                 break;
3442         }
3443
3444         kfree_skb(skb);
3445         return err;
3446 }
3447
3448 static void l2cap_check_srej_gap(struct l2cap_chan *chan, u8 tx_seq)
3449 {
3450         struct sk_buff *skb;
3451         u16 control;
3452
3453         while ((skb = skb_peek(&chan->srej_q))) {
3454                 if (bt_cb(skb)->tx_seq != tx_seq)
3455                         break;
3456
3457                 skb = skb_dequeue(&chan->srej_q);
3458                 control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
3459                 l2cap_ertm_reassembly_sdu(chan, skb, control);
3460                 chan->buffer_seq_srej =
3461                         (chan->buffer_seq_srej + 1) % 64;
3462                 tx_seq = (tx_seq + 1) % 64;
3463         }
3464 }
3465
3466 static void l2cap_resend_srejframe(struct l2cap_chan *chan, u8 tx_seq)
3467 {
3468         struct srej_list *l, *tmp;
3469         u16 control;
3470
3471         list_for_each_entry_safe(l, tmp, &chan->srej_l, list) {
3472                 if (l->tx_seq == tx_seq) {
3473                         list_del(&l->list);
3474                         kfree(l);
3475                         return;
3476                 }
3477                 control = L2CAP_SUPER_SELECT_REJECT;
3478                 control |= l->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3479                 l2cap_send_sframe(chan, control);
3480                 list_del(&l->list);
3481                 list_add_tail(&l->list, &chan->srej_l);
3482         }
3483 }
3484
3485 static void l2cap_send_srejframe(struct l2cap_chan *chan, u8 tx_seq)
3486 {
3487         struct srej_list *new;
3488         u16 control;
3489
3490         while (tx_seq != chan->expected_tx_seq) {
3491                 control = L2CAP_SUPER_SELECT_REJECT;
3492                 control |= chan->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3493                 l2cap_send_sframe(chan, control);
3494
3495                 new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC);
3496                 new->tx_seq = chan->expected_tx_seq;
3497                 chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64;
3498                 list_add_tail(&new->list, &chan->srej_l);
3499         }
3500         chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64;
3501 }
3502
3503 static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u16 rx_control, struct sk_buff *skb)
3504 {
3505         u8 tx_seq = __get_txseq(rx_control);
3506         u8 req_seq = __get_reqseq(rx_control);
3507         u8 sar = rx_control >> L2CAP_CTRL_SAR_SHIFT;
3508         int tx_seq_offset, expected_tx_seq_offset;
3509         int num_to_ack = (chan->tx_win/6) + 1;
3510         int err = 0;
3511
3512         BT_DBG("chan %p len %d tx_seq %d rx_control 0x%4.4x", chan, skb->len,
3513                                                         tx_seq, rx_control);
3514
3515         if (L2CAP_CTRL_FINAL & rx_control &&
3516                         test_bit(CONN_WAIT_F, &chan->conn_state)) {
3517                 __clear_monitor_timer(chan);
3518                 if (chan->unacked_frames > 0)
3519                         __set_retrans_timer(chan);
3520                 clear_bit(CONN_WAIT_F, &chan->conn_state);
3521         }
3522
3523         chan->expected_ack_seq = req_seq;
3524         l2cap_drop_acked_frames(chan);
3525
3526         if (tx_seq == chan->expected_tx_seq)
3527                 goto expected;
3528
3529         tx_seq_offset = (tx_seq - chan->buffer_seq) % 64;
3530         if (tx_seq_offset < 0)
3531                 tx_seq_offset += 64;
3532
3533         /* invalid tx_seq */
3534         if (tx_seq_offset >= chan->tx_win) {
3535                 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3536                 goto drop;
3537         }
3538
3539         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state))
3540                 goto drop;
3541
3542         if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
3543                 struct srej_list *first;
3544
3545                 first = list_first_entry(&chan->srej_l,
3546                                 struct srej_list, list);
3547                 if (tx_seq == first->tx_seq) {
3548                         l2cap_add_to_srej_queue(chan, skb, tx_seq, sar);
3549                         l2cap_check_srej_gap(chan, tx_seq);
3550
3551                         list_del(&first->list);
3552                         kfree(first);
3553
3554                         if (list_empty(&chan->srej_l)) {
3555                                 chan->buffer_seq = chan->buffer_seq_srej;
3556                                 clear_bit(CONN_SREJ_SENT, &chan->conn_state);
3557                                 l2cap_send_ack(chan);
3558                                 BT_DBG("chan %p, Exit SREJ_SENT", chan);
3559                         }
3560                 } else {
3561                         struct srej_list *l;
3562
3563                         /* duplicated tx_seq */
3564                         if (l2cap_add_to_srej_queue(chan, skb, tx_seq, sar) < 0)
3565                                 goto drop;
3566
3567                         list_for_each_entry(l, &chan->srej_l, list) {
3568                                 if (l->tx_seq == tx_seq) {
3569                                         l2cap_resend_srejframe(chan, tx_seq);
3570                                         return 0;
3571                                 }
3572                         }
3573                         l2cap_send_srejframe(chan, tx_seq);
3574                 }
3575         } else {
3576                 expected_tx_seq_offset =
3577                         (chan->expected_tx_seq - chan->buffer_seq) % 64;
3578                 if (expected_tx_seq_offset < 0)
3579                         expected_tx_seq_offset += 64;
3580
3581                 /* duplicated tx_seq */
3582                 if (tx_seq_offset < expected_tx_seq_offset)
3583                         goto drop;
3584
3585                 set_bit(CONN_SREJ_SENT, &chan->conn_state);
3586
3587                 BT_DBG("chan %p, Enter SREJ", chan);
3588
3589                 INIT_LIST_HEAD(&chan->srej_l);
3590                 chan->buffer_seq_srej = chan->buffer_seq;
3591
3592                 __skb_queue_head_init(&chan->srej_q);
3593                 __skb_queue_head_init(&chan->busy_q);
3594                 l2cap_add_to_srej_queue(chan, skb, tx_seq, sar);
3595
3596                 set_bit(CONN_SEND_PBIT, &chan->conn_state);
3597
3598                 l2cap_send_srejframe(chan, tx_seq);
3599
3600                 __clear_ack_timer(chan);
3601         }
3602         return 0;
3603
3604 expected:
3605         chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64;
3606
3607         if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
3608                 bt_cb(skb)->tx_seq = tx_seq;
3609                 bt_cb(skb)->sar = sar;
3610                 __skb_queue_tail(&chan->srej_q, skb);
3611                 return 0;
3612         }
3613
3614         err = l2cap_push_rx_skb(chan, skb, rx_control);
3615         if (err < 0)
3616                 return 0;
3617
3618         if (rx_control & L2CAP_CTRL_FINAL) {
3619                 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
3620                         l2cap_retransmit_frames(chan);
3621         }
3622
3623         __set_ack_timer(chan);
3624
3625         chan->num_acked = (chan->num_acked + 1) % num_to_ack;
3626         if (chan->num_acked == num_to_ack - 1)
3627                 l2cap_send_ack(chan);
3628
3629         return 0;
3630
3631 drop:
3632         kfree_skb(skb);
3633         return 0;
3634 }
3635
3636 static inline void l2cap_data_channel_rrframe(struct l2cap_chan *chan, u16 rx_control)
3637 {
3638         BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, __get_reqseq(rx_control),
3639                                                 rx_control);
3640
3641         chan->expected_ack_seq = __get_reqseq(rx_control);
3642         l2cap_drop_acked_frames(chan);
3643
3644         if (rx_control & L2CAP_CTRL_POLL) {
3645                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
3646                 if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
3647                         if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
3648                                         (chan->unacked_frames > 0))
3649                                 __set_retrans_timer(chan);
3650
3651                         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3652                         l2cap_send_srejtail(chan);
3653                 } else {
3654                         l2cap_send_i_or_rr_or_rnr(chan);
3655                 }
3656
3657         } else if (rx_control & L2CAP_CTRL_FINAL) {
3658                 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3659
3660                 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
3661                         l2cap_retransmit_frames(chan);
3662
3663         } else {
3664                 if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
3665                                 (chan->unacked_frames > 0))
3666                         __set_retrans_timer(chan);
3667
3668                 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3669                 if (test_bit(CONN_SREJ_SENT, &chan->conn_state))
3670                         l2cap_send_ack(chan);
3671                 else
3672                         l2cap_ertm_send(chan);
3673         }
3674 }
3675
3676 static inline void l2cap_data_channel_rejframe(struct l2cap_chan *chan, u16 rx_control)
3677 {
3678         u8 tx_seq = __get_reqseq(rx_control);
3679
3680         BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, tx_seq, rx_control);
3681
3682         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3683
3684         chan->expected_ack_seq = tx_seq;
3685         l2cap_drop_acked_frames(chan);
3686
3687         if (rx_control & L2CAP_CTRL_FINAL) {
3688                 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
3689                         l2cap_retransmit_frames(chan);
3690         } else {
3691                 l2cap_retransmit_frames(chan);
3692
3693                 if (test_bit(CONN_WAIT_F, &chan->conn_state))
3694                         set_bit(CONN_REJ_ACT, &chan->conn_state);
3695         }
3696 }
3697 static inline void l2cap_data_channel_srejframe(struct l2cap_chan *chan, u16 rx_control)
3698 {
3699         u8 tx_seq = __get_reqseq(rx_control);
3700
3701         BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, tx_seq, rx_control);
3702
3703         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3704
3705         if (rx_control & L2CAP_CTRL_POLL) {
3706                 chan->expected_ack_seq = tx_seq;
3707                 l2cap_drop_acked_frames(chan);
3708
3709                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
3710                 l2cap_retransmit_one_frame(chan, tx_seq);
3711
3712                 l2cap_ertm_send(chan);
3713
3714                 if (test_bit(CONN_WAIT_F, &chan->conn_state)) {
3715                         chan->srej_save_reqseq = tx_seq;
3716                         set_bit(CONN_SREJ_ACT, &chan->conn_state);
3717                 }
3718         } else if (rx_control & L2CAP_CTRL_FINAL) {
3719                 if (test_bit(CONN_SREJ_ACT, &chan->conn_state) &&
3720                                 chan->srej_save_reqseq == tx_seq)
3721                         clear_bit(CONN_SREJ_ACT, &chan->conn_state);
3722                 else
3723                         l2cap_retransmit_one_frame(chan, tx_seq);
3724         } else {
3725                 l2cap_retransmit_one_frame(chan, tx_seq);
3726                 if (test_bit(CONN_WAIT_F, &chan->conn_state)) {
3727                         chan->srej_save_reqseq = tx_seq;
3728                         set_bit(CONN_SREJ_ACT, &chan->conn_state);
3729                 }
3730         }
3731 }
3732
3733 static inline void l2cap_data_channel_rnrframe(struct l2cap_chan *chan, u16 rx_control)
3734 {
3735         u8 tx_seq = __get_reqseq(rx_control);
3736
3737         BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, tx_seq, rx_control);
3738
3739         set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3740         chan->expected_ack_seq = tx_seq;
3741         l2cap_drop_acked_frames(chan);
3742
3743         if (rx_control & L2CAP_CTRL_POLL)
3744                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
3745
3746         if (!test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
3747                 __clear_retrans_timer(chan);
3748                 if (rx_control & L2CAP_CTRL_POLL)
3749                         l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_FINAL);
3750                 return;
3751         }
3752
3753         if (rx_control & L2CAP_CTRL_POLL)
3754                 l2cap_send_srejtail(chan);
3755         else
3756                 l2cap_send_sframe(chan, L2CAP_SUPER_RCV_READY);
3757 }
3758
3759 static inline int l2cap_data_channel_sframe(struct l2cap_chan *chan, u16 rx_control, struct sk_buff *skb)
3760 {
3761         BT_DBG("chan %p rx_control 0x%4.4x len %d", chan, rx_control, skb->len);
3762
3763         if (L2CAP_CTRL_FINAL & rx_control &&
3764                         test_bit(CONN_WAIT_F, &chan->conn_state)) {
3765                 __clear_monitor_timer(chan);
3766                 if (chan->unacked_frames > 0)
3767                         __set_retrans_timer(chan);
3768                 clear_bit(CONN_WAIT_F, &chan->conn_state);
3769         }
3770
3771         switch (rx_control & L2CAP_CTRL_SUPERVISE) {
3772         case L2CAP_SUPER_RCV_READY:
3773                 l2cap_data_channel_rrframe(chan, rx_control);
3774                 break;
3775
3776         case L2CAP_SUPER_REJECT:
3777                 l2cap_data_channel_rejframe(chan, rx_control);
3778                 break;
3779
3780         case L2CAP_SUPER_SELECT_REJECT:
3781                 l2cap_data_channel_srejframe(chan, rx_control);
3782                 break;
3783
3784         case L2CAP_SUPER_RCV_NOT_READY:
3785                 l2cap_data_channel_rnrframe(chan, rx_control);
3786                 break;
3787         }
3788
3789         kfree_skb(skb);
3790         return 0;
3791 }
3792
3793 static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb)
3794 {
3795         struct l2cap_chan *chan = l2cap_pi(sk)->chan;
3796         u16 control;
3797         u8 req_seq;
3798         int len, next_tx_seq_offset, req_seq_offset;
3799
3800         control = get_unaligned_le16(skb->data);
3801         skb_pull(skb, 2);
3802         len = skb->len;
3803
3804         /*
3805          * We can just drop the corrupted I-frame here.
3806          * Receiver will miss it and start proper recovery
3807          * procedures and ask retransmission.
3808          */
3809         if (l2cap_check_fcs(chan, skb))
3810                 goto drop;
3811
3812         if (__is_sar_start(control) && __is_iframe(control))
3813                 len -= 2;
3814
3815         if (chan->fcs == L2CAP_FCS_CRC16)
3816                 len -= 2;
3817
3818         if (len > chan->mps) {
3819                 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3820                 goto drop;
3821         }
3822
3823         req_seq = __get_reqseq(control);
3824         req_seq_offset = (req_seq - chan->expected_ack_seq) % 64;
3825         if (req_seq_offset < 0)
3826                 req_seq_offset += 64;
3827
3828         next_tx_seq_offset =
3829                 (chan->next_tx_seq - chan->expected_ack_seq) % 64;
3830         if (next_tx_seq_offset < 0)
3831                 next_tx_seq_offset += 64;
3832
3833         /* check for invalid req-seq */
3834         if (req_seq_offset > next_tx_seq_offset) {
3835                 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3836                 goto drop;
3837         }
3838
3839         if (__is_iframe(control)) {
3840                 if (len < 0) {
3841                         l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3842                         goto drop;
3843                 }
3844
3845                 l2cap_data_channel_iframe(chan, control, skb);
3846         } else {
3847                 if (len != 0) {
3848                         BT_ERR("%d", len);
3849                         l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3850                         goto drop;
3851                 }
3852
3853                 l2cap_data_channel_sframe(chan, control, skb);
3854         }
3855
3856         return 0;
3857
3858 drop:
3859         kfree_skb(skb);
3860         return 0;
3861 }
3862
3863 static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
3864 {
3865         struct l2cap_chan *chan;
3866         struct sock *sk = NULL;
3867         u16 control;
3868         u8 tx_seq;
3869         int len;
3870
3871         chan = l2cap_get_chan_by_scid(conn, cid);
3872         if (!chan) {
3873                 BT_DBG("unknown cid 0x%4.4x", cid);
3874                 goto drop;
3875         }
3876
3877         sk = chan->sk;
3878
3879         BT_DBG("chan %p, len %d", chan, skb->len);
3880
3881         if (chan->state != BT_CONNECTED)
3882                 goto drop;
3883
3884         switch (chan->mode) {
3885         case L2CAP_MODE_BASIC:
3886                 /* If socket recv buffers overflows we drop data here
3887                  * which is *bad* because L2CAP has to be reliable.
3888                  * But we don't have any other choice. L2CAP doesn't
3889                  * provide flow control mechanism. */
3890
3891                 if (chan->imtu < skb->len)
3892                         goto drop;
3893
3894                 if (!chan->ops->recv(chan->data, skb))
3895                         goto done;
3896                 break;
3897
3898         case L2CAP_MODE_ERTM:
3899                 if (!sock_owned_by_user(sk)) {
3900                         l2cap_ertm_data_rcv(sk, skb);
3901                 } else {
3902                         if (sk_add_backlog(sk, skb))
3903                                 goto drop;
3904                 }
3905
3906                 goto done;
3907
3908         case L2CAP_MODE_STREAMING:
3909                 control = get_unaligned_le16(skb->data);
3910                 skb_pull(skb, 2);
3911                 len = skb->len;
3912
3913                 if (l2cap_check_fcs(chan, skb))
3914                         goto drop;
3915
3916                 if (__is_sar_start(control))
3917                         len -= 2;
3918
3919                 if (chan->fcs == L2CAP_FCS_CRC16)
3920                         len -= 2;
3921
3922                 if (len > chan->mps || len < 0 || __is_sframe(control))
3923                         goto drop;
3924
3925                 tx_seq = __get_txseq(control);
3926
3927                 if (chan->expected_tx_seq == tx_seq)
3928                         chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64;
3929                 else
3930                         chan->expected_tx_seq = (tx_seq + 1) % 64;
3931
3932                 l2cap_streaming_reassembly_sdu(chan, skb, control);
3933
3934                 goto done;
3935
3936         default:
3937                 BT_DBG("chan %p: bad mode 0x%2.2x", chan, chan->mode);
3938                 break;
3939         }
3940
3941 drop:
3942         kfree_skb(skb);
3943
3944 done:
3945         if (sk)
3946                 bh_unlock_sock(sk);
3947
3948         return 0;
3949 }
3950
3951 static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb)
3952 {
3953         struct sock *sk = NULL;
3954         struct l2cap_chan *chan;
3955
3956         chan = l2cap_global_chan_by_psm(0, psm, conn->src);
3957         if (!chan)
3958                 goto drop;
3959
3960         sk = chan->sk;
3961
3962         bh_lock_sock(sk);
3963
3964         BT_DBG("sk %p, len %d", sk, skb->len);
3965
3966         if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
3967                 goto drop;
3968
3969         if (chan->imtu < skb->len)
3970                 goto drop;
3971
3972         if (!chan->ops->recv(chan->data, skb))
3973                 goto done;
3974
3975 drop:
3976         kfree_skb(skb);
3977
3978 done:
3979         if (sk)
3980                 bh_unlock_sock(sk);
3981         return 0;
3982 }
3983
3984 static inline int l2cap_att_channel(struct l2cap_conn *conn, __le16 cid, struct sk_buff *skb)
3985 {
3986         struct sock *sk = NULL;
3987         struct l2cap_chan *chan;
3988
3989         chan = l2cap_global_chan_by_scid(0, cid, conn->src);
3990         if (!chan)
3991                 goto drop;
3992
3993         sk = chan->sk;
3994
3995         bh_lock_sock(sk);
3996
3997         BT_DBG("sk %p, len %d", sk, skb->len);
3998
3999         if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
4000                 goto drop;
4001
4002         if (chan->imtu < skb->len)
4003                 goto drop;
4004
4005         if (!chan->ops->recv(chan->data, skb))
4006                 goto done;
4007
4008 drop:
4009         kfree_skb(skb);
4010
4011 done:
4012         if (sk)
4013                 bh_unlock_sock(sk);
4014         return 0;
4015 }
4016
4017 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
4018 {
4019         struct l2cap_hdr *lh = (void *) skb->data;
4020         u16 cid, len;
4021         __le16 psm;
4022
4023         skb_pull(skb, L2CAP_HDR_SIZE);
4024         cid = __le16_to_cpu(lh->cid);
4025         len = __le16_to_cpu(lh->len);
4026
4027         if (len != skb->len) {
4028                 kfree_skb(skb);
4029                 return;
4030         }
4031
4032         BT_DBG("len %d, cid 0x%4.4x", len, cid);
4033
4034         switch (cid) {
4035         case L2CAP_CID_LE_SIGNALING:
4036         case L2CAP_CID_SIGNALING:
4037                 l2cap_sig_channel(conn, skb);
4038                 break;
4039
4040         case L2CAP_CID_CONN_LESS:
4041                 psm = get_unaligned_le16(skb->data);
4042                 skb_pull(skb, 2);
4043                 l2cap_conless_channel(conn, psm, skb);
4044                 break;
4045
4046         case L2CAP_CID_LE_DATA:
4047                 l2cap_att_channel(conn, cid, skb);
4048                 break;
4049
4050         case L2CAP_CID_SMP:
4051                 if (smp_sig_channel(conn, skb))
4052                         l2cap_conn_del(conn->hcon, EACCES);
4053                 break;
4054
4055         default:
4056                 l2cap_data_channel(conn, cid, skb);
4057                 break;
4058         }
4059 }
4060
4061 /* ---- L2CAP interface with lower layer (HCI) ---- */
4062
4063 static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
4064 {
4065         int exact = 0, lm1 = 0, lm2 = 0;
4066         struct l2cap_chan *c;
4067
4068         if (type != ACL_LINK)
4069                 return -EINVAL;
4070
4071         BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
4072
4073         /* Find listening sockets and check their link_mode */
4074         read_lock(&chan_list_lock);
4075         list_for_each_entry(c, &chan_list, global_l) {
4076                 struct sock *sk = c->sk;
4077
4078                 if (c->state != BT_LISTEN)
4079                         continue;
4080
4081                 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
4082                         lm1 |= HCI_LM_ACCEPT;
4083                         if (c->role_switch)
4084                                 lm1 |= HCI_LM_MASTER;
4085                         exact++;
4086                 } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
4087                         lm2 |= HCI_LM_ACCEPT;
4088                         if (c->role_switch)
4089                                 lm2 |= HCI_LM_MASTER;
4090                 }
4091         }
4092         read_unlock(&chan_list_lock);
4093
4094         return exact ? lm1 : lm2;
4095 }
4096
4097 static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
4098 {
4099         struct l2cap_conn *conn;
4100
4101         BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
4102
4103         if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK))
4104                 return -EINVAL;
4105
4106         if (!status) {
4107                 conn = l2cap_conn_add(hcon, status);
4108                 if (conn)
4109                         l2cap_conn_ready(conn);
4110         } else
4111                 l2cap_conn_del(hcon, bt_err(status));
4112
4113         return 0;
4114 }
4115
4116 static int l2cap_disconn_ind(struct hci_conn *hcon)
4117 {
4118         struct l2cap_conn *conn = hcon->l2cap_data;
4119
4120         BT_DBG("hcon %p", hcon);
4121
4122         if ((hcon->type != ACL_LINK && hcon->type != LE_LINK) || !conn)
4123                 return 0x13;
4124
4125         return conn->disc_reason;
4126 }
4127
4128 static int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
4129 {
4130         BT_DBG("hcon %p reason %d", hcon, reason);
4131
4132         if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK))
4133                 return -EINVAL;
4134
4135         l2cap_conn_del(hcon, bt_err(reason));
4136
4137         return 0;
4138 }
4139
4140 static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt)
4141 {
4142         if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED)
4143                 return;
4144
4145         if (encrypt == 0x00) {
4146                 if (chan->sec_level == BT_SECURITY_MEDIUM) {
4147                         __clear_chan_timer(chan);
4148                         __set_chan_timer(chan, HZ * 5);
4149                 } else if (chan->sec_level == BT_SECURITY_HIGH)
4150                         l2cap_chan_close(chan, ECONNREFUSED);
4151         } else {
4152                 if (chan->sec_level == BT_SECURITY_MEDIUM)
4153                         __clear_chan_timer(chan);
4154         }
4155 }
4156
4157 static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
4158 {
4159         struct l2cap_conn *conn = hcon->l2cap_data;
4160         struct l2cap_chan *chan;
4161
4162         if (!conn)
4163                 return 0;
4164
4165         BT_DBG("conn %p", conn);
4166
4167         read_lock(&conn->chan_lock);
4168
4169         list_for_each_entry(chan, &conn->chan_l, list) {
4170                 struct sock *sk = chan->sk;
4171
4172                 bh_lock_sock(sk);
4173
4174                 BT_DBG("chan->scid %d", chan->scid);
4175
4176                 if (chan->scid == L2CAP_CID_LE_DATA) {
4177                         if (!status && encrypt) {
4178                                 chan->sec_level = hcon->sec_level;
4179                                 del_timer(&conn->security_timer);
4180                                 l2cap_chan_ready(sk);
4181                         }
4182
4183                         bh_unlock_sock(sk);
4184                         continue;
4185                 }
4186
4187                 if (test_bit(CONF_CONNECT_PEND, &chan->conf_state)) {
4188                         bh_unlock_sock(sk);
4189                         continue;
4190                 }
4191
4192                 if (!status && (chan->state == BT_CONNECTED ||
4193                                                 chan->state == BT_CONFIG)) {
4194                         l2cap_check_encryption(chan, encrypt);
4195                         bh_unlock_sock(sk);
4196                         continue;
4197                 }
4198
4199                 if (chan->state == BT_CONNECT) {
4200                         if (!status) {
4201                                 struct l2cap_conn_req req;
4202                                 req.scid = cpu_to_le16(chan->scid);
4203                                 req.psm  = chan->psm;
4204
4205                                 chan->ident = l2cap_get_ident(conn);
4206                                 set_bit(CONF_CONNECT_PEND, &chan->conf_state);
4207
4208                                 l2cap_send_cmd(conn, chan->ident,
4209                                         L2CAP_CONN_REQ, sizeof(req), &req);
4210                         } else {
4211                                 __clear_chan_timer(chan);
4212                                 __set_chan_timer(chan, HZ / 10);
4213                         }
4214                 } else if (chan->state == BT_CONNECT2) {
4215                         struct l2cap_conn_rsp rsp;
4216                         __u16 res, stat;
4217
4218                         if (!status) {
4219                                 if (bt_sk(sk)->defer_setup) {
4220                                         struct sock *parent = bt_sk(sk)->parent;
4221                                         res = L2CAP_CR_PEND;
4222                                         stat = L2CAP_CS_AUTHOR_PEND;
4223                                         parent->sk_data_ready(parent, 0);
4224                                 } else {
4225                                         l2cap_state_change(chan, BT_CONFIG);
4226                                         res = L2CAP_CR_SUCCESS;
4227                                         stat = L2CAP_CS_NO_INFO;
4228                                 }
4229                         } else {
4230                                 l2cap_state_change(chan, BT_DISCONN);
4231                                 __set_chan_timer(chan, HZ / 10);
4232                                 res = L2CAP_CR_SEC_BLOCK;
4233                                 stat = L2CAP_CS_NO_INFO;
4234                         }
4235
4236                         rsp.scid   = cpu_to_le16(chan->dcid);
4237                         rsp.dcid   = cpu_to_le16(chan->scid);
4238                         rsp.result = cpu_to_le16(res);
4239                         rsp.status = cpu_to_le16(stat);
4240                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
4241                                                         sizeof(rsp), &rsp);
4242                 }
4243
4244                 bh_unlock_sock(sk);
4245         }
4246
4247         read_unlock(&conn->chan_lock);
4248
4249         return 0;
4250 }
4251
4252 static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
4253 {
4254         struct l2cap_conn *conn = hcon->l2cap_data;
4255
4256         if (!conn)
4257                 conn = l2cap_conn_add(hcon, 0);
4258
4259         if (!conn)
4260                 goto drop;
4261
4262         BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
4263
4264         if (!(flags & ACL_CONT)) {
4265                 struct l2cap_hdr *hdr;
4266                 struct l2cap_chan *chan;
4267                 u16 cid;
4268                 int len;
4269
4270                 if (conn->rx_len) {
4271                         BT_ERR("Unexpected start frame (len %d)", skb->len);
4272                         kfree_skb(conn->rx_skb);
4273                         conn->rx_skb = NULL;
4274                         conn->rx_len = 0;
4275                         l2cap_conn_unreliable(conn, ECOMM);
4276                 }
4277
4278                 /* Start fragment always begin with Basic L2CAP header */
4279                 if (skb->len < L2CAP_HDR_SIZE) {
4280                         BT_ERR("Frame is too short (len %d)", skb->len);
4281                         l2cap_conn_unreliable(conn, ECOMM);
4282                         goto drop;
4283                 }
4284
4285                 hdr = (struct l2cap_hdr *) skb->data;
4286                 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
4287                 cid = __le16_to_cpu(hdr->cid);
4288
4289                 if (len == skb->len) {
4290                         /* Complete frame received */
4291                         l2cap_recv_frame(conn, skb);
4292                         return 0;
4293                 }
4294
4295                 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
4296
4297                 if (skb->len > len) {
4298                         BT_ERR("Frame is too long (len %d, expected len %d)",
4299                                 skb->len, len);
4300                         l2cap_conn_unreliable(conn, ECOMM);
4301                         goto drop;
4302                 }
4303
4304                 chan = l2cap_get_chan_by_scid(conn, cid);
4305
4306                 if (chan && chan->sk) {
4307                         struct sock *sk = chan->sk;
4308
4309                         if (chan->imtu < len - L2CAP_HDR_SIZE) {
4310                                 BT_ERR("Frame exceeding recv MTU (len %d, "
4311                                                         "MTU %d)", len,
4312                                                         chan->imtu);
4313                                 bh_unlock_sock(sk);
4314                                 l2cap_conn_unreliable(conn, ECOMM);
4315                                 goto drop;
4316                         }
4317                         bh_unlock_sock(sk);
4318                 }
4319
4320                 /* Allocate skb for the complete frame (with header) */
4321                 conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC);
4322                 if (!conn->rx_skb)
4323                         goto drop;
4324
4325                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
4326                                                                 skb->len);
4327                 conn->rx_len = len - skb->len;
4328         } else {
4329                 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
4330
4331                 if (!conn->rx_len) {
4332                         BT_ERR("Unexpected continuation frame (len %d)", skb->len);
4333                         l2cap_conn_unreliable(conn, ECOMM);
4334                         goto drop;
4335                 }
4336
4337                 if (skb->len > conn->rx_len) {
4338                         BT_ERR("Fragment is too long (len %d, expected %d)",
4339                                         skb->len, conn->rx_len);
4340                         kfree_skb(conn->rx_skb);
4341                         conn->rx_skb = NULL;
4342                         conn->rx_len = 0;
4343                         l2cap_conn_unreliable(conn, ECOMM);
4344                         goto drop;
4345                 }
4346
4347                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
4348                                                                 skb->len);
4349                 conn->rx_len -= skb->len;
4350
4351                 if (!conn->rx_len) {
4352                         /* Complete frame received */
4353                         l2cap_recv_frame(conn, conn->rx_skb);
4354                         conn->rx_skb = NULL;
4355                 }
4356         }
4357
4358 drop:
4359         kfree_skb(skb);
4360         return 0;
4361 }
4362
4363 static int l2cap_debugfs_show(struct seq_file *f, void *p)
4364 {
4365         struct l2cap_chan *c;
4366
4367         read_lock_bh(&chan_list_lock);
4368
4369         list_for_each_entry(c, &chan_list, global_l) {
4370                 struct sock *sk = c->sk;
4371
4372                 seq_printf(f, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
4373                                         batostr(&bt_sk(sk)->src),
4374                                         batostr(&bt_sk(sk)->dst),
4375                                         c->state, __le16_to_cpu(c->psm),
4376                                         c->scid, c->dcid, c->imtu, c->omtu,
4377                                         c->sec_level, c->mode);
4378 }
4379
4380         read_unlock_bh(&chan_list_lock);
4381
4382         return 0;
4383 }
4384
4385 static int l2cap_debugfs_open(struct inode *inode, struct file *file)
4386 {
4387         return single_open(file, l2cap_debugfs_show, inode->i_private);
4388 }
4389
4390 static const struct file_operations l2cap_debugfs_fops = {
4391         .open           = l2cap_debugfs_open,
4392         .read           = seq_read,
4393         .llseek         = seq_lseek,
4394         .release        = single_release,
4395 };
4396
4397 static struct dentry *l2cap_debugfs;
4398
4399 static struct hci_proto l2cap_hci_proto = {
4400         .name           = "L2CAP",
4401         .id             = HCI_PROTO_L2CAP,
4402         .connect_ind    = l2cap_connect_ind,
4403         .connect_cfm    = l2cap_connect_cfm,
4404         .disconn_ind    = l2cap_disconn_ind,
4405         .disconn_cfm    = l2cap_disconn_cfm,
4406         .security_cfm   = l2cap_security_cfm,
4407         .recv_acldata   = l2cap_recv_acldata
4408 };
4409
4410 int __init l2cap_init(void)
4411 {
4412         int err;
4413
4414         err = l2cap_init_sockets();
4415         if (err < 0)
4416                 return err;
4417
4418         _busy_wq = create_singlethread_workqueue("l2cap");
4419         if (!_busy_wq) {
4420                 err = -ENOMEM;
4421                 goto error;
4422         }
4423
4424         err = hci_register_proto(&l2cap_hci_proto);
4425         if (err < 0) {
4426                 BT_ERR("L2CAP protocol registration failed");
4427                 bt_sock_unregister(BTPROTO_L2CAP);
4428                 goto error;
4429         }
4430
4431         if (bt_debugfs) {
4432                 l2cap_debugfs = debugfs_create_file("l2cap", 0444,
4433                                         bt_debugfs, NULL, &l2cap_debugfs_fops);
4434                 if (!l2cap_debugfs)
4435                         BT_ERR("Failed to create L2CAP debug file");
4436         }
4437
4438         return 0;
4439
4440 error:
4441         destroy_workqueue(_busy_wq);
4442         l2cap_cleanup_sockets();
4443         return err;
4444 }
4445
4446 void l2cap_exit(void)
4447 {
4448         debugfs_remove(l2cap_debugfs);
4449
4450         flush_workqueue(_busy_wq);
4451         destroy_workqueue(_busy_wq);
4452
4453         if (hci_unregister_proto(&l2cap_hci_proto) < 0)
4454                 BT_ERR("L2CAP protocol unregistration failed");
4455
4456         l2cap_cleanup_sockets();
4457 }
4458
4459 module_param(disable_ertm, bool, 0644);
4460 MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode");