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