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