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