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