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