net/bluetooth/l2cap_core.c

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