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