net/bluetooth/l2cap_core.c

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