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 -EFAULT;
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(-ENOMEM);
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(-ENOMEM);
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(-ENOMEM);
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         return bt_sock_recvmsg(iocb, sock, msg, len, flags);
1955 }
1956
1957 static int l2cap_sock_setsockopt_old(struct socket *sock, int optname, char __user *optval, unsigned int optlen)
1958 {
1959         struct sock *sk = sock->sk;
1960         struct l2cap_options opts;
1961         int len, err = 0;
1962         u32 opt;
1963
1964         BT_DBG("sk %p", sk);
1965
1966         lock_sock(sk);
1967
1968         switch (optname) {
1969         case L2CAP_OPTIONS:
1970                 if (sk->sk_state == BT_CONNECTED) {
1971                         err = -EINVAL;
1972                         break;
1973                 }
1974
1975                 opts.imtu     = l2cap_pi(sk)->imtu;
1976                 opts.omtu     = l2cap_pi(sk)->omtu;
1977                 opts.flush_to = l2cap_pi(sk)->flush_to;
1978                 opts.mode     = l2cap_pi(sk)->mode;
1979                 opts.fcs      = l2cap_pi(sk)->fcs;
1980                 opts.max_tx   = l2cap_pi(sk)->max_tx;
1981                 opts.txwin_size = (__u16)l2cap_pi(sk)->tx_win;
1982
1983                 len = min_t(unsigned int, sizeof(opts), optlen);
1984                 if (copy_from_user((char *) &opts, optval, len)) {
1985                         err = -EFAULT;
1986                         break;
1987                 }
1988
1989                 if (opts.txwin_size > L2CAP_DEFAULT_TX_WINDOW) {
1990                         err = -EINVAL;
1991                         break;
1992                 }
1993
1994                 l2cap_pi(sk)->mode = opts.mode;
1995                 switch (l2cap_pi(sk)->mode) {
1996                 case L2CAP_MODE_BASIC:
1997                         l2cap_pi(sk)->conf_state &= ~L2CAP_CONF_STATE2_DEVICE;
1998                         break;
1999                 case L2CAP_MODE_ERTM:
2000                 case L2CAP_MODE_STREAMING:
2001                         if (!disable_ertm)
2002                                 break;
2003                         /* fall through */
2004                 default:
2005                         err = -EINVAL;
2006                         break;
2007                 }
2008
2009                 l2cap_pi(sk)->imtu = opts.imtu;
2010                 l2cap_pi(sk)->omtu = opts.omtu;
2011                 l2cap_pi(sk)->fcs  = opts.fcs;
2012                 l2cap_pi(sk)->max_tx = opts.max_tx;
2013                 l2cap_pi(sk)->tx_win = (__u8)opts.txwin_size;
2014                 break;
2015
2016         case L2CAP_LM:
2017                 if (get_user(opt, (u32 __user *) optval)) {
2018                         err = -EFAULT;
2019                         break;
2020                 }
2021
2022                 if (opt & L2CAP_LM_AUTH)
2023                         l2cap_pi(sk)->sec_level = BT_SECURITY_LOW;
2024                 if (opt & L2CAP_LM_ENCRYPT)
2025                         l2cap_pi(sk)->sec_level = BT_SECURITY_MEDIUM;
2026                 if (opt & L2CAP_LM_SECURE)
2027                         l2cap_pi(sk)->sec_level = BT_SECURITY_HIGH;
2028
2029                 l2cap_pi(sk)->role_switch    = (opt & L2CAP_LM_MASTER);
2030                 l2cap_pi(sk)->force_reliable = (opt & L2CAP_LM_RELIABLE);
2031                 break;
2032
2033         default:
2034                 err = -ENOPROTOOPT;
2035                 break;
2036         }
2037
2038         release_sock(sk);
2039         return err;
2040 }
2041
2042 static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname, char __user *optval, unsigned int optlen)
2043 {
2044         struct sock *sk = sock->sk;
2045         struct bt_security sec;
2046         int len, err = 0;
2047         u32 opt;
2048
2049         BT_DBG("sk %p", sk);
2050
2051         if (level == SOL_L2CAP)
2052                 return l2cap_sock_setsockopt_old(sock, optname, optval, optlen);
2053
2054         if (level != SOL_BLUETOOTH)
2055                 return -ENOPROTOOPT;
2056
2057         lock_sock(sk);
2058
2059         switch (optname) {
2060         case BT_SECURITY:
2061                 if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM
2062                                 && sk->sk_type != SOCK_RAW) {
2063                         err = -EINVAL;
2064                         break;
2065                 }
2066
2067                 sec.level = BT_SECURITY_LOW;
2068
2069                 len = min_t(unsigned int, sizeof(sec), optlen);
2070                 if (copy_from_user((char *) &sec, optval, len)) {
2071                         err = -EFAULT;
2072                         break;
2073                 }
2074
2075                 if (sec.level < BT_SECURITY_LOW ||
2076                                         sec.level > BT_SECURITY_HIGH) {
2077                         err = -EINVAL;
2078                         break;
2079                 }
2080
2081                 l2cap_pi(sk)->sec_level = sec.level;
2082                 break;
2083
2084         case BT_DEFER_SETUP:
2085                 if (sk->sk_state != BT_BOUND && sk->sk_state != BT_LISTEN) {
2086                         err = -EINVAL;
2087                         break;
2088                 }
2089
2090                 if (get_user(opt, (u32 __user *) optval)) {
2091                         err = -EFAULT;
2092                         break;
2093                 }
2094
2095                 bt_sk(sk)->defer_setup = opt;
2096                 break;
2097
2098         default:
2099                 err = -ENOPROTOOPT;
2100                 break;
2101         }
2102
2103         release_sock(sk);
2104         return err;
2105 }
2106
2107 static int l2cap_sock_getsockopt_old(struct socket *sock, int optname, char __user *optval, int __user *optlen)
2108 {
2109         struct sock *sk = sock->sk;
2110         struct l2cap_options opts;
2111         struct l2cap_conninfo cinfo;
2112         int len, err = 0;
2113         u32 opt;
2114
2115         BT_DBG("sk %p", sk);
2116
2117         if (get_user(len, optlen))
2118                 return -EFAULT;
2119
2120         lock_sock(sk);
2121
2122         switch (optname) {
2123         case L2CAP_OPTIONS:
2124                 opts.imtu     = l2cap_pi(sk)->imtu;
2125                 opts.omtu     = l2cap_pi(sk)->omtu;
2126                 opts.flush_to = l2cap_pi(sk)->flush_to;
2127                 opts.mode     = l2cap_pi(sk)->mode;
2128                 opts.fcs      = l2cap_pi(sk)->fcs;
2129                 opts.max_tx   = l2cap_pi(sk)->max_tx;
2130                 opts.txwin_size = (__u16)l2cap_pi(sk)->tx_win;
2131
2132                 len = min_t(unsigned int, len, sizeof(opts));
2133                 if (copy_to_user(optval, (char *) &opts, len))
2134                         err = -EFAULT;
2135
2136                 break;
2137
2138         case L2CAP_LM:
2139                 switch (l2cap_pi(sk)->sec_level) {
2140                 case BT_SECURITY_LOW:
2141                         opt = L2CAP_LM_AUTH;
2142                         break;
2143                 case BT_SECURITY_MEDIUM:
2144                         opt = L2CAP_LM_AUTH | L2CAP_LM_ENCRYPT;
2145                         break;
2146                 case BT_SECURITY_HIGH:
2147                         opt = L2CAP_LM_AUTH | L2CAP_LM_ENCRYPT |
2148                                                         L2CAP_LM_SECURE;
2149                         break;
2150                 default:
2151                         opt = 0;
2152                         break;
2153                 }
2154
2155                 if (l2cap_pi(sk)->role_switch)
2156                         opt |= L2CAP_LM_MASTER;
2157
2158                 if (l2cap_pi(sk)->force_reliable)
2159                         opt |= L2CAP_LM_RELIABLE;
2160
2161                 if (put_user(opt, (u32 __user *) optval))
2162                         err = -EFAULT;
2163                 break;
2164
2165         case L2CAP_CONNINFO:
2166                 if (sk->sk_state != BT_CONNECTED &&
2167                                         !(sk->sk_state == BT_CONNECT2 &&
2168                                                 bt_sk(sk)->defer_setup)) {
2169                         err = -ENOTCONN;
2170                         break;
2171                 }
2172
2173                 cinfo.hci_handle = l2cap_pi(sk)->conn->hcon->handle;
2174                 memcpy(cinfo.dev_class, l2cap_pi(sk)->conn->hcon->dev_class, 3);
2175
2176                 len = min_t(unsigned int, len, sizeof(cinfo));
2177                 if (copy_to_user(optval, (char *) &cinfo, len))
2178                         err = -EFAULT;
2179
2180                 break;
2181
2182         default:
2183                 err = -ENOPROTOOPT;
2184                 break;
2185         }
2186
2187         release_sock(sk);
2188         return err;
2189 }
2190
2191 static int l2cap_sock_getsockopt(struct socket *sock, int level, int optname, char __user *optval, int __user *optlen)
2192 {
2193         struct sock *sk = sock->sk;
2194         struct bt_security sec;
2195         int len, err = 0;
2196
2197         BT_DBG("sk %p", sk);
2198
2199         if (level == SOL_L2CAP)
2200                 return l2cap_sock_getsockopt_old(sock, optname, optval, optlen);
2201
2202         if (level != SOL_BLUETOOTH)
2203                 return -ENOPROTOOPT;
2204
2205         if (get_user(len, optlen))
2206                 return -EFAULT;
2207
2208         lock_sock(sk);
2209
2210         switch (optname) {
2211         case BT_SECURITY:
2212                 if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM
2213                                 && sk->sk_type != SOCK_RAW) {
2214                         err = -EINVAL;
2215                         break;
2216                 }
2217
2218                 sec.level = l2cap_pi(sk)->sec_level;
2219
2220                 len = min_t(unsigned int, len, sizeof(sec));
2221                 if (copy_to_user(optval, (char *) &sec, len))
2222                         err = -EFAULT;
2223
2224                 break;
2225
2226         case BT_DEFER_SETUP:
2227                 if (sk->sk_state != BT_BOUND && sk->sk_state != BT_LISTEN) {
2228                         err = -EINVAL;
2229                         break;
2230                 }
2231
2232                 if (put_user(bt_sk(sk)->defer_setup, (u32 __user *) optval))
2233                         err = -EFAULT;
2234
2235                 break;
2236
2237         default:
2238                 err = -ENOPROTOOPT;
2239                 break;
2240         }
2241
2242         release_sock(sk);
2243         return err;
2244 }
2245
2246 static int l2cap_sock_shutdown(struct socket *sock, int how)
2247 {
2248         struct sock *sk = sock->sk;
2249         int err = 0;
2250
2251         BT_DBG("sock %p, sk %p", sock, sk);
2252
2253         if (!sk)
2254                 return 0;
2255
2256         lock_sock(sk);
2257         if (!sk->sk_shutdown) {
2258                 if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM)
2259                         err = __l2cap_wait_ack(sk);
2260
2261                 sk->sk_shutdown = SHUTDOWN_MASK;
2262                 l2cap_sock_clear_timer(sk);
2263                 __l2cap_sock_close(sk, 0);
2264
2265                 if (sock_flag(sk, SOCK_LINGER) && sk->sk_lingertime)
2266                         err = bt_sock_wait_state(sk, BT_CLOSED,
2267                                                         sk->sk_lingertime);
2268         }
2269
2270         if (!err && sk->sk_err)
2271                 err = -sk->sk_err;
2272
2273         release_sock(sk);
2274         return err;
2275 }
2276
2277 static int l2cap_sock_release(struct socket *sock)
2278 {
2279         struct sock *sk = sock->sk;
2280         int err;
2281
2282         BT_DBG("sock %p, sk %p", sock, sk);
2283
2284         if (!sk)
2285                 return 0;
2286
2287         err = l2cap_sock_shutdown(sock, 2);
2288
2289         sock_orphan(sk);
2290         l2cap_sock_kill(sk);
2291         return err;
2292 }
2293
2294 static void l2cap_chan_ready(struct sock *sk)
2295 {
2296         struct sock *parent = bt_sk(sk)->parent;
2297
2298         BT_DBG("sk %p, parent %p", sk, parent);
2299
2300         l2cap_pi(sk)->conf_state = 0;
2301         l2cap_sock_clear_timer(sk);
2302
2303         if (!parent) {
2304                 /* Outgoing channel.
2305                  * Wake up socket sleeping on connect.
2306                  */
2307                 sk->sk_state = BT_CONNECTED;
2308                 sk->sk_state_change(sk);
2309         } else {
2310                 /* Incoming channel.
2311                  * Wake up socket sleeping on accept.
2312                  */
2313                 parent->sk_data_ready(parent, 0);
2314         }
2315 }
2316
2317 /* Copy frame to all raw sockets on that connection */
2318 static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
2319 {
2320         struct l2cap_chan_list *l = &conn->chan_list;
2321         struct sk_buff *nskb;
2322         struct sock *sk;
2323
2324         BT_DBG("conn %p", conn);
2325
2326         read_lock(&l->lock);
2327         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
2328                 if (sk->sk_type != SOCK_RAW)
2329                         continue;
2330
2331                 /* Don't send frame to the socket it came from */
2332                 if (skb->sk == sk)
2333                         continue;
2334                 nskb = skb_clone(skb, GFP_ATOMIC);
2335                 if (!nskb)
2336                         continue;
2337
2338                 if (sock_queue_rcv_skb(sk, nskb))
2339                         kfree_skb(nskb);
2340         }
2341         read_unlock(&l->lock);
2342 }
2343
2344 /* ---- L2CAP signalling commands ---- */
2345 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
2346                                 u8 code, u8 ident, u16 dlen, void *data)
2347 {
2348         struct sk_buff *skb, **frag;
2349         struct l2cap_cmd_hdr *cmd;
2350         struct l2cap_hdr *lh;
2351         int len, count;
2352
2353         BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %d",
2354                         conn, code, ident, dlen);
2355
2356         len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen;
2357         count = min_t(unsigned int, conn->mtu, len);
2358
2359         skb = bt_skb_alloc(count, GFP_ATOMIC);
2360         if (!skb)
2361                 return NULL;
2362
2363         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
2364         lh->len = cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen);
2365         lh->cid = cpu_to_le16(L2CAP_CID_SIGNALING);
2366
2367         cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE);
2368         cmd->code  = code;
2369         cmd->ident = ident;
2370         cmd->len   = cpu_to_le16(dlen);
2371
2372         if (dlen) {
2373                 count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE;
2374                 memcpy(skb_put(skb, count), data, count);
2375                 data += count;
2376         }
2377
2378         len -= skb->len;
2379
2380         /* Continuation fragments (no L2CAP header) */
2381         frag = &skb_shinfo(skb)->frag_list;
2382         while (len) {
2383                 count = min_t(unsigned int, conn->mtu, len);
2384
2385                 *frag = bt_skb_alloc(count, GFP_ATOMIC);
2386                 if (!*frag)
2387                         goto fail;
2388
2389                 memcpy(skb_put(*frag, count), data, count);
2390
2391                 len  -= count;
2392                 data += count;
2393
2394                 frag = &(*frag)->next;
2395         }
2396
2397         return skb;
2398
2399 fail:
2400         kfree_skb(skb);
2401         return NULL;
2402 }
2403
2404 static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val)
2405 {
2406         struct l2cap_conf_opt *opt = *ptr;
2407         int len;
2408
2409         len = L2CAP_CONF_OPT_SIZE + opt->len;
2410         *ptr += len;
2411
2412         *type = opt->type;
2413         *olen = opt->len;
2414
2415         switch (opt->len) {
2416         case 1:
2417                 *val = *((u8 *) opt->val);
2418                 break;
2419
2420         case 2:
2421                 *val = __le16_to_cpu(*((__le16 *) opt->val));
2422                 break;
2423
2424         case 4:
2425                 *val = __le32_to_cpu(*((__le32 *) opt->val));
2426                 break;
2427
2428         default:
2429                 *val = (unsigned long) opt->val;
2430                 break;
2431         }
2432
2433         BT_DBG("type 0x%2.2x len %d val 0x%lx", *type, opt->len, *val);
2434         return len;
2435 }
2436
2437 static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val)
2438 {
2439         struct l2cap_conf_opt *opt = *ptr;
2440
2441         BT_DBG("type 0x%2.2x len %d val 0x%lx", type, len, val);
2442
2443         opt->type = type;
2444         opt->len  = len;
2445
2446         switch (len) {
2447         case 1:
2448                 *((u8 *) opt->val)  = val;
2449                 break;
2450
2451         case 2:
2452                 *((__le16 *) opt->val) = cpu_to_le16(val);
2453                 break;
2454
2455         case 4:
2456                 *((__le32 *) opt->val) = cpu_to_le32(val);
2457                 break;
2458
2459         default:
2460                 memcpy(opt->val, (void *) val, len);
2461                 break;
2462         }
2463
2464         *ptr += L2CAP_CONF_OPT_SIZE + len;
2465 }
2466
2467 static void l2cap_ack_timeout(unsigned long arg)
2468 {
2469         struct sock *sk = (void *) arg;
2470
2471         bh_lock_sock(sk);
2472         l2cap_send_ack(l2cap_pi(sk));
2473         bh_unlock_sock(sk);
2474 }
2475
2476 static inline void l2cap_ertm_init(struct sock *sk)
2477 {
2478         l2cap_pi(sk)->expected_ack_seq = 0;
2479         l2cap_pi(sk)->unacked_frames = 0;
2480         l2cap_pi(sk)->buffer_seq = 0;
2481         l2cap_pi(sk)->num_acked = 0;
2482         l2cap_pi(sk)->frames_sent = 0;
2483
2484         setup_timer(&l2cap_pi(sk)->retrans_timer,
2485                         l2cap_retrans_timeout, (unsigned long) sk);
2486         setup_timer(&l2cap_pi(sk)->monitor_timer,
2487                         l2cap_monitor_timeout, (unsigned long) sk);
2488         setup_timer(&l2cap_pi(sk)->ack_timer,
2489                         l2cap_ack_timeout, (unsigned long) sk);
2490
2491         __skb_queue_head_init(SREJ_QUEUE(sk));
2492         __skb_queue_head_init(BUSY_QUEUE(sk));
2493
2494         INIT_WORK(&l2cap_pi(sk)->busy_work, l2cap_busy_work);
2495
2496         sk->sk_backlog_rcv = l2cap_ertm_data_rcv;
2497 }
2498
2499 static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
2500 {
2501         switch (mode) {
2502         case L2CAP_MODE_STREAMING:
2503         case L2CAP_MODE_ERTM:
2504                 if (l2cap_mode_supported(mode, remote_feat_mask))
2505                         return mode;
2506                 /* fall through */
2507         default:
2508                 return L2CAP_MODE_BASIC;
2509         }
2510 }
2511
2512 static int l2cap_build_conf_req(struct sock *sk, void *data)
2513 {
2514         struct l2cap_pinfo *pi = l2cap_pi(sk);
2515         struct l2cap_conf_req *req = data;
2516         struct l2cap_conf_rfc rfc = { .mode = pi->mode };
2517         void *ptr = req->data;
2518
2519         BT_DBG("sk %p", sk);
2520
2521         if (pi->num_conf_req || pi->num_conf_rsp)
2522                 goto done;
2523
2524         switch (pi->mode) {
2525         case L2CAP_MODE_STREAMING:
2526         case L2CAP_MODE_ERTM:
2527                 if (pi->conf_state & L2CAP_CONF_STATE2_DEVICE)
2528                         break;
2529
2530                 /* fall through */
2531         default:
2532                 pi->mode = l2cap_select_mode(rfc.mode, pi->conn->feat_mask);
2533                 break;
2534         }
2535
2536 done:
2537         switch (pi->mode) {
2538         case L2CAP_MODE_BASIC:
2539                 if (pi->imtu != L2CAP_DEFAULT_MTU)
2540                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->imtu);
2541
2542                 if (!(pi->conn->feat_mask & L2CAP_FEAT_ERTM) &&
2543                                 !(pi->conn->feat_mask & L2CAP_FEAT_STREAMING))
2544                         break;
2545
2546                 rfc.mode            = L2CAP_MODE_BASIC;
2547                 rfc.txwin_size      = 0;
2548                 rfc.max_transmit    = 0;
2549                 rfc.retrans_timeout = 0;
2550                 rfc.monitor_timeout = 0;
2551                 rfc.max_pdu_size    = 0;
2552
2553                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
2554                                                         (unsigned long) &rfc);
2555                 break;
2556
2557         case L2CAP_MODE_ERTM:
2558                 rfc.mode            = L2CAP_MODE_ERTM;
2559                 rfc.txwin_size      = pi->tx_win;
2560                 rfc.max_transmit    = pi->max_tx;
2561                 rfc.retrans_timeout = 0;
2562                 rfc.monitor_timeout = 0;
2563                 rfc.max_pdu_size    = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE);
2564                 if (L2CAP_DEFAULT_MAX_PDU_SIZE > pi->conn->mtu - 10)
2565                         rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10);
2566
2567                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
2568                                                         (unsigned long) &rfc);
2569
2570                 if (!(pi->conn->feat_mask & L2CAP_FEAT_FCS))
2571                         break;
2572
2573                 if (pi->fcs == L2CAP_FCS_NONE ||
2574                                 pi->conf_state & L2CAP_CONF_NO_FCS_RECV) {
2575                         pi->fcs = L2CAP_FCS_NONE;
2576                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, pi->fcs);
2577                 }
2578                 break;
2579
2580         case L2CAP_MODE_STREAMING:
2581                 rfc.mode            = L2CAP_MODE_STREAMING;
2582                 rfc.txwin_size      = 0;
2583                 rfc.max_transmit    = 0;
2584                 rfc.retrans_timeout = 0;
2585                 rfc.monitor_timeout = 0;
2586                 rfc.max_pdu_size    = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE);
2587                 if (L2CAP_DEFAULT_MAX_PDU_SIZE > pi->conn->mtu - 10)
2588                         rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10);
2589
2590                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
2591                                                         (unsigned long) &rfc);
2592
2593                 if (!(pi->conn->feat_mask & L2CAP_FEAT_FCS))
2594                         break;
2595
2596                 if (pi->fcs == L2CAP_FCS_NONE ||
2597                                 pi->conf_state & L2CAP_CONF_NO_FCS_RECV) {
2598                         pi->fcs = L2CAP_FCS_NONE;
2599                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, pi->fcs);
2600                 }
2601                 break;
2602         }
2603
2604         /* FIXME: Need actual value of the flush timeout */
2605         //if (flush_to != L2CAP_DEFAULT_FLUSH_TO)
2606         //   l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO, 2, pi->flush_to);
2607
2608         req->dcid  = cpu_to_le16(pi->dcid);
2609         req->flags = cpu_to_le16(0);
2610
2611         return ptr - data;
2612 }
2613
2614 static int l2cap_parse_conf_req(struct sock *sk, void *data)
2615 {
2616         struct l2cap_pinfo *pi = l2cap_pi(sk);
2617         struct l2cap_conf_rsp *rsp = data;
2618         void *ptr = rsp->data;
2619         void *req = pi->conf_req;
2620         int len = pi->conf_len;
2621         int type, hint, olen;
2622         unsigned long val;
2623         struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
2624         u16 mtu = L2CAP_DEFAULT_MTU;
2625         u16 result = L2CAP_CONF_SUCCESS;
2626
2627         BT_DBG("sk %p", sk);
2628
2629         while (len >= L2CAP_CONF_OPT_SIZE) {
2630                 len -= l2cap_get_conf_opt(&req, &type, &olen, &val);
2631
2632                 hint  = type & L2CAP_CONF_HINT;
2633                 type &= L2CAP_CONF_MASK;
2634
2635                 switch (type) {
2636                 case L2CAP_CONF_MTU:
2637                         mtu = val;
2638                         break;
2639
2640                 case L2CAP_CONF_FLUSH_TO:
2641                         pi->flush_to = val;
2642                         break;
2643
2644                 case L2CAP_CONF_QOS:
2645                         break;
2646
2647                 case L2CAP_CONF_RFC:
2648                         if (olen == sizeof(rfc))
2649                                 memcpy(&rfc, (void *) val, olen);
2650                         break;
2651
2652                 case L2CAP_CONF_FCS:
2653                         if (val == L2CAP_FCS_NONE)
2654                                 pi->conf_state |= L2CAP_CONF_NO_FCS_RECV;
2655
2656                         break;
2657
2658                 default:
2659                         if (hint)
2660                                 break;
2661
2662                         result = L2CAP_CONF_UNKNOWN;
2663                         *((u8 *) ptr++) = type;
2664                         break;
2665                 }
2666         }
2667
2668         if (pi->num_conf_rsp || pi->num_conf_req > 1)
2669                 goto done;
2670
2671         switch (pi->mode) {
2672         case L2CAP_MODE_STREAMING:
2673         case L2CAP_MODE_ERTM:
2674                 if (!(pi->conf_state & L2CAP_CONF_STATE2_DEVICE)) {
2675                         pi->mode = l2cap_select_mode(rfc.mode,
2676                                         pi->conn->feat_mask);
2677                         break;
2678                 }
2679
2680                 if (pi->mode != rfc.mode)
2681                         return -ECONNREFUSED;
2682
2683                 break;
2684         }
2685
2686 done:
2687         if (pi->mode != rfc.mode) {
2688                 result = L2CAP_CONF_UNACCEPT;
2689                 rfc.mode = pi->mode;
2690
2691                 if (pi->num_conf_rsp == 1)
2692                         return -ECONNREFUSED;
2693
2694                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2695                                         sizeof(rfc), (unsigned long) &rfc);
2696         }
2697
2698
2699         if (result == L2CAP_CONF_SUCCESS) {
2700                 /* Configure output options and let the other side know
2701                  * which ones we don't like. */
2702
2703                 if (mtu < L2CAP_DEFAULT_MIN_MTU)
2704                         result = L2CAP_CONF_UNACCEPT;
2705                 else {
2706                         pi->omtu = mtu;
2707                         pi->conf_state |= L2CAP_CONF_MTU_DONE;
2708                 }
2709                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->omtu);
2710
2711                 switch (rfc.mode) {
2712                 case L2CAP_MODE_BASIC:
2713                         pi->fcs = L2CAP_FCS_NONE;
2714                         pi->conf_state |= L2CAP_CONF_MODE_DONE;
2715                         break;
2716
2717                 case L2CAP_MODE_ERTM:
2718                         pi->remote_tx_win = rfc.txwin_size;
2719                         pi->remote_max_tx = rfc.max_transmit;
2720
2721                         if (le16_to_cpu(rfc.max_pdu_size) > pi->conn->mtu - 10)
2722                                 rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10);
2723
2724                         pi->remote_mps = le16_to_cpu(rfc.max_pdu_size);
2725
2726                         rfc.retrans_timeout =
2727                                 le16_to_cpu(L2CAP_DEFAULT_RETRANS_TO);
2728                         rfc.monitor_timeout =
2729                                 le16_to_cpu(L2CAP_DEFAULT_MONITOR_TO);
2730
2731                         pi->conf_state |= L2CAP_CONF_MODE_DONE;
2732
2733                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2734                                         sizeof(rfc), (unsigned long) &rfc);
2735
2736                         break;
2737
2738                 case L2CAP_MODE_STREAMING:
2739                         if (le16_to_cpu(rfc.max_pdu_size) > pi->conn->mtu - 10)
2740                                 rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10);
2741
2742                         pi->remote_mps = le16_to_cpu(rfc.max_pdu_size);
2743
2744                         pi->conf_state |= L2CAP_CONF_MODE_DONE;
2745
2746                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2747                                         sizeof(rfc), (unsigned long) &rfc);
2748
2749                         break;
2750
2751                 default:
2752                         result = L2CAP_CONF_UNACCEPT;
2753
2754                         memset(&rfc, 0, sizeof(rfc));
2755                         rfc.mode = pi->mode;
2756                 }
2757
2758                 if (result == L2CAP_CONF_SUCCESS)
2759                         pi->conf_state |= L2CAP_CONF_OUTPUT_DONE;
2760         }
2761         rsp->scid   = cpu_to_le16(pi->dcid);
2762         rsp->result = cpu_to_le16(result);
2763         rsp->flags  = cpu_to_le16(0x0000);
2764
2765         return ptr - data;
2766 }
2767
2768 static int l2cap_parse_conf_rsp(struct sock *sk, void *rsp, int len, void *data, u16 *result)
2769 {
2770         struct l2cap_pinfo *pi = l2cap_pi(sk);
2771         struct l2cap_conf_req *req = data;
2772         void *ptr = req->data;
2773         int type, olen;
2774         unsigned long val;
2775         struct l2cap_conf_rfc rfc;
2776
2777         BT_DBG("sk %p, rsp %p, len %d, req %p", sk, rsp, len, data);
2778
2779         while (len >= L2CAP_CONF_OPT_SIZE) {
2780                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
2781
2782                 switch (type) {
2783                 case L2CAP_CONF_MTU:
2784                         if (val < L2CAP_DEFAULT_MIN_MTU) {
2785                                 *result = L2CAP_CONF_UNACCEPT;
2786                                 pi->imtu = L2CAP_DEFAULT_MIN_MTU;
2787                         } else
2788                                 pi->imtu = val;
2789                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->imtu);
2790                         break;
2791
2792                 case L2CAP_CONF_FLUSH_TO:
2793                         pi->flush_to = val;
2794                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
2795                                                         2, pi->flush_to);
2796                         break;
2797
2798                 case L2CAP_CONF_RFC:
2799                         if (olen == sizeof(rfc))
2800                                 memcpy(&rfc, (void *)val, olen);
2801
2802                         if ((pi->conf_state & L2CAP_CONF_STATE2_DEVICE) &&
2803                                                         rfc.mode != pi->mode)
2804                                 return -ECONNREFUSED;
2805
2806                         pi->fcs = 0;
2807
2808                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2809                                         sizeof(rfc), (unsigned long) &rfc);
2810                         break;
2811                 }
2812         }
2813
2814         if (pi->mode == L2CAP_MODE_BASIC && pi->mode != rfc.mode)
2815                 return -ECONNREFUSED;
2816
2817         pi->mode = rfc.mode;
2818
2819         if (*result == L2CAP_CONF_SUCCESS) {
2820                 switch (rfc.mode) {
2821                 case L2CAP_MODE_ERTM:
2822                         pi->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
2823                         pi->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
2824                         pi->mps    = le16_to_cpu(rfc.max_pdu_size);
2825                         break;
2826                 case L2CAP_MODE_STREAMING:
2827                         pi->mps    = le16_to_cpu(rfc.max_pdu_size);
2828                 }
2829         }
2830
2831         req->dcid   = cpu_to_le16(pi->dcid);
2832         req->flags  = cpu_to_le16(0x0000);
2833
2834         return ptr - data;
2835 }
2836
2837 static int l2cap_build_conf_rsp(struct sock *sk, void *data, u16 result, u16 flags)
2838 {
2839         struct l2cap_conf_rsp *rsp = data;
2840         void *ptr = rsp->data;
2841
2842         BT_DBG("sk %p", sk);
2843
2844         rsp->scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
2845         rsp->result = cpu_to_le16(result);
2846         rsp->flags  = cpu_to_le16(flags);
2847
2848         return ptr - data;
2849 }
2850
2851 static void l2cap_conf_rfc_get(struct sock *sk, void *rsp, int len)
2852 {
2853         struct l2cap_pinfo *pi = l2cap_pi(sk);
2854         int type, olen;
2855         unsigned long val;
2856         struct l2cap_conf_rfc rfc;
2857
2858         BT_DBG("sk %p, rsp %p, len %d", sk, rsp, len);
2859
2860         if ((pi->mode != L2CAP_MODE_ERTM) && (pi->mode != L2CAP_MODE_STREAMING))
2861                 return;
2862
2863         while (len >= L2CAP_CONF_OPT_SIZE) {
2864                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
2865
2866                 switch (type) {
2867                 case L2CAP_CONF_RFC:
2868                         if (olen == sizeof(rfc))
2869                                 memcpy(&rfc, (void *)val, olen);
2870                         goto done;
2871                 }
2872         }
2873
2874 done:
2875         switch (rfc.mode) {
2876         case L2CAP_MODE_ERTM:
2877                 pi->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
2878                 pi->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
2879                 pi->mps    = le16_to_cpu(rfc.max_pdu_size);
2880                 break;
2881         case L2CAP_MODE_STREAMING:
2882                 pi->mps    = le16_to_cpu(rfc.max_pdu_size);
2883         }
2884 }
2885
2886 static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2887 {
2888         struct l2cap_cmd_rej *rej = (struct l2cap_cmd_rej *) data;
2889
2890         if (rej->reason != 0x0000)
2891                 return 0;
2892
2893         if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
2894                                         cmd->ident == conn->info_ident) {
2895                 del_timer(&conn->info_timer);
2896
2897                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2898                 conn->info_ident = 0;
2899
2900                 l2cap_conn_start(conn);
2901         }
2902
2903         return 0;
2904 }
2905
2906 static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2907 {
2908         struct l2cap_chan_list *list = &conn->chan_list;
2909         struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
2910         struct l2cap_conn_rsp rsp;
2911         struct sock *parent, *uninitialized_var(sk);
2912         int result, status = L2CAP_CS_NO_INFO;
2913
2914         u16 dcid = 0, scid = __le16_to_cpu(req->scid);
2915         __le16 psm = req->psm;
2916
2917         BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid);
2918
2919         /* Check if we have socket listening on psm */
2920         parent = l2cap_get_sock_by_psm(BT_LISTEN, psm, conn->src);
2921         if (!parent) {
2922                 result = L2CAP_CR_BAD_PSM;
2923                 goto sendresp;
2924         }
2925
2926         /* Check if the ACL is secure enough (if not SDP) */
2927         if (psm != cpu_to_le16(0x0001) &&
2928                                 !hci_conn_check_link_mode(conn->hcon)) {
2929                 conn->disc_reason = 0x05;
2930                 result = L2CAP_CR_SEC_BLOCK;
2931                 goto response;
2932         }
2933
2934         result = L2CAP_CR_NO_MEM;
2935
2936         /* Check for backlog size */
2937         if (sk_acceptq_is_full(parent)) {
2938                 BT_DBG("backlog full %d", parent->sk_ack_backlog);
2939                 goto response;
2940         }
2941
2942         sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, GFP_ATOMIC);
2943         if (!sk)
2944                 goto response;
2945
2946         write_lock_bh(&list->lock);
2947
2948         /* Check if we already have channel with that dcid */
2949         if (__l2cap_get_chan_by_dcid(list, scid)) {
2950                 write_unlock_bh(&list->lock);
2951                 sock_set_flag(sk, SOCK_ZAPPED);
2952                 l2cap_sock_kill(sk);
2953                 goto response;
2954         }
2955
2956         hci_conn_hold(conn->hcon);
2957
2958         l2cap_sock_init(sk, parent);
2959         bacpy(&bt_sk(sk)->src, conn->src);
2960         bacpy(&bt_sk(sk)->dst, conn->dst);
2961         l2cap_pi(sk)->psm  = psm;
2962         l2cap_pi(sk)->dcid = scid;
2963
2964         __l2cap_chan_add(conn, sk, parent);
2965         dcid = l2cap_pi(sk)->scid;
2966
2967         l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
2968
2969         l2cap_pi(sk)->ident = cmd->ident;
2970
2971         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
2972                 if (l2cap_check_security(sk)) {
2973                         if (bt_sk(sk)->defer_setup) {
2974                                 sk->sk_state = BT_CONNECT2;
2975                                 result = L2CAP_CR_PEND;
2976                                 status = L2CAP_CS_AUTHOR_PEND;
2977                                 parent->sk_data_ready(parent, 0);
2978                         } else {
2979                                 sk->sk_state = BT_CONFIG;
2980                                 result = L2CAP_CR_SUCCESS;
2981                                 status = L2CAP_CS_NO_INFO;
2982                         }
2983                 } else {
2984                         sk->sk_state = BT_CONNECT2;
2985                         result = L2CAP_CR_PEND;
2986                         status = L2CAP_CS_AUTHEN_PEND;
2987                 }
2988         } else {
2989                 sk->sk_state = BT_CONNECT2;
2990                 result = L2CAP_CR_PEND;
2991                 status = L2CAP_CS_NO_INFO;
2992         }
2993
2994         write_unlock_bh(&list->lock);
2995
2996 response:
2997         bh_unlock_sock(parent);
2998
2999 sendresp:
3000         rsp.scid   = cpu_to_le16(scid);
3001         rsp.dcid   = cpu_to_le16(dcid);
3002         rsp.result = cpu_to_le16(result);
3003         rsp.status = cpu_to_le16(status);
3004         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
3005
3006         if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
3007                 struct l2cap_info_req info;
3008                 info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
3009
3010                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
3011                 conn->info_ident = l2cap_get_ident(conn);
3012
3013                 mod_timer(&conn->info_timer, jiffies +
3014                                         msecs_to_jiffies(L2CAP_INFO_TIMEOUT));
3015
3016                 l2cap_send_cmd(conn, conn->info_ident,
3017                                         L2CAP_INFO_REQ, sizeof(info), &info);
3018         }
3019
3020         if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT) &&
3021                                 result == L2CAP_CR_SUCCESS) {
3022                 u8 buf[128];
3023                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
3024                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3025                                         l2cap_build_conf_req(sk, buf), buf);
3026                 l2cap_pi(sk)->num_conf_req++;
3027         }
3028
3029         return 0;
3030 }
3031
3032 static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3033 {
3034         struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
3035         u16 scid, dcid, result, status;
3036         struct sock *sk;
3037         u8 req[128];
3038
3039         scid   = __le16_to_cpu(rsp->scid);
3040         dcid   = __le16_to_cpu(rsp->dcid);
3041         result = __le16_to_cpu(rsp->result);
3042         status = __le16_to_cpu(rsp->status);
3043
3044         BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status);
3045
3046         if (scid) {
3047                 sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
3048                 if (!sk)
3049                         return -EFAULT;
3050         } else {
3051                 sk = l2cap_get_chan_by_ident(&conn->chan_list, cmd->ident);
3052                 if (!sk)
3053                         return -EFAULT;
3054         }
3055
3056         switch (result) {
3057         case L2CAP_CR_SUCCESS:
3058                 sk->sk_state = BT_CONFIG;
3059                 l2cap_pi(sk)->ident = 0;
3060                 l2cap_pi(sk)->dcid = dcid;
3061                 l2cap_pi(sk)->conf_state &= ~L2CAP_CONF_CONNECT_PEND;
3062
3063                 if (l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT)
3064                         break;
3065
3066                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
3067
3068                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3069                                         l2cap_build_conf_req(sk, req), req);
3070                 l2cap_pi(sk)->num_conf_req++;
3071                 break;
3072
3073         case L2CAP_CR_PEND:
3074                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND;
3075                 break;
3076
3077         default:
3078                 l2cap_chan_del(sk, ECONNREFUSED);
3079                 break;
3080         }
3081
3082         bh_unlock_sock(sk);
3083         return 0;
3084 }
3085
3086 static inline void set_default_fcs(struct l2cap_pinfo *pi)
3087 {
3088         /* FCS is enabled only in ERTM or streaming mode, if one or both
3089          * sides request it.
3090          */
3091         if (pi->mode != L2CAP_MODE_ERTM && pi->mode != L2CAP_MODE_STREAMING)
3092                 pi->fcs = L2CAP_FCS_NONE;
3093         else if (!(pi->conf_state & L2CAP_CONF_NO_FCS_RECV))
3094                 pi->fcs = L2CAP_FCS_CRC16;
3095 }
3096
3097 static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
3098 {
3099         struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
3100         u16 dcid, flags;
3101         u8 rsp[64];
3102         struct sock *sk;
3103         int len;
3104
3105         dcid  = __le16_to_cpu(req->dcid);
3106         flags = __le16_to_cpu(req->flags);
3107
3108         BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
3109
3110         sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid);
3111         if (!sk)
3112                 return -ENOENT;
3113
3114         if (sk->sk_state == BT_DISCONN)
3115                 goto unlock;
3116
3117         /* Reject if config buffer is too small. */
3118         len = cmd_len - sizeof(*req);
3119         if (l2cap_pi(sk)->conf_len + len > sizeof(l2cap_pi(sk)->conf_req)) {
3120                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
3121                                 l2cap_build_conf_rsp(sk, rsp,
3122                                         L2CAP_CONF_REJECT, flags), rsp);
3123                 goto unlock;
3124         }
3125
3126         /* Store config. */
3127         memcpy(l2cap_pi(sk)->conf_req + l2cap_pi(sk)->conf_len, req->data, len);
3128         l2cap_pi(sk)->conf_len += len;
3129
3130         if (flags & 0x0001) {
3131                 /* Incomplete config. Send empty response. */
3132                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
3133                                 l2cap_build_conf_rsp(sk, rsp,
3134                                         L2CAP_CONF_SUCCESS, 0x0001), rsp);
3135                 goto unlock;
3136         }
3137
3138         /* Complete config. */
3139         len = l2cap_parse_conf_req(sk, rsp);
3140         if (len < 0) {
3141                 l2cap_send_disconn_req(conn, sk, ECONNRESET);
3142                 goto unlock;
3143         }
3144
3145         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
3146         l2cap_pi(sk)->num_conf_rsp++;
3147
3148         /* Reset config buffer. */
3149         l2cap_pi(sk)->conf_len = 0;
3150
3151         if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE))
3152                 goto unlock;
3153
3154         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_INPUT_DONE) {
3155                 set_default_fcs(l2cap_pi(sk));
3156
3157                 sk->sk_state = BT_CONNECTED;
3158
3159                 l2cap_pi(sk)->next_tx_seq = 0;
3160                 l2cap_pi(sk)->expected_tx_seq = 0;
3161                 __skb_queue_head_init(TX_QUEUE(sk));
3162                 if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM)
3163                         l2cap_ertm_init(sk);
3164
3165                 l2cap_chan_ready(sk);
3166                 goto unlock;
3167         }
3168
3169         if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT)) {
3170                 u8 buf[64];
3171                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3172                                         l2cap_build_conf_req(sk, buf), buf);
3173                 l2cap_pi(sk)->num_conf_req++;
3174         }
3175
3176 unlock:
3177         bh_unlock_sock(sk);
3178         return 0;
3179 }
3180
3181 static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3182 {
3183         struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
3184         u16 scid, flags, result;
3185         struct sock *sk;
3186         int len = cmd->len - sizeof(*rsp);
3187
3188         scid   = __le16_to_cpu(rsp->scid);
3189         flags  = __le16_to_cpu(rsp->flags);
3190         result = __le16_to_cpu(rsp->result);
3191
3192         BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x",
3193                         scid, flags, result);
3194
3195         sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
3196         if (!sk)
3197                 return 0;
3198
3199         switch (result) {
3200         case L2CAP_CONF_SUCCESS:
3201                 l2cap_conf_rfc_get(sk, rsp->data, len);
3202                 break;
3203
3204         case L2CAP_CONF_UNACCEPT:
3205                 if (l2cap_pi(sk)->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
3206                         char req[64];
3207
3208                         if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
3209                                 l2cap_send_disconn_req(conn, sk, ECONNRESET);
3210                                 goto done;
3211                         }
3212
3213                         /* throw out any old stored conf requests */
3214                         result = L2CAP_CONF_SUCCESS;
3215                         len = l2cap_parse_conf_rsp(sk, rsp->data,
3216                                                         len, req, &result);
3217                         if (len < 0) {
3218                                 l2cap_send_disconn_req(conn, sk, ECONNRESET);
3219                                 goto done;
3220                         }
3221
3222                         l2cap_send_cmd(conn, l2cap_get_ident(conn),
3223                                                 L2CAP_CONF_REQ, len, req);
3224                         l2cap_pi(sk)->num_conf_req++;
3225                         if (result != L2CAP_CONF_SUCCESS)
3226                                 goto done;
3227                         break;
3228                 }
3229
3230         default:
3231                 sk->sk_err = ECONNRESET;
3232                 l2cap_sock_set_timer(sk, HZ * 5);
3233                 l2cap_send_disconn_req(conn, sk, ECONNRESET);
3234                 goto done;
3235         }
3236
3237         if (flags & 0x01)
3238                 goto done;
3239
3240         l2cap_pi(sk)->conf_state |= L2CAP_CONF_INPUT_DONE;
3241
3242         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE) {
3243                 set_default_fcs(l2cap_pi(sk));
3244
3245                 sk->sk_state = BT_CONNECTED;
3246                 l2cap_pi(sk)->next_tx_seq = 0;
3247                 l2cap_pi(sk)->expected_tx_seq = 0;
3248                 __skb_queue_head_init(TX_QUEUE(sk));
3249                 if (l2cap_pi(sk)->mode ==  L2CAP_MODE_ERTM)
3250                         l2cap_ertm_init(sk);
3251
3252                 l2cap_chan_ready(sk);
3253         }
3254
3255 done:
3256         bh_unlock_sock(sk);
3257         return 0;
3258 }
3259
3260 static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3261 {
3262         struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
3263         struct l2cap_disconn_rsp rsp;
3264         u16 dcid, scid;
3265         struct sock *sk;
3266
3267         scid = __le16_to_cpu(req->scid);
3268         dcid = __le16_to_cpu(req->dcid);
3269
3270         BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
3271
3272         sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid);
3273         if (!sk)
3274                 return 0;
3275
3276         rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid);
3277         rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid);
3278         l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
3279
3280         sk->sk_shutdown = SHUTDOWN_MASK;
3281
3282         l2cap_chan_del(sk, ECONNRESET);
3283         bh_unlock_sock(sk);
3284
3285         l2cap_sock_kill(sk);
3286         return 0;
3287 }
3288
3289 static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3290 {
3291         struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
3292         u16 dcid, scid;
3293         struct sock *sk;
3294
3295         scid = __le16_to_cpu(rsp->scid);
3296         dcid = __le16_to_cpu(rsp->dcid);
3297
3298         BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
3299
3300         sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
3301         if (!sk)
3302                 return 0;
3303
3304         l2cap_chan_del(sk, 0);
3305         bh_unlock_sock(sk);
3306
3307         l2cap_sock_kill(sk);
3308         return 0;
3309 }
3310
3311 static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3312 {
3313         struct l2cap_info_req *req = (struct l2cap_info_req *) data;
3314         u16 type;
3315
3316         type = __le16_to_cpu(req->type);
3317
3318         BT_DBG("type 0x%4.4x", type);
3319
3320         if (type == L2CAP_IT_FEAT_MASK) {
3321                 u8 buf[8];
3322                 u32 feat_mask = l2cap_feat_mask;
3323                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
3324                 rsp->type   = cpu_to_le16(L2CAP_IT_FEAT_MASK);
3325                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
3326                 if (!disable_ertm)
3327                         feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
3328                                                          | L2CAP_FEAT_FCS;
3329                 put_unaligned_le32(feat_mask, rsp->data);
3330                 l2cap_send_cmd(conn, cmd->ident,
3331                                         L2CAP_INFO_RSP, sizeof(buf), buf);
3332         } else if (type == L2CAP_IT_FIXED_CHAN) {
3333                 u8 buf[12];
3334                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
3335                 rsp->type   = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
3336                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
3337                 memcpy(buf + 4, l2cap_fixed_chan, 8);
3338                 l2cap_send_cmd(conn, cmd->ident,
3339                                         L2CAP_INFO_RSP, sizeof(buf), buf);
3340         } else {
3341                 struct l2cap_info_rsp rsp;
3342                 rsp.type   = cpu_to_le16(type);
3343                 rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP);
3344                 l2cap_send_cmd(conn, cmd->ident,
3345                                         L2CAP_INFO_RSP, sizeof(rsp), &rsp);
3346         }
3347
3348         return 0;
3349 }
3350
3351 static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3352 {
3353         struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
3354         u16 type, result;
3355
3356         type   = __le16_to_cpu(rsp->type);
3357         result = __le16_to_cpu(rsp->result);
3358
3359         BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
3360
3361         del_timer(&conn->info_timer);
3362
3363         if (result != L2CAP_IR_SUCCESS) {
3364                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3365                 conn->info_ident = 0;
3366
3367                 l2cap_conn_start(conn);
3368
3369                 return 0;
3370         }
3371
3372         if (type == L2CAP_IT_FEAT_MASK) {
3373                 conn->feat_mask = get_unaligned_le32(rsp->data);
3374
3375                 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
3376                         struct l2cap_info_req req;
3377                         req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
3378
3379                         conn->info_ident = l2cap_get_ident(conn);
3380
3381                         l2cap_send_cmd(conn, conn->info_ident,
3382                                         L2CAP_INFO_REQ, sizeof(req), &req);
3383                 } else {
3384                         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3385                         conn->info_ident = 0;
3386
3387                         l2cap_conn_start(conn);
3388                 }
3389         } else if (type == L2CAP_IT_FIXED_CHAN) {
3390                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3391                 conn->info_ident = 0;
3392
3393                 l2cap_conn_start(conn);
3394         }
3395
3396         return 0;
3397 }
3398
3399 static inline void l2cap_sig_channel(struct l2cap_conn *conn, struct sk_buff *skb)
3400 {
3401         u8 *data = skb->data;
3402         int len = skb->len;
3403         struct l2cap_cmd_hdr cmd;
3404         int err = 0;
3405
3406         l2cap_raw_recv(conn, skb);
3407
3408         while (len >= L2CAP_CMD_HDR_SIZE) {
3409                 u16 cmd_len;
3410                 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
3411                 data += L2CAP_CMD_HDR_SIZE;
3412                 len  -= L2CAP_CMD_HDR_SIZE;
3413
3414                 cmd_len = le16_to_cpu(cmd.len);
3415
3416                 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, cmd.ident);
3417
3418                 if (cmd_len > len || !cmd.ident) {
3419                         BT_DBG("corrupted command");
3420                         break;
3421                 }
3422
3423                 switch (cmd.code) {
3424                 case L2CAP_COMMAND_REJ:
3425                         l2cap_command_rej(conn, &cmd, data);
3426                         break;
3427
3428                 case L2CAP_CONN_REQ:
3429                         err = l2cap_connect_req(conn, &cmd, data);
3430                         break;
3431
3432                 case L2CAP_CONN_RSP:
3433                         err = l2cap_connect_rsp(conn, &cmd, data);
3434                         break;
3435
3436                 case L2CAP_CONF_REQ:
3437                         err = l2cap_config_req(conn, &cmd, cmd_len, data);
3438                         break;
3439
3440                 case L2CAP_CONF_RSP:
3441                         err = l2cap_config_rsp(conn, &cmd, data);
3442                         break;
3443
3444                 case L2CAP_DISCONN_REQ:
3445                         err = l2cap_disconnect_req(conn, &cmd, data);
3446                         break;
3447
3448                 case L2CAP_DISCONN_RSP:
3449                         err = l2cap_disconnect_rsp(conn, &cmd, data);
3450                         break;
3451
3452                 case L2CAP_ECHO_REQ:
3453                         l2cap_send_cmd(conn, cmd.ident, L2CAP_ECHO_RSP, cmd_len, data);
3454                         break;
3455
3456                 case L2CAP_ECHO_RSP:
3457                         break;
3458
3459                 case L2CAP_INFO_REQ:
3460                         err = l2cap_information_req(conn, &cmd, data);
3461                         break;
3462
3463                 case L2CAP_INFO_RSP:
3464                         err = l2cap_information_rsp(conn, &cmd, data);
3465                         break;
3466
3467                 default:
3468                         BT_ERR("Unknown signaling command 0x%2.2x", cmd.code);
3469                         err = -EINVAL;
3470                         break;
3471                 }
3472
3473                 if (err) {
3474                         struct l2cap_cmd_rej rej;
3475                         BT_DBG("error %d", err);
3476
3477                         /* FIXME: Map err to a valid reason */
3478                         rej.reason = cpu_to_le16(0);
3479                         l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
3480                 }
3481
3482                 data += cmd_len;
3483                 len  -= cmd_len;
3484         }
3485
3486         kfree_skb(skb);
3487 }
3488
3489 static int l2cap_check_fcs(struct l2cap_pinfo *pi,  struct sk_buff *skb)
3490 {
3491         u16 our_fcs, rcv_fcs;
3492         int hdr_size = L2CAP_HDR_SIZE + 2;
3493
3494         if (pi->fcs == L2CAP_FCS_CRC16) {
3495                 skb_trim(skb, skb->len - 2);
3496                 rcv_fcs = get_unaligned_le16(skb->data + skb->len);
3497                 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
3498
3499                 if (our_fcs != rcv_fcs)
3500                         return -EBADMSG;
3501         }
3502         return 0;
3503 }
3504
3505 static inline void l2cap_send_i_or_rr_or_rnr(struct sock *sk)
3506 {
3507         struct l2cap_pinfo *pi = l2cap_pi(sk);
3508         u16 control = 0;
3509
3510         pi->frames_sent = 0;
3511
3512         control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3513
3514         if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
3515                 control |= L2CAP_SUPER_RCV_NOT_READY;
3516                 l2cap_send_sframe(pi, control);
3517                 pi->conn_state |= L2CAP_CONN_RNR_SENT;
3518         }
3519
3520         if (pi->conn_state & L2CAP_CONN_REMOTE_BUSY)
3521                 l2cap_retransmit_frames(sk);
3522
3523         l2cap_ertm_send(sk);
3524
3525         if (!(pi->conn_state & L2CAP_CONN_LOCAL_BUSY) &&
3526                         pi->frames_sent == 0) {
3527                 control |= L2CAP_SUPER_RCV_READY;
3528                 l2cap_send_sframe(pi, control);
3529         }
3530 }
3531
3532 static int l2cap_add_to_srej_queue(struct sock *sk, struct sk_buff *skb, u8 tx_seq, u8 sar)
3533 {
3534         struct sk_buff *next_skb;
3535         struct l2cap_pinfo *pi = l2cap_pi(sk);
3536         int tx_seq_offset, next_tx_seq_offset;
3537
3538         bt_cb(skb)->tx_seq = tx_seq;
3539         bt_cb(skb)->sar = sar;
3540
3541         next_skb = skb_peek(SREJ_QUEUE(sk));
3542         if (!next_skb) {
3543                 __skb_queue_tail(SREJ_QUEUE(sk), skb);
3544                 return 0;
3545         }
3546
3547         tx_seq_offset = (tx_seq - pi->buffer_seq) % 64;
3548         if (tx_seq_offset < 0)
3549                 tx_seq_offset += 64;
3550
3551         do {
3552                 if (bt_cb(next_skb)->tx_seq == tx_seq)
3553                         return -EINVAL;
3554
3555                 next_tx_seq_offset = (bt_cb(next_skb)->tx_seq -
3556                                                 pi->buffer_seq) % 64;
3557                 if (next_tx_seq_offset < 0)
3558                         next_tx_seq_offset += 64;
3559
3560                 if (next_tx_seq_offset > tx_seq_offset) {
3561                         __skb_queue_before(SREJ_QUEUE(sk), next_skb, skb);
3562                         return 0;
3563                 }
3564
3565                 if (skb_queue_is_last(SREJ_QUEUE(sk), next_skb))
3566                         break;
3567
3568         } while ((next_skb = skb_queue_next(SREJ_QUEUE(sk), next_skb)));
3569
3570         __skb_queue_tail(SREJ_QUEUE(sk), skb);
3571
3572         return 0;
3573 }
3574
3575 static int l2cap_ertm_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)
3576 {
3577         struct l2cap_pinfo *pi = l2cap_pi(sk);
3578         struct sk_buff *_skb;
3579         int err;
3580
3581         switch (control & L2CAP_CTRL_SAR) {
3582         case L2CAP_SDU_UNSEGMENTED:
3583                 if (pi->conn_state & L2CAP_CONN_SAR_SDU)
3584                         goto drop;
3585
3586                 err = sock_queue_rcv_skb(sk, skb);
3587                 if (!err)
3588                         return err;
3589
3590                 break;
3591
3592         case L2CAP_SDU_START:
3593                 if (pi->conn_state & L2CAP_CONN_SAR_SDU)
3594                         goto drop;
3595
3596                 pi->sdu_len = get_unaligned_le16(skb->data);
3597
3598                 if (pi->sdu_len > pi->imtu)
3599                         goto disconnect;
3600
3601                 pi->sdu = bt_skb_alloc(pi->sdu_len, GFP_ATOMIC);
3602                 if (!pi->sdu)
3603                         return -ENOMEM;
3604
3605                 /* pull sdu_len bytes only after alloc, because of Local Busy
3606                  * condition we have to be sure that this will be executed
3607                  * only once, i.e., when alloc does not fail */
3608                 skb_pull(skb, 2);
3609
3610                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3611
3612                 pi->conn_state |= L2CAP_CONN_SAR_SDU;
3613                 pi->partial_sdu_len = skb->len;
3614                 break;
3615
3616         case L2CAP_SDU_CONTINUE:
3617                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
3618                         goto disconnect;
3619
3620                 if (!pi->sdu)
3621                         goto disconnect;
3622
3623                 pi->partial_sdu_len += skb->len;
3624                 if (pi->partial_sdu_len > pi->sdu_len)
3625                         goto drop;
3626
3627                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3628
3629                 break;
3630
3631         case L2CAP_SDU_END:
3632                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
3633                         goto disconnect;
3634
3635                 if (!pi->sdu)
3636                         goto disconnect;
3637
3638                 if (!(pi->conn_state & L2CAP_CONN_SAR_RETRY)) {
3639                         pi->partial_sdu_len += skb->len;
3640
3641                         if (pi->partial_sdu_len > pi->imtu)
3642                                 goto drop;
3643
3644                         if (pi->partial_sdu_len != pi->sdu_len)
3645                                 goto drop;
3646
3647                         memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3648                 }
3649
3650                 _skb = skb_clone(pi->sdu, GFP_ATOMIC);
3651                 if (!_skb) {
3652                         pi->conn_state |= L2CAP_CONN_SAR_RETRY;
3653                         return -ENOMEM;
3654                 }
3655
3656                 err = sock_queue_rcv_skb(sk, _skb);
3657                 if (err < 0) {
3658                         kfree_skb(_skb);
3659                         pi->conn_state |= L2CAP_CONN_SAR_RETRY;
3660                         return err;
3661                 }
3662
3663                 pi->conn_state &= ~L2CAP_CONN_SAR_RETRY;
3664                 pi->conn_state &= ~L2CAP_CONN_SAR_SDU;
3665
3666                 kfree_skb(pi->sdu);
3667                 break;
3668         }
3669
3670         kfree_skb(skb);
3671         return 0;
3672
3673 drop:
3674         kfree_skb(pi->sdu);
3675         pi->sdu = NULL;
3676
3677 disconnect:
3678         l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
3679         kfree_skb(skb);
3680         return 0;
3681 }
3682
3683 static int l2cap_try_push_rx_skb(struct sock *sk)
3684 {
3685         struct l2cap_pinfo *pi = l2cap_pi(sk);
3686         struct sk_buff *skb;
3687         u16 control;
3688         int err;
3689
3690         while ((skb = skb_dequeue(BUSY_QUEUE(sk)))) {
3691                 control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
3692                 err = l2cap_ertm_reassembly_sdu(sk, skb, control);
3693                 if (err < 0) {
3694                         skb_queue_head(BUSY_QUEUE(sk), skb);
3695                         return -EBUSY;
3696                 }
3697
3698                 pi->buffer_seq = (pi->buffer_seq + 1) % 64;
3699         }
3700
3701         if (!(pi->conn_state & L2CAP_CONN_RNR_SENT))
3702                 goto done;
3703
3704         control = pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3705         control |= L2CAP_SUPER_RCV_READY | L2CAP_CTRL_POLL;
3706         l2cap_send_sframe(pi, control);
3707         l2cap_pi(sk)->retry_count = 1;
3708
3709         del_timer(&pi->retrans_timer);
3710         __mod_monitor_timer();
3711
3712         l2cap_pi(sk)->conn_state |= L2CAP_CONN_WAIT_F;
3713
3714 done:
3715         pi->conn_state &= ~L2CAP_CONN_LOCAL_BUSY;
3716         pi->conn_state &= ~L2CAP_CONN_RNR_SENT;
3717
3718         BT_DBG("sk %p, Exit local busy", sk);
3719
3720         return 0;
3721 }
3722
3723 static void l2cap_busy_work(struct work_struct *work)
3724 {
3725         DECLARE_WAITQUEUE(wait, current);
3726         struct l2cap_pinfo *pi =
3727                 container_of(work, struct l2cap_pinfo, busy_work);
3728         struct sock *sk = (struct sock *)pi;
3729         int n_tries = 0, timeo = HZ/5, err;
3730         struct sk_buff *skb;
3731
3732         lock_sock(sk);
3733
3734         add_wait_queue(sk_sleep(sk), &wait);
3735         while ((skb = skb_peek(BUSY_QUEUE(sk)))) {
3736                 set_current_state(TASK_INTERRUPTIBLE);
3737
3738                 if (n_tries++ > L2CAP_LOCAL_BUSY_TRIES) {
3739                         err = -EBUSY;
3740                         l2cap_send_disconn_req(pi->conn, sk, EBUSY);
3741                         break;
3742                 }
3743
3744                 if (!timeo)
3745                         timeo = HZ/5;
3746
3747                 if (signal_pending(current)) {
3748                         err = sock_intr_errno(timeo);
3749                         break;
3750                 }
3751
3752                 release_sock(sk);
3753                 timeo = schedule_timeout(timeo);
3754                 lock_sock(sk);
3755
3756                 err = sock_error(sk);
3757                 if (err)
3758                         break;
3759
3760                 if (l2cap_try_push_rx_skb(sk) == 0)
3761                         break;
3762         }
3763
3764         set_current_state(TASK_RUNNING);
3765         remove_wait_queue(sk_sleep(sk), &wait);
3766
3767         release_sock(sk);
3768 }
3769
3770 static int l2cap_push_rx_skb(struct sock *sk, struct sk_buff *skb, u16 control)
3771 {
3772         struct l2cap_pinfo *pi = l2cap_pi(sk);
3773         int sctrl, err;
3774
3775         if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
3776                 bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT;
3777                 __skb_queue_tail(BUSY_QUEUE(sk), skb);
3778                 return l2cap_try_push_rx_skb(sk);
3779
3780
3781         }
3782
3783         err = l2cap_ertm_reassembly_sdu(sk, skb, control);
3784         if (err >= 0) {
3785                 pi->buffer_seq = (pi->buffer_seq + 1) % 64;
3786                 return err;
3787         }
3788
3789         /* Busy Condition */
3790         BT_DBG("sk %p, Enter local busy", sk);
3791
3792         pi->conn_state |= L2CAP_CONN_LOCAL_BUSY;
3793         bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT;
3794         __skb_queue_tail(BUSY_QUEUE(sk), skb);
3795
3796         sctrl = pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3797         sctrl |= L2CAP_SUPER_RCV_NOT_READY;
3798         l2cap_send_sframe(pi, sctrl);
3799
3800         pi->conn_state |= L2CAP_CONN_RNR_SENT;
3801
3802         del_timer(&pi->ack_timer);
3803
3804         queue_work(_busy_wq, &pi->busy_work);
3805
3806         return err;
3807 }
3808
3809 static int l2cap_streaming_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)
3810 {
3811         struct l2cap_pinfo *pi = l2cap_pi(sk);
3812         struct sk_buff *_skb;
3813         int err = -EINVAL;
3814
3815         /*
3816          * TODO: We have to notify the userland if some data is lost with the
3817          * Streaming Mode.
3818          */
3819
3820         switch (control & L2CAP_CTRL_SAR) {
3821         case L2CAP_SDU_UNSEGMENTED:
3822                 if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
3823                         kfree_skb(pi->sdu);
3824                         break;
3825                 }
3826
3827                 err = sock_queue_rcv_skb(sk, skb);
3828                 if (!err)
3829                         return 0;
3830
3831                 break;
3832
3833         case L2CAP_SDU_START:
3834                 if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
3835                         kfree_skb(pi->sdu);
3836                         break;
3837                 }
3838
3839                 pi->sdu_len = get_unaligned_le16(skb->data);
3840                 skb_pull(skb, 2);
3841
3842                 if (pi->sdu_len > pi->imtu) {
3843                         err = -EMSGSIZE;
3844                         break;
3845                 }
3846
3847                 pi->sdu = bt_skb_alloc(pi->sdu_len, GFP_ATOMIC);
3848                 if (!pi->sdu) {
3849                         err = -ENOMEM;
3850                         break;
3851                 }
3852
3853                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3854
3855                 pi->conn_state |= L2CAP_CONN_SAR_SDU;
3856                 pi->partial_sdu_len = skb->len;
3857                 err = 0;
3858                 break;
3859
3860         case L2CAP_SDU_CONTINUE:
3861                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
3862                         break;
3863
3864                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3865
3866                 pi->partial_sdu_len += skb->len;
3867                 if (pi->partial_sdu_len > pi->sdu_len)
3868                         kfree_skb(pi->sdu);
3869                 else
3870                         err = 0;
3871
3872                 break;
3873
3874         case L2CAP_SDU_END:
3875                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
3876                         break;
3877
3878                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3879
3880                 pi->conn_state &= ~L2CAP_CONN_SAR_SDU;
3881                 pi->partial_sdu_len += skb->len;
3882
3883                 if (pi->partial_sdu_len > pi->imtu)
3884                         goto drop;
3885
3886                 if (pi->partial_sdu_len == pi->sdu_len) {
3887                         _skb = skb_clone(pi->sdu, GFP_ATOMIC);
3888                         err = sock_queue_rcv_skb(sk, _skb);
3889                         if (err < 0)
3890                                 kfree_skb(_skb);
3891                 }
3892                 err = 0;
3893
3894 drop:
3895                 kfree_skb(pi->sdu);
3896                 break;
3897         }
3898
3899         kfree_skb(skb);
3900         return err;
3901 }
3902
3903 static void l2cap_check_srej_gap(struct sock *sk, u8 tx_seq)
3904 {
3905         struct sk_buff *skb;
3906         u16 control;
3907
3908         while ((skb = skb_peek(SREJ_QUEUE(sk)))) {
3909                 if (bt_cb(skb)->tx_seq != tx_seq)
3910                         break;
3911
3912                 skb = skb_dequeue(SREJ_QUEUE(sk));
3913                 control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
3914                 l2cap_ertm_reassembly_sdu(sk, skb, control);
3915                 l2cap_pi(sk)->buffer_seq_srej =
3916                         (l2cap_pi(sk)->buffer_seq_srej + 1) % 64;
3917                 tx_seq = (tx_seq + 1) % 64;
3918         }
3919 }
3920
3921 static void l2cap_resend_srejframe(struct sock *sk, u8 tx_seq)
3922 {
3923         struct l2cap_pinfo *pi = l2cap_pi(sk);
3924         struct srej_list *l, *tmp;
3925         u16 control;
3926
3927         list_for_each_entry_safe(l, tmp, SREJ_LIST(sk), list) {
3928                 if (l->tx_seq == tx_seq) {
3929                         list_del(&l->list);
3930                         kfree(l);
3931                         return;
3932                 }
3933                 control = L2CAP_SUPER_SELECT_REJECT;
3934                 control |= l->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3935                 l2cap_send_sframe(pi, control);
3936                 list_del(&l->list);
3937                 list_add_tail(&l->list, SREJ_LIST(sk));
3938         }
3939 }
3940
3941 static void l2cap_send_srejframe(struct sock *sk, u8 tx_seq)
3942 {
3943         struct l2cap_pinfo *pi = l2cap_pi(sk);
3944         struct srej_list *new;
3945         u16 control;
3946
3947         while (tx_seq != pi->expected_tx_seq) {
3948                 control = L2CAP_SUPER_SELECT_REJECT;
3949                 control |= pi->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3950                 l2cap_send_sframe(pi, control);
3951
3952                 new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC);
3953                 new->tx_seq = pi->expected_tx_seq;
3954                 pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3955                 list_add_tail(&new->list, SREJ_LIST(sk));
3956         }
3957         pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3958 }
3959
3960 static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
3961 {
3962         struct l2cap_pinfo *pi = l2cap_pi(sk);
3963         u8 tx_seq = __get_txseq(rx_control);
3964         u8 req_seq = __get_reqseq(rx_control);
3965         u8 sar = rx_control >> L2CAP_CTRL_SAR_SHIFT;
3966         int tx_seq_offset, expected_tx_seq_offset;
3967         int num_to_ack = (pi->tx_win/6) + 1;
3968         int err = 0;
3969
3970         BT_DBG("sk %p len %d tx_seq %d rx_control 0x%4.4x", sk, skb->len, tx_seq,
3971                                                                 rx_control);
3972
3973         if (L2CAP_CTRL_FINAL & rx_control &&
3974                         l2cap_pi(sk)->conn_state & L2CAP_CONN_WAIT_F) {
3975                 del_timer(&pi->monitor_timer);
3976                 if (pi->unacked_frames > 0)
3977                         __mod_retrans_timer();
3978                 pi->conn_state &= ~L2CAP_CONN_WAIT_F;
3979         }
3980
3981         pi->expected_ack_seq = req_seq;
3982         l2cap_drop_acked_frames(sk);
3983
3984         if (tx_seq == pi->expected_tx_seq)
3985                 goto expected;
3986
3987         tx_seq_offset = (tx_seq - pi->buffer_seq) % 64;
3988         if (tx_seq_offset < 0)
3989                 tx_seq_offset += 64;
3990
3991         /* invalid tx_seq */
3992         if (tx_seq_offset >= pi->tx_win) {
3993                 l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
3994                 goto drop;
3995         }
3996
3997         if (pi->conn_state == L2CAP_CONN_LOCAL_BUSY)
3998                 goto drop;
3999
4000         if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
4001                 struct srej_list *first;
4002
4003                 first = list_first_entry(SREJ_LIST(sk),
4004                                 struct srej_list, list);
4005                 if (tx_seq == first->tx_seq) {
4006                         l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
4007                         l2cap_check_srej_gap(sk, tx_seq);
4008
4009                         list_del(&first->list);
4010                         kfree(first);
4011
4012                         if (list_empty(SREJ_LIST(sk))) {
4013                                 pi->buffer_seq = pi->buffer_seq_srej;
4014                                 pi->conn_state &= ~L2CAP_CONN_SREJ_SENT;
4015                                 l2cap_send_ack(pi);
4016                                 BT_DBG("sk %p, Exit SREJ_SENT", sk);
4017                         }
4018                 } else {
4019                         struct srej_list *l;
4020
4021                         /* duplicated tx_seq */
4022                         if (l2cap_add_to_srej_queue(sk, skb, tx_seq, sar) < 0)
4023                                 goto drop;
4024
4025                         list_for_each_entry(l, SREJ_LIST(sk), list) {
4026                                 if (l->tx_seq == tx_seq) {
4027                                         l2cap_resend_srejframe(sk, tx_seq);
4028                                         return 0;
4029                                 }
4030                         }
4031                         l2cap_send_srejframe(sk, tx_seq);
4032                 }
4033         } else {
4034                 expected_tx_seq_offset =
4035                         (pi->expected_tx_seq - pi->buffer_seq) % 64;
4036                 if (expected_tx_seq_offset < 0)
4037                         expected_tx_seq_offset += 64;
4038
4039                 /* duplicated tx_seq */
4040                 if (tx_seq_offset < expected_tx_seq_offset)
4041                         goto drop;
4042
4043                 pi->conn_state |= L2CAP_CONN_SREJ_SENT;
4044
4045                 BT_DBG("sk %p, Enter SREJ", sk);
4046
4047                 INIT_LIST_HEAD(SREJ_LIST(sk));
4048                 pi->buffer_seq_srej = pi->buffer_seq;
4049
4050                 __skb_queue_head_init(SREJ_QUEUE(sk));
4051                 __skb_queue_head_init(BUSY_QUEUE(sk));
4052                 l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
4053
4054                 pi->conn_state |= L2CAP_CONN_SEND_PBIT;
4055
4056                 l2cap_send_srejframe(sk, tx_seq);
4057
4058                 del_timer(&pi->ack_timer);
4059         }
4060         return 0;
4061
4062 expected:
4063         pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
4064
4065         if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
4066                 bt_cb(skb)->tx_seq = tx_seq;
4067                 bt_cb(skb)->sar = sar;
4068                 __skb_queue_tail(SREJ_QUEUE(sk), skb);
4069                 return 0;
4070         }
4071
4072         err = l2cap_push_rx_skb(sk, skb, rx_control);
4073         if (err < 0)
4074                 return 0;
4075
4076         if (rx_control & L2CAP_CTRL_FINAL) {
4077                 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
4078                         pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
4079                 else
4080                         l2cap_retransmit_frames(sk);
4081         }
4082
4083         __mod_ack_timer();
4084
4085         pi->num_acked = (pi->num_acked + 1) % num_to_ack;
4086         if (pi->num_acked == num_to_ack - 1)
4087                 l2cap_send_ack(pi);
4088
4089         return 0;
4090
4091 drop:
4092         kfree_skb(skb);
4093         return 0;
4094 }
4095
4096 static inline void l2cap_data_channel_rrframe(struct sock *sk, u16 rx_control)
4097 {
4098         struct l2cap_pinfo *pi = l2cap_pi(sk);
4099
4100         BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, __get_reqseq(rx_control),
4101                                                 rx_control);
4102
4103         pi->expected_ack_seq = __get_reqseq(rx_control);
4104         l2cap_drop_acked_frames(sk);
4105
4106         if (rx_control & L2CAP_CTRL_POLL) {
4107                 pi->conn_state |= L2CAP_CONN_SEND_FBIT;
4108                 if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
4109                         if ((pi->conn_state & L2CAP_CONN_REMOTE_BUSY) &&
4110                                         (pi->unacked_frames > 0))
4111                                 __mod_retrans_timer();
4112
4113                         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
4114                         l2cap_send_srejtail(sk);
4115                 } else {
4116                         l2cap_send_i_or_rr_or_rnr(sk);
4117                 }
4118
4119         } else if (rx_control & L2CAP_CTRL_FINAL) {
4120                 pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
4121
4122                 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
4123                         pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
4124                 else
4125                         l2cap_retransmit_frames(sk);
4126
4127         } else {
4128                 if ((pi->conn_state & L2CAP_CONN_REMOTE_BUSY) &&
4129                                 (pi->unacked_frames > 0))
4130                         __mod_retrans_timer();
4131
4132                 pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
4133                 if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
4134                         l2cap_send_ack(pi);
4135                 } else {
4136                         l2cap_ertm_send(sk);
4137                 }
4138         }
4139 }
4140
4141 static inline void l2cap_data_channel_rejframe(struct sock *sk, u16 rx_control)
4142 {
4143         struct l2cap_pinfo *pi = l2cap_pi(sk);
4144         u8 tx_seq = __get_reqseq(rx_control);
4145
4146         BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, tx_seq, rx_control);
4147
4148         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
4149
4150         pi->expected_ack_seq = tx_seq;
4151         l2cap_drop_acked_frames(sk);
4152
4153         if (rx_control & L2CAP_CTRL_FINAL) {
4154                 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
4155                         pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
4156                 else
4157                         l2cap_retransmit_frames(sk);
4158         } else {
4159                 l2cap_retransmit_frames(sk);
4160
4161                 if (pi->conn_state & L2CAP_CONN_WAIT_F)
4162                         pi->conn_state |= L2CAP_CONN_REJ_ACT;
4163         }
4164 }
4165 static inline void l2cap_data_channel_srejframe(struct sock *sk, u16 rx_control)
4166 {
4167         struct l2cap_pinfo *pi = l2cap_pi(sk);
4168         u8 tx_seq = __get_reqseq(rx_control);
4169
4170         BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, tx_seq, rx_control);
4171
4172         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
4173
4174         if (rx_control & L2CAP_CTRL_POLL) {
4175                 pi->expected_ack_seq = tx_seq;
4176                 l2cap_drop_acked_frames(sk);
4177
4178                 pi->conn_state |= L2CAP_CONN_SEND_FBIT;
4179                 l2cap_retransmit_one_frame(sk, tx_seq);
4180
4181                 l2cap_ertm_send(sk);
4182
4183                 if (pi->conn_state & L2CAP_CONN_WAIT_F) {
4184                         pi->srej_save_reqseq = tx_seq;
4185                         pi->conn_state |= L2CAP_CONN_SREJ_ACT;
4186                 }
4187         } else if (rx_control & L2CAP_CTRL_FINAL) {
4188                 if ((pi->conn_state & L2CAP_CONN_SREJ_ACT) &&
4189                                 pi->srej_save_reqseq == tx_seq)
4190                         pi->conn_state &= ~L2CAP_CONN_SREJ_ACT;
4191                 else
4192                         l2cap_retransmit_one_frame(sk, tx_seq);
4193         } else {
4194                 l2cap_retransmit_one_frame(sk, tx_seq);
4195                 if (pi->conn_state & L2CAP_CONN_WAIT_F) {
4196                         pi->srej_save_reqseq = tx_seq;
4197                         pi->conn_state |= L2CAP_CONN_SREJ_ACT;
4198                 }
4199         }
4200 }
4201
4202 static inline void l2cap_data_channel_rnrframe(struct sock *sk, u16 rx_control)
4203 {
4204         struct l2cap_pinfo *pi = l2cap_pi(sk);
4205         u8 tx_seq = __get_reqseq(rx_control);
4206
4207         BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, tx_seq, rx_control);
4208
4209         pi->conn_state |= L2CAP_CONN_REMOTE_BUSY;
4210         pi->expected_ack_seq = tx_seq;
4211         l2cap_drop_acked_frames(sk);
4212
4213         if (rx_control & L2CAP_CTRL_POLL)
4214                 pi->conn_state |= L2CAP_CONN_SEND_FBIT;
4215
4216         if (!(pi->conn_state & L2CAP_CONN_SREJ_SENT)) {
4217                 del_timer(&pi->retrans_timer);
4218                 if (rx_control & L2CAP_CTRL_POLL)
4219                         l2cap_send_rr_or_rnr(pi, L2CAP_CTRL_FINAL);
4220                 return;
4221         }
4222
4223         if (rx_control & L2CAP_CTRL_POLL)
4224                 l2cap_send_srejtail(sk);
4225         else
4226                 l2cap_send_sframe(pi, L2CAP_SUPER_RCV_READY);
4227 }
4228
4229 static inline int l2cap_data_channel_sframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
4230 {
4231         BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
4232
4233         if (L2CAP_CTRL_FINAL & rx_control &&
4234                         l2cap_pi(sk)->conn_state & L2CAP_CONN_WAIT_F) {
4235                 del_timer(&l2cap_pi(sk)->monitor_timer);
4236                 if (l2cap_pi(sk)->unacked_frames > 0)
4237                         __mod_retrans_timer();
4238                 l2cap_pi(sk)->conn_state &= ~L2CAP_CONN_WAIT_F;
4239         }
4240
4241         switch (rx_control & L2CAP_CTRL_SUPERVISE) {
4242         case L2CAP_SUPER_RCV_READY:
4243                 l2cap_data_channel_rrframe(sk, rx_control);
4244                 break;
4245
4246         case L2CAP_SUPER_REJECT:
4247                 l2cap_data_channel_rejframe(sk, rx_control);
4248                 break;
4249
4250         case L2CAP_SUPER_SELECT_REJECT:
4251                 l2cap_data_channel_srejframe(sk, rx_control);
4252                 break;
4253
4254         case L2CAP_SUPER_RCV_NOT_READY:
4255                 l2cap_data_channel_rnrframe(sk, rx_control);
4256                 break;
4257         }
4258
4259         kfree_skb(skb);
4260         return 0;
4261 }
4262
4263 static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb)
4264 {
4265         struct l2cap_pinfo *pi = l2cap_pi(sk);
4266         u16 control;
4267         u8 req_seq;
4268         int len, next_tx_seq_offset, req_seq_offset;
4269
4270         control = get_unaligned_le16(skb->data);
4271         skb_pull(skb, 2);
4272         len = skb->len;
4273
4274         /*
4275          * We can just drop the corrupted I-frame here.
4276          * Receiver will miss it and start proper recovery
4277          * procedures and ask retransmission.
4278          */
4279         if (l2cap_check_fcs(pi, skb))
4280                 goto drop;
4281
4282         if (__is_sar_start(control) && __is_iframe(control))
4283                 len -= 2;
4284
4285         if (pi->fcs == L2CAP_FCS_CRC16)
4286                 len -= 2;
4287
4288         if (len > pi->mps) {
4289                 l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
4290                 goto drop;
4291         }
4292
4293         req_seq = __get_reqseq(control);
4294         req_seq_offset = (req_seq - pi->expected_ack_seq) % 64;
4295         if (req_seq_offset < 0)
4296                 req_seq_offset += 64;
4297
4298         next_tx_seq_offset =
4299                 (pi->next_tx_seq - pi->expected_ack_seq) % 64;
4300         if (next_tx_seq_offset < 0)
4301                 next_tx_seq_offset += 64;
4302
4303         /* check for invalid req-seq */
4304         if (req_seq_offset > next_tx_seq_offset) {
4305                 l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
4306                 goto drop;
4307         }
4308
4309         if (__is_iframe(control)) {
4310                 if (len < 0) {
4311                         l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
4312                         goto drop;
4313                 }
4314
4315                 l2cap_data_channel_iframe(sk, control, skb);
4316         } else {
4317                 if (len != 0) {
4318                         BT_ERR("%d", len);
4319                         l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
4320                         goto drop;
4321                 }
4322
4323                 l2cap_data_channel_sframe(sk, control, skb);
4324         }
4325
4326         return 0;
4327
4328 drop:
4329         kfree_skb(skb);
4330         return 0;
4331 }
4332
4333 static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
4334 {
4335         struct sock *sk;
4336         struct l2cap_pinfo *pi;
4337         u16 control;
4338         u8 tx_seq;
4339         int len;
4340
4341         sk = l2cap_get_chan_by_scid(&conn->chan_list, cid);
4342         if (!sk) {
4343                 BT_DBG("unknown cid 0x%4.4x", cid);
4344                 goto drop;
4345         }
4346
4347         pi = l2cap_pi(sk);
4348
4349         BT_DBG("sk %p, len %d", sk, skb->len);
4350
4351         if (sk->sk_state != BT_CONNECTED)
4352                 goto drop;
4353
4354         switch (pi->mode) {
4355         case L2CAP_MODE_BASIC:
4356                 /* If socket recv buffers overflows we drop data here
4357                  * which is *bad* because L2CAP has to be reliable.
4358                  * But we don't have any other choice. L2CAP doesn't
4359                  * provide flow control mechanism. */
4360
4361                 if (pi->imtu < skb->len)
4362                         goto drop;
4363
4364                 if (!sock_queue_rcv_skb(sk, skb))
4365                         goto done;
4366                 break;
4367
4368         case L2CAP_MODE_ERTM:
4369                 if (!sock_owned_by_user(sk)) {
4370                         l2cap_ertm_data_rcv(sk, skb);
4371                 } else {
4372                         if (sk_add_backlog(sk, skb))
4373                                 goto drop;
4374                 }
4375
4376                 goto done;
4377
4378         case L2CAP_MODE_STREAMING:
4379                 control = get_unaligned_le16(skb->data);
4380                 skb_pull(skb, 2);
4381                 len = skb->len;
4382
4383                 if (l2cap_check_fcs(pi, skb))
4384                         goto drop;
4385
4386                 if (__is_sar_start(control))
4387                         len -= 2;
4388
4389                 if (pi->fcs == L2CAP_FCS_CRC16)
4390                         len -= 2;
4391
4392                 if (len > pi->mps || len < 0 || __is_sframe(control))
4393                         goto drop;
4394
4395                 tx_seq = __get_txseq(control);
4396
4397                 if (pi->expected_tx_seq == tx_seq)
4398                         pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
4399                 else
4400                         pi->expected_tx_seq = (tx_seq + 1) % 64;
4401
4402                 l2cap_streaming_reassembly_sdu(sk, skb, control);
4403
4404                 goto done;
4405
4406         default:
4407                 BT_DBG("sk %p: bad mode 0x%2.2x", sk, pi->mode);
4408                 break;
4409         }
4410
4411 drop:
4412         kfree_skb(skb);
4413
4414 done:
4415         if (sk)
4416                 bh_unlock_sock(sk);
4417
4418         return 0;
4419 }
4420
4421 static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb)
4422 {
4423         struct sock *sk;
4424
4425         sk = l2cap_get_sock_by_psm(0, psm, conn->src);
4426         if (!sk)
4427                 goto drop;
4428
4429         BT_DBG("sk %p, len %d", sk, skb->len);
4430
4431         if (sk->sk_state != BT_BOUND && sk->sk_state != BT_CONNECTED)
4432                 goto drop;
4433
4434         if (l2cap_pi(sk)->imtu < skb->len)
4435                 goto drop;
4436
4437         if (!sock_queue_rcv_skb(sk, skb))
4438                 goto done;
4439
4440 drop:
4441         kfree_skb(skb);
4442
4443 done:
4444         if (sk)
4445                 bh_unlock_sock(sk);
4446         return 0;
4447 }
4448
4449 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
4450 {
4451         struct l2cap_hdr *lh = (void *) skb->data;
4452         u16 cid, len;
4453         __le16 psm;
4454
4455         skb_pull(skb, L2CAP_HDR_SIZE);
4456         cid = __le16_to_cpu(lh->cid);
4457         len = __le16_to_cpu(lh->len);
4458
4459         if (len != skb->len) {
4460                 kfree_skb(skb);
4461                 return;
4462         }
4463
4464         BT_DBG("len %d, cid 0x%4.4x", len, cid);
4465
4466         switch (cid) {
4467         case L2CAP_CID_SIGNALING:
4468                 l2cap_sig_channel(conn, skb);
4469                 break;
4470
4471         case L2CAP_CID_CONN_LESS:
4472                 psm = get_unaligned_le16(skb->data);
4473                 skb_pull(skb, 2);
4474                 l2cap_conless_channel(conn, psm, skb);
4475                 break;
4476
4477         default:
4478                 l2cap_data_channel(conn, cid, skb);
4479                 break;
4480         }
4481 }
4482
4483 /* ---- L2CAP interface with lower layer (HCI) ---- */
4484
4485 static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
4486 {
4487         int exact = 0, lm1 = 0, lm2 = 0;
4488         register struct sock *sk;
4489         struct hlist_node *node;
4490
4491         if (type != ACL_LINK)
4492                 return -EINVAL;
4493
4494         BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
4495
4496         /* Find listening sockets and check their link_mode */
4497         read_lock(&l2cap_sk_list.lock);
4498         sk_for_each(sk, node, &l2cap_sk_list.head) {
4499                 if (sk->sk_state != BT_LISTEN)
4500                         continue;
4501
4502                 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
4503                         lm1 |= HCI_LM_ACCEPT;
4504                         if (l2cap_pi(sk)->role_switch)
4505                                 lm1 |= HCI_LM_MASTER;
4506                         exact++;
4507                 } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
4508                         lm2 |= HCI_LM_ACCEPT;
4509                         if (l2cap_pi(sk)->role_switch)
4510                                 lm2 |= HCI_LM_MASTER;
4511                 }
4512         }
4513         read_unlock(&l2cap_sk_list.lock);
4514
4515         return exact ? lm1 : lm2;
4516 }
4517
4518 static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
4519 {
4520         struct l2cap_conn *conn;
4521
4522         BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
4523
4524         if (hcon->type != ACL_LINK)
4525                 return -EINVAL;
4526
4527         if (!status) {
4528                 conn = l2cap_conn_add(hcon, status);
4529                 if (conn)
4530                         l2cap_conn_ready(conn);
4531         } else
4532                 l2cap_conn_del(hcon, bt_err(status));
4533
4534         return 0;
4535 }
4536
4537 static int l2cap_disconn_ind(struct hci_conn *hcon)
4538 {
4539         struct l2cap_conn *conn = hcon->l2cap_data;
4540
4541         BT_DBG("hcon %p", hcon);
4542
4543         if (hcon->type != ACL_LINK || !conn)
4544                 return 0x13;
4545
4546         return conn->disc_reason;
4547 }
4548
4549 static int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
4550 {
4551         BT_DBG("hcon %p reason %d", hcon, reason);
4552
4553         if (hcon->type != ACL_LINK)
4554                 return -EINVAL;
4555
4556         l2cap_conn_del(hcon, bt_err(reason));
4557
4558         return 0;
4559 }
4560
4561 static inline void l2cap_check_encryption(struct sock *sk, u8 encrypt)
4562 {
4563         if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM)
4564                 return;
4565
4566         if (encrypt == 0x00) {
4567                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_MEDIUM) {
4568                         l2cap_sock_clear_timer(sk);
4569                         l2cap_sock_set_timer(sk, HZ * 5);
4570                 } else if (l2cap_pi(sk)->sec_level == BT_SECURITY_HIGH)
4571                         __l2cap_sock_close(sk, ECONNREFUSED);
4572         } else {
4573                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_MEDIUM)
4574                         l2cap_sock_clear_timer(sk);
4575         }
4576 }
4577
4578 static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
4579 {
4580         struct l2cap_chan_list *l;
4581         struct l2cap_conn *conn = hcon->l2cap_data;
4582         struct sock *sk;
4583
4584         if (!conn)
4585                 return 0;
4586
4587         l = &conn->chan_list;
4588
4589         BT_DBG("conn %p", conn);
4590
4591         read_lock(&l->lock);
4592
4593         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
4594                 bh_lock_sock(sk);
4595
4596                 if (l2cap_pi(sk)->conf_state & L2CAP_CONF_CONNECT_PEND) {
4597                         bh_unlock_sock(sk);
4598                         continue;
4599                 }
4600
4601                 if (!status && (sk->sk_state == BT_CONNECTED ||
4602                                                 sk->sk_state == BT_CONFIG)) {
4603                         l2cap_check_encryption(sk, encrypt);
4604                         bh_unlock_sock(sk);
4605                         continue;
4606                 }
4607
4608                 if (sk->sk_state == BT_CONNECT) {
4609                         if (!status) {
4610                                 struct l2cap_conn_req req;
4611                                 req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
4612                                 req.psm  = l2cap_pi(sk)->psm;
4613
4614                                 l2cap_pi(sk)->ident = l2cap_get_ident(conn);
4615                                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND;
4616
4617                                 l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
4618                                         L2CAP_CONN_REQ, sizeof(req), &req);
4619                         } else {
4620                                 l2cap_sock_clear_timer(sk);
4621                                 l2cap_sock_set_timer(sk, HZ / 10);
4622                         }
4623                 } else if (sk->sk_state == BT_CONNECT2) {
4624                         struct l2cap_conn_rsp rsp;
4625                         __u16 result;
4626
4627                         if (!status) {
4628                                 sk->sk_state = BT_CONFIG;
4629                                 result = L2CAP_CR_SUCCESS;
4630                         } else {
4631                                 sk->sk_state = BT_DISCONN;
4632                                 l2cap_sock_set_timer(sk, HZ / 10);
4633                                 result = L2CAP_CR_SEC_BLOCK;
4634                         }
4635
4636                         rsp.scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
4637                         rsp.dcid   = cpu_to_le16(l2cap_pi(sk)->scid);
4638                         rsp.result = cpu_to_le16(result);
4639                         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
4640                         l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
4641                                         L2CAP_CONN_RSP, sizeof(rsp), &rsp);
4642                 }
4643
4644                 bh_unlock_sock(sk);
4645         }
4646
4647         read_unlock(&l->lock);
4648
4649         return 0;
4650 }
4651
4652 static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
4653 {
4654         struct l2cap_conn *conn = hcon->l2cap_data;
4655
4656         if (!conn && !(conn = l2cap_conn_add(hcon, 0)))
4657                 goto drop;
4658
4659         BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
4660
4661         if (flags & ACL_START) {
4662                 struct l2cap_hdr *hdr;
4663                 int len;
4664
4665                 if (conn->rx_len) {
4666                         BT_ERR("Unexpected start frame (len %d)", skb->len);
4667                         kfree_skb(conn->rx_skb);
4668                         conn->rx_skb = NULL;
4669                         conn->rx_len = 0;
4670                         l2cap_conn_unreliable(conn, ECOMM);
4671                 }
4672
4673                 if (skb->len < 2) {
4674                         BT_ERR("Frame is too short (len %d)", skb->len);
4675                         l2cap_conn_unreliable(conn, ECOMM);
4676                         goto drop;
4677                 }
4678
4679                 hdr = (struct l2cap_hdr *) skb->data;
4680                 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
4681
4682                 if (len == skb->len) {
4683                         /* Complete frame received */
4684                         l2cap_recv_frame(conn, skb);
4685                         return 0;
4686                 }
4687
4688                 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
4689
4690                 if (skb->len > len) {
4691                         BT_ERR("Frame is too long (len %d, expected len %d)",
4692                                 skb->len, len);
4693                         l2cap_conn_unreliable(conn, ECOMM);
4694                         goto drop;
4695                 }
4696
4697                 /* Allocate skb for the complete frame (with header) */
4698                 conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC);
4699                 if (!conn->rx_skb)
4700                         goto drop;
4701
4702                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
4703                                                                 skb->len);
4704                 conn->rx_len = len - skb->len;
4705         } else {
4706                 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
4707
4708                 if (!conn->rx_len) {
4709                         BT_ERR("Unexpected continuation frame (len %d)", skb->len);
4710                         l2cap_conn_unreliable(conn, ECOMM);
4711                         goto drop;
4712                 }
4713
4714                 if (skb->len > conn->rx_len) {
4715                         BT_ERR("Fragment is too long (len %d, expected %d)",
4716                                         skb->len, conn->rx_len);
4717                         kfree_skb(conn->rx_skb);
4718                         conn->rx_skb = NULL;
4719                         conn->rx_len = 0;
4720                         l2cap_conn_unreliable(conn, ECOMM);
4721                         goto drop;
4722                 }
4723
4724                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
4725                                                                 skb->len);
4726                 conn->rx_len -= skb->len;
4727
4728                 if (!conn->rx_len) {
4729                         /* Complete frame received */
4730                         l2cap_recv_frame(conn, conn->rx_skb);
4731                         conn->rx_skb = NULL;
4732                 }
4733         }
4734
4735 drop:
4736         kfree_skb(skb);
4737         return 0;
4738 }
4739
4740 static int l2cap_debugfs_show(struct seq_file *f, void *p)
4741 {
4742         struct sock *sk;
4743         struct hlist_node *node;
4744
4745         read_lock_bh(&l2cap_sk_list.lock);
4746
4747         sk_for_each(sk, node, &l2cap_sk_list.head) {
4748                 struct l2cap_pinfo *pi = l2cap_pi(sk);
4749
4750                 seq_printf(f, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d\n",
4751                                         batostr(&bt_sk(sk)->src),
4752                                         batostr(&bt_sk(sk)->dst),
4753                                         sk->sk_state, __le16_to_cpu(pi->psm),
4754                                         pi->scid, pi->dcid,
4755                                         pi->imtu, pi->omtu, pi->sec_level);
4756         }
4757
4758         read_unlock_bh(&l2cap_sk_list.lock);
4759
4760         return 0;
4761 }
4762
4763 static int l2cap_debugfs_open(struct inode *inode, struct file *file)
4764 {
4765         return single_open(file, l2cap_debugfs_show, inode->i_private);
4766 }
4767
4768 static const struct file_operations l2cap_debugfs_fops = {
4769         .open           = l2cap_debugfs_open,
4770         .read           = seq_read,
4771         .llseek         = seq_lseek,
4772         .release        = single_release,
4773 };
4774
4775 static struct dentry *l2cap_debugfs;
4776
4777 static const struct proto_ops l2cap_sock_ops = {
4778         .family         = PF_BLUETOOTH,
4779         .owner          = THIS_MODULE,
4780         .release        = l2cap_sock_release,
4781         .bind           = l2cap_sock_bind,
4782         .connect        = l2cap_sock_connect,
4783         .listen         = l2cap_sock_listen,
4784         .accept         = l2cap_sock_accept,
4785         .getname        = l2cap_sock_getname,
4786         .sendmsg        = l2cap_sock_sendmsg,
4787         .recvmsg        = l2cap_sock_recvmsg,
4788         .poll           = bt_sock_poll,
4789         .ioctl          = bt_sock_ioctl,
4790         .mmap           = sock_no_mmap,
4791         .socketpair     = sock_no_socketpair,
4792         .shutdown       = l2cap_sock_shutdown,
4793         .setsockopt     = l2cap_sock_setsockopt,
4794         .getsockopt     = l2cap_sock_getsockopt
4795 };
4796
4797 static const struct net_proto_family l2cap_sock_family_ops = {
4798         .family = PF_BLUETOOTH,
4799         .owner  = THIS_MODULE,
4800         .create = l2cap_sock_create,
4801 };
4802
4803 static struct hci_proto l2cap_hci_proto = {
4804         .name           = "L2CAP",
4805         .id             = HCI_PROTO_L2CAP,
4806         .connect_ind    = l2cap_connect_ind,
4807         .connect_cfm    = l2cap_connect_cfm,
4808         .disconn_ind    = l2cap_disconn_ind,
4809         .disconn_cfm    = l2cap_disconn_cfm,
4810         .security_cfm   = l2cap_security_cfm,
4811         .recv_acldata   = l2cap_recv_acldata
4812 };
4813
4814 static int __init l2cap_init(void)
4815 {
4816         int err;
4817
4818         err = proto_register(&l2cap_proto, 0);
4819         if (err < 0)
4820                 return err;
4821
4822         _busy_wq = create_singlethread_workqueue("l2cap");
4823         if (!_busy_wq)
4824                 goto error;
4825
4826         err = bt_sock_register(BTPROTO_L2CAP, &l2cap_sock_family_ops);
4827         if (err < 0) {
4828                 BT_ERR("L2CAP socket registration failed");
4829                 goto error;
4830         }
4831
4832         err = hci_register_proto(&l2cap_hci_proto);
4833         if (err < 0) {
4834                 BT_ERR("L2CAP protocol registration failed");
4835                 bt_sock_unregister(BTPROTO_L2CAP);
4836                 goto error;
4837         }
4838
4839         if (bt_debugfs) {
4840                 l2cap_debugfs = debugfs_create_file("l2cap", 0444,
4841                                         bt_debugfs, NULL, &l2cap_debugfs_fops);
4842                 if (!l2cap_debugfs)
4843                         BT_ERR("Failed to create L2CAP debug file");
4844         }
4845
4846         BT_INFO("L2CAP ver %s", VERSION);
4847         BT_INFO("L2CAP socket layer initialized");
4848
4849         return 0;
4850
4851 error:
4852         proto_unregister(&l2cap_proto);
4853         return err;
4854 }
4855
4856 static void __exit l2cap_exit(void)
4857 {
4858         debugfs_remove(l2cap_debugfs);
4859
4860         flush_workqueue(_busy_wq);
4861         destroy_workqueue(_busy_wq);
4862
4863         if (bt_sock_unregister(BTPROTO_L2CAP) < 0)
4864                 BT_ERR("L2CAP socket unregistration failed");
4865
4866         if (hci_unregister_proto(&l2cap_hci_proto) < 0)
4867                 BT_ERR("L2CAP protocol unregistration failed");
4868
4869         proto_unregister(&l2cap_proto);
4870 }
4871
4872 void l2cap_load(void)
4873 {
4874         /* Dummy function to trigger automatic L2CAP module loading by
4875          * other modules that use L2CAP sockets but don't use any other
4876          * symbols from it. */
4877 }
4878 EXPORT_SYMBOL(l2cap_load);
4879
4880 module_init(l2cap_init);
4881 module_exit(l2cap_exit);
4882
4883 module_param(disable_ertm, bool, 0644);
4884 MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode");
4885
4886 MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
4887 MODULE_DESCRIPTION("Bluetooth L2CAP ver " VERSION);
4888 MODULE_VERSION(VERSION);
4889 MODULE_LICENSE("GPL");
4890 MODULE_ALIAS("bt-proto-0");