net/bluetooth/l2cap.c

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