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 (!l2cap_mode_supported(pi->mode, pi->conn->feat_mask))
2621                         return -ECONNREFUSED;
2622                 break;
2623         default:
2624                 pi->mode = l2cap_select_mode(rfc.mode, pi->conn->feat_mask);
2625                 break;
2626         }
2627
2628 done:
2629         if (pi->mode != rfc.mode) {
2630                 result = L2CAP_CONF_UNACCEPT;
2631                 rfc.mode = pi->mode;
2632
2633                 if (pi->num_conf_rsp == 1)
2634                         return -ECONNREFUSED;
2635
2636                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2637                                         sizeof(rfc), (unsigned long) &rfc);
2638         }
2639
2640
2641         if (result == L2CAP_CONF_SUCCESS) {
2642                 /* Configure output options and let the other side know
2643                  * which ones we don't like. */
2644
2645                 if (mtu < L2CAP_DEFAULT_MIN_MTU)
2646                         result = L2CAP_CONF_UNACCEPT;
2647                 else {
2648                         pi->omtu = mtu;
2649                         pi->conf_state |= L2CAP_CONF_MTU_DONE;
2650                 }
2651                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->omtu);
2652
2653                 switch (rfc.mode) {
2654                 case L2CAP_MODE_BASIC:
2655                         pi->fcs = L2CAP_FCS_NONE;
2656                         pi->conf_state |= L2CAP_CONF_MODE_DONE;
2657                         break;
2658
2659                 case L2CAP_MODE_ERTM:
2660                         pi->remote_tx_win = rfc.txwin_size;
2661                         pi->remote_max_tx = rfc.max_transmit;
2662                         if (rfc.max_pdu_size > pi->conn->mtu - 10)
2663                                 rfc.max_pdu_size = le16_to_cpu(pi->conn->mtu - 10);
2664
2665                         pi->remote_mps = le16_to_cpu(rfc.max_pdu_size);
2666
2667                         rfc.retrans_timeout =
2668                                 le16_to_cpu(L2CAP_DEFAULT_RETRANS_TO);
2669                         rfc.monitor_timeout =
2670                                 le16_to_cpu(L2CAP_DEFAULT_MONITOR_TO);
2671
2672                         pi->conf_state |= L2CAP_CONF_MODE_DONE;
2673
2674                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2675                                         sizeof(rfc), (unsigned long) &rfc);
2676
2677                         break;
2678
2679                 case L2CAP_MODE_STREAMING:
2680                         if (rfc.max_pdu_size > pi->conn->mtu - 10)
2681                                 rfc.max_pdu_size = le16_to_cpu(pi->conn->mtu - 10);
2682
2683                         pi->remote_mps = le16_to_cpu(rfc.max_pdu_size);
2684
2685                         pi->conf_state |= L2CAP_CONF_MODE_DONE;
2686
2687                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2688                                         sizeof(rfc), (unsigned long) &rfc);
2689
2690                         break;
2691
2692                 default:
2693                         result = L2CAP_CONF_UNACCEPT;
2694
2695                         memset(&rfc, 0, sizeof(rfc));
2696                         rfc.mode = pi->mode;
2697                 }
2698
2699                 if (result == L2CAP_CONF_SUCCESS)
2700                         pi->conf_state |= L2CAP_CONF_OUTPUT_DONE;
2701         }
2702         rsp->scid   = cpu_to_le16(pi->dcid);
2703         rsp->result = cpu_to_le16(result);
2704         rsp->flags  = cpu_to_le16(0x0000);
2705
2706         return ptr - data;
2707 }
2708
2709 static int l2cap_parse_conf_rsp(struct sock *sk, void *rsp, int len, void *data, u16 *result)
2710 {
2711         struct l2cap_pinfo *pi = l2cap_pi(sk);
2712         struct l2cap_conf_req *req = data;
2713         void *ptr = req->data;
2714         int type, olen;
2715         unsigned long val;
2716         struct l2cap_conf_rfc rfc;
2717
2718         BT_DBG("sk %p, rsp %p, len %d, req %p", sk, rsp, len, data);
2719
2720         while (len >= L2CAP_CONF_OPT_SIZE) {
2721                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
2722
2723                 switch (type) {
2724                 case L2CAP_CONF_MTU:
2725                         if (val < L2CAP_DEFAULT_MIN_MTU) {
2726                                 *result = L2CAP_CONF_UNACCEPT;
2727                                 pi->omtu = L2CAP_DEFAULT_MIN_MTU;
2728                         } else
2729                                 pi->omtu = val;
2730                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->omtu);
2731                         break;
2732
2733                 case L2CAP_CONF_FLUSH_TO:
2734                         pi->flush_to = val;
2735                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
2736                                                         2, pi->flush_to);
2737                         break;
2738
2739                 case L2CAP_CONF_RFC:
2740                         if (olen == sizeof(rfc))
2741                                 memcpy(&rfc, (void *)val, olen);
2742
2743                         if ((pi->conf_state & L2CAP_CONF_STATE2_DEVICE) &&
2744                                                         rfc.mode != pi->mode)
2745                                 return -ECONNREFUSED;
2746
2747                         pi->mode = rfc.mode;
2748                         pi->fcs = 0;
2749
2750                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2751                                         sizeof(rfc), (unsigned long) &rfc);
2752                         break;
2753                 }
2754         }
2755
2756         if (*result == L2CAP_CONF_SUCCESS) {
2757                 switch (rfc.mode) {
2758                 case L2CAP_MODE_ERTM:
2759                         pi->remote_tx_win   = rfc.txwin_size;
2760                         pi->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
2761                         pi->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
2762                         pi->mps    = le16_to_cpu(rfc.max_pdu_size);
2763                         break;
2764                 case L2CAP_MODE_STREAMING:
2765                         pi->mps    = le16_to_cpu(rfc.max_pdu_size);
2766                 }
2767         }
2768
2769         req->dcid   = cpu_to_le16(pi->dcid);
2770         req->flags  = cpu_to_le16(0x0000);
2771
2772         return ptr - data;
2773 }
2774
2775 static int l2cap_build_conf_rsp(struct sock *sk, void *data, u16 result, u16 flags)
2776 {
2777         struct l2cap_conf_rsp *rsp = data;
2778         void *ptr = rsp->data;
2779
2780         BT_DBG("sk %p", sk);
2781
2782         rsp->scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
2783         rsp->result = cpu_to_le16(result);
2784         rsp->flags  = cpu_to_le16(flags);
2785
2786         return ptr - data;
2787 }
2788
2789 static void l2cap_conf_rfc_get(struct sock *sk, void *rsp, int len)
2790 {
2791         struct l2cap_pinfo *pi = l2cap_pi(sk);
2792         int type, olen;
2793         unsigned long val;
2794         struct l2cap_conf_rfc rfc;
2795
2796         BT_DBG("sk %p, rsp %p, len %d", sk, rsp, len);
2797
2798         if ((pi->mode != L2CAP_MODE_ERTM) && (pi->mode != L2CAP_MODE_STREAMING))
2799                 return;
2800
2801         while (len >= L2CAP_CONF_OPT_SIZE) {
2802                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
2803
2804                 switch (type) {
2805                 case L2CAP_CONF_RFC:
2806                         if (olen == sizeof(rfc))
2807                                 memcpy(&rfc, (void *)val, olen);
2808                         goto done;
2809                 }
2810         }
2811
2812 done:
2813         switch (rfc.mode) {
2814         case L2CAP_MODE_ERTM:
2815                 pi->remote_tx_win   = rfc.txwin_size;
2816                 pi->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
2817                 pi->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
2818                 pi->mps    = le16_to_cpu(rfc.max_pdu_size);
2819                 break;
2820         case L2CAP_MODE_STREAMING:
2821                 pi->mps    = le16_to_cpu(rfc.max_pdu_size);
2822         }
2823 }
2824
2825 static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2826 {
2827         struct l2cap_cmd_rej *rej = (struct l2cap_cmd_rej *) data;
2828
2829         if (rej->reason != 0x0000)
2830                 return 0;
2831
2832         if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
2833                                         cmd->ident == conn->info_ident) {
2834                 del_timer(&conn->info_timer);
2835
2836                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2837                 conn->info_ident = 0;
2838
2839                 l2cap_conn_start(conn);
2840         }
2841
2842         return 0;
2843 }
2844
2845 static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2846 {
2847         struct l2cap_chan_list *list = &conn->chan_list;
2848         struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
2849         struct l2cap_conn_rsp rsp;
2850         struct sock *sk, *parent;
2851         int result, status = L2CAP_CS_NO_INFO;
2852
2853         u16 dcid = 0, scid = __le16_to_cpu(req->scid);
2854         __le16 psm = req->psm;
2855
2856         BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid);
2857
2858         /* Check if we have socket listening on psm */
2859         parent = l2cap_get_sock_by_psm(BT_LISTEN, psm, conn->src);
2860         if (!parent) {
2861                 result = L2CAP_CR_BAD_PSM;
2862                 goto sendresp;
2863         }
2864
2865         /* Check if the ACL is secure enough (if not SDP) */
2866         if (psm != cpu_to_le16(0x0001) &&
2867                                 !hci_conn_check_link_mode(conn->hcon)) {
2868                 conn->disc_reason = 0x05;
2869                 result = L2CAP_CR_SEC_BLOCK;
2870                 goto response;
2871         }
2872
2873         result = L2CAP_CR_NO_MEM;
2874
2875         /* Check for backlog size */
2876         if (sk_acceptq_is_full(parent)) {
2877                 BT_DBG("backlog full %d", parent->sk_ack_backlog);
2878                 goto response;
2879         }
2880
2881         sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, GFP_ATOMIC);
2882         if (!sk)
2883                 goto response;
2884
2885         write_lock_bh(&list->lock);
2886
2887         /* Check if we already have channel with that dcid */
2888         if (__l2cap_get_chan_by_dcid(list, scid)) {
2889                 write_unlock_bh(&list->lock);
2890                 sock_set_flag(sk, SOCK_ZAPPED);
2891                 l2cap_sock_kill(sk);
2892                 goto response;
2893         }
2894
2895         hci_conn_hold(conn->hcon);
2896
2897         l2cap_sock_init(sk, parent);
2898         bacpy(&bt_sk(sk)->src, conn->src);
2899         bacpy(&bt_sk(sk)->dst, conn->dst);
2900         l2cap_pi(sk)->psm  = psm;
2901         l2cap_pi(sk)->dcid = scid;
2902
2903         __l2cap_chan_add(conn, sk, parent);
2904         dcid = l2cap_pi(sk)->scid;
2905
2906         l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
2907
2908         l2cap_pi(sk)->ident = cmd->ident;
2909
2910         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
2911                 if (l2cap_check_security(sk)) {
2912                         if (bt_sk(sk)->defer_setup) {
2913                                 sk->sk_state = BT_CONNECT2;
2914                                 result = L2CAP_CR_PEND;
2915                                 status = L2CAP_CS_AUTHOR_PEND;
2916                                 parent->sk_data_ready(parent, 0);
2917                         } else {
2918                                 sk->sk_state = BT_CONFIG;
2919                                 result = L2CAP_CR_SUCCESS;
2920                                 status = L2CAP_CS_NO_INFO;
2921                         }
2922                 } else {
2923                         sk->sk_state = BT_CONNECT2;
2924                         result = L2CAP_CR_PEND;
2925                         status = L2CAP_CS_AUTHEN_PEND;
2926                 }
2927         } else {
2928                 sk->sk_state = BT_CONNECT2;
2929                 result = L2CAP_CR_PEND;
2930                 status = L2CAP_CS_NO_INFO;
2931         }
2932
2933         write_unlock_bh(&list->lock);
2934
2935 response:
2936         bh_unlock_sock(parent);
2937
2938 sendresp:
2939         rsp.scid   = cpu_to_le16(scid);
2940         rsp.dcid   = cpu_to_le16(dcid);
2941         rsp.result = cpu_to_le16(result);
2942         rsp.status = cpu_to_le16(status);
2943         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
2944
2945         if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
2946                 struct l2cap_info_req info;
2947                 info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
2948
2949                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
2950                 conn->info_ident = l2cap_get_ident(conn);
2951
2952                 mod_timer(&conn->info_timer, jiffies +
2953                                         msecs_to_jiffies(L2CAP_INFO_TIMEOUT));
2954
2955                 l2cap_send_cmd(conn, conn->info_ident,
2956                                         L2CAP_INFO_REQ, sizeof(info), &info);
2957         }
2958
2959         return 0;
2960 }
2961
2962 static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2963 {
2964         struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
2965         u16 scid, dcid, result, status;
2966         struct sock *sk;
2967         u8 req[128];
2968
2969         scid   = __le16_to_cpu(rsp->scid);
2970         dcid   = __le16_to_cpu(rsp->dcid);
2971         result = __le16_to_cpu(rsp->result);
2972         status = __le16_to_cpu(rsp->status);
2973
2974         BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status);
2975
2976         if (scid) {
2977                 sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
2978                 if (!sk)
2979                         return 0;
2980         } else {
2981                 sk = l2cap_get_chan_by_ident(&conn->chan_list, cmd->ident);
2982                 if (!sk)
2983                         return 0;
2984         }
2985
2986         switch (result) {
2987         case L2CAP_CR_SUCCESS:
2988                 sk->sk_state = BT_CONFIG;
2989                 l2cap_pi(sk)->ident = 0;
2990                 l2cap_pi(sk)->dcid = dcid;
2991                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
2992                 l2cap_pi(sk)->conf_state &= ~L2CAP_CONF_CONNECT_PEND;
2993
2994                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2995                                         l2cap_build_conf_req(sk, req), req);
2996                 l2cap_pi(sk)->num_conf_req++;
2997                 break;
2998
2999         case L2CAP_CR_PEND:
3000                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND;
3001                 break;
3002
3003         default:
3004                 l2cap_chan_del(sk, ECONNREFUSED);
3005                 break;
3006         }
3007
3008         bh_unlock_sock(sk);
3009         return 0;
3010 }
3011
3012 static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
3013 {
3014         struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
3015         u16 dcid, flags;
3016         u8 rsp[64];
3017         struct sock *sk;
3018         int len;
3019
3020         dcid  = __le16_to_cpu(req->dcid);
3021         flags = __le16_to_cpu(req->flags);
3022
3023         BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
3024
3025         sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid);
3026         if (!sk)
3027                 return -ENOENT;
3028
3029         if (sk->sk_state == BT_DISCONN)
3030                 goto unlock;
3031
3032         /* Reject if config buffer is too small. */
3033         len = cmd_len - sizeof(*req);
3034         if (l2cap_pi(sk)->conf_len + len > sizeof(l2cap_pi(sk)->conf_req)) {
3035                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
3036                                 l2cap_build_conf_rsp(sk, rsp,
3037                                         L2CAP_CONF_REJECT, flags), rsp);
3038                 goto unlock;
3039         }
3040
3041         /* Store config. */
3042         memcpy(l2cap_pi(sk)->conf_req + l2cap_pi(sk)->conf_len, req->data, len);
3043         l2cap_pi(sk)->conf_len += len;
3044
3045         if (flags & 0x0001) {
3046                 /* Incomplete config. Send empty response. */
3047                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
3048                                 l2cap_build_conf_rsp(sk, rsp,
3049                                         L2CAP_CONF_SUCCESS, 0x0001), rsp);
3050                 goto unlock;
3051         }
3052
3053         /* Complete config. */
3054         len = l2cap_parse_conf_req(sk, rsp);
3055         if (len < 0) {
3056                 l2cap_send_disconn_req(conn, sk, ECONNRESET);
3057                 goto unlock;
3058         }
3059
3060         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
3061         l2cap_pi(sk)->num_conf_rsp++;
3062
3063         /* Reset config buffer. */
3064         l2cap_pi(sk)->conf_len = 0;
3065
3066         if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE))
3067                 goto unlock;
3068
3069         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_INPUT_DONE) {
3070                 if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_NO_FCS_RECV) ||
3071                     l2cap_pi(sk)->fcs != L2CAP_FCS_NONE)
3072                         l2cap_pi(sk)->fcs = L2CAP_FCS_CRC16;
3073
3074                 sk->sk_state = BT_CONNECTED;
3075
3076                 l2cap_pi(sk)->next_tx_seq = 0;
3077                 l2cap_pi(sk)->expected_tx_seq = 0;
3078                 __skb_queue_head_init(TX_QUEUE(sk));
3079                 if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM)
3080                         l2cap_ertm_init(sk);
3081
3082                 l2cap_chan_ready(sk);
3083                 goto unlock;
3084         }
3085
3086         if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT)) {
3087                 u8 buf[64];
3088                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3089                                         l2cap_build_conf_req(sk, buf), buf);
3090                 l2cap_pi(sk)->num_conf_req++;
3091         }
3092
3093 unlock:
3094         bh_unlock_sock(sk);
3095         return 0;
3096 }
3097
3098 static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3099 {
3100         struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
3101         u16 scid, flags, result;
3102         struct sock *sk;
3103         int len = cmd->len - sizeof(*rsp);
3104
3105         scid   = __le16_to_cpu(rsp->scid);
3106         flags  = __le16_to_cpu(rsp->flags);
3107         result = __le16_to_cpu(rsp->result);
3108
3109         BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x",
3110                         scid, flags, result);
3111
3112         sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
3113         if (!sk)
3114                 return 0;
3115
3116         switch (result) {
3117         case L2CAP_CONF_SUCCESS:
3118                 l2cap_conf_rfc_get(sk, rsp->data, len);
3119                 break;
3120
3121         case L2CAP_CONF_UNACCEPT:
3122                 if (l2cap_pi(sk)->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
3123                         char req[64];
3124
3125                         if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
3126                                 l2cap_send_disconn_req(conn, sk, ECONNRESET);
3127                                 goto done;
3128                         }
3129
3130                         /* throw out any old stored conf requests */
3131                         result = L2CAP_CONF_SUCCESS;
3132                         len = l2cap_parse_conf_rsp(sk, rsp->data,
3133                                                         len, req, &result);
3134                         if (len < 0) {
3135                                 l2cap_send_disconn_req(conn, sk, ECONNRESET);
3136                                 goto done;
3137                         }
3138
3139                         l2cap_send_cmd(conn, l2cap_get_ident(conn),
3140                                                 L2CAP_CONF_REQ, len, req);
3141                         l2cap_pi(sk)->num_conf_req++;
3142                         if (result != L2CAP_CONF_SUCCESS)
3143                                 goto done;
3144                         break;
3145                 }
3146
3147         default:
3148                 sk->sk_err = ECONNRESET;
3149                 l2cap_sock_set_timer(sk, HZ * 5);
3150                 l2cap_send_disconn_req(conn, sk, ECONNRESET);
3151                 goto done;
3152         }
3153
3154         if (flags & 0x01)
3155                 goto done;
3156
3157         l2cap_pi(sk)->conf_state |= L2CAP_CONF_INPUT_DONE;
3158
3159         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE) {
3160                 if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_NO_FCS_RECV) ||
3161                     l2cap_pi(sk)->fcs != L2CAP_FCS_NONE)
3162                         l2cap_pi(sk)->fcs = L2CAP_FCS_CRC16;
3163
3164                 sk->sk_state = BT_CONNECTED;
3165                 l2cap_pi(sk)->next_tx_seq = 0;
3166                 l2cap_pi(sk)->expected_tx_seq = 0;
3167                 __skb_queue_head_init(TX_QUEUE(sk));
3168                 if (l2cap_pi(sk)->mode ==  L2CAP_MODE_ERTM)
3169                         l2cap_ertm_init(sk);
3170
3171                 l2cap_chan_ready(sk);
3172         }
3173
3174 done:
3175         bh_unlock_sock(sk);
3176         return 0;
3177 }
3178
3179 static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3180 {
3181         struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
3182         struct l2cap_disconn_rsp rsp;
3183         u16 dcid, scid;
3184         struct sock *sk;
3185
3186         scid = __le16_to_cpu(req->scid);
3187         dcid = __le16_to_cpu(req->dcid);
3188
3189         BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
3190
3191         sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid);
3192         if (!sk)
3193                 return 0;
3194
3195         rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid);
3196         rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid);
3197         l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
3198
3199         sk->sk_shutdown = SHUTDOWN_MASK;
3200
3201         l2cap_chan_del(sk, ECONNRESET);
3202         bh_unlock_sock(sk);
3203
3204         l2cap_sock_kill(sk);
3205         return 0;
3206 }
3207
3208 static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3209 {
3210         struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
3211         u16 dcid, scid;
3212         struct sock *sk;
3213
3214         scid = __le16_to_cpu(rsp->scid);
3215         dcid = __le16_to_cpu(rsp->dcid);
3216
3217         BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
3218
3219         sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
3220         if (!sk)
3221                 return 0;
3222
3223         l2cap_chan_del(sk, 0);
3224         bh_unlock_sock(sk);
3225
3226         l2cap_sock_kill(sk);
3227         return 0;
3228 }
3229
3230 static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3231 {
3232         struct l2cap_info_req *req = (struct l2cap_info_req *) data;
3233         u16 type;
3234
3235         type = __le16_to_cpu(req->type);
3236
3237         BT_DBG("type 0x%4.4x", type);
3238
3239         if (type == L2CAP_IT_FEAT_MASK) {
3240                 u8 buf[8];
3241                 u32 feat_mask = l2cap_feat_mask;
3242                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
3243                 rsp->type   = cpu_to_le16(L2CAP_IT_FEAT_MASK);
3244                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
3245                 if (enable_ertm)
3246                         feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
3247                                                          | L2CAP_FEAT_FCS;
3248                 put_unaligned_le32(feat_mask, rsp->data);
3249                 l2cap_send_cmd(conn, cmd->ident,
3250                                         L2CAP_INFO_RSP, sizeof(buf), buf);
3251         } else if (type == L2CAP_IT_FIXED_CHAN) {
3252                 u8 buf[12];
3253                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
3254                 rsp->type   = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
3255                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
3256                 memcpy(buf + 4, l2cap_fixed_chan, 8);
3257                 l2cap_send_cmd(conn, cmd->ident,
3258                                         L2CAP_INFO_RSP, sizeof(buf), buf);
3259         } else {
3260                 struct l2cap_info_rsp rsp;
3261                 rsp.type   = cpu_to_le16(type);
3262                 rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP);
3263                 l2cap_send_cmd(conn, cmd->ident,
3264                                         L2CAP_INFO_RSP, sizeof(rsp), &rsp);
3265         }
3266
3267         return 0;
3268 }
3269
3270 static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3271 {
3272         struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
3273         u16 type, result;
3274
3275         type   = __le16_to_cpu(rsp->type);
3276         result = __le16_to_cpu(rsp->result);
3277
3278         BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
3279
3280         del_timer(&conn->info_timer);
3281
3282         if (type == L2CAP_IT_FEAT_MASK) {
3283                 conn->feat_mask = get_unaligned_le32(rsp->data);
3284
3285                 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
3286                         struct l2cap_info_req req;
3287                         req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
3288
3289                         conn->info_ident = l2cap_get_ident(conn);
3290
3291                         l2cap_send_cmd(conn, conn->info_ident,
3292                                         L2CAP_INFO_REQ, sizeof(req), &req);
3293                 } else {
3294                         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3295                         conn->info_ident = 0;
3296
3297                         l2cap_conn_start(conn);
3298                 }
3299         } else if (type == L2CAP_IT_FIXED_CHAN) {
3300                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3301                 conn->info_ident = 0;
3302
3303                 l2cap_conn_start(conn);
3304         }
3305
3306         return 0;
3307 }
3308
3309 static inline void l2cap_sig_channel(struct l2cap_conn *conn, struct sk_buff *skb)
3310 {
3311         u8 *data = skb->data;
3312         int len = skb->len;
3313         struct l2cap_cmd_hdr cmd;
3314         int err = 0;
3315
3316         l2cap_raw_recv(conn, skb);
3317
3318         while (len >= L2CAP_CMD_HDR_SIZE) {
3319                 u16 cmd_len;
3320                 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
3321                 data += L2CAP_CMD_HDR_SIZE;
3322                 len  -= L2CAP_CMD_HDR_SIZE;
3323
3324                 cmd_len = le16_to_cpu(cmd.len);
3325
3326                 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, cmd.ident);
3327
3328                 if (cmd_len > len || !cmd.ident) {
3329                         BT_DBG("corrupted command");
3330                         break;
3331                 }
3332
3333                 switch (cmd.code) {
3334                 case L2CAP_COMMAND_REJ:
3335                         l2cap_command_rej(conn, &cmd, data);
3336                         break;
3337
3338                 case L2CAP_CONN_REQ:
3339                         err = l2cap_connect_req(conn, &cmd, data);
3340                         break;
3341
3342                 case L2CAP_CONN_RSP:
3343                         err = l2cap_connect_rsp(conn, &cmd, data);
3344                         break;
3345
3346                 case L2CAP_CONF_REQ:
3347                         err = l2cap_config_req(conn, &cmd, cmd_len, data);
3348                         break;
3349
3350                 case L2CAP_CONF_RSP:
3351                         err = l2cap_config_rsp(conn, &cmd, data);
3352                         break;
3353
3354                 case L2CAP_DISCONN_REQ:
3355                         err = l2cap_disconnect_req(conn, &cmd, data);
3356                         break;
3357
3358                 case L2CAP_DISCONN_RSP:
3359                         err = l2cap_disconnect_rsp(conn, &cmd, data);
3360                         break;
3361
3362                 case L2CAP_ECHO_REQ:
3363                         l2cap_send_cmd(conn, cmd.ident, L2CAP_ECHO_RSP, cmd_len, data);
3364                         break;
3365
3366                 case L2CAP_ECHO_RSP:
3367                         break;
3368
3369                 case L2CAP_INFO_REQ:
3370                         err = l2cap_information_req(conn, &cmd, data);
3371                         break;
3372
3373                 case L2CAP_INFO_RSP:
3374                         err = l2cap_information_rsp(conn, &cmd, data);
3375                         break;
3376
3377                 default:
3378                         BT_ERR("Unknown signaling command 0x%2.2x", cmd.code);
3379                         err = -EINVAL;
3380                         break;
3381                 }
3382
3383                 if (err) {
3384                         struct l2cap_cmd_rej rej;
3385                         BT_DBG("error %d", err);
3386
3387                         /* FIXME: Map err to a valid reason */
3388                         rej.reason = cpu_to_le16(0);
3389                         l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
3390                 }
3391
3392                 data += cmd_len;
3393                 len  -= cmd_len;
3394         }
3395
3396         kfree_skb(skb);
3397 }
3398
3399 static int l2cap_check_fcs(struct l2cap_pinfo *pi,  struct sk_buff *skb)
3400 {
3401         u16 our_fcs, rcv_fcs;
3402         int hdr_size = L2CAP_HDR_SIZE + 2;
3403
3404         if (pi->fcs == L2CAP_FCS_CRC16) {
3405                 skb_trim(skb, skb->len - 2);
3406                 rcv_fcs = get_unaligned_le16(skb->data + skb->len);
3407                 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
3408
3409                 if (our_fcs != rcv_fcs)
3410                         return -EINVAL;
3411         }
3412         return 0;
3413 }
3414
3415 static inline void l2cap_send_i_or_rr_or_rnr(struct sock *sk)
3416 {
3417         struct l2cap_pinfo *pi = l2cap_pi(sk);
3418         u16 control = 0;
3419
3420         pi->frames_sent = 0;
3421
3422         control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3423
3424         if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
3425                 control |= L2CAP_SUPER_RCV_NOT_READY;
3426                 l2cap_send_sframe(pi, control);
3427                 pi->conn_state |= L2CAP_CONN_RNR_SENT;
3428         }
3429
3430         if (pi->conn_state & L2CAP_CONN_REMOTE_BUSY)
3431                 l2cap_retransmit_frames(sk);
3432
3433         spin_lock_bh(&pi->send_lock);
3434         l2cap_ertm_send(sk);
3435         spin_unlock_bh(&pi->send_lock);
3436
3437         if (!(pi->conn_state & L2CAP_CONN_LOCAL_BUSY) &&
3438                         pi->frames_sent == 0) {
3439                 control |= L2CAP_SUPER_RCV_READY;
3440                 l2cap_send_sframe(pi, control);
3441         }
3442 }
3443
3444 static int l2cap_add_to_srej_queue(struct sock *sk, struct sk_buff *skb, u8 tx_seq, u8 sar)
3445 {
3446         struct sk_buff *next_skb;
3447         struct l2cap_pinfo *pi = l2cap_pi(sk);
3448         int tx_seq_offset, next_tx_seq_offset;
3449
3450         bt_cb(skb)->tx_seq = tx_seq;
3451         bt_cb(skb)->sar = sar;
3452
3453         next_skb = skb_peek(SREJ_QUEUE(sk));
3454         if (!next_skb) {
3455                 __skb_queue_tail(SREJ_QUEUE(sk), skb);
3456                 return 0;
3457         }
3458
3459         tx_seq_offset = (tx_seq - pi->buffer_seq) % 64;
3460         if (tx_seq_offset < 0)
3461                 tx_seq_offset += 64;
3462
3463         do {
3464                 if (bt_cb(next_skb)->tx_seq == tx_seq)
3465                         return -EINVAL;
3466
3467                 next_tx_seq_offset = (bt_cb(next_skb)->tx_seq -
3468                                                 pi->buffer_seq) % 64;
3469                 if (next_tx_seq_offset < 0)
3470                         next_tx_seq_offset += 64;
3471
3472                 if (next_tx_seq_offset > tx_seq_offset) {
3473                         __skb_queue_before(SREJ_QUEUE(sk), next_skb, skb);
3474                         return 0;
3475                 }
3476
3477                 if (skb_queue_is_last(SREJ_QUEUE(sk), next_skb))
3478                         break;
3479
3480         } while ((next_skb = skb_queue_next(SREJ_QUEUE(sk), next_skb)));
3481
3482         __skb_queue_tail(SREJ_QUEUE(sk), skb);
3483
3484         return 0;
3485 }
3486
3487 static int l2cap_ertm_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)
3488 {
3489         struct l2cap_pinfo *pi = l2cap_pi(sk);
3490         struct sk_buff *_skb;
3491         int err;
3492
3493         switch (control & L2CAP_CTRL_SAR) {
3494         case L2CAP_SDU_UNSEGMENTED:
3495                 if (pi->conn_state & L2CAP_CONN_SAR_SDU)
3496                         goto drop;
3497
3498                 err = sock_queue_rcv_skb(sk, skb);
3499                 if (!err)
3500                         return err;
3501
3502                 break;
3503
3504         case L2CAP_SDU_START:
3505                 if (pi->conn_state & L2CAP_CONN_SAR_SDU)
3506                         goto drop;
3507
3508                 pi->sdu_len = get_unaligned_le16(skb->data);
3509
3510                 if (pi->sdu_len > pi->imtu)
3511                         goto disconnect;
3512
3513                 pi->sdu = bt_skb_alloc(pi->sdu_len, GFP_ATOMIC);
3514                 if (!pi->sdu)
3515                         return -ENOMEM;
3516
3517                 /* pull sdu_len bytes only after alloc, because of Local Busy
3518                  * condition we have to be sure that this will be executed
3519                  * only once, i.e., when alloc does not fail */
3520                 skb_pull(skb, 2);
3521
3522                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3523
3524                 pi->conn_state |= L2CAP_CONN_SAR_SDU;
3525                 pi->partial_sdu_len = skb->len;
3526                 break;
3527
3528         case L2CAP_SDU_CONTINUE:
3529                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
3530                         goto disconnect;
3531
3532                 if (!pi->sdu)
3533                         goto disconnect;
3534
3535                 pi->partial_sdu_len += skb->len;
3536                 if (pi->partial_sdu_len > pi->sdu_len)
3537                         goto drop;
3538
3539                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3540
3541                 break;
3542
3543         case L2CAP_SDU_END:
3544                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
3545                         goto disconnect;
3546
3547                 if (!pi->sdu)
3548                         goto disconnect;
3549
3550                 if (!(pi->conn_state & L2CAP_CONN_SAR_RETRY)) {
3551                         pi->partial_sdu_len += skb->len;
3552
3553                         if (pi->partial_sdu_len > pi->imtu)
3554                                 goto drop;
3555
3556                         if (pi->partial_sdu_len != pi->sdu_len)
3557                                 goto drop;
3558
3559                         memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3560                 }
3561
3562                 _skb = skb_clone(pi->sdu, GFP_ATOMIC);
3563                 if (!_skb) {
3564                         pi->conn_state |= L2CAP_CONN_SAR_RETRY;
3565                         return -ENOMEM;
3566                 }
3567
3568                 err = sock_queue_rcv_skb(sk, _skb);
3569                 if (err < 0) {
3570                         kfree_skb(_skb);
3571                         pi->conn_state |= L2CAP_CONN_SAR_RETRY;
3572                         return err;
3573                 }
3574
3575                 pi->conn_state &= ~L2CAP_CONN_SAR_RETRY;
3576                 pi->conn_state &= ~L2CAP_CONN_SAR_SDU;
3577
3578                 kfree_skb(pi->sdu);
3579                 break;
3580         }
3581
3582         kfree_skb(skb);
3583         return 0;
3584
3585 drop:
3586         kfree_skb(pi->sdu);
3587         pi->sdu = NULL;
3588
3589 disconnect:
3590         l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
3591         kfree_skb(skb);
3592         return 0;
3593 }
3594
3595 static void l2cap_busy_work(struct work_struct *work)
3596 {
3597         DECLARE_WAITQUEUE(wait, current);
3598         struct l2cap_pinfo *pi =
3599                 container_of(work, struct l2cap_pinfo, busy_work);
3600         struct sock *sk = (struct sock *)pi;
3601         int n_tries = 0, timeo = HZ/5, err;
3602         struct sk_buff *skb;
3603         u16 control;
3604
3605         lock_sock(sk);
3606
3607         add_wait_queue(sk_sleep(sk), &wait);
3608         while ((skb = skb_peek(BUSY_QUEUE(sk)))) {
3609                 set_current_state(TASK_INTERRUPTIBLE);
3610
3611                 if (n_tries++ > L2CAP_LOCAL_BUSY_TRIES) {
3612                         err = -EBUSY;
3613                         l2cap_send_disconn_req(pi->conn, sk, EBUSY);
3614                         goto done;
3615                 }
3616
3617                 if (!timeo)
3618                         timeo = HZ/5;
3619
3620                 if (signal_pending(current)) {
3621                         err = sock_intr_errno(timeo);
3622                         goto done;
3623                 }
3624
3625                 release_sock(sk);
3626                 timeo = schedule_timeout(timeo);
3627                 lock_sock(sk);
3628
3629                 err = sock_error(sk);
3630                 if (err)
3631                         goto done;
3632
3633                 while ((skb = skb_dequeue(BUSY_QUEUE(sk)))) {
3634                         control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
3635                         err = l2cap_ertm_reassembly_sdu(sk, skb, control);
3636                         if (err < 0) {
3637                                 skb_queue_head(BUSY_QUEUE(sk), skb);
3638                                 break;
3639                         }
3640
3641                         pi->buffer_seq = (pi->buffer_seq + 1) % 64;
3642                 }
3643
3644                 if (!skb)
3645                         break;
3646         }
3647
3648         if (!(pi->conn_state & L2CAP_CONN_RNR_SENT))
3649                 goto done;
3650
3651         control = pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3652         control |= L2CAP_SUPER_RCV_READY | L2CAP_CTRL_POLL;
3653         l2cap_send_sframe(pi, control);
3654         l2cap_pi(sk)->retry_count = 1;
3655
3656         del_timer(&pi->retrans_timer);
3657         __mod_monitor_timer();
3658
3659         l2cap_pi(sk)->conn_state |= L2CAP_CONN_WAIT_F;
3660
3661 done:
3662         pi->conn_state &= ~L2CAP_CONN_LOCAL_BUSY;
3663         pi->conn_state &= ~L2CAP_CONN_RNR_SENT;
3664
3665         BT_DBG("sk %p, Exit local busy", sk);
3666
3667         set_current_state(TASK_RUNNING);
3668         remove_wait_queue(sk_sleep(sk), &wait);
3669
3670         release_sock(sk);
3671 }
3672
3673 static int l2cap_push_rx_skb(struct sock *sk, struct sk_buff *skb, u16 control)
3674 {
3675         struct l2cap_pinfo *pi = l2cap_pi(sk);
3676         int sctrl, err;
3677
3678         if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
3679                 bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT;
3680                 __skb_queue_tail(BUSY_QUEUE(sk), skb);
3681                 return -EBUSY;
3682         }
3683
3684         err = l2cap_ertm_reassembly_sdu(sk, skb, control);
3685         if (err >= 0) {
3686                 pi->buffer_seq = (pi->buffer_seq + 1) % 64;
3687                 return err;
3688         }
3689
3690         /* Busy Condition */
3691         BT_DBG("sk %p, Enter local busy", sk);
3692
3693         pi->conn_state |= L2CAP_CONN_LOCAL_BUSY;
3694         bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT;
3695         __skb_queue_tail(BUSY_QUEUE(sk), skb);
3696
3697         sctrl = pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3698         sctrl |= L2CAP_SUPER_RCV_NOT_READY;
3699         l2cap_send_sframe(pi, sctrl);
3700
3701         pi->conn_state |= L2CAP_CONN_RNR_SENT;
3702
3703         del_timer(&pi->ack_timer);
3704
3705         queue_work(_busy_wq, &pi->busy_work);
3706
3707         return err;
3708 }
3709
3710 static int l2cap_streaming_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)
3711 {
3712         struct l2cap_pinfo *pi = l2cap_pi(sk);
3713         struct sk_buff *_skb;
3714         int err = -EINVAL;
3715
3716         /*
3717          * TODO: We have to notify the userland if some data is lost with the
3718          * Streaming Mode.
3719          */
3720
3721         switch (control & L2CAP_CTRL_SAR) {
3722         case L2CAP_SDU_UNSEGMENTED:
3723                 if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
3724                         kfree_skb(pi->sdu);
3725                         break;
3726                 }
3727
3728                 err = sock_queue_rcv_skb(sk, skb);
3729                 if (!err)
3730                         return 0;
3731
3732                 break;
3733
3734         case L2CAP_SDU_START:
3735                 if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
3736                         kfree_skb(pi->sdu);
3737                         break;
3738                 }
3739
3740                 pi->sdu_len = get_unaligned_le16(skb->data);
3741                 skb_pull(skb, 2);
3742
3743                 if (pi->sdu_len > pi->imtu) {
3744                         err = -EMSGSIZE;
3745                         break;
3746                 }
3747
3748                 pi->sdu = bt_skb_alloc(pi->sdu_len, GFP_ATOMIC);
3749                 if (!pi->sdu) {
3750                         err = -ENOMEM;
3751                         break;
3752                 }
3753
3754                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3755
3756                 pi->conn_state |= L2CAP_CONN_SAR_SDU;
3757                 pi->partial_sdu_len = skb->len;
3758                 err = 0;
3759                 break;
3760
3761         case L2CAP_SDU_CONTINUE:
3762                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
3763                         break;
3764
3765                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3766
3767                 pi->partial_sdu_len += skb->len;
3768                 if (pi->partial_sdu_len > pi->sdu_len)
3769                         kfree_skb(pi->sdu);
3770                 else
3771                         err = 0;
3772
3773                 break;
3774
3775         case L2CAP_SDU_END:
3776                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
3777                         break;
3778
3779                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3780
3781                 pi->conn_state &= ~L2CAP_CONN_SAR_SDU;
3782                 pi->partial_sdu_len += skb->len;
3783
3784                 if (pi->partial_sdu_len > pi->imtu)
3785                         goto drop;
3786
3787                 if (pi->partial_sdu_len == pi->sdu_len) {
3788                         _skb = skb_clone(pi->sdu, GFP_ATOMIC);
3789                         err = sock_queue_rcv_skb(sk, _skb);
3790                         if (err < 0)
3791                                 kfree_skb(_skb);
3792                 }
3793                 err = 0;
3794
3795 drop:
3796                 kfree_skb(pi->sdu);
3797                 break;
3798         }
3799
3800         kfree_skb(skb);
3801         return err;
3802 }
3803
3804 static void l2cap_check_srej_gap(struct sock *sk, u8 tx_seq)
3805 {
3806         struct sk_buff *skb;
3807         u16 control;
3808
3809         while ((skb = skb_peek(SREJ_QUEUE(sk)))) {
3810                 if (bt_cb(skb)->tx_seq != tx_seq)
3811                         break;
3812
3813                 skb = skb_dequeue(SREJ_QUEUE(sk));
3814                 control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
3815                 l2cap_ertm_reassembly_sdu(sk, skb, control);
3816                 l2cap_pi(sk)->buffer_seq_srej =
3817                         (l2cap_pi(sk)->buffer_seq_srej + 1) % 64;
3818                 tx_seq = (tx_seq + 1) % 64;
3819         }
3820 }
3821
3822 static void l2cap_resend_srejframe(struct sock *sk, u8 tx_seq)
3823 {
3824         struct l2cap_pinfo *pi = l2cap_pi(sk);
3825         struct srej_list *l, *tmp;
3826         u16 control;
3827
3828         list_for_each_entry_safe(l, tmp, SREJ_LIST(sk), list) {
3829                 if (l->tx_seq == tx_seq) {
3830                         list_del(&l->list);
3831                         kfree(l);
3832                         return;
3833                 }
3834                 control = L2CAP_SUPER_SELECT_REJECT;
3835                 control |= l->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3836                 l2cap_send_sframe(pi, control);
3837                 list_del(&l->list);
3838                 list_add_tail(&l->list, SREJ_LIST(sk));
3839         }
3840 }
3841
3842 static void l2cap_send_srejframe(struct sock *sk, u8 tx_seq)
3843 {
3844         struct l2cap_pinfo *pi = l2cap_pi(sk);
3845         struct srej_list *new;
3846         u16 control;
3847
3848         while (tx_seq != pi->expected_tx_seq) {
3849                 control = L2CAP_SUPER_SELECT_REJECT;
3850                 control |= pi->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3851                 l2cap_send_sframe(pi, control);
3852
3853                 new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC);
3854                 new->tx_seq = pi->expected_tx_seq;
3855                 pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3856                 list_add_tail(&new->list, SREJ_LIST(sk));
3857         }
3858         pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3859 }
3860
3861 static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
3862 {
3863         struct l2cap_pinfo *pi = l2cap_pi(sk);
3864         u8 tx_seq = __get_txseq(rx_control);
3865         u8 req_seq = __get_reqseq(rx_control);
3866         u8 sar = rx_control >> L2CAP_CTRL_SAR_SHIFT;
3867         int tx_seq_offset, expected_tx_seq_offset;
3868         int num_to_ack = (pi->tx_win/6) + 1;
3869         int err = 0;
3870
3871         BT_DBG("sk %p len %d tx_seq %d rx_control 0x%4.4x", sk, skb->len, tx_seq,
3872                                                                 rx_control);
3873
3874         if (L2CAP_CTRL_FINAL & rx_control &&
3875                         l2cap_pi(sk)->conn_state & L2CAP_CONN_WAIT_F) {
3876                 del_timer(&pi->monitor_timer);
3877                 if (pi->unacked_frames > 0)
3878                         __mod_retrans_timer();
3879                 pi->conn_state &= ~L2CAP_CONN_WAIT_F;
3880         }
3881
3882         pi->expected_ack_seq = req_seq;
3883         l2cap_drop_acked_frames(sk);
3884
3885         if (tx_seq == pi->expected_tx_seq)
3886                 goto expected;
3887
3888         tx_seq_offset = (tx_seq - pi->buffer_seq) % 64;
3889         if (tx_seq_offset < 0)
3890                 tx_seq_offset += 64;
3891
3892         /* invalid tx_seq */
3893         if (tx_seq_offset >= pi->tx_win) {
3894                 l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
3895                 goto drop;
3896         }
3897
3898         if (pi->conn_state == L2CAP_CONN_LOCAL_BUSY)
3899                 goto drop;
3900
3901         if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
3902                 struct srej_list *first;
3903
3904                 first = list_first_entry(SREJ_LIST(sk),
3905                                 struct srej_list, list);
3906                 if (tx_seq == first->tx_seq) {
3907                         l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
3908                         l2cap_check_srej_gap(sk, tx_seq);
3909
3910                         list_del(&first->list);
3911                         kfree(first);
3912
3913                         if (list_empty(SREJ_LIST(sk))) {
3914                                 pi->buffer_seq = pi->buffer_seq_srej;
3915                                 pi->conn_state &= ~L2CAP_CONN_SREJ_SENT;
3916                                 l2cap_send_ack(pi);
3917                                 BT_DBG("sk %p, Exit SREJ_SENT", sk);
3918                         }
3919                 } else {
3920                         struct srej_list *l;
3921
3922                         /* duplicated tx_seq */
3923                         if (l2cap_add_to_srej_queue(sk, skb, tx_seq, sar) < 0)
3924                                 goto drop;
3925
3926                         list_for_each_entry(l, SREJ_LIST(sk), list) {
3927                                 if (l->tx_seq == tx_seq) {
3928                                         l2cap_resend_srejframe(sk, tx_seq);
3929                                         return 0;
3930                                 }
3931                         }
3932                         l2cap_send_srejframe(sk, tx_seq);
3933                 }
3934         } else {
3935                 expected_tx_seq_offset =
3936                         (pi->expected_tx_seq - pi->buffer_seq) % 64;
3937                 if (expected_tx_seq_offset < 0)
3938                         expected_tx_seq_offset += 64;
3939
3940                 /* duplicated tx_seq */
3941                 if (tx_seq_offset < expected_tx_seq_offset)
3942                         goto drop;
3943
3944                 pi->conn_state |= L2CAP_CONN_SREJ_SENT;
3945
3946                 BT_DBG("sk %p, Enter SREJ", sk);
3947
3948                 INIT_LIST_HEAD(SREJ_LIST(sk));
3949                 pi->buffer_seq_srej = pi->buffer_seq;
3950
3951                 __skb_queue_head_init(SREJ_QUEUE(sk));
3952                 __skb_queue_head_init(BUSY_QUEUE(sk));
3953                 l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
3954
3955                 pi->conn_state |= L2CAP_CONN_SEND_PBIT;
3956
3957                 l2cap_send_srejframe(sk, tx_seq);
3958
3959                 del_timer(&pi->ack_timer);
3960         }
3961         return 0;
3962
3963 expected:
3964         pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3965
3966         if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
3967                 bt_cb(skb)->tx_seq = tx_seq;
3968                 bt_cb(skb)->sar = sar;
3969                 __skb_queue_tail(SREJ_QUEUE(sk), skb);
3970                 return 0;
3971         }
3972
3973         err = l2cap_push_rx_skb(sk, skb, rx_control);
3974         if (err < 0)
3975                 return 0;
3976
3977         if (rx_control & L2CAP_CTRL_FINAL) {
3978                 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
3979                         pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
3980                 else
3981                         l2cap_retransmit_frames(sk);
3982         }
3983
3984         __mod_ack_timer();
3985
3986         pi->num_acked = (pi->num_acked + 1) % num_to_ack;
3987         if (pi->num_acked == num_to_ack - 1)
3988                 l2cap_send_ack(pi);
3989
3990         return 0;
3991
3992 drop:
3993         kfree_skb(skb);
3994         return 0;
3995 }
3996
3997 static inline void l2cap_data_channel_rrframe(struct sock *sk, u16 rx_control)
3998 {
3999         struct l2cap_pinfo *pi = l2cap_pi(sk);
4000
4001         BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, __get_reqseq(rx_control),
4002                                                 rx_control);
4003
4004         pi->expected_ack_seq = __get_reqseq(rx_control);
4005         l2cap_drop_acked_frames(sk);
4006
4007         if (rx_control & L2CAP_CTRL_POLL) {
4008                 pi->conn_state |= L2CAP_CONN_SEND_FBIT;
4009                 if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
4010                         if ((pi->conn_state & L2CAP_CONN_REMOTE_BUSY) &&
4011                                         (pi->unacked_frames > 0))
4012                                 __mod_retrans_timer();
4013
4014                         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
4015                         l2cap_send_srejtail(sk);
4016                 } else {
4017                         l2cap_send_i_or_rr_or_rnr(sk);
4018                 }
4019
4020         } else if (rx_control & L2CAP_CTRL_FINAL) {
4021                 pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
4022
4023                 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
4024                         pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
4025                 else
4026                         l2cap_retransmit_frames(sk);
4027
4028         } else {
4029                 if ((pi->conn_state & L2CAP_CONN_REMOTE_BUSY) &&
4030                                 (pi->unacked_frames > 0))
4031                         __mod_retrans_timer();
4032
4033                 pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
4034                 if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
4035                         l2cap_send_ack(pi);
4036                 } else {
4037                         spin_lock_bh(&pi->send_lock);
4038                         l2cap_ertm_send(sk);
4039                         spin_unlock_bh(&pi->send_lock);
4040                 }
4041         }
4042 }
4043
4044 static inline void l2cap_data_channel_rejframe(struct sock *sk, u16 rx_control)
4045 {
4046         struct l2cap_pinfo *pi = l2cap_pi(sk);
4047         u8 tx_seq = __get_reqseq(rx_control);
4048
4049         BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, tx_seq, rx_control);
4050
4051         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
4052
4053         pi->expected_ack_seq = tx_seq;
4054         l2cap_drop_acked_frames(sk);
4055
4056         if (rx_control & L2CAP_CTRL_FINAL) {
4057                 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
4058                         pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
4059                 else
4060                         l2cap_retransmit_frames(sk);
4061         } else {
4062                 l2cap_retransmit_frames(sk);
4063
4064                 if (pi->conn_state & L2CAP_CONN_WAIT_F)
4065                         pi->conn_state |= L2CAP_CONN_REJ_ACT;
4066         }
4067 }
4068 static inline void l2cap_data_channel_srejframe(struct sock *sk, u16 rx_control)
4069 {
4070         struct l2cap_pinfo *pi = l2cap_pi(sk);
4071         u8 tx_seq = __get_reqseq(rx_control);
4072
4073         BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, tx_seq, rx_control);
4074
4075         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
4076
4077         if (rx_control & L2CAP_CTRL_POLL) {
4078                 pi->expected_ack_seq = tx_seq;
4079                 l2cap_drop_acked_frames(sk);
4080
4081                 pi->conn_state |= L2CAP_CONN_SEND_FBIT;
4082                 l2cap_retransmit_one_frame(sk, tx_seq);
4083
4084                 spin_lock_bh(&pi->send_lock);
4085                 l2cap_ertm_send(sk);
4086                 spin_unlock_bh(&pi->send_lock);
4087
4088                 if (pi->conn_state & L2CAP_CONN_WAIT_F) {
4089                         pi->srej_save_reqseq = tx_seq;
4090                         pi->conn_state |= L2CAP_CONN_SREJ_ACT;
4091                 }
4092         } else if (rx_control & L2CAP_CTRL_FINAL) {
4093                 if ((pi->conn_state & L2CAP_CONN_SREJ_ACT) &&
4094                                 pi->srej_save_reqseq == tx_seq)
4095                         pi->conn_state &= ~L2CAP_CONN_SREJ_ACT;
4096                 else
4097                         l2cap_retransmit_one_frame(sk, tx_seq);
4098         } else {
4099                 l2cap_retransmit_one_frame(sk, tx_seq);
4100                 if (pi->conn_state & L2CAP_CONN_WAIT_F) {
4101                         pi->srej_save_reqseq = tx_seq;
4102                         pi->conn_state |= L2CAP_CONN_SREJ_ACT;
4103                 }
4104         }
4105 }
4106
4107 static inline void l2cap_data_channel_rnrframe(struct sock *sk, u16 rx_control)
4108 {
4109         struct l2cap_pinfo *pi = l2cap_pi(sk);
4110         u8 tx_seq = __get_reqseq(rx_control);
4111
4112         BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, tx_seq, rx_control);
4113
4114         pi->conn_state |= L2CAP_CONN_REMOTE_BUSY;
4115         pi->expected_ack_seq = tx_seq;
4116         l2cap_drop_acked_frames(sk);
4117
4118         if (rx_control & L2CAP_CTRL_POLL)
4119                 pi->conn_state |= L2CAP_CONN_SEND_FBIT;
4120
4121         if (!(pi->conn_state & L2CAP_CONN_SREJ_SENT)) {
4122                 del_timer(&pi->retrans_timer);
4123                 if (rx_control & L2CAP_CTRL_POLL)
4124                         l2cap_send_rr_or_rnr(pi, L2CAP_CTRL_FINAL);
4125                 return;
4126         }
4127
4128         if (rx_control & L2CAP_CTRL_POLL)
4129                 l2cap_send_srejtail(sk);
4130         else
4131                 l2cap_send_sframe(pi, L2CAP_SUPER_RCV_READY);
4132 }
4133
4134 static inline int l2cap_data_channel_sframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
4135 {
4136         BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
4137
4138         if (L2CAP_CTRL_FINAL & rx_control &&
4139                         l2cap_pi(sk)->conn_state & L2CAP_CONN_WAIT_F) {
4140                 del_timer(&l2cap_pi(sk)->monitor_timer);
4141                 if (l2cap_pi(sk)->unacked_frames > 0)
4142                         __mod_retrans_timer();
4143                 l2cap_pi(sk)->conn_state &= ~L2CAP_CONN_WAIT_F;
4144         }
4145
4146         switch (rx_control & L2CAP_CTRL_SUPERVISE) {
4147         case L2CAP_SUPER_RCV_READY:
4148                 l2cap_data_channel_rrframe(sk, rx_control);
4149                 break;
4150
4151         case L2CAP_SUPER_REJECT:
4152                 l2cap_data_channel_rejframe(sk, rx_control);
4153                 break;
4154
4155         case L2CAP_SUPER_SELECT_REJECT:
4156                 l2cap_data_channel_srejframe(sk, rx_control);
4157                 break;
4158
4159         case L2CAP_SUPER_RCV_NOT_READY:
4160                 l2cap_data_channel_rnrframe(sk, rx_control);
4161                 break;
4162         }
4163
4164         kfree_skb(skb);
4165         return 0;
4166 }
4167
4168 static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
4169 {
4170         struct sock *sk;
4171         struct l2cap_pinfo *pi;
4172         u16 control;
4173         u8 tx_seq, req_seq;
4174         int len, next_tx_seq_offset, req_seq_offset;
4175
4176         sk = l2cap_get_chan_by_scid(&conn->chan_list, cid);
4177         if (!sk) {
4178                 BT_DBG("unknown cid 0x%4.4x", cid);
4179                 goto drop;
4180         }
4181
4182         pi = l2cap_pi(sk);
4183
4184         BT_DBG("sk %p, len %d", sk, skb->len);
4185
4186         if (sk->sk_state != BT_CONNECTED)
4187                 goto drop;
4188
4189         switch (pi->mode) {
4190         case L2CAP_MODE_BASIC:
4191                 /* If socket recv buffers overflows we drop data here
4192                  * which is *bad* because L2CAP has to be reliable.
4193                  * But we don't have any other choice. L2CAP doesn't
4194                  * provide flow control mechanism. */
4195
4196                 if (pi->imtu < skb->len)
4197                         goto drop;
4198
4199                 if (!sock_queue_rcv_skb(sk, skb))
4200                         goto done;
4201                 break;
4202
4203         case L2CAP_MODE_ERTM:
4204                 control = get_unaligned_le16(skb->data);
4205                 skb_pull(skb, 2);
4206                 len = skb->len;
4207
4208                 /*
4209                  * We can just drop the corrupted I-frame here.
4210                  * Receiver will miss it and start proper recovery
4211                  * procedures and ask retransmission.
4212                  */
4213                 if (l2cap_check_fcs(pi, skb))
4214                         goto drop;
4215
4216                 if (__is_sar_start(control) && __is_iframe(control))
4217                         len -= 2;
4218
4219                 if (pi->fcs == L2CAP_FCS_CRC16)
4220                         len -= 2;
4221
4222                 if (len > pi->mps) {
4223                         l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
4224                         goto drop;
4225                 }
4226
4227                 req_seq = __get_reqseq(control);
4228                 req_seq_offset = (req_seq - pi->expected_ack_seq) % 64;
4229                 if (req_seq_offset < 0)
4230                         req_seq_offset += 64;
4231
4232                 next_tx_seq_offset =
4233                         (pi->next_tx_seq - pi->expected_ack_seq) % 64;
4234                 if (next_tx_seq_offset < 0)
4235                         next_tx_seq_offset += 64;
4236
4237                 /* check for invalid req-seq */
4238                 if (req_seq_offset > next_tx_seq_offset) {
4239                         l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
4240                         goto drop;
4241                 }
4242
4243                 if (__is_iframe(control)) {
4244                         if (len < 0) {
4245                                 l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
4246                                 goto drop;
4247                         }
4248
4249                         l2cap_data_channel_iframe(sk, control, skb);
4250                 } else {
4251                         if (len != 0) {
4252                                 l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
4253                                 goto drop;
4254                         }
4255
4256                         l2cap_data_channel_sframe(sk, control, skb);
4257                 }
4258
4259                 goto done;
4260
4261         case L2CAP_MODE_STREAMING:
4262                 control = get_unaligned_le16(skb->data);
4263                 skb_pull(skb, 2);
4264                 len = skb->len;
4265
4266                 if (l2cap_check_fcs(pi, skb))
4267                         goto drop;
4268
4269                 if (__is_sar_start(control))
4270                         len -= 2;
4271
4272                 if (pi->fcs == L2CAP_FCS_CRC16)
4273                         len -= 2;
4274
4275                 if (len > pi->mps || len < 0 || __is_sframe(control))
4276                         goto drop;
4277
4278                 tx_seq = __get_txseq(control);
4279
4280                 if (pi->expected_tx_seq == tx_seq)
4281                         pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
4282                 else
4283                         pi->expected_tx_seq = (tx_seq + 1) % 64;
4284
4285                 l2cap_streaming_reassembly_sdu(sk, skb, control);
4286
4287                 goto done;
4288
4289         default:
4290                 BT_DBG("sk %p: bad mode 0x%2.2x", sk, pi->mode);
4291                 break;
4292         }
4293
4294 drop:
4295         kfree_skb(skb);
4296
4297 done:
4298         if (sk)
4299                 bh_unlock_sock(sk);
4300
4301         return 0;
4302 }
4303
4304 static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb)
4305 {
4306         struct sock *sk;
4307
4308         sk = l2cap_get_sock_by_psm(0, psm, conn->src);
4309         if (!sk)
4310                 goto drop;
4311
4312         BT_DBG("sk %p, len %d", sk, skb->len);
4313
4314         if (sk->sk_state != BT_BOUND && sk->sk_state != BT_CONNECTED)
4315                 goto drop;
4316
4317         if (l2cap_pi(sk)->imtu < skb->len)
4318                 goto drop;
4319
4320         if (!sock_queue_rcv_skb(sk, skb))
4321                 goto done;
4322
4323 drop:
4324         kfree_skb(skb);
4325
4326 done:
4327         if (sk)
4328                 bh_unlock_sock(sk);
4329         return 0;
4330 }
4331
4332 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
4333 {
4334         struct l2cap_hdr *lh = (void *) skb->data;
4335         u16 cid, len;
4336         __le16 psm;
4337
4338         skb_pull(skb, L2CAP_HDR_SIZE);
4339         cid = __le16_to_cpu(lh->cid);
4340         len = __le16_to_cpu(lh->len);
4341
4342         if (len != skb->len) {
4343                 kfree_skb(skb);
4344                 return;
4345         }
4346
4347         BT_DBG("len %d, cid 0x%4.4x", len, cid);
4348
4349         switch (cid) {
4350         case L2CAP_CID_SIGNALING:
4351                 l2cap_sig_channel(conn, skb);
4352                 break;
4353
4354         case L2CAP_CID_CONN_LESS:
4355                 psm = get_unaligned_le16(skb->data);
4356                 skb_pull(skb, 2);
4357                 l2cap_conless_channel(conn, psm, skb);
4358                 break;
4359
4360         default:
4361                 l2cap_data_channel(conn, cid, skb);
4362                 break;
4363         }
4364 }
4365
4366 /* ---- L2CAP interface with lower layer (HCI) ---- */
4367
4368 static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
4369 {
4370         int exact = 0, lm1 = 0, lm2 = 0;
4371         register struct sock *sk;
4372         struct hlist_node *node;
4373
4374         if (type != ACL_LINK)
4375                 return 0;
4376
4377         BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
4378
4379         /* Find listening sockets and check their link_mode */
4380         read_lock(&l2cap_sk_list.lock);
4381         sk_for_each(sk, node, &l2cap_sk_list.head) {
4382                 if (sk->sk_state != BT_LISTEN)
4383                         continue;
4384
4385                 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
4386                         lm1 |= HCI_LM_ACCEPT;
4387                         if (l2cap_pi(sk)->role_switch)
4388                                 lm1 |= HCI_LM_MASTER;
4389                         exact++;
4390                 } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
4391                         lm2 |= HCI_LM_ACCEPT;
4392                         if (l2cap_pi(sk)->role_switch)
4393                                 lm2 |= HCI_LM_MASTER;
4394                 }
4395         }
4396         read_unlock(&l2cap_sk_list.lock);
4397
4398         return exact ? lm1 : lm2;
4399 }
4400
4401 static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
4402 {
4403         struct l2cap_conn *conn;
4404
4405         BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
4406
4407         if (hcon->type != ACL_LINK)
4408                 return 0;
4409
4410         if (!status) {
4411                 conn = l2cap_conn_add(hcon, status);
4412                 if (conn)
4413                         l2cap_conn_ready(conn);
4414         } else
4415                 l2cap_conn_del(hcon, bt_err(status));
4416
4417         return 0;
4418 }
4419
4420 static int l2cap_disconn_ind(struct hci_conn *hcon)
4421 {
4422         struct l2cap_conn *conn = hcon->l2cap_data;
4423
4424         BT_DBG("hcon %p", hcon);
4425
4426         if (hcon->type != ACL_LINK || !conn)
4427                 return 0x13;
4428
4429         return conn->disc_reason;
4430 }
4431
4432 static int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
4433 {
4434         BT_DBG("hcon %p reason %d", hcon, reason);
4435
4436         if (hcon->type != ACL_LINK)
4437                 return 0;
4438
4439         l2cap_conn_del(hcon, bt_err(reason));
4440
4441         return 0;
4442 }
4443
4444 static inline void l2cap_check_encryption(struct sock *sk, u8 encrypt)
4445 {
4446         if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM)
4447                 return;
4448
4449         if (encrypt == 0x00) {
4450                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_MEDIUM) {
4451                         l2cap_sock_clear_timer(sk);
4452                         l2cap_sock_set_timer(sk, HZ * 5);
4453                 } else if (l2cap_pi(sk)->sec_level == BT_SECURITY_HIGH)
4454                         __l2cap_sock_close(sk, ECONNREFUSED);
4455         } else {
4456                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_MEDIUM)
4457                         l2cap_sock_clear_timer(sk);
4458         }
4459 }
4460
4461 static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
4462 {
4463         struct l2cap_chan_list *l;
4464         struct l2cap_conn *conn = hcon->l2cap_data;
4465         struct sock *sk;
4466
4467         if (!conn)
4468                 return 0;
4469
4470         l = &conn->chan_list;
4471
4472         BT_DBG("conn %p", conn);
4473
4474         read_lock(&l->lock);
4475
4476         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
4477                 bh_lock_sock(sk);
4478
4479                 if (l2cap_pi(sk)->conf_state & L2CAP_CONF_CONNECT_PEND) {
4480                         bh_unlock_sock(sk);
4481                         continue;
4482                 }
4483
4484                 if (!status && (sk->sk_state == BT_CONNECTED ||
4485                                                 sk->sk_state == BT_CONFIG)) {
4486                         l2cap_check_encryption(sk, encrypt);
4487                         bh_unlock_sock(sk);
4488                         continue;
4489                 }
4490
4491                 if (sk->sk_state == BT_CONNECT) {
4492                         if (!status) {
4493                                 struct l2cap_conn_req req;
4494                                 req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
4495                                 req.psm  = l2cap_pi(sk)->psm;
4496
4497                                 l2cap_pi(sk)->ident = l2cap_get_ident(conn);
4498                                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND;
4499
4500                                 l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
4501                                         L2CAP_CONN_REQ, sizeof(req), &req);
4502                         } else {
4503                                 l2cap_sock_clear_timer(sk);
4504                                 l2cap_sock_set_timer(sk, HZ / 10);
4505                         }
4506                 } else if (sk->sk_state == BT_CONNECT2) {
4507                         struct l2cap_conn_rsp rsp;
4508                         __u16 result;
4509
4510                         if (!status) {
4511                                 sk->sk_state = BT_CONFIG;
4512                                 result = L2CAP_CR_SUCCESS;
4513                         } else {
4514                                 sk->sk_state = BT_DISCONN;
4515                                 l2cap_sock_set_timer(sk, HZ / 10);
4516                                 result = L2CAP_CR_SEC_BLOCK;
4517                         }
4518
4519                         rsp.scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
4520                         rsp.dcid   = cpu_to_le16(l2cap_pi(sk)->scid);
4521                         rsp.result = cpu_to_le16(result);
4522                         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
4523                         l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
4524                                         L2CAP_CONN_RSP, sizeof(rsp), &rsp);
4525                 }
4526
4527                 bh_unlock_sock(sk);
4528         }
4529
4530         read_unlock(&l->lock);
4531
4532         return 0;
4533 }
4534
4535 static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
4536 {
4537         struct l2cap_conn *conn = hcon->l2cap_data;
4538
4539         if (!conn && !(conn = l2cap_conn_add(hcon, 0)))
4540                 goto drop;
4541
4542         BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
4543
4544         if (flags & ACL_START) {
4545                 struct l2cap_hdr *hdr;
4546                 int len;
4547
4548                 if (conn->rx_len) {
4549                         BT_ERR("Unexpected start frame (len %d)", skb->len);
4550                         kfree_skb(conn->rx_skb);
4551                         conn->rx_skb = NULL;
4552                         conn->rx_len = 0;
4553                         l2cap_conn_unreliable(conn, ECOMM);
4554                 }
4555
4556                 if (skb->len < 2) {
4557                         BT_ERR("Frame is too short (len %d)", skb->len);
4558                         l2cap_conn_unreliable(conn, ECOMM);
4559                         goto drop;
4560                 }
4561
4562                 hdr = (struct l2cap_hdr *) skb->data;
4563                 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
4564
4565                 if (len == skb->len) {
4566                         /* Complete frame received */
4567                         l2cap_recv_frame(conn, skb);
4568                         return 0;
4569                 }
4570
4571                 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
4572
4573                 if (skb->len > len) {
4574                         BT_ERR("Frame is too long (len %d, expected len %d)",
4575                                 skb->len, len);
4576                         l2cap_conn_unreliable(conn, ECOMM);
4577                         goto drop;
4578                 }
4579
4580                 /* Allocate skb for the complete frame (with header) */
4581                 conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC);
4582                 if (!conn->rx_skb)
4583                         goto drop;
4584
4585                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
4586                                                                 skb->len);
4587                 conn->rx_len = len - skb->len;
4588         } else {
4589                 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
4590
4591                 if (!conn->rx_len) {
4592                         BT_ERR("Unexpected continuation frame (len %d)", skb->len);
4593                         l2cap_conn_unreliable(conn, ECOMM);
4594                         goto drop;
4595                 }
4596
4597                 if (skb->len > conn->rx_len) {
4598                         BT_ERR("Fragment is too long (len %d, expected %d)",
4599                                         skb->len, conn->rx_len);
4600                         kfree_skb(conn->rx_skb);
4601                         conn->rx_skb = NULL;
4602                         conn->rx_len = 0;
4603                         l2cap_conn_unreliable(conn, ECOMM);
4604                         goto drop;
4605                 }
4606
4607                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
4608                                                                 skb->len);
4609                 conn->rx_len -= skb->len;
4610
4611                 if (!conn->rx_len) {
4612                         /* Complete frame received */
4613                         l2cap_recv_frame(conn, conn->rx_skb);
4614                         conn->rx_skb = NULL;
4615                 }
4616         }
4617
4618 drop:
4619         kfree_skb(skb);
4620         return 0;
4621 }
4622
4623 static int l2cap_debugfs_show(struct seq_file *f, void *p)
4624 {
4625         struct sock *sk;
4626         struct hlist_node *node;
4627
4628         read_lock_bh(&l2cap_sk_list.lock);
4629
4630         sk_for_each(sk, node, &l2cap_sk_list.head) {
4631                 struct l2cap_pinfo *pi = l2cap_pi(sk);
4632
4633                 seq_printf(f, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d\n",
4634                                         batostr(&bt_sk(sk)->src),
4635                                         batostr(&bt_sk(sk)->dst),
4636                                         sk->sk_state, __le16_to_cpu(pi->psm),
4637                                         pi->scid, pi->dcid,
4638                                         pi->imtu, pi->omtu, pi->sec_level);
4639         }
4640
4641         read_unlock_bh(&l2cap_sk_list.lock);
4642
4643         return 0;
4644 }
4645
4646 static int l2cap_debugfs_open(struct inode *inode, struct file *file)
4647 {
4648         return single_open(file, l2cap_debugfs_show, inode->i_private);
4649 }
4650
4651 static const struct file_operations l2cap_debugfs_fops = {
4652         .open           = l2cap_debugfs_open,
4653         .read           = seq_read,
4654         .llseek         = seq_lseek,
4655         .release        = single_release,
4656 };
4657
4658 static struct dentry *l2cap_debugfs;
4659
4660 static const struct proto_ops l2cap_sock_ops = {
4661         .family         = PF_BLUETOOTH,
4662         .owner          = THIS_MODULE,
4663         .release        = l2cap_sock_release,
4664         .bind           = l2cap_sock_bind,
4665         .connect        = l2cap_sock_connect,
4666         .listen         = l2cap_sock_listen,
4667         .accept         = l2cap_sock_accept,
4668         .getname        = l2cap_sock_getname,
4669         .sendmsg        = l2cap_sock_sendmsg,
4670         .recvmsg        = l2cap_sock_recvmsg,
4671         .poll           = bt_sock_poll,
4672         .ioctl          = bt_sock_ioctl,
4673         .mmap           = sock_no_mmap,
4674         .socketpair     = sock_no_socketpair,
4675         .shutdown       = l2cap_sock_shutdown,
4676         .setsockopt     = l2cap_sock_setsockopt,
4677         .getsockopt     = l2cap_sock_getsockopt
4678 };
4679
4680 static const struct net_proto_family l2cap_sock_family_ops = {
4681         .family = PF_BLUETOOTH,
4682         .owner  = THIS_MODULE,
4683         .create = l2cap_sock_create,
4684 };
4685
4686 static struct hci_proto l2cap_hci_proto = {
4687         .name           = "L2CAP",
4688         .id             = HCI_PROTO_L2CAP,
4689         .connect_ind    = l2cap_connect_ind,
4690         .connect_cfm    = l2cap_connect_cfm,
4691         .disconn_ind    = l2cap_disconn_ind,
4692         .disconn_cfm    = l2cap_disconn_cfm,
4693         .security_cfm   = l2cap_security_cfm,
4694         .recv_acldata   = l2cap_recv_acldata
4695 };
4696
4697 static int __init l2cap_init(void)
4698 {
4699         int err;
4700
4701         err = proto_register(&l2cap_proto, 0);
4702         if (err < 0)
4703                 return err;
4704
4705         _busy_wq = create_singlethread_workqueue("l2cap");
4706         if (!_busy_wq)
4707                 goto error;
4708
4709         err = bt_sock_register(BTPROTO_L2CAP, &l2cap_sock_family_ops);
4710         if (err < 0) {
4711                 BT_ERR("L2CAP socket registration failed");
4712                 goto error;
4713         }
4714
4715         err = hci_register_proto(&l2cap_hci_proto);
4716         if (err < 0) {
4717                 BT_ERR("L2CAP protocol registration failed");
4718                 bt_sock_unregister(BTPROTO_L2CAP);
4719                 goto error;
4720         }
4721
4722         if (bt_debugfs) {
4723                 l2cap_debugfs = debugfs_create_file("l2cap", 0444,
4724                                         bt_debugfs, NULL, &l2cap_debugfs_fops);
4725                 if (!l2cap_debugfs)
4726                         BT_ERR("Failed to create L2CAP debug file");
4727         }
4728
4729         BT_INFO("L2CAP ver %s", VERSION);
4730         BT_INFO("L2CAP socket layer initialized");
4731
4732         return 0;
4733
4734 error:
4735         proto_unregister(&l2cap_proto);
4736         return err;
4737 }
4738
4739 static void __exit l2cap_exit(void)
4740 {
4741         debugfs_remove(l2cap_debugfs);
4742
4743         flush_workqueue(_busy_wq);
4744         destroy_workqueue(_busy_wq);
4745
4746         if (bt_sock_unregister(BTPROTO_L2CAP) < 0)
4747                 BT_ERR("L2CAP socket unregistration failed");
4748
4749         if (hci_unregister_proto(&l2cap_hci_proto) < 0)
4750                 BT_ERR("L2CAP protocol unregistration failed");
4751
4752         proto_unregister(&l2cap_proto);
4753 }
4754
4755 void l2cap_load(void)
4756 {
4757         /* Dummy function to trigger automatic L2CAP module loading by
4758          * other modules that use L2CAP sockets but don't use any other
4759          * symbols from it. */
4760 }
4761 EXPORT_SYMBOL(l2cap_load);
4762
4763 module_init(l2cap_init);
4764 module_exit(l2cap_exit);
4765
4766 module_param(enable_ertm, bool, 0644);
4767 MODULE_PARM_DESC(enable_ertm, "Enable enhanced retransmission mode");
4768
4769 MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
4770 MODULE_DESCRIPTION("Bluetooth L2CAP ver " VERSION);
4771 MODULE_VERSION(VERSION);
4772 MODULE_LICENSE("GPL");
4773 MODULE_ALIAS("bt-proto-0");