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