net/bluetooth/l2cap.c

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