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