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

   1 /*
   2    BlueZ - Bluetooth protocol stack for Linux
   3    Copyright (C) 2000-2001 Qualcomm Incorporated
   4
   5    Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.com>
   6
   7    This program is free software; you can redistribute it and/or modify
   8    it under the terms of the GNU General Public License version 2 as
   9    published by the Free Software Foundation;
  10
  11    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
  12    OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  13    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
  14    IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY
  15    CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES
  16    WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
  17    ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
  18    OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  19
  20    ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS,
  21    COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS
  22    SOFTWARE IS DISCLAIMED.
  23 */
  24
  25 /* Bluetooth L2CAP core and sockets. */
  26
  27 #include <linux/module.h>
  28
  29 #include <linux/types.h>
  30 #include <linux/capability.h>
  31 #include <linux/errno.h>
  32 #include <linux/kernel.h>
  33 #include <linux/sched.h>
  34 #include <linux/slab.h>
  35 #include <linux/poll.h>
  36 #include <linux/fcntl.h>
  37 #include <linux/init.h>
  38 #include <linux/interrupt.h>
  39 #include <linux/socket.h>
  40 #include <linux/skbuff.h>
  41 #include <linux/list.h>
  42 #include <linux/device.h>
  43 #include <linux/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->remote_mps, control, len);
