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