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 <net/sock.h>
  44
  45 #include <asm/system.h>
  46 #include <asm/uaccess.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 #ifndef CONFIG_BT_L2CAP_DEBUG
  54 #undef  BT_DBG
  55 #define BT_DBG(D...)
  56 #endif
  57
  58 #define VERSION "2.9"
  59
  60 static u32 l2cap_feat_mask = 0x0000;
  61
  62 static const struct proto_ops l2cap_sock_ops;
  63
  64 static struct bt_sock_list l2cap_sk_list = {
  65         .lock = RW_LOCK_UNLOCKED
  66 };
  67
  68 static void __l2cap_sock_close(struct sock *sk, int reason);
  69 static void l2cap_sock_close(struct sock *sk);
  70 static void l2cap_sock_kill(struct sock *sk);
  71
  72 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
  73                                 u8 code, u8 ident, u16 dlen, void *data);
  74
  75 /* ---- L2CAP timers ---- */
  76 static void l2cap_sock_timeout(unsigned long arg)
  77 {
  78         struct sock *sk = (struct sock *) arg;
  79
  80         BT_DBG("sock %p state %d", sk, sk->sk_state);
  81
  82         bh_lock_sock(sk);
  83         __l2cap_sock_close(sk, ETIMEDOUT);
  84         bh_unlock_sock(sk);
  85
  86         l2cap_sock_kill(sk);
  87         sock_put(sk);
  88 }
  89
  90 static void l2cap_sock_set_timer(struct sock *sk, long timeout)
  91 {
  92         BT_DBG("sk %p state %d timeout %ld", sk, sk->sk_state, timeout);
  93         sk_reset_timer(sk, &sk->sk_timer, jiffies + timeout);
  94 }
  95
  96 static void l2cap_sock_clear_timer(struct sock *sk)
  97 {
  98         BT_DBG("sock %p state %d", sk, sk->sk_state);
  99         sk_stop_timer(sk, &sk->sk_timer);
 100 }
 101
 102 static void l2cap_sock_init_timer(struct sock *sk)
 103 {
 104         init_timer(&sk->sk_timer);
 105         sk->sk_timer.function = l2cap_sock_timeout;
 106         sk->sk_timer.data = (unsigned long)sk;
 107 }
 108
 109 /* ---- L2CAP channels ---- */
 110 static struct sock *__l2cap_get_chan_by_dcid(struct l2cap_chan_list *l, u16 cid)
 111 {
 112         struct sock *s;
 113         for (s = l->head; s; s = l2cap_pi(s)->next_c) {
 114                 if (l2cap_pi(s)->dcid == cid)
 115                         break;
 116         }
 117         return s;
 118 }
 119
 120 static struct sock *__l2cap_get_chan_by_scid(struct l2cap_chan_list *l, u16 cid)
 121 {
 122         struct sock *s;
 123         for (s = l->head; s; s = l2cap_pi(s)->next_c) {
 124                 if (l2cap_pi(s)->scid == cid)
 125                         break;
 126         }
 127         return s;
 128 }
 129
 130 /* Find channel with given SCID.
 131  * Returns locked socket */
 132 static inline struct sock *l2cap_get_chan_by_scid(struct l2cap_chan_list *l, u16 cid)
 133 {
 134         struct sock *s;
 135         read_lock(&l->lock);
 136         s = __l2cap_get_chan_by_scid(l, cid);
 137         if (s) bh_lock_sock(s);
 138         read_unlock(&l->lock);
 139         return s;
 140 }
 141
 142 static struct sock *__l2cap_get_chan_by_ident(struct l2cap_chan_list *l, u8 ident)
 143 {
 144         struct sock *s;
 145         for (s = l->head; s; s = l2cap_pi(s)->next_c) {
 146                 if (l2cap_pi(s)->ident == ident)
 147                         break;
 148         }
 149         return s;
 150 }
 151
 152 static inline struct sock *l2cap_get_chan_by_ident(struct l2cap_chan_list *l, u8 ident)
 153 {
 154         struct sock *s;
 155         read_lock(&l->lock);
 156         s = __l2cap_get_chan_by_ident(l, ident);
 157         if (s) bh_lock_sock(s);
 158         read_unlock(&l->lock);
 159         return s;
 160 }
 161
 162 static u16 l2cap_alloc_cid(struct l2cap_chan_list *l)
 163 {
 164         u16 cid = 0x0040;
 165
 166         for (; cid < 0xffff; cid++) {
 167                 if(!__l2cap_get_chan_by_scid(l, cid))
 168                         return cid;
 169         }
 170
 171         return 0;
 172 }
 173
 174 static inline void __l2cap_chan_link(struct l2cap_chan_list *l, struct sock *sk)
 175 {
 176         sock_hold(sk);
 177
 178         if (l->head)
 179                 l2cap_pi(l->head)->prev_c = sk;
 180
 181         l2cap_pi(sk)->next_c = l->head;
 182         l2cap_pi(sk)->prev_c = NULL;
 183         l->head = sk;
 184 }
 185
 186 static inline void l2cap_chan_unlink(struct l2cap_chan_list *l, struct sock *sk)
 187 {
 188         struct sock *next = l2cap_pi(sk)->next_c, *prev = l2cap_pi(sk)->prev_c;
 189
 190         write_lock_bh(&l->lock);
 191         if (sk == l->head)
 192                 l->head = next;
 193
 194         if (next)
 195                 l2cap_pi(next)->prev_c = prev;
 196         if (prev)
 197                 l2cap_pi(prev)->next_c = next;
 198         write_unlock_bh(&l->lock);
 199
 200         __sock_put(sk);
 201 }
 202
 203 static void __l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct sock *parent)
 204 {
 205         struct l2cap_chan_list *l = &conn->chan_list;
 206
 207         BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn, l2cap_pi(sk)->psm, l2cap_pi(sk)->dcid);
 208
 209         l2cap_pi(sk)->conn = conn;
 210
 211         if (sk->sk_type == SOCK_SEQPACKET) {
 212                 /* Alloc CID for connection-oriented socket */
 213                 l2cap_pi(sk)->scid = l2cap_alloc_cid(l);
 214         } else if (sk->sk_type == SOCK_DGRAM) {
 215                 /* Connectionless socket */
 216                 l2cap_pi(sk)->scid = 0x0002;
 217                 l2cap_pi(sk)->dcid = 0x0002;
 218                 l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU;
 219         } else {
 220                 /* Raw socket can send/recv signalling messages only */
 221                 l2cap_pi(sk)->scid = 0x0001;
 222                 l2cap_pi(sk)->dcid = 0x0001;
 223                 l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU;
 224         }
 225
 226         __l2cap_chan_link(l, sk);
 227
 228         if (parent)
 229                 bt_accept_enqueue(parent, sk);
 230 }
 231
 232 /* Delete channel.
 233  * Must be called on the locked socket. */
 234 static void l2cap_chan_del(struct sock *sk, int err)
 235 {
 236         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
 237         struct sock *parent = bt_sk(sk)->parent;
 238
 239         l2cap_sock_clear_timer(sk);
 240
 241         BT_DBG("sk %p, conn %p, err %d", sk, conn, err);
 242
 243         if (conn) {
 244                 /* Unlink from channel list */
 245                 l2cap_chan_unlink(&conn->chan_list, sk);
 246                 l2cap_pi(sk)->conn = NULL;
 247                 hci_conn_put(conn->hcon);
 248         }
 249
 250         sk->sk_state  = BT_CLOSED;
 251         sock_set_flag(sk, SOCK_ZAPPED);
 252
 253         if (err)
 254                 sk->sk_err = err;
 255
 256         if (parent) {
 257                 bt_accept_unlink(sk);
 258                 parent->sk_data_ready(parent, 0);
 259         } else
 260                 sk->sk_state_change(sk);
 261 }
 262
 263 static inline u8 l2cap_get_ident(struct l2cap_conn *conn)
 264 {
 265         u8 id;
 266
 267         /* Get next available identificator.
 268          *    1 - 128 are used by kernel.
 269          *  129 - 199 are reserved.
 270          *  200 - 254 are used by utilities like l2ping, etc.
 271          */
 272
 273         spin_lock_bh(&conn->lock);
 274
 275         if (++conn->tx_ident > 128)
 276                 conn->tx_ident = 1;
 277
 278         id = conn->tx_ident;
 279
 280         spin_unlock_bh(&conn->lock);
 281
 282         return id;
 283 }
 284
 285 static inline int l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, void *data)
 286 {
 287         struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data);
 288
 289         BT_DBG("code 0x%2.2x", code);
 290
 291         if (!skb)
 292                 return -ENOMEM;
 293
 294         return hci_send_acl(conn->hcon, skb, 0);
 295 }
 296
 297 /* ---- L2CAP connections ---- */
 298 static void l2cap_conn_start(struct l2cap_conn *conn)
 299 {
 300         struct l2cap_chan_list *l = &conn->chan_list;
 301         struct sock *sk;
 302
 303         BT_DBG("conn %p", conn);
 304
 305         read_lock(&l->lock);
 306
 307         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
 308                 bh_lock_sock(sk);
 309
 310                 if (sk->sk_type != SOCK_SEQPACKET) {
 311                         l2cap_sock_clear_timer(sk);
 312                         sk->sk_state = BT_CONNECTED;
 313                         sk->sk_state_change(sk);
 314                 } else if (sk->sk_state == BT_CONNECT) {
 315                         struct l2cap_conn_req req;
 316                         l2cap_pi(sk)->ident = l2cap_get_ident(conn);
 317                         req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
 318                         req.psm  = l2cap_pi(sk)->psm;
 319                         l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
 320                                         L2CAP_CONN_REQ, sizeof(req), &req);
 321                 }
 322
 323                 bh_unlock_sock(sk);
 324         }
 325
 326         read_unlock(&l->lock);
 327 }
 328
 329 static void l2cap_conn_ready(struct l2cap_conn *conn)
 330 {
 331         BT_DBG("conn %p", conn);
 332
 333         if (conn->chan_list.head || !hlist_empty(&l2cap_sk_list.head)) {
 334                 struct l2cap_info_req req;
 335
 336                 req.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
 337
 338                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
 339                 conn->info_ident = l2cap_get_ident(conn);
 340
 341                 mod_timer(&conn->info_timer,
 342                         jiffies + msecs_to_jiffies(L2CAP_INFO_TIMEOUT));
 343
 344                 l2cap_send_cmd(conn, conn->info_ident,
 345                                         L2CAP_INFO_REQ, sizeof(req), &req);
 346         }
 347 }
 348
 349 /* Notify sockets that we cannot guaranty reliability anymore */
 350 static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err)
 351 {
 352         struct l2cap_chan_list *l = &conn->chan_list;
 353         struct sock *sk;
 354
 355         BT_DBG("conn %p", conn);
 356
 357         read_lock(&l->lock);
 358
 359         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
 360                 if (l2cap_pi(sk)->link_mode & L2CAP_LM_RELIABLE)
 361                         sk->sk_err = err;
 362         }
 363
 364         read_unlock(&l->lock);
 365 }
 366
 367 static void l2cap_info_timeout(unsigned long arg)
 368 {
 369         struct l2cap_conn *conn = (void *) arg;
 370
 371         conn->info_ident = 0;
 372
 373         l2cap_conn_start(conn);
 374 }
 375
 376 static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status)
 377 {
 378         struct l2cap_conn *conn = hcon->l2cap_data;
 379
 380         if (conn || status)
 381                 return conn;
 382
 383         conn = kzalloc(sizeof(struct l2cap_conn), GFP_ATOMIC);
 384         if (!conn)
 385                 return NULL;
 386
 387         hcon->l2cap_data = conn;
 388         conn->hcon = hcon;
 389
 390         BT_DBG("hcon %p conn %p", hcon, conn);
 391
 392         conn->mtu = hcon->hdev->acl_mtu;
 393         conn->src = &hcon->hdev->bdaddr;
 394         conn->dst = &hcon->dst;
 395
 396         conn->feat_mask = 0;
 397
 398         init_timer(&conn->info_timer);
 399         conn->info_timer.function = l2cap_info_timeout;
 400         conn->info_timer.data = (unsigned long) conn;
 401
 402         spin_lock_init(&conn->lock);
 403         rwlock_init(&conn->chan_list.lock);
 404
 405         return conn;
 406 }
 407
 408 static void l2cap_conn_del(struct hci_conn *hcon, int err)
 409 {
 410         struct l2cap_conn *conn = hcon->l2cap_data;
 411         struct sock *sk;
 412
 413         if (!conn)
 414                 return;
 415
 416         BT_DBG("hcon %p conn %p, err %d", hcon, conn, err);
 417
 418         if (conn->rx_skb)
 419                 kfree_skb(conn->rx_skb);
 420
 421         /* Kill channels */
 422         while ((sk = conn->chan_list.head)) {
 423                 bh_lock_sock(sk);
 424                 l2cap_chan_del(sk, err);
 425                 bh_unlock_sock(sk);
 426                 l2cap_sock_kill(sk);
 427         }
 428
 429         hcon->l2cap_data = NULL;
 430         kfree(conn);
 431 }
 432
 433 static inline void l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct sock *parent)
 434 {
 435         struct l2cap_chan_list *l = &conn->chan_list;
 436         write_lock_bh(&l->lock);
 437         __l2cap_chan_add(conn, sk, parent);
 438         write_unlock_bh(&l->lock);
 439 }
 440
 441 /* ---- Socket interface ---- */
 442 static struct sock *__l2cap_get_sock_by_addr(__le16 psm, bdaddr_t *src)
 443 {
 444         struct sock *sk;
 445         struct hlist_node *node;
 446         sk_for_each(sk, node, &l2cap_sk_list.head)
 447                 if (l2cap_pi(sk)->sport == psm && !bacmp(&bt_sk(sk)->src, src))
 448                         goto found;
 449         sk = NULL;
 450 found:
 451         return sk;
 452 }
 453
 454 /* Find socket with psm and source bdaddr.
 455  * Returns closest match.
 456  */
 457 static struct sock *__l2cap_get_sock_by_psm(int state, __le16 psm, bdaddr_t *src)
 458 {
 459         struct sock *sk = NULL, *sk1 = NULL;
 460         struct hlist_node *node;
 461
 462         sk_for_each(sk, node, &l2cap_sk_list.head) {
 463                 if (state && sk->sk_state != state)
 464                         continue;
 465
 466                 if (l2cap_pi(sk)->psm == psm) {
 467                         /* Exact match. */
 468                         if (!bacmp(&bt_sk(sk)->src, src))
 469                                 break;
 470
 471                         /* Closest match */
 472                         if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
 473                                 sk1 = sk;
 474                 }
 475         }
 476         return node ? sk : sk1;
 477 }
 478
 479 /* Find socket with given address (psm, src).
 480  * Returns locked socket */
 481 static inline struct sock *l2cap_get_sock_by_psm(int state, __le16 psm, bdaddr_t *src)
 482 {
 483         struct sock *s;
 484         read_lock(&l2cap_sk_list.lock);
 485         s = __l2cap_get_sock_by_psm(state, psm, src);
 486         if (s) bh_lock_sock(s);
 487         read_unlock(&l2cap_sk_list.lock);
 488         return s;
 489 }
 490
 491 static void l2cap_sock_destruct(struct sock *sk)
 492 {
 493         BT_DBG("sk %p", sk);
 494
 495         skb_queue_purge(&sk->sk_receive_queue);
 496         skb_queue_purge(&sk->sk_write_queue);
 497 }
 498
 499 static void l2cap_sock_cleanup_listen(struct sock *parent)
 500 {
 501         struct sock *sk;
 502
 503         BT_DBG("parent %p", parent);
 504
 505         /* Close not yet accepted channels */
 506         while ((sk = bt_accept_dequeue(parent, NULL)))
 507                 l2cap_sock_close(sk);
 508
 509         parent->sk_state  = BT_CLOSED;
 510         sock_set_flag(parent, SOCK_ZAPPED);
 511 }
 512
 513 /* Kill socket (only if zapped and orphan)
 514  * Must be called on unlocked socket.
 515  */
 516 static void l2cap_sock_kill(struct sock *sk)
 517 {
 518         if (!sock_flag(sk, SOCK_ZAPPED) || sk->sk_socket)
 519                 return;
 520
 521         BT_DBG("sk %p state %d", sk, sk->sk_state);
 522
 523         /* Kill poor orphan */
 524         bt_sock_unlink(&l2cap_sk_list, sk);
 525         sock_set_flag(sk, SOCK_DEAD);
 526         sock_put(sk);
 527 }
 528
 529 static void __l2cap_sock_close(struct sock *sk, int reason)
 530 {
 531         BT_DBG("sk %p state %d socket %p", sk, sk->sk_state, sk->sk_socket);
 532
 533         switch (sk->sk_state) {
 534         case BT_LISTEN:
 535                 l2cap_sock_cleanup_listen(sk);
 536                 break;
 537
 538         case BT_CONNECTED:
 539         case BT_CONFIG:
 540         case BT_CONNECT2:
 541                 if (sk->sk_type == SOCK_SEQPACKET) {
 542                         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
 543                         struct l2cap_disconn_req req;
 544
 545                         sk->sk_state = BT_DISCONN;
 546                         l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
 547
 548                         req.dcid = cpu_to_le16(l2cap_pi(sk)->dcid);
 549                         req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
 550                         l2cap_send_cmd(conn, l2cap_get_ident(conn),
 551                                         L2CAP_DISCONN_REQ, sizeof(req), &req);
 552                 } else {
 553                         l2cap_chan_del(sk, reason);
 554                 }
 555                 break;
 556
 557         case BT_CONNECT:
 558         case BT_DISCONN:
 559                 l2cap_chan_del(sk, reason);
 560                 break;
 561
 562         default:
 563                 sock_set_flag(sk, SOCK_ZAPPED);
 564                 break;
 565         }
 566 }
 567
 568 /* Must be called on unlocked socket. */
 569 static void l2cap_sock_close(struct sock *sk)
 570 {
 571         l2cap_sock_clear_timer(sk);
 572         lock_sock(sk);
 573         __l2cap_sock_close(sk, ECONNRESET);
 574         release_sock(sk);
 575         l2cap_sock_kill(sk);
 576 }
 577
 578 static void l2cap_sock_init(struct sock *sk, struct sock *parent)
 579 {
 580         struct l2cap_pinfo *pi = l2cap_pi(sk);
 581
 582         BT_DBG("sk %p", sk);
 583
 584         if (parent) {
 585                 sk->sk_type = parent->sk_type;
 586                 pi->imtu = l2cap_pi(parent)->imtu;
 587                 pi->omtu = l2cap_pi(parent)->omtu;
 588                 pi->link_mode = l2cap_pi(parent)->link_mode;
 589         } else {
 590                 pi->imtu = L2CAP_DEFAULT_MTU;
 591                 pi->omtu = 0;
 592                 pi->link_mode = 0;
 593         }
 594
 595         /* Default config options */
 596         pi->conf_len = 0;
 597         pi->flush_to = L2CAP_DEFAULT_FLUSH_TO;
 598 }
 599
 600 static struct proto l2cap_proto = {
 601         .name           = "L2CAP",
 602         .owner          = THIS_MODULE,
 603         .obj_size       = sizeof(struct l2cap_pinfo)
 604 };
 605
 606 static struct sock *l2cap_sock_alloc(struct net *net, struct socket *sock, int proto, gfp_t prio)
 607 {
 608         struct sock *sk;
 609
 610         sk = sk_alloc(net, PF_BLUETOOTH, prio, &l2cap_proto, 1);
 611         if (!sk)
 612                 return NULL;
 613
 614         sock_init_data(sock, sk);
 615         INIT_LIST_HEAD(&bt_sk(sk)->accept_q);
 616
 617         sk->sk_destruct = l2cap_sock_destruct;
 618         sk->sk_sndtimeo = msecs_to_jiffies(L2CAP_CONN_TIMEOUT);
 619
 620         sock_reset_flag(sk, SOCK_ZAPPED);
 621
 622         sk->sk_protocol = proto;
 623         sk->sk_state    = BT_OPEN;
 624
 625         l2cap_sock_init_timer(sk);
 626
 627         bt_sock_link(&l2cap_sk_list, sk);
 628         return sk;
 629 }
 630
 631 static int l2cap_sock_create(struct net *net, struct socket *sock, int protocol)
 632 {
 633         struct sock *sk;
 634
 635         BT_DBG("sock %p", sock);
 636
 637         sock->state = SS_UNCONNECTED;
 638
 639         if (sock->type != SOCK_SEQPACKET &&
 640                         sock->type != SOCK_DGRAM && sock->type != SOCK_RAW)
 641                 return -ESOCKTNOSUPPORT;
 642
 643         if (sock->type == SOCK_RAW && !capable(CAP_NET_RAW))
 644                 return -EPERM;
 645
 646         sock->ops = &l2cap_sock_ops;
 647
 648         sk = l2cap_sock_alloc(net, sock, protocol, GFP_ATOMIC);
 649         if (!sk)
 650                 return -ENOMEM;
 651
 652         l2cap_sock_init(sk, NULL);
 653         return 0;
 654 }
 655
 656 static int l2cap_sock_bind(struct socket *sock, struct sockaddr *addr, int addr_len)
 657 {
 658         struct sockaddr_l2 *la = (struct sockaddr_l2 *) addr;
 659         struct sock *sk = sock->sk;
 660         int err = 0;
 661
 662         BT_DBG("sk %p, %s %d", sk, batostr(&la->l2_bdaddr), la->l2_psm);
 663
 664         if (!addr || addr->sa_family != AF_BLUETOOTH)
 665                 return -EINVAL;
 666
 667         lock_sock(sk);
 668
 669         if (sk->sk_state != BT_OPEN) {
 670                 err = -EBADFD;
 671                 goto done;
 672         }
 673
 674         if (la->l2_psm && btohs(la->l2_psm) < 0x1001 &&
 675                                 !capable(CAP_NET_BIND_SERVICE)) {
 676                 err = -EACCES;
 677                 goto done;
 678         }
 679
 680         write_lock_bh(&l2cap_sk_list.lock);
 681
 682         if (la->l2_psm && __l2cap_get_sock_by_addr(la->l2_psm, &la->l2_bdaddr)) {
 683                 err = -EADDRINUSE;
 684         } else {
 685                 /* Save source address */
 686                 bacpy(&bt_sk(sk)->src, &la->l2_bdaddr);
 687                 l2cap_pi(sk)->psm   = la->l2_psm;
 688                 l2cap_pi(sk)->sport = la->l2_psm;
 689                 sk->sk_state = BT_BOUND;
 690         }
 691
 692         write_unlock_bh(&l2cap_sk_list.lock);
 693
 694 done:
 695         release_sock(sk);
 696         return err;
 697 }
 698
 699 static int l2cap_do_connect(struct sock *sk)
 700 {
 701         bdaddr_t *src = &bt_sk(sk)->src;
 702         bdaddr_t *dst = &bt_sk(sk)->dst;
 703         struct l2cap_conn *conn;
 704         struct hci_conn *hcon;
 705         struct hci_dev *hdev;
 706         int err = 0;
 707
 708         BT_DBG("%s -> %s psm 0x%2.2x", batostr(src), batostr(dst), l2cap_pi(sk)->psm);
 709
 710         if (!(hdev = hci_get_route(dst, src)))
 711                 return -EHOSTUNREACH;
 712
 713         hci_dev_lock_bh(hdev);
 714
 715         err = -ENOMEM;
 716
 717         hcon = hci_connect(hdev, ACL_LINK, dst);
 718         if (!hcon)
 719                 goto done;
 720
 721         conn = l2cap_conn_add(hcon, 0);
 722         if (!conn) {
 723                 hci_conn_put(hcon);
 724                 goto done;
 725         }
 726
 727         err = 0;
 728
 729         /* Update source addr of the socket */
 730         bacpy(src, conn->src);
 731
 732         l2cap_chan_add(conn, sk, NULL);
 733
 734         sk->sk_state = BT_CONNECT;
 735         l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
 736
 737         if (hcon->state == BT_CONNECTED) {
 738                 if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT)) {
 739                         l2cap_conn_ready(conn);
 740                         goto done;
 741                 }
 742
 743                 if (sk->sk_type == SOCK_SEQPACKET) {
 744                         struct l2cap_conn_req req;
 745                         l2cap_pi(sk)->ident = l2cap_get_ident(conn);
 746                         req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
 747                         req.psm  = l2cap_pi(sk)->psm;
 748                         l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
 749                                         L2CAP_CONN_REQ, sizeof(req), &req);
 750                 } else {
 751                         l2cap_sock_clear_timer(sk);
 752                         sk->sk_state = BT_CONNECTED;
 753                 }
 754         }
 755
 756 done:
 757         hci_dev_unlock_bh(hdev);
 758         hci_dev_put(hdev);
 759         return err;
 760 }
 761
 762 static int l2cap_sock_connect(struct socket *sock, struct sockaddr *addr, int alen, int flags)
 763 {
 764         struct sockaddr_l2 *la = (struct sockaddr_l2 *) addr;
 765         struct sock *sk = sock->sk;
 766         int err = 0;
 767
 768         lock_sock(sk);
 769
 770         BT_DBG("sk %p", sk);
 771
 772         if (addr->sa_family != AF_BLUETOOTH || alen < sizeof(struct sockaddr_l2)) {
 773                 err = -EINVAL;
 774                 goto done;
 775         }
 776
 777         if (sk->sk_type == SOCK_SEQPACKET && !la->l2_psm) {
 778                 err = -EINVAL;
 779                 goto done;
 780         }
 781
 782         switch(sk->sk_state) {
 783         case BT_CONNECT:
 784         case BT_CONNECT2:
 785         case BT_CONFIG:
 786                 /* Already connecting */
 787                 goto wait;
 788
 789         case BT_CONNECTED:
 790                 /* Already connected */
 791                 goto done;
 792
 793         case BT_OPEN:
 794         case BT_BOUND:
 795                 /* Can connect */
 796                 break;
 797
 798         default:
 799                 err = -EBADFD;
 800                 goto done;
 801         }
 802
 803         /* Set destination address and psm */
 804         bacpy(&bt_sk(sk)->dst, &la->l2_bdaddr);
 805         l2cap_pi(sk)->psm = la->l2_psm;
 806
 807         if ((err = l2cap_do_connect(sk)))
 808                 goto done;
 809
 810 wait:
 811         err = bt_sock_wait_state(sk, BT_CONNECTED,
 812                         sock_sndtimeo(sk, flags & O_NONBLOCK));
 813 done:
 814         release_sock(sk);
 815         return err;
 816 }
 817
 818 static int l2cap_sock_listen(struct socket *sock, int backlog)
 819 {
 820         struct sock *sk = sock->sk;
 821         int err = 0;
 822
 823         BT_DBG("sk %p backlog %d", sk, backlog);
 824
 825         lock_sock(sk);
 826
 827         if (sk->sk_state != BT_BOUND || sock->type != SOCK_SEQPACKET) {
 828                 err = -EBADFD;
 829                 goto done;
 830         }
 831
 832         if (!l2cap_pi(sk)->psm) {
 833                 bdaddr_t *src = &bt_sk(sk)->src;
 834                 u16 psm;
 835
 836                 err = -EINVAL;
 837
 838                 write_lock_bh(&l2cap_sk_list.lock);
 839
 840                 for (psm = 0x1001; psm < 0x1100; psm += 2)
 841                         if (!__l2cap_get_sock_by_addr(htobs(psm), src)) {
 842                                 l2cap_pi(sk)->psm   = htobs(psm);
 843                                 l2cap_pi(sk)->sport = htobs(psm);
 844                                 err = 0;
 845                                 break;
 846                         }
 847
 848                 write_unlock_bh(&l2cap_sk_list.lock);
 849
 850                 if (err < 0)
 851                         goto done;
 852         }
 853
 854         sk->sk_max_ack_backlog = backlog;
 855         sk->sk_ack_backlog = 0;
 856         sk->sk_state = BT_LISTEN;
 857
 858 done:
 859         release_sock(sk);
 860         return err;
 861 }
 862
 863 static int l2cap_sock_accept(struct socket *sock, struct socket *newsock, int flags)
 864 {
 865         DECLARE_WAITQUEUE(wait, current);
 866         struct sock *sk = sock->sk, *nsk;
 867         long timeo;
 868         int err = 0;
 869
 870         lock_sock_nested(sk, SINGLE_DEPTH_NESTING);
 871
 872         if (sk->sk_state != BT_LISTEN) {
 873                 err = -EBADFD;
 874                 goto done;
 875         }
 876
 877         timeo = sock_rcvtimeo(sk, flags & O_NONBLOCK);
 878
 879         BT_DBG("sk %p timeo %ld", sk, timeo);
 880
 881         /* Wait for an incoming connection. (wake-one). */
 882         add_wait_queue_exclusive(sk->sk_sleep, &wait);
 883         while (!(nsk = bt_accept_dequeue(sk, newsock))) {
 884                 set_current_state(TASK_INTERRUPTIBLE);
 885                 if (!timeo) {
 886                         err = -EAGAIN;
 887                         break;
 888                 }
 889
 890                 release_sock(sk);
 891                 timeo = schedule_timeout(timeo);
 892                 lock_sock_nested(sk, SINGLE_DEPTH_NESTING);
 893
 894                 if (sk->sk_state != BT_LISTEN) {
 895                         err = -EBADFD;
 896                         break;
 897                 }
 898
 899                 if (signal_pending(current)) {
 900                         err = sock_intr_errno(timeo);
 901                         break;
 902                 }
 903         }
 904         set_current_state(TASK_RUNNING);
 905         remove_wait_queue(sk->sk_sleep, &wait);
 906
 907         if (err)
 908                 goto done;
 909
 910         newsock->state = SS_CONNECTED;
 911
 912         BT_DBG("new socket %p", nsk);
 913
 914 done:
 915         release_sock(sk);
 916         return err;
 917 }
 918
 919 static int l2cap_sock_getname(struct socket *sock, struct sockaddr *addr, int *len, int peer)
 920 {
 921         struct sockaddr_l2 *la = (struct sockaddr_l2 *) addr;
 922         struct sock *sk = sock->sk;
 923
 924         BT_DBG("sock %p, sk %p", sock, sk);
 925
 926         addr->sa_family = AF_BLUETOOTH;
 927         *len = sizeof(struct sockaddr_l2);
 928
 929         if (peer)
 930                 bacpy(&la->l2_bdaddr, &bt_sk(sk)->dst);
 931         else
 932                 bacpy(&la->l2_bdaddr, &bt_sk(sk)->src);
 933
 934         la->l2_psm = l2cap_pi(sk)->psm;
 935         return 0;
 936 }
 937
 938 static inline int l2cap_do_send(struct sock *sk, struct msghdr *msg, int len)
 939 {
 940         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
 941         struct sk_buff *skb, **frag;
 942         int err, hlen, count, sent=0;
 943         struct l2cap_hdr *lh;
 944
 945         BT_DBG("sk %p len %d", sk, len);
 946
 947         /* First fragment (with L2CAP header) */
 948         if (sk->sk_type == SOCK_DGRAM)
 949                 hlen = L2CAP_HDR_SIZE + 2;
 950         else
 951                 hlen = L2CAP_HDR_SIZE;
 952
 953         count = min_t(unsigned int, (conn->mtu - hlen), len);
 954
 955         skb = bt_skb_send_alloc(sk, hlen + count,
 956                         msg->msg_flags & MSG_DONTWAIT, &err);
 957         if (!skb)
 958                 return err;
 959
 960         /* Create L2CAP header */
 961         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
 962         lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
 963         lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
 964
 965         if (sk->sk_type == SOCK_DGRAM)
 966                 put_unaligned(l2cap_pi(sk)->psm, (__le16 *) skb_put(skb, 2));
 967
 968         if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count)) {
 969                 err = -EFAULT;
 970                 goto fail;
 971         }
 972
 973         sent += count;
 974         len  -= count;
 975
 976         /* Continuation fragments (no L2CAP header) */
 977         frag = &skb_shinfo(skb)->frag_list;
 978         while (len) {
 979                 count = min_t(unsigned int, conn->mtu, len);
 980
 981                 *frag = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err);
 982                 if (!*frag)
 983                         goto fail;
 984
 985                 if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count)) {
 986                         err = -EFAULT;
 987                         goto fail;
 988                 }
 989
 990                 sent += count;
 991                 len  -= count;
 992
 993                 frag = &(*frag)->next;
 994         }
 995
 996         if ((err = hci_send_acl(conn->hcon, skb, 0)) < 0)
 997                 goto fail;
 998
 999         return sent;
