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_sleep(sk), &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_sleep(sk), &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         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3383
3384         spin_lock_bh(&pi->send_lock);
3385         l2cap_ertm_send(sk);
3386         spin_unlock_bh(&pi->send_lock);
3387
3388         if (!(pi->conn_state & L2CAP_CONN_LOCAL_BUSY) &&
3389                         pi->frames_sent == 0) {
3390                 control |= L2CAP_SUPER_RCV_READY;
3391                 l2cap_send_sframe(pi, control);
3392         }
3393 }
3394
3395 static int l2cap_add_to_srej_queue(struct sock *sk, struct sk_buff *skb, u8 tx_seq, u8 sar)
3396 {
3397         struct sk_buff *next_skb;
3398
3399         bt_cb(skb)->tx_seq = tx_seq;
3400         bt_cb(skb)->sar = sar;
3401
3402         next_skb = skb_peek(SREJ_QUEUE(sk));
3403         if (!next_skb) {
3404                 __skb_queue_tail(SREJ_QUEUE(sk), skb);
3405                 return 0;
3406         }
3407
3408         do {
3409                 if (bt_cb(next_skb)->tx_seq == tx_seq)
3410                         return -EINVAL;
3411
3412                 if (bt_cb(next_skb)->tx_seq > tx_seq) {
3413                         __skb_queue_before(SREJ_QUEUE(sk), next_skb, skb);
3414                         return 0;
3415                 }
3416
3417                 if (skb_queue_is_last(SREJ_QUEUE(sk), next_skb))
3418                         break;
3419
3420         } while ((next_skb = skb_queue_next(SREJ_QUEUE(sk), next_skb)));
3421
3422         __skb_queue_tail(SREJ_QUEUE(sk), skb);
3423
3424         return 0;
3425 }
3426
3427 static int l2cap_ertm_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)
3428 {
3429         struct l2cap_pinfo *pi = l2cap_pi(sk);
3430         struct sk_buff *_skb;
3431         int err;
3432
3433         switch (control & L2CAP_CTRL_SAR) {
3434         case L2CAP_SDU_UNSEGMENTED:
3435                 if (pi->conn_state & L2CAP_CONN_SAR_SDU)
3436                         goto drop;
3437
3438                 err = sock_queue_rcv_skb(sk, skb);
3439                 if (!err)
3440                         return err;
3441
3442                 break;
3443
3444         case L2CAP_SDU_START:
3445                 if (pi->conn_state & L2CAP_CONN_SAR_SDU)
3446                         goto drop;
3447
3448                 pi->sdu_len = get_unaligned_le16(skb->data);
3449
3450                 if (pi->sdu_len > pi->imtu)
3451                         goto disconnect;
3452
3453                 pi->sdu = bt_skb_alloc(pi->sdu_len, GFP_ATOMIC);
3454                 if (!pi->sdu)
3455                         return -ENOMEM;
3456
3457                 /* pull sdu_len bytes only after alloc, because of Local Busy
3458                  * condition we have to be sure that this will be executed
3459                  * only once, i.e., when alloc does not fail */
3460                 skb_pull(skb, 2);
3461
3462                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3463
3464                 pi->conn_state |= L2CAP_CONN_SAR_SDU;
3465                 pi->partial_sdu_len = skb->len;
3466                 break;
3467
3468         case L2CAP_SDU_CONTINUE:
3469                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
3470                         goto disconnect;
3471
3472                 if (!pi->sdu)
3473                         goto disconnect;
3474
3475                 pi->partial_sdu_len += skb->len;
3476                 if (pi->partial_sdu_len > pi->sdu_len)
3477                         goto drop;
3478
3479                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3480
3481                 break;
3482
3483         case L2CAP_SDU_END:
3484                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
3485                         goto disconnect;
3486
3487                 if (!pi->sdu)
3488                         goto disconnect;
3489
3490                 if (!(pi->conn_state & L2CAP_CONN_SAR_RETRY)) {
3491                         pi->partial_sdu_len += skb->len;
3492
3493                         if (pi->partial_sdu_len > pi->imtu)
3494                                 goto drop;
3495
3496                         if (pi->partial_sdu_len != pi->sdu_len)
3497                                 goto drop;
3498
3499                         memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3500                 }
3501
3502                 _skb = skb_clone(pi->sdu, GFP_ATOMIC);
3503                 if (!_skb) {
3504                         pi->conn_state |= L2CAP_CONN_SAR_RETRY;
3505                         return -ENOMEM;
3506                 }
3507
3508                 err = sock_queue_rcv_skb(sk, _skb);
3509                 if (err < 0) {
3510                         kfree_skb(_skb);
3511                         pi->conn_state |= L2CAP_CONN_SAR_RETRY;
3512                         return err;
3513                 }
3514
3515                 pi->conn_state &= ~L2CAP_CONN_SAR_RETRY;
3516                 pi->conn_state &= ~L2CAP_CONN_SAR_SDU;
3517
3518                 kfree_skb(pi->sdu);
3519                 break;
3520         }
3521
3522         kfree_skb(skb);
3523         return 0;
3524
3525 drop:
3526         kfree_skb(pi->sdu);
3527         pi->sdu = NULL;
3528
3529 disconnect:
3530         l2cap_send_disconn_req(pi->conn, sk);
3531         kfree_skb(skb);
3532         return 0;
3533 }
3534
3535 static void l2cap_busy_work(struct work_struct *work)
3536 {
3537         DECLARE_WAITQUEUE(wait, current);
3538         struct l2cap_pinfo *pi =
3539                 container_of(work, struct l2cap_pinfo, busy_work);
3540         struct sock *sk = (struct sock *)pi;
3541         int n_tries = 0, timeo = HZ/5, err;
3542         struct sk_buff *skb;
3543         u16 control;
3544
3545         lock_sock(sk);
3546
3547         add_wait_queue(sk_sleep(sk), &wait);
3548         while ((skb = skb_peek(BUSY_QUEUE(sk)))) {
3549                 set_current_state(TASK_INTERRUPTIBLE);
3550
3551                 if (n_tries++ > L2CAP_LOCAL_BUSY_TRIES) {
3552                         err = -EBUSY;
3553                         l2cap_send_disconn_req(pi->conn, sk);
3554                         goto done;
3555                 }
3556
3557                 if (!timeo)
3558                         timeo = HZ/5;
3559
3560                 if (signal_pending(current)) {
3561                         err = sock_intr_errno(timeo);
3562                         goto done;
3563                 }
3564
3565                 release_sock(sk);
3566                 timeo = schedule_timeout(timeo);
3567                 lock_sock(sk);
3568
3569                 err = sock_error(sk);
3570                 if (err)
3571                         goto done;
3572
3573                 while ((skb = skb_dequeue(BUSY_QUEUE(sk)))) {
3574                         control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
3575                         err = l2cap_ertm_reassembly_sdu(sk, skb, control);
3576                         if (err < 0) {
3577                                 skb_queue_head(BUSY_QUEUE(sk), skb);
3578                                 break;
3579                         }
3580
3581                         pi->buffer_seq = (pi->buffer_seq + 1) % 64;
3582                 }
3583
3584                 if (!skb)
3585                         break;
3586         }
3587
3588         if (!(pi->conn_state & L2CAP_CONN_RNR_SENT))
3589                 goto done;
3590
3591         control = pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3592         control |= L2CAP_SUPER_RCV_READY | L2CAP_CTRL_POLL;
3593         l2cap_send_sframe(pi, control);
3594         l2cap_pi(sk)->retry_count = 1;
3595
3596         del_timer(&pi->retrans_timer);
3597         __mod_monitor_timer();
3598
3599         l2cap_pi(sk)->conn_state |= L2CAP_CONN_WAIT_F;
3600
3601 done:
3602         pi->conn_state &= ~L2CAP_CONN_LOCAL_BUSY;
3603         pi->conn_state &= ~L2CAP_CONN_RNR_SENT;
3604
3605         set_current_state(TASK_RUNNING);
3606         remove_wait_queue(sk_sleep(sk), &wait);
3607
3608         release_sock(sk);
3609 }
3610
3611 static int l2cap_push_rx_skb(struct sock *sk, struct sk_buff *skb, u16 control)
3612 {
3613         struct l2cap_pinfo *pi = l2cap_pi(sk);
3614         int sctrl, err;
3615
3616         if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
3617                 bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT;
3618                 __skb_queue_tail(BUSY_QUEUE(sk), skb);
3619                 return -EBUSY;
3620         }
3621
3622         err = l2cap_ertm_reassembly_sdu(sk, skb, control);
3623         if (err >= 0) {
3624                 pi->buffer_seq = (pi->buffer_seq + 1) % 64;
3625                 return err;
3626         }
3627
3628         /* Busy Condition */
3629         pi->conn_state |= L2CAP_CONN_LOCAL_BUSY;
3630         bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT;
3631         __skb_queue_tail(BUSY_QUEUE(sk), skb);
3632
3633         sctrl = pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3634         sctrl |= L2CAP_SUPER_RCV_NOT_READY;
3635         l2cap_send_sframe(pi, sctrl);
3636
3637         pi->conn_state |= L2CAP_CONN_RNR_SENT;
3638
3639         queue_work(_busy_wq, &pi->busy_work);
3640
3641         return err;
3642 }
3643
3644 static int l2cap_streaming_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)
3645 {
3646         struct l2cap_pinfo *pi = l2cap_pi(sk);
3647         struct sk_buff *_skb;
3648         int err = -EINVAL;
3649
3650         /*
3651          * TODO: We have to notify the userland if some data is lost with the
3652          * Streaming Mode.
3653          */
3654
3655         switch (control & L2CAP_CTRL_SAR) {
3656         case L2CAP_SDU_UNSEGMENTED:
3657                 if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
3658                         kfree_skb(pi->sdu);
3659                         break;
3660                 }
3661
3662                 err = sock_queue_rcv_skb(sk, skb);
3663                 if (!err)
3664                         return 0;
3665
3666                 break;
3667
3668         case L2CAP_SDU_START:
3669                 if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
3670                         kfree_skb(pi->sdu);
3671                         break;
3672                 }
3673
3674                 pi->sdu_len = get_unaligned_le16(skb->data);
3675                 skb_pull(skb, 2);
3676
3677                 if (pi->sdu_len > pi->imtu) {
3678                         err = -EMSGSIZE;
3679                         break;
3680                 }
3681
3682                 pi->sdu = bt_skb_alloc(pi->sdu_len, GFP_ATOMIC);
3683                 if (!pi->sdu) {
3684                         err = -ENOMEM;
3685                         break;
3686                 }
3687
3688                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3689
3690                 pi->conn_state |= L2CAP_CONN_SAR_SDU;
3691                 pi->partial_sdu_len = skb->len;
3692                 err = 0;
3693                 break;
3694
3695         case L2CAP_SDU_CONTINUE:
3696                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
3697                         break;
3698
3699                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3700
3701                 pi->partial_sdu_len += skb->len;
3702                 if (pi->partial_sdu_len > pi->sdu_len)
3703                         kfree_skb(pi->sdu);
3704                 else
3705                         err = 0;
3706
3707                 break;
3708
3709         case L2CAP_SDU_END:
3710                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
3711                         break;
3712
3713                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3714
3715                 pi->conn_state &= ~L2CAP_CONN_SAR_SDU;
3716                 pi->partial_sdu_len += skb->len;
3717
3718                 if (pi->partial_sdu_len > pi->imtu)
3719                         goto drop;
3720
3721                 if (pi->partial_sdu_len == pi->sdu_len) {
3722                         _skb = skb_clone(pi->sdu, GFP_ATOMIC);
3723                         err = sock_queue_rcv_skb(sk, _skb);
3724                         if (err < 0)
3725                                 kfree_skb(_skb);
3726                 }
3727                 err = 0;
3728
3729 drop:
3730                 kfree_skb(pi->sdu);
3731                 break;
3732         }
3733
3734         kfree_skb(skb);
3735         return err;
3736 }
3737
3738 static void l2cap_check_srej_gap(struct sock *sk, u8 tx_seq)
3739 {
3740         struct sk_buff *skb;
3741         u16 control;
3742
3743         while ((skb = skb_peek(SREJ_QUEUE(sk)))) {
3744                 if (bt_cb(skb)->tx_seq != tx_seq)
3745                         break;
3746
3747                 skb = skb_dequeue(SREJ_QUEUE(sk));
3748                 control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
3749                 l2cap_ertm_reassembly_sdu(sk, skb, control);
3750                 l2cap_pi(sk)->buffer_seq_srej =
3751                         (l2cap_pi(sk)->buffer_seq_srej + 1) % 64;
3752                 tx_seq++;
3753         }
3754 }
3755
3756 static void l2cap_resend_srejframe(struct sock *sk, u8 tx_seq)
3757 {
3758         struct l2cap_pinfo *pi = l2cap_pi(sk);
3759         struct srej_list *l, *tmp;
3760         u16 control;
3761
3762         list_for_each_entry_safe(l, tmp, SREJ_LIST(sk), list) {
3763                 if (l->tx_seq == tx_seq) {
3764                         list_del(&l->list);
3765                         kfree(l);
3766                         return;
3767                 }
3768                 control = L2CAP_SUPER_SELECT_REJECT;
3769                 control |= l->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3770                 l2cap_send_sframe(pi, control);
3771                 list_del(&l->list);
3772                 list_add_tail(&l->list, SREJ_LIST(sk));
3773         }
3774 }
3775
3776 static void l2cap_send_srejframe(struct sock *sk, u8 tx_seq)
3777 {
3778         struct l2cap_pinfo *pi = l2cap_pi(sk);
3779         struct srej_list *new;
3780         u16 control;
3781
3782         while (tx_seq != pi->expected_tx_seq) {
3783                 control = L2CAP_SUPER_SELECT_REJECT;
3784                 control |= pi->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3785                 l2cap_send_sframe(pi, control);
3786
3787                 new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC);
3788                 new->tx_seq = pi->expected_tx_seq++;
3789                 list_add_tail(&new->list, SREJ_LIST(sk));
3790         }
3791         pi->expected_tx_seq++;
3792 }
3793
3794 static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
3795 {
3796         struct l2cap_pinfo *pi = l2cap_pi(sk);
3797         u8 tx_seq = __get_txseq(rx_control);
3798         u8 req_seq = __get_reqseq(rx_control);
3799         u8 sar = rx_control >> L2CAP_CTRL_SAR_SHIFT;
3800         u8 tx_seq_offset, expected_tx_seq_offset;
3801         int num_to_ack = (pi->tx_win/6) + 1;
3802         int err = 0;
3803
3804         BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
3805
3806         if (L2CAP_CTRL_FINAL & rx_control &&
3807                         l2cap_pi(sk)->conn_state & L2CAP_CONN_WAIT_F) {
3808                 del_timer(&pi->monitor_timer);
3809                 if (pi->unacked_frames > 0)
3810                         __mod_retrans_timer();
3811                 pi->conn_state &= ~L2CAP_CONN_WAIT_F;
3812         }
3813
3814         pi->expected_ack_seq = req_seq;
3815         l2cap_drop_acked_frames(sk);
3816
3817         if (tx_seq == pi->expected_tx_seq)
3818                 goto expected;
3819
3820         tx_seq_offset = (tx_seq - pi->buffer_seq) % 64;
3821         if (tx_seq_offset < 0)
3822                 tx_seq_offset += 64;
3823
3824         /* invalid tx_seq */
3825         if (tx_seq_offset >= pi->tx_win) {
3826                 l2cap_send_disconn_req(pi->conn, sk);
3827                 goto drop;
3828         }
3829
3830         if (pi->conn_state == L2CAP_CONN_LOCAL_BUSY)
3831                 goto drop;
3832
3833         if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
3834                 struct srej_list *first;
3835
3836                 first = list_first_entry(SREJ_LIST(sk),
3837                                 struct srej_list, list);
3838                 if (tx_seq == first->tx_seq) {
3839                         l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
3840                         l2cap_check_srej_gap(sk, tx_seq);
3841
3842                         list_del(&first->list);
3843                         kfree(first);
3844
3845                         if (list_empty(SREJ_LIST(sk))) {
3846                                 pi->buffer_seq = pi->buffer_seq_srej;
3847                                 pi->conn_state &= ~L2CAP_CONN_SREJ_SENT;
3848                                 l2cap_send_ack(pi);
3849                         }
3850                 } else {
3851                         struct srej_list *l;
3852
3853                         /* duplicated tx_seq */
3854                         if (l2cap_add_to_srej_queue(sk, skb, tx_seq, sar) < 0)
3855                                 goto drop;
3856
3857                         list_for_each_entry(l, SREJ_LIST(sk), list) {
3858                                 if (l->tx_seq == tx_seq) {
3859                                         l2cap_resend_srejframe(sk, tx_seq);
3860                                         return 0;
3861                                 }
3862                         }
3863                         l2cap_send_srejframe(sk, tx_seq);
3864                 }
3865         } else {
3866                 expected_tx_seq_offset =
3867                         (pi->expected_tx_seq - pi->buffer_seq) % 64;
3868                 if (expected_tx_seq_offset < 0)
3869                         expected_tx_seq_offset += 64;
3870
3871                 /* duplicated tx_seq */
3872                 if (tx_seq_offset < expected_tx_seq_offset)
3873                         goto drop;
3874
3875                 pi->conn_state |= L2CAP_CONN_SREJ_SENT;
3876
3877                 INIT_LIST_HEAD(SREJ_LIST(sk));
3878                 pi->buffer_seq_srej = pi->buffer_seq;
3879
3880                 __skb_queue_head_init(SREJ_QUEUE(sk));
3881                 __skb_queue_head_init(BUSY_QUEUE(sk));
3882                 l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
3883
3884                 pi->conn_state |= L2CAP_CONN_SEND_PBIT;
3885
3886                 l2cap_send_srejframe(sk, tx_seq);
3887         }
3888         return 0;
3889
3890 expected:
3891         pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3892
3893         if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
3894                 bt_cb(skb)->tx_seq = tx_seq;
3895                 bt_cb(skb)->sar = sar;
3896                 __skb_queue_tail(SREJ_QUEUE(sk), skb);
3897                 return 0;
3898         }
3899
3900         if (rx_control & L2CAP_CTRL_FINAL) {
3901                 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
3902                         pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
3903                 else
3904                         l2cap_retransmit_frames(sk);
3905         }
3906
3907         err = l2cap_push_rx_skb(sk, skb, rx_control);
3908         if (err < 0)
3909                 return 0;
3910
3911         __mod_ack_timer();
3912
3913         pi->num_acked = (pi->num_acked + 1) % num_to_ack;
3914         if (pi->num_acked == num_to_ack - 1)
3915                 l2cap_send_ack(pi);
3916
3917         return 0;
3918
3919 drop:
3920         kfree_skb(skb);
3921         return 0;
3922 }
3923
3924 static inline void l2cap_data_channel_rrframe(struct sock *sk, u16 rx_control)
3925 {
3926         struct l2cap_pinfo *pi = l2cap_pi(sk);
3927
3928         pi->expected_ack_seq = __get_reqseq(rx_control);
3929         l2cap_drop_acked_frames(sk);
3930
3931         if (rx_control & L2CAP_CTRL_POLL) {
3932                 if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
3933                         if ((pi->conn_state & L2CAP_CONN_REMOTE_BUSY) &&
3934                                         (pi->unacked_frames > 0))
3935                                 __mod_retrans_timer();
3936
3937                         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3938                         l2cap_send_srejtail(sk);
3939                 } else {
3940                         l2cap_send_i_or_rr_or_rnr(sk);
3941                 }
3942
3943         } else if (rx_control & L2CAP_CTRL_FINAL) {
3944                 pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3945
3946                 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
3947                         pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
3948                 else
3949                         l2cap_retransmit_frames(sk);
3950
3951         } else {
3952                 if ((pi->conn_state & L2CAP_CONN_REMOTE_BUSY) &&
3953                                 (pi->unacked_frames > 0))
3954                         __mod_retrans_timer();
3955
3956                 pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3957                 if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
3958                         l2cap_send_ack(pi);
3959                 } else {
3960                         spin_lock_bh(&pi->send_lock);
3961                         l2cap_ertm_send(sk);
3962                         spin_unlock_bh(&pi->send_lock);
3963                 }
3964         }
3965 }
3966
3967 static inline void l2cap_data_channel_rejframe(struct sock *sk, u16 rx_control)
3968 {
3969         struct l2cap_pinfo *pi = l2cap_pi(sk);
3970         u8 tx_seq = __get_reqseq(rx_control);
3971
3972         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3973
3974         pi->expected_ack_seq = tx_seq;
3975         l2cap_drop_acked_frames(sk);
3976
3977         if (rx_control & L2CAP_CTRL_FINAL) {
3978                 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
3979                         pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
3980                 else
3981                         l2cap_retransmit_frames(sk);
3982         } else {
3983                 l2cap_retransmit_frames(sk);
3984
3985                 if (pi->conn_state & L2CAP_CONN_WAIT_F)
3986                         pi->conn_state |= L2CAP_CONN_REJ_ACT;
3987         }
3988 }
3989 static inline void l2cap_data_channel_srejframe(struct sock *sk, u16 rx_control)
3990 {
3991         struct l2cap_pinfo *pi = l2cap_pi(sk);
3992         u8 tx_seq = __get_reqseq(rx_control);
3993
3994         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3995
3996         if (rx_control & L2CAP_CTRL_POLL) {
3997                 pi->expected_ack_seq = tx_seq;
3998                 l2cap_drop_acked_frames(sk);
3999                 l2cap_retransmit_one_frame(sk, tx_seq);
4000
4001                 spin_lock_bh(&pi->send_lock);
4002                 l2cap_ertm_send(sk);
4003                 spin_unlock_bh(&pi->send_lock);
4004
4005                 if (pi->conn_state & L2CAP_CONN_WAIT_F) {
4006                         pi->srej_save_reqseq = tx_seq;
4007                         pi->conn_state |= L2CAP_CONN_SREJ_ACT;
4008                 }
4009         } else if (rx_control & L2CAP_CTRL_FINAL) {
4010                 if ((pi->conn_state & L2CAP_CONN_SREJ_ACT) &&
4011                                 pi->srej_save_reqseq == tx_seq)
4012                         pi->conn_state &= ~L2CAP_CONN_SREJ_ACT;
4013                 else
4014                         l2cap_retransmit_one_frame(sk, tx_seq);
4015         } else {
4016                 l2cap_retransmit_one_frame(sk, tx_seq);
4017                 if (pi->conn_state & L2CAP_CONN_WAIT_F) {
4018                         pi->srej_save_reqseq = tx_seq;
4019                         pi->conn_state |= L2CAP_CONN_SREJ_ACT;
4020                 }
4021         }
4022 }
4023
4024 static inline void l2cap_data_channel_rnrframe(struct sock *sk, u16 rx_control)
4025 {
4026         struct l2cap_pinfo *pi = l2cap_pi(sk);
4027         u8 tx_seq = __get_reqseq(rx_control);
4028
4029         pi->conn_state |= L2CAP_CONN_REMOTE_BUSY;
4030         pi->expected_ack_seq = tx_seq;
4031         l2cap_drop_acked_frames(sk);
4032
4033         if (!(pi->conn_state & L2CAP_CONN_SREJ_SENT)) {
4034                 del_timer(&pi->retrans_timer);
4035                 if (rx_control & L2CAP_CTRL_POLL)
4036                         l2cap_send_rr_or_rnr(pi, L2CAP_CTRL_FINAL);
4037                 return;
4038         }
4039
4040         if (rx_control & L2CAP_CTRL_POLL)
4041                 l2cap_send_srejtail(sk);
4042         else
4043                 l2cap_send_sframe(pi, L2CAP_SUPER_RCV_READY);
4044 }
4045
4046 static inline int l2cap_data_channel_sframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
4047 {
4048         BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
4049
4050         if (L2CAP_CTRL_FINAL & rx_control &&
4051                         l2cap_pi(sk)->conn_state & L2CAP_CONN_WAIT_F) {
4052                 del_timer(&l2cap_pi(sk)->monitor_timer);
4053                 if (l2cap_pi(sk)->unacked_frames > 0)
4054                         __mod_retrans_timer();
4055                 l2cap_pi(sk)->conn_state &= ~L2CAP_CONN_WAIT_F;
4056         }
4057
4058         switch (rx_control & L2CAP_CTRL_SUPERVISE) {
4059         case L2CAP_SUPER_RCV_READY:
4060                 l2cap_data_channel_rrframe(sk, rx_control);
4061                 break;
4062
4063         case L2CAP_SUPER_REJECT:
4064                 l2cap_data_channel_rejframe(sk, rx_control);
4065                 break;
4066
4067         case L2CAP_SUPER_SELECT_REJECT:
4068                 l2cap_data_channel_srejframe(sk, rx_control);
4069                 break;
4070
4071         case L2CAP_SUPER_RCV_NOT_READY:
4072                 l2cap_data_channel_rnrframe(sk, rx_control);
4073                 break;
4074         }
4075
4076         kfree_skb(skb);
4077         return 0;
4078 }
4079
4080 static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
4081 {
4082         struct sock *sk;
4083         struct l2cap_pinfo *pi;
4084         u16 control, len;
4085         u8 tx_seq, req_seq, next_tx_seq_offset, req_seq_offset;
4086
4087         sk = l2cap_get_chan_by_scid(&conn->chan_list, cid);
4088         if (!sk) {
4089                 BT_DBG("unknown cid 0x%4.4x", cid);
4090                 goto drop;
4091         }
4092
4093         pi = l2cap_pi(sk);
4094
4095         BT_DBG("sk %p, len %d", sk, skb->len);
4096
4097         if (sk->sk_state != BT_CONNECTED)
4098                 goto drop;
4099
4100         switch (pi->mode) {
4101         case L2CAP_MODE_BASIC:
4102                 /* If socket recv buffers overflows we drop data here
4103                  * which is *bad* because L2CAP has to be reliable.
4104                  * But we don't have any other choice. L2CAP doesn't
4105                  * provide flow control mechanism. */
4106
4107                 if (pi->imtu < skb->len)
4108                         goto drop;
4109
4110                 if (!sock_queue_rcv_skb(sk, skb))
4111                         goto done;
4112                 break;
4113
4114         case L2CAP_MODE_ERTM:
4115                 control = get_unaligned_le16(skb->data);
4116                 skb_pull(skb, 2);
4117                 len = skb->len;
4118
4119                 if (__is_sar_start(control))
4120                         len -= 2;
4121
4122                 if (pi->fcs == L2CAP_FCS_CRC16)
4123                         len -= 2;
4124
4125                 /*
4126                  * We can just drop the corrupted I-frame here.
4127                  * Receiver will miss it and start proper recovery
4128                  * procedures and ask retransmission.
4129                  */
4130                 if (len > pi->mps) {
4131                         l2cap_send_disconn_req(pi->conn, sk);
4132                         goto drop;
4133                 }
4134
4135                 if (l2cap_check_fcs(pi, skb))
4136                         goto drop;
4137
4138                 req_seq = __get_reqseq(control);
4139                 req_seq_offset = (req_seq - pi->expected_ack_seq) % 64;
4140                 if (req_seq_offset < 0)
4141                         req_seq_offset += 64;
4142
4143                 next_tx_seq_offset =
4144                         (pi->next_tx_seq - pi->expected_ack_seq) % 64;
4145                 if (next_tx_seq_offset < 0)
4146                         next_tx_seq_offset += 64;
4147
4148                 /* check for invalid req-seq */
4149                 if (req_seq_offset > next_tx_seq_offset) {
4150                         l2cap_send_disconn_req(pi->conn, sk);
4151                         goto drop;
4152                 }
4153
4154                 if (__is_iframe(control)) {
4155                         if (len < 4) {
4156                                 l2cap_send_disconn_req(pi->conn, sk);
4157                                 goto drop;
4158                         }
4159
4160                         l2cap_data_channel_iframe(sk, control, skb);
4161                 } else {
4162                         if (len != 0) {
4163                                 l2cap_send_disconn_req(pi->conn, sk);
4164                                 goto drop;
4165                         }
4166
4167                         l2cap_data_channel_sframe(sk, control, skb);
4168                 }
4169
4170                 goto done;
4171
4172         case L2CAP_MODE_STREAMING:
4173                 control = get_unaligned_le16(skb->data);
4174                 skb_pull(skb, 2);
4175                 len = skb->len;
4176
4177                 if (__is_sar_start(control))
4178                         len -= 2;
4179
4180                 if (pi->fcs == L2CAP_FCS_CRC16)
4181                         len -= 2;
4182
4183                 if (len > pi->mps || len < 4 || __is_sframe(control))
4184                         goto drop;
4185
4186                 if (l2cap_check_fcs(pi, skb))
4187                         goto drop;
4188
4189                 tx_seq = __get_txseq(control);
4190
4191                 if (pi->expected_tx_seq == tx_seq)
4192                         pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
4193                 else
4194                         pi->expected_tx_seq = (tx_seq + 1) % 64;
4195
4196                 l2cap_streaming_reassembly_sdu(sk, skb, control);
4197
4198                 goto done;
4199
4200         default:
4201                 BT_DBG("sk %p: bad mode 0x%2.2x", sk, pi->mode);
4202                 break;
4203         }
4204
4205 drop:
4206         kfree_skb(skb);
4207
4208 done:
4209         if (sk)
4210                 bh_unlock_sock(sk);
4211
4212         return 0;
4213 }
4214
4215 static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb)
4216 {
4217         struct sock *sk;
4218
4219         sk = l2cap_get_sock_by_psm(0, psm, conn->src);
4220         if (!sk)
4221                 goto drop;
4222
4223         BT_DBG("sk %p, len %d", sk, skb->len);
4224
4225         if (sk->sk_state != BT_BOUND && sk->sk_state != BT_CONNECTED)
4226                 goto drop;
4227
4228         if (l2cap_pi(sk)->imtu < skb->len)
4229                 goto drop;
4230
4231         if (!sock_queue_rcv_skb(sk, skb))
4232                 goto done;
4233
4234 drop:
4235         kfree_skb(skb);
4236
4237 done:
4238         if (sk)
4239                 bh_unlock_sock(sk);
4240         return 0;
4241 }
4242
4243 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
4244 {
4245         struct l2cap_hdr *lh = (void *) skb->data;
4246         u16 cid, len;
4247         __le16 psm;
4248
4249         skb_pull(skb, L2CAP_HDR_SIZE);
4250         cid = __le16_to_cpu(lh->cid);
4251         len = __le16_to_cpu(lh->len);
4252
4253         if (len != skb->len) {
4254                 kfree_skb(skb);
4255                 return;
4256         }
4257
4258         BT_DBG("len %d, cid 0x%4.4x", len, cid);
4259
4260         switch (cid) {
4261         case L2CAP_CID_SIGNALING:
4262                 l2cap_sig_channel(conn, skb);
4263                 break;
4264
4265         case L2CAP_CID_CONN_LESS:
4266                 psm = get_unaligned_le16(skb->data);
4267                 skb_pull(skb, 2);
4268                 l2cap_conless_channel(conn, psm, skb);
4269                 break;
4270
4271         default:
4272                 l2cap_data_channel(conn, cid, skb);
4273                 break;
4274         }
4275 }
4276
4277 /* ---- L2CAP interface with lower layer (HCI) ---- */
4278
4279 static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
4280 {
4281         int exact = 0, lm1 = 0, lm2 = 0;
4282         register struct sock *sk;
4283         struct hlist_node *node;
4284
4285         if (type != ACL_LINK)
4286                 return 0;
4287
4288         BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
4289
4290         /* Find listening sockets and check their link_mode */
4291         read_lock(&l2cap_sk_list.lock);
4292         sk_for_each(sk, node, &l2cap_sk_list.head) {
4293                 if (sk->sk_state != BT_LISTEN)
4294                         continue;
4295
4296                 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
4297                         lm1 |= HCI_LM_ACCEPT;
4298                         if (l2cap_pi(sk)->role_switch)
4299                                 lm1 |= HCI_LM_MASTER;
4300                         exact++;
4301                 } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
4302                         lm2 |= HCI_LM_ACCEPT;
4303                         if (l2cap_pi(sk)->role_switch)
4304                                 lm2 |= HCI_LM_MASTER;
4305                 }
4306         }
4307         read_unlock(&l2cap_sk_list.lock);
4308
4309         return exact ? lm1 : lm2;
4310 }
4311
4312 static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
4313 {
4314         struct l2cap_conn *conn;
4315
4316         BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
4317
4318         if (hcon->type != ACL_LINK)
4319                 return 0;
4320
4321         if (!status) {
4322                 conn = l2cap_conn_add(hcon, status);
4323                 if (conn)
4324                         l2cap_conn_ready(conn);
4325         } else
4326                 l2cap_conn_del(hcon, bt_err(status));
4327
4328         return 0;
4329 }
4330
4331 static int l2cap_disconn_ind(struct hci_conn *hcon)
4332 {
4333         struct l2cap_conn *conn = hcon->l2cap_data;
4334
4335         BT_DBG("hcon %p", hcon);
4336
4337         if (hcon->type != ACL_LINK || !conn)
4338                 return 0x13;
4339
4340         return conn->disc_reason;
4341 }
4342
4343 static int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
4344 {
4345         BT_DBG("hcon %p reason %d", hcon, reason);
4346
4347         if (hcon->type != ACL_LINK)
4348                 return 0;
4349
4350         l2cap_conn_del(hcon, bt_err(reason));
4351
4352         return 0;
4353 }
4354
4355 static inline void l2cap_check_encryption(struct sock *sk, u8 encrypt)
4356 {
4357         if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM)
4358                 return;
4359
4360         if (encrypt == 0x00) {
4361                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_MEDIUM) {
4362                         l2cap_sock_clear_timer(sk);
4363                         l2cap_sock_set_timer(sk, HZ * 5);
4364                 } else if (l2cap_pi(sk)->sec_level == BT_SECURITY_HIGH)
4365                         __l2cap_sock_close(sk, ECONNREFUSED);
4366         } else {
4367                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_MEDIUM)
4368                         l2cap_sock_clear_timer(sk);
4369         }
4370 }
4371
4372 static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
4373 {
4374         struct l2cap_chan_list *l;
4375         struct l2cap_conn *conn = hcon->l2cap_data;
4376         struct sock *sk;
4377
4378         if (!conn)
4379                 return 0;
4380
4381         l = &conn->chan_list;
4382
4383         BT_DBG("conn %p", conn);
4384
4385         read_lock(&l->lock);
4386
4387         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
4388                 bh_lock_sock(sk);
4389
4390                 if (l2cap_pi(sk)->conf_state & L2CAP_CONF_CONNECT_PEND) {
4391                         bh_unlock_sock(sk);
4392                         continue;
4393                 }
4394
4395                 if (!status && (sk->sk_state == BT_CONNECTED ||
4396                                                 sk->sk_state == BT_CONFIG)) {
4397                         l2cap_check_encryption(sk, encrypt);
4398                         bh_unlock_sock(sk);
4399                         continue;
4400                 }
4401
4402                 if (sk->sk_state == BT_CONNECT) {
4403                         if (!status) {
4404                                 struct l2cap_conn_req req;
4405                                 req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
4406                                 req.psm  = l2cap_pi(sk)->psm;
4407
4408                                 l2cap_pi(sk)->ident = l2cap_get_ident(conn);
4409
4410                                 l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
4411                                         L2CAP_CONN_REQ, sizeof(req), &req);
4412                         } else {
4413                                 l2cap_sock_clear_timer(sk);
4414                                 l2cap_sock_set_timer(sk, HZ / 10);
4415                         }
4416                 } else if (sk->sk_state == BT_CONNECT2) {
4417                         struct l2cap_conn_rsp rsp;
4418                         __u16 result;
4419
4420                         if (!status) {
4421                                 sk->sk_state = BT_CONFIG;
4422                                 result = L2CAP_CR_SUCCESS;
4423                         } else {
4424                                 sk->sk_state = BT_DISCONN;
4425                                 l2cap_sock_set_timer(sk, HZ / 10);
4426                                 result = L2CAP_CR_SEC_BLOCK;
4427                         }
4428
4429                         rsp.scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
4430                         rsp.dcid   = cpu_to_le16(l2cap_pi(sk)->scid);
4431                         rsp.result = cpu_to_le16(result);
4432                         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
4433                         l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
4434                                         L2CAP_CONN_RSP, sizeof(rsp), &rsp);
4435                 }
4436
4437                 bh_unlock_sock(sk);
4438         }
4439
4440         read_unlock(&l->lock);
4441
4442         return 0;
4443 }
4444
4445 static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
4446 {
4447         struct l2cap_conn *conn = hcon->l2cap_data;
4448
4449         if (!conn && !(conn = l2cap_conn_add(hcon, 0)))
4450                 goto drop;
4451
4452         BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
4453
4454         if (flags & ACL_START) {
4455                 struct l2cap_hdr *hdr;
4456                 int len;
4457
4458                 if (conn->rx_len) {
4459                         BT_ERR("Unexpected start frame (len %d)", skb->len);
4460                         kfree_skb(conn->rx_skb);
4461                         conn->rx_skb = NULL;
4462                         conn->rx_len = 0;
4463                         l2cap_conn_unreliable(conn, ECOMM);
4464                 }
4465
4466                 if (skb->len < 2) {
4467                         BT_ERR("Frame is too short (len %d)", skb->len);
4468                         l2cap_conn_unreliable(conn, ECOMM);
4469                         goto drop;
4470                 }
4471
4472                 hdr = (struct l2cap_hdr *) skb->data;
4473                 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
4474
4475                 if (len == skb->len) {
4476                         /* Complete frame received */
4477                         l2cap_recv_frame(conn, skb);
4478                         return 0;
4479                 }
4480
4481                 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
4482
4483                 if (skb->len > len) {
4484                         BT_ERR("Frame is too long (len %d, expected len %d)",
4485                                 skb->len, len);
4486                         l2cap_conn_unreliable(conn, ECOMM);
4487                         goto drop;
4488                 }
4489
4490                 /* Allocate skb for the complete frame (with header) */
4491                 conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC);
4492                 if (!conn->rx_skb)
4493                         goto drop;
4494
4495                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
4496                                                                 skb->len);
4497                 conn->rx_len = len - skb->len;
4498         } else {
4499                 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
4500
4501                 if (!conn->rx_len) {
4502                         BT_ERR("Unexpected continuation frame (len %d)", skb->len);
4503                         l2cap_conn_unreliable(conn, ECOMM);
4504                         goto drop;
4505                 }
4506
4507                 if (skb->len > conn->rx_len) {
4508                         BT_ERR("Fragment is too long (len %d, expected %d)",
4509                                         skb->len, conn->rx_len);
4510                         kfree_skb(conn->rx_skb);
4511                         conn->rx_skb = NULL;
4512                         conn->rx_len = 0;
4513                         l2cap_conn_unreliable(conn, ECOMM);
4514                         goto drop;
4515                 }
4516
4517                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
4518                                                                 skb->len);
4519                 conn->rx_len -= skb->len;
4520
4521                 if (!conn->rx_len) {
4522                         /* Complete frame received */
4523                         l2cap_recv_frame(conn, conn->rx_skb);
4524                         conn->rx_skb = NULL;
4525                 }
4526         }
4527
4528 drop:
4529         kfree_skb(skb);
4530         return 0;
4531 }
4532
4533 static int l2cap_debugfs_show(struct seq_file *f, void *p)
4534 {
4535         struct sock *sk;
4536         struct hlist_node *node;
4537
4538         read_lock_bh(&l2cap_sk_list.lock);
4539
4540         sk_for_each(sk, node, &l2cap_sk_list.head) {
4541                 struct l2cap_pinfo *pi = l2cap_pi(sk);
4542
4543                 seq_printf(f, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d\n",
4544                                         batostr(&bt_sk(sk)->src),
4545                                         batostr(&bt_sk(sk)->dst),
4546                                         sk->sk_state, __le16_to_cpu(pi->psm),
4547                                         pi->scid, pi->dcid,
4548                                         pi->imtu, pi->omtu, pi->sec_level);
4549         }
4550
4551         read_unlock_bh(&l2cap_sk_list.lock);
4552
4553         return 0;
4554 }
4555
4556 static int l2cap_debugfs_open(struct inode *inode, struct file *file)
4557 {
4558         return single_open(file, l2cap_debugfs_show, inode->i_private);
4559 }
4560
4561 static const struct file_operations l2cap_debugfs_fops = {
4562         .open           = l2cap_debugfs_open,
4563         .read           = seq_read,
4564         .llseek         = seq_lseek,
4565         .release        = single_release,
4566 };
4567
4568 static struct dentry *l2cap_debugfs;
4569
4570 static const struct proto_ops l2cap_sock_ops = {
4571         .family         = PF_BLUETOOTH,
4572         .owner          = THIS_MODULE,
4573         .release        = l2cap_sock_release,
4574         .bind           = l2cap_sock_bind,
4575         .connect        = l2cap_sock_connect,
4576         .listen         = l2cap_sock_listen,
4577         .accept         = l2cap_sock_accept,
4578         .getname        = l2cap_sock_getname,
4579         .sendmsg        = l2cap_sock_sendmsg,
4580         .recvmsg        = l2cap_sock_recvmsg,
4581         .poll           = bt_sock_poll,
4582         .ioctl          = bt_sock_ioctl,
4583         .mmap           = sock_no_mmap,
4584         .socketpair     = sock_no_socketpair,
4585         .shutdown       = l2cap_sock_shutdown,
4586         .setsockopt     = l2cap_sock_setsockopt,
4587         .getsockopt     = l2cap_sock_getsockopt
4588 };
4589
4590 static const struct net_proto_family l2cap_sock_family_ops = {
4591         .family = PF_BLUETOOTH,
4592         .owner  = THIS_MODULE,
4593         .create = l2cap_sock_create,
4594 };
4595
4596 static struct hci_proto l2cap_hci_proto = {
4597         .name           = "L2CAP",
4598         .id             = HCI_PROTO_L2CAP,
4599         .connect_ind    = l2cap_connect_ind,
4600         .connect_cfm    = l2cap_connect_cfm,
4601         .disconn_ind    = l2cap_disconn_ind,
4602         .disconn_cfm    = l2cap_disconn_cfm,
4603         .security_cfm   = l2cap_security_cfm,
4604         .recv_acldata   = l2cap_recv_acldata
4605 };
4606
4607 static int __init l2cap_init(void)
4608 {
4609         int err;
4610
4611         err = proto_register(&l2cap_proto, 0);
4612         if (err < 0)
4613                 return err;
4614
4615         _busy_wq = create_singlethread_workqueue("l2cap");
4616         if (!_busy_wq)
4617                 goto error;
4618
4619         err = bt_sock_register(BTPROTO_L2CAP, &l2cap_sock_family_ops);
4620         if (err < 0) {
4621                 BT_ERR("L2CAP socket registration failed");
4622                 goto error;
4623         }
4624
4625         err = hci_register_proto(&l2cap_hci_proto);
4626         if (err < 0) {
4627                 BT_ERR("L2CAP protocol registration failed");
4628                 bt_sock_unregister(BTPROTO_L2CAP);
4629                 goto error;
4630         }
4631
4632         if (bt_debugfs) {
4633                 l2cap_debugfs = debugfs_create_file("l2cap", 0444,
4634                                         bt_debugfs, NULL, &l2cap_debugfs_fops);
4635                 if (!l2cap_debugfs)
4636                         BT_ERR("Failed to create L2CAP debug file");
4637         }
4638
4639         BT_INFO("L2CAP ver %s", VERSION);
4640         BT_INFO("L2CAP socket layer initialized");
4641
4642         return 0;
4643
4644 error:
4645         proto_unregister(&l2cap_proto);
4646         return err;
4647 }
4648
4649 static void __exit l2cap_exit(void)
4650 {
4651         debugfs_remove(l2cap_debugfs);
4652
4653         flush_workqueue(_busy_wq);
4654         destroy_workqueue(_busy_wq);
4655
4656         if (bt_sock_unregister(BTPROTO_L2CAP) < 0)
4657                 BT_ERR("L2CAP socket unregistration failed");
4658
4659         if (hci_unregister_proto(&l2cap_hci_proto) < 0)
4660                 BT_ERR("L2CAP protocol unregistration failed");
4661
4662         proto_unregister(&l2cap_proto);
4663 }
4664
4665 void l2cap_load(void)
4666 {
4667         /* Dummy function to trigger automatic L2CAP module loading by
4668          * other modules that use L2CAP sockets but don't use any other
4669          * symbols from it. */
4670         return;
4671 }
4672 EXPORT_SYMBOL(l2cap_load);
4673
4674 module_init(l2cap_init);
4675 module_exit(l2cap_exit);
4676
4677 module_param(enable_ertm, bool, 0644);
4678 MODULE_PARM_DESC(enable_ertm, "Enable enhanced retransmission mode");
4679
4680 module_param(max_transmit, uint, 0644);
4681 MODULE_PARM_DESC(max_transmit, "Max transmit value (default = 3)");
4682
4683 module_param(tx_window, uint, 0644);
4684 MODULE_PARM_DESC(tx_window, "Transmission window size value (default = 63)");
4685
4686 MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
4687 MODULE_DESCRIPTION("Bluetooth L2CAP ver " VERSION);
4688 MODULE_VERSION(VERSION);
4689 MODULE_LICENSE("GPL");
4690 MODULE_ALIAS("bt-proto-0");