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