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