net/bluetooth/l2cap_core.c

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