Pileus Git - ~andy/linux/blob - net/bluetooth/l2cap.c

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