net/bluetooth/l2cap_core.c

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