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