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