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