net/bluetooth/l2cap_core.c

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