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