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