SAFE public projects git trees. - safe/jmp/linux-2.6/blob - net/bluetooth/l2cap.c

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