1610         if (IS_ERR(skb))
1611                 return PTR_ERR(skb);
1612
1613         __skb_queue_tail(&sar_queue, skb);
1614         len -= pi->remote_mps;
1615         size += pi->remote_mps;
1616         control = 0;
1617
1618         while (len > 0) {
1619                 size_t buflen;
1620
1621                 if (len > pi->remote_mps) {
1622                         control |= L2CAP_SDU_CONTINUE;
1623                         buflen = pi->remote_mps;
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->remote_mps) {
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 = cpu_to_le16(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 = cpu_to_le16(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                         if (rfc.max_pdu_size > pi->conn->mtu - 10)
2486                                 rfc.max_pdu_size = le16_to_cpu(pi->conn->mtu - 10);
2487
2488                         pi->remote_mps = le16_to_cpu(rfc.max_pdu_size);
2489
2490                         rfc.retrans_timeout =
2491                                 le16_to_cpu(L2CAP_DEFAULT_RETRANS_TO);
2492                         rfc.monitor_timeout =
2493                                 le16_to_cpu(L2CAP_DEFAULT_MONITOR_TO);
2494
2495                         pi->conf_state |= L2CAP_CONF_MODE_DONE;
2496
2497                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2498                                         sizeof(rfc), (unsigned long) &rfc);
2499
2500                         break;
2501
2502                 case L2CAP_MODE_STREAMING:
2503                         if (rfc.max_pdu_size > pi->conn->mtu - 10)
2504                                 rfc.max_pdu_size = le16_to_cpu(pi->conn->mtu - 10);
2505
2506                         pi->remote_mps = le16_to_cpu(rfc.max_pdu_size);
2507
2508                         pi->conf_state |= L2CAP_CONF_MODE_DONE;
2509
2510                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2511                                         sizeof(rfc), (unsigned long) &rfc);
2512
2513                         break;
2514
2515                 default:
2516                         result = L2CAP_CONF_UNACCEPT;
2517
2518                         memset(&rfc, 0, sizeof(rfc));
2519                         rfc.mode = pi->mode;
2520                 }
2521
2522                 if (result == L2CAP_CONF_SUCCESS)
2523                         pi->conf_state |= L2CAP_CONF_OUTPUT_DONE;
2524         }
2525         rsp->scid   = cpu_to_le16(pi->dcid);
2526         rsp->result = cpu_to_le16(result);
2527         rsp->flags  = cpu_to_le16(0x0000);
2528
2529         return ptr - data;
2530 }
2531
2532 static int l2cap_parse_conf_rsp(struct sock *sk, void *rsp, int len, void *data, u16 *result)
2533 {
2534         struct l2cap_pinfo *pi = l2cap_pi(sk);
2535         struct l2cap_conf_req *req = data;
2536         void *ptr = req->data;
2537         int type, olen;
2538         unsigned long val;
2539         struct l2cap_conf_rfc rfc;
2540
2541         BT_DBG("sk %p, rsp %p, len %d, req %p", sk, rsp, len, data);
2542
2543         while (len >= L2CAP_CONF_OPT_SIZE) {
2544                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
2545
2546                 switch (type) {
2547                 case L2CAP_CONF_MTU:
2548                         if (val < L2CAP_DEFAULT_MIN_MTU) {
2549                                 *result = L2CAP_CONF_UNACCEPT;
2550                                 pi->omtu = L2CAP_DEFAULT_MIN_MTU;
2551                         } else
2552                                 pi->omtu = val;
2553                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->omtu);
2554                         break;
2555
2556                 case L2CAP_CONF_FLUSH_TO:
2557                         pi->flush_to = val;
2558                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
2559                                                         2, pi->flush_to);
2560                         break;
2561
2562                 case L2CAP_CONF_RFC:
2563                         if (olen == sizeof(rfc))
2564                                 memcpy(&rfc, (void *)val, olen);
2565
2566                         if ((pi->conf_state & L2CAP_CONF_STATE2_DEVICE) &&
2567                                                         rfc.mode != pi->mode)
2568                                 return -ECONNREFUSED;
2569
2570                         pi->mode = rfc.mode;
2571                         pi->fcs = 0;
2572
2573                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2574                                         sizeof(rfc), (unsigned long) &rfc);
2575                         break;
2576                 }
2577         }
2578
2579         if (*result == L2CAP_CONF_SUCCESS) {
2580                 switch (rfc.mode) {
2581                 case L2CAP_MODE_ERTM:
2582                         pi->remote_tx_win   = rfc.txwin_size;
2583                         pi->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
2584                         pi->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
2585                         pi->mps    = le16_to_cpu(rfc.max_pdu_size);
2586                         break;
2587                 case L2CAP_MODE_STREAMING:
2588                         pi->mps    = le16_to_cpu(rfc.max_pdu_size);
2589                 }
2590         }
2591
2592         req->dcid   = cpu_to_le16(pi->dcid);
2593         req->flags  = cpu_to_le16(0x0000);
2594
2595         return ptr - data;
2596 }
2597
2598 static int l2cap_build_conf_rsp(struct sock *sk, void *data, u16 result, u16 flags)
2599 {
2600         struct l2cap_conf_rsp *rsp = data;
2601         void *ptr = rsp->data;
2602
2603         BT_DBG("sk %p", sk);
2604
2605         rsp->scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
2606         rsp->result = cpu_to_le16(result);
2607         rsp->flags  = cpu_to_le16(flags);
2608
2609         return ptr - data;
2610 }
2611
2612 static void l2cap_conf_rfc_get(struct sock *sk, void *rsp, int len)
2613 {
2614         struct l2cap_pinfo *pi = l2cap_pi(sk);
2615         int type, olen;
2616         unsigned long val;
2617         struct l2cap_conf_rfc rfc;
2618
2619         BT_DBG("sk %p, rsp %p, len %d", sk, rsp, len);
2620
2621         if ((pi->mode != L2CAP_MODE_ERTM) && (pi->mode != L2CAP_MODE_STREAMING))
2622                 return;
2623
2624         while (len >= L2CAP_CONF_OPT_SIZE) {
2625                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
2626
2627                 switch (type) {
2628                 case L2CAP_CONF_RFC:
2629                         if (olen == sizeof(rfc))
2630                                 memcpy(&rfc, (void *)val, olen);
2631                         goto done;
2632                 }
2633         }
2634
2635 done:
2636         switch (rfc.mode) {
2637         case L2CAP_MODE_ERTM:
2638                 pi->remote_tx_win   = rfc.txwin_size;
2639                 pi->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
2640                 pi->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
2641                 pi->mps    = le16_to_cpu(rfc.max_pdu_size);
2642                 break;
2643         case L2CAP_MODE_STREAMING:
2644                 pi->mps    = le16_to_cpu(rfc.max_pdu_size);
2645         }
2646 }
2647
2648 static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2649 {
2650         struct l2cap_cmd_rej *rej = (struct l2cap_cmd_rej *) data;
2651
2652         if (rej->reason != 0x0000)
2653                 return 0;
2654
2655         if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
2656                                         cmd->ident == conn->info_ident) {
2657                 del_timer(&conn->info_timer);
2658
2659                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2660                 conn->info_ident = 0;
2661
2662                 l2cap_conn_start(conn);
2663         }
2664
2665         return 0;
2666 }
2667
2668 static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2669 {
2670         struct l2cap_chan_list *list = &conn->chan_list;
2671         struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
2672         struct l2cap_conn_rsp rsp;
2673         struct sock *sk, *parent;
2674         int result, status = L2CAP_CS_NO_INFO;
2675
2676         u16 dcid = 0, scid = __le16_to_cpu(req->scid);
2677         __le16 psm = req->psm;
2678
2679         BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid);
2680
2681         /* Check if we have socket listening on psm */
2682         parent = l2cap_get_sock_by_psm(BT_LISTEN, psm, conn->src);
2683         if (!parent) {
2684                 result = L2CAP_CR_BAD_PSM;
2685                 goto sendresp;
2686         }
2687
2688         /* Check if the ACL is secure enough (if not SDP) */
2689         if (psm != cpu_to_le16(0x0001) &&
2690                                 !hci_conn_check_link_mode(conn->hcon)) {
2691                 conn->disc_reason = 0x05;
2692                 result = L2CAP_CR_SEC_BLOCK;
2693                 goto response;
2694         }
2695
2696         result = L2CAP_CR_NO_MEM;
2697
2698         /* Check for backlog size */
2699         if (sk_acceptq_is_full(parent)) {
2700                 BT_DBG("backlog full %d", parent->sk_ack_backlog);
2701                 goto response;
2702         }
2703
2704         sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, GFP_ATOMIC);
2705         if (!sk)
2706                 goto response;
2707
2708         write_lock_bh(&list->lock);
2709
2710         /* Check if we already have channel with that dcid */
2711         if (__l2cap_get_chan_by_dcid(list, scid)) {
2712                 write_unlock_bh(&list->lock);
2713                 sock_set_flag(sk, SOCK_ZAPPED);
2714                 l2cap_sock_kill(sk);
2715                 goto response;
2716         }
2717
2718         hci_conn_hold(conn->hcon);
2719
2720         l2cap_sock_init(sk, parent);
2721         bacpy(&bt_sk(sk)->src, conn->src);
2722         bacpy(&bt_sk(sk)->dst, conn->dst);
2723         l2cap_pi(sk)->psm  = psm;
2724         l2cap_pi(sk)->dcid = scid;
2725
2726         __l2cap_chan_add(conn, sk, parent);
2727         dcid = l2cap_pi(sk)->scid;
2728
2729         l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
2730
2731         l2cap_pi(sk)->ident = cmd->ident;
2732
2733         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
2734                 if (l2cap_check_security(sk)) {
2735                         if (bt_sk(sk)->defer_setup) {
2736                                 sk->sk_state = BT_CONNECT2;
2737                                 result = L2CAP_CR_PEND;
2738                                 status = L2CAP_CS_AUTHOR_PEND;
2739                                 parent->sk_data_ready(parent, 0);
2740                         } else {
2741                                 sk->sk_state = BT_CONFIG;
2742                                 result = L2CAP_CR_SUCCESS;
2743                                 status = L2CAP_CS_NO_INFO;
2744                         }
2745                 } else {
2746                         sk->sk_state = BT_CONNECT2;
2747                         result = L2CAP_CR_PEND;
2748                         status = L2CAP_CS_AUTHEN_PEND;
2749                 }
2750         } else {
2751                 sk->sk_state = BT_CONNECT2;
2752                 result = L2CAP_CR_PEND;
2753                 status = L2CAP_CS_NO_INFO;
2754         }
2755
2756         write_unlock_bh(&list->lock);
2757
2758 response:
2759         bh_unlock_sock(parent);
2760
2761 sendresp:
2762         rsp.scid   = cpu_to_le16(scid);
2763         rsp.dcid   = cpu_to_le16(dcid);
2764         rsp.result = cpu_to_le16(result);
2765         rsp.status = cpu_to_le16(status);
2766         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
2767
2768         if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
2769                 struct l2cap_info_req info;
2770                 info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
2771
2772                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
2773                 conn->info_ident = l2cap_get_ident(conn);
2774
2775                 mod_timer(&conn->info_timer, jiffies +
2776                                         msecs_to_jiffies(L2CAP_INFO_TIMEOUT));
2777
2778                 l2cap_send_cmd(conn, conn->info_ident,
2779                                         L2CAP_INFO_REQ, sizeof(info), &info);
2780         }
2781
2782         return 0;
2783 }
2784
2785 static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2786 {
2787         struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
2788         u16 scid, dcid, result, status;
2789         struct sock *sk;
2790         u8 req[128];
2791
2792         scid   = __le16_to_cpu(rsp->scid);
2793         dcid   = __le16_to_cpu(rsp->dcid);
2794         result = __le16_to_cpu(rsp->result);
2795         status = __le16_to_cpu(rsp->status);
2796
2797         BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status);
2798
2799         if (scid) {
2800                 sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
2801                 if (!sk)
2802                         return 0;
2803         } else {
2804                 sk = l2cap_get_chan_by_ident(&conn->chan_list, cmd->ident);
2805                 if (!sk)
2806                         return 0;
2807         }
2808
2809         switch (result) {
2810         case L2CAP_CR_SUCCESS:
2811                 sk->sk_state = BT_CONFIG;
2812                 l2cap_pi(sk)->ident = 0;
2813                 l2cap_pi(sk)->dcid = dcid;
2814                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
2815
2816                 l2cap_pi(sk)->conf_state &= ~L2CAP_CONF_CONNECT_PEND;
2817
2818                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2819                                         l2cap_build_conf_req(sk, req), req);
2820                 l2cap_pi(sk)->num_conf_req++;
2821                 break;
2822
2823         case L2CAP_CR_PEND:
2824                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND;
2825                 break;
2826
2827         default:
2828                 l2cap_chan_del(sk, ECONNREFUSED);
2829                 break;
2830         }
2831
2832         bh_unlock_sock(sk);
2833         return 0;
2834 }
2835
2836 static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
2837 {
2838         struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
2839         u16 dcid, flags;
2840         u8 rsp[64];
2841         struct sock *sk;
2842         int len;
2843
2844         dcid  = __le16_to_cpu(req->dcid);
2845         flags = __le16_to_cpu(req->flags);
2846
2847         BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
2848
2849         sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid);
2850         if (!sk)
2851                 return -ENOENT;
2852
2853         if (sk->sk_state == BT_DISCONN)
2854                 goto unlock;
2855
2856         /* Reject if config buffer is too small. */
2857         len = cmd_len - sizeof(*req);
2858         if (l2cap_pi(sk)->conf_len + len > sizeof(l2cap_pi(sk)->conf_req)) {
2859                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2860                                 l2cap_build_conf_rsp(sk, rsp,
2861                                         L2CAP_CONF_REJECT, flags), rsp);
2862                 goto unlock;
2863         }
2864
2865         /* Store config. */
2866         memcpy(l2cap_pi(sk)->conf_req + l2cap_pi(sk)->conf_len, req->data, len);
2867         l2cap_pi(sk)->conf_len += len;
2868
2869         if (flags & 0x0001) {
2870                 /* Incomplete config. Send empty response. */
2871                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2872                                 l2cap_build_conf_rsp(sk, rsp,
2873                                         L2CAP_CONF_SUCCESS, 0x0001), rsp);
2874                 goto unlock;
2875         }
2876
2877         /* Complete config. */
2878         len = l2cap_parse_conf_req(sk, rsp);
2879         if (len < 0) {
2880                 l2cap_send_disconn_req(conn, sk);
2881                 goto unlock;
2882         }
2883
2884         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
2885         l2cap_pi(sk)->num_conf_rsp++;
2886
2887         /* Reset config buffer. */
2888         l2cap_pi(sk)->conf_len = 0;
2889
2890         if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE))
2891                 goto unlock;
2892
2893         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_INPUT_DONE) {
2894                 if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_NO_FCS_RECV) ||
2895                     l2cap_pi(sk)->fcs != L2CAP_FCS_NONE)
2896                         l2cap_pi(sk)->fcs = L2CAP_FCS_CRC16;
2897
2898                 sk->sk_state = BT_CONNECTED;
2899
2900                 l2cap_pi(sk)->next_tx_seq = 0;
2901                 l2cap_pi(sk)->expected_tx_seq = 0;
2902                 __skb_queue_head_init(TX_QUEUE(sk));
2903                 if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM)
2904                         l2cap_ertm_init(sk);
2905
2906                 l2cap_chan_ready(sk);
2907                 goto unlock;
2908         }
2909
2910         if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT)) {
2911                 u8 buf[64];
2912                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2913                                         l2cap_build_conf_req(sk, buf), buf);
2914                 l2cap_pi(sk)->num_conf_req++;
2915         }
2916
2917 unlock:
2918         bh_unlock_sock(sk);
2919         return 0;
2920 }
2921
2922 static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2923 {
2924         struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
2925         u16 scid, flags, result;
2926         struct sock *sk;
2927         int len = cmd->len - sizeof(*rsp);
2928
2929         scid   = __le16_to_cpu(rsp->scid);
2930         flags  = __le16_to_cpu(rsp->flags);
2931         result = __le16_to_cpu(rsp->result);
2932
2933         BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x",
2934                         scid, flags, result);
2935
2936         sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
2937         if (!sk)
2938                 return 0;
2939
2940         switch (result) {
2941         case L2CAP_CONF_SUCCESS:
2942                 l2cap_conf_rfc_get(sk, rsp->data, len);
2943                 break;
2944
2945         case L2CAP_CONF_UNACCEPT:
2946                 if (l2cap_pi(sk)->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
2947                         char req[64];
2948
2949                         if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
2950                                 l2cap_send_disconn_req(conn, sk);
2951                                 goto done;
2952                         }
2953
2954                         /* throw out any old stored conf requests */
2955                         result = L2CAP_CONF_SUCCESS;
2956                         len = l2cap_parse_conf_rsp(sk, rsp->data,
2957                                                         len, req, &result);
2958                         if (len < 0) {
2959                                 l2cap_send_disconn_req(conn, sk);
2960                                 goto done;
2961                         }
2962
2963                         l2cap_send_cmd(conn, l2cap_get_ident(conn),
2964                                                 L2CAP_CONF_REQ, len, req);
2965                         l2cap_pi(sk)->num_conf_req++;
2966                         if (result != L2CAP_CONF_SUCCESS)
2967                                 goto done;
2968                         break;
2969                 }
2970
2971         default:
2972                 sk->sk_state = BT_DISCONN;
2973                 sk->sk_err = ECONNRESET;
2974                 l2cap_sock_set_timer(sk, HZ * 5);
2975                 l2cap_send_disconn_req(conn, sk);
2976                 goto done;
2977         }
2978
2979         if (flags & 0x01)
2980                 goto done;
2981
2982         l2cap_pi(sk)->conf_state |= L2CAP_CONF_INPUT_DONE;
2983
2984         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE) {
2985                 if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_NO_FCS_RECV) ||
2986                     l2cap_pi(sk)->fcs != L2CAP_FCS_NONE)
2987                         l2cap_pi(sk)->fcs = L2CAP_FCS_CRC16;
2988
2989                 sk->sk_state = BT_CONNECTED;
2990                 l2cap_pi(sk)->next_tx_seq = 0;
2991                 l2cap_pi(sk)->expected_tx_seq = 0;
2992                 __skb_queue_head_init(TX_QUEUE(sk));
2993                 if (l2cap_pi(sk)->mode ==  L2CAP_MODE_ERTM)
2994                         l2cap_ertm_init(sk);
2995
2996                 l2cap_chan_ready(sk);
2997         }
2998
2999 done:
3000         bh_unlock_sock(sk);
3001         return 0;
3002 }
3003
3004 static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3005 {
3006         struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
3007         struct l2cap_disconn_rsp rsp;
3008         u16 dcid, scid;
3009         struct sock *sk;
3010
3011         scid = __le16_to_cpu(req->scid);
3012         dcid = __le16_to_cpu(req->dcid);
3013
3014         BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
3015
3016         sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid);
3017         if (!sk)
3018                 return 0;
3019
3020         rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid);
3021         rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid);
3022         l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
3023
3024         sk->sk_shutdown = SHUTDOWN_MASK;
3025
3026         skb_queue_purge(TX_QUEUE(sk));
3027
3028         if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM) {
3029                 skb_queue_purge(SREJ_QUEUE(sk));
3030                 del_timer(&l2cap_pi(sk)->retrans_timer);
3031                 del_timer(&l2cap_pi(sk)->monitor_timer);
3032                 del_timer(&l2cap_pi(sk)->ack_timer);
3033         }
3034
3035         l2cap_chan_del(sk, ECONNRESET);
3036         bh_unlock_sock(sk);
3037
3038         l2cap_sock_kill(sk);
3039         return 0;
3040 }
3041
3042 static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3043 {
3044         struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
3045         u16 dcid, scid;
3046         struct sock *sk;
3047
3048         scid = __le16_to_cpu(rsp->scid);
3049         dcid = __le16_to_cpu(rsp->dcid);
3050
3051         BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
3052
3053         sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
3054         if (!sk)
3055                 return 0;
3056
3057         skb_queue_purge(TX_QUEUE(sk));
3058
3059         if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM) {
3060                 skb_queue_purge(SREJ_QUEUE(sk));
3061                 del_timer(&l2cap_pi(sk)->retrans_timer);
3062                 del_timer(&l2cap_pi(sk)->monitor_timer);
3063                 del_timer(&l2cap_pi(sk)->ack_timer);
3064         }
3065
3066         l2cap_chan_del(sk, 0);
3067         bh_unlock_sock(sk);
3068
3069         l2cap_sock_kill(sk);
3070         return 0;
3071 }
3072
3073 static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3074 {
3075         struct l2cap_info_req *req = (struct l2cap_info_req *) data;
3076         u16 type;
3077
3078         type = __le16_to_cpu(req->type);
3079
3080         BT_DBG("type 0x%4.4x", type);
3081
3082         if (type == L2CAP_IT_FEAT_MASK) {
3083                 u8 buf[8];
3084                 u32 feat_mask = l2cap_feat_mask;
3085                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
3086                 rsp->type   = cpu_to_le16(L2CAP_IT_FEAT_MASK);
3087                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
3088                 if (enable_ertm)
3089                         feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
3090                                                          | L2CAP_FEAT_FCS;
3091                 put_unaligned_le32(feat_mask, rsp->data);
3092                 l2cap_send_cmd(conn, cmd->ident,
3093                                         L2CAP_INFO_RSP, sizeof(buf), buf);
3094         } else if (type == L2CAP_IT_FIXED_CHAN) {
3095                 u8 buf[12];
3096                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
3097                 rsp->type   = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
3098                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
3099                 memcpy(buf + 4, l2cap_fixed_chan, 8);
3100                 l2cap_send_cmd(conn, cmd->ident,
3101                                         L2CAP_INFO_RSP, sizeof(buf), buf);
3102         } else {
3103                 struct l2cap_info_rsp rsp;
3104                 rsp.type   = cpu_to_le16(type);
3105                 rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP);
3106                 l2cap_send_cmd(conn, cmd->ident,
3107                                         L2CAP_INFO_RSP, sizeof(rsp), &rsp);
3108         }
3109
3110         return 0;
3111 }
3112
3113 static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3114 {
3115         struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
3116         u16 type, result;
3117
3118         type   = __le16_to_cpu(rsp->type);
3119         result = __le16_to_cpu(rsp->result);
3120
3121         BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
3122
3123         del_timer(&conn->info_timer);
3124
3125         if (type == L2CAP_IT_FEAT_MASK) {
3126                 conn->feat_mask = get_unaligned_le32(rsp->data);
3127
3128                 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
3129                         struct l2cap_info_req req;
3130                         req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
3131
3132                         conn->info_ident = l2cap_get_ident(conn);
3133
3134                         l2cap_send_cmd(conn, conn->info_ident,
3135                                         L2CAP_INFO_REQ, sizeof(req), &req);
3136                 } else {
3137                         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3138                         conn->info_ident = 0;
3139
3140                         l2cap_conn_start(conn);
3141                 }
3142         } else if (type == L2CAP_IT_FIXED_CHAN) {
3143                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3144                 conn->info_ident = 0;
3145
3146                 l2cap_conn_start(conn);
3147         }
3148
3149         return 0;
3150 }
3151
3152 static inline void l2cap_sig_channel(struct l2cap_conn *conn, struct sk_buff *skb)
3153 {
3154         u8 *data = skb->data;
3155         int len = skb->len;
3156         struct l2cap_cmd_hdr cmd;
3157         int err = 0;
3158
3159         l2cap_raw_recv(conn, skb);
3160
3161         while (len >= L2CAP_CMD_HDR_SIZE) {
3162                 u16 cmd_len;
3163                 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
3164                 data += L2CAP_CMD_HDR_SIZE;
3165                 len  -= L2CAP_CMD_HDR_SIZE;
3166
3167                 cmd_len = le16_to_cpu(cmd.len);
3168
3169                 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, cmd.ident);
3170
3171                 if (cmd_len > len || !cmd.ident) {
3172                         BT_DBG("corrupted command");
3173                         break;
3174                 }
3175
3176                 switch (cmd.code) {
3177                 case L2CAP_COMMAND_REJ:
3178                         l2cap_command_rej(conn, &cmd, data);
3179                         break;
3180
3181                 case L2CAP_CONN_REQ:
3182                         err = l2cap_connect_req(conn, &cmd, data);
3183                         break;
3184
3185                 case L2CAP_CONN_RSP:
3186                         err = l2cap_connect_rsp(conn, &cmd, data);
3187                         break;
3188
3189                 case L2CAP_CONF_REQ:
3190                         err = l2cap_config_req(conn, &cmd, cmd_len, data);
3191                         break;
3192
3193                 case L2CAP_CONF_RSP:
3194                         err = l2cap_config_rsp(conn, &cmd, data);
3195                         break;
3196
3197                 case L2CAP_DISCONN_REQ:
3198                         err = l2cap_disconnect_req(conn, &cmd, data);
3199                         break;
3200
3201                 case L2CAP_DISCONN_RSP:
3202                         err = l2cap_disconnect_rsp(conn, &cmd, data);
3203                         break;
3204
3205                 case L2CAP_ECHO_REQ:
3206                         l2cap_send_cmd(conn, cmd.ident, L2CAP_ECHO_RSP, cmd_len, data);
3207                         break;
3208
3209                 case L2CAP_ECHO_RSP:
3210                         break;
3211
3212                 case L2CAP_INFO_REQ:
3213                         err = l2cap_information_req(conn, &cmd, data);
3214                         break;
3215
3216                 case L2CAP_INFO_RSP:
3217                         err = l2cap_information_rsp(conn, &cmd, data);
3218                         break;
3219
3220                 default:
3221                         BT_ERR("Unknown signaling command 0x%2.2x", cmd.code);
3222                         err = -EINVAL;
3223                         break;
3224                 }
3225
3226                 if (err) {
3227                         struct l2cap_cmd_rej rej;
3228                         BT_DBG("error %d", err);
3229
3230                         /* FIXME: Map err to a valid reason */
3231                         rej.reason = cpu_to_le16(0);
3232                         l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
3233                 }
3234
3235                 data += cmd_len;
3236                 len  -= cmd_len;
3237         }
3238
3239         kfree_skb(skb);
3240 }
3241
3242 static int l2cap_check_fcs(struct l2cap_pinfo *pi,  struct sk_buff *skb)
3243 {
3244         u16 our_fcs, rcv_fcs;
3245         int hdr_size = L2CAP_HDR_SIZE + 2;
3246
3247         if (pi->fcs == L2CAP_FCS_CRC16) {
3248                 skb_trim(skb, skb->len - 2);
3249                 rcv_fcs = get_unaligned_le16(skb->data + skb->len);
3250                 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
3251
3252                 if (our_fcs != rcv_fcs)
3253                         return -EINVAL;
3254         }
3255         return 0;
3256 }
3257
3258 static inline void l2cap_send_i_or_rr_or_rnr(struct sock *sk)
3259 {
3260         struct l2cap_pinfo *pi = l2cap_pi(sk);
3261         u16 control = 0;
3262
3263         pi->frames_sent = 0;
3264         pi->conn_state |= L2CAP_CONN_SEND_FBIT;
3265
3266         control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3267
3268         if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
3269                 control |= L2CAP_SUPER_RCV_NOT_READY | L2CAP_CTRL_FINAL;
3270                 l2cap_send_sframe(pi, control);
3271                 pi->conn_state &= ~L2CAP_CONN_SEND_FBIT;
3272         }
3273
3274         if (pi->conn_state & L2CAP_CONN_REMOTE_BUSY && pi->unacked_frames > 0)
3275                 __mod_retrans_timer();
3276
3277         l2cap_ertm_send(sk);
3278
3279         if (!(pi->conn_state & L2CAP_CONN_LOCAL_BUSY) &&
3280                         pi->frames_sent == 0) {
3281                 control |= L2CAP_SUPER_RCV_READY;
3282                 l2cap_send_sframe(pi, control);
3283         }
3284 }
3285
3286 static void l2cap_add_to_srej_queue(struct sock *sk, struct sk_buff *skb, u8 tx_seq, u8 sar)
3287 {
3288         struct sk_buff *next_skb;
3289
3290         bt_cb(skb)->tx_seq = tx_seq;
3291         bt_cb(skb)->sar = sar;
3292
3293         next_skb = skb_peek(SREJ_QUEUE(sk));
3294         if (!next_skb) {
3295                 __skb_queue_tail(SREJ_QUEUE(sk), skb);
3296                 return;
3297         }
3298
3299         do {
3300                 if (bt_cb(next_skb)->tx_seq > tx_seq) {
3301                         __skb_queue_before(SREJ_QUEUE(sk), next_skb, skb);
3302                         return;
3303                 }
3304
3305                 if (skb_queue_is_last(SREJ_QUEUE(sk), next_skb))
3306                         break;
3307
3308         } while((next_skb = skb_queue_next(SREJ_QUEUE(sk), next_skb)));
3309
3310         __skb_queue_tail(SREJ_QUEUE(sk), skb);
3311 }
3312
3313 static int l2cap_sar_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)
3314 {
3315         struct l2cap_pinfo *pi = l2cap_pi(sk);
3316         struct sk_buff *_skb;
3317         int err = -EINVAL;
3318
3319         switch (control & L2CAP_CTRL_SAR) {
3320         case L2CAP_SDU_UNSEGMENTED:
3321                 if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
3322                         kfree_skb(pi->sdu);
3323                         break;
3324                 }
3325
3326                 err = sock_queue_rcv_skb(sk, skb);
3327                 if (!err)
3328                         return 0;
3329
3330                 break;
3331
3332         case L2CAP_SDU_START:
3333                 if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
3334                         kfree_skb(pi->sdu);
3335                         break;
3336                 }
3337
3338                 pi->sdu_len = get_unaligned_le16(skb->data);
3339                 skb_pull(skb, 2);
3340
3341                 if (pi->sdu_len > pi->imtu) {
3342                         err = -EMSGSIZE;
3343                         break;
3344                 }
3345
3346                 pi->sdu = bt_skb_alloc(pi->sdu_len, GFP_ATOMIC);
3347                 if (!pi->sdu) {
3348                         err = -ENOMEM;
3349                         break;
3350                 }
3351
3352                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3353
3354                 pi->conn_state |= L2CAP_CONN_SAR_SDU;
3355                 pi->partial_sdu_len = skb->len;
3356                 err = 0;
3357                 break;
3358
3359         case L2CAP_SDU_CONTINUE:
3360                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
3361                         break;
3362
3363                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3364
3365                 pi->partial_sdu_len += skb->len;
3366                 if (pi->partial_sdu_len > pi->sdu_len)
3367                         kfree_skb(pi->sdu);
3368                 else
3369                         err = 0;
3370
3371                 break;
3372
3373         case L2CAP_SDU_END:
3374                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
3375                         break;
3376
3377                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3378
3379                 pi->conn_state &= ~L2CAP_CONN_SAR_SDU;
3380                 pi->partial_sdu_len += skb->len;
3381
3382                 if (pi->partial_sdu_len > pi->imtu)
3383                         goto drop;
3384
3385                 if (pi->partial_sdu_len == pi->sdu_len) {
3386                         _skb = skb_clone(pi->sdu, GFP_ATOMIC);
3387                         err = sock_queue_rcv_skb(sk, _skb);
3388                         if (err < 0)
3389                                 kfree_skb(_skb);
3390                 }
3391                 err = 0;
3392
3393 drop:
3394                 kfree_skb(pi->sdu);
3395                 break;
3396         }
3397
3398         kfree_skb(skb);
3399         return err;
3400 }
3401
3402 static void l2cap_check_srej_gap(struct sock *sk, u8 tx_seq)
3403 {
3404         struct sk_buff *skb;
3405         u16 control = 0;
3406
3407         while((skb = skb_peek(SREJ_QUEUE(sk)))) {
3408                 if (bt_cb(skb)->tx_seq != tx_seq)
3409                         break;
3410
3411                 skb = skb_dequeue(SREJ_QUEUE(sk));
3412                 control |= bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
3413                 l2cap_sar_reassembly_sdu(sk, skb, control);
3414                 l2cap_pi(sk)->buffer_seq_srej =
3415                         (l2cap_pi(sk)->buffer_seq_srej + 1) % 64;
3416                 tx_seq++;
3417         }
3418 }
3419
3420 static void l2cap_resend_srejframe(struct sock *sk, u8 tx_seq)
3421 {
3422         struct l2cap_pinfo *pi = l2cap_pi(sk);
3423         struct srej_list *l, *tmp;
3424         u16 control;
3425
3426         list_for_each_entry_safe(l,tmp, SREJ_LIST(sk), list) {
3427                 if (l->tx_seq == tx_seq) {
3428                         list_del(&l->list);
3429                         kfree(l);
3430                         return;
3431                 }
3432                 control = L2CAP_SUPER_SELECT_REJECT;
3433                 control |= l->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3434                 l2cap_send_sframe(pi, control);
3435                 list_del(&l->list);
3436                 list_add_tail(&l->list, SREJ_LIST(sk));
3437         }
3438 }
3439
3440 static void l2cap_send_srejframe(struct sock *sk, u8 tx_seq)
3441 {
3442         struct l2cap_pinfo *pi = l2cap_pi(sk);
3443         struct srej_list *new;
3444         u16 control;
3445
3446         while (tx_seq != pi->expected_tx_seq) {
3447                 control = L2CAP_SUPER_SELECT_REJECT;
3448                 control |= pi->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3449                 l2cap_send_sframe(pi, control);
3450
3451                 new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC);
3452                 new->tx_seq = pi->expected_tx_seq++;
3453                 list_add_tail(&new->list, SREJ_LIST(sk));
3454         }
3455         pi->expected_tx_seq++;
3456 }
3457
3458 static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
3459 {
3460         struct l2cap_pinfo *pi = l2cap_pi(sk);
3461         u8 tx_seq = __get_txseq(rx_control);
3462         u8 req_seq = __get_reqseq(rx_control);
3463         u8 sar = rx_control >> L2CAP_CTRL_SAR_SHIFT;
3464         int err = 0;
3465
3466         BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
3467
3468         if (L2CAP_CTRL_FINAL & rx_control) {
3469                 del_timer(&pi->monitor_timer);
3470                 if (pi->unacked_frames > 0)
3471                         __mod_retrans_timer();
3472                 pi->conn_state &= ~L2CAP_CONN_WAIT_F;
3473         }
3474
3475         pi->expected_ack_seq = req_seq;
3476         l2cap_drop_acked_frames(sk);
3477
3478         if (tx_seq == pi->expected_tx_seq)
3479                 goto expected;
3480
3481         if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
3482                 struct srej_list *first;
3483
3484                 first = list_first_entry(SREJ_LIST(sk),
3485                                 struct srej_list, list);
3486                 if (tx_seq == first->tx_seq) {
3487                         l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
3488                         l2cap_check_srej_gap(sk, tx_seq);
3489
3490                         list_del(&first->list);
3491                         kfree(first);
3492
3493                         if (list_empty(SREJ_LIST(sk))) {
3494                                 pi->buffer_seq = pi->buffer_seq_srej;
3495                                 pi->conn_state &= ~L2CAP_CONN_SREJ_SENT;
3496                         }
3497                 } else {
3498                         struct srej_list *l;
3499                         l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
3500
3501                         list_for_each_entry(l, SREJ_LIST(sk), list) {
3502                                 if (l->tx_seq == tx_seq) {
3503                                         l2cap_resend_srejframe(sk, tx_seq);
3504                                         return 0;
3505                                 }
3506                         }
3507                         l2cap_send_srejframe(sk, tx_seq);
3508                 }
3509         } else {
3510                 pi->conn_state |= L2CAP_CONN_SREJ_SENT;
3511
3512                 INIT_LIST_HEAD(SREJ_LIST(sk));
3513                 pi->buffer_seq_srej = pi->buffer_seq;
3514
3515                 __skb_queue_head_init(SREJ_QUEUE(sk));
3516                 l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
3517
3518                 pi->conn_state |= L2CAP_CONN_SEND_PBIT;
3519
3520                 l2cap_send_srejframe(sk, tx_seq);
3521         }
3522         return 0;
3523
3524 expected:
3525         pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3526
3527         if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
3528                 l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
3529                 return 0;
3530         }
3531
3532         if (rx_control & L2CAP_CTRL_FINAL) {
3533                 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
3534                         pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
3535                 else {
3536                         sk->sk_send_head = TX_QUEUE(sk)->next;
3537                         pi->next_tx_seq = pi->expected_ack_seq;
3538                         l2cap_ertm_send(sk);
3539                 }
3540         }
3541
3542         pi->buffer_seq = (pi->buffer_seq + 1) % 64;
3543
3544         err = l2cap_sar_reassembly_sdu(sk, skb, rx_control);
3545         if (err < 0)
3546                 return err;
3547
3548         __mod_ack_timer();
3549
3550         pi->num_to_ack = (pi->num_to_ack + 1) % L2CAP_DEFAULT_NUM_TO_ACK;
3551         if (pi->num_to_ack == L2CAP_DEFAULT_NUM_TO_ACK - 1)
3552                 l2cap_send_ack(pi);
3553
3554         return 0;
3555 }
3556
3557 static inline void l2cap_data_channel_rrframe(struct sock *sk, u16 rx_control)
3558 {
3559         struct l2cap_pinfo *pi = l2cap_pi(sk);
3560
3561         pi->expected_ack_seq = __get_reqseq(rx_control);
3562         l2cap_drop_acked_frames(sk);
3563
3564         if (rx_control & L2CAP_CTRL_POLL) {
3565                 if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
3566                         if ((pi->conn_state & L2CAP_CONN_REMOTE_BUSY) &&
3567                                         (pi->unacked_frames > 0))
3568                                 __mod_retrans_timer();
3569
3570                         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3571                         l2cap_send_srejtail(sk);
3572                 } else {
3573                         l2cap_send_i_or_rr_or_rnr(sk);
3574                         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3575                 }
3576
3577         } else if (rx_control & L2CAP_CTRL_FINAL) {
3578                 pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3579
3580                 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
3581                         pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
3582                 else {
3583                         sk->sk_send_head = TX_QUEUE(sk)->next;
3584                         pi->next_tx_seq = pi->expected_ack_seq;
3585                         l2cap_ertm_send(sk);
3586                 }
3587
3588         } else {
3589                 if ((pi->conn_state & L2CAP_CONN_REMOTE_BUSY) &&
3590                                 (pi->unacked_frames > 0))
3591                         __mod_retrans_timer();
3592
3593                 pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3594                 if (pi->conn_state & L2CAP_CONN_SREJ_SENT)
3595                         l2cap_send_ack(pi);
3596                 else
3597                         l2cap_ertm_send(sk);
3598         }
3599 }
3600
3601 static inline void l2cap_data_channel_rejframe(struct sock *sk, u16 rx_control)
3602 {
3603         struct l2cap_pinfo *pi = l2cap_pi(sk);
3604         u8 tx_seq = __get_reqseq(rx_control);
3605
3606         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3607
3608         pi->expected_ack_seq = tx_seq;
3609         l2cap_drop_acked_frames(sk);
3610
3611         if (rx_control & L2CAP_CTRL_FINAL) {
3612                 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
3613                         pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
3614                 else {
3615                         sk->sk_send_head = TX_QUEUE(sk)->next;
3616                         pi->next_tx_seq = pi->expected_ack_seq;
3617                         l2cap_ertm_send(sk);
3618                 }
3619         } else {
3620                 sk->sk_send_head = TX_QUEUE(sk)->next;
3621                 pi->next_tx_seq = pi->expected_ack_seq;
3622                 l2cap_ertm_send(sk);
3623
3624                 if (pi->conn_state & L2CAP_CONN_WAIT_F) {
3625                         pi->srej_save_reqseq = tx_seq;
3626                         pi->conn_state |= L2CAP_CONN_REJ_ACT;
3627                 }
3628         }
3629 }
3630 static inline void l2cap_data_channel_srejframe(struct sock *sk, u16 rx_control)
3631 {
3632         struct l2cap_pinfo *pi = l2cap_pi(sk);
3633         u8 tx_seq = __get_reqseq(rx_control);
3634
3635         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3636
3637         if (rx_control & L2CAP_CTRL_POLL) {
3638                 pi->expected_ack_seq = tx_seq;
3639                 l2cap_drop_acked_frames(sk);
3640                 l2cap_retransmit_frame(sk, tx_seq);
3641                 l2cap_ertm_send(sk);
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         } else if (rx_control & L2CAP_CTRL_FINAL) {
3647                 if ((pi->conn_state & L2CAP_CONN_SREJ_ACT) &&
3648                                 pi->srej_save_reqseq == tx_seq)
3649                         pi->conn_state &= ~L2CAP_CONN_SREJ_ACT;
3650                 else
3651                         l2cap_retransmit_frame(sk, tx_seq);
3652         } else {
3653                 l2cap_retransmit_frame(sk, tx_seq);
3654                 if (pi->conn_state & L2CAP_CONN_WAIT_F) {
3655                         pi->srej_save_reqseq = tx_seq;
3656                         pi->conn_state |= L2CAP_CONN_SREJ_ACT;
3657                 }
3658         }
3659 }
3660
3661 static inline void l2cap_data_channel_rnrframe(struct sock *sk, u16 rx_control)
3662 {
3663         struct l2cap_pinfo *pi = l2cap_pi(sk);
3664         u8 tx_seq = __get_reqseq(rx_control);
3665
3666         pi->conn_state |= L2CAP_CONN_REMOTE_BUSY;
3667         pi->expected_ack_seq = tx_seq;
3668         l2cap_drop_acked_frames(sk);
3669
3670         if (!(pi->conn_state & L2CAP_CONN_SREJ_SENT)) {
3671                 del_timer(&pi->retrans_timer);
3672                 if (rx_control & L2CAP_CTRL_POLL) {
3673                         u16 control = L2CAP_CTRL_FINAL;
3674                         l2cap_send_rr_or_rnr(pi, control);
3675                 }
3676                 return;
3677         }
3678
3679         if (rx_control & L2CAP_CTRL_POLL)
3680                 l2cap_send_srejtail(sk);
3681         else
3682                 l2cap_send_sframe(pi, L2CAP_SUPER_RCV_READY);
3683 }
3684
3685 static inline int l2cap_data_channel_sframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
3686 {
3687         BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
3688
3689         if (L2CAP_CTRL_FINAL & rx_control) {
3690                 del_timer(&l2cap_pi(sk)->monitor_timer);
3691                 if (l2cap_pi(sk)->unacked_frames > 0)
3692                         __mod_retrans_timer();
3693                 l2cap_pi(sk)->conn_state &= ~L2CAP_CONN_WAIT_F;
3694         }
3695
3696         switch (rx_control & L2CAP_CTRL_SUPERVISE) {
3697         case L2CAP_SUPER_RCV_READY:
3698                 l2cap_data_channel_rrframe(sk, rx_control);
3699                 break;
3700
3701         case L2CAP_SUPER_REJECT:
3702                 l2cap_data_channel_rejframe(sk, rx_control);
3703                 break;
3704
3705         case L2CAP_SUPER_SELECT_REJECT:
3706                 l2cap_data_channel_srejframe(sk, rx_control);
3707                 break;
3708
3709         case L2CAP_SUPER_RCV_NOT_READY:
3710                 l2cap_data_channel_rnrframe(sk, rx_control);
3711                 break;
3712         }
3713
3714         kfree_skb(skb);
3715         return 0;
3716 }
3717
3718 static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
3719 {
3720         struct sock *sk;
3721         struct l2cap_pinfo *pi;
3722         u16 control, len;
3723         u8 tx_seq;
3724
3725         sk = l2cap_get_chan_by_scid(&conn->chan_list, cid);
3726         if (!sk) {
3727                 BT_DBG("unknown cid 0x%4.4x", cid);
3728                 goto drop;
3729         }
3730
3731         pi = l2cap_pi(sk);
3732
3733         BT_DBG("sk %p, len %d", sk, skb->len);
3734
3735         if (sk->sk_state != BT_CONNECTED)
3736                 goto drop;
3737
3738         switch (pi->mode) {
3739         case L2CAP_MODE_BASIC:
3740                 /* If socket recv buffers overflows we drop data here
3741                  * which is *bad* because L2CAP has to be reliable.
3742                  * But we don't have any other choice. L2CAP doesn't
3743                  * provide flow control mechanism. */
3744
3745                 if (pi->imtu < skb->len)
3746                         goto drop;
3747
3748                 if (!sock_queue_rcv_skb(sk, skb))
3749                         goto done;
3750                 break;
3751
3752         case L2CAP_MODE_ERTM:
3753                 control = get_unaligned_le16(skb->data);
3754                 skb_pull(skb, 2);
3755                 len = skb->len;
3756
3757                 if (__is_sar_start(control))
3758                         len -= 2;
3759
3760                 if (pi->fcs == L2CAP_FCS_CRC16)
3761                         len -= 2;
3762
3763                 /*
3764                  * We can just drop the corrupted I-frame here.
3765                  * Receiver will miss it and start proper recovery
3766                  * procedures and ask retransmission.
3767                  */
3768                 if (len > pi->mps)
3769                         goto drop;
3770
3771                 if (l2cap_check_fcs(pi, skb))
3772                         goto drop;
3773
3774                 if (__is_iframe(control)) {
3775                         if (len < 4)
3776                                 goto drop;
3777
3778                         l2cap_data_channel_iframe(sk, control, skb);
3779                 } else {
3780                         if (len != 0)
3781                                 goto drop;
3782
3783                         l2cap_data_channel_sframe(sk, control, skb);
3784                 }
3785
3786                 goto done;
3787
3788         case L2CAP_MODE_STREAMING:
3789                 control = get_unaligned_le16(skb->data);
3790                 skb_pull(skb, 2);
3791                 len = skb->len;
3792
3793                 if (__is_sar_start(control))
3794                         len -= 2;
3795
3796                 if (pi->fcs == L2CAP_FCS_CRC16)
3797                         len -= 2;
3798
3799                 if (len > pi->mps || len < 4 || __is_sframe(control))
3800                         goto drop;
3801
3802                 if (l2cap_check_fcs(pi, skb))
3803                         goto drop;
3804
3805                 tx_seq = __get_txseq(control);
3806
3807                 if (pi->expected_tx_seq == tx_seq)
3808                         pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3809                 else
3810                         pi->expected_tx_seq = (tx_seq + 1) % 64;
3811
3812                 l2cap_sar_reassembly_sdu(sk, skb, control);
3813
3814                 goto done;
3815
3816         default:
3817                 BT_DBG("sk %p: bad mode 0x%2.2x", sk, pi->mode);
3818                 break;
3819         }
3820
3821 drop:
3822         kfree_skb(skb);
3823
3824 done:
3825         if (sk)
3826                 bh_unlock_sock(sk);
3827
3828         return 0;
3829 }
3830
3831 static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb)
3832 {
3833         struct sock *sk;
3834
3835         sk = l2cap_get_sock_by_psm(0, psm, conn->src);
3836         if (!sk)
3837                 goto drop;
3838
3839         BT_DBG("sk %p, len %d", sk, skb->len);
3840
3841         if (sk->sk_state != BT_BOUND && sk->sk_state != BT_CONNECTED)
3842                 goto drop;
3843
3844         if (l2cap_pi(sk)->imtu < skb->len)
3845                 goto drop;
3846
3847         if (!sock_queue_rcv_skb(sk, skb))
3848                 goto done;
3849
3850 drop:
3851         kfree_skb(skb);
3852
3853 done:
3854         if (sk)
3855                 bh_unlock_sock(sk);
3856         return 0;
3857 }
3858
3859 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
3860 {
3861         struct l2cap_hdr *lh = (void *) skb->data;
3862         u16 cid, len;
3863         __le16 psm;
3864
3865         skb_pull(skb, L2CAP_HDR_SIZE);
3866         cid = __le16_to_cpu(lh->cid);
3867         len = __le16_to_cpu(lh->len);
3868
3869         if (len != skb->len) {
3870                 kfree_skb(skb);
3871                 return;
3872         }
3873
3874         BT_DBG("len %d, cid 0x%4.4x", len, cid);
3875
3876         switch (cid) {
3877         case L2CAP_CID_SIGNALING:
3878                 l2cap_sig_channel(conn, skb);
3879                 break;
3880
3881         case L2CAP_CID_CONN_LESS:
3882                 psm = get_unaligned_le16(skb->data);
3883                 skb_pull(skb, 2);
3884                 l2cap_conless_channel(conn, psm, skb);
3885                 break;
3886
3887         default:
3888                 l2cap_data_channel(conn, cid, skb);
3889                 break;
3890         }
3891 }
3892
3893 /* ---- L2CAP interface with lower layer (HCI) ---- */
3894
3895 static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
3896 {
3897         int exact = 0, lm1 = 0, lm2 = 0;
3898         register struct sock *sk;
3899         struct hlist_node *node;
3900
3901         if (type != ACL_LINK)
3902                 return 0;
3903
3904         BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
3905
3906         /* Find listening sockets and check their link_mode */
3907         read_lock(&l2cap_sk_list.lock);
3908         sk_for_each(sk, node, &l2cap_sk_list.head) {
3909                 if (sk->sk_state != BT_LISTEN)
3910                         continue;
3911
3912                 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
3913                         lm1 |= HCI_LM_ACCEPT;
3914                         if (l2cap_pi(sk)->role_switch)
3915                                 lm1 |= HCI_LM_MASTER;
3916                         exact++;
3917                 } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
3918                         lm2 |= HCI_LM_ACCEPT;
3919                         if (l2cap_pi(sk)->role_switch)
3920                                 lm2 |= HCI_LM_MASTER;
3921                 }
3922         }
3923         read_unlock(&l2cap_sk_list.lock);
3924
3925         return exact ? lm1 : lm2;
3926 }
3927
3928 static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
3929 {
3930         struct l2cap_conn *conn;
3931
3932         BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
3933
3934         if (hcon->type != ACL_LINK)
3935                 return 0;
3936
3937         if (!status) {
3938                 conn = l2cap_conn_add(hcon, status);
3939                 if (conn)
3940                         l2cap_conn_ready(conn);
3941         } else
3942                 l2cap_conn_del(hcon, bt_err(status));
3943
3944         return 0;
3945 }
3946
3947 static int l2cap_disconn_ind(struct hci_conn *hcon)
3948 {
3949         struct l2cap_conn *conn = hcon->l2cap_data;
3950
3951         BT_DBG("hcon %p", hcon);
3952
3953         if (hcon->type != ACL_LINK || !conn)
3954                 return 0x13;
3955
3956         return conn->disc_reason;
3957 }
3958
3959 static int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
3960 {
3961         BT_DBG("hcon %p reason %d", hcon, reason);
3962
3963         if (hcon->type != ACL_LINK)
3964                 return 0;
3965
3966         l2cap_conn_del(hcon, bt_err(reason));
3967
3968         return 0;
3969 }
3970
3971 static inline void l2cap_check_encryption(struct sock *sk, u8 encrypt)
3972 {
3973         if (sk->sk_type != SOCK_SEQPACKET)
3974                 return;
3975
3976         if (encrypt == 0x00) {
3977                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_MEDIUM) {
3978                         l2cap_sock_clear_timer(sk);
3979                         l2cap_sock_set_timer(sk, HZ * 5);
3980                 } else if (l2cap_pi(sk)->sec_level == BT_SECURITY_HIGH)
3981                         __l2cap_sock_close(sk, ECONNREFUSED);
3982         } else {
3983                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_MEDIUM)
3984                         l2cap_sock_clear_timer(sk);
3985         }
3986 }
3987
3988 static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
3989 {
3990         struct l2cap_chan_list *l;
3991         struct l2cap_conn *conn = hcon->l2cap_data;
3992         struct sock *sk;
3993
3994         if (!conn)
3995                 return 0;
3996
3997         l = &conn->chan_list;
3998
3999         BT_DBG("conn %p", conn);
4000
4001         read_lock(&l->lock);
4002
4003         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
4004                 bh_lock_sock(sk);
4005
4006                 if (l2cap_pi(sk)->conf_state & L2CAP_CONF_CONNECT_PEND) {
4007                         bh_unlock_sock(sk);
4008                         continue;
4009                 }
4010
4011                 if (!status && (sk->sk_state == BT_CONNECTED ||
4012                                                 sk->sk_state == BT_CONFIG)) {
4013                         l2cap_check_encryption(sk, encrypt);
4014                         bh_unlock_sock(sk);
4015                         continue;
4016                 }
4017
4018                 if (sk->sk_state == BT_CONNECT) {
4019                         if (!status) {
4020                                 struct l2cap_conn_req req;
4021                                 req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
4022                                 req.psm  = l2cap_pi(sk)->psm;
4023
4024                                 l2cap_pi(sk)->ident = l2cap_get_ident(conn);
4025
4026                                 l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
4027                                         L2CAP_CONN_REQ, sizeof(req), &req);
4028                         } else {
4029                                 l2cap_sock_clear_timer(sk);
4030                                 l2cap_sock_set_timer(sk, HZ / 10);
4031                         }
4032                 } else if (sk->sk_state == BT_CONNECT2) {
4033                         struct l2cap_conn_rsp rsp;
4034                         __u16 result;
4035
4036                         if (!status) {
4037                                 sk->sk_state = BT_CONFIG;
4038                                 result = L2CAP_CR_SUCCESS;
4039                         } else {
4040                                 sk->sk_state = BT_DISCONN;
4041                                 l2cap_sock_set_timer(sk, HZ / 10);
4042                                 result = L2CAP_CR_SEC_BLOCK;
4043                         }
4044
4045                         rsp.scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
4046                         rsp.dcid   = cpu_to_le16(l2cap_pi(sk)->scid);
4047                         rsp.result = cpu_to_le16(result);
4048                         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
4049                         l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
4050                                         L2CAP_CONN_RSP, sizeof(rsp), &rsp);
4051                 }
4052
4053                 bh_unlock_sock(sk);
4054         }
4055
4056         read_unlock(&l->lock);
4057
4058         return 0;
4059 }
4060
4061 static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
4062 {
4063         struct l2cap_conn *conn = hcon->l2cap_data;
4064
4065         if (!conn && !(conn = l2cap_conn_add(hcon, 0)))
4066                 goto drop;
4067
4068         BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
4069
4070         if (flags & ACL_START) {
4071                 struct l2cap_hdr *hdr;
4072                 int len;
4073
4074                 if (conn->rx_len) {
4075                         BT_ERR("Unexpected start frame (len %d)", skb->len);
4076                         kfree_skb(conn->rx_skb);
4077                         conn->rx_skb = NULL;
4078                         conn->rx_len = 0;
4079                         l2cap_conn_unreliable(conn, ECOMM);
4080                 }
4081
4082                 if (skb->len < 2) {
4083                         BT_ERR("Frame is too short (len %d)", skb->len);
4084                         l2cap_conn_unreliable(conn, ECOMM);
4085                         goto drop;
4086                 }
4087
4088                 hdr = (struct l2cap_hdr *) skb->data;
4089                 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
4090
4091                 if (len == skb->len) {
4092                         /* Complete frame received */
4093                         l2cap_recv_frame(conn, skb);
4094                         return 0;
4095                 }
4096
4097                 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
4098
4099                 if (skb->len > len) {
4100                         BT_ERR("Frame is too long (len %d, expected len %d)",
4101                                 skb->len, len);
4102                         l2cap_conn_unreliable(conn, ECOMM);
4103                         goto drop;
4104                 }
4105
4106                 /* Allocate skb for the complete frame (with header) */
4107                 conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC);
4108                 if (!conn->rx_skb)
4109                         goto drop;
4110
4111                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
4112                                                                 skb->len);
4113                 conn->rx_len = len - skb->len;
4114         } else {
4115                 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
4116
4117                 if (!conn->rx_len) {
4118                         BT_ERR("Unexpected continuation frame (len %d)", skb->len);
4119                         l2cap_conn_unreliable(conn, ECOMM);
4120                         goto drop;
4121                 }
4122
4123                 if (skb->len > conn->rx_len) {
4124                         BT_ERR("Fragment is too long (len %d, expected %d)",
4125                                         skb->len, conn->rx_len);
4126                         kfree_skb(conn->rx_skb);
4127                         conn->rx_skb = NULL;
4128                         conn->rx_len = 0;
4129                         l2cap_conn_unreliable(conn, ECOMM);
4130                         goto drop;
4131                 }
4132
4133                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
4134                                                                 skb->len);
4135                 conn->rx_len -= skb->len;
4136
4137                 if (!conn->rx_len) {
4138                         /* Complete frame received */
4139                         l2cap_recv_frame(conn, conn->rx_skb);
4140                         conn->rx_skb = NULL;
4141                 }
4142         }
4143
4144 drop:
4145         kfree_skb(skb);
4146         return 0;
4147 }
4148
4149 static int l2cap_debugfs_show(struct seq_file *f, void *p)
4150 {
4151         struct sock *sk;
4152         struct hlist_node *node;
4153
4154         read_lock_bh(&l2cap_sk_list.lock);
4155
4156         sk_for_each(sk, node, &l2cap_sk_list.head) {
4157                 struct l2cap_pinfo *pi = l2cap_pi(sk);
4158
4159                 seq_printf(f, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d\n",
4160                                         batostr(&bt_sk(sk)->src),
4161                                         batostr(&bt_sk(sk)->dst),
4162                                         sk->sk_state, __le16_to_cpu(pi->psm),
4163                                         pi->scid, pi->dcid,
4164                                         pi->imtu, pi->omtu, pi->sec_level);
4165         }
4166
4167         read_unlock_bh(&l2cap_sk_list.lock);
4168
4169         return 0;
4170 }
4171
4172 static int l2cap_debugfs_open(struct inode *inode, struct file *file)
4173 {
4174         return single_open(file, l2cap_debugfs_show, inode->i_private);
4175 }
4176
4177 static const struct file_operations l2cap_debugfs_fops = {
4178         .open           = l2cap_debugfs_open,
4179         .read           = seq_read,
4180         .llseek         = seq_lseek,
4181         .release        = single_release,
4182 };
4183
4184 static struct dentry *l2cap_debugfs;
4185
4186 static const struct proto_ops l2cap_sock_ops = {
4187         .family         = PF_BLUETOOTH,
4188         .owner          = THIS_MODULE,
4189         .release        = l2cap_sock_release,
4190         .bind           = l2cap_sock_bind,
4191         .connect        = l2cap_sock_connect,
4192         .listen         = l2cap_sock_listen,
4193         .accept         = l2cap_sock_accept,
4194         .getname        = l2cap_sock_getname,
4195         .sendmsg        = l2cap_sock_sendmsg,
4196         .recvmsg        = l2cap_sock_recvmsg,
4197         .poll           = bt_sock_poll,
4198         .ioctl          = bt_sock_ioctl,
4199         .mmap           = sock_no_mmap,
4200         .socketpair     = sock_no_socketpair,
4201         .shutdown       = l2cap_sock_shutdown,
4202         .setsockopt     = l2cap_sock_setsockopt,
4203         .getsockopt     = l2cap_sock_getsockopt
4204 };
4205
4206 static const struct net_proto_family l2cap_sock_family_ops = {
4207         .family = PF_BLUETOOTH,
4208         .owner  = THIS_MODULE,
4209         .create = l2cap_sock_create,
4210 };
4211
4212 static struct hci_proto l2cap_hci_proto = {
4213         .name           = "L2CAP",
4214         .id             = HCI_PROTO_L2CAP,
4215         .connect_ind    = l2cap_connect_ind,
4216         .connect_cfm    = l2cap_connect_cfm,
4217         .disconn_ind    = l2cap_disconn_ind,
4218         .disconn_cfm    = l2cap_disconn_cfm,
4219         .security_cfm   = l2cap_security_cfm,
4220         .recv_acldata   = l2cap_recv_acldata
4221 };
4222
4223 static int __init l2cap_init(void)
4224 {
4225         int err;
4226
4227         err = proto_register(&l2cap_proto, 0);
4228         if (err < 0)
4229                 return err;
4230
4231         err = bt_sock_register(BTPROTO_L2CAP, &l2cap_sock_family_ops);
4232         if (err < 0) {
4233                 BT_ERR("L2CAP socket registration failed");
4234                 goto error;
4235         }
4236
4237         err = hci_register_proto(&l2cap_hci_proto);
4238         if (err < 0) {
4239                 BT_ERR("L2CAP protocol registration failed");
4240                 bt_sock_unregister(BTPROTO_L2CAP);
4241                 goto error;
4242         }
4243
4244         if (bt_debugfs) {
4245                 l2cap_debugfs = debugfs_create_file("l2cap", 0444,
4246                                         bt_debugfs, NULL, &l2cap_debugfs_fops);
4247                 if (!l2cap_debugfs)
4248                         BT_ERR("Failed to create L2CAP debug file");
4249         }
4250
4251         BT_INFO("L2CAP ver %s", VERSION);
4252         BT_INFO("L2CAP socket layer initialized");
4253
4254         return 0;
4255
4256 error:
4257         proto_unregister(&l2cap_proto);
4258         return err;
4259 }
4260
4261 static void __exit l2cap_exit(void)
4262 {
4263         debugfs_remove(l2cap_debugfs);
4264
4265         if (bt_sock_unregister(BTPROTO_L2CAP) < 0)
4266                 BT_ERR("L2CAP socket unregistration failed");
4267
4268         if (hci_unregister_proto(&l2cap_hci_proto) < 0)
4269                 BT_ERR("L2CAP protocol unregistration failed");
4270
4271         proto_unregister(&l2cap_proto);
4272 }
4273
4274 void l2cap_load(void)
4275 {
4276         /* Dummy function to trigger automatic L2CAP module loading by
4277          * other modules that use L2CAP sockets but don't use any other
4278          * symbols from it. */
4279         return;
4280 }
4281 EXPORT_SYMBOL(l2cap_load);
4282
4283 module_init(l2cap_init);
4284 module_exit(l2cap_exit);
4285
4286 module_param(enable_ertm, bool, 0644);
4287 MODULE_PARM_DESC(enable_ertm, "Enable enhanced retransmission mode");
4288
4289 module_param(max_transmit, uint, 0644);
4290 MODULE_PARM_DESC(max_transmit, "Max transmit value (default = 3)");
4291
4292 MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
4293 MODULE_DESCRIPTION("Bluetooth L2CAP ver " VERSION);
4294 MODULE_VERSION(VERSION);
4295 MODULE_LICENSE("GPL");
4296 MODULE_ALIAS("bt-proto-0");