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