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                 pi->unacked_frames++;
1120                 pi->frames_sent++;
1121
1122                 if (skb_queue_is_last(TX_QUEUE(sk), skb))
1123                         sk->sk_send_head = NULL;
1124                 else
1125                         sk->sk_send_head = skb_queue_next(TX_QUEUE(sk), skb);
1126
1127                 nsent++;
1128         }
1129
1130         return nsent;
1131 }
1132
1133 static int l2cap_retransmit_frames(struct sock *sk)
1134 {
1135         struct l2cap_pinfo *pi = l2cap_pi(sk);
1136         int ret;
1137
1138         if (!skb_queue_empty(TX_QUEUE(sk)))
1139                 sk->sk_send_head = TX_QUEUE(sk)->next;
1140
1141         pi->next_tx_seq = pi->expected_ack_seq;
1142         ret = l2cap_ertm_send(sk);
1143         return ret;
1144 }
1145
1146 static void l2cap_send_ack(struct l2cap_pinfo *pi)
1147 {
1148         struct sock *sk = (struct sock *)pi;
1149         u16 control = 0;
1150
1151         control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
1152
1153         if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
1154                 control |= L2CAP_SUPER_RCV_NOT_READY;
1155                 pi->conn_state |= L2CAP_CONN_RNR_SENT;
1156                 l2cap_send_sframe(pi, control);
1157                 return;
1158         }
1159
1160         if (l2cap_ertm_send(sk) > 0)
1161                 return;
1162
1163         control |= L2CAP_SUPER_RCV_READY;
1164         l2cap_send_sframe(pi, control);
1165 }
1166
1167 static void l2cap_send_srejtail(struct sock *sk)
1168 {
1169         struct srej_list *tail;
1170         u16 control;
1171
1172         control = L2CAP_SUPER_SELECT_REJECT;
1173         control |= L2CAP_CTRL_FINAL;
1174
1175         tail = list_entry(SREJ_LIST(sk)->prev, struct srej_list, list);
1176         control |= tail->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
1177
1178         l2cap_send_sframe(l2cap_pi(sk), control);
1179 }
1180
1181 static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, int len, int count, struct sk_buff *skb)
1182 {
1183         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1184         struct sk_buff **frag;
1185         int err, sent = 0;
1186
1187         if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count))
1188                 return -EFAULT;
1189
1190         sent += count;
1191         len  -= count;
1192
1193         /* Continuation fragments (no L2CAP header) */
1194         frag = &skb_shinfo(skb)->frag_list;
1195         while (len) {
1196                 count = min_t(unsigned int, conn->mtu, len);
1197
1198                 *frag = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err);
1199                 if (!*frag)
1200                         return err;
1201                 if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count))
1202                         return -EFAULT;
1203
1204                 sent += count;
1205                 len  -= count;
1206
1207                 frag = &(*frag)->next;
1208         }
1209
1210         return sent;
1211 }
1212
1213 struct sk_buff *l2cap_create_connless_pdu(struct sock *sk, struct msghdr *msg, size_t len)
1214 {
1215         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1216         struct sk_buff *skb;
1217         int err, count, hlen = L2CAP_HDR_SIZE + 2;
1218         struct l2cap_hdr *lh;
1219
1220         BT_DBG("sk %p len %d", sk, (int)len);
1221
1222         count = min_t(unsigned int, (conn->mtu - hlen), len);
1223         skb = bt_skb_send_alloc(sk, count + hlen,
1224                         msg->msg_flags & MSG_DONTWAIT, &err);
1225         if (!skb)
1226                 return ERR_PTR(err);
1227
1228         /* Create L2CAP header */
1229         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1230         lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
1231         lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1232         put_unaligned_le16(l2cap_pi(sk)->psm, skb_put(skb, 2));
1233
1234         err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1235         if (unlikely(err < 0)) {
1236                 kfree_skb(skb);
1237                 return ERR_PTR(err);
1238         }
1239         return skb;
1240 }
1241
1242 struct sk_buff *l2cap_create_basic_pdu(struct sock *sk, struct msghdr *msg, size_t len)
1243 {
1244         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1245         struct sk_buff *skb;
1246         int err, count, hlen = L2CAP_HDR_SIZE;
1247         struct l2cap_hdr *lh;
1248
1249         BT_DBG("sk %p len %d", sk, (int)len);
1250
1251         count = min_t(unsigned int, (conn->mtu - hlen), len);
1252         skb = bt_skb_send_alloc(sk, count + hlen,
1253                         msg->msg_flags & MSG_DONTWAIT, &err);
1254         if (!skb)
1255                 return ERR_PTR(err);
1256
1257         /* Create L2CAP header */
1258         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1259         lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
1260         lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1261
1262         err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1263         if (unlikely(err < 0)) {
1264                 kfree_skb(skb);
1265                 return ERR_PTR(err);
1266         }
1267         return skb;
1268 }
1269
1270 struct sk_buff *l2cap_create_iframe_pdu(struct sock *sk, struct msghdr *msg, size_t len, u16 control, u16 sdulen)
1271 {
1272         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1273         struct sk_buff *skb;
1274         int err, count, hlen = L2CAP_HDR_SIZE + 2;
1275         struct l2cap_hdr *lh;
1276
1277         BT_DBG("sk %p len %d", sk, (int)len);
1278
1279         if (!conn)
1280                 return ERR_PTR(-ENOTCONN);
1281
1282         if (sdulen)
1283                 hlen += 2;
1284
1285         if (l2cap_pi(sk)->fcs == L2CAP_FCS_CRC16)
1286                 hlen += 2;
1287
1288         count = min_t(unsigned int, (conn->mtu - hlen), len);
1289         skb = bt_skb_send_alloc(sk, count + hlen,
1290                         msg->msg_flags & MSG_DONTWAIT, &err);
1291         if (!skb)
1292                 return ERR_PTR(err);
1293
1294         /* Create L2CAP header */
1295         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1296         lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
1297         lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1298         put_unaligned_le16(control, skb_put(skb, 2));
1299         if (sdulen)
1300                 put_unaligned_le16(sdulen, skb_put(skb, 2));
1301
1302         err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1303         if (unlikely(err < 0)) {
1304                 kfree_skb(skb);
1305                 return ERR_PTR(err);
1306         }
1307
1308         if (l2cap_pi(sk)->fcs == L2CAP_FCS_CRC16)
1309                 put_unaligned_le16(0, skb_put(skb, 2));
1310
1311         bt_cb(skb)->retries = 0;
1312         return skb;
1313 }
1314
1315 int l2cap_sar_segment_sdu(struct sock *sk, struct msghdr *msg, size_t len)
1316 {
1317         struct l2cap_pinfo *pi = l2cap_pi(sk);
1318         struct sk_buff *skb;
1319         struct sk_buff_head sar_queue;
1320         u16 control;
1321         size_t size = 0;
1322
1323         skb_queue_head_init(&sar_queue);
1324         control = L2CAP_SDU_START;
1325         skb = l2cap_create_iframe_pdu(sk, msg, pi->remote_mps, control, len);
1326         if (IS_ERR(skb))
1327                 return PTR_ERR(skb);
1328
1329         __skb_queue_tail(&sar_queue, skb);
1330         len -= pi->remote_mps;
1331         size += pi->remote_mps;
1332
1333         while (len > 0) {
1334                 size_t buflen;
1335
1336                 if (len > pi->remote_mps) {
1337                         control = L2CAP_SDU_CONTINUE;
1338                         buflen = pi->remote_mps;
1339                 } else {
1340                         control = L2CAP_SDU_END;
1341                         buflen = len;
1342                 }
1343
1344                 skb = l2cap_create_iframe_pdu(sk, msg, buflen, control, 0);
1345                 if (IS_ERR(skb)) {
1346                         skb_queue_purge(&sar_queue);
1347                         return PTR_ERR(skb);
1348                 }
1349
1350                 __skb_queue_tail(&sar_queue, skb);
1351                 len -= buflen;
1352                 size += buflen;
1353         }
1354         skb_queue_splice_tail(&sar_queue, TX_QUEUE(sk));
1355         if (sk->sk_send_head == NULL)
1356                 sk->sk_send_head = sar_queue.next;
1357
1358         return size;
1359 }
1360
1361 static void l2cap_chan_ready(struct sock *sk)
1362 {
1363         struct sock *parent = bt_sk(sk)->parent;
1364
1365         BT_DBG("sk %p, parent %p", sk, parent);
1366
1367         l2cap_pi(sk)->conf_state = 0;
1368         l2cap_sock_clear_timer(sk);
1369
1370         if (!parent) {
1371                 /* Outgoing channel.
1372                  * Wake up socket sleeping on connect.
1373                  */
1374                 sk->sk_state = BT_CONNECTED;
1375                 sk->sk_state_change(sk);
1376         } else {
1377                 /* Incoming channel.
1378                  * Wake up socket sleeping on accept.
1379                  */
1380                 parent->sk_data_ready(parent, 0);
1381         }
1382 }
1383
1384 /* Copy frame to all raw sockets on that connection */
1385 static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
1386 {
1387         struct l2cap_chan_list *l = &conn->chan_list;
1388         struct sk_buff *nskb;
1389         struct sock *sk;
1390
1391         BT_DBG("conn %p", conn);
1392
1393         read_lock(&l->lock);
1394         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
1395                 if (sk->sk_type != SOCK_RAW)
1396                         continue;
1397
1398                 /* Don't send frame to the socket it came from */
1399                 if (skb->sk == sk)
1400                         continue;
1401                 nskb = skb_clone(skb, GFP_ATOMIC);
1402                 if (!nskb)
1403                         continue;
1404
1405                 if (sock_queue_rcv_skb(sk, nskb))
1406                         kfree_skb(nskb);
1407         }
1408         read_unlock(&l->lock);
1409 }
1410
1411 /* ---- L2CAP signalling commands ---- */
1412 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
1413                                 u8 code, u8 ident, u16 dlen, void *data)
1414 {
1415         struct sk_buff *skb, **frag;
1416         struct l2cap_cmd_hdr *cmd;
1417         struct l2cap_hdr *lh;
1418         int len, count;
1419
1420         BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %d",
1421                         conn, code, ident, dlen);
1422
1423         len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen;
1424         count = min_t(unsigned int, conn->mtu, len);
1425
1426         skb = bt_skb_alloc(count, GFP_ATOMIC);
1427         if (!skb)
1428                 return NULL;
1429
1430         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1431         lh->len = cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen);
1432
1433         if (conn->hcon->type == LE_LINK)
1434                 lh->cid = cpu_to_le16(L2CAP_CID_LE_SIGNALING);
1435         else
1436                 lh->cid = cpu_to_le16(L2CAP_CID_SIGNALING);
1437
1438         cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE);
1439         cmd->code  = code;
1440         cmd->ident = ident;
1441         cmd->len   = cpu_to_le16(dlen);
1442
1443         if (dlen) {
1444                 count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE;
1445                 memcpy(skb_put(skb, count), data, count);
1446                 data += count;
1447         }
1448
1449         len -= skb->len;
1450
1451         /* Continuation fragments (no L2CAP header) */
1452         frag = &skb_shinfo(skb)->frag_list;
1453         while (len) {
1454                 count = min_t(unsigned int, conn->mtu, len);
1455
1456                 *frag = bt_skb_alloc(count, GFP_ATOMIC);
1457                 if (!*frag)
1458                         goto fail;
1459
1460                 memcpy(skb_put(*frag, count), data, count);
1461
1462                 len  -= count;
1463                 data += count;
1464
1465                 frag = &(*frag)->next;
1466         }
1467
1468         return skb;
1469
1470 fail:
1471         kfree_skb(skb);
1472         return NULL;
1473 }
1474
1475 static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val)
1476 {
1477         struct l2cap_conf_opt *opt = *ptr;
1478         int len;
1479
1480         len = L2CAP_CONF_OPT_SIZE + opt->len;
1481         *ptr += len;
1482
1483         *type = opt->type;
1484         *olen = opt->len;
1485
1486         switch (opt->len) {
1487         case 1:
1488                 *val = *((u8 *) opt->val);
1489                 break;
1490
1491         case 2:
1492                 *val = get_unaligned_le16(opt->val);
1493                 break;
1494
1495         case 4:
1496                 *val = get_unaligned_le32(opt->val);
1497                 break;
1498
1499         default:
1500                 *val = (unsigned long) opt->val;
1501                 break;
1502         }
1503
1504         BT_DBG("type 0x%2.2x len %d val 0x%lx", *type, opt->len, *val);
1505         return len;
1506 }
1507
1508 static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val)
1509 {
1510         struct l2cap_conf_opt *opt = *ptr;
1511
1512         BT_DBG("type 0x%2.2x len %d val 0x%lx", type, len, val);
1513
1514         opt->type = type;
1515         opt->len  = len;
1516
1517         switch (len) {
1518         case 1:
1519                 *((u8 *) opt->val)  = val;
1520                 break;
1521
1522         case 2:
1523                 put_unaligned_le16(val, opt->val);
1524                 break;
1525
1526         case 4:
1527                 put_unaligned_le32(val, opt->val);
1528                 break;
1529
1530         default:
1531                 memcpy(opt->val, (void *) val, len);
1532                 break;
1533         }
1534
1535         *ptr += L2CAP_CONF_OPT_SIZE + len;
1536 }
1537
1538 static void l2cap_ack_timeout(unsigned long arg)
1539 {
1540         struct sock *sk = (void *) arg;
1541
1542         bh_lock_sock(sk);
1543         l2cap_send_ack(l2cap_pi(sk));
1544         bh_unlock_sock(sk);
1545 }
1546
1547 static inline void l2cap_ertm_init(struct sock *sk)
1548 {
1549         l2cap_pi(sk)->expected_ack_seq = 0;
1550         l2cap_pi(sk)->unacked_frames = 0;
1551         l2cap_pi(sk)->buffer_seq = 0;
1552         l2cap_pi(sk)->num_acked = 0;
1553         l2cap_pi(sk)->frames_sent = 0;
1554
1555         setup_timer(&l2cap_pi(sk)->retrans_timer,
1556                         l2cap_retrans_timeout, (unsigned long) sk);
1557         setup_timer(&l2cap_pi(sk)->monitor_timer,
1558                         l2cap_monitor_timeout, (unsigned long) sk);
1559         setup_timer(&l2cap_pi(sk)->ack_timer,
1560                         l2cap_ack_timeout, (unsigned long) sk);
1561
1562         __skb_queue_head_init(SREJ_QUEUE(sk));
1563         __skb_queue_head_init(BUSY_QUEUE(sk));
1564
1565         INIT_WORK(&l2cap_pi(sk)->busy_work, l2cap_busy_work);
1566
1567         sk->sk_backlog_rcv = l2cap_ertm_data_rcv;
1568 }
1569
1570 static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
1571 {
1572         switch (mode) {
1573         case L2CAP_MODE_STREAMING:
1574         case L2CAP_MODE_ERTM:
1575                 if (l2cap_mode_supported(mode, remote_feat_mask))
1576                         return mode;
1577                 /* fall through */
1578         default:
1579                 return L2CAP_MODE_BASIC;
1580         }
1581 }
1582
1583 int l2cap_build_conf_req(struct sock *sk, void *data)
1584 {
1585         struct l2cap_pinfo *pi = l2cap_pi(sk);
1586         struct l2cap_conf_req *req = data;
1587         struct l2cap_conf_rfc rfc = { .mode = pi->mode };
1588         void *ptr = req->data;
1589
1590         BT_DBG("sk %p", sk);
1591
1592         if (pi->num_conf_req || pi->num_conf_rsp)
1593                 goto done;
1594
1595         switch (pi->mode) {
1596         case L2CAP_MODE_STREAMING:
1597         case L2CAP_MODE_ERTM:
1598                 if (pi->conf_state & L2CAP_CONF_STATE2_DEVICE)
1599                         break;
1600
1601                 /* fall through */
1602         default:
1603                 pi->mode = l2cap_select_mode(rfc.mode, pi->conn->feat_mask);
1604                 break;
1605         }
1606
1607 done:
1608         if (pi->imtu != L2CAP_DEFAULT_MTU)
1609                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->imtu);
1610
1611         switch (pi->mode) {
1612         case L2CAP_MODE_BASIC:
1613                 if (!(pi->conn->feat_mask & L2CAP_FEAT_ERTM) &&
1614                                 !(pi->conn->feat_mask & L2CAP_FEAT_STREAMING))
1615                         break;
1616
1617                 rfc.mode            = L2CAP_MODE_BASIC;
1618                 rfc.txwin_size      = 0;
1619                 rfc.max_transmit    = 0;
1620                 rfc.retrans_timeout = 0;
1621                 rfc.monitor_timeout = 0;
1622                 rfc.max_pdu_size    = 0;
1623
1624                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
1625                                                         (unsigned long) &rfc);
1626                 break;
1627
1628         case L2CAP_MODE_ERTM:
1629                 rfc.mode            = L2CAP_MODE_ERTM;
1630                 rfc.txwin_size      = pi->tx_win;
1631                 rfc.max_transmit    = pi->max_tx;
1632                 rfc.retrans_timeout = 0;
1633                 rfc.monitor_timeout = 0;
1634                 rfc.max_pdu_size    = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE);
1635                 if (L2CAP_DEFAULT_MAX_PDU_SIZE > pi->conn->mtu - 10)
1636                         rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10);
1637
1638                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
1639                                                         (unsigned long) &rfc);
1640
1641                 if (!(pi->conn->feat_mask & L2CAP_FEAT_FCS))
1642                         break;
1643
1644                 if (pi->fcs == L2CAP_FCS_NONE ||
1645                                 pi->conf_state & L2CAP_CONF_NO_FCS_RECV) {
1646                         pi->fcs = L2CAP_FCS_NONE;
1647                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, pi->fcs);
1648                 }
1649                 break;
1650
1651         case L2CAP_MODE_STREAMING:
1652                 rfc.mode            = L2CAP_MODE_STREAMING;
1653                 rfc.txwin_size      = 0;
1654                 rfc.max_transmit    = 0;
1655                 rfc.retrans_timeout = 0;
1656                 rfc.monitor_timeout = 0;
1657                 rfc.max_pdu_size    = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE);
1658                 if (L2CAP_DEFAULT_MAX_PDU_SIZE > pi->conn->mtu - 10)
1659                         rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10);
1660
1661                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
1662                                                         (unsigned long) &rfc);
1663
1664                 if (!(pi->conn->feat_mask & L2CAP_FEAT_FCS))
1665                         break;
1666
1667                 if (pi->fcs == L2CAP_FCS_NONE ||
1668                                 pi->conf_state & L2CAP_CONF_NO_FCS_RECV) {
1669                         pi->fcs = L2CAP_FCS_NONE;
1670                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, pi->fcs);
1671                 }
1672                 break;
1673         }
1674
1675         req->dcid  = cpu_to_le16(pi->dcid);
1676         req->flags = cpu_to_le16(0);
1677
1678         return ptr - data;
1679 }
1680
1681 static int l2cap_parse_conf_req(struct sock *sk, void *data)
1682 {
1683         struct l2cap_pinfo *pi = l2cap_pi(sk);
1684         struct l2cap_conf_rsp *rsp = data;
1685         void *ptr = rsp->data;
1686         void *req = pi->conf_req;
1687         int len = pi->conf_len;
1688         int type, hint, olen;
1689         unsigned long val;
1690         struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
1691         u16 mtu = L2CAP_DEFAULT_MTU;
1692         u16 result = L2CAP_CONF_SUCCESS;
1693
1694         BT_DBG("sk %p", sk);
1695
1696         while (len >= L2CAP_CONF_OPT_SIZE) {
1697                 len -= l2cap_get_conf_opt(&req, &type, &olen, &val);
1698
1699                 hint  = type & L2CAP_CONF_HINT;
1700                 type &= L2CAP_CONF_MASK;
1701
1702                 switch (type) {
1703                 case L2CAP_CONF_MTU:
1704                         mtu = val;
1705                         break;
1706
1707                 case L2CAP_CONF_FLUSH_TO:
1708                         pi->flush_to = val;
1709                         break;
1710
1711                 case L2CAP_CONF_QOS:
1712                         break;
1713
1714                 case L2CAP_CONF_RFC:
1715                         if (olen == sizeof(rfc))
1716                                 memcpy(&rfc, (void *) val, olen);
1717                         break;
1718
1719                 case L2CAP_CONF_FCS:
1720                         if (val == L2CAP_FCS_NONE)
1721                                 pi->conf_state |= L2CAP_CONF_NO_FCS_RECV;
1722
1723                         break;
1724
1725                 default:
1726                         if (hint)
1727                                 break;
1728
1729                         result = L2CAP_CONF_UNKNOWN;
1730                         *((u8 *) ptr++) = type;
1731                         break;
1732                 }
1733         }
1734
1735         if (pi->num_conf_rsp || pi->num_conf_req > 1)
1736                 goto done;
1737
1738         switch (pi->mode) {
1739         case L2CAP_MODE_STREAMING:
1740         case L2CAP_MODE_ERTM:
1741                 if (!(pi->conf_state & L2CAP_CONF_STATE2_DEVICE)) {
1742                         pi->mode = l2cap_select_mode(rfc.mode,
1743                                         pi->conn->feat_mask);
1744                         break;
1745                 }
1746
1747                 if (pi->mode != rfc.mode)
1748                         return -ECONNREFUSED;
1749
1750                 break;
1751         }
1752
1753 done:
1754         if (pi->mode != rfc.mode) {
1755                 result = L2CAP_CONF_UNACCEPT;
1756                 rfc.mode = pi->mode;
1757
1758                 if (pi->num_conf_rsp == 1)
1759                         return -ECONNREFUSED;
1760
1761                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
1762                                         sizeof(rfc), (unsigned long) &rfc);
1763         }
1764
1765
1766         if (result == L2CAP_CONF_SUCCESS) {
1767                 /* Configure output options and let the other side know
1768                  * which ones we don't like. */
1769
1770                 if (mtu < L2CAP_DEFAULT_MIN_MTU)
1771                         result = L2CAP_CONF_UNACCEPT;
1772                 else {
1773                         pi->omtu = mtu;
1774                         pi->conf_state |= L2CAP_CONF_MTU_DONE;
1775                 }
1776                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->omtu);
1777
1778                 switch (rfc.mode) {
1779                 case L2CAP_MODE_BASIC:
1780                         pi->fcs = L2CAP_FCS_NONE;
1781                         pi->conf_state |= L2CAP_CONF_MODE_DONE;
1782                         break;
1783
1784                 case L2CAP_MODE_ERTM:
1785                         pi->remote_tx_win = rfc.txwin_size;
1786                         pi->remote_max_tx = rfc.max_transmit;
1787
1788                         if (le16_to_cpu(rfc.max_pdu_size) > pi->conn->mtu - 10)
1789                                 rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10);
1790
1791                         pi->remote_mps = le16_to_cpu(rfc.max_pdu_size);
1792
1793                         rfc.retrans_timeout =
1794                                 le16_to_cpu(L2CAP_DEFAULT_RETRANS_TO);
1795                         rfc.monitor_timeout =
1796                                 le16_to_cpu(L2CAP_DEFAULT_MONITOR_TO);
1797
1798                         pi->conf_state |= L2CAP_CONF_MODE_DONE;
1799
1800                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
1801                                         sizeof(rfc), (unsigned long) &rfc);
1802
1803                         break;
1804
1805                 case L2CAP_MODE_STREAMING:
1806                         if (le16_to_cpu(rfc.max_pdu_size) > pi->conn->mtu - 10)
1807                                 rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10);
1808
1809                         pi->remote_mps = le16_to_cpu(rfc.max_pdu_size);
1810
1811                         pi->conf_state |= L2CAP_CONF_MODE_DONE;
1812
1813                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
1814                                         sizeof(rfc), (unsigned long) &rfc);
1815
1816                         break;
1817
1818                 default:
1819                         result = L2CAP_CONF_UNACCEPT;
1820
1821                         memset(&rfc, 0, sizeof(rfc));
1822                         rfc.mode = pi->mode;
1823                 }
1824
1825                 if (result == L2CAP_CONF_SUCCESS)
1826                         pi->conf_state |= L2CAP_CONF_OUTPUT_DONE;
1827         }
1828         rsp->scid   = cpu_to_le16(pi->dcid);
1829         rsp->result = cpu_to_le16(result);
1830         rsp->flags  = cpu_to_le16(0x0000);
1831
1832         return ptr - data;
1833 }
1834
1835 static int l2cap_parse_conf_rsp(struct sock *sk, void *rsp, int len, void *data, u16 *result)
1836 {
1837         struct l2cap_pinfo *pi = l2cap_pi(sk);
1838         struct l2cap_conf_req *req = data;
1839         void *ptr = req->data;
1840         int type, olen;
1841         unsigned long val;
1842         struct l2cap_conf_rfc rfc;
1843
1844         BT_DBG("sk %p, rsp %p, len %d, req %p", sk, rsp, len, data);
1845
1846         while (len >= L2CAP_CONF_OPT_SIZE) {
1847                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
1848
1849                 switch (type) {
1850                 case L2CAP_CONF_MTU:
1851                         if (val < L2CAP_DEFAULT_MIN_MTU) {
1852                                 *result = L2CAP_CONF_UNACCEPT;
1853                                 pi->imtu = L2CAP_DEFAULT_MIN_MTU;
1854                         } else
1855                                 pi->imtu = val;
1856                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->imtu);
1857                         break;
1858
1859                 case L2CAP_CONF_FLUSH_TO:
1860                         pi->flush_to = val;
1861                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
1862                                                         2, pi->flush_to);
1863                         break;
1864
1865                 case L2CAP_CONF_RFC:
1866                         if (olen == sizeof(rfc))
1867                                 memcpy(&rfc, (void *)val, olen);
1868
1869                         if ((pi->conf_state & L2CAP_CONF_STATE2_DEVICE) &&
1870                                                         rfc.mode != pi->mode)
1871                                 return -ECONNREFUSED;
1872
1873                         pi->fcs = 0;
1874
1875                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
1876                                         sizeof(rfc), (unsigned long) &rfc);
1877                         break;
1878                 }
1879         }
1880
1881         if (pi->mode == L2CAP_MODE_BASIC && pi->mode != rfc.mode)
1882                 return -ECONNREFUSED;
1883
1884         pi->mode = rfc.mode;
1885
1886         if (*result == L2CAP_CONF_SUCCESS) {
1887                 switch (rfc.mode) {
1888                 case L2CAP_MODE_ERTM:
1889                         pi->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
1890                         pi->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
1891                         pi->mps    = le16_to_cpu(rfc.max_pdu_size);
1892                         break;
1893                 case L2CAP_MODE_STREAMING:
1894                         pi->mps    = le16_to_cpu(rfc.max_pdu_size);
1895                 }
1896         }
1897
1898         req->dcid   = cpu_to_le16(pi->dcid);
1899         req->flags  = cpu_to_le16(0x0000);
1900
1901         return ptr - data;
1902 }
1903
1904 static int l2cap_build_conf_rsp(struct sock *sk, void *data, u16 result, u16 flags)
1905 {
1906         struct l2cap_conf_rsp *rsp = data;
1907         void *ptr = rsp->data;
1908
1909         BT_DBG("sk %p", sk);
1910
1911         rsp->scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
1912         rsp->result = cpu_to_le16(result);
1913         rsp->flags  = cpu_to_le16(flags);
1914
1915         return ptr - data;
1916 }
1917
1918 static void l2cap_conf_rfc_get(struct sock *sk, void *rsp, int len)
1919 {
1920         struct l2cap_pinfo *pi = l2cap_pi(sk);
1921         int type, olen;
1922         unsigned long val;
1923         struct l2cap_conf_rfc rfc;
1924
1925         BT_DBG("sk %p, rsp %p, len %d", sk, rsp, len);
1926
1927         if ((pi->mode != L2CAP_MODE_ERTM) && (pi->mode != L2CAP_MODE_STREAMING))
1928                 return;
1929
1930         while (len >= L2CAP_CONF_OPT_SIZE) {
1931                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
1932
1933                 switch (type) {
1934                 case L2CAP_CONF_RFC:
1935                         if (olen == sizeof(rfc))
1936                                 memcpy(&rfc, (void *)val, olen);
1937                         goto done;
1938                 }
1939         }
1940
1941 done:
1942         switch (rfc.mode) {
1943         case L2CAP_MODE_ERTM:
1944                 pi->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
1945                 pi->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
1946                 pi->mps    = le16_to_cpu(rfc.max_pdu_size);
1947                 break;
1948         case L2CAP_MODE_STREAMING:
1949                 pi->mps    = le16_to_cpu(rfc.max_pdu_size);
1950         }
1951 }
1952
1953 static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1954 {
1955         struct l2cap_cmd_rej *rej = (struct l2cap_cmd_rej *) data;
1956
1957         if (rej->reason != 0x0000)
1958                 return 0;
1959
1960         if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
1961                                         cmd->ident == conn->info_ident) {
1962                 del_timer(&conn->info_timer);
1963
1964                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
1965                 conn->info_ident = 0;
1966
1967                 l2cap_conn_start(conn);
1968         }
1969
1970         return 0;
1971 }
1972
1973 static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1974 {
1975         struct l2cap_chan_list *list = &conn->chan_list;
1976         struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
1977         struct l2cap_conn_rsp rsp;
1978         struct sock *parent, *sk = NULL;
1979         int result, status = L2CAP_CS_NO_INFO;
1980
1981         u16 dcid = 0, scid = __le16_to_cpu(req->scid);
1982         __le16 psm = req->psm;
1983
1984         BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid);
1985
1986         /* Check if we have socket listening on psm */
1987         parent = l2cap_get_sock_by_psm(BT_LISTEN, psm, conn->src);
1988         if (!parent) {
1989                 result = L2CAP_CR_BAD_PSM;
1990                 goto sendresp;
1991         }
1992
1993         bh_lock_sock(parent);
1994
1995         /* Check if the ACL is secure enough (if not SDP) */
1996         if (psm != cpu_to_le16(0x0001) &&
1997                                 !hci_conn_check_link_mode(conn->hcon)) {
1998                 conn->disc_reason = 0x05;
1999                 result = L2CAP_CR_SEC_BLOCK;
2000                 goto response;
2001         }
2002
2003         result = L2CAP_CR_NO_MEM;
2004
2005         /* Check for backlog size */
2006         if (sk_acceptq_is_full(parent)) {
2007                 BT_DBG("backlog full %d", parent->sk_ack_backlog);
2008                 goto response;
2009         }
2010
2011         sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, GFP_ATOMIC);
2012         if (!sk)
2013                 goto response;
2014
2015         write_lock_bh(&list->lock);
2016
2017         /* Check if we already have channel with that dcid */
2018         if (__l2cap_get_chan_by_dcid(list, scid)) {
2019                 write_unlock_bh(&list->lock);
2020                 sock_set_flag(sk, SOCK_ZAPPED);
2021                 l2cap_sock_kill(sk);
2022                 goto response;
2023         }
2024
2025         hci_conn_hold(conn->hcon);
2026
2027         l2cap_sock_init(sk, parent);
2028         bacpy(&bt_sk(sk)->src, conn->src);
2029         bacpy(&bt_sk(sk)->dst, conn->dst);
2030         l2cap_pi(sk)->psm  = psm;
2031         l2cap_pi(sk)->dcid = scid;
2032
2033         __l2cap_chan_add(conn, sk, parent);
2034         dcid = l2cap_pi(sk)->scid;
2035
2036         l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
2037
2038         l2cap_pi(sk)->ident = cmd->ident;
2039
2040         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
2041                 if (l2cap_check_security(sk)) {
2042                         if (bt_sk(sk)->defer_setup) {
2043                                 sk->sk_state = BT_CONNECT2;
2044                                 result = L2CAP_CR_PEND;
2045                                 status = L2CAP_CS_AUTHOR_PEND;
2046                                 parent->sk_data_ready(parent, 0);
2047                         } else {
2048                                 sk->sk_state = BT_CONFIG;
2049                                 result = L2CAP_CR_SUCCESS;
2050                                 status = L2CAP_CS_NO_INFO;
2051                         }
2052                 } else {
2053                         sk->sk_state = BT_CONNECT2;
2054                         result = L2CAP_CR_PEND;
2055                         status = L2CAP_CS_AUTHEN_PEND;
2056                 }
2057         } else {
2058                 sk->sk_state = BT_CONNECT2;
2059                 result = L2CAP_CR_PEND;
2060                 status = L2CAP_CS_NO_INFO;
2061         }
2062
2063         write_unlock_bh(&list->lock);
2064
2065 response:
2066         bh_unlock_sock(parent);
2067
2068 sendresp:
2069         rsp.scid   = cpu_to_le16(scid);
2070         rsp.dcid   = cpu_to_le16(dcid);
2071         rsp.result = cpu_to_le16(result);
2072         rsp.status = cpu_to_le16(status);
2073         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
2074
2075         if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
2076                 struct l2cap_info_req info;
2077                 info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
2078
2079                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
2080                 conn->info_ident = l2cap_get_ident(conn);
2081
2082                 mod_timer(&conn->info_timer, jiffies +
2083                                         msecs_to_jiffies(L2CAP_INFO_TIMEOUT));
2084
2085                 l2cap_send_cmd(conn, conn->info_ident,
2086                                         L2CAP_INFO_REQ, sizeof(info), &info);
2087         }
2088
2089         if (sk && !(l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT) &&
2090                                 result == L2CAP_CR_SUCCESS) {
2091                 u8 buf[128];
2092                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
2093                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2094                                         l2cap_build_conf_req(sk, buf), buf);
2095                 l2cap_pi(sk)->num_conf_req++;
2096         }
2097
2098         return 0;
2099 }
2100
2101 static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2102 {
2103         struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
2104         u16 scid, dcid, result, status;
2105         struct sock *sk;
2106         u8 req[128];
2107
2108         scid   = __le16_to_cpu(rsp->scid);
2109         dcid   = __le16_to_cpu(rsp->dcid);
2110         result = __le16_to_cpu(rsp->result);
2111         status = __le16_to_cpu(rsp->status);
2112
2113         BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status);
2114
2115         if (scid) {
2116                 sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
2117                 if (!sk)
2118                         return -EFAULT;
2119         } else {
2120                 sk = l2cap_get_chan_by_ident(&conn->chan_list, cmd->ident);
2121                 if (!sk)
2122                         return -EFAULT;
2123         }
2124
2125         switch (result) {
2126         case L2CAP_CR_SUCCESS:
2127                 sk->sk_state = BT_CONFIG;
2128                 l2cap_pi(sk)->ident = 0;
2129                 l2cap_pi(sk)->dcid = dcid;
2130                 l2cap_pi(sk)->conf_state &= ~L2CAP_CONF_CONNECT_PEND;
2131
2132                 if (l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT)
2133                         break;
2134
2135                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
2136
2137                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2138                                         l2cap_build_conf_req(sk, req), req);
2139                 l2cap_pi(sk)->num_conf_req++;
2140                 break;
2141
2142         case L2CAP_CR_PEND:
2143                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND;
2144                 break;
2145
2146         default:
2147                 /* don't delete l2cap channel if sk is owned by user */
2148                 if (sock_owned_by_user(sk)) {
2149                         sk->sk_state = BT_DISCONN;
2150                         l2cap_sock_clear_timer(sk);
2151                         l2cap_sock_set_timer(sk, HZ / 5);
2152                         break;
2153                 }
2154
2155                 l2cap_chan_del(sk, ECONNREFUSED);
2156                 break;
2157         }
2158
2159         bh_unlock_sock(sk);
2160         return 0;
2161 }
2162
2163 static inline void set_default_fcs(struct l2cap_pinfo *pi)
2164 {
2165         /* FCS is enabled only in ERTM or streaming mode, if one or both
2166          * sides request it.
2167          */
2168         if (pi->mode != L2CAP_MODE_ERTM && pi->mode != L2CAP_MODE_STREAMING)
2169                 pi->fcs = L2CAP_FCS_NONE;
2170         else if (!(pi->conf_state & L2CAP_CONF_NO_FCS_RECV))
2171                 pi->fcs = L2CAP_FCS_CRC16;
2172 }
2173
2174 static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
2175 {
2176         struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
2177         u16 dcid, flags;
2178         u8 rsp[64];
2179         struct sock *sk;
2180         int len;
2181
2182         dcid  = __le16_to_cpu(req->dcid);
2183         flags = __le16_to_cpu(req->flags);
2184
2185         BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
2186
2187         sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid);
2188         if (!sk)
2189                 return -ENOENT;
2190
2191         if (sk->sk_state != BT_CONFIG) {
2192                 struct l2cap_cmd_rej rej;
2193
2194                 rej.reason = cpu_to_le16(0x0002);
2195                 l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
2196                                 sizeof(rej), &rej);
2197                 goto unlock;
2198         }
2199
2200         /* Reject if config buffer is too small. */
2201         len = cmd_len - sizeof(*req);
2202         if (l2cap_pi(sk)->conf_len + len > sizeof(l2cap_pi(sk)->conf_req)) {
2203                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2204                                 l2cap_build_conf_rsp(sk, rsp,
2205                                         L2CAP_CONF_REJECT, flags), rsp);
2206                 goto unlock;
2207         }
2208
2209         /* Store config. */
2210         memcpy(l2cap_pi(sk)->conf_req + l2cap_pi(sk)->conf_len, req->data, len);
2211         l2cap_pi(sk)->conf_len += len;
2212
2213         if (flags & 0x0001) {
2214                 /* Incomplete config. Send empty response. */
2215                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2216                                 l2cap_build_conf_rsp(sk, rsp,
2217                                         L2CAP_CONF_SUCCESS, 0x0001), rsp);
2218                 goto unlock;
2219         }
2220
2221         /* Complete config. */
2222         len = l2cap_parse_conf_req(sk, rsp);
2223         if (len < 0) {
2224                 l2cap_send_disconn_req(conn, sk, ECONNRESET);
2225                 goto unlock;
2226         }
2227
2228         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
2229         l2cap_pi(sk)->num_conf_rsp++;
2230
2231         /* Reset config buffer. */
2232         l2cap_pi(sk)->conf_len = 0;
2233
2234         if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE))
2235                 goto unlock;
2236
2237         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_INPUT_DONE) {
2238                 set_default_fcs(l2cap_pi(sk));
2239
2240                 sk->sk_state = BT_CONNECTED;
2241
2242                 l2cap_pi(sk)->next_tx_seq = 0;
2243                 l2cap_pi(sk)->expected_tx_seq = 0;
2244                 __skb_queue_head_init(TX_QUEUE(sk));
2245                 if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM)
2246                         l2cap_ertm_init(sk);
2247
2248                 l2cap_chan_ready(sk);
2249                 goto unlock;
2250         }
2251
2252         if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT)) {
2253                 u8 buf[64];
2254                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
2255                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2256                                         l2cap_build_conf_req(sk, buf), buf);
2257                 l2cap_pi(sk)->num_conf_req++;
2258         }
2259
2260 unlock:
2261         bh_unlock_sock(sk);
2262         return 0;
2263 }
2264
2265 static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2266 {
2267         struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
2268         u16 scid, flags, result;
2269         struct sock *sk;
2270         int len = cmd->len - sizeof(*rsp);
2271
2272         scid   = __le16_to_cpu(rsp->scid);
2273         flags  = __le16_to_cpu(rsp->flags);
2274         result = __le16_to_cpu(rsp->result);
2275
2276         BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x",
2277                         scid, flags, result);
2278
2279         sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
2280         if (!sk)
2281                 return 0;
2282
2283         switch (result) {
2284         case L2CAP_CONF_SUCCESS:
2285                 l2cap_conf_rfc_get(sk, rsp->data, len);
2286                 break;
2287
2288         case L2CAP_CONF_UNACCEPT:
2289                 if (l2cap_pi(sk)->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
2290                         char req[64];
2291
2292                         if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
2293                                 l2cap_send_disconn_req(conn, sk, ECONNRESET);
2294                                 goto done;
2295                         }
2296
2297                         /* throw out any old stored conf requests */
2298                         result = L2CAP_CONF_SUCCESS;
2299                         len = l2cap_parse_conf_rsp(sk, rsp->data,
2300                                                         len, req, &result);
2301                         if (len < 0) {
2302                                 l2cap_send_disconn_req(conn, sk, ECONNRESET);
2303                                 goto done;
2304                         }
2305
2306                         l2cap_send_cmd(conn, l2cap_get_ident(conn),
2307                                                 L2CAP_CONF_REQ, len, req);
2308                         l2cap_pi(sk)->num_conf_req++;
2309                         if (result != L2CAP_CONF_SUCCESS)
2310                                 goto done;
2311                         break;
2312                 }
2313
2314         default:
2315                 sk->sk_err = ECONNRESET;
2316                 l2cap_sock_set_timer(sk, HZ * 5);
2317                 l2cap_send_disconn_req(conn, sk, ECONNRESET);
2318                 goto done;
2319         }
2320
2321         if (flags & 0x01)
2322                 goto done;
2323
2324         l2cap_pi(sk)->conf_state |= L2CAP_CONF_INPUT_DONE;
2325
2326         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE) {
2327                 set_default_fcs(l2cap_pi(sk));
2328
2329                 sk->sk_state = BT_CONNECTED;
2330                 l2cap_pi(sk)->next_tx_seq = 0;
2331                 l2cap_pi(sk)->expected_tx_seq = 0;
2332                 __skb_queue_head_init(TX_QUEUE(sk));
2333                 if (l2cap_pi(sk)->mode ==  L2CAP_MODE_ERTM)
2334                         l2cap_ertm_init(sk);
2335
2336                 l2cap_chan_ready(sk);
2337         }
2338
2339 done:
2340         bh_unlock_sock(sk);
2341         return 0;
2342 }
2343
2344 static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2345 {
2346         struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
2347         struct l2cap_disconn_rsp rsp;
2348         u16 dcid, scid;
2349         struct sock *sk;
2350
2351         scid = __le16_to_cpu(req->scid);
2352         dcid = __le16_to_cpu(req->dcid);
2353
2354         BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
2355
2356         sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid);
2357         if (!sk)
2358                 return 0;
2359
2360         rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid);
2361         rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid);
2362         l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
2363
2364         sk->sk_shutdown = SHUTDOWN_MASK;
2365
2366         /* don't delete l2cap channel if sk is owned by user */
2367         if (sock_owned_by_user(sk)) {
2368                 sk->sk_state = BT_DISCONN;
2369                 l2cap_sock_clear_timer(sk);
2370                 l2cap_sock_set_timer(sk, HZ / 5);
2371                 bh_unlock_sock(sk);
2372                 return 0;
2373         }
2374
2375         l2cap_chan_del(sk, ECONNRESET);
2376         bh_unlock_sock(sk);
2377
2378         l2cap_sock_kill(sk);
2379         return 0;
2380 }
2381
2382 static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2383 {
2384         struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
2385         u16 dcid, scid;
2386         struct sock *sk;
2387
2388         scid = __le16_to_cpu(rsp->scid);
2389         dcid = __le16_to_cpu(rsp->dcid);
2390
2391         BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
2392
2393         sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
2394         if (!sk)
2395                 return 0;
2396
2397         /* don't delete l2cap channel if sk is owned by user */
2398         if (sock_owned_by_user(sk)) {
2399                 sk->sk_state = BT_DISCONN;
2400                 l2cap_sock_clear_timer(sk);
2401                 l2cap_sock_set_timer(sk, HZ / 5);
2402                 bh_unlock_sock(sk);
2403                 return 0;
2404         }
2405
2406         l2cap_chan_del(sk, 0);
2407         bh_unlock_sock(sk);
2408
2409         l2cap_sock_kill(sk);
2410         return 0;
2411 }
2412
2413 static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2414 {
2415         struct l2cap_info_req *req = (struct l2cap_info_req *) data;
2416         u16 type;
2417
2418         type = __le16_to_cpu(req->type);
2419
2420         BT_DBG("type 0x%4.4x", type);
2421
2422         if (type == L2CAP_IT_FEAT_MASK) {
2423                 u8 buf[8];
2424                 u32 feat_mask = l2cap_feat_mask;
2425                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
2426                 rsp->type   = cpu_to_le16(L2CAP_IT_FEAT_MASK);
2427                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
2428                 if (!disable_ertm)
2429                         feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
2430                                                          | L2CAP_FEAT_FCS;
2431                 put_unaligned_le32(feat_mask, rsp->data);
2432                 l2cap_send_cmd(conn, cmd->ident,
2433                                         L2CAP_INFO_RSP, sizeof(buf), buf);
2434         } else if (type == L2CAP_IT_FIXED_CHAN) {
2435                 u8 buf[12];
2436                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
2437                 rsp->type   = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
2438                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
2439                 memcpy(buf + 4, l2cap_fixed_chan, 8);
2440                 l2cap_send_cmd(conn, cmd->ident,
2441                                         L2CAP_INFO_RSP, sizeof(buf), buf);
2442         } else {
2443                 struct l2cap_info_rsp rsp;
2444                 rsp.type   = cpu_to_le16(type);
2445                 rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP);
2446                 l2cap_send_cmd(conn, cmd->ident,
2447                                         L2CAP_INFO_RSP, sizeof(rsp), &rsp);
2448         }
2449
2450         return 0;
2451 }
2452
2453 static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2454 {
2455         struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
2456         u16 type, result;
2457
2458         type   = __le16_to_cpu(rsp->type);
2459         result = __le16_to_cpu(rsp->result);
2460
2461         BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
2462
2463         del_timer(&conn->info_timer);
2464
2465         if (result != L2CAP_IR_SUCCESS) {
2466                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2467                 conn->info_ident = 0;
2468
2469                 l2cap_conn_start(conn);
2470
2471                 return 0;
2472         }
2473
2474         if (type == L2CAP_IT_FEAT_MASK) {
2475                 conn->feat_mask = get_unaligned_le32(rsp->data);
2476
2477                 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
2478                         struct l2cap_info_req req;
2479                         req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
2480
2481                         conn->info_ident = l2cap_get_ident(conn);
2482
2483                         l2cap_send_cmd(conn, conn->info_ident,
2484                                         L2CAP_INFO_REQ, sizeof(req), &req);
2485                 } else {
2486                         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2487                         conn->info_ident = 0;
2488
2489                         l2cap_conn_start(conn);
2490                 }
2491         } else if (type == L2CAP_IT_FIXED_CHAN) {
2492                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2493                 conn->info_ident = 0;
2494
2495                 l2cap_conn_start(conn);
2496         }
2497
2498         return 0;
2499 }
2500
2501 static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency,
2502                                                         u16 to_multiplier)
2503 {
2504         u16 max_latency;
2505
2506         if (min > max || min < 6 || max > 3200)
2507                 return -EINVAL;
2508
2509         if (to_multiplier < 10 || to_multiplier > 3200)
2510                 return -EINVAL;
2511
2512         if (max >= to_multiplier * 8)
2513                 return -EINVAL;
2514
2515         max_latency = (to_multiplier * 8 / max) - 1;
2516         if (latency > 499 || latency > max_latency)
2517                 return -EINVAL;
2518
2519         return 0;
2520 }
2521
2522 static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
2523                                         struct l2cap_cmd_hdr *cmd, u8 *data)
2524 {
2525         struct hci_conn *hcon = conn->hcon;
2526         struct l2cap_conn_param_update_req *req;
2527         struct l2cap_conn_param_update_rsp rsp;
2528         u16 min, max, latency, to_multiplier, cmd_len;
2529         int err;
2530
2531         if (!(hcon->link_mode & HCI_LM_MASTER))
2532                 return -EINVAL;
2533
2534         cmd_len = __le16_to_cpu(cmd->len);
2535         if (cmd_len != sizeof(struct l2cap_conn_param_update_req))
2536                 return -EPROTO;
2537
2538         req = (struct l2cap_conn_param_update_req *) data;
2539         min             = __le16_to_cpu(req->min);
2540         max             = __le16_to_cpu(req->max);
2541         latency         = __le16_to_cpu(req->latency);
2542         to_multiplier   = __le16_to_cpu(req->to_multiplier);
2543
2544         BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x",
2545                                                 min, max, latency, to_multiplier);
2546
2547         memset(&rsp, 0, sizeof(rsp));
2548
2549         err = l2cap_check_conn_param(min, max, latency, to_multiplier);
2550         if (err)
2551                 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_REJECTED);
2552         else
2553                 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED);
2554
2555         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP,
2556                                                         sizeof(rsp), &rsp);
2557
2558         if (!err)
2559                 hci_le_conn_update(hcon, min, max, latency, to_multiplier);
2560
2561         return 0;
2562 }
2563
2564 static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
2565                         struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
2566 {
2567         int err = 0;
2568
2569         switch (cmd->code) {
2570         case L2CAP_COMMAND_REJ:
2571                 l2cap_command_rej(conn, cmd, data);
2572                 break;
2573
2574         case L2CAP_CONN_REQ:
2575                 err = l2cap_connect_req(conn, cmd, data);
2576                 break;
2577
2578         case L2CAP_CONN_RSP:
2579                 err = l2cap_connect_rsp(conn, cmd, data);
2580                 break;
2581
2582         case L2CAP_CONF_REQ:
2583                 err = l2cap_config_req(conn, cmd, cmd_len, data);
2584                 break;
2585
2586         case L2CAP_CONF_RSP:
2587                 err = l2cap_config_rsp(conn, cmd, data);
2588                 break;
2589
2590         case L2CAP_DISCONN_REQ:
2591                 err = l2cap_disconnect_req(conn, cmd, data);
2592                 break;
2593
2594         case L2CAP_DISCONN_RSP:
2595                 err = l2cap_disconnect_rsp(conn, cmd, data);
2596                 break;
2597
2598         case L2CAP_ECHO_REQ:
2599                 l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data);
2600                 break;
2601
2602         case L2CAP_ECHO_RSP:
2603                 break;
2604
2605         case L2CAP_INFO_REQ:
2606                 err = l2cap_information_req(conn, cmd, data);
2607                 break;
2608
2609         case L2CAP_INFO_RSP:
2610                 err = l2cap_information_rsp(conn, cmd, data);
2611                 break;
2612
2613         default:
2614                 BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code);
2615                 err = -EINVAL;
2616                 break;
2617         }
2618
2619         return err;
2620 }
2621
2622 static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
2623                                         struct l2cap_cmd_hdr *cmd, u8 *data)
2624 {
2625         switch (cmd->code) {
2626         case L2CAP_COMMAND_REJ:
2627                 return 0;
2628
2629         case L2CAP_CONN_PARAM_UPDATE_REQ:
2630                 return l2cap_conn_param_update_req(conn, cmd, data);
2631
2632         case L2CAP_CONN_PARAM_UPDATE_RSP:
2633                 return 0;
2634
2635         default:
2636                 BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code);
2637                 return -EINVAL;
2638         }
2639 }
2640
2641 static inline void l2cap_sig_channel(struct l2cap_conn *conn,
2642                                                         struct sk_buff *skb)
2643 {
2644         u8 *data = skb->data;
2645         int len = skb->len;
2646         struct l2cap_cmd_hdr cmd;
2647         int err;
2648
2649         l2cap_raw_recv(conn, skb);
2650
2651         while (len >= L2CAP_CMD_HDR_SIZE) {
2652                 u16 cmd_len;
2653                 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
2654                 data += L2CAP_CMD_HDR_SIZE;
2655                 len  -= L2CAP_CMD_HDR_SIZE;
2656
2657                 cmd_len = le16_to_cpu(cmd.len);
2658
2659                 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, cmd.ident);
2660
2661                 if (cmd_len > len || !cmd.ident) {
2662                         BT_DBG("corrupted command");
2663                         break;
2664                 }
2665
2666                 if (conn->hcon->type == LE_LINK)
2667                         err = l2cap_le_sig_cmd(conn, &cmd, data);
2668                 else
2669                         err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data);
2670
2671                 if (err) {
2672                         struct l2cap_cmd_rej rej;
2673                         BT_DBG("error %d", err);
2674
2675                         /* FIXME: Map err to a valid reason */
2676                         rej.reason = cpu_to_le16(0);
2677                         l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
2678                 }
2679
2680                 data += cmd_len;
2681                 len  -= cmd_len;
2682         }
2683
2684         kfree_skb(skb);
2685 }
2686
2687 static int l2cap_check_fcs(struct l2cap_pinfo *pi,  struct sk_buff *skb)
2688 {
2689         u16 our_fcs, rcv_fcs;
2690         int hdr_size = L2CAP_HDR_SIZE + 2;
2691
2692         if (pi->fcs == L2CAP_FCS_CRC16) {
2693                 skb_trim(skb, skb->len - 2);
2694                 rcv_fcs = get_unaligned_le16(skb->data + skb->len);
2695                 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
2696
2697                 if (our_fcs != rcv_fcs)
2698                         return -EBADMSG;
2699         }
2700         return 0;
2701 }
2702
2703 static inline void l2cap_send_i_or_rr_or_rnr(struct sock *sk)
2704 {
2705         struct l2cap_pinfo *pi = l2cap_pi(sk);
2706         u16 control = 0;
2707
2708         pi->frames_sent = 0;
2709
2710         control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
2711
2712         if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
2713                 control |= L2CAP_SUPER_RCV_NOT_READY;
2714                 l2cap_send_sframe(pi, control);
2715                 pi->conn_state |= L2CAP_CONN_RNR_SENT;
2716         }
2717
2718         if (pi->conn_state & L2CAP_CONN_REMOTE_BUSY)
2719                 l2cap_retransmit_frames(sk);
2720
2721         l2cap_ertm_send(sk);
2722
2723         if (!(pi->conn_state & L2CAP_CONN_LOCAL_BUSY) &&
2724                         pi->frames_sent == 0) {
2725                 control |= L2CAP_SUPER_RCV_READY;
2726                 l2cap_send_sframe(pi, control);
2727         }
2728 }
2729
2730 static int l2cap_add_to_srej_queue(struct sock *sk, struct sk_buff *skb, u8 tx_seq, u8 sar)
2731 {
2732         struct sk_buff *next_skb;
2733         struct l2cap_pinfo *pi = l2cap_pi(sk);
2734         int tx_seq_offset, next_tx_seq_offset;
2735
2736         bt_cb(skb)->tx_seq = tx_seq;
2737         bt_cb(skb)->sar = sar;
2738
2739         next_skb = skb_peek(SREJ_QUEUE(sk));
2740         if (!next_skb) {
2741                 __skb_queue_tail(SREJ_QUEUE(sk), skb);
2742                 return 0;
2743         }
2744
2745         tx_seq_offset = (tx_seq - pi->buffer_seq) % 64;
2746         if (tx_seq_offset < 0)
2747                 tx_seq_offset += 64;
2748
2749         do {
2750                 if (bt_cb(next_skb)->tx_seq == tx_seq)
2751                         return -EINVAL;
2752
2753                 next_tx_seq_offset = (bt_cb(next_skb)->tx_seq -
2754                                                 pi->buffer_seq) % 64;
2755                 if (next_tx_seq_offset < 0)
2756                         next_tx_seq_offset += 64;
2757
2758                 if (next_tx_seq_offset > tx_seq_offset) {
2759                         __skb_queue_before(SREJ_QUEUE(sk), next_skb, skb);
2760                         return 0;
2761                 }
2762
2763                 if (skb_queue_is_last(SREJ_QUEUE(sk), next_skb))
2764                         break;
2765
2766         } while ((next_skb = skb_queue_next(SREJ_QUEUE(sk), next_skb)));
2767
2768         __skb_queue_tail(SREJ_QUEUE(sk), skb);
2769
2770         return 0;
2771 }
2772
2773 static int l2cap_ertm_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)
2774 {
2775         struct l2cap_pinfo *pi = l2cap_pi(sk);
2776         struct sk_buff *_skb;
2777         int err;
2778
2779         switch (control & L2CAP_CTRL_SAR) {
2780         case L2CAP_SDU_UNSEGMENTED:
2781                 if (pi->conn_state & L2CAP_CONN_SAR_SDU)
2782                         goto drop;
2783
2784                 err = sock_queue_rcv_skb(sk, skb);
2785                 if (!err)
2786                         return err;
2787
2788                 break;
2789
2790         case L2CAP_SDU_START:
2791                 if (pi->conn_state & L2CAP_CONN_SAR_SDU)
2792                         goto drop;
2793
2794                 pi->sdu_len = get_unaligned_le16(skb->data);
2795
2796                 if (pi->sdu_len > pi->imtu)
2797                         goto disconnect;
2798
2799                 pi->sdu = bt_skb_alloc(pi->sdu_len, GFP_ATOMIC);
2800                 if (!pi->sdu)
2801                         return -ENOMEM;
2802
2803                 /* pull sdu_len bytes only after alloc, because of Local Busy
2804                  * condition we have to be sure that this will be executed
2805                  * only once, i.e., when alloc does not fail */
2806                 skb_pull(skb, 2);
2807
2808                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
2809
2810                 pi->conn_state |= L2CAP_CONN_SAR_SDU;
2811                 pi->partial_sdu_len = skb->len;
2812                 break;
2813
2814         case L2CAP_SDU_CONTINUE:
2815                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
2816                         goto disconnect;
2817
2818                 if (!pi->sdu)
2819                         goto disconnect;
2820
2821                 pi->partial_sdu_len += skb->len;
2822                 if (pi->partial_sdu_len > pi->sdu_len)
2823                         goto drop;
2824
2825                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
2826
2827                 break;
2828
2829         case L2CAP_SDU_END:
2830                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
2831                         goto disconnect;
2832
2833                 if (!pi->sdu)
2834                         goto disconnect;
2835
2836                 if (!(pi->conn_state & L2CAP_CONN_SAR_RETRY)) {
2837                         pi->partial_sdu_len += skb->len;
2838
2839                         if (pi->partial_sdu_len > pi->imtu)
2840                                 goto drop;
2841
2842                         if (pi->partial_sdu_len != pi->sdu_len)
2843                                 goto drop;
2844
2845                         memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
2846                 }
2847
2848                 _skb = skb_clone(pi->sdu, GFP_ATOMIC);
2849                 if (!_skb) {
2850                         pi->conn_state |= L2CAP_CONN_SAR_RETRY;
2851                         return -ENOMEM;
2852                 }
2853
2854                 err = sock_queue_rcv_skb(sk, _skb);
2855                 if (err < 0) {
2856                         kfree_skb(_skb);
2857                         pi->conn_state |= L2CAP_CONN_SAR_RETRY;
2858                         return err;
2859                 }
2860
2861                 pi->conn_state &= ~L2CAP_CONN_SAR_RETRY;
2862                 pi->conn_state &= ~L2CAP_CONN_SAR_SDU;
2863
2864                 kfree_skb(pi->sdu);
2865                 break;
2866         }
2867
2868         kfree_skb(skb);
2869         return 0;
2870
2871 drop:
2872         kfree_skb(pi->sdu);
2873         pi->sdu = NULL;
2874
2875 disconnect:
2876         l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
2877         kfree_skb(skb);
2878         return 0;
2879 }
2880
2881 static int l2cap_try_push_rx_skb(struct sock *sk)
2882 {
2883         struct l2cap_pinfo *pi = l2cap_pi(sk);
2884         struct sk_buff *skb;
2885         u16 control;
2886         int err;
2887
2888         while ((skb = skb_dequeue(BUSY_QUEUE(sk)))) {
2889                 control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
2890                 err = l2cap_ertm_reassembly_sdu(sk, skb, control);
2891                 if (err < 0) {
2892                         skb_queue_head(BUSY_QUEUE(sk), skb);
2893                         return -EBUSY;
2894                 }
2895
2896                 pi->buffer_seq = (pi->buffer_seq + 1) % 64;
2897         }
2898
2899         if (!(pi->conn_state & L2CAP_CONN_RNR_SENT))
2900                 goto done;
2901
2902         control = pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
2903         control |= L2CAP_SUPER_RCV_READY | L2CAP_CTRL_POLL;
2904         l2cap_send_sframe(pi, control);
2905         l2cap_pi(sk)->retry_count = 1;
2906
2907         del_timer(&pi->retrans_timer);
2908         __mod_monitor_timer();
2909
2910         l2cap_pi(sk)->conn_state |= L2CAP_CONN_WAIT_F;
2911
2912 done:
2913         pi->conn_state &= ~L2CAP_CONN_LOCAL_BUSY;
2914         pi->conn_state &= ~L2CAP_CONN_RNR_SENT;
2915
2916         BT_DBG("sk %p, Exit local busy", sk);
2917
2918         return 0;
2919 }
2920
2921 static void l2cap_busy_work(struct work_struct *work)
2922 {
2923         DECLARE_WAITQUEUE(wait, current);
2924         struct l2cap_pinfo *pi =
2925                 container_of(work, struct l2cap_pinfo, busy_work);
2926         struct sock *sk = (struct sock *)pi;
2927         int n_tries = 0, timeo = HZ/5, err;
2928         struct sk_buff *skb;
2929
2930         lock_sock(sk);
2931
2932         add_wait_queue(sk_sleep(sk), &wait);
2933         while ((skb = skb_peek(BUSY_QUEUE(sk)))) {
2934                 set_current_state(TASK_INTERRUPTIBLE);
2935
2936                 if (n_tries++ > L2CAP_LOCAL_BUSY_TRIES) {
2937                         err = -EBUSY;
2938                         l2cap_send_disconn_req(pi->conn, sk, EBUSY);
2939                         break;
2940                 }
2941
2942                 if (!timeo)
2943                         timeo = HZ/5;
2944
2945                 if (signal_pending(current)) {
2946                         err = sock_intr_errno(timeo);
2947                         break;
2948                 }
2949
2950                 release_sock(sk);
2951                 timeo = schedule_timeout(timeo);
2952                 lock_sock(sk);
2953
2954                 err = sock_error(sk);
2955                 if (err)
2956                         break;
2957
2958                 if (l2cap_try_push_rx_skb(sk) == 0)
2959                         break;
2960         }
2961
2962         set_current_state(TASK_RUNNING);
2963         remove_wait_queue(sk_sleep(sk), &wait);
2964
2965         release_sock(sk);
2966 }
2967
2968 static int l2cap_push_rx_skb(struct sock *sk, struct sk_buff *skb, u16 control)
2969 {
2970         struct l2cap_pinfo *pi = l2cap_pi(sk);
2971         int sctrl, err;
2972
2973         if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
2974                 bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT;
2975                 __skb_queue_tail(BUSY_QUEUE(sk), skb);
2976                 return l2cap_try_push_rx_skb(sk);
2977
2978
2979         }
2980
2981         err = l2cap_ertm_reassembly_sdu(sk, skb, control);
2982         if (err >= 0) {
2983                 pi->buffer_seq = (pi->buffer_seq + 1) % 64;
2984                 return err;
2985         }
2986
2987         /* Busy Condition */
2988         BT_DBG("sk %p, Enter local busy", sk);
2989
2990         pi->conn_state |= L2CAP_CONN_LOCAL_BUSY;
2991         bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT;
2992         __skb_queue_tail(BUSY_QUEUE(sk), skb);
2993
2994         sctrl = pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
2995         sctrl |= L2CAP_SUPER_RCV_NOT_READY;
2996         l2cap_send_sframe(pi, sctrl);
2997
2998         pi->conn_state |= L2CAP_CONN_RNR_SENT;
2999
3000         del_timer(&pi->ack_timer);
3001
3002         queue_work(_busy_wq, &pi->busy_work);
3003
3004         return err;
3005 }
3006
3007 static int l2cap_streaming_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)
3008 {
3009         struct l2cap_pinfo *pi = l2cap_pi(sk);
3010         struct sk_buff *_skb;
3011         int err = -EINVAL;
3012
3013         /*
3014          * TODO: We have to notify the userland if some data is lost with the
3015          * Streaming Mode.
3016          */
3017
3018         switch (control & L2CAP_CTRL_SAR) {
3019         case L2CAP_SDU_UNSEGMENTED:
3020                 if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
3021                         kfree_skb(pi->sdu);
3022                         break;
3023                 }
3024
3025                 err = sock_queue_rcv_skb(sk, skb);
3026                 if (!err)
3027                         return 0;
3028
3029                 break;
3030
3031         case L2CAP_SDU_START:
3032                 if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
3033                         kfree_skb(pi->sdu);
3034                         break;
3035                 }
3036
3037                 pi->sdu_len = get_unaligned_le16(skb->data);
3038                 skb_pull(skb, 2);
3039
3040                 if (pi->sdu_len > pi->imtu) {
3041                         err = -EMSGSIZE;
3042                         break;
3043                 }
3044
3045                 pi->sdu = bt_skb_alloc(pi->sdu_len, GFP_ATOMIC);
3046                 if (!pi->sdu) {
3047                         err = -ENOMEM;
3048                         break;
3049                 }
3050
3051                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3052
3053                 pi->conn_state |= L2CAP_CONN_SAR_SDU;
3054                 pi->partial_sdu_len = skb->len;
3055                 err = 0;
3056                 break;
3057
3058         case L2CAP_SDU_CONTINUE:
3059                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
3060                         break;
3061
3062                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3063
3064                 pi->partial_sdu_len += skb->len;
3065                 if (pi->partial_sdu_len > pi->sdu_len)
3066                         kfree_skb(pi->sdu);
3067                 else
3068                         err = 0;
3069
3070                 break;
3071
3072         case L2CAP_SDU_END:
3073                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
3074                         break;
3075
3076                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3077
3078                 pi->conn_state &= ~L2CAP_CONN_SAR_SDU;
3079                 pi->partial_sdu_len += skb->len;
3080
3081                 if (pi->partial_sdu_len > pi->imtu)
3082                         goto drop;
3083
3084                 if (pi->partial_sdu_len == pi->sdu_len) {
3085                         _skb = skb_clone(pi->sdu, GFP_ATOMIC);
3086                         err = sock_queue_rcv_skb(sk, _skb);
3087                         if (err < 0)
3088                                 kfree_skb(_skb);
3089                 }
3090                 err = 0;
3091
3092 drop:
3093                 kfree_skb(pi->sdu);
3094                 break;
3095         }
3096
3097         kfree_skb(skb);
3098         return err;
3099 }
3100
3101 static void l2cap_check_srej_gap(struct sock *sk, u8 tx_seq)
3102 {
3103         struct sk_buff *skb;
3104         u16 control;
3105
3106         while ((skb = skb_peek(SREJ_QUEUE(sk)))) {
3107                 if (bt_cb(skb)->tx_seq != tx_seq)
3108                         break;
3109
3110                 skb = skb_dequeue(SREJ_QUEUE(sk));
3111                 control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
3112                 l2cap_ertm_reassembly_sdu(sk, skb, control);
3113                 l2cap_pi(sk)->buffer_seq_srej =
3114                         (l2cap_pi(sk)->buffer_seq_srej + 1) % 64;
3115                 tx_seq = (tx_seq + 1) % 64;
3116         }
3117 }
3118
3119 static void l2cap_resend_srejframe(struct sock *sk, u8 tx_seq)
3120 {
3121         struct l2cap_pinfo *pi = l2cap_pi(sk);
3122         struct srej_list *l, *tmp;
3123         u16 control;
3124
3125         list_for_each_entry_safe(l, tmp, SREJ_LIST(sk), list) {
3126                 if (l->tx_seq == tx_seq) {
3127                         list_del(&l->list);
3128                         kfree(l);
3129                         return;
3130                 }
3131                 control = L2CAP_SUPER_SELECT_REJECT;
3132                 control |= l->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3133                 l2cap_send_sframe(pi, control);
3134                 list_del(&l->list);
3135                 list_add_tail(&l->list, SREJ_LIST(sk));
3136         }
3137 }
3138
3139 static void l2cap_send_srejframe(struct sock *sk, u8 tx_seq)
3140 {
3141         struct l2cap_pinfo *pi = l2cap_pi(sk);
3142         struct srej_list *new;
3143         u16 control;
3144
3145         while (tx_seq != pi->expected_tx_seq) {
3146                 control = L2CAP_SUPER_SELECT_REJECT;
3147                 control |= pi->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3148                 l2cap_send_sframe(pi, control);
3149
3150                 new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC);
3151                 new->tx_seq = pi->expected_tx_seq;
3152                 pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3153                 list_add_tail(&new->list, SREJ_LIST(sk));
3154         }
3155         pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3156 }
3157
3158 static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
3159 {
3160         struct l2cap_pinfo *pi = l2cap_pi(sk);
3161         u8 tx_seq = __get_txseq(rx_control);
3162         u8 req_seq = __get_reqseq(rx_control);
3163         u8 sar = rx_control >> L2CAP_CTRL_SAR_SHIFT;
3164         int tx_seq_offset, expected_tx_seq_offset;
3165         int num_to_ack = (pi->tx_win/6) + 1;
3166         int err = 0;
3167
3168         BT_DBG("sk %p len %d tx_seq %d rx_control 0x%4.4x", sk, skb->len, tx_seq,
3169                                                                 rx_control);
3170
3171         if (L2CAP_CTRL_FINAL & rx_control &&
3172                         l2cap_pi(sk)->conn_state & L2CAP_CONN_WAIT_F) {
3173                 del_timer(&pi->monitor_timer);
3174                 if (pi->unacked_frames > 0)
3175                         __mod_retrans_timer();
3176                 pi->conn_state &= ~L2CAP_CONN_WAIT_F;
3177         }
3178
3179         pi->expected_ack_seq = req_seq;
3180         l2cap_drop_acked_frames(sk);
3181
3182         if (tx_seq == pi->expected_tx_seq)
3183                 goto expected;
3184
3185         tx_seq_offset = (tx_seq - pi->buffer_seq) % 64;
3186         if (tx_seq_offset < 0)
3187                 tx_seq_offset += 64;
3188
3189         /* invalid tx_seq */
3190         if (tx_seq_offset >= pi->tx_win) {
3191                 l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
3192                 goto drop;
3193         }
3194
3195         if (pi->conn_state == L2CAP_CONN_LOCAL_BUSY)
3196                 goto drop;
3197
3198         if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
3199                 struct srej_list *first;
3200
3201                 first = list_first_entry(SREJ_LIST(sk),
3202                                 struct srej_list, list);
3203                 if (tx_seq == first->tx_seq) {
3204                         l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
3205                         l2cap_check_srej_gap(sk, tx_seq);
3206
3207                         list_del(&first->list);
3208                         kfree(first);
3209
3210                         if (list_empty(SREJ_LIST(sk))) {
3211                                 pi->buffer_seq = pi->buffer_seq_srej;
3212                                 pi->conn_state &= ~L2CAP_CONN_SREJ_SENT;
3213                                 l2cap_send_ack(pi);
3214                                 BT_DBG("sk %p, Exit SREJ_SENT", sk);
3215                         }
3216                 } else {
3217                         struct srej_list *l;
3218
3219                         /* duplicated tx_seq */
3220                         if (l2cap_add_to_srej_queue(sk, skb, tx_seq, sar) < 0)
3221                                 goto drop;
3222
3223                         list_for_each_entry(l, SREJ_LIST(sk), list) {
3224                                 if (l->tx_seq == tx_seq) {
3225                                         l2cap_resend_srejframe(sk, tx_seq);
3226                                         return 0;
3227                                 }
3228                         }
3229                         l2cap_send_srejframe(sk, tx_seq);
3230                 }
3231         } else {
3232                 expected_tx_seq_offset =
3233                         (pi->expected_tx_seq - pi->buffer_seq) % 64;
3234                 if (expected_tx_seq_offset < 0)
3235                         expected_tx_seq_offset += 64;
3236
3237                 /* duplicated tx_seq */
3238                 if (tx_seq_offset < expected_tx_seq_offset)
3239                         goto drop;
3240
3241                 pi->conn_state |= L2CAP_CONN_SREJ_SENT;
3242
3243                 BT_DBG("sk %p, Enter SREJ", sk);
3244
3245                 INIT_LIST_HEAD(SREJ_LIST(sk));
3246                 pi->buffer_seq_srej = pi->buffer_seq;
3247
3248                 __skb_queue_head_init(SREJ_QUEUE(sk));
3249                 __skb_queue_head_init(BUSY_QUEUE(sk));
3250                 l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
3251
3252                 pi->conn_state |= L2CAP_CONN_SEND_PBIT;
3253
3254                 l2cap_send_srejframe(sk, tx_seq);
3255
3256                 del_timer(&pi->ack_timer);
3257         }
3258         return 0;
3259
3260 expected:
3261         pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3262
3263         if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
3264                 bt_cb(skb)->tx_seq = tx_seq;
3265                 bt_cb(skb)->sar = sar;
3266                 __skb_queue_tail(SREJ_QUEUE(sk), skb);
3267                 return 0;
3268         }
3269
3270         err = l2cap_push_rx_skb(sk, skb, rx_control);
3271         if (err < 0)
3272                 return 0;
3273
3274         if (rx_control & L2CAP_CTRL_FINAL) {
3275                 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
3276                         pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
3277                 else
3278                         l2cap_retransmit_frames(sk);
3279         }
3280
3281         __mod_ack_timer();
3282
3283         pi->num_acked = (pi->num_acked + 1) % num_to_ack;
3284         if (pi->num_acked == num_to_ack - 1)
3285                 l2cap_send_ack(pi);
3286
3287         return 0;
3288
3289 drop:
3290         kfree_skb(skb);
3291         return 0;
3292 }
3293
3294 static inline void l2cap_data_channel_rrframe(struct sock *sk, u16 rx_control)
3295 {
3296         struct l2cap_pinfo *pi = l2cap_pi(sk);
3297
3298         BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, __get_reqseq(rx_control),
3299                                                 rx_control);
3300
3301         pi->expected_ack_seq = __get_reqseq(rx_control);
3302         l2cap_drop_acked_frames(sk);
3303
3304         if (rx_control & L2CAP_CTRL_POLL) {
3305                 pi->conn_state |= L2CAP_CONN_SEND_FBIT;
3306                 if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
3307                         if ((pi->conn_state & L2CAP_CONN_REMOTE_BUSY) &&
3308                                         (pi->unacked_frames > 0))
3309                                 __mod_retrans_timer();
3310
3311                         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3312                         l2cap_send_srejtail(sk);
3313                 } else {
3314                         l2cap_send_i_or_rr_or_rnr(sk);
3315                 }
3316
3317         } else if (rx_control & L2CAP_CTRL_FINAL) {
3318                 pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3319
3320                 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
3321                         pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
3322                 else
3323                         l2cap_retransmit_frames(sk);
3324
3325         } else {
3326                 if ((pi->conn_state & L2CAP_CONN_REMOTE_BUSY) &&
3327                                 (pi->unacked_frames > 0))
3328                         __mod_retrans_timer();
3329
3330                 pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3331                 if (pi->conn_state & L2CAP_CONN_SREJ_SENT)
3332                         l2cap_send_ack(pi);
3333                 else
3334                         l2cap_ertm_send(sk);
3335         }
3336 }
3337
3338 static inline void l2cap_data_channel_rejframe(struct sock *sk, u16 rx_control)
3339 {
3340         struct l2cap_pinfo *pi = l2cap_pi(sk);
3341         u8 tx_seq = __get_reqseq(rx_control);
3342
3343         BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, tx_seq, rx_control);
3344
3345         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3346
3347         pi->expected_ack_seq = tx_seq;
3348         l2cap_drop_acked_frames(sk);
3349
3350         if (rx_control & L2CAP_CTRL_FINAL) {
3351                 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
3352                         pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
3353                 else
3354                         l2cap_retransmit_frames(sk);
3355         } else {
3356                 l2cap_retransmit_frames(sk);
3357
3358                 if (pi->conn_state & L2CAP_CONN_WAIT_F)
3359                         pi->conn_state |= L2CAP_CONN_REJ_ACT;
3360         }
3361 }
3362 static inline void l2cap_data_channel_srejframe(struct sock *sk, u16 rx_control)
3363 {
3364         struct l2cap_pinfo *pi = l2cap_pi(sk);
3365         u8 tx_seq = __get_reqseq(rx_control);
3366
3367         BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, tx_seq, rx_control);
3368
3369         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3370
3371         if (rx_control & L2CAP_CTRL_POLL) {
3372                 pi->expected_ack_seq = tx_seq;
3373                 l2cap_drop_acked_frames(sk);
3374
3375                 pi->conn_state |= L2CAP_CONN_SEND_FBIT;
3376                 l2cap_retransmit_one_frame(sk, tx_seq);
3377
3378                 l2cap_ertm_send(sk);
3379
3380                 if (pi->conn_state & L2CAP_CONN_WAIT_F) {
3381                         pi->srej_save_reqseq = tx_seq;
3382                         pi->conn_state |= L2CAP_CONN_SREJ_ACT;
3383                 }
3384         } else if (rx_control & L2CAP_CTRL_FINAL) {
3385                 if ((pi->conn_state & L2CAP_CONN_SREJ_ACT) &&
3386                                 pi->srej_save_reqseq == tx_seq)
3387                         pi->conn_state &= ~L2CAP_CONN_SREJ_ACT;
3388                 else
3389                         l2cap_retransmit_one_frame(sk, tx_seq);
3390         } else {
3391                 l2cap_retransmit_one_frame(sk, tx_seq);
3392                 if (pi->conn_state & L2CAP_CONN_WAIT_F) {
3393                         pi->srej_save_reqseq = tx_seq;
3394                         pi->conn_state |= L2CAP_CONN_SREJ_ACT;
3395                 }
3396         }
3397 }
3398
3399 static inline void l2cap_data_channel_rnrframe(struct sock *sk, u16 rx_control)
3400 {
3401         struct l2cap_pinfo *pi = l2cap_pi(sk);
3402         u8 tx_seq = __get_reqseq(rx_control);
3403
3404         BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, tx_seq, rx_control);
3405
3406         pi->conn_state |= L2CAP_CONN_REMOTE_BUSY;
3407         pi->expected_ack_seq = tx_seq;
3408         l2cap_drop_acked_frames(sk);
3409
3410         if (rx_control & L2CAP_CTRL_POLL)
3411                 pi->conn_state |= L2CAP_CONN_SEND_FBIT;
3412
3413         if (!(pi->conn_state & L2CAP_CONN_SREJ_SENT)) {
3414                 del_timer(&pi->retrans_timer);
3415                 if (rx_control & L2CAP_CTRL_POLL)
3416                         l2cap_send_rr_or_rnr(pi, L2CAP_CTRL_FINAL);
3417                 return;
3418         }
3419
3420         if (rx_control & L2CAP_CTRL_POLL)
3421                 l2cap_send_srejtail(sk);
3422         else
3423                 l2cap_send_sframe(pi, L2CAP_SUPER_RCV_READY);
3424 }
3425
3426 static inline int l2cap_data_channel_sframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
3427 {
3428         BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
3429
3430         if (L2CAP_CTRL_FINAL & rx_control &&
3431                         l2cap_pi(sk)->conn_state & L2CAP_CONN_WAIT_F) {
3432                 del_timer(&l2cap_pi(sk)->monitor_timer);
3433                 if (l2cap_pi(sk)->unacked_frames > 0)
3434                         __mod_retrans_timer();
3435                 l2cap_pi(sk)->conn_state &= ~L2CAP_CONN_WAIT_F;
3436         }
3437
3438         switch (rx_control & L2CAP_CTRL_SUPERVISE) {
3439         case L2CAP_SUPER_RCV_READY:
3440                 l2cap_data_channel_rrframe(sk, rx_control);
3441                 break;
3442
3443         case L2CAP_SUPER_REJECT:
3444                 l2cap_data_channel_rejframe(sk, rx_control);
3445                 break;
3446
3447         case L2CAP_SUPER_SELECT_REJECT:
3448                 l2cap_data_channel_srejframe(sk, rx_control);
3449                 break;
3450
3451         case L2CAP_SUPER_RCV_NOT_READY:
3452                 l2cap_data_channel_rnrframe(sk, rx_control);
3453                 break;
3454         }
3455
3456         kfree_skb(skb);
3457         return 0;
3458 }
3459
3460 static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb)
3461 {
3462         struct l2cap_pinfo *pi = l2cap_pi(sk);
3463         u16 control;
3464         u8 req_seq;
3465         int len, next_tx_seq_offset, req_seq_offset;
3466
3467         control = get_unaligned_le16(skb->data);
3468         skb_pull(skb, 2);
3469         len = skb->len;
3470
3471         /*
3472          * We can just drop the corrupted I-frame here.
3473          * Receiver will miss it and start proper recovery
3474          * procedures and ask retransmission.
3475          */
3476         if (l2cap_check_fcs(pi, skb))
3477                 goto drop;
3478
3479         if (__is_sar_start(control) && __is_iframe(control))
3480                 len -= 2;
3481
3482         if (pi->fcs == L2CAP_FCS_CRC16)
3483                 len -= 2;
3484
3485         if (len > pi->mps) {
3486                 l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
3487                 goto drop;
3488         }
3489
3490         req_seq = __get_reqseq(control);
3491         req_seq_offset = (req_seq - pi->expected_ack_seq) % 64;
3492         if (req_seq_offset < 0)
3493                 req_seq_offset += 64;
3494
3495         next_tx_seq_offset =
3496                 (pi->next_tx_seq - pi->expected_ack_seq) % 64;
3497         if (next_tx_seq_offset < 0)
3498                 next_tx_seq_offset += 64;
3499
3500         /* check for invalid req-seq */
3501         if (req_seq_offset > next_tx_seq_offset) {
3502                 l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
3503                 goto drop;
3504         }
3505
3506         if (__is_iframe(control)) {
3507                 if (len < 0) {
3508                         l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
3509                         goto drop;
3510                 }
3511
3512                 l2cap_data_channel_iframe(sk, control, skb);
3513         } else {
3514                 if (len != 0) {
3515                         BT_ERR("%d", len);
3516                         l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
3517                         goto drop;
3518                 }
3519
3520                 l2cap_data_channel_sframe(sk, control, skb);
3521         }
3522
3523         return 0;
3524
3525 drop:
3526         kfree_skb(skb);
3527         return 0;
3528 }
3529
3530 static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
3531 {
3532         struct sock *sk;
3533         struct l2cap_pinfo *pi;
3534         u16 control;
3535         u8 tx_seq;
3536         int len;
3537
3538         sk = l2cap_get_chan_by_scid(&conn->chan_list, cid);
3539         if (!sk) {
3540                 BT_DBG("unknown cid 0x%4.4x", cid);
3541                 goto drop;
3542         }
3543
3544         pi = l2cap_pi(sk);
3545
3546         BT_DBG("sk %p, len %d", sk, skb->len);
3547
3548         if (sk->sk_state != BT_CONNECTED)
3549                 goto drop;
3550
3551         switch (pi->mode) {
3552         case L2CAP_MODE_BASIC:
3553                 /* If socket recv buffers overflows we drop data here
3554                  * which is *bad* because L2CAP has to be reliable.
3555                  * But we don't have any other choice. L2CAP doesn't
3556                  * provide flow control mechanism. */
3557
3558                 if (pi->imtu < skb->len)
3559                         goto drop;
3560
3561                 if (!sock_queue_rcv_skb(sk, skb))
3562                         goto done;
3563                 break;
3564
3565         case L2CAP_MODE_ERTM:
3566                 if (!sock_owned_by_user(sk)) {
3567                         l2cap_ertm_data_rcv(sk, skb);
3568                 } else {
3569                         if (sk_add_backlog(sk, skb))
3570                                 goto drop;
3571                 }
3572
3573                 goto done;
3574
3575         case L2CAP_MODE_STREAMING:
3576                 control = get_unaligned_le16(skb->data);
3577                 skb_pull(skb, 2);
3578                 len = skb->len;
3579
3580                 if (l2cap_check_fcs(pi, skb))
3581                         goto drop;
3582
3583                 if (__is_sar_start(control))
3584                         len -= 2;
3585
3586                 if (pi->fcs == L2CAP_FCS_CRC16)
3587                         len -= 2;
3588
3589                 if (len > pi->mps || len < 0 || __is_sframe(control))
3590                         goto drop;
3591
3592                 tx_seq = __get_txseq(control);
3593
3594                 if (pi->expected_tx_seq == tx_seq)
3595                         pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3596                 else
3597                         pi->expected_tx_seq = (tx_seq + 1) % 64;
3598
3599                 l2cap_streaming_reassembly_sdu(sk, skb, control);
3600
3601                 goto done;
3602
3603         default:
3604                 BT_DBG("sk %p: bad mode 0x%2.2x", sk, pi->mode);
3605                 break;
3606         }
3607
3608 drop:
3609         kfree_skb(skb);
3610
3611 done:
3612         if (sk)
3613                 bh_unlock_sock(sk);
3614
3615         return 0;
3616 }
3617
3618 static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb)
3619 {
3620         struct sock *sk;
3621
3622         sk = l2cap_get_sock_by_psm(0, psm, conn->src);
3623         if (!sk)
3624                 goto drop;
3625
3626         bh_lock_sock(sk);
3627
3628         BT_DBG("sk %p, len %d", sk, skb->len);
3629
3630         if (sk->sk_state != BT_BOUND && sk->sk_state != BT_CONNECTED)
3631                 goto drop;
3632
3633         if (l2cap_pi(sk)->imtu < skb->len)
3634                 goto drop;
3635
3636         if (!sock_queue_rcv_skb(sk, skb))
3637                 goto done;
3638
3639 drop:
3640         kfree_skb(skb);
3641
3642 done:
3643         if (sk)
3644                 bh_unlock_sock(sk);
3645         return 0;
3646 }
3647
3648 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
3649 {
3650         struct l2cap_hdr *lh = (void *) skb->data;
3651         u16 cid, len;
3652         __le16 psm;
3653
3654         skb_pull(skb, L2CAP_HDR_SIZE);
3655         cid = __le16_to_cpu(lh->cid);
3656         len = __le16_to_cpu(lh->len);
3657
3658         if (len != skb->len) {
3659                 kfree_skb(skb);
3660                 return;
3661         }
3662
3663         BT_DBG("len %d, cid 0x%4.4x", len, cid);
3664
3665         switch (cid) {
3666         case L2CAP_CID_LE_SIGNALING:
3667         case L2CAP_CID_SIGNALING:
3668                 l2cap_sig_channel(conn, skb);
3669                 break;
3670
3671         case L2CAP_CID_CONN_LESS:
3672                 psm = get_unaligned_le16(skb->data);
3673                 skb_pull(skb, 2);
3674                 l2cap_conless_channel(conn, psm, skb);
3675                 break;
3676
3677         default:
3678                 l2cap_data_channel(conn, cid, skb);
3679                 break;
3680         }
3681 }
3682
3683 /* ---- L2CAP interface with lower layer (HCI) ---- */
3684
3685 static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
3686 {
3687         int exact = 0, lm1 = 0, lm2 = 0;
3688         register struct sock *sk;
3689         struct hlist_node *node;
3690
3691         if (type != ACL_LINK)
3692                 return -EINVAL;
3693
3694         BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
3695
3696         /* Find listening sockets and check their link_mode */
3697         read_lock(&l2cap_sk_list.lock);
3698         sk_for_each(sk, node, &l2cap_sk_list.head) {
3699                 if (sk->sk_state != BT_LISTEN)
3700                         continue;
3701
3702                 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
3703                         lm1 |= HCI_LM_ACCEPT;
3704                         if (l2cap_pi(sk)->role_switch)
3705                                 lm1 |= HCI_LM_MASTER;
3706                         exact++;
3707                 } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
3708                         lm2 |= HCI_LM_ACCEPT;
3709                         if (l2cap_pi(sk)->role_switch)
3710                                 lm2 |= HCI_LM_MASTER;
3711                 }
3712         }
3713         read_unlock(&l2cap_sk_list.lock);
3714
3715         return exact ? lm1 : lm2;
3716 }
3717
3718 static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
3719 {
3720         struct l2cap_conn *conn;
3721
3722         BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
3723
3724         if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK))
3725                 return -EINVAL;
3726
3727         if (!status) {
3728                 conn = l2cap_conn_add(hcon, status);
3729                 if (conn)
3730                         l2cap_conn_ready(conn);
3731         } else
3732                 l2cap_conn_del(hcon, bt_err(status));
3733
3734         return 0;
3735 }
3736
3737 static int l2cap_disconn_ind(struct hci_conn *hcon)
3738 {
3739         struct l2cap_conn *conn = hcon->l2cap_data;
3740
3741         BT_DBG("hcon %p", hcon);
3742
3743         if (hcon->type != ACL_LINK || !conn)
3744                 return 0x13;
3745
3746         return conn->disc_reason;
3747 }
3748
3749 static int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
3750 {
3751         BT_DBG("hcon %p reason %d", hcon, reason);
3752
3753         if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK))
3754                 return -EINVAL;
3755
3756         l2cap_conn_del(hcon, bt_err(reason));
3757
3758         return 0;
3759 }
3760
3761 static inline void l2cap_check_encryption(struct sock *sk, u8 encrypt)
3762 {
3763         if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM)
3764                 return;
3765
3766         if (encrypt == 0x00) {
3767                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_MEDIUM) {
3768                         l2cap_sock_clear_timer(sk);
3769                         l2cap_sock_set_timer(sk, HZ * 5);
3770                 } else if (l2cap_pi(sk)->sec_level == BT_SECURITY_HIGH)
3771                         __l2cap_sock_close(sk, ECONNREFUSED);
3772         } else {
3773                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_MEDIUM)
3774                         l2cap_sock_clear_timer(sk);
3775         }
3776 }
3777
3778 static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
3779 {
3780         struct l2cap_chan_list *l;
3781         struct l2cap_conn *conn = hcon->l2cap_data;
3782         struct sock *sk;
3783
3784         if (!conn)
3785                 return 0;
3786
3787         l = &conn->chan_list;
3788
3789         BT_DBG("conn %p", conn);
3790
3791         read_lock(&l->lock);
3792
3793         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
3794                 bh_lock_sock(sk);
3795
3796                 if (l2cap_pi(sk)->conf_state & L2CAP_CONF_CONNECT_PEND) {
3797                         bh_unlock_sock(sk);
3798                         continue;
3799                 }
3800
3801                 if (!status && (sk->sk_state == BT_CONNECTED ||
3802                                                 sk->sk_state == BT_CONFIG)) {
3803                         l2cap_check_encryption(sk, encrypt);
3804                         bh_unlock_sock(sk);
3805                         continue;
3806                 }
3807
3808                 if (sk->sk_state == BT_CONNECT) {
3809                         if (!status) {
3810                                 struct l2cap_conn_req req;
3811                                 req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
3812                                 req.psm  = l2cap_pi(sk)->psm;
3813
3814                                 l2cap_pi(sk)->ident = l2cap_get_ident(conn);
3815                                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND;
3816
3817                                 l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
3818                                         L2CAP_CONN_REQ, sizeof(req), &req);
3819                         } else {
3820                                 l2cap_sock_clear_timer(sk);
3821                                 l2cap_sock_set_timer(sk, HZ / 10);
3822                         }
3823                 } else if (sk->sk_state == BT_CONNECT2) {
3824                         struct l2cap_conn_rsp rsp;
3825                         __u16 result;
3826
3827                         if (!status) {
3828                                 sk->sk_state = BT_CONFIG;
3829                                 result = L2CAP_CR_SUCCESS;
3830                         } else {
3831                                 sk->sk_state = BT_DISCONN;
3832                                 l2cap_sock_set_timer(sk, HZ / 10);
3833                                 result = L2CAP_CR_SEC_BLOCK;
3834                         }
3835
3836                         rsp.scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
3837                         rsp.dcid   = cpu_to_le16(l2cap_pi(sk)->scid);
3838                         rsp.result = cpu_to_le16(result);
3839                         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
3840                         l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
3841                                         L2CAP_CONN_RSP, sizeof(rsp), &rsp);
3842                 }
3843
3844                 bh_unlock_sock(sk);
3845         }
3846
3847         read_unlock(&l->lock);
3848
3849         return 0;
3850 }
3851
3852 static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
3853 {
3854         struct l2cap_conn *conn = hcon->l2cap_data;
3855
3856         if (!conn)
3857                 conn = l2cap_conn_add(hcon, 0);
3858
3859         if (!conn)
3860                 goto drop;
3861
3862         BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
3863
3864         if (!(flags & ACL_CONT)) {
3865                 struct l2cap_hdr *hdr;
3866                 struct sock *sk;
3867                 u16 cid;
3868                 int len;
3869
3870                 if (conn->rx_len) {
3871                         BT_ERR("Unexpected start frame (len %d)", skb->len);
3872                         kfree_skb(conn->rx_skb);
3873                         conn->rx_skb = NULL;
3874                         conn->rx_len = 0;
3875                         l2cap_conn_unreliable(conn, ECOMM);
3876                 }
3877
3878                 /* Start fragment always begin with Basic L2CAP header */
3879                 if (skb->len < L2CAP_HDR_SIZE) {
3880                         BT_ERR("Frame is too short (len %d)", skb->len);
3881                         l2cap_conn_unreliable(conn, ECOMM);
3882                         goto drop;
3883                 }
3884
3885                 hdr = (struct l2cap_hdr *) skb->data;
3886                 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
3887                 cid = __le16_to_cpu(hdr->cid);
3888
3889                 if (len == skb->len) {
3890                         /* Complete frame received */
3891                         l2cap_recv_frame(conn, skb);
3892                         return 0;
3893                 }
3894
3895                 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
3896
3897                 if (skb->len > len) {
3898                         BT_ERR("Frame is too long (len %d, expected len %d)",
3899                                 skb->len, len);
3900                         l2cap_conn_unreliable(conn, ECOMM);
3901                         goto drop;
3902                 }
3903
3904                 sk = l2cap_get_chan_by_scid(&conn->chan_list, cid);
3905
3906                 if (sk && l2cap_pi(sk)->imtu < len - L2CAP_HDR_SIZE) {
3907                         BT_ERR("Frame exceeding recv MTU (len %d, MTU %d)",
3908                                         len, l2cap_pi(sk)->imtu);
3909                         bh_unlock_sock(sk);
3910                         l2cap_conn_unreliable(conn, ECOMM);
3911                         goto drop;
3912                 }
3913
3914                 if (sk)
3915                         bh_unlock_sock(sk);
3916
3917                 /* Allocate skb for the complete frame (with header) */
3918                 conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC);
3919                 if (!conn->rx_skb)
3920                         goto drop;
3921
3922                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
3923                                                                 skb->len);
3924                 conn->rx_len = len - skb->len;
3925         } else {
3926                 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
3927
3928                 if (!conn->rx_len) {
3929                         BT_ERR("Unexpected continuation frame (len %d)", skb->len);
3930                         l2cap_conn_unreliable(conn, ECOMM);
3931                         goto drop;
3932                 }
3933
3934                 if (skb->len > conn->rx_len) {
3935                         BT_ERR("Fragment is too long (len %d, expected %d)",
3936                                         skb->len, conn->rx_len);
3937                         kfree_skb(conn->rx_skb);
3938                         conn->rx_skb = NULL;
3939                         conn->rx_len = 0;
3940                         l2cap_conn_unreliable(conn, ECOMM);
3941                         goto drop;
3942                 }
3943
3944                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
3945                                                                 skb->len);
3946                 conn->rx_len -= skb->len;
3947
3948                 if (!conn->rx_len) {
3949                         /* Complete frame received */
3950                         l2cap_recv_frame(conn, conn->rx_skb);
3951                         conn->rx_skb = NULL;
3952                 }
3953         }
3954
3955 drop:
3956         kfree_skb(skb);
3957         return 0;
3958 }
3959
3960 static int l2cap_debugfs_show(struct seq_file *f, void *p)
3961 {
3962         struct sock *sk;
3963         struct hlist_node *node;
3964
3965         read_lock_bh(&l2cap_sk_list.lock);
3966
3967         sk_for_each(sk, node, &l2cap_sk_list.head) {
3968                 struct l2cap_pinfo *pi = l2cap_pi(sk);
3969
3970                 seq_printf(f, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
3971                                         batostr(&bt_sk(sk)->src),
3972                                         batostr(&bt_sk(sk)->dst),
3973                                         sk->sk_state, __le16_to_cpu(pi->psm),
3974                                         pi->scid, pi->dcid,
3975                                         pi->imtu, pi->omtu, pi->sec_level,
3976                                         pi->mode);
3977         }
3978
3979         read_unlock_bh(&l2cap_sk_list.lock);
3980
3981         return 0;
3982 }
3983
3984 static int l2cap_debugfs_open(struct inode *inode, struct file *file)
3985 {
3986         return single_open(file, l2cap_debugfs_show, inode->i_private);
3987 }
3988
3989 static const struct file_operations l2cap_debugfs_fops = {
3990         .open           = l2cap_debugfs_open,
3991         .read           = seq_read,
3992         .llseek         = seq_lseek,
3993         .release        = single_release,
3994 };
3995
3996 static struct dentry *l2cap_debugfs;
3997
3998 static struct hci_proto l2cap_hci_proto = {
3999         .name           = "L2CAP",
4000         .id             = HCI_PROTO_L2CAP,
4001         .connect_ind    = l2cap_connect_ind,
4002         .connect_cfm    = l2cap_connect_cfm,
4003         .disconn_ind    = l2cap_disconn_ind,
4004         .disconn_cfm    = l2cap_disconn_cfm,
4005         .security_cfm   = l2cap_security_cfm,
4006         .recv_acldata   = l2cap_recv_acldata
4007 };
4008
4009 int __init l2cap_init(void)
4010 {
4011         int err;
4012
4013         err = l2cap_init_sockets();
4014         if (err < 0)
4015                 return err;
4016
4017         _busy_wq = create_singlethread_workqueue("l2cap");
4018         if (!_busy_wq) {
4019                 err = -ENOMEM;
4020                 goto error;
4021         }
4022
4023         err = hci_register_proto(&l2cap_hci_proto);
4024         if (err < 0) {
4025                 BT_ERR("L2CAP protocol registration failed");
4026                 bt_sock_unregister(BTPROTO_L2CAP);
4027                 goto error;
4028         }
4029
4030         if (bt_debugfs) {
4031                 l2cap_debugfs = debugfs_create_file("l2cap", 0444,
4032                                         bt_debugfs, NULL, &l2cap_debugfs_fops);
4033                 if (!l2cap_debugfs)
4034                         BT_ERR("Failed to create L2CAP debug file");
4035         }
4036
4037         return 0;
4038
4039 error:
4040         destroy_workqueue(_busy_wq);
4041         l2cap_cleanup_sockets();
4042         return err;
4043 }
4044
4045 void l2cap_exit(void)
4046 {
4047         debugfs_remove(l2cap_debugfs);
4048
4049         flush_workqueue(_busy_wq);
4050         destroy_workqueue(_busy_wq);
4051
4052         if (hci_unregister_proto(&l2cap_hci_proto) < 0)
4053                 BT_ERR("L2CAP protocol unregistration failed");
4054
4055         l2cap_cleanup_sockets();
4056 }
4057
4058 module_param(disable_ertm, bool, 0644);
4059 MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode");