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.6"
  61
  62 static struct proto_ops l2cap_sock_ops;
  63
  64 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 (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         if (get_user(len, optlen))
 835                 return -EFAULT;
 836
 837         lock_sock(sk);
 838
 839         switch (optname) {
 840         case L2CAP_OPTIONS:
 841                 opts.imtu     = l2cap_pi(sk)->imtu;
 842                 opts.omtu     = l2cap_pi(sk)->omtu;
 843                 opts.flush_to = l2cap_pi(sk)->flush_to;
 844
 845                 len = min_t(unsigned int, len, sizeof(opts));
 846                 if (copy_to_user(optval, (char *)&opts, len))
 847                         err = -EFAULT;
 848
 849                 break;
 850
 851         case L2CAP_LM:
 852                 if (put_user(l2cap_pi(sk)->link_mode, (u32 __user *)optval))
 853                         err = -EFAULT;
 854                 break;
 855
 856         case L2CAP_CONNINFO:
 857                 if (sk->sk_state != BT_CONNECTED) {
 858                         err = -ENOTCONN;
 859                         break;
 860                 }
 861
 862                 cinfo.hci_handle = l2cap_pi(sk)->conn->hcon->handle;
 863
 864                 len = min_t(unsigned int, len, sizeof(cinfo));
 865                 if (copy_to_user(optval, (char *)&cinfo, len))
 866                         err = -EFAULT;
 867
 868                 break;
 869
 870         default:
 871                 err = -ENOPROTOOPT;
 872                 break;
 873         }
 874
 875         release_sock(sk);
 876         return err;
 877 }
 878
 879 static int l2cap_sock_shutdown(struct socket *sock, int how)
 880 {
 881         struct sock *sk = sock->sk;
 882         int err = 0;
 883
 884         BT_DBG("sock %p, sk %p", sock, sk);
 885
 886         if (!sk)
 887                 return 0;
 888
 889         lock_sock(sk);
 890         if (!sk->sk_shutdown) {
 891                 sk->sk_shutdown = SHUTDOWN_MASK;
 892                 l2cap_sock_clear_timer(sk);
 893                 __l2cap_sock_close(sk, 0);
 894
 895                 if (sock_flag(sk, SOCK_LINGER) && sk->sk_lingertime)
 896                         err = bt_sock_wait_state(sk, BT_CLOSED, sk->sk_lingertime);
 897         }
 898         release_sock(sk);
 899         return err;
 900 }
 901
 902 static int l2cap_sock_release(struct socket *sock)
 903 {
 904         struct sock *sk = sock->sk;
 905         int err;
 906
 907         BT_DBG("sock %p, sk %p", sock, sk);
 908
 909         if (!sk)
 910                 return 0;
 911
 912         err = l2cap_sock_shutdown(sock, 2);
 913
 914         sock_orphan(sk);
 915         l2cap_sock_kill(sk);
 916         return err;
 917 }
 918
 919 /* ---- L2CAP channels ---- */
 920 static struct sock *__l2cap_get_chan_by_dcid(struct l2cap_chan_list *l, u16 cid)
 921 {
 922         struct sock *s;
 923         for (s = l->head; s; s = l2cap_pi(s)->next_c) {
 924                 if (l2cap_pi(s)->dcid == cid)
 925                         break;
 926         }
 927         return s;
 928 }
 929
 930 static struct sock *__l2cap_get_chan_by_scid(struct l2cap_chan_list *l, u16 cid)
 931 {
 932         struct sock *s;
 933         for (s = l->head; s; s = l2cap_pi(s)->next_c) {
 934                 if (l2cap_pi(s)->scid == cid)
 935                         break;
 936         }
 937         return s;
 938 }
 939
 940 /* Find channel with given SCID.
 941  * Returns locked socket */
 942 static inline struct sock *l2cap_get_chan_by_scid(struct l2cap_chan_list *l, u16 cid)
 943 {
 944         struct sock *s;
 945         read_lock(&l->lock);
 946         s = __l2cap_get_chan_by_scid(l, cid);
 947         if (s) bh_lock_sock(s);
 948         read_unlock(&l->lock);
 949         return s;
 950 }
 951
 952 static struct sock *__l2cap_get_chan_by_ident(struct l2cap_chan_list *l, u8 ident)
 953 {
 954         struct sock *s;
 955         for (s = l->head; s; s = l2cap_pi(s)->next_c) {
 956                 if (l2cap_pi(s)->ident == ident)
 957                         break;
 958         }
 959         return s;
 960 }
 961
 962 static inline struct sock *l2cap_get_chan_by_ident(struct l2cap_chan_list *l, u8 ident)
 963 {
 964         struct sock *s;
 965         read_lock(&l->lock);
 966         s = __l2cap_get_chan_by_ident(l, ident);
 967         if (s) bh_lock_sock(s);
 968         read_unlock(&l->lock);
 969         return s;
 970 }
 971
 972 static u16 l2cap_alloc_cid(struct l2cap_chan_list *l)
 973 {
 974         u16 cid = 0x0040;
 975
 976         for (; cid < 0xffff; cid++) {
 977                 if(!__l2cap_get_chan_by_scid(l, cid))
 978                         return cid;
 979         }
 980
 981         return 0;
 982 }
 983
 984 static inline void __l2cap_chan_link(struct l2cap_chan_list *l, struct sock *sk)
 985 {
 986         sock_hold(sk);
 987
 988         if (l->head)
 989                 l2cap_pi(l->head)->prev_c = sk;
 990
 991         l2cap_pi(sk)->next_c = l->head;
 992         l2cap_pi(sk)->prev_c = NULL;
 993         l->head = sk;
 994 }
 995
 996 static inline void l2cap_chan_unlink(struct l2cap_chan_list *l, struct sock *sk)
 997 {
 998         struct sock *next = l2cap_pi(sk)->next_c, *prev = l2cap_pi(sk)->prev_c;
 999
1000         write_lock(&l->lock);
1001         if (sk == l->head)
1002                 l->head = next;
1003
1004         if (next)
1005                 l2cap_pi(next)->prev_c = prev;
1006         if (prev)
1007                 l2cap_pi(prev)->next_c = next;
1008         write_unlock(&l->lock);
1009
1010         __sock_put(sk);
1011 }
1012
1013 static void __l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct sock *parent)
1014 {
1015         struct l2cap_chan_list *l = &conn->chan_list;
1016
1017         BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn, l2cap_pi(sk)->psm, l2cap_pi(sk)->dcid);
1018
1019         l2cap_pi(sk)->conn = conn;
1020
1021         if (sk->sk_type == SOCK_SEQPACKET) {
1022                 /* Alloc CID for connection-oriented socket */
1023                 l2cap_pi(sk)->scid = l2cap_alloc_cid(l);
1024         } else if (sk->sk_type == SOCK_DGRAM) {
1025                 /* Connectionless socket */
1026                 l2cap_pi(sk)->scid = 0x0002;
1027                 l2cap_pi(sk)->dcid = 0x0002;
1028                 l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU;
1029         } else {
1030                 /* Raw socket can send/recv signalling messages only */
1031                 l2cap_pi(sk)->scid = 0x0001;
1032                 l2cap_pi(sk)->dcid = 0x0001;
1033                 l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU;
1034         }
1035
1036         __l2cap_chan_link(l, sk);
1037
1038         if (parent)
1039                 bt_accept_enqueue(parent, sk);
1040 }
1041
1042 /* Delete channel.
1043  * Must be called on the locked socket. */
1044 static void l2cap_chan_del(struct sock *sk, int err)
1045 {
1046         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1047         struct sock *parent = bt_sk(sk)->parent;
1048
1049         l2cap_sock_clear_timer(sk);
1050
1051         BT_DBG("sk %p, conn %p, err %d", sk, conn, err);
1052
1053         if (conn) {
1054                 /* Unlink from channel list */
1055                 l2cap_chan_unlink(&conn->chan_list, sk);
1056                 l2cap_pi(sk)->conn = NULL;
1057                 hci_conn_put(conn->hcon);
1058         }
1059
1060         sk->sk_state  = BT_CLOSED;
1061         sk->sk_zapped = 1;
1062
1063         if (err)
1064                 sk->sk_err = err;
1065
1066         if (parent) {
1067                 bt_accept_unlink(sk);
1068                 parent->sk_data_ready(parent, 0);
1069         } else
1070                 sk->sk_state_change(sk);
1071 }
1072
1073 static void l2cap_conn_ready(struct l2cap_conn *conn)
1074 {
1075         struct l2cap_chan_list *l = &conn->chan_list;
1076         struct sock *sk;
1077
1078         BT_DBG("conn %p", conn);
1079
1080         read_lock(&l->lock);
1081
1082         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
1083                 bh_lock_sock(sk);
1084
1085                 if (sk->sk_type != SOCK_SEQPACKET) {
1086                         l2cap_sock_clear_timer(sk);
1087                         sk->sk_state = BT_CONNECTED;
1088                         sk->sk_state_change(sk);
1089                 } else if (sk->sk_state == BT_CONNECT) {
1090                         struct l2cap_conn_req req;
1091                         l2cap_pi(sk)->ident = l2cap_get_ident(conn);
1092                         req.scid = __cpu_to_le16(l2cap_pi(sk)->scid);
1093                         req.psm  = l2cap_pi(sk)->psm;
1094                         l2cap_send_cmd(conn, l2cap_pi(sk)->ident, L2CAP_CONN_REQ, sizeof(req), &req);
1095                 }
1096
1097                 bh_unlock_sock(sk);
1098         }
1099
1100         read_unlock(&l->lock);
1101 }
1102
1103 /* Notify sockets that we cannot guaranty reliability anymore */
1104 static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err)
1105 {
1106         struct l2cap_chan_list *l = &conn->chan_list;
1107         struct sock *sk;
1108
1109         BT_DBG("conn %p", conn);
1110
1111         read_lock(&l->lock);
1112         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
1113                 if (l2cap_pi(sk)->link_mode & L2CAP_LM_RELIABLE)
1114                         sk->sk_err = err;
1115         }
1116         read_unlock(&l->lock);
1117 }
1118
1119 static void l2cap_chan_ready(struct sock *sk)
1120 {
1121         struct sock *parent = bt_sk(sk)->parent;
1122
1123         BT_DBG("sk %p, parent %p", sk, parent);
1124
1125         l2cap_pi(sk)->conf_state = 0;
1126         l2cap_sock_clear_timer(sk);
1127
1128         if (!parent) {
1129                 /* Outgoing channel.
1130                  * Wake up socket sleeping on connect.
1131                  */
1132                 sk->sk_state = BT_CONNECTED;
1133                 sk->sk_state_change(sk);
1134         } else {
1135                 /* Incoming channel.
1136                  * Wake up socket sleeping on accept.
1137                  */
1138                 parent->sk_data_ready(parent, 0);
1139         }
1140 }
1141
1142 /* Copy frame to all raw sockets on that connection */
1143 static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
1144 {
1145         struct l2cap_chan_list *l = &conn->chan_list;
1146         struct sk_buff *nskb;
1147         struct sock * sk;
1148
1149         BT_DBG("conn %p", conn);
1150
1151         read_lock(&l->lock);
1152         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
1153                 if (sk->sk_type != SOCK_RAW)
1154                         continue;
1155
1156                 /* Don't send frame to the socket it came from */
1157                 if (skb->sk == sk)
1158                         continue;
1159
1160                 if (!(nskb = skb_clone(skb, GFP_ATOMIC)))
1161                         continue;
1162
1163                 if (sock_queue_rcv_skb(sk, nskb))
1164                         kfree_skb(nskb);
1165         }
1166         read_unlock(&l->lock);
1167 }
1168
1169 /* ---- L2CAP signalling commands ---- */
1170 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
1171                                 u8 code, u8 ident, u16 dlen, void *data)
1172 {
1173         struct sk_buff *skb, **frag;
1174         struct l2cap_cmd_hdr *cmd;
1175         struct l2cap_hdr *lh;
1176         int len, count;
1177
1178         BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %d", conn, code, ident, dlen);
1179
1180         len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen;
1181         count = min_t(unsigned int, conn->mtu, len);
1182
1183         skb = bt_skb_alloc(count, GFP_ATOMIC);
1184         if (!skb)
1185                 return NULL;
1186
1187         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1188         lh->len = __cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen);
1189         lh->cid = __cpu_to_le16(0x0001);
1190
1191         cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE);
1192         cmd->code  = code;
1193         cmd->ident = ident;
1194         cmd->len   = __cpu_to_le16(dlen);
1195
1196         if (dlen) {
1197                 count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE;
1198                 memcpy(skb_put(skb, count), data, count);
1199                 data += count;
1200         }
1201
1202         len -= skb->len;
1203
1204         /* Continuation fragments (no L2CAP header) */
1205         frag = &skb_shinfo(skb)->frag_list;
1206         while (len) {
1207                 count = min_t(unsigned int, conn->mtu, len);
1208
1209                 *frag = bt_skb_alloc(count, GFP_ATOMIC);
1210                 if (!*frag)
1211                         goto fail;
1212
1213                 memcpy(skb_put(*frag, count), data, count);
1214
1215                 len  -= count;
1216                 data += count;
1217
1218                 frag = &(*frag)->next;
1219         }
1220
1221         return skb;
1222
1223 fail:
1224         kfree_skb(skb);
1225         return NULL;
1226 }
1227
1228 static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val)
1229 {
1230         struct l2cap_conf_opt *opt = *ptr;
1231         int len;
1232
1233         len = L2CAP_CONF_OPT_SIZE + opt->len;
1234         *ptr += len;
1235
1236         *type = opt->type;
1237         *olen = opt->len;
1238
1239         switch (opt->len) {
1240         case 1:
1241                 *val = *((u8 *) opt->val);
1242                 break;
1243
1244         case 2:
1245                 *val = __le16_to_cpu(*((u16 *)opt->val));
1246                 break;
1247
1248         case 4:
1249                 *val = __le32_to_cpu(*((u32 *)opt->val));
1250                 break;
1251
1252         default:
1253                 *val = (unsigned long) opt->val;
1254                 break;
1255         }
1256
1257         BT_DBG("type 0x%2.2x len %d val 0x%lx", *type, opt->len, *val);
1258         return len;
1259 }
1260
1261 static inline void l2cap_parse_conf_req(struct sock *sk, void *data, int len)
1262 {
1263         int type, hint, olen;
1264         unsigned long val;
1265         void *ptr = data;
1266
1267         BT_DBG("sk %p len %d", sk, len);
1268
1269         while (len >= L2CAP_CONF_OPT_SIZE) {
1270                 len -= l2cap_get_conf_opt(&ptr, &type, &olen, &val);
1271
1272                 hint  = type & 0x80;
1273                 type &= 0x7f;
1274
1275                 switch (type) {
1276                 case L2CAP_CONF_MTU:
1277                         l2cap_pi(sk)->conf_mtu = val;
1278                         break;
1279
1280                 case L2CAP_CONF_FLUSH_TO:
1281                         l2cap_pi(sk)->flush_to = val;
1282                         break;
1283
1284                 case L2CAP_CONF_QOS:
1285                         break;
1286
1287                 default:
1288                         if (hint)
1289                                 break;
1290
1291                         /* FIXME: Reject unknown option */
1292                         break;
1293                 }
1294         }
1295 }
1296
1297 static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val)
1298 {
1299         struct l2cap_conf_opt *opt = *ptr;
1300
1301         BT_DBG("type 0x%2.2x len %d val 0x%lx", type, len, val);
1302
1303         opt->type = type;
1304         opt->len  = len;
1305
1306         switch (len) {
1307         case 1:
1308                 *((u8 *) opt->val)  = val;
1309                 break;
1310
1311         case 2:
1312                 *((u16 *) opt->val) = __cpu_to_le16(val);
1313                 break;
1314
1315         case 4:
1316                 *((u32 *) opt->val) = __cpu_to_le32(val);
1317                 break;
1318
1319         default:
1320                 memcpy(opt->val, (void *) val, len);
1321                 break;
1322         }
1323
1324         *ptr += L2CAP_CONF_OPT_SIZE + len;
1325 }
1326
1327 static int l2cap_build_conf_req(struct sock *sk, void *data)
1328 {
1329         struct l2cap_pinfo *pi = l2cap_pi(sk);
1330         struct l2cap_conf_req *req = data;
1331         void *ptr = req->data;
1332
1333         BT_DBG("sk %p", sk);
1334
1335         if (pi->imtu != L2CAP_DEFAULT_MTU)
1336                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->imtu);
1337
1338         /* FIXME: Need actual value of the flush timeout */
1339         //if (flush_to != L2CAP_DEFAULT_FLUSH_TO)
1340         //   l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO, 2, pi->flush_to);
1341
1342         req->dcid  = __cpu_to_le16(pi->dcid);
1343         req->flags = __cpu_to_le16(0);
1344
1345         return ptr - data;
1346 }
1347
1348 static inline int l2cap_conf_output(struct sock *sk, void **ptr)
1349 {
1350         struct l2cap_pinfo *pi = l2cap_pi(sk);
1351         int result = 0;
1352
1353         /* Configure output options and let the other side know
1354          * which ones we don't like. */
1355         if (pi->conf_mtu < pi->omtu) {
1356                 l2cap_add_conf_opt(ptr, L2CAP_CONF_MTU, 2, pi->omtu);
1357                 result = L2CAP_CONF_UNACCEPT;
1358         } else {
1359                 pi->omtu = pi->conf_mtu;
1360         }
1361
1362         BT_DBG("sk %p result %d", sk, result);
1363         return result;
1364 }
1365
1366 static int l2cap_build_conf_rsp(struct sock *sk, void *data, int *result)
1367 {
1368         struct l2cap_conf_rsp *rsp = data;
1369         void *ptr = rsp->data;
1370         u16 flags = 0;
1371
1372         BT_DBG("sk %p complete %d", sk, result ? 1 : 0);
1373
1374         if (result)
1375                 *result = l2cap_conf_output(sk, &ptr);
1376         else
1377                 flags = 0x0001;
1378
1379         rsp->scid   = __cpu_to_le16(l2cap_pi(sk)->dcid);
1380         rsp->result = __cpu_to_le16(result ? *result : 0);
1381         rsp->flags  = __cpu_to_le16(flags);
1382
1383         return ptr - data;
1384 }
1385
1386 static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1387 {
1388         struct l2cap_chan_list *list = &conn->chan_list;
1389         struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
1390         struct l2cap_conn_rsp rsp;
1391         struct sock *sk, *parent;
1392         int result = 0, status = 0;
1393
1394         u16 dcid = 0, scid = __le16_to_cpu(req->scid);
1395         u16 psm  = req->psm;
1396
1397         BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid);
1398
1399         /* Check if we have socket listening on psm */
1400         parent = l2cap_get_sock_by_psm(BT_LISTEN, psm, conn->src);
1401         if (!parent) {
1402                 result = L2CAP_CR_BAD_PSM;
1403                 goto sendresp;
1404         }
1405
1406         result = L2CAP_CR_NO_MEM;
1407
1408         /* Check for backlog size */
1409         if (parent->sk_ack_backlog > parent->sk_max_ack_backlog) {
1410                 BT_DBG("backlog full %d", parent->sk_ack_backlog);
1411                 goto response;
1412         }
1413
1414         sk = l2cap_sock_alloc(NULL, BTPROTO_L2CAP, GFP_ATOMIC);
1415         if (!sk)
1416                 goto response;
1417
1418         write_lock(&list->lock);
1419
1420         /* Check if we already have channel with that dcid */
1421         if (__l2cap_get_chan_by_dcid(list, scid)) {
1422                 write_unlock(&list->lock);
1423                 sk->sk_zapped = 1;
1424                 l2cap_sock_kill(sk);
1425                 goto response;
1426         }
1427
1428         hci_conn_hold(conn->hcon);
1429
1430         l2cap_sock_init(sk, parent);
1431         bacpy(&bt_sk(sk)->src, conn->src);
1432         bacpy(&bt_sk(sk)->dst, conn->dst);
1433         l2cap_pi(sk)->psm  = psm;
1434         l2cap_pi(sk)->dcid = scid;
1435
1436         __l2cap_chan_add(conn, sk, parent);
1437         dcid = l2cap_pi(sk)->scid;
1438
1439         l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
1440
1441         /* Service level security */
1442         result = L2CAP_CR_PEND;
1443         status = L2CAP_CS_AUTHEN_PEND;
1444         sk->sk_state = BT_CONNECT2;
1445         l2cap_pi(sk)->ident = cmd->ident;
1446
1447         if ((l2cap_pi(sk)->link_mode & L2CAP_LM_ENCRYPT) ||
1448                         (l2cap_pi(sk)->link_mode & L2CAP_LM_SECURE)) {
1449                 if (!hci_conn_encrypt(conn->hcon))
1450                         goto done;
1451         } else if (l2cap_pi(sk)->link_mode & L2CAP_LM_AUTH) {
1452                 if (!hci_conn_auth(conn->hcon))
1453                         goto done;
1454         }
1455
1456         sk->sk_state = BT_CONFIG;
1457         result = status = 0;
1458
1459 done:
1460         write_unlock(&list->lock);
1461
1462 response:
1463         bh_unlock_sock(parent);
1464
1465 sendresp:
1466         rsp.scid   = __cpu_to_le16(scid);
1467         rsp.dcid   = __cpu_to_le16(dcid);
1468         rsp.result = __cpu_to_le16(result);
1469         rsp.status = __cpu_to_le16(status);
1470         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
1471         return 0;
1472 }
1473
1474 static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1475 {
1476         struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
1477         u16 scid, dcid, result, status;
1478         struct sock *sk;
1479         u8 req[128];
1480
1481         scid   = __le16_to_cpu(rsp->scid);
1482         dcid   = __le16_to_cpu(rsp->dcid);
1483         result = __le16_to_cpu(rsp->result);
1484         status = __le16_to_cpu(rsp->status);
1485
1486         BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status);
1487
1488         if (scid) {
1489                 if (!(sk = l2cap_get_chan_by_scid(&conn->chan_list, scid)))
1490                         return 0;
1491         } else {
1492                 if (!(sk = l2cap_get_chan_by_ident(&conn->chan_list, cmd->ident)))
1493                         return 0;
1494         }
1495
1496         switch (result) {
1497         case L2CAP_CR_SUCCESS:
1498                 sk->sk_state = BT_CONFIG;
1499                 l2cap_pi(sk)->ident = 0;
1500                 l2cap_pi(sk)->dcid = dcid;
1501                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
1502
1503                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
1504                                         l2cap_build_conf_req(sk, req), req);
1505                 break;
1506
1507         case L2CAP_CR_PEND:
1508                 break;
1509
1510         default:
1511                 l2cap_chan_del(sk, ECONNREFUSED);
1512                 break;
1513         }
1514
1515         bh_unlock_sock(sk);
1516         return 0;
1517 }
1518
1519 static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1520 {
1521         struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
1522         u16 dcid, flags;
1523         u8 rsp[64];
1524         struct sock *sk;
1525         int result;
1526
1527         dcid  = __le16_to_cpu(req->dcid);
1528         flags = __le16_to_cpu(req->flags);
1529
1530         BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
1531
1532         if (!(sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid)))
1533                 return -ENOENT;
1534
1535         l2cap_parse_conf_req(sk, req->data, cmd->len - sizeof(*req));
1536
1537         if (flags & 0x0001) {
1538                 /* Incomplete config. Send empty response. */
1539                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
1540                                 l2cap_build_conf_rsp(sk, rsp, NULL), rsp);
1541                 goto unlock;
1542         }
1543
1544         /* Complete config. */
1545         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
1546                         l2cap_build_conf_rsp(sk, rsp, &result), rsp);
1547
1548         if (result)
1549                 goto unlock;
1550
1551         /* Output config done */
1552         l2cap_pi(sk)->conf_state |= L2CAP_CONF_OUTPUT_DONE;
1553
1554         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_INPUT_DONE) {
1555                 sk->sk_state = BT_CONNECTED;
1556                 l2cap_chan_ready(sk);
1557         } else if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT)) {
1558                 u8 req[64];
1559                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
1560                                         l2cap_build_conf_req(sk, req), req);
1561         }
1562
1563 unlock:
1564         bh_unlock_sock(sk);
1565         return 0;
1566 }
1567
1568 static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1569 {
1570         struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
1571         u16 scid, flags, result;
1572         struct sock *sk;
1573
1574         scid   = __le16_to_cpu(rsp->scid);
1575         flags  = __le16_to_cpu(rsp->flags);
1576         result = __le16_to_cpu(rsp->result);
1577
1578         BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x", scid, flags, result);
1579
1580         if (!(sk = l2cap_get_chan_by_scid(&conn->chan_list, scid)))
1581                 return 0;
1582
1583         switch (result) {
1584         case L2CAP_CONF_SUCCESS:
1585                 break;
1586
1587         case L2CAP_CONF_UNACCEPT:
1588                 if (++l2cap_pi(sk)->conf_retry < L2CAP_CONF_MAX_RETRIES) {
1589                         char req[128];
1590                         /* It does not make sense to adjust L2CAP parameters
1591                          * that are currently defined in the spec. We simply
1592                          * resend config request that we sent earlier. It is
1593                          * stupid, but it helps qualification testing which
1594                          * expects at least some response from us. */
1595                         l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
1596                                                 l2cap_build_conf_req(sk, req), req);
1597                         goto done;
1598                 }
1599
1600         default:
1601                 sk->sk_state = BT_DISCONN;
1602                 sk->sk_err   = ECONNRESET;
1603                 l2cap_sock_set_timer(sk, HZ * 5);
1604                 {
1605                         struct l2cap_disconn_req req;
1606                         req.dcid = __cpu_to_le16(l2cap_pi(sk)->dcid);
1607                         req.scid = __cpu_to_le16(l2cap_pi(sk)->scid);
1608                         l2cap_send_cmd(conn, l2cap_get_ident(conn),
1609                                         L2CAP_DISCONN_REQ, sizeof(req), &req);
1610                 }
1611                 goto done;
1612         }
1613
1614         if (flags & 0x01)
1615                 goto done;
1616
1617         /* Input config done */
1618         l2cap_pi(sk)->conf_state |= L2CAP_CONF_INPUT_DONE;
1619
1620         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE) {
1621                 sk->sk_state = BT_CONNECTED;
1622                 l2cap_chan_ready(sk);
1623         }
1624
1625 done:
1626         bh_unlock_sock(sk);
1627         return 0;
1628 }
1629
1630 static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1631 {
1632         struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
1633         struct l2cap_disconn_rsp rsp;
1634         u16 dcid, scid;
1635         struct sock *sk;
1636
1637         scid = __le16_to_cpu(req->scid);
1638         dcid = __le16_to_cpu(req->dcid);
1639
1640         BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
1641
1642         if (!(sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid)))
1643                 return 0;
1644
1645         rsp.dcid = __cpu_to_le16(l2cap_pi(sk)->scid);
1646         rsp.scid = __cpu_to_le16(l2cap_pi(sk)->dcid);
1647         l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
1648
1649         sk->sk_shutdown = SHUTDOWN_MASK;
1650
1651         l2cap_chan_del(sk, ECONNRESET);
1652         bh_unlock_sock(sk);
1653
1654         l2cap_sock_kill(sk);
1655         return 0;
1656 }
1657
1658 static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1659 {
1660         struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
1661         u16 dcid, scid;
1662         struct sock *sk;
1663
1664         scid = __le16_to_cpu(rsp->scid);
1665         dcid = __le16_to_cpu(rsp->dcid);
1666
1667         BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
1668
1669         if (!(sk = l2cap_get_chan_by_scid(&conn->chan_list, scid)))
1670                 return 0;
1671
1672         l2cap_chan_del(sk, 0);
1673         bh_unlock_sock(sk);
1674
1675         l2cap_sock_kill(sk);
1676         return 0;
1677 }
1678
1679 static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1680 {
1681         struct l2cap_info_req *req = (struct l2cap_info_req *) data;
1682         struct l2cap_info_rsp rsp;
1683         u16 type;
1684
1685         type = __le16_to_cpu(req->type);
1686
1687         BT_DBG("type 0x%4.4x", type);
1688
1689         rsp.type   = __cpu_to_le16(type);
1690         rsp.result = __cpu_to_le16(L2CAP_IR_NOTSUPP);
1691         l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(rsp), &rsp);
1692
1693         return 0;
1694 }
1695
1696 static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1697 {
1698         struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
1699         u16 type, result;
1700
1701         type   = __le16_to_cpu(rsp->type);
1702         result = __le16_to_cpu(rsp->result);
1703
1704         BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
1705
1706         return 0;
1707 }
1708
1709 static inline void l2cap_sig_channel(struct l2cap_conn *conn, struct sk_buff *skb)
1710 {
1711         u8 *data = skb->data;
1712         int len = skb->len;
1713         struct l2cap_cmd_hdr cmd;
1714         int err = 0;
1715
1716         l2cap_raw_recv(conn, skb);
1717
1718         while (len >= L2CAP_CMD_HDR_SIZE) {
1719                 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
1720                 data += L2CAP_CMD_HDR_SIZE;
1721                 len  -= L2CAP_CMD_HDR_SIZE;
1722
1723                 cmd.len = __le16_to_cpu(cmd.len);
1724
1725                 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd.len, cmd.ident);
1726
1727                 if (cmd.len > len || !cmd.ident) {
1728                         BT_DBG("corrupted command");
1729                         break;
1730                 }
1731
1732                 switch (cmd.code) {
1733                 case L2CAP_COMMAND_REJ:
1734                         /* FIXME: We should process this */
1735                         break;
1736
1737                 case L2CAP_CONN_REQ:
1738                         err = l2cap_connect_req(conn, &cmd, data);
1739                         break;
1740
1741                 case L2CAP_CONN_RSP:
1742                         err = l2cap_connect_rsp(conn, &cmd, data);
1743                         break;
1744
1745                 case L2CAP_CONF_REQ:
1746                         err = l2cap_config_req(conn, &cmd, data);
1747                         break;
1748
1749                 case L2CAP_CONF_RSP:
1750                         err = l2cap_config_rsp(conn, &cmd, data);
1751                         break;
1752
1753                 case L2CAP_DISCONN_REQ:
1754                         err = l2cap_disconnect_req(conn, &cmd, data);
1755                         break;
1756
1757                 case L2CAP_DISCONN_RSP:
1758                         err = l2cap_disconnect_rsp(conn, &cmd, data);
1759                         break;
1760
1761                 case L2CAP_ECHO_REQ:
1762                         l2cap_send_cmd(conn, cmd.ident, L2CAP_ECHO_RSP, cmd.len, data);
1763                         break;
1764
1765                 case L2CAP_ECHO_RSP:
1766                         break;
1767
1768                 case L2CAP_INFO_REQ:
1769                         err = l2cap_information_req(conn, &cmd, data);
1770                         break;
1771
1772                 case L2CAP_INFO_RSP:
1773                         err = l2cap_information_rsp(conn, &cmd, data);
1774                         break;
1775
1776                 default:
1777                         BT_ERR("Unknown signaling command 0x%2.2x", cmd.code);
1778                         err = -EINVAL;
1779                         break;
1780                 }
1781
1782                 if (err) {
1783                         struct l2cap_cmd_rej rej;
1784                         BT_DBG("error %d", err);
1785
1786                         /* FIXME: Map err to a valid reason */
1787                         rej.reason = __cpu_to_le16(0);
1788                         l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
1789                 }
1790
1791                 data += cmd.len;
1792                 len  -= cmd.len;
1793         }
1794
1795         kfree_skb(skb);
1796 }
1797
1798 static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
1799 {
1800         struct sock *sk;
1801
1802         sk = l2cap_get_chan_by_scid(&conn->chan_list, cid);
1803         if (!sk) {
1804                 BT_DBG("unknown cid 0x%4.4x", cid);
1805                 goto drop;
1806         }
1807
1808         BT_DBG("sk %p, len %d", sk, skb->len);
1809
1810         if (sk->sk_state != BT_CONNECTED)
1811                 goto drop;
1812
1813         if (l2cap_pi(sk)->imtu < skb->len)
1814                 goto drop;
1815
1816         /* If socket recv buffers overflows we drop data here
1817          * which is *bad* because L2CAP has to be reliable.
1818          * But we don't have any other choice. L2CAP doesn't
1819          * provide flow control mechanism. */
1820
1821         if (!sock_queue_rcv_skb(sk, skb))
1822                 goto done;
1823
1824 drop:
1825         kfree_skb(skb);
1826
1827 done:
1828         if (sk) bh_unlock_sock(sk);
1829         return 0;
1830 }
1831
1832 static inline int l2cap_conless_channel(struct l2cap_conn *conn, u16 psm, struct sk_buff *skb)
1833 {
1834         struct sock *sk;
1835
1836         sk = l2cap_get_sock_by_psm(0, psm, conn->src);
1837         if (!sk)
1838                 goto drop;
1839
1840         BT_DBG("sk %p, len %d", sk, skb->len);
1841
1842         if (sk->sk_state != BT_BOUND && sk->sk_state != BT_CONNECTED)
1843                 goto drop;
1844
1845         if (l2cap_pi(sk)->imtu < skb->len)
1846                 goto drop;
1847
1848         if (!sock_queue_rcv_skb(sk, skb))
1849                 goto done;
1850
1851 drop:
1852         kfree_skb(skb);
1853
1854 done:
1855         if (sk) bh_unlock_sock(sk);
1856         return 0;
1857 }
1858
1859 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
1860 {
1861         struct l2cap_hdr *lh = (void *) skb->data;
1862         u16 cid, psm, len;
1863
1864         skb_pull(skb, L2CAP_HDR_SIZE);
1865         cid = __le16_to_cpu(lh->cid);
1866         len = __le16_to_cpu(lh->len);
1867
1868         BT_DBG("len %d, cid 0x%4.4x", len, cid);
1869
1870         switch (cid) {
1871         case 0x0001:
1872                 l2cap_sig_channel(conn, skb);
1873                 break;
1874
1875         case 0x0002:
1876                 psm = get_unaligned((u16 *) skb->data);
1877                 skb_pull(skb, 2);
1878                 l2cap_conless_channel(conn, psm, skb);
1879                 break;
1880
1881         default:
1882                 l2cap_data_channel(conn, cid, skb);
1883                 break;
1884         }
1885 }
1886
1887 /* ---- L2CAP interface with lower layer (HCI) ---- */
1888
1889 static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
1890 {
1891         int exact = 0, lm1 = 0, lm2 = 0;
1892         register struct sock *sk;
1893         struct hlist_node *node;
1894
1895         if (type != ACL_LINK)
1896                 return 0;
1897
1898         BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
1899
1900         /* Find listening sockets and check their link_mode */
1901         read_lock(&l2cap_sk_list.lock);
1902         sk_for_each(sk, node, &l2cap_sk_list.head) {
1903                 if (sk->sk_state != BT_LISTEN)
1904                         continue;
1905
1906                 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
1907                         lm1 |= (HCI_LM_ACCEPT | l2cap_pi(sk)->link_mode);
1908                         exact++;
1909                 } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
1910                         lm2 |= (HCI_LM_ACCEPT | l2cap_pi(sk)->link_mode);
1911         }
1912         read_unlock(&l2cap_sk_list.lock);
1913
1914         return exact ? lm1 : lm2;
1915 }
1916
1917 static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
1918 {
1919         BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
1920
1921         if (hcon->type != ACL_LINK)
1922                 return 0;
1923
1924         if (!status) {
1925                 struct l2cap_conn *conn;
1926
1927                 conn = l2cap_conn_add(hcon, status);
1928                 if (conn)
1929                         l2cap_conn_ready(conn);
1930         } else
1931                 l2cap_conn_del(hcon, bt_err(status));
1932
1933         return 0;
1934 }
1935
1936 static int l2cap_disconn_ind(struct hci_conn *hcon, u8 reason)
1937 {
1938         BT_DBG("hcon %p reason %d", hcon, reason);
1939
1940         if (hcon->type != ACL_LINK)
1941                 return 0;
1942
1943         l2cap_conn_del(hcon, bt_err(reason));
1944         return 0;
1945 }
1946
1947 static int l2cap_auth_cfm(struct hci_conn *hcon, u8 status)
1948 {
1949         struct l2cap_chan_list *l;
1950         struct l2cap_conn *conn;
1951         struct l2cap_conn_rsp rsp;
1952         struct sock *sk;
1953         int result;
1954
1955         if (!(conn = hcon->l2cap_data))
1956                 return 0;
1957         l = &conn->chan_list;
1958
1959         BT_DBG("conn %p", conn);
1960
1961         read_lock(&l->lock);
1962
1963         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
1964                 bh_lock_sock(sk);
1965
1966                 if (sk->sk_state != BT_CONNECT2 ||
1967                                 (l2cap_pi(sk)->link_mode & L2CAP_LM_ENCRYPT) ||
1968                                 (l2cap_pi(sk)->link_mode & L2CAP_LM_SECURE)) {
1969                         bh_unlock_sock(sk);
1970                         continue;
1971                 }
1972
1973                 if (!status) {
1974                         sk->sk_state = BT_CONFIG;
1975                         result = 0;
1976                 } else {
1977                         sk->sk_state = BT_DISCONN;
1978                         l2cap_sock_set_timer(sk, HZ/10);
1979                         result = L2CAP_CR_SEC_BLOCK;
1980                 }
1981
1982                 rsp.scid   = __cpu_to_le16(l2cap_pi(sk)->dcid);
1983                 rsp.dcid   = __cpu_to_le16(l2cap_pi(sk)->scid);
1984                 rsp.result = __cpu_to_le16(result);
1985                 rsp.status = __cpu_to_le16(0);
1986                 l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
1987                                 L2CAP_CONN_RSP, sizeof(rsp), &rsp);
1988
1989                 bh_unlock_sock(sk);
1990         }
1991
1992         read_unlock(&l->lock);
1993         return 0;
1994 }
1995
1996 static int l2cap_encrypt_cfm(struct hci_conn *hcon, u8 status)
1997 {
1998         struct l2cap_chan_list *l;
1999         struct l2cap_conn *conn;
2000         struct l2cap_conn_rsp rsp;
2001         struct sock *sk;
2002         int result;
2003
2004         if (!(conn = hcon->l2cap_data))
2005                 return 0;
2006         l = &conn->chan_list;
2007
2008         BT_DBG("conn %p", conn);
2009
2010         read_lock(&l->lock);
2011
2012         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
2013                 bh_lock_sock(sk);
2014
2015                 if (sk->sk_state != BT_CONNECT2) {
2016                         bh_unlock_sock(sk);
2017                         continue;
2018                 }
2019
2020                 if (!status) {
2021                         sk->sk_state = BT_CONFIG;
2022                         result = 0;
2023                 } else {
2024                         sk->sk_state = BT_DISCONN;
2025                         l2cap_sock_set_timer(sk, HZ/10);
2026                         result = L2CAP_CR_SEC_BLOCK;
2027                 }
2028
2029                 rsp.scid   = __cpu_to_le16(l2cap_pi(sk)->dcid);
2030                 rsp.dcid   = __cpu_to_le16(l2cap_pi(sk)->scid);
2031                 rsp.result = __cpu_to_le16(result);
2032                 rsp.status = __cpu_to_le16(0);
2033                 l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
2034                                 L2CAP_CONN_RSP, sizeof(rsp), &rsp);
2035
2036                 if (l2cap_pi(sk)->link_mode & L2CAP_LM_SECURE)
2037                         hci_conn_change_link_key(hcon);
2038
2039                 bh_unlock_sock(sk);
2040         }
2041
2042         read_unlock(&l->lock);
2043         return 0;
2044 }
2045
2046 static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
2047 {
2048         struct l2cap_conn *conn = hcon->l2cap_data;
2049
2050         if (!conn && !(conn = l2cap_conn_add(hcon, 0)))
2051                 goto drop;
2052
2053         BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
2054
2055         if (flags & ACL_START) {
2056                 struct l2cap_hdr *hdr;
2057                 int len;
2058
2059                 if (conn->rx_len) {
2060                         BT_ERR("Unexpected start frame (len %d)", skb->len);
2061                         kfree_skb(conn->rx_skb);
2062                         conn->rx_skb = NULL;
2063                         conn->rx_len = 0;
2064                         l2cap_conn_unreliable(conn, ECOMM);
2065                 }
2066
2067                 if (skb->len < 2) {
2068                         BT_ERR("Frame is too short (len %d)", skb->len);
2069                         l2cap_conn_unreliable(conn, ECOMM);
2070                         goto drop;
2071                 }
2072
2073                 hdr = (struct l2cap_hdr *) skb->data;
2074                 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
2075
2076                 if (len == skb->len) {
2077                         /* Complete frame received */
2078                         l2cap_recv_frame(conn, skb);
2079                         return 0;
2080                 }
2081
2082                 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
2083
2084                 if (skb->len > len) {
2085                         BT_ERR("Frame is too long (len %d, expected len %d)",
2086                                 skb->len, len);
2087                         l2cap_conn_unreliable(conn, ECOMM);
2088                         goto drop;
2089                 }
2090
2091                 /* Allocate skb for the complete frame (with header) */
2092                 if (!(conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC)))
2093                         goto drop;
2094
2095                 memcpy(skb_put(conn->rx_skb, skb->len), skb->data, skb->len);
2096                 conn->rx_len = len - skb->len;
2097         } else {
2098                 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
2099
2100                 if (!conn->rx_len) {
2101                         BT_ERR("Unexpected continuation frame (len %d)", skb->len);
2102                         l2cap_conn_unreliable(conn, ECOMM);
2103                         goto drop;
2104                 }
2105
2106                 if (skb->len > conn->rx_len) {
2107                         BT_ERR("Fragment is too long (len %d, expected %d)",
2108                                         skb->len, conn->rx_len);
2109                         kfree_skb(conn->rx_skb);
2110                         conn->rx_skb = NULL;
2111                         conn->rx_len = 0;
2112                         l2cap_conn_unreliable(conn, ECOMM);
2113                         goto drop;
2114                 }
2115
2116                 memcpy(skb_put(conn->rx_skb, skb->len), skb->data, skb->len);
2117                 conn->rx_len -= skb->len;
2118
2119                 if (!conn->rx_len) {
2120                         /* Complete frame received */
2121                         l2cap_recv_frame(conn, conn->rx_skb);
2122                         conn->rx_skb = NULL;
2123                 }
2124         }
2125
2126 drop:
2127         kfree_skb(skb);
2128         return 0;
2129 }
2130
2131 /* ---- Proc fs support ---- */
2132 #ifdef CONFIG_PROC_FS
2133 static void *l2cap_seq_start(struct seq_file *seq, loff_t *pos)
2134 {
2135         struct sock *sk;
2136         struct hlist_node *node;
2137         loff_t l = *pos;
2138
2139         read_lock_bh(&l2cap_sk_list.lock);
2140
2141         sk_for_each(sk, node, &l2cap_sk_list.head)
2142                 if (!l--)
2143                         goto found;
2144         sk = NULL;
2145 found:
2146         return sk;
2147 }
2148
2149 static void *l2cap_seq_next(struct seq_file *seq, void *e, loff_t *pos)
2150 {
2151         (*pos)++;
2152         return sk_next(e);
2153 }
2154
2155 static void l2cap_seq_stop(struct seq_file *seq, void *e)
2156 {
2157         read_unlock_bh(&l2cap_sk_list.lock);
2158 }
2159
2160 static int  l2cap_seq_show(struct seq_file *seq, void *e)
2161 {
2162         struct sock *sk = e;
2163         struct l2cap_pinfo *pi = l2cap_pi(sk);
2164
2165         seq_printf(seq, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d 0x%x\n",
2166                         batostr(&bt_sk(sk)->src), batostr(&bt_sk(sk)->dst),
2167                         sk->sk_state, pi->psm, pi->scid, pi->dcid, pi->imtu,
2168                         pi->omtu, pi->link_mode);
2169         return 0;
2170 }
2171
2172 static struct seq_operations l2cap_seq_ops = {
2173         .start  = l2cap_seq_start,
2174         .next   = l2cap_seq_next,
2175         .stop   = l2cap_seq_stop,
2176         .show   = l2cap_seq_show
2177 };
2178
2179 static int l2cap_seq_open(struct inode *inode, struct file *file)
2180 {
2181         return seq_open(file, &l2cap_seq_ops);
2182 }
2183
2184 static struct file_operations l2cap_seq_fops = {
2185         .owner          = THIS_MODULE,
2186         .open           = l2cap_seq_open,
2187         .read           = seq_read,
2188         .llseek         = seq_lseek,
2189         .release        = seq_release,
2190 };
2191
2192 static int __init l2cap_proc_init(void)
2193 {
2194         struct proc_dir_entry *p = create_proc_entry("l2cap", S_IRUGO, proc_bt);
2195         if (!p)
2196                 return -ENOMEM;
2197         p->owner     = THIS_MODULE;
2198         p->proc_fops = &l2cap_seq_fops;
2199         return 0;
2200 }
2201
2202 static void __exit l2cap_proc_cleanup(void)
2203 {
2204         remove_proc_entry("l2cap", proc_bt);
2205 }
2206
2207 #else /* CONFIG_PROC_FS */
2208
2209 static int __init l2cap_proc_init(void)
2210 {
2211         return 0;
2212 }
2213
2214 static void __exit l2cap_proc_cleanup(void)
2215 {
2216         return;
2217 }
2218 #endif /* CONFIG_PROC_FS */
2219
2220 static struct proto_ops l2cap_sock_ops = {
2221         .family         = PF_BLUETOOTH,
2222         .owner          = THIS_MODULE,
2223         .release        = l2cap_sock_release,
2224         .bind           = l2cap_sock_bind,
2225         .connect        = l2cap_sock_connect,
2226         .listen         = l2cap_sock_listen,
2227         .accept         = l2cap_sock_accept,
2228         .getname        = l2cap_sock_getname,
2229         .sendmsg        = l2cap_sock_sendmsg,
2230         .recvmsg        = bt_sock_recvmsg,
2231         .poll           = bt_sock_poll,
2232         .mmap           = sock_no_mmap,
2233         .socketpair     = sock_no_socketpair,
2234         .ioctl          = sock_no_ioctl,
2235         .shutdown       = l2cap_sock_shutdown,
2236         .setsockopt     = l2cap_sock_setsockopt,
2237         .getsockopt     = l2cap_sock_getsockopt
2238 };
2239
2240 static struct net_proto_family l2cap_sock_family_ops = {
2241         .family = PF_BLUETOOTH,
2242         .owner  = THIS_MODULE,
2243         .create = l2cap_sock_create,
2244 };
2245
2246 static struct hci_proto l2cap_hci_proto = {
2247         .name           = "L2CAP",
2248         .id             = HCI_PROTO_L2CAP,
2249         .connect_ind    = l2cap_connect_ind,
2250         .connect_cfm    = l2cap_connect_cfm,
2251         .disconn_ind    = l2cap_disconn_ind,
2252         .auth_cfm       = l2cap_auth_cfm,
2253         .encrypt_cfm    = l2cap_encrypt_cfm,
2254         .recv_acldata   = l2cap_recv_acldata
2255 };
2256
2257 static int __init l2cap_init(void)
2258 {
2259         int err;
2260
2261         if ((err = bt_sock_register(BTPROTO_L2CAP, &l2cap_sock_family_ops))) {
2262                 BT_ERR("L2CAP socket registration failed");
2263                 return err;
2264         }
2265
2266         if ((err = hci_register_proto(&l2cap_hci_proto))) {
2267                 BT_ERR("L2CAP protocol registration failed");
2268                 return err;
2269         }
2270
2271         l2cap_proc_init();
2272
2273         BT_INFO("L2CAP ver %s", VERSION);
2274         BT_INFO("L2CAP socket layer initialized");
2275
2276         return 0;
2277 }
2278
2279 static void __exit l2cap_exit(void)
2280 {
2281         l2cap_proc_cleanup();
2282
2283         /* Unregister socket and protocol */
2284         if (bt_sock_unregister(BTPROTO_L2CAP))
2285                 BT_ERR("L2CAP socket unregistration failed");
2286
2287         if (hci_unregister_proto(&l2cap_hci_proto))
2288                 BT_ERR("L2CAP protocol unregistration failed");
2289 }
2290
2291 void l2cap_load(void)
2292 {
2293         /* Dummy function to trigger automatic L2CAP module loading by
2294          * other modules that use L2CAP sockets but don't use any other
2295          * symbols from it. */
2296         return;
2297 }
2298 EXPORT_SYMBOL(l2cap_load);
2299
2300 module_init(l2cap_init);
2301 module_exit(l2cap_exit);
2302
2303 MODULE_AUTHOR("Maxim Krasnyansky <maxk@qualcomm.com>, Marcel Holtmann <marcel@holtmann.org>");
2304 MODULE_DESCRIPTION("Bluetooth L2CAP ver " VERSION);
2305 MODULE_VERSION(VERSION);
2306 MODULE_LICENSE("GPL");
2307 MODULE_ALIAS("bt-proto-0");