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