SAFE public projects git trees. - safe/jmp/linux-2.6/blob - 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_drop_acked_frames(struct sock *sk)
1182 {
1183         struct sk_buff *skb;
1184
1185         while ((skb = skb_peek(TX_QUEUE(sk)))) {
1186                 if (bt_cb(skb)->tx_seq == l2cap_pi(sk)->expected_ack_seq)
1187                         break;
1188
1189                 skb = skb_dequeue(TX_QUEUE(sk));
1190                 kfree_skb(skb);
1191
1192                 l2cap_pi(sk)->unacked_frames--;
1193         }
1194
1195         return;
1196 }
1197
1198 static inline int l2cap_do_send(struct sock *sk, struct sk_buff *skb)
1199 {
1200         struct l2cap_pinfo *pi = l2cap_pi(sk);
1201         int err;
1202
1203         BT_DBG("sk %p, skb %p len %d", sk, skb, skb->len);
1204
1205         err = hci_send_acl(pi->conn->hcon, skb, 0);
1206         if (err < 0)
1207                 kfree_skb(skb);
1208
1209         return err;
1210 }
1211
1212 static int l2cap_ertm_send(struct sock *sk)
1213 {
1214         struct sk_buff *skb, *tx_skb;
1215         struct l2cap_pinfo *pi = l2cap_pi(sk);
1216         u16 control;
1217         int err;
1218
1219         while ((skb = sk->sk_send_head) && (!l2cap_tx_window_full(sk))) {
1220                 tx_skb = skb_clone(skb, GFP_ATOMIC);
1221
1222                 control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE);
1223                 control |= (pi->req_seq << L2CAP_CTRL_REQSEQ_SHIFT)
1224                                 | (pi->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT);
1225                 put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);
1226
1227                 err = l2cap_do_send(sk, tx_skb);
1228                 if (err < 0) {
1229                         l2cap_send_disconn_req(pi->conn, sk);
1230                         return err;
1231                 }
1232
1233                 bt_cb(skb)->tx_seq = pi->next_tx_seq;
1234                 pi->next_tx_seq = (pi->next_tx_seq + 1) % 64;
1235
1236                 pi->unacked_frames++;
1237
1238                 if (skb_queue_is_last(TX_QUEUE(sk), skb))
1239                         sk->sk_send_head = NULL;
1240                 else
1241                         sk->sk_send_head = skb_queue_next(TX_QUEUE(sk), skb);
1242         }
1243
1244         return 0;
1245 }
1246
1247 static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, int len, int count, struct sk_buff *skb)
1248 {
1249         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1250         struct sk_buff **frag;
1251         int err, sent = 0;
1252
1253         if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count)) {
1254                 return -EFAULT;
1255         }
1256
1257         sent += count;
1258         len  -= count;
1259
1260         /* Continuation fragments (no L2CAP header) */
1261         frag = &skb_shinfo(skb)->frag_list;
1262         while (len) {
1263                 count = min_t(unsigned int, conn->mtu, len);
1264
1265                 *frag = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err);
1266                 if (!*frag)
1267                         return -EFAULT;
1268                 if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count))
1269                         return -EFAULT;
1270
1271                 sent += count;
1272                 len  -= count;
1273
1274                 frag = &(*frag)->next;
1275         }
1276
1277         return sent;
1278 }
1279
1280 static struct sk_buff *l2cap_create_connless_pdu(struct sock *sk, struct msghdr *msg, size_t len)
1281 {
1282         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1283         struct sk_buff *skb;
1284         int err, count, hlen = L2CAP_HDR_SIZE + 2;
1285         struct l2cap_hdr *lh;
1286
1287         BT_DBG("sk %p len %d", sk, (int)len);
1288
1289         count = min_t(unsigned int, (conn->mtu - hlen), len);
1290         skb = bt_skb_send_alloc(sk, count + hlen,
1291                         msg->msg_flags & MSG_DONTWAIT, &err);
1292         if (!skb)
1293                 return ERR_PTR(-ENOMEM);
1294
1295         /* Create L2CAP header */
1296         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1297         lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
1298         lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1299         put_unaligned_le16(l2cap_pi(sk)->psm, skb_put(skb, 2));
1300
1301         err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1302         if (unlikely(err < 0)) {
1303                 kfree_skb(skb);
1304                 return ERR_PTR(err);
1305         }
1306         return skb;
1307 }
1308
1309 static struct sk_buff *l2cap_create_basic_pdu(struct sock *sk, struct msghdr *msg, size_t len)
1310 {
1311         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1312         struct sk_buff *skb;
1313         int err, count, hlen = L2CAP_HDR_SIZE;
1314         struct l2cap_hdr *lh;
1315
1316         BT_DBG("sk %p len %d", sk, (int)len);
1317
1318         count = min_t(unsigned int, (conn->mtu - hlen), len);
1319         skb = bt_skb_send_alloc(sk, count + hlen,
1320                         msg->msg_flags & MSG_DONTWAIT, &err);
1321         if (!skb)
1322                 return ERR_PTR(-ENOMEM);
1323
1324         /* Create L2CAP header */
1325         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1326         lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
1327         lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1328
1329         err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1330         if (unlikely(err < 0)) {
1331                 kfree_skb(skb);
1332                 return ERR_PTR(err);
1333         }
1334         return skb;
1335 }
1336
1337 static struct sk_buff *l2cap_create_ertm_pdu(struct sock *sk, struct msghdr *msg, size_t len, u16 control, u16 sdulen)
1338 {
1339         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1340         struct sk_buff *skb;
1341         int err, count, hlen = L2CAP_HDR_SIZE + 2;
1342         struct l2cap_hdr *lh;
1343
1344         BT_DBG("sk %p len %d", sk, (int)len);
1345
1346         if (sdulen)
1347                 hlen += 2;
1348
1349         count = min_t(unsigned int, (conn->mtu - hlen), len);
1350         skb = bt_skb_send_alloc(sk, count + hlen,
1351                         msg->msg_flags & MSG_DONTWAIT, &err);
1352         if (!skb)
1353                 return ERR_PTR(-ENOMEM);
1354
1355         /* Create L2CAP header */
1356         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1357         lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
1358         lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1359         put_unaligned_le16(control, skb_put(skb, 2));
1360         if (sdulen)
1361                 put_unaligned_le16(sdulen, skb_put(skb, 2));
1362
1363         err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1364         if (unlikely(err < 0)) {
1365                 kfree_skb(skb);
1366                 return ERR_PTR(err);
1367         }
1368         return skb;
1369 }
1370
1371 static inline int l2cap_sar_segment_sdu(struct sock *sk, struct msghdr *msg, size_t len)
1372 {
1373         struct l2cap_pinfo *pi = l2cap_pi(sk);
1374         struct sk_buff *skb;
1375         struct sk_buff_head sar_queue;
1376         u16 control;
1377         size_t size = 0;
1378
1379         __skb_queue_head_init(&sar_queue);
1380         control = L2CAP_SDU_START;
1381         skb = l2cap_create_ertm_pdu(sk, msg, pi->max_pdu_size, control, len);
1382         if (IS_ERR(skb))
1383                 return PTR_ERR(skb);
1384
1385         __skb_queue_tail(&sar_queue, skb);
1386         len -= pi->max_pdu_size;
1387         size +=pi->max_pdu_size;
1388         control = 0;
1389
1390         while (len > 0) {
1391                 size_t buflen;
1392
1393                 if (len > pi->max_pdu_size) {
1394                         control |= L2CAP_SDU_CONTINUE;
1395                         buflen = pi->max_pdu_size;
1396                 } else {
1397                         control |= L2CAP_SDU_END;
1398                         buflen = len;
1399                 }
1400
1401                 skb = l2cap_create_ertm_pdu(sk, msg, buflen, control, 0);
1402                 if (IS_ERR(skb)) {
1403                         skb_queue_purge(&sar_queue);
1404                         return PTR_ERR(skb);
1405                 }
1406
1407                 __skb_queue_tail(&sar_queue, skb);
1408                 len -= buflen;
1409                 size += buflen;
1410                 control = 0;
1411         }
1412         skb_queue_splice_tail(&sar_queue, TX_QUEUE(sk));
1413         if (sk->sk_send_head == NULL)
1414                 sk->sk_send_head = sar_queue.next;
1415
1416         return size;
1417 }
1418
1419 static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len)
1420 {
1421         struct sock *sk = sock->sk;
1422         struct l2cap_pinfo *pi = l2cap_pi(sk);
1423         struct sk_buff *skb;
1424         u16 control;
1425         int err;
1426
1427         BT_DBG("sock %p, sk %p", sock, sk);
1428
1429         err = sock_error(sk);
1430         if (err)
1431                 return err;
1432
1433         if (msg->msg_flags & MSG_OOB)
1434                 return -EOPNOTSUPP;
1435
1436         /* Check outgoing MTU */
1437         if (sk->sk_type == SOCK_SEQPACKET && pi->mode == L2CAP_MODE_BASIC
1438                         && len > pi->omtu)
1439                 return -EINVAL;
1440
1441         lock_sock(sk);
1442
1443         if (sk->sk_state != BT_CONNECTED) {
1444                 err = -ENOTCONN;
1445                 goto done;
1446         }
1447
1448         /* Connectionless channel */
1449         if (sk->sk_type == SOCK_DGRAM) {
1450                 skb = l2cap_create_connless_pdu(sk, msg, len);
1451                 err = l2cap_do_send(sk, skb);
1452                 goto done;
1453         }
1454
1455         switch (pi->mode) {
1456         case L2CAP_MODE_BASIC:
1457                 /* Create a basic PDU */
1458                 skb = l2cap_create_basic_pdu(sk, msg, len);
1459                 if (IS_ERR(skb)) {
1460                         err = PTR_ERR(skb);
1461                         goto done;
1462                 }
1463
1464                 err = l2cap_do_send(sk, skb);
1465                 if (!err)
1466                         err = len;
1467                 break;
1468
1469         case L2CAP_MODE_ERTM:
1470                 /* Entire SDU fits into one PDU */
1471                 if (len <= pi->max_pdu_size) {
1472                         control = L2CAP_SDU_UNSEGMENTED;
1473                         skb = l2cap_create_ertm_pdu(sk, msg, len, control, 0);
1474                         if (IS_ERR(skb)) {
1475                                 err = PTR_ERR(skb);
1476                                 goto done;
1477                         }
1478                         __skb_queue_tail(TX_QUEUE(sk), skb);
1479                         if (sk->sk_send_head == NULL)
1480                                 sk->sk_send_head = skb;
1481                 } else {
1482                 /* Segment SDU into multiples PDUs */
1483                         err = l2cap_sar_segment_sdu(sk, msg, len);
1484                         if (err < 0)
1485                                 goto done;
1486                 }
1487
1488                 err = l2cap_ertm_send(sk);
1489                 if (!err)
1490                         err = len;
1491                 break;
1492
1493         default:
1494                 BT_DBG("bad state %1.1x", pi->mode);
1495                 err = -EINVAL;
1496         }
1497
1498 done:
1499         release_sock(sk);
1500         return err;
1501 }
1502
1503 static int l2cap_sock_recvmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len, int flags)
1504 {
1505         struct sock *sk = sock->sk;
1506
1507         lock_sock(sk);
1508
1509         if (sk->sk_state == BT_CONNECT2 && bt_sk(sk)->defer_setup) {
1510                 struct l2cap_conn_rsp rsp;
1511
1512                 sk->sk_state = BT_CONFIG;
1513
1514                 rsp.scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
1515                 rsp.dcid   = cpu_to_le16(l2cap_pi(sk)->scid);
1516                 rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
1517                 rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
1518                 l2cap_send_cmd(l2cap_pi(sk)->conn, l2cap_pi(sk)->ident,
1519                                         L2CAP_CONN_RSP, sizeof(rsp), &rsp);
1520
1521                 release_sock(sk);
1522                 return 0;
1523         }
1524
1525         release_sock(sk);
1526
1527         return bt_sock_recvmsg(iocb, sock, msg, len, flags);
1528 }
1529
1530 static int l2cap_sock_setsockopt_old(struct socket *sock, int optname, char __user *optval, int optlen)
1531 {
1532         struct sock *sk = sock->sk;
1533         struct l2cap_options opts;
1534         int len, err = 0;
1535         u32 opt;
1536
1537         BT_DBG("sk %p", sk);
1538
1539         lock_sock(sk);
1540
1541         switch (optname) {
1542         case L2CAP_OPTIONS:
1543                 opts.imtu     = l2cap_pi(sk)->imtu;
1544                 opts.omtu     = l2cap_pi(sk)->omtu;
1545                 opts.flush_to = l2cap_pi(sk)->flush_to;
1546                 opts.mode     = l2cap_pi(sk)->mode;
1547
1548                 len = min_t(unsigned int, sizeof(opts), optlen);
1549                 if (copy_from_user((char *) &opts, optval, len)) {
1550                         err = -EFAULT;
1551                         break;
1552                 }
1553
1554                 l2cap_pi(sk)->imtu = opts.imtu;
1555                 l2cap_pi(sk)->omtu = opts.omtu;
1556                 l2cap_pi(sk)->mode = opts.mode;
1557                 break;
1558
1559         case L2CAP_LM:
1560                 if (get_user(opt, (u32 __user *) optval)) {
1561                         err = -EFAULT;
1562                         break;
1563                 }
1564
1565                 if (opt & L2CAP_LM_AUTH)
1566                         l2cap_pi(sk)->sec_level = BT_SECURITY_LOW;
1567                 if (opt & L2CAP_LM_ENCRYPT)
1568                         l2cap_pi(sk)->sec_level = BT_SECURITY_MEDIUM;
1569                 if (opt & L2CAP_LM_SECURE)
1570                         l2cap_pi(sk)->sec_level = BT_SECURITY_HIGH;
1571
1572                 l2cap_pi(sk)->role_switch    = (opt & L2CAP_LM_MASTER);
1573                 l2cap_pi(sk)->force_reliable = (opt & L2CAP_LM_RELIABLE);
1574                 break;
1575
1576         default:
1577                 err = -ENOPROTOOPT;
1578                 break;
1579         }
1580
1581         release_sock(sk);
1582         return err;
1583 }
1584
1585 static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname, char __user *optval, int optlen)
1586 {
1587         struct sock *sk = sock->sk;
1588         struct bt_security sec;
1589         int len, err = 0;
1590         u32 opt;
1591
1592         BT_DBG("sk %p", sk);
1593
1594         if (level == SOL_L2CAP)
1595                 return l2cap_sock_setsockopt_old(sock, optname, optval, optlen);
1596
1597         if (level != SOL_BLUETOOTH)
1598                 return -ENOPROTOOPT;
1599
1600         lock_sock(sk);
1601
1602         switch (optname) {
1603         case BT_SECURITY:
1604                 if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_RAW) {
1605                         err = -EINVAL;
1606                         break;
1607                 }
1608
1609                 sec.level = BT_SECURITY_LOW;
1610
1611                 len = min_t(unsigned int, sizeof(sec), optlen);
1612                 if (copy_from_user((char *) &sec, optval, len)) {
1613                         err = -EFAULT;
1614                         break;
1615                 }
1616
1617                 if (sec.level < BT_SECURITY_LOW ||
1618                                         sec.level > BT_SECURITY_HIGH) {
1619                         err = -EINVAL;
1620                         break;
1621                 }
1622
1623                 l2cap_pi(sk)->sec_level = sec.level;
1624                 break;
1625
1626         case BT_DEFER_SETUP:
1627                 if (sk->sk_state != BT_BOUND && sk->sk_state != BT_LISTEN) {
1628                         err = -EINVAL;
1629                         break;
1630                 }
1631
1632                 if (get_user(opt, (u32 __user *) optval)) {
1633                         err = -EFAULT;
1634                         break;
1635                 }
1636
1637                 bt_sk(sk)->defer_setup = opt;
1638                 break;
1639
1640         default:
1641                 err = -ENOPROTOOPT;
1642                 break;
1643         }
1644
1645         release_sock(sk);
1646         return err;
1647 }
1648
1649 static int l2cap_sock_getsockopt_old(struct socket *sock, int optname, char __user *optval, int __user *optlen)
1650 {
1651         struct sock *sk = sock->sk;
1652         struct l2cap_options opts;
1653         struct l2cap_conninfo cinfo;
1654         int len, err = 0;
1655         u32 opt;
1656
1657         BT_DBG("sk %p", sk);
1658
1659         if (get_user(len, optlen))
1660                 return -EFAULT;
1661
1662         lock_sock(sk);
1663
1664         switch (optname) {
1665         case L2CAP_OPTIONS:
1666                 opts.imtu     = l2cap_pi(sk)->imtu;
1667                 opts.omtu     = l2cap_pi(sk)->omtu;
1668                 opts.flush_to = l2cap_pi(sk)->flush_to;
1669                 opts.mode     = l2cap_pi(sk)->mode;
1670
1671                 len = min_t(unsigned int, len, sizeof(opts));
1672                 if (copy_to_user(optval, (char *) &opts, len))
1673                         err = -EFAULT;
1674
1675                 break;
1676
1677         case L2CAP_LM:
1678                 switch (l2cap_pi(sk)->sec_level) {
1679                 case BT_SECURITY_LOW:
1680                         opt = L2CAP_LM_AUTH;
1681                         break;
1682                 case BT_SECURITY_MEDIUM:
1683                         opt = L2CAP_LM_AUTH | L2CAP_LM_ENCRYPT;
1684                         break;
1685                 case BT_SECURITY_HIGH:
1686                         opt = L2CAP_LM_AUTH | L2CAP_LM_ENCRYPT |
1687                                                         L2CAP_LM_SECURE;
1688                         break;
1689                 default:
1690                         opt = 0;
1691                         break;
1692                 }
1693
1694                 if (l2cap_pi(sk)->role_switch)
1695                         opt |= L2CAP_LM_MASTER;
1696
1697                 if (l2cap_pi(sk)->force_reliable)
1698                         opt |= L2CAP_LM_RELIABLE;
1699
1700                 if (put_user(opt, (u32 __user *) optval))
1701                         err = -EFAULT;
1702                 break;
1703
1704         case L2CAP_CONNINFO:
1705                 if (sk->sk_state != BT_CONNECTED &&
1706                                         !(sk->sk_state == BT_CONNECT2 &&
1707                                                 bt_sk(sk)->defer_setup)) {
1708                         err = -ENOTCONN;
1709                         break;
1710                 }
1711
1712                 cinfo.hci_handle = l2cap_pi(sk)->conn->hcon->handle;
1713                 memcpy(cinfo.dev_class, l2cap_pi(sk)->conn->hcon->dev_class, 3);
1714
1715                 len = min_t(unsigned int, len, sizeof(cinfo));
1716                 if (copy_to_user(optval, (char *) &cinfo, len))
1717                         err = -EFAULT;
1718
1719                 break;
1720
1721         default:
1722                 err = -ENOPROTOOPT;
1723                 break;
1724         }
1725
1726         release_sock(sk);
1727         return err;
1728 }
1729
1730 static int l2cap_sock_getsockopt(struct socket *sock, int level, int optname, char __user *optval, int __user *optlen)
1731 {
1732         struct sock *sk = sock->sk;
1733         struct bt_security sec;
1734         int len, err = 0;
1735
1736         BT_DBG("sk %p", sk);
1737
1738         if (level == SOL_L2CAP)
1739                 return l2cap_sock_getsockopt_old(sock, optname, optval, optlen);
1740
1741         if (level != SOL_BLUETOOTH)
1742                 return -ENOPROTOOPT;
1743
1744         if (get_user(len, optlen))
1745                 return -EFAULT;
1746
1747         lock_sock(sk);
1748
1749         switch (optname) {
1750         case BT_SECURITY:
1751                 if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_RAW) {
1752                         err = -EINVAL;
1753                         break;
1754                 }
1755
1756                 sec.level = l2cap_pi(sk)->sec_level;
1757
1758                 len = min_t(unsigned int, len, sizeof(sec));
1759                 if (copy_to_user(optval, (char *) &sec, len))
1760                         err = -EFAULT;
1761
1762                 break;
1763
1764         case BT_DEFER_SETUP:
1765                 if (sk->sk_state != BT_BOUND && sk->sk_state != BT_LISTEN) {
1766                         err = -EINVAL;
1767                         break;
1768                 }
1769
1770                 if (put_user(bt_sk(sk)->defer_setup, (u32 __user *) optval))
1771                         err = -EFAULT;
1772
1773                 break;
1774
1775         default:
1776                 err = -ENOPROTOOPT;
1777                 break;
1778         }
1779
1780         release_sock(sk);
1781         return err;
1782 }
1783
1784 static int l2cap_sock_shutdown(struct socket *sock, int how)
1785 {
1786         struct sock *sk = sock->sk;
1787         int err = 0;
1788
1789         BT_DBG("sock %p, sk %p", sock, sk);
1790
1791         if (!sk)
1792                 return 0;
1793
1794         lock_sock(sk);
1795         if (!sk->sk_shutdown) {
1796                 sk->sk_shutdown = SHUTDOWN_MASK;
1797                 l2cap_sock_clear_timer(sk);
1798                 __l2cap_sock_close(sk, 0);
1799
1800                 if (sock_flag(sk, SOCK_LINGER) && sk->sk_lingertime)
1801                         err = bt_sock_wait_state(sk, BT_CLOSED,
1802                                                         sk->sk_lingertime);
1803         }
1804         release_sock(sk);
1805         return err;
1806 }
1807
1808 static int l2cap_sock_release(struct socket *sock)
1809 {
1810         struct sock *sk = sock->sk;
1811         int err;
1812
1813         BT_DBG("sock %p, sk %p", sock, sk);
1814
1815         if (!sk)
1816                 return 0;
1817
1818         err = l2cap_sock_shutdown(sock, 2);
1819
1820         sock_orphan(sk);
1821         l2cap_sock_kill(sk);
1822         return err;
1823 }
1824
1825 static void l2cap_chan_ready(struct sock *sk)
1826 {
1827         struct sock *parent = bt_sk(sk)->parent;
1828
1829         BT_DBG("sk %p, parent %p", sk, parent);
1830
1831         l2cap_pi(sk)->conf_state = 0;
1832         l2cap_sock_clear_timer(sk);
1833
1834         if (!parent) {
1835                 /* Outgoing channel.
1836                  * Wake up socket sleeping on connect.
1837                  */
1838                 sk->sk_state = BT_CONNECTED;
1839                 sk->sk_state_change(sk);
1840         } else {
1841                 /* Incoming channel.
1842                  * Wake up socket sleeping on accept.
1843                  */
1844                 parent->sk_data_ready(parent, 0);
1845         }
1846 }
1847
1848 /* Copy frame to all raw sockets on that connection */
1849 static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
1850 {
1851         struct l2cap_chan_list *l = &conn->chan_list;
1852         struct sk_buff *nskb;
1853         struct sock *sk;
1854
1855         BT_DBG("conn %p", conn);
1856
1857         read_lock(&l->lock);
1858         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
1859                 if (sk->sk_type != SOCK_RAW)
1860                         continue;
1861
1862                 /* Don't send frame to the socket it came from */
1863                 if (skb->sk == sk)
1864                         continue;
1865                 nskb = skb_clone(skb, GFP_ATOMIC);
1866                 if (!nskb)
1867                         continue;
1868
1869                 if (sock_queue_rcv_skb(sk, nskb))
1870                         kfree_skb(nskb);
1871         }
1872         read_unlock(&l->lock);
1873 }
1874
1875 /* ---- L2CAP signalling commands ---- */
1876 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
1877                                 u8 code, u8 ident, u16 dlen, void *data)
1878 {
1879         struct sk_buff *skb, **frag;
1880         struct l2cap_cmd_hdr *cmd;
1881         struct l2cap_hdr *lh;
1882         int len, count;
1883
1884         BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %d",
1885                         conn, code, ident, dlen);
1886
1887         len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen;
1888         count = min_t(unsigned int, conn->mtu, len);
1889
1890         skb = bt_skb_alloc(count, GFP_ATOMIC);
1891         if (!skb)
1892                 return NULL;
1893
1894         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1895         lh->len = cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen);
1896         lh->cid = cpu_to_le16(L2CAP_CID_SIGNALING);
1897
1898         cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE);
1899         cmd->code  = code;
1900         cmd->ident = ident;
1901         cmd->len   = cpu_to_le16(dlen);
1902
1903         if (dlen) {
1904                 count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE;
1905                 memcpy(skb_put(skb, count), data, count);
1906                 data += count;
1907         }
1908
1909         len -= skb->len;
1910
1911         /* Continuation fragments (no L2CAP header) */
1912         frag = &skb_shinfo(skb)->frag_list;
1913         while (len) {
1914                 count = min_t(unsigned int, conn->mtu, len);
1915
1916                 *frag = bt_skb_alloc(count, GFP_ATOMIC);
1917                 if (!*frag)
1918                         goto fail;
1919
1920                 memcpy(skb_put(*frag, count), data, count);
1921
1922                 len  -= count;
1923                 data += count;
1924
1925                 frag = &(*frag)->next;
1926         }
1927
1928         return skb;
1929
1930 fail:
1931         kfree_skb(skb);
1932         return NULL;
1933 }
1934
1935 static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val)
1936 {
1937         struct l2cap_conf_opt *opt = *ptr;
1938         int len;
1939
1940         len = L2CAP_CONF_OPT_SIZE + opt->len;
1941         *ptr += len;
1942
1943         *type = opt->type;
1944         *olen = opt->len;
1945
1946         switch (opt->len) {
1947         case 1:
1948                 *val = *((u8 *) opt->val);
1949                 break;
1950
1951         case 2:
1952                 *val = __le16_to_cpu(*((__le16 *) opt->val));
1953                 break;
1954
1955         case 4:
1956                 *val = __le32_to_cpu(*((__le32 *) opt->val));
1957                 break;
1958
1959         default:
1960                 *val = (unsigned long) opt->val;
1961                 break;
1962         }
1963
1964         BT_DBG("type 0x%2.2x len %d val 0x%lx", *type, opt->len, *val);
1965         return len;
1966 }
1967
1968 static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val)
1969 {
1970         struct l2cap_conf_opt *opt = *ptr;
1971
1972         BT_DBG("type 0x%2.2x len %d val 0x%lx", type, len, val);
1973
1974         opt->type = type;
1975         opt->len  = len;
1976
1977         switch (len) {
1978         case 1:
1979                 *((u8 *) opt->val)  = val;
1980                 break;
1981
1982         case 2:
1983                 *((__le16 *) opt->val) = cpu_to_le16(val);
1984                 break;
1985
1986         case 4:
1987                 *((__le32 *) opt->val) = cpu_to_le32(val);
1988                 break;
1989
1990         default:
1991                 memcpy(opt->val, (void *) val, len);
1992                 break;
1993         }
1994
1995         *ptr += L2CAP_CONF_OPT_SIZE + len;
1996 }
1997
1998 static int l2cap_mode_supported(__u8 mode, __u32 feat_mask)
1999 {
2000         u32 local_feat_mask = l2cap_feat_mask;
2001         if (enable_ertm)
2002                 local_feat_mask |= L2CAP_FEAT_ERTM;
2003
2004         switch (mode) {
2005         case L2CAP_MODE_ERTM:
2006                 return L2CAP_FEAT_ERTM & feat_mask & local_feat_mask;
2007         case L2CAP_MODE_STREAMING:
2008                 return L2CAP_FEAT_STREAMING & feat_mask & local_feat_mask;
2009         default:
2010                 return 0x00;
2011         }
2012 }
2013
2014 static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
2015 {
2016         switch (mode) {
2017         case L2CAP_MODE_STREAMING:
2018         case L2CAP_MODE_ERTM:
2019                 if (l2cap_mode_supported(mode, remote_feat_mask))
2020                         return mode;
2021                 /* fall through */
2022         default:
2023                 return L2CAP_MODE_BASIC;
2024         }
2025 }
2026
2027 static int l2cap_build_conf_req(struct sock *sk, void *data)
2028 {
2029         struct l2cap_pinfo *pi = l2cap_pi(sk);
2030         struct l2cap_conf_req *req = data;
2031         struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_ERTM };
2032         void *ptr = req->data;
2033
2034         BT_DBG("sk %p", sk);
2035
2036         if (pi->num_conf_req || pi->num_conf_rsp)
2037                 goto done;
2038
2039         switch (pi->mode) {
2040         case L2CAP_MODE_STREAMING:
2041         case L2CAP_MODE_ERTM:
2042                 pi->conf_state |= L2CAP_CONF_STATE2_DEVICE;
2043                 if (!l2cap_mode_supported(pi->mode, pi->conn->feat_mask))
2044                         l2cap_send_disconn_req(pi->conn, sk);
2045                 break;
2046         default:
2047                 pi->mode = l2cap_select_mode(rfc.mode, pi->conn->feat_mask);
2048                 break;
2049         }
2050
2051 done:
2052         switch (pi->mode) {
2053         case L2CAP_MODE_BASIC:
2054                 if (pi->imtu != L2CAP_DEFAULT_MTU)
2055                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->imtu);
2056                 break;
2057
2058         case L2CAP_MODE_ERTM:
2059                 rfc.mode            = L2CAP_MODE_ERTM;
2060                 rfc.txwin_size      = L2CAP_DEFAULT_TX_WINDOW;
2061                 rfc.max_transmit    = L2CAP_DEFAULT_MAX_RECEIVE;
2062                 rfc.retrans_timeout = 0;
2063                 rfc.monitor_timeout = 0;
2064                 rfc.max_pdu_size    = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE);
2065
2066                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2067                                         sizeof(rfc), (unsigned long) &rfc);
2068                 break;
2069
2070         case L2CAP_MODE_STREAMING:
2071                 rfc.mode            = L2CAP_MODE_STREAMING;
2072                 rfc.txwin_size      = 0;
2073                 rfc.max_transmit    = 0;
2074                 rfc.retrans_timeout = 0;
2075                 rfc.monitor_timeout = 0;
2076                 rfc.max_pdu_size    = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE);
2077
2078                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2079                                         sizeof(rfc), (unsigned long) &rfc);
2080                 break;
2081         }
2082
2083         /* FIXME: Need actual value of the flush timeout */
2084         //if (flush_to != L2CAP_DEFAULT_FLUSH_TO)
2085         //   l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO, 2, pi->flush_to);
2086
2087         req->dcid  = cpu_to_le16(pi->dcid);
2088         req->flags = cpu_to_le16(0);
2089
2090         return ptr - data;
2091 }
2092
2093 static int l2cap_parse_conf_req(struct sock *sk, void *data)
2094 {
2095         struct l2cap_pinfo *pi = l2cap_pi(sk);
2096         struct l2cap_conf_rsp *rsp = data;
2097         void *ptr = rsp->data;
2098         void *req = pi->conf_req;
2099         int len = pi->conf_len;
2100         int type, hint, olen;
2101         unsigned long val;
2102         struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
2103         u16 mtu = L2CAP_DEFAULT_MTU;
2104         u16 result = L2CAP_CONF_SUCCESS;
2105
2106         BT_DBG("sk %p", sk);
2107
2108         while (len >= L2CAP_CONF_OPT_SIZE) {
2109                 len -= l2cap_get_conf_opt(&req, &type, &olen, &val);
2110
2111                 hint  = type & L2CAP_CONF_HINT;
2112                 type &= L2CAP_CONF_MASK;
2113
2114                 switch (type) {
2115                 case L2CAP_CONF_MTU:
2116                         mtu = val;
2117                         break;
2118
2119                 case L2CAP_CONF_FLUSH_TO:
2120                         pi->flush_to = val;
2121                         break;
2122
2123                 case L2CAP_CONF_QOS:
2124                         break;
2125
2126                 case L2CAP_CONF_RFC:
2127                         if (olen == sizeof(rfc))
2128                                 memcpy(&rfc, (void *) val, olen);
2129                         break;
2130
2131                 default:
2132                         if (hint)
2133                                 break;
2134
2135                         result = L2CAP_CONF_UNKNOWN;
2136                         *((u8 *) ptr++) = type;
2137                         break;
2138                 }
2139         }
2140
2141         if (pi->num_conf_rsp || pi->num_conf_req)
2142                 goto done;
2143
2144         switch (pi->mode) {
2145         case L2CAP_MODE_STREAMING:
2146         case L2CAP_MODE_ERTM:
2147                 pi->conf_state |= L2CAP_CONF_STATE2_DEVICE;
2148                 if (!l2cap_mode_supported(pi->mode, pi->conn->feat_mask))
2149                         return -ECONNREFUSED;
2150                 break;
2151         default:
2152                 pi->mode = l2cap_select_mode(rfc.mode, pi->conn->feat_mask);
2153                 break;
2154         }
2155
2156 done:
2157         if (pi->mode != rfc.mode) {
2158                 result = L2CAP_CONF_UNACCEPT;
2159                 rfc.mode = pi->mode;
2160
2161                 if (pi->num_conf_rsp == 1)
2162                         return -ECONNREFUSED;
2163
2164                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2165                                         sizeof(rfc), (unsigned long) &rfc);
2166         }
2167
2168
2169         if (result == L2CAP_CONF_SUCCESS) {
2170                 /* Configure output options and let the other side know
2171                  * which ones we don't like. */
2172
2173                 if (mtu < L2CAP_DEFAULT_MIN_MTU)
2174                         result = L2CAP_CONF_UNACCEPT;
2175                 else {
2176                         pi->omtu = mtu;
2177                         pi->conf_state |= L2CAP_CONF_MTU_DONE;
2178                 }
2179                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->omtu);
2180
2181                 switch (rfc.mode) {
2182                 case L2CAP_MODE_BASIC:
2183                         pi->fcs = L2CAP_FCS_NONE;
2184                         pi->conf_state |= L2CAP_CONF_MODE_DONE;
2185                         break;
2186
2187                 case L2CAP_MODE_ERTM:
2188                         pi->remote_tx_win = rfc.txwin_size;
2189                         pi->remote_max_tx = rfc.max_transmit;
2190                         pi->max_pdu_size = rfc.max_pdu_size;
2191
2192                         rfc.retrans_timeout = L2CAP_DEFAULT_RETRANS_TO;
2193                         rfc.monitor_timeout = L2CAP_DEFAULT_MONITOR_TO;
2194
2195                         pi->conf_state |= L2CAP_CONF_MODE_DONE;
2196                         break;
2197
2198                 case L2CAP_MODE_STREAMING:
2199                         pi->remote_tx_win = rfc.txwin_size;
2200                         pi->max_pdu_size = rfc.max_pdu_size;
2201
2202                         pi->conf_state |= L2CAP_CONF_MODE_DONE;
2203                         break;
2204
2205                 default:
2206                         result = L2CAP_CONF_UNACCEPT;
2207
2208                         memset(&rfc, 0, sizeof(rfc));
2209                         rfc.mode = pi->mode;
2210                 }
2211
2212                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2213                                         sizeof(rfc), (unsigned long) &rfc);
2214
2215                 if (result == L2CAP_CONF_SUCCESS)
2216                         pi->conf_state |= L2CAP_CONF_OUTPUT_DONE;
2217         }
2218         rsp->scid   = cpu_to_le16(pi->dcid);
2219         rsp->result = cpu_to_le16(result);
2220         rsp->flags  = cpu_to_le16(0x0000);
2221
2222         return ptr - data;
2223 }
2224
2225 static int l2cap_parse_conf_rsp(struct sock *sk, void *rsp, int len, void *data, u16 *result)
2226 {
2227         struct l2cap_pinfo *pi = l2cap_pi(sk);
2228         struct l2cap_conf_req *req = data;
2229         void *ptr = req->data;
2230         int type, olen;
2231         unsigned long val;
2232         struct l2cap_conf_rfc rfc;
2233
2234         BT_DBG("sk %p, rsp %p, len %d, req %p", sk, rsp, len, data);
2235
2236         while (len >= L2CAP_CONF_OPT_SIZE) {
2237                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
2238
2239                 switch (type) {
2240                 case L2CAP_CONF_MTU:
2241                         if (val < L2CAP_DEFAULT_MIN_MTU) {
2242                                 *result = L2CAP_CONF_UNACCEPT;
2243                                 pi->omtu = L2CAP_DEFAULT_MIN_MTU;
2244                         } else
2245                                 pi->omtu = val;
2246                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->omtu);
2247                         break;
2248
2249                 case L2CAP_CONF_FLUSH_TO:
2250                         pi->flush_to = val;
2251                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
2252                                                         2, pi->flush_to);
2253                         break;
2254
2255                 case L2CAP_CONF_RFC:
2256                         if (olen == sizeof(rfc))
2257                                 memcpy(&rfc, (void *)val, olen);
2258
2259                         if ((pi->conf_state & L2CAP_CONF_STATE2_DEVICE) &&
2260                                                         rfc.mode != pi->mode)
2261                                 return -ECONNREFUSED;
2262
2263                         pi->mode = rfc.mode;
2264                         pi->fcs = 0;
2265
2266                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2267                                         sizeof(rfc), (unsigned long) &rfc);
2268                         break;
2269                 }
2270         }
2271
2272         if (*result == L2CAP_CONF_SUCCESS) {
2273                 switch (rfc.mode) {
2274                 case L2CAP_MODE_ERTM:
2275                         pi->remote_tx_win   = rfc.txwin_size;
2276                         pi->retrans_timeout = rfc.retrans_timeout;
2277                         pi->monitor_timeout = rfc.monitor_timeout;
2278                         pi->max_pdu_size    = le16_to_cpu(rfc.max_pdu_size);
2279                         break;
2280                 case L2CAP_MODE_STREAMING:
2281                         pi->max_pdu_size    = le16_to_cpu(rfc.max_pdu_size);
2282                         break;
2283                 }
2284         }
2285
2286         req->dcid   = cpu_to_le16(pi->dcid);
2287         req->flags  = cpu_to_le16(0x0000);
2288
2289         return ptr - data;
2290 }
2291
2292 static int l2cap_build_conf_rsp(struct sock *sk, void *data, u16 result, u16 flags)
2293 {
2294         struct l2cap_conf_rsp *rsp = data;
2295         void *ptr = rsp->data;
2296
2297         BT_DBG("sk %p", sk);
2298
2299         rsp->scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
2300         rsp->result = cpu_to_le16(result);
2301         rsp->flags  = cpu_to_le16(flags);
2302
2303         return ptr - data;
2304 }
2305
2306 static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2307 {
2308         struct l2cap_cmd_rej *rej = (struct l2cap_cmd_rej *) data;
2309
2310         if (rej->reason != 0x0000)
2311                 return 0;
2312
2313         if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
2314                                         cmd->ident == conn->info_ident) {
2315                 del_timer(&conn->info_timer);
2316
2317                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2318                 conn->info_ident = 0;
2319
2320                 l2cap_conn_start(conn);
2321         }
2322
2323         return 0;
2324 }
2325
2326 static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2327 {
2328         struct l2cap_chan_list *list = &conn->chan_list;
2329         struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
2330         struct l2cap_conn_rsp rsp;
2331         struct sock *sk, *parent;
2332         int result, status = L2CAP_CS_NO_INFO;
2333
2334         u16 dcid = 0, scid = __le16_to_cpu(req->scid);
2335         __le16 psm = req->psm;
2336
2337         BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid);
2338
2339         /* Check if we have socket listening on psm */
2340         parent = l2cap_get_sock_by_psm(BT_LISTEN, psm, conn->src);
2341         if (!parent) {
2342                 result = L2CAP_CR_BAD_PSM;
2343                 goto sendresp;
2344         }
2345
2346         /* Check if the ACL is secure enough (if not SDP) */
2347         if (psm != cpu_to_le16(0x0001) &&
2348                                 !hci_conn_check_link_mode(conn->hcon)) {
2349                 conn->disc_reason = 0x05;
2350                 result = L2CAP_CR_SEC_BLOCK;
2351                 goto response;
2352         }
2353
2354         result = L2CAP_CR_NO_MEM;
2355
2356         /* Check for backlog size */
2357         if (sk_acceptq_is_full(parent)) {
2358                 BT_DBG("backlog full %d", parent->sk_ack_backlog);
2359                 goto response;
2360         }
2361
2362         sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, GFP_ATOMIC);
2363         if (!sk)
2364                 goto response;
2365
2366         write_lock_bh(&list->lock);
2367
2368         /* Check if we already have channel with that dcid */
2369         if (__l2cap_get_chan_by_dcid(list, scid)) {
2370                 write_unlock_bh(&list->lock);
2371                 sock_set_flag(sk, SOCK_ZAPPED);
2372                 l2cap_sock_kill(sk);
2373                 goto response;
2374         }
2375
2376         hci_conn_hold(conn->hcon);
2377
2378         l2cap_sock_init(sk, parent);
2379         bacpy(&bt_sk(sk)->src, conn->src);
2380         bacpy(&bt_sk(sk)->dst, conn->dst);
2381         l2cap_pi(sk)->psm  = psm;
2382         l2cap_pi(sk)->dcid = scid;
2383
2384         __l2cap_chan_add(conn, sk, parent);
2385         dcid = l2cap_pi(sk)->scid;
2386
2387         l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
2388
2389         l2cap_pi(sk)->ident = cmd->ident;
2390
2391         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
2392                 if (l2cap_check_security(sk)) {
2393                         if (bt_sk(sk)->defer_setup) {
2394                                 sk->sk_state = BT_CONNECT2;
2395                                 result = L2CAP_CR_PEND;
2396                                 status = L2CAP_CS_AUTHOR_PEND;
2397                                 parent->sk_data_ready(parent, 0);
2398                         } else {
2399                                 sk->sk_state = BT_CONFIG;
2400                                 result = L2CAP_CR_SUCCESS;
2401                                 status = L2CAP_CS_NO_INFO;
2402                         }
2403                 } else {
2404                         sk->sk_state = BT_CONNECT2;
2405                         result = L2CAP_CR_PEND;
2406                         status = L2CAP_CS_AUTHEN_PEND;
2407                 }
2408         } else {
2409                 sk->sk_state = BT_CONNECT2;
2410                 result = L2CAP_CR_PEND;
2411                 status = L2CAP_CS_NO_INFO;
2412         }
2413
2414         write_unlock_bh(&list->lock);
2415
2416 response:
2417         bh_unlock_sock(parent);
2418
2419 sendresp:
2420         rsp.scid   = cpu_to_le16(scid);
2421         rsp.dcid   = cpu_to_le16(dcid);
2422         rsp.result = cpu_to_le16(result);
2423         rsp.status = cpu_to_le16(status);
2424         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
2425
2426         if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
2427                 struct l2cap_info_req info;
2428                 info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
2429
2430                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
2431                 conn->info_ident = l2cap_get_ident(conn);
2432
2433                 mod_timer(&conn->info_timer, jiffies +
2434                                         msecs_to_jiffies(L2CAP_INFO_TIMEOUT));
2435
2436                 l2cap_send_cmd(conn, conn->info_ident,
2437                                         L2CAP_INFO_REQ, sizeof(info), &info);
2438         }
2439
2440         return 0;
2441 }
2442
2443 static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2444 {
2445         struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
2446         u16 scid, dcid, result, status;
2447         struct sock *sk;
2448         u8 req[128];
2449
2450         scid   = __le16_to_cpu(rsp->scid);
2451         dcid   = __le16_to_cpu(rsp->dcid);
2452         result = __le16_to_cpu(rsp->result);
2453         status = __le16_to_cpu(rsp->status);
2454
2455         BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status);
2456
2457         if (scid) {
2458                 sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
2459                 if (!sk)
2460                         return 0;
2461         } else {
2462                 sk = l2cap_get_chan_by_ident(&conn->chan_list, cmd->ident);
2463                 if (!sk)
2464                         return 0;
2465         }
2466
2467         switch (result) {
2468         case L2CAP_CR_SUCCESS:
2469                 sk->sk_state = BT_CONFIG;
2470                 l2cap_pi(sk)->ident = 0;
2471                 l2cap_pi(sk)->dcid = dcid;
2472                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
2473
2474                 l2cap_pi(sk)->conf_state &= ~L2CAP_CONF_CONNECT_PEND;
2475
2476                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2477                                         l2cap_build_conf_req(sk, req), req);
2478                 l2cap_pi(sk)->num_conf_req++;
2479                 break;
2480
2481         case L2CAP_CR_PEND:
2482                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND;
2483                 break;
2484
2485         default:
2486                 l2cap_chan_del(sk, ECONNREFUSED);
2487                 break;
2488         }
2489
2490         bh_unlock_sock(sk);
2491         return 0;
2492 }
2493
2494 static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
2495 {
2496         struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
2497         u16 dcid, flags;
2498         u8 rsp[64];
2499         struct sock *sk;
2500         int len;
2501
2502         dcid  = __le16_to_cpu(req->dcid);
2503         flags = __le16_to_cpu(req->flags);
2504
2505         BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
2506
2507         sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid);
2508         if (!sk)
2509                 return -ENOENT;
2510
2511         if (sk->sk_state == BT_DISCONN)
2512                 goto unlock;
2513
2514         /* Reject if config buffer is too small. */
2515         len = cmd_len - sizeof(*req);
2516         if (l2cap_pi(sk)->conf_len + len > sizeof(l2cap_pi(sk)->conf_req)) {
2517                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2518                                 l2cap_build_conf_rsp(sk, rsp,
2519                                         L2CAP_CONF_REJECT, flags), rsp);
2520                 goto unlock;
2521         }
2522
2523         /* Store config. */
2524         memcpy(l2cap_pi(sk)->conf_req + l2cap_pi(sk)->conf_len, req->data, len);
2525         l2cap_pi(sk)->conf_len += len;
2526
2527         if (flags & 0x0001) {
2528                 /* Incomplete config. Send empty response. */
2529                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2530                                 l2cap_build_conf_rsp(sk, rsp,
2531                                         L2CAP_CONF_SUCCESS, 0x0001), rsp);
2532                 goto unlock;
2533         }
2534
2535         /* Complete config. */
2536         len = l2cap_parse_conf_req(sk, rsp);
2537         if (len < 0) {
2538                 l2cap_send_disconn_req(conn, sk);
2539                 goto unlock;
2540         }
2541
2542         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
2543         l2cap_pi(sk)->num_conf_rsp++;
2544
2545         /* Reset config buffer. */
2546         l2cap_pi(sk)->conf_len = 0;
2547
2548         if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE))
2549                 goto unlock;
2550
2551         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_INPUT_DONE) {
2552                 sk->sk_state = BT_CONNECTED;
2553                 l2cap_pi(sk)->next_tx_seq = 0;
2554                 l2cap_pi(sk)->expected_ack_seq = 0;
2555                 l2cap_pi(sk)->unacked_frames = 0;
2556                 __skb_queue_head_init(TX_QUEUE(sk));
2557                 l2cap_chan_ready(sk);
2558                 goto unlock;
2559         }
2560
2561         if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT)) {
2562                 u8 buf[64];
2563                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2564                                         l2cap_build_conf_req(sk, buf), buf);
2565                 l2cap_pi(sk)->num_conf_req++;
2566         }
2567
2568 unlock:
2569         bh_unlock_sock(sk);
2570         return 0;
2571 }
2572
2573 static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2574 {
2575         struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
2576         u16 scid, flags, result;
2577         struct sock *sk;
2578
2579         scid   = __le16_to_cpu(rsp->scid);
2580         flags  = __le16_to_cpu(rsp->flags);
2581         result = __le16_to_cpu(rsp->result);
2582
2583         BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x",
2584                         scid, flags, result);
2585
2586         sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
2587         if (!sk)
2588                 return 0;
2589
2590         switch (result) {
2591         case L2CAP_CONF_SUCCESS:
2592                 break;
2593
2594         case L2CAP_CONF_UNACCEPT:
2595                 if (l2cap_pi(sk)->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
2596                         int len = cmd->len - sizeof(*rsp);
2597                         char req[64];
2598
2599                         /* throw out any old stored conf requests */
2600                         result = L2CAP_CONF_SUCCESS;
2601                         len = l2cap_parse_conf_rsp(sk, rsp->data,
2602                                                         len, req, &result);
2603                         if (len < 0) {
2604                                 l2cap_send_disconn_req(conn, sk);
2605                                 goto done;
2606                         }
2607
2608                         l2cap_send_cmd(conn, l2cap_get_ident(conn),
2609                                                 L2CAP_CONF_REQ, len, req);
2610                         l2cap_pi(sk)->num_conf_req++;
2611                         if (result != L2CAP_CONF_SUCCESS)
2612                                 goto done;
2613                         break;
2614                 }
2615
2616         default:
2617                 sk->sk_state = BT_DISCONN;
2618                 sk->sk_err = ECONNRESET;
2619                 l2cap_sock_set_timer(sk, HZ * 5);
2620                 l2cap_send_disconn_req(conn, sk);
2621                 goto done;
2622         }
2623
2624         if (flags & 0x01)
2625                 goto done;
2626
2627         l2cap_pi(sk)->conf_state |= L2CAP_CONF_INPUT_DONE;
2628
2629         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE) {
2630                 sk->sk_state = BT_CONNECTED;
2631                 l2cap_pi(sk)->expected_tx_seq = 0;
2632                 l2cap_pi(sk)->num_to_ack = 0;
2633                 __skb_queue_head_init(TX_QUEUE(sk));
2634                 l2cap_chan_ready(sk);
2635         }
2636
2637 done:
2638         bh_unlock_sock(sk);
2639         return 0;
2640 }
2641
2642 static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2643 {
2644         struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
2645         struct l2cap_disconn_rsp rsp;
2646         u16 dcid, scid;
2647         struct sock *sk;
2648
2649         scid = __le16_to_cpu(req->scid);
2650         dcid = __le16_to_cpu(req->dcid);
2651
2652         BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
2653
2654         sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid);
2655         if (!sk)
2656                 return 0;
2657
2658         rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid);
2659         rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid);
2660         l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
2661
2662         sk->sk_shutdown = SHUTDOWN_MASK;
2663
2664         skb_queue_purge(TX_QUEUE(sk));
2665
2666         l2cap_chan_del(sk, ECONNRESET);
2667         bh_unlock_sock(sk);
2668
2669         l2cap_sock_kill(sk);
2670         return 0;
2671 }
2672
2673 static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2674 {
2675         struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
2676         u16 dcid, scid;
2677         struct sock *sk;
2678
2679         scid = __le16_to_cpu(rsp->scid);
2680         dcid = __le16_to_cpu(rsp->dcid);
2681
2682         BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
2683
2684         sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
2685         if (!sk)
2686                 return 0;
2687
2688         skb_queue_purge(TX_QUEUE(sk));
2689
2690         l2cap_chan_del(sk, 0);
2691         bh_unlock_sock(sk);
2692
2693         l2cap_sock_kill(sk);
2694         return 0;
2695 }
2696
2697 static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2698 {
2699         struct l2cap_info_req *req = (struct l2cap_info_req *) data;
2700         u16 type;
2701
2702         type = __le16_to_cpu(req->type);
2703
2704         BT_DBG("type 0x%4.4x", type);
2705
2706         if (type == L2CAP_IT_FEAT_MASK) {
2707                 u8 buf[8];
2708                 u32 feat_mask = l2cap_feat_mask;
2709                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
2710                 rsp->type   = cpu_to_le16(L2CAP_IT_FEAT_MASK);
2711                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
2712                 if (enable_ertm)
2713                         feat_mask |= L2CAP_FEAT_ERTM;
2714                 put_unaligned(cpu_to_le32(feat_mask), (__le32 *) rsp->data);
2715                 l2cap_send_cmd(conn, cmd->ident,
2716                                         L2CAP_INFO_RSP, sizeof(buf), buf);
2717         } else if (type == L2CAP_IT_FIXED_CHAN) {
2718                 u8 buf[12];
2719                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
2720                 rsp->type   = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
2721                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
2722                 memcpy(buf + 4, l2cap_fixed_chan, 8);
2723                 l2cap_send_cmd(conn, cmd->ident,
2724                                         L2CAP_INFO_RSP, sizeof(buf), buf);
2725         } else {
2726                 struct l2cap_info_rsp rsp;
2727                 rsp.type   = cpu_to_le16(type);
2728                 rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP);
2729                 l2cap_send_cmd(conn, cmd->ident,
2730                                         L2CAP_INFO_RSP, sizeof(rsp), &rsp);
2731         }
2732
2733         return 0;
2734 }
2735
2736 static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2737 {
2738         struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
2739         u16 type, result;
2740
2741         type   = __le16_to_cpu(rsp->type);
2742         result = __le16_to_cpu(rsp->result);
2743
2744         BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
2745
2746         del_timer(&conn->info_timer);
2747
2748         if (type == L2CAP_IT_FEAT_MASK) {
2749                 conn->feat_mask = get_unaligned_le32(rsp->data);
2750
2751                 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
2752                         struct l2cap_info_req req;
2753                         req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
2754
2755                         conn->info_ident = l2cap_get_ident(conn);
2756
2757                         l2cap_send_cmd(conn, conn->info_ident,
2758                                         L2CAP_INFO_REQ, sizeof(req), &req);
2759                 } else {
2760                         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2761                         conn->info_ident = 0;
2762
2763                         l2cap_conn_start(conn);
2764                 }
2765         } else if (type == L2CAP_IT_FIXED_CHAN) {
2766                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2767                 conn->info_ident = 0;
2768
2769                 l2cap_conn_start(conn);
2770         }
2771
2772         return 0;
2773 }
2774
2775 static inline void l2cap_sig_channel(struct l2cap_conn *conn, struct sk_buff *skb)
2776 {
2777         u8 *data = skb->data;
2778         int len = skb->len;
2779         struct l2cap_cmd_hdr cmd;
2780         int err = 0;
2781
2782         l2cap_raw_recv(conn, skb);
2783
2784         while (len >= L2CAP_CMD_HDR_SIZE) {
2785                 u16 cmd_len;
2786                 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
2787                 data += L2CAP_CMD_HDR_SIZE;
2788                 len  -= L2CAP_CMD_HDR_SIZE;
2789
2790                 cmd_len = le16_to_cpu(cmd.len);
2791
2792                 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, cmd.ident);
2793
2794                 if (cmd_len > len || !cmd.ident) {
2795                         BT_DBG("corrupted command");
2796                         break;
2797                 }
2798
2799                 switch (cmd.code) {
2800                 case L2CAP_COMMAND_REJ:
2801                         l2cap_command_rej(conn, &cmd, data);
2802                         break;
2803
2804                 case L2CAP_CONN_REQ:
2805                         err = l2cap_connect_req(conn, &cmd, data);
2806                         break;
2807
2808                 case L2CAP_CONN_RSP:
2809                         err = l2cap_connect_rsp(conn, &cmd, data);
2810                         break;
2811
2812                 case L2CAP_CONF_REQ:
2813                         err = l2cap_config_req(conn, &cmd, cmd_len, data);
2814                         break;
2815
2816                 case L2CAP_CONF_RSP:
2817                         err = l2cap_config_rsp(conn, &cmd, data);
2818                         break;
2819
2820                 case L2CAP_DISCONN_REQ:
2821                         err = l2cap_disconnect_req(conn, &cmd, data);
2822                         break;
2823
2824                 case L2CAP_DISCONN_RSP:
2825                         err = l2cap_disconnect_rsp(conn, &cmd, data);
2826                         break;
2827
2828                 case L2CAP_ECHO_REQ:
2829                         l2cap_send_cmd(conn, cmd.ident, L2CAP_ECHO_RSP, cmd_len, data);
2830                         break;
2831
2832                 case L2CAP_ECHO_RSP:
2833                         break;
2834
2835                 case L2CAP_INFO_REQ:
2836                         err = l2cap_information_req(conn, &cmd, data);
2837                         break;
2838
2839                 case L2CAP_INFO_RSP:
2840                         err = l2cap_information_rsp(conn, &cmd, data);
2841                         break;
2842
2843                 default:
2844                         BT_ERR("Unknown signaling command 0x%2.2x", cmd.code);
2845                         err = -EINVAL;
2846                         break;
2847                 }
2848
2849                 if (err) {
2850                         struct l2cap_cmd_rej rej;
2851                         BT_DBG("error %d", err);
2852
2853                         /* FIXME: Map err to a valid reason */
2854                         rej.reason = cpu_to_le16(0);
2855                         l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
2856                 }
2857
2858                 data += cmd_len;
2859                 len  -= cmd_len;
2860         }
2861
2862         kfree_skb(skb);
2863 }
2864
2865 static int l2cap_sar_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)
2866 {
2867         struct l2cap_pinfo *pi = l2cap_pi(sk);
2868         struct sk_buff *_skb;
2869         int err = -EINVAL;
2870
2871         switch (control & L2CAP_CTRL_SAR) {
2872         case L2CAP_SDU_UNSEGMENTED:
2873                 if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
2874                         kfree_skb(pi->sdu);
2875                         break;
2876                 }
2877
2878                 err = sock_queue_rcv_skb(sk, skb);
2879                 if (!err)
2880                         return 0;
2881
2882                 break;
2883
2884         case L2CAP_SDU_START:
2885                 if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
2886                         kfree_skb(pi->sdu);
2887                         break;
2888                 }
2889
2890                 pi->sdu_len = get_unaligned_le16(skb->data);
2891                 skb_pull(skb, 2);
2892
2893                 pi->sdu = bt_skb_alloc(pi->sdu_len, GFP_ATOMIC);
2894                 if (!pi->sdu) {
2895                         err = -ENOMEM;
2896                         break;
2897                 }
2898
2899                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
2900
2901                 pi->conn_state |= L2CAP_CONN_SAR_SDU;
2902                 pi->partial_sdu_len = skb->len;
2903                 err = 0;
2904                 break;
2905
2906         case L2CAP_SDU_CONTINUE:
2907                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
2908                         break;
2909
2910                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
2911
2912                 pi->partial_sdu_len += skb->len;
2913                 if (pi->partial_sdu_len > pi->sdu_len)
2914                         kfree_skb(pi->sdu);
2915                 else
2916                         err = 0;
2917
2918                 break;
2919
2920         case L2CAP_SDU_END:
2921                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
2922                         break;
2923
2924                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
2925
2926                 pi->conn_state &= ~L2CAP_CONN_SAR_SDU;
2927                 pi->partial_sdu_len += skb->len;
2928
2929                 if (pi->partial_sdu_len == pi->sdu_len) {
2930                         _skb = skb_clone(pi->sdu, GFP_ATOMIC);
2931                         err = sock_queue_rcv_skb(sk, _skb);
2932                         if (err < 0)
2933                                 kfree_skb(_skb);
2934                 }
2935                 kfree_skb(pi->sdu);
2936                 err = 0;
2937
2938                 break;
2939         }
2940
2941         kfree_skb(skb);
2942         return err;
2943 }
2944
2945 static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
2946 {
2947         struct l2cap_pinfo *pi = l2cap_pi(sk);
2948         u8 tx_seq = __get_txseq(rx_control);
2949         u16 tx_control = 0;
2950         int err = 0;
2951
2952         BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
2953
2954         if (tx_seq == pi->expected_tx_seq) {
2955                 if (pi->conn_state & L2CAP_CONN_UNDER_REJ)
2956                         pi->conn_state &= ~L2CAP_CONN_UNDER_REJ;
2957
2958                 err = l2cap_sar_reassembly_sdu(sk, skb, rx_control);
2959                 if (err < 0)
2960                         return err;
2961
2962                 pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
2963                 pi->num_to_ack = (pi->num_to_ack + 1) % L2CAP_DEFAULT_NUM_TO_ACK;
2964                 if (pi->num_to_ack == L2CAP_DEFAULT_NUM_TO_ACK - 1) {
2965                         tx_control |= L2CAP_SUPER_RCV_READY;
2966                         tx_control |= pi->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
2967                         goto send;
2968                 }
2969         } else {
2970                 /* Unexpected txSeq. Send a REJ S-frame */
2971                 kfree_skb(skb);
2972                 if (!(pi->conn_state & L2CAP_CONN_UNDER_REJ)) {
2973                         tx_control |= L2CAP_SUPER_REJECT;
2974                         tx_control |= pi->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
2975                         pi->conn_state |= L2CAP_CONN_UNDER_REJ;
2976
2977                         goto send;
2978                 }
2979         }
2980         return 0;
2981
2982 send:
2983         return l2cap_send_sframe(pi, tx_control);
2984 }
2985
2986 static inline int l2cap_data_channel_sframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
2987 {
2988         struct l2cap_pinfo *pi = l2cap_pi(sk);
2989
2990         BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
2991
2992         switch (rx_control & L2CAP_CTRL_SUPERVISE) {
2993         case L2CAP_SUPER_RCV_READY:
2994                 pi->expected_ack_seq = __get_reqseq(rx_control);
2995                 l2cap_drop_acked_frames(sk);
2996                 l2cap_ertm_send(sk);
2997                 break;
2998
2999         case L2CAP_SUPER_REJECT:
3000                 pi->expected_ack_seq = __get_reqseq(rx_control);
3001                 l2cap_drop_acked_frames(sk);
3002
3003                 sk->sk_send_head = TX_QUEUE(sk)->next;
3004                 pi->next_tx_seq = pi->expected_ack_seq;
3005
3006                 l2cap_ertm_send(sk);
3007
3008                 break;
3009
3010         case L2CAP_SUPER_RCV_NOT_READY:
3011         case L2CAP_SUPER_SELECT_REJECT:
3012                 break;
3013         }
3014
3015         return 0;
3016 }
3017
3018 static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
3019 {
3020         struct sock *sk;
3021         u16 control, len;
3022         int err;
3023
3024         sk = l2cap_get_chan_by_scid(&conn->chan_list, cid);
3025         if (!sk) {
3026                 BT_DBG("unknown cid 0x%4.4x", cid);
3027                 goto drop;
3028         }
3029
3030         BT_DBG("sk %p, len %d", sk, skb->len);
3031
3032         if (sk->sk_state != BT_CONNECTED)
3033                 goto drop;
3034
3035         switch (l2cap_pi(sk)->mode) {
3036         case L2CAP_MODE_BASIC:
3037                 /* If socket recv buffers overflows we drop data here
3038                  * which is *bad* because L2CAP has to be reliable.
3039                  * But we don't have any other choice. L2CAP doesn't
3040                  * provide flow control mechanism. */
3041
3042                 if (l2cap_pi(sk)->imtu < skb->len)
3043                         goto drop;
3044
3045                 if (!sock_queue_rcv_skb(sk, skb))
3046                         goto done;
3047                 break;
3048
3049         case L2CAP_MODE_ERTM:
3050                 control = get_unaligned_le16(skb->data);
3051                 skb_pull(skb, 2);
3052                 len = skb->len;
3053
3054                 if (__is_sar_start(control))
3055                         len -= 2;
3056
3057                 /*
3058                  * We can just drop the corrupted I-frame here.
3059                  * Receiver will miss it and start proper recovery
3060                  * procedures and ask retransmission.
3061                  */
3062                 if (len > L2CAP_DEFAULT_MAX_PDU_SIZE)
3063                         goto drop;
3064
3065                 if (__is_iframe(control))
3066                         err = l2cap_data_channel_iframe(sk, control, skb);
3067                 else
3068                         err = l2cap_data_channel_sframe(sk, control, skb);
3069
3070                 if (!err)
3071                         goto done;
3072                 break;
3073
3074         default:
3075                 BT_DBG("sk %p: bad mode 0x%2.2x", sk, l2cap_pi(sk)->mode);
3076                 break;
3077         }
3078
3079 drop:
3080         kfree_skb(skb);
3081
3082 done:
3083         if (sk)
3084                 bh_unlock_sock(sk);
3085
3086         return 0;
3087 }
3088
3089 static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb)
3090 {
3091         struct sock *sk;
3092
3093         sk = l2cap_get_sock_by_psm(0, psm, conn->src);
3094         if (!sk)
3095                 goto drop;
3096
3097         BT_DBG("sk %p, len %d", sk, skb->len);
3098
3099         if (sk->sk_state != BT_BOUND && sk->sk_state != BT_CONNECTED)
3100                 goto drop;
3101
3102         if (l2cap_pi(sk)->imtu < skb->len)
3103                 goto drop;
3104
3105         if (!sock_queue_rcv_skb(sk, skb))
3106                 goto done;
3107
3108 drop:
3109         kfree_skb(skb);
3110
3111 done:
3112         if (sk)
3113                 bh_unlock_sock(sk);
3114         return 0;
3115 }
3116
3117 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
3118 {
3119         struct l2cap_hdr *lh = (void *) skb->data;
3120         u16 cid, len;
3121         __le16 psm;
3122
3123         skb_pull(skb, L2CAP_HDR_SIZE);
3124         cid = __le16_to_cpu(lh->cid);
3125         len = __le16_to_cpu(lh->len);
3126
3127         if (len != skb->len) {
3128                 kfree_skb(skb);
3129                 return;
3130         }
3131
3132         BT_DBG("len %d, cid 0x%4.4x", len, cid);
3133
3134         switch (cid) {
3135         case L2CAP_CID_SIGNALING:
3136                 l2cap_sig_channel(conn, skb);
3137                 break;
3138
3139         case L2CAP_CID_CONN_LESS:
3140                 psm = get_unaligned((__le16 *) skb->data);
3141                 skb_pull(skb, 2);
3142                 l2cap_conless_channel(conn, psm, skb);
3143                 break;
3144
3145         default:
3146                 l2cap_data_channel(conn, cid, skb);
3147                 break;
3148         }
3149 }
3150
3151 /* ---- L2CAP interface with lower layer (HCI) ---- */
3152
3153 static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
3154 {
3155         int exact = 0, lm1 = 0, lm2 = 0;
3156         register struct sock *sk;
3157         struct hlist_node *node;
3158
3159         if (type != ACL_LINK)
3160                 return 0;
3161
3162         BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
3163
3164         /* Find listening sockets and check their link_mode */
3165         read_lock(&l2cap_sk_list.lock);
3166         sk_for_each(sk, node, &l2cap_sk_list.head) {
3167                 if (sk->sk_state != BT_LISTEN)
3168                         continue;
3169
3170                 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
3171                         lm1 |= HCI_LM_ACCEPT;
3172                         if (l2cap_pi(sk)->role_switch)
3173                                 lm1 |= HCI_LM_MASTER;
3174                         exact++;
3175                 } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
3176                         lm2 |= HCI_LM_ACCEPT;
3177                         if (l2cap_pi(sk)->role_switch)
3178                                 lm2 |= HCI_LM_MASTER;
3179                 }
3180         }
3181         read_unlock(&l2cap_sk_list.lock);
3182
3183         return exact ? lm1 : lm2;
3184 }
3185
3186 static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
3187 {
3188         struct l2cap_conn *conn;
3189
3190         BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
3191
3192         if (hcon->type != ACL_LINK)
3193                 return 0;
3194
3195         if (!status) {
3196                 conn = l2cap_conn_add(hcon, status);
3197                 if (conn)
3198                         l2cap_conn_ready(conn);
3199         } else
3200                 l2cap_conn_del(hcon, bt_err(status));
3201
3202         return 0;
3203 }
3204
3205 static int l2cap_disconn_ind(struct hci_conn *hcon)
3206 {
3207         struct l2cap_conn *conn = hcon->l2cap_data;
3208
3209         BT_DBG("hcon %p", hcon);
3210
3211         if (hcon->type != ACL_LINK || !conn)
3212                 return 0x13;
3213
3214         return conn->disc_reason;
3215 }
3216
3217 static int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
3218 {
3219         BT_DBG("hcon %p reason %d", hcon, reason);
3220
3221         if (hcon->type != ACL_LINK)
3222                 return 0;
3223
3224         l2cap_conn_del(hcon, bt_err(reason));
3225
3226         return 0;
3227 }
3228
3229 static inline void l2cap_check_encryption(struct sock *sk, u8 encrypt)
3230 {
3231         if (sk->sk_type != SOCK_SEQPACKET)
3232                 return;
3233
3234         if (encrypt == 0x00) {
3235                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_MEDIUM) {
3236                         l2cap_sock_clear_timer(sk);
3237                         l2cap_sock_set_timer(sk, HZ * 5);
3238                 } else if (l2cap_pi(sk)->sec_level == BT_SECURITY_HIGH)
3239                         __l2cap_sock_close(sk, ECONNREFUSED);
3240         } else {
3241                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_MEDIUM)
3242                         l2cap_sock_clear_timer(sk);
3243         }
3244 }
3245
3246 static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
3247 {
3248         struct l2cap_chan_list *l;
3249         struct l2cap_conn *conn = hcon->l2cap_data;
3250         struct sock *sk;
3251
3252         if (!conn)
3253                 return 0;
3254
3255         l = &conn->chan_list;
3256
3257         BT_DBG("conn %p", conn);
3258
3259         read_lock(&l->lock);
3260
3261         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
3262                 bh_lock_sock(sk);
3263
3264                 if (l2cap_pi(sk)->conf_state & L2CAP_CONF_CONNECT_PEND) {
3265                         bh_unlock_sock(sk);
3266                         continue;
3267                 }
3268
3269                 if (!status && (sk->sk_state == BT_CONNECTED ||
3270                                                 sk->sk_state == BT_CONFIG)) {
3271                         l2cap_check_encryption(sk, encrypt);
3272                         bh_unlock_sock(sk);
3273                         continue;
3274                 }
3275
3276                 if (sk->sk_state == BT_CONNECT) {
3277                         if (!status) {
3278                                 struct l2cap_conn_req req;
3279                                 req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
3280                                 req.psm  = l2cap_pi(sk)->psm;
3281
3282                                 l2cap_pi(sk)->ident = l2cap_get_ident(conn);
3283
3284                                 l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
3285                                         L2CAP_CONN_REQ, sizeof(req), &req);
3286                         } else {
3287                                 l2cap_sock_clear_timer(sk);
3288                                 l2cap_sock_set_timer(sk, HZ / 10);
3289                         }
3290                 } else if (sk->sk_state == BT_CONNECT2) {
3291                         struct l2cap_conn_rsp rsp;
3292                         __u16 result;
3293
3294                         if (!status) {
3295                                 sk->sk_state = BT_CONFIG;
3296                                 result = L2CAP_CR_SUCCESS;
3297                         } else {
3298                                 sk->sk_state = BT_DISCONN;
3299                                 l2cap_sock_set_timer(sk, HZ / 10);
3300                                 result = L2CAP_CR_SEC_BLOCK;
3301                         }
3302
3303                         rsp.scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
3304                         rsp.dcid   = cpu_to_le16(l2cap_pi(sk)->scid);
3305                         rsp.result = cpu_to_le16(result);
3306                         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
3307                         l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
3308                                         L2CAP_CONN_RSP, sizeof(rsp), &rsp);
3309                 }
3310
3311                 bh_unlock_sock(sk);
3312         }
3313
3314         read_unlock(&l->lock);
3315
3316         return 0;
3317 }
3318
3319 static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
3320 {
3321         struct l2cap_conn *conn = hcon->l2cap_data;
3322
3323         if (!conn && !(conn = l2cap_conn_add(hcon, 0)))
3324                 goto drop;
3325
3326         BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
3327
3328         if (flags & ACL_START) {
3329                 struct l2cap_hdr *hdr;
3330                 int len;
3331
3332                 if (conn->rx_len) {
3333                         BT_ERR("Unexpected start frame (len %d)", skb->len);
3334                         kfree_skb(conn->rx_skb);
3335                         conn->rx_skb = NULL;
3336                         conn->rx_len = 0;
3337                         l2cap_conn_unreliable(conn, ECOMM);
3338                 }
3339
3340                 if (skb->len < 2) {
3341                         BT_ERR("Frame is too short (len %d)", skb->len);
3342                         l2cap_conn_unreliable(conn, ECOMM);
3343                         goto drop;
3344                 }
3345
3346                 hdr = (struct l2cap_hdr *) skb->data;
3347                 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
3348
3349                 if (len == skb->len) {
3350                         /* Complete frame received */
3351                         l2cap_recv_frame(conn, skb);
3352                         return 0;
3353                 }
3354
3355                 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
3356
3357                 if (skb->len > len) {
3358                         BT_ERR("Frame is too long (len %d, expected len %d)",
3359                                 skb->len, len);
3360                         l2cap_conn_unreliable(conn, ECOMM);
3361                         goto drop;
3362                 }
3363
3364                 /* Allocate skb for the complete frame (with header) */
3365                 conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC);
3366                 if (!conn->rx_skb)
3367                         goto drop;
3368
3369                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
3370                                                                 skb->len);
3371                 conn->rx_len = len - skb->len;
3372         } else {
3373                 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
3374
3375                 if (!conn->rx_len) {
3376                         BT_ERR("Unexpected continuation frame (len %d)", skb->len);
3377                         l2cap_conn_unreliable(conn, ECOMM);
3378                         goto drop;
3379                 }
3380
3381                 if (skb->len > conn->rx_len) {
3382                         BT_ERR("Fragment is too long (len %d, expected %d)",
3383                                         skb->len, conn->rx_len);
3384                         kfree_skb(conn->rx_skb);
3385                         conn->rx_skb = NULL;
3386                         conn->rx_len = 0;
3387                         l2cap_conn_unreliable(conn, ECOMM);
3388                         goto drop;
3389                 }
3390
3391                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
3392                                                                 skb->len);
3393                 conn->rx_len -= skb->len;
3394
3395                 if (!conn->rx_len) {
3396                         /* Complete frame received */
3397                         l2cap_recv_frame(conn, conn->rx_skb);
3398                         conn->rx_skb = NULL;
3399                 }
3400         }
3401
3402 drop:
3403         kfree_skb(skb);
3404         return 0;
3405 }
3406
3407 static ssize_t l2cap_sysfs_show(struct class *dev, char *buf)
3408 {
3409         struct sock *sk;
3410         struct hlist_node *node;
3411         char *str = buf;
3412
3413         read_lock_bh(&l2cap_sk_list.lock);
3414
3415         sk_for_each(sk, node, &l2cap_sk_list.head) {
3416                 struct l2cap_pinfo *pi = l2cap_pi(sk);
3417
3418                 str += sprintf(str, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d\n",
3419                                 batostr(&bt_sk(sk)->src), batostr(&bt_sk(sk)->dst),
3420                                 sk->sk_state, __le16_to_cpu(pi->psm), pi->scid,
3421                                 pi->dcid, pi->imtu, pi->omtu, pi->sec_level);
3422         }
3423
3424         read_unlock_bh(&l2cap_sk_list.lock);
3425
3426         return str - buf;
3427 }
3428
3429 static CLASS_ATTR(l2cap, S_IRUGO, l2cap_sysfs_show, NULL);
3430
3431 static const struct proto_ops l2cap_sock_ops = {
3432         .family         = PF_BLUETOOTH,
3433         .owner          = THIS_MODULE,
3434         .release        = l2cap_sock_release,
3435         .bind           = l2cap_sock_bind,
3436         .connect        = l2cap_sock_connect,
3437         .listen         = l2cap_sock_listen,
3438         .accept         = l2cap_sock_accept,
3439         .getname        = l2cap_sock_getname,
3440         .sendmsg        = l2cap_sock_sendmsg,
3441         .recvmsg        = l2cap_sock_recvmsg,
3442         .poll           = bt_sock_poll,
3443         .ioctl          = bt_sock_ioctl,
3444         .mmap           = sock_no_mmap,
3445         .socketpair     = sock_no_socketpair,
3446         .shutdown       = l2cap_sock_shutdown,
3447         .setsockopt     = l2cap_sock_setsockopt,
3448         .getsockopt     = l2cap_sock_getsockopt
3449 };
3450
3451 static struct net_proto_family l2cap_sock_family_ops = {
3452         .family = PF_BLUETOOTH,
3453         .owner  = THIS_MODULE,
3454         .create = l2cap_sock_create,
3455 };
3456
3457 static struct hci_proto l2cap_hci_proto = {
3458         .name           = "L2CAP",
3459         .id             = HCI_PROTO_L2CAP,
3460         .connect_ind    = l2cap_connect_ind,
3461         .connect_cfm    = l2cap_connect_cfm,
3462         .disconn_ind    = l2cap_disconn_ind,
3463         .disconn_cfm    = l2cap_disconn_cfm,
3464         .security_cfm   = l2cap_security_cfm,
3465         .recv_acldata   = l2cap_recv_acldata
3466 };
3467
3468 static int __init l2cap_init(void)
3469 {
3470         int err;
3471
3472         err = proto_register(&l2cap_proto, 0);
3473         if (err < 0)
3474                 return err;
3475
3476         err = bt_sock_register(BTPROTO_L2CAP, &l2cap_sock_family_ops);
3477         if (err < 0) {
3478                 BT_ERR("L2CAP socket registration failed");
3479                 goto error;
3480         }
3481
3482         err = hci_register_proto(&l2cap_hci_proto);
3483         if (err < 0) {
3484                 BT_ERR("L2CAP protocol registration failed");
3485                 bt_sock_unregister(BTPROTO_L2CAP);
3486                 goto error;
3487         }
3488
3489         if (class_create_file(bt_class, &class_attr_l2cap) < 0)
3490                 BT_ERR("Failed to create L2CAP info file");
3491
3492         BT_INFO("L2CAP ver %s", VERSION);
3493         BT_INFO("L2CAP socket layer initialized");
3494
3495         return 0;
3496
3497 error:
3498         proto_unregister(&l2cap_proto);
3499         return err;
3500 }
3501
3502 static void __exit l2cap_exit(void)
3503 {
3504         class_remove_file(bt_class, &class_attr_l2cap);
3505
3506         if (bt_sock_unregister(BTPROTO_L2CAP) < 0)
3507                 BT_ERR("L2CAP socket unregistration failed");
3508
3509         if (hci_unregister_proto(&l2cap_hci_proto) < 0)
3510                 BT_ERR("L2CAP protocol unregistration failed");
3511
3512         proto_unregister(&l2cap_proto);
3513 }
3514
3515 void l2cap_load(void)
3516 {
3517         /* Dummy function to trigger automatic L2CAP module loading by
3518          * other modules that use L2CAP sockets but don't use any other
3519          * symbols from it. */
3520         return;
3521 }
3522 EXPORT_SYMBOL(l2cap_load);
3523
3524 module_init(l2cap_init);
3525 module_exit(l2cap_exit);
3526
3527 module_param(enable_ertm, bool, 0644);
3528 MODULE_PARM_DESC(enable_ertm, "Enable enhanced retransmission mode");
3529
3530 MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
3531 MODULE_DESCRIPTION("Bluetooth L2CAP ver " VERSION);
3532 MODULE_VERSION(VERSION);
3533 MODULE_LICENSE("GPL");
3534 MODULE_ALIAS("bt-proto-0");