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