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