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