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         queue_work(_busy_wq, &pi->busy_work);
3669
3670         return err;
3671 }
3672
3673 static int l2cap_streaming_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)
3674 {
3675         struct l2cap_pinfo *pi = l2cap_pi(sk);
3676         struct sk_buff *_skb;
3677         int err = -EINVAL;
3678
3679         /*
3680          * TODO: We have to notify the userland if some data is lost with the
3681          * Streaming Mode.
3682          */
3683
3684         switch (control & L2CAP_CTRL_SAR) {
3685         case L2CAP_SDU_UNSEGMENTED:
3686                 if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
3687                         kfree_skb(pi->sdu);
3688                         break;
3689                 }
3690
3691                 err = sock_queue_rcv_skb(sk, skb);
3692                 if (!err)
3693                         return 0;
3694
3695                 break;
3696
3697         case L2CAP_SDU_START:
3698                 if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
3699                         kfree_skb(pi->sdu);
3700                         break;
3701                 }
3702
3703                 pi->sdu_len = get_unaligned_le16(skb->data);
3704                 skb_pull(skb, 2);
3705
3706                 if (pi->sdu_len > pi->imtu) {
3707                         err = -EMSGSIZE;
3708                         break;
3709                 }
3710
3711                 pi->sdu = bt_skb_alloc(pi->sdu_len, GFP_ATOMIC);
3712                 if (!pi->sdu) {
3713                         err = -ENOMEM;
3714                         break;
3715                 }
3716
3717                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3718
3719                 pi->conn_state |= L2CAP_CONN_SAR_SDU;
3720                 pi->partial_sdu_len = skb->len;
3721                 err = 0;
3722                 break;
3723
3724         case L2CAP_SDU_CONTINUE:
3725                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
3726                         break;
3727
3728                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3729
3730                 pi->partial_sdu_len += skb->len;
3731                 if (pi->partial_sdu_len > pi->sdu_len)
3732                         kfree_skb(pi->sdu);
3733                 else
3734                         err = 0;
3735
3736                 break;
3737
3738         case L2CAP_SDU_END:
3739                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
3740                         break;
3741
3742                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3743
3744                 pi->conn_state &= ~L2CAP_CONN_SAR_SDU;
3745                 pi->partial_sdu_len += skb->len;
3746
3747                 if (pi->partial_sdu_len > pi->imtu)
3748                         goto drop;
3749
3750                 if (pi->partial_sdu_len == pi->sdu_len) {
3751                         _skb = skb_clone(pi->sdu, GFP_ATOMIC);
3752                         err = sock_queue_rcv_skb(sk, _skb);
3753                         if (err < 0)
3754                                 kfree_skb(_skb);
3755                 }
3756                 err = 0;
3757
3758 drop:
3759                 kfree_skb(pi->sdu);
3760                 break;
3761         }
3762
3763         kfree_skb(skb);
3764         return err;
3765 }
3766
3767 static void l2cap_check_srej_gap(struct sock *sk, u8 tx_seq)
3768 {
3769         struct sk_buff *skb;
3770         u16 control;
3771
3772         while ((skb = skb_peek(SREJ_QUEUE(sk)))) {
3773                 if (bt_cb(skb)->tx_seq != tx_seq)
3774                         break;
3775
3776                 skb = skb_dequeue(SREJ_QUEUE(sk));
3777                 control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
3778                 l2cap_ertm_reassembly_sdu(sk, skb, control);
3779                 l2cap_pi(sk)->buffer_seq_srej =
3780                         (l2cap_pi(sk)->buffer_seq_srej + 1) % 64;
3781                 tx_seq = (tx_seq + 1) % 64;
3782         }
3783 }
3784
3785 static void l2cap_resend_srejframe(struct sock *sk, u8 tx_seq)
3786 {
3787         struct l2cap_pinfo *pi = l2cap_pi(sk);
3788         struct srej_list *l, *tmp;
3789         u16 control;
3790
3791         list_for_each_entry_safe(l, tmp, SREJ_LIST(sk), list) {
3792                 if (l->tx_seq == tx_seq) {
3793                         list_del(&l->list);
3794                         kfree(l);
3795                         return;
3796                 }
3797                 control = L2CAP_SUPER_SELECT_REJECT;
3798                 control |= l->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3799                 l2cap_send_sframe(pi, control);
3800                 list_del(&l->list);
3801                 list_add_tail(&l->list, SREJ_LIST(sk));
3802         }
3803 }
3804
3805 static void l2cap_send_srejframe(struct sock *sk, u8 tx_seq)
3806 {
3807         struct l2cap_pinfo *pi = l2cap_pi(sk);
3808         struct srej_list *new;
3809         u16 control;
3810
3811         while (tx_seq != pi->expected_tx_seq) {
3812                 control = L2CAP_SUPER_SELECT_REJECT;
3813                 control |= pi->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3814                 l2cap_send_sframe(pi, control);
3815
3816                 new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC);
3817                 new->tx_seq = pi->expected_tx_seq;
3818                 pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3819                 list_add_tail(&new->list, SREJ_LIST(sk));
3820         }
3821         pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3822 }
3823
3824 static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
3825 {
3826         struct l2cap_pinfo *pi = l2cap_pi(sk);
3827         u8 tx_seq = __get_txseq(rx_control);
3828         u8 req_seq = __get_reqseq(rx_control);
3829         u8 sar = rx_control >> L2CAP_CTRL_SAR_SHIFT;
3830         int tx_seq_offset, expected_tx_seq_offset;
3831         int num_to_ack = (pi->tx_win/6) + 1;
3832         int err = 0;
3833
3834         BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
3835
3836         if (L2CAP_CTRL_FINAL & rx_control &&
3837                         l2cap_pi(sk)->conn_state & L2CAP_CONN_WAIT_F) {
3838                 del_timer(&pi->monitor_timer);
3839                 if (pi->unacked_frames > 0)
3840                         __mod_retrans_timer();
3841                 pi->conn_state &= ~L2CAP_CONN_WAIT_F;
3842         }
3843
3844         pi->expected_ack_seq = req_seq;
3845         l2cap_drop_acked_frames(sk);
3846
3847         if (tx_seq == pi->expected_tx_seq)
3848                 goto expected;
3849
3850         tx_seq_offset = (tx_seq - pi->buffer_seq) % 64;
3851         if (tx_seq_offset < 0)
3852                 tx_seq_offset += 64;
3853
3854         /* invalid tx_seq */
3855         if (tx_seq_offset >= pi->tx_win) {
3856                 l2cap_send_disconn_req(pi->conn, sk);
3857                 goto drop;
3858         }
3859
3860         if (pi->conn_state == L2CAP_CONN_LOCAL_BUSY)
3861                 goto drop;
3862
3863         if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
3864                 struct srej_list *first;
3865
3866                 first = list_first_entry(SREJ_LIST(sk),
3867                                 struct srej_list, list);
3868                 if (tx_seq == first->tx_seq) {
3869                         l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
3870                         l2cap_check_srej_gap(sk, tx_seq);
3871
3872                         list_del(&first->list);
3873                         kfree(first);
3874
3875                         if (list_empty(SREJ_LIST(sk))) {
3876                                 pi->buffer_seq = pi->buffer_seq_srej;
3877                                 pi->conn_state &= ~L2CAP_CONN_SREJ_SENT;
3878                                 l2cap_send_ack(pi);
3879                         }
3880                 } else {
3881                         struct srej_list *l;
3882
3883                         /* duplicated tx_seq */
3884                         if (l2cap_add_to_srej_queue(sk, skb, tx_seq, sar) < 0)
3885                                 goto drop;
3886
3887                         list_for_each_entry(l, SREJ_LIST(sk), list) {
3888                                 if (l->tx_seq == tx_seq) {
3889                                         l2cap_resend_srejframe(sk, tx_seq);
3890                                         return 0;
3891                                 }
3892                         }
3893                         l2cap_send_srejframe(sk, tx_seq);
3894                 }
3895         } else {
3896                 expected_tx_seq_offset =
3897                         (pi->expected_tx_seq - pi->buffer_seq) % 64;
3898                 if (expected_tx_seq_offset < 0)
3899                         expected_tx_seq_offset += 64;
3900
3901                 /* duplicated tx_seq */
3902                 if (tx_seq_offset < expected_tx_seq_offset)
3903                         goto drop;
3904
3905                 pi->conn_state |= L2CAP_CONN_SREJ_SENT;
3906
3907                 INIT_LIST_HEAD(SREJ_LIST(sk));
3908                 pi->buffer_seq_srej = pi->buffer_seq;
3909
3910                 __skb_queue_head_init(SREJ_QUEUE(sk));
3911                 __skb_queue_head_init(BUSY_QUEUE(sk));
3912                 l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
3913
3914                 pi->conn_state |= L2CAP_CONN_SEND_PBIT;
3915
3916                 l2cap_send_srejframe(sk, tx_seq);
3917         }
3918         return 0;
3919
3920 expected:
3921         pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3922
3923         if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
3924                 bt_cb(skb)->tx_seq = tx_seq;
3925                 bt_cb(skb)->sar = sar;
3926                 __skb_queue_tail(SREJ_QUEUE(sk), skb);
3927                 return 0;
3928         }
3929
3930         if (rx_control & L2CAP_CTRL_FINAL) {
3931                 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
3932                         pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
3933                 else
3934                         l2cap_retransmit_frames(sk);
3935         }
3936
3937         err = l2cap_push_rx_skb(sk, skb, rx_control);
3938         if (err < 0)
3939                 return 0;
3940
3941         __mod_ack_timer();
3942
3943         pi->num_acked = (pi->num_acked + 1) % num_to_ack;
3944         if (pi->num_acked == num_to_ack - 1)
3945                 l2cap_send_ack(pi);
3946
3947         return 0;
3948
3949 drop:
3950         kfree_skb(skb);
3951         return 0;
3952 }
3953
3954 static inline void l2cap_data_channel_rrframe(struct sock *sk, u16 rx_control)
3955 {
3956         struct l2cap_pinfo *pi = l2cap_pi(sk);
3957
3958         pi->expected_ack_seq = __get_reqseq(rx_control);
3959         l2cap_drop_acked_frames(sk);
3960
3961         if (rx_control & L2CAP_CTRL_POLL) {
3962                 if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
3963                         if ((pi->conn_state & L2CAP_CONN_REMOTE_BUSY) &&
3964                                         (pi->unacked_frames > 0))
3965                                 __mod_retrans_timer();
3966
3967                         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3968                         l2cap_send_srejtail(sk);
3969                 } else {
3970                         l2cap_send_i_or_rr_or_rnr(sk);
3971                 }
3972
3973         } else if (rx_control & L2CAP_CTRL_FINAL) {
3974                 pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3975
3976                 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
3977                         pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
3978                 else
3979                         l2cap_retransmit_frames(sk);
3980
3981         } else {
3982                 if ((pi->conn_state & L2CAP_CONN_REMOTE_BUSY) &&
3983                                 (pi->unacked_frames > 0))
3984                         __mod_retrans_timer();
3985
3986                 pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3987                 if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
3988                         l2cap_send_ack(pi);
3989                 } else {
3990                         spin_lock_bh(&pi->send_lock);
3991                         l2cap_ertm_send(sk);
3992                         spin_unlock_bh(&pi->send_lock);
3993                 }
3994         }
3995 }
3996
3997 static inline void l2cap_data_channel_rejframe(struct sock *sk, u16 rx_control)
3998 {
3999         struct l2cap_pinfo *pi = l2cap_pi(sk);
4000         u8 tx_seq = __get_reqseq(rx_control);
4001
4002         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
4003
4004         pi->expected_ack_seq = tx_seq;
4005         l2cap_drop_acked_frames(sk);
4006
4007         if (rx_control & L2CAP_CTRL_FINAL) {
4008                 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
4009                         pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
4010                 else
4011                         l2cap_retransmit_frames(sk);
4012         } else {
4013                 l2cap_retransmit_frames(sk);
4014
4015                 if (pi->conn_state & L2CAP_CONN_WAIT_F)
4016                         pi->conn_state |= L2CAP_CONN_REJ_ACT;
4017         }
4018 }
4019 static inline void l2cap_data_channel_srejframe(struct sock *sk, u16 rx_control)
4020 {
4021         struct l2cap_pinfo *pi = l2cap_pi(sk);
4022         u8 tx_seq = __get_reqseq(rx_control);
4023
4024         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
4025
4026         if (rx_control & L2CAP_CTRL_POLL) {
4027                 pi->expected_ack_seq = tx_seq;
4028                 l2cap_drop_acked_frames(sk);
4029                 l2cap_retransmit_one_frame(sk, tx_seq);
4030
4031                 spin_lock_bh(&pi->send_lock);
4032                 l2cap_ertm_send(sk);
4033                 spin_unlock_bh(&pi->send_lock);
4034
4035                 if (pi->conn_state & L2CAP_CONN_WAIT_F) {
4036                         pi->srej_save_reqseq = tx_seq;
4037                         pi->conn_state |= L2CAP_CONN_SREJ_ACT;
4038                 }
4039         } else if (rx_control & L2CAP_CTRL_FINAL) {
4040                 if ((pi->conn_state & L2CAP_CONN_SREJ_ACT) &&
4041                                 pi->srej_save_reqseq == tx_seq)
4042                         pi->conn_state &= ~L2CAP_CONN_SREJ_ACT;
4043                 else
4044                         l2cap_retransmit_one_frame(sk, tx_seq);
4045         } else {
4046                 l2cap_retransmit_one_frame(sk, tx_seq);
4047                 if (pi->conn_state & L2CAP_CONN_WAIT_F) {
4048                         pi->srej_save_reqseq = tx_seq;
4049                         pi->conn_state |= L2CAP_CONN_SREJ_ACT;
4050                 }
4051         }
4052 }
4053
4054 static inline void l2cap_data_channel_rnrframe(struct sock *sk, u16 rx_control)
4055 {
4056         struct l2cap_pinfo *pi = l2cap_pi(sk);
4057         u8 tx_seq = __get_reqseq(rx_control);
4058
4059         pi->conn_state |= L2CAP_CONN_REMOTE_BUSY;
4060         pi->expected_ack_seq = tx_seq;
4061         l2cap_drop_acked_frames(sk);
4062
4063         if (!(pi->conn_state & L2CAP_CONN_SREJ_SENT)) {
4064                 del_timer(&pi->retrans_timer);
4065                 if (rx_control & L2CAP_CTRL_POLL)
4066                         l2cap_send_rr_or_rnr(pi, L2CAP_CTRL_FINAL);
4067                 return;
4068         }
4069
4070         if (rx_control & L2CAP_CTRL_POLL)
4071                 l2cap_send_srejtail(sk);
4072         else
4073                 l2cap_send_sframe(pi, L2CAP_SUPER_RCV_READY);
4074 }
4075
4076 static inline int l2cap_data_channel_sframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
4077 {
4078         BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
4079
4080         if (L2CAP_CTRL_FINAL & rx_control &&
4081                         l2cap_pi(sk)->conn_state & L2CAP_CONN_WAIT_F) {
4082                 del_timer(&l2cap_pi(sk)->monitor_timer);
4083                 if (l2cap_pi(sk)->unacked_frames > 0)
4084                         __mod_retrans_timer();
4085                 l2cap_pi(sk)->conn_state &= ~L2CAP_CONN_WAIT_F;
4086         }
4087
4088         switch (rx_control & L2CAP_CTRL_SUPERVISE) {
4089         case L2CAP_SUPER_RCV_READY:
4090                 l2cap_data_channel_rrframe(sk, rx_control);
4091                 break;
4092
4093         case L2CAP_SUPER_REJECT:
4094                 l2cap_data_channel_rejframe(sk, rx_control);
4095                 break;
4096
4097         case L2CAP_SUPER_SELECT_REJECT:
4098                 l2cap_data_channel_srejframe(sk, rx_control);
4099                 break;
4100
4101         case L2CAP_SUPER_RCV_NOT_READY:
4102                 l2cap_data_channel_rnrframe(sk, rx_control);
4103                 break;
4104         }
4105
4106         kfree_skb(skb);
4107         return 0;
4108 }
4109
4110 static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
4111 {
4112         struct sock *sk;
4113         struct l2cap_pinfo *pi;
4114         u16 control;
4115         u8 tx_seq, req_seq;
4116         int len, next_tx_seq_offset, req_seq_offset;
4117
4118         sk = l2cap_get_chan_by_scid(&conn->chan_list, cid);
4119         if (!sk) {
4120                 BT_DBG("unknown cid 0x%4.4x", cid);
4121                 goto drop;
4122         }
4123
4124         pi = l2cap_pi(sk);
4125
4126         BT_DBG("sk %p, len %d", sk, skb->len);
4127
4128         if (sk->sk_state != BT_CONNECTED)
4129                 goto drop;
4130
4131         switch (pi->mode) {
4132         case L2CAP_MODE_BASIC:
4133                 /* If socket recv buffers overflows we drop data here
4134                  * which is *bad* because L2CAP has to be reliable.
4135                  * But we don't have any other choice. L2CAP doesn't
4136                  * provide flow control mechanism. */
4137
4138                 if (pi->imtu < skb->len)
4139                         goto drop;
4140
4141                 if (!sock_queue_rcv_skb(sk, skb))
4142                         goto done;
4143                 break;
4144
4145         case L2CAP_MODE_ERTM:
4146                 control = get_unaligned_le16(skb->data);
4147                 skb_pull(skb, 2);
4148                 len = skb->len;
4149
4150                 if (__is_sar_start(control) && __is_iframe(control))
4151                         len -= 2;
4152
4153                 if (pi->fcs == L2CAP_FCS_CRC16)
4154                         len -= 2;
4155
4156                 /*
4157                  * We can just drop the corrupted I-frame here.
4158                  * Receiver will miss it and start proper recovery
4159                  * procedures and ask retransmission.
4160                  */
4161                 if (len > pi->mps) {
4162                         l2cap_send_disconn_req(pi->conn, sk);
4163                         goto drop;
4164                 }
4165
4166                 if (l2cap_check_fcs(pi, skb))
4167                         goto drop;
4168
4169                 req_seq = __get_reqseq(control);
4170                 req_seq_offset = (req_seq - pi->expected_ack_seq) % 64;
4171                 if (req_seq_offset < 0)
4172                         req_seq_offset += 64;
4173
4174                 next_tx_seq_offset =
4175                         (pi->next_tx_seq - pi->expected_ack_seq) % 64;
4176                 if (next_tx_seq_offset < 0)
4177                         next_tx_seq_offset += 64;
4178
4179                 /* check for invalid req-seq */
4180                 if (req_seq_offset > next_tx_seq_offset) {
4181                         l2cap_send_disconn_req(pi->conn, sk);
4182                         goto drop;
4183                 }
4184
4185                 if (__is_iframe(control)) {
4186                         if (len < 0) {
4187                                 l2cap_send_disconn_req(pi->conn, sk);
4188                                 goto drop;
4189                         }
4190
4191                         l2cap_data_channel_iframe(sk, control, skb);
4192                 } else {
4193                         if (len != 0) {
4194                                 l2cap_send_disconn_req(pi->conn, sk);
4195                                 goto drop;
4196                         }
4197
4198                         l2cap_data_channel_sframe(sk, control, skb);
4199                 }
4200
4201                 goto done;
4202
4203         case L2CAP_MODE_STREAMING:
4204                 control = get_unaligned_le16(skb->data);
4205                 skb_pull(skb, 2);
4206                 len = skb->len;
4207
4208                 if (__is_sar_start(control))
4209                         len -= 2;
4210
4211                 if (pi->fcs == L2CAP_FCS_CRC16)
4212                         len -= 2;
4213
4214                 if (len > pi->mps || len < 0 || __is_sframe(control))
4215                         goto drop;
4216
4217                 if (l2cap_check_fcs(pi, skb))
4218                         goto drop;
4219
4220                 tx_seq = __get_txseq(control);
4221
4222                 if (pi->expected_tx_seq == tx_seq)
4223                         pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
4224                 else
4225                         pi->expected_tx_seq = (tx_seq + 1) % 64;
4226
4227                 l2cap_streaming_reassembly_sdu(sk, skb, control);
4228
4229                 goto done;
4230
4231         default:
4232                 BT_DBG("sk %p: bad mode 0x%2.2x", sk, pi->mode);
4233                 break;
4234         }
4235
4236 drop:
4237         kfree_skb(skb);
4238
4239 done:
4240         if (sk)
4241                 bh_unlock_sock(sk);
4242
4243         return 0;
4244 }
4245
4246 static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb)
4247 {
4248         struct sock *sk;
4249
4250         sk = l2cap_get_sock_by_psm(0, psm, conn->src);
4251         if (!sk)
4252                 goto drop;
4253
4254         BT_DBG("sk %p, len %d", sk, skb->len);
4255
4256         if (sk->sk_state != BT_BOUND && sk->sk_state != BT_CONNECTED)
4257                 goto drop;
4258
4259         if (l2cap_pi(sk)->imtu < skb->len)
4260                 goto drop;
4261
4262         if (!sock_queue_rcv_skb(sk, skb))
4263                 goto done;
4264
4265 drop:
4266         kfree_skb(skb);
4267
4268 done:
4269         if (sk)
4270                 bh_unlock_sock(sk);
4271         return 0;
4272 }
4273
4274 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
4275 {
4276         struct l2cap_hdr *lh = (void *) skb->data;
4277         u16 cid, len;
4278         __le16 psm;
4279
4280         skb_pull(skb, L2CAP_HDR_SIZE);
4281         cid = __le16_to_cpu(lh->cid);
4282         len = __le16_to_cpu(lh->len);
4283
4284         if (len != skb->len) {
4285                 kfree_skb(skb);
4286                 return;
4287         }
4288
4289         BT_DBG("len %d, cid 0x%4.4x", len, cid);
4290
4291         switch (cid) {
4292         case L2CAP_CID_SIGNALING:
4293                 l2cap_sig_channel(conn, skb);
4294                 break;
4295
4296         case L2CAP_CID_CONN_LESS:
4297                 psm = get_unaligned_le16(skb->data);
4298                 skb_pull(skb, 2);
4299                 l2cap_conless_channel(conn, psm, skb);
4300                 break;
4301
4302         default:
4303                 l2cap_data_channel(conn, cid, skb);
4304                 break;
4305         }
4306 }
4307
4308 /* ---- L2CAP interface with lower layer (HCI) ---- */
4309
4310 static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
4311 {
4312         int exact = 0, lm1 = 0, lm2 = 0;
4313         register struct sock *sk;
4314         struct hlist_node *node;
4315
4316         if (type != ACL_LINK)
4317                 return 0;
4318
4319         BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
4320
4321         /* Find listening sockets and check their link_mode */
4322         read_lock(&l2cap_sk_list.lock);
4323         sk_for_each(sk, node, &l2cap_sk_list.head) {
4324                 if (sk->sk_state != BT_LISTEN)
4325                         continue;
4326
4327                 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
4328                         lm1 |= HCI_LM_ACCEPT;
4329                         if (l2cap_pi(sk)->role_switch)
4330                                 lm1 |= HCI_LM_MASTER;
4331                         exact++;
4332                 } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
4333                         lm2 |= HCI_LM_ACCEPT;
4334                         if (l2cap_pi(sk)->role_switch)
4335                                 lm2 |= HCI_LM_MASTER;
4336                 }
4337         }
4338         read_unlock(&l2cap_sk_list.lock);
4339
4340         return exact ? lm1 : lm2;
4341 }
4342
4343 static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
4344 {
4345         struct l2cap_conn *conn;
4346
4347         BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
4348
4349         if (hcon->type != ACL_LINK)
4350                 return 0;
4351
4352         if (!status) {
4353                 conn = l2cap_conn_add(hcon, status);
4354                 if (conn)
4355                         l2cap_conn_ready(conn);
4356         } else
4357                 l2cap_conn_del(hcon, bt_err(status));
4358
4359         return 0;
4360 }
4361
4362 static int l2cap_disconn_ind(struct hci_conn *hcon)
4363 {
4364         struct l2cap_conn *conn = hcon->l2cap_data;
4365
4366         BT_DBG("hcon %p", hcon);
4367
4368         if (hcon->type != ACL_LINK || !conn)
4369                 return 0x13;
4370
4371         return conn->disc_reason;
4372 }
4373
4374 static int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
4375 {
4376         BT_DBG("hcon %p reason %d", hcon, reason);
4377
4378         if (hcon->type != ACL_LINK)
4379                 return 0;
4380
4381         l2cap_conn_del(hcon, bt_err(reason));
4382
4383         return 0;
4384 }
4385
4386 static inline void l2cap_check_encryption(struct sock *sk, u8 encrypt)
4387 {
4388         if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM)
4389                 return;
4390
4391         if (encrypt == 0x00) {
4392                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_MEDIUM) {
4393                         l2cap_sock_clear_timer(sk);
4394                         l2cap_sock_set_timer(sk, HZ * 5);
4395                 } else if (l2cap_pi(sk)->sec_level == BT_SECURITY_HIGH)
4396                         __l2cap_sock_close(sk, ECONNREFUSED);
4397         } else {
4398                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_MEDIUM)
4399                         l2cap_sock_clear_timer(sk);
4400         }
4401 }
4402
4403 static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
4404 {
4405         struct l2cap_chan_list *l;
4406         struct l2cap_conn *conn = hcon->l2cap_data;
4407         struct sock *sk;
4408
4409         if (!conn)
4410                 return 0;
4411
4412         l = &conn->chan_list;
4413
4414         BT_DBG("conn %p", conn);
4415
4416         read_lock(&l->lock);
4417
4418         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
4419                 bh_lock_sock(sk);
4420
4421                 if (l2cap_pi(sk)->conf_state & L2CAP_CONF_CONNECT_PEND) {
4422                         bh_unlock_sock(sk);
4423                         continue;
4424                 }
4425
4426                 if (!status && (sk->sk_state == BT_CONNECTED ||
4427                                                 sk->sk_state == BT_CONFIG)) {
4428                         l2cap_check_encryption(sk, encrypt);
4429                         bh_unlock_sock(sk);
4430                         continue;
4431                 }
4432
4433                 if (sk->sk_state == BT_CONNECT) {
4434                         if (!status) {
4435                                 struct l2cap_conn_req req;
4436                                 req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
4437                                 req.psm  = l2cap_pi(sk)->psm;
4438
4439                                 l2cap_pi(sk)->ident = l2cap_get_ident(conn);
4440                                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND;
4441
4442                                 l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
4443                                         L2CAP_CONN_REQ, sizeof(req), &req);
4444                         } else {
4445                                 l2cap_sock_clear_timer(sk);
4446                                 l2cap_sock_set_timer(sk, HZ / 10);
4447                         }
4448                 } else if (sk->sk_state == BT_CONNECT2) {
4449                         struct l2cap_conn_rsp rsp;
4450                         __u16 result;
4451
4452                         if (!status) {
4453                                 sk->sk_state = BT_CONFIG;
4454                                 result = L2CAP_CR_SUCCESS;
4455                         } else {
4456                                 sk->sk_state = BT_DISCONN;
4457                                 l2cap_sock_set_timer(sk, HZ / 10);
4458                                 result = L2CAP_CR_SEC_BLOCK;
4459                         }
4460
4461                         rsp.scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
4462                         rsp.dcid   = cpu_to_le16(l2cap_pi(sk)->scid);
4463                         rsp.result = cpu_to_le16(result);
4464                         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
4465                         l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
4466                                         L2CAP_CONN_RSP, sizeof(rsp), &rsp);
4467                 }
4468
4469                 bh_unlock_sock(sk);
4470         }
4471
4472         read_unlock(&l->lock);
4473
4474         return 0;
4475 }
4476
4477 static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
4478 {
4479         struct l2cap_conn *conn = hcon->l2cap_data;
4480
4481         if (!conn && !(conn = l2cap_conn_add(hcon, 0)))
4482                 goto drop;
4483
4484         BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
4485
4486         if (flags & ACL_START) {
4487                 struct l2cap_hdr *hdr;
4488                 int len;
4489
4490                 if (conn->rx_len) {
4491                         BT_ERR("Unexpected start frame (len %d)", skb->len);
4492                         kfree_skb(conn->rx_skb);
4493                         conn->rx_skb = NULL;
4494                         conn->rx_len = 0;
4495                         l2cap_conn_unreliable(conn, ECOMM);
4496                 }
4497
4498                 if (skb->len < 2) {
4499                         BT_ERR("Frame is too short (len %d)", skb->len);
4500                         l2cap_conn_unreliable(conn, ECOMM);
4501                         goto drop;
4502                 }
4503
4504                 hdr = (struct l2cap_hdr *) skb->data;
4505                 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
4506
4507                 if (len == skb->len) {
4508                         /* Complete frame received */
4509                         l2cap_recv_frame(conn, skb);
4510                         return 0;
4511                 }
4512
4513                 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
4514
4515                 if (skb->len > len) {
4516                         BT_ERR("Frame is too long (len %d, expected len %d)",
4517                                 skb->len, len);
4518                         l2cap_conn_unreliable(conn, ECOMM);
4519                         goto drop;
4520                 }
4521
4522                 /* Allocate skb for the complete frame (with header) */
4523                 conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC);
4524                 if (!conn->rx_skb)
4525                         goto drop;
4526
4527                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
4528                                                                 skb->len);
4529                 conn->rx_len = len - skb->len;
4530         } else {
4531                 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
4532
4533                 if (!conn->rx_len) {
4534                         BT_ERR("Unexpected continuation frame (len %d)", skb->len);
4535                         l2cap_conn_unreliable(conn, ECOMM);
4536                         goto drop;
4537                 }
4538
4539                 if (skb->len > conn->rx_len) {
4540                         BT_ERR("Fragment is too long (len %d, expected %d)",
4541                                         skb->len, conn->rx_len);
4542                         kfree_skb(conn->rx_skb);
4543                         conn->rx_skb = NULL;
4544                         conn->rx_len = 0;
4545                         l2cap_conn_unreliable(conn, ECOMM);
4546                         goto drop;
4547                 }
4548
4549                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
4550                                                                 skb->len);
4551                 conn->rx_len -= skb->len;
4552
4553                 if (!conn->rx_len) {
4554                         /* Complete frame received */
4555                         l2cap_recv_frame(conn, conn->rx_skb);
4556                         conn->rx_skb = NULL;
4557                 }
4558         }
4559
4560 drop:
4561         kfree_skb(skb);
4562         return 0;
4563 }
4564
4565 static int l2cap_debugfs_show(struct seq_file *f, void *p)
4566 {
4567         struct sock *sk;
4568         struct hlist_node *node;
4569
4570         read_lock_bh(&l2cap_sk_list.lock);
4571
4572         sk_for_each(sk, node, &l2cap_sk_list.head) {
4573                 struct l2cap_pinfo *pi = l2cap_pi(sk);
4574
4575                 seq_printf(f, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d\n",
4576                                         batostr(&bt_sk(sk)->src),
4577                                         batostr(&bt_sk(sk)->dst),
4578                                         sk->sk_state, __le16_to_cpu(pi->psm),
4579                                         pi->scid, pi->dcid,
4580                                         pi->imtu, pi->omtu, pi->sec_level);
4581         }
4582
4583         read_unlock_bh(&l2cap_sk_list.lock);
4584
4585         return 0;
4586 }
4587
4588 static int l2cap_debugfs_open(struct inode *inode, struct file *file)
4589 {
4590         return single_open(file, l2cap_debugfs_show, inode->i_private);
4591 }
4592
4593 static const struct file_operations l2cap_debugfs_fops = {
4594         .open           = l2cap_debugfs_open,
4595         .read           = seq_read,
4596         .llseek         = seq_lseek,
4597         .release        = single_release,
4598 };
4599
4600 static struct dentry *l2cap_debugfs;
4601
4602 static const struct proto_ops l2cap_sock_ops = {
4603         .family         = PF_BLUETOOTH,
4604         .owner          = THIS_MODULE,
4605         .release        = l2cap_sock_release,
4606         .bind           = l2cap_sock_bind,
4607         .connect        = l2cap_sock_connect,
4608         .listen         = l2cap_sock_listen,
4609         .accept         = l2cap_sock_accept,
4610         .getname        = l2cap_sock_getname,
4611         .sendmsg        = l2cap_sock_sendmsg,
4612         .recvmsg        = l2cap_sock_recvmsg,
4613         .poll           = bt_sock_poll,
4614         .ioctl          = bt_sock_ioctl,
4615         .mmap           = sock_no_mmap,
4616         .socketpair     = sock_no_socketpair,
4617         .shutdown       = l2cap_sock_shutdown,
4618         .setsockopt     = l2cap_sock_setsockopt,
4619         .getsockopt     = l2cap_sock_getsockopt
4620 };
4621
4622 static const struct net_proto_family l2cap_sock_family_ops = {
4623         .family = PF_BLUETOOTH,
4624         .owner  = THIS_MODULE,
4625         .create = l2cap_sock_create,
4626 };
4627
4628 static struct hci_proto l2cap_hci_proto = {
4629         .name           = "L2CAP",
4630         .id             = HCI_PROTO_L2CAP,
4631         .connect_ind    = l2cap_connect_ind,
4632         .connect_cfm    = l2cap_connect_cfm,
4633         .disconn_ind    = l2cap_disconn_ind,
4634         .disconn_cfm    = l2cap_disconn_cfm,
4635         .security_cfm   = l2cap_security_cfm,
4636         .recv_acldata   = l2cap_recv_acldata
4637 };
4638
4639 static int __init l2cap_init(void)
4640 {
4641         int err;
4642
4643         err = proto_register(&l2cap_proto, 0);
4644         if (err < 0)
4645                 return err;
4646
4647         _busy_wq = create_singlethread_workqueue("l2cap");
4648         if (!_busy_wq)
4649                 goto error;
4650
4651         err = bt_sock_register(BTPROTO_L2CAP, &l2cap_sock_family_ops);
4652         if (err < 0) {
4653                 BT_ERR("L2CAP socket registration failed");
4654                 goto error;
4655         }
4656
4657         err = hci_register_proto(&l2cap_hci_proto);
4658         if (err < 0) {
4659                 BT_ERR("L2CAP protocol registration failed");
4660                 bt_sock_unregister(BTPROTO_L2CAP);
4661                 goto error;
4662         }
4663
4664         if (bt_debugfs) {
4665                 l2cap_debugfs = debugfs_create_file("l2cap", 0444,
4666                                         bt_debugfs, NULL, &l2cap_debugfs_fops);
4667                 if (!l2cap_debugfs)
4668                         BT_ERR("Failed to create L2CAP debug file");
4669         }
4670
4671         BT_INFO("L2CAP ver %s", VERSION);
4672         BT_INFO("L2CAP socket layer initialized");
4673
4674         return 0;
4675
4676 error:
4677         proto_unregister(&l2cap_proto);
4678         return err;
4679 }
4680
4681 static void __exit l2cap_exit(void)
4682 {
4683         debugfs_remove(l2cap_debugfs);
4684
4685         flush_workqueue(_busy_wq);
4686         destroy_workqueue(_busy_wq);
4687
4688         if (bt_sock_unregister(BTPROTO_L2CAP) < 0)
4689                 BT_ERR("L2CAP socket unregistration failed");
4690
4691         if (hci_unregister_proto(&l2cap_hci_proto) < 0)
4692                 BT_ERR("L2CAP protocol unregistration failed");
4693
4694         proto_unregister(&l2cap_proto);
4695 }
4696
4697 void l2cap_load(void)
4698 {
4699         /* Dummy function to trigger automatic L2CAP module loading by
4700          * other modules that use L2CAP sockets but don't use any other
4701          * symbols from it. */
4702 }
4703 EXPORT_SYMBOL(l2cap_load);
4704
4705 module_init(l2cap_init);
4706 module_exit(l2cap_exit);
4707
4708 module_param(enable_ertm, bool, 0644);
4709 MODULE_PARM_DESC(enable_ertm, "Enable enhanced retransmission mode");
4710
4711 MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
4712 MODULE_DESCRIPTION("Bluetooth L2CAP ver " VERSION);
4713 MODULE_VERSION(VERSION);
4714 MODULE_LICENSE("GPL");
4715 MODULE_ALIAS("bt-proto-0");