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)
 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
 209 /* Delete channel.
 210  * Must be called on the locked socket. */
 211 void l2cap_chan_del(struct sock *sk, int err)
 212 {
 213         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
 214         struct sock *parent = bt_sk(sk)->parent;
 215
 216         l2cap_sock_clear_timer(sk);
 217
 218         BT_DBG("sk %p, conn %p, err %d", sk, conn, err);
 219
 220         if (conn) {
 221                 /* Unlink from channel list */
 222                 l2cap_chan_unlink(&conn->chan_list, sk);
 223                 l2cap_pi(sk)->conn = NULL;
 224                 hci_conn_put(conn->hcon);
 225         }
 226
 227         sk->sk_state = BT_CLOSED;
 228         sock_set_flag(sk, SOCK_ZAPPED);
 229
 230         if (err)
 231                 sk->sk_err = err;
 232
 233         if (parent) {
 234                 bt_accept_unlink(sk);
 235                 parent->sk_data_ready(parent, 0);
 236         } else
 237                 sk->sk_state_change(sk);
 238
 239         skb_queue_purge(TX_QUEUE(sk));
 240
 241         if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM) {
 242                 struct srej_list *l, *tmp;
 243
 244                 del_timer(&l2cap_pi(sk)->retrans_timer);
 245                 del_timer(&l2cap_pi(sk)->monitor_timer);
 246                 del_timer(&l2cap_pi(sk)->ack_timer);
 247
 248                 skb_queue_purge(SREJ_QUEUE(sk));
 249                 skb_queue_purge(BUSY_QUEUE(sk));
 250
 251                 list_for_each_entry_safe(l, tmp, SREJ_LIST(sk), list) {
 252                         list_del(&l->list);
 253                         kfree(l);
 254                 }
 255         }
 256 }
 257
 258 static inline u8 l2cap_get_auth_type(struct sock *sk)
 259 {
 260         if (sk->sk_type == SOCK_RAW) {
 261                 switch (l2cap_pi(sk)->sec_level) {
 262                 case BT_SECURITY_HIGH:
 263                         return HCI_AT_DEDICATED_BONDING_MITM;
 264                 case BT_SECURITY_MEDIUM:
 265                         return HCI_AT_DEDICATED_BONDING;
 266                 default:
 267                         return HCI_AT_NO_BONDING;
 268                 }
 269         } else if (l2cap_pi(sk)->psm == cpu_to_le16(0x0001)) {
 270                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_LOW)
 271                         l2cap_pi(sk)->sec_level = BT_SECURITY_SDP;
 272
 273                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_HIGH)
 274                         return HCI_AT_NO_BONDING_MITM;
 275                 else
 276                         return HCI_AT_NO_BONDING;
 277         } else {
 278                 switch (l2cap_pi(sk)->sec_level) {
 279                 case BT_SECURITY_HIGH:
 280                         return HCI_AT_GENERAL_BONDING_MITM;
 281                 case BT_SECURITY_MEDIUM:
 282                         return HCI_AT_GENERAL_BONDING;
 283                 default:
 284                         return HCI_AT_NO_BONDING;
 285                 }
 286         }
 287 }
 288
 289 /* Service level security */
 290 static inline int l2cap_check_security(struct sock *sk)
 291 {
 292         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
 293         __u8 auth_type;
 294
 295         auth_type = l2cap_get_auth_type(sk);
 296
 297         return hci_conn_security(conn->hcon, l2cap_pi(sk)->sec_level,
 298                                                                 auth_type);
 299 }
 300
 301 u8 l2cap_get_ident(struct l2cap_conn *conn)
 302 {
 303         u8 id;
 304
 305         /* Get next available identificator.
 306          *    1 - 128 are used by kernel.
 307          *  129 - 199 are reserved.
 308          *  200 - 254 are used by utilities like l2ping, etc.
 309          */
 310
 311         spin_lock_bh(&conn->lock);
 312
 313         if (++conn->tx_ident > 128)
 314                 conn->tx_ident = 1;
 315
 316         id = conn->tx_ident;
 317
 318         spin_unlock_bh(&conn->lock);
 319
 320         return id;
 321 }
 322
 323 void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, void *data)
 324 {
 325         struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data);
 326         u8 flags;
 327
 328         BT_DBG("code 0x%2.2x", code);
 329
 330         if (!skb)
 331                 return;
 332
 333         if (lmp_no_flush_capable(conn->hcon->hdev))
 334                 flags = ACL_START_NO_FLUSH;
 335         else
 336                 flags = ACL_START;
 337
 338         hci_send_acl(conn->hcon, skb, flags);
 339 }
 340
 341 static inline void l2cap_send_sframe(struct l2cap_pinfo *pi, u16 control)
 342 {
 343         struct sk_buff *skb;
 344         struct l2cap_hdr *lh;
 345         struct l2cap_conn *conn = pi->conn;
 346         struct sock *sk = (struct sock *)pi;
 347         int count, hlen = L2CAP_HDR_SIZE + 2;
 348         u8 flags;
 349
 350         if (sk->sk_state != BT_CONNECTED)
 351                 return;
 352
 353         if (pi->fcs == L2CAP_FCS_CRC16)
 354                 hlen += 2;
 355
 356         BT_DBG("pi %p, control 0x%2.2x", pi, control);
 357
 358         count = min_t(unsigned int, conn->mtu, hlen);
 359         control |= L2CAP_CTRL_FRAME_TYPE;
 360
 361         if (pi->conn_state & L2CAP_CONN_SEND_FBIT) {
 362                 control |= L2CAP_CTRL_FINAL;
 363                 pi->conn_state &= ~L2CAP_CONN_SEND_FBIT;
 364         }
 365
 366         if (pi->conn_state & L2CAP_CONN_SEND_PBIT) {
 367                 control |= L2CAP_CTRL_POLL;
 368                 pi->conn_state &= ~L2CAP_CONN_SEND_PBIT;
 369         }
 370
 371         skb = bt_skb_alloc(count, GFP_ATOMIC);
 372         if (!skb)
 373                 return;
 374
 375         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
 376         lh->len = cpu_to_le16(hlen - L2CAP_HDR_SIZE);
 377         lh->cid = cpu_to_le16(pi->dcid);
 378         put_unaligned_le16(control, skb_put(skb, 2));
 379
 380         if (pi->fcs == L2CAP_FCS_CRC16) {
 381                 u16 fcs = crc16(0, (u8 *)lh, count - 2);
 382                 put_unaligned_le16(fcs, skb_put(skb, 2));
 383         }
 384
 385         if (lmp_no_flush_capable(conn->hcon->hdev))
 386                 flags = ACL_START_NO_FLUSH;
 387         else
 388                 flags = ACL_START;
 389
 390         hci_send_acl(pi->conn->hcon, skb, flags);
 391 }
 392
 393 static inline void l2cap_send_rr_or_rnr(struct l2cap_pinfo *pi, u16 control)
 394 {
 395         if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
 396                 control |= L2CAP_SUPER_RCV_NOT_READY;
 397                 pi->conn_state |= L2CAP_CONN_RNR_SENT;
 398         } else
 399                 control |= L2CAP_SUPER_RCV_READY;
 400
 401         control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
 402
 403         l2cap_send_sframe(pi, control);
 404 }
 405
 406 static inline int __l2cap_no_conn_pending(struct sock *sk)
 407 {
 408         return !(l2cap_pi(sk)->conf_state & L2CAP_CONF_CONNECT_PEND);
 409 }
 410
 411 static void l2cap_do_start(struct sock *sk)
 412 {
 413         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
 414
 415         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) {
 416                 if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE))
 417                         return;
 418
 419                 if (l2cap_check_security(sk) && __l2cap_no_conn_pending(sk)) {
 420                         struct l2cap_conn_req req;
 421                         req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
 422                         req.psm  = l2cap_pi(sk)->psm;
 423
 424                         l2cap_pi(sk)->ident = l2cap_get_ident(conn);
 425                         l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND;
 426
 427                         l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
 428                                         L2CAP_CONN_REQ, sizeof(req), &req);
 429                 }
 430         } else {
 431                 struct l2cap_info_req req;
 432                 req.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
 433
 434                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
 435                 conn->info_ident = l2cap_get_ident(conn);
 436
 437                 mod_timer(&conn->info_timer, jiffies +
 438                                         msecs_to_jiffies(L2CAP_INFO_TIMEOUT));
 439
 440                 l2cap_send_cmd(conn, conn->info_ident,
 441                                         L2CAP_INFO_REQ, sizeof(req), &req);
 442         }
 443 }
 444
 445 static inline int l2cap_mode_supported(__u8 mode, __u32 feat_mask)
 446 {
 447         u32 local_feat_mask = l2cap_feat_mask;
 448         if (!disable_ertm)
 449                 local_feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING;
 450
 451         switch (mode) {
 452         case L2CAP_MODE_ERTM:
 453                 return L2CAP_FEAT_ERTM & feat_mask & local_feat_mask;
 454         case L2CAP_MODE_STREAMING:
 455                 return L2CAP_FEAT_STREAMING & feat_mask & local_feat_mask;
 456         default:
 457                 return 0x00;
 458         }
 459 }
 460
 461 void l2cap_send_disconn_req(struct l2cap_conn *conn, struct sock *sk, int err)
 462 {
 463         struct l2cap_disconn_req req;
 464
 465         if (!conn)
 466                 return;
 467
 468         skb_queue_purge(TX_QUEUE(sk));
 469
 470         if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM) {
 471                 del_timer(&l2cap_pi(sk)->retrans_timer);
 472                 del_timer(&l2cap_pi(sk)->monitor_timer);
 473                 del_timer(&l2cap_pi(sk)->ack_timer);
 474         }
 475
 476         req.dcid = cpu_to_le16(l2cap_pi(sk)->dcid);
 477         req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
 478         l2cap_send_cmd(conn, l2cap_get_ident(conn),
 479                         L2CAP_DISCONN_REQ, sizeof(req), &req);
 480
 481         sk->sk_state = BT_DISCONN;
 482         sk->sk_err = err;
 483 }
 484
 485 /* ---- L2CAP connections ---- */
 486 static void l2cap_conn_start(struct l2cap_conn *conn)
 487 {
 488         struct l2cap_chan_list *l = &conn->chan_list;
 489         struct sock_del_list del, *tmp1, *tmp2;
 490         struct sock *sk;
 491
 492         BT_DBG("conn %p", conn);
 493
 494         INIT_LIST_HEAD(&del.list);
 495
 496         read_lock(&l->lock);
 497
 498         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
 499                 bh_lock_sock(sk);
 500
 501                 if (sk->sk_type != SOCK_SEQPACKET &&
 502                                 sk->sk_type != SOCK_STREAM) {
 503                         bh_unlock_sock(sk);
 504                         continue;
 505                 }
 506
 507                 if (sk->sk_state == BT_CONNECT) {
 508                         struct l2cap_conn_req req;
 509
 510                         if (!l2cap_check_security(sk) ||
 511                                         !__l2cap_no_conn_pending(sk)) {
 512                                 bh_unlock_sock(sk);
 513                                 continue;
 514                         }
 515
 516                         if (!l2cap_mode_supported(l2cap_pi(sk)->mode,
 517                                         conn->feat_mask)
 518                                         && l2cap_pi(sk)->conf_state &
 519                                         L2CAP_CONF_STATE2_DEVICE) {
 520                                 tmp1 = kzalloc(sizeof(struct sock_del_list),
 521                                                 GFP_ATOMIC);
 522                                 tmp1->sk = sk;
 523                                 list_add_tail(&tmp1->list, &del.list);
 524                                 bh_unlock_sock(sk);
 525                                 continue;
 526                         }
 527
 528                         req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
 529                         req.psm  = l2cap_pi(sk)->psm;
 530
 531                         l2cap_pi(sk)->ident = l2cap_get_ident(conn);
 532                         l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND;
 533
 534                         l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
 535                                 L2CAP_CONN_REQ, sizeof(req), &req);
 536
 537                 } else if (sk->sk_state == BT_CONNECT2) {
 538                         struct l2cap_conn_rsp rsp;
 539                         char buf[128];
 540                         rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid);
 541                         rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid);
 542
 543                         if (l2cap_check_security(sk)) {
 544                                 if (bt_sk(sk)->defer_setup) {
 545                                         struct sock *parent = bt_sk(sk)->parent;
 546                                         rsp.result = cpu_to_le16(L2CAP_CR_PEND);
 547                                         rsp.status = cpu_to_le16(L2CAP_CS_AUTHOR_PEND);
 548                                         parent->sk_data_ready(parent, 0);
 549
 550                                 } else {
 551                                         sk->sk_state = BT_CONFIG;
 552                                         rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
 553                                         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
 554                                 }
 555                         } else {
 556                                 rsp.result = cpu_to_le16(L2CAP_CR_PEND);
 557                                 rsp.status = cpu_to_le16(L2CAP_CS_AUTHEN_PEND);
 558                         }
 559
 560                         l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
 561                                         L2CAP_CONN_RSP, sizeof(rsp), &rsp);
 562
 563                         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT ||
 564                                         rsp.result != L2CAP_CR_SUCCESS) {
 565                                 bh_unlock_sock(sk);
 566                                 continue;
 567                         }
 568
 569                         l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
 570                         l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
 571                                                 l2cap_build_conf_req(sk, buf), buf);
 572                         l2cap_pi(sk)->num_conf_req++;
 573                 }
 574
 575                 bh_unlock_sock(sk);
 576         }
 577
 578         read_unlock(&l->lock);
 579
 580         list_for_each_entry_safe(tmp1, tmp2, &del.list, list) {
 581                 bh_lock_sock(tmp1->sk);
 582                 __l2cap_sock_close(tmp1->sk, ECONNRESET);
 583                 bh_unlock_sock(tmp1->sk);
 584                 list_del(&tmp1->list);
 585                 kfree(tmp1);
 586         }
 587 }
 588
 589 /* Find socket with cid and source bdaddr.
 590  * Returns closest match, locked.
 591  */
 592 static struct sock *l2cap_get_sock_by_scid(int state, __le16 cid, bdaddr_t *src)
 593 {
 594         struct sock *s, *sk = NULL, *sk1 = NULL;
 595         struct hlist_node *node;
 596
 597         read_lock(&l2cap_sk_list.lock);
 598
 599         sk_for_each(sk, node, &l2cap_sk_list.head) {
 600                 if (state && sk->sk_state != state)
 601                         continue;
 602
 603                 if (l2cap_pi(sk)->scid == cid) {
 604                         /* Exact match. */
 605                         if (!bacmp(&bt_sk(sk)->src, src))
 606                                 break;
 607
 608                         /* Closest match */
 609                         if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
 610                                 sk1 = sk;
 611                 }
 612         }
 613         s = node ? sk : sk1;
 614         if (s)
 615                 bh_lock_sock(s);
 616         read_unlock(&l2cap_sk_list.lock);
 617
 618         return s;
 619 }
 620
 621 static void l2cap_le_conn_ready(struct l2cap_conn *conn)
 622 {
 623         struct l2cap_chan_list *list = &conn->chan_list;
 624         struct sock *parent, *uninitialized_var(sk);
 625
 626         BT_DBG("");
 627
 628         /* Check if we have socket listening on cid */
 629         parent = l2cap_get_sock_by_scid(BT_LISTEN, L2CAP_CID_LE_DATA,
 630                                                         conn->src);
 631         if (!parent)
 632                 return;
 633
 634         /* Check for backlog size */
 635         if (sk_acceptq_is_full(parent)) {
 636                 BT_DBG("backlog full %d", parent->sk_ack_backlog);
 637                 goto clean;
 638         }
 639
 640         sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, GFP_ATOMIC);
 641         if (!sk)
 642                 goto clean;
 643
 644         write_lock_bh(&list->lock);
 645
 646         hci_conn_hold(conn->hcon);
 647
 648         l2cap_sock_init(sk, parent);
 649         bacpy(&bt_sk(sk)->src, conn->src);
 650         bacpy(&bt_sk(sk)->dst, conn->dst);
 651
 652         bt_accept_enqueue(parent, sk);
 653
 654         __l2cap_chan_add(conn, sk);
 655
 656         l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
 657
 658         sk->sk_state = BT_CONNECTED;
 659         parent->sk_data_ready(parent, 0);
 660
 661         write_unlock_bh(&list->lock);
 662
 663 clean:
 664         bh_unlock_sock(parent);
 665 }
 666
 667 static void l2cap_conn_ready(struct l2cap_conn *conn)
 668 {
 669         struct l2cap_chan_list *l = &conn->chan_list;
 670         struct sock *sk;
 671
 672         BT_DBG("conn %p", conn);
 673
 674         if (!conn->hcon->out && conn->hcon->type == LE_LINK)
 675                 l2cap_le_conn_ready(conn);
 676
 677         read_lock(&l->lock);
 678
 679         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
 680                 bh_lock_sock(sk);
 681
 682                 if (conn->hcon->type == LE_LINK) {
 683                         l2cap_sock_clear_timer(sk);
 684                         sk->sk_state = BT_CONNECTED;
 685                         sk->sk_state_change(sk);
 686                 }
 687
 688                 if (sk->sk_type != SOCK_SEQPACKET &&
 689                                 sk->sk_type != SOCK_STREAM) {
 690                         l2cap_sock_clear_timer(sk);
 691                         sk->sk_state = BT_CONNECTED;
 692                         sk->sk_state_change(sk);
 693                 } else if (sk->sk_state == BT_CONNECT)
 694                         l2cap_do_start(sk);
 695
 696                 bh_unlock_sock(sk);
 697         }
 698
 699         read_unlock(&l->lock);
 700 }
 701
 702 /* Notify sockets that we cannot guaranty reliability anymore */
 703 static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err)
 704 {
 705         struct l2cap_chan_list *l = &conn->chan_list;
 706         struct sock *sk;
 707
 708         BT_DBG("conn %p", conn);
 709
 710         read_lock(&l->lock);
 711
 712         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
 713                 if (l2cap_pi(sk)->force_reliable)
 714                         sk->sk_err = err;
 715         }
 716
 717         read_unlock(&l->lock);
 718 }
 719
 720 static void l2cap_info_timeout(unsigned long arg)
 721 {
 722         struct l2cap_conn *conn = (void *) arg;
 723
 724         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
 725         conn->info_ident = 0;
 726
 727         l2cap_conn_start(conn);
 728 }
 729
 730 static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status)
 731 {
 732         struct l2cap_conn *conn = hcon->l2cap_data;
 733
 734         if (conn || status)
 735                 return conn;
 736
 737         conn = kzalloc(sizeof(struct l2cap_conn), GFP_ATOMIC);
 738         if (!conn)
 739                 return NULL;
 740
 741         hcon->l2cap_data = conn;
 742         conn->hcon = hcon;
 743
 744         BT_DBG("hcon %p conn %p", hcon, conn);
 745
 746         if (hcon->hdev->le_mtu && hcon->type == LE_LINK)
 747                 conn->mtu = hcon->hdev->le_mtu;
 748         else
 749                 conn->mtu = hcon->hdev->acl_mtu;
 750
 751         conn->src = &hcon->hdev->bdaddr;
 752         conn->dst = &hcon->dst;
 753
 754         conn->feat_mask = 0;
 755
 756         spin_lock_init(&conn->lock);
 757         rwlock_init(&conn->chan_list.lock);
 758
 759         if (hcon->type != LE_LINK)
 760                 setup_timer(&conn->info_timer, l2cap_info_timeout,
 761                                                 (unsigned long) conn);
 762
 763         conn->disc_reason = 0x13;
 764
 765         return conn;
 766 }
 767
 768 static void l2cap_conn_del(struct hci_conn *hcon, int err)
 769 {
 770         struct l2cap_conn *conn = hcon->l2cap_data;
 771         struct sock *sk;
 772
 773         if (!conn)
 774                 return;
 775
 776         BT_DBG("hcon %p conn %p, err %d", hcon, conn, err);
 777
 778         kfree_skb(conn->rx_skb);
 779
 780         /* Kill channels */
 781         while ((sk = conn->chan_list.head)) {
 782                 bh_lock_sock(sk);
 783                 l2cap_chan_del(sk, err);
 784                 bh_unlock_sock(sk);
 785                 l2cap_sock_kill(sk);
 786         }
 787
 788         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT)
 789                 del_timer_sync(&conn->info_timer);
 790
 791         hcon->l2cap_data = NULL;
 792         kfree(conn);
 793 }
 794
 795 static inline void l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk)
 796 {
 797         struct l2cap_chan_list *l = &conn->chan_list;
 798         write_lock_bh(&l->lock);
 799         __l2cap_chan_add(conn, sk);
 800         write_unlock_bh(&l->lock);
 801 }
 802
 803 /* ---- Socket interface ---- */
 804
 805 /* Find socket with psm and source bdaddr.
 806  * Returns closest match.
 807  */
 808 static struct sock *l2cap_get_sock_by_psm(int state, __le16 psm, bdaddr_t *src)
 809 {
 810         struct sock *sk = NULL, *sk1 = NULL;
 811         struct hlist_node *node;
 812
 813         read_lock(&l2cap_sk_list.lock);
 814
 815         sk_for_each(sk, node, &l2cap_sk_list.head) {
 816                 if (state && sk->sk_state != state)
 817                         continue;
 818
 819                 if (l2cap_pi(sk)->psm == psm) {
 820                         /* Exact match. */
 821                         if (!bacmp(&bt_sk(sk)->src, src))
 822                                 break;
 823
 824                         /* Closest match */
 825                         if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
 826                                 sk1 = sk;
 827                 }
 828         }
 829
 830         read_unlock(&l2cap_sk_list.lock);
 831
 832         return node ? sk : sk1;
 833 }
 834
 835 int l2cap_do_connect(struct sock *sk)
 836 {
 837         bdaddr_t *src = &bt_sk(sk)->src;
 838         bdaddr_t *dst = &bt_sk(sk)->dst;
 839         struct l2cap_conn *conn;
 840         struct hci_conn *hcon;
 841         struct hci_dev *hdev;
 842         __u8 auth_type;
 843         int err;
 844
 845         BT_DBG("%s -> %s psm 0x%2.2x", batostr(src), batostr(dst),
 846                                                         l2cap_pi(sk)->psm);
 847
 848         hdev = hci_get_route(dst, src);
 849         if (!hdev)
 850                 return -EHOSTUNREACH;
 851
 852         hci_dev_lock_bh(hdev);
 853
 854         auth_type = l2cap_get_auth_type(sk);
 855
 856         if (l2cap_pi(sk)->dcid == L2CAP_CID_LE_DATA)
 857                 hcon = hci_connect(hdev, LE_LINK, dst,
 858                                         l2cap_pi(sk)->sec_level, auth_type);
 859         else
 860                 hcon = hci_connect(hdev, ACL_LINK, dst,
 861                                         l2cap_pi(sk)->sec_level, auth_type);
 862
 863         if (IS_ERR(hcon)) {
 864                 err = PTR_ERR(hcon);
 865                 goto done;
 866         }
 867
 868         conn = l2cap_conn_add(hcon, 0);
 869         if (!conn) {
 870                 hci_conn_put(hcon);
 871                 err = -ENOMEM;
 872                 goto done;
 873         }
 874
 875         /* Update source addr of the socket */
 876         bacpy(src, conn->src);
 877
 878         l2cap_chan_add(conn, sk);
 879
 880         sk->sk_state = BT_CONNECT;
 881         l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
 882
 883         if (hcon->state == BT_CONNECTED) {
 884                 if (sk->sk_type != SOCK_SEQPACKET &&
 885                                 sk->sk_type != SOCK_STREAM) {
 886                         l2cap_sock_clear_timer(sk);
 887                         if (l2cap_check_security(sk))
 888                                 sk->sk_state = BT_CONNECTED;
 889                 } else
 890                         l2cap_do_start(sk);
 891         }
 892
 893         err = 0;
 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         req->dcid  = cpu_to_le16(pi->dcid);
1675         req->flags = cpu_to_le16(0);
1676
1677         return ptr - data;
1678 }
1679
1680 static int l2cap_parse_conf_req(struct sock *sk, void *data)
1681 {
1682         struct l2cap_pinfo *pi = l2cap_pi(sk);
1683         struct l2cap_conf_rsp *rsp = data;
1684         void *ptr = rsp->data;
1685         void *req = pi->conf_req;
1686         int len = pi->conf_len;
1687         int type, hint, olen;
1688         unsigned long val;
1689         struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
1690         u16 mtu = L2CAP_DEFAULT_MTU;
1691         u16 result = L2CAP_CONF_SUCCESS;
1692
1693         BT_DBG("sk %p", sk);
1694
1695         while (len >= L2CAP_CONF_OPT_SIZE) {
1696                 len -= l2cap_get_conf_opt(&req, &type, &olen, &val);
1697
1698                 hint  = type & L2CAP_CONF_HINT;
1699                 type &= L2CAP_CONF_MASK;
1700
1701                 switch (type) {
1702                 case L2CAP_CONF_MTU:
1703                         mtu = val;
1704                         break;
1705
1706                 case L2CAP_CONF_FLUSH_TO:
1707                         pi->flush_to = val;
1708                         break;
1709
1710                 case L2CAP_CONF_QOS:
1711                         break;
1712
1713                 case L2CAP_CONF_RFC:
1714                         if (olen == sizeof(rfc))
1715                                 memcpy(&rfc, (void *) val, olen);
1716                         break;
1717
1718                 case L2CAP_CONF_FCS:
1719                         if (val == L2CAP_FCS_NONE)
1720                                 pi->conf_state |= L2CAP_CONF_NO_FCS_RECV;
1721
1722                         break;
1723
1724                 default:
1725                         if (hint)
1726                                 break;
1727
1728                         result = L2CAP_CONF_UNKNOWN;
1729                         *((u8 *) ptr++) = type;
1730                         break;
1731                 }
1732         }
1733
1734         if (pi->num_conf_rsp || pi->num_conf_req > 1)
1735                 goto done;
1736
1737         switch (pi->mode) {
1738         case L2CAP_MODE_STREAMING:
1739         case L2CAP_MODE_ERTM:
1740                 if (!(pi->conf_state & L2CAP_CONF_STATE2_DEVICE)) {
1741                         pi->mode = l2cap_select_mode(rfc.mode,
1742                                         pi->conn->feat_mask);
1743                         break;
1744                 }
1745
1746                 if (pi->mode != rfc.mode)
1747                         return -ECONNREFUSED;
1748
1749                 break;
1750         }
1751
1752 done:
1753         if (pi->mode != rfc.mode) {
1754                 result = L2CAP_CONF_UNACCEPT;
1755                 rfc.mode = pi->mode;
1756
1757                 if (pi->num_conf_rsp == 1)
1758                         return -ECONNREFUSED;
1759
1760                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
1761                                         sizeof(rfc), (unsigned long) &rfc);
1762         }
1763
1764
1765         if (result == L2CAP_CONF_SUCCESS) {
1766                 /* Configure output options and let the other side know
1767                  * which ones we don't like. */
1768
1769                 if (mtu < L2CAP_DEFAULT_MIN_MTU)
1770                         result = L2CAP_CONF_UNACCEPT;
1771                 else {
1772                         pi->omtu = mtu;
1773                         pi->conf_state |= L2CAP_CONF_MTU_DONE;
1774                 }
1775                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->omtu);
1776
1777                 switch (rfc.mode) {
1778                 case L2CAP_MODE_BASIC:
1779                         pi->fcs = L2CAP_FCS_NONE;
1780                         pi->conf_state |= L2CAP_CONF_MODE_DONE;
1781                         break;
1782
1783                 case L2CAP_MODE_ERTM:
1784                         pi->remote_tx_win = rfc.txwin_size;
1785                         pi->remote_max_tx = rfc.max_transmit;
1786
1787                         if (le16_to_cpu(rfc.max_pdu_size) > pi->conn->mtu - 10)
1788                                 rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10);
1789
1790                         pi->remote_mps = le16_to_cpu(rfc.max_pdu_size);
1791
1792                         rfc.retrans_timeout =
1793                                 le16_to_cpu(L2CAP_DEFAULT_RETRANS_TO);
1794                         rfc.monitor_timeout =
1795                                 le16_to_cpu(L2CAP_DEFAULT_MONITOR_TO);
1796
1797                         pi->conf_state |= L2CAP_CONF_MODE_DONE;
1798
1799                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
1800                                         sizeof(rfc), (unsigned long) &rfc);
1801
1802                         break;
1803
1804                 case L2CAP_MODE_STREAMING:
1805                         if (le16_to_cpu(rfc.max_pdu_size) > pi->conn->mtu - 10)
1806                                 rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10);
1807
1808                         pi->remote_mps = le16_to_cpu(rfc.max_pdu_size);
1809
1810                         pi->conf_state |= L2CAP_CONF_MODE_DONE;
1811
1812                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
1813                                         sizeof(rfc), (unsigned long) &rfc);
1814
1815                         break;
1816
1817                 default:
1818                         result = L2CAP_CONF_UNACCEPT;
1819
1820                         memset(&rfc, 0, sizeof(rfc));
1821                         rfc.mode = pi->mode;
1822                 }
1823
1824                 if (result == L2CAP_CONF_SUCCESS)
1825                         pi->conf_state |= L2CAP_CONF_OUTPUT_DONE;
1826         }
1827         rsp->scid   = cpu_to_le16(pi->dcid);
1828         rsp->result = cpu_to_le16(result);
1829         rsp->flags  = cpu_to_le16(0x0000);
1830
1831         return ptr - data;
1832 }
1833
1834 static int l2cap_parse_conf_rsp(struct sock *sk, void *rsp, int len, void *data, u16 *result)
1835 {
1836         struct l2cap_pinfo *pi = l2cap_pi(sk);
1837         struct l2cap_conf_req *req = data;
1838         void *ptr = req->data;
1839         int type, olen;
1840         unsigned long val;
1841         struct l2cap_conf_rfc rfc;
1842
1843         BT_DBG("sk %p, rsp %p, len %d, req %p", sk, rsp, len, data);
1844
1845         while (len >= L2CAP_CONF_OPT_SIZE) {
1846                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
1847
1848                 switch (type) {
1849                 case L2CAP_CONF_MTU:
1850                         if (val < L2CAP_DEFAULT_MIN_MTU) {
1851                                 *result = L2CAP_CONF_UNACCEPT;
1852                                 pi->imtu = L2CAP_DEFAULT_MIN_MTU;
1853                         } else
1854                                 pi->imtu = val;
1855                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->imtu);
1856                         break;
1857
1858                 case L2CAP_CONF_FLUSH_TO:
1859                         pi->flush_to = val;
1860                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
1861                                                         2, pi->flush_to);
1862                         break;
1863
1864                 case L2CAP_CONF_RFC:
1865                         if (olen == sizeof(rfc))
1866                                 memcpy(&rfc, (void *)val, olen);
1867
1868                         if ((pi->conf_state & L2CAP_CONF_STATE2_DEVICE) &&
1869                                                         rfc.mode != pi->mode)
1870                                 return -ECONNREFUSED;
1871
1872                         pi->fcs = 0;
1873
1874                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
1875                                         sizeof(rfc), (unsigned long) &rfc);
1876                         break;
1877                 }
1878         }
1879
1880         if (pi->mode == L2CAP_MODE_BASIC && pi->mode != rfc.mode)
1881                 return -ECONNREFUSED;
1882
1883         pi->mode = rfc.mode;
1884
1885         if (*result == L2CAP_CONF_SUCCESS) {
1886                 switch (rfc.mode) {
1887                 case L2CAP_MODE_ERTM:
1888                         pi->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
1889                         pi->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
1890                         pi->mps    = le16_to_cpu(rfc.max_pdu_size);
1891                         break;
1892                 case L2CAP_MODE_STREAMING:
1893                         pi->mps    = le16_to_cpu(rfc.max_pdu_size);
1894                 }
1895         }
1896
1897         req->dcid   = cpu_to_le16(pi->dcid);
1898         req->flags  = cpu_to_le16(0x0000);
1899
1900         return ptr - data;
1901 }
1902
1903 static int l2cap_build_conf_rsp(struct sock *sk, void *data, u16 result, u16 flags)
1904 {
1905         struct l2cap_conf_rsp *rsp = data;
1906         void *ptr = rsp->data;
1907
1908         BT_DBG("sk %p", sk);
1909
1910         rsp->scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
1911         rsp->result = cpu_to_le16(result);
1912         rsp->flags  = cpu_to_le16(flags);
1913
1914         return ptr - data;
1915 }
1916
1917 static void l2cap_conf_rfc_get(struct sock *sk, void *rsp, int len)
1918 {
1919         struct l2cap_pinfo *pi = l2cap_pi(sk);
1920         int type, olen;
1921         unsigned long val;
1922         struct l2cap_conf_rfc rfc;
1923
1924         BT_DBG("sk %p, rsp %p, len %d", sk, rsp, len);
1925
1926         if ((pi->mode != L2CAP_MODE_ERTM) && (pi->mode != L2CAP_MODE_STREAMING))
1927                 return;
1928
1929         while (len >= L2CAP_CONF_OPT_SIZE) {
1930                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
1931
1932                 switch (type) {
1933                 case L2CAP_CONF_RFC:
1934                         if (olen == sizeof(rfc))
1935                                 memcpy(&rfc, (void *)val, olen);
1936                         goto done;
1937                 }
1938         }
1939
1940 done:
1941         switch (rfc.mode) {
1942         case L2CAP_MODE_ERTM:
1943                 pi->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
1944                 pi->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
1945                 pi->mps    = le16_to_cpu(rfc.max_pdu_size);
1946                 break;
1947         case L2CAP_MODE_STREAMING:
1948                 pi->mps    = le16_to_cpu(rfc.max_pdu_size);
1949         }
1950 }
1951
1952 static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1953 {
1954         struct l2cap_cmd_rej *rej = (struct l2cap_cmd_rej *) data;
1955
1956         if (rej->reason != 0x0000)
1957                 return 0;
1958
1959         if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
1960                                         cmd->ident == conn->info_ident) {
1961                 del_timer(&conn->info_timer);
1962
1963                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
1964                 conn->info_ident = 0;
1965
1966                 l2cap_conn_start(conn);
1967         }
1968
1969         return 0;
1970 }
1971
1972 static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1973 {
1974         struct l2cap_chan_list *list = &conn->chan_list;
1975         struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
1976         struct l2cap_conn_rsp rsp;
1977         struct sock *parent, *sk = NULL;
1978         int result, status = L2CAP_CS_NO_INFO;
1979
1980         u16 dcid = 0, scid = __le16_to_cpu(req->scid);
1981         __le16 psm = req->psm;
1982
1983         BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid);
1984
1985         /* Check if we have socket listening on psm */
1986         parent = l2cap_get_sock_by_psm(BT_LISTEN, psm, conn->src);
1987         if (!parent) {
1988                 result = L2CAP_CR_BAD_PSM;
1989                 goto sendresp;
1990         }
1991
1992         bh_lock_sock(parent);
1993
1994         /* Check if the ACL is secure enough (if not SDP) */
1995         if (psm != cpu_to_le16(0x0001) &&
1996                                 !hci_conn_check_link_mode(conn->hcon)) {
1997                 conn->disc_reason = 0x05;
1998                 result = L2CAP_CR_SEC_BLOCK;
1999                 goto response;
2000         }
2001
2002         result = L2CAP_CR_NO_MEM;
2003
2004         /* Check for backlog size */
2005         if (sk_acceptq_is_full(parent)) {
2006                 BT_DBG("backlog full %d", parent->sk_ack_backlog);
2007                 goto response;
2008         }
2009
2010         sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, GFP_ATOMIC);
2011         if (!sk)
2012                 goto response;
2013
2014         write_lock_bh(&list->lock);
2015
2016         /* Check if we already have channel with that dcid */
2017         if (__l2cap_get_chan_by_dcid(list, scid)) {
2018                 write_unlock_bh(&list->lock);
2019                 sock_set_flag(sk, SOCK_ZAPPED);
2020                 l2cap_sock_kill(sk);
2021                 goto response;
2022         }
2023
2024         hci_conn_hold(conn->hcon);
2025
2026         l2cap_sock_init(sk, parent);
2027         bacpy(&bt_sk(sk)->src, conn->src);
2028         bacpy(&bt_sk(sk)->dst, conn->dst);
2029         l2cap_pi(sk)->psm  = psm;
2030         l2cap_pi(sk)->dcid = scid;
2031
2032         bt_accept_enqueue(parent, sk);
2033
2034         __l2cap_chan_add(conn, sk);
2035         dcid = l2cap_pi(sk)->scid;
2036
2037         l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
2038
2039         l2cap_pi(sk)->ident = cmd->ident;
2040
2041         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
2042                 if (l2cap_check_security(sk)) {
2043                         if (bt_sk(sk)->defer_setup) {
2044                                 sk->sk_state = BT_CONNECT2;
2045                                 result = L2CAP_CR_PEND;
2046                                 status = L2CAP_CS_AUTHOR_PEND;
2047                                 parent->sk_data_ready(parent, 0);
2048                         } else {
2049                                 sk->sk_state = BT_CONFIG;
2050                                 result = L2CAP_CR_SUCCESS;
2051                                 status = L2CAP_CS_NO_INFO;
2052                         }
2053                 } else {
2054                         sk->sk_state = BT_CONNECT2;
2055                         result = L2CAP_CR_PEND;
2056                         status = L2CAP_CS_AUTHEN_PEND;
2057                 }
2058         } else {
2059                 sk->sk_state = BT_CONNECT2;
2060                 result = L2CAP_CR_PEND;
2061                 status = L2CAP_CS_NO_INFO;
2062         }
2063
2064         write_unlock_bh(&list->lock);
2065
2066 response:
2067         bh_unlock_sock(parent);
2068
2069 sendresp:
2070         rsp.scid   = cpu_to_le16(scid);
2071         rsp.dcid   = cpu_to_le16(dcid);
2072         rsp.result = cpu_to_le16(result);
2073         rsp.status = cpu_to_le16(status);
2074         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
2075
2076         if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
2077                 struct l2cap_info_req info;
2078                 info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
2079
2080                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
2081                 conn->info_ident = l2cap_get_ident(conn);
2082
2083                 mod_timer(&conn->info_timer, jiffies +
2084                                         msecs_to_jiffies(L2CAP_INFO_TIMEOUT));
2085
2086                 l2cap_send_cmd(conn, conn->info_ident,
2087                                         L2CAP_INFO_REQ, sizeof(info), &info);
2088         }
2089
2090         if (sk && !(l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT) &&
2091                                 result == L2CAP_CR_SUCCESS) {
2092                 u8 buf[128];
2093                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
2094                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2095                                         l2cap_build_conf_req(sk, buf), buf);
2096                 l2cap_pi(sk)->num_conf_req++;
2097         }
2098
2099         return 0;
2100 }
2101
2102 static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2103 {
2104         struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
2105         u16 scid, dcid, result, status;
2106         struct sock *sk;
2107         u8 req[128];
2108
2109         scid   = __le16_to_cpu(rsp->scid);
2110         dcid   = __le16_to_cpu(rsp->dcid);
2111         result = __le16_to_cpu(rsp->result);
2112         status = __le16_to_cpu(rsp->status);
2113
2114         BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status);
2115
2116         if (scid) {
2117                 sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
2118                 if (!sk)
2119                         return -EFAULT;
2120         } else {
2121                 sk = l2cap_get_chan_by_ident(&conn->chan_list, cmd->ident);
2122                 if (!sk)
2123                         return -EFAULT;
2124         }
2125
2126         switch (result) {
2127         case L2CAP_CR_SUCCESS:
2128                 sk->sk_state = BT_CONFIG;
2129                 l2cap_pi(sk)->ident = 0;
2130                 l2cap_pi(sk)->dcid = dcid;
2131                 l2cap_pi(sk)->conf_state &= ~L2CAP_CONF_CONNECT_PEND;
2132
2133                 if (l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT)
2134                         break;
2135
2136                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
2137
2138                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2139                                         l2cap_build_conf_req(sk, req), req);
2140                 l2cap_pi(sk)->num_conf_req++;
2141                 break;
2142
2143         case L2CAP_CR_PEND:
2144                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND;
2145                 break;
2146
2147         default:
2148                 /* don't delete l2cap channel if sk is owned by user */
2149                 if (sock_owned_by_user(sk)) {
2150                         sk->sk_state = BT_DISCONN;
2151                         l2cap_sock_clear_timer(sk);
2152                         l2cap_sock_set_timer(sk, HZ / 5);
2153                         break;
2154                 }
2155
2156                 l2cap_chan_del(sk, ECONNREFUSED);
2157                 break;
2158         }
2159
2160         bh_unlock_sock(sk);
2161         return 0;
2162 }
2163
2164 static inline void set_default_fcs(struct l2cap_pinfo *pi)
2165 {
2166         /* FCS is enabled only in ERTM or streaming mode, if one or both
2167          * sides request it.
2168          */
2169         if (pi->mode != L2CAP_MODE_ERTM && pi->mode != L2CAP_MODE_STREAMING)
2170                 pi->fcs = L2CAP_FCS_NONE;
2171         else if (!(pi->conf_state & L2CAP_CONF_NO_FCS_RECV))
2172                 pi->fcs = L2CAP_FCS_CRC16;
2173 }
2174
2175 static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
2176 {
2177         struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
2178         u16 dcid, flags;
2179         u8 rsp[64];
2180         struct sock *sk;
2181         int len;
2182
2183         dcid  = __le16_to_cpu(req->dcid);
2184         flags = __le16_to_cpu(req->flags);
2185
2186         BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
2187
2188         sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid);
2189         if (!sk)
2190                 return -ENOENT;
2191
2192         if (sk->sk_state != BT_CONFIG) {
2193                 struct l2cap_cmd_rej rej;
2194
2195                 rej.reason = cpu_to_le16(0x0002);
2196                 l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
2197                                 sizeof(rej), &rej);
2198                 goto unlock;
2199         }
2200
2201         /* Reject if config buffer is too small. */
2202         len = cmd_len - sizeof(*req);
2203         if (l2cap_pi(sk)->conf_len + len > sizeof(l2cap_pi(sk)->conf_req)) {
2204                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2205                                 l2cap_build_conf_rsp(sk, rsp,
2206                                         L2CAP_CONF_REJECT, flags), rsp);
2207                 goto unlock;
2208         }
2209
2210         /* Store config. */
2211         memcpy(l2cap_pi(sk)->conf_req + l2cap_pi(sk)->conf_len, req->data, len);
2212         l2cap_pi(sk)->conf_len += len;
2213
2214         if (flags & 0x0001) {
2215                 /* Incomplete config. Send empty response. */
2216                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2217                                 l2cap_build_conf_rsp(sk, rsp,
2218                                         L2CAP_CONF_SUCCESS, 0x0001), rsp);
2219                 goto unlock;
2220         }
2221
2222         /* Complete config. */
2223         len = l2cap_parse_conf_req(sk, rsp);
2224         if (len < 0) {
2225                 l2cap_send_disconn_req(conn, sk, ECONNRESET);
2226                 goto unlock;
2227         }
2228
2229         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
2230         l2cap_pi(sk)->num_conf_rsp++;
2231
2232         /* Reset config buffer. */
2233         l2cap_pi(sk)->conf_len = 0;
2234
2235         if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE))
2236                 goto unlock;
2237
2238         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_INPUT_DONE) {
2239                 set_default_fcs(l2cap_pi(sk));
2240
2241                 sk->sk_state = BT_CONNECTED;
2242
2243                 l2cap_pi(sk)->next_tx_seq = 0;
2244                 l2cap_pi(sk)->expected_tx_seq = 0;
2245                 __skb_queue_head_init(TX_QUEUE(sk));
2246                 if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM)
2247                         l2cap_ertm_init(sk);
2248
2249                 l2cap_chan_ready(sk);
2250                 goto unlock;
2251         }
2252
2253         if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT)) {
2254                 u8 buf[64];
2255                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
2256                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2257                                         l2cap_build_conf_req(sk, buf), buf);
2258                 l2cap_pi(sk)->num_conf_req++;
2259         }
2260
2261 unlock:
2262         bh_unlock_sock(sk);
2263         return 0;
2264 }
2265
2266 static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2267 {
2268         struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
2269         u16 scid, flags, result;
2270         struct sock *sk;
2271         int len = cmd->len - sizeof(*rsp);
2272
2273         scid   = __le16_to_cpu(rsp->scid);
2274         flags  = __le16_to_cpu(rsp->flags);
2275         result = __le16_to_cpu(rsp->result);
2276
2277         BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x",
2278                         scid, flags, result);
2279
2280         sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
2281         if (!sk)
2282                 return 0;
2283
2284         switch (result) {
2285         case L2CAP_CONF_SUCCESS:
2286                 l2cap_conf_rfc_get(sk, rsp->data, len);
2287                 break;
2288
2289         case L2CAP_CONF_UNACCEPT:
2290                 if (l2cap_pi(sk)->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
2291                         char req[64];
2292
2293                         if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
2294                                 l2cap_send_disconn_req(conn, sk, ECONNRESET);
2295                                 goto done;
2296                         }
2297
2298                         /* throw out any old stored conf requests */
2299                         result = L2CAP_CONF_SUCCESS;
2300                         len = l2cap_parse_conf_rsp(sk, rsp->data,
2301                                                         len, req, &result);
2302                         if (len < 0) {
2303                                 l2cap_send_disconn_req(conn, sk, ECONNRESET);
2304                                 goto done;
2305                         }
2306
2307                         l2cap_send_cmd(conn, l2cap_get_ident(conn),
2308                                                 L2CAP_CONF_REQ, len, req);
2309                         l2cap_pi(sk)->num_conf_req++;
2310                         if (result != L2CAP_CONF_SUCCESS)
2311                                 goto done;
2312                         break;
2313                 }
2314
2315         default:
2316                 sk->sk_err = ECONNRESET;
2317                 l2cap_sock_set_timer(sk, HZ * 5);
2318                 l2cap_send_disconn_req(conn, sk, ECONNRESET);
2319                 goto done;
2320         }
2321
2322         if (flags & 0x01)
2323                 goto done;
2324
2325         l2cap_pi(sk)->conf_state |= L2CAP_CONF_INPUT_DONE;
2326
2327         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE) {
2328                 set_default_fcs(l2cap_pi(sk));
2329
2330                 sk->sk_state = BT_CONNECTED;
2331                 l2cap_pi(sk)->next_tx_seq = 0;
2332                 l2cap_pi(sk)->expected_tx_seq = 0;
2333                 __skb_queue_head_init(TX_QUEUE(sk));
2334                 if (l2cap_pi(sk)->mode ==  L2CAP_MODE_ERTM)
2335                         l2cap_ertm_init(sk);
2336
2337                 l2cap_chan_ready(sk);
2338         }
2339
2340 done:
2341         bh_unlock_sock(sk);
2342         return 0;
2343 }
2344
2345 static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2346 {
2347         struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
2348         struct l2cap_disconn_rsp rsp;
2349         u16 dcid, scid;
2350         struct sock *sk;
2351
2352         scid = __le16_to_cpu(req->scid);
2353         dcid = __le16_to_cpu(req->dcid);
2354
2355         BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
2356
2357         sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid);
2358         if (!sk)
2359                 return 0;
2360
2361         rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid);
2362         rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid);
2363         l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
2364
2365         sk->sk_shutdown = SHUTDOWN_MASK;
2366
2367         /* don't delete l2cap channel if sk is owned by user */
2368         if (sock_owned_by_user(sk)) {
2369                 sk->sk_state = BT_DISCONN;
2370                 l2cap_sock_clear_timer(sk);
2371                 l2cap_sock_set_timer(sk, HZ / 5);
2372                 bh_unlock_sock(sk);
2373                 return 0;
2374         }
2375
2376         l2cap_chan_del(sk, ECONNRESET);
2377         bh_unlock_sock(sk);
2378
2379         l2cap_sock_kill(sk);
2380         return 0;
2381 }
2382
2383 static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2384 {
2385         struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
2386         u16 dcid, scid;
2387         struct sock *sk;
2388
2389         scid = __le16_to_cpu(rsp->scid);
2390         dcid = __le16_to_cpu(rsp->dcid);
2391
2392         BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
2393
2394         sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
2395         if (!sk)
2396                 return 0;
2397
2398         /* don't delete l2cap channel if sk is owned by user */
2399         if (sock_owned_by_user(sk)) {
2400                 sk->sk_state = BT_DISCONN;
2401                 l2cap_sock_clear_timer(sk);
2402                 l2cap_sock_set_timer(sk, HZ / 5);
2403                 bh_unlock_sock(sk);
2404                 return 0;
2405         }
2406
2407         l2cap_chan_del(sk, 0);
2408         bh_unlock_sock(sk);
2409
2410         l2cap_sock_kill(sk);
2411         return 0;
2412 }
2413
2414 static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2415 {
2416         struct l2cap_info_req *req = (struct l2cap_info_req *) data;
2417         u16 type;
2418
2419         type = __le16_to_cpu(req->type);
2420
2421         BT_DBG("type 0x%4.4x", type);
2422
2423         if (type == L2CAP_IT_FEAT_MASK) {
2424                 u8 buf[8];
2425                 u32 feat_mask = l2cap_feat_mask;
2426                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
2427                 rsp->type   = cpu_to_le16(L2CAP_IT_FEAT_MASK);
2428                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
2429                 if (!disable_ertm)
2430                         feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
2431                                                          | L2CAP_FEAT_FCS;
2432                 put_unaligned_le32(feat_mask, rsp->data);
2433                 l2cap_send_cmd(conn, cmd->ident,
2434                                         L2CAP_INFO_RSP, sizeof(buf), buf);
2435         } else if (type == L2CAP_IT_FIXED_CHAN) {
2436                 u8 buf[12];
2437                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
2438                 rsp->type   = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
2439                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
2440                 memcpy(buf + 4, l2cap_fixed_chan, 8);
2441                 l2cap_send_cmd(conn, cmd->ident,
2442                                         L2CAP_INFO_RSP, sizeof(buf), buf);
2443         } else {
2444                 struct l2cap_info_rsp rsp;
2445                 rsp.type   = cpu_to_le16(type);
2446                 rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP);
2447                 l2cap_send_cmd(conn, cmd->ident,
2448                                         L2CAP_INFO_RSP, sizeof(rsp), &rsp);
2449         }
2450
2451         return 0;
2452 }
2453
2454 static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2455 {
2456         struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
2457         u16 type, result;
2458
2459         type   = __le16_to_cpu(rsp->type);
2460         result = __le16_to_cpu(rsp->result);
2461
2462         BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
2463
2464         del_timer(&conn->info_timer);
2465
2466         if (result != L2CAP_IR_SUCCESS) {
2467                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2468                 conn->info_ident = 0;
2469
2470                 l2cap_conn_start(conn);
2471
2472                 return 0;
2473         }
2474
2475         if (type == L2CAP_IT_FEAT_MASK) {
2476                 conn->feat_mask = get_unaligned_le32(rsp->data);
2477
2478                 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
2479                         struct l2cap_info_req req;
2480                         req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
2481
2482                         conn->info_ident = l2cap_get_ident(conn);
2483
2484                         l2cap_send_cmd(conn, conn->info_ident,
2485                                         L2CAP_INFO_REQ, sizeof(req), &req);
2486                 } else {
2487                         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2488                         conn->info_ident = 0;
2489
2490                         l2cap_conn_start(conn);
2491                 }
2492         } else if (type == L2CAP_IT_FIXED_CHAN) {
2493                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2494                 conn->info_ident = 0;
2495
2496                 l2cap_conn_start(conn);
2497         }
2498
2499         return 0;
2500 }
2501
2502 static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency,
2503                                                         u16 to_multiplier)
2504 {
2505         u16 max_latency;
2506
2507         if (min > max || min < 6 || max > 3200)
2508                 return -EINVAL;
2509
2510         if (to_multiplier < 10 || to_multiplier > 3200)
2511                 return -EINVAL;
2512
2513         if (max >= to_multiplier * 8)
2514                 return -EINVAL;
2515
2516         max_latency = (to_multiplier * 8 / max) - 1;
2517         if (latency > 499 || latency > max_latency)
2518                 return -EINVAL;
2519
2520         return 0;
2521 }
2522
2523 static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
2524                                         struct l2cap_cmd_hdr *cmd, u8 *data)
2525 {
2526         struct hci_conn *hcon = conn->hcon;
2527         struct l2cap_conn_param_update_req *req;
2528         struct l2cap_conn_param_update_rsp rsp;
2529         u16 min, max, latency, to_multiplier, cmd_len;
2530         int err;
2531
2532         if (!(hcon->link_mode & HCI_LM_MASTER))
2533                 return -EINVAL;
2534
2535         cmd_len = __le16_to_cpu(cmd->len);
2536         if (cmd_len != sizeof(struct l2cap_conn_param_update_req))
2537                 return -EPROTO;
2538
2539         req = (struct l2cap_conn_param_update_req *) data;
2540         min             = __le16_to_cpu(req->min);
2541         max             = __le16_to_cpu(req->max);
2542         latency         = __le16_to_cpu(req->latency);
2543         to_multiplier   = __le16_to_cpu(req->to_multiplier);
2544
2545         BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x",
2546                                                 min, max, latency, to_multiplier);
2547
2548         memset(&rsp, 0, sizeof(rsp));
2549
2550         err = l2cap_check_conn_param(min, max, latency, to_multiplier);
2551         if (err)
2552                 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_REJECTED);
2553         else
2554                 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED);
2555
2556         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP,
2557                                                         sizeof(rsp), &rsp);
2558
2559         if (!err)
2560                 hci_le_conn_update(hcon, min, max, latency, to_multiplier);
2561
2562         return 0;
2563 }
2564
2565 static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
2566                         struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
2567 {
2568         int err = 0;
2569
2570         switch (cmd->code) {
2571         case L2CAP_COMMAND_REJ:
2572                 l2cap_command_rej(conn, cmd, data);
2573                 break;
2574
2575         case L2CAP_CONN_REQ:
2576                 err = l2cap_connect_req(conn, cmd, data);
2577                 break;
2578
2579         case L2CAP_CONN_RSP:
2580                 err = l2cap_connect_rsp(conn, cmd, data);
2581                 break;
2582
2583         case L2CAP_CONF_REQ:
2584                 err = l2cap_config_req(conn, cmd, cmd_len, data);
2585                 break;
2586
2587         case L2CAP_CONF_RSP:
2588                 err = l2cap_config_rsp(conn, cmd, data);
2589                 break;
2590
2591         case L2CAP_DISCONN_REQ:
2592                 err = l2cap_disconnect_req(conn, cmd, data);
2593                 break;
2594
2595         case L2CAP_DISCONN_RSP:
2596                 err = l2cap_disconnect_rsp(conn, cmd, data);
2597                 break;
2598
2599         case L2CAP_ECHO_REQ:
2600                 l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data);
2601                 break;
2602
2603         case L2CAP_ECHO_RSP:
2604                 break;
2605
2606         case L2CAP_INFO_REQ:
2607                 err = l2cap_information_req(conn, cmd, data);
2608                 break;
2609
2610         case L2CAP_INFO_RSP:
2611                 err = l2cap_information_rsp(conn, cmd, data);
2612                 break;
2613
2614         default:
2615                 BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code);
2616                 err = -EINVAL;
2617                 break;
2618         }
2619
2620         return err;
2621 }
2622
2623 static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
2624                                         struct l2cap_cmd_hdr *cmd, u8 *data)
2625 {
2626         switch (cmd->code) {
2627         case L2CAP_COMMAND_REJ:
2628                 return 0;
2629
2630         case L2CAP_CONN_PARAM_UPDATE_REQ:
2631                 return l2cap_conn_param_update_req(conn, cmd, data);
2632
2633         case L2CAP_CONN_PARAM_UPDATE_RSP:
2634                 return 0;
2635
2636         default:
2637                 BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code);
2638                 return -EINVAL;
2639         }
2640 }
2641
2642 static inline void l2cap_sig_channel(struct l2cap_conn *conn,
2643                                                         struct sk_buff *skb)
2644 {
2645         u8 *data = skb->data;
2646         int len = skb->len;
2647         struct l2cap_cmd_hdr cmd;
2648         int err;
2649
2650         l2cap_raw_recv(conn, skb);
2651
2652         while (len >= L2CAP_CMD_HDR_SIZE) {
2653                 u16 cmd_len;
2654                 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
2655                 data += L2CAP_CMD_HDR_SIZE;
2656                 len  -= L2CAP_CMD_HDR_SIZE;
2657
2658                 cmd_len = le16_to_cpu(cmd.len);
2659
2660                 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, cmd.ident);
2661
2662                 if (cmd_len > len || !cmd.ident) {
2663                         BT_DBG("corrupted command");
2664                         break;
2665                 }
2666
2667                 if (conn->hcon->type == LE_LINK)
2668                         err = l2cap_le_sig_cmd(conn, &cmd, data);
2669                 else
2670                         err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data);
2671
2672                 if (err) {
2673                         struct l2cap_cmd_rej rej;
2674
2675                         BT_ERR("Wrong link type (%d)", err);
2676
2677                         /* FIXME: Map err to a valid reason */
2678                         rej.reason = cpu_to_le16(0);
2679                         l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
2680                 }
2681
2682                 data += cmd_len;
2683                 len  -= cmd_len;
2684         }
2685
2686         kfree_skb(skb);
2687 }
2688
2689 static int l2cap_check_fcs(struct l2cap_pinfo *pi,  struct sk_buff *skb)
2690 {
2691         u16 our_fcs, rcv_fcs;
2692         int hdr_size = L2CAP_HDR_SIZE + 2;
2693
2694         if (pi->fcs == L2CAP_FCS_CRC16) {
2695                 skb_trim(skb, skb->len - 2);
2696                 rcv_fcs = get_unaligned_le16(skb->data + skb->len);
2697                 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
2698
2699                 if (our_fcs != rcv_fcs)
2700                         return -EBADMSG;
2701         }
2702         return 0;
2703 }
2704
2705 static inline void l2cap_send_i_or_rr_or_rnr(struct sock *sk)
2706 {
2707         struct l2cap_pinfo *pi = l2cap_pi(sk);
2708         u16 control = 0;
2709
2710         pi->frames_sent = 0;
2711
2712         control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
2713
2714         if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
2715                 control |= L2CAP_SUPER_RCV_NOT_READY;
2716                 l2cap_send_sframe(pi, control);
2717                 pi->conn_state |= L2CAP_CONN_RNR_SENT;
2718         }
2719
2720         if (pi->conn_state & L2CAP_CONN_REMOTE_BUSY)
2721                 l2cap_retransmit_frames(sk);
2722
2723         l2cap_ertm_send(sk);
2724
2725         if (!(pi->conn_state & L2CAP_CONN_LOCAL_BUSY) &&
2726                         pi->frames_sent == 0) {
2727                 control |= L2CAP_SUPER_RCV_READY;
2728                 l2cap_send_sframe(pi, control);
2729         }
2730 }
2731
2732 static int l2cap_add_to_srej_queue(struct sock *sk, struct sk_buff *skb, u8 tx_seq, u8 sar)
2733 {
2734         struct sk_buff *next_skb;
2735         struct l2cap_pinfo *pi = l2cap_pi(sk);
2736         int tx_seq_offset, next_tx_seq_offset;
2737
2738         bt_cb(skb)->tx_seq = tx_seq;
2739         bt_cb(skb)->sar = sar;
2740
2741         next_skb = skb_peek(SREJ_QUEUE(sk));
2742         if (!next_skb) {
2743                 __skb_queue_tail(SREJ_QUEUE(sk), skb);
2744                 return 0;
2745         }
2746
2747         tx_seq_offset = (tx_seq - pi->buffer_seq) % 64;
2748         if (tx_seq_offset < 0)
2749                 tx_seq_offset += 64;
2750
2751         do {
2752                 if (bt_cb(next_skb)->tx_seq == tx_seq)
2753                         return -EINVAL;
2754
2755                 next_tx_seq_offset = (bt_cb(next_skb)->tx_seq -
2756                                                 pi->buffer_seq) % 64;
2757                 if (next_tx_seq_offset < 0)
2758                         next_tx_seq_offset += 64;
2759
2760                 if (next_tx_seq_offset > tx_seq_offset) {
2761                         __skb_queue_before(SREJ_QUEUE(sk), next_skb, skb);
2762                         return 0;
2763                 }
2764
2765                 if (skb_queue_is_last(SREJ_QUEUE(sk), next_skb))
2766                         break;
2767
2768         } while ((next_skb = skb_queue_next(SREJ_QUEUE(sk), next_skb)));
2769
2770         __skb_queue_tail(SREJ_QUEUE(sk), skb);
2771
2772         return 0;
2773 }
2774
2775 static int l2cap_ertm_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)
2776 {
2777         struct l2cap_pinfo *pi = l2cap_pi(sk);
2778         struct sk_buff *_skb;
2779         int err;
2780
2781         switch (control & L2CAP_CTRL_SAR) {
2782         case L2CAP_SDU_UNSEGMENTED:
2783                 if (pi->conn_state & L2CAP_CONN_SAR_SDU)
2784                         goto drop;
2785
2786                 err = sock_queue_rcv_skb(sk, skb);
2787                 if (!err)
2788                         return err;
2789
2790                 break;
2791
2792         case L2CAP_SDU_START:
2793                 if (pi->conn_state & L2CAP_CONN_SAR_SDU)
2794                         goto drop;
2795
2796                 pi->sdu_len = get_unaligned_le16(skb->data);
2797
2798                 if (pi->sdu_len > pi->imtu)
2799                         goto disconnect;
2800
2801                 pi->sdu = bt_skb_alloc(pi->sdu_len, GFP_ATOMIC);
2802                 if (!pi->sdu)
2803                         return -ENOMEM;
2804
2805                 /* pull sdu_len bytes only after alloc, because of Local Busy
2806                  * condition we have to be sure that this will be executed
2807                  * only once, i.e., when alloc does not fail */
2808                 skb_pull(skb, 2);
2809
2810                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
2811
2812                 pi->conn_state |= L2CAP_CONN_SAR_SDU;
2813                 pi->partial_sdu_len = skb->len;
2814                 break;
2815
2816         case L2CAP_SDU_CONTINUE:
2817                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
2818                         goto disconnect;
2819
2820                 if (!pi->sdu)
2821                         goto disconnect;
2822
2823                 pi->partial_sdu_len += skb->len;
2824                 if (pi->partial_sdu_len > pi->sdu_len)
2825                         goto drop;
2826
2827                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
2828
2829                 break;
2830
2831         case L2CAP_SDU_END:
2832                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
2833                         goto disconnect;
2834
2835                 if (!pi->sdu)
2836                         goto disconnect;
2837
2838                 if (!(pi->conn_state & L2CAP_CONN_SAR_RETRY)) {
2839                         pi->partial_sdu_len += skb->len;
2840
2841                         if (pi->partial_sdu_len > pi->imtu)
2842                                 goto drop;
2843
2844                         if (pi->partial_sdu_len != pi->sdu_len)
2845                                 goto drop;
2846
2847                         memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
2848                 }
2849
2850                 _skb = skb_clone(pi->sdu, GFP_ATOMIC);
2851                 if (!_skb) {
2852                         pi->conn_state |= L2CAP_CONN_SAR_RETRY;
2853                         return -ENOMEM;
2854                 }
2855
2856                 err = sock_queue_rcv_skb(sk, _skb);
2857                 if (err < 0) {
2858                         kfree_skb(_skb);
2859                         pi->conn_state |= L2CAP_CONN_SAR_RETRY;
2860                         return err;
2861                 }
2862
2863                 pi->conn_state &= ~L2CAP_CONN_SAR_RETRY;
2864                 pi->conn_state &= ~L2CAP_CONN_SAR_SDU;
2865
2866                 kfree_skb(pi->sdu);
2867                 break;
2868         }
2869
2870         kfree_skb(skb);
2871         return 0;
2872
2873 drop:
2874         kfree_skb(pi->sdu);
2875         pi->sdu = NULL;
2876
2877 disconnect:
2878         l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
2879         kfree_skb(skb);
2880         return 0;
2881 }
2882
2883 static int l2cap_try_push_rx_skb(struct sock *sk)
2884 {
2885         struct l2cap_pinfo *pi = l2cap_pi(sk);
2886         struct sk_buff *skb;
2887         u16 control;
2888         int err;
2889
2890         while ((skb = skb_dequeue(BUSY_QUEUE(sk)))) {
2891                 control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
2892                 err = l2cap_ertm_reassembly_sdu(sk, skb, control);
2893                 if (err < 0) {
2894                         skb_queue_head(BUSY_QUEUE(sk), skb);
2895                         return -EBUSY;
2896                 }
2897
2898                 pi->buffer_seq = (pi->buffer_seq + 1) % 64;
2899         }
2900
2901         if (!(pi->conn_state & L2CAP_CONN_RNR_SENT))
2902                 goto done;
2903
2904         control = pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
2905         control |= L2CAP_SUPER_RCV_READY | L2CAP_CTRL_POLL;
2906         l2cap_send_sframe(pi, control);
2907         l2cap_pi(sk)->retry_count = 1;
2908
2909         del_timer(&pi->retrans_timer);
2910         __mod_monitor_timer();
2911
2912         l2cap_pi(sk)->conn_state |= L2CAP_CONN_WAIT_F;
2913
2914 done:
2915         pi->conn_state &= ~L2CAP_CONN_LOCAL_BUSY;
2916         pi->conn_state &= ~L2CAP_CONN_RNR_SENT;
2917
2918         BT_DBG("sk %p, Exit local busy", sk);
2919
2920         return 0;
2921 }
2922
2923 static void l2cap_busy_work(struct work_struct *work)
2924 {
2925         DECLARE_WAITQUEUE(wait, current);
2926         struct l2cap_pinfo *pi =
2927                 container_of(work, struct l2cap_pinfo, busy_work);
2928         struct sock *sk = (struct sock *)pi;
2929         int n_tries = 0, timeo = HZ/5, err;
2930         struct sk_buff *skb;
2931
2932         lock_sock(sk);
2933
2934         add_wait_queue(sk_sleep(sk), &wait);
2935         while ((skb = skb_peek(BUSY_QUEUE(sk)))) {
2936                 set_current_state(TASK_INTERRUPTIBLE);
2937
2938                 if (n_tries++ > L2CAP_LOCAL_BUSY_TRIES) {
2939                         err = -EBUSY;
2940                         l2cap_send_disconn_req(pi->conn, sk, EBUSY);
2941                         break;
2942                 }
2943
2944                 if (!timeo)
2945                         timeo = HZ/5;
2946
2947                 if (signal_pending(current)) {
2948                         err = sock_intr_errno(timeo);
2949                         break;
2950                 }
2951
2952                 release_sock(sk);
2953                 timeo = schedule_timeout(timeo);
2954                 lock_sock(sk);
2955
2956                 err = sock_error(sk);
2957                 if (err)
2958                         break;
2959
2960                 if (l2cap_try_push_rx_skb(sk) == 0)
2961                         break;
2962         }
2963
2964         set_current_state(TASK_RUNNING);
2965         remove_wait_queue(sk_sleep(sk), &wait);
2966
2967         release_sock(sk);
2968 }
2969
2970 static int l2cap_push_rx_skb(struct sock *sk, struct sk_buff *skb, u16 control)
2971 {
2972         struct l2cap_pinfo *pi = l2cap_pi(sk);
2973         int sctrl, err;
2974
2975         if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
2976                 bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT;
2977                 __skb_queue_tail(BUSY_QUEUE(sk), skb);
2978                 return l2cap_try_push_rx_skb(sk);
2979
2980
2981         }
2982
2983         err = l2cap_ertm_reassembly_sdu(sk, skb, control);
2984         if (err >= 0) {
2985                 pi->buffer_seq = (pi->buffer_seq + 1) % 64;
2986                 return err;
2987         }
2988
2989         /* Busy Condition */
2990         BT_DBG("sk %p, Enter local busy", sk);
2991
2992         pi->conn_state |= L2CAP_CONN_LOCAL_BUSY;
2993         bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT;
2994         __skb_queue_tail(BUSY_QUEUE(sk), skb);
2995
2996         sctrl = pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
2997         sctrl |= L2CAP_SUPER_RCV_NOT_READY;
2998         l2cap_send_sframe(pi, sctrl);
2999
3000         pi->conn_state |= L2CAP_CONN_RNR_SENT;
3001
3002         del_timer(&pi->ack_timer);
3003
3004         queue_work(_busy_wq, &pi->busy_work);
3005
3006         return err;
3007 }
3008
3009 static int l2cap_streaming_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)
3010 {
3011         struct l2cap_pinfo *pi = l2cap_pi(sk);
3012         struct sk_buff *_skb;
3013         int err = -EINVAL;
3014
3015         /*
3016          * TODO: We have to notify the userland if some data is lost with the
3017          * Streaming Mode.
3018          */
3019
3020         switch (control & L2CAP_CTRL_SAR) {
3021         case L2CAP_SDU_UNSEGMENTED:
3022                 if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
3023                         kfree_skb(pi->sdu);
3024                         break;
3025                 }
3026
3027                 err = sock_queue_rcv_skb(sk, skb);
3028                 if (!err)
3029                         return 0;
3030
3031                 break;
3032
3033         case L2CAP_SDU_START:
3034                 if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
3035                         kfree_skb(pi->sdu);
3036                         break;
3037                 }
3038
3039                 pi->sdu_len = get_unaligned_le16(skb->data);
3040                 skb_pull(skb, 2);
3041
3042                 if (pi->sdu_len > pi->imtu) {
3043                         err = -EMSGSIZE;
3044                         break;
3045                 }
3046
3047                 pi->sdu = bt_skb_alloc(pi->sdu_len, GFP_ATOMIC);
3048                 if (!pi->sdu) {
3049                         err = -ENOMEM;
3050                         break;
3051                 }
3052
3053                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3054
3055                 pi->conn_state |= L2CAP_CONN_SAR_SDU;
3056                 pi->partial_sdu_len = skb->len;
3057                 err = 0;
3058                 break;
3059
3060         case L2CAP_SDU_CONTINUE:
3061                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
3062                         break;
3063
3064                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3065
3066                 pi->partial_sdu_len += skb->len;
3067                 if (pi->partial_sdu_len > pi->sdu_len)
3068                         kfree_skb(pi->sdu);
3069                 else
3070                         err = 0;
3071
3072                 break;
3073
3074         case L2CAP_SDU_END:
3075                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
3076                         break;
3077
3078                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3079
3080                 pi->conn_state &= ~L2CAP_CONN_SAR_SDU;
3081                 pi->partial_sdu_len += skb->len;
3082
3083                 if (pi->partial_sdu_len > pi->imtu)
3084                         goto drop;
3085
3086                 if (pi->partial_sdu_len == pi->sdu_len) {
3087                         _skb = skb_clone(pi->sdu, GFP_ATOMIC);
3088                         err = sock_queue_rcv_skb(sk, _skb);
3089                         if (err < 0)
3090                                 kfree_skb(_skb);
3091                 }
3092                 err = 0;
3093
3094 drop:
3095                 kfree_skb(pi->sdu);
3096                 break;
3097         }
3098
3099         kfree_skb(skb);
3100         return err;
3101 }
3102
3103 static void l2cap_check_srej_gap(struct sock *sk, u8 tx_seq)
3104 {
3105         struct sk_buff *skb;
3106         u16 control;
3107
3108         while ((skb = skb_peek(SREJ_QUEUE(sk)))) {
3109                 if (bt_cb(skb)->tx_seq != tx_seq)
3110                         break;
3111
3112                 skb = skb_dequeue(SREJ_QUEUE(sk));
3113                 control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
3114                 l2cap_ertm_reassembly_sdu(sk, skb, control);
3115                 l2cap_pi(sk)->buffer_seq_srej =
3116                         (l2cap_pi(sk)->buffer_seq_srej + 1) % 64;
3117                 tx_seq = (tx_seq + 1) % 64;
3118         }
3119 }
3120
3121 static void l2cap_resend_srejframe(struct sock *sk, u8 tx_seq)
3122 {
3123         struct l2cap_pinfo *pi = l2cap_pi(sk);
3124         struct srej_list *l, *tmp;
3125         u16 control;
3126
3127         list_for_each_entry_safe(l, tmp, SREJ_LIST(sk), list) {
3128                 if (l->tx_seq == tx_seq) {
3129                         list_del(&l->list);
3130                         kfree(l);
3131                         return;
3132                 }
3133                 control = L2CAP_SUPER_SELECT_REJECT;
3134                 control |= l->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3135                 l2cap_send_sframe(pi, control);
3136                 list_del(&l->list);
3137                 list_add_tail(&l->list, SREJ_LIST(sk));
3138         }
3139 }
3140
3141 static void l2cap_send_srejframe(struct sock *sk, u8 tx_seq)
3142 {
3143         struct l2cap_pinfo *pi = l2cap_pi(sk);
3144         struct srej_list *new;
3145         u16 control;
3146
3147         while (tx_seq != pi->expected_tx_seq) {
3148                 control = L2CAP_SUPER_SELECT_REJECT;
3149                 control |= pi->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3150                 l2cap_send_sframe(pi, control);
3151
3152                 new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC);
3153                 new->tx_seq = pi->expected_tx_seq;
3154                 pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3155                 list_add_tail(&new->list, SREJ_LIST(sk));
3156         }
3157         pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3158 }
3159
3160 static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
3161 {
3162         struct l2cap_pinfo *pi = l2cap_pi(sk);
3163         u8 tx_seq = __get_txseq(rx_control);
3164         u8 req_seq = __get_reqseq(rx_control);
3165         u8 sar = rx_control >> L2CAP_CTRL_SAR_SHIFT;
3166         int tx_seq_offset, expected_tx_seq_offset;
3167         int num_to_ack = (pi->tx_win/6) + 1;
3168         int err = 0;
3169
3170         BT_DBG("sk %p len %d tx_seq %d rx_control 0x%4.4x", sk, skb->len, tx_seq,
3171                                                                 rx_control);
3172
3173         if (L2CAP_CTRL_FINAL & rx_control &&
3174                         l2cap_pi(sk)->conn_state & L2CAP_CONN_WAIT_F) {
3175                 del_timer(&pi->monitor_timer);
3176                 if (pi->unacked_frames > 0)
3177                         __mod_retrans_timer();
3178                 pi->conn_state &= ~L2CAP_CONN_WAIT_F;
3179         }
3180
3181         pi->expected_ack_seq = req_seq;
3182         l2cap_drop_acked_frames(sk);
3183
3184         if (tx_seq == pi->expected_tx_seq)
3185                 goto expected;
3186
3187         tx_seq_offset = (tx_seq - pi->buffer_seq) % 64;
3188         if (tx_seq_offset < 0)
3189                 tx_seq_offset += 64;
3190
3191         /* invalid tx_seq */
3192         if (tx_seq_offset >= pi->tx_win) {
3193                 l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
3194                 goto drop;
3195         }
3196
3197         if (pi->conn_state == L2CAP_CONN_LOCAL_BUSY)
3198                 goto drop;
3199
3200         if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
3201                 struct srej_list *first;
3202
3203                 first = list_first_entry(SREJ_LIST(sk),
3204                                 struct srej_list, list);
3205                 if (tx_seq == first->tx_seq) {
3206                         l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
3207                         l2cap_check_srej_gap(sk, tx_seq);
3208
3209                         list_del(&first->list);
3210                         kfree(first);
3211
3212                         if (list_empty(SREJ_LIST(sk))) {
3213                                 pi->buffer_seq = pi->buffer_seq_srej;
3214                                 pi->conn_state &= ~L2CAP_CONN_SREJ_SENT;
3215                                 l2cap_send_ack(pi);
3216                                 BT_DBG("sk %p, Exit SREJ_SENT", sk);
3217                         }
3218                 } else {
3219                         struct srej_list *l;
3220
3221                         /* duplicated tx_seq */
3222                         if (l2cap_add_to_srej_queue(sk, skb, tx_seq, sar) < 0)
3223                                 goto drop;
3224
3225                         list_for_each_entry(l, SREJ_LIST(sk), list) {
3226                                 if (l->tx_seq == tx_seq) {
3227                                         l2cap_resend_srejframe(sk, tx_seq);
3228                                         return 0;
3229                                 }
3230                         }
3231                         l2cap_send_srejframe(sk, tx_seq);
3232                 }
3233         } else {
3234                 expected_tx_seq_offset =
3235                         (pi->expected_tx_seq - pi->buffer_seq) % 64;
3236                 if (expected_tx_seq_offset < 0)
3237                         expected_tx_seq_offset += 64;
3238
3239                 /* duplicated tx_seq */
3240                 if (tx_seq_offset < expected_tx_seq_offset)
3241                         goto drop;
3242
3243                 pi->conn_state |= L2CAP_CONN_SREJ_SENT;
3244
3245                 BT_DBG("sk %p, Enter SREJ", sk);
3246
3247                 INIT_LIST_HEAD(SREJ_LIST(sk));
3248                 pi->buffer_seq_srej = pi->buffer_seq;
3249
3250                 __skb_queue_head_init(SREJ_QUEUE(sk));
3251                 __skb_queue_head_init(BUSY_QUEUE(sk));
3252                 l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
3253
3254                 pi->conn_state |= L2CAP_CONN_SEND_PBIT;
3255
3256                 l2cap_send_srejframe(sk, tx_seq);
3257
3258                 del_timer(&pi->ack_timer);
3259         }
3260         return 0;
3261
3262 expected:
3263         pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3264
3265         if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
3266                 bt_cb(skb)->tx_seq = tx_seq;
3267                 bt_cb(skb)->sar = sar;
3268                 __skb_queue_tail(SREJ_QUEUE(sk), skb);
3269                 return 0;
3270         }
3271
3272         err = l2cap_push_rx_skb(sk, skb, rx_control);
3273         if (err < 0)
3274                 return 0;
3275
3276         if (rx_control & L2CAP_CTRL_FINAL) {
3277                 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
3278                         pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
3279                 else
3280                         l2cap_retransmit_frames(sk);
3281         }
3282
3283         __mod_ack_timer();
3284
3285         pi->num_acked = (pi->num_acked + 1) % num_to_ack;
3286         if (pi->num_acked == num_to_ack - 1)
3287                 l2cap_send_ack(pi);
3288
3289         return 0;
3290
3291 drop:
3292         kfree_skb(skb);
3293         return 0;
3294 }
3295
3296 static inline void l2cap_data_channel_rrframe(struct sock *sk, u16 rx_control)
3297 {
3298         struct l2cap_pinfo *pi = l2cap_pi(sk);
3299
3300         BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, __get_reqseq(rx_control),
3301                                                 rx_control);
3302
3303         pi->expected_ack_seq = __get_reqseq(rx_control);
3304         l2cap_drop_acked_frames(sk);
3305
3306         if (rx_control & L2CAP_CTRL_POLL) {
3307                 pi->conn_state |= L2CAP_CONN_SEND_FBIT;
3308                 if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
3309                         if ((pi->conn_state & L2CAP_CONN_REMOTE_BUSY) &&
3310                                         (pi->unacked_frames > 0))
3311                                 __mod_retrans_timer();
3312
3313                         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3314                         l2cap_send_srejtail(sk);
3315                 } else {
3316                         l2cap_send_i_or_rr_or_rnr(sk);
3317                 }
3318
3319         } else if (rx_control & L2CAP_CTRL_FINAL) {
3320                 pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3321
3322                 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
3323                         pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
3324                 else
3325                         l2cap_retransmit_frames(sk);
3326
3327         } else {
3328                 if ((pi->conn_state & L2CAP_CONN_REMOTE_BUSY) &&
3329                                 (pi->unacked_frames > 0))
3330                         __mod_retrans_timer();
3331
3332                 pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3333                 if (pi->conn_state & L2CAP_CONN_SREJ_SENT)
3334                         l2cap_send_ack(pi);
3335                 else
3336                         l2cap_ertm_send(sk);
3337         }
3338 }
3339
3340 static inline void l2cap_data_channel_rejframe(struct sock *sk, u16 rx_control)
3341 {
3342         struct l2cap_pinfo *pi = l2cap_pi(sk);
3343         u8 tx_seq = __get_reqseq(rx_control);
3344
3345         BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, tx_seq, rx_control);
3346
3347         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3348
3349         pi->expected_ack_seq = tx_seq;
3350         l2cap_drop_acked_frames(sk);
3351
3352         if (rx_control & L2CAP_CTRL_FINAL) {
3353                 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
3354                         pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
3355                 else
3356                         l2cap_retransmit_frames(sk);
3357         } else {
3358                 l2cap_retransmit_frames(sk);
3359
3360                 if (pi->conn_state & L2CAP_CONN_WAIT_F)
3361                         pi->conn_state |= L2CAP_CONN_REJ_ACT;
3362         }
3363 }
3364 static inline void l2cap_data_channel_srejframe(struct sock *sk, u16 rx_control)
3365 {
3366         struct l2cap_pinfo *pi = l2cap_pi(sk);
3367         u8 tx_seq = __get_reqseq(rx_control);
3368
3369         BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, tx_seq, rx_control);
3370
3371         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3372
3373         if (rx_control & L2CAP_CTRL_POLL) {
3374                 pi->expected_ack_seq = tx_seq;
3375                 l2cap_drop_acked_frames(sk);
3376
3377                 pi->conn_state |= L2CAP_CONN_SEND_FBIT;
3378                 l2cap_retransmit_one_frame(sk, tx_seq);
3379
3380                 l2cap_ertm_send(sk);
3381
3382                 if (pi->conn_state & L2CAP_CONN_WAIT_F) {
3383                         pi->srej_save_reqseq = tx_seq;
3384                         pi->conn_state |= L2CAP_CONN_SREJ_ACT;
3385                 }
3386         } else if (rx_control & L2CAP_CTRL_FINAL) {
3387                 if ((pi->conn_state & L2CAP_CONN_SREJ_ACT) &&
3388                                 pi->srej_save_reqseq == tx_seq)
3389                         pi->conn_state &= ~L2CAP_CONN_SREJ_ACT;
3390                 else
3391                         l2cap_retransmit_one_frame(sk, tx_seq);
3392         } else {
3393                 l2cap_retransmit_one_frame(sk, tx_seq);
3394                 if (pi->conn_state & L2CAP_CONN_WAIT_F) {
3395                         pi->srej_save_reqseq = tx_seq;
3396                         pi->conn_state |= L2CAP_CONN_SREJ_ACT;
3397                 }
3398         }
3399 }
3400
3401 static inline void l2cap_data_channel_rnrframe(struct sock *sk, u16 rx_control)
3402 {
3403         struct l2cap_pinfo *pi = l2cap_pi(sk);
3404         u8 tx_seq = __get_reqseq(rx_control);
3405
3406         BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, tx_seq, rx_control);
3407
3408         pi->conn_state |= L2CAP_CONN_REMOTE_BUSY;
3409         pi->expected_ack_seq = tx_seq;
3410         l2cap_drop_acked_frames(sk);
3411
3412         if (rx_control & L2CAP_CTRL_POLL)
3413                 pi->conn_state |= L2CAP_CONN_SEND_FBIT;
3414
3415         if (!(pi->conn_state & L2CAP_CONN_SREJ_SENT)) {
3416                 del_timer(&pi->retrans_timer);
3417                 if (rx_control & L2CAP_CTRL_POLL)
3418                         l2cap_send_rr_or_rnr(pi, L2CAP_CTRL_FINAL);
3419                 return;
3420         }
3421
3422         if (rx_control & L2CAP_CTRL_POLL)
3423                 l2cap_send_srejtail(sk);
3424         else
3425                 l2cap_send_sframe(pi, L2CAP_SUPER_RCV_READY);
3426 }
3427
3428 static inline int l2cap_data_channel_sframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
3429 {
3430         BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
3431
3432         if (L2CAP_CTRL_FINAL & rx_control &&
3433                         l2cap_pi(sk)->conn_state & L2CAP_CONN_WAIT_F) {
3434                 del_timer(&l2cap_pi(sk)->monitor_timer);
3435                 if (l2cap_pi(sk)->unacked_frames > 0)
3436                         __mod_retrans_timer();
3437                 l2cap_pi(sk)->conn_state &= ~L2CAP_CONN_WAIT_F;
3438         }
3439
3440         switch (rx_control & L2CAP_CTRL_SUPERVISE) {
3441         case L2CAP_SUPER_RCV_READY:
3442                 l2cap_data_channel_rrframe(sk, rx_control);
3443                 break;
3444
3445         case L2CAP_SUPER_REJECT:
3446                 l2cap_data_channel_rejframe(sk, rx_control);
3447                 break;
3448
3449         case L2CAP_SUPER_SELECT_REJECT:
3450                 l2cap_data_channel_srejframe(sk, rx_control);
3451                 break;
3452
3453         case L2CAP_SUPER_RCV_NOT_READY:
3454                 l2cap_data_channel_rnrframe(sk, rx_control);
3455                 break;
3456         }
3457
3458         kfree_skb(skb);
3459         return 0;
3460 }
3461
3462 static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb)
3463 {
3464         struct l2cap_pinfo *pi = l2cap_pi(sk);
3465         u16 control;
3466         u8 req_seq;
3467         int len, next_tx_seq_offset, req_seq_offset;
3468
3469         control = get_unaligned_le16(skb->data);
3470         skb_pull(skb, 2);
3471         len = skb->len;
3472
3473         /*
3474          * We can just drop the corrupted I-frame here.
3475          * Receiver will miss it and start proper recovery
3476          * procedures and ask retransmission.
3477          */
3478         if (l2cap_check_fcs(pi, skb))
3479                 goto drop;
3480
3481         if (__is_sar_start(control) && __is_iframe(control))
3482                 len -= 2;
3483
3484         if (pi->fcs == L2CAP_FCS_CRC16)
3485                 len -= 2;
3486
3487         if (len > pi->mps) {
3488                 l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
3489                 goto drop;
3490         }
3491
3492         req_seq = __get_reqseq(control);
3493         req_seq_offset = (req_seq - pi->expected_ack_seq) % 64;
3494         if (req_seq_offset < 0)
3495                 req_seq_offset += 64;
3496
3497         next_tx_seq_offset =
3498                 (pi->next_tx_seq - pi->expected_ack_seq) % 64;
3499         if (next_tx_seq_offset < 0)
3500                 next_tx_seq_offset += 64;
3501
3502         /* check for invalid req-seq */
3503         if (req_seq_offset > next_tx_seq_offset) {
3504                 l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
3505                 goto drop;
3506         }
3507
3508         if (__is_iframe(control)) {
3509                 if (len < 0) {
3510                         l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
3511                         goto drop;
3512                 }
3513
3514                 l2cap_data_channel_iframe(sk, control, skb);
3515         } else {
3516                 if (len != 0) {
3517                         BT_ERR("%d", len);
3518                         l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
3519                         goto drop;
3520                 }
3521
3522                 l2cap_data_channel_sframe(sk, control, skb);
3523         }
3524
3525         return 0;
3526
3527 drop:
3528         kfree_skb(skb);
3529         return 0;
3530 }
3531
3532 static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
3533 {
3534         struct sock *sk;
3535         struct l2cap_pinfo *pi;
3536         u16 control;
3537         u8 tx_seq;
3538         int len;
3539
3540         sk = l2cap_get_chan_by_scid(&conn->chan_list, cid);
3541         if (!sk) {
3542                 BT_DBG("unknown cid 0x%4.4x", cid);
3543                 goto drop;
3544         }
3545
3546         pi = l2cap_pi(sk);
3547
3548         BT_DBG("sk %p, len %d", sk, skb->len);
3549
3550         if (sk->sk_state != BT_CONNECTED)
3551                 goto drop;
3552
3553         switch (pi->mode) {
3554         case L2CAP_MODE_BASIC:
3555                 /* If socket recv buffers overflows we drop data here
3556                  * which is *bad* because L2CAP has to be reliable.
3557                  * But we don't have any other choice. L2CAP doesn't
3558                  * provide flow control mechanism. */
3559
3560                 if (pi->imtu < skb->len)
3561                         goto drop;
3562
3563                 if (!sock_queue_rcv_skb(sk, skb))
3564                         goto done;
3565                 break;
3566
3567         case L2CAP_MODE_ERTM:
3568                 if (!sock_owned_by_user(sk)) {
3569                         l2cap_ertm_data_rcv(sk, skb);
3570                 } else {
3571                         if (sk_add_backlog(sk, skb))
3572                                 goto drop;
3573                 }
3574
3575                 goto done;
3576
3577         case L2CAP_MODE_STREAMING:
3578                 control = get_unaligned_le16(skb->data);
3579                 skb_pull(skb, 2);
3580                 len = skb->len;
3581
3582                 if (l2cap_check_fcs(pi, skb))
3583                         goto drop;
3584
3585                 if (__is_sar_start(control))
3586                         len -= 2;
3587
3588                 if (pi->fcs == L2CAP_FCS_CRC16)
3589                         len -= 2;
3590
3591                 if (len > pi->mps || len < 0 || __is_sframe(control))
3592                         goto drop;
3593
3594                 tx_seq = __get_txseq(control);
3595
3596                 if (pi->expected_tx_seq == tx_seq)
3597                         pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3598                 else
3599                         pi->expected_tx_seq = (tx_seq + 1) % 64;
3600
3601                 l2cap_streaming_reassembly_sdu(sk, skb, control);
3602
3603                 goto done;
3604
3605         default:
3606                 BT_DBG("sk %p: bad mode 0x%2.2x", sk, pi->mode);
3607                 break;
3608         }
3609
3610 drop:
3611         kfree_skb(skb);
3612
3613 done:
3614         if (sk)
3615                 bh_unlock_sock(sk);
3616
3617         return 0;
3618 }
3619
3620 static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb)
3621 {
3622         struct sock *sk;
3623
3624         sk = l2cap_get_sock_by_psm(0, psm, conn->src);
3625         if (!sk)
3626                 goto drop;
3627
3628         bh_lock_sock(sk);
3629
3630         BT_DBG("sk %p, len %d", sk, skb->len);
3631
3632         if (sk->sk_state != BT_BOUND && sk->sk_state != BT_CONNECTED)
3633                 goto drop;
3634
3635         if (l2cap_pi(sk)->imtu < skb->len)
3636                 goto drop;
3637
3638         if (!sock_queue_rcv_skb(sk, skb))
3639                 goto done;
3640
3641 drop:
3642         kfree_skb(skb);
3643
3644 done:
3645         if (sk)
3646                 bh_unlock_sock(sk);
3647         return 0;
3648 }
3649
3650 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
3651 {
3652         struct l2cap_hdr *lh = (void *) skb->data;
3653         u16 cid, len;
3654         __le16 psm;
3655
3656         skb_pull(skb, L2CAP_HDR_SIZE);
3657         cid = __le16_to_cpu(lh->cid);
3658         len = __le16_to_cpu(lh->len);
3659
3660         if (len != skb->len) {
3661                 kfree_skb(skb);
3662                 return;
3663         }
3664
3665         BT_DBG("len %d, cid 0x%4.4x", len, cid);
3666
3667         switch (cid) {
3668         case L2CAP_CID_LE_SIGNALING:
3669         case L2CAP_CID_SIGNALING:
3670                 l2cap_sig_channel(conn, skb);
3671                 break;
3672
3673         case L2CAP_CID_CONN_LESS:
3674                 psm = get_unaligned_le16(skb->data);
3675                 skb_pull(skb, 2);
3676                 l2cap_conless_channel(conn, psm, skb);
3677                 break;
3678
3679         default:
3680                 l2cap_data_channel(conn, cid, skb);
3681                 break;
3682         }
3683 }
3684
3685 /* ---- L2CAP interface with lower layer (HCI) ---- */
3686
3687 static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
3688 {
3689         int exact = 0, lm1 = 0, lm2 = 0;
3690         register struct sock *sk;
3691         struct hlist_node *node;
3692
3693         if (type != ACL_LINK)
3694                 return -EINVAL;
3695
3696         BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
3697
3698         /* Find listening sockets and check their link_mode */
3699         read_lock(&l2cap_sk_list.lock);
3700         sk_for_each(sk, node, &l2cap_sk_list.head) {
3701                 if (sk->sk_state != BT_LISTEN)
3702                         continue;
3703
3704                 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
3705                         lm1 |= HCI_LM_ACCEPT;
3706                         if (l2cap_pi(sk)->role_switch)
3707                                 lm1 |= HCI_LM_MASTER;
3708                         exact++;
3709                 } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
3710                         lm2 |= HCI_LM_ACCEPT;
3711                         if (l2cap_pi(sk)->role_switch)
3712                                 lm2 |= HCI_LM_MASTER;
3713                 }
3714         }
3715         read_unlock(&l2cap_sk_list.lock);
3716
3717         return exact ? lm1 : lm2;
3718 }
3719
3720 static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
3721 {
3722         struct l2cap_conn *conn;
3723
3724         BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
3725
3726         if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK))
3727                 return -EINVAL;
3728
3729         if (!status) {
3730                 conn = l2cap_conn_add(hcon, status);
3731                 if (conn)
3732                         l2cap_conn_ready(conn);
3733         } else
3734                 l2cap_conn_del(hcon, bt_err(status));
3735
3736         return 0;
3737 }
3738
3739 static int l2cap_disconn_ind(struct hci_conn *hcon)
3740 {
3741         struct l2cap_conn *conn = hcon->l2cap_data;
3742
3743         BT_DBG("hcon %p", hcon);
3744
3745         if (hcon->type != ACL_LINK || !conn)
3746                 return 0x13;
3747
3748         return conn->disc_reason;
3749 }
3750
3751 static int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
3752 {
3753         BT_DBG("hcon %p reason %d", hcon, reason);
3754
3755         if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK))
3756                 return -EINVAL;
3757
3758         l2cap_conn_del(hcon, bt_err(reason));
3759
3760         return 0;
3761 }
3762
3763 static inline void l2cap_check_encryption(struct sock *sk, u8 encrypt)
3764 {
3765         if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM)
3766                 return;
3767
3768         if (encrypt == 0x00) {
3769                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_MEDIUM) {
3770                         l2cap_sock_clear_timer(sk);
3771                         l2cap_sock_set_timer(sk, HZ * 5);
3772                 } else if (l2cap_pi(sk)->sec_level == BT_SECURITY_HIGH)
3773                         __l2cap_sock_close(sk, ECONNREFUSED);
3774         } else {
3775                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_MEDIUM)
3776                         l2cap_sock_clear_timer(sk);
3777         }
3778 }
3779
3780 static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
3781 {
3782         struct l2cap_chan_list *l;
3783         struct l2cap_conn *conn = hcon->l2cap_data;
3784         struct sock *sk;
3785
3786         if (!conn)
3787                 return 0;
3788
3789         l = &conn->chan_list;
3790
3791         BT_DBG("conn %p", conn);
3792
3793         read_lock(&l->lock);
3794
3795         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
3796                 bh_lock_sock(sk);
3797
3798                 if (l2cap_pi(sk)->conf_state & L2CAP_CONF_CONNECT_PEND) {
3799                         bh_unlock_sock(sk);
3800                         continue;
3801                 }
3802
3803                 if (!status && (sk->sk_state == BT_CONNECTED ||
3804                                                 sk->sk_state == BT_CONFIG)) {
3805                         l2cap_check_encryption(sk, encrypt);
3806                         bh_unlock_sock(sk);
3807                         continue;
3808                 }
3809
3810                 if (sk->sk_state == BT_CONNECT) {
3811                         if (!status) {
3812                                 struct l2cap_conn_req req;
3813                                 req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
3814                                 req.psm  = l2cap_pi(sk)->psm;
3815
3816                                 l2cap_pi(sk)->ident = l2cap_get_ident(conn);
3817                                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND;
3818
3819                                 l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
3820                                         L2CAP_CONN_REQ, sizeof(req), &req);
3821                         } else {
3822                                 l2cap_sock_clear_timer(sk);
3823                                 l2cap_sock_set_timer(sk, HZ / 10);
3824                         }
3825                 } else if (sk->sk_state == BT_CONNECT2) {
3826                         struct l2cap_conn_rsp rsp;
3827                         __u16 result;
3828
3829                         if (!status) {
3830                                 sk->sk_state = BT_CONFIG;
3831                                 result = L2CAP_CR_SUCCESS;
3832                         } else {
3833                                 sk->sk_state = BT_DISCONN;
3834                                 l2cap_sock_set_timer(sk, HZ / 10);
3835                                 result = L2CAP_CR_SEC_BLOCK;
3836                         }
3837
3838                         rsp.scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
3839                         rsp.dcid   = cpu_to_le16(l2cap_pi(sk)->scid);
3840                         rsp.result = cpu_to_le16(result);
3841                         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
3842                         l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
3843                                         L2CAP_CONN_RSP, sizeof(rsp), &rsp);
3844                 }
3845
3846                 bh_unlock_sock(sk);
3847         }
3848
3849         read_unlock(&l->lock);
3850
3851         return 0;
3852 }
3853
3854 static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
3855 {
3856         struct l2cap_conn *conn = hcon->l2cap_data;
3857
3858         if (!conn)
3859                 conn = l2cap_conn_add(hcon, 0);
3860
3861         if (!conn)
3862                 goto drop;
3863
3864         BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
3865
3866         if (!(flags & ACL_CONT)) {
3867                 struct l2cap_hdr *hdr;
3868                 struct sock *sk;
3869                 u16 cid;
3870                 int len;
3871
3872                 if (conn->rx_len) {
3873                         BT_ERR("Unexpected start frame (len %d)", skb->len);
3874                         kfree_skb(conn->rx_skb);
3875                         conn->rx_skb = NULL;
3876                         conn->rx_len = 0;
3877                         l2cap_conn_unreliable(conn, ECOMM);
3878                 }
3879
3880                 /* Start fragment always begin with Basic L2CAP header */
3881                 if (skb->len < L2CAP_HDR_SIZE) {
3882                         BT_ERR("Frame is too short (len %d)", skb->len);
3883                         l2cap_conn_unreliable(conn, ECOMM);
3884                         goto drop;
3885                 }
3886
3887                 hdr = (struct l2cap_hdr *) skb->data;
3888                 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
3889                 cid = __le16_to_cpu(hdr->cid);
3890
3891                 if (len == skb->len) {
3892                         /* Complete frame received */
3893                         l2cap_recv_frame(conn, skb);
3894                         return 0;
3895                 }
3896
3897                 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
3898
3899                 if (skb->len > len) {
3900                         BT_ERR("Frame is too long (len %d, expected len %d)",
3901                                 skb->len, len);
3902                         l2cap_conn_unreliable(conn, ECOMM);
3903                         goto drop;
3904                 }
3905
3906                 sk = l2cap_get_chan_by_scid(&conn->chan_list, cid);
3907
3908                 if (sk && l2cap_pi(sk)->imtu < len - L2CAP_HDR_SIZE) {
3909                         BT_ERR("Frame exceeding recv MTU (len %d, MTU %d)",
3910                                         len, l2cap_pi(sk)->imtu);
3911                         bh_unlock_sock(sk);
3912                         l2cap_conn_unreliable(conn, ECOMM);
3913                         goto drop;
3914                 }
3915
3916                 if (sk)
3917                         bh_unlock_sock(sk);
3918
3919                 /* Allocate skb for the complete frame (with header) */
3920                 conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC);
3921                 if (!conn->rx_skb)
3922                         goto drop;
3923
3924                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
3925                                                                 skb->len);
3926                 conn->rx_len = len - skb->len;
3927         } else {
3928                 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
3929
3930                 if (!conn->rx_len) {
3931                         BT_ERR("Unexpected continuation frame (len %d)", skb->len);
3932                         l2cap_conn_unreliable(conn, ECOMM);
3933                         goto drop;
3934                 }
3935
3936                 if (skb->len > conn->rx_len) {
3937                         BT_ERR("Fragment is too long (len %d, expected %d)",
3938                                         skb->len, conn->rx_len);
3939                         kfree_skb(conn->rx_skb);
3940                         conn->rx_skb = NULL;
3941                         conn->rx_len = 0;
3942                         l2cap_conn_unreliable(conn, ECOMM);
3943                         goto drop;
3944                 }
3945
3946                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
3947                                                                 skb->len);
3948                 conn->rx_len -= skb->len;
3949
3950                 if (!conn->rx_len) {
3951                         /* Complete frame received */
3952                         l2cap_recv_frame(conn, conn->rx_skb);
3953                         conn->rx_skb = NULL;
3954                 }
3955         }
3956
3957 drop:
3958         kfree_skb(skb);
3959         return 0;
3960 }
3961
3962 static int l2cap_debugfs_show(struct seq_file *f, void *p)
3963 {
3964         struct sock *sk;
3965         struct hlist_node *node;
3966
3967         read_lock_bh(&l2cap_sk_list.lock);
3968
3969         sk_for_each(sk, node, &l2cap_sk_list.head) {
3970                 struct l2cap_pinfo *pi = l2cap_pi(sk);
3971
3972                 seq_printf(f, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
3973                                         batostr(&bt_sk(sk)->src),
3974                                         batostr(&bt_sk(sk)->dst),
3975                                         sk->sk_state, __le16_to_cpu(pi->psm),
3976                                         pi->scid, pi->dcid,
3977                                         pi->imtu, pi->omtu, pi->sec_level,
3978                                         pi->mode);
3979         }
3980
3981         read_unlock_bh(&l2cap_sk_list.lock);
3982
3983         return 0;
3984 }
3985
3986 static int l2cap_debugfs_open(struct inode *inode, struct file *file)
3987 {
3988         return single_open(file, l2cap_debugfs_show, inode->i_private);
3989 }
3990
3991 static const struct file_operations l2cap_debugfs_fops = {
3992         .open           = l2cap_debugfs_open,
3993         .read           = seq_read,
3994         .llseek         = seq_lseek,
3995         .release        = single_release,
3996 };
3997
3998 static struct dentry *l2cap_debugfs;
3999
4000 static struct hci_proto l2cap_hci_proto = {
4001         .name           = "L2CAP",
4002         .id             = HCI_PROTO_L2CAP,
4003         .connect_ind    = l2cap_connect_ind,
4004         .connect_cfm    = l2cap_connect_cfm,
4005         .disconn_ind    = l2cap_disconn_ind,
4006         .disconn_cfm    = l2cap_disconn_cfm,
4007         .security_cfm   = l2cap_security_cfm,
4008         .recv_acldata   = l2cap_recv_acldata
4009 };
4010
4011 int __init l2cap_init(void)
4012 {
4013         int err;
4014
4015         err = l2cap_init_sockets();
4016         if (err < 0)
4017                 return err;
4018
4019         _busy_wq = create_singlethread_workqueue("l2cap");
4020         if (!_busy_wq) {
4021                 err = -ENOMEM;
4022                 goto error;
4023         }
4024
4025         err = hci_register_proto(&l2cap_hci_proto);
4026         if (err < 0) {
4027                 BT_ERR("L2CAP protocol registration failed");
4028                 bt_sock_unregister(BTPROTO_L2CAP);
4029                 goto error;
4030         }
4031
4032         if (bt_debugfs) {
4033                 l2cap_debugfs = debugfs_create_file("l2cap", 0444,
4034                                         bt_debugfs, NULL, &l2cap_debugfs_fops);
4035                 if (!l2cap_debugfs)
4036                         BT_ERR("Failed to create L2CAP debug file");
4037         }
4038
4039         return 0;
4040
4041 error:
4042         destroy_workqueue(_busy_wq);
4043         l2cap_cleanup_sockets();
4044         return err;
4045 }
4046
4047 void l2cap_exit(void)
4048 {
4049         debugfs_remove(l2cap_debugfs);
4050
4051         flush_workqueue(_busy_wq);
4052         destroy_workqueue(_busy_wq);
4053
4054         if (hci_unregister_proto(&l2cap_hci_proto) < 0)
4055                 BT_ERR("L2CAP protocol unregistration failed");
4056
4057         l2cap_cleanup_sockets();
4058 }
4059
4060 module_param(disable_ertm, bool, 0644);
4061 MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode");