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 <linux/debugfs.h>
  44 #include <linux/seq_file.h>
  45 #include <linux/uaccess.h>
  46 #include <linux/crc16.h>
  47 #include <net/sock.h>
  48
  49 #include <asm/system.h>
  50 #include <asm/unaligned.h>
  51
  52 #include <net/bluetooth/bluetooth.h>
  53 #include <net/bluetooth/hci_core.h>
  54 #include <net/bluetooth/l2cap.h>
  55
  56 #define VERSION "2.14"
  57
  58 static int enable_ertm = 0;
  59 static int max_transmit = L2CAP_DEFAULT_MAX_TX;
  60 static int tx_window = L2CAP_DEFAULT_TX_WINDOW;
  61
  62 static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN;
  63 static u8 l2cap_fixed_chan[8] = { 0x02, };
  64
  65 static const struct proto_ops l2cap_sock_ops;
  66
  67 static struct bt_sock_list l2cap_sk_list = {
  68         .lock = __RW_LOCK_UNLOCKED(l2cap_sk_list.lock)
  69 };
  70
  71 static void __l2cap_sock_close(struct sock *sk, int reason);
  72 static void l2cap_sock_close(struct sock *sk);
  73 static void l2cap_sock_kill(struct sock *sk);
  74
  75 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
  76                                 u8 code, u8 ident, u16 dlen, void *data);
  77
  78 /* ---- L2CAP timers ---- */
  79 static void l2cap_sock_timeout(unsigned long arg)
  80 {
  81         struct sock *sk = (struct sock *) arg;
  82         int reason;
  83
  84         BT_DBG("sock %p state %d", sk, sk->sk_state);
  85
  86         bh_lock_sock(sk);
  87
  88         if (sk->sk_state == BT_CONNECTED || sk->sk_state == BT_CONFIG)
  89                 reason = ECONNREFUSED;
  90         else if (sk->sk_state == BT_CONNECT &&
  91                                 l2cap_pi(sk)->sec_level != BT_SECURITY_SDP)
  92                 reason = ECONNREFUSED;
  93         else
  94                 reason = ETIMEDOUT;
  95
  96         __l2cap_sock_close(sk, reason);
  97
  98         bh_unlock_sock(sk);
  99
 100         l2cap_sock_kill(sk);
 101         sock_put(sk);
 102 }
 103
 104 static void l2cap_sock_set_timer(struct sock *sk, long timeout)
 105 {
 106         BT_DBG("sk %p state %d timeout %ld", sk, sk->sk_state, timeout);
 107         sk_reset_timer(sk, &sk->sk_timer, jiffies + timeout);
 108 }
 109
 110 static void l2cap_sock_clear_timer(struct sock *sk)
 111 {
 112         BT_DBG("sock %p state %d", sk, sk->sk_state);
 113         sk_stop_timer(sk, &sk->sk_timer);
 114 }
 115
 116 /* ---- L2CAP channels ---- */
 117 static struct sock *__l2cap_get_chan_by_dcid(struct l2cap_chan_list *l, u16 cid)
 118 {
 119         struct sock *s;
 120         for (s = l->head; s; s = l2cap_pi(s)->next_c) {
 121                 if (l2cap_pi(s)->dcid == cid)
 122                         break;
 123         }
 124         return s;
 125 }
 126
 127 static struct sock *__l2cap_get_chan_by_scid(struct l2cap_chan_list *l, u16 cid)
 128 {
 129         struct sock *s;
 130         for (s = l->head; s; s = l2cap_pi(s)->next_c) {
 131                 if (l2cap_pi(s)->scid == cid)
 132                         break;
 133         }
 134         return s;
 135 }
 136
 137 /* Find channel with given SCID.
 138  * Returns locked socket */
 139 static inline struct sock *l2cap_get_chan_by_scid(struct l2cap_chan_list *l, u16 cid)
 140 {
 141         struct sock *s;
 142         read_lock(&l->lock);
 143         s = __l2cap_get_chan_by_scid(l, cid);
 144         if (s)
 145                 bh_lock_sock(s);
 146         read_unlock(&l->lock);
 147         return s;
 148 }
 149
 150 static struct sock *__l2cap_get_chan_by_ident(struct l2cap_chan_list *l, u8 ident)
 151 {
 152         struct sock *s;
 153         for (s = l->head; s; s = l2cap_pi(s)->next_c) {
 154                 if (l2cap_pi(s)->ident == ident)
 155                         break;
 156         }
 157         return s;
 158 }
 159
 160 static inline struct sock *l2cap_get_chan_by_ident(struct l2cap_chan_list *l, u8 ident)
 161 {
 162         struct sock *s;
 163         read_lock(&l->lock);
 164         s = __l2cap_get_chan_by_ident(l, ident);
 165         if (s)
 166                 bh_lock_sock(s);
 167         read_unlock(&l->lock);
 168         return s;
 169 }
 170
 171 static u16 l2cap_alloc_cid(struct l2cap_chan_list *l)
 172 {
 173         u16 cid = L2CAP_CID_DYN_START;
 174
 175         for (; cid < L2CAP_CID_DYN_END; cid++) {
 176                 if (!__l2cap_get_chan_by_scid(l, cid))
 177                         return cid;
 178         }
 179
 180         return 0;
 181 }
 182
 183 static inline void __l2cap_chan_link(struct l2cap_chan_list *l, struct sock *sk)
 184 {
 185         sock_hold(sk);
 186
 187         if (l->head)
 188                 l2cap_pi(l->head)->prev_c = sk;
 189
 190         l2cap_pi(sk)->next_c = l->head;
 191         l2cap_pi(sk)->prev_c = NULL;
 192         l->head = sk;
 193 }
 194
 195 static inline void l2cap_chan_unlink(struct l2cap_chan_list *l, struct sock *sk)
 196 {
 197         struct sock *next = l2cap_pi(sk)->next_c, *prev = l2cap_pi(sk)->prev_c;
 198
 199         write_lock_bh(&l->lock);
 200         if (sk == l->head)
 201                 l->head = next;
 202
 203         if (next)
 204                 l2cap_pi(next)->prev_c = prev;
 205         if (prev)
 206                 l2cap_pi(prev)->next_c = next;
 207         write_unlock_bh(&l->lock);
 208
 209         __sock_put(sk);
 210 }
 211
 212 static void __l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct sock *parent)
 213 {
 214         struct l2cap_chan_list *l = &conn->chan_list;
 215
 216         BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn,
 217                         l2cap_pi(sk)->psm, l2cap_pi(sk)->dcid);
 218
 219         conn->disc_reason = 0x13;
 220
 221         l2cap_pi(sk)->conn = conn;
 222
 223         if (sk->sk_type == SOCK_SEQPACKET) {
 224                 /* Alloc CID for connection-oriented socket */
 225                 l2cap_pi(sk)->scid = l2cap_alloc_cid(l);
 226         } else if (sk->sk_type == SOCK_DGRAM) {
 227                 /* Connectionless socket */
 228                 l2cap_pi(sk)->scid = L2CAP_CID_CONN_LESS;
 229                 l2cap_pi(sk)->dcid = L2CAP_CID_CONN_LESS;
 230                 l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU;
 231         } else {
 232                 /* Raw socket can send/recv signalling messages only */
 233                 l2cap_pi(sk)->scid = L2CAP_CID_SIGNALING;
 234                 l2cap_pi(sk)->dcid = L2CAP_CID_SIGNALING;
 235                 l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU;
 236         }
 237
 238         __l2cap_chan_link(l, sk);
 239
 240         if (parent)
 241                 bt_accept_enqueue(parent, sk);
 242 }
 243
 244 /* Delete channel.
 245  * Must be called on the locked socket. */
 246 static void l2cap_chan_del(struct sock *sk, int err)
 247 {
 248         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
 249         struct sock *parent = bt_sk(sk)->parent;
 250
 251         l2cap_sock_clear_timer(sk);
 252
 253         BT_DBG("sk %p, conn %p, err %d", sk, conn, err);
 254
 255         if (conn) {
 256                 /* Unlink from channel list */
 257                 l2cap_chan_unlink(&conn->chan_list, sk);
 258                 l2cap_pi(sk)->conn = NULL;
 259                 hci_conn_put(conn->hcon);
 260         }
 261
 262         sk->sk_state = BT_CLOSED;
 263         sock_set_flag(sk, SOCK_ZAPPED);
 264
 265         if (err)
 266                 sk->sk_err = err;
 267
 268         if (parent) {
 269                 bt_accept_unlink(sk);
 270                 parent->sk_data_ready(parent, 0);
 271         } else
 272                 sk->sk_state_change(sk);
 273 }
 274
 275 /* Service level security */
 276 static inline int l2cap_check_security(struct sock *sk)
 277 {
 278         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
 279         __u8 auth_type;
 280
 281         if (l2cap_pi(sk)->psm == cpu_to_le16(0x0001)) {
 282                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_HIGH)
 283                         auth_type = HCI_AT_NO_BONDING_MITM;
 284                 else
 285                         auth_type = HCI_AT_NO_BONDING;
 286
 287                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_LOW)
 288                         l2cap_pi(sk)->sec_level = BT_SECURITY_SDP;
 289         } else {
 290                 switch (l2cap_pi(sk)->sec_level) {
 291                 case BT_SECURITY_HIGH:
 292                         auth_type = HCI_AT_GENERAL_BONDING_MITM;
 293                         break;
 294                 case BT_SECURITY_MEDIUM:
 295                         auth_type = HCI_AT_GENERAL_BONDING;
 296                         break;
 297                 default:
 298                         auth_type = HCI_AT_NO_BONDING;
 299                         break;
 300                 }
 301         }
 302
 303         return hci_conn_security(conn->hcon, l2cap_pi(sk)->sec_level,
 304                                                                 auth_type);
 305 }
 306
 307 static inline u8 l2cap_get_ident(struct l2cap_conn *conn)
 308 {
 309         u8 id;
 310
 311         /* Get next available identificator.
 312          *    1 - 128 are used by kernel.
 313          *  129 - 199 are reserved.
 314          *  200 - 254 are used by utilities like l2ping, etc.
 315          */
 316
 317         spin_lock_bh(&conn->lock);
 318
 319         if (++conn->tx_ident > 128)
 320                 conn->tx_ident = 1;
 321
 322         id = conn->tx_ident;
 323
 324         spin_unlock_bh(&conn->lock);
 325
 326         return id;
 327 }
 328
 329 static inline int l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, void *data)
 330 {
 331         struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data);
 332
 333         BT_DBG("code 0x%2.2x", code);
 334
 335         if (!skb)
 336                 return -ENOMEM;
 337
 338         return hci_send_acl(conn->hcon, skb, 0);
 339 }
 340
 341 static inline int l2cap_send_sframe(struct l2cap_pinfo *pi, u16 control)
 342 {
 343         struct sk_buff *skb;
 344         struct l2cap_hdr *lh;
 345         struct l2cap_conn *conn = pi->conn;
 346         int count, hlen = L2CAP_HDR_SIZE + 2;
 347
 348         if (pi->fcs == L2CAP_FCS_CRC16)
 349                 hlen += 2;
 350
 351         BT_DBG("pi %p, control 0x%2.2x", pi, control);
 352
 353         count = min_t(unsigned int, conn->mtu, hlen);
 354         control |= L2CAP_CTRL_FRAME_TYPE;
 355
 356         if (pi->conn_state & L2CAP_CONN_SEND_FBIT) {
 357                 control |= L2CAP_CTRL_FINAL;
 358                 pi->conn_state &= ~L2CAP_CONN_SEND_FBIT;
 359         }
 360
 361         if (pi->conn_state & L2CAP_CONN_SEND_PBIT) {
 362                 control |= L2CAP_CTRL_POLL;
 363                 pi->conn_state &= ~L2CAP_CONN_SEND_PBIT;
 364         }
 365
 366         skb = bt_skb_alloc(count, GFP_ATOMIC);
 367         if (!skb)
 368                 return -ENOMEM;
 369
 370         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
 371         lh->len = cpu_to_le16(hlen - L2CAP_HDR_SIZE);
 372         lh->cid = cpu_to_le16(pi->dcid);
 373         put_unaligned_le16(control, skb_put(skb, 2));
 374
 375         if (pi->fcs == L2CAP_FCS_CRC16) {
 376                 u16 fcs = crc16(0, (u8 *)lh, count - 2);
 377                 put_unaligned_le16(fcs, skb_put(skb, 2));
 378         }
 379
 380         return hci_send_acl(pi->conn->hcon, skb, 0);
 381 }
 382
 383 static inline int l2cap_send_rr_or_rnr(struct l2cap_pinfo *pi, u16 control)
 384 {
 385         if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY)
 386                 control |= L2CAP_SUPER_RCV_NOT_READY;
 387         else
 388                 control |= L2CAP_SUPER_RCV_READY;
 389
 390         control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
 391
 392         return l2cap_send_sframe(pi, control);
 393 }
 394
 395 static void l2cap_do_start(struct sock *sk)
 396 {
 397         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
 398
 399         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) {
 400                 if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE))
 401                         return;
 402
 403                 if (l2cap_check_security(sk)) {
 404                         struct l2cap_conn_req req;
 405                         req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
 406                         req.psm  = l2cap_pi(sk)->psm;
 407
 408                         l2cap_pi(sk)->ident = l2cap_get_ident(conn);
 409
 410                         l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
 411                                         L2CAP_CONN_REQ, sizeof(req), &req);
 412                 }
 413         } else {
 414                 struct l2cap_info_req req;
 415                 req.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
 416
 417                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
 418                 conn->info_ident = l2cap_get_ident(conn);
 419
 420                 mod_timer(&conn->info_timer, jiffies +
 421                                         msecs_to_jiffies(L2CAP_INFO_TIMEOUT));
 422
 423                 l2cap_send_cmd(conn, conn->info_ident,
 424                                         L2CAP_INFO_REQ, sizeof(req), &req);
 425         }
 426 }
 427
 428 static void l2cap_send_disconn_req(struct l2cap_conn *conn, struct sock *sk)
 429 {
 430         struct l2cap_disconn_req req;
 431
 432         req.dcid = cpu_to_le16(l2cap_pi(sk)->dcid);
 433         req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
 434         l2cap_send_cmd(conn, l2cap_get_ident(conn),
 435                         L2CAP_DISCONN_REQ, sizeof(req), &req);
 436 }
 437
 438 /* ---- L2CAP connections ---- */
 439 static void l2cap_conn_start(struct l2cap_conn *conn)
 440 {
 441         struct l2cap_chan_list *l = &conn->chan_list;
 442         struct sock *sk;
 443
 444         BT_DBG("conn %p", conn);
 445
 446         read_lock(&l->lock);
 447
 448         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
 449                 bh_lock_sock(sk);
 450
 451                 if (sk->sk_type != SOCK_SEQPACKET) {
 452                         bh_unlock_sock(sk);
 453                         continue;
 454                 }
 455
 456                 if (sk->sk_state == BT_CONNECT) {
 457                         if (l2cap_check_security(sk)) {
 458                                 struct l2cap_conn_req req;
 459                                 req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
 460                                 req.psm  = l2cap_pi(sk)->psm;
 461
 462                                 l2cap_pi(sk)->ident = l2cap_get_ident(conn);
 463
 464                                 l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
 465                                         L2CAP_CONN_REQ, sizeof(req), &req);
 466                         }
 467                 } else if (sk->sk_state == BT_CONNECT2) {
 468                         struct l2cap_conn_rsp rsp;
 469                         rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid);
 470                         rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid);
 471
 472                         if (l2cap_check_security(sk)) {
 473                                 if (bt_sk(sk)->defer_setup) {
 474                                         struct sock *parent = bt_sk(sk)->parent;
 475                                         rsp.result = cpu_to_le16(L2CAP_CR_PEND);
 476                                         rsp.status = cpu_to_le16(L2CAP_CS_AUTHOR_PEND);
 477                                         parent->sk_data_ready(parent, 0);
 478
 479                                 } else {
 480                                         sk->sk_state = BT_CONFIG;
 481                                         rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
 482                                         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
 483                                 }
 484                         } else {
 485                                 rsp.result = cpu_to_le16(L2CAP_CR_PEND);
 486                                 rsp.status = cpu_to_le16(L2CAP_CS_AUTHEN_PEND);
 487                         }
 488
 489                         l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
 490                                         L2CAP_CONN_RSP, sizeof(rsp), &rsp);
 491                 }
 492
 493                 bh_unlock_sock(sk);
 494         }
 495
 496         read_unlock(&l->lock);
 497 }
 498
 499 static void l2cap_conn_ready(struct l2cap_conn *conn)
 500 {
 501         struct l2cap_chan_list *l = &conn->chan_list;
 502         struct sock *sk;
 503
 504         BT_DBG("conn %p", conn);
 505
 506         read_lock(&l->lock);
 507
 508         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
 509                 bh_lock_sock(sk);
 510
 511                 if (sk->sk_type != SOCK_SEQPACKET) {
 512                         l2cap_sock_clear_timer(sk);
 513                         sk->sk_state = BT_CONNECTED;
 514                         sk->sk_state_change(sk);
 515                 } else if (sk->sk_state == BT_CONNECT)
 516                         l2cap_do_start(sk);
 517
 518                 bh_unlock_sock(sk);
 519         }
 520
 521         read_unlock(&l->lock);
 522 }
 523
 524 /* Notify sockets that we cannot guaranty reliability anymore */
 525 static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err)
 526 {
 527         struct l2cap_chan_list *l = &conn->chan_list;
 528         struct sock *sk;
 529
 530         BT_DBG("conn %p", conn);
 531
 532         read_lock(&l->lock);
 533
 534         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
 535                 if (l2cap_pi(sk)->force_reliable)
 536                         sk->sk_err = err;
 537         }
 538
 539         read_unlock(&l->lock);
 540 }
 541
 542 static void l2cap_info_timeout(unsigned long arg)
 543 {
 544         struct l2cap_conn *conn = (void *) arg;
 545
 546         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
 547         conn->info_ident = 0;
 548
 549         l2cap_conn_start(conn);
 550 }
 551
 552 static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status)
 553 {
 554         struct l2cap_conn *conn = hcon->l2cap_data;
 555
 556         if (conn || status)
 557                 return conn;
 558
 559         conn = kzalloc(sizeof(struct l2cap_conn), GFP_ATOMIC);
 560         if (!conn)
 561                 return NULL;
 562
 563         hcon->l2cap_data = conn;
 564         conn->hcon = hcon;
 565
 566         BT_DBG("hcon %p conn %p", hcon, conn);
 567
 568         conn->mtu = hcon->hdev->acl_mtu;
 569         conn->src = &hcon->hdev->bdaddr;
 570         conn->dst = &hcon->dst;
 571
 572         conn->feat_mask = 0;
 573
 574         spin_lock_init(&conn->lock);
 575         rwlock_init(&conn->chan_list.lock);
 576
 577         setup_timer(&conn->info_timer, l2cap_info_timeout,
 578                                                 (unsigned long) conn);
 579
 580         conn->disc_reason = 0x13;
 581
 582         return conn;
 583 }
 584
 585 static void l2cap_conn_del(struct hci_conn *hcon, int err)
 586 {
 587         struct l2cap_conn *conn = hcon->l2cap_data;
 588         struct sock *sk;
 589
 590         if (!conn)
 591                 return;
 592
 593         BT_DBG("hcon %p conn %p, err %d", hcon, conn, err);
 594
 595         kfree_skb(conn->rx_skb);
 596
 597         /* Kill channels */
 598         while ((sk = conn->chan_list.head)) {
 599                 bh_lock_sock(sk);
 600                 l2cap_chan_del(sk, err);
 601                 bh_unlock_sock(sk);
 602                 l2cap_sock_kill(sk);
 603         }
 604
 605         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT)
 606                 del_timer_sync(&conn->info_timer);
 607
 608         hcon->l2cap_data = NULL;
 609         kfree(conn);
 610 }
 611
 612 static inline void l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct sock *parent)
 613 {
 614         struct l2cap_chan_list *l = &conn->chan_list;
 615         write_lock_bh(&l->lock);
 616         __l2cap_chan_add(conn, sk, parent);
 617         write_unlock_bh(&l->lock);
 618 }
 619
 620 /* ---- Socket interface ---- */
 621 static struct sock *__l2cap_get_sock_by_addr(__le16 psm, bdaddr_t *src)
 622 {
 623         struct sock *sk;
 624         struct hlist_node *node;
 625         sk_for_each(sk, node, &l2cap_sk_list.head)
 626                 if (l2cap_pi(sk)->sport == psm && !bacmp(&bt_sk(sk)->src, src))
 627                         goto found;
 628         sk = NULL;
 629 found:
 630         return sk;
 631 }
 632
 633 /* Find socket with psm and source bdaddr.
 634  * Returns closest match.
 635  */
 636 static struct sock *__l2cap_get_sock_by_psm(int state, __le16 psm, bdaddr_t *src)
 637 {
 638         struct sock *sk = NULL, *sk1 = NULL;
 639         struct hlist_node *node;
 640
 641         sk_for_each(sk, node, &l2cap_sk_list.head) {
 642                 if (state && sk->sk_state != state)
 643                         continue;
 644
 645                 if (l2cap_pi(sk)->psm == psm) {
 646                         /* Exact match. */
 647                         if (!bacmp(&bt_sk(sk)->src, src))
 648                                 break;
 649
 650                         /* Closest match */
 651                         if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
 652                                 sk1 = sk;
 653                 }
 654         }
 655         return node ? sk : sk1;
 656 }
 657
 658 /* Find socket with given address (psm, src).
 659  * Returns locked socket */
 660 static inline struct sock *l2cap_get_sock_by_psm(int state, __le16 psm, bdaddr_t *src)
 661 {
 662         struct sock *s;
 663         read_lock(&l2cap_sk_list.lock);
 664         s = __l2cap_get_sock_by_psm(state, psm, src);
 665         if (s)
 666                 bh_lock_sock(s);
 667         read_unlock(&l2cap_sk_list.lock);
 668         return s;
 669 }
 670
 671 static void l2cap_sock_destruct(struct sock *sk)
 672 {
 673         BT_DBG("sk %p", sk);
 674
 675         skb_queue_purge(&sk->sk_receive_queue);
 676         skb_queue_purge(&sk->sk_write_queue);
 677 }
 678
 679 static void l2cap_sock_cleanup_listen(struct sock *parent)
 680 {
 681         struct sock *sk;
 682
 683         BT_DBG("parent %p", parent);
 684
 685         /* Close not yet accepted channels */
 686         while ((sk = bt_accept_dequeue(parent, NULL)))
 687                 l2cap_sock_close(sk);
 688
 689         parent->sk_state = BT_CLOSED;
 690         sock_set_flag(parent, SOCK_ZAPPED);
 691 }
 692
 693 /* Kill socket (only if zapped and orphan)
 694  * Must be called on unlocked socket.
 695  */
 696 static void l2cap_sock_kill(struct sock *sk)
 697 {
 698         if (!sock_flag(sk, SOCK_ZAPPED) || sk->sk_socket)
 699                 return;
 700
 701         BT_DBG("sk %p state %d", sk, sk->sk_state);
 702
 703         /* Kill poor orphan */
 704         bt_sock_unlink(&l2cap_sk_list, sk);
 705         sock_set_flag(sk, SOCK_DEAD);
 706         sock_put(sk);
 707 }
 708
 709 static void __l2cap_sock_close(struct sock *sk, int reason)
 710 {
 711         BT_DBG("sk %p state %d socket %p", sk, sk->sk_state, sk->sk_socket);
 712
 713         switch (sk->sk_state) {
 714         case BT_LISTEN:
 715                 l2cap_sock_cleanup_listen(sk);
 716                 break;
 717
 718         case BT_CONNECTED:
 719         case BT_CONFIG:
 720                 if (sk->sk_type == SOCK_SEQPACKET) {
 721                         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
 722
 723                         sk->sk_state = BT_DISCONN;
 724                         l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
 725                         l2cap_send_disconn_req(conn, sk);
 726                 } else
 727                         l2cap_chan_del(sk, reason);
 728                 break;
 729
 730         case BT_CONNECT2:
 731                 if (sk->sk_type == SOCK_SEQPACKET) {
 732                         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
 733                         struct l2cap_conn_rsp rsp;
 734                         __u16 result;
 735
 736                         if (bt_sk(sk)->defer_setup)
 737                                 result = L2CAP_CR_SEC_BLOCK;
 738                         else
 739                                 result = L2CAP_CR_BAD_PSM;
 740
 741                         rsp.scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
 742                         rsp.dcid   = cpu_to_le16(l2cap_pi(sk)->scid);
 743                         rsp.result = cpu_to_le16(result);
 744                         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
 745                         l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
 746                                         L2CAP_CONN_RSP, sizeof(rsp), &rsp);
 747                 } else
 748                         l2cap_chan_del(sk, reason);
 749                 break;
 750
 751         case BT_CONNECT:
 752         case BT_DISCONN:
 753                 l2cap_chan_del(sk, reason);
 754                 break;
 755
 756         default:
 757                 sock_set_flag(sk, SOCK_ZAPPED);
 758                 break;
 759         }
 760 }
 761
 762 /* Must be called on unlocked socket. */
 763 static void l2cap_sock_close(struct sock *sk)
 764 {
 765         l2cap_sock_clear_timer(sk);
 766         lock_sock(sk);
 767         __l2cap_sock_close(sk, ECONNRESET);
 768         release_sock(sk);
 769         l2cap_sock_kill(sk);
 770 }
 771
 772 static void l2cap_sock_init(struct sock *sk, struct sock *parent)
 773 {
 774         struct l2cap_pinfo *pi = l2cap_pi(sk);
 775
 776         BT_DBG("sk %p", sk);
 777
 778         if (parent) {
 779                 sk->sk_type = parent->sk_type;
 780                 bt_sk(sk)->defer_setup = bt_sk(parent)->defer_setup;
 781
 782                 pi->imtu = l2cap_pi(parent)->imtu;
 783                 pi->omtu = l2cap_pi(parent)->omtu;
 784                 pi->mode = l2cap_pi(parent)->mode;
 785                 pi->fcs  = l2cap_pi(parent)->fcs;
 786                 pi->max_tx = l2cap_pi(parent)->max_tx;
 787                 pi->tx_win = l2cap_pi(parent)->tx_win;
 788                 pi->sec_level = l2cap_pi(parent)->sec_level;
 789                 pi->role_switch = l2cap_pi(parent)->role_switch;
 790                 pi->force_reliable = l2cap_pi(parent)->force_reliable;
 791         } else {
 792                 pi->imtu = L2CAP_DEFAULT_MTU;
 793                 pi->omtu = 0;
 794                 pi->mode = L2CAP_MODE_BASIC;
 795                 pi->max_tx = max_transmit;
 796                 pi->fcs  = L2CAP_FCS_CRC16;
 797                 pi->tx_win = tx_window;
 798                 pi->sec_level = BT_SECURITY_LOW;
 799                 pi->role_switch = 0;
 800                 pi->force_reliable = 0;
 801         }
 802
 803         /* Default config options */
 804         pi->conf_len = 0;
 805         pi->flush_to = L2CAP_DEFAULT_FLUSH_TO;
 806         skb_queue_head_init(TX_QUEUE(sk));
 807         skb_queue_head_init(SREJ_QUEUE(sk));
 808         INIT_LIST_HEAD(SREJ_LIST(sk));
 809 }
 810
 811 static struct proto l2cap_proto = {
 812         .name           = "L2CAP",
 813         .owner          = THIS_MODULE,
 814         .obj_size       = sizeof(struct l2cap_pinfo)
 815 };
 816
 817 static struct sock *l2cap_sock_alloc(struct net *net, struct socket *sock, int proto, gfp_t prio)
 818 {
 819         struct sock *sk;
 820
 821         sk = sk_alloc(net, PF_BLUETOOTH, prio, &l2cap_proto);
 822         if (!sk)
 823                 return NULL;
 824
 825         sock_init_data(sock, sk);
 826         INIT_LIST_HEAD(&bt_sk(sk)->accept_q);
 827
 828         sk->sk_destruct = l2cap_sock_destruct;
 829         sk->sk_sndtimeo = msecs_to_jiffies(L2CAP_CONN_TIMEOUT);
 830
 831         sock_reset_flag(sk, SOCK_ZAPPED);
 832
 833         sk->sk_protocol = proto;
 834         sk->sk_state = BT_OPEN;
 835
 836         setup_timer(&sk->sk_timer, l2cap_sock_timeout, (unsigned long) sk);
 837
 838         bt_sock_link(&l2cap_sk_list, sk);
 839         return sk;
 840 }
 841
 842 static int l2cap_sock_create(struct net *net, struct socket *sock, int protocol,
 843                              int kern)
 844 {
 845         struct sock *sk;
 846
 847         BT_DBG("sock %p", sock);
 848
 849         sock->state = SS_UNCONNECTED;
 850
 851         if (sock->type != SOCK_SEQPACKET &&
 852                         sock->type != SOCK_DGRAM && sock->type != SOCK_RAW)
 853                 return -ESOCKTNOSUPPORT;
 854
 855         if (sock->type == SOCK_RAW && !kern && !capable(CAP_NET_RAW))
 856                 return -EPERM;
 857
 858         sock->ops = &l2cap_sock_ops;
 859
 860         sk = l2cap_sock_alloc(net, sock, protocol, GFP_ATOMIC);
 861         if (!sk)
 862                 return -ENOMEM;
 863
 864         l2cap_sock_init(sk, NULL);
 865         return 0;
 866 }
 867
 868 static int l2cap_sock_bind(struct socket *sock, struct sockaddr *addr, int alen)
 869 {
 870         struct sock *sk = sock->sk;
 871         struct sockaddr_l2 la;
 872         int len, err = 0;
 873
 874         BT_DBG("sk %p", sk);
 875
 876         if (!addr || addr->sa_family != AF_BLUETOOTH)
 877                 return -EINVAL;
 878
 879         memset(&la, 0, sizeof(la));
 880         len = min_t(unsigned int, sizeof(la), alen);
 881         memcpy(&la, addr, len);
 882
 883         if (la.l2_cid)
 884                 return -EINVAL;
 885
 886         lock_sock(sk);
 887
 888         if (sk->sk_state != BT_OPEN) {
 889                 err = -EBADFD;
 890                 goto done;
 891         }
 892
 893         if (la.l2_psm && __le16_to_cpu(la.l2_psm) < 0x1001 &&
 894                                 !capable(CAP_NET_BIND_SERVICE)) {
 895                 err = -EACCES;
 896                 goto done;
 897         }
 898
 899         write_lock_bh(&l2cap_sk_list.lock);
 900
 901         if (la.l2_psm && __l2cap_get_sock_by_addr(la.l2_psm, &la.l2_bdaddr)) {
 902                 err = -EADDRINUSE;
 903         } else {
 904                 /* Save source address */
 905                 bacpy(&bt_sk(sk)->src, &la.l2_bdaddr);
 906                 l2cap_pi(sk)->psm   = la.l2_psm;
 907                 l2cap_pi(sk)->sport = la.l2_psm;
 908                 sk->sk_state = BT_BOUND;
 909
 910                 if (__le16_to_cpu(la.l2_psm) == 0x0001 ||
 911                                         __le16_to_cpu(la.l2_psm) == 0x0003)
 912                         l2cap_pi(sk)->sec_level = BT_SECURITY_SDP;
 913         }
 914
 915         write_unlock_bh(&l2cap_sk_list.lock);
 916
 917 done:
 918         release_sock(sk);
 919         return err;
 920 }
 921
 922 static int l2cap_do_connect(struct sock *sk)
 923 {
 924         bdaddr_t *src = &bt_sk(sk)->src;
 925         bdaddr_t *dst = &bt_sk(sk)->dst;
 926         struct l2cap_conn *conn;
 927         struct hci_conn *hcon;
 928         struct hci_dev *hdev;
 929         __u8 auth_type;
 930         int err;
 931
 932         BT_DBG("%s -> %s psm 0x%2.2x", batostr(src), batostr(dst),
 933                                                         l2cap_pi(sk)->psm);
 934
 935         hdev = hci_get_route(dst, src);
 936         if (!hdev)
 937                 return -EHOSTUNREACH;
 938
 939         hci_dev_lock_bh(hdev);
 940
 941         err = -ENOMEM;
 942
 943         if (sk->sk_type == SOCK_RAW) {
 944                 switch (l2cap_pi(sk)->sec_level) {
 945                 case BT_SECURITY_HIGH:
 946                         auth_type = HCI_AT_DEDICATED_BONDING_MITM;
 947                         break;
 948                 case BT_SECURITY_MEDIUM:
 949                         auth_type = HCI_AT_DEDICATED_BONDING;
 950                         break;
 951                 default:
 952                         auth_type = HCI_AT_NO_BONDING;
 953                         break;
 954                 }
 955         } else if (l2cap_pi(sk)->psm == cpu_to_le16(0x0001)) {
 956                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_HIGH)
 957                         auth_type = HCI_AT_NO_BONDING_MITM;
 958                 else
 959                         auth_type = HCI_AT_NO_BONDING;
 960
 961                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_LOW)
 962                         l2cap_pi(sk)->sec_level = BT_SECURITY_SDP;
 963         } else {
 964                 switch (l2cap_pi(sk)->sec_level) {
 965                 case BT_SECURITY_HIGH:
 966                         auth_type = HCI_AT_GENERAL_BONDING_MITM;
 967                         break;
 968                 case BT_SECURITY_MEDIUM:
 969                         auth_type = HCI_AT_GENERAL_BONDING;
 970                         break;
 971                 default:
 972                         auth_type = HCI_AT_NO_BONDING;
 973                         break;
 974                 }
 975         }
 976
 977         hcon = hci_connect(hdev, ACL_LINK, dst,
 978                                         l2cap_pi(sk)->sec_level, auth_type);
 979         if (!hcon)
 980                 goto done;
 981
 982         conn = l2cap_conn_add(hcon, 0);
 983         if (!conn) {
 984                 hci_conn_put(hcon);
 985                 goto done;
 986         }
 987
 988         err = 0;
 989
 990         /* Update source addr of the socket */
 991         bacpy(src, conn->src);
 992
 993         l2cap_chan_add(conn, sk, NULL);
 994
 995         sk->sk_state = BT_CONNECT;
 996         l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
 997
 998         if (hcon->state == BT_CONNECTED) {
 999                 if (sk->sk_type != SOCK_SEQPACKET) {
1000                         l2cap_sock_clear_timer(sk);
1001                         sk->sk_state = BT_CONNECTED;
1002                 } else
1003                         l2cap_do_start(sk);
1004         }
1005
1006 done:
1007         hci_dev_unlock_bh(hdev);
1008         hci_dev_put(hdev);
1009         return err;
1010 }
1011
1012 static int l2cap_sock_connect(struct socket *sock, struct sockaddr *addr, int alen, int flags)
1013 {
1014         struct sock *sk = sock->sk;
1015         struct sockaddr_l2 la;
1016         int len, err = 0;
1017
1018         BT_DBG("sk %p", sk);
1019
1020         if (!addr || alen < sizeof(addr->sa_family) ||
1021             addr->sa_family != AF_BLUETOOTH)
1022                 return -EINVAL;
1023
1024         memset(&la, 0, sizeof(la));
1025         len = min_t(unsigned int, sizeof(la), alen);
1026         memcpy(&la, addr, len);
1027
1028         if (la.l2_cid)
1029                 return -EINVAL;
1030
1031         lock_sock(sk);
1032
1033         if (sk->sk_type == SOCK_SEQPACKET && !la.l2_psm) {
1034                 err = -EINVAL;
1035                 goto done;
1036         }
1037
1038         switch (l2cap_pi(sk)->mode) {
1039         case L2CAP_MODE_BASIC:
1040                 break;
1041         case L2CAP_MODE_ERTM:
1042         case L2CAP_MODE_STREAMING:
1043                 if (enable_ertm)
1044                         break;
1045                 /* fall through */
1046         default:
1047                 err = -ENOTSUPP;
1048                 goto done;
1049         }
1050
1051         switch (sk->sk_state) {
1052         case BT_CONNECT:
1053         case BT_CONNECT2:
1054         case BT_CONFIG:
1055                 /* Already connecting */
1056                 goto wait;
1057
1058         case BT_CONNECTED:
1059                 /* Already connected */
1060                 goto done;
1061
1062         case BT_OPEN:
1063         case BT_BOUND:
1064                 /* Can connect */
1065                 break;
1066
1067         default:
1068                 err = -EBADFD;
1069                 goto done;
1070         }
1071
1072         /* Set destination address and psm */
1073         bacpy(&bt_sk(sk)->dst, &la.l2_bdaddr);
1074         l2cap_pi(sk)->psm = la.l2_psm;
1075
1076         err = l2cap_do_connect(sk);
1077         if (err)
1078                 goto done;
1079
1080 wait:
1081         err = bt_sock_wait_state(sk, BT_CONNECTED,
1082                         sock_sndtimeo(sk, flags & O_NONBLOCK));
1083 done:
1084         release_sock(sk);
1085         return err;
1086 }
1087
1088 static int l2cap_sock_listen(struct socket *sock, int backlog)
1089 {
1090         struct sock *sk = sock->sk;
1091         int err = 0;
1092
1093         BT_DBG("sk %p backlog %d", sk, backlog);
1094
1095         lock_sock(sk);
1096
1097         if (sk->sk_state != BT_BOUND || sock->type != SOCK_SEQPACKET) {
1098                 err = -EBADFD;
1099                 goto done;
1100         }
1101
1102         switch (l2cap_pi(sk)->mode) {
1103         case L2CAP_MODE_BASIC:
1104                 break;
1105         case L2CAP_MODE_ERTM:
1106         case L2CAP_MODE_STREAMING:
1107                 if (enable_ertm)
1108                         break;
1109                 /* fall through */
1110         default:
1111                 err = -ENOTSUPP;
1112                 goto done;
1113         }
1114
1115         if (!l2cap_pi(sk)->psm) {
1116                 bdaddr_t *src = &bt_sk(sk)->src;
1117                 u16 psm;
1118
1119                 err = -EINVAL;
1120
1121                 write_lock_bh(&l2cap_sk_list.lock);
1122
1123                 for (psm = 0x1001; psm < 0x1100; psm += 2)
1124                         if (!__l2cap_get_sock_by_addr(cpu_to_le16(psm), src)) {
1125                                 l2cap_pi(sk)->psm   = cpu_to_le16(psm);
1126                                 l2cap_pi(sk)->sport = cpu_to_le16(psm);
1127                                 err = 0;
1128                                 break;
1129                         }
1130
1131                 write_unlock_bh(&l2cap_sk_list.lock);
1132
1133                 if (err < 0)
1134                         goto done;
1135         }
1136
1137         sk->sk_max_ack_backlog = backlog;
1138         sk->sk_ack_backlog = 0;
1139         sk->sk_state = BT_LISTEN;
1140
1141 done:
1142         release_sock(sk);
1143         return err;
1144 }
1145
1146 static int l2cap_sock_accept(struct socket *sock, struct socket *newsock, int flags)
1147 {
1148         DECLARE_WAITQUEUE(wait, current);
1149         struct sock *sk = sock->sk, *nsk;
1150         long timeo;
1151         int err = 0;
1152
1153         lock_sock_nested(sk, SINGLE_DEPTH_NESTING);
1154
1155         if (sk->sk_state != BT_LISTEN) {
1156                 err = -EBADFD;
1157                 goto done;
1158         }
1159
1160         timeo = sock_rcvtimeo(sk, flags & O_NONBLOCK);
1161
1162         BT_DBG("sk %p timeo %ld", sk, timeo);
1163
1164         /* Wait for an incoming connection. (wake-one). */
1165         add_wait_queue_exclusive(sk_sleep(sk), &wait);
1166         while (!(nsk = bt_accept_dequeue(sk, newsock))) {
1167                 set_current_state(TASK_INTERRUPTIBLE);
1168                 if (!timeo) {
1169                         err = -EAGAIN;
1170                         break;
1171                 }
1172
1173                 release_sock(sk);
1174                 timeo = schedule_timeout(timeo);
1175                 lock_sock_nested(sk, SINGLE_DEPTH_NESTING);
1176
1177                 if (sk->sk_state != BT_LISTEN) {
1178                         err = -EBADFD;
1179                         break;
1180                 }
1181
1182                 if (signal_pending(current)) {
1183                         err = sock_intr_errno(timeo);
1184                         break;
1185                 }
1186         }
1187         set_current_state(TASK_RUNNING);
1188         remove_wait_queue(sk_sleep(sk), &wait);
1189
1190         if (err)
1191                 goto done;
1192
1193         newsock->state = SS_CONNECTED;
1194
1195         BT_DBG("new socket %p", nsk);
1196
1197 done:
1198         release_sock(sk);
1199         return err;
1200 }
1201
1202 static int l2cap_sock_getname(struct socket *sock, struct sockaddr *addr, int *len, int peer)
1203 {
1204         struct sockaddr_l2 *la = (struct sockaddr_l2 *) addr;
1205         struct sock *sk = sock->sk;
1206
1207         BT_DBG("sock %p, sk %p", sock, sk);
1208
1209         addr->sa_family = AF_BLUETOOTH;
1210         *len = sizeof(struct sockaddr_l2);
1211
1212         if (peer) {
1213                 la->l2_psm = l2cap_pi(sk)->psm;
1214                 bacpy(&la->l2_bdaddr, &bt_sk(sk)->dst);
1215                 la->l2_cid = cpu_to_le16(l2cap_pi(sk)->dcid);
1216         } else {
1217                 la->l2_psm = l2cap_pi(sk)->sport;
1218                 bacpy(&la->l2_bdaddr, &bt_sk(sk)->src);
1219                 la->l2_cid = cpu_to_le16(l2cap_pi(sk)->scid);
1220         }
1221
1222         return 0;
1223 }
1224
1225 static void l2cap_monitor_timeout(unsigned long arg)
1226 {
1227         struct sock *sk = (void *) arg;
1228         u16 control;
1229
1230         bh_lock_sock(sk);
1231         if (l2cap_pi(sk)->retry_count >= l2cap_pi(sk)->remote_max_tx) {
1232                 l2cap_send_disconn_req(l2cap_pi(sk)->conn, sk);
1233                 bh_unlock_sock(sk);
1234                 return;
1235         }
1236
1237         l2cap_pi(sk)->retry_count++;
1238         __mod_monitor_timer();
1239
1240         control = L2CAP_CTRL_POLL;
1241         l2cap_send_rr_or_rnr(l2cap_pi(sk), control);
1242         bh_unlock_sock(sk);
1243 }
1244
1245 static void l2cap_retrans_timeout(unsigned long arg)
1246 {
1247         struct sock *sk = (void *) arg;
1248         u16 control;
1249
1250         bh_lock_sock(sk);
1251         l2cap_pi(sk)->retry_count = 1;
1252         __mod_monitor_timer();
1253
1254         l2cap_pi(sk)->conn_state |= L2CAP_CONN_WAIT_F;
1255
1256         control = L2CAP_CTRL_POLL;
1257         l2cap_send_rr_or_rnr(l2cap_pi(sk), control);
1258         bh_unlock_sock(sk);
1259 }
1260
1261 static void l2cap_drop_acked_frames(struct sock *sk)
1262 {
1263         struct sk_buff *skb;
1264
1265         while ((skb = skb_peek(TX_QUEUE(sk)))) {
1266                 if (bt_cb(skb)->tx_seq == l2cap_pi(sk)->expected_ack_seq)
1267                         break;
1268
1269                 skb = skb_dequeue(TX_QUEUE(sk));
1270                 kfree_skb(skb);
1271
1272                 l2cap_pi(sk)->unacked_frames--;
1273         }
1274
1275         if (!l2cap_pi(sk)->unacked_frames)
1276                 del_timer(&l2cap_pi(sk)->retrans_timer);
1277
1278         return;
1279 }
1280
1281 static inline int l2cap_do_send(struct sock *sk, struct sk_buff *skb)
1282 {
1283         struct l2cap_pinfo *pi = l2cap_pi(sk);
1284         int err;
1285
1286         BT_DBG("sk %p, skb %p len %d", sk, skb, skb->len);
1287
1288         err = hci_send_acl(pi->conn->hcon, skb, 0);
1289         if (err < 0)
1290                 kfree_skb(skb);
1291
1292         return err;
1293 }
1294
1295 static int l2cap_streaming_send(struct sock *sk)
1296 {
1297         struct sk_buff *skb, *tx_skb;
1298         struct l2cap_pinfo *pi = l2cap_pi(sk);
1299         u16 control, fcs;
1300         int err;
1301
1302         while ((skb = sk->sk_send_head)) {
1303                 tx_skb = skb_clone(skb, GFP_ATOMIC);
1304
1305                 control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE);
1306                 control |= pi->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT;
1307                 put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);
1308
1309                 if (pi->fcs == L2CAP_FCS_CRC16) {
1310                         fcs = crc16(0, (u8 *)tx_skb->data, tx_skb->len - 2);
1311                         put_unaligned_le16(fcs, tx_skb->data + tx_skb->len - 2);
1312                 }
1313
1314                 err = l2cap_do_send(sk, tx_skb);
1315                 if (err < 0) {
1316                         l2cap_send_disconn_req(pi->conn, sk);
1317                         return err;
1318                 }
1319
1320                 pi->next_tx_seq = (pi->next_tx_seq + 1) % 64;
1321
1322                 if (skb_queue_is_last(TX_QUEUE(sk), skb))
1323                         sk->sk_send_head = NULL;
1324                 else
1325                         sk->sk_send_head = skb_queue_next(TX_QUEUE(sk), skb);
1326
1327                 skb = skb_dequeue(TX_QUEUE(sk));
1328                 kfree_skb(skb);
1329         }
1330         return 0;
1331 }
1332
1333 static int l2cap_retransmit_frame(struct sock *sk, u8 tx_seq)
1334 {
1335         struct l2cap_pinfo *pi = l2cap_pi(sk);
1336         struct sk_buff *skb, *tx_skb;
1337         u16 control, fcs;
1338         int err;
1339
1340         skb = skb_peek(TX_QUEUE(sk));
1341         do {
1342                 if (bt_cb(skb)->tx_seq != tx_seq) {
1343                         if (skb_queue_is_last(TX_QUEUE(sk), skb))
1344                                 break;
1345                         skb = skb_queue_next(TX_QUEUE(sk), skb);
1346                         continue;
1347                 }
1348
1349                 if (pi->remote_max_tx &&
1350                                 bt_cb(skb)->retries == pi->remote_max_tx) {
1351                         l2cap_send_disconn_req(pi->conn, sk);
1352                         break;
1353                 }
1354
1355                 tx_skb = skb_clone(skb, GFP_ATOMIC);
1356                 bt_cb(skb)->retries++;
1357                 control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE);
1358                 control |= (pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT)
1359                                 | (tx_seq << L2CAP_CTRL_TXSEQ_SHIFT);
1360                 put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);
1361
1362                 if (pi->fcs == L2CAP_FCS_CRC16) {
1363                         fcs = crc16(0, (u8 *)tx_skb->data, tx_skb->len - 2);
1364                         put_unaligned_le16(fcs, tx_skb->data + tx_skb->len - 2);
1365                 }
1366
1367                 err = l2cap_do_send(sk, tx_skb);
1368                 if (err < 0) {
1369                         l2cap_send_disconn_req(pi->conn, sk);
1370                         return err;
1371                 }
1372                 break;
1373         } while(1);
1374         return 0;
1375 }
1376
1377 static int l2cap_ertm_send(struct sock *sk)
1378 {
1379         struct sk_buff *skb, *tx_skb;
1380         struct l2cap_pinfo *pi = l2cap_pi(sk);
1381         u16 control, fcs;
1382         int err, nsent = 0;
1383
1384         if (pi->conn_state & L2CAP_CONN_WAIT_F)
1385                 return 0;
1386
1387         while ((skb = sk->sk_send_head) && (!l2cap_tx_window_full(sk)) &&
1388                !(pi->conn_state & L2CAP_CONN_REMOTE_BUSY)) {
1389
1390                 if (pi->remote_max_tx &&
1391                                 bt_cb(skb)->retries == pi->remote_max_tx) {
1392                         l2cap_send_disconn_req(pi->conn, sk);
1393                         break;
1394                 }
1395
1396                 tx_skb = skb_clone(skb, GFP_ATOMIC);
1397
1398                 bt_cb(skb)->retries++;
1399
1400                 control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE);
1401                 if (pi->conn_state & L2CAP_CONN_SEND_FBIT) {
1402                         control |= L2CAP_CTRL_FINAL;
1403                         pi->conn_state &= ~L2CAP_CONN_SEND_FBIT;
1404                 }
1405                 control |= (pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT)
1406                                 | (pi->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT);
1407                 put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);
1408
1409
1410                 if (pi->fcs == L2CAP_FCS_CRC16) {
1411                         fcs = crc16(0, (u8 *)skb->data, tx_skb->len - 2);
1412                         put_unaligned_le16(fcs, skb->data + tx_skb->len - 2);
1413                 }
1414
1415                 err = l2cap_do_send(sk, tx_skb);
1416                 if (err < 0) {
1417                         l2cap_send_disconn_req(pi->conn, sk);
1418                         return err;
1419                 }
1420                 __mod_retrans_timer();
1421
1422                 bt_cb(skb)->tx_seq = pi->next_tx_seq;
1423                 pi->next_tx_seq = (pi->next_tx_seq + 1) % 64;
1424
1425                 pi->unacked_frames++;
1426                 pi->frames_sent++;
1427
1428                 if (skb_queue_is_last(TX_QUEUE(sk), skb))
1429                         sk->sk_send_head = NULL;
1430                 else
1431                         sk->sk_send_head = skb_queue_next(TX_QUEUE(sk), skb);
1432
1433                 nsent++;
1434         }
1435
1436         return nsent;
1437 }
1438
1439 static int l2cap_send_ack(struct l2cap_pinfo *pi)
1440 {
1441         struct sock *sk = (struct sock *)pi;
1442         u16 control = 0;
1443
1444         control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
1445
1446         if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
1447                 control |= L2CAP_SUPER_RCV_NOT_READY;
1448                 return l2cap_send_sframe(pi, control);
1449         } else if (l2cap_ertm_send(sk) == 0) {
1450                 control |= L2CAP_SUPER_RCV_READY;
1451                 return l2cap_send_sframe(pi, control);
1452         }
1453         return 0;
1454 }
1455
1456 static int l2cap_send_srejtail(struct sock *sk)
1457 {
1458         struct srej_list *tail;
1459         u16 control;
1460
1461         control = L2CAP_SUPER_SELECT_REJECT;
1462         control |= L2CAP_CTRL_FINAL;
1463
1464         tail = list_entry(SREJ_LIST(sk)->prev, struct srej_list, list);
1465         control |= tail->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
1466
1467         l2cap_send_sframe(l2cap_pi(sk), control);
1468
1469         return 0;
1470 }
1471
1472 static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, int len, int count, struct sk_buff *skb)
1473 {
1474         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1475         struct sk_buff **frag;
1476         int err, sent = 0;
1477
1478         if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count)) {
1479                 return -EFAULT;
1480         }
1481
1482         sent += count;
1483         len  -= count;
1484
1485         /* Continuation fragments (no L2CAP header) */
1486         frag = &skb_shinfo(skb)->frag_list;
1487         while (len) {
1488                 count = min_t(unsigned int, conn->mtu, len);
1489
1490                 *frag = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err);
1491                 if (!*frag)
1492                         return -EFAULT;
1493                 if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count))
1494                         return -EFAULT;
1495
1496                 sent += count;
1497                 len  -= count;
1498
1499                 frag = &(*frag)->next;
1500         }
1501
1502         return sent;
1503 }
1504
1505 static struct sk_buff *l2cap_create_connless_pdu(struct sock *sk, struct msghdr *msg, size_t len)
1506 {
1507         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1508         struct sk_buff *skb;
1509         int err, count, hlen = L2CAP_HDR_SIZE + 2;
1510         struct l2cap_hdr *lh;
1511
1512         BT_DBG("sk %p len %d", sk, (int)len);
1513
1514         count = min_t(unsigned int, (conn->mtu - hlen), len);
1515         skb = bt_skb_send_alloc(sk, count + hlen,
1516                         msg->msg_flags & MSG_DONTWAIT, &err);
1517         if (!skb)
1518                 return ERR_PTR(-ENOMEM);
1519
1520         /* Create L2CAP header */
1521         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1522         lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
1523         lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1524         put_unaligned_le16(l2cap_pi(sk)->psm, skb_put(skb, 2));
1525
1526         err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1527         if (unlikely(err < 0)) {
1528                 kfree_skb(skb);
1529                 return ERR_PTR(err);
1530         }
1531         return skb;
1532 }
1533
1534 static struct sk_buff *l2cap_create_basic_pdu(struct sock *sk, struct msghdr *msg, size_t len)
1535 {
1536         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1537         struct sk_buff *skb;
1538         int err, count, hlen = L2CAP_HDR_SIZE;
1539         struct l2cap_hdr *lh;
1540
1541         BT_DBG("sk %p len %d", sk, (int)len);
1542
1543         count = min_t(unsigned int, (conn->mtu - hlen), len);
1544         skb = bt_skb_send_alloc(sk, count + hlen,
1545                         msg->msg_flags & MSG_DONTWAIT, &err);
1546         if (!skb)
1547                 return ERR_PTR(-ENOMEM);
1548
1549         /* Create L2CAP header */
1550         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1551         lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
1552         lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1553
1554         err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1555         if (unlikely(err < 0)) {
1556                 kfree_skb(skb);
1557                 return ERR_PTR(err);
1558         }
1559         return skb;
1560 }
1561
1562 static struct sk_buff *l2cap_create_iframe_pdu(struct sock *sk, struct msghdr *msg, size_t len, u16 control, u16 sdulen)
1563 {
1564         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1565         struct sk_buff *skb;
1566         int err, count, hlen = L2CAP_HDR_SIZE + 2;
1567         struct l2cap_hdr *lh;
1568
1569         BT_DBG("sk %p len %d", sk, (int)len);
1570
1571         if (sdulen)
1572                 hlen += 2;
1573
1574         if (l2cap_pi(sk)->fcs == L2CAP_FCS_CRC16)
1575                 hlen += 2;
1576
1577         count = min_t(unsigned int, (conn->mtu - hlen), len);
1578         skb = bt_skb_send_alloc(sk, count + hlen,
1579                         msg->msg_flags & MSG_DONTWAIT, &err);
1580         if (!skb)
1581                 return ERR_PTR(-ENOMEM);
1582
1583         /* Create L2CAP header */
1584         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1585         lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
1586         lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1587         put_unaligned_le16(control, skb_put(skb, 2));
1588         if (sdulen)
1589                 put_unaligned_le16(sdulen, skb_put(skb, 2));
1590
1591         err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1592         if (unlikely(err < 0)) {
1593                 kfree_skb(skb);
1594                 return ERR_PTR(err);
1595         }
1596
1597         if (l2cap_pi(sk)->fcs == L2CAP_FCS_CRC16)
1598                 put_unaligned_le16(0, skb_put(skb, 2));
1599
1600         bt_cb(skb)->retries = 0;
1601         return skb;
1602 }
1603
1604 static inline int l2cap_sar_segment_sdu(struct sock *sk, struct msghdr *msg, size_t len)
1605 {
1606         struct l2cap_pinfo *pi = l2cap_pi(sk);
1607         struct sk_buff *skb;
1608         struct sk_buff_head sar_queue;
1609         u16 control;
1610         size_t size = 0;
1611
1612         __skb_queue_head_init(&sar_queue);
1613         control = L2CAP_SDU_START;
1614         skb = l2cap_create_iframe_pdu(sk, msg, pi->remote_mps, control, len);
1615         if (IS_ERR(skb))
1616                 return PTR_ERR(skb);
1617
1618         __skb_queue_tail(&sar_queue, skb);
1619         len -= pi->remote_mps;
1620         size += pi->remote_mps;
1621         control = 0;
1622
1623         while (len > 0) {
1624                 size_t buflen;
1625
1626                 if (len > pi->remote_mps) {
1627                         control |= L2CAP_SDU_CONTINUE;
1628                         buflen = pi->remote_mps;
1629                 } else {
1630                         control |= L2CAP_SDU_END;
1631                         buflen = len;
1632                 }
1633
1634                 skb = l2cap_create_iframe_pdu(sk, msg, buflen, control, 0);
1635                 if (IS_ERR(skb)) {
1636                         skb_queue_purge(&sar_queue);
1637                         return PTR_ERR(skb);
1638                 }
1639
1640                 __skb_queue_tail(&sar_queue, skb);
1641                 len -= buflen;
1642                 size += buflen;
1643                 control = 0;
1644         }
1645         skb_queue_splice_tail(&sar_queue, TX_QUEUE(sk));
1646         if (sk->sk_send_head == NULL)
1647                 sk->sk_send_head = sar_queue.next;
1648
1649         return size;
1650 }
1651
1652 static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len)
1653 {
1654         struct sock *sk = sock->sk;
1655         struct l2cap_pinfo *pi = l2cap_pi(sk);
1656         struct sk_buff *skb;
1657         u16 control;
1658         int err;
1659
1660         BT_DBG("sock %p, sk %p", sock, sk);
1661
1662         err = sock_error(sk);
1663         if (err)
1664                 return err;
1665
1666         if (msg->msg_flags & MSG_OOB)
1667                 return -EOPNOTSUPP;
1668
1669         lock_sock(sk);
1670
1671         if (sk->sk_state != BT_CONNECTED) {
1672                 err = -ENOTCONN;
1673                 goto done;
1674         }
1675
1676         /* Connectionless channel */
1677         if (sk->sk_type == SOCK_DGRAM) {
1678                 skb = l2cap_create_connless_pdu(sk, msg, len);
1679                 if (IS_ERR(skb))
1680                         err = PTR_ERR(skb);
1681                 else
1682                         err = l2cap_do_send(sk, skb);
1683                 goto done;
1684         }
1685
1686         switch (pi->mode) {
1687         case L2CAP_MODE_BASIC:
1688                 /* Check outgoing MTU */
1689                 if (len > pi->omtu) {
1690                         err = -EINVAL;
1691                         goto done;
1692                 }
1693
1694                 /* Create a basic PDU */
1695                 skb = l2cap_create_basic_pdu(sk, msg, len);
1696                 if (IS_ERR(skb)) {
1697                         err = PTR_ERR(skb);
1698                         goto done;
1699                 }
1700
1701                 err = l2cap_do_send(sk, skb);
1702                 if (!err)
1703                         err = len;
1704                 break;
1705
1706         case L2CAP_MODE_ERTM:
1707         case L2CAP_MODE_STREAMING:
1708                 /* Entire SDU fits into one PDU */
1709                 if (len <= pi->remote_mps) {
1710                         control = L2CAP_SDU_UNSEGMENTED;
1711                         skb = l2cap_create_iframe_pdu(sk, msg, len, control, 0);
1712                         if (IS_ERR(skb)) {
1713                                 err = PTR_ERR(skb);
1714                                 goto done;
1715                         }
1716                         __skb_queue_tail(TX_QUEUE(sk), skb);
1717                         if (sk->sk_send_head == NULL)
1718                                 sk->sk_send_head = skb;
1719                 } else {
1720                 /* Segment SDU into multiples PDUs */
1721                         err = l2cap_sar_segment_sdu(sk, msg, len);
1722                         if (err < 0)
1723                                 goto done;
1724                 }
1725
1726                 if (pi->mode == L2CAP_MODE_STREAMING)
1727                         err = l2cap_streaming_send(sk);
1728                 else
1729                         err = l2cap_ertm_send(sk);
1730
1731                 if (err >= 0)
1732                         err = len;
1733                 break;
1734
1735         default:
1736                 BT_DBG("bad state %1.1x", pi->mode);
1737                 err = -EINVAL;
1738         }
1739
1740 done:
1741         release_sock(sk);
1742         return err;
1743 }
1744
1745 static int l2cap_sock_recvmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len, int flags)
1746 {
1747         struct sock *sk = sock->sk;
1748
1749         lock_sock(sk);
1750
1751         if (sk->sk_state == BT_CONNECT2 && bt_sk(sk)->defer_setup) {
1752                 struct l2cap_conn_rsp rsp;
1753
1754                 sk->sk_state = BT_CONFIG;
1755
1756                 rsp.scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
1757                 rsp.dcid   = cpu_to_le16(l2cap_pi(sk)->scid);
1758                 rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
1759                 rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
1760                 l2cap_send_cmd(l2cap_pi(sk)->conn, l2cap_pi(sk)->ident,
1761                                         L2CAP_CONN_RSP, sizeof(rsp), &rsp);
1762
1763                 release_sock(sk);
1764                 return 0;
1765         }
1766
1767         release_sock(sk);
1768
1769         return bt_sock_recvmsg(iocb, sock, msg, len, flags);
1770 }
1771
1772 static int l2cap_sock_setsockopt_old(struct socket *sock, int optname, char __user *optval, unsigned int optlen)
1773 {
1774         struct sock *sk = sock->sk;
1775         struct l2cap_options opts;
1776         int len, err = 0;
1777         u32 opt;
1778
1779         BT_DBG("sk %p", sk);
1780
1781         lock_sock(sk);
1782
1783         switch (optname) {
1784         case L2CAP_OPTIONS:
1785                 opts.imtu     = l2cap_pi(sk)->imtu;
1786                 opts.omtu     = l2cap_pi(sk)->omtu;
1787                 opts.flush_to = l2cap_pi(sk)->flush_to;
1788                 opts.mode     = l2cap_pi(sk)->mode;
1789                 opts.fcs      = l2cap_pi(sk)->fcs;
1790                 opts.max_tx   = l2cap_pi(sk)->max_tx;
1791                 opts.txwin_size = (__u16)l2cap_pi(sk)->tx_win;
1792
1793                 len = min_t(unsigned int, sizeof(opts), optlen);
1794                 if (copy_from_user((char *) &opts, optval, len)) {
1795                         err = -EFAULT;
1796                         break;
1797                 }
1798
1799                 l2cap_pi(sk)->imtu = opts.imtu;
1800                 l2cap_pi(sk)->omtu = opts.omtu;
1801                 l2cap_pi(sk)->mode = opts.mode;
1802                 l2cap_pi(sk)->fcs  = opts.fcs;
1803                 l2cap_pi(sk)->max_tx = opts.max_tx;
1804                 l2cap_pi(sk)->tx_win = (__u8)opts.txwin_size;
1805                 break;
1806
1807         case L2CAP_LM:
1808                 if (get_user(opt, (u32 __user *) optval)) {
1809                         err = -EFAULT;
1810                         break;
1811                 }
1812
1813                 if (opt & L2CAP_LM_AUTH)
1814                         l2cap_pi(sk)->sec_level = BT_SECURITY_LOW;
1815                 if (opt & L2CAP_LM_ENCRYPT)
1816                         l2cap_pi(sk)->sec_level = BT_SECURITY_MEDIUM;
1817                 if (opt & L2CAP_LM_SECURE)
1818                         l2cap_pi(sk)->sec_level = BT_SECURITY_HIGH;
1819
1820                 l2cap_pi(sk)->role_switch    = (opt & L2CAP_LM_MASTER);
1821                 l2cap_pi(sk)->force_reliable = (opt & L2CAP_LM_RELIABLE);
1822                 break;
1823
1824         default:
1825                 err = -ENOPROTOOPT;
1826                 break;
1827         }
1828
1829         release_sock(sk);
1830         return err;
1831 }
1832
1833 static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname, char __user *optval, unsigned int optlen)
1834 {
1835         struct sock *sk = sock->sk;
1836         struct bt_security sec;
1837         int len, err = 0;
1838         u32 opt;
1839
1840         BT_DBG("sk %p", sk);
1841
1842         if (level == SOL_L2CAP)
1843                 return l2cap_sock_setsockopt_old(sock, optname, optval, optlen);
1844
1845         if (level != SOL_BLUETOOTH)
1846                 return -ENOPROTOOPT;
1847
1848         lock_sock(sk);
1849
1850         switch (optname) {
1851         case BT_SECURITY:
1852                 if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_RAW) {
1853                         err = -EINVAL;
1854                         break;
1855                 }
1856
1857                 sec.level = BT_SECURITY_LOW;
1858
1859                 len = min_t(unsigned int, sizeof(sec), optlen);
1860                 if (copy_from_user((char *) &sec, optval, len)) {
1861                         err = -EFAULT;
1862                         break;
1863                 }
1864
1865                 if (sec.level < BT_SECURITY_LOW ||
1866                                         sec.level > BT_SECURITY_HIGH) {
1867                         err = -EINVAL;
1868                         break;
1869                 }
1870
1871                 l2cap_pi(sk)->sec_level = sec.level;
1872                 break;
1873
1874         case BT_DEFER_SETUP:
1875                 if (sk->sk_state != BT_BOUND && sk->sk_state != BT_LISTEN) {
1876                         err = -EINVAL;
1877                         break;
1878                 }
1879
1880                 if (get_user(opt, (u32 __user *) optval)) {
1881                         err = -EFAULT;
1882                         break;
1883                 }
1884
1885                 bt_sk(sk)->defer_setup = opt;
1886                 break;
1887
1888         default:
1889                 err = -ENOPROTOOPT;
1890                 break;
1891         }
1892
1893         release_sock(sk);
1894         return err;
1895 }
1896
1897 static int l2cap_sock_getsockopt_old(struct socket *sock, int optname, char __user *optval, int __user *optlen)
1898 {
1899         struct sock *sk = sock->sk;
1900         struct l2cap_options opts;
1901         struct l2cap_conninfo cinfo;
1902         int len, err = 0;
1903         u32 opt;
1904
1905         BT_DBG("sk %p", sk);
1906
1907         if (get_user(len, optlen))
1908                 return -EFAULT;
1909
1910         lock_sock(sk);
1911
1912         switch (optname) {
1913         case L2CAP_OPTIONS:
1914                 opts.imtu     = l2cap_pi(sk)->imtu;
1915                 opts.omtu     = l2cap_pi(sk)->omtu;
1916                 opts.flush_to = l2cap_pi(sk)->flush_to;
1917                 opts.mode     = l2cap_pi(sk)->mode;
1918                 opts.fcs      = l2cap_pi(sk)->fcs;
1919                 opts.max_tx   = l2cap_pi(sk)->max_tx;
1920                 opts.txwin_size = (__u16)l2cap_pi(sk)->tx_win;
1921
1922                 len = min_t(unsigned int, len, sizeof(opts));
1923                 if (copy_to_user(optval, (char *) &opts, len))
1924                         err = -EFAULT;
1925
1926                 break;
1927
1928         case L2CAP_LM:
1929                 switch (l2cap_pi(sk)->sec_level) {
1930                 case BT_SECURITY_LOW:
1931                         opt = L2CAP_LM_AUTH;
1932                         break;
1933                 case BT_SECURITY_MEDIUM:
1934                         opt = L2CAP_LM_AUTH | L2CAP_LM_ENCRYPT;
1935                         break;
1936                 case BT_SECURITY_HIGH:
1937                         opt = L2CAP_LM_AUTH | L2CAP_LM_ENCRYPT |
1938                                                         L2CAP_LM_SECURE;
1939                         break;
1940                 default:
1941                         opt = 0;
1942                         break;
1943                 }
1944
1945                 if (l2cap_pi(sk)->role_switch)
1946                         opt |= L2CAP_LM_MASTER;
1947
1948                 if (l2cap_pi(sk)->force_reliable)
1949                         opt |= L2CAP_LM_RELIABLE;
1950
1951                 if (put_user(opt, (u32 __user *) optval))
1952                         err = -EFAULT;
1953                 break;
1954
1955         case L2CAP_CONNINFO:
1956                 if (sk->sk_state != BT_CONNECTED &&
1957                                         !(sk->sk_state == BT_CONNECT2 &&
1958                                                 bt_sk(sk)->defer_setup)) {
1959                         err = -ENOTCONN;
1960                         break;
1961                 }
1962
1963                 cinfo.hci_handle = l2cap_pi(sk)->conn->hcon->handle;
1964                 memcpy(cinfo.dev_class, l2cap_pi(sk)->conn->hcon->dev_class, 3);
1965
1966                 len = min_t(unsigned int, len, sizeof(cinfo));
1967                 if (copy_to_user(optval, (char *) &cinfo, len))
1968                         err = -EFAULT;
1969
1970                 break;
1971
1972         default:
1973                 err = -ENOPROTOOPT;
1974                 break;
1975         }
1976
1977         release_sock(sk);
1978         return err;
1979 }
1980
1981 static int l2cap_sock_getsockopt(struct socket *sock, int level, int optname, char __user *optval, int __user *optlen)
1982 {
1983         struct sock *sk = sock->sk;
1984         struct bt_security sec;
1985         int len, err = 0;
1986
1987         BT_DBG("sk %p", sk);
1988
1989         if (level == SOL_L2CAP)
1990                 return l2cap_sock_getsockopt_old(sock, optname, optval, optlen);
1991
1992         if (level != SOL_BLUETOOTH)
1993                 return -ENOPROTOOPT;
1994
1995         if (get_user(len, optlen))
1996                 return -EFAULT;
1997
1998         lock_sock(sk);
1999
2000         switch (optname) {
2001         case BT_SECURITY:
2002                 if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_RAW) {
2003                         err = -EINVAL;
2004                         break;
2005                 }
2006
2007                 sec.level = l2cap_pi(sk)->sec_level;
2008
2009                 len = min_t(unsigned int, len, sizeof(sec));
2010                 if (copy_to_user(optval, (char *) &sec, len))
2011                         err = -EFAULT;
2012
2013                 break;
2014
2015         case BT_DEFER_SETUP:
2016                 if (sk->sk_state != BT_BOUND && sk->sk_state != BT_LISTEN) {
2017                         err = -EINVAL;
2018                         break;
2019                 }
2020
2021                 if (put_user(bt_sk(sk)->defer_setup, (u32 __user *) optval))
2022                         err = -EFAULT;
2023
2024                 break;
2025
2026         default:
2027                 err = -ENOPROTOOPT;
2028                 break;
2029         }
2030
2031         release_sock(sk);
2032         return err;
2033 }
2034
2035 static int l2cap_sock_shutdown(struct socket *sock, int how)
2036 {
2037         struct sock *sk = sock->sk;
2038         int err = 0;
2039
2040         BT_DBG("sock %p, sk %p", sock, sk);
2041
2042         if (!sk)
2043                 return 0;
2044
2045         lock_sock(sk);
2046         if (!sk->sk_shutdown) {
2047                 sk->sk_shutdown = SHUTDOWN_MASK;
2048                 l2cap_sock_clear_timer(sk);
2049                 __l2cap_sock_close(sk, 0);
2050
2051                 if (sock_flag(sk, SOCK_LINGER) && sk->sk_lingertime)
2052                         err = bt_sock_wait_state(sk, BT_CLOSED,
2053                                                         sk->sk_lingertime);
2054         }
2055         release_sock(sk);
2056         return err;
2057 }
2058
2059 static int l2cap_sock_release(struct socket *sock)
2060 {
2061         struct sock *sk = sock->sk;
2062         int err;
2063
2064         BT_DBG("sock %p, sk %p", sock, sk);
2065
2066         if (!sk)
2067                 return 0;
2068
2069         err = l2cap_sock_shutdown(sock, 2);
2070
2071         sock_orphan(sk);
2072         l2cap_sock_kill(sk);
2073         return err;
2074 }
2075
2076 static void l2cap_chan_ready(struct sock *sk)
2077 {
2078         struct sock *parent = bt_sk(sk)->parent;
2079
2080         BT_DBG("sk %p, parent %p", sk, parent);
2081
2082         l2cap_pi(sk)->conf_state = 0;
2083         l2cap_sock_clear_timer(sk);
2084
2085         if (!parent) {
2086                 /* Outgoing channel.
2087                  * Wake up socket sleeping on connect.
2088                  */
2089                 sk->sk_state = BT_CONNECTED;
2090                 sk->sk_state_change(sk);
2091         } else {
2092                 /* Incoming channel.
2093                  * Wake up socket sleeping on accept.
2094                  */
2095                 parent->sk_data_ready(parent, 0);
2096         }
2097 }
2098
2099 /* Copy frame to all raw sockets on that connection */
2100 static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
2101 {
2102         struct l2cap_chan_list *l = &conn->chan_list;
2103         struct sk_buff *nskb;
2104         struct sock *sk;
2105
2106         BT_DBG("conn %p", conn);
2107
2108         read_lock(&l->lock);
2109         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
2110                 if (sk->sk_type != SOCK_RAW)
2111                         continue;
2112
2113                 /* Don't send frame to the socket it came from */
2114                 if (skb->sk == sk)
2115                         continue;
2116                 nskb = skb_clone(skb, GFP_ATOMIC);
2117                 if (!nskb)
2118                         continue;
2119
2120                 if (sock_queue_rcv_skb(sk, nskb))
2121                         kfree_skb(nskb);
2122         }
2123         read_unlock(&l->lock);
2124 }
2125
2126 /* ---- L2CAP signalling commands ---- */
2127 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
2128                                 u8 code, u8 ident, u16 dlen, void *data)
2129 {
2130         struct sk_buff *skb, **frag;
2131         struct l2cap_cmd_hdr *cmd;
2132         struct l2cap_hdr *lh;
2133         int len, count;
2134
2135         BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %d",
2136                         conn, code, ident, dlen);
2137
2138         len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen;
2139         count = min_t(unsigned int, conn->mtu, len);
2140
2141         skb = bt_skb_alloc(count, GFP_ATOMIC);
2142         if (!skb)
2143                 return NULL;
2144
2145         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
2146         lh->len = cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen);
2147         lh->cid = cpu_to_le16(L2CAP_CID_SIGNALING);
2148
2149         cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE);
2150         cmd->code  = code;
2151         cmd->ident = ident;
2152         cmd->len   = cpu_to_le16(dlen);
2153
2154         if (dlen) {
2155                 count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE;
2156                 memcpy(skb_put(skb, count), data, count);
2157                 data += count;
2158         }
2159
2160         len -= skb->len;
2161
2162         /* Continuation fragments (no L2CAP header) */
2163         frag = &skb_shinfo(skb)->frag_list;
2164         while (len) {
2165                 count = min_t(unsigned int, conn->mtu, len);
2166
2167                 *frag = bt_skb_alloc(count, GFP_ATOMIC);
2168                 if (!*frag)
2169                         goto fail;
2170
2171                 memcpy(skb_put(*frag, count), data, count);
2172
2173                 len  -= count;
2174                 data += count;
2175
2176                 frag = &(*frag)->next;
2177         }
2178
2179         return skb;
2180
2181 fail:
2182         kfree_skb(skb);
2183         return NULL;
2184 }
2185
2186 static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val)
2187 {
2188         struct l2cap_conf_opt *opt = *ptr;
2189         int len;
2190
2191         len = L2CAP_CONF_OPT_SIZE + opt->len;
2192         *ptr += len;
2193
2194         *type = opt->type;
2195         *olen = opt->len;
2196
2197         switch (opt->len) {
2198         case 1:
2199                 *val = *((u8 *) opt->val);
2200                 break;
2201
2202         case 2:
2203                 *val = __le16_to_cpu(*((__le16 *) opt->val));
2204                 break;
2205
2206         case 4:
2207                 *val = __le32_to_cpu(*((__le32 *) opt->val));
2208                 break;
2209
2210         default:
2211                 *val = (unsigned long) opt->val;
2212                 break;
2213         }
2214
2215         BT_DBG("type 0x%2.2x len %d val 0x%lx", *type, opt->len, *val);
2216         return len;
2217 }
2218
2219 static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val)
2220 {
2221         struct l2cap_conf_opt *opt = *ptr;
2222
2223         BT_DBG("type 0x%2.2x len %d val 0x%lx", type, len, val);
2224
2225         opt->type = type;
2226         opt->len  = len;
2227
2228         switch (len) {
2229         case 1:
2230                 *((u8 *) opt->val)  = val;
2231                 break;
2232
2233         case 2:
2234                 *((__le16 *) opt->val) = cpu_to_le16(val);
2235                 break;
2236
2237         case 4:
2238                 *((__le32 *) opt->val) = cpu_to_le32(val);
2239                 break;
2240
2241         default:
2242                 memcpy(opt->val, (void *) val, len);
2243                 break;
2244         }
2245
2246         *ptr += L2CAP_CONF_OPT_SIZE + len;
2247 }
2248
2249 static void l2cap_ack_timeout(unsigned long arg)
2250 {
2251         struct sock *sk = (void *) arg;
2252
2253         bh_lock_sock(sk);
2254         l2cap_send_ack(l2cap_pi(sk));
2255         bh_unlock_sock(sk);
2256 }
2257
2258 static inline void l2cap_ertm_init(struct sock *sk)
2259 {
2260         l2cap_pi(sk)->expected_ack_seq = 0;
2261         l2cap_pi(sk)->unacked_frames = 0;
2262         l2cap_pi(sk)->buffer_seq = 0;
2263         l2cap_pi(sk)->num_acked = 0;
2264         l2cap_pi(sk)->frames_sent = 0;
2265
2266         setup_timer(&l2cap_pi(sk)->retrans_timer,
2267                         l2cap_retrans_timeout, (unsigned long) sk);
2268         setup_timer(&l2cap_pi(sk)->monitor_timer,
2269                         l2cap_monitor_timeout, (unsigned long) sk);
2270         setup_timer(&l2cap_pi(sk)->ack_timer,
2271                         l2cap_ack_timeout, (unsigned long) sk);
2272
2273         __skb_queue_head_init(SREJ_QUEUE(sk));
2274 }
2275
2276 static int l2cap_mode_supported(__u8 mode, __u32 feat_mask)
2277 {
2278         u32 local_feat_mask = l2cap_feat_mask;
2279         if (enable_ertm)
2280                 local_feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING;
2281
2282         switch (mode) {
2283         case L2CAP_MODE_ERTM:
2284                 return L2CAP_FEAT_ERTM & feat_mask & local_feat_mask;
2285         case L2CAP_MODE_STREAMING:
2286                 return L2CAP_FEAT_STREAMING & feat_mask & local_feat_mask;
2287         default:
2288                 return 0x00;
2289         }
2290 }
2291
2292 static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
2293 {
2294         switch (mode) {
2295         case L2CAP_MODE_STREAMING:
2296         case L2CAP_MODE_ERTM:
2297                 if (l2cap_mode_supported(mode, remote_feat_mask))
2298                         return mode;
2299                 /* fall through */
2300         default:
2301                 return L2CAP_MODE_BASIC;
2302         }
2303 }
2304
2305 static int l2cap_build_conf_req(struct sock *sk, void *data)
2306 {
2307         struct l2cap_pinfo *pi = l2cap_pi(sk);
2308         struct l2cap_conf_req *req = data;
2309         struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
2310         void *ptr = req->data;
2311
2312         BT_DBG("sk %p", sk);
2313
2314         if (pi->num_conf_req || pi->num_conf_rsp)
2315                 goto done;
2316
2317         switch (pi->mode) {
2318         case L2CAP_MODE_STREAMING:
2319         case L2CAP_MODE_ERTM:
2320                 pi->conf_state |= L2CAP_CONF_STATE2_DEVICE;
2321                 if (!l2cap_mode_supported(pi->mode, pi->conn->feat_mask))
2322                         l2cap_send_disconn_req(pi->conn, sk);
2323                 break;
2324         default:
2325                 pi->mode = l2cap_select_mode(rfc.mode, pi->conn->feat_mask);
2326                 break;
2327         }
2328
2329 done:
2330         switch (pi->mode) {
2331         case L2CAP_MODE_BASIC:
2332                 if (pi->imtu != L2CAP_DEFAULT_MTU)
2333                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->imtu);
2334                 break;
2335
2336         case L2CAP_MODE_ERTM:
2337                 rfc.mode            = L2CAP_MODE_ERTM;
2338                 rfc.txwin_size      = pi->tx_win;
2339                 rfc.max_transmit    = pi->max_tx;
2340                 rfc.retrans_timeout = 0;
2341                 rfc.monitor_timeout = 0;
2342                 rfc.max_pdu_size    = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE);
2343                 if (L2CAP_DEFAULT_MAX_PDU_SIZE > pi->conn->mtu - 10)
2344                         rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10);
2345
2346                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2347                                         sizeof(rfc), (unsigned long) &rfc);
2348
2349                 if (!(pi->conn->feat_mask & L2CAP_FEAT_FCS))
2350                         break;
2351
2352                 if (pi->fcs == L2CAP_FCS_NONE ||
2353                                 pi->conf_state & L2CAP_CONF_NO_FCS_RECV) {
2354                         pi->fcs = L2CAP_FCS_NONE;
2355                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, pi->fcs);
2356                 }
2357                 break;
2358
2359         case L2CAP_MODE_STREAMING:
2360                 rfc.mode            = L2CAP_MODE_STREAMING;
2361                 rfc.txwin_size      = 0;
2362                 rfc.max_transmit    = 0;
2363                 rfc.retrans_timeout = 0;
2364                 rfc.monitor_timeout = 0;
2365                 rfc.max_pdu_size    = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE);
2366                 if (L2CAP_DEFAULT_MAX_PDU_SIZE > pi->conn->mtu - 10)
2367                         rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10);
2368
2369                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2370                                         sizeof(rfc), (unsigned long) &rfc);
2371
2372                 if (!(pi->conn->feat_mask & L2CAP_FEAT_FCS))
2373                         break;
2374
2375                 if (pi->fcs == L2CAP_FCS_NONE ||
2376                                 pi->conf_state & L2CAP_CONF_NO_FCS_RECV) {
2377                         pi->fcs = L2CAP_FCS_NONE;
2378                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, pi->fcs);
2379                 }
2380                 break;
2381         }
2382
2383         /* FIXME: Need actual value of the flush timeout */
2384         //if (flush_to != L2CAP_DEFAULT_FLUSH_TO)
2385         //   l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO, 2, pi->flush_to);
2386
2387         req->dcid  = cpu_to_le16(pi->dcid);
2388         req->flags = cpu_to_le16(0);
2389
2390         return ptr - data;
2391 }
2392
2393 static int l2cap_parse_conf_req(struct sock *sk, void *data)
2394 {
2395         struct l2cap_pinfo *pi = l2cap_pi(sk);
2396         struct l2cap_conf_rsp *rsp = data;
2397         void *ptr = rsp->data;
2398         void *req = pi->conf_req;
2399         int len = pi->conf_len;
2400         int type, hint, olen;
2401         unsigned long val;
2402         struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
2403         u16 mtu = L2CAP_DEFAULT_MTU;
2404         u16 result = L2CAP_CONF_SUCCESS;
2405
2406         BT_DBG("sk %p", sk);
2407
2408         while (len >= L2CAP_CONF_OPT_SIZE) {
2409                 len -= l2cap_get_conf_opt(&req, &type, &olen, &val);
2410
2411                 hint  = type & L2CAP_CONF_HINT;
2412                 type &= L2CAP_CONF_MASK;
2413
2414                 switch (type) {
2415                 case L2CAP_CONF_MTU:
2416                         mtu = val;
2417                         break;
2418
2419                 case L2CAP_CONF_FLUSH_TO:
2420                         pi->flush_to = val;
2421                         break;
2422
2423                 case L2CAP_CONF_QOS:
2424                         break;
2425
2426                 case L2CAP_CONF_RFC:
2427                         if (olen == sizeof(rfc))
2428                                 memcpy(&rfc, (void *) val, olen);
2429                         break;
2430
2431                 case L2CAP_CONF_FCS:
2432                         if (val == L2CAP_FCS_NONE)
2433                                 pi->conf_state |= L2CAP_CONF_NO_FCS_RECV;
2434
2435                         break;
2436
2437                 default:
2438                         if (hint)
2439                                 break;
2440
2441                         result = L2CAP_CONF_UNKNOWN;
2442                         *((u8 *) ptr++) = type;
2443                         break;
2444                 }
2445         }
2446
2447         if (pi->num_conf_rsp || pi->num_conf_req)
2448                 goto done;
2449
2450         switch (pi->mode) {
2451         case L2CAP_MODE_STREAMING:
2452         case L2CAP_MODE_ERTM:
2453                 pi->conf_state |= L2CAP_CONF_STATE2_DEVICE;
2454                 if (!l2cap_mode_supported(pi->mode, pi->conn->feat_mask))
2455                         return -ECONNREFUSED;
2456                 break;
2457         default:
2458                 pi->mode = l2cap_select_mode(rfc.mode, pi->conn->feat_mask);
2459                 break;
2460         }
2461
2462 done:
2463         if (pi->mode != rfc.mode) {
2464                 result = L2CAP_CONF_UNACCEPT;
2465                 rfc.mode = pi->mode;
2466
2467                 if (pi->num_conf_rsp == 1)
2468                         return -ECONNREFUSED;
2469
2470                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2471                                         sizeof(rfc), (unsigned long) &rfc);
2472         }
2473
2474
2475         if (result == L2CAP_CONF_SUCCESS) {
2476                 /* Configure output options and let the other side know
2477                  * which ones we don't like. */
2478
2479                 if (mtu < L2CAP_DEFAULT_MIN_MTU)
2480                         result = L2CAP_CONF_UNACCEPT;
2481                 else {
2482                         pi->omtu = mtu;
2483                         pi->conf_state |= L2CAP_CONF_MTU_DONE;
2484                 }
2485                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->omtu);
2486
2487                 switch (rfc.mode) {
2488                 case L2CAP_MODE_BASIC:
2489                         pi->fcs = L2CAP_FCS_NONE;
2490                         pi->conf_state |= L2CAP_CONF_MODE_DONE;
2491                         break;
2492
2493                 case L2CAP_MODE_ERTM:
2494                         pi->remote_tx_win = rfc.txwin_size;
2495                         pi->remote_max_tx = rfc.max_transmit;
2496                         if (rfc.max_pdu_size > pi->conn->mtu - 10)
2497                                 rfc.max_pdu_size = le16_to_cpu(pi->conn->mtu - 10);
2498
2499                         pi->remote_mps = le16_to_cpu(rfc.max_pdu_size);
2500
2501                         rfc.retrans_timeout =
2502                                 le16_to_cpu(L2CAP_DEFAULT_RETRANS_TO);
2503                         rfc.monitor_timeout =
2504                                 le16_to_cpu(L2CAP_DEFAULT_MONITOR_TO);
2505
2506                         pi->conf_state |= L2CAP_CONF_MODE_DONE;
2507
2508                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2509                                         sizeof(rfc), (unsigned long) &rfc);
2510
2511                         break;
2512
2513                 case L2CAP_MODE_STREAMING:
2514                         if (rfc.max_pdu_size > pi->conn->mtu - 10)
2515                                 rfc.max_pdu_size = le16_to_cpu(pi->conn->mtu - 10);
2516
2517                         pi->remote_mps = le16_to_cpu(rfc.max_pdu_size);
2518
2519                         pi->conf_state |= L2CAP_CONF_MODE_DONE;
2520
2521                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2522                                         sizeof(rfc), (unsigned long) &rfc);
2523
2524                         break;
2525
2526                 default:
2527                         result = L2CAP_CONF_UNACCEPT;
2528
2529                         memset(&rfc, 0, sizeof(rfc));
2530                         rfc.mode = pi->mode;
2531                 }
2532
2533                 if (result == L2CAP_CONF_SUCCESS)
2534                         pi->conf_state |= L2CAP_CONF_OUTPUT_DONE;
2535         }
2536         rsp->scid   = cpu_to_le16(pi->dcid);
2537         rsp->result = cpu_to_le16(result);
2538         rsp->flags  = cpu_to_le16(0x0000);
2539
2540         return ptr - data;
2541 }
2542
2543 static int l2cap_parse_conf_rsp(struct sock *sk, void *rsp, int len, void *data, u16 *result)
2544 {
2545         struct l2cap_pinfo *pi = l2cap_pi(sk);
2546         struct l2cap_conf_req *req = data;
2547         void *ptr = req->data;
2548         int type, olen;
2549         unsigned long val;
2550         struct l2cap_conf_rfc rfc;
2551
2552         BT_DBG("sk %p, rsp %p, len %d, req %p", sk, rsp, len, data);
2553
2554         while (len >= L2CAP_CONF_OPT_SIZE) {
2555                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
2556
2557                 switch (type) {
2558                 case L2CAP_CONF_MTU:
2559                         if (val < L2CAP_DEFAULT_MIN_MTU) {
2560                                 *result = L2CAP_CONF_UNACCEPT;
2561                                 pi->omtu = L2CAP_DEFAULT_MIN_MTU;
2562                         } else
2563                                 pi->omtu = val;
2564                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->omtu);
2565                         break;
2566
2567                 case L2CAP_CONF_FLUSH_TO:
2568                         pi->flush_to = val;
2569                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
2570                                                         2, pi->flush_to);
2571                         break;
2572
2573                 case L2CAP_CONF_RFC:
2574                         if (olen == sizeof(rfc))
2575                                 memcpy(&rfc, (void *)val, olen);
2576
2577                         if ((pi->conf_state & L2CAP_CONF_STATE2_DEVICE) &&
2578                                                         rfc.mode != pi->mode)
2579                                 return -ECONNREFUSED;
2580
2581                         pi->mode = rfc.mode;
2582                         pi->fcs = 0;
2583
2584                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2585                                         sizeof(rfc), (unsigned long) &rfc);
2586                         break;
2587                 }
2588         }
2589
2590         if (*result == L2CAP_CONF_SUCCESS) {
2591                 switch (rfc.mode) {
2592                 case L2CAP_MODE_ERTM:
2593                         pi->remote_tx_win   = rfc.txwin_size;
2594                         pi->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
2595                         pi->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
2596                         pi->mps    = le16_to_cpu(rfc.max_pdu_size);
2597                         break;
2598                 case L2CAP_MODE_STREAMING:
2599                         pi->mps    = le16_to_cpu(rfc.max_pdu_size);
2600                 }
2601         }
2602
2603         req->dcid   = cpu_to_le16(pi->dcid);
2604         req->flags  = cpu_to_le16(0x0000);
2605
2606         return ptr - data;
2607 }
2608
2609 static int l2cap_build_conf_rsp(struct sock *sk, void *data, u16 result, u16 flags)
2610 {
2611         struct l2cap_conf_rsp *rsp = data;
2612         void *ptr = rsp->data;
2613
2614         BT_DBG("sk %p", sk);
2615
2616         rsp->scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
2617         rsp->result = cpu_to_le16(result);
2618         rsp->flags  = cpu_to_le16(flags);
2619
2620         return ptr - data;
2621 }
2622
2623 static void l2cap_conf_rfc_get(struct sock *sk, void *rsp, int len)
2624 {
2625         struct l2cap_pinfo *pi = l2cap_pi(sk);
2626         int type, olen;
2627         unsigned long val;
2628         struct l2cap_conf_rfc rfc;
2629
2630         BT_DBG("sk %p, rsp %p, len %d", sk, rsp, len);
2631
2632         if ((pi->mode != L2CAP_MODE_ERTM) && (pi->mode != L2CAP_MODE_STREAMING))
2633                 return;
2634
2635         while (len >= L2CAP_CONF_OPT_SIZE) {
2636                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
2637
2638                 switch (type) {
2639                 case L2CAP_CONF_RFC:
2640                         if (olen == sizeof(rfc))
2641                                 memcpy(&rfc, (void *)val, olen);
2642                         goto done;
2643                 }
2644         }
2645
2646 done:
2647         switch (rfc.mode) {
2648         case L2CAP_MODE_ERTM:
2649                 pi->remote_tx_win   = rfc.txwin_size;
2650                 pi->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
2651                 pi->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
2652                 pi->mps    = le16_to_cpu(rfc.max_pdu_size);
2653                 break;
2654         case L2CAP_MODE_STREAMING:
2655                 pi->mps    = le16_to_cpu(rfc.max_pdu_size);
2656         }
2657 }
2658
2659 static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2660 {
2661         struct l2cap_cmd_rej *rej = (struct l2cap_cmd_rej *) data;
2662
2663         if (rej->reason != 0x0000)
2664                 return 0;
2665
2666         if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
2667                                         cmd->ident == conn->info_ident) {
2668                 del_timer(&conn->info_timer);
2669
2670                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2671                 conn->info_ident = 0;
2672
2673                 l2cap_conn_start(conn);
2674         }
2675
2676         return 0;
2677 }
2678
2679 static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2680 {
2681         struct l2cap_chan_list *list = &conn->chan_list;
2682         struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
2683         struct l2cap_conn_rsp rsp;
2684         struct sock *sk, *parent;
2685         int result, status = L2CAP_CS_NO_INFO;
2686
2687         u16 dcid = 0, scid = __le16_to_cpu(req->scid);
2688         __le16 psm = req->psm;
2689
2690         BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid);
2691
2692         /* Check if we have socket listening on psm */
2693         parent = l2cap_get_sock_by_psm(BT_LISTEN, psm, conn->src);
2694         if (!parent) {
2695                 result = L2CAP_CR_BAD_PSM;
2696                 goto sendresp;
2697         }
2698
2699         /* Check if the ACL is secure enough (if not SDP) */
2700         if (psm != cpu_to_le16(0x0001) &&
2701                                 !hci_conn_check_link_mode(conn->hcon)) {
2702                 conn->disc_reason = 0x05;
2703                 result = L2CAP_CR_SEC_BLOCK;
2704                 goto response;
2705         }
2706
2707         result = L2CAP_CR_NO_MEM;
2708
2709         /* Check for backlog size */
2710         if (sk_acceptq_is_full(parent)) {
2711                 BT_DBG("backlog full %d", parent->sk_ack_backlog);
2712                 goto response;
2713         }
2714
2715         sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, GFP_ATOMIC);
2716         if (!sk)
2717                 goto response;
2718
2719         write_lock_bh(&list->lock);
2720
2721         /* Check if we already have channel with that dcid */
2722         if (__l2cap_get_chan_by_dcid(list, scid)) {
2723                 write_unlock_bh(&list->lock);
2724                 sock_set_flag(sk, SOCK_ZAPPED);
2725                 l2cap_sock_kill(sk);
2726                 goto response;
2727         }
2728
2729         hci_conn_hold(conn->hcon);
2730
2731         l2cap_sock_init(sk, parent);
2732         bacpy(&bt_sk(sk)->src, conn->src);
2733         bacpy(&bt_sk(sk)->dst, conn->dst);
2734         l2cap_pi(sk)->psm  = psm;
2735         l2cap_pi(sk)->dcid = scid;
2736
2737         __l2cap_chan_add(conn, sk, parent);
2738         dcid = l2cap_pi(sk)->scid;
2739
2740         l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
2741
2742         l2cap_pi(sk)->ident = cmd->ident;
2743
2744         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
2745                 if (l2cap_check_security(sk)) {
2746                         if (bt_sk(sk)->defer_setup) {
2747                                 sk->sk_state = BT_CONNECT2;
2748                                 result = L2CAP_CR_PEND;
2749                                 status = L2CAP_CS_AUTHOR_PEND;
2750                                 parent->sk_data_ready(parent, 0);
2751                         } else {
2752                                 sk->sk_state = BT_CONFIG;
2753                                 result = L2CAP_CR_SUCCESS;
2754                                 status = L2CAP_CS_NO_INFO;
2755                         }
2756                 } else {
2757                         sk->sk_state = BT_CONNECT2;
2758                         result = L2CAP_CR_PEND;
2759                         status = L2CAP_CS_AUTHEN_PEND;
2760                 }
2761         } else {
2762                 sk->sk_state = BT_CONNECT2;
2763                 result = L2CAP_CR_PEND;
2764                 status = L2CAP_CS_NO_INFO;
2765         }
2766
2767         write_unlock_bh(&list->lock);
2768
2769 response:
2770         bh_unlock_sock(parent);
2771
2772 sendresp:
2773         rsp.scid   = cpu_to_le16(scid);
2774         rsp.dcid   = cpu_to_le16(dcid);
2775         rsp.result = cpu_to_le16(result);
2776         rsp.status = cpu_to_le16(status);
2777         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
2778
2779         if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
2780                 struct l2cap_info_req info;
2781                 info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
2782
2783                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
2784                 conn->info_ident = l2cap_get_ident(conn);
2785
2786                 mod_timer(&conn->info_timer, jiffies +
2787                                         msecs_to_jiffies(L2CAP_INFO_TIMEOUT));
2788
2789                 l2cap_send_cmd(conn, conn->info_ident,
2790                                         L2CAP_INFO_REQ, sizeof(info), &info);
2791         }
2792
2793         return 0;
2794 }
2795
2796 static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2797 {
2798         struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
2799         u16 scid, dcid, result, status;
2800         struct sock *sk;
2801         u8 req[128];
2802
2803         scid   = __le16_to_cpu(rsp->scid);
2804         dcid   = __le16_to_cpu(rsp->dcid);
2805         result = __le16_to_cpu(rsp->result);
2806         status = __le16_to_cpu(rsp->status);
2807
2808         BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status);
2809
2810         if (scid) {
2811                 sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
2812                 if (!sk)
2813                         return 0;
2814         } else {
2815                 sk = l2cap_get_chan_by_ident(&conn->chan_list, cmd->ident);
2816                 if (!sk)
2817                         return 0;
2818         }
2819
2820         switch (result) {
2821         case L2CAP_CR_SUCCESS:
2822                 sk->sk_state = BT_CONFIG;
2823                 l2cap_pi(sk)->ident = 0;
2824                 l2cap_pi(sk)->dcid = dcid;
2825                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
2826
2827                 l2cap_pi(sk)->conf_state &= ~L2CAP_CONF_CONNECT_PEND;
2828
2829                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2830                                         l2cap_build_conf_req(sk, req), req);
2831                 l2cap_pi(sk)->num_conf_req++;
2832                 break;
2833
2834         case L2CAP_CR_PEND:
2835                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND;
2836                 break;
2837
2838         default:
2839                 l2cap_chan_del(sk, ECONNREFUSED);
2840                 break;
2841         }
2842
2843         bh_unlock_sock(sk);
2844         return 0;
2845 }
2846
2847 static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
2848 {
2849         struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
2850         u16 dcid, flags;
2851         u8 rsp[64];
2852         struct sock *sk;
2853         int len;
2854
2855         dcid  = __le16_to_cpu(req->dcid);
2856         flags = __le16_to_cpu(req->flags);
2857
2858         BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
2859
2860         sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid);
2861         if (!sk)
2862                 return -ENOENT;
2863
2864         if (sk->sk_state == BT_DISCONN)
2865                 goto unlock;
2866
2867         /* Reject if config buffer is too small. */
2868         len = cmd_len - sizeof(*req);
2869         if (l2cap_pi(sk)->conf_len + len > sizeof(l2cap_pi(sk)->conf_req)) {
2870                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2871                                 l2cap_build_conf_rsp(sk, rsp,
2872                                         L2CAP_CONF_REJECT, flags), rsp);
2873                 goto unlock;
2874         }
2875
2876         /* Store config. */
2877         memcpy(l2cap_pi(sk)->conf_req + l2cap_pi(sk)->conf_len, req->data, len);
2878         l2cap_pi(sk)->conf_len += len;
2879
2880         if (flags & 0x0001) {
2881                 /* Incomplete config. Send empty response. */
2882                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2883                                 l2cap_build_conf_rsp(sk, rsp,
2884                                         L2CAP_CONF_SUCCESS, 0x0001), rsp);
2885                 goto unlock;
2886         }
2887
2888         /* Complete config. */
2889         len = l2cap_parse_conf_req(sk, rsp);
2890         if (len < 0) {
2891                 l2cap_send_disconn_req(conn, sk);
2892                 goto unlock;
2893         }
2894
2895         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
2896         l2cap_pi(sk)->num_conf_rsp++;
2897
2898         /* Reset config buffer. */
2899         l2cap_pi(sk)->conf_len = 0;
2900
2901         if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE))
2902                 goto unlock;
2903
2904         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_INPUT_DONE) {
2905                 if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_NO_FCS_RECV) ||
2906                     l2cap_pi(sk)->fcs != L2CAP_FCS_NONE)
2907                         l2cap_pi(sk)->fcs = L2CAP_FCS_CRC16;
2908
2909                 sk->sk_state = BT_CONNECTED;
2910
2911                 l2cap_pi(sk)->next_tx_seq = 0;
2912                 l2cap_pi(sk)->expected_tx_seq = 0;
2913                 __skb_queue_head_init(TX_QUEUE(sk));
2914                 if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM)
2915                         l2cap_ertm_init(sk);
2916
2917                 l2cap_chan_ready(sk);
2918                 goto unlock;
2919         }
2920
2921         if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT)) {
2922                 u8 buf[64];
2923                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2924                                         l2cap_build_conf_req(sk, buf), buf);
2925                 l2cap_pi(sk)->num_conf_req++;
2926         }
2927
2928 unlock:
2929         bh_unlock_sock(sk);
2930         return 0;
2931 }
2932
2933 static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2934 {
2935         struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
2936         u16 scid, flags, result;
2937         struct sock *sk;
2938         int len = cmd->len - sizeof(*rsp);
2939
2940         scid   = __le16_to_cpu(rsp->scid);
2941         flags  = __le16_to_cpu(rsp->flags);
2942         result = __le16_to_cpu(rsp->result);
2943
2944         BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x",
2945                         scid, flags, result);
2946
2947         sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
2948         if (!sk)
2949                 return 0;
2950
2951         switch (result) {
2952         case L2CAP_CONF_SUCCESS:
2953                 l2cap_conf_rfc_get(sk, rsp->data, len);
2954                 break;
2955
2956         case L2CAP_CONF_UNACCEPT:
2957                 if (l2cap_pi(sk)->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
2958                         char req[64];
2959
2960                         if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
2961                                 l2cap_send_disconn_req(conn, sk);
2962                                 goto done;
2963                         }
2964
2965                         /* throw out any old stored conf requests */
2966                         result = L2CAP_CONF_SUCCESS;
2967                         len = l2cap_parse_conf_rsp(sk, rsp->data,
2968                                                         len, req, &result);
2969                         if (len < 0) {
2970                                 l2cap_send_disconn_req(conn, sk);
2971                                 goto done;
2972                         }
2973
2974                         l2cap_send_cmd(conn, l2cap_get_ident(conn),
2975                                                 L2CAP_CONF_REQ, len, req);
2976                         l2cap_pi(sk)->num_conf_req++;
2977                         if (result != L2CAP_CONF_SUCCESS)
2978                                 goto done;
2979                         break;
2980                 }
2981
2982         default:
2983                 sk->sk_state = BT_DISCONN;
2984                 sk->sk_err = ECONNRESET;
2985                 l2cap_sock_set_timer(sk, HZ * 5);
2986                 l2cap_send_disconn_req(conn, sk);
2987                 goto done;
2988         }
2989
2990         if (flags & 0x01)
2991                 goto done;
2992
2993         l2cap_pi(sk)->conf_state |= L2CAP_CONF_INPUT_DONE;
2994
2995         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE) {
2996                 if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_NO_FCS_RECV) ||
2997                     l2cap_pi(sk)->fcs != L2CAP_FCS_NONE)
2998                         l2cap_pi(sk)->fcs = L2CAP_FCS_CRC16;
2999
3000                 sk->sk_state = BT_CONNECTED;
3001                 l2cap_pi(sk)->next_tx_seq = 0;
3002                 l2cap_pi(sk)->expected_tx_seq = 0;
3003                 __skb_queue_head_init(TX_QUEUE(sk));
3004                 if (l2cap_pi(sk)->mode ==  L2CAP_MODE_ERTM)
3005                         l2cap_ertm_init(sk);
3006
3007                 l2cap_chan_ready(sk);
3008         }
3009
3010 done:
3011         bh_unlock_sock(sk);
3012         return 0;
3013 }
3014
3015 static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3016 {
3017         struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
3018         struct l2cap_disconn_rsp rsp;
3019         u16 dcid, scid;
3020         struct sock *sk;
3021
3022         scid = __le16_to_cpu(req->scid);
3023         dcid = __le16_to_cpu(req->dcid);
3024
3025         BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
3026
3027         sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid);
3028         if (!sk)
3029                 return 0;
3030
3031         rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid);
3032         rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid);
3033         l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
3034
3035         sk->sk_shutdown = SHUTDOWN_MASK;
3036
3037         skb_queue_purge(TX_QUEUE(sk));
3038
3039         if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM) {
3040                 skb_queue_purge(SREJ_QUEUE(sk));
3041                 del_timer(&l2cap_pi(sk)->retrans_timer);
3042                 del_timer(&l2cap_pi(sk)->monitor_timer);
3043                 del_timer(&l2cap_pi(sk)->ack_timer);
3044         }
3045
3046         l2cap_chan_del(sk, ECONNRESET);
3047         bh_unlock_sock(sk);
3048
3049         l2cap_sock_kill(sk);
3050         return 0;
3051 }
3052
3053 static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3054 {
3055         struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
3056         u16 dcid, scid;
3057         struct sock *sk;
3058
3059         scid = __le16_to_cpu(rsp->scid);
3060         dcid = __le16_to_cpu(rsp->dcid);
3061
3062         BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
3063
3064         sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
3065         if (!sk)
3066                 return 0;
3067
3068         skb_queue_purge(TX_QUEUE(sk));
3069
3070         if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM) {
3071                 skb_queue_purge(SREJ_QUEUE(sk));
3072                 del_timer(&l2cap_pi(sk)->retrans_timer);
3073                 del_timer(&l2cap_pi(sk)->monitor_timer);
3074                 del_timer(&l2cap_pi(sk)->ack_timer);
3075         }
3076
3077         l2cap_chan_del(sk, 0);
3078         bh_unlock_sock(sk);
3079
3080         l2cap_sock_kill(sk);
3081         return 0;
3082 }
3083
3084 static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3085 {
3086         struct l2cap_info_req *req = (struct l2cap_info_req *) data;
3087         u16 type;
3088
3089         type = __le16_to_cpu(req->type);
3090
3091         BT_DBG("type 0x%4.4x", type);
3092
3093         if (type == L2CAP_IT_FEAT_MASK) {
3094                 u8 buf[8];
3095                 u32 feat_mask = l2cap_feat_mask;
3096                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
3097                 rsp->type   = cpu_to_le16(L2CAP_IT_FEAT_MASK);
3098                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
3099                 if (enable_ertm)
3100                         feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
3101                                                          | L2CAP_FEAT_FCS;
3102                 put_unaligned_le32(feat_mask, rsp->data);
3103                 l2cap_send_cmd(conn, cmd->ident,
3104                                         L2CAP_INFO_RSP, sizeof(buf), buf);
3105         } else if (type == L2CAP_IT_FIXED_CHAN) {
3106                 u8 buf[12];
3107                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
3108                 rsp->type   = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
3109                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
3110                 memcpy(buf + 4, l2cap_fixed_chan, 8);
3111                 l2cap_send_cmd(conn, cmd->ident,
3112                                         L2CAP_INFO_RSP, sizeof(buf), buf);
3113         } else {
3114                 struct l2cap_info_rsp rsp;
3115                 rsp.type   = cpu_to_le16(type);
3116                 rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP);
3117                 l2cap_send_cmd(conn, cmd->ident,
3118                                         L2CAP_INFO_RSP, sizeof(rsp), &rsp);
3119         }
3120
3121         return 0;
3122 }
3123
3124 static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3125 {
3126         struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
3127         u16 type, result;
3128
3129         type   = __le16_to_cpu(rsp->type);
3130         result = __le16_to_cpu(rsp->result);
3131
3132         BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
3133
3134         del_timer(&conn->info_timer);
3135
3136         if (type == L2CAP_IT_FEAT_MASK) {
3137                 conn->feat_mask = get_unaligned_le32(rsp->data);
3138
3139                 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
3140                         struct l2cap_info_req req;
3141                         req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
3142
3143                         conn->info_ident = l2cap_get_ident(conn);
3144
3145                         l2cap_send_cmd(conn, conn->info_ident,
3146                                         L2CAP_INFO_REQ, sizeof(req), &req);
3147                 } else {
3148                         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3149                         conn->info_ident = 0;
3150
3151                         l2cap_conn_start(conn);
3152                 }
3153         } else if (type == L2CAP_IT_FIXED_CHAN) {
3154                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3155                 conn->info_ident = 0;
3156
3157                 l2cap_conn_start(conn);
3158         }
3159
3160         return 0;
3161 }
3162
3163 static inline void l2cap_sig_channel(struct l2cap_conn *conn, struct sk_buff *skb)
3164 {
3165         u8 *data = skb->data;
3166         int len = skb->len;
3167         struct l2cap_cmd_hdr cmd;
3168         int err = 0;
3169
3170         l2cap_raw_recv(conn, skb);
3171
3172         while (len >= L2CAP_CMD_HDR_SIZE) {
3173                 u16 cmd_len;
3174                 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
3175                 data += L2CAP_CMD_HDR_SIZE;
3176                 len  -= L2CAP_CMD_HDR_SIZE;
3177
3178                 cmd_len = le16_to_cpu(cmd.len);
3179
3180                 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, cmd.ident);
3181
3182                 if (cmd_len > len || !cmd.ident) {
3183                         BT_DBG("corrupted command");
3184                         break;
3185                 }
3186
3187                 switch (cmd.code) {
3188                 case L2CAP_COMMAND_REJ:
3189                         l2cap_command_rej(conn, &cmd, data);
3190                         break;
3191
3192                 case L2CAP_CONN_REQ:
3193                         err = l2cap_connect_req(conn, &cmd, data);
3194                         break;
3195
3196                 case L2CAP_CONN_RSP:
3197                         err = l2cap_connect_rsp(conn, &cmd, data);
3198                         break;
3199
3200                 case L2CAP_CONF_REQ:
3201                         err = l2cap_config_req(conn, &cmd, cmd_len, data);
3202                         break;
3203
3204                 case L2CAP_CONF_RSP:
3205                         err = l2cap_config_rsp(conn, &cmd, data);
3206                         break;
3207
3208                 case L2CAP_DISCONN_REQ:
3209                         err = l2cap_disconnect_req(conn, &cmd, data);
3210                         break;
3211
3212                 case L2CAP_DISCONN_RSP:
3213                         err = l2cap_disconnect_rsp(conn, &cmd, data);
3214                         break;
3215
3216                 case L2CAP_ECHO_REQ:
3217                         l2cap_send_cmd(conn, cmd.ident, L2CAP_ECHO_RSP, cmd_len, data);
3218                         break;
3219
3220                 case L2CAP_ECHO_RSP:
3221                         break;
3222
3223                 case L2CAP_INFO_REQ:
3224                         err = l2cap_information_req(conn, &cmd, data);
3225                         break;
3226
3227                 case L2CAP_INFO_RSP:
3228                         err = l2cap_information_rsp(conn, &cmd, data);
3229                         break;
3230
3231                 default:
3232                         BT_ERR("Unknown signaling command 0x%2.2x", cmd.code);
3233                         err = -EINVAL;
3234                         break;
3235                 }
3236
3237                 if (err) {
3238                         struct l2cap_cmd_rej rej;
3239                         BT_DBG("error %d", err);
3240
3241                         /* FIXME: Map err to a valid reason */
3242                         rej.reason = cpu_to_le16(0);
3243                         l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
3244                 }
3245
3246                 data += cmd_len;
3247                 len  -= cmd_len;
3248         }
3249
3250         kfree_skb(skb);
3251 }
3252
3253 static int l2cap_check_fcs(struct l2cap_pinfo *pi,  struct sk_buff *skb)
3254 {
3255         u16 our_fcs, rcv_fcs;
3256         int hdr_size = L2CAP_HDR_SIZE + 2;
3257
3258         if (pi->fcs == L2CAP_FCS_CRC16) {
3259                 skb_trim(skb, skb->len - 2);
3260                 rcv_fcs = get_unaligned_le16(skb->data + skb->len);
3261                 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
3262
3263                 if (our_fcs != rcv_fcs)
3264                         return -EINVAL;
3265         }
3266         return 0;
3267 }
3268
3269 static inline void l2cap_send_i_or_rr_or_rnr(struct sock *sk)
3270 {
3271         struct l2cap_pinfo *pi = l2cap_pi(sk);
3272         u16 control = 0;
3273
3274         pi->frames_sent = 0;
3275         pi->conn_state |= L2CAP_CONN_SEND_FBIT;
3276
3277         control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3278
3279         if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
3280                 control |= L2CAP_SUPER_RCV_NOT_READY | L2CAP_CTRL_FINAL;
3281                 l2cap_send_sframe(pi, control);
3282                 pi->conn_state &= ~L2CAP_CONN_SEND_FBIT;
3283         }
3284
3285         if (pi->conn_state & L2CAP_CONN_REMOTE_BUSY && pi->unacked_frames > 0)
3286                 __mod_retrans_timer();
3287
3288         l2cap_ertm_send(sk);
3289
3290         if (!(pi->conn_state & L2CAP_CONN_LOCAL_BUSY) &&
3291                         pi->frames_sent == 0) {
3292                 control |= L2CAP_SUPER_RCV_READY;
3293                 l2cap_send_sframe(pi, control);
3294         }
3295 }
3296
3297 static void l2cap_add_to_srej_queue(struct sock *sk, struct sk_buff *skb, u8 tx_seq, u8 sar)
3298 {
3299         struct sk_buff *next_skb;
3300
3301         bt_cb(skb)->tx_seq = tx_seq;
3302         bt_cb(skb)->sar = sar;
3303
3304         next_skb = skb_peek(SREJ_QUEUE(sk));
3305         if (!next_skb) {
3306                 __skb_queue_tail(SREJ_QUEUE(sk), skb);
3307                 return;
3308         }
3309
3310         do {
3311                 if (bt_cb(next_skb)->tx_seq > tx_seq) {
3312                         __skb_queue_before(SREJ_QUEUE(sk), next_skb, skb);
3313                         return;
3314                 }
3315
3316                 if (skb_queue_is_last(SREJ_QUEUE(sk), next_skb))
3317                         break;
3318
3319         } while((next_skb = skb_queue_next(SREJ_QUEUE(sk), next_skb)));
3320
3321         __skb_queue_tail(SREJ_QUEUE(sk), skb);
3322 }
3323
3324 static int l2cap_sar_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)
3325 {
3326         struct l2cap_pinfo *pi = l2cap_pi(sk);
3327         struct sk_buff *_skb;
3328         int err = -EINVAL;
3329
3330         switch (control & L2CAP_CTRL_SAR) {
3331         case L2CAP_SDU_UNSEGMENTED:
3332                 if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
3333                         kfree_skb(pi->sdu);
3334                         break;
3335                 }
3336
3337                 err = sock_queue_rcv_skb(sk, skb);
3338                 if (!err)
3339                         return 0;
3340
3341                 break;
3342
3343         case L2CAP_SDU_START:
3344                 if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
3345                         kfree_skb(pi->sdu);
3346                         break;
3347                 }
3348
3349                 pi->sdu_len = get_unaligned_le16(skb->data);
3350                 skb_pull(skb, 2);
3351
3352                 if (pi->sdu_len > pi->imtu) {
3353                         err = -EMSGSIZE;
3354                         break;
3355                 }
3356
3357                 pi->sdu = bt_skb_alloc(pi->sdu_len, GFP_ATOMIC);
3358                 if (!pi->sdu) {
3359                         err = -ENOMEM;
3360                         break;
3361                 }
3362
3363                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3364
3365                 pi->conn_state |= L2CAP_CONN_SAR_SDU;
3366                 pi->partial_sdu_len = skb->len;
3367                 err = 0;
3368                 break;
3369
3370         case L2CAP_SDU_CONTINUE:
3371                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
3372                         break;
3373
3374                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3375
3376                 pi->partial_sdu_len += skb->len;
3377                 if (pi->partial_sdu_len > pi->sdu_len)
3378                         kfree_skb(pi->sdu);
3379                 else
3380                         err = 0;
3381
3382                 break;
3383
3384         case L2CAP_SDU_END:
3385                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
3386                         break;
3387
3388                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3389
3390                 pi->conn_state &= ~L2CAP_CONN_SAR_SDU;
3391                 pi->partial_sdu_len += skb->len;
3392
3393                 if (pi->partial_sdu_len > pi->imtu)
3394                         goto drop;
3395
3396                 if (pi->partial_sdu_len == pi->sdu_len) {
3397                         _skb = skb_clone(pi->sdu, GFP_ATOMIC);
3398                         err = sock_queue_rcv_skb(sk, _skb);
3399                         if (err < 0)
3400                                 kfree_skb(_skb);
3401                 }
3402                 err = 0;
3403
3404 drop:
3405                 kfree_skb(pi->sdu);
3406                 break;
3407         }
3408
3409         kfree_skb(skb);
3410         return err;
3411 }
3412
3413 static void l2cap_check_srej_gap(struct sock *sk, u8 tx_seq)
3414 {
3415         struct sk_buff *skb;
3416         u16 control = 0;
3417
3418         while((skb = skb_peek(SREJ_QUEUE(sk)))) {
3419                 if (bt_cb(skb)->tx_seq != tx_seq)
3420                         break;
3421
3422                 skb = skb_dequeue(SREJ_QUEUE(sk));
3423                 control |= bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
3424                 l2cap_sar_reassembly_sdu(sk, skb, control);
3425                 l2cap_pi(sk)->buffer_seq_srej =
3426                         (l2cap_pi(sk)->buffer_seq_srej + 1) % 64;
3427                 tx_seq++;
3428         }
3429 }
3430
3431 static void l2cap_resend_srejframe(struct sock *sk, u8 tx_seq)
3432 {
3433         struct l2cap_pinfo *pi = l2cap_pi(sk);
3434         struct srej_list *l, *tmp;
3435         u16 control;
3436
3437         list_for_each_entry_safe(l,tmp, SREJ_LIST(sk), list) {
3438                 if (l->tx_seq == tx_seq) {
3439                         list_del(&l->list);
3440                         kfree(l);
3441                         return;
3442                 }
3443                 control = L2CAP_SUPER_SELECT_REJECT;
3444                 control |= l->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3445                 l2cap_send_sframe(pi, control);
3446                 list_del(&l->list);
3447                 list_add_tail(&l->list, SREJ_LIST(sk));
3448         }
3449 }
3450
3451 static void l2cap_send_srejframe(struct sock *sk, u8 tx_seq)
3452 {
3453         struct l2cap_pinfo *pi = l2cap_pi(sk);
3454         struct srej_list *new;
3455         u16 control;
3456
3457         while (tx_seq != pi->expected_tx_seq) {
3458                 control = L2CAP_SUPER_SELECT_REJECT;
3459                 control |= pi->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3460                 l2cap_send_sframe(pi, control);
3461
3462                 new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC);
3463                 new->tx_seq = pi->expected_tx_seq++;
3464                 list_add_tail(&new->list, SREJ_LIST(sk));
3465         }
3466         pi->expected_tx_seq++;
3467 }
3468
3469 static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
3470 {
3471         struct l2cap_pinfo *pi = l2cap_pi(sk);
3472         u8 tx_seq = __get_txseq(rx_control);
3473         u8 req_seq = __get_reqseq(rx_control);
3474         u8 sar = rx_control >> L2CAP_CTRL_SAR_SHIFT;
3475         int num_to_ack = (pi->tx_win/6) + 1;
3476         int err = 0;
3477
3478         BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
3479
3480         if (L2CAP_CTRL_FINAL & rx_control) {
3481                 del_timer(&pi->monitor_timer);
3482                 if (pi->unacked_frames > 0)
3483                         __mod_retrans_timer();
3484                 pi->conn_state &= ~L2CAP_CONN_WAIT_F;
3485         }
3486
3487         pi->expected_ack_seq = req_seq;
3488         l2cap_drop_acked_frames(sk);
3489
3490         if (tx_seq == pi->expected_tx_seq)
3491                 goto expected;
3492
3493         if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
3494                 struct srej_list *first;
3495
3496                 first = list_first_entry(SREJ_LIST(sk),
3497                                 struct srej_list, list);
3498                 if (tx_seq == first->tx_seq) {
3499                         l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
3500                         l2cap_check_srej_gap(sk, tx_seq);
3501
3502                         list_del(&first->list);
3503                         kfree(first);
3504
3505                         if (list_empty(SREJ_LIST(sk))) {
3506                                 pi->buffer_seq = pi->buffer_seq_srej;
3507                                 pi->conn_state &= ~L2CAP_CONN_SREJ_SENT;
3508                                 l2cap_send_ack(pi);
3509                         }
3510                 } else {
3511                         struct srej_list *l;
3512                         l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
3513
3514                         list_for_each_entry(l, SREJ_LIST(sk), list) {
3515                                 if (l->tx_seq == tx_seq) {
3516                                         l2cap_resend_srejframe(sk, tx_seq);
3517                                         return 0;
3518                                 }
3519                         }
3520                         l2cap_send_srejframe(sk, tx_seq);
3521                 }
3522         } else {
3523                 pi->conn_state |= L2CAP_CONN_SREJ_SENT;
3524
3525                 INIT_LIST_HEAD(SREJ_LIST(sk));
3526                 pi->buffer_seq_srej = pi->buffer_seq;
3527
3528                 __skb_queue_head_init(SREJ_QUEUE(sk));
3529                 l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
3530
3531                 pi->conn_state |= L2CAP_CONN_SEND_PBIT;
3532
3533                 l2cap_send_srejframe(sk, tx_seq);
3534         }
3535         return 0;
3536
3537 expected:
3538         pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3539
3540         if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
3541                 l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
3542                 return 0;
3543         }
3544
3545         if (rx_control & L2CAP_CTRL_FINAL) {
3546                 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
3547                         pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
3548                 else {
3549                         if (!skb_queue_empty(TX_QUEUE(sk)))
3550                                 sk->sk_send_head = TX_QUEUE(sk)->next;
3551                         pi->next_tx_seq = pi->expected_ack_seq;
3552                         l2cap_ertm_send(sk);
3553                 }
3554         }
3555
3556         pi->buffer_seq = (pi->buffer_seq + 1) % 64;
3557
3558         err = l2cap_sar_reassembly_sdu(sk, skb, rx_control);
3559         if (err < 0)
3560                 return err;
3561
3562         __mod_ack_timer();
3563
3564         pi->num_acked = (pi->num_acked + 1) % num_to_ack;
3565         if (pi->num_acked == num_to_ack - 1)
3566                 l2cap_send_ack(pi);
3567
3568         return 0;
3569 }
3570
3571 static inline void l2cap_data_channel_rrframe(struct sock *sk, u16 rx_control)
3572 {
3573         struct l2cap_pinfo *pi = l2cap_pi(sk);
3574
3575         pi->expected_ack_seq = __get_reqseq(rx_control);
3576         l2cap_drop_acked_frames(sk);
3577
3578         if (rx_control & L2CAP_CTRL_POLL) {
3579                 if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
3580                         if ((pi->conn_state & L2CAP_CONN_REMOTE_BUSY) &&
3581                                         (pi->unacked_frames > 0))
3582                                 __mod_retrans_timer();
3583
3584                         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3585                         l2cap_send_srejtail(sk);
3586                 } else {
3587                         l2cap_send_i_or_rr_or_rnr(sk);
3588                         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3589                 }
3590
3591         } else if (rx_control & L2CAP_CTRL_FINAL) {
3592                 pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3593
3594                 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
3595                         pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
3596                 else {
3597                         if (!skb_queue_empty(TX_QUEUE(sk)))
3598                                 sk->sk_send_head = TX_QUEUE(sk)->next;
3599                         pi->next_tx_seq = pi->expected_ack_seq;
3600                         l2cap_ertm_send(sk);
3601                 }
3602
3603         } else {
3604                 if ((pi->conn_state & L2CAP_CONN_REMOTE_BUSY) &&
3605                                 (pi->unacked_frames > 0))
3606                         __mod_retrans_timer();
3607
3608                 pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3609                 if (pi->conn_state & L2CAP_CONN_SREJ_SENT)
3610                         l2cap_send_ack(pi);
3611                 else
3612                         l2cap_ertm_send(sk);
3613         }
3614 }
3615
3616 static inline void l2cap_data_channel_rejframe(struct sock *sk, u16 rx_control)
3617 {
3618         struct l2cap_pinfo *pi = l2cap_pi(sk);
3619         u8 tx_seq = __get_reqseq(rx_control);
3620
3621         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3622
3623         pi->expected_ack_seq = tx_seq;
3624         l2cap_drop_acked_frames(sk);
3625
3626         if (rx_control & L2CAP_CTRL_FINAL) {
3627                 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
3628                         pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
3629                 else {
3630                         if (!skb_queue_empty(TX_QUEUE(sk)))
3631                                 sk->sk_send_head = TX_QUEUE(sk)->next;
3632                         pi->next_tx_seq = pi->expected_ack_seq;
3633                         l2cap_ertm_send(sk);
3634                 }
3635         } else {
3636                 if (!skb_queue_empty(TX_QUEUE(sk)))
3637                         sk->sk_send_head = TX_QUEUE(sk)->next;
3638                 pi->next_tx_seq = pi->expected_ack_seq;
3639                 l2cap_ertm_send(sk);
3640
3641                 if (pi->conn_state & L2CAP_CONN_WAIT_F) {
3642                         pi->srej_save_reqseq = tx_seq;
3643                         pi->conn_state |= L2CAP_CONN_REJ_ACT;
3644                 }
3645         }
3646 }
3647 static inline void l2cap_data_channel_srejframe(struct sock *sk, u16 rx_control)
3648 {
3649         struct l2cap_pinfo *pi = l2cap_pi(sk);
3650         u8 tx_seq = __get_reqseq(rx_control);
3651
3652         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3653
3654         if (rx_control & L2CAP_CTRL_POLL) {
3655                 pi->expected_ack_seq = tx_seq;
3656                 l2cap_drop_acked_frames(sk);
3657                 l2cap_retransmit_frame(sk, tx_seq);
3658                 l2cap_ertm_send(sk);
3659                 if (pi->conn_state & L2CAP_CONN_WAIT_F) {
3660                         pi->srej_save_reqseq = tx_seq;
3661                         pi->conn_state |= L2CAP_CONN_SREJ_ACT;
3662                 }
3663         } else if (rx_control & L2CAP_CTRL_FINAL) {
3664                 if ((pi->conn_state & L2CAP_CONN_SREJ_ACT) &&
3665                                 pi->srej_save_reqseq == tx_seq)
3666                         pi->conn_state &= ~L2CAP_CONN_SREJ_ACT;
3667                 else
3668                         l2cap_retransmit_frame(sk, tx_seq);
3669         } else {
3670                 l2cap_retransmit_frame(sk, tx_seq);
3671                 if (pi->conn_state & L2CAP_CONN_WAIT_F) {
3672                         pi->srej_save_reqseq = tx_seq;
3673                         pi->conn_state |= L2CAP_CONN_SREJ_ACT;
3674                 }
3675         }
3676 }
3677
3678 static inline void l2cap_data_channel_rnrframe(struct sock *sk, u16 rx_control)
3679 {
3680         struct l2cap_pinfo *pi = l2cap_pi(sk);
3681         u8 tx_seq = __get_reqseq(rx_control);
3682
3683         pi->conn_state |= L2CAP_CONN_REMOTE_BUSY;
3684         pi->expected_ack_seq = tx_seq;
3685         l2cap_drop_acked_frames(sk);
3686
3687         if (!(pi->conn_state & L2CAP_CONN_SREJ_SENT)) {
3688                 del_timer(&pi->retrans_timer);
3689                 if (rx_control & L2CAP_CTRL_POLL) {
3690                         u16 control = L2CAP_CTRL_FINAL;
3691                         l2cap_send_rr_or_rnr(pi, control);
3692                 }
3693                 return;
3694         }
3695
3696         if (rx_control & L2CAP_CTRL_POLL)
3697                 l2cap_send_srejtail(sk);
3698         else
3699                 l2cap_send_sframe(pi, L2CAP_SUPER_RCV_READY);
3700 }
3701
3702 static inline int l2cap_data_channel_sframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
3703 {
3704         BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
3705
3706         if (L2CAP_CTRL_FINAL & rx_control) {
3707                 del_timer(&l2cap_pi(sk)->monitor_timer);
3708                 if (l2cap_pi(sk)->unacked_frames > 0)
3709                         __mod_retrans_timer();
3710                 l2cap_pi(sk)->conn_state &= ~L2CAP_CONN_WAIT_F;
3711         }
3712
3713         switch (rx_control & L2CAP_CTRL_SUPERVISE) {
3714         case L2CAP_SUPER_RCV_READY:
3715                 l2cap_data_channel_rrframe(sk, rx_control);
3716                 break;
3717
3718         case L2CAP_SUPER_REJECT:
3719                 l2cap_data_channel_rejframe(sk, rx_control);
3720                 break;
3721
3722         case L2CAP_SUPER_SELECT_REJECT:
3723                 l2cap_data_channel_srejframe(sk, rx_control);
3724                 break;
3725
3726         case L2CAP_SUPER_RCV_NOT_READY:
3727                 l2cap_data_channel_rnrframe(sk, rx_control);
3728                 break;
3729         }
3730
3731         kfree_skb(skb);
3732         return 0;
3733 }
3734
3735 static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
3736 {
3737         struct sock *sk;
3738         struct l2cap_pinfo *pi;
3739         u16 control, len;
3740         u8 tx_seq;
3741
3742         sk = l2cap_get_chan_by_scid(&conn->chan_list, cid);
3743         if (!sk) {
3744                 BT_DBG("unknown cid 0x%4.4x", cid);
3745                 goto drop;
3746         }
3747
3748         pi = l2cap_pi(sk);
3749
3750         BT_DBG("sk %p, len %d", sk, skb->len);
3751
3752         if (sk->sk_state != BT_CONNECTED)
3753                 goto drop;
3754
3755         switch (pi->mode) {
3756         case L2CAP_MODE_BASIC:
3757                 /* If socket recv buffers overflows we drop data here
3758                  * which is *bad* because L2CAP has to be reliable.
3759                  * But we don't have any other choice. L2CAP doesn't
3760                  * provide flow control mechanism. */
3761
3762                 if (pi->imtu < skb->len)
3763                         goto drop;
3764
3765                 if (!sock_queue_rcv_skb(sk, skb))
3766                         goto done;
3767                 break;
3768
3769         case L2CAP_MODE_ERTM:
3770                 control = get_unaligned_le16(skb->data);
3771                 skb_pull(skb, 2);
3772                 len = skb->len;
3773
3774                 if (__is_sar_start(control))
3775                         len -= 2;
3776
3777                 if (pi->fcs == L2CAP_FCS_CRC16)
3778                         len -= 2;
3779
3780                 /*
3781                  * We can just drop the corrupted I-frame here.
3782                  * Receiver will miss it and start proper recovery
3783                  * procedures and ask retransmission.
3784                  */
3785                 if (len > pi->mps)
3786                         goto drop;
3787
3788                 if (l2cap_check_fcs(pi, skb))
3789                         goto drop;
3790
3791                 if (__is_iframe(control)) {
3792                         if (len < 4)
3793                                 goto drop;
3794
3795                         l2cap_data_channel_iframe(sk, control, skb);
3796                 } else {
3797                         if (len != 0)
3798                                 goto drop;
3799
3800                         l2cap_data_channel_sframe(sk, control, skb);
3801                 }
3802
3803                 goto done;
3804
3805         case L2CAP_MODE_STREAMING:
3806                 control = get_unaligned_le16(skb->data);
3807                 skb_pull(skb, 2);
3808                 len = skb->len;
3809
3810                 if (__is_sar_start(control))
3811                         len -= 2;
3812
3813                 if (pi->fcs == L2CAP_FCS_CRC16)
3814                         len -= 2;
3815
3816                 if (len > pi->mps || len < 4 || __is_sframe(control))
3817                         goto drop;
3818
3819                 if (l2cap_check_fcs(pi, skb))
3820                         goto drop;
3821
3822                 tx_seq = __get_txseq(control);
3823
3824                 if (pi->expected_tx_seq == tx_seq)
3825                         pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3826                 else
3827                         pi->expected_tx_seq = (tx_seq + 1) % 64;
3828
3829                 l2cap_sar_reassembly_sdu(sk, skb, control);
3830
3831                 goto done;
3832
3833         default:
3834                 BT_DBG("sk %p: bad mode 0x%2.2x", sk, pi->mode);
3835                 break;
3836         }
3837
3838 drop:
3839         kfree_skb(skb);
3840
3841 done:
3842         if (sk)
3843                 bh_unlock_sock(sk);
3844
3845         return 0;
3846 }
3847
3848 static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb)
3849 {
3850         struct sock *sk;
3851
3852         sk = l2cap_get_sock_by_psm(0, psm, conn->src);
3853         if (!sk)
3854                 goto drop;
3855
3856         BT_DBG("sk %p, len %d", sk, skb->len);
3857
3858         if (sk->sk_state != BT_BOUND && sk->sk_state != BT_CONNECTED)
3859                 goto drop;
3860
3861         if (l2cap_pi(sk)->imtu < skb->len)
3862                 goto drop;
3863
3864         if (!sock_queue_rcv_skb(sk, skb))
3865                 goto done;
3866
3867 drop:
3868         kfree_skb(skb);
3869
3870 done:
3871         if (sk)
3872                 bh_unlock_sock(sk);
3873         return 0;
3874 }
3875
3876 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
3877 {
3878         struct l2cap_hdr *lh = (void *) skb->data;
3879         u16 cid, len;
3880         __le16 psm;
3881
3882         skb_pull(skb, L2CAP_HDR_SIZE);
3883         cid = __le16_to_cpu(lh->cid);
3884         len = __le16_to_cpu(lh->len);
3885
3886         if (len != skb->len) {
3887                 kfree_skb(skb);
3888                 return;
3889         }
3890
3891         BT_DBG("len %d, cid 0x%4.4x", len, cid);
3892
3893         switch (cid) {
3894         case L2CAP_CID_SIGNALING:
3895                 l2cap_sig_channel(conn, skb);
3896                 break;
3897
3898         case L2CAP_CID_CONN_LESS:
3899                 psm = get_unaligned_le16(skb->data);
3900                 skb_pull(skb, 2);
3901                 l2cap_conless_channel(conn, psm, skb);
3902                 break;
3903
3904         default:
3905                 l2cap_data_channel(conn, cid, skb);
3906                 break;
3907         }
3908 }
3909
3910 /* ---- L2CAP interface with lower layer (HCI) ---- */
3911
3912 static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
3913 {
3914         int exact = 0, lm1 = 0, lm2 = 0;
3915         register struct sock *sk;
3916         struct hlist_node *node;
3917
3918         if (type != ACL_LINK)
3919                 return 0;
3920
3921         BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
3922
3923         /* Find listening sockets and check their link_mode */
3924         read_lock(&l2cap_sk_list.lock);
3925         sk_for_each(sk, node, &l2cap_sk_list.head) {
3926                 if (sk->sk_state != BT_LISTEN)
3927                         continue;
3928
3929                 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
3930                         lm1 |= HCI_LM_ACCEPT;
3931                         if (l2cap_pi(sk)->role_switch)
3932                                 lm1 |= HCI_LM_MASTER;
3933                         exact++;
3934                 } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
3935                         lm2 |= HCI_LM_ACCEPT;
3936                         if (l2cap_pi(sk)->role_switch)
3937                                 lm2 |= HCI_LM_MASTER;
3938                 }
3939         }
3940         read_unlock(&l2cap_sk_list.lock);
3941
3942         return exact ? lm1 : lm2;
3943 }
3944
3945 static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
3946 {
3947         struct l2cap_conn *conn;
3948
3949         BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
3950
3951         if (hcon->type != ACL_LINK)
3952                 return 0;
3953
3954         if (!status) {
3955                 conn = l2cap_conn_add(hcon, status);
3956                 if (conn)
3957                         l2cap_conn_ready(conn);
3958         } else
3959                 l2cap_conn_del(hcon, bt_err(status));
3960
3961         return 0;
3962 }
3963
3964 static int l2cap_disconn_ind(struct hci_conn *hcon)
3965 {
3966         struct l2cap_conn *conn = hcon->l2cap_data;
3967
3968         BT_DBG("hcon %p", hcon);
3969
3970         if (hcon->type != ACL_LINK || !conn)
3971                 return 0x13;
3972
3973         return conn->disc_reason;
3974 }
3975
3976 static int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
3977 {
3978         BT_DBG("hcon %p reason %d", hcon, reason);
3979
3980         if (hcon->type != ACL_LINK)
3981                 return 0;
3982
3983         l2cap_conn_del(hcon, bt_err(reason));
3984
3985         return 0;
3986 }
3987
3988 static inline void l2cap_check_encryption(struct sock *sk, u8 encrypt)
3989 {
3990         if (sk->sk_type != SOCK_SEQPACKET)
3991                 return;
3992
3993         if (encrypt == 0x00) {
3994                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_MEDIUM) {
3995                         l2cap_sock_clear_timer(sk);
3996                         l2cap_sock_set_timer(sk, HZ * 5);
3997                 } else if (l2cap_pi(sk)->sec_level == BT_SECURITY_HIGH)
3998                         __l2cap_sock_close(sk, ECONNREFUSED);
3999         } else {
4000                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_MEDIUM)
4001                         l2cap_sock_clear_timer(sk);
4002         }
4003 }
4004
4005 static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
4006 {
4007         struct l2cap_chan_list *l;
4008         struct l2cap_conn *conn = hcon->l2cap_data;
4009         struct sock *sk;
4010
4011         if (!conn)
4012                 return 0;
4013
4014         l = &conn->chan_list;
4015
4016         BT_DBG("conn %p", conn);
4017
4018         read_lock(&l->lock);
4019
4020         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
4021                 bh_lock_sock(sk);
4022
4023                 if (l2cap_pi(sk)->conf_state & L2CAP_CONF_CONNECT_PEND) {
4024                         bh_unlock_sock(sk);
4025                         continue;
4026                 }
4027
4028                 if (!status && (sk->sk_state == BT_CONNECTED ||
4029                                                 sk->sk_state == BT_CONFIG)) {
4030                         l2cap_check_encryption(sk, encrypt);
4031                         bh_unlock_sock(sk);
4032                         continue;
4033                 }
4034
4035                 if (sk->sk_state == BT_CONNECT) {
4036                         if (!status) {
4037                                 struct l2cap_conn_req req;
4038                                 req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
4039                                 req.psm  = l2cap_pi(sk)->psm;
4040
4041                                 l2cap_pi(sk)->ident = l2cap_get_ident(conn);
4042
4043                                 l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
4044                                         L2CAP_CONN_REQ, sizeof(req), &req);
4045                         } else {
4046                                 l2cap_sock_clear_timer(sk);
4047                                 l2cap_sock_set_timer(sk, HZ / 10);
4048                         }
4049                 } else if (sk->sk_state == BT_CONNECT2) {
4050                         struct l2cap_conn_rsp rsp;
4051                         __u16 result;
4052
4053                         if (!status) {
4054                                 sk->sk_state = BT_CONFIG;
4055                                 result = L2CAP_CR_SUCCESS;
4056                         } else {
4057                                 sk->sk_state = BT_DISCONN;
4058                                 l2cap_sock_set_timer(sk, HZ / 10);
4059                                 result = L2CAP_CR_SEC_BLOCK;
4060                         }
4061
4062                         rsp.scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
4063                         rsp.dcid   = cpu_to_le16(l2cap_pi(sk)->scid);
4064                         rsp.result = cpu_to_le16(result);
4065                         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
4066                         l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
4067                                         L2CAP_CONN_RSP, sizeof(rsp), &rsp);
4068                 }
4069
4070                 bh_unlock_sock(sk);
4071         }
4072
4073         read_unlock(&l->lock);
4074
4075         return 0;
4076 }
4077
4078 static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
4079 {
4080         struct l2cap_conn *conn = hcon->l2cap_data;
4081
4082         if (!conn && !(conn = l2cap_conn_add(hcon, 0)))
4083                 goto drop;
4084
4085         BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
4086
4087         if (flags & ACL_START) {
4088                 struct l2cap_hdr *hdr;
4089                 int len;
4090
4091                 if (conn->rx_len) {
4092                         BT_ERR("Unexpected start frame (len %d)", skb->len);
4093                         kfree_skb(conn->rx_skb);
4094                         conn->rx_skb = NULL;
4095                         conn->rx_len = 0;
4096                         l2cap_conn_unreliable(conn, ECOMM);
4097                 }
4098
4099                 if (skb->len < 2) {
4100                         BT_ERR("Frame is too short (len %d)", skb->len);
4101                         l2cap_conn_unreliable(conn, ECOMM);
4102                         goto drop;
4103                 }
4104
4105                 hdr = (struct l2cap_hdr *) skb->data;
4106                 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
4107
4108                 if (len == skb->len) {
4109                         /* Complete frame received */
4110                         l2cap_recv_frame(conn, skb);
4111                         return 0;
4112                 }
4113
4114                 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
4115
4116                 if (skb->len > len) {
4117                         BT_ERR("Frame is too long (len %d, expected len %d)",
4118                                 skb->len, len);
4119                         l2cap_conn_unreliable(conn, ECOMM);
4120                         goto drop;
4121                 }
4122
4123                 /* Allocate skb for the complete frame (with header) */
4124                 conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC);
4125                 if (!conn->rx_skb)
4126                         goto drop;
4127
4128                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
4129                                                                 skb->len);
4130                 conn->rx_len = len - skb->len;
4131         } else {
4132                 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
4133
4134                 if (!conn->rx_len) {
4135                         BT_ERR("Unexpected continuation frame (len %d)", skb->len);
4136                         l2cap_conn_unreliable(conn, ECOMM);
4137                         goto drop;
4138                 }
4139
4140                 if (skb->len > conn->rx_len) {
4141                         BT_ERR("Fragment is too long (len %d, expected %d)",
4142                                         skb->len, conn->rx_len);
4143                         kfree_skb(conn->rx_skb);
4144                         conn->rx_skb = NULL;
4145                         conn->rx_len = 0;
4146                         l2cap_conn_unreliable(conn, ECOMM);
4147                         goto drop;
4148                 }
4149
4150                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
4151                                                                 skb->len);
4152                 conn->rx_len -= skb->len;
4153
4154                 if (!conn->rx_len) {
4155                         /* Complete frame received */
4156                         l2cap_recv_frame(conn, conn->rx_skb);
4157                         conn->rx_skb = NULL;
4158                 }
4159         }
4160
4161 drop:
4162         kfree_skb(skb);
4163         return 0;
4164 }
4165
4166 static int l2cap_debugfs_show(struct seq_file *f, void *p)
4167 {
4168         struct sock *sk;
4169         struct hlist_node *node;
4170
4171         read_lock_bh(&l2cap_sk_list.lock);
4172
4173         sk_for_each(sk, node, &l2cap_sk_list.head) {
4174                 struct l2cap_pinfo *pi = l2cap_pi(sk);
4175
4176                 seq_printf(f, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d\n",
4177                                         batostr(&bt_sk(sk)->src),
4178                                         batostr(&bt_sk(sk)->dst),
4179                                         sk->sk_state, __le16_to_cpu(pi->psm),
4180                                         pi->scid, pi->dcid,
4181                                         pi->imtu, pi->omtu, pi->sec_level);
4182         }
4183
4184         read_unlock_bh(&l2cap_sk_list.lock);
4185
4186         return 0;
4187 }
4188
4189 static int l2cap_debugfs_open(struct inode *inode, struct file *file)
4190 {
4191         return single_open(file, l2cap_debugfs_show, inode->i_private);
4192 }
4193
4194 static const struct file_operations l2cap_debugfs_fops = {
4195         .open           = l2cap_debugfs_open,
4196         .read           = seq_read,
4197         .llseek         = seq_lseek,
4198         .release        = single_release,
4199 };
4200
4201 static struct dentry *l2cap_debugfs;
4202
4203 static const struct proto_ops l2cap_sock_ops = {
4204         .family         = PF_BLUETOOTH,
4205         .owner          = THIS_MODULE,
4206         .release        = l2cap_sock_release,
4207         .bind           = l2cap_sock_bind,
4208         .connect        = l2cap_sock_connect,
4209         .listen         = l2cap_sock_listen,
4210         .accept         = l2cap_sock_accept,
4211         .getname        = l2cap_sock_getname,
4212         .sendmsg        = l2cap_sock_sendmsg,
4213         .recvmsg        = l2cap_sock_recvmsg,
4214         .poll           = bt_sock_poll,
4215         .ioctl          = bt_sock_ioctl,
4216         .mmap           = sock_no_mmap,
4217         .socketpair     = sock_no_socketpair,
4218         .shutdown       = l2cap_sock_shutdown,
4219         .setsockopt     = l2cap_sock_setsockopt,
4220         .getsockopt     = l2cap_sock_getsockopt
4221 };
4222
4223 static const struct net_proto_family l2cap_sock_family_ops = {
4224         .family = PF_BLUETOOTH,
4225         .owner  = THIS_MODULE,
4226         .create = l2cap_sock_create,
4227 };
4228
4229 static struct hci_proto l2cap_hci_proto = {
4230         .name           = "L2CAP",
4231         .id             = HCI_PROTO_L2CAP,
4232         .connect_ind    = l2cap_connect_ind,
4233         .connect_cfm    = l2cap_connect_cfm,
4234         .disconn_ind    = l2cap_disconn_ind,
4235         .disconn_cfm    = l2cap_disconn_cfm,
4236         .security_cfm   = l2cap_security_cfm,
4237         .recv_acldata   = l2cap_recv_acldata
4238 };
4239
4240 static int __init l2cap_init(void)
4241 {
4242         int err;
4243
4244         err = proto_register(&l2cap_proto, 0);
4245         if (err < 0)
4246                 return err;
4247
4248         err = bt_sock_register(BTPROTO_L2CAP, &l2cap_sock_family_ops);
4249         if (err < 0) {
4250                 BT_ERR("L2CAP socket registration failed");
4251                 goto error;
4252         }
4253
4254         err = hci_register_proto(&l2cap_hci_proto);
4255         if (err < 0) {
4256                 BT_ERR("L2CAP protocol registration failed");
4257                 bt_sock_unregister(BTPROTO_L2CAP);
4258                 goto error;
4259         }
4260
4261         if (bt_debugfs) {
4262                 l2cap_debugfs = debugfs_create_file("l2cap", 0444,
4263                                         bt_debugfs, NULL, &l2cap_debugfs_fops);
4264                 if (!l2cap_debugfs)
4265                         BT_ERR("Failed to create L2CAP debug file");
4266         }
4267
4268         BT_INFO("L2CAP ver %s", VERSION);
4269         BT_INFO("L2CAP socket layer initialized");
4270
4271         return 0;
4272
4273 error:
4274         proto_unregister(&l2cap_proto);
4275         return err;
4276 }
4277
4278 static void __exit l2cap_exit(void)
4279 {
4280         debugfs_remove(l2cap_debugfs);
4281
4282         if (bt_sock_unregister(BTPROTO_L2CAP) < 0)
4283                 BT_ERR("L2CAP socket unregistration failed");
4284
4285         if (hci_unregister_proto(&l2cap_hci_proto) < 0)
4286                 BT_ERR("L2CAP protocol unregistration failed");
4287
4288         proto_unregister(&l2cap_proto);
4289 }
4290
4291 void l2cap_load(void)
4292 {
4293         /* Dummy function to trigger automatic L2CAP module loading by
4294          * other modules that use L2CAP sockets but don't use any other
4295          * symbols from it. */
4296         return;
4297 }
4298 EXPORT_SYMBOL(l2cap_load);
4299
4300 module_init(l2cap_init);
4301 module_exit(l2cap_exit);
4302
4303 module_param(enable_ertm, bool, 0644);
4304 MODULE_PARM_DESC(enable_ertm, "Enable enhanced retransmission mode");
4305
4306 module_param(max_transmit, uint, 0644);
4307 MODULE_PARM_DESC(max_transmit, "Max transmit value (default = 3)");
4308
4309 module_param(tx_window, uint, 0644);
4310 MODULE_PARM_DESC(tx_window, "Transmission window size value (default = 63)");
4311
4312 MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
4313 MODULE_DESCRIPTION("Bluetooth L2CAP ver " VERSION);
4314 MODULE_VERSION(VERSION);
4315 MODULE_LICENSE("GPL");
4316 MODULE_ALIAS("bt-proto-0");