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