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