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