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