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