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                         l2cap_pi(sk)->unacked_frames) {
1267                 if (bt_cb(skb)->tx_seq == l2cap_pi(sk)->expected_ack_seq)
1268                         break;
1269
1270                 skb = skb_dequeue(TX_QUEUE(sk));
1271                 kfree_skb(skb);
1272
1273                 l2cap_pi(sk)->unacked_frames--;
1274         }
1275
1276         if (!l2cap_pi(sk)->unacked_frames)
1277                 del_timer(&l2cap_pi(sk)->retrans_timer);
1278
1279         return;
1280 }
1281
1282 static inline int l2cap_do_send(struct sock *sk, struct sk_buff *skb)
1283 {
1284         struct l2cap_pinfo *pi = l2cap_pi(sk);
1285         int err;
1286
1287         BT_DBG("sk %p, skb %p len %d", sk, skb, skb->len);
1288
1289         err = hci_send_acl(pi->conn->hcon, skb, 0);
1290         if (err < 0)
1291                 kfree_skb(skb);
1292
1293         return err;
1294 }
1295
1296 static int l2cap_streaming_send(struct sock *sk)
1297 {
1298         struct sk_buff *skb, *tx_skb;
1299         struct l2cap_pinfo *pi = l2cap_pi(sk);
1300         u16 control, fcs;
1301         int err;
1302
1303         while ((skb = sk->sk_send_head)) {
1304                 tx_skb = skb_clone(skb, GFP_ATOMIC);
1305
1306                 control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE);
1307                 control |= pi->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT;
1308                 put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);
1309
1310                 if (pi->fcs == L2CAP_FCS_CRC16) {
1311                         fcs = crc16(0, (u8 *)tx_skb->data, tx_skb->len - 2);
1312                         put_unaligned_le16(fcs, tx_skb->data + tx_skb->len - 2);
1313                 }
1314
1315                 err = l2cap_do_send(sk, tx_skb);
1316                 if (err < 0) {
1317                         l2cap_send_disconn_req(pi->conn, sk);
1318                         return err;
1319                 }
1320
1321                 pi->next_tx_seq = (pi->next_tx_seq + 1) % 64;
1322
1323                 if (skb_queue_is_last(TX_QUEUE(sk), skb))
1324                         sk->sk_send_head = NULL;
1325                 else
1326                         sk->sk_send_head = skb_queue_next(TX_QUEUE(sk), skb);
1327
1328                 skb = skb_dequeue(TX_QUEUE(sk));
1329                 kfree_skb(skb);
1330         }
1331         return 0;
1332 }
1333
1334 static int l2cap_retransmit_frame(struct sock *sk, u8 tx_seq)
1335 {
1336         struct l2cap_pinfo *pi = l2cap_pi(sk);
1337         struct sk_buff *skb, *tx_skb;
1338         u16 control, fcs;
1339         int err;
1340
1341         skb = skb_peek(TX_QUEUE(sk));
1342         do {
1343                 if (bt_cb(skb)->tx_seq != tx_seq) {
1344                         if (skb_queue_is_last(TX_QUEUE(sk), skb))
1345                                 break;
1346                         skb = skb_queue_next(TX_QUEUE(sk), skb);
1347                         continue;
1348                 }
1349
1350                 if (pi->remote_max_tx &&
1351                                 bt_cb(skb)->retries == pi->remote_max_tx) {
1352                         l2cap_send_disconn_req(pi->conn, sk);
1353                         break;
1354                 }
1355
1356                 tx_skb = skb_clone(skb, GFP_ATOMIC);
1357                 bt_cb(skb)->retries++;
1358                 control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE);
1359                 control |= (pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT)
1360                                 | (tx_seq << L2CAP_CTRL_TXSEQ_SHIFT);
1361                 put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);
1362
1363                 if (pi->fcs == L2CAP_FCS_CRC16) {
1364                         fcs = crc16(0, (u8 *)tx_skb->data, tx_skb->len - 2);
1365                         put_unaligned_le16(fcs, tx_skb->data + tx_skb->len - 2);
1366                 }
1367
1368                 err = l2cap_do_send(sk, tx_skb);
1369                 if (err < 0) {
1370                         l2cap_send_disconn_req(pi->conn, sk);
1371                         return err;
1372                 }
1373                 break;
1374         } while(1);
1375         return 0;
1376 }
1377
1378 static int l2cap_ertm_send(struct sock *sk)
1379 {
1380         struct sk_buff *skb, *tx_skb;
1381         struct l2cap_pinfo *pi = l2cap_pi(sk);
1382         u16 control, fcs;
1383         int err, nsent = 0;
1384
1385         if (pi->conn_state & L2CAP_CONN_WAIT_F)
1386                 return 0;
1387
1388         while ((skb = sk->sk_send_head) && (!l2cap_tx_window_full(sk)) &&
1389                !(pi->conn_state & L2CAP_CONN_REMOTE_BUSY)) {
1390
1391                 if (pi->remote_max_tx &&
1392                                 bt_cb(skb)->retries == pi->remote_max_tx) {
1393                         l2cap_send_disconn_req(pi->conn, sk);
1394                         break;
1395                 }
1396
1397                 tx_skb = skb_clone(skb, GFP_ATOMIC);
1398
1399                 bt_cb(skb)->retries++;
1400
1401                 control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE);
1402                 if (pi->conn_state & L2CAP_CONN_SEND_FBIT) {
1403                         control |= L2CAP_CTRL_FINAL;
1404                         pi->conn_state &= ~L2CAP_CONN_SEND_FBIT;
1405                 }
1406                 control |= (pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT)
1407                                 | (pi->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT);
1408                 put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);
1409
1410
1411                 if (pi->fcs == L2CAP_FCS_CRC16) {
1412                         fcs = crc16(0, (u8 *)skb->data, tx_skb->len - 2);
1413                         put_unaligned_le16(fcs, skb->data + tx_skb->len - 2);
1414                 }
1415
1416                 err = l2cap_do_send(sk, tx_skb);
1417                 if (err < 0) {
1418                         l2cap_send_disconn_req(pi->conn, sk);
1419                         return err;
1420                 }
1421                 __mod_retrans_timer();
1422
1423                 bt_cb(skb)->tx_seq = pi->next_tx_seq;
1424                 pi->next_tx_seq = (pi->next_tx_seq + 1) % 64;
1425
1426                 pi->unacked_frames++;
1427                 pi->frames_sent++;
1428
1429                 if (skb_queue_is_last(TX_QUEUE(sk), skb))
1430                         sk->sk_send_head = NULL;
1431                 else
1432                         sk->sk_send_head = skb_queue_next(TX_QUEUE(sk), skb);
1433
1434                 nsent++;
1435         }
1436
1437         return nsent;
1438 }
1439
1440 static int l2cap_send_ack(struct l2cap_pinfo *pi)
1441 {
1442         struct sock *sk = (struct sock *)pi;
1443         u16 control = 0;
1444
1445         control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
1446
1447         if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
1448                 control |= L2CAP_SUPER_RCV_NOT_READY;
1449                 return l2cap_send_sframe(pi, control);
1450         } else if (l2cap_ertm_send(sk) == 0) {
1451                 control |= L2CAP_SUPER_RCV_READY;
1452                 return l2cap_send_sframe(pi, control);
1453         }
1454         return 0;
1455 }
1456
1457 static int l2cap_send_srejtail(struct sock *sk)
1458 {
1459         struct srej_list *tail;
1460         u16 control;
1461
1462         control = L2CAP_SUPER_SELECT_REJECT;
1463         control |= L2CAP_CTRL_FINAL;
1464
1465         tail = list_entry(SREJ_LIST(sk)->prev, struct srej_list, list);
1466         control |= tail->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
1467
1468         l2cap_send_sframe(l2cap_pi(sk), control);
1469
1470         return 0;
1471 }
1472
1473 static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, int len, int count, struct sk_buff *skb)
1474 {
1475         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1476         struct sk_buff **frag;
1477         int err, sent = 0;
1478
1479         if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count)) {
1480                 return -EFAULT;
1481         }
1482
1483         sent += count;
1484         len  -= count;
1485
1486         /* Continuation fragments (no L2CAP header) */
1487         frag = &skb_shinfo(skb)->frag_list;
1488         while (len) {
1489                 count = min_t(unsigned int, conn->mtu, len);
1490
1491                 *frag = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err);
1492                 if (!*frag)
1493                         return -EFAULT;
1494                 if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count))
1495                         return -EFAULT;
1496
1497                 sent += count;
1498                 len  -= count;
1499
1500                 frag = &(*frag)->next;
1501         }
1502
1503         return sent;
1504 }
1505
1506 static struct sk_buff *l2cap_create_connless_pdu(struct sock *sk, struct msghdr *msg, size_t len)
1507 {
1508         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1509         struct sk_buff *skb;
1510         int err, count, hlen = L2CAP_HDR_SIZE + 2;
1511         struct l2cap_hdr *lh;
1512
1513         BT_DBG("sk %p len %d", sk, (int)len);
1514
1515         count = min_t(unsigned int, (conn->mtu - hlen), len);
1516         skb = bt_skb_send_alloc(sk, count + hlen,
1517                         msg->msg_flags & MSG_DONTWAIT, &err);
1518         if (!skb)
1519                 return ERR_PTR(-ENOMEM);
1520
1521         /* Create L2CAP header */
1522         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1523         lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
1524         lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1525         put_unaligned_le16(l2cap_pi(sk)->psm, skb_put(skb, 2));
1526
1527         err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1528         if (unlikely(err < 0)) {
1529                 kfree_skb(skb);
1530                 return ERR_PTR(err);
1531         }
1532         return skb;
1533 }
1534
1535 static struct sk_buff *l2cap_create_basic_pdu(struct sock *sk, struct msghdr *msg, size_t len)
1536 {
1537         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1538         struct sk_buff *skb;
1539         int err, count, hlen = L2CAP_HDR_SIZE;
1540         struct l2cap_hdr *lh;
1541
1542         BT_DBG("sk %p len %d", sk, (int)len);
1543
1544         count = min_t(unsigned int, (conn->mtu - hlen), len);
1545         skb = bt_skb_send_alloc(sk, count + hlen,
1546                         msg->msg_flags & MSG_DONTWAIT, &err);
1547         if (!skb)
1548                 return ERR_PTR(-ENOMEM);
1549
1550         /* Create L2CAP header */
1551         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1552         lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
1553         lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1554
1555         err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1556         if (unlikely(err < 0)) {
1557                 kfree_skb(skb);
1558                 return ERR_PTR(err);
1559         }
1560         return skb;
1561 }
1562
1563 static struct sk_buff *l2cap_create_iframe_pdu(struct sock *sk, struct msghdr *msg, size_t len, u16 control, u16 sdulen)
1564 {
1565         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1566         struct sk_buff *skb;
1567         int err, count, hlen = L2CAP_HDR_SIZE + 2;
1568         struct l2cap_hdr *lh;
1569
1570         BT_DBG("sk %p len %d", sk, (int)len);
1571
1572         if (!conn)
1573                 return ERR_PTR(-ENOTCONN);
1574
1575         if (sdulen)
1576                 hlen += 2;
1577
1578         if (l2cap_pi(sk)->fcs == L2CAP_FCS_CRC16)
1579                 hlen += 2;
1580
1581         count = min_t(unsigned int, (conn->mtu - hlen), len);
1582         skb = bt_skb_send_alloc(sk, count + hlen,
1583                         msg->msg_flags & MSG_DONTWAIT, &err);
1584         if (!skb)
1585                 return ERR_PTR(-ENOMEM);
1586
1587         /* Create L2CAP header */
1588         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1589         lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
1590         lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1591         put_unaligned_le16(control, skb_put(skb, 2));
1592         if (sdulen)
1593                 put_unaligned_le16(sdulen, skb_put(skb, 2));
1594
1595         err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1596         if (unlikely(err < 0)) {
1597                 kfree_skb(skb);
1598                 return ERR_PTR(err);
1599         }
1600
1601         if (l2cap_pi(sk)->fcs == L2CAP_FCS_CRC16)
1602                 put_unaligned_le16(0, skb_put(skb, 2));
1603
1604         bt_cb(skb)->retries = 0;
1605         return skb;
1606 }
1607
1608 static inline int l2cap_sar_segment_sdu(struct sock *sk, struct msghdr *msg, size_t len)
1609 {
1610         struct l2cap_pinfo *pi = l2cap_pi(sk);
1611         struct sk_buff *skb;
1612         struct sk_buff_head sar_queue;
1613         u16 control;
1614         size_t size = 0;
1615
1616         __skb_queue_head_init(&sar_queue);
1617         control = L2CAP_SDU_START;
1618         skb = l2cap_create_iframe_pdu(sk, msg, pi->remote_mps, control, len);
1619         if (IS_ERR(skb))
1620                 return PTR_ERR(skb);
1621
1622         __skb_queue_tail(&sar_queue, skb);
1623         len -= pi->remote_mps;
1624         size += pi->remote_mps;
1625         control = 0;
1626
1627         while (len > 0) {
1628                 size_t buflen;
1629
1630                 if (len > pi->remote_mps) {
1631                         control |= L2CAP_SDU_CONTINUE;
1632                         buflen = pi->remote_mps;
1633                 } else {
1634                         control |= L2CAP_SDU_END;
1635                         buflen = len;
1636                 }
1637
1638                 skb = l2cap_create_iframe_pdu(sk, msg, buflen, control, 0);
1639                 if (IS_ERR(skb)) {
1640                         skb_queue_purge(&sar_queue);
1641                         return PTR_ERR(skb);
1642                 }
1643
1644                 __skb_queue_tail(&sar_queue, skb);
1645                 len -= buflen;
1646                 size += buflen;
1647                 control = 0;
1648         }
1649         skb_queue_splice_tail(&sar_queue, TX_QUEUE(sk));
1650         if (sk->sk_send_head == NULL)
1651                 sk->sk_send_head = sar_queue.next;
1652
1653         return size;
1654 }
1655
1656 static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len)
1657 {
1658         struct sock *sk = sock->sk;
1659         struct l2cap_pinfo *pi = l2cap_pi(sk);
1660         struct sk_buff *skb;
1661         u16 control;
1662         int err;
1663
1664         BT_DBG("sock %p, sk %p", sock, sk);
1665
1666         err = sock_error(sk);
1667         if (err)
1668                 return err;
1669
1670         if (msg->msg_flags & MSG_OOB)
1671                 return -EOPNOTSUPP;
1672
1673         lock_sock(sk);
1674
1675         if (sk->sk_state != BT_CONNECTED) {
1676                 err = -ENOTCONN;
1677                 goto done;
1678         }
1679
1680         /* Connectionless channel */
1681         if (sk->sk_type == SOCK_DGRAM) {
1682                 skb = l2cap_create_connless_pdu(sk, msg, len);
1683                 if (IS_ERR(skb))
1684                         err = PTR_ERR(skb);
1685                 else
1686                         err = l2cap_do_send(sk, skb);
1687                 goto done;
1688         }
1689
1690         switch (pi->mode) {
1691         case L2CAP_MODE_BASIC:
1692                 /* Check outgoing MTU */
1693                 if (len > pi->omtu) {
1694                         err = -EINVAL;
1695                         goto done;
1696                 }
1697
1698                 /* Create a basic PDU */
1699                 skb = l2cap_create_basic_pdu(sk, msg, len);
1700                 if (IS_ERR(skb)) {
1701                         err = PTR_ERR(skb);
1702                         goto done;
1703                 }
1704
1705                 err = l2cap_do_send(sk, skb);
1706                 if (!err)
1707                         err = len;
1708                 break;
1709
1710         case L2CAP_MODE_ERTM:
1711         case L2CAP_MODE_STREAMING:
1712                 /* Entire SDU fits into one PDU */
1713                 if (len <= pi->remote_mps) {
1714                         control = L2CAP_SDU_UNSEGMENTED;
1715                         skb = l2cap_create_iframe_pdu(sk, msg, len, control, 0);
1716                         if (IS_ERR(skb)) {
1717                                 err = PTR_ERR(skb);
1718                                 goto done;
1719                         }
1720                         __skb_queue_tail(TX_QUEUE(sk), skb);
1721                         if (sk->sk_send_head == NULL)
1722                                 sk->sk_send_head = skb;
1723                 } else {
1724                 /* Segment SDU into multiples PDUs */
1725                         err = l2cap_sar_segment_sdu(sk, msg, len);
1726                         if (err < 0)
1727                                 goto done;
1728                 }
1729
1730                 if (pi->mode == L2CAP_MODE_STREAMING)
1731                         err = l2cap_streaming_send(sk);
1732                 else
1733                         err = l2cap_ertm_send(sk);
1734
1735                 if (err >= 0)
1736                         err = len;
1737                 break;
1738
1739         default:
1740                 BT_DBG("bad state %1.1x", pi->mode);
1741                 err = -EINVAL;
1742         }
1743
1744 done:
1745         release_sock(sk);
1746         return err;
1747 }
1748
1749 static int l2cap_sock_recvmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len, int flags)
1750 {
1751         struct sock *sk = sock->sk;
1752
1753         lock_sock(sk);
1754
1755         if (sk->sk_state == BT_CONNECT2 && bt_sk(sk)->defer_setup) {
1756                 struct l2cap_conn_rsp rsp;
1757
1758                 sk->sk_state = BT_CONFIG;
1759
1760                 rsp.scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
1761                 rsp.dcid   = cpu_to_le16(l2cap_pi(sk)->scid);
1762                 rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
1763                 rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
1764                 l2cap_send_cmd(l2cap_pi(sk)->conn, l2cap_pi(sk)->ident,
1765                                         L2CAP_CONN_RSP, sizeof(rsp), &rsp);
1766
1767                 release_sock(sk);
1768                 return 0;
1769         }
1770
1771         release_sock(sk);
1772
1773         return bt_sock_recvmsg(iocb, sock, msg, len, flags);
1774 }
1775
1776 static int l2cap_sock_setsockopt_old(struct socket *sock, int optname, char __user *optval, unsigned int optlen)
1777 {
1778         struct sock *sk = sock->sk;
1779         struct l2cap_options opts;
1780         int len, err = 0;
1781         u32 opt;
1782
1783         BT_DBG("sk %p", sk);
1784
1785         lock_sock(sk);
1786
1787         switch (optname) {
1788         case L2CAP_OPTIONS:
1789                 opts.imtu     = l2cap_pi(sk)->imtu;
1790                 opts.omtu     = l2cap_pi(sk)->omtu;
1791                 opts.flush_to = l2cap_pi(sk)->flush_to;
1792                 opts.mode     = l2cap_pi(sk)->mode;
1793                 opts.fcs      = l2cap_pi(sk)->fcs;
1794                 opts.max_tx   = l2cap_pi(sk)->max_tx;
1795                 opts.txwin_size = (__u16)l2cap_pi(sk)->tx_win;
1796
1797                 len = min_t(unsigned int, sizeof(opts), optlen);
1798                 if (copy_from_user((char *) &opts, optval, len)) {
1799                         err = -EFAULT;
1800                         break;
1801                 }
1802
1803                 l2cap_pi(sk)->imtu = opts.imtu;
1804                 l2cap_pi(sk)->omtu = opts.omtu;
1805                 l2cap_pi(sk)->mode = opts.mode;
1806                 l2cap_pi(sk)->fcs  = opts.fcs;
1807                 l2cap_pi(sk)->max_tx = opts.max_tx;
1808                 l2cap_pi(sk)->tx_win = (__u8)opts.txwin_size;
1809                 break;
1810
1811         case L2CAP_LM:
1812                 if (get_user(opt, (u32 __user *) optval)) {
1813                         err = -EFAULT;
1814                         break;
1815                 }
1816
1817                 if (opt & L2CAP_LM_AUTH)
1818                         l2cap_pi(sk)->sec_level = BT_SECURITY_LOW;
1819                 if (opt & L2CAP_LM_ENCRYPT)
1820                         l2cap_pi(sk)->sec_level = BT_SECURITY_MEDIUM;
1821                 if (opt & L2CAP_LM_SECURE)
1822                         l2cap_pi(sk)->sec_level = BT_SECURITY_HIGH;
1823
1824                 l2cap_pi(sk)->role_switch    = (opt & L2CAP_LM_MASTER);
1825                 l2cap_pi(sk)->force_reliable = (opt & L2CAP_LM_RELIABLE);
1826                 break;
1827
1828         default:
1829                 err = -ENOPROTOOPT;
1830                 break;
1831         }
1832
1833         release_sock(sk);
1834         return err;
1835 }
1836
1837 static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname, char __user *optval, unsigned int optlen)
1838 {
1839         struct sock *sk = sock->sk;
1840         struct bt_security sec;
1841         int len, err = 0;
1842         u32 opt;
1843
1844         BT_DBG("sk %p", sk);
1845
1846         if (level == SOL_L2CAP)
1847                 return l2cap_sock_setsockopt_old(sock, optname, optval, optlen);
1848
1849         if (level != SOL_BLUETOOTH)
1850                 return -ENOPROTOOPT;
1851
1852         lock_sock(sk);
1853
1854         switch (optname) {
1855         case BT_SECURITY:
1856                 if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_RAW) {
1857                         err = -EINVAL;
1858                         break;
1859                 }
1860
1861                 sec.level = BT_SECURITY_LOW;
1862
1863                 len = min_t(unsigned int, sizeof(sec), optlen);
1864                 if (copy_from_user((char *) &sec, optval, len)) {
1865                         err = -EFAULT;
1866                         break;
1867                 }
1868
1869                 if (sec.level < BT_SECURITY_LOW ||
1870                                         sec.level > BT_SECURITY_HIGH) {
1871                         err = -EINVAL;
1872                         break;
1873                 }
1874
1875                 l2cap_pi(sk)->sec_level = sec.level;
1876                 break;
1877
1878         case BT_DEFER_SETUP:
1879                 if (sk->sk_state != BT_BOUND && sk->sk_state != BT_LISTEN) {
1880                         err = -EINVAL;
1881                         break;
1882                 }
1883
1884                 if (get_user(opt, (u32 __user *) optval)) {
1885                         err = -EFAULT;
1886                         break;
1887                 }
1888
1889                 bt_sk(sk)->defer_setup = opt;
1890                 break;
1891
1892         default:
1893                 err = -ENOPROTOOPT;
1894                 break;
1895         }
1896
1897         release_sock(sk);
1898         return err;
1899 }
1900
1901 static int l2cap_sock_getsockopt_old(struct socket *sock, int optname, char __user *optval, int __user *optlen)
1902 {
1903         struct sock *sk = sock->sk;
1904         struct l2cap_options opts;
1905         struct l2cap_conninfo cinfo;
1906         int len, err = 0;
1907         u32 opt;
1908
1909         BT_DBG("sk %p", sk);
1910
1911         if (get_user(len, optlen))
1912                 return -EFAULT;
1913
1914         lock_sock(sk);
1915
1916         switch (optname) {
1917         case L2CAP_OPTIONS:
1918                 opts.imtu     = l2cap_pi(sk)->imtu;
1919                 opts.omtu     = l2cap_pi(sk)->omtu;
1920                 opts.flush_to = l2cap_pi(sk)->flush_to;
1921                 opts.mode     = l2cap_pi(sk)->mode;
1922                 opts.fcs      = l2cap_pi(sk)->fcs;
1923                 opts.max_tx   = l2cap_pi(sk)->max_tx;
1924                 opts.txwin_size = (__u16)l2cap_pi(sk)->tx_win;
1925
1926                 len = min_t(unsigned int, len, sizeof(opts));
1927                 if (copy_to_user(optval, (char *) &opts, len))
1928                         err = -EFAULT;
1929
1930                 break;
1931
1932         case L2CAP_LM:
1933                 switch (l2cap_pi(sk)->sec_level) {
1934                 case BT_SECURITY_LOW:
1935                         opt = L2CAP_LM_AUTH;
1936                         break;
1937                 case BT_SECURITY_MEDIUM:
1938                         opt = L2CAP_LM_AUTH | L2CAP_LM_ENCRYPT;
1939                         break;
1940                 case BT_SECURITY_HIGH:
1941                         opt = L2CAP_LM_AUTH | L2CAP_LM_ENCRYPT |
1942                                                         L2CAP_LM_SECURE;
1943                         break;
1944                 default:
1945                         opt = 0;
1946                         break;
1947                 }
1948
1949                 if (l2cap_pi(sk)->role_switch)
1950                         opt |= L2CAP_LM_MASTER;
1951
1952                 if (l2cap_pi(sk)->force_reliable)
1953                         opt |= L2CAP_LM_RELIABLE;
1954
1955                 if (put_user(opt, (u32 __user *) optval))
1956                         err = -EFAULT;
1957                 break;
1958
1959         case L2CAP_CONNINFO:
1960                 if (sk->sk_state != BT_CONNECTED &&
1961                                         !(sk->sk_state == BT_CONNECT2 &&
1962                                                 bt_sk(sk)->defer_setup)) {
1963                         err = -ENOTCONN;
1964                         break;
1965                 }
1966
1967                 cinfo.hci_handle = l2cap_pi(sk)->conn->hcon->handle;
1968                 memcpy(cinfo.dev_class, l2cap_pi(sk)->conn->hcon->dev_class, 3);
1969
1970                 len = min_t(unsigned int, len, sizeof(cinfo));
1971                 if (copy_to_user(optval, (char *) &cinfo, len))
1972                         err = -EFAULT;
1973
1974                 break;
1975
1976         default:
1977                 err = -ENOPROTOOPT;
1978                 break;
1979         }
1980
1981         release_sock(sk);
1982         return err;
1983 }
1984
1985 static int l2cap_sock_getsockopt(struct socket *sock, int level, int optname, char __user *optval, int __user *optlen)
1986 {
1987         struct sock *sk = sock->sk;
1988         struct bt_security sec;
1989         int len, err = 0;
1990
1991         BT_DBG("sk %p", sk);
1992
1993         if (level == SOL_L2CAP)
1994                 return l2cap_sock_getsockopt_old(sock, optname, optval, optlen);
1995
1996         if (level != SOL_BLUETOOTH)
1997                 return -ENOPROTOOPT;
1998
1999         if (get_user(len, optlen))
2000                 return -EFAULT;
2001
2002         lock_sock(sk);
2003
2004         switch (optname) {
2005         case BT_SECURITY:
2006                 if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_RAW) {
2007                         err = -EINVAL;
2008                         break;
2009                 }
2010
2011                 sec.level = l2cap_pi(sk)->sec_level;
2012
2013                 len = min_t(unsigned int, len, sizeof(sec));
2014                 if (copy_to_user(optval, (char *) &sec, len))
2015                         err = -EFAULT;
2016
2017                 break;
2018
2019         case BT_DEFER_SETUP:
2020                 if (sk->sk_state != BT_BOUND && sk->sk_state != BT_LISTEN) {
2021                         err = -EINVAL;
2022                         break;
2023                 }
2024
2025                 if (put_user(bt_sk(sk)->defer_setup, (u32 __user *) optval))
2026                         err = -EFAULT;
2027
2028                 break;
2029
2030         default:
2031                 err = -ENOPROTOOPT;
2032                 break;
2033         }
2034
2035         release_sock(sk);
2036         return err;
2037 }
2038
2039 static int l2cap_sock_shutdown(struct socket *sock, int how)
2040 {
2041         struct sock *sk = sock->sk;
2042         int err = 0;
2043
2044         BT_DBG("sock %p, sk %p", sock, sk);
2045
2046         if (!sk)
2047                 return 0;
2048
2049         lock_sock(sk);
2050         if (!sk->sk_shutdown) {
2051                 sk->sk_shutdown = SHUTDOWN_MASK;
2052                 l2cap_sock_clear_timer(sk);
2053                 __l2cap_sock_close(sk, 0);
2054
2055                 if (sock_flag(sk, SOCK_LINGER) && sk->sk_lingertime)
2056                         err = bt_sock_wait_state(sk, BT_CLOSED,
2057                                                         sk->sk_lingertime);
2058         }
2059         release_sock(sk);
2060         return err;
2061 }
2062
2063 static int l2cap_sock_release(struct socket *sock)
2064 {
2065         struct sock *sk = sock->sk;
2066         int err;
2067
2068         BT_DBG("sock %p, sk %p", sock, sk);
2069
2070         if (!sk)
2071                 return 0;
2072
2073         err = l2cap_sock_shutdown(sock, 2);
2074
2075         sock_orphan(sk);
2076         l2cap_sock_kill(sk);
2077         return err;
2078 }
2079
2080 static void l2cap_chan_ready(struct sock *sk)
2081 {
2082         struct sock *parent = bt_sk(sk)->parent;
2083
2084         BT_DBG("sk %p, parent %p", sk, parent);
2085
2086         l2cap_pi(sk)->conf_state = 0;
2087         l2cap_sock_clear_timer(sk);
2088
2089         if (!parent) {
2090                 /* Outgoing channel.
2091                  * Wake up socket sleeping on connect.
2092                  */
2093                 sk->sk_state = BT_CONNECTED;
2094                 sk->sk_state_change(sk);
2095         } else {
2096                 /* Incoming channel.
2097                  * Wake up socket sleeping on accept.
2098                  */
2099                 parent->sk_data_ready(parent, 0);
2100         }
2101 }
2102
2103 /* Copy frame to all raw sockets on that connection */
2104 static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
2105 {
2106         struct l2cap_chan_list *l = &conn->chan_list;
2107         struct sk_buff *nskb;
2108         struct sock *sk;
2109
2110         BT_DBG("conn %p", conn);
2111
2112         read_lock(&l->lock);
2113         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
2114                 if (sk->sk_type != SOCK_RAW)
2115                         continue;
2116
2117                 /* Don't send frame to the socket it came from */
2118                 if (skb->sk == sk)
2119                         continue;
2120                 nskb = skb_clone(skb, GFP_ATOMIC);
2121                 if (!nskb)
2122                         continue;
2123
2124                 if (sock_queue_rcv_skb(sk, nskb))
2125                         kfree_skb(nskb);
2126         }
2127         read_unlock(&l->lock);
2128 }
2129
2130 /* ---- L2CAP signalling commands ---- */
2131 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
2132                                 u8 code, u8 ident, u16 dlen, void *data)
2133 {
2134         struct sk_buff *skb, **frag;
2135         struct l2cap_cmd_hdr *cmd;
2136         struct l2cap_hdr *lh;
2137         int len, count;
2138
2139         BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %d",
2140                         conn, code, ident, dlen);
2141
2142         len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen;
2143         count = min_t(unsigned int, conn->mtu, len);
2144
2145         skb = bt_skb_alloc(count, GFP_ATOMIC);
2146         if (!skb)
2147                 return NULL;
2148
2149         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
2150         lh->len = cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen);
2151         lh->cid = cpu_to_le16(L2CAP_CID_SIGNALING);
2152
2153         cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE);
2154         cmd->code  = code;
2155         cmd->ident = ident;
2156         cmd->len   = cpu_to_le16(dlen);
2157
2158         if (dlen) {
2159                 count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE;
2160                 memcpy(skb_put(skb, count), data, count);
2161                 data += count;
2162         }
2163
2164         len -= skb->len;
2165
2166         /* Continuation fragments (no L2CAP header) */
2167         frag = &skb_shinfo(skb)->frag_list;
2168         while (len) {
2169                 count = min_t(unsigned int, conn->mtu, len);
2170
2171                 *frag = bt_skb_alloc(count, GFP_ATOMIC);
2172                 if (!*frag)
2173                         goto fail;
2174
2175                 memcpy(skb_put(*frag, count), data, count);
2176
2177                 len  -= count;
2178                 data += count;
2179
2180                 frag = &(*frag)->next;
2181         }
2182
2183         return skb;
2184
2185 fail:
2186         kfree_skb(skb);
2187         return NULL;
2188 }
2189
2190 static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val)
2191 {
2192         struct l2cap_conf_opt *opt = *ptr;
2193         int len;
2194
2195         len = L2CAP_CONF_OPT_SIZE + opt->len;
2196         *ptr += len;
2197
2198         *type = opt->type;
2199         *olen = opt->len;
2200
2201         switch (opt->len) {
2202         case 1:
2203                 *val = *((u8 *) opt->val);
2204                 break;
2205
2206         case 2:
2207                 *val = __le16_to_cpu(*((__le16 *) opt->val));
2208                 break;
2209
2210         case 4:
2211                 *val = __le32_to_cpu(*((__le32 *) opt->val));
2212                 break;
2213
2214         default:
2215                 *val = (unsigned long) opt->val;
2216                 break;
2217         }
2218
2219         BT_DBG("type 0x%2.2x len %d val 0x%lx", *type, opt->len, *val);
2220         return len;
2221 }
2222
2223 static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val)
2224 {
2225         struct l2cap_conf_opt *opt = *ptr;
2226
2227         BT_DBG("type 0x%2.2x len %d val 0x%lx", type, len, val);
2228
2229         opt->type = type;
2230         opt->len  = len;
2231
2232         switch (len) {
2233         case 1:
2234                 *((u8 *) opt->val)  = val;
2235                 break;
2236
2237         case 2:
2238                 *((__le16 *) opt->val) = cpu_to_le16(val);
2239                 break;
2240
2241         case 4:
2242                 *((__le32 *) opt->val) = cpu_to_le32(val);
2243                 break;
2244
2245         default:
2246                 memcpy(opt->val, (void *) val, len);
2247                 break;
2248         }
2249
2250         *ptr += L2CAP_CONF_OPT_SIZE + len;
2251 }
2252
2253 static void l2cap_ack_timeout(unsigned long arg)
2254 {
2255         struct sock *sk = (void *) arg;
2256
2257         bh_lock_sock(sk);
2258         l2cap_send_ack(l2cap_pi(sk));
2259         bh_unlock_sock(sk);
2260 }
2261
2262 static inline void l2cap_ertm_init(struct sock *sk)
2263 {
2264         l2cap_pi(sk)->expected_ack_seq = 0;
2265         l2cap_pi(sk)->unacked_frames = 0;
2266         l2cap_pi(sk)->buffer_seq = 0;
2267         l2cap_pi(sk)->num_acked = 0;
2268         l2cap_pi(sk)->frames_sent = 0;
2269
2270         setup_timer(&l2cap_pi(sk)->retrans_timer,
2271                         l2cap_retrans_timeout, (unsigned long) sk);
2272         setup_timer(&l2cap_pi(sk)->monitor_timer,
2273                         l2cap_monitor_timeout, (unsigned long) sk);
2274         setup_timer(&l2cap_pi(sk)->ack_timer,
2275                         l2cap_ack_timeout, (unsigned long) sk);
2276
2277         __skb_queue_head_init(SREJ_QUEUE(sk));
2278 }
2279
2280 static int l2cap_mode_supported(__u8 mode, __u32 feat_mask)
2281 {
2282         u32 local_feat_mask = l2cap_feat_mask;
2283         if (enable_ertm)
2284                 local_feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING;
2285
2286         switch (mode) {
2287         case L2CAP_MODE_ERTM:
2288                 return L2CAP_FEAT_ERTM & feat_mask & local_feat_mask;
2289         case L2CAP_MODE_STREAMING:
2290                 return L2CAP_FEAT_STREAMING & feat_mask & local_feat_mask;
2291         default:
2292                 return 0x00;
2293         }
2294 }
2295
2296 static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
2297 {
2298         switch (mode) {
2299         case L2CAP_MODE_STREAMING:
2300         case L2CAP_MODE_ERTM:
2301                 if (l2cap_mode_supported(mode, remote_feat_mask))
2302                         return mode;
2303                 /* fall through */
2304         default:
2305                 return L2CAP_MODE_BASIC;
2306         }
2307 }
2308
2309 static int l2cap_build_conf_req(struct sock *sk, void *data)
2310 {
2311         struct l2cap_pinfo *pi = l2cap_pi(sk);
2312         struct l2cap_conf_req *req = data;
2313         struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
2314         void *ptr = req->data;
2315
2316         BT_DBG("sk %p", sk);
2317
2318         if (pi->num_conf_req || pi->num_conf_rsp)
2319                 goto done;
2320
2321         switch (pi->mode) {
2322         case L2CAP_MODE_STREAMING:
2323         case L2CAP_MODE_ERTM:
2324                 pi->conf_state |= L2CAP_CONF_STATE2_DEVICE;
2325                 if (!l2cap_mode_supported(pi->mode, pi->conn->feat_mask))
2326                         l2cap_send_disconn_req(pi->conn, sk);
2327                 break;
2328         default:
2329                 pi->mode = l2cap_select_mode(rfc.mode, pi->conn->feat_mask);
2330                 break;
2331         }
2332
2333 done:
2334         switch (pi->mode) {
2335         case L2CAP_MODE_BASIC:
2336                 if (pi->imtu != L2CAP_DEFAULT_MTU)
2337                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->imtu);
2338                 break;
2339
2340         case L2CAP_MODE_ERTM:
2341                 rfc.mode            = L2CAP_MODE_ERTM;
2342                 rfc.txwin_size      = pi->tx_win;
2343                 rfc.max_transmit    = pi->max_tx;
2344                 rfc.retrans_timeout = 0;
2345                 rfc.monitor_timeout = 0;
2346                 rfc.max_pdu_size    = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE);
2347                 if (L2CAP_DEFAULT_MAX_PDU_SIZE > pi->conn->mtu - 10)
2348                         rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10);
2349
2350                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2351                                         sizeof(rfc), (unsigned long) &rfc);
2352
2353                 if (!(pi->conn->feat_mask & L2CAP_FEAT_FCS))
2354                         break;
2355
2356                 if (pi->fcs == L2CAP_FCS_NONE ||
2357                                 pi->conf_state & L2CAP_CONF_NO_FCS_RECV) {
2358                         pi->fcs = L2CAP_FCS_NONE;
2359                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, pi->fcs);
2360                 }
2361                 break;
2362
2363         case L2CAP_MODE_STREAMING:
2364                 rfc.mode            = L2CAP_MODE_STREAMING;
2365                 rfc.txwin_size      = 0;
2366                 rfc.max_transmit    = 0;
2367                 rfc.retrans_timeout = 0;
2368                 rfc.monitor_timeout = 0;
2369                 rfc.max_pdu_size    = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE);
2370                 if (L2CAP_DEFAULT_MAX_PDU_SIZE > pi->conn->mtu - 10)
2371                         rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10);
2372
2373                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2374                                         sizeof(rfc), (unsigned long) &rfc);
2375
2376                 if (!(pi->conn->feat_mask & L2CAP_FEAT_FCS))
2377                         break;
2378
2379                 if (pi->fcs == L2CAP_FCS_NONE ||
2380                                 pi->conf_state & L2CAP_CONF_NO_FCS_RECV) {
2381                         pi->fcs = L2CAP_FCS_NONE;
2382                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, pi->fcs);
2383                 }
2384                 break;
2385         }
2386
2387         /* FIXME: Need actual value of the flush timeout */
2388         //if (flush_to != L2CAP_DEFAULT_FLUSH_TO)
2389         //   l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO, 2, pi->flush_to);
2390
2391         req->dcid  = cpu_to_le16(pi->dcid);
2392         req->flags = cpu_to_le16(0);
2393
2394         return ptr - data;
2395 }
2396
2397 static int l2cap_parse_conf_req(struct sock *sk, void *data)
2398 {
2399         struct l2cap_pinfo *pi = l2cap_pi(sk);
2400         struct l2cap_conf_rsp *rsp = data;
2401         void *ptr = rsp->data;
2402         void *req = pi->conf_req;
2403         int len = pi->conf_len;
2404         int type, hint, olen;
2405         unsigned long val;
2406         struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
2407         u16 mtu = L2CAP_DEFAULT_MTU;
2408         u16 result = L2CAP_CONF_SUCCESS;
2409
2410         BT_DBG("sk %p", sk);
2411
2412         while (len >= L2CAP_CONF_OPT_SIZE) {
2413                 len -= l2cap_get_conf_opt(&req, &type, &olen, &val);
2414
2415                 hint  = type & L2CAP_CONF_HINT;
2416                 type &= L2CAP_CONF_MASK;
2417
2418                 switch (type) {
2419                 case L2CAP_CONF_MTU:
2420                         mtu = val;
2421                         break;
2422
2423                 case L2CAP_CONF_FLUSH_TO:
2424                         pi->flush_to = val;
2425                         break;
2426
2427                 case L2CAP_CONF_QOS:
2428                         break;
2429
2430                 case L2CAP_CONF_RFC:
2431                         if (olen == sizeof(rfc))
2432                                 memcpy(&rfc, (void *) val, olen);
2433                         break;
2434
2435                 case L2CAP_CONF_FCS:
2436                         if (val == L2CAP_FCS_NONE)
2437                                 pi->conf_state |= L2CAP_CONF_NO_FCS_RECV;
2438
2439                         break;
2440
2441                 default:
2442                         if (hint)
2443                                 break;
2444
2445                         result = L2CAP_CONF_UNKNOWN;
2446                         *((u8 *) ptr++) = type;
2447                         break;
2448                 }
2449         }
2450
2451         if (pi->num_conf_rsp || pi->num_conf_req)
2452                 goto done;
2453
2454         switch (pi->mode) {
2455         case L2CAP_MODE_STREAMING:
2456         case L2CAP_MODE_ERTM:
2457                 pi->conf_state |= L2CAP_CONF_STATE2_DEVICE;
2458                 if (!l2cap_mode_supported(pi->mode, pi->conn->feat_mask))
2459                         return -ECONNREFUSED;
2460                 break;
2461         default:
2462                 pi->mode = l2cap_select_mode(rfc.mode, pi->conn->feat_mask);
2463                 break;
2464         }
2465
2466 done:
2467         if (pi->mode != rfc.mode) {
2468                 result = L2CAP_CONF_UNACCEPT;
2469                 rfc.mode = pi->mode;
2470
2471                 if (pi->num_conf_rsp == 1)
2472                         return -ECONNREFUSED;
2473
2474                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2475                                         sizeof(rfc), (unsigned long) &rfc);
2476         }
2477
2478
2479         if (result == L2CAP_CONF_SUCCESS) {
2480                 /* Configure output options and let the other side know
2481                  * which ones we don't like. */
2482
2483                 if (mtu < L2CAP_DEFAULT_MIN_MTU)
2484                         result = L2CAP_CONF_UNACCEPT;
2485                 else {
2486                         pi->omtu = mtu;
2487                         pi->conf_state |= L2CAP_CONF_MTU_DONE;
2488                 }
2489                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->omtu);
2490
2491                 switch (rfc.mode) {
2492                 case L2CAP_MODE_BASIC:
2493                         pi->fcs = L2CAP_FCS_NONE;
2494                         pi->conf_state |= L2CAP_CONF_MODE_DONE;
2495                         break;
2496
2497                 case L2CAP_MODE_ERTM:
2498                         pi->remote_tx_win = rfc.txwin_size;
2499                         pi->remote_max_tx = rfc.max_transmit;
2500                         if (rfc.max_pdu_size > pi->conn->mtu - 10)
2501                                 rfc.max_pdu_size = le16_to_cpu(pi->conn->mtu - 10);
2502
2503                         pi->remote_mps = le16_to_cpu(rfc.max_pdu_size);
2504
2505                         rfc.retrans_timeout =
2506                                 le16_to_cpu(L2CAP_DEFAULT_RETRANS_TO);
2507                         rfc.monitor_timeout =
2508                                 le16_to_cpu(L2CAP_DEFAULT_MONITOR_TO);
2509
2510                         pi->conf_state |= L2CAP_CONF_MODE_DONE;
2511
2512                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2513                                         sizeof(rfc), (unsigned long) &rfc);
2514
2515                         break;
2516
2517                 case L2CAP_MODE_STREAMING:
2518                         if (rfc.max_pdu_size > pi->conn->mtu - 10)
2519                                 rfc.max_pdu_size = le16_to_cpu(pi->conn->mtu - 10);
2520
2521                         pi->remote_mps = le16_to_cpu(rfc.max_pdu_size);
2522
2523                         pi->conf_state |= L2CAP_CONF_MODE_DONE;
2524
2525                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2526                                         sizeof(rfc), (unsigned long) &rfc);
2527
2528                         break;
2529
2530                 default:
2531                         result = L2CAP_CONF_UNACCEPT;
2532
2533                         memset(&rfc, 0, sizeof(rfc));
2534                         rfc.mode = pi->mode;
2535                 }
2536
2537                 if (result == L2CAP_CONF_SUCCESS)
2538                         pi->conf_state |= L2CAP_CONF_OUTPUT_DONE;
2539         }
2540         rsp->scid   = cpu_to_le16(pi->dcid);
2541         rsp->result = cpu_to_le16(result);
2542         rsp->flags  = cpu_to_le16(0x0000);
2543
2544         return ptr - data;
2545 }
2546
2547 static int l2cap_parse_conf_rsp(struct sock *sk, void *rsp, int len, void *data, u16 *result)
2548 {
2549         struct l2cap_pinfo *pi = l2cap_pi(sk);
2550         struct l2cap_conf_req *req = data;
2551         void *ptr = req->data;
2552         int type, olen;
2553         unsigned long val;
2554         struct l2cap_conf_rfc rfc;
2555
2556         BT_DBG("sk %p, rsp %p, len %d, req %p", sk, rsp, len, data);
2557
2558         while (len >= L2CAP_CONF_OPT_SIZE) {
2559                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
2560
2561                 switch (type) {
2562                 case L2CAP_CONF_MTU:
2563                         if (val < L2CAP_DEFAULT_MIN_MTU) {
2564                                 *result = L2CAP_CONF_UNACCEPT;
2565                                 pi->omtu = L2CAP_DEFAULT_MIN_MTU;
2566                         } else
2567                                 pi->omtu = val;
2568                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->omtu);
2569                         break;
2570
2571                 case L2CAP_CONF_FLUSH_TO:
2572                         pi->flush_to = val;
2573                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
2574                                                         2, pi->flush_to);
2575                         break;
2576
2577                 case L2CAP_CONF_RFC:
2578                         if (olen == sizeof(rfc))
2579                                 memcpy(&rfc, (void *)val, olen);
2580
2581                         if ((pi->conf_state & L2CAP_CONF_STATE2_DEVICE) &&
2582                                                         rfc.mode != pi->mode)
2583                                 return -ECONNREFUSED;
2584
2585                         pi->mode = rfc.mode;
2586                         pi->fcs = 0;
2587
2588                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2589                                         sizeof(rfc), (unsigned long) &rfc);
2590                         break;
2591                 }
2592         }
2593
2594         if (*result == L2CAP_CONF_SUCCESS) {
2595                 switch (rfc.mode) {
2596                 case L2CAP_MODE_ERTM:
2597                         pi->remote_tx_win   = rfc.txwin_size;
2598                         pi->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
2599                         pi->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
2600                         pi->mps    = le16_to_cpu(rfc.max_pdu_size);
2601                         break;
2602                 case L2CAP_MODE_STREAMING:
2603                         pi->mps    = le16_to_cpu(rfc.max_pdu_size);
2604                 }
2605         }
2606
2607         req->dcid   = cpu_to_le16(pi->dcid);
2608         req->flags  = cpu_to_le16(0x0000);
2609
2610         return ptr - data;
2611 }
2612
2613 static int l2cap_build_conf_rsp(struct sock *sk, void *data, u16 result, u16 flags)
2614 {
2615         struct l2cap_conf_rsp *rsp = data;
2616         void *ptr = rsp->data;
2617
2618         BT_DBG("sk %p", sk);
2619
2620         rsp->scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
2621         rsp->result = cpu_to_le16(result);
2622         rsp->flags  = cpu_to_le16(flags);
2623
2624         return ptr - data;
2625 }
2626
2627 static void l2cap_conf_rfc_get(struct sock *sk, void *rsp, int len)
2628 {
2629         struct l2cap_pinfo *pi = l2cap_pi(sk);
2630         int type, olen;
2631         unsigned long val;
2632         struct l2cap_conf_rfc rfc;
2633
2634         BT_DBG("sk %p, rsp %p, len %d", sk, rsp, len);
2635
2636         if ((pi->mode != L2CAP_MODE_ERTM) && (pi->mode != L2CAP_MODE_STREAMING))
2637                 return;
2638
2639         while (len >= L2CAP_CONF_OPT_SIZE) {
2640                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
2641
2642                 switch (type) {
2643                 case L2CAP_CONF_RFC:
2644                         if (olen == sizeof(rfc))
2645                                 memcpy(&rfc, (void *)val, olen);
2646                         goto done;
2647                 }
2648         }
2649
2650 done:
2651         switch (rfc.mode) {
2652         case L2CAP_MODE_ERTM:
2653                 pi->remote_tx_win   = rfc.txwin_size;
2654                 pi->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
2655                 pi->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
2656                 pi->mps    = le16_to_cpu(rfc.max_pdu_size);
2657                 break;
2658         case L2CAP_MODE_STREAMING:
2659                 pi->mps    = le16_to_cpu(rfc.max_pdu_size);
2660         }
2661 }
2662
2663 static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2664 {
2665         struct l2cap_cmd_rej *rej = (struct l2cap_cmd_rej *) data;
2666
2667         if (rej->reason != 0x0000)
2668                 return 0;
2669
2670         if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
2671                                         cmd->ident == conn->info_ident) {
2672                 del_timer(&conn->info_timer);
2673
2674                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2675                 conn->info_ident = 0;
2676
2677                 l2cap_conn_start(conn);
2678         }
2679
2680         return 0;
2681 }
2682
2683 static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2684 {
2685         struct l2cap_chan_list *list = &conn->chan_list;
2686         struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
2687         struct l2cap_conn_rsp rsp;
2688         struct sock *sk, *parent;
2689         int result, status = L2CAP_CS_NO_INFO;
2690
2691         u16 dcid = 0, scid = __le16_to_cpu(req->scid);
2692         __le16 psm = req->psm;
2693
2694         BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid);
2695
2696         /* Check if we have socket listening on psm */
2697         parent = l2cap_get_sock_by_psm(BT_LISTEN, psm, conn->src);
2698         if (!parent) {
2699                 result = L2CAP_CR_BAD_PSM;
2700                 goto sendresp;
2701         }
2702
2703         /* Check if the ACL is secure enough (if not SDP) */
2704         if (psm != cpu_to_le16(0x0001) &&
2705                                 !hci_conn_check_link_mode(conn->hcon)) {
2706                 conn->disc_reason = 0x05;
2707                 result = L2CAP_CR_SEC_BLOCK;
2708                 goto response;
2709         }
2710
2711         result = L2CAP_CR_NO_MEM;
2712
2713         /* Check for backlog size */
2714         if (sk_acceptq_is_full(parent)) {
2715                 BT_DBG("backlog full %d", parent->sk_ack_backlog);
2716                 goto response;
2717         }
2718
2719         sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, GFP_ATOMIC);
2720         if (!sk)
2721                 goto response;
2722
2723         write_lock_bh(&list->lock);
2724
2725         /* Check if we already have channel with that dcid */
2726         if (__l2cap_get_chan_by_dcid(list, scid)) {
2727                 write_unlock_bh(&list->lock);
2728                 sock_set_flag(sk, SOCK_ZAPPED);
2729                 l2cap_sock_kill(sk);
2730                 goto response;
2731         }
2732
2733         hci_conn_hold(conn->hcon);
2734
2735         l2cap_sock_init(sk, parent);
2736         bacpy(&bt_sk(sk)->src, conn->src);
2737         bacpy(&bt_sk(sk)->dst, conn->dst);
2738         l2cap_pi(sk)->psm  = psm;
2739         l2cap_pi(sk)->dcid = scid;
2740
2741         __l2cap_chan_add(conn, sk, parent);
2742         dcid = l2cap_pi(sk)->scid;
2743
2744         l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
2745
2746         l2cap_pi(sk)->ident = cmd->ident;
2747
2748         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
2749                 if (l2cap_check_security(sk)) {
2750                         if (bt_sk(sk)->defer_setup) {
2751                                 sk->sk_state = BT_CONNECT2;
2752                                 result = L2CAP_CR_PEND;
2753                                 status = L2CAP_CS_AUTHOR_PEND;
2754                                 parent->sk_data_ready(parent, 0);
2755                         } else {
2756                                 sk->sk_state = BT_CONFIG;
2757                                 result = L2CAP_CR_SUCCESS;
2758                                 status = L2CAP_CS_NO_INFO;
2759                         }
2760                 } else {
2761                         sk->sk_state = BT_CONNECT2;
2762                         result = L2CAP_CR_PEND;
2763                         status = L2CAP_CS_AUTHEN_PEND;
2764                 }
2765         } else {
2766                 sk->sk_state = BT_CONNECT2;
2767                 result = L2CAP_CR_PEND;
2768                 status = L2CAP_CS_NO_INFO;
2769         }
2770
2771         write_unlock_bh(&list->lock);
2772
2773 response:
2774         bh_unlock_sock(parent);
2775
2776 sendresp:
2777         rsp.scid   = cpu_to_le16(scid);
2778         rsp.dcid   = cpu_to_le16(dcid);
2779         rsp.result = cpu_to_le16(result);
2780         rsp.status = cpu_to_le16(status);
2781         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
2782
2783         if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
2784                 struct l2cap_info_req info;
2785                 info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
2786
2787                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
2788                 conn->info_ident = l2cap_get_ident(conn);
2789
2790                 mod_timer(&conn->info_timer, jiffies +
2791                                         msecs_to_jiffies(L2CAP_INFO_TIMEOUT));
2792
2793                 l2cap_send_cmd(conn, conn->info_ident,
2794                                         L2CAP_INFO_REQ, sizeof(info), &info);
2795         }
2796
2797         return 0;
2798 }
2799
2800 static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2801 {
2802         struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
2803         u16 scid, dcid, result, status;
2804         struct sock *sk;
2805         u8 req[128];
2806
2807         scid   = __le16_to_cpu(rsp->scid);
2808         dcid   = __le16_to_cpu(rsp->dcid);
2809         result = __le16_to_cpu(rsp->result);
2810         status = __le16_to_cpu(rsp->status);
2811
2812         BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status);
2813
2814         if (scid) {
2815                 sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
2816                 if (!sk)
2817                         return 0;
2818         } else {
2819                 sk = l2cap_get_chan_by_ident(&conn->chan_list, cmd->ident);
2820                 if (!sk)
2821                         return 0;
2822         }
2823
2824         switch (result) {
2825         case L2CAP_CR_SUCCESS:
2826                 sk->sk_state = BT_CONFIG;
2827                 l2cap_pi(sk)->ident = 0;
2828                 l2cap_pi(sk)->dcid = dcid;
2829                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
2830
2831                 l2cap_pi(sk)->conf_state &= ~L2CAP_CONF_CONNECT_PEND;
2832
2833                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2834                                         l2cap_build_conf_req(sk, req), req);
2835                 l2cap_pi(sk)->num_conf_req++;
2836                 break;
2837
2838         case L2CAP_CR_PEND:
2839                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND;
2840                 break;
2841
2842         default:
2843                 l2cap_chan_del(sk, ECONNREFUSED);
2844                 break;
2845         }
2846
2847         bh_unlock_sock(sk);
2848         return 0;
2849 }
2850
2851 static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
2852 {
2853         struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
2854         u16 dcid, flags;
2855         u8 rsp[64];
2856         struct sock *sk;
2857         int len;
2858
2859         dcid  = __le16_to_cpu(req->dcid);
2860         flags = __le16_to_cpu(req->flags);
2861
2862         BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
2863
2864         sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid);
2865         if (!sk)
2866                 return -ENOENT;
2867
2868         if (sk->sk_state == BT_DISCONN)
2869                 goto unlock;
2870
2871         /* Reject if config buffer is too small. */
2872         len = cmd_len - sizeof(*req);
2873         if (l2cap_pi(sk)->conf_len + len > sizeof(l2cap_pi(sk)->conf_req)) {
2874                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2875                                 l2cap_build_conf_rsp(sk, rsp,
2876                                         L2CAP_CONF_REJECT, flags), rsp);
2877                 goto unlock;
2878         }
2879
2880         /* Store config. */
2881         memcpy(l2cap_pi(sk)->conf_req + l2cap_pi(sk)->conf_len, req->data, len);
2882         l2cap_pi(sk)->conf_len += len;
2883
2884         if (flags & 0x0001) {
2885                 /* Incomplete config. Send empty response. */
2886                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2887                                 l2cap_build_conf_rsp(sk, rsp,
2888                                         L2CAP_CONF_SUCCESS, 0x0001), rsp);
2889                 goto unlock;
2890         }
2891
2892         /* Complete config. */
2893         len = l2cap_parse_conf_req(sk, rsp);
2894         if (len < 0) {
2895                 l2cap_send_disconn_req(conn, sk);
2896                 goto unlock;
2897         }
2898
2899         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
2900         l2cap_pi(sk)->num_conf_rsp++;
2901
2902         /* Reset config buffer. */
2903         l2cap_pi(sk)->conf_len = 0;
2904
2905         if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE))
2906                 goto unlock;
2907
2908         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_INPUT_DONE) {
2909                 if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_NO_FCS_RECV) ||
2910                     l2cap_pi(sk)->fcs != L2CAP_FCS_NONE)
2911                         l2cap_pi(sk)->fcs = L2CAP_FCS_CRC16;
2912
2913                 sk->sk_state = BT_CONNECTED;
2914
2915                 l2cap_pi(sk)->next_tx_seq = 0;
2916                 l2cap_pi(sk)->expected_tx_seq = 0;
2917                 __skb_queue_head_init(TX_QUEUE(sk));
2918                 if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM)
2919                         l2cap_ertm_init(sk);
2920
2921                 l2cap_chan_ready(sk);
2922                 goto unlock;
2923         }
2924
2925         if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT)) {
2926                 u8 buf[64];
2927                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2928                                         l2cap_build_conf_req(sk, buf), buf);
2929                 l2cap_pi(sk)->num_conf_req++;
2930         }
2931
2932 unlock:
2933         bh_unlock_sock(sk);
2934         return 0;
2935 }
2936
2937 static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2938 {
2939         struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
2940         u16 scid, flags, result;
2941         struct sock *sk;
2942         int len = cmd->len - sizeof(*rsp);
2943
2944         scid   = __le16_to_cpu(rsp->scid);
2945         flags  = __le16_to_cpu(rsp->flags);
2946         result = __le16_to_cpu(rsp->result);
2947
2948         BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x",
2949                         scid, flags, result);
2950
2951         sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
2952         if (!sk)
2953                 return 0;
2954
2955         switch (result) {
2956         case L2CAP_CONF_SUCCESS:
2957                 l2cap_conf_rfc_get(sk, rsp->data, len);
2958                 break;
2959
2960         case L2CAP_CONF_UNACCEPT:
2961                 if (l2cap_pi(sk)->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
2962                         char req[64];
2963
2964                         if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
2965                                 l2cap_send_disconn_req(conn, sk);
2966                                 goto done;
2967                         }
2968
2969                         /* throw out any old stored conf requests */
2970                         result = L2CAP_CONF_SUCCESS;
2971                         len = l2cap_parse_conf_rsp(sk, rsp->data,
2972                                                         len, req, &result);
2973                         if (len < 0) {
2974                                 l2cap_send_disconn_req(conn, sk);
2975                                 goto done;
2976                         }
2977
2978                         l2cap_send_cmd(conn, l2cap_get_ident(conn),
2979                                                 L2CAP_CONF_REQ, len, req);
2980                         l2cap_pi(sk)->num_conf_req++;
2981                         if (result != L2CAP_CONF_SUCCESS)
2982                                 goto done;
2983                         break;
2984                 }
2985
2986         default:
2987                 sk->sk_state = BT_DISCONN;
2988                 sk->sk_err = ECONNRESET;
2989                 l2cap_sock_set_timer(sk, HZ * 5);
2990                 l2cap_send_disconn_req(conn, sk);
2991                 goto done;
2992         }
2993
2994         if (flags & 0x01)
2995                 goto done;
2996
2997         l2cap_pi(sk)->conf_state |= L2CAP_CONF_INPUT_DONE;
2998
2999         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE) {
3000                 if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_NO_FCS_RECV) ||
3001                     l2cap_pi(sk)->fcs != L2CAP_FCS_NONE)
3002                         l2cap_pi(sk)->fcs = L2CAP_FCS_CRC16;
3003
3004                 sk->sk_state = BT_CONNECTED;
3005                 l2cap_pi(sk)->next_tx_seq = 0;
3006                 l2cap_pi(sk)->expected_tx_seq = 0;
3007                 __skb_queue_head_init(TX_QUEUE(sk));
3008                 if (l2cap_pi(sk)->mode ==  L2CAP_MODE_ERTM)
3009                         l2cap_ertm_init(sk);
3010
3011                 l2cap_chan_ready(sk);
3012         }
3013
3014 done:
3015         bh_unlock_sock(sk);
3016         return 0;
3017 }
3018
3019 static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3020 {
3021         struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
3022         struct l2cap_disconn_rsp rsp;
3023         u16 dcid, scid;
3024         struct sock *sk;
3025
3026         scid = __le16_to_cpu(req->scid);
3027         dcid = __le16_to_cpu(req->dcid);
3028
3029         BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
3030
3031         sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid);
3032         if (!sk)
3033                 return 0;
3034
3035         rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid);
3036         rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid);
3037         l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
3038
3039         sk->sk_shutdown = SHUTDOWN_MASK;
3040
3041         skb_queue_purge(TX_QUEUE(sk));
3042
3043         if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM) {
3044                 skb_queue_purge(SREJ_QUEUE(sk));
3045                 del_timer(&l2cap_pi(sk)->retrans_timer);
3046                 del_timer(&l2cap_pi(sk)->monitor_timer);
3047                 del_timer(&l2cap_pi(sk)->ack_timer);
3048         }
3049
3050         l2cap_chan_del(sk, ECONNRESET);
3051         bh_unlock_sock(sk);
3052
3053         l2cap_sock_kill(sk);
3054         return 0;
3055 }
3056
3057 static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3058 {
3059         struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
3060         u16 dcid, scid;
3061         struct sock *sk;
3062
3063         scid = __le16_to_cpu(rsp->scid);
3064         dcid = __le16_to_cpu(rsp->dcid);
3065
3066         BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
3067
3068         sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
3069         if (!sk)
3070                 return 0;
3071
3072         skb_queue_purge(TX_QUEUE(sk));
3073
3074         if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM) {
3075                 skb_queue_purge(SREJ_QUEUE(sk));
3076                 del_timer(&l2cap_pi(sk)->retrans_timer);
3077                 del_timer(&l2cap_pi(sk)->monitor_timer);
3078                 del_timer(&l2cap_pi(sk)->ack_timer);
3079         }
3080
3081         l2cap_chan_del(sk, 0);
3082         bh_unlock_sock(sk);
3083
3084         l2cap_sock_kill(sk);
3085         return 0;
3086 }
3087
3088 static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3089 {
3090         struct l2cap_info_req *req = (struct l2cap_info_req *) data;
3091         u16 type;
3092
3093         type = __le16_to_cpu(req->type);
3094
3095         BT_DBG("type 0x%4.4x", type);
3096
3097         if (type == L2CAP_IT_FEAT_MASK) {
3098                 u8 buf[8];
3099                 u32 feat_mask = l2cap_feat_mask;
3100                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
3101                 rsp->type   = cpu_to_le16(L2CAP_IT_FEAT_MASK);
3102                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
3103                 if (enable_ertm)
3104                         feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
3105                                                          | L2CAP_FEAT_FCS;
3106                 put_unaligned_le32(feat_mask, rsp->data);
3107                 l2cap_send_cmd(conn, cmd->ident,
3108                                         L2CAP_INFO_RSP, sizeof(buf), buf);
3109         } else if (type == L2CAP_IT_FIXED_CHAN) {
3110                 u8 buf[12];
3111                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
3112                 rsp->type   = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
3113                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
3114                 memcpy(buf + 4, l2cap_fixed_chan, 8);
3115                 l2cap_send_cmd(conn, cmd->ident,
3116                                         L2CAP_INFO_RSP, sizeof(buf), buf);
3117         } else {
3118                 struct l2cap_info_rsp rsp;
3119                 rsp.type   = cpu_to_le16(type);
3120                 rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP);
3121                 l2cap_send_cmd(conn, cmd->ident,
3122                                         L2CAP_INFO_RSP, sizeof(rsp), &rsp);
3123         }
3124
3125         return 0;
3126 }
3127
3128 static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3129 {
3130         struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
3131         u16 type, result;
3132
3133         type   = __le16_to_cpu(rsp->type);
3134         result = __le16_to_cpu(rsp->result);
3135
3136         BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
3137
3138         del_timer(&conn->info_timer);
3139
3140         if (type == L2CAP_IT_FEAT_MASK) {
3141                 conn->feat_mask = get_unaligned_le32(rsp->data);
3142
3143                 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
3144                         struct l2cap_info_req req;
3145                         req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
3146
3147                         conn->info_ident = l2cap_get_ident(conn);
3148
3149                         l2cap_send_cmd(conn, conn->info_ident,
3150                                         L2CAP_INFO_REQ, sizeof(req), &req);
3151                 } else {
3152                         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3153                         conn->info_ident = 0;
3154
3155                         l2cap_conn_start(conn);
3156                 }
3157         } else if (type == L2CAP_IT_FIXED_CHAN) {
3158                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3159                 conn->info_ident = 0;
3160
3161                 l2cap_conn_start(conn);
3162         }
3163
3164         return 0;
3165 }
3166
3167 static inline void l2cap_sig_channel(struct l2cap_conn *conn, struct sk_buff *skb)
3168 {
3169         u8 *data = skb->data;
3170         int len = skb->len;
3171         struct l2cap_cmd_hdr cmd;
3172         int err = 0;
3173
3174         l2cap_raw_recv(conn, skb);
3175
3176         while (len >= L2CAP_CMD_HDR_SIZE) {
3177                 u16 cmd_len;
3178                 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
3179                 data += L2CAP_CMD_HDR_SIZE;
3180                 len  -= L2CAP_CMD_HDR_SIZE;
3181
3182                 cmd_len = le16_to_cpu(cmd.len);
3183
3184                 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, cmd.ident);
3185
3186                 if (cmd_len > len || !cmd.ident) {
3187                         BT_DBG("corrupted command");
3188                         break;
3189                 }
3190
3191                 switch (cmd.code) {
3192                 case L2CAP_COMMAND_REJ:
3193                         l2cap_command_rej(conn, &cmd, data);
3194                         break;
3195
3196                 case L2CAP_CONN_REQ:
3197                         err = l2cap_connect_req(conn, &cmd, data);
3198                         break;
3199
3200                 case L2CAP_CONN_RSP:
3201                         err = l2cap_connect_rsp(conn, &cmd, data);
3202                         break;
3203
3204                 case L2CAP_CONF_REQ:
3205                         err = l2cap_config_req(conn, &cmd, cmd_len, data);
3206                         break;
3207
3208                 case L2CAP_CONF_RSP:
3209                         err = l2cap_config_rsp(conn, &cmd, data);
3210                         break;
3211
3212                 case L2CAP_DISCONN_REQ:
3213                         err = l2cap_disconnect_req(conn, &cmd, data);
3214                         break;
3215
3216                 case L2CAP_DISCONN_RSP:
3217                         err = l2cap_disconnect_rsp(conn, &cmd, data);
3218                         break;
3219
3220                 case L2CAP_ECHO_REQ:
3221                         l2cap_send_cmd(conn, cmd.ident, L2CAP_ECHO_RSP, cmd_len, data);
3222                         break;
3223
3224                 case L2CAP_ECHO_RSP:
3225                         break;
3226
3227                 case L2CAP_INFO_REQ:
3228                         err = l2cap_information_req(conn, &cmd, data);
3229                         break;
3230
3231                 case L2CAP_INFO_RSP:
3232                         err = l2cap_information_rsp(conn, &cmd, data);
3233                         break;
3234
3235                 default:
3236                         BT_ERR("Unknown signaling command 0x%2.2x", cmd.code);
3237                         err = -EINVAL;
3238                         break;
3239                 }
3240
3241                 if (err) {
3242                         struct l2cap_cmd_rej rej;
3243                         BT_DBG("error %d", err);
3244
3245                         /* FIXME: Map err to a valid reason */
3246                         rej.reason = cpu_to_le16(0);
3247                         l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
3248                 }
3249
3250                 data += cmd_len;
3251                 len  -= cmd_len;
3252         }
3253
3254         kfree_skb(skb);
3255 }
3256
3257 static int l2cap_check_fcs(struct l2cap_pinfo *pi,  struct sk_buff *skb)
3258 {
3259         u16 our_fcs, rcv_fcs;
3260         int hdr_size = L2CAP_HDR_SIZE + 2;
3261
3262         if (pi->fcs == L2CAP_FCS_CRC16) {
3263                 skb_trim(skb, skb->len - 2);
3264                 rcv_fcs = get_unaligned_le16(skb->data + skb->len);
3265                 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
3266
3267                 if (our_fcs != rcv_fcs)
3268                         return -EINVAL;
3269         }
3270         return 0;
3271 }
3272
3273 static inline void l2cap_send_i_or_rr_or_rnr(struct sock *sk)
3274 {
3275         struct l2cap_pinfo *pi = l2cap_pi(sk);
3276         u16 control = 0;
3277
3278         pi->frames_sent = 0;
3279         pi->conn_state |= L2CAP_CONN_SEND_FBIT;
3280
3281         control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3282
3283         if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
3284                 control |= L2CAP_SUPER_RCV_NOT_READY | L2CAP_CTRL_FINAL;
3285                 l2cap_send_sframe(pi, control);
3286                 pi->conn_state &= ~L2CAP_CONN_SEND_FBIT;
3287         }
3288
3289         if (pi->conn_state & L2CAP_CONN_REMOTE_BUSY && pi->unacked_frames > 0)
3290                 __mod_retrans_timer();
3291
3292         l2cap_ertm_send(sk);
3293
3294         if (!(pi->conn_state & L2CAP_CONN_LOCAL_BUSY) &&
3295                         pi->frames_sent == 0) {
3296                 control |= L2CAP_SUPER_RCV_READY;
3297                 l2cap_send_sframe(pi, control);
3298         }
3299 }
3300
3301 static void l2cap_add_to_srej_queue(struct sock *sk, struct sk_buff *skb, u8 tx_seq, u8 sar)
3302 {
3303         struct sk_buff *next_skb;
3304
3305         bt_cb(skb)->tx_seq = tx_seq;
3306         bt_cb(skb)->sar = sar;
3307
3308         next_skb = skb_peek(SREJ_QUEUE(sk));
3309         if (!next_skb) {
3310                 __skb_queue_tail(SREJ_QUEUE(sk), skb);
3311                 return;
3312         }
3313
3314         do {
3315                 if (bt_cb(next_skb)->tx_seq > tx_seq) {
3316                         __skb_queue_before(SREJ_QUEUE(sk), next_skb, skb);
3317                         return;
3318                 }
3319
3320                 if (skb_queue_is_last(SREJ_QUEUE(sk), next_skb))
3321                         break;
3322
3323         } while((next_skb = skb_queue_next(SREJ_QUEUE(sk), next_skb)));
3324
3325         __skb_queue_tail(SREJ_QUEUE(sk), skb);
3326 }
3327
3328 static int l2cap_sar_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)
3329 {
3330         struct l2cap_pinfo *pi = l2cap_pi(sk);
3331         struct sk_buff *_skb;
3332         int err = -EINVAL;
3333
3334         switch (control & L2CAP_CTRL_SAR) {
3335         case L2CAP_SDU_UNSEGMENTED:
3336                 if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
3337                         kfree_skb(pi->sdu);
3338                         break;
3339                 }
3340
3341                 err = sock_queue_rcv_skb(sk, skb);
3342                 if (!err)
3343                         return 0;
3344
3345                 break;
3346
3347         case L2CAP_SDU_START:
3348                 if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
3349                         kfree_skb(pi->sdu);
3350                         break;
3351                 }
3352
3353                 pi->sdu_len = get_unaligned_le16(skb->data);
3354                 skb_pull(skb, 2);
3355
3356                 if (pi->sdu_len > pi->imtu) {
3357                         err = -EMSGSIZE;
3358                         break;
3359                 }
3360
3361                 pi->sdu = bt_skb_alloc(pi->sdu_len, GFP_ATOMIC);
3362                 if (!pi->sdu) {
3363                         err = -ENOMEM;
3364                         break;
3365                 }
3366
3367                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3368
3369                 pi->conn_state |= L2CAP_CONN_SAR_SDU;
3370                 pi->partial_sdu_len = skb->len;
3371                 err = 0;
3372                 break;
3373
3374         case L2CAP_SDU_CONTINUE:
3375                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
3376                         break;
3377
3378                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3379
3380                 pi->partial_sdu_len += skb->len;
3381                 if (pi->partial_sdu_len > pi->sdu_len)
3382                         kfree_skb(pi->sdu);
3383                 else
3384                         err = 0;
3385
3386                 break;
3387
3388         case L2CAP_SDU_END:
3389                 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
3390                         break;
3391
3392                 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3393
3394                 pi->conn_state &= ~L2CAP_CONN_SAR_SDU;
3395                 pi->partial_sdu_len += skb->len;
3396
3397                 if (pi->partial_sdu_len > pi->imtu)
3398                         goto drop;
3399
3400                 if (pi->partial_sdu_len == pi->sdu_len) {
3401                         _skb = skb_clone(pi->sdu, GFP_ATOMIC);
3402                         err = sock_queue_rcv_skb(sk, _skb);
3403                         if (err < 0)
3404                                 kfree_skb(_skb);
3405                 }
3406                 err = 0;
3407
3408 drop:
3409                 kfree_skb(pi->sdu);
3410                 break;
3411         }
3412
3413         kfree_skb(skb);
3414         return err;
3415 }
3416
3417 static void l2cap_check_srej_gap(struct sock *sk, u8 tx_seq)
3418 {
3419         struct sk_buff *skb;
3420         u16 control = 0;
3421
3422         while((skb = skb_peek(SREJ_QUEUE(sk)))) {
3423                 if (bt_cb(skb)->tx_seq != tx_seq)
3424                         break;
3425
3426                 skb = skb_dequeue(SREJ_QUEUE(sk));
3427                 control |= bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
3428                 l2cap_sar_reassembly_sdu(sk, skb, control);
3429                 l2cap_pi(sk)->buffer_seq_srej =
3430                         (l2cap_pi(sk)->buffer_seq_srej + 1) % 64;
3431                 tx_seq++;
3432         }
3433 }
3434
3435 static void l2cap_resend_srejframe(struct sock *sk, u8 tx_seq)
3436 {
3437         struct l2cap_pinfo *pi = l2cap_pi(sk);
3438         struct srej_list *l, *tmp;
3439         u16 control;
3440
3441         list_for_each_entry_safe(l,tmp, SREJ_LIST(sk), list) {
3442                 if (l->tx_seq == tx_seq) {
3443                         list_del(&l->list);
3444                         kfree(l);
3445                         return;
3446                 }
3447                 control = L2CAP_SUPER_SELECT_REJECT;
3448                 control |= l->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3449                 l2cap_send_sframe(pi, control);
3450                 list_del(&l->list);
3451                 list_add_tail(&l->list, SREJ_LIST(sk));
3452         }
3453 }
3454
3455 static void l2cap_send_srejframe(struct sock *sk, u8 tx_seq)
3456 {
3457         struct l2cap_pinfo *pi = l2cap_pi(sk);
3458         struct srej_list *new;
3459         u16 control;
3460
3461         while (tx_seq != pi->expected_tx_seq) {
3462                 control = L2CAP_SUPER_SELECT_REJECT;
3463                 control |= pi->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3464                 l2cap_send_sframe(pi, control);
3465
3466                 new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC);
3467                 new->tx_seq = pi->expected_tx_seq++;
3468                 list_add_tail(&new->list, SREJ_LIST(sk));
3469         }
3470         pi->expected_tx_seq++;
3471 }
3472
3473 static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
3474 {
3475         struct l2cap_pinfo *pi = l2cap_pi(sk);
3476         u8 tx_seq = __get_txseq(rx_control);
3477         u8 req_seq = __get_reqseq(rx_control);
3478         u8 sar = rx_control >> L2CAP_CTRL_SAR_SHIFT;
3479         int num_to_ack = (pi->tx_win/6) + 1;
3480         int err = 0;
3481
3482         BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
3483
3484         if (L2CAP_CTRL_FINAL & rx_control) {
3485                 del_timer(&pi->monitor_timer);
3486                 if (pi->unacked_frames > 0)
3487                         __mod_retrans_timer();
3488                 pi->conn_state &= ~L2CAP_CONN_WAIT_F;
3489         }
3490
3491         pi->expected_ack_seq = req_seq;
3492         l2cap_drop_acked_frames(sk);
3493
3494         if (tx_seq == pi->expected_tx_seq)
3495                 goto expected;
3496
3497         if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
3498                 struct srej_list *first;
3499
3500                 first = list_first_entry(SREJ_LIST(sk),
3501                                 struct srej_list, list);
3502                 if (tx_seq == first->tx_seq) {
3503                         l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
3504                         l2cap_check_srej_gap(sk, tx_seq);
3505
3506                         list_del(&first->list);
3507                         kfree(first);
3508
3509                         if (list_empty(SREJ_LIST(sk))) {
3510                                 pi->buffer_seq = pi->buffer_seq_srej;
3511                                 pi->conn_state &= ~L2CAP_CONN_SREJ_SENT;
3512                                 l2cap_send_ack(pi);
3513                         }
3514                 } else {
3515                         struct srej_list *l;
3516                         l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
3517
3518                         list_for_each_entry(l, SREJ_LIST(sk), list) {
3519                                 if (l->tx_seq == tx_seq) {
3520                                         l2cap_resend_srejframe(sk, tx_seq);
3521                                         return 0;
3522                                 }
3523                         }
3524                         l2cap_send_srejframe(sk, tx_seq);
3525                 }
3526         } else {
3527                 pi->conn_state |= L2CAP_CONN_SREJ_SENT;
3528
3529                 INIT_LIST_HEAD(SREJ_LIST(sk));
3530                 pi->buffer_seq_srej = pi->buffer_seq;
3531
3532                 __skb_queue_head_init(SREJ_QUEUE(sk));
3533                 l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
3534
3535                 pi->conn_state |= L2CAP_CONN_SEND_PBIT;
3536
3537                 l2cap_send_srejframe(sk, tx_seq);
3538         }
3539         return 0;
3540
3541 expected:
3542         pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3543
3544         if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
3545                 l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
3546                 return 0;
3547         }
3548
3549         if (rx_control & L2CAP_CTRL_FINAL) {
3550                 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
3551                         pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
3552                 else {
3553                         if (!skb_queue_empty(TX_QUEUE(sk)))
3554                                 sk->sk_send_head = TX_QUEUE(sk)->next;
3555                         pi->next_tx_seq = pi->expected_ack_seq;
3556                         l2cap_ertm_send(sk);
3557                 }
3558         }
3559
3560         pi->buffer_seq = (pi->buffer_seq + 1) % 64;
3561
3562         err = l2cap_sar_reassembly_sdu(sk, skb, rx_control);
3563         if (err < 0)
3564                 return err;
3565
3566         __mod_ack_timer();
3567
3568         pi->num_acked = (pi->num_acked + 1) % num_to_ack;
3569         if (pi->num_acked == num_to_ack - 1)
3570                 l2cap_send_ack(pi);
3571
3572         return 0;
3573 }
3574
3575 static inline void l2cap_data_channel_rrframe(struct sock *sk, u16 rx_control)
3576 {
3577         struct l2cap_pinfo *pi = l2cap_pi(sk);
3578
3579         pi->expected_ack_seq = __get_reqseq(rx_control);
3580         l2cap_drop_acked_frames(sk);
3581
3582         if (rx_control & L2CAP_CTRL_POLL) {
3583                 if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
3584                         if ((pi->conn_state & L2CAP_CONN_REMOTE_BUSY) &&
3585                                         (pi->unacked_frames > 0))
3586                                 __mod_retrans_timer();
3587
3588                         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3589                         l2cap_send_srejtail(sk);
3590                 } else {
3591                         l2cap_send_i_or_rr_or_rnr(sk);
3592                         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3593                 }
3594
3595         } else if (rx_control & L2CAP_CTRL_FINAL) {
3596                 pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3597
3598                 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
3599                         pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
3600                 else {
3601                         if (!skb_queue_empty(TX_QUEUE(sk)))
3602                                 sk->sk_send_head = TX_QUEUE(sk)->next;
3603                         pi->next_tx_seq = pi->expected_ack_seq;
3604                         l2cap_ertm_send(sk);
3605                 }
3606
3607         } else {
3608                 if ((pi->conn_state & L2CAP_CONN_REMOTE_BUSY) &&
3609                                 (pi->unacked_frames > 0))
3610                         __mod_retrans_timer();
3611
3612                 pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3613                 if (pi->conn_state & L2CAP_CONN_SREJ_SENT)
3614                         l2cap_send_ack(pi);
3615                 else
3616                         l2cap_ertm_send(sk);
3617         }
3618 }
3619
3620 static inline void l2cap_data_channel_rejframe(struct sock *sk, u16 rx_control)
3621 {
3622         struct l2cap_pinfo *pi = l2cap_pi(sk);
3623         u8 tx_seq = __get_reqseq(rx_control);
3624
3625         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3626
3627         pi->expected_ack_seq = tx_seq;
3628         l2cap_drop_acked_frames(sk);
3629
3630         if (rx_control & L2CAP_CTRL_FINAL) {
3631                 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
3632                         pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
3633                 else {
3634                         if (!skb_queue_empty(TX_QUEUE(sk)))
3635                                 sk->sk_send_head = TX_QUEUE(sk)->next;
3636                         pi->next_tx_seq = pi->expected_ack_seq;
3637                         l2cap_ertm_send(sk);
3638                 }
3639         } else {
3640                 if (!skb_queue_empty(TX_QUEUE(sk)))
3641                         sk->sk_send_head = TX_QUEUE(sk)->next;
3642                 pi->next_tx_seq = pi->expected_ack_seq;
3643                 l2cap_ertm_send(sk);
3644
3645                 if (pi->conn_state & L2CAP_CONN_WAIT_F) {
3646                         pi->srej_save_reqseq = tx_seq;
3647                         pi->conn_state |= L2CAP_CONN_REJ_ACT;
3648                 }
3649         }
3650 }
3651 static inline void l2cap_data_channel_srejframe(struct sock *sk, u16 rx_control)
3652 {
3653         struct l2cap_pinfo *pi = l2cap_pi(sk);
3654         u8 tx_seq = __get_reqseq(rx_control);
3655
3656         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3657
3658         if (rx_control & L2CAP_CTRL_POLL) {
3659                 pi->expected_ack_seq = tx_seq;
3660                 l2cap_drop_acked_frames(sk);
3661                 l2cap_retransmit_frame(sk, tx_seq);
3662                 l2cap_ertm_send(sk);
3663                 if (pi->conn_state & L2CAP_CONN_WAIT_F) {
3664                         pi->srej_save_reqseq = tx_seq;
3665                         pi->conn_state |= L2CAP_CONN_SREJ_ACT;
3666                 }
3667         } else if (rx_control & L2CAP_CTRL_FINAL) {
3668                 if ((pi->conn_state & L2CAP_CONN_SREJ_ACT) &&
3669                                 pi->srej_save_reqseq == tx_seq)
3670                         pi->conn_state &= ~L2CAP_CONN_SREJ_ACT;
3671                 else
3672                         l2cap_retransmit_frame(sk, tx_seq);
3673         } else {
3674                 l2cap_retransmit_frame(sk, tx_seq);
3675                 if (pi->conn_state & L2CAP_CONN_WAIT_F) {
3676                         pi->srej_save_reqseq = tx_seq;
3677                         pi->conn_state |= L2CAP_CONN_SREJ_ACT;
3678                 }
3679         }
3680 }
3681
3682 static inline void l2cap_data_channel_rnrframe(struct sock *sk, u16 rx_control)
3683 {
3684         struct l2cap_pinfo *pi = l2cap_pi(sk);
3685         u8 tx_seq = __get_reqseq(rx_control);
3686
3687         pi->conn_state |= L2CAP_CONN_REMOTE_BUSY;
3688         pi->expected_ack_seq = tx_seq;
3689         l2cap_drop_acked_frames(sk);
3690
3691         if (!(pi->conn_state & L2CAP_CONN_SREJ_SENT)) {
3692                 del_timer(&pi->retrans_timer);
3693                 if (rx_control & L2CAP_CTRL_POLL) {
3694                         u16 control = L2CAP_CTRL_FINAL;
3695                         l2cap_send_rr_or_rnr(pi, control);
3696                 }
3697                 return;
3698         }
3699
3700         if (rx_control & L2CAP_CTRL_POLL)
3701                 l2cap_send_srejtail(sk);
3702         else
3703                 l2cap_send_sframe(pi, L2CAP_SUPER_RCV_READY);
3704 }
3705
3706 static inline int l2cap_data_channel_sframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
3707 {
3708         BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
3709
3710         if (L2CAP_CTRL_FINAL & rx_control) {
3711                 del_timer(&l2cap_pi(sk)->monitor_timer);
3712                 if (l2cap_pi(sk)->unacked_frames > 0)
3713                         __mod_retrans_timer();
3714                 l2cap_pi(sk)->conn_state &= ~L2CAP_CONN_WAIT_F;
3715         }
3716
3717         switch (rx_control & L2CAP_CTRL_SUPERVISE) {
3718         case L2CAP_SUPER_RCV_READY:
3719                 l2cap_data_channel_rrframe(sk, rx_control);
3720                 break;
3721
3722         case L2CAP_SUPER_REJECT:
3723                 l2cap_data_channel_rejframe(sk, rx_control);
3724                 break;
3725
3726         case L2CAP_SUPER_SELECT_REJECT:
3727                 l2cap_data_channel_srejframe(sk, rx_control);
3728                 break;
3729
3730         case L2CAP_SUPER_RCV_NOT_READY:
3731                 l2cap_data_channel_rnrframe(sk, rx_control);
3732                 break;
3733         }
3734
3735         kfree_skb(skb);
3736         return 0;
3737 }
3738
3739 static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
3740 {
3741         struct sock *sk;
3742         struct l2cap_pinfo *pi;
3743         u16 control, len;
3744         u8 tx_seq;
3745
3746         sk = l2cap_get_chan_by_scid(&conn->chan_list, cid);
3747         if (!sk) {
3748                 BT_DBG("unknown cid 0x%4.4x", cid);
3749                 goto drop;
3750         }
3751
3752         pi = l2cap_pi(sk);
3753
3754         BT_DBG("sk %p, len %d", sk, skb->len);
3755
3756         if (sk->sk_state != BT_CONNECTED)
3757                 goto drop;
3758
3759         switch (pi->mode) {
3760         case L2CAP_MODE_BASIC:
3761                 /* If socket recv buffers overflows we drop data here
3762                  * which is *bad* because L2CAP has to be reliable.
3763                  * But we don't have any other choice. L2CAP doesn't
3764                  * provide flow control mechanism. */
3765
3766                 if (pi->imtu < skb->len)
3767                         goto drop;
3768
3769                 if (!sock_queue_rcv_skb(sk, skb))
3770                         goto done;
3771                 break;
3772
3773         case L2CAP_MODE_ERTM:
3774                 control = get_unaligned_le16(skb->data);
3775                 skb_pull(skb, 2);
3776                 len = skb->len;
3777
3778                 if (__is_sar_start(control))
3779                         len -= 2;
3780
3781                 if (pi->fcs == L2CAP_FCS_CRC16)
3782                         len -= 2;
3783
3784                 /*
3785                  * We can just drop the corrupted I-frame here.
3786                  * Receiver will miss it and start proper recovery
3787                  * procedures and ask retransmission.
3788                  */
3789                 if (len > pi->mps)
3790                         goto drop;
3791
3792                 if (l2cap_check_fcs(pi, skb))
3793                         goto drop;
3794
3795                 if (__is_iframe(control)) {
3796                         if (len < 4)
3797                                 goto drop;
3798
3799                         l2cap_data_channel_iframe(sk, control, skb);
3800                 } else {
3801                         if (len != 0)
3802                                 goto drop;
3803
3804                         l2cap_data_channel_sframe(sk, control, skb);
3805                 }
3806
3807                 goto done;
3808
3809         case L2CAP_MODE_STREAMING:
3810                 control = get_unaligned_le16(skb->data);
3811                 skb_pull(skb, 2);
3812                 len = skb->len;
3813
3814                 if (__is_sar_start(control))
3815                         len -= 2;
3816
3817                 if (pi->fcs == L2CAP_FCS_CRC16)
3818                         len -= 2;
3819
3820                 if (len > pi->mps || len < 4 || __is_sframe(control))
3821                         goto drop;
3822
3823                 if (l2cap_check_fcs(pi, skb))
3824                         goto drop;
3825
3826                 tx_seq = __get_txseq(control);
3827
3828                 if (pi->expected_tx_seq == tx_seq)
3829                         pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3830                 else
3831                         pi->expected_tx_seq = (tx_seq + 1) % 64;
3832
3833                 l2cap_sar_reassembly_sdu(sk, skb, control);
3834
3835                 goto done;
3836
3837         default:
3838                 BT_DBG("sk %p: bad mode 0x%2.2x", sk, pi->mode);
3839                 break;
3840         }
3841
3842 drop:
3843         kfree_skb(skb);
3844
3845 done:
3846         if (sk)
3847                 bh_unlock_sock(sk);
3848
3849         return 0;
3850 }
3851
3852 static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb)
3853 {
3854         struct sock *sk;
3855
3856         sk = l2cap_get_sock_by_psm(0, psm, conn->src);
3857         if (!sk)
3858                 goto drop;
3859
3860         BT_DBG("sk %p, len %d", sk, skb->len);
3861
3862         if (sk->sk_state != BT_BOUND && sk->sk_state != BT_CONNECTED)
3863                 goto drop;
3864
3865         if (l2cap_pi(sk)->imtu < skb->len)
3866                 goto drop;
3867
3868         if (!sock_queue_rcv_skb(sk, skb))
3869                 goto done;
3870
3871 drop:
3872         kfree_skb(skb);
3873
3874 done:
3875         if (sk)
3876                 bh_unlock_sock(sk);
3877         return 0;
3878 }
3879
3880 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
3881 {
3882         struct l2cap_hdr *lh = (void *) skb->data;
3883         u16 cid, len;
3884         __le16 psm;
3885
3886         skb_pull(skb, L2CAP_HDR_SIZE);
3887         cid = __le16_to_cpu(lh->cid);
3888         len = __le16_to_cpu(lh->len);
3889
3890         if (len != skb->len) {
3891                 kfree_skb(skb);
3892                 return;
3893         }
3894
3895         BT_DBG("len %d, cid 0x%4.4x", len, cid);
3896
3897         switch (cid) {
3898         case L2CAP_CID_SIGNALING:
3899                 l2cap_sig_channel(conn, skb);
3900                 break;
3901
3902         case L2CAP_CID_CONN_LESS:
3903                 psm = get_unaligned_le16(skb->data);
3904                 skb_pull(skb, 2);
3905                 l2cap_conless_channel(conn, psm, skb);
3906                 break;
3907
3908         default:
3909                 l2cap_data_channel(conn, cid, skb);
3910                 break;
3911         }
3912 }
3913
3914 /* ---- L2CAP interface with lower layer (HCI) ---- */
3915
3916 static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
3917 {
3918         int exact = 0, lm1 = 0, lm2 = 0;
3919         register struct sock *sk;
3920         struct hlist_node *node;
3921
3922         if (type != ACL_LINK)
3923                 return 0;
3924
3925         BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
3926
3927         /* Find listening sockets and check their link_mode */
3928         read_lock(&l2cap_sk_list.lock);
3929         sk_for_each(sk, node, &l2cap_sk_list.head) {
3930                 if (sk->sk_state != BT_LISTEN)
3931                         continue;
3932
3933                 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
3934                         lm1 |= HCI_LM_ACCEPT;
3935                         if (l2cap_pi(sk)->role_switch)
3936                                 lm1 |= HCI_LM_MASTER;
3937                         exact++;
3938                 } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
3939                         lm2 |= HCI_LM_ACCEPT;
3940                         if (l2cap_pi(sk)->role_switch)
3941                                 lm2 |= HCI_LM_MASTER;
3942                 }
3943         }
3944         read_unlock(&l2cap_sk_list.lock);
3945
3946         return exact ? lm1 : lm2;
3947 }
3948
3949 static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
3950 {
3951         struct l2cap_conn *conn;
3952
3953         BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
3954
3955         if (hcon->type != ACL_LINK)
3956                 return 0;
3957
3958         if (!status) {
3959                 conn = l2cap_conn_add(hcon, status);
3960                 if (conn)
3961                         l2cap_conn_ready(conn);
3962         } else
3963                 l2cap_conn_del(hcon, bt_err(status));
3964
3965         return 0;
3966 }
3967
3968 static int l2cap_disconn_ind(struct hci_conn *hcon)
3969 {
3970         struct l2cap_conn *conn = hcon->l2cap_data;
3971
3972         BT_DBG("hcon %p", hcon);
3973
3974         if (hcon->type != ACL_LINK || !conn)
3975                 return 0x13;
3976
3977         return conn->disc_reason;
3978 }
3979
3980 static int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
3981 {
3982         BT_DBG("hcon %p reason %d", hcon, reason);
3983
3984         if (hcon->type != ACL_LINK)
3985                 return 0;
3986
3987         l2cap_conn_del(hcon, bt_err(reason));
3988
3989         return 0;
3990 }
3991
3992 static inline void l2cap_check_encryption(struct sock *sk, u8 encrypt)
3993 {
3994         if (sk->sk_type != SOCK_SEQPACKET)
3995                 return;
3996
3997         if (encrypt == 0x00) {
3998                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_MEDIUM) {
3999                         l2cap_sock_clear_timer(sk);
4000                         l2cap_sock_set_timer(sk, HZ * 5);
4001                 } else if (l2cap_pi(sk)->sec_level == BT_SECURITY_HIGH)
4002                         __l2cap_sock_close(sk, ECONNREFUSED);
4003         } else {
4004                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_MEDIUM)
4005                         l2cap_sock_clear_timer(sk);
4006         }
4007 }
4008
4009 static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
4010 {
4011         struct l2cap_chan_list *l;
4012         struct l2cap_conn *conn = hcon->l2cap_data;
4013         struct sock *sk;
4014
4015         if (!conn)
4016                 return 0;
4017
4018         l = &conn->chan_list;
4019
4020         BT_DBG("conn %p", conn);
4021
4022         read_lock(&l->lock);
4023
4024         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
4025                 bh_lock_sock(sk);
4026
4027                 if (l2cap_pi(sk)->conf_state & L2CAP_CONF_CONNECT_PEND) {
4028                         bh_unlock_sock(sk);
4029                         continue;
4030                 }
4031
4032                 if (!status && (sk->sk_state == BT_CONNECTED ||
4033                                                 sk->sk_state == BT_CONFIG)) {
4034                         l2cap_check_encryption(sk, encrypt);
4035                         bh_unlock_sock(sk);
4036                         continue;
4037                 }
4038
4039                 if (sk->sk_state == BT_CONNECT) {
4040                         if (!status) {
4041                                 struct l2cap_conn_req req;
4042                                 req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
4043                                 req.psm  = l2cap_pi(sk)->psm;
4044
4045                                 l2cap_pi(sk)->ident = l2cap_get_ident(conn);
4046
4047                                 l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
4048                                         L2CAP_CONN_REQ, sizeof(req), &req);
4049                         } else {
4050                                 l2cap_sock_clear_timer(sk);
4051                                 l2cap_sock_set_timer(sk, HZ / 10);
4052                         }
4053                 } else if (sk->sk_state == BT_CONNECT2) {
4054                         struct l2cap_conn_rsp rsp;
4055                         __u16 result;
4056
4057                         if (!status) {
4058                                 sk->sk_state = BT_CONFIG;
4059                                 result = L2CAP_CR_SUCCESS;
4060                         } else {
4061                                 sk->sk_state = BT_DISCONN;
4062                                 l2cap_sock_set_timer(sk, HZ / 10);
4063                                 result = L2CAP_CR_SEC_BLOCK;
4064                         }
4065
4066                         rsp.scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
4067                         rsp.dcid   = cpu_to_le16(l2cap_pi(sk)->scid);
4068                         rsp.result = cpu_to_le16(result);
4069                         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
4070                         l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
4071                                         L2CAP_CONN_RSP, sizeof(rsp), &rsp);
4072                 }
4073
4074                 bh_unlock_sock(sk);
4075         }
4076
4077         read_unlock(&l->lock);
4078
4079         return 0;
4080 }
4081
4082 static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
4083 {
4084         struct l2cap_conn *conn = hcon->l2cap_data;
4085
4086         if (!conn && !(conn = l2cap_conn_add(hcon, 0)))
4087                 goto drop;
4088
4089         BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
4090
4091         if (flags & ACL_START) {
4092                 struct l2cap_hdr *hdr;
4093                 int len;
4094
4095                 if (conn->rx_len) {
4096                         BT_ERR("Unexpected start frame (len %d)", skb->len);
4097                         kfree_skb(conn->rx_skb);
4098                         conn->rx_skb = NULL;
4099                         conn->rx_len = 0;
4100                         l2cap_conn_unreliable(conn, ECOMM);
4101                 }
4102
4103                 if (skb->len < 2) {
4104                         BT_ERR("Frame is too short (len %d)", skb->len);
4105                         l2cap_conn_unreliable(conn, ECOMM);
4106                         goto drop;
4107                 }
4108
4109                 hdr = (struct l2cap_hdr *) skb->data;
4110                 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
4111
4112                 if (len == skb->len) {
4113                         /* Complete frame received */
4114                         l2cap_recv_frame(conn, skb);
4115                         return 0;
4116                 }
4117
4118                 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
4119
4120                 if (skb->len > len) {
4121                         BT_ERR("Frame is too long (len %d, expected len %d)",
4122                                 skb->len, len);
4123                         l2cap_conn_unreliable(conn, ECOMM);
4124                         goto drop;
4125                 }
4126
4127                 /* Allocate skb for the complete frame (with header) */
4128                 conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC);
4129                 if (!conn->rx_skb)
4130                         goto drop;
4131
4132                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
4133                                                                 skb->len);
4134                 conn->rx_len = len - skb->len;
4135         } else {
4136                 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
4137
4138                 if (!conn->rx_len) {
4139                         BT_ERR("Unexpected continuation frame (len %d)", skb->len);
4140                         l2cap_conn_unreliable(conn, ECOMM);
4141                         goto drop;
4142                 }
4143
4144                 if (skb->len > conn->rx_len) {
4145                         BT_ERR("Fragment is too long (len %d, expected %d)",
4146                                         skb->len, conn->rx_len);
4147                         kfree_skb(conn->rx_skb);
4148                         conn->rx_skb = NULL;
4149                         conn->rx_len = 0;
4150                         l2cap_conn_unreliable(conn, ECOMM);
4151                         goto drop;
4152                 }
4153
4154                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
4155                                                                 skb->len);
4156                 conn->rx_len -= skb->len;
4157
4158                 if (!conn->rx_len) {
4159                         /* Complete frame received */
4160                         l2cap_recv_frame(conn, conn->rx_skb);
4161                         conn->rx_skb = NULL;
4162                 }
4163         }
4164
4165 drop:
4166         kfree_skb(skb);
4167         return 0;
4168 }
4169
4170 static int l2cap_debugfs_show(struct seq_file *f, void *p)
4171 {
4172         struct sock *sk;
4173         struct hlist_node *node;
4174
4175         read_lock_bh(&l2cap_sk_list.lock);
4176
4177         sk_for_each(sk, node, &l2cap_sk_list.head) {
4178                 struct l2cap_pinfo *pi = l2cap_pi(sk);
4179
4180                 seq_printf(f, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d\n",
4181                                         batostr(&bt_sk(sk)->src),
4182                                         batostr(&bt_sk(sk)->dst),
4183                                         sk->sk_state, __le16_to_cpu(pi->psm),
4184                                         pi->scid, pi->dcid,
4185                                         pi->imtu, pi->omtu, pi->sec_level);
4186         }
4187
4188         read_unlock_bh(&l2cap_sk_list.lock);
4189
4190         return 0;
4191 }
4192
4193 static int l2cap_debugfs_open(struct inode *inode, struct file *file)
4194 {
4195         return single_open(file, l2cap_debugfs_show, inode->i_private);
4196 }
4197
4198 static const struct file_operations l2cap_debugfs_fops = {
4199         .open           = l2cap_debugfs_open,
4200         .read           = seq_read,
4201         .llseek         = seq_lseek,
4202         .release        = single_release,
4203 };
4204
4205 static struct dentry *l2cap_debugfs;
4206
4207 static const struct proto_ops l2cap_sock_ops = {
4208         .family         = PF_BLUETOOTH,
4209         .owner          = THIS_MODULE,
4210         .release        = l2cap_sock_release,
4211         .bind           = l2cap_sock_bind,
4212         .connect        = l2cap_sock_connect,
4213         .listen         = l2cap_sock_listen,
4214         .accept         = l2cap_sock_accept,
4215         .getname        = l2cap_sock_getname,
4216         .sendmsg        = l2cap_sock_sendmsg,
4217         .recvmsg        = l2cap_sock_recvmsg,
4218         .poll           = bt_sock_poll,
4219         .ioctl          = bt_sock_ioctl,
4220         .mmap           = sock_no_mmap,
4221         .socketpair     = sock_no_socketpair,
4222         .shutdown       = l2cap_sock_shutdown,
4223         .setsockopt     = l2cap_sock_setsockopt,
4224         .getsockopt     = l2cap_sock_getsockopt
4225 };
4226
4227 static const struct net_proto_family l2cap_sock_family_ops = {
4228         .family = PF_BLUETOOTH,
4229         .owner  = THIS_MODULE,
4230         .create = l2cap_sock_create,
4231 };
4232
4233 static struct hci_proto l2cap_hci_proto = {
4234         .name           = "L2CAP",
4235         .id             = HCI_PROTO_L2CAP,
4236         .connect_ind    = l2cap_connect_ind,
4237         .connect_cfm    = l2cap_connect_cfm,
4238         .disconn_ind    = l2cap_disconn_ind,
4239         .disconn_cfm    = l2cap_disconn_cfm,
4240         .security_cfm   = l2cap_security_cfm,
4241         .recv_acldata   = l2cap_recv_acldata
4242 };
4243
4244 static int __init l2cap_init(void)
4245 {
4246         int err;
4247
4248         err = proto_register(&l2cap_proto, 0);
4249         if (err < 0)
4250                 return err;
4251
4252         err = bt_sock_register(BTPROTO_L2CAP, &l2cap_sock_family_ops);
4253         if (err < 0) {
4254                 BT_ERR("L2CAP socket registration failed");
4255                 goto error;
4256         }
4257
4258         err = hci_register_proto(&l2cap_hci_proto);
4259         if (err < 0) {
4260                 BT_ERR("L2CAP protocol registration failed");
4261                 bt_sock_unregister(BTPROTO_L2CAP);
4262                 goto error;
4263         }
4264
4265         if (bt_debugfs) {
4266                 l2cap_debugfs = debugfs_create_file("l2cap", 0444,
4267                                         bt_debugfs, NULL, &l2cap_debugfs_fops);
4268                 if (!l2cap_debugfs)
4269                         BT_ERR("Failed to create L2CAP debug file");
4270         }
4271
4272         BT_INFO("L2CAP ver %s", VERSION);
4273         BT_INFO("L2CAP socket layer initialized");
4274
4275         return 0;
4276
4277 error:
4278         proto_unregister(&l2cap_proto);
4279         return err;
4280 }
4281
4282 static void __exit l2cap_exit(void)
4283 {
4284         debugfs_remove(l2cap_debugfs);
4285
4286         if (bt_sock_unregister(BTPROTO_L2CAP) < 0)
4287                 BT_ERR("L2CAP socket unregistration failed");
4288
4289         if (hci_unregister_proto(&l2cap_hci_proto) < 0)
4290                 BT_ERR("L2CAP protocol unregistration failed");
4291
4292         proto_unregister(&l2cap_proto);
4293 }
4294
4295 void l2cap_load(void)
4296 {
4297         /* Dummy function to trigger automatic L2CAP module loading by
4298          * other modules that use L2CAP sockets but don't use any other
4299          * symbols from it. */
4300         return;
4301 }
4302 EXPORT_SYMBOL(l2cap_load);
4303
4304 module_init(l2cap_init);
4305 module_exit(l2cap_exit);
4306
4307 module_param(enable_ertm, bool, 0644);
4308 MODULE_PARM_DESC(enable_ertm, "Enable enhanced retransmission mode");
4309
4310 module_param(max_transmit, uint, 0644);
4311 MODULE_PARM_DESC(max_transmit, "Max transmit value (default = 3)");
4312
4313 module_param(tx_window, uint, 0644);
4314 MODULE_PARM_DESC(tx_window, "Transmission window size value (default = 63)");
4315
4316 MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
4317 MODULE_DESCRIPTION("Bluetooth L2CAP ver " VERSION);
4318 MODULE_VERSION(VERSION);
4319 MODULE_LICENSE("GPL");
4320 MODULE_ALIAS("bt-proto-0");