net/bluetooth/l2cap.c

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