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