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