1000
1001 fail:
1002         kfree_skb(skb);
1003         return err;
1004 }
1005
1006 static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len)
1007 {
1008         struct sock *sk = sock->sk;
1009         int err = 0;
1010
1011         BT_DBG("sock %p, sk %p", sock, sk);
1012
1013         err = sock_error(sk);
1014         if (err)
1015                 return err;
1016
1017         if (msg->msg_flags & MSG_OOB)
1018                 return -EOPNOTSUPP;
1019
1020         /* Check outgoing MTU */
1021         if (sk->sk_type != SOCK_RAW && len > l2cap_pi(sk)->omtu)
1022                 return -EINVAL;
1023
1024         lock_sock(sk);
1025
1026         if (sk->sk_state == BT_CONNECTED)
1027                 err = l2cap_do_send(sk, msg, len);
1028         else
1029                 err = -ENOTCONN;
1030
1031         release_sock(sk);
1032         return err;
1033 }
1034
1035 static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname, char __user *optval, int optlen)
1036 {
1037         struct sock *sk = sock->sk;
1038         struct l2cap_options opts;
1039         int err = 0, len;
1040         u32 opt;
1041
1042         BT_DBG("sk %p", sk);
1043
1044         lock_sock(sk);
1045
1046         switch (optname) {
1047         case L2CAP_OPTIONS:
1048                 opts.imtu     = l2cap_pi(sk)->imtu;
1049                 opts.omtu     = l2cap_pi(sk)->omtu;
1050                 opts.flush_to = l2cap_pi(sk)->flush_to;
1051                 opts.mode     = 0x00;
1052
1053                 len = min_t(unsigned int, sizeof(opts), optlen);
1054                 if (copy_from_user((char *) &opts, optval, len)) {
1055                         err = -EFAULT;
1056                         break;
1057                 }
1058
1059                 l2cap_pi(sk)->imtu  = opts.imtu;
1060                 l2cap_pi(sk)->omtu  = opts.omtu;
1061                 break;
1062
1063         case L2CAP_LM:
1064                 if (get_user(opt, (u32 __user *) optval)) {
1065                         err = -EFAULT;
1066                         break;
1067                 }
1068
1069                 l2cap_pi(sk)->link_mode = opt;
1070                 break;
1071
1072         default:
1073                 err = -ENOPROTOOPT;
1074                 break;
1075         }
1076
1077         release_sock(sk);
1078         return err;
1079 }
1080
1081 static int l2cap_sock_getsockopt(struct socket *sock, int level, int optname, char __user *optval, int __user *optlen)
1082 {
1083         struct sock *sk = sock->sk;
1084         struct l2cap_options opts;
1085         struct l2cap_conninfo cinfo;
1086         int len, err = 0;
1087
1088         BT_DBG("sk %p", sk);
1089
1090         if (get_user(len, optlen))
1091                 return -EFAULT;
1092
1093         lock_sock(sk);
1094
1095         switch (optname) {
1096         case L2CAP_OPTIONS:
1097                 opts.imtu     = l2cap_pi(sk)->imtu;
1098                 opts.omtu     = l2cap_pi(sk)->omtu;
1099                 opts.flush_to = l2cap_pi(sk)->flush_to;
1100                 opts.mode     = 0x00;
1101
1102                 len = min_t(unsigned int, len, sizeof(opts));
1103                 if (copy_to_user(optval, (char *) &opts, len))
1104                         err = -EFAULT;
1105
1106                 break;
1107
1108         case L2CAP_LM:
1109                 if (put_user(l2cap_pi(sk)->link_mode, (u32 __user *) optval))
1110                         err = -EFAULT;
1111                 break;
1112
1113         case L2CAP_CONNINFO:
1114                 if (sk->sk_state != BT_CONNECTED) {
1115                         err = -ENOTCONN;
1116                         break;
1117                 }
1118
1119                 cinfo.hci_handle = l2cap_pi(sk)->conn->hcon->handle;
1120                 memcpy(cinfo.dev_class, l2cap_pi(sk)->conn->hcon->dev_class, 3);
1121
1122                 len = min_t(unsigned int, len, sizeof(cinfo));
1123                 if (copy_to_user(optval, (char *) &cinfo, len))
1124                         err = -EFAULT;
1125
1126                 break;
1127
1128         default:
1129                 err = -ENOPROTOOPT;
1130                 break;
1131         }
1132
1133         release_sock(sk);
1134         return err;
1135 }
1136
1137 static int l2cap_sock_shutdown(struct socket *sock, int how)
1138 {
1139         struct sock *sk = sock->sk;
1140         int err = 0;
1141
1142         BT_DBG("sock %p, sk %p", sock, sk);
1143
1144         if (!sk)
1145                 return 0;
1146
1147         lock_sock(sk);
1148         if (!sk->sk_shutdown) {
1149                 sk->sk_shutdown = SHUTDOWN_MASK;
1150                 l2cap_sock_clear_timer(sk);
1151                 __l2cap_sock_close(sk, 0);
1152
1153                 if (sock_flag(sk, SOCK_LINGER) && sk->sk_lingertime)
1154                         err = bt_sock_wait_state(sk, BT_CLOSED, sk->sk_lingertime);
1155         }
1156         release_sock(sk);
1157         return err;
1158 }
1159
1160 static int l2cap_sock_release(struct socket *sock)
1161 {
1162         struct sock *sk = sock->sk;
1163         int err;
1164
1165         BT_DBG("sock %p, sk %p", sock, sk);
1166
1167         if (!sk)
1168                 return 0;
1169
1170         err = l2cap_sock_shutdown(sock, 2);
1171
1172         sock_orphan(sk);
1173         l2cap_sock_kill(sk);
1174         return err;
1175 }
1176
1177 static void l2cap_chan_ready(struct sock *sk)
1178 {
1179         struct sock *parent = bt_sk(sk)->parent;
1180
1181         BT_DBG("sk %p, parent %p", sk, parent);
1182
1183         l2cap_pi(sk)->conf_state = 0;
1184         l2cap_sock_clear_timer(sk);
1185
1186         if (!parent) {
1187                 /* Outgoing channel.
1188                  * Wake up socket sleeping on connect.
1189                  */
1190                 sk->sk_state = BT_CONNECTED;
1191                 sk->sk_state_change(sk);
1192         } else {
1193                 /* Incoming channel.
1194                  * Wake up socket sleeping on accept.
1195                  */
1196                 parent->sk_data_ready(parent, 0);
1197         }
1198 }
1199
1200 /* Copy frame to all raw sockets on that connection */
1201 static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
1202 {
1203         struct l2cap_chan_list *l = &conn->chan_list;
1204         struct sk_buff *nskb;
1205         struct sock * sk;
1206
1207         BT_DBG("conn %p", conn);
1208
1209         read_lock(&l->lock);
1210         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
1211                 if (sk->sk_type != SOCK_RAW)
1212                         continue;
1213
1214                 /* Don't send frame to the socket it came from */
1215                 if (skb->sk == sk)
1216                         continue;
1217
1218                 if (!(nskb = skb_clone(skb, GFP_ATOMIC)))
1219                         continue;
1220
1221                 if (sock_queue_rcv_skb(sk, nskb))
1222                         kfree_skb(nskb);
1223         }
1224         read_unlock(&l->lock);
1225 }
1226
1227 /* ---- L2CAP signalling commands ---- */
1228 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
1229                                 u8 code, u8 ident, u16 dlen, void *data)
1230 {
1231         struct sk_buff *skb, **frag;
1232         struct l2cap_cmd_hdr *cmd;
1233         struct l2cap_hdr *lh;
1234         int len, count;
1235
1236         BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %d", conn, code, ident, dlen);
1237
1238         len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen;
1239         count = min_t(unsigned int, conn->mtu, len);
1240
1241         skb = bt_skb_alloc(count, GFP_ATOMIC);
1242         if (!skb)
1243                 return NULL;
1244
1245         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1246         lh->len = cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen);
1247         lh->cid = cpu_to_le16(0x0001);
1248
1249         cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE);
1250         cmd->code  = code;
1251         cmd->ident = ident;
1252         cmd->len   = cpu_to_le16(dlen);
1253
1254         if (dlen) {
1255                 count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE;
1256                 memcpy(skb_put(skb, count), data, count);
1257                 data += count;
1258         }
1259
1260         len -= skb->len;
1261
1262         /* Continuation fragments (no L2CAP header) */
1263         frag = &skb_shinfo(skb)->frag_list;
1264         while (len) {
1265                 count = min_t(unsigned int, conn->mtu, len);
1266
1267                 *frag = bt_skb_alloc(count, GFP_ATOMIC);
1268                 if (!*frag)
1269                         goto fail;
1270
1271                 memcpy(skb_put(*frag, count), data, count);
1272
1273                 len  -= count;
1274                 data += count;
1275
1276                 frag = &(*frag)->next;
1277         }
1278
1279         return skb;
1280
1281 fail:
1282         kfree_skb(skb);
1283         return NULL;
1284 }
1285
1286 static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val)
1287 {
1288         struct l2cap_conf_opt *opt = *ptr;
1289         int len;
1290
1291         len = L2CAP_CONF_OPT_SIZE + opt->len;
1292         *ptr += len;
1293
1294         *type = opt->type;
1295         *olen = opt->len;
1296
1297         switch (opt->len) {
1298         case 1:
1299                 *val = *((u8 *) opt->val);
1300                 break;
1301
1302         case 2:
1303                 *val = __le16_to_cpu(*((__le16 *) opt->val));
1304                 break;
1305
1306         case 4:
1307                 *val = __le32_to_cpu(*((__le32 *) opt->val));
1308                 break;
1309
1310         default:
1311                 *val = (unsigned long) opt->val;
1312                 break;
1313         }
1314
1315         BT_DBG("type 0x%2.2x len %d val 0x%lx", *type, opt->len, *val);
1316         return len;
1317 }
1318
1319 static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val)
1320 {
1321         struct l2cap_conf_opt *opt = *ptr;
1322
1323         BT_DBG("type 0x%2.2x len %d val 0x%lx", type, len, val);
1324
1325         opt->type = type;
1326         opt->len  = len;
1327
1328         switch (len) {
1329         case 1:
1330                 *((u8 *) opt->val)  = val;
1331                 break;
1332
1333         case 2:
1334                 *((__le16 *) opt->val) = cpu_to_le16(val);
1335                 break;
1336
1337         case 4:
1338                 *((__le32 *) opt->val) = cpu_to_le32(val);
1339                 break;
1340
1341         default:
1342                 memcpy(opt->val, (void *) val, len);
1343                 break;
1344         }
1345
1346         *ptr += L2CAP_CONF_OPT_SIZE + len;
1347 }
1348
1349 static int l2cap_build_conf_req(struct sock *sk, void *data)
1350 {
1351         struct l2cap_pinfo *pi = l2cap_pi(sk);
1352         struct l2cap_conf_req *req = data;
1353         void *ptr = req->data;
1354
1355         BT_DBG("sk %p", sk);
1356
1357         if (pi->imtu != L2CAP_DEFAULT_MTU)
1358                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->imtu);
1359
1360         /* FIXME: Need actual value of the flush timeout */
1361         //if (flush_to != L2CAP_DEFAULT_FLUSH_TO)
1362         //   l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO, 2, pi->flush_to);
1363
1364         req->dcid  = cpu_to_le16(pi->dcid);
1365         req->flags = cpu_to_le16(0);
1366
1367         return ptr - data;
1368 }
1369
1370 static int l2cap_parse_conf_req(struct sock *sk, void *data)
1371 {
1372         struct l2cap_pinfo *pi = l2cap_pi(sk);
1373         struct l2cap_conf_rsp *rsp = data;
1374         void *ptr = rsp->data;
1375         void *req = pi->conf_req;
1376         int len = pi->conf_len;
1377         int type, hint, olen;
1378         unsigned long val;
1379         u16 mtu = L2CAP_DEFAULT_MTU;
1380         u16 result = L2CAP_CONF_SUCCESS;
1381
1382         BT_DBG("sk %p", sk);
1383
1384         while (len >= L2CAP_CONF_OPT_SIZE) {
1385                 len -= l2cap_get_conf_opt(&req, &type, &olen, &val);
1386
1387                 hint  = type & 0x80;
1388                 type &= 0x7f;
1389
1390                 switch (type) {
1391                 case L2CAP_CONF_MTU:
1392                         mtu = val;
1393                         break;
1394
1395                 case L2CAP_CONF_FLUSH_TO:
1396                         pi->flush_to = val;
1397                         break;
1398
1399                 case L2CAP_CONF_QOS:
1400                         break;
1401
1402                 default:
1403                         if (hint)
1404                                 break;
1405
1406                         result = L2CAP_CONF_UNKNOWN;
1407                         *((u8 *) ptr++) = type;
1408                         break;
1409                 }
1410         }
1411
1412         if (result == L2CAP_CONF_SUCCESS) {
1413                 /* Configure output options and let the other side know
1414                  * which ones we don't like. */
1415
1416                 if (mtu < pi->omtu)
1417                         result = L2CAP_CONF_UNACCEPT;
1418                 else {
1419                         pi->omtu = mtu;
1420                         pi->conf_state |= L2CAP_CONF_OUTPUT_DONE;
1421                 }
1422
1423                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->omtu);
1424         }
1425
1426         rsp->scid   = cpu_to_le16(pi->dcid);
1427         rsp->result = cpu_to_le16(result);
1428         rsp->flags  = cpu_to_le16(0x0000);
1429
1430         return ptr - data;
1431 }
1432
1433 static int l2cap_build_conf_rsp(struct sock *sk, void *data, u16 result, u16 flags)
1434 {
1435         struct l2cap_conf_rsp *rsp = data;
1436         void *ptr = rsp->data;
1437
1438         BT_DBG("sk %p", sk);
1439
1440         rsp->scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
1441         rsp->result = cpu_to_le16(result);
1442         rsp->flags  = cpu_to_le16(flags);
1443
1444         return ptr - data;
1445 }
1446
1447 static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1448 {
1449         struct l2cap_cmd_rej *rej = (struct l2cap_cmd_rej *) data;
1450
1451         if (rej->reason != 0x0000)
1452                 return 0;
1453
1454         if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
1455                                         cmd->ident == conn->info_ident) {
1456                 conn->info_ident = 0;
1457                 del_timer(&conn->info_timer);
1458                 l2cap_conn_start(conn);
1459         }
1460
1461         return 0;
1462 }
1463
1464 static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1465 {
1466         struct l2cap_chan_list *list = &conn->chan_list;
1467         struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
1468         struct l2cap_conn_rsp rsp;
1469         struct sock *sk, *parent;
1470         int result = 0, status = 0;
1471
1472         u16 dcid = 0, scid = __le16_to_cpu(req->scid);
1473         __le16 psm  = req->psm;
1474
1475         BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid);
1476
1477         /* Check if we have socket listening on psm */
1478         parent = l2cap_get_sock_by_psm(BT_LISTEN, psm, conn->src);
1479         if (!parent) {
1480                 result = L2CAP_CR_BAD_PSM;
1481                 goto sendresp;
1482         }
1483
1484         result = L2CAP_CR_NO_MEM;
1485
1486         /* Check for backlog size */
1487         if (sk_acceptq_is_full(parent)) {
1488                 BT_DBG("backlog full %d", parent->sk_ack_backlog);
1489                 goto response;
1490         }
1491
1492         sk = l2cap_sock_alloc(parent->sk_net, NULL, BTPROTO_L2CAP, GFP_ATOMIC);
1493         if (!sk)
1494                 goto response;
1495
1496         write_lock_bh(&list->lock);
1497
1498         /* Check if we already have channel with that dcid */
1499         if (__l2cap_get_chan_by_dcid(list, scid)) {
1500                 write_unlock_bh(&list->lock);
1501                 sock_set_flag(sk, SOCK_ZAPPED);
1502                 l2cap_sock_kill(sk);
1503                 goto response;
1504         }
1505
1506         hci_conn_hold(conn->hcon);
1507
1508         l2cap_sock_init(sk, parent);
1509         bacpy(&bt_sk(sk)->src, conn->src);
1510         bacpy(&bt_sk(sk)->dst, conn->dst);
1511         l2cap_pi(sk)->psm  = psm;
1512         l2cap_pi(sk)->dcid = scid;
1513
1514         __l2cap_chan_add(conn, sk, parent);
1515         dcid = l2cap_pi(sk)->scid;
1516
1517         l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
1518
1519         /* Service level security */
1520         result = L2CAP_CR_PEND;
1521         status = L2CAP_CS_AUTHEN_PEND;
1522         sk->sk_state = BT_CONNECT2;
1523         l2cap_pi(sk)->ident = cmd->ident;
1524
1525         if ((l2cap_pi(sk)->link_mode & L2CAP_LM_ENCRYPT) ||
1526                         (l2cap_pi(sk)->link_mode & L2CAP_LM_SECURE)) {
1527                 if (!hci_conn_encrypt(conn->hcon))
1528                         goto done;
1529         } else if (l2cap_pi(sk)->link_mode & L2CAP_LM_AUTH) {
1530                 if (!hci_conn_auth(conn->hcon))
1531                         goto done;
1532         }
1533
1534         sk->sk_state = BT_CONFIG;
1535         result = status = 0;
1536
1537 done:
1538         write_unlock_bh(&list->lock);
1539
1540 response:
1541         bh_unlock_sock(parent);
1542
1543 sendresp:
1544         rsp.scid   = cpu_to_le16(scid);
1545         rsp.dcid   = cpu_to_le16(dcid);
1546         rsp.result = cpu_to_le16(result);
1547         rsp.status = cpu_to_le16(status);
1548         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
1549         return 0;
1550 }
1551
1552 static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1553 {
1554         struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
1555         u16 scid, dcid, result, status;
1556         struct sock *sk;
1557         u8 req[128];
1558
1559         scid   = __le16_to_cpu(rsp->scid);
1560         dcid   = __le16_to_cpu(rsp->dcid);
1561         result = __le16_to_cpu(rsp->result);
1562         status = __le16_to_cpu(rsp->status);
1563
1564         BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status);
1565
1566         if (scid) {
1567                 if (!(sk = l2cap_get_chan_by_scid(&conn->chan_list, scid)))
1568                         return 0;
1569         } else {
1570                 if (!(sk = l2cap_get_chan_by_ident(&conn->chan_list, cmd->ident)))
1571                         return 0;
1572         }
1573
1574         switch (result) {
1575         case L2CAP_CR_SUCCESS:
1576                 sk->sk_state = BT_CONFIG;
1577                 l2cap_pi(sk)->ident = 0;
1578                 l2cap_pi(sk)->dcid = dcid;
1579                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
1580
1581                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
1582                                         l2cap_build_conf_req(sk, req), req);
1583                 break;
1584
1585         case L2CAP_CR_PEND:
1586                 break;
1587
1588         default:
1589                 l2cap_chan_del(sk, ECONNREFUSED);
1590                 break;
1591         }
1592
1593         bh_unlock_sock(sk);
1594         return 0;
1595 }
1596
1597 static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
1598 {
1599         struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
1600         u16 dcid, flags;
1601         u8 rsp[64];
1602         struct sock *sk;
1603         int len;
1604
1605         dcid  = __le16_to_cpu(req->dcid);
1606         flags = __le16_to_cpu(req->flags);
1607
1608         BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
1609
1610         if (!(sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid)))
1611                 return -ENOENT;
1612
1613         if (sk->sk_state == BT_DISCONN)
1614                 goto unlock;
1615
1616         /* Reject if config buffer is too small. */
1617         len = cmd_len - sizeof(*req);
1618         if (l2cap_pi(sk)->conf_len + len > sizeof(l2cap_pi(sk)->conf_req)) {
1619                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
1620                                 l2cap_build_conf_rsp(sk, rsp,
1621                                         L2CAP_CONF_REJECT, flags), rsp);
1622                 goto unlock;
1623         }
1624
1625         /* Store config. */
1626         memcpy(l2cap_pi(sk)->conf_req + l2cap_pi(sk)->conf_len, req->data, len);
1627         l2cap_pi(sk)->conf_len += len;
1628
1629         if (flags & 0x0001) {
1630                 /* Incomplete config. Send empty response. */
1631                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
1632                                 l2cap_build_conf_rsp(sk, rsp,
1633                                         L2CAP_CONF_SUCCESS, 0x0001), rsp);
1634                 goto unlock;
1635         }
1636
1637         /* Complete config. */
1638         len = l2cap_parse_conf_req(sk, rsp);
1639         if (len < 0)
1640                 goto unlock;
1641
1642         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
1643
1644         /* Reset config buffer. */
1645         l2cap_pi(sk)->conf_len = 0;
1646
1647         if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE))
1648                 goto unlock;
1649
1650         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_INPUT_DONE) {
1651                 sk->sk_state = BT_CONNECTED;
1652                 l2cap_chan_ready(sk);
1653                 goto unlock;
1654         }
1655
1656         if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT)) {
1657                 u8 req[64];
1658                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
1659                                         l2cap_build_conf_req(sk, req), req);
1660         }
1661
1662 unlock:
1663         bh_unlock_sock(sk);
1664         return 0;
1665 }
1666
1667 static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1668 {
1669         struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
1670         u16 scid, flags, result;
1671         struct sock *sk;
1672
1673         scid   = __le16_to_cpu(rsp->scid);
1674         flags  = __le16_to_cpu(rsp->flags);
1675         result = __le16_to_cpu(rsp->result);
1676
1677         BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x", scid, flags, result);
1678
1679         if (!(sk = l2cap_get_chan_by_scid(&conn->chan_list, scid)))
1680                 return 0;
1681
1682         switch (result) {
1683         case L2CAP_CONF_SUCCESS:
1684                 break;
1685
1686         case L2CAP_CONF_UNACCEPT:
1687                 if (++l2cap_pi(sk)->conf_retry < L2CAP_CONF_MAX_RETRIES) {
1688                         char req[128];
1689                         /* It does not make sense to adjust L2CAP parameters
1690                          * that are currently defined in the spec. We simply
1691                          * resend config request that we sent earlier. It is
1692                          * stupid, but it helps qualification testing which
1693                          * expects at least some response from us. */
1694                         l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
1695                                                 l2cap_build_conf_req(sk, req), req);
1696                         goto done;
1697                 }
1698
1699         default:
1700                 sk->sk_state = BT_DISCONN;
1701                 sk->sk_err   = ECONNRESET;
1702                 l2cap_sock_set_timer(sk, HZ * 5);
1703                 {
1704                         struct l2cap_disconn_req req;
1705                         req.dcid = cpu_to_le16(l2cap_pi(sk)->dcid);
1706                         req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
1707                         l2cap_send_cmd(conn, l2cap_get_ident(conn),
1708                                         L2CAP_DISCONN_REQ, sizeof(req), &req);
1709                 }
1710                 goto done;
1711         }
1712
1713         if (flags & 0x01)
1714                 goto done;
1715
1716         l2cap_pi(sk)->conf_state |= L2CAP_CONF_INPUT_DONE;
1717
1718         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE) {
1719                 sk->sk_state = BT_CONNECTED;
1720                 l2cap_chan_ready(sk);
1721         }
1722
1723 done:
1724         bh_unlock_sock(sk);
1725         return 0;
1726 }
1727
1728 static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1729 {
1730         struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
1731         struct l2cap_disconn_rsp rsp;
1732         u16 dcid, scid;
1733         struct sock *sk;
1734
1735         scid = __le16_to_cpu(req->scid);
1736         dcid = __le16_to_cpu(req->dcid);
1737
1738         BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
1739
1740         if (!(sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid)))
1741                 return 0;
1742
1743         rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid);
1744         rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid);
1745         l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
1746
1747         sk->sk_shutdown = SHUTDOWN_MASK;
1748
1749         l2cap_chan_del(sk, ECONNRESET);
1750         bh_unlock_sock(sk);
1751
1752         l2cap_sock_kill(sk);
1753         return 0;
1754 }
1755
1756 static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1757 {
1758         struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
1759         u16 dcid, scid;
1760         struct sock *sk;
1761
1762         scid = __le16_to_cpu(rsp->scid);
1763         dcid = __le16_to_cpu(rsp->dcid);
1764
1765         BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
1766
1767         if (!(sk = l2cap_get_chan_by_scid(&conn->chan_list, scid)))
1768                 return 0;
1769
1770         l2cap_chan_del(sk, 0);
1771         bh_unlock_sock(sk);
1772
1773         l2cap_sock_kill(sk);
1774         return 0;
1775 }
1776
1777 static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1778 {
1779         struct l2cap_info_req *req = (struct l2cap_info_req *) data;
1780         u16 type;
1781
1782         type = __le16_to_cpu(req->type);
1783
1784         BT_DBG("type 0x%4.4x", type);
1785
1786         if (type == L2CAP_IT_FEAT_MASK) {
1787                 u8 buf[8];
1788                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
1789                 rsp->type   = cpu_to_le16(L2CAP_IT_FEAT_MASK);
1790                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
1791                 put_unaligned(cpu_to_le32(l2cap_feat_mask), (__le32 *) rsp->data);
1792                 l2cap_send_cmd(conn, cmd->ident,
1793                                         L2CAP_INFO_RSP, sizeof(buf), buf);
1794         } else {
1795                 struct l2cap_info_rsp rsp;
1796                 rsp.type   = cpu_to_le16(type);
1797                 rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP);
1798                 l2cap_send_cmd(conn, cmd->ident,
1799                                         L2CAP_INFO_RSP, sizeof(rsp), &rsp);
1800         }
1801
1802         return 0;
1803 }
1804
1805 static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1806 {
1807         struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
1808         u16 type, result;
1809
1810         type   = __le16_to_cpu(rsp->type);
1811         result = __le16_to_cpu(rsp->result);
1812
1813         BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
1814
1815         conn->info_ident = 0;
1816
1817         del_timer(&conn->info_timer);
1818
1819         if (type == L2CAP_IT_FEAT_MASK)
1820                 conn->feat_mask = __le32_to_cpu(get_unaligned((__le32 *) rsp->data));
1821
1822         l2cap_conn_start(conn);
1823
1824         return 0;
1825 }
1826
1827 static inline void l2cap_sig_channel(struct l2cap_conn *conn, struct sk_buff *skb)
1828 {
1829         u8 *data = skb->data;
1830         int len = skb->len;
1831         struct l2cap_cmd_hdr cmd;
1832         int err = 0;
1833
1834         l2cap_raw_recv(conn, skb);
1835
1836         while (len >= L2CAP_CMD_HDR_SIZE) {
1837                 u16 cmd_len;
1838                 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
1839                 data += L2CAP_CMD_HDR_SIZE;
1840                 len  -= L2CAP_CMD_HDR_SIZE;
1841
1842                 cmd_len = le16_to_cpu(cmd.len);
1843
1844                 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, cmd.ident);
1845
1846                 if (cmd_len > len || !cmd.ident) {
1847                         BT_DBG("corrupted command");
1848                         break;
1849                 }
1850
1851                 switch (cmd.code) {
1852                 case L2CAP_COMMAND_REJ:
1853                         l2cap_command_rej(conn, &cmd, data);
1854                         break;
1855
1856                 case L2CAP_CONN_REQ:
1857                         err = l2cap_connect_req(conn, &cmd, data);
1858                         break;
1859
1860                 case L2CAP_CONN_RSP:
1861                         err = l2cap_connect_rsp(conn, &cmd, data);
1862                         break;
1863
1864                 case L2CAP_CONF_REQ:
1865                         err = l2cap_config_req(conn, &cmd, cmd_len, data);
1866                         break;
1867
1868                 case L2CAP_CONF_RSP:
1869                         err = l2cap_config_rsp(conn, &cmd, data);
1870                         break;
1871
1872                 case L2CAP_DISCONN_REQ:
1873                         err = l2cap_disconnect_req(conn, &cmd, data);
1874                         break;
1875
1876                 case L2CAP_DISCONN_RSP:
1877                         err = l2cap_disconnect_rsp(conn, &cmd, data);
1878                         break;
1879
1880                 case L2CAP_ECHO_REQ:
1881                         l2cap_send_cmd(conn, cmd.ident, L2CAP_ECHO_RSP, cmd_len, data);
1882                         break;
1883
1884                 case L2CAP_ECHO_RSP:
1885                         break;
1886
1887                 case L2CAP_INFO_REQ:
1888                         err = l2cap_information_req(conn, &cmd, data);
1889                         break;
1890
1891                 case L2CAP_INFO_RSP:
1892                         err = l2cap_information_rsp(conn, &cmd, data);
1893                         break;
1894
1895                 default:
1896                         BT_ERR("Unknown signaling command 0x%2.2x", cmd.code);
1897                         err = -EINVAL;
1898                         break;
1899                 }
1900
1901                 if (err) {
1902                         struct l2cap_cmd_rej rej;
1903                         BT_DBG("error %d", err);
1904
1905                         /* FIXME: Map err to a valid reason */
1906                         rej.reason = cpu_to_le16(0);
1907                         l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
1908                 }
1909
1910                 data += cmd_len;
1911                 len  -= cmd_len;
1912         }
1913
1914         kfree_skb(skb);
1915 }
1916
1917 static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
1918 {
1919         struct sock *sk;
1920
1921         sk = l2cap_get_chan_by_scid(&conn->chan_list, cid);
1922         if (!sk) {
1923                 BT_DBG("unknown cid 0x%4.4x", cid);
1924                 goto drop;
1925         }
1926
1927         BT_DBG("sk %p, len %d", sk, skb->len);
1928
1929         if (sk->sk_state != BT_CONNECTED)
1930                 goto drop;
1931
1932         if (l2cap_pi(sk)->imtu < skb->len)
1933                 goto drop;
1934
1935         /* If socket recv buffers overflows we drop data here
1936          * which is *bad* because L2CAP has to be reliable.
1937          * But we don't have any other choice. L2CAP doesn't
1938          * provide flow control mechanism. */
1939
1940         if (!sock_queue_rcv_skb(sk, skb))
1941                 goto done;
1942
1943 drop:
1944         kfree_skb(skb);
1945
1946 done:
1947         if (sk)
1948                 bh_unlock_sock(sk);
1949
1950         return 0;
1951 }
1952
1953 static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb)
1954 {
1955         struct sock *sk;
1956
1957         sk = l2cap_get_sock_by_psm(0, psm, conn->src);
1958         if (!sk)
1959                 goto drop;
1960
1961         BT_DBG("sk %p, len %d", sk, skb->len);
1962
1963         if (sk->sk_state != BT_BOUND && sk->sk_state != BT_CONNECTED)
1964                 goto drop;
1965
1966         if (l2cap_pi(sk)->imtu < skb->len)
1967                 goto drop;
1968
1969         if (!sock_queue_rcv_skb(sk, skb))
1970                 goto done;
1971
1972 drop:
1973         kfree_skb(skb);
1974
1975 done:
1976         if (sk) bh_unlock_sock(sk);
1977         return 0;
1978 }
1979
1980 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
1981 {
1982         struct l2cap_hdr *lh = (void *) skb->data;
1983         u16 cid, len;
1984         __le16 psm;
1985
1986         skb_pull(skb, L2CAP_HDR_SIZE);
1987         cid = __le16_to_cpu(lh->cid);
1988         len = __le16_to_cpu(lh->len);
1989
1990         BT_DBG("len %d, cid 0x%4.4x", len, cid);
1991
1992         switch (cid) {
1993         case 0x0001:
1994                 l2cap_sig_channel(conn, skb);
1995                 break;
1996
1997         case 0x0002:
1998                 psm = get_unaligned((__le16 *) skb->data);
1999                 skb_pull(skb, 2);
2000                 l2cap_conless_channel(conn, psm, skb);
2001                 break;
2002
2003         default:
2004                 l2cap_data_channel(conn, cid, skb);
2005                 break;
2006         }
2007 }
2008
2009 /* ---- L2CAP interface with lower layer (HCI) ---- */
2010
2011 static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
2012 {
2013         int exact = 0, lm1 = 0, lm2 = 0;
2014         register struct sock *sk;
2015         struct hlist_node *node;
2016
2017         if (type != ACL_LINK)
2018                 return 0;
2019
2020         BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
2021
2022         /* Find listening sockets and check their link_mode */
2023         read_lock(&l2cap_sk_list.lock);
2024         sk_for_each(sk, node, &l2cap_sk_list.head) {
2025                 if (sk->sk_state != BT_LISTEN)
2026                         continue;
2027
2028                 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
2029                         lm1 |= (HCI_LM_ACCEPT | l2cap_pi(sk)->link_mode);
2030                         exact++;
2031                 } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
2032                         lm2 |= (HCI_LM_ACCEPT | l2cap_pi(sk)->link_mode);
2033         }
2034         read_unlock(&l2cap_sk_list.lock);
2035
2036         return exact ? lm1 : lm2;
2037 }
2038
2039 static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
2040 {
2041         struct l2cap_conn *conn;
2042
2043         BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
2044
2045         if (hcon->type != ACL_LINK)
2046                 return 0;
2047
2048         if (!status) {
2049                 conn = l2cap_conn_add(hcon, status);
2050                 if (conn)
2051                         l2cap_conn_ready(conn);
2052         } else
2053                 l2cap_conn_del(hcon, bt_err(status));
2054
2055         return 0;
2056 }
2057
2058 static int l2cap_disconn_ind(struct hci_conn *hcon, u8 reason)
2059 {
2060         BT_DBG("hcon %p reason %d", hcon, reason);
2061
2062         if (hcon->type != ACL_LINK)
2063                 return 0;
2064
2065         l2cap_conn_del(hcon, bt_err(reason));
2066
2067         return 0;
2068 }
2069
2070 static int l2cap_auth_cfm(struct hci_conn *hcon, u8 status)
2071 {
2072         struct l2cap_chan_list *l;
2073         struct l2cap_conn *conn = conn = hcon->l2cap_data;
2074         struct l2cap_conn_rsp rsp;
2075         struct sock *sk;
2076         int result;
2077
2078         if (!conn)
2079                 return 0;
2080
2081         l = &conn->chan_list;
2082
2083         BT_DBG("conn %p", conn);
2084
2085         read_lock(&l->lock);
2086
2087         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
2088                 bh_lock_sock(sk);
2089
2090                 if (sk->sk_state != BT_CONNECT2 ||
2091                                 (l2cap_pi(sk)->link_mode & L2CAP_LM_ENCRYPT) ||
2092                                 (l2cap_pi(sk)->link_mode & L2CAP_LM_SECURE)) {
2093                         bh_unlock_sock(sk);
2094                         continue;
2095                 }
2096
2097                 if (!status) {
2098                         sk->sk_state = BT_CONFIG;
2099                         result = 0;
2100                 } else {
2101                         sk->sk_state = BT_DISCONN;
2102                         l2cap_sock_set_timer(sk, HZ/10);
2103                         result = L2CAP_CR_SEC_BLOCK;
2104                 }
2105
2106                 rsp.scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
2107                 rsp.dcid   = cpu_to_le16(l2cap_pi(sk)->scid);
2108                 rsp.result = cpu_to_le16(result);
2109                 rsp.status = cpu_to_le16(0);
2110                 l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
2111                                 L2CAP_CONN_RSP, sizeof(rsp), &rsp);
2112
2113                 bh_unlock_sock(sk);
2114         }
2115
2116         read_unlock(&l->lock);
2117         return 0;
2118 }
2119
2120 static int l2cap_encrypt_cfm(struct hci_conn *hcon, u8 status)
2121 {
2122         struct l2cap_chan_list *l;
2123         struct l2cap_conn *conn = hcon->l2cap_data;
2124         struct l2cap_conn_rsp rsp;
2125         struct sock *sk;
2126         int result;
2127
2128         if (!conn)
2129                 return 0;
2130
2131         l = &conn->chan_list;
2132
2133         BT_DBG("conn %p", conn);
2134
2135         read_lock(&l->lock);
2136
2137         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
2138                 bh_lock_sock(sk);
2139
2140                 if (sk->sk_state != BT_CONNECT2) {
2141                         bh_unlock_sock(sk);
2142                         continue;
2143                 }
2144
2145                 if (!status) {
2146                         sk->sk_state = BT_CONFIG;
2147                         result = 0;
2148                 } else {
2149                         sk->sk_state = BT_DISCONN;
2150                         l2cap_sock_set_timer(sk, HZ/10);
2151                         result = L2CAP_CR_SEC_BLOCK;
2152                 }
2153
2154                 rsp.scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
2155                 rsp.dcid   = cpu_to_le16(l2cap_pi(sk)->scid);
2156                 rsp.result = cpu_to_le16(result);
2157                 rsp.status = cpu_to_le16(0);
2158                 l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
2159                                 L2CAP_CONN_RSP, sizeof(rsp), &rsp);
2160
2161                 if (l2cap_pi(sk)->link_mode & L2CAP_LM_SECURE)
2162                         hci_conn_change_link_key(hcon);
2163
2164                 bh_unlock_sock(sk);
2165         }
2166
2167         read_unlock(&l->lock);
2168         return 0;
2169 }
2170
2171 static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
2172 {
2173         struct l2cap_conn *conn = hcon->l2cap_data;
2174
2175         if (!conn && !(conn = l2cap_conn_add(hcon, 0)))
2176                 goto drop;
2177
2178         BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
2179
2180         if (flags & ACL_START) {
2181                 struct l2cap_hdr *hdr;
2182                 int len;
2183
2184                 if (conn->rx_len) {
2185                         BT_ERR("Unexpected start frame (len %d)", skb->len);
2186                         kfree_skb(conn->rx_skb);
2187                         conn->rx_skb = NULL;
2188                         conn->rx_len = 0;
2189                         l2cap_conn_unreliable(conn, ECOMM);
2190                 }
2191
2192                 if (skb->len < 2) {
2193                         BT_ERR("Frame is too short (len %d)", skb->len);
2194                         l2cap_conn_unreliable(conn, ECOMM);
2195                         goto drop;
2196                 }
2197
2198                 hdr = (struct l2cap_hdr *) skb->data;
2199                 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
2200
2201                 if (len == skb->len) {
2202                         /* Complete frame received */
2203                         l2cap_recv_frame(conn, skb);
2204                         return 0;
2205                 }
2206
2207                 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
2208
2209                 if (skb->len > len) {
2210                         BT_ERR("Frame is too long (len %d, expected len %d)",
2211                                 skb->len, len);
2212                         l2cap_conn_unreliable(conn, ECOMM);
2213                         goto drop;
2214                 }
2215
2216                 /* Allocate skb for the complete frame (with header) */
2217                 if (!(conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC)))
2218                         goto drop;
2219
2220                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
2221                               skb->len);
2222                 conn->rx_len = len - skb->len;
2223         } else {
2224                 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
2225
2226                 if (!conn->rx_len) {
2227                         BT_ERR("Unexpected continuation frame (len %d)", skb->len);
2228                         l2cap_conn_unreliable(conn, ECOMM);
2229                         goto drop;
2230                 }
2231
2232                 if (skb->len > conn->rx_len) {
2233                         BT_ERR("Fragment is too long (len %d, expected %d)",
2234                                         skb->len, conn->rx_len);
2235                         kfree_skb(conn->rx_skb);
2236                         conn->rx_skb = NULL;
2237                         conn->rx_len = 0;
2238                         l2cap_conn_unreliable(conn, ECOMM);
2239                         goto drop;
2240                 }
2241
2242                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
2243                               skb->len);
2244                 conn->rx_len -= skb->len;
2245
2246                 if (!conn->rx_len) {
2247                         /* Complete frame received */
2248                         l2cap_recv_frame(conn, conn->rx_skb);
2249                         conn->rx_skb = NULL;
2250                 }
2251         }
2252
2253 drop:
2254         kfree_skb(skb);
2255         return 0;
2256 }
2257
2258 static ssize_t l2cap_sysfs_show(struct class *dev, char *buf)
2259 {
2260         struct sock *sk;
2261         struct hlist_node *node;
2262         char *str = buf;
2263
2264         read_lock_bh(&l2cap_sk_list.lock);
2265
2266         sk_for_each(sk, node, &l2cap_sk_list.head) {
2267                 struct l2cap_pinfo *pi = l2cap_pi(sk);
2268
2269                 str += sprintf(str, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d 0x%x\n",
2270                                 batostr(&bt_sk(sk)->src), batostr(&bt_sk(sk)->dst),
2271                                 sk->sk_state, btohs(pi->psm), pi->scid, pi->dcid,
2272                                 pi->imtu, pi->omtu, pi->link_mode);
2273         }
2274
2275         read_unlock_bh(&l2cap_sk_list.lock);
2276
2277         return (str - buf);
2278 }
2279
2280 static CLASS_ATTR(l2cap, S_IRUGO, l2cap_sysfs_show, NULL);
2281
2282 static const struct proto_ops l2cap_sock_ops = {
2283         .family         = PF_BLUETOOTH,
2284         .owner          = THIS_MODULE,
2285         .release        = l2cap_sock_release,
2286         .bind           = l2cap_sock_bind,
2287         .connect        = l2cap_sock_connect,
2288         .listen         = l2cap_sock_listen,
2289         .accept         = l2cap_sock_accept,
2290         .getname        = l2cap_sock_getname,
2291         .sendmsg        = l2cap_sock_sendmsg,
2292         .recvmsg        = bt_sock_recvmsg,
2293         .poll           = bt_sock_poll,
2294         .mmap           = sock_no_mmap,
2295         .socketpair     = sock_no_socketpair,
2296         .ioctl          = sock_no_ioctl,
2297         .shutdown       = l2cap_sock_shutdown,
2298         .setsockopt     = l2cap_sock_setsockopt,
2299         .getsockopt     = l2cap_sock_getsockopt
2300 };
2301
2302 static struct net_proto_family l2cap_sock_family_ops = {
2303         .family = PF_BLUETOOTH,
2304         .owner  = THIS_MODULE,
2305         .create = l2cap_sock_create,
2306 };
2307
2308 static struct hci_proto l2cap_hci_proto = {
2309         .name           = "L2CAP",
2310         .id             = HCI_PROTO_L2CAP,
2311         .connect_ind    = l2cap_connect_ind,
2312         .connect_cfm    = l2cap_connect_cfm,
2313         .disconn_ind    = l2cap_disconn_ind,
2314         .auth_cfm       = l2cap_auth_cfm,
2315         .encrypt_cfm    = l2cap_encrypt_cfm,
2316         .recv_acldata   = l2cap_recv_acldata
2317 };
2318
2319 static int __init l2cap_init(void)
2320 {
2321         int err;
2322
2323         err = proto_register(&l2cap_proto, 0);
2324         if (err < 0)
2325                 return err;
2326
2327         err = bt_sock_register(BTPROTO_L2CAP, &l2cap_sock_family_ops);
2328         if (err < 0) {
2329                 BT_ERR("L2CAP socket registration failed");
2330                 goto error;
2331         }
2332
2333         err = hci_register_proto(&l2cap_hci_proto);
2334         if (err < 0) {
2335                 BT_ERR("L2CAP protocol registration failed");
2336                 bt_sock_unregister(BTPROTO_L2CAP);
2337                 goto error;
2338         }
2339
2340         if (class_create_file(bt_class, &class_attr_l2cap) < 0)
2341                 BT_ERR("Failed to create L2CAP info file");
2342
2343         BT_INFO("L2CAP ver %s", VERSION);
2344         BT_INFO("L2CAP socket layer initialized");
2345
2346         return 0;
2347
2348 error:
2349         proto_unregister(&l2cap_proto);
2350         return err;
2351 }
2352
2353 static void __exit l2cap_exit(void)
2354 {
2355         class_remove_file(bt_class, &class_attr_l2cap);
2356
2357         if (bt_sock_unregister(BTPROTO_L2CAP) < 0)
2358                 BT_ERR("L2CAP socket unregistration failed");
2359
2360         if (hci_unregister_proto(&l2cap_hci_proto) < 0)
2361                 BT_ERR("L2CAP protocol unregistration failed");
2362
2363         proto_unregister(&l2cap_proto);
2364 }
2365
2366 void l2cap_load(void)
2367 {
2368         /* Dummy function to trigger automatic L2CAP module loading by
2369          * other modules that use L2CAP sockets but don't use any other
2370          * symbols from it. */
2371         return;
2372 }
2373 EXPORT_SYMBOL(l2cap_load);
2374
2375 module_init(l2cap_init);
2376 module_exit(l2cap_exit);
2377
2378 MODULE_AUTHOR("Maxim Krasnyansky <maxk@qualcomm.com>, Marcel Holtmann <marcel@holtmann.org>");
2379 MODULE_DESCRIPTION("Bluetooth L2CAP ver " VERSION);
2380 MODULE_VERSION(VERSION);
2381 MODULE_LICENSE("GPL");
2382 MODULE_ALIAS("bt-proto-0");