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);
  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), 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         len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen;
1741         count = min_t(unsigned int, conn->mtu, len);
1742
1743         skb = bt_skb_alloc(count, GFP_ATOMIC);
1744         if (!skb)
1745                 return NULL;
1746
1747         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1748         lh->len = cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen);
1749
1750         if (conn->hcon->type == LE_LINK)
1751                 lh->cid = cpu_to_le16(L2CAP_CID_LE_SIGNALING);
1752         else
1753                 lh->cid = cpu_to_le16(L2CAP_CID_SIGNALING);
1754
1755         cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE);
1756         cmd->code  = code;
1757         cmd->ident = ident;
1758         cmd->len   = cpu_to_le16(dlen);
1759
1760         if (dlen) {
1761                 count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE;
1762                 memcpy(skb_put(skb, count), data, count);
1763                 data += count;
1764         }
1765
1766         len -= skb->len;
1767
1768         /* Continuation fragments (no L2CAP header) */
1769         frag = &skb_shinfo(skb)->frag_list;
1770         while (len) {
1771                 count = min_t(unsigned int, conn->mtu, len);
1772
1773                 *frag = bt_skb_alloc(count, GFP_ATOMIC);
1774                 if (!*frag)
1775                         goto fail;
1776
1777                 memcpy(skb_put(*frag, count), data, count);
1778
1779                 len  -= count;
1780                 data += count;
1781
1782                 frag = &(*frag)->next;
1783         }
1784
1785         return skb;
1786
1787 fail:
1788         kfree_skb(skb);
1789         return NULL;
1790 }
1791
1792 static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val)
1793 {
1794         struct l2cap_conf_opt *opt = *ptr;
1795         int len;
1796
1797         len = L2CAP_CONF_OPT_SIZE + opt->len;
1798         *ptr += len;
1799
1800         *type = opt->type;
1801         *olen = opt->len;
1802
1803         switch (opt->len) {
1804         case 1:
1805                 *val = *((u8 *) opt->val);
1806                 break;
1807
1808         case 2:
1809                 *val = get_unaligned_le16(opt->val);
1810                 break;
1811
1812         case 4:
1813                 *val = get_unaligned_le32(opt->val);
1814                 break;
1815
1816         default:
1817                 *val = (unsigned long) opt->val;
1818                 break;
1819         }
1820
1821         BT_DBG("type 0x%2.2x len %d val 0x%lx", *type, opt->len, *val);
1822         return len;
1823 }
1824
1825 static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val)
1826 {
1827         struct l2cap_conf_opt *opt = *ptr;
1828
1829         BT_DBG("type 0x%2.2x len %d val 0x%lx", type, len, val);
1830
1831         opt->type = type;
1832         opt->len  = len;
1833
1834         switch (len) {
1835         case 1:
1836                 *((u8 *) opt->val)  = val;
1837                 break;
1838
1839         case 2:
1840                 put_unaligned_le16(val, opt->val);
1841                 break;
1842
1843         case 4:
1844                 put_unaligned_le32(val, opt->val);
1845                 break;
1846
1847         default:
1848                 memcpy(opt->val, (void *) val, len);
1849                 break;
1850         }
1851
1852         *ptr += L2CAP_CONF_OPT_SIZE + len;
1853 }
1854
1855 static void l2cap_ack_timeout(unsigned long arg)
1856 {
1857         struct l2cap_chan *chan = (void *) arg;
1858
1859         bh_lock_sock(chan->sk);
1860         l2cap_send_ack(chan);
1861         bh_unlock_sock(chan->sk);
1862 }
1863
1864 static inline void l2cap_ertm_init(struct l2cap_chan *chan)
1865 {
1866         struct sock *sk = chan->sk;
1867
1868         chan->expected_ack_seq = 0;
1869         chan->unacked_frames = 0;
1870         chan->buffer_seq = 0;
1871         chan->num_acked = 0;
1872         chan->frames_sent = 0;
1873
1874         setup_timer(&chan->retrans_timer, l2cap_retrans_timeout,
1875                                                         (unsigned long) chan);
1876         setup_timer(&chan->monitor_timer, l2cap_monitor_timeout,
1877                                                         (unsigned long) chan);
1878         setup_timer(&chan->ack_timer, l2cap_ack_timeout, (unsigned long) chan);
1879
1880         skb_queue_head_init(&chan->srej_q);
1881
1882         INIT_LIST_HEAD(&chan->srej_l);
1883
1884
1885         sk->sk_backlog_rcv = l2cap_ertm_data_rcv;
1886 }
1887
1888 static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
1889 {
1890         switch (mode) {
1891         case L2CAP_MODE_STREAMING:
1892         case L2CAP_MODE_ERTM:
1893                 if (l2cap_mode_supported(mode, remote_feat_mask))
1894                         return mode;
1895                 /* fall through */
1896         default:
1897                 return L2CAP_MODE_BASIC;
1898         }
1899 }
1900
1901 static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data)
1902 {
1903         struct l2cap_conf_req *req = data;
1904         struct l2cap_conf_rfc rfc = { .mode = chan->mode };
1905         void *ptr = req->data;
1906
1907         BT_DBG("chan %p", chan);
1908
1909         if (chan->num_conf_req || chan->num_conf_rsp)
1910                 goto done;
1911
1912         switch (chan->mode) {
1913         case L2CAP_MODE_STREAMING:
1914         case L2CAP_MODE_ERTM:
1915                 if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state))
1916                         break;
1917
1918                 /* fall through */
1919         default:
1920                 chan->mode = l2cap_select_mode(rfc.mode, chan->conn->feat_mask);
1921                 break;
1922         }
1923
1924 done:
1925         if (chan->imtu != L2CAP_DEFAULT_MTU)
1926                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu);
1927
1928         switch (chan->mode) {
1929         case L2CAP_MODE_BASIC:
1930                 if (!(chan->conn->feat_mask & L2CAP_FEAT_ERTM) &&
1931                                 !(chan->conn->feat_mask & L2CAP_FEAT_STREAMING))
1932                         break;
1933
1934                 rfc.mode            = L2CAP_MODE_BASIC;
1935                 rfc.txwin_size      = 0;
1936                 rfc.max_transmit    = 0;
1937                 rfc.retrans_timeout = 0;
1938                 rfc.monitor_timeout = 0;
1939                 rfc.max_pdu_size    = 0;
1940
1941                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
1942                                                         (unsigned long) &rfc);
1943                 break;
1944
1945         case L2CAP_MODE_ERTM:
1946                 rfc.mode            = L2CAP_MODE_ERTM;
1947                 rfc.txwin_size      = chan->tx_win;
1948                 rfc.max_transmit    = chan->max_tx;
1949                 rfc.retrans_timeout = 0;
1950                 rfc.monitor_timeout = 0;
1951                 rfc.max_pdu_size    = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE);
1952                 if (L2CAP_DEFAULT_MAX_PDU_SIZE > chan->conn->mtu - 10)
1953                         rfc.max_pdu_size = cpu_to_le16(chan->conn->mtu - 10);
1954
1955                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
1956                                                         (unsigned long) &rfc);
1957
1958                 if (!(chan->conn->feat_mask & L2CAP_FEAT_FCS))
1959                         break;
1960
1961                 if (chan->fcs == L2CAP_FCS_NONE ||
1962                                 test_bit(CONF_NO_FCS_RECV, &chan->conf_state)) {
1963                         chan->fcs = L2CAP_FCS_NONE;
1964                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, chan->fcs);
1965                 }
1966                 break;
1967
1968         case L2CAP_MODE_STREAMING:
1969                 rfc.mode            = L2CAP_MODE_STREAMING;
1970                 rfc.txwin_size      = 0;
1971                 rfc.max_transmit    = 0;
1972                 rfc.retrans_timeout = 0;
1973                 rfc.monitor_timeout = 0;
1974                 rfc.max_pdu_size    = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE);
1975                 if (L2CAP_DEFAULT_MAX_PDU_SIZE > chan->conn->mtu - 10)
1976                         rfc.max_pdu_size = cpu_to_le16(chan->conn->mtu - 10);
1977
1978                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
1979                                                         (unsigned long) &rfc);
1980
1981                 if (!(chan->conn->feat_mask & L2CAP_FEAT_FCS))
1982                         break;
1983
1984                 if (chan->fcs == L2CAP_FCS_NONE ||
1985                                 test_bit(CONF_NO_FCS_RECV, &chan->conf_state)) {
1986                         chan->fcs = L2CAP_FCS_NONE;
1987                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, chan->fcs);
1988                 }
1989                 break;
1990         }
1991
1992         req->dcid  = cpu_to_le16(chan->dcid);
1993         req->flags = cpu_to_le16(0);
1994
1995         return ptr - data;
1996 }
1997
1998 static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data)
1999 {
2000         struct l2cap_conf_rsp *rsp = data;
2001         void *ptr = rsp->data;
2002         void *req = chan->conf_req;
2003         int len = chan->conf_len;
2004         int type, hint, olen;
2005         unsigned long val;
2006         struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
2007         u16 mtu = L2CAP_DEFAULT_MTU;
2008         u16 result = L2CAP_CONF_SUCCESS;
2009
2010         BT_DBG("chan %p", chan);
2011
2012         while (len >= L2CAP_CONF_OPT_SIZE) {
2013                 len -= l2cap_get_conf_opt(&req, &type, &olen, &val);
2014
2015                 hint  = type & L2CAP_CONF_HINT;
2016                 type &= L2CAP_CONF_MASK;
2017
2018                 switch (type) {
2019                 case L2CAP_CONF_MTU:
2020                         mtu = val;
2021                         break;
2022
2023                 case L2CAP_CONF_FLUSH_TO:
2024                         chan->flush_to = val;
2025                         break;
2026
2027                 case L2CAP_CONF_QOS:
2028                         break;
2029
2030                 case L2CAP_CONF_RFC:
2031                         if (olen == sizeof(rfc))
2032                                 memcpy(&rfc, (void *) val, olen);
2033                         break;
2034
2035                 case L2CAP_CONF_FCS:
2036                         if (val == L2CAP_FCS_NONE)
2037                                 set_bit(CONF_NO_FCS_RECV, &chan->conf_state);
2038
2039                         break;
2040
2041                 default:
2042                         if (hint)
2043                                 break;
2044
2045                         result = L2CAP_CONF_UNKNOWN;
2046                         *((u8 *) ptr++) = type;
2047                         break;
2048                 }
2049         }
2050
2051         if (chan->num_conf_rsp || chan->num_conf_req > 1)
2052                 goto done;
2053
2054         switch (chan->mode) {
2055         case L2CAP_MODE_STREAMING:
2056         case L2CAP_MODE_ERTM:
2057                 if (!test_bit(CONF_STATE2_DEVICE, &chan->conf_state)) {
2058                         chan->mode = l2cap_select_mode(rfc.mode,
2059                                         chan->conn->feat_mask);
2060                         break;
2061                 }
2062
2063                 if (chan->mode != rfc.mode)
2064                         return -ECONNREFUSED;
2065
2066                 break;
2067         }
2068
2069 done:
2070         if (chan->mode != rfc.mode) {
2071                 result = L2CAP_CONF_UNACCEPT;
2072                 rfc.mode = chan->mode;
2073
2074                 if (chan->num_conf_rsp == 1)
2075                         return -ECONNREFUSED;
2076
2077                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2078                                         sizeof(rfc), (unsigned long) &rfc);
2079         }
2080
2081
2082         if (result == L2CAP_CONF_SUCCESS) {
2083                 /* Configure output options and let the other side know
2084                  * which ones we don't like. */
2085
2086                 if (mtu < L2CAP_DEFAULT_MIN_MTU)
2087                         result = L2CAP_CONF_UNACCEPT;
2088                 else {
2089                         chan->omtu = mtu;
2090                         set_bit(CONF_MTU_DONE, &chan->conf_state);
2091                 }
2092                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->omtu);
2093
2094                 switch (rfc.mode) {
2095                 case L2CAP_MODE_BASIC:
2096                         chan->fcs = L2CAP_FCS_NONE;
2097                         set_bit(CONF_MODE_DONE, &chan->conf_state);
2098                         break;
2099
2100                 case L2CAP_MODE_ERTM:
2101                         chan->remote_tx_win = rfc.txwin_size;
2102                         chan->remote_max_tx = rfc.max_transmit;
2103
2104                         if (le16_to_cpu(rfc.max_pdu_size) > chan->conn->mtu - 10)
2105                                 rfc.max_pdu_size = cpu_to_le16(chan->conn->mtu - 10);
2106
2107                         chan->remote_mps = le16_to_cpu(rfc.max_pdu_size);
2108
2109                         rfc.retrans_timeout =
2110                                 le16_to_cpu(L2CAP_DEFAULT_RETRANS_TO);
2111                         rfc.monitor_timeout =
2112                                 le16_to_cpu(L2CAP_DEFAULT_MONITOR_TO);
2113
2114                         set_bit(CONF_MODE_DONE, &chan->conf_state);
2115
2116                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2117                                         sizeof(rfc), (unsigned long) &rfc);
2118
2119                         break;
2120
2121                 case L2CAP_MODE_STREAMING:
2122                         if (le16_to_cpu(rfc.max_pdu_size) > chan->conn->mtu - 10)
2123                                 rfc.max_pdu_size = cpu_to_le16(chan->conn->mtu - 10);
2124
2125                         chan->remote_mps = le16_to_cpu(rfc.max_pdu_size);
2126
2127                         set_bit(CONF_MODE_DONE, &chan->conf_state);
2128
2129                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2130                                         sizeof(rfc), (unsigned long) &rfc);
2131
2132                         break;
2133
2134                 default:
2135                         result = L2CAP_CONF_UNACCEPT;
2136
2137                         memset(&rfc, 0, sizeof(rfc));
2138                         rfc.mode = chan->mode;
2139                 }
2140
2141                 if (result == L2CAP_CONF_SUCCESS)
2142                         set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
2143         }
2144         rsp->scid   = cpu_to_le16(chan->dcid);
2145         rsp->result = cpu_to_le16(result);
2146         rsp->flags  = cpu_to_le16(0x0000);
2147
2148         return ptr - data;
2149 }
2150
2151 static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, void *data, u16 *result)
2152 {
2153         struct l2cap_conf_req *req = data;
2154         void *ptr = req->data;
2155         int type, olen;
2156         unsigned long val;
2157         struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
2158
2159         BT_DBG("chan %p, rsp %p, len %d, req %p", chan, rsp, len, data);
2160
2161         while (len >= L2CAP_CONF_OPT_SIZE) {
2162                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
2163
2164                 switch (type) {
2165                 case L2CAP_CONF_MTU:
2166                         if (val < L2CAP_DEFAULT_MIN_MTU) {
2167                                 *result = L2CAP_CONF_UNACCEPT;
2168                                 chan->imtu = L2CAP_DEFAULT_MIN_MTU;
2169                         } else
2170                                 chan->imtu = val;
2171                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu);
2172                         break;
2173
2174                 case L2CAP_CONF_FLUSH_TO:
2175                         chan->flush_to = val;
2176                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
2177                                                         2, chan->flush_to);
2178                         break;
2179
2180                 case L2CAP_CONF_RFC:
2181                         if (olen == sizeof(rfc))
2182                                 memcpy(&rfc, (void *)val, olen);
2183
2184                         if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state) &&
2185                                                         rfc.mode != chan->mode)
2186                                 return -ECONNREFUSED;
2187
2188                         chan->fcs = 0;
2189
2190                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2191                                         sizeof(rfc), (unsigned long) &rfc);
2192                         break;
2193                 }
2194         }
2195
2196         if (chan->mode == L2CAP_MODE_BASIC && chan->mode != rfc.mode)
2197                 return -ECONNREFUSED;
2198
2199         chan->mode = rfc.mode;
2200
2201         if (*result == L2CAP_CONF_SUCCESS) {
2202                 switch (rfc.mode) {
2203                 case L2CAP_MODE_ERTM:
2204                         chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
2205                         chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
2206                         chan->mps    = le16_to_cpu(rfc.max_pdu_size);
2207                         break;
2208                 case L2CAP_MODE_STREAMING:
2209                         chan->mps    = le16_to_cpu(rfc.max_pdu_size);
2210                 }
2211         }
2212
2213         req->dcid   = cpu_to_le16(chan->dcid);
2214         req->flags  = cpu_to_le16(0x0000);
2215
2216         return ptr - data;
2217 }
2218
2219 static int l2cap_build_conf_rsp(struct l2cap_chan *chan, void *data, u16 result, u16 flags)
2220 {
2221         struct l2cap_conf_rsp *rsp = data;
2222         void *ptr = rsp->data;
2223
2224         BT_DBG("chan %p", chan);
2225
2226         rsp->scid   = cpu_to_le16(chan->dcid);
2227         rsp->result = cpu_to_le16(result);
2228         rsp->flags  = cpu_to_le16(flags);
2229
2230         return ptr - data;
2231 }
2232
2233 void __l2cap_connect_rsp_defer(struct l2cap_chan *chan)
2234 {
2235         struct l2cap_conn_rsp rsp;
2236         struct l2cap_conn *conn = chan->conn;
2237         u8 buf[128];
2238
2239         rsp.scid   = cpu_to_le16(chan->dcid);
2240         rsp.dcid   = cpu_to_le16(chan->scid);
2241         rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
2242         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
2243         l2cap_send_cmd(conn, chan->ident,
2244                                 L2CAP_CONN_RSP, sizeof(rsp), &rsp);
2245
2246         if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
2247                 return;
2248
2249         l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2250                         l2cap_build_conf_req(chan, buf), buf);
2251         chan->num_conf_req++;
2252 }
2253
2254 static void l2cap_conf_rfc_get(struct l2cap_chan *chan, void *rsp, int len)
2255 {
2256         int type, olen;
2257         unsigned long val;
2258         struct l2cap_conf_rfc rfc;
2259
2260         BT_DBG("chan %p, rsp %p, len %d", chan, rsp, len);
2261
2262         if ((chan->mode != L2CAP_MODE_ERTM) && (chan->mode != L2CAP_MODE_STREAMING))
2263                 return;
2264
2265         while (len >= L2CAP_CONF_OPT_SIZE) {
2266                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
2267
2268                 if (type != L2CAP_CONF_RFC)
2269                         continue;
2270
2271                 if (olen != sizeof(rfc))
2272                         break;
2273
2274                 memcpy(&rfc, (void *)val, olen);
2275                 goto done;
2276         }
2277
2278         /* Use sane default values in case a misbehaving remote device
2279          * did not send an RFC option.
2280          */
2281         rfc.mode = chan->mode;
2282         rfc.retrans_timeout = cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO);
2283         rfc.monitor_timeout = cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO);
2284         rfc.max_pdu_size = cpu_to_le16(chan->imtu);
2285
2286         BT_ERR("Expected RFC option was not found, using defaults");
2287
2288 done:
2289         switch (rfc.mode) {
2290         case L2CAP_MODE_ERTM:
2291                 chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
2292                 chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
2293                 chan->mps    = le16_to_cpu(rfc.max_pdu_size);
2294                 break;
2295         case L2CAP_MODE_STREAMING:
2296                 chan->mps    = le16_to_cpu(rfc.max_pdu_size);
2297         }
2298 }
2299
2300 static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2301 {
2302         struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data;
2303
2304         if (rej->reason != L2CAP_REJ_NOT_UNDERSTOOD)
2305                 return 0;
2306
2307         if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
2308                                         cmd->ident == conn->info_ident) {
2309                 del_timer(&conn->info_timer);
2310
2311                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2312                 conn->info_ident = 0;
2313
2314                 l2cap_conn_start(conn);
2315         }
2316
2317         return 0;
2318 }
2319
2320 static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2321 {
2322         struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
2323         struct l2cap_conn_rsp rsp;
2324         struct l2cap_chan *chan = NULL, *pchan;
2325         struct sock *parent, *sk = NULL;
2326         int result, status = L2CAP_CS_NO_INFO;
2327
2328         u16 dcid = 0, scid = __le16_to_cpu(req->scid);
2329         __le16 psm = req->psm;
2330
2331         BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid);
2332
2333         /* Check if we have socket listening on psm */
2334         pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, conn->src);
2335         if (!pchan) {
2336                 result = L2CAP_CR_BAD_PSM;
2337                 goto sendresp;
2338         }
2339
2340         parent = pchan->sk;
2341
2342         bh_lock_sock(parent);
2343
2344         /* Check if the ACL is secure enough (if not SDP) */
2345         if (psm != cpu_to_le16(0x0001) &&
2346                                 !hci_conn_check_link_mode(conn->hcon)) {
2347                 conn->disc_reason = 0x05;
2348                 result = L2CAP_CR_SEC_BLOCK;
2349                 goto response;
2350         }
2351
2352         result = L2CAP_CR_NO_MEM;
2353
2354         /* Check for backlog size */
2355         if (sk_acceptq_is_full(parent)) {
2356                 BT_DBG("backlog full %d", parent->sk_ack_backlog);
2357                 goto response;
2358         }
2359
2360         chan = pchan->ops->new_connection(pchan->data);
2361         if (!chan)
2362                 goto response;
2363
2364         sk = chan->sk;
2365
2366         write_lock_bh(&conn->chan_lock);
2367
2368         /* Check if we already have channel with that dcid */
2369         if (__l2cap_get_chan_by_dcid(conn, scid)) {
2370                 write_unlock_bh(&conn->chan_lock);
2371                 sock_set_flag(sk, SOCK_ZAPPED);
2372                 chan->ops->close(chan->data);
2373                 goto response;
2374         }
2375
2376         hci_conn_hold(conn->hcon);
2377
2378         bacpy(&bt_sk(sk)->src, conn->src);
2379         bacpy(&bt_sk(sk)->dst, conn->dst);
2380         chan->psm  = psm;
2381         chan->dcid = scid;
2382
2383         bt_accept_enqueue(parent, sk);
2384
2385         __l2cap_chan_add(conn, chan);
2386
2387         dcid = chan->scid;
2388
2389         __set_chan_timer(chan, sk->sk_sndtimeo);
2390
2391         chan->ident = cmd->ident;
2392
2393         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
2394                 if (l2cap_check_security(chan)) {
2395                         if (bt_sk(sk)->defer_setup) {
2396                                 l2cap_state_change(chan, BT_CONNECT2);
2397                                 result = L2CAP_CR_PEND;
2398                                 status = L2CAP_CS_AUTHOR_PEND;
2399                                 parent->sk_data_ready(parent, 0);
2400                         } else {
2401                                 l2cap_state_change(chan, BT_CONFIG);
2402                                 result = L2CAP_CR_SUCCESS;
2403                                 status = L2CAP_CS_NO_INFO;
2404                         }
2405                 } else {
2406                         l2cap_state_change(chan, BT_CONNECT2);
2407                         result = L2CAP_CR_PEND;
2408                         status = L2CAP_CS_AUTHEN_PEND;
2409                 }
2410         } else {
2411                 l2cap_state_change(chan, BT_CONNECT2);
2412                 result = L2CAP_CR_PEND;
2413                 status = L2CAP_CS_NO_INFO;
2414         }
2415
2416         write_unlock_bh(&conn->chan_lock);
2417
2418 response:
2419         bh_unlock_sock(parent);
2420
2421 sendresp:
2422         rsp.scid   = cpu_to_le16(scid);
2423         rsp.dcid   = cpu_to_le16(dcid);
2424         rsp.result = cpu_to_le16(result);
2425         rsp.status = cpu_to_le16(status);
2426         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
2427
2428         if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
2429                 struct l2cap_info_req info;
2430                 info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
2431
2432                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
2433                 conn->info_ident = l2cap_get_ident(conn);
2434
2435                 mod_timer(&conn->info_timer, jiffies +
2436                                         msecs_to_jiffies(L2CAP_INFO_TIMEOUT));
2437
2438                 l2cap_send_cmd(conn, conn->info_ident,
2439                                         L2CAP_INFO_REQ, sizeof(info), &info);
2440         }
2441
2442         if (chan && !test_bit(CONF_REQ_SENT, &chan->conf_state) &&
2443                                 result == L2CAP_CR_SUCCESS) {
2444                 u8 buf[128];
2445                 set_bit(CONF_REQ_SENT, &chan->conf_state);
2446                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2447                                         l2cap_build_conf_req(chan, buf), buf);
2448                 chan->num_conf_req++;
2449         }
2450
2451         return 0;
2452 }
2453
2454 static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2455 {
2456         struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
2457         u16 scid, dcid, result, status;
2458         struct l2cap_chan *chan;
2459         struct sock *sk;
2460         u8 req[128];
2461
2462         scid   = __le16_to_cpu(rsp->scid);
2463         dcid   = __le16_to_cpu(rsp->dcid);
2464         result = __le16_to_cpu(rsp->result);
2465         status = __le16_to_cpu(rsp->status);
2466
2467         BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status);
2468
2469         if (scid) {
2470                 chan = l2cap_get_chan_by_scid(conn, scid);
2471                 if (!chan)
2472                         return -EFAULT;
2473         } else {
2474                 chan = l2cap_get_chan_by_ident(conn, cmd->ident);
2475                 if (!chan)
2476                         return -EFAULT;
2477         }
2478
2479         sk = chan->sk;
2480
2481         switch (result) {
2482         case L2CAP_CR_SUCCESS:
2483                 l2cap_state_change(chan, BT_CONFIG);
2484                 chan->ident = 0;
2485                 chan->dcid = dcid;
2486                 clear_bit(CONF_CONNECT_PEND, &chan->conf_state);
2487
2488                 if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
2489                         break;
2490
2491                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2492                                         l2cap_build_conf_req(chan, req), req);
2493                 chan->num_conf_req++;
2494                 break;
2495
2496         case L2CAP_CR_PEND:
2497                 set_bit(CONF_CONNECT_PEND, &chan->conf_state);
2498                 break;
2499
2500         default:
2501                 /* don't delete l2cap channel if sk is owned by user */
2502                 if (sock_owned_by_user(sk)) {
2503                         l2cap_state_change(chan, BT_DISCONN);
2504                         __clear_chan_timer(chan);
2505                         __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
2506                         break;
2507                 }
2508
2509                 l2cap_chan_del(chan, ECONNREFUSED);
2510                 break;
2511         }
2512
2513         bh_unlock_sock(sk);
2514         return 0;
2515 }
2516
2517 static inline void set_default_fcs(struct l2cap_chan *chan)
2518 {
2519         /* FCS is enabled only in ERTM or streaming mode, if one or both
2520          * sides request it.
2521          */
2522         if (chan->mode != L2CAP_MODE_ERTM && chan->mode != L2CAP_MODE_STREAMING)
2523                 chan->fcs = L2CAP_FCS_NONE;
2524         else if (!test_bit(CONF_NO_FCS_RECV, &chan->conf_state))
2525                 chan->fcs = L2CAP_FCS_CRC16;
2526 }
2527
2528 static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
2529 {
2530         struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
2531         u16 dcid, flags;
2532         u8 rsp[64];
2533         struct l2cap_chan *chan;
2534         struct sock *sk;
2535         int len;
2536
2537         dcid  = __le16_to_cpu(req->dcid);
2538         flags = __le16_to_cpu(req->flags);
2539
2540         BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
2541
2542         chan = l2cap_get_chan_by_scid(conn, dcid);
2543         if (!chan)
2544                 return -ENOENT;
2545
2546         sk = chan->sk;
2547
2548         if (chan->state != BT_CONFIG && chan->state != BT_CONNECT2) {
2549                 struct l2cap_cmd_rej_cid rej;
2550
2551                 rej.reason = cpu_to_le16(L2CAP_REJ_INVALID_CID);
2552                 rej.scid = cpu_to_le16(chan->scid);
2553                 rej.dcid = cpu_to_le16(chan->dcid);
2554
2555                 l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
2556                                 sizeof(rej), &rej);
2557                 goto unlock;
2558         }
2559
2560         /* Reject if config buffer is too small. */
2561         len = cmd_len - sizeof(*req);
2562         if (len < 0 || chan->conf_len + len > sizeof(chan->conf_req)) {
2563                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2564                                 l2cap_build_conf_rsp(chan, rsp,
2565                                         L2CAP_CONF_REJECT, flags), rsp);
2566                 goto unlock;
2567         }
2568
2569         /* Store config. */
2570         memcpy(chan->conf_req + chan->conf_len, req->data, len);
2571         chan->conf_len += len;
2572
2573         if (flags & 0x0001) {
2574                 /* Incomplete config. Send empty response. */
2575                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2576                                 l2cap_build_conf_rsp(chan, rsp,
2577                                         L2CAP_CONF_SUCCESS, 0x0001), rsp);
2578                 goto unlock;
2579         }
2580
2581         /* Complete config. */
2582         len = l2cap_parse_conf_req(chan, rsp);
2583         if (len < 0) {
2584                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
2585                 goto unlock;
2586         }
2587
2588         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
2589         chan->num_conf_rsp++;
2590
2591         /* Reset config buffer. */
2592         chan->conf_len = 0;
2593
2594         if (!test_bit(CONF_OUTPUT_DONE, &chan->conf_state))
2595                 goto unlock;
2596
2597         if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
2598                 set_default_fcs(chan);
2599
2600                 l2cap_state_change(chan, BT_CONNECTED);
2601
2602                 chan->next_tx_seq = 0;
2603                 chan->expected_tx_seq = 0;
2604                 skb_queue_head_init(&chan->tx_q);
2605                 if (chan->mode == L2CAP_MODE_ERTM)
2606                         l2cap_ertm_init(chan);
2607
2608                 l2cap_chan_ready(sk);
2609                 goto unlock;
2610         }
2611
2612         if (!test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) {
2613                 u8 buf[64];
2614                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2615                                         l2cap_build_conf_req(chan, buf), buf);
2616                 chan->num_conf_req++;
2617         }
2618
2619 unlock:
2620         bh_unlock_sock(sk);
2621         return 0;
2622 }
2623
2624 static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2625 {
2626         struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
2627         u16 scid, flags, result;
2628         struct l2cap_chan *chan;
2629         struct sock *sk;
2630         int len = cmd->len - sizeof(*rsp);
2631
2632         scid   = __le16_to_cpu(rsp->scid);
2633         flags  = __le16_to_cpu(rsp->flags);
2634         result = __le16_to_cpu(rsp->result);
2635
2636         BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x",
2637                         scid, flags, result);
2638
2639         chan = l2cap_get_chan_by_scid(conn, scid);
2640         if (!chan)
2641                 return 0;
2642
2643         sk = chan->sk;
2644
2645         switch (result) {
2646         case L2CAP_CONF_SUCCESS:
2647                 l2cap_conf_rfc_get(chan, rsp->data, len);
2648                 break;
2649
2650         case L2CAP_CONF_UNACCEPT:
2651                 if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
2652                         char req[64];
2653
2654                         if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
2655                                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
2656                                 goto done;
2657                         }
2658
2659                         /* throw out any old stored conf requests */
2660                         result = L2CAP_CONF_SUCCESS;
2661                         len = l2cap_parse_conf_rsp(chan, rsp->data, len,
2662                                                                 req, &result);
2663                         if (len < 0) {
2664                                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
2665                                 goto done;
2666                         }
2667
2668                         l2cap_send_cmd(conn, l2cap_get_ident(conn),
2669                                                 L2CAP_CONF_REQ, len, req);
2670                         chan->num_conf_req++;
2671                         if (result != L2CAP_CONF_SUCCESS)
2672                                 goto done;
2673                         break;
2674                 }
2675
2676         default:
2677                 sk->sk_err = ECONNRESET;
2678                 __set_chan_timer(chan, L2CAP_DISC_REJ_TIMEOUT);
2679                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
2680                 goto done;
2681         }
2682
2683         if (flags & 0x01)
2684                 goto done;
2685
2686         set_bit(CONF_INPUT_DONE, &chan->conf_state);
2687
2688         if (test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) {
2689                 set_default_fcs(chan);
2690
2691                 l2cap_state_change(chan, BT_CONNECTED);
2692                 chan->next_tx_seq = 0;
2693                 chan->expected_tx_seq = 0;
2694                 skb_queue_head_init(&chan->tx_q);
2695                 if (chan->mode ==  L2CAP_MODE_ERTM)
2696                         l2cap_ertm_init(chan);
2697
2698                 l2cap_chan_ready(sk);
2699         }
2700
2701 done:
2702         bh_unlock_sock(sk);
2703         return 0;
2704 }
2705
2706 static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2707 {
2708         struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
2709         struct l2cap_disconn_rsp rsp;
2710         u16 dcid, scid;
2711         struct l2cap_chan *chan;
2712         struct sock *sk;
2713
2714         scid = __le16_to_cpu(req->scid);
2715         dcid = __le16_to_cpu(req->dcid);
2716
2717         BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
2718
2719         chan = l2cap_get_chan_by_scid(conn, dcid);
2720         if (!chan)
2721                 return 0;
2722
2723         sk = chan->sk;
2724
2725         rsp.dcid = cpu_to_le16(chan->scid);
2726         rsp.scid = cpu_to_le16(chan->dcid);
2727         l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
2728
2729         sk->sk_shutdown = SHUTDOWN_MASK;
2730
2731         /* don't delete l2cap channel if sk is owned by user */
2732         if (sock_owned_by_user(sk)) {
2733                 l2cap_state_change(chan, BT_DISCONN);
2734                 __clear_chan_timer(chan);
2735                 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
2736                 bh_unlock_sock(sk);
2737                 return 0;
2738         }
2739
2740         l2cap_chan_del(chan, ECONNRESET);
2741         bh_unlock_sock(sk);
2742
2743         chan->ops->close(chan->data);
2744         return 0;
2745 }
2746
2747 static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2748 {
2749         struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
2750         u16 dcid, scid;
2751         struct l2cap_chan *chan;
2752         struct sock *sk;
2753
2754         scid = __le16_to_cpu(rsp->scid);
2755         dcid = __le16_to_cpu(rsp->dcid);
2756
2757         BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
2758
2759         chan = l2cap_get_chan_by_scid(conn, scid);
2760         if (!chan)
2761                 return 0;
2762
2763         sk = chan->sk;
2764
2765         /* don't delete l2cap channel if sk is owned by user */
2766         if (sock_owned_by_user(sk)) {
2767                 l2cap_state_change(chan,BT_DISCONN);
2768                 __clear_chan_timer(chan);
2769                 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
2770                 bh_unlock_sock(sk);
2771                 return 0;
2772         }
2773
2774         l2cap_chan_del(chan, 0);
2775         bh_unlock_sock(sk);
2776
2777         chan->ops->close(chan->data);
2778         return 0;
2779 }
2780
2781 static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2782 {
2783         struct l2cap_info_req *req = (struct l2cap_info_req *) data;
2784         u16 type;
2785
2786         type = __le16_to_cpu(req->type);
2787
2788         BT_DBG("type 0x%4.4x", type);
2789
2790         if (type == L2CAP_IT_FEAT_MASK) {
2791                 u8 buf[8];
2792                 u32 feat_mask = l2cap_feat_mask;
2793                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
2794                 rsp->type   = cpu_to_le16(L2CAP_IT_FEAT_MASK);
2795                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
2796                 if (!disable_ertm)
2797                         feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
2798                                                          | L2CAP_FEAT_FCS;
2799                 put_unaligned_le32(feat_mask, rsp->data);
2800                 l2cap_send_cmd(conn, cmd->ident,
2801                                         L2CAP_INFO_RSP, sizeof(buf), buf);
2802         } else if (type == L2CAP_IT_FIXED_CHAN) {
2803                 u8 buf[12];
2804                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
2805                 rsp->type   = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
2806                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
2807                 memcpy(buf + 4, l2cap_fixed_chan, 8);
2808                 l2cap_send_cmd(conn, cmd->ident,
2809                                         L2CAP_INFO_RSP, sizeof(buf), buf);
2810         } else {
2811                 struct l2cap_info_rsp rsp;
2812                 rsp.type   = cpu_to_le16(type);
2813                 rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP);
2814                 l2cap_send_cmd(conn, cmd->ident,
2815                                         L2CAP_INFO_RSP, sizeof(rsp), &rsp);
2816         }
2817
2818         return 0;
2819 }
2820
2821 static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2822 {
2823         struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
2824         u16 type, result;
2825
2826         type   = __le16_to_cpu(rsp->type);
2827         result = __le16_to_cpu(rsp->result);
2828
2829         BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
2830
2831         /* L2CAP Info req/rsp are unbound to channels, add extra checks */
2832         if (cmd->ident != conn->info_ident ||
2833                         conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)
2834                 return 0;
2835
2836         del_timer(&conn->info_timer);
2837
2838         if (result != L2CAP_IR_SUCCESS) {
2839                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2840                 conn->info_ident = 0;
2841
2842                 l2cap_conn_start(conn);
2843
2844                 return 0;
2845         }
2846
2847         if (type == L2CAP_IT_FEAT_MASK) {
2848                 conn->feat_mask = get_unaligned_le32(rsp->data);
2849
2850                 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
2851                         struct l2cap_info_req req;
2852                         req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
2853
2854                         conn->info_ident = l2cap_get_ident(conn);
2855
2856                         l2cap_send_cmd(conn, conn->info_ident,
2857                                         L2CAP_INFO_REQ, sizeof(req), &req);
2858                 } else {
2859                         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2860                         conn->info_ident = 0;
2861
2862                         l2cap_conn_start(conn);
2863                 }
2864         } else if (type == L2CAP_IT_FIXED_CHAN) {
2865                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2866                 conn->info_ident = 0;
2867
2868                 l2cap_conn_start(conn);
2869         }
2870
2871         return 0;
2872 }
2873
2874 static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency,
2875                                                         u16 to_multiplier)
2876 {
2877         u16 max_latency;
2878
2879         if (min > max || min < 6 || max > 3200)
2880                 return -EINVAL;
2881
2882         if (to_multiplier < 10 || to_multiplier > 3200)
2883                 return -EINVAL;
2884
2885         if (max >= to_multiplier * 8)
2886                 return -EINVAL;
2887
2888         max_latency = (to_multiplier * 8 / max) - 1;
2889         if (latency > 499 || latency > max_latency)
2890                 return -EINVAL;
2891
2892         return 0;
2893 }
2894
2895 static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
2896                                         struct l2cap_cmd_hdr *cmd, u8 *data)
2897 {
2898         struct hci_conn *hcon = conn->hcon;
2899         struct l2cap_conn_param_update_req *req;
2900         struct l2cap_conn_param_update_rsp rsp;
2901         u16 min, max, latency, to_multiplier, cmd_len;
2902         int err;
2903
2904         if (!(hcon->link_mode & HCI_LM_MASTER))
2905                 return -EINVAL;
2906
2907         cmd_len = __le16_to_cpu(cmd->len);
2908         if (cmd_len != sizeof(struct l2cap_conn_param_update_req))
2909                 return -EPROTO;
2910
2911         req = (struct l2cap_conn_param_update_req *) data;
2912         min             = __le16_to_cpu(req->min);
2913         max             = __le16_to_cpu(req->max);
2914         latency         = __le16_to_cpu(req->latency);
2915         to_multiplier   = __le16_to_cpu(req->to_multiplier);
2916
2917         BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x",
2918                                                 min, max, latency, to_multiplier);
2919
2920         memset(&rsp, 0, sizeof(rsp));
2921
2922         err = l2cap_check_conn_param(min, max, latency, to_multiplier);
2923         if (err)
2924                 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_REJECTED);
2925         else
2926                 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED);
2927
2928         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP,
2929                                                         sizeof(rsp), &rsp);
2930
2931         if (!err)
2932                 hci_le_conn_update(hcon, min, max, latency, to_multiplier);
2933
2934         return 0;
2935 }
2936
2937 static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
2938                         struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
2939 {
2940         int err = 0;
2941
2942         switch (cmd->code) {
2943         case L2CAP_COMMAND_REJ:
2944                 l2cap_command_rej(conn, cmd, data);
2945                 break;
2946
2947         case L2CAP_CONN_REQ:
2948                 err = l2cap_connect_req(conn, cmd, data);
2949                 break;
2950
2951         case L2CAP_CONN_RSP:
2952                 err = l2cap_connect_rsp(conn, cmd, data);
2953                 break;
2954
2955         case L2CAP_CONF_REQ:
2956                 err = l2cap_config_req(conn, cmd, cmd_len, data);
2957                 break;
2958
2959         case L2CAP_CONF_RSP:
2960                 err = l2cap_config_rsp(conn, cmd, data);
2961                 break;
2962
2963         case L2CAP_DISCONN_REQ:
2964                 err = l2cap_disconnect_req(conn, cmd, data);
2965                 break;
2966
2967         case L2CAP_DISCONN_RSP:
2968                 err = l2cap_disconnect_rsp(conn, cmd, data);
2969                 break;
2970
2971         case L2CAP_ECHO_REQ:
2972                 l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data);
2973                 break;
2974
2975         case L2CAP_ECHO_RSP:
2976                 break;
2977
2978         case L2CAP_INFO_REQ:
2979                 err = l2cap_information_req(conn, cmd, data);
2980                 break;
2981
2982         case L2CAP_INFO_RSP:
2983                 err = l2cap_information_rsp(conn, cmd, data);
2984                 break;
2985
2986         default:
2987                 BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code);
2988                 err = -EINVAL;
2989                 break;
2990         }
2991
2992         return err;
2993 }
2994
2995 static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
2996                                         struct l2cap_cmd_hdr *cmd, u8 *data)
2997 {
2998         switch (cmd->code) {
2999         case L2CAP_COMMAND_REJ:
3000                 return 0;
3001
3002         case L2CAP_CONN_PARAM_UPDATE_REQ:
3003                 return l2cap_conn_param_update_req(conn, cmd, data);
3004
3005         case L2CAP_CONN_PARAM_UPDATE_RSP:
3006                 return 0;
3007
3008         default:
3009                 BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code);
3010                 return -EINVAL;
3011         }
3012 }
3013
3014 static inline void l2cap_sig_channel(struct l2cap_conn *conn,
3015                                                         struct sk_buff *skb)
3016 {
3017         u8 *data = skb->data;
3018         int len = skb->len;
3019         struct l2cap_cmd_hdr cmd;
3020         int err;
3021
3022         l2cap_raw_recv(conn, skb);
3023
3024         while (len >= L2CAP_CMD_HDR_SIZE) {
3025                 u16 cmd_len;
3026                 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
3027                 data += L2CAP_CMD_HDR_SIZE;
3028                 len  -= L2CAP_CMD_HDR_SIZE;
3029
3030                 cmd_len = le16_to_cpu(cmd.len);
3031
3032                 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, cmd.ident);
3033
3034                 if (cmd_len > len || !cmd.ident) {
3035                         BT_DBG("corrupted command");
3036                         break;
3037                 }
3038
3039                 if (conn->hcon->type == LE_LINK)
3040                         err = l2cap_le_sig_cmd(conn, &cmd, data);
3041                 else
3042                         err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data);
3043
3044                 if (err) {
3045                         struct l2cap_cmd_rej_unk rej;
3046
3047                         BT_ERR("Wrong link type (%d)", err);
3048
3049                         /* FIXME: Map err to a valid reason */
3050                         rej.reason = cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
3051                         l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
3052                 }
3053
3054                 data += cmd_len;
3055                 len  -= cmd_len;
3056         }
3057
3058         kfree_skb(skb);
3059 }
3060
3061 static int l2cap_check_fcs(struct l2cap_chan *chan,  struct sk_buff *skb)
3062 {
3063         u16 our_fcs, rcv_fcs;
3064         int hdr_size = L2CAP_HDR_SIZE + 2;
3065
3066         if (chan->fcs == L2CAP_FCS_CRC16) {
3067                 skb_trim(skb, skb->len - 2);
3068                 rcv_fcs = get_unaligned_le16(skb->data + skb->len);
3069                 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
3070
3071                 if (our_fcs != rcv_fcs)
3072                         return -EBADMSG;
3073         }
3074         return 0;
3075 }
3076
3077 static inline void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan)
3078 {
3079         u16 control = 0;
3080
3081         chan->frames_sent = 0;
3082
3083         control |= chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3084
3085         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
3086                 control |= L2CAP_SUPER_RCV_NOT_READY;
3087                 l2cap_send_sframe(chan, control);
3088                 set_bit(CONN_RNR_SENT, &chan->conn_state);
3089         }
3090
3091         if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
3092                 l2cap_retransmit_frames(chan);
3093
3094         l2cap_ertm_send(chan);
3095
3096         if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
3097                         chan->frames_sent == 0) {
3098                 control |= L2CAP_SUPER_RCV_READY;
3099                 l2cap_send_sframe(chan, control);
3100         }
3101 }
3102
3103 static int l2cap_add_to_srej_queue(struct l2cap_chan *chan, struct sk_buff *skb, u8 tx_seq, u8 sar)
3104 {
3105         struct sk_buff *next_skb;
3106         int tx_seq_offset, next_tx_seq_offset;
3107
3108         bt_cb(skb)->tx_seq = tx_seq;
3109         bt_cb(skb)->sar = sar;
3110
3111         next_skb = skb_peek(&chan->srej_q);
3112         if (!next_skb) {
3113                 __skb_queue_tail(&chan->srej_q, skb);
3114                 return 0;
3115         }
3116
3117         tx_seq_offset = (tx_seq - chan->buffer_seq) % 64;
3118         if (tx_seq_offset < 0)
3119                 tx_seq_offset += 64;
3120
3121         do {
3122                 if (bt_cb(next_skb)->tx_seq == tx_seq)
3123                         return -EINVAL;
3124
3125                 next_tx_seq_offset = (bt_cb(next_skb)->tx_seq -
3126                                                 chan->buffer_seq) % 64;
3127                 if (next_tx_seq_offset < 0)
3128                         next_tx_seq_offset += 64;
3129
3130                 if (next_tx_seq_offset > tx_seq_offset) {
3131                         __skb_queue_before(&chan->srej_q, next_skb, skb);
3132                         return 0;
3133                 }
3134
3135                 if (skb_queue_is_last(&chan->srej_q, next_skb))
3136                         break;
3137
3138         } while ((next_skb = skb_queue_next(&chan->srej_q, next_skb)));
3139
3140         __skb_queue_tail(&chan->srej_q, skb);
3141
3142         return 0;
3143 }
3144
3145 static void append_skb_frag(struct sk_buff *skb,
3146                         struct sk_buff *new_frag, struct sk_buff **last_frag)
3147 {
3148         /* skb->len reflects data in skb as well as all fragments
3149          * skb->data_len reflects only data in fragments
3150          */
3151         if (!skb_has_frag_list(skb))
3152                 skb_shinfo(skb)->frag_list = new_frag;
3153
3154         new_frag->next = NULL;
3155
3156         (*last_frag)->next = new_frag;
3157         *last_frag = new_frag;
3158
3159         skb->len += new_frag->len;
3160         skb->data_len += new_frag->len;
3161         skb->truesize += new_frag->truesize;
3162 }
3163
3164 static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u16 control)
3165 {
3166         int err = -EINVAL;
3167
3168         switch (control & L2CAP_CTRL_SAR) {
3169         case L2CAP_SDU_UNSEGMENTED:
3170                 if (chan->sdu)
3171                         break;
3172
3173                 err = chan->ops->recv(chan->data, skb);
3174                 break;
3175
3176         case L2CAP_SDU_START:
3177                 if (chan->sdu)
3178                         break;
3179
3180                 chan->sdu_len = get_unaligned_le16(skb->data);
3181                 skb_pull(skb, 2);
3182
3183                 if (chan->sdu_len > chan->imtu) {
3184                         err = -EMSGSIZE;
3185                         break;
3186                 }
3187
3188                 if (skb->len >= chan->sdu_len)
3189                         break;
3190
3191                 chan->sdu = skb;
3192                 chan->sdu_last_frag = skb;
3193
3194                 skb = NULL;
3195                 err = 0;
3196                 break;
3197
3198         case L2CAP_SDU_CONTINUE:
3199                 if (!chan->sdu)
3200                         break;
3201
3202                 append_skb_frag(chan->sdu, skb,
3203                                 &chan->sdu_last_frag);
3204                 skb = NULL;
3205
3206                 if (chan->sdu->len >= chan->sdu_len)
3207                         break;
3208
3209                 err = 0;
3210                 break;
3211
3212         case L2CAP_SDU_END:
3213                 if (!chan->sdu)
3214                         break;
3215
3216                 append_skb_frag(chan->sdu, skb,
3217                                 &chan->sdu_last_frag);
3218                 skb = NULL;
3219
3220                 if (chan->sdu->len != chan->sdu_len)
3221                         break;
3222
3223                 err = chan->ops->recv(chan->data, chan->sdu);
3224
3225                 if (!err) {
3226                         /* Reassembly complete */
3227                         chan->sdu = NULL;
3228                         chan->sdu_last_frag = NULL;
3229                         chan->sdu_len = 0;
3230                 }
3231                 break;
3232         }
3233
3234         if (err) {
3235                 kfree_skb(skb);
3236                 kfree_skb(chan->sdu);
3237                 chan->sdu = NULL;
3238                 chan->sdu_last_frag = NULL;
3239                 chan->sdu_len = 0;
3240         }
3241
3242         return err;
3243 }
3244
3245 static void l2cap_ertm_enter_local_busy(struct l2cap_chan *chan)
3246 {
3247         u16 control;
3248
3249         BT_DBG("chan %p, Enter local busy", chan);
3250
3251         set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
3252
3253         control = chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3254         control |= L2CAP_SUPER_RCV_NOT_READY;
3255         l2cap_send_sframe(chan, control);
3256
3257         set_bit(CONN_RNR_SENT, &chan->conn_state);
3258
3259         __clear_ack_timer(chan);
3260 }
3261
3262 static void l2cap_ertm_exit_local_busy(struct l2cap_chan *chan)
3263 {
3264         u16 control;
3265
3266         if (!test_bit(CONN_RNR_SENT, &chan->conn_state))
3267                 goto done;
3268
3269         control = chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3270         control |= L2CAP_SUPER_RCV_READY | L2CAP_CTRL_POLL;
3271         l2cap_send_sframe(chan, control);
3272         chan->retry_count = 1;
3273
3274         __clear_retrans_timer(chan);
3275         __set_monitor_timer(chan);
3276
3277         set_bit(CONN_WAIT_F, &chan->conn_state);
3278
3279 done:
3280         clear_bit(CONN_LOCAL_BUSY, &chan->conn_state);
3281         clear_bit(CONN_RNR_SENT, &chan->conn_state);
3282
3283         BT_DBG("chan %p, Exit local busy", chan);
3284 }
3285
3286 void l2cap_chan_busy(struct l2cap_chan *chan, int busy)
3287 {
3288         if (chan->mode == L2CAP_MODE_ERTM) {
3289                 if (busy)
3290                         l2cap_ertm_enter_local_busy(chan);
3291                 else
3292                         l2cap_ertm_exit_local_busy(chan);
3293         }
3294 }
3295
3296 static void l2cap_check_srej_gap(struct l2cap_chan *chan, u8 tx_seq)
3297 {
3298         struct sk_buff *skb;
3299         u16 control;
3300
3301         while ((skb = skb_peek(&chan->srej_q)) &&
3302                         !test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
3303                 int err;
3304
3305                 if (bt_cb(skb)->tx_seq != tx_seq)
3306                         break;
3307
3308                 skb = skb_dequeue(&chan->srej_q);
3309                 control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
3310                 err = l2cap_reassemble_sdu(chan, skb, control);
3311
3312                 if (err < 0) {
3313                         l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3314                         break;
3315                 }
3316
3317                 chan->buffer_seq_srej =
3318                         (chan->buffer_seq_srej + 1) % 64;
3319                 tx_seq = (tx_seq + 1) % 64;
3320         }
3321 }
3322
3323 static void l2cap_resend_srejframe(struct l2cap_chan *chan, u8 tx_seq)
3324 {
3325         struct srej_list *l, *tmp;
3326         u16 control;
3327
3328         list_for_each_entry_safe(l, tmp, &chan->srej_l, list) {
3329                 if (l->tx_seq == tx_seq) {
3330                         list_del(&l->list);
3331                         kfree(l);
3332                         return;
3333                 }
3334                 control = L2CAP_SUPER_SELECT_REJECT;
3335                 control |= l->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3336                 l2cap_send_sframe(chan, control);
3337                 list_del(&l->list);
3338                 list_add_tail(&l->list, &chan->srej_l);
3339         }
3340 }
3341
3342 static void l2cap_send_srejframe(struct l2cap_chan *chan, u8 tx_seq)
3343 {
3344         struct srej_list *new;
3345         u16 control;
3346
3347         while (tx_seq != chan->expected_tx_seq) {
3348                 control = L2CAP_SUPER_SELECT_REJECT;
3349                 control |= chan->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3350                 l2cap_send_sframe(chan, control);
3351
3352                 new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC);
3353                 new->tx_seq = chan->expected_tx_seq;
3354                 chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64;
3355                 list_add_tail(&new->list, &chan->srej_l);
3356         }
3357         chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64;
3358 }
3359
3360 static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u16 rx_control, struct sk_buff *skb)
3361 {
3362         u8 tx_seq = __get_txseq(rx_control);
3363         u8 req_seq = __get_reqseq(rx_control);
3364         u8 sar = rx_control >> L2CAP_CTRL_SAR_SHIFT;
3365         int tx_seq_offset, expected_tx_seq_offset;
3366         int num_to_ack = (chan->tx_win/6) + 1;
3367         int err = 0;
3368
3369         BT_DBG("chan %p len %d tx_seq %d rx_control 0x%4.4x", chan, skb->len,
3370                                                         tx_seq, rx_control);
3371
3372         if (L2CAP_CTRL_FINAL & rx_control &&
3373                         test_bit(CONN_WAIT_F, &chan->conn_state)) {
3374                 __clear_monitor_timer(chan);
3375                 if (chan->unacked_frames > 0)
3376                         __set_retrans_timer(chan);
3377                 clear_bit(CONN_WAIT_F, &chan->conn_state);
3378         }
3379
3380         chan->expected_ack_seq = req_seq;
3381         l2cap_drop_acked_frames(chan);
3382
3383         tx_seq_offset = (tx_seq - chan->buffer_seq) % 64;
3384         if (tx_seq_offset < 0)
3385                 tx_seq_offset += 64;
3386
3387         /* invalid tx_seq */
3388         if (tx_seq_offset >= chan->tx_win) {
3389                 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3390                 goto drop;
3391         }
3392
3393         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state))
3394                 goto drop;
3395
3396         if (tx_seq == chan->expected_tx_seq)
3397                 goto expected;
3398
3399         if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
3400                 struct srej_list *first;
3401
3402                 first = list_first_entry(&chan->srej_l,
3403                                 struct srej_list, list);
3404                 if (tx_seq == first->tx_seq) {
3405                         l2cap_add_to_srej_queue(chan, skb, tx_seq, sar);
3406                         l2cap_check_srej_gap(chan, tx_seq);
3407
3408                         list_del(&first->list);
3409                         kfree(first);
3410
3411                         if (list_empty(&chan->srej_l)) {
3412                                 chan->buffer_seq = chan->buffer_seq_srej;
3413                                 clear_bit(CONN_SREJ_SENT, &chan->conn_state);
3414                                 l2cap_send_ack(chan);
3415                                 BT_DBG("chan %p, Exit SREJ_SENT", chan);
3416                         }
3417                 } else {
3418                         struct srej_list *l;
3419
3420                         /* duplicated tx_seq */
3421                         if (l2cap_add_to_srej_queue(chan, skb, tx_seq, sar) < 0)
3422                                 goto drop;
3423
3424                         list_for_each_entry(l, &chan->srej_l, list) {
3425                                 if (l->tx_seq == tx_seq) {
3426                                         l2cap_resend_srejframe(chan, tx_seq);
3427                                         return 0;
3428                                 }
3429                         }
3430                         l2cap_send_srejframe(chan, tx_seq);
3431                 }
3432         } else {
3433                 expected_tx_seq_offset =
3434                         (chan->expected_tx_seq - chan->buffer_seq) % 64;
3435                 if (expected_tx_seq_offset < 0)
3436                         expected_tx_seq_offset += 64;
3437
3438                 /* duplicated tx_seq */
3439                 if (tx_seq_offset < expected_tx_seq_offset)
3440                         goto drop;
3441
3442                 set_bit(CONN_SREJ_SENT, &chan->conn_state);
3443
3444                 BT_DBG("chan %p, Enter SREJ", chan);
3445
3446                 INIT_LIST_HEAD(&chan->srej_l);
3447                 chan->buffer_seq_srej = chan->buffer_seq;
3448
3449                 __skb_queue_head_init(&chan->srej_q);
3450                 l2cap_add_to_srej_queue(chan, skb, tx_seq, sar);
3451
3452                 set_bit(CONN_SEND_PBIT, &chan->conn_state);
3453
3454                 l2cap_send_srejframe(chan, tx_seq);
3455
3456                 __clear_ack_timer(chan);
3457         }
3458         return 0;
3459
3460 expected:
3461         chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64;
3462
3463         if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
3464                 bt_cb(skb)->tx_seq = tx_seq;
3465                 bt_cb(skb)->sar = sar;
3466                 __skb_queue_tail(&chan->srej_q, skb);
3467                 return 0;
3468         }
3469
3470         err = l2cap_reassemble_sdu(chan, skb, rx_control);
3471         chan->buffer_seq = (chan->buffer_seq + 1) % 64;
3472         if (err < 0) {
3473                 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3474                 return err;
3475         }
3476
3477         if (rx_control & L2CAP_CTRL_FINAL) {
3478                 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
3479                         l2cap_retransmit_frames(chan);
3480         }
3481
3482         __set_ack_timer(chan);
3483
3484         chan->num_acked = (chan->num_acked + 1) % num_to_ack;
3485         if (chan->num_acked == num_to_ack - 1)
3486                 l2cap_send_ack(chan);
3487
3488         return 0;
3489
3490 drop:
3491         kfree_skb(skb);
3492         return 0;
3493 }
3494
3495 static inline void l2cap_data_channel_rrframe(struct l2cap_chan *chan, u16 rx_control)
3496 {
3497         BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, __get_reqseq(rx_control),
3498                                                 rx_control);
3499
3500         chan->expected_ack_seq = __get_reqseq(rx_control);
3501         l2cap_drop_acked_frames(chan);
3502
3503         if (rx_control & L2CAP_CTRL_POLL) {
3504                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
3505                 if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
3506                         if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
3507                                         (chan->unacked_frames > 0))
3508                                 __set_retrans_timer(chan);
3509
3510                         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3511                         l2cap_send_srejtail(chan);
3512                 } else {
3513                         l2cap_send_i_or_rr_or_rnr(chan);
3514                 }
3515
3516         } else if (rx_control & L2CAP_CTRL_FINAL) {
3517                 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3518
3519                 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
3520                         l2cap_retransmit_frames(chan);
3521
3522         } else {
3523                 if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
3524                                 (chan->unacked_frames > 0))
3525                         __set_retrans_timer(chan);
3526
3527                 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3528                 if (test_bit(CONN_SREJ_SENT, &chan->conn_state))
3529                         l2cap_send_ack(chan);
3530                 else
3531                         l2cap_ertm_send(chan);
3532         }
3533 }
3534
3535 static inline void l2cap_data_channel_rejframe(struct l2cap_chan *chan, u16 rx_control)
3536 {
3537         u8 tx_seq = __get_reqseq(rx_control);
3538
3539         BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, tx_seq, rx_control);
3540
3541         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3542
3543         chan->expected_ack_seq = tx_seq;
3544         l2cap_drop_acked_frames(chan);
3545
3546         if (rx_control & L2CAP_CTRL_FINAL) {
3547                 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
3548                         l2cap_retransmit_frames(chan);
3549         } else {
3550                 l2cap_retransmit_frames(chan);
3551
3552                 if (test_bit(CONN_WAIT_F, &chan->conn_state))
3553                         set_bit(CONN_REJ_ACT, &chan->conn_state);
3554         }
3555 }
3556 static inline void l2cap_data_channel_srejframe(struct l2cap_chan *chan, u16 rx_control)
3557 {
3558         u8 tx_seq = __get_reqseq(rx_control);
3559
3560         BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, tx_seq, rx_control);
3561
3562         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3563
3564         if (rx_control & L2CAP_CTRL_POLL) {
3565                 chan->expected_ack_seq = tx_seq;
3566                 l2cap_drop_acked_frames(chan);
3567
3568                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
3569                 l2cap_retransmit_one_frame(chan, tx_seq);
3570
3571                 l2cap_ertm_send(chan);
3572
3573                 if (test_bit(CONN_WAIT_F, &chan->conn_state)) {
3574                         chan->srej_save_reqseq = tx_seq;
3575                         set_bit(CONN_SREJ_ACT, &chan->conn_state);
3576                 }
3577         } else if (rx_control & L2CAP_CTRL_FINAL) {
3578                 if (test_bit(CONN_SREJ_ACT, &chan->conn_state) &&
3579                                 chan->srej_save_reqseq == tx_seq)
3580                         clear_bit(CONN_SREJ_ACT, &chan->conn_state);
3581                 else
3582                         l2cap_retransmit_one_frame(chan, tx_seq);
3583         } else {
3584                 l2cap_retransmit_one_frame(chan, tx_seq);
3585                 if (test_bit(CONN_WAIT_F, &chan->conn_state)) {
3586                         chan->srej_save_reqseq = tx_seq;
3587                         set_bit(CONN_SREJ_ACT, &chan->conn_state);
3588                 }
3589         }
3590 }
3591
3592 static inline void l2cap_data_channel_rnrframe(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         set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3599         chan->expected_ack_seq = tx_seq;
3600         l2cap_drop_acked_frames(chan);
3601
3602         if (rx_control & L2CAP_CTRL_POLL)
3603                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
3604
3605         if (!test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
3606                 __clear_retrans_timer(chan);
3607                 if (rx_control & L2CAP_CTRL_POLL)
3608                         l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_FINAL);
3609                 return;
3610         }
3611
3612         if (rx_control & L2CAP_CTRL_POLL)
3613                 l2cap_send_srejtail(chan);
3614         else
3615                 l2cap_send_sframe(chan, L2CAP_SUPER_RCV_READY);
3616 }
3617
3618 static inline int l2cap_data_channel_sframe(struct l2cap_chan *chan, u16 rx_control, struct sk_buff *skb)
3619 {
3620         BT_DBG("chan %p rx_control 0x%4.4x len %d", chan, rx_control, skb->len);
3621
3622         if (L2CAP_CTRL_FINAL & rx_control &&
3623                         test_bit(CONN_WAIT_F, &chan->conn_state)) {
3624                 __clear_monitor_timer(chan);
3625                 if (chan->unacked_frames > 0)
3626                         __set_retrans_timer(chan);
3627                 clear_bit(CONN_WAIT_F, &chan->conn_state);
3628         }
3629
3630         switch (rx_control & L2CAP_CTRL_SUPERVISE) {
3631         case L2CAP_SUPER_RCV_READY:
3632                 l2cap_data_channel_rrframe(chan, rx_control);
3633                 break;
3634
3635         case L2CAP_SUPER_REJECT:
3636                 l2cap_data_channel_rejframe(chan, rx_control);
3637                 break;
3638
3639         case L2CAP_SUPER_SELECT_REJECT:
3640                 l2cap_data_channel_srejframe(chan, rx_control);
3641                 break;
3642
3643         case L2CAP_SUPER_RCV_NOT_READY:
3644                 l2cap_data_channel_rnrframe(chan, rx_control);
3645                 break;
3646         }
3647
3648         kfree_skb(skb);
3649         return 0;
3650 }
3651
3652 static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb)
3653 {
3654         struct l2cap_chan *chan = l2cap_pi(sk)->chan;
3655         u16 control;
3656         u8 req_seq;
3657         int len, next_tx_seq_offset, req_seq_offset;
3658
3659         control = get_unaligned_le16(skb->data);
3660         skb_pull(skb, 2);
3661         len = skb->len;
3662
3663         /*
3664          * We can just drop the corrupted I-frame here.
3665          * Receiver will miss it and start proper recovery
3666          * procedures and ask retransmission.
3667          */
3668         if (l2cap_check_fcs(chan, skb))
3669                 goto drop;
3670
3671         if (__is_sar_start(control) && __is_iframe(control))
3672                 len -= 2;
3673
3674         if (chan->fcs == L2CAP_FCS_CRC16)
3675                 len -= 2;
3676
3677         if (len > chan->mps) {
3678                 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3679                 goto drop;
3680         }
3681
3682         req_seq = __get_reqseq(control);
3683         req_seq_offset = (req_seq - chan->expected_ack_seq) % 64;
3684         if (req_seq_offset < 0)
3685                 req_seq_offset += 64;
3686
3687         next_tx_seq_offset =
3688                 (chan->next_tx_seq - chan->expected_ack_seq) % 64;
3689         if (next_tx_seq_offset < 0)
3690                 next_tx_seq_offset += 64;
3691
3692         /* check for invalid req-seq */
3693         if (req_seq_offset > next_tx_seq_offset) {
3694                 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3695                 goto drop;
3696         }
3697
3698         if (__is_iframe(control)) {
3699                 if (len < 0) {
3700                         l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3701                         goto drop;
3702                 }
3703
3704                 l2cap_data_channel_iframe(chan, control, skb);
3705         } else {
3706                 if (len != 0) {
3707                         BT_ERR("%d", len);
3708                         l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3709                         goto drop;
3710                 }
3711
3712                 l2cap_data_channel_sframe(chan, control, skb);
3713         }
3714
3715         return 0;
3716
3717 drop:
3718         kfree_skb(skb);
3719         return 0;
3720 }
3721
3722 static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
3723 {
3724         struct l2cap_chan *chan;
3725         struct sock *sk = NULL;
3726         u16 control;
3727         u8 tx_seq;
3728         int len;
3729
3730         chan = l2cap_get_chan_by_scid(conn, cid);
3731         if (!chan) {
3732                 BT_DBG("unknown cid 0x%4.4x", cid);
3733                 goto drop;
3734         }
3735
3736         sk = chan->sk;
3737
3738         BT_DBG("chan %p, len %d", chan, skb->len);
3739
3740         if (chan->state != BT_CONNECTED)
3741                 goto drop;
3742
3743         switch (chan->mode) {
3744         case L2CAP_MODE_BASIC:
3745                 /* If socket recv buffers overflows we drop data here
3746                  * which is *bad* because L2CAP has to be reliable.
3747                  * But we don't have any other choice. L2CAP doesn't
3748                  * provide flow control mechanism. */
3749
3750                 if (chan->imtu < skb->len)
3751                         goto drop;
3752
3753                 if (!chan->ops->recv(chan->data, skb))
3754                         goto done;
3755                 break;
3756
3757         case L2CAP_MODE_ERTM:
3758                 if (!sock_owned_by_user(sk)) {
3759                         l2cap_ertm_data_rcv(sk, skb);
3760                 } else {
3761                         if (sk_add_backlog(sk, skb))
3762                                 goto drop;
3763                 }
3764
3765                 goto done;
3766
3767         case L2CAP_MODE_STREAMING:
3768                 control = get_unaligned_le16(skb->data);
3769                 skb_pull(skb, 2);
3770                 len = skb->len;
3771
3772                 if (l2cap_check_fcs(chan, skb))
3773                         goto drop;
3774
3775                 if (__is_sar_start(control))
3776                         len -= 2;
3777
3778                 if (chan->fcs == L2CAP_FCS_CRC16)
3779                         len -= 2;
3780
3781                 if (len > chan->mps || len < 0 || __is_sframe(control))
3782                         goto drop;
3783
3784                 tx_seq = __get_txseq(control);
3785
3786                 if (chan->expected_tx_seq != tx_seq) {
3787                         /* Frame(s) missing - must discard partial SDU */
3788                         kfree_skb(chan->sdu);
3789                         chan->sdu = NULL;
3790                         chan->sdu_last_frag = NULL;
3791                         chan->sdu_len = 0;
3792
3793                         /* TODO: Notify userland of missing data */
3794                 }
3795
3796                 chan->expected_tx_seq = (tx_seq + 1) % 64;
3797
3798                 if (l2cap_reassemble_sdu(chan, skb, control) == -EMSGSIZE)
3799                         l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3800
3801                 goto done;
3802
3803         default:
3804                 BT_DBG("chan %p: bad mode 0x%2.2x", chan, chan->mode);
3805                 break;
3806         }
3807
3808 drop:
3809         kfree_skb(skb);
3810
3811 done:
3812         if (sk)
3813                 bh_unlock_sock(sk);
3814
3815         return 0;
3816 }
3817
3818 static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb)
3819 {
3820         struct sock *sk = NULL;
3821         struct l2cap_chan *chan;
3822
3823         chan = l2cap_global_chan_by_psm(0, psm, conn->src);
3824         if (!chan)
3825                 goto drop;
3826
3827         sk = chan->sk;
3828
3829         bh_lock_sock(sk);
3830
3831         BT_DBG("sk %p, len %d", sk, skb->len);
3832
3833         if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
3834                 goto drop;
3835
3836         if (chan->imtu < skb->len)
3837                 goto drop;
3838
3839         if (!chan->ops->recv(chan->data, skb))
3840                 goto done;
3841
3842 drop:
3843         kfree_skb(skb);
3844
3845 done:
3846         if (sk)
3847                 bh_unlock_sock(sk);
3848         return 0;
3849 }
3850
3851 static inline int l2cap_att_channel(struct l2cap_conn *conn, __le16 cid, struct sk_buff *skb)
3852 {
3853         struct sock *sk = NULL;
3854         struct l2cap_chan *chan;
3855
3856         chan = l2cap_global_chan_by_scid(0, cid, conn->src);
3857         if (!chan)
3858                 goto drop;
3859
3860         sk = chan->sk;
3861
3862         bh_lock_sock(sk);
3863
3864         BT_DBG("sk %p, len %d", sk, skb->len);
3865
3866         if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
3867                 goto drop;
3868
3869         if (chan->imtu < skb->len)
3870                 goto drop;
3871
3872         if (!chan->ops->recv(chan->data, skb))
3873                 goto done;
3874
3875 drop:
3876         kfree_skb(skb);
3877
3878 done:
3879         if (sk)
3880                 bh_unlock_sock(sk);
3881         return 0;
3882 }
3883
3884 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
3885 {
3886         struct l2cap_hdr *lh = (void *) skb->data;
3887         u16 cid, len;
3888         __le16 psm;
3889
3890         skb_pull(skb, L2CAP_HDR_SIZE);
3891         cid = __le16_to_cpu(lh->cid);
3892         len = __le16_to_cpu(lh->len);
3893
3894         if (len != skb->len) {
3895                 kfree_skb(skb);
3896                 return;
3897         }
3898
3899         BT_DBG("len %d, cid 0x%4.4x", len, cid);
3900
3901         switch (cid) {
3902         case L2CAP_CID_LE_SIGNALING:
3903         case L2CAP_CID_SIGNALING:
3904                 l2cap_sig_channel(conn, skb);
3905                 break;
3906
3907         case L2CAP_CID_CONN_LESS:
3908                 psm = get_unaligned_le16(skb->data);
3909                 skb_pull(skb, 2);
3910                 l2cap_conless_channel(conn, psm, skb);
3911                 break;
3912
3913         case L2CAP_CID_LE_DATA:
3914                 l2cap_att_channel(conn, cid, skb);
3915                 break;
3916
3917         case L2CAP_CID_SMP:
3918                 if (smp_sig_channel(conn, skb))
3919                         l2cap_conn_del(conn->hcon, EACCES);
3920                 break;
3921
3922         default:
3923                 l2cap_data_channel(conn, cid, skb);
3924                 break;
3925         }
3926 }
3927
3928 /* ---- L2CAP interface with lower layer (HCI) ---- */
3929
3930 static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
3931 {
3932         int exact = 0, lm1 = 0, lm2 = 0;
3933         struct l2cap_chan *c;
3934
3935         if (type != ACL_LINK)
3936                 return -EINVAL;
3937
3938         BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
3939
3940         /* Find listening sockets and check their link_mode */
3941         read_lock(&chan_list_lock);
3942         list_for_each_entry(c, &chan_list, global_l) {
3943                 struct sock *sk = c->sk;
3944
3945                 if (c->state != BT_LISTEN)
3946                         continue;
3947
3948                 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
3949                         lm1 |= HCI_LM_ACCEPT;
3950                         if (c->role_switch)
3951                                 lm1 |= HCI_LM_MASTER;
3952                         exact++;
3953                 } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
3954                         lm2 |= HCI_LM_ACCEPT;
3955                         if (c->role_switch)
3956                                 lm2 |= HCI_LM_MASTER;
3957                 }
3958         }
3959         read_unlock(&chan_list_lock);
3960
3961         return exact ? lm1 : lm2;
3962 }
3963
3964 static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
3965 {
3966         struct l2cap_conn *conn;
3967
3968         BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
3969
3970         if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK))
3971                 return -EINVAL;
3972
3973         if (!status) {
3974                 conn = l2cap_conn_add(hcon, status);
3975                 if (conn)
3976                         l2cap_conn_ready(conn);
3977         } else
3978                 l2cap_conn_del(hcon, bt_to_errno(status));
3979
3980         return 0;
3981 }
3982
3983 static int l2cap_disconn_ind(struct hci_conn *hcon)
3984 {
3985         struct l2cap_conn *conn = hcon->l2cap_data;
3986
3987         BT_DBG("hcon %p", hcon);
3988
3989         if ((hcon->type != ACL_LINK && hcon->type != LE_LINK) || !conn)
3990                 return 0x13;
3991
3992         return conn->disc_reason;
3993 }
3994
3995 static int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
3996 {
3997         BT_DBG("hcon %p reason %d", hcon, reason);
3998
3999         if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK))
4000                 return -EINVAL;
4001
4002         l2cap_conn_del(hcon, bt_to_errno(reason));
4003
4004         return 0;
4005 }
4006
4007 static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt)
4008 {
4009         if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED)
4010                 return;
4011
4012         if (encrypt == 0x00) {
4013                 if (chan->sec_level == BT_SECURITY_MEDIUM) {
4014                         __clear_chan_timer(chan);
4015                         __set_chan_timer(chan, L2CAP_ENC_TIMEOUT);
4016                 } else if (chan->sec_level == BT_SECURITY_HIGH)
4017                         l2cap_chan_close(chan, ECONNREFUSED);
4018         } else {
4019                 if (chan->sec_level == BT_SECURITY_MEDIUM)
4020                         __clear_chan_timer(chan);
4021         }
4022 }
4023
4024 static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
4025 {
4026         struct l2cap_conn *conn = hcon->l2cap_data;
4027         struct l2cap_chan *chan;
4028
4029         if (!conn)
4030                 return 0;
4031
4032         BT_DBG("conn %p", conn);
4033
4034         if (hcon->type == LE_LINK) {
4035                 smp_distribute_keys(conn, 0);
4036                 del_timer(&conn->security_timer);
4037         }
4038
4039         read_lock(&conn->chan_lock);
4040
4041         list_for_each_entry(chan, &conn->chan_l, list) {
4042                 struct sock *sk = chan->sk;
4043
4044                 bh_lock_sock(sk);
4045
4046                 BT_DBG("chan->scid %d", chan->scid);
4047
4048                 if (chan->scid == L2CAP_CID_LE_DATA) {
4049                         if (!status && encrypt) {
4050                                 chan->sec_level = hcon->sec_level;
4051                                 l2cap_chan_ready(sk);
4052                         }
4053
4054                         bh_unlock_sock(sk);
4055                         continue;
4056                 }
4057
4058                 if (test_bit(CONF_CONNECT_PEND, &chan->conf_state)) {
4059                         bh_unlock_sock(sk);
4060                         continue;
4061                 }
4062
4063                 if (!status && (chan->state == BT_CONNECTED ||
4064                                                 chan->state == BT_CONFIG)) {
4065                         l2cap_check_encryption(chan, encrypt);
4066                         bh_unlock_sock(sk);
4067                         continue;
4068                 }
4069
4070                 if (chan->state == BT_CONNECT) {
4071                         if (!status) {
4072                                 struct l2cap_conn_req req;
4073                                 req.scid = cpu_to_le16(chan->scid);
4074                                 req.psm  = chan->psm;
4075
4076                                 chan->ident = l2cap_get_ident(conn);
4077                                 set_bit(CONF_CONNECT_PEND, &chan->conf_state);
4078
4079                                 l2cap_send_cmd(conn, chan->ident,
4080                                         L2CAP_CONN_REQ, sizeof(req), &req);
4081                         } else {
4082                                 __clear_chan_timer(chan);
4083                                 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
4084                         }
4085                 } else if (chan->state == BT_CONNECT2) {
4086                         struct l2cap_conn_rsp rsp;
4087                         __u16 res, stat;
4088
4089                         if (!status) {
4090                                 if (bt_sk(sk)->defer_setup) {
4091                                         struct sock *parent = bt_sk(sk)->parent;
4092                                         res = L2CAP_CR_PEND;
4093                                         stat = L2CAP_CS_AUTHOR_PEND;
4094                                         if (parent)
4095                                                 parent->sk_data_ready(parent, 0);
4096                                 } else {
4097                                         l2cap_state_change(chan, BT_CONFIG);
4098                                         res = L2CAP_CR_SUCCESS;
4099                                         stat = L2CAP_CS_NO_INFO;
4100                                 }
4101                         } else {
4102                                 l2cap_state_change(chan, BT_DISCONN);
4103                                 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
4104                                 res = L2CAP_CR_SEC_BLOCK;
4105                                 stat = L2CAP_CS_NO_INFO;
4106                         }
4107
4108                         rsp.scid   = cpu_to_le16(chan->dcid);
4109                         rsp.dcid   = cpu_to_le16(chan->scid);
4110                         rsp.result = cpu_to_le16(res);
4111                         rsp.status = cpu_to_le16(stat);
4112                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
4113                                                         sizeof(rsp), &rsp);
4114                 }
4115
4116                 bh_unlock_sock(sk);
4117         }
4118
4119         read_unlock(&conn->chan_lock);
4120
4121         return 0;
4122 }
4123
4124 static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
4125 {
4126         struct l2cap_conn *conn = hcon->l2cap_data;
4127
4128         if (!conn)
4129                 conn = l2cap_conn_add(hcon, 0);
4130
4131         if (!conn)
4132                 goto drop;
4133
4134         BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
4135
4136         if (!(flags & ACL_CONT)) {
4137                 struct l2cap_hdr *hdr;
4138                 struct l2cap_chan *chan;
4139                 u16 cid;
4140                 int len;
4141
4142                 if (conn->rx_len) {
4143                         BT_ERR("Unexpected start frame (len %d)", skb->len);
4144                         kfree_skb(conn->rx_skb);
4145                         conn->rx_skb = NULL;
4146                         conn->rx_len = 0;
4147                         l2cap_conn_unreliable(conn, ECOMM);
4148                 }
4149
4150                 /* Start fragment always begin with Basic L2CAP header */
4151                 if (skb->len < L2CAP_HDR_SIZE) {
4152                         BT_ERR("Frame is too short (len %d)", skb->len);
4153                         l2cap_conn_unreliable(conn, ECOMM);
4154                         goto drop;
4155                 }
4156
4157                 hdr = (struct l2cap_hdr *) skb->data;
4158                 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
4159                 cid = __le16_to_cpu(hdr->cid);
4160
4161                 if (len == skb->len) {
4162                         /* Complete frame received */
4163                         l2cap_recv_frame(conn, skb);
4164                         return 0;
4165                 }
4166
4167                 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
4168
4169                 if (skb->len > len) {
4170                         BT_ERR("Frame is too long (len %d, expected len %d)",
4171                                 skb->len, len);
4172                         l2cap_conn_unreliable(conn, ECOMM);
4173                         goto drop;
4174                 }
4175
4176                 chan = l2cap_get_chan_by_scid(conn, cid);
4177
4178                 if (chan && chan->sk) {
4179                         struct sock *sk = chan->sk;
4180
4181                         if (chan->imtu < len - L2CAP_HDR_SIZE) {
4182                                 BT_ERR("Frame exceeding recv MTU (len %d, "
4183                                                         "MTU %d)", len,
4184                                                         chan->imtu);
4185                                 bh_unlock_sock(sk);
4186                                 l2cap_conn_unreliable(conn, ECOMM);
4187                                 goto drop;
4188                         }
4189                         bh_unlock_sock(sk);
4190                 }
4191
4192                 /* Allocate skb for the complete frame (with header) */
4193                 conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC);
4194                 if (!conn->rx_skb)
4195                         goto drop;
4196
4197                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
4198                                                                 skb->len);
4199                 conn->rx_len = len - skb->len;
4200         } else {
4201                 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
4202
4203                 if (!conn->rx_len) {
4204                         BT_ERR("Unexpected continuation frame (len %d)", skb->len);
4205                         l2cap_conn_unreliable(conn, ECOMM);
4206                         goto drop;
4207                 }
4208
4209                 if (skb->len > conn->rx_len) {
4210                         BT_ERR("Fragment is too long (len %d, expected %d)",
4211                                         skb->len, conn->rx_len);
4212                         kfree_skb(conn->rx_skb);
4213                         conn->rx_skb = NULL;
4214                         conn->rx_len = 0;
4215                         l2cap_conn_unreliable(conn, ECOMM);
4216                         goto drop;
4217                 }
4218
4219                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
4220                                                                 skb->len);
4221                 conn->rx_len -= skb->len;
4222
4223                 if (!conn->rx_len) {
4224                         /* Complete frame received */
4225                         l2cap_recv_frame(conn, conn->rx_skb);
4226                         conn->rx_skb = NULL;
4227                 }
4228         }
4229
4230 drop:
4231         kfree_skb(skb);
4232         return 0;
4233 }
4234
4235 static int l2cap_debugfs_show(struct seq_file *f, void *p)
4236 {
4237         struct l2cap_chan *c;
4238
4239         read_lock_bh(&chan_list_lock);
4240
4241         list_for_each_entry(c, &chan_list, global_l) {
4242                 struct sock *sk = c->sk;
4243
4244                 seq_printf(f, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
4245                                         batostr(&bt_sk(sk)->src),
4246                                         batostr(&bt_sk(sk)->dst),
4247                                         c->state, __le16_to_cpu(c->psm),
4248                                         c->scid, c->dcid, c->imtu, c->omtu,
4249                                         c->sec_level, c->mode);
4250 }
4251
4252         read_unlock_bh(&chan_list_lock);
4253
4254         return 0;
4255 }
4256
4257 static int l2cap_debugfs_open(struct inode *inode, struct file *file)
4258 {
4259         return single_open(file, l2cap_debugfs_show, inode->i_private);
4260 }
4261
4262 static const struct file_operations l2cap_debugfs_fops = {
4263         .open           = l2cap_debugfs_open,
4264         .read           = seq_read,
4265         .llseek         = seq_lseek,
4266         .release        = single_release,
4267 };
4268
4269 static struct dentry *l2cap_debugfs;
4270
4271 static struct hci_proto l2cap_hci_proto = {
4272         .name           = "L2CAP",
4273         .id             = HCI_PROTO_L2CAP,
4274         .connect_ind    = l2cap_connect_ind,
4275         .connect_cfm    = l2cap_connect_cfm,
4276         .disconn_ind    = l2cap_disconn_ind,
4277         .disconn_cfm    = l2cap_disconn_cfm,
4278         .security_cfm   = l2cap_security_cfm,
4279         .recv_acldata   = l2cap_recv_acldata
4280 };
4281
4282 int __init l2cap_init(void)
4283 {
4284         int err;
4285
4286         err = l2cap_init_sockets();
4287         if (err < 0)
4288                 return err;
4289
4290         err = hci_register_proto(&l2cap_hci_proto);
4291         if (err < 0) {
4292                 BT_ERR("L2CAP protocol registration failed");
4293                 bt_sock_unregister(BTPROTO_L2CAP);
4294                 goto error;
4295         }
4296
4297         if (bt_debugfs) {
4298                 l2cap_debugfs = debugfs_create_file("l2cap", 0444,
4299                                         bt_debugfs, NULL, &l2cap_debugfs_fops);
4300                 if (!l2cap_debugfs)
4301                         BT_ERR("Failed to create L2CAP debug file");
4302         }
4303
4304         return 0;
4305
4306 error:
4307         l2cap_cleanup_sockets();
4308         return err;
4309 }
4310
4311 void l2cap_exit(void)
4312 {
4313         debugfs_remove(l2cap_debugfs);
4314
4315         if (hci_unregister_proto(&l2cap_hci_proto) < 0)
4316                 BT_ERR("L2CAP protocol unregistration failed");
4317
4318         l2cap_cleanup_sockets();
4319 }
4320
4321 module_param(disable_ertm, bool, 0644);
4322 MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode");