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

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