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