net/bluetooth/l2cap.c

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