net/bluetooth/l2cap.c

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