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