net/bluetooth/l2cap.c

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