net/bluetooth/l2cap.c

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