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