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