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
2709                         if (le16_to_cpu(rfc.max_pdu_size) > pi->conn->mtu - 10)
2710                                 rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10);
2711
2712                         pi->remote_mps = le16_to_cpu(rfc.max_pdu_size);
2713
2714                         rfc.retrans_timeout =
2715                                 le16_to_cpu(L2CAP_DEFAULT_RETRANS_TO);
2716                         rfc.monitor_timeout =
2717                                 le16_to_cpu(L2CAP_DEFAULT_MONITOR_TO);
2718
2719                         pi->conf_state |= L2CAP_CONF_MODE_DONE;
2720
2721                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2722                                         sizeof(rfc), (unsigned long) &rfc);
2723
2724                         break;
2725
2726                 case L2CAP_MODE_STREAMING:
2727                         if (le16_to_cpu(rfc.max_pdu_size) > pi->conn->mtu - 10)
2728                                 rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10);
2729
2730                         pi->remote_mps = le16_to_cpu(rfc.max_pdu_size);
2731
2732                         pi->conf_state |= L2CAP_CONF_MODE_DONE;
2733
2734                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2735                                         sizeof(rfc), (unsigned long) &rfc);
2736
2737                         break;
2738
2739                 default:
2740                         result = L2CAP_CONF_UNACCEPT;
2741
2742                         memset(&rfc, 0, sizeof(rfc));
2743                         rfc.mode = pi->mode;
2744                 }
2745
2746                 if (result == L2CAP_CONF_SUCCESS)
2747                         pi->conf_state |= L2CAP_CONF_OUTPUT_DONE;
2748         }
2749         rsp->scid   = cpu_to_le16(pi->dcid);
2750         rsp->result = cpu_to_le16(result);
2751         rsp->flags  = cpu_to_le16(0x0000);
2752
2753         return ptr - data;
2754 }
2755
2756 static int l2cap_parse_conf_rsp(struct sock *sk, void *rsp, int len, void *data, u16 *result)
2757 {
2758         struct l2cap_pinfo *pi = l2cap_pi(sk);
2759         struct l2cap_conf_req *req = data;
2760         void *ptr = req->data;
2761         int type, olen;
2762         unsigned long val;
2763         struct l2cap_conf_rfc rfc;
2764
2765         BT_DBG("sk %p, rsp %p, len %d, req %p", sk, rsp, len, data);
2766
2767         while (len >= L2CAP_CONF_OPT_SIZE) {
2768                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
2769
2770                 switch (type) {
2771                 case L2CAP_CONF_MTU:
2772                         if (val < L2CAP_DEFAULT_MIN_MTU) {
2773                                 *result = L2CAP_CONF_UNACCEPT;
2774                                 pi->omtu = L2CAP_DEFAULT_MIN_MTU;
2775                         } else
2776                                 pi->omtu = val;
2777                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->omtu);
2778                         break;
2779
2780                 case L2CAP_CONF_FLUSH_TO:
2781                         pi->flush_to = val;
2782                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
2783                                                         2, pi->flush_to);
2784                         break;
2785
2786                 case L2CAP_CONF_RFC:
2787                         if (olen == sizeof(rfc))
2788                                 memcpy(&rfc, (void *)val, olen);
2789
2790                         if ((pi->conf_state & L2CAP_CONF_STATE2_DEVICE) &&
2791                                                         rfc.mode != pi->mode)
2792                                 return -ECONNREFUSED;
2793
2794                         pi->fcs = 0;
2795
2796                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2797                                         sizeof(rfc), (unsigned long) &rfc);
2798                         break;
2799                 }
2800         }
2801
2802         if (pi->mode == L2CAP_MODE_BASIC && pi->mode != rfc.mode)
2803                 return -ECONNREFUSED;
2804
2805         pi->mode = rfc.mode;
2806
2807         if (*result == L2CAP_CONF_SUCCESS) {
2808                 switch (rfc.mode) {
2809                 case L2CAP_MODE_ERTM:
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->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
2866                 pi->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
2867                 pi->mps    = le16_to_cpu(rfc.max_pdu_size);
2868                 break;
2869         case L2CAP_MODE_STREAMING:
2870                 pi->mps    = le16_to_cpu(rfc.max_pdu_size);
2871         }
2872 }
2873
2874 static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2875 {
2876         struct l2cap_cmd_rej *rej = (struct l2cap_cmd_rej *) data;
2877
2878         if (rej->reason != 0x0000)
2879                 return 0;
2880
2881         if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
2882                                         cmd->ident == conn->info_ident) {
2883                 del_timer(&conn->info_timer);
2884
2885                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2886                 conn->info_ident = 0;
2887
2888                 l2cap_conn_start(conn);
2889         }
2890
2891         return 0;
2892 }
2893
2894 static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2895 {
2896         struct l2cap_chan_list *list = &conn->chan_list;
2897         struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
2898         struct l2cap_conn_rsp rsp;
2899         struct sock *parent, *uninitialized_var(sk);
2900         int result, status = L2CAP_CS_NO_INFO;
2901
2902         u16 dcid = 0, scid = __le16_to_cpu(req->scid);
2903         __le16 psm = req->psm;
2904
2905         BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid);
2906
2907         /* Check if we have socket listening on psm */
2908         parent = l2cap_get_sock_by_psm(BT_LISTEN, psm, conn->src);
2909         if (!parent) {
2910                 result = L2CAP_CR_BAD_PSM;
2911                 goto sendresp;
2912         }
2913
2914         /* Check if the ACL is secure enough (if not SDP) */
2915         if (psm != cpu_to_le16(0x0001) &&
2916                                 !hci_conn_check_link_mode(conn->hcon)) {
2917                 conn->disc_reason = 0x05;
2918                 result = L2CAP_CR_SEC_BLOCK;
2919                 goto response;
2920         }
2921
2922         result = L2CAP_CR_NO_MEM;
2923
2924         /* Check for backlog size */
2925         if (sk_acceptq_is_full(parent)) {
2926                 BT_DBG("backlog full %d", parent->sk_ack_backlog);
2927                 goto response;
2928         }
2929
2930         sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, GFP_ATOMIC);
2931         if (!sk)
2932                 goto response;
2933
2934         write_lock_bh(&list->lock);
2935
2936         /* Check if we already have channel with that dcid */
2937         if (__l2cap_get_chan_by_dcid(list, scid)) {
2938                 write_unlock_bh(&list->lock);
2939                 sock_set_flag(sk, SOCK_ZAPPED);
2940                 l2cap_sock_kill(sk);
2941                 goto response;
2942         }
2943
2944         hci_conn_hold(conn->hcon);
2945
2946         l2cap_sock_init(sk, parent);
2947         bacpy(&bt_sk(sk)->src, conn->src);
2948         bacpy(&bt_sk(sk)->dst, conn->dst);
2949         l2cap_pi(sk)->psm  = psm;
2950         l2cap_pi(sk)->dcid = scid;
2951
2952         __l2cap_chan_add(conn, sk, parent);
2953         dcid = l2cap_pi(sk)->scid;
2954
2955         l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
2956
2957         l2cap_pi(sk)->ident = cmd->ident;
2958
2959         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
2960                 if (l2cap_check_security(sk)) {
2961                         if (bt_sk(sk)->defer_setup) {
2962                                 sk->sk_state = BT_CONNECT2;
2963                                 result = L2CAP_CR_PEND;
2964                                 status = L2CAP_CS_AUTHOR_PEND;
2965                                 parent->sk_data_ready(parent, 0);
2966                         } else {
2967                                 sk->sk_state = BT_CONFIG;
2968                                 result = L2CAP_CR_SUCCESS;
2969                                 status = L2CAP_CS_NO_INFO;
2970                         }
2971                 } else {
2972                         sk->sk_state = BT_CONNECT2;
2973                         result = L2CAP_CR_PEND;
2974                         status = L2CAP_CS_AUTHEN_PEND;
2975                 }
2976         } else {
2977                 sk->sk_state = BT_CONNECT2;
2978                 result = L2CAP_CR_PEND;
2979                 status = L2CAP_CS_NO_INFO;
2980         }
2981
2982         write_unlock_bh(&list->lock);
2983
2984 response:
2985         bh_unlock_sock(parent);
2986
2987 sendresp:
2988         rsp.scid   = cpu_to_le16(scid);
2989         rsp.dcid   = cpu_to_le16(dcid);
2990         rsp.result = cpu_to_le16(result);
2991         rsp.status = cpu_to_le16(status);
2992         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
2993
2994         if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
2995                 struct l2cap_info_req info;
2996                 info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
2997
2998                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
2999                 conn->info_ident = l2cap_get_ident(conn);
3000
3001                 mod_timer(&conn->info_timer, jiffies +
3002                                         msecs_to_jiffies(L2CAP_INFO_TIMEOUT));
3003
3004                 l2cap_send_cmd(conn, conn->info_ident,
3005                                         L2CAP_INFO_REQ, sizeof(info), &info);
3006         }
3007
3008         if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT) &&
3009                                 result == L2CAP_CR_SUCCESS) {
3010                 u8 buf[128];
3011                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
3012                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3013                                         l2cap_build_conf_req(sk, buf), buf);
3014                 l2cap_pi(sk)->num_conf_req++;
3015         }
3016
3017         return 0;
3018 }
3019
3020 static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3021 {
3022         struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
3023         u16 scid, dcid, result, status;
3024         struct sock *sk;
3025         u8 req[128];
3026
3027         scid   = __le16_to_cpu(rsp->scid);
3028         dcid   = __le16_to_cpu(rsp->dcid);
3029         result = __le16_to_cpu(rsp->result);
3030         status = __le16_to_cpu(rsp->status);
3031
3032         BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status);
3033
3034         if (scid) {
3035                 sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
3036                 if (!sk)
3037                         return -EFAULT;
3038         } else {
3039                 sk = l2cap_get_chan_by_ident(&conn->chan_list, cmd->ident);
3040                 if (!sk)
3041                         return -EFAULT;
3042         }
3043
3044         switch (result) {
3045         case L2CAP_CR_SUCCESS:
3046                 sk->sk_state = BT_CONFIG;
3047                 l2cap_pi(sk)->ident = 0;
3048                 l2cap_pi(sk)->dcid = dcid;
3049                 l2cap_pi(sk)->conf_state &= ~L2CAP_CONF_CONNECT_PEND;
3050
3051                 if (l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT)
3052                         break;
3053
3054                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
3055
3056                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3057                                         l2cap_build_conf_req(sk, req), req);
3058                 l2cap_pi(sk)->num_conf_req++;
3059                 break;
3060
3061         case L2CAP_CR_PEND:
3062                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND;
3063                 break;
3064
3065         default:
3066                 l2cap_chan_del(sk, ECONNREFUSED);
3067                 break;
3068         }
3069
3070         bh_unlock_sock(sk);
3071         return 0;
3072 }
3073
3074 static inline void set_default_fcs(struct l2cap_pinfo *pi)
3075 {
3076         /* FCS is enabled only in ERTM or streaming mode, if one or both
3077          * sides request it.
3078          */
3079         if (pi->mode != L2CAP_MODE_ERTM && pi->mode != L2CAP_MODE_STREAMING)
3080                 pi->fcs = L2CAP_FCS_NONE;
3081         else if (!(pi->conf_state & L2CAP_CONF_NO_FCS_RECV))
3082                 pi->fcs = L2CAP_FCS_CRC16;
3083 }
3084
3085 static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
3086 {
3087         struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
3088         u16 dcid, flags;
3089         u8 rsp[64];
3090         struct sock *sk;
3091         int len;
3092
3093         dcid  = __le16_to_cpu(req->dcid);
3094         flags = __le16_to_cpu(req->flags);
3095
3096         BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
3097
3098         sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid);
3099         if (!sk)
3100                 return -ENOENT;
3101
3102         if (sk->sk_state != BT_CONFIG) {
3103                 struct l2cap_cmd_rej rej;
3104
3105                 rej.reason = cpu_to_le16(0x0002);
3106                 l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
3107                                 sizeof(rej), &rej);
3108                 goto unlock;
3109         }
3110
3111         /* Reject if config buffer is too small. */
3112         len = cmd_len - sizeof(*req);
3113         if (l2cap_pi(sk)->conf_len + len > sizeof(l2cap_pi(sk)->conf_req)) {
3114                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
3115                                 l2cap_build_conf_rsp(sk, rsp,
3116                                         L2CAP_CONF_REJECT, flags), rsp);
3117                 goto unlock;
3118         }
3119
3120         /* Store config. */
3121         memcpy(l2cap_pi(sk)->conf_req + l2cap_pi(sk)->conf_len, req->data, len);
3122         l2cap_pi(sk)->conf_len += len;
3123
3124         if (flags & 0x0001) {
3125                 /* Incomplete config. Send empty response. */
3126                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
3127                                 l2cap_build_conf_rsp(sk, rsp,
3128                                         L2CAP_CONF_SUCCESS, 0x0001), rsp);
3129                 goto unlock;
3130         }
3131
3132         /* Complete config. */
3133         len = l2cap_parse_conf_req(sk, rsp);
3134         if (len < 0) {
3135                 l2cap_send_disconn_req(conn, sk, ECONNRESET);
3136                 goto unlock;
3137         }
3138
3139         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
3140         l2cap_pi(sk)->num_conf_rsp++;
3141
3142         /* Reset config buffer. */
3143         l2cap_pi(sk)->conf_len = 0;
3144
3145         if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE))
3146                 goto unlock;
3147
3148         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_INPUT_DONE) {
3149                 set_default_fcs(l2cap_pi(sk));
3150
3151                 sk->sk_state = BT_CONNECTED;
3152
3153                 l2cap_pi(sk)->next_tx_seq = 0;
3154                 l2cap_pi(sk)->expected_tx_seq = 0;
3155                 __skb_queue_head_init(TX_QUEUE(sk));
3156                 if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM)
3157                         l2cap_ertm_init(sk);
3158
3159                 l2cap_chan_ready(sk);
3160                 goto unlock;
3161         }
3162
3163         if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT)) {
3164                 u8 buf[64];
3165                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3166                                         l2cap_build_conf_req(sk, buf), buf);
3167                 l2cap_pi(sk)->num_conf_req++;
3168         }
3169
3170 unlock:
3171         bh_unlock_sock(sk);
3172         return 0;
3173 }
3174
3175 static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3176 {
3177         struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
3178         u16 scid, flags, result;
3179         struct sock *sk;
3180         int len = cmd->len - sizeof(*rsp);
3181
3182         scid   = __le16_to_cpu(rsp->scid);
3183         flags  = __le16_to_cpu(rsp->flags);
3184         result = __le16_to_cpu(rsp->result);
3185
3186         BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x",
3187                         scid, flags, result);
3188
3189         sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
3190         if (!sk)
3191                 return 0;
3192
3193         switch (result) {
3194         case L2CAP_CONF_SUCCESS:
3195                 l2cap_conf_rfc_get(sk, rsp->data, len);
3196                 break;
3197
3198         case L2CAP_CONF_UNACCEPT:
3199                 if (l2cap_pi(sk)->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
3200                         char req[64];
3201
3202                         if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
3203                                 l2cap_send_disconn_req(conn, sk, ECONNRESET);
3204                                 goto done;
3205                         }
3206
3207                         /* throw out any old stored conf requests */
3208                         result = L2CAP_CONF_SUCCESS;
3209                         len = l2cap_parse_conf_rsp(sk, rsp->data,
3210                                                         len, req, &result);
3211                         if (len < 0) {
3212                                 l2cap_send_disconn_req(conn, sk, ECONNRESET);
3213                                 goto done;
3214                         }
3215
3216                         l2cap_send_cmd(conn, l2cap_get_ident(conn),
3217                                                 L2CAP_CONF_REQ, len, req);
3218                         l2cap_pi(sk)->num_conf_req++;
3219                         if (result != L2CAP_CONF_SUCCESS)
3220                                 goto done;
3221                         break;
3222                 }
3223
3224         default:
3225                 sk->sk_err = ECONNRESET;
3226                 l2cap_sock_set_timer(sk, HZ * 5);
3227                 l2cap_send_disconn_req(conn, sk, ECONNRESET);
3228                 goto done;
3229         }
3230
3231         if (flags & 0x01)
3232                 goto done;
3233
3234         l2cap_pi(sk)->conf_state |= L2CAP_CONF_INPUT_DONE;
3235
3236         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE) {
3237                 set_default_fcs(l2cap_pi(sk));
3238
3239                 sk->sk_state = BT_CONNECTED;
3240                 l2cap_pi(sk)->next_tx_seq = 0;
3241                 l2cap_pi(sk)->expected_tx_seq = 0;
3242                 __skb_queue_head_init(TX_QUEUE(sk));
3243                 if (l2cap_pi(sk)->mode ==  L2CAP_MODE_ERTM)
3244                         l2cap_ertm_init(sk);
3245
3246                 l2cap_chan_ready(sk);
3247         }
3248
3249 done:
3250         bh_unlock_sock(sk);
3251         return 0;
3252 }
3253
3254 static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3255 {
3256         struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
3257         struct l2cap_disconn_rsp rsp;
3258         u16 dcid, scid;
3259         struct sock *sk;
3260
3261         scid = __le16_to_cpu(req->scid);
3262         dcid = __le16_to_cpu(req->dcid);
3263
3264         BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
3265
3266         sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid);
3267         if (!sk)
3268                 return 0;
3269
3270         rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid);
3271         rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid);
3272         l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
3273
3274         sk->sk_shutdown = SHUTDOWN_MASK;
3275
3276         l2cap_chan_del(sk, ECONNRESET);
3277         bh_unlock_sock(sk);
3278
3279         l2cap_sock_kill(sk);
3280         return 0;
3281 }
3282
3283 static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3284 {
3285         struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
3286         u16 dcid, scid;
3287         struct sock *sk;
3288
3289         scid = __le16_to_cpu(rsp->scid);
3290         dcid = __le16_to_cpu(rsp->dcid);
3291
3292         BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
3293
3294         sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
3295         if (!sk)
3296                 return 0;
3297
3298         l2cap_chan_del(sk, 0);
3299         bh_unlock_sock(sk);
3300
3301         l2cap_sock_kill(sk);
3302         return 0;
3303 }
3304
3305 static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3306 {
3307         struct l2cap_info_req *req = (struct l2cap_info_req *) data;
3308         u16 type;
3309
3310         type = __le16_to_cpu(req->type);
3311
3312         BT_DBG("type 0x%4.4x", type);
3313
3314         if (type == L2CAP_IT_FEAT_MASK) {
3315                 u8 buf[8];
3316                 u32 feat_mask = l2cap_feat_mask;
3317                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
3318                 rsp->type   = cpu_to_le16(L2CAP_IT_FEAT_MASK);
3319                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
3320                 if (!disable_ertm)
3321                         feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
3322                                                          | L2CAP_FEAT_FCS;
3323                 put_unaligned_le32(feat_mask, rsp->data);
3324                 l2cap_send_cmd(conn, cmd->ident,
3325                                         L2CAP_INFO_RSP, sizeof(buf), buf);
3326         } else if (type == L2CAP_IT_FIXED_CHAN) {
3327                 u8 buf[12];
3328                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
3329                 rsp->type   = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
3330                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
3331                 memcpy(buf + 4, l2cap_fixed_chan, 8);
3332                 l2cap_send_cmd(conn, cmd->ident,
3333                                         L2CAP_INFO_RSP, sizeof(buf), buf);
3334         } else {
3335                 struct l2cap_info_rsp rsp;
3336                 rsp.type   = cpu_to_le16(type);
3337                 rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP);
3338                 l2cap_send_cmd(conn, cmd->ident,
3339                                         L2CAP_INFO_RSP, sizeof(rsp), &rsp);
3340         }
3341
3342         return 0;
3343 }
3344
3345 static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3346 {
3347         struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
3348         u16 type, result;
3349
3350         type   = __le16_to_cpu(rsp->type);
3351         result = __le16_to_cpu(rsp->result);
3352
3353         BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
3354
3355         del_timer(&conn->info_timer);
3356
3357         if (result != L2CAP_IR_SUCCESS) {
3358                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3359                 conn->info_ident = 0;
3360
3361                 l2cap_conn_start(conn);
3362
3363                 return 0;
3364         }
3365
3366         if (type == L2CAP_IT_FEAT_MASK) {
3367                 conn->feat_mask = get_unaligned_le32(rsp->data);
3368
3369                 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
3370                         struct l2cap_info_req req;
3371                         req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
3372
3373                         conn->info_ident = l2cap_get_ident(conn);
3374
3375                         l2cap_send_cmd(conn, conn->info_ident,
3376                                         L2CAP_INFO_REQ, sizeof(req), &req);
3377                 } else {
3378                         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3379                         conn->info_ident = 0;
3380
3381                         l2cap_conn_start(conn);
3382                 }
3383         } else if (type == L2CAP_IT_FIXED_CHAN) {
3384                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3385                 conn->info_ident = 0;
3386
3387                 l2cap_conn_start(conn);
3388         }
3389
3390         return 0;
3391 }
3392
3393 static inline void l2cap_sig_channel(struct l2cap_conn *conn, struct sk_buff *skb)
3394 {
3395         u8 *data = skb->data;
3396         int len = skb->len;
3397         struct l2cap_cmd_hdr cmd;
3398         int err = 0;
3399
3400         l2cap_raw_recv(conn, skb);
3401
3402         while (len >= L2CAP_CMD_HDR_SIZE) {
3403                 u16 cmd_len;
3404                 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
3405                 data += L2CAP_CMD_HDR_SIZE;
3406                 len  -= L2CAP_CMD_HDR_SIZE;
3407
3408                 cmd_len = le16_to_cpu(cmd.len);
3409
3410                 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, cmd.ident);
3411
3412                 if (cmd_len > len || !cmd.ident) {
3413                         BT_DBG("corrupted command");
3414                         break;
3415                 }
3416
3417                 switch (cmd.code) {
3418                 case L2CAP_COMMAND_REJ:
3419                         l2cap_command_rej(conn, &cmd, data);
3420                         break;
3421
3422                 case L2CAP_CONN_REQ:
3423                         err = l2cap_connect_req(conn, &cmd, data);
3424                         break;
3425
3426                 case L2CAP_CONN_RSP:
3427                         err = l2cap_connect_rsp(conn, &cmd, data);
3428                         break;
3429
3430                 case L2CAP_CONF_REQ:
3431                         err = l2cap_config_req(conn, &cmd, cmd_len, data);
3432                         break;
3433
3434                 case L2CAP_CONF_RSP:
3435                         err = l2cap_config_rsp(conn, &cmd, data);
3436                         break;
3437
3438                 case L2CAP_DISCONN_REQ:
3439                         err = l2cap_disconnect_req(conn, &cmd, data);
3440                         break;
3441
3442                 case L2CAP_DISCONN_RSP:
3443                         err = l2cap_disconnect_rsp(conn, &cmd, data);
3444                         break;
3445
3446                 case L2CAP_ECHO_REQ:
3447                         l2cap_send_cmd(conn, cmd.ident, L2CAP_ECHO_RSP, cmd_len, data);
3448                         break;
3449
3450                 case L2CAP_ECHO_RSP:
3451                         break;
3452
3453                 case L2CAP_INFO_REQ:
3454                         err = l2cap_information_req(conn, &cmd, data);
3455                         break;
3456
3457                 case L2CAP_INFO_RSP:
3458                         err = l2cap_information_rsp(conn, &cmd, data);
3459                         break;
3460
3461                 default:
3462                         BT_ERR("Unknown signaling command 0x%2.2x", cmd.code);
3463                         err = -EINVAL;
3464                         break;
3465                 }
3466
3467                 if (err) {
3468                         struct l2cap_cmd_rej rej;
3469                         BT_DBG("error %d", err);
3470
3471                         /* FIXME: Map err to a valid reason */
3472                         rej.reason = cpu_to_le16(0);
3473                         l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
3474                 }
3475
3476                 data += cmd_len;
3477                 len  -= cmd_len;
3478         }
3479
3480         kfree_skb(skb);
3481 }
3482
3483 static int l2cap_check_fcs(struct l2cap_pinfo *pi,  struct sk_buff *skb)
3484 {
3485         u16 our_fcs, rcv_fcs;
3486         int hdr_size = L2CAP_HDR_SIZE + 2;
3487
3488         if (pi->fcs == L2CAP_FCS_CRC16) {
3489                 skb_trim(skb, skb->len - 2);
3490                 rcv_fcs = get_unaligned_le16(skb->data + skb->len);
3491                 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
3492
3493                 if (our_fcs != rcv_fcs)
3494                         return -EBADMSG;
3495         }
3496         return 0;
3497 }
3498
3499 static inline void l2cap_send_i_or_rr_or_rnr(struct sock *sk)
3500 {
3501         struct l2cap_pinfo *pi = l2cap_pi(sk);
3502         u16 control = 0;
3503
3504         pi->frames_sent = 0;
3505
3506         control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3507
3508         if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
3509                 control |= L2CAP_SUPER_RCV_NOT_READY;
3510                 l2cap_send_sframe(pi, control);
3511                 pi->conn_state |= L2CAP_CONN_RNR_SENT;
3512         }
3513
3514         if (pi->conn_state & L2CAP_CONN_REMOTE_BUSY)
3515                 l2cap_retransmit_frames(sk);
3516
3517         l2cap_ertm_send(sk);
3518
3519         if (!(pi->conn_state & L2CAP_CONN_LOCAL_BUSY) &&
3520                         pi->frames_sent == 0) {
3521                 control |= L2CAP_SUPER_RCV_READY;
3522                 l2cap_send_sframe(pi, control);
3523         }
3524 }
3525
3526 static int l2cap_add_to_srej_queue(struct sock *sk, struct sk_buff *skb, u8 tx_seq, u8 sar)
3527 {
3528         struct sk_buff *next_skb;
3529         struct l2cap_pinfo *pi = l2cap_pi(sk);
3530         int tx_seq_offset, next_tx_seq_offset;
3531
3532         bt_cb(skb)->tx_seq = tx_seq;
3533         bt_cb(skb)->sar = sar;
3534
3535         next_skb = skb_peek(SREJ_QUEUE(sk));
3536         if (!next_skb) {
3537                 __skb_queue_tail(SREJ_QUEUE(sk), skb);
3538                 return 0;
3539         }
3540
3541         tx_seq_offset = (tx_seq - pi->buffer_seq) % 64;
3542         if (tx_seq_offset < 0)
3543                 tx_seq_offset += 64;
3544
3545         do {
3546                 if (bt_cb(next_skb)->tx_seq == tx_seq)
3547                         return -EINVAL;
3548
3549                 next_tx_seq_offset = (bt_cb(next_skb)->tx_seq -
3550                                                 pi->buffer_seq) % 64;
3551                 if (next_tx_seq_offset < 0)
3552                         next_tx_seq_offset += 64;
3553
3554                 if (next_tx_seq_offset > tx_seq_offset) {
3555                         __skb_queue_before(SREJ_QUEUE(sk), next_skb, skb);
3556                         return 0;
3557                 }
3558
3559                 if (skb_queue_is_last(SREJ_QUEUE(sk), next_skb))
3560                         break;
3561
3562         } while ((next_skb = skb_queue_next(SREJ_QUEUE(sk), next_skb)));
3563
3564         __skb_queue_tail(SREJ_QUEUE(sk), skb);
3565
3566         return 0;
3567 }
3568
3569 static int l2cap_ertm_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)
3570 {
3571         struct l2cap_pinfo *pi = l2cap_pi(sk);
3572         struct sk_buff *_skb;
3573         int err;
3574
3575         switch (control & L2CAP_CTRL_SAR) {
3576         case L2CAP_SDU_UNSEGMENTED:
3577                 if (pi->conn_state & L2CAP_CONN_SAR_SDU)
3578                         goto drop;
3579
3580                 err = sock_queue_rcv_skb(sk, skb);
3581                 if (!err)
3582                         return err;
3583
3584                 break;
3585
3586         case L2CAP_SDU_START:
3587                 if (pi->conn_state & L2CAP_CONN_SAR_SDU)
3588                         goto drop;
3589
3590                 pi->sdu_len = get_unaligned_le16(skb->data);
3591
3592                 if (pi->sdu_len > pi->imtu)
3593                         goto disconnect;
3594
3595                 pi->sdu = bt_skb_alloc(pi->sdu_len, GFP_ATOMIC);
3596                 if (!pi->sdu)
3597                         return -ENOMEM;
3598
3599                 /* pull sdu_len bytes only after alloc, because of Local Busy
3600                  * condition we have to be sure that this will be executed
3601                  * only once, i.e., when alloc does not fail */
3602                 skb_pull(skb, 2);
3603
3604                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3605
3606                 pi->conn_state |= L2CAP_CONN_SAR_SDU;
3607                 pi->partial_sdu_len = skb->len;
3608                 break;
3609
3610         case L2CAP_SDU_CONTINUE:
3611                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
3612                         goto disconnect;
3613
3614                 if (!pi->sdu)
3615                         goto disconnect;
3616
3617                 pi->partial_sdu_len += skb->len;
3618                 if (pi->partial_sdu_len > pi->sdu_len)
3619                         goto drop;
3620
3621                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3622
3623                 break;
3624
3625         case L2CAP_SDU_END:
3626                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
3627                         goto disconnect;
3628
3629                 if (!pi->sdu)
3630                         goto disconnect;
3631
3632                 if (!(pi->conn_state & L2CAP_CONN_SAR_RETRY)) {
3633                         pi->partial_sdu_len += skb->len;
3634
3635                         if (pi->partial_sdu_len > pi->imtu)
3636                                 goto drop;
3637
3638                         if (pi->partial_sdu_len != pi->sdu_len)
3639                                 goto drop;
3640
3641                         memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3642                 }
3643
3644                 _skb = skb_clone(pi->sdu, GFP_ATOMIC);
3645                 if (!_skb) {
3646                         pi->conn_state |= L2CAP_CONN_SAR_RETRY;
3647                         return -ENOMEM;
3648                 }
3649
3650                 err = sock_queue_rcv_skb(sk, _skb);
3651                 if (err < 0) {
3652                         kfree_skb(_skb);
3653                         pi->conn_state |= L2CAP_CONN_SAR_RETRY;
3654                         return err;
3655                 }
3656
3657                 pi->conn_state &= ~L2CAP_CONN_SAR_RETRY;
3658                 pi->conn_state &= ~L2CAP_CONN_SAR_SDU;
3659
3660                 kfree_skb(pi->sdu);
3661                 break;
3662         }
3663
3664         kfree_skb(skb);
3665         return 0;
3666
3667 drop:
3668         kfree_skb(pi->sdu);
3669         pi->sdu = NULL;
3670
3671 disconnect:
3672         l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
3673         kfree_skb(skb);
3674         return 0;
3675 }
3676
3677 static int l2cap_try_push_rx_skb(struct sock *sk)
3678 {
3679         struct l2cap_pinfo *pi = l2cap_pi(sk);
3680         struct sk_buff *skb;
3681         u16 control;
3682         int err;
3683
3684         while ((skb = skb_dequeue(BUSY_QUEUE(sk)))) {
3685                 control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
3686                 err = l2cap_ertm_reassembly_sdu(sk, skb, control);
3687                 if (err < 0) {
3688                         skb_queue_head(BUSY_QUEUE(sk), skb);
3689                         return -EBUSY;
3690                 }
3691
3692                 pi->buffer_seq = (pi->buffer_seq + 1) % 64;
3693         }
3694
3695         if (!(pi->conn_state & L2CAP_CONN_RNR_SENT))
3696                 goto done;
3697
3698         control = pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3699         control |= L2CAP_SUPER_RCV_READY | L2CAP_CTRL_POLL;
3700         l2cap_send_sframe(pi, control);
3701         l2cap_pi(sk)->retry_count = 1;
3702
3703         del_timer(&pi->retrans_timer);
3704         __mod_monitor_timer();
3705
3706         l2cap_pi(sk)->conn_state |= L2CAP_CONN_WAIT_F;
3707
3708 done:
3709         pi->conn_state &= ~L2CAP_CONN_LOCAL_BUSY;
3710         pi->conn_state &= ~L2CAP_CONN_RNR_SENT;
3711
3712         BT_DBG("sk %p, Exit local busy", sk);
3713
3714         return 0;
3715 }
3716
3717 static void l2cap_busy_work(struct work_struct *work)
3718 {
3719         DECLARE_WAITQUEUE(wait, current);
3720         struct l2cap_pinfo *pi =
3721                 container_of(work, struct l2cap_pinfo, busy_work);
3722         struct sock *sk = (struct sock *)pi;
3723         int n_tries = 0, timeo = HZ/5, err;
3724         struct sk_buff *skb;
3725
3726         lock_sock(sk);
3727
3728         add_wait_queue(sk_sleep(sk), &wait);
3729         while ((skb = skb_peek(BUSY_QUEUE(sk)))) {
3730                 set_current_state(TASK_INTERRUPTIBLE);
3731
3732                 if (n_tries++ > L2CAP_LOCAL_BUSY_TRIES) {
3733                         err = -EBUSY;
3734                         l2cap_send_disconn_req(pi->conn, sk, EBUSY);
3735                         break;
3736                 }
3737
3738                 if (!timeo)
3739                         timeo = HZ/5;
3740
3741                 if (signal_pending(current)) {
3742                         err = sock_intr_errno(timeo);
3743                         break;
3744                 }
3745
3746                 release_sock(sk);
3747                 timeo = schedule_timeout(timeo);
3748                 lock_sock(sk);
3749
3750                 err = sock_error(sk);
3751                 if (err)
3752                         break;
3753
3754                 if (l2cap_try_push_rx_skb(sk) == 0)
3755                         break;
3756         }
3757
3758         set_current_state(TASK_RUNNING);
3759         remove_wait_queue(sk_sleep(sk), &wait);
3760
3761         release_sock(sk);
3762 }
3763
3764 static int l2cap_push_rx_skb(struct sock *sk, struct sk_buff *skb, u16 control)
3765 {
3766         struct l2cap_pinfo *pi = l2cap_pi(sk);
3767         int sctrl, err;
3768
3769         if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
3770                 bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT;
3771                 __skb_queue_tail(BUSY_QUEUE(sk), skb);
3772                 return l2cap_try_push_rx_skb(sk);
3773
3774
3775         }
3776
3777         err = l2cap_ertm_reassembly_sdu(sk, skb, control);
3778         if (err >= 0) {
3779                 pi->buffer_seq = (pi->buffer_seq + 1) % 64;
3780                 return err;
3781         }
3782
3783         /* Busy Condition */
3784         BT_DBG("sk %p, Enter local busy", sk);
3785
3786         pi->conn_state |= L2CAP_CONN_LOCAL_BUSY;
3787         bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT;
3788         __skb_queue_tail(BUSY_QUEUE(sk), skb);
3789
3790         sctrl = pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3791         sctrl |= L2CAP_SUPER_RCV_NOT_READY;
3792         l2cap_send_sframe(pi, sctrl);
3793
3794         pi->conn_state |= L2CAP_CONN_RNR_SENT;
3795
3796         del_timer(&pi->ack_timer);
3797
3798         queue_work(_busy_wq, &pi->busy_work);
3799
3800         return err;
3801 }
3802
3803 static int l2cap_streaming_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)
3804 {
3805         struct l2cap_pinfo *pi = l2cap_pi(sk);
3806         struct sk_buff *_skb;
3807         int err = -EINVAL;
3808
3809         /*
3810          * TODO: We have to notify the userland if some data is lost with the
3811          * Streaming Mode.
3812          */
3813
3814         switch (control & L2CAP_CTRL_SAR) {
3815         case L2CAP_SDU_UNSEGMENTED:
3816                 if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
3817                         kfree_skb(pi->sdu);
3818                         break;
3819                 }
3820
3821                 err = sock_queue_rcv_skb(sk, skb);
3822                 if (!err)
3823                         return 0;
3824
3825                 break;
3826
3827         case L2CAP_SDU_START:
3828                 if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
3829                         kfree_skb(pi->sdu);
3830                         break;
3831                 }
3832
3833                 pi->sdu_len = get_unaligned_le16(skb->data);
3834                 skb_pull(skb, 2);
3835
3836                 if (pi->sdu_len > pi->imtu) {
3837                         err = -EMSGSIZE;
3838                         break;
3839                 }
3840
3841                 pi->sdu = bt_skb_alloc(pi->sdu_len, GFP_ATOMIC);
3842                 if (!pi->sdu) {
3843                         err = -ENOMEM;
3844                         break;
3845                 }
3846
3847                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3848
3849                 pi->conn_state |= L2CAP_CONN_SAR_SDU;
3850                 pi->partial_sdu_len = skb->len;
3851                 err = 0;
3852                 break;
3853
3854         case L2CAP_SDU_CONTINUE:
3855                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
3856                         break;
3857
3858                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3859
3860                 pi->partial_sdu_len += skb->len;
3861                 if (pi->partial_sdu_len > pi->sdu_len)
3862                         kfree_skb(pi->sdu);
3863                 else
3864                         err = 0;
3865
3866                 break;
3867
3868         case L2CAP_SDU_END:
3869                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
3870                         break;
3871
3872                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3873
3874                 pi->conn_state &= ~L2CAP_CONN_SAR_SDU;
3875                 pi->partial_sdu_len += skb->len;
3876
3877                 if (pi->partial_sdu_len > pi->imtu)
3878                         goto drop;
3879
3880                 if (pi->partial_sdu_len == pi->sdu_len) {
3881                         _skb = skb_clone(pi->sdu, GFP_ATOMIC);
3882                         err = sock_queue_rcv_skb(sk, _skb);
3883                         if (err < 0)
3884                                 kfree_skb(_skb);
3885                 }
3886                 err = 0;
3887
3888 drop:
3889                 kfree_skb(pi->sdu);
3890                 break;
3891         }
3892
3893         kfree_skb(skb);
3894         return err;
3895 }
3896
3897 static void l2cap_check_srej_gap(struct sock *sk, u8 tx_seq)
3898 {
3899         struct sk_buff *skb;
3900         u16 control;
3901
3902         while ((skb = skb_peek(SREJ_QUEUE(sk)))) {
3903                 if (bt_cb(skb)->tx_seq != tx_seq)
3904                         break;
3905
3906                 skb = skb_dequeue(SREJ_QUEUE(sk));
3907                 control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
3908                 l2cap_ertm_reassembly_sdu(sk, skb, control);
3909                 l2cap_pi(sk)->buffer_seq_srej =
3910                         (l2cap_pi(sk)->buffer_seq_srej + 1) % 64;
3911                 tx_seq = (tx_seq + 1) % 64;
3912         }
3913 }
3914
3915 static void l2cap_resend_srejframe(struct sock *sk, u8 tx_seq)
3916 {
3917         struct l2cap_pinfo *pi = l2cap_pi(sk);
3918         struct srej_list *l, *tmp;
3919         u16 control;
3920
3921         list_for_each_entry_safe(l, tmp, SREJ_LIST(sk), list) {
3922                 if (l->tx_seq == tx_seq) {
3923                         list_del(&l->list);
3924                         kfree(l);
3925                         return;
3926                 }
3927                 control = L2CAP_SUPER_SELECT_REJECT;
3928                 control |= l->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3929                 l2cap_send_sframe(pi, control);
3930                 list_del(&l->list);
3931                 list_add_tail(&l->list, SREJ_LIST(sk));
3932         }
3933 }
3934
3935 static void l2cap_send_srejframe(struct sock *sk, u8 tx_seq)
3936 {
3937         struct l2cap_pinfo *pi = l2cap_pi(sk);
3938         struct srej_list *new;
3939         u16 control;
3940
3941         while (tx_seq != pi->expected_tx_seq) {
3942                 control = L2CAP_SUPER_SELECT_REJECT;
3943                 control |= pi->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3944                 l2cap_send_sframe(pi, control);
3945
3946                 new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC);
3947                 new->tx_seq = pi->expected_tx_seq;
3948                 pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3949                 list_add_tail(&new->list, SREJ_LIST(sk));
3950         }
3951         pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3952 }
3953
3954 static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
3955 {
3956         struct l2cap_pinfo *pi = l2cap_pi(sk);
3957         u8 tx_seq = __get_txseq(rx_control);
3958         u8 req_seq = __get_reqseq(rx_control);
3959         u8 sar = rx_control >> L2CAP_CTRL_SAR_SHIFT;
3960         int tx_seq_offset, expected_tx_seq_offset;
3961         int num_to_ack = (pi->tx_win/6) + 1;
3962         int err = 0;
3963
3964         BT_DBG("sk %p len %d tx_seq %d rx_control 0x%4.4x", sk, skb->len, tx_seq,
3965                                                                 rx_control);
3966
3967         if (L2CAP_CTRL_FINAL & rx_control &&
3968                         l2cap_pi(sk)->conn_state & L2CAP_CONN_WAIT_F) {
3969                 del_timer(&pi->monitor_timer);
3970                 if (pi->unacked_frames > 0)
3971                         __mod_retrans_timer();
3972                 pi->conn_state &= ~L2CAP_CONN_WAIT_F;
3973         }
3974
3975         pi->expected_ack_seq = req_seq;
3976         l2cap_drop_acked_frames(sk);
3977
3978         if (tx_seq == pi->expected_tx_seq)
3979                 goto expected;
3980
3981         tx_seq_offset = (tx_seq - pi->buffer_seq) % 64;
3982         if (tx_seq_offset < 0)
3983                 tx_seq_offset += 64;
3984
3985         /* invalid tx_seq */
3986         if (tx_seq_offset >= pi->tx_win) {
3987                 l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
3988                 goto drop;
3989         }
3990
3991         if (pi->conn_state == L2CAP_CONN_LOCAL_BUSY)
3992                 goto drop;
3993
3994         if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
3995                 struct srej_list *first;
3996
3997                 first = list_first_entry(SREJ_LIST(sk),
3998                                 struct srej_list, list);
3999                 if (tx_seq == first->tx_seq) {
4000                         l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
4001                         l2cap_check_srej_gap(sk, tx_seq);
4002
4003                         list_del(&first->list);
4004                         kfree(first);
4005
4006                         if (list_empty(SREJ_LIST(sk))) {
4007                                 pi->buffer_seq = pi->buffer_seq_srej;
4008                                 pi->conn_state &= ~L2CAP_CONN_SREJ_SENT;
4009                                 l2cap_send_ack(pi);
4010                                 BT_DBG("sk %p, Exit SREJ_SENT", sk);
4011                         }
4012                 } else {
4013                         struct srej_list *l;
4014
4015                         /* duplicated tx_seq */
4016                         if (l2cap_add_to_srej_queue(sk, skb, tx_seq, sar) < 0)
4017                                 goto drop;
4018
4019                         list_for_each_entry(l, SREJ_LIST(sk), list) {
4020                                 if (l->tx_seq == tx_seq) {
4021                                         l2cap_resend_srejframe(sk, tx_seq);
4022                                         return 0;
4023                                 }
4024                         }
4025                         l2cap_send_srejframe(sk, tx_seq);
4026                 }
4027         } else {
4028                 expected_tx_seq_offset =
4029                         (pi->expected_tx_seq - pi->buffer_seq) % 64;
4030                 if (expected_tx_seq_offset < 0)
4031                         expected_tx_seq_offset += 64;
4032
4033                 /* duplicated tx_seq */
4034                 if (tx_seq_offset < expected_tx_seq_offset)
4035                         goto drop;
4036
4037                 pi->conn_state |= L2CAP_CONN_SREJ_SENT;
4038
4039                 BT_DBG("sk %p, Enter SREJ", sk);
4040
4041                 INIT_LIST_HEAD(SREJ_LIST(sk));
4042                 pi->buffer_seq_srej = pi->buffer_seq;
4043
4044                 __skb_queue_head_init(SREJ_QUEUE(sk));
4045                 __skb_queue_head_init(BUSY_QUEUE(sk));
4046                 l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
4047
4048                 pi->conn_state |= L2CAP_CONN_SEND_PBIT;
4049
4050                 l2cap_send_srejframe(sk, tx_seq);
4051
4052                 del_timer(&pi->ack_timer);
4053         }
4054         return 0;
4055
4056 expected:
4057         pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
4058
4059         if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
4060                 bt_cb(skb)->tx_seq = tx_seq;
4061                 bt_cb(skb)->sar = sar;
4062                 __skb_queue_tail(SREJ_QUEUE(sk), skb);
4063                 return 0;
4064         }
4065
4066         err = l2cap_push_rx_skb(sk, skb, rx_control);
4067         if (err < 0)
4068                 return 0;
4069
4070         if (rx_control & L2CAP_CTRL_FINAL) {
4071                 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
4072                         pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
4073                 else
4074                         l2cap_retransmit_frames(sk);
4075         }
4076
4077         __mod_ack_timer();
4078
4079         pi->num_acked = (pi->num_acked + 1) % num_to_ack;
4080         if (pi->num_acked == num_to_ack - 1)
4081                 l2cap_send_ack(pi);
4082
4083         return 0;
4084
4085 drop:
4086         kfree_skb(skb);
4087         return 0;
4088 }
4089
4090 static inline void l2cap_data_channel_rrframe(struct sock *sk, u16 rx_control)
4091 {
4092         struct l2cap_pinfo *pi = l2cap_pi(sk);
4093
4094         BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, __get_reqseq(rx_control),
4095                                                 rx_control);
4096
4097         pi->expected_ack_seq = __get_reqseq(rx_control);
4098         l2cap_drop_acked_frames(sk);
4099
4100         if (rx_control & L2CAP_CTRL_POLL) {
4101                 pi->conn_state |= L2CAP_CONN_SEND_FBIT;
4102                 if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
4103                         if ((pi->conn_state & L2CAP_CONN_REMOTE_BUSY) &&
4104                                         (pi->unacked_frames > 0))
4105                                 __mod_retrans_timer();
4106
4107                         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
4108                         l2cap_send_srejtail(sk);
4109                 } else {
4110                         l2cap_send_i_or_rr_or_rnr(sk);
4111                 }
4112
4113         } else if (rx_control & L2CAP_CTRL_FINAL) {
4114                 pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
4115
4116                 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
4117                         pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
4118                 else
4119                         l2cap_retransmit_frames(sk);
4120
4121         } else {
4122                 if ((pi->conn_state & L2CAP_CONN_REMOTE_BUSY) &&
4123                                 (pi->unacked_frames > 0))
4124                         __mod_retrans_timer();
4125
4126                 pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
4127                 if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
4128                         l2cap_send_ack(pi);
4129                 } else {
4130                         l2cap_ertm_send(sk);
4131                 }
4132         }
4133 }
4134
4135 static inline void l2cap_data_channel_rejframe(struct sock *sk, u16 rx_control)
4136 {
4137         struct l2cap_pinfo *pi = l2cap_pi(sk);
4138         u8 tx_seq = __get_reqseq(rx_control);
4139
4140         BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, tx_seq, rx_control);
4141
4142         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
4143
4144         pi->expected_ack_seq = tx_seq;
4145         l2cap_drop_acked_frames(sk);
4146
4147         if (rx_control & L2CAP_CTRL_FINAL) {
4148                 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
4149                         pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
4150                 else
4151                         l2cap_retransmit_frames(sk);
4152         } else {
4153                 l2cap_retransmit_frames(sk);
4154
4155                 if (pi->conn_state & L2CAP_CONN_WAIT_F)
4156                         pi->conn_state |= L2CAP_CONN_REJ_ACT;
4157         }
4158 }
4159 static inline void l2cap_data_channel_srejframe(struct sock *sk, u16 rx_control)
4160 {
4161         struct l2cap_pinfo *pi = l2cap_pi(sk);
4162         u8 tx_seq = __get_reqseq(rx_control);
4163
4164         BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, tx_seq, rx_control);
4165
4166         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
4167
4168         if (rx_control & L2CAP_CTRL_POLL) {
4169                 pi->expected_ack_seq = tx_seq;
4170                 l2cap_drop_acked_frames(sk);
4171
4172                 pi->conn_state |= L2CAP_CONN_SEND_FBIT;
4173                 l2cap_retransmit_one_frame(sk, tx_seq);
4174
4175                 l2cap_ertm_send(sk);
4176
4177                 if (pi->conn_state & L2CAP_CONN_WAIT_F) {
4178                         pi->srej_save_reqseq = tx_seq;
4179                         pi->conn_state |= L2CAP_CONN_SREJ_ACT;
4180                 }
4181         } else if (rx_control & L2CAP_CTRL_FINAL) {
4182                 if ((pi->conn_state & L2CAP_CONN_SREJ_ACT) &&
4183                                 pi->srej_save_reqseq == tx_seq)
4184                         pi->conn_state &= ~L2CAP_CONN_SREJ_ACT;
4185                 else
4186                         l2cap_retransmit_one_frame(sk, tx_seq);
4187         } else {
4188                 l2cap_retransmit_one_frame(sk, tx_seq);
4189                 if (pi->conn_state & L2CAP_CONN_WAIT_F) {
4190                         pi->srej_save_reqseq = tx_seq;
4191                         pi->conn_state |= L2CAP_CONN_SREJ_ACT;
4192                 }
4193         }
4194 }
4195
4196 static inline void l2cap_data_channel_rnrframe(struct sock *sk, u16 rx_control)
4197 {
4198         struct l2cap_pinfo *pi = l2cap_pi(sk);
4199         u8 tx_seq = __get_reqseq(rx_control);
4200
4201         BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, tx_seq, rx_control);
4202
4203         pi->conn_state |= L2CAP_CONN_REMOTE_BUSY;
4204         pi->expected_ack_seq = tx_seq;
4205         l2cap_drop_acked_frames(sk);
4206
4207         if (rx_control & L2CAP_CTRL_POLL)
4208                 pi->conn_state |= L2CAP_CONN_SEND_FBIT;
4209
4210         if (!(pi->conn_state & L2CAP_CONN_SREJ_SENT)) {
4211                 del_timer(&pi->retrans_timer);
4212                 if (rx_control & L2CAP_CTRL_POLL)
4213                         l2cap_send_rr_or_rnr(pi, L2CAP_CTRL_FINAL);
4214                 return;
4215         }
4216
4217         if (rx_control & L2CAP_CTRL_POLL)
4218                 l2cap_send_srejtail(sk);
4219         else
4220                 l2cap_send_sframe(pi, L2CAP_SUPER_RCV_READY);
4221 }
4222
4223 static inline int l2cap_data_channel_sframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
4224 {
4225         BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
4226
4227         if (L2CAP_CTRL_FINAL & rx_control &&
4228                         l2cap_pi(sk)->conn_state & L2CAP_CONN_WAIT_F) {
4229                 del_timer(&l2cap_pi(sk)->monitor_timer);
4230                 if (l2cap_pi(sk)->unacked_frames > 0)
4231                         __mod_retrans_timer();
4232                 l2cap_pi(sk)->conn_state &= ~L2CAP_CONN_WAIT_F;
4233         }
4234
4235         switch (rx_control & L2CAP_CTRL_SUPERVISE) {
4236         case L2CAP_SUPER_RCV_READY:
4237                 l2cap_data_channel_rrframe(sk, rx_control);
4238                 break;
4239
4240         case L2CAP_SUPER_REJECT:
4241                 l2cap_data_channel_rejframe(sk, rx_control);
4242                 break;
4243
4244         case L2CAP_SUPER_SELECT_REJECT:
4245                 l2cap_data_channel_srejframe(sk, rx_control);
4246                 break;
4247
4248         case L2CAP_SUPER_RCV_NOT_READY:
4249                 l2cap_data_channel_rnrframe(sk, rx_control);
4250                 break;
4251         }
4252
4253         kfree_skb(skb);
4254         return 0;
4255 }
4256
4257 static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb)
4258 {
4259         struct l2cap_pinfo *pi = l2cap_pi(sk);
4260         u16 control;
4261         u8 req_seq;
4262         int len, next_tx_seq_offset, req_seq_offset;
4263
4264         control = get_unaligned_le16(skb->data);
4265         skb_pull(skb, 2);
4266         len = skb->len;
4267
4268         /*
4269          * We can just drop the corrupted I-frame here.
4270          * Receiver will miss it and start proper recovery
4271          * procedures and ask retransmission.
4272          */
4273         if (l2cap_check_fcs(pi, skb))
4274                 goto drop;
4275
4276         if (__is_sar_start(control) && __is_iframe(control))
4277                 len -= 2;
4278
4279         if (pi->fcs == L2CAP_FCS_CRC16)
4280                 len -= 2;
4281
4282         if (len > pi->mps) {
4283                 l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
4284                 goto drop;
4285         }
4286
4287         req_seq = __get_reqseq(control);
4288         req_seq_offset = (req_seq - pi->expected_ack_seq) % 64;
4289         if (req_seq_offset < 0)
4290                 req_seq_offset += 64;
4291
4292         next_tx_seq_offset =
4293                 (pi->next_tx_seq - pi->expected_ack_seq) % 64;
4294         if (next_tx_seq_offset < 0)
4295                 next_tx_seq_offset += 64;
4296
4297         /* check for invalid req-seq */
4298         if (req_seq_offset > next_tx_seq_offset) {
4299                 l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
4300                 goto drop;
4301         }
4302
4303         if (__is_iframe(control)) {
4304                 if (len < 0) {
4305                         l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
4306                         goto drop;
4307                 }
4308
4309                 l2cap_data_channel_iframe(sk, control, skb);
4310         } else {
4311                 if (len != 0) {
4312                         BT_ERR("%d", len);
4313                         l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
4314                         goto drop;
4315                 }
4316
4317                 l2cap_data_channel_sframe(sk, control, skb);
4318         }
4319
4320         return 0;
4321
4322 drop:
4323         kfree_skb(skb);
4324         return 0;
4325 }
4326
4327 static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
4328 {
4329         struct sock *sk;
4330         struct l2cap_pinfo *pi;
4331         u16 control;
4332         u8 tx_seq;
4333         int len;
4334
4335         sk = l2cap_get_chan_by_scid(&conn->chan_list, cid);
4336         if (!sk) {
4337                 BT_DBG("unknown cid 0x%4.4x", cid);
4338                 goto drop;
4339         }
4340
4341         pi = l2cap_pi(sk);
4342
4343         BT_DBG("sk %p, len %d", sk, skb->len);
4344
4345         if (sk->sk_state != BT_CONNECTED)
4346                 goto drop;
4347
4348         switch (pi->mode) {
4349         case L2CAP_MODE_BASIC:
4350                 /* If socket recv buffers overflows we drop data here
4351                  * which is *bad* because L2CAP has to be reliable.
4352                  * But we don't have any other choice. L2CAP doesn't
4353                  * provide flow control mechanism. */
4354
4355                 if (pi->imtu < skb->len)
4356                         goto drop;
4357
4358                 if (!sock_queue_rcv_skb(sk, skb))
4359                         goto done;
4360                 break;
4361
4362         case L2CAP_MODE_ERTM:
4363                 if (!sock_owned_by_user(sk)) {
4364                         l2cap_ertm_data_rcv(sk, skb);
4365                 } else {
4366                         if (sk_add_backlog(sk, skb))
4367                                 goto drop;
4368                 }
4369
4370                 goto done;
4371
4372         case L2CAP_MODE_STREAMING:
4373                 control = get_unaligned_le16(skb->data);
4374                 skb_pull(skb, 2);
4375                 len = skb->len;
4376
4377                 if (l2cap_check_fcs(pi, skb))
4378                         goto drop;
4379
4380                 if (__is_sar_start(control))
4381                         len -= 2;
4382
4383                 if (pi->fcs == L2CAP_FCS_CRC16)
4384                         len -= 2;
4385
4386                 if (len > pi->mps || len < 0 || __is_sframe(control))
4387                         goto drop;
4388
4389                 tx_seq = __get_txseq(control);
4390
4391                 if (pi->expected_tx_seq == tx_seq)
4392                         pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
4393                 else
4394                         pi->expected_tx_seq = (tx_seq + 1) % 64;
4395
4396                 l2cap_streaming_reassembly_sdu(sk, skb, control);
4397
4398                 goto done;
4399
4400         default:
4401                 BT_DBG("sk %p: bad mode 0x%2.2x", sk, pi->mode);
4402                 break;
4403         }
4404
4405 drop:
4406         kfree_skb(skb);
4407
4408 done:
4409         if (sk)
4410                 bh_unlock_sock(sk);
4411
4412         return 0;
4413 }
4414
4415 static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb)
4416 {
4417         struct sock *sk;
4418
4419         sk = l2cap_get_sock_by_psm(0, psm, conn->src);
4420         if (!sk)
4421                 goto drop;
4422
4423         BT_DBG("sk %p, len %d", sk, skb->len);
4424
4425         if (sk->sk_state != BT_BOUND && sk->sk_state != BT_CONNECTED)
4426                 goto drop;
4427
4428         if (l2cap_pi(sk)->imtu < skb->len)
4429                 goto drop;
4430
4431         if (!sock_queue_rcv_skb(sk, skb))
4432                 goto done;
4433
4434 drop:
4435         kfree_skb(skb);
4436
4437 done:
4438         if (sk)
4439                 bh_unlock_sock(sk);
4440         return 0;
4441 }
4442
4443 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
4444 {
4445         struct l2cap_hdr *lh = (void *) skb->data;
4446         u16 cid, len;
4447         __le16 psm;
4448
4449         skb_pull(skb, L2CAP_HDR_SIZE);
4450         cid = __le16_to_cpu(lh->cid);
4451         len = __le16_to_cpu(lh->len);
4452
4453         if (len != skb->len) {
4454                 kfree_skb(skb);
4455                 return;
4456         }
4457
4458         BT_DBG("len %d, cid 0x%4.4x", len, cid);
4459
4460         switch (cid) {
4461         case L2CAP_CID_SIGNALING:
4462                 l2cap_sig_channel(conn, skb);
4463                 break;
4464
4465         case L2CAP_CID_CONN_LESS:
4466                 psm = get_unaligned_le16(skb->data);
4467                 skb_pull(skb, 2);
4468                 l2cap_conless_channel(conn, psm, skb);
4469                 break;
4470
4471         default:
4472                 l2cap_data_channel(conn, cid, skb);
4473                 break;
4474         }
4475 }
4476
4477 /* ---- L2CAP interface with lower layer (HCI) ---- */
4478
4479 static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
4480 {
4481         int exact = 0, lm1 = 0, lm2 = 0;
4482         register struct sock *sk;
4483         struct hlist_node *node;
4484
4485         if (type != ACL_LINK)
4486                 return -EINVAL;
4487
4488         BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
4489
4490         /* Find listening sockets and check their link_mode */
4491         read_lock(&l2cap_sk_list.lock);
4492         sk_for_each(sk, node, &l2cap_sk_list.head) {
4493                 if (sk->sk_state != BT_LISTEN)
4494                         continue;
4495
4496                 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
4497                         lm1 |= HCI_LM_ACCEPT;
4498                         if (l2cap_pi(sk)->role_switch)
4499                                 lm1 |= HCI_LM_MASTER;
4500                         exact++;
4501                 } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
4502                         lm2 |= HCI_LM_ACCEPT;
4503                         if (l2cap_pi(sk)->role_switch)
4504                                 lm2 |= HCI_LM_MASTER;
4505                 }
4506         }
4507         read_unlock(&l2cap_sk_list.lock);
4508
4509         return exact ? lm1 : lm2;
4510 }
4511
4512 static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
4513 {
4514         struct l2cap_conn *conn;
4515
4516         BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
4517
4518         if (hcon->type != ACL_LINK)
4519                 return -EINVAL;
4520
4521         if (!status) {
4522                 conn = l2cap_conn_add(hcon, status);
4523                 if (conn)
4524                         l2cap_conn_ready(conn);
4525         } else
4526                 l2cap_conn_del(hcon, bt_err(status));
4527
4528         return 0;
4529 }
4530
4531 static int l2cap_disconn_ind(struct hci_conn *hcon)
4532 {
4533         struct l2cap_conn *conn = hcon->l2cap_data;
4534
4535         BT_DBG("hcon %p", hcon);
4536
4537         if (hcon->type != ACL_LINK || !conn)
4538                 return 0x13;
4539
4540         return conn->disc_reason;
4541 }
4542
4543 static int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
4544 {
4545         BT_DBG("hcon %p reason %d", hcon, reason);
4546
4547         if (hcon->type != ACL_LINK)
4548                 return -EINVAL;
4549
4550         l2cap_conn_del(hcon, bt_err(reason));
4551
4552         return 0;
4553 }
4554
4555 static inline void l2cap_check_encryption(struct sock *sk, u8 encrypt)
4556 {
4557         if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM)
4558                 return;
4559
4560         if (encrypt == 0x00) {
4561                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_MEDIUM) {
4562                         l2cap_sock_clear_timer(sk);
4563                         l2cap_sock_set_timer(sk, HZ * 5);
4564                 } else if (l2cap_pi(sk)->sec_level == BT_SECURITY_HIGH)
4565                         __l2cap_sock_close(sk, ECONNREFUSED);
4566         } else {
4567                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_MEDIUM)
4568                         l2cap_sock_clear_timer(sk);
4569         }
4570 }
4571
4572 static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
4573 {
4574         struct l2cap_chan_list *l;
4575         struct l2cap_conn *conn = hcon->l2cap_data;
4576         struct sock *sk;
4577
4578         if (!conn)
4579                 return 0;
4580
4581         l = &conn->chan_list;
4582
4583         BT_DBG("conn %p", conn);
4584
4585         read_lock(&l->lock);
4586
4587         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
4588                 bh_lock_sock(sk);
4589
4590                 if (l2cap_pi(sk)->conf_state & L2CAP_CONF_CONNECT_PEND) {
4591                         bh_unlock_sock(sk);
4592                         continue;
4593                 }
4594
4595                 if (!status && (sk->sk_state == BT_CONNECTED ||
4596                                                 sk->sk_state == BT_CONFIG)) {
4597                         l2cap_check_encryption(sk, encrypt);
4598                         bh_unlock_sock(sk);
4599                         continue;
4600                 }
4601
4602                 if (sk->sk_state == BT_CONNECT) {
4603                         if (!status) {
4604                                 struct l2cap_conn_req req;
4605                                 req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
4606                                 req.psm  = l2cap_pi(sk)->psm;
4607
4608                                 l2cap_pi(sk)->ident = l2cap_get_ident(conn);
4609                                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND;
4610
4611                                 l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
4612                                         L2CAP_CONN_REQ, sizeof(req), &req);
4613                         } else {
4614                                 l2cap_sock_clear_timer(sk);
4615                                 l2cap_sock_set_timer(sk, HZ / 10);
4616                         }
4617                 } else if (sk->sk_state == BT_CONNECT2) {
4618                         struct l2cap_conn_rsp rsp;
4619                         __u16 result;
4620
4621                         if (!status) {
4622                                 sk->sk_state = BT_CONFIG;
4623                                 result = L2CAP_CR_SUCCESS;
4624                         } else {
4625                                 sk->sk_state = BT_DISCONN;
4626                                 l2cap_sock_set_timer(sk, HZ / 10);
4627                                 result = L2CAP_CR_SEC_BLOCK;
4628                         }
4629
4630                         rsp.scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
4631                         rsp.dcid   = cpu_to_le16(l2cap_pi(sk)->scid);
4632                         rsp.result = cpu_to_le16(result);
4633                         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
4634                         l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
4635                                         L2CAP_CONN_RSP, sizeof(rsp), &rsp);
4636                 }
4637
4638                 bh_unlock_sock(sk);
4639         }
4640
4641         read_unlock(&l->lock);
4642
4643         return 0;
4644 }
4645
4646 static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
4647 {
4648         struct l2cap_conn *conn = hcon->l2cap_data;
4649
4650         if (!conn && !(conn = l2cap_conn_add(hcon, 0)))
4651                 goto drop;
4652
4653         BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
4654
4655         if (flags & ACL_START) {
4656                 struct l2cap_hdr *hdr;
4657                 int len;
4658
4659                 if (conn->rx_len) {
4660                         BT_ERR("Unexpected start frame (len %d)", skb->len);
4661                         kfree_skb(conn->rx_skb);
4662                         conn->rx_skb = NULL;
4663                         conn->rx_len = 0;
4664                         l2cap_conn_unreliable(conn, ECOMM);
4665                 }
4666
4667                 if (skb->len < 2) {
4668                         BT_ERR("Frame is too short (len %d)", skb->len);
4669                         l2cap_conn_unreliable(conn, ECOMM);
4670                         goto drop;
4671                 }
4672
4673                 hdr = (struct l2cap_hdr *) skb->data;
4674                 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
4675
4676                 if (len == skb->len) {
4677                         /* Complete frame received */
4678                         l2cap_recv_frame(conn, skb);
4679                         return 0;
4680                 }
4681
4682                 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
4683
4684                 if (skb->len > len) {
4685                         BT_ERR("Frame is too long (len %d, expected len %d)",
4686                                 skb->len, len);
4687                         l2cap_conn_unreliable(conn, ECOMM);
4688                         goto drop;
4689                 }
4690
4691                 /* Allocate skb for the complete frame (with header) */
4692                 conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC);
4693                 if (!conn->rx_skb)
4694                         goto drop;
4695
4696                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
4697                                                                 skb->len);
4698                 conn->rx_len = len - skb->len;
4699         } else {
4700                 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
4701
4702                 if (!conn->rx_len) {
4703                         BT_ERR("Unexpected continuation frame (len %d)", skb->len);
4704                         l2cap_conn_unreliable(conn, ECOMM);
4705                         goto drop;
4706                 }
4707
4708                 if (skb->len > conn->rx_len) {
4709                         BT_ERR("Fragment is too long (len %d, expected %d)",
4710                                         skb->len, conn->rx_len);
4711                         kfree_skb(conn->rx_skb);
4712                         conn->rx_skb = NULL;
4713                         conn->rx_len = 0;
4714                         l2cap_conn_unreliable(conn, ECOMM);
4715                         goto drop;
4716                 }
4717
4718                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
4719                                                                 skb->len);
4720                 conn->rx_len -= skb->len;
4721
4722                 if (!conn->rx_len) {
4723                         /* Complete frame received */
4724                         l2cap_recv_frame(conn, conn->rx_skb);
4725                         conn->rx_skb = NULL;
4726                 }
4727         }
4728
4729 drop:
4730         kfree_skb(skb);
4731         return 0;
4732 }
4733
4734 static int l2cap_debugfs_show(struct seq_file *f, void *p)
4735 {
4736         struct sock *sk;
4737         struct hlist_node *node;
4738
4739         read_lock_bh(&l2cap_sk_list.lock);
4740
4741         sk_for_each(sk, node, &l2cap_sk_list.head) {
4742                 struct l2cap_pinfo *pi = l2cap_pi(sk);
4743
4744                 seq_printf(f, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d\n",
4745                                         batostr(&bt_sk(sk)->src),
4746                                         batostr(&bt_sk(sk)->dst),
4747                                         sk->sk_state, __le16_to_cpu(pi->psm),
4748                                         pi->scid, pi->dcid,
4749                                         pi->imtu, pi->omtu, pi->sec_level);
4750         }
4751
4752         read_unlock_bh(&l2cap_sk_list.lock);
4753
4754         return 0;
4755 }
4756
4757 static int l2cap_debugfs_open(struct inode *inode, struct file *file)
4758 {
4759         return single_open(file, l2cap_debugfs_show, inode->i_private);
4760 }
4761
4762 static const struct file_operations l2cap_debugfs_fops = {
4763         .open           = l2cap_debugfs_open,
4764         .read           = seq_read,
4765         .llseek         = seq_lseek,
4766         .release        = single_release,
4767 };
4768
4769 static struct dentry *l2cap_debugfs;
4770
4771 static const struct proto_ops l2cap_sock_ops = {
4772         .family         = PF_BLUETOOTH,
4773         .owner          = THIS_MODULE,
4774         .release        = l2cap_sock_release,
4775         .bind           = l2cap_sock_bind,
4776         .connect        = l2cap_sock_connect,
4777         .listen         = l2cap_sock_listen,
4778         .accept         = l2cap_sock_accept,
4779         .getname        = l2cap_sock_getname,
4780         .sendmsg        = l2cap_sock_sendmsg,
4781         .recvmsg        = l2cap_sock_recvmsg,
4782         .poll           = bt_sock_poll,
4783         .ioctl          = bt_sock_ioctl,
4784         .mmap           = sock_no_mmap,
4785         .socketpair     = sock_no_socketpair,
4786         .shutdown       = l2cap_sock_shutdown,
4787         .setsockopt     = l2cap_sock_setsockopt,
4788         .getsockopt     = l2cap_sock_getsockopt
4789 };
4790
4791 static const struct net_proto_family l2cap_sock_family_ops = {
4792         .family = PF_BLUETOOTH,
4793         .owner  = THIS_MODULE,
4794         .create = l2cap_sock_create,
4795 };
4796
4797 static struct hci_proto l2cap_hci_proto = {
4798         .name           = "L2CAP",
4799         .id             = HCI_PROTO_L2CAP,
4800         .connect_ind    = l2cap_connect_ind,
4801         .connect_cfm    = l2cap_connect_cfm,
4802         .disconn_ind    = l2cap_disconn_ind,
4803         .disconn_cfm    = l2cap_disconn_cfm,
4804         .security_cfm   = l2cap_security_cfm,
4805         .recv_acldata   = l2cap_recv_acldata
4806 };
4807
4808 static int __init l2cap_init(void)
4809 {
4810         int err;
4811
4812         err = proto_register(&l2cap_proto, 0);
4813         if (err < 0)
4814                 return err;
4815
4816         _busy_wq = create_singlethread_workqueue("l2cap");
4817         if (!_busy_wq)
4818                 goto error;
4819
4820         err = bt_sock_register(BTPROTO_L2CAP, &l2cap_sock_family_ops);
4821         if (err < 0) {
4822                 BT_ERR("L2CAP socket registration failed");
4823                 goto error;
4824         }
4825
4826         err = hci_register_proto(&l2cap_hci_proto);
4827         if (err < 0) {
4828                 BT_ERR("L2CAP protocol registration failed");
4829                 bt_sock_unregister(BTPROTO_L2CAP);
4830                 goto error;
4831         }
4832
4833         if (bt_debugfs) {
4834                 l2cap_debugfs = debugfs_create_file("l2cap", 0444,
4835                                         bt_debugfs, NULL, &l2cap_debugfs_fops);
4836                 if (!l2cap_debugfs)
4837                         BT_ERR("Failed to create L2CAP debug file");
4838         }
4839
4840         BT_INFO("L2CAP ver %s", VERSION);
4841         BT_INFO("L2CAP socket layer initialized");
4842
4843         return 0;
4844
4845 error:
4846         proto_unregister(&l2cap_proto);
4847         return err;
4848 }
4849
4850 static void __exit l2cap_exit(void)
4851 {
4852         debugfs_remove(l2cap_debugfs);
4853
4854         flush_workqueue(_busy_wq);
4855         destroy_workqueue(_busy_wq);
4856
4857         if (bt_sock_unregister(BTPROTO_L2CAP) < 0)
4858                 BT_ERR("L2CAP socket unregistration failed");
4859
4860         if (hci_unregister_proto(&l2cap_hci_proto) < 0)
4861                 BT_ERR("L2CAP protocol unregistration failed");
4862
4863         proto_unregister(&l2cap_proto);
4864 }
4865
4866 void l2cap_load(void)
4867 {
4868         /* Dummy function to trigger automatic L2CAP module loading by
4869          * other modules that use L2CAP sockets but don't use any other
4870          * symbols from it. */
4871 }
4872 EXPORT_SYMBOL(l2cap_load);
4873
4874 module_init(l2cap_init);
4875 module_exit(l2cap_exit);
4876
4877 module_param(disable_ertm, bool, 0644);
4878 MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode");
4879
4880 MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
4881 MODULE_DESCRIPTION("Bluetooth L2CAP ver " VERSION);
4882 MODULE_VERSION(VERSION);
4883 MODULE_LICENSE("GPL");
4884 MODULE_ALIAS("bt-proto-0");