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