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