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