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