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 Copyright (C) 2011 ProFUSION Embedded Systems 7 Copyright (c) 2012 Code Aurora Forum. All rights reserved. 8 9 Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.com> 10 11 This program is free software; you can redistribute it and/or modify 12 it under the terms of the GNU General Public License version 2 as 13 published by the Free Software Foundation; 14 15 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 16 OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. 18 IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY 19 CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES 20 WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 21 ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 22 OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 23 24 ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, 25 COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS 26 SOFTWARE IS DISCLAIMED. 27 */ 28 29 /* Bluetooth L2CAP core. */ 30 31 #include <linux/module.h> 32 33 #include <linux/debugfs.h> 34 #include <linux/crc16.h> 35 #include <linux/filter.h> 36 37 #include <net/bluetooth/bluetooth.h> 38 #include <net/bluetooth/hci_core.h> 39 #include <net/bluetooth/l2cap.h> 40 41 #include "smp.h" 42 #include "a2mp.h" 43 #include "amp.h" 44 45 #define LE_FLOWCTL_MAX_CREDITS 65535 46 47 bool disable_ertm; 48 49 static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN | L2CAP_FEAT_UCD; 50 51 static LIST_HEAD(chan_list); 52 static DEFINE_RWLOCK(chan_list_lock); 53 54 static u16 le_max_credits = L2CAP_LE_MAX_CREDITS; 55 static u16 le_default_mps = L2CAP_LE_DEFAULT_MPS; 56 57 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn, 58 u8 code, u8 ident, u16 dlen, void *data); 59 static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, 60 void *data); 61 static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data, size_t data_size); 62 static void l2cap_send_disconn_req(struct l2cap_chan *chan, int err); 63 64 static void l2cap_tx(struct l2cap_chan *chan, struct l2cap_ctrl *control, 65 struct sk_buff_head *skbs, u8 event); 66 67 static inline u8 bdaddr_type(u8 link_type, u8 bdaddr_type) 68 { 69 if (link_type == LE_LINK) { 70 if (bdaddr_type == ADDR_LE_DEV_PUBLIC) 71 return BDADDR_LE_PUBLIC; 72 else 73 return BDADDR_LE_RANDOM; 74 } 75 76 return BDADDR_BREDR; 77 } 78 79 static inline u8 bdaddr_src_type(struct hci_conn *hcon) 80 { 81 return bdaddr_type(hcon->type, hcon->src_type); 82 } 83 84 static inline u8 bdaddr_dst_type(struct hci_conn *hcon) 85 { 86 return bdaddr_type(hcon->type, hcon->dst_type); 87 } 88 89 /* ---- L2CAP channels ---- */ 90 91 static struct l2cap_chan *__l2cap_get_chan_by_dcid(struct l2cap_conn *conn, 92 u16 cid) 93 { 94 struct l2cap_chan *c; 95 96 list_for_each_entry(c, &conn->chan_l, list) { 97 if (c->dcid == cid) 98 return c; 99 } 100 return NULL; 101 } 102 103 static struct l2cap_chan *__l2cap_get_chan_by_scid(struct l2cap_conn *conn, 104 u16 cid) 105 { 106 struct l2cap_chan *c; 107 108 list_for_each_entry(c, &conn->chan_l, list) { 109 if (c->scid == cid) 110 return c; 111 } 112 return NULL; 113 } 114 115 /* Find channel with given SCID. 116 * Returns locked channel. */ 117 static struct l2cap_chan *l2cap_get_chan_by_scid(struct l2cap_conn *conn, 118 u16 cid) 119 { 120 struct l2cap_chan *c; 121 122 mutex_lock(&conn->chan_lock); 123 c = __l2cap_get_chan_by_scid(conn, cid); 124 if (c) 125 l2cap_chan_lock(c); 126 mutex_unlock(&conn->chan_lock); 127 128 return c; 129 } 130 131 /* Find channel with given DCID. 132 * Returns locked channel. 133 */ 134 static struct l2cap_chan *l2cap_get_chan_by_dcid(struct l2cap_conn *conn, 135 u16 cid) 136 { 137 struct l2cap_chan *c; 138 139 mutex_lock(&conn->chan_lock); 140 c = __l2cap_get_chan_by_dcid(conn, cid); 141 if (c) 142 l2cap_chan_lock(c); 143 mutex_unlock(&conn->chan_lock); 144 145 return c; 146 } 147 148 static struct l2cap_chan *__l2cap_get_chan_by_ident(struct l2cap_conn *conn, 149 u8 ident) 150 { 151 struct l2cap_chan *c; 152 153 list_for_each_entry(c, &conn->chan_l, list) { 154 if (c->ident == ident) 155 return c; 156 } 157 return NULL; 158 } 159 160 static struct l2cap_chan *l2cap_get_chan_by_ident(struct l2cap_conn *conn, 161 u8 ident) 162 { 163 struct l2cap_chan *c; 164 165 mutex_lock(&conn->chan_lock); 166 c = __l2cap_get_chan_by_ident(conn, ident); 167 if (c) 168 l2cap_chan_lock(c); 169 mutex_unlock(&conn->chan_lock); 170 171 return c; 172 } 173 174 static struct l2cap_chan *__l2cap_global_chan_by_addr(__le16 psm, bdaddr_t *src) 175 { 176 struct l2cap_chan *c; 177 178 list_for_each_entry(c, &chan_list, global_l) { 179 if (c->sport == psm && !bacmp(&c->src, src)) 180 return c; 181 } 182 return NULL; 183 } 184 185 int l2cap_add_psm(struct l2cap_chan *chan, bdaddr_t *src, __le16 psm) 186 { 187 int err; 188 189 write_lock(&chan_list_lock); 190 191 if (psm && __l2cap_global_chan_by_addr(psm, src)) { 192 err = -EADDRINUSE; 193 goto done; 194 } 195 196 if (psm) { 197 chan->psm = psm; 198 chan->sport = psm; 199 err = 0; 200 } else { 201 u16 p, start, end, incr; 202 203 if (chan->src_type == BDADDR_BREDR) { 204 start = L2CAP_PSM_DYN_START; 205 end = L2CAP_PSM_AUTO_END; 206 incr = 2; 207 } else { 208 start = L2CAP_PSM_LE_DYN_START; 209 end = L2CAP_PSM_LE_DYN_END; 210 incr = 1; 211 } 212 213 err = -EINVAL; 214 for (p = start; p <= end; p += incr) 215 if (!__l2cap_global_chan_by_addr(cpu_to_le16(p), src)) { 216 chan->psm = cpu_to_le16(p); 217 chan->sport = cpu_to_le16(p); 218 err = 0; 219 break; 220 } 221 } 222 223 done: 224 write_unlock(&chan_list_lock); 225 return err; 226 } 227 EXPORT_SYMBOL_GPL(l2cap_add_psm); 228 229 int l2cap_add_scid(struct l2cap_chan *chan, __u16 scid) 230 { 231 write_lock(&chan_list_lock); 232 233 /* Override the defaults (which are for conn-oriented) */ 234 chan->omtu = L2CAP_DEFAULT_MTU; 235 chan->chan_type = L2CAP_CHAN_FIXED; 236 237 chan->scid = scid; 238 239 write_unlock(&chan_list_lock); 240 241 return 0; 242 } 243 244 static u16 l2cap_alloc_cid(struct l2cap_conn *conn) 245 { 246 u16 cid, dyn_end; 247 248 if (conn->hcon->type == LE_LINK) 249 dyn_end = L2CAP_CID_LE_DYN_END; 250 else 251 dyn_end = L2CAP_CID_DYN_END; 252 253 for (cid = L2CAP_CID_DYN_START; cid <= dyn_end; cid++) { 254 if (!__l2cap_get_chan_by_scid(conn, cid)) 255 return cid; 256 } 257 258 return 0; 259 } 260 261 static void l2cap_state_change(struct l2cap_chan *chan, int state) 262 { 263 BT_DBG("chan %p %s -> %s", chan, state_to_string(chan->state), 264 state_to_string(state)); 265 266 chan->state = state; 267 chan->ops->state_change(chan, state, 0); 268 } 269 270 static inline void l2cap_state_change_and_error(struct l2cap_chan *chan, 271 int state, int err) 272 { 273 chan->state = state; 274 chan->ops->state_change(chan, chan->state, err); 275 } 276 277 static inline void l2cap_chan_set_err(struct l2cap_chan *chan, int err) 278 { 279 chan->ops->state_change(chan, chan->state, err); 280 } 281 282 static void __set_retrans_timer(struct l2cap_chan *chan) 283 { 284 if (!delayed_work_pending(&chan->monitor_timer) && 285 chan->retrans_timeout) { 286 l2cap_set_timer(chan, &chan->retrans_timer, 287 msecs_to_jiffies(chan->retrans_timeout)); 288 } 289 } 290 291 static void __set_monitor_timer(struct l2cap_chan *chan) 292 { 293 __clear_retrans_timer(chan); 294 if (chan->monitor_timeout) { 295 l2cap_set_timer(chan, &chan->monitor_timer, 296 msecs_to_jiffies(chan->monitor_timeout)); 297 } 298 } 299 300 static struct sk_buff *l2cap_ertm_seq_in_queue(struct sk_buff_head *head, 301 u16 seq) 302 { 303 struct sk_buff *skb; 304 305 skb_queue_walk(head, skb) { 306 if (bt_cb(skb)->l2cap.txseq == seq) 307 return skb; 308 } 309 310 return NULL; 311 } 312 313 /* ---- L2CAP sequence number lists ---- */ 314 315 /* For ERTM, ordered lists of sequence numbers must be tracked for 316 * SREJ requests that are received and for frames that are to be 317 * retransmitted. These seq_list functions implement a singly-linked 318 * list in an array, where membership in the list can also be checked 319 * in constant time. Items can also be added to the tail of the list 320 * and removed from the head in constant time, without further memory 321 * allocs or frees. 322 */ 323 324 static int l2cap_seq_list_init(struct l2cap_seq_list *seq_list, u16 size) 325 { 326 size_t alloc_size, i; 327 328 /* Allocated size is a power of 2 to map sequence numbers 329 * (which may be up to 14 bits) in to a smaller array that is 330 * sized for the negotiated ERTM transmit windows. 331 */ 332 alloc_size = roundup_pow_of_two(size); 333 334 seq_list->list = kmalloc(sizeof(u16) * alloc_size, GFP_KERNEL); 335 if (!seq_list->list) 336 return -ENOMEM; 337 338 seq_list->mask = alloc_size - 1; 339 seq_list->head = L2CAP_SEQ_LIST_CLEAR; 340 seq_list->tail = L2CAP_SEQ_LIST_CLEAR; 341 for (i = 0; i < alloc_size; i++) 342 seq_list->list[i] = L2CAP_SEQ_LIST_CLEAR; 343 344 return 0; 345 } 346 347 static inline void l2cap_seq_list_free(struct l2cap_seq_list *seq_list) 348 { 349 kfree(seq_list->list); 350 } 351 352 static inline bool l2cap_seq_list_contains(struct l2cap_seq_list *seq_list, 353 u16 seq) 354 { 355 /* Constant-time check for list membership */ 356 return seq_list->list[seq & seq_list->mask] != L2CAP_SEQ_LIST_CLEAR; 357 } 358 359 static inline u16 l2cap_seq_list_pop(struct l2cap_seq_list *seq_list) 360 { 361 u16 seq = seq_list->head; 362 u16 mask = seq_list->mask; 363 364 seq_list->head = seq_list->list[seq & mask]; 365 seq_list->list[seq & mask] = L2CAP_SEQ_LIST_CLEAR; 366 367 if (seq_list->head == L2CAP_SEQ_LIST_TAIL) { 368 seq_list->head = L2CAP_SEQ_LIST_CLEAR; 369 seq_list->tail = L2CAP_SEQ_LIST_CLEAR; 370 } 371 372 return seq; 373 } 374 375 static void l2cap_seq_list_clear(struct l2cap_seq_list *seq_list) 376 { 377 u16 i; 378 379 if (seq_list->head == L2CAP_SEQ_LIST_CLEAR) 380 return; 381 382 for (i = 0; i <= seq_list->mask; i++) 383 seq_list->list[i] = L2CAP_SEQ_LIST_CLEAR; 384 385 seq_list->head = L2CAP_SEQ_LIST_CLEAR; 386 seq_list->tail = L2CAP_SEQ_LIST_CLEAR; 387 } 388 389 static void l2cap_seq_list_append(struct l2cap_seq_list *seq_list, u16 seq) 390 { 391 u16 mask = seq_list->mask; 392 393 /* All appends happen in constant time */ 394 395 if (seq_list->list[seq & mask] != L2CAP_SEQ_LIST_CLEAR) 396 return; 397 398 if (seq_list->tail == L2CAP_SEQ_LIST_CLEAR) 399 seq_list->head = seq; 400 else 401 seq_list->list[seq_list->tail & mask] = seq; 402 403 seq_list->tail = seq; 404 seq_list->list[seq & mask] = L2CAP_SEQ_LIST_TAIL; 405 } 406 407 static void l2cap_chan_timeout(struct work_struct *work) 408 { 409 struct l2cap_chan *chan = container_of(work, struct l2cap_chan, 410 chan_timer.work); 411 struct l2cap_conn *conn = chan->conn; 412 int reason; 413 414 BT_DBG("chan %p state %s", chan, state_to_string(chan->state)); 415 416 mutex_lock(&conn->chan_lock); 417 l2cap_chan_lock(chan); 418 419 if (chan->state == BT_CONNECTED || chan->state == BT_CONFIG) 420 reason = ECONNREFUSED; 421 else if (chan->state == BT_CONNECT && 422 chan->sec_level != BT_SECURITY_SDP) 423 reason = ECONNREFUSED; 424 else 425 reason = ETIMEDOUT; 426 427 l2cap_chan_close(chan, reason); 428 429 l2cap_chan_unlock(chan); 430 431 chan->ops->close(chan); 432 mutex_unlock(&conn->chan_lock); 433 434 l2cap_chan_put(chan); 435 } 436 437 struct l2cap_chan *l2cap_chan_create(void) 438 { 439 struct l2cap_chan *chan; 440 441 chan = kzalloc(sizeof(*chan), GFP_ATOMIC); 442 if (!chan) 443 return NULL; 444 445 mutex_init(&chan->lock); 446 447 /* Set default lock nesting level */ 448 atomic_set(&chan->nesting, L2CAP_NESTING_NORMAL); 449 450 write_lock(&chan_list_lock); 451 list_add(&chan->global_l, &chan_list); 452 write_unlock(&chan_list_lock); 453 454 INIT_DELAYED_WORK(&chan->chan_timer, l2cap_chan_timeout); 455 456 chan->state = BT_OPEN; 457 458 kref_init(&chan->kref); 459 460 /* This flag is cleared in l2cap_chan_ready() */ 461 set_bit(CONF_NOT_COMPLETE, &chan->conf_state); 462 463 BT_DBG("chan %p", chan); 464 465 return chan; 466 } 467 EXPORT_SYMBOL_GPL(l2cap_chan_create); 468 469 static void l2cap_chan_destroy(struct kref *kref) 470 { 471 struct l2cap_chan *chan = container_of(kref, struct l2cap_chan, kref); 472 473 BT_DBG("chan %p", chan); 474 475 write_lock(&chan_list_lock); 476 list_del(&chan->global_l); 477 write_unlock(&chan_list_lock); 478 479 kfree(chan); 480 } 481 482 void l2cap_chan_hold(struct l2cap_chan *c) 483 { 484 BT_DBG("chan %p orig refcnt %d", c, kref_read(&c->kref)); 485 486 kref_get(&c->kref); 487 } 488 489 void l2cap_chan_put(struct l2cap_chan *c) 490 { 491 BT_DBG("chan %p orig refcnt %d", c, kref_read(&c->kref)); 492 493 kref_put(&c->kref, l2cap_chan_destroy); 494 } 495 EXPORT_SYMBOL_GPL(l2cap_chan_put); 496 497 void l2cap_chan_set_defaults(struct l2cap_chan *chan) 498 { 499 chan->fcs = L2CAP_FCS_CRC16; 500 chan->max_tx = L2CAP_DEFAULT_MAX_TX; 501 chan->tx_win = L2CAP_DEFAULT_TX_WINDOW; 502 chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW; 503 chan->remote_max_tx = chan->max_tx; 504 chan->remote_tx_win = chan->tx_win; 505 chan->ack_win = L2CAP_DEFAULT_TX_WINDOW; 506 chan->sec_level = BT_SECURITY_LOW; 507 chan->flush_to = L2CAP_DEFAULT_FLUSH_TO; 508 chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO; 509 chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO; 510 chan->conf_state = 0; 511 512 set_bit(FLAG_FORCE_ACTIVE, &chan->flags); 513 } 514 EXPORT_SYMBOL_GPL(l2cap_chan_set_defaults); 515 516 static void l2cap_le_flowctl_init(struct l2cap_chan *chan) 517 { 518 chan->sdu = NULL; 519 chan->sdu_last_frag = NULL; 520 chan->sdu_len = 0; 521 chan->tx_credits = 0; 522 chan->rx_credits = le_max_credits; 523 chan->mps = min_t(u16, chan->imtu, le_default_mps); 524 525 skb_queue_head_init(&chan->tx_q); 526 } 527 528 void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan) 529 { 530 BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn, 531 __le16_to_cpu(chan->psm), chan->dcid); 532 533 conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM; 534 535 chan->conn = conn; 536 537 switch (chan->chan_type) { 538 case L2CAP_CHAN_CONN_ORIENTED: 539 /* Alloc CID for connection-oriented socket */ 540 chan->scid = l2cap_alloc_cid(conn); 541 if (conn->hcon->type == ACL_LINK) 542 chan->omtu = L2CAP_DEFAULT_MTU; 543 break; 544 545 case L2CAP_CHAN_CONN_LESS: 546 /* Connectionless socket */ 547 chan->scid = L2CAP_CID_CONN_LESS; 548 chan->dcid = L2CAP_CID_CONN_LESS; 549 chan->omtu = L2CAP_DEFAULT_MTU; 550 break; 551 552 case L2CAP_CHAN_FIXED: 553 /* Caller will set CID and CID specific MTU values */ 554 break; 555 556 default: 557 /* Raw socket can send/recv signalling messages only */ 558 chan->scid = L2CAP_CID_SIGNALING; 559 chan->dcid = L2CAP_CID_SIGNALING; 560 chan->omtu = L2CAP_DEFAULT_MTU; 561 } 562 563 chan->local_id = L2CAP_BESTEFFORT_ID; 564 chan->local_stype = L2CAP_SERV_BESTEFFORT; 565 chan->local_msdu = L2CAP_DEFAULT_MAX_SDU_SIZE; 566 chan->local_sdu_itime = L2CAP_DEFAULT_SDU_ITIME; 567 chan->local_acc_lat = L2CAP_DEFAULT_ACC_LAT; 568 chan->local_flush_to = L2CAP_EFS_DEFAULT_FLUSH_TO; 569 570 l2cap_chan_hold(chan); 571 572 /* Only keep a reference for fixed channels if they requested it */ 573 if (chan->chan_type != L2CAP_CHAN_FIXED || 574 test_bit(FLAG_HOLD_HCI_CONN, &chan->flags)) 575 hci_conn_hold(conn->hcon); 576 577 list_add(&chan->list, &conn->chan_l); 578 } 579 580 void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan) 581 { 582 mutex_lock(&conn->chan_lock); 583 __l2cap_chan_add(conn, chan); 584 mutex_unlock(&conn->chan_lock); 585 } 586 587 void l2cap_chan_del(struct l2cap_chan *chan, int err) 588 { 589 struct l2cap_conn *conn = chan->conn; 590 591 __clear_chan_timer(chan); 592 593 BT_DBG("chan %p, conn %p, err %d, state %s", chan, conn, err, 594 state_to_string(chan->state)); 595 596 chan->ops->teardown(chan, err); 597 598 if (conn) { 599 struct amp_mgr *mgr = conn->hcon->amp_mgr; 600 /* Delete from channel list */ 601 list_del(&chan->list); 602 603 l2cap_chan_put(chan); 604 605 chan->conn = NULL; 606 607 /* Reference was only held for non-fixed channels or 608 * fixed channels that explicitly requested it using the 609 * FLAG_HOLD_HCI_CONN flag. 610 */ 611 if (chan->chan_type != L2CAP_CHAN_FIXED || 612 test_bit(FLAG_HOLD_HCI_CONN, &chan->flags)) 613 hci_conn_drop(conn->hcon); 614 615 if (mgr && mgr->bredr_chan == chan) 616 mgr->bredr_chan = NULL; 617 } 618 619 if (chan->hs_hchan) { 620 struct hci_chan *hs_hchan = chan->hs_hchan; 621 622 BT_DBG("chan %p disconnect hs_hchan %p", chan, hs_hchan); 623 amp_disconnect_logical_link(hs_hchan); 624 } 625 626 if (test_bit(CONF_NOT_COMPLETE, &chan->conf_state)) 627 return; 628 629 switch(chan->mode) { 630 case L2CAP_MODE_BASIC: 631 break; 632 633 case L2CAP_MODE_LE_FLOWCTL: 634 skb_queue_purge(&chan->tx_q); 635 break; 636 637 case L2CAP_MODE_ERTM: 638 __clear_retrans_timer(chan); 639 __clear_monitor_timer(chan); 640 __clear_ack_timer(chan); 641 642 skb_queue_purge(&chan->srej_q); 643 644 l2cap_seq_list_free(&chan->srej_list); 645 l2cap_seq_list_free(&chan->retrans_list); 646 647 /* fall through */ 648 649 case L2CAP_MODE_STREAMING: 650 skb_queue_purge(&chan->tx_q); 651 break; 652 } 653 654 return; 655 } 656 EXPORT_SYMBOL_GPL(l2cap_chan_del); 657 658 static void l2cap_conn_update_id_addr(struct work_struct *work) 659 { 660 struct l2cap_conn *conn = container_of(work, struct l2cap_conn, 661 id_addr_update_work); 662 struct hci_conn *hcon = conn->hcon; 663 struct l2cap_chan *chan; 664 665 mutex_lock(&conn->chan_lock); 666 667 list_for_each_entry(chan, &conn->chan_l, list) { 668 l2cap_chan_lock(chan); 669 bacpy(&chan->dst, &hcon->dst); 670 chan->dst_type = bdaddr_dst_type(hcon); 671 l2cap_chan_unlock(chan); 672 } 673 674 mutex_unlock(&conn->chan_lock); 675 } 676 677 static void l2cap_chan_le_connect_reject(struct l2cap_chan *chan) 678 { 679 struct l2cap_conn *conn = chan->conn; 680 struct l2cap_le_conn_rsp rsp; 681 u16 result; 682 683 if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) 684 result = L2CAP_CR_AUTHORIZATION; 685 else 686 result = L2CAP_CR_BAD_PSM; 687 688 l2cap_state_change(chan, BT_DISCONN); 689 690 rsp.dcid = cpu_to_le16(chan->scid); 691 rsp.mtu = cpu_to_le16(chan->imtu); 692 rsp.mps = cpu_to_le16(chan->mps); 693 rsp.credits = cpu_to_le16(chan->rx_credits); 694 rsp.result = cpu_to_le16(result); 695 696 l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CONN_RSP, sizeof(rsp), 697 &rsp); 698 } 699 700 static void l2cap_chan_connect_reject(struct l2cap_chan *chan) 701 { 702 struct l2cap_conn *conn = chan->conn; 703 struct l2cap_conn_rsp rsp; 704 u16 result; 705 706 if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) 707 result = L2CAP_CR_SEC_BLOCK; 708 else 709 result = L2CAP_CR_BAD_PSM; 710 711 l2cap_state_change(chan, BT_DISCONN); 712 713 rsp.scid = cpu_to_le16(chan->dcid); 714 rsp.dcid = cpu_to_le16(chan->scid); 715 rsp.result = cpu_to_le16(result); 716 rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); 717 718 l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp); 719 } 720 721 void l2cap_chan_close(struct l2cap_chan *chan, int reason) 722 { 723 struct l2cap_conn *conn = chan->conn; 724 725 BT_DBG("chan %p state %s", chan, state_to_string(chan->state)); 726 727 switch (chan->state) { 728 case BT_LISTEN: 729 chan->ops->teardown(chan, 0); 730 break; 731 732 case BT_CONNECTED: 733 case BT_CONFIG: 734 if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED) { 735 __set_chan_timer(chan, chan->ops->get_sndtimeo(chan)); 736 l2cap_send_disconn_req(chan, reason); 737 } else 738 l2cap_chan_del(chan, reason); 739 break; 740 741 case BT_CONNECT2: 742 if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED) { 743 if (conn->hcon->type == ACL_LINK) 744 l2cap_chan_connect_reject(chan); 745 else if (conn->hcon->type == LE_LINK) 746 l2cap_chan_le_connect_reject(chan); 747 } 748 749 l2cap_chan_del(chan, reason); 750 break; 751 752 case BT_CONNECT: 753 case BT_DISCONN: 754 l2cap_chan_del(chan, reason); 755 break; 756 757 default: 758 chan->ops->teardown(chan, 0); 759 break; 760 } 761 } 762 EXPORT_SYMBOL(l2cap_chan_close); 763 764 static inline u8 l2cap_get_auth_type(struct l2cap_chan *chan) 765 { 766 switch (chan->chan_type) { 767 case L2CAP_CHAN_RAW: 768 switch (chan->sec_level) { 769 case BT_SECURITY_HIGH: 770 case BT_SECURITY_FIPS: 771 return HCI_AT_DEDICATED_BONDING_MITM; 772 case BT_SECURITY_MEDIUM: 773 return HCI_AT_DEDICATED_BONDING; 774 default: 775 return HCI_AT_NO_BONDING; 776 } 777 break; 778 case L2CAP_CHAN_CONN_LESS: 779 if (chan->psm == cpu_to_le16(L2CAP_PSM_3DSP)) { 780 if (chan->sec_level == BT_SECURITY_LOW) 781 chan->sec_level = BT_SECURITY_SDP; 782 } 783 if (chan->sec_level == BT_SECURITY_HIGH || 784 chan->sec_level == BT_SECURITY_FIPS) 785 return HCI_AT_NO_BONDING_MITM; 786 else 787 return HCI_AT_NO_BONDING; 788 break; 789 case L2CAP_CHAN_CONN_ORIENTED: 790 if (chan->psm == cpu_to_le16(L2CAP_PSM_SDP)) { 791 if (chan->sec_level == BT_SECURITY_LOW) 792 chan->sec_level = BT_SECURITY_SDP; 793 794 if (chan->sec_level == BT_SECURITY_HIGH || 795 chan->sec_level == BT_SECURITY_FIPS) 796 return HCI_AT_NO_BONDING_MITM; 797 else 798 return HCI_AT_NO_BONDING; 799 } 800 /* fall through */ 801 default: 802 switch (chan->sec_level) { 803 case BT_SECURITY_HIGH: 804 case BT_SECURITY_FIPS: 805 return HCI_AT_GENERAL_BONDING_MITM; 806 case BT_SECURITY_MEDIUM: 807 return HCI_AT_GENERAL_BONDING; 808 default: 809 return HCI_AT_NO_BONDING; 810 } 811 break; 812 } 813 } 814 815 /* Service level security */ 816 int l2cap_chan_check_security(struct l2cap_chan *chan, bool initiator) 817 { 818 struct l2cap_conn *conn = chan->conn; 819 __u8 auth_type; 820 821 if (conn->hcon->type == LE_LINK) 822 return smp_conn_security(conn->hcon, chan->sec_level); 823 824 auth_type = l2cap_get_auth_type(chan); 825 826 return hci_conn_security(conn->hcon, chan->sec_level, auth_type, 827 initiator); 828 } 829 830 static u8 l2cap_get_ident(struct l2cap_conn *conn) 831 { 832 u8 id; 833 834 /* Get next available identificator. 835 * 1 - 128 are used by kernel. 836 * 129 - 199 are reserved. 837 * 200 - 254 are used by utilities like l2ping, etc. 838 */ 839 840 mutex_lock(&conn->ident_lock); 841 842 if (++conn->tx_ident > 128) 843 conn->tx_ident = 1; 844 845 id = conn->tx_ident; 846 847 mutex_unlock(&conn->ident_lock); 848 849 return id; 850 } 851 852 static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, 853 void *data) 854 { 855 struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data); 856 u8 flags; 857 858 BT_DBG("code 0x%2.2x", code); 859 860 if (!skb) 861 return; 862 863 /* Use NO_FLUSH if supported or we have an LE link (which does 864 * not support auto-flushing packets) */ 865 if (lmp_no_flush_capable(conn->hcon->hdev) || 866 conn->hcon->type == LE_LINK) 867 flags = ACL_START_NO_FLUSH; 868 else 869 flags = ACL_START; 870 871 bt_cb(skb)->force_active = BT_POWER_FORCE_ACTIVE_ON; 872 skb->priority = HCI_PRIO_MAX; 873 874 hci_send_acl(conn->hchan, skb, flags); 875 } 876 877 static bool __chan_is_moving(struct l2cap_chan *chan) 878 { 879 return chan->move_state != L2CAP_MOVE_STABLE && 880 chan->move_state != L2CAP_MOVE_WAIT_PREPARE; 881 } 882 883 static void l2cap_do_send(struct l2cap_chan *chan, struct sk_buff *skb) 884 { 885 struct hci_conn *hcon = chan->conn->hcon; 886 u16 flags; 887 888 BT_DBG("chan %p, skb %p len %d priority %u", chan, skb, skb->len, 889 skb->priority); 890 891 if (chan->hs_hcon && !__chan_is_moving(chan)) { 892 if (chan->hs_hchan) 893 hci_send_acl(chan->hs_hchan, skb, ACL_COMPLETE); 894 else 895 kfree_skb(skb); 896 897 return; 898 } 899 900 /* Use NO_FLUSH for LE links (where this is the only option) or 901 * if the BR/EDR link supports it and flushing has not been 902 * explicitly requested (through FLAG_FLUSHABLE). 903 */ 904 if (hcon->type == LE_LINK || 905 (!test_bit(FLAG_FLUSHABLE, &chan->flags) && 906 lmp_no_flush_capable(hcon->hdev))) 907 flags = ACL_START_NO_FLUSH; 908 else 909 flags = ACL_START; 910 911 bt_cb(skb)->force_active = test_bit(FLAG_FORCE_ACTIVE, &chan->flags); 912 hci_send_acl(chan->conn->hchan, skb, flags); 913 } 914 915 static void __unpack_enhanced_control(u16 enh, struct l2cap_ctrl *control) 916 { 917 control->reqseq = (enh & L2CAP_CTRL_REQSEQ) >> L2CAP_CTRL_REQSEQ_SHIFT; 918 control->final = (enh & L2CAP_CTRL_FINAL) >> L2CAP_CTRL_FINAL_SHIFT; 919 920 if (enh & L2CAP_CTRL_FRAME_TYPE) { 921 /* S-Frame */ 922 control->sframe = 1; 923 control->poll = (enh & L2CAP_CTRL_POLL) >> L2CAP_CTRL_POLL_SHIFT; 924 control->super = (enh & L2CAP_CTRL_SUPERVISE) >> L2CAP_CTRL_SUPER_SHIFT; 925 926 control->sar = 0; 927 control->txseq = 0; 928 } else { 929 /* I-Frame */ 930 control->sframe = 0; 931 control->sar = (enh & L2CAP_CTRL_SAR) >> L2CAP_CTRL_SAR_SHIFT; 932 control->txseq = (enh & L2CAP_CTRL_TXSEQ) >> L2CAP_CTRL_TXSEQ_SHIFT; 933 934 control->poll = 0; 935 control->super = 0; 936 } 937 } 938 939 static void __unpack_extended_control(u32 ext, struct l2cap_ctrl *control) 940 { 941 control->reqseq = (ext & L2CAP_EXT_CTRL_REQSEQ) >> L2CAP_EXT_CTRL_REQSEQ_SHIFT; 942 control->final = (ext & L2CAP_EXT_CTRL_FINAL) >> L2CAP_EXT_CTRL_FINAL_SHIFT; 943 944 if (ext & L2CAP_EXT_CTRL_FRAME_TYPE) { 945 /* S-Frame */ 946 control->sframe = 1; 947 control->poll = (ext & L2CAP_EXT_CTRL_POLL) >> L2CAP_EXT_CTRL_POLL_SHIFT; 948 control->super = (ext & L2CAP_EXT_CTRL_SUPERVISE) >> L2CAP_EXT_CTRL_SUPER_SHIFT; 949 950 control->sar = 0; 951 control->txseq = 0; 952 } else { 953 /* I-Frame */ 954 control->sframe = 0; 955 control->sar = (ext & L2CAP_EXT_CTRL_SAR) >> L2CAP_EXT_CTRL_SAR_SHIFT; 956 control->txseq = (ext & L2CAP_EXT_CTRL_TXSEQ) >> L2CAP_EXT_CTRL_TXSEQ_SHIFT; 957 958 control->poll = 0; 959 control->super = 0; 960 } 961 } 962 963 static inline void __unpack_control(struct l2cap_chan *chan, 964 struct sk_buff *skb) 965 { 966 if (test_bit(FLAG_EXT_CTRL, &chan->flags)) { 967 __unpack_extended_control(get_unaligned_le32(skb->data), 968 &bt_cb(skb)->l2cap); 969 skb_pull(skb, L2CAP_EXT_CTRL_SIZE); 970 } else { 971 __unpack_enhanced_control(get_unaligned_le16(skb->data), 972 &bt_cb(skb)->l2cap); 973 skb_pull(skb, L2CAP_ENH_CTRL_SIZE); 974 } 975 } 976 977 static u32 __pack_extended_control(struct l2cap_ctrl *control) 978 { 979 u32 packed; 980 981 packed = control->reqseq << L2CAP_EXT_CTRL_REQSEQ_SHIFT; 982 packed |= control->final << L2CAP_EXT_CTRL_FINAL_SHIFT; 983 984 if (control->sframe) { 985 packed |= control->poll << L2CAP_EXT_CTRL_POLL_SHIFT; 986 packed |= control->super << L2CAP_EXT_CTRL_SUPER_SHIFT; 987 packed |= L2CAP_EXT_CTRL_FRAME_TYPE; 988 } else { 989 packed |= control->sar << L2CAP_EXT_CTRL_SAR_SHIFT; 990 packed |= control->txseq << L2CAP_EXT_CTRL_TXSEQ_SHIFT; 991 } 992 993 return packed; 994 } 995 996 static u16 __pack_enhanced_control(struct l2cap_ctrl *control) 997 { 998 u16 packed; 999 1000 packed = control->reqseq << L2CAP_CTRL_REQSEQ_SHIFT; 1001 packed |= control->final << L2CAP_CTRL_FINAL_SHIFT; 1002 1003 if (control->sframe) { 1004 packed |= control->poll << L2CAP_CTRL_POLL_SHIFT; 1005 packed |= control->super << L2CAP_CTRL_SUPER_SHIFT; 1006 packed |= L2CAP_CTRL_FRAME_TYPE; 1007 } else { 1008 packed |= control->sar << L2CAP_CTRL_SAR_SHIFT; 1009 packed |= control->txseq << L2CAP_CTRL_TXSEQ_SHIFT; 1010 } 1011 1012 return packed; 1013 } 1014 1015 static inline void __pack_control(struct l2cap_chan *chan, 1016 struct l2cap_ctrl *control, 1017 struct sk_buff *skb) 1018 { 1019 if (test_bit(FLAG_EXT_CTRL, &chan->flags)) { 1020 put_unaligned_le32(__pack_extended_control(control), 1021 skb->data + L2CAP_HDR_SIZE); 1022 } else { 1023 put_unaligned_le16(__pack_enhanced_control(control), 1024 skb->data + L2CAP_HDR_SIZE); 1025 } 1026 } 1027 1028 static inline unsigned int __ertm_hdr_size(struct l2cap_chan *chan) 1029 { 1030 if (test_bit(FLAG_EXT_CTRL, &chan->flags)) 1031 return L2CAP_EXT_HDR_SIZE; 1032 else 1033 return L2CAP_ENH_HDR_SIZE; 1034 } 1035 1036 static struct sk_buff *l2cap_create_sframe_pdu(struct l2cap_chan *chan, 1037 u32 control) 1038 { 1039 struct sk_buff *skb; 1040 struct l2cap_hdr *lh; 1041 int hlen = __ertm_hdr_size(chan); 1042 1043 if (chan->fcs == L2CAP_FCS_CRC16) 1044 hlen += L2CAP_FCS_SIZE; 1045 1046 skb = bt_skb_alloc(hlen, GFP_KERNEL); 1047 1048 if (!skb) 1049 return ERR_PTR(-ENOMEM); 1050 1051 lh = skb_put(skb, L2CAP_HDR_SIZE); 1052 lh->len = cpu_to_le16(hlen - L2CAP_HDR_SIZE); 1053 lh->cid = cpu_to_le16(chan->dcid); 1054 1055 if (test_bit(FLAG_EXT_CTRL, &chan->flags)) 1056 put_unaligned_le32(control, skb_put(skb, L2CAP_EXT_CTRL_SIZE)); 1057 else 1058 put_unaligned_le16(control, skb_put(skb, L2CAP_ENH_CTRL_SIZE)); 1059 1060 if (chan->fcs == L2CAP_FCS_CRC16) { 1061 u16 fcs = crc16(0, (u8 *)skb->data, skb->len); 1062 put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE)); 1063 } 1064 1065 skb->priority = HCI_PRIO_MAX; 1066 return skb; 1067 } 1068 1069 static void l2cap_send_sframe(struct l2cap_chan *chan, 1070 struct l2cap_ctrl *control) 1071 { 1072 struct sk_buff *skb; 1073 u32 control_field; 1074 1075 BT_DBG("chan %p, control %p", chan, control); 1076 1077 if (!control->sframe) 1078 return; 1079 1080 if (__chan_is_moving(chan)) 1081 return; 1082 1083 if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state) && 1084 !control->poll) 1085 control->final = 1; 1086 1087 if (control->super == L2CAP_SUPER_RR) 1088 clear_bit(CONN_RNR_SENT, &chan->conn_state); 1089 else if (control->super == L2CAP_SUPER_RNR) 1090 set_bit(CONN_RNR_SENT, &chan->conn_state); 1091 1092 if (control->super != L2CAP_SUPER_SREJ) { 1093 chan->last_acked_seq = control->reqseq; 1094 __clear_ack_timer(chan); 1095 } 1096 1097 BT_DBG("reqseq %d, final %d, poll %d, super %d", control->reqseq, 1098 control->final, control->poll, control->super); 1099 1100 if (test_bit(FLAG_EXT_CTRL, &chan->flags)) 1101 control_field = __pack_extended_control(control); 1102 else 1103 control_field = __pack_enhanced_control(control); 1104 1105 skb = l2cap_create_sframe_pdu(chan, control_field); 1106 if (!IS_ERR(skb)) 1107 l2cap_do_send(chan, skb); 1108 } 1109 1110 static void l2cap_send_rr_or_rnr(struct l2cap_chan *chan, bool poll) 1111 { 1112 struct l2cap_ctrl control; 1113 1114 BT_DBG("chan %p, poll %d", chan, poll); 1115 1116 memset(&control, 0, sizeof(control)); 1117 control.sframe = 1; 1118 control.poll = poll; 1119 1120 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) 1121 control.super = L2CAP_SUPER_RNR; 1122 else 1123 control.super = L2CAP_SUPER_RR; 1124 1125 control.reqseq = chan->buffer_seq; 1126 l2cap_send_sframe(chan, &control); 1127 } 1128 1129 static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan) 1130 { 1131 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) 1132 return true; 1133 1134 return !test_bit(CONF_CONNECT_PEND, &chan->conf_state); 1135 } 1136 1137 static bool __amp_capable(struct l2cap_chan *chan) 1138 { 1139 struct l2cap_conn *conn = chan->conn; 1140 struct hci_dev *hdev; 1141 bool amp_available = false; 1142 1143 if (!(conn->local_fixed_chan & L2CAP_FC_A2MP)) 1144 return false; 1145 1146 if (!(conn->remote_fixed_chan & L2CAP_FC_A2MP)) 1147 return false; 1148 1149 read_lock(&hci_dev_list_lock); 1150 list_for_each_entry(hdev, &hci_dev_list, list) { 1151 if (hdev->amp_type != AMP_TYPE_BREDR && 1152 test_bit(HCI_UP, &hdev->flags)) { 1153 amp_available = true; 1154 break; 1155 } 1156 } 1157 read_unlock(&hci_dev_list_lock); 1158 1159 if (chan->chan_policy == BT_CHANNEL_POLICY_AMP_PREFERRED) 1160 return amp_available; 1161 1162 return false; 1163 } 1164 1165 static bool l2cap_check_efs(struct l2cap_chan *chan) 1166 { 1167 /* Check EFS parameters */ 1168 return true; 1169 } 1170 1171 void l2cap_send_conn_req(struct l2cap_chan *chan) 1172 { 1173 struct l2cap_conn *conn = chan->conn; 1174 struct l2cap_conn_req req; 1175 1176 req.scid = cpu_to_le16(chan->scid); 1177 req.psm = chan->psm; 1178 1179 chan->ident = l2cap_get_ident(conn); 1180 1181 set_bit(CONF_CONNECT_PEND, &chan->conf_state); 1182 1183 l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_REQ, sizeof(req), &req); 1184 } 1185 1186 static void l2cap_send_create_chan_req(struct l2cap_chan *chan, u8 amp_id) 1187 { 1188 struct l2cap_create_chan_req req; 1189 req.scid = cpu_to_le16(chan->scid); 1190 req.psm = chan->psm; 1191 req.amp_id = amp_id; 1192 1193 chan->ident = l2cap_get_ident(chan->conn); 1194 1195 l2cap_send_cmd(chan->conn, chan->ident, L2CAP_CREATE_CHAN_REQ, 1196 sizeof(req), &req); 1197 } 1198 1199 static void l2cap_move_setup(struct l2cap_chan *chan) 1200 { 1201 struct sk_buff *skb; 1202 1203 BT_DBG("chan %p", chan); 1204 1205 if (chan->mode != L2CAP_MODE_ERTM) 1206 return; 1207 1208 __clear_retrans_timer(chan); 1209 __clear_monitor_timer(chan); 1210 __clear_ack_timer(chan); 1211 1212 chan->retry_count = 0; 1213 skb_queue_walk(&chan->tx_q, skb) { 1214 if (bt_cb(skb)->l2cap.retries) 1215 bt_cb(skb)->l2cap.retries = 1; 1216 else 1217 break; 1218 } 1219 1220 chan->expected_tx_seq = chan->buffer_seq; 1221 1222 clear_bit(CONN_REJ_ACT, &chan->conn_state); 1223 clear_bit(CONN_SREJ_ACT, &chan->conn_state); 1224 l2cap_seq_list_clear(&chan->retrans_list); 1225 l2cap_seq_list_clear(&chan->srej_list); 1226 skb_queue_purge(&chan->srej_q); 1227 1228 chan->tx_state = L2CAP_TX_STATE_XMIT; 1229 chan->rx_state = L2CAP_RX_STATE_MOVE; 1230 1231 set_bit(CONN_REMOTE_BUSY, &chan->conn_state); 1232 } 1233 1234 static void l2cap_move_done(struct l2cap_chan *chan) 1235 { 1236 u8 move_role = chan->move_role; 1237 BT_DBG("chan %p", chan); 1238 1239 chan->move_state = L2CAP_MOVE_STABLE; 1240 chan->move_role = L2CAP_MOVE_ROLE_NONE; 1241 1242 if (chan->mode != L2CAP_MODE_ERTM) 1243 return; 1244 1245 switch (move_role) { 1246 case L2CAP_MOVE_ROLE_INITIATOR: 1247 l2cap_tx(chan, NULL, NULL, L2CAP_EV_EXPLICIT_POLL); 1248 chan->rx_state = L2CAP_RX_STATE_WAIT_F; 1249 break; 1250 case L2CAP_MOVE_ROLE_RESPONDER: 1251 chan->rx_state = L2CAP_RX_STATE_WAIT_P; 1252 break; 1253 } 1254 } 1255 1256 static void l2cap_chan_ready(struct l2cap_chan *chan) 1257 { 1258 /* The channel may have already been flagged as connected in 1259 * case of receiving data before the L2CAP info req/rsp 1260 * procedure is complete. 1261 */ 1262 if (chan->state == BT_CONNECTED) 1263 return; 1264 1265 /* This clears all conf flags, including CONF_NOT_COMPLETE */ 1266 chan->conf_state = 0; 1267 __clear_chan_timer(chan); 1268 1269 if (chan->mode == L2CAP_MODE_LE_FLOWCTL && !chan->tx_credits) 1270 chan->ops->suspend(chan); 1271 1272 chan->state = BT_CONNECTED; 1273 1274 chan->ops->ready(chan); 1275 } 1276 1277 static void l2cap_le_connect(struct l2cap_chan *chan) 1278 { 1279 struct l2cap_conn *conn = chan->conn; 1280 struct l2cap_le_conn_req req; 1281 1282 if (test_and_set_bit(FLAG_LE_CONN_REQ_SENT, &chan->flags)) 1283 return; 1284 1285 req.psm = chan->psm; 1286 req.scid = cpu_to_le16(chan->scid); 1287 req.mtu = cpu_to_le16(chan->imtu); 1288 req.mps = cpu_to_le16(chan->mps); 1289 req.credits = cpu_to_le16(chan->rx_credits); 1290 1291 chan->ident = l2cap_get_ident(conn); 1292 1293 l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CONN_REQ, 1294 sizeof(req), &req); 1295 } 1296 1297 static void l2cap_le_start(struct l2cap_chan *chan) 1298 { 1299 struct l2cap_conn *conn = chan->conn; 1300 1301 if (!smp_conn_security(conn->hcon, chan->sec_level)) 1302 return; 1303 1304 if (!chan->psm) { 1305 l2cap_chan_ready(chan); 1306 return; 1307 } 1308 1309 if (chan->state == BT_CONNECT) 1310 l2cap_le_connect(chan); 1311 } 1312 1313 static void l2cap_start_connection(struct l2cap_chan *chan) 1314 { 1315 if (__amp_capable(chan)) { 1316 BT_DBG("chan %p AMP capable: discover AMPs", chan); 1317 a2mp_discover_amp(chan); 1318 } else if (chan->conn->hcon->type == LE_LINK) { 1319 l2cap_le_start(chan); 1320 } else { 1321 l2cap_send_conn_req(chan); 1322 } 1323 } 1324 1325 static void l2cap_request_info(struct l2cap_conn *conn) 1326 { 1327 struct l2cap_info_req req; 1328 1329 if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) 1330 return; 1331 1332 req.type = cpu_to_le16(L2CAP_IT_FEAT_MASK); 1333 1334 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT; 1335 conn->info_ident = l2cap_get_ident(conn); 1336 1337 schedule_delayed_work(&conn->info_timer, L2CAP_INFO_TIMEOUT); 1338 1339 l2cap_send_cmd(conn, conn->info_ident, L2CAP_INFO_REQ, 1340 sizeof(req), &req); 1341 } 1342 1343 static void l2cap_do_start(struct l2cap_chan *chan) 1344 { 1345 struct l2cap_conn *conn = chan->conn; 1346 1347 if (conn->hcon->type == LE_LINK) { 1348 l2cap_le_start(chan); 1349 return; 1350 } 1351 1352 if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT)) { 1353 l2cap_request_info(conn); 1354 return; 1355 } 1356 1357 if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)) 1358 return; 1359 1360 if (l2cap_chan_check_security(chan, true) && 1361 __l2cap_no_conn_pending(chan)) 1362 l2cap_start_connection(chan); 1363 } 1364 1365 static inline int l2cap_mode_supported(__u8 mode, __u32 feat_mask) 1366 { 1367 u32 local_feat_mask = l2cap_feat_mask; 1368 if (!disable_ertm) 1369 local_feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING; 1370 1371 switch (mode) { 1372 case L2CAP_MODE_ERTM: 1373 return L2CAP_FEAT_ERTM & feat_mask & local_feat_mask; 1374 case L2CAP_MODE_STREAMING: 1375 return L2CAP_FEAT_STREAMING & feat_mask & local_feat_mask; 1376 default: 1377 return 0x00; 1378 } 1379 } 1380 1381 static void l2cap_send_disconn_req(struct l2cap_chan *chan, int err) 1382 { 1383 struct l2cap_conn *conn = chan->conn; 1384 struct l2cap_disconn_req req; 1385 1386 if (!conn) 1387 return; 1388 1389 if (chan->mode == L2CAP_MODE_ERTM && chan->state == BT_CONNECTED) { 1390 __clear_retrans_timer(chan); 1391 __clear_monitor_timer(chan); 1392 __clear_ack_timer(chan); 1393 } 1394 1395 if (chan->scid == L2CAP_CID_A2MP) { 1396 l2cap_state_change(chan, BT_DISCONN); 1397 return; 1398 } 1399 1400 req.dcid = cpu_to_le16(chan->dcid); 1401 req.scid = cpu_to_le16(chan->scid); 1402 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_DISCONN_REQ, 1403 sizeof(req), &req); 1404 1405 l2cap_state_change_and_error(chan, BT_DISCONN, err); 1406 } 1407 1408 /* ---- L2CAP connections ---- */ 1409 static void l2cap_conn_start(struct l2cap_conn *conn) 1410 { 1411 struct l2cap_chan *chan, *tmp; 1412 1413 BT_DBG("conn %p", conn); 1414 1415 mutex_lock(&conn->chan_lock); 1416 1417 list_for_each_entry_safe(chan, tmp, &conn->chan_l, list) { 1418 l2cap_chan_lock(chan); 1419 1420 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) { 1421 l2cap_chan_ready(chan); 1422 l2cap_chan_unlock(chan); 1423 continue; 1424 } 1425 1426 if (chan->state == BT_CONNECT) { 1427 if (!l2cap_chan_check_security(chan, true) || 1428 !__l2cap_no_conn_pending(chan)) { 1429 l2cap_chan_unlock(chan); 1430 continue; 1431 } 1432 1433 if (!l2cap_mode_supported(chan->mode, conn->feat_mask) 1434 && test_bit(CONF_STATE2_DEVICE, 1435 &chan->conf_state)) { 1436 l2cap_chan_close(chan, ECONNRESET); 1437 l2cap_chan_unlock(chan); 1438 continue; 1439 } 1440 1441 l2cap_start_connection(chan); 1442 1443 } else if (chan->state == BT_CONNECT2) { 1444 struct l2cap_conn_rsp rsp; 1445 char buf[128]; 1446 rsp.scid = cpu_to_le16(chan->dcid); 1447 rsp.dcid = cpu_to_le16(chan->scid); 1448 1449 if (l2cap_chan_check_security(chan, false)) { 1450 if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) { 1451 rsp.result = cpu_to_le16(L2CAP_CR_PEND); 1452 rsp.status = cpu_to_le16(L2CAP_CS_AUTHOR_PEND); 1453 chan->ops->defer(chan); 1454 1455 } else { 1456 l2cap_state_change(chan, BT_CONFIG); 1457 rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS); 1458 rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); 1459 } 1460 } else { 1461 rsp.result = cpu_to_le16(L2CAP_CR_PEND); 1462 rsp.status = cpu_to_le16(L2CAP_CS_AUTHEN_PEND); 1463 } 1464 1465 l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP, 1466 sizeof(rsp), &rsp); 1467 1468 if (test_bit(CONF_REQ_SENT, &chan->conf_state) || 1469 rsp.result != L2CAP_CR_SUCCESS) { 1470 l2cap_chan_unlock(chan); 1471 continue; 1472 } 1473 1474 set_bit(CONF_REQ_SENT, &chan->conf_state); 1475 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, 1476 l2cap_build_conf_req(chan, buf, sizeof(buf)), buf); 1477 chan->num_conf_req++; 1478 } 1479 1480 l2cap_chan_unlock(chan); 1481 } 1482 1483 mutex_unlock(&conn->chan_lock); 1484 } 1485 1486 static void l2cap_le_conn_ready(struct l2cap_conn *conn) 1487 { 1488 struct hci_conn *hcon = conn->hcon; 1489 struct hci_dev *hdev = hcon->hdev; 1490 1491 BT_DBG("%s conn %p", hdev->name, conn); 1492 1493 /* For outgoing pairing which doesn't necessarily have an 1494 * associated socket (e.g. mgmt_pair_device). 1495 */ 1496 if (hcon->out) 1497 smp_conn_security(hcon, hcon->pending_sec_level); 1498 1499 /* For LE slave connections, make sure the connection interval 1500 * is in the range of the minium and maximum interval that has 1501 * been configured for this connection. If not, then trigger 1502 * the connection update procedure. 1503 */ 1504 if (hcon->role == HCI_ROLE_SLAVE && 1505 (hcon->le_conn_interval < hcon->le_conn_min_interval || 1506 hcon->le_conn_interval > hcon->le_conn_max_interval)) { 1507 struct l2cap_conn_param_update_req req; 1508 1509 req.min = cpu_to_le16(hcon->le_conn_min_interval); 1510 req.max = cpu_to_le16(hcon->le_conn_max_interval); 1511 req.latency = cpu_to_le16(hcon->le_conn_latency); 1512 req.to_multiplier = cpu_to_le16(hcon->le_supv_timeout); 1513 1514 l2cap_send_cmd(conn, l2cap_get_ident(conn), 1515 L2CAP_CONN_PARAM_UPDATE_REQ, sizeof(req), &req); 1516 } 1517 } 1518 1519 static void l2cap_conn_ready(struct l2cap_conn *conn) 1520 { 1521 struct l2cap_chan *chan; 1522 struct hci_conn *hcon = conn->hcon; 1523 1524 BT_DBG("conn %p", conn); 1525 1526 if (hcon->type == ACL_LINK) 1527 l2cap_request_info(conn); 1528 1529 mutex_lock(&conn->chan_lock); 1530 1531 list_for_each_entry(chan, &conn->chan_l, list) { 1532 1533 l2cap_chan_lock(chan); 1534 1535 if (chan->scid == L2CAP_CID_A2MP) { 1536 l2cap_chan_unlock(chan); 1537 continue; 1538 } 1539 1540 if (hcon->type == LE_LINK) { 1541 l2cap_le_start(chan); 1542 } else if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) { 1543 if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) 1544 l2cap_chan_ready(chan); 1545 } else if (chan->state == BT_CONNECT) { 1546 l2cap_do_start(chan); 1547 } 1548 1549 l2cap_chan_unlock(chan); 1550 } 1551 1552 mutex_unlock(&conn->chan_lock); 1553 1554 if (hcon->type == LE_LINK) 1555 l2cap_le_conn_ready(conn); 1556 1557 queue_work(hcon->hdev->workqueue, &conn->pending_rx_work); 1558 } 1559 1560 /* Notify sockets that we cannot guaranty reliability anymore */ 1561 static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err) 1562 { 1563 struct l2cap_chan *chan; 1564 1565 BT_DBG("conn %p", conn); 1566 1567 mutex_lock(&conn->chan_lock); 1568 1569 list_for_each_entry(chan, &conn->chan_l, list) { 1570 if (test_bit(FLAG_FORCE_RELIABLE, &chan->flags)) 1571 l2cap_chan_set_err(chan, err); 1572 } 1573 1574 mutex_unlock(&conn->chan_lock); 1575 } 1576 1577 static void l2cap_info_timeout(struct work_struct *work) 1578 { 1579 struct l2cap_conn *conn = container_of(work, struct l2cap_conn, 1580 info_timer.work); 1581 1582 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; 1583 conn->info_ident = 0; 1584 1585 l2cap_conn_start(conn); 1586 } 1587 1588 /* 1589 * l2cap_user 1590 * External modules can register l2cap_user objects on l2cap_conn. The ->probe 1591 * callback is called during registration. The ->remove callback is called 1592 * during unregistration. 1593 * An l2cap_user object can either be explicitly unregistered or when the 1594 * underlying l2cap_conn object is deleted. This guarantees that l2cap->hcon, 1595 * l2cap->hchan, .. are valid as long as the remove callback hasn't been called. 1596 * External modules must own a reference to the l2cap_conn object if they intend 1597 * to call l2cap_unregister_user(). The l2cap_conn object might get destroyed at 1598 * any time if they don't. 1599 */ 1600 1601 int l2cap_register_user(struct l2cap_conn *conn, struct l2cap_user *user) 1602 { 1603 struct hci_dev *hdev = conn->hcon->hdev; 1604 int ret; 1605 1606 /* We need to check whether l2cap_conn is registered. If it is not, we 1607 * must not register the l2cap_user. l2cap_conn_del() is unregisters 1608 * l2cap_conn objects, but doesn't provide its own locking. Instead, it 1609 * relies on the parent hci_conn object to be locked. This itself relies 1610 * on the hci_dev object to be locked. So we must lock the hci device 1611 * here, too. */ 1612 1613 hci_dev_lock(hdev); 1614 1615 if (!list_empty(&user->list)) { 1616 ret = -EINVAL; 1617 goto out_unlock; 1618 } 1619 1620 /* conn->hchan is NULL after l2cap_conn_del() was called */ 1621 if (!conn->hchan) { 1622 ret = -ENODEV; 1623 goto out_unlock; 1624 } 1625 1626 ret = user->probe(conn, user); 1627 if (ret) 1628 goto out_unlock; 1629 1630 list_add(&user->list, &conn->users); 1631 ret = 0; 1632 1633 out_unlock: 1634 hci_dev_unlock(hdev); 1635 return ret; 1636 } 1637 EXPORT_SYMBOL(l2cap_register_user); 1638 1639 void l2cap_unregister_user(struct l2cap_conn *conn, struct l2cap_user *user) 1640 { 1641 struct hci_dev *hdev = conn->hcon->hdev; 1642 1643 hci_dev_lock(hdev); 1644 1645 if (list_empty(&user->list)) 1646 goto out_unlock; 1647 1648 list_del_init(&user->list); 1649 user->remove(conn, user); 1650 1651 out_unlock: 1652 hci_dev_unlock(hdev); 1653 } 1654 EXPORT_SYMBOL(l2cap_unregister_user); 1655 1656 static void l2cap_unregister_all_users(struct l2cap_conn *conn) 1657 { 1658 struct l2cap_user *user; 1659 1660 while (!list_empty(&conn->users)) { 1661 user = list_first_entry(&conn->users, struct l2cap_user, list); 1662 list_del_init(&user->list); 1663 user->remove(conn, user); 1664 } 1665 } 1666 1667 static void l2cap_conn_del(struct hci_conn *hcon, int err) 1668 { 1669 struct l2cap_conn *conn = hcon->l2cap_data; 1670 struct l2cap_chan *chan, *l; 1671 1672 if (!conn) 1673 return; 1674 1675 BT_DBG("hcon %p conn %p, err %d", hcon, conn, err); 1676 1677 kfree_skb(conn->rx_skb); 1678 1679 skb_queue_purge(&conn->pending_rx); 1680 1681 /* We can not call flush_work(&conn->pending_rx_work) here since we 1682 * might block if we are running on a worker from the same workqueue 1683 * pending_rx_work is waiting on. 1684 */ 1685 if (work_pending(&conn->pending_rx_work)) 1686 cancel_work_sync(&conn->pending_rx_work); 1687 1688 if (work_pending(&conn->id_addr_update_work)) 1689 cancel_work_sync(&conn->id_addr_update_work); 1690 1691 l2cap_unregister_all_users(conn); 1692 1693 /* Force the connection to be immediately dropped */ 1694 hcon->disc_timeout = 0; 1695 1696 mutex_lock(&conn->chan_lock); 1697 1698 /* Kill channels */ 1699 list_for_each_entry_safe(chan, l, &conn->chan_l, list) { 1700 l2cap_chan_hold(chan); 1701 l2cap_chan_lock(chan); 1702 1703 l2cap_chan_del(chan, err); 1704 1705 l2cap_chan_unlock(chan); 1706 1707 chan->ops->close(chan); 1708 l2cap_chan_put(chan); 1709 } 1710 1711 mutex_unlock(&conn->chan_lock); 1712 1713 hci_chan_del(conn->hchan); 1714 1715 if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) 1716 cancel_delayed_work_sync(&conn->info_timer); 1717 1718 hcon->l2cap_data = NULL; 1719 conn->hchan = NULL; 1720 l2cap_conn_put(conn); 1721 } 1722 1723 static void l2cap_conn_free(struct kref *ref) 1724 { 1725 struct l2cap_conn *conn = container_of(ref, struct l2cap_conn, ref); 1726 1727 hci_conn_put(conn->hcon); 1728 kfree(conn); 1729 } 1730 1731 struct l2cap_conn *l2cap_conn_get(struct l2cap_conn *conn) 1732 { 1733 kref_get(&conn->ref); 1734 return conn; 1735 } 1736 EXPORT_SYMBOL(l2cap_conn_get); 1737 1738 void l2cap_conn_put(struct l2cap_conn *conn) 1739 { 1740 kref_put(&conn->ref, l2cap_conn_free); 1741 } 1742 EXPORT_SYMBOL(l2cap_conn_put); 1743 1744 /* ---- Socket interface ---- */ 1745 1746 /* Find socket with psm and source / destination bdaddr. 1747 * Returns closest match. 1748 */ 1749 static struct l2cap_chan *l2cap_global_chan_by_psm(int state, __le16 psm, 1750 bdaddr_t *src, 1751 bdaddr_t *dst, 1752 u8 link_type) 1753 { 1754 struct l2cap_chan *c, *c1 = NULL; 1755 1756 read_lock(&chan_list_lock); 1757 1758 list_for_each_entry(c, &chan_list, global_l) { 1759 if (state && c->state != state) 1760 continue; 1761 1762 if (link_type == ACL_LINK && c->src_type != BDADDR_BREDR) 1763 continue; 1764 1765 if (link_type == LE_LINK && c->src_type == BDADDR_BREDR) 1766 continue; 1767 1768 if (c->psm == psm) { 1769 int src_match, dst_match; 1770 int src_any, dst_any; 1771 1772 /* Exact match. */ 1773 src_match = !bacmp(&c->src, src); 1774 dst_match = !bacmp(&c->dst, dst); 1775 if (src_match && dst_match) { 1776 l2cap_chan_hold(c); 1777 read_unlock(&chan_list_lock); 1778 return c; 1779 } 1780 1781 /* Closest match */ 1782 src_any = !bacmp(&c->src, BDADDR_ANY); 1783 dst_any = !bacmp(&c->dst, BDADDR_ANY); 1784 if ((src_match && dst_any) || (src_any && dst_match) || 1785 (src_any && dst_any)) 1786 c1 = c; 1787 } 1788 } 1789 1790 if (c1) 1791 l2cap_chan_hold(c1); 1792 1793 read_unlock(&chan_list_lock); 1794 1795 return c1; 1796 } 1797 1798 static void l2cap_monitor_timeout(struct work_struct *work) 1799 { 1800 struct l2cap_chan *chan = container_of(work, struct l2cap_chan, 1801 monitor_timer.work); 1802 1803 BT_DBG("chan %p", chan); 1804 1805 l2cap_chan_lock(chan); 1806 1807 if (!chan->conn) { 1808 l2cap_chan_unlock(chan); 1809 l2cap_chan_put(chan); 1810 return; 1811 } 1812 1813 l2cap_tx(chan, NULL, NULL, L2CAP_EV_MONITOR_TO); 1814 1815 l2cap_chan_unlock(chan); 1816 l2cap_chan_put(chan); 1817 } 1818 1819 static void l2cap_retrans_timeout(struct work_struct *work) 1820 { 1821 struct l2cap_chan *chan = container_of(work, struct l2cap_chan, 1822 retrans_timer.work); 1823 1824 BT_DBG("chan %p", chan); 1825 1826 l2cap_chan_lock(chan); 1827 1828 if (!chan->conn) { 1829 l2cap_chan_unlock(chan); 1830 l2cap_chan_put(chan); 1831 return; 1832 } 1833 1834 l2cap_tx(chan, NULL, NULL, L2CAP_EV_RETRANS_TO); 1835 l2cap_chan_unlock(chan); 1836 l2cap_chan_put(chan); 1837 } 1838 1839 static void l2cap_streaming_send(struct l2cap_chan *chan, 1840 struct sk_buff_head *skbs) 1841 { 1842 struct sk_buff *skb; 1843 struct l2cap_ctrl *control; 1844 1845 BT_DBG("chan %p, skbs %p", chan, skbs); 1846 1847 if (__chan_is_moving(chan)) 1848 return; 1849 1850 skb_queue_splice_tail_init(skbs, &chan->tx_q); 1851 1852 while (!skb_queue_empty(&chan->tx_q)) { 1853 1854 skb = skb_dequeue(&chan->tx_q); 1855 1856 bt_cb(skb)->l2cap.retries = 1; 1857 control = &bt_cb(skb)->l2cap; 1858 1859 control->reqseq = 0; 1860 control->txseq = chan->next_tx_seq; 1861 1862 __pack_control(chan, control, skb); 1863 1864 if (chan->fcs == L2CAP_FCS_CRC16) { 1865 u16 fcs = crc16(0, (u8 *) skb->data, skb->len); 1866 put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE)); 1867 } 1868 1869 l2cap_do_send(chan, skb); 1870 1871 BT_DBG("Sent txseq %u", control->txseq); 1872 1873 chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq); 1874 chan->frames_sent++; 1875 } 1876 } 1877 1878 static int l2cap_ertm_send(struct l2cap_chan *chan) 1879 { 1880 struct sk_buff *skb, *tx_skb; 1881 struct l2cap_ctrl *control; 1882 int sent = 0; 1883 1884 BT_DBG("chan %p", chan); 1885 1886 if (chan->state != BT_CONNECTED) 1887 return -ENOTCONN; 1888 1889 if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state)) 1890 return 0; 1891 1892 if (__chan_is_moving(chan)) 1893 return 0; 1894 1895 while (chan->tx_send_head && 1896 chan->unacked_frames < chan->remote_tx_win && 1897 chan->tx_state == L2CAP_TX_STATE_XMIT) { 1898 1899 skb = chan->tx_send_head; 1900 1901 bt_cb(skb)->l2cap.retries = 1; 1902 control = &bt_cb(skb)->l2cap; 1903 1904 if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state)) 1905 control->final = 1; 1906 1907 control->reqseq = chan->buffer_seq; 1908 chan->last_acked_seq = chan->buffer_seq; 1909 control->txseq = chan->next_tx_seq; 1910 1911 __pack_control(chan, control, skb); 1912 1913 if (chan->fcs == L2CAP_FCS_CRC16) { 1914 u16 fcs = crc16(0, (u8 *) skb->data, skb->len); 1915 put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE)); 1916 } 1917 1918 /* Clone after data has been modified. Data is assumed to be 1919 read-only (for locking purposes) on cloned sk_buffs. 1920 */ 1921 tx_skb = skb_clone(skb, GFP_KERNEL); 1922 1923 if (!tx_skb) 1924 break; 1925 1926 __set_retrans_timer(chan); 1927 1928 chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq); 1929 chan->unacked_frames++; 1930 chan->frames_sent++; 1931 sent++; 1932 1933 if (skb_queue_is_last(&chan->tx_q, skb)) 1934 chan->tx_send_head = NULL; 1935 else 1936 chan->tx_send_head = skb_queue_next(&chan->tx_q, skb); 1937 1938 l2cap_do_send(chan, tx_skb); 1939 BT_DBG("Sent txseq %u", control->txseq); 1940 } 1941 1942 BT_DBG("Sent %d, %u unacked, %u in ERTM queue", sent, 1943 chan->unacked_frames, skb_queue_len(&chan->tx_q)); 1944 1945 return sent; 1946 } 1947 1948 static void l2cap_ertm_resend(struct l2cap_chan *chan) 1949 { 1950 struct l2cap_ctrl control; 1951 struct sk_buff *skb; 1952 struct sk_buff *tx_skb; 1953 u16 seq; 1954 1955 BT_DBG("chan %p", chan); 1956 1957 if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state)) 1958 return; 1959 1960 if (__chan_is_moving(chan)) 1961 return; 1962 1963 while (chan->retrans_list.head != L2CAP_SEQ_LIST_CLEAR) { 1964 seq = l2cap_seq_list_pop(&chan->retrans_list); 1965 1966 skb = l2cap_ertm_seq_in_queue(&chan->tx_q, seq); 1967 if (!skb) { 1968 BT_DBG("Error: Can't retransmit seq %d, frame missing", 1969 seq); 1970 continue; 1971 } 1972 1973 bt_cb(skb)->l2cap.retries++; 1974 control = bt_cb(skb)->l2cap; 1975 1976 if (chan->max_tx != 0 && 1977 bt_cb(skb)->l2cap.retries > chan->max_tx) { 1978 BT_DBG("Retry limit exceeded (%d)", chan->max_tx); 1979 l2cap_send_disconn_req(chan, ECONNRESET); 1980 l2cap_seq_list_clear(&chan->retrans_list); 1981 break; 1982 } 1983 1984 control.reqseq = chan->buffer_seq; 1985 if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state)) 1986 control.final = 1; 1987 else 1988 control.final = 0; 1989 1990 if (skb_cloned(skb)) { 1991 /* Cloned sk_buffs are read-only, so we need a 1992 * writeable copy 1993 */ 1994 tx_skb = skb_copy(skb, GFP_KERNEL); 1995 } else { 1996 tx_skb = skb_clone(skb, GFP_KERNEL); 1997 } 1998 1999 if (!tx_skb) { 2000 l2cap_seq_list_clear(&chan->retrans_list); 2001 break; 2002 } 2003 2004 /* Update skb contents */ 2005 if (test_bit(FLAG_EXT_CTRL, &chan->flags)) { 2006 put_unaligned_le32(__pack_extended_control(&control), 2007 tx_skb->data + L2CAP_HDR_SIZE); 2008 } else { 2009 put_unaligned_le16(__pack_enhanced_control(&control), 2010 tx_skb->data + L2CAP_HDR_SIZE); 2011 } 2012 2013 /* Update FCS */ 2014 if (chan->fcs == L2CAP_FCS_CRC16) { 2015 u16 fcs = crc16(0, (u8 *) tx_skb->data, 2016 tx_skb->len - L2CAP_FCS_SIZE); 2017 put_unaligned_le16(fcs, skb_tail_pointer(tx_skb) - 2018 L2CAP_FCS_SIZE); 2019 } 2020 2021 l2cap_do_send(chan, tx_skb); 2022 2023 BT_DBG("Resent txseq %d", control.txseq); 2024 2025 chan->last_acked_seq = chan->buffer_seq; 2026 } 2027 } 2028 2029 static void l2cap_retransmit(struct l2cap_chan *chan, 2030 struct l2cap_ctrl *control) 2031 { 2032 BT_DBG("chan %p, control %p", chan, control); 2033 2034 l2cap_seq_list_append(&chan->retrans_list, control->reqseq); 2035 l2cap_ertm_resend(chan); 2036 } 2037 2038 static void l2cap_retransmit_all(struct l2cap_chan *chan, 2039 struct l2cap_ctrl *control) 2040 { 2041 struct sk_buff *skb; 2042 2043 BT_DBG("chan %p, control %p", chan, control); 2044 2045 if (control->poll) 2046 set_bit(CONN_SEND_FBIT, &chan->conn_state); 2047 2048 l2cap_seq_list_clear(&chan->retrans_list); 2049 2050 if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state)) 2051 return; 2052 2053 if (chan->unacked_frames) { 2054 skb_queue_walk(&chan->tx_q, skb) { 2055 if (bt_cb(skb)->l2cap.txseq == control->reqseq || 2056 skb == chan->tx_send_head) 2057 break; 2058 } 2059 2060 skb_queue_walk_from(&chan->tx_q, skb) { 2061 if (skb == chan->tx_send_head) 2062 break; 2063 2064 l2cap_seq_list_append(&chan->retrans_list, 2065 bt_cb(skb)->l2cap.txseq); 2066 } 2067 2068 l2cap_ertm_resend(chan); 2069 } 2070 } 2071 2072 static void l2cap_send_ack(struct l2cap_chan *chan) 2073 { 2074 struct l2cap_ctrl control; 2075 u16 frames_to_ack = __seq_offset(chan, chan->buffer_seq, 2076 chan->last_acked_seq); 2077 int threshold; 2078 2079 BT_DBG("chan %p last_acked_seq %d buffer_seq %d", 2080 chan, chan->last_acked_seq, chan->buffer_seq); 2081 2082 memset(&control, 0, sizeof(control)); 2083 control.sframe = 1; 2084 2085 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state) && 2086 chan->rx_state == L2CAP_RX_STATE_RECV) { 2087 __clear_ack_timer(chan); 2088 control.super = L2CAP_SUPER_RNR; 2089 control.reqseq = chan->buffer_seq; 2090 l2cap_send_sframe(chan, &control); 2091 } else { 2092 if (!test_bit(CONN_REMOTE_BUSY, &chan->conn_state)) { 2093 l2cap_ertm_send(chan); 2094 /* If any i-frames were sent, they included an ack */ 2095 if (chan->buffer_seq == chan->last_acked_seq) 2096 frames_to_ack = 0; 2097 } 2098 2099 /* Ack now if the window is 3/4ths full. 2100 * Calculate without mul or div 2101 */ 2102 threshold = chan->ack_win; 2103 threshold += threshold << 1; 2104 threshold >>= 2; 2105 2106 BT_DBG("frames_to_ack %u, threshold %d", frames_to_ack, 2107 threshold); 2108 2109 if (frames_to_ack >= threshold) { 2110 __clear_ack_timer(chan); 2111 control.super = L2CAP_SUPER_RR; 2112 control.reqseq = chan->buffer_seq; 2113 l2cap_send_sframe(chan, &control); 2114 frames_to_ack = 0; 2115 } 2116 2117 if (frames_to_ack) 2118 __set_ack_timer(chan); 2119 } 2120 } 2121 2122 static inline int l2cap_skbuff_fromiovec(struct l2cap_chan *chan, 2123 struct msghdr *msg, int len, 2124 int count, struct sk_buff *skb) 2125 { 2126 struct l2cap_conn *conn = chan->conn; 2127 struct sk_buff **frag; 2128 int sent = 0; 2129 2130 if (!copy_from_iter_full(skb_put(skb, count), count, &msg->msg_iter)) 2131 return -EFAULT; 2132 2133 sent += count; 2134 len -= count; 2135 2136 /* Continuation fragments (no L2CAP header) */ 2137 frag = &skb_shinfo(skb)->frag_list; 2138 while (len) { 2139 struct sk_buff *tmp; 2140 2141 count = min_t(unsigned int, conn->mtu, len); 2142 2143 tmp = chan->ops->alloc_skb(chan, 0, count, 2144 msg->msg_flags & MSG_DONTWAIT); 2145 if (IS_ERR(tmp)) 2146 return PTR_ERR(tmp); 2147 2148 *frag = tmp; 2149 2150 if (!copy_from_iter_full(skb_put(*frag, count), count, 2151 &msg->msg_iter)) 2152 return -EFAULT; 2153 2154 sent += count; 2155 len -= count; 2156 2157 skb->len += (*frag)->len; 2158 skb->data_len += (*frag)->len; 2159 2160 frag = &(*frag)->next; 2161 } 2162 2163 return sent; 2164 } 2165 2166 static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan, 2167 struct msghdr *msg, size_t len) 2168 { 2169 struct l2cap_conn *conn = chan->conn; 2170 struct sk_buff *skb; 2171 int err, count, hlen = L2CAP_HDR_SIZE + L2CAP_PSMLEN_SIZE; 2172 struct l2cap_hdr *lh; 2173 2174 BT_DBG("chan %p psm 0x%2.2x len %zu", chan, 2175 __le16_to_cpu(chan->psm), len); 2176 2177 count = min_t(unsigned int, (conn->mtu - hlen), len); 2178 2179 skb = chan->ops->alloc_skb(chan, hlen, count, 2180 msg->msg_flags & MSG_DONTWAIT); 2181 if (IS_ERR(skb)) 2182 return skb; 2183 2184 /* Create L2CAP header */ 2185 lh = skb_put(skb, L2CAP_HDR_SIZE); 2186 lh->cid = cpu_to_le16(chan->dcid); 2187 lh->len = cpu_to_le16(len + L2CAP_PSMLEN_SIZE); 2188 put_unaligned(chan->psm, (__le16 *) skb_put(skb, L2CAP_PSMLEN_SIZE)); 2189 2190 err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb); 2191 if (unlikely(err < 0)) { 2192 kfree_skb(skb); 2193 return ERR_PTR(err); 2194 } 2195 return skb; 2196 } 2197 2198 static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan, 2199 struct msghdr *msg, size_t len) 2200 { 2201 struct l2cap_conn *conn = chan->conn; 2202 struct sk_buff *skb; 2203 int err, count; 2204 struct l2cap_hdr *lh; 2205 2206 BT_DBG("chan %p len %zu", chan, len); 2207 2208 count = min_t(unsigned int, (conn->mtu - L2CAP_HDR_SIZE), len); 2209 2210 skb = chan->ops->alloc_skb(chan, L2CAP_HDR_SIZE, count, 2211 msg->msg_flags & MSG_DONTWAIT); 2212 if (IS_ERR(skb)) 2213 return skb; 2214 2215 /* Create L2CAP header */ 2216 lh = skb_put(skb, L2CAP_HDR_SIZE); 2217 lh->cid = cpu_to_le16(chan->dcid); 2218 lh->len = cpu_to_le16(len); 2219 2220 err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb); 2221 if (unlikely(err < 0)) { 2222 kfree_skb(skb); 2223 return ERR_PTR(err); 2224 } 2225 return skb; 2226 } 2227 2228 static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan, 2229 struct msghdr *msg, size_t len, 2230 u16 sdulen) 2231 { 2232 struct l2cap_conn *conn = chan->conn; 2233 struct sk_buff *skb; 2234 int err, count, hlen; 2235 struct l2cap_hdr *lh; 2236 2237 BT_DBG("chan %p len %zu", chan, len); 2238 2239 if (!conn) 2240 return ERR_PTR(-ENOTCONN); 2241 2242 hlen = __ertm_hdr_size(chan); 2243 2244 if (sdulen) 2245 hlen += L2CAP_SDULEN_SIZE; 2246 2247 if (chan->fcs == L2CAP_FCS_CRC16) 2248 hlen += L2CAP_FCS_SIZE; 2249 2250 count = min_t(unsigned int, (conn->mtu - hlen), len); 2251 2252 skb = chan->ops->alloc_skb(chan, hlen, count, 2253 msg->msg_flags & MSG_DONTWAIT); 2254 if (IS_ERR(skb)) 2255 return skb; 2256 2257 /* Create L2CAP header */ 2258 lh = skb_put(skb, L2CAP_HDR_SIZE); 2259 lh->cid = cpu_to_le16(chan->dcid); 2260 lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE)); 2261 2262 /* Control header is populated later */ 2263 if (test_bit(FLAG_EXT_CTRL, &chan->flags)) 2264 put_unaligned_le32(0, skb_put(skb, L2CAP_EXT_CTRL_SIZE)); 2265 else 2266 put_unaligned_le16(0, skb_put(skb, L2CAP_ENH_CTRL_SIZE)); 2267 2268 if (sdulen) 2269 put_unaligned_le16(sdulen, skb_put(skb, L2CAP_SDULEN_SIZE)); 2270 2271 err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb); 2272 if (unlikely(err < 0)) { 2273 kfree_skb(skb); 2274 return ERR_PTR(err); 2275 } 2276 2277 bt_cb(skb)->l2cap.fcs = chan->fcs; 2278 bt_cb(skb)->l2cap.retries = 0; 2279 return skb; 2280 } 2281 2282 static int l2cap_segment_sdu(struct l2cap_chan *chan, 2283 struct sk_buff_head *seg_queue, 2284 struct msghdr *msg, size_t len) 2285 { 2286 struct sk_buff *skb; 2287 u16 sdu_len; 2288 size_t pdu_len; 2289 u8 sar; 2290 2291 BT_DBG("chan %p, msg %p, len %zu", chan, msg, len); 2292 2293 /* It is critical that ERTM PDUs fit in a single HCI fragment, 2294 * so fragmented skbs are not used. The HCI layer's handling 2295 * of fragmented skbs is not compatible with ERTM's queueing. 2296 */ 2297 2298 /* PDU size is derived from the HCI MTU */ 2299 pdu_len = chan->conn->mtu; 2300 2301 /* Constrain PDU size for BR/EDR connections */ 2302 if (!chan->hs_hcon) 2303 pdu_len = min_t(size_t, pdu_len, L2CAP_BREDR_MAX_PAYLOAD); 2304 2305 /* Adjust for largest possible L2CAP overhead. */ 2306 if (chan->fcs) 2307 pdu_len -= L2CAP_FCS_SIZE; 2308 2309 pdu_len -= __ertm_hdr_size(chan); 2310 2311 /* Remote device may have requested smaller PDUs */ 2312 pdu_len = min_t(size_t, pdu_len, chan->remote_mps); 2313 2314 if (len <= pdu_len) { 2315 sar = L2CAP_SAR_UNSEGMENTED; 2316 sdu_len = 0; 2317 pdu_len = len; 2318 } else { 2319 sar = L2CAP_SAR_START; 2320 sdu_len = len; 2321 } 2322 2323 while (len > 0) { 2324 skb = l2cap_create_iframe_pdu(chan, msg, pdu_len, sdu_len); 2325 2326 if (IS_ERR(skb)) { 2327 __skb_queue_purge(seg_queue); 2328 return PTR_ERR(skb); 2329 } 2330 2331 bt_cb(skb)->l2cap.sar = sar; 2332 __skb_queue_tail(seg_queue, skb); 2333 2334 len -= pdu_len; 2335 if (sdu_len) 2336 sdu_len = 0; 2337 2338 if (len <= pdu_len) { 2339 sar = L2CAP_SAR_END; 2340 pdu_len = len; 2341 } else { 2342 sar = L2CAP_SAR_CONTINUE; 2343 } 2344 } 2345 2346 return 0; 2347 } 2348 2349 static struct sk_buff *l2cap_create_le_flowctl_pdu(struct l2cap_chan *chan, 2350 struct msghdr *msg, 2351 size_t len, u16 sdulen) 2352 { 2353 struct l2cap_conn *conn = chan->conn; 2354 struct sk_buff *skb; 2355 int err, count, hlen; 2356 struct l2cap_hdr *lh; 2357 2358 BT_DBG("chan %p len %zu", chan, len); 2359 2360 if (!conn) 2361 return ERR_PTR(-ENOTCONN); 2362 2363 hlen = L2CAP_HDR_SIZE; 2364 2365 if (sdulen) 2366 hlen += L2CAP_SDULEN_SIZE; 2367 2368 count = min_t(unsigned int, (conn->mtu - hlen), len); 2369 2370 skb = chan->ops->alloc_skb(chan, hlen, count, 2371 msg->msg_flags & MSG_DONTWAIT); 2372 if (IS_ERR(skb)) 2373 return skb; 2374 2375 /* Create L2CAP header */ 2376 lh = skb_put(skb, L2CAP_HDR_SIZE); 2377 lh->cid = cpu_to_le16(chan->dcid); 2378 lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE)); 2379 2380 if (sdulen) 2381 put_unaligned_le16(sdulen, skb_put(skb, L2CAP_SDULEN_SIZE)); 2382 2383 err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb); 2384 if (unlikely(err < 0)) { 2385 kfree_skb(skb); 2386 return ERR_PTR(err); 2387 } 2388 2389 return skb; 2390 } 2391 2392 static int l2cap_segment_le_sdu(struct l2cap_chan *chan, 2393 struct sk_buff_head *seg_queue, 2394 struct msghdr *msg, size_t len) 2395 { 2396 struct sk_buff *skb; 2397 size_t pdu_len; 2398 u16 sdu_len; 2399 2400 BT_DBG("chan %p, msg %p, len %zu", chan, msg, len); 2401 2402 sdu_len = len; 2403 pdu_len = chan->remote_mps - L2CAP_SDULEN_SIZE; 2404 2405 while (len > 0) { 2406 if (len <= pdu_len) 2407 pdu_len = len; 2408 2409 skb = l2cap_create_le_flowctl_pdu(chan, msg, pdu_len, sdu_len); 2410 if (IS_ERR(skb)) { 2411 __skb_queue_purge(seg_queue); 2412 return PTR_ERR(skb); 2413 } 2414 2415 __skb_queue_tail(seg_queue, skb); 2416 2417 len -= pdu_len; 2418 2419 if (sdu_len) { 2420 sdu_len = 0; 2421 pdu_len += L2CAP_SDULEN_SIZE; 2422 } 2423 } 2424 2425 return 0; 2426 } 2427 2428 static void l2cap_le_flowctl_send(struct l2cap_chan *chan) 2429 { 2430 int sent = 0; 2431 2432 BT_DBG("chan %p", chan); 2433 2434 while (chan->tx_credits && !skb_queue_empty(&chan->tx_q)) { 2435 l2cap_do_send(chan, skb_dequeue(&chan->tx_q)); 2436 chan->tx_credits--; 2437 sent++; 2438 } 2439 2440 BT_DBG("Sent %d credits %u queued %u", sent, chan->tx_credits, 2441 skb_queue_len(&chan->tx_q)); 2442 } 2443 2444 int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len) 2445 { 2446 struct sk_buff *skb; 2447 int err; 2448 struct sk_buff_head seg_queue; 2449 2450 if (!chan->conn) 2451 return -ENOTCONN; 2452 2453 /* Connectionless channel */ 2454 if (chan->chan_type == L2CAP_CHAN_CONN_LESS) { 2455 skb = l2cap_create_connless_pdu(chan, msg, len); 2456 if (IS_ERR(skb)) 2457 return PTR_ERR(skb); 2458 2459 /* Channel lock is released before requesting new skb and then 2460 * reacquired thus we need to recheck channel state. 2461 */ 2462 if (chan->state != BT_CONNECTED) { 2463 kfree_skb(skb); 2464 return -ENOTCONN; 2465 } 2466 2467 l2cap_do_send(chan, skb); 2468 return len; 2469 } 2470 2471 switch (chan->mode) { 2472 case L2CAP_MODE_LE_FLOWCTL: 2473 /* Check outgoing MTU */ 2474 if (len > chan->omtu) 2475 return -EMSGSIZE; 2476 2477 __skb_queue_head_init(&seg_queue); 2478 2479 err = l2cap_segment_le_sdu(chan, &seg_queue, msg, len); 2480 2481 if (chan->state != BT_CONNECTED) { 2482 __skb_queue_purge(&seg_queue); 2483 err = -ENOTCONN; 2484 } 2485 2486 if (err) 2487 return err; 2488 2489 skb_queue_splice_tail_init(&seg_queue, &chan->tx_q); 2490 2491 l2cap_le_flowctl_send(chan); 2492 2493 if (!chan->tx_credits) 2494 chan->ops->suspend(chan); 2495 2496 err = len; 2497 2498 break; 2499 2500 case L2CAP_MODE_BASIC: 2501 /* Check outgoing MTU */ 2502 if (len > chan->omtu) 2503 return -EMSGSIZE; 2504 2505 /* Create a basic PDU */ 2506 skb = l2cap_create_basic_pdu(chan, msg, len); 2507 if (IS_ERR(skb)) 2508 return PTR_ERR(skb); 2509 2510 /* Channel lock is released before requesting new skb and then 2511 * reacquired thus we need to recheck channel state. 2512 */ 2513 if (chan->state != BT_CONNECTED) { 2514 kfree_skb(skb); 2515 return -ENOTCONN; 2516 } 2517 2518 l2cap_do_send(chan, skb); 2519 err = len; 2520 break; 2521 2522 case L2CAP_MODE_ERTM: 2523 case L2CAP_MODE_STREAMING: 2524 /* Check outgoing MTU */ 2525 if (len > chan->omtu) { 2526 err = -EMSGSIZE; 2527 break; 2528 } 2529 2530 __skb_queue_head_init(&seg_queue); 2531 2532 /* Do segmentation before calling in to the state machine, 2533 * since it's possible to block while waiting for memory 2534 * allocation. 2535 */ 2536 err = l2cap_segment_sdu(chan, &seg_queue, msg, len); 2537 2538 /* The channel could have been closed while segmenting, 2539 * check that it is still connected. 2540 */ 2541 if (chan->state != BT_CONNECTED) { 2542 __skb_queue_purge(&seg_queue); 2543 err = -ENOTCONN; 2544 } 2545 2546 if (err) 2547 break; 2548 2549 if (chan->mode == L2CAP_MODE_ERTM) 2550 l2cap_tx(chan, NULL, &seg_queue, L2CAP_EV_DATA_REQUEST); 2551 else 2552 l2cap_streaming_send(chan, &seg_queue); 2553 2554 err = len; 2555 2556 /* If the skbs were not queued for sending, they'll still be in 2557 * seg_queue and need to be purged. 2558 */ 2559 __skb_queue_purge(&seg_queue); 2560 break; 2561 2562 default: 2563 BT_DBG("bad state %1.1x", chan->mode); 2564 err = -EBADFD; 2565 } 2566 2567 return err; 2568 } 2569 EXPORT_SYMBOL_GPL(l2cap_chan_send); 2570 2571 static void l2cap_send_srej(struct l2cap_chan *chan, u16 txseq) 2572 { 2573 struct l2cap_ctrl control; 2574 u16 seq; 2575 2576 BT_DBG("chan %p, txseq %u", chan, txseq); 2577 2578 memset(&control, 0, sizeof(control)); 2579 control.sframe = 1; 2580 control.super = L2CAP_SUPER_SREJ; 2581 2582 for (seq = chan->expected_tx_seq; seq != txseq; 2583 seq = __next_seq(chan, seq)) { 2584 if (!l2cap_ertm_seq_in_queue(&chan->srej_q, seq)) { 2585 control.reqseq = seq; 2586 l2cap_send_sframe(chan, &control); 2587 l2cap_seq_list_append(&chan->srej_list, seq); 2588 } 2589 } 2590 2591 chan->expected_tx_seq = __next_seq(chan, txseq); 2592 } 2593 2594 static void l2cap_send_srej_tail(struct l2cap_chan *chan) 2595 { 2596 struct l2cap_ctrl control; 2597 2598 BT_DBG("chan %p", chan); 2599 2600 if (chan->srej_list.tail == L2CAP_SEQ_LIST_CLEAR) 2601 return; 2602 2603 memset(&control, 0, sizeof(control)); 2604 control.sframe = 1; 2605 control.super = L2CAP_SUPER_SREJ; 2606 control.reqseq = chan->srej_list.tail; 2607 l2cap_send_sframe(chan, &control); 2608 } 2609 2610 static void l2cap_send_srej_list(struct l2cap_chan *chan, u16 txseq) 2611 { 2612 struct l2cap_ctrl control; 2613 u16 initial_head; 2614 u16 seq; 2615 2616 BT_DBG("chan %p, txseq %u", chan, txseq); 2617 2618 memset(&control, 0, sizeof(control)); 2619 control.sframe = 1; 2620 control.super = L2CAP_SUPER_SREJ; 2621 2622 /* Capture initial list head to allow only one pass through the list. */ 2623 initial_head = chan->srej_list.head; 2624 2625 do { 2626 seq = l2cap_seq_list_pop(&chan->srej_list); 2627 if (seq == txseq || seq == L2CAP_SEQ_LIST_CLEAR) 2628 break; 2629 2630 control.reqseq = seq; 2631 l2cap_send_sframe(chan, &control); 2632 l2cap_seq_list_append(&chan->srej_list, seq); 2633 } while (chan->srej_list.head != initial_head); 2634 } 2635 2636 static void l2cap_process_reqseq(struct l2cap_chan *chan, u16 reqseq) 2637 { 2638 struct sk_buff *acked_skb; 2639 u16 ackseq; 2640 2641 BT_DBG("chan %p, reqseq %u", chan, reqseq); 2642 2643 if (chan->unacked_frames == 0 || reqseq == chan->expected_ack_seq) 2644 return; 2645 2646 BT_DBG("expected_ack_seq %u, unacked_frames %u", 2647 chan->expected_ack_seq, chan->unacked_frames); 2648 2649 for (ackseq = chan->expected_ack_seq; ackseq != reqseq; 2650 ackseq = __next_seq(chan, ackseq)) { 2651 2652 acked_skb = l2cap_ertm_seq_in_queue(&chan->tx_q, ackseq); 2653 if (acked_skb) { 2654 skb_unlink(acked_skb, &chan->tx_q); 2655 kfree_skb(acked_skb); 2656 chan->unacked_frames--; 2657 } 2658 } 2659 2660 chan->expected_ack_seq = reqseq; 2661 2662 if (chan->unacked_frames == 0) 2663 __clear_retrans_timer(chan); 2664 2665 BT_DBG("unacked_frames %u", chan->unacked_frames); 2666 } 2667 2668 static void l2cap_abort_rx_srej_sent(struct l2cap_chan *chan) 2669 { 2670 BT_DBG("chan %p", chan); 2671 2672 chan->expected_tx_seq = chan->buffer_seq; 2673 l2cap_seq_list_clear(&chan->srej_list); 2674 skb_queue_purge(&chan->srej_q); 2675 chan->rx_state = L2CAP_RX_STATE_RECV; 2676 } 2677 2678 static void l2cap_tx_state_xmit(struct l2cap_chan *chan, 2679 struct l2cap_ctrl *control, 2680 struct sk_buff_head *skbs, u8 event) 2681 { 2682 BT_DBG("chan %p, control %p, skbs %p, event %d", chan, control, skbs, 2683 event); 2684 2685 switch (event) { 2686 case L2CAP_EV_DATA_REQUEST: 2687 if (chan->tx_send_head == NULL) 2688 chan->tx_send_head = skb_peek(skbs); 2689 2690 skb_queue_splice_tail_init(skbs, &chan->tx_q); 2691 l2cap_ertm_send(chan); 2692 break; 2693 case L2CAP_EV_LOCAL_BUSY_DETECTED: 2694 BT_DBG("Enter LOCAL_BUSY"); 2695 set_bit(CONN_LOCAL_BUSY, &chan->conn_state); 2696 2697 if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) { 2698 /* The SREJ_SENT state must be aborted if we are to 2699 * enter the LOCAL_BUSY state. 2700 */ 2701 l2cap_abort_rx_srej_sent(chan); 2702 } 2703 2704 l2cap_send_ack(chan); 2705 2706 break; 2707 case L2CAP_EV_LOCAL_BUSY_CLEAR: 2708 BT_DBG("Exit LOCAL_BUSY"); 2709 clear_bit(CONN_LOCAL_BUSY, &chan->conn_state); 2710 2711 if (test_bit(CONN_RNR_SENT, &chan->conn_state)) { 2712 struct l2cap_ctrl local_control; 2713 2714 memset(&local_control, 0, sizeof(local_control)); 2715 local_control.sframe = 1; 2716 local_control.super = L2CAP_SUPER_RR; 2717 local_control.poll = 1; 2718 local_control.reqseq = chan->buffer_seq; 2719 l2cap_send_sframe(chan, &local_control); 2720 2721 chan->retry_count = 1; 2722 __set_monitor_timer(chan); 2723 chan->tx_state = L2CAP_TX_STATE_WAIT_F; 2724 } 2725 break; 2726 case L2CAP_EV_RECV_REQSEQ_AND_FBIT: 2727 l2cap_process_reqseq(chan, control->reqseq); 2728 break; 2729 case L2CAP_EV_EXPLICIT_POLL: 2730 l2cap_send_rr_or_rnr(chan, 1); 2731 chan->retry_count = 1; 2732 __set_monitor_timer(chan); 2733 __clear_ack_timer(chan); 2734 chan->tx_state = L2CAP_TX_STATE_WAIT_F; 2735 break; 2736 case L2CAP_EV_RETRANS_TO: 2737 l2cap_send_rr_or_rnr(chan, 1); 2738 chan->retry_count = 1; 2739 __set_monitor_timer(chan); 2740 chan->tx_state = L2CAP_TX_STATE_WAIT_F; 2741 break; 2742 case L2CAP_EV_RECV_FBIT: 2743 /* Nothing to process */ 2744 break; 2745 default: 2746 break; 2747 } 2748 } 2749 2750 static void l2cap_tx_state_wait_f(struct l2cap_chan *chan, 2751 struct l2cap_ctrl *control, 2752 struct sk_buff_head *skbs, u8 event) 2753 { 2754 BT_DBG("chan %p, control %p, skbs %p, event %d", chan, control, skbs, 2755 event); 2756 2757 switch (event) { 2758 case L2CAP_EV_DATA_REQUEST: 2759 if (chan->tx_send_head == NULL) 2760 chan->tx_send_head = skb_peek(skbs); 2761 /* Queue data, but don't send. */ 2762 skb_queue_splice_tail_init(skbs, &chan->tx_q); 2763 break; 2764 case L2CAP_EV_LOCAL_BUSY_DETECTED: 2765 BT_DBG("Enter LOCAL_BUSY"); 2766 set_bit(CONN_LOCAL_BUSY, &chan->conn_state); 2767 2768 if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) { 2769 /* The SREJ_SENT state must be aborted if we are to 2770 * enter the LOCAL_BUSY state. 2771 */ 2772 l2cap_abort_rx_srej_sent(chan); 2773 } 2774 2775 l2cap_send_ack(chan); 2776 2777 break; 2778 case L2CAP_EV_LOCAL_BUSY_CLEAR: 2779 BT_DBG("Exit LOCAL_BUSY"); 2780 clear_bit(CONN_LOCAL_BUSY, &chan->conn_state); 2781 2782 if (test_bit(CONN_RNR_SENT, &chan->conn_state)) { 2783 struct l2cap_ctrl local_control; 2784 memset(&local_control, 0, sizeof(local_control)); 2785 local_control.sframe = 1; 2786 local_control.super = L2CAP_SUPER_RR; 2787 local_control.poll = 1; 2788 local_control.reqseq = chan->buffer_seq; 2789 l2cap_send_sframe(chan, &local_control); 2790 2791 chan->retry_count = 1; 2792 __set_monitor_timer(chan); 2793 chan->tx_state = L2CAP_TX_STATE_WAIT_F; 2794 } 2795 break; 2796 case L2CAP_EV_RECV_REQSEQ_AND_FBIT: 2797 l2cap_process_reqseq(chan, control->reqseq); 2798 2799 /* Fall through */ 2800 2801 case L2CAP_EV_RECV_FBIT: 2802 if (control && control->final) { 2803 __clear_monitor_timer(chan); 2804 if (chan->unacked_frames > 0) 2805 __set_retrans_timer(chan); 2806 chan->retry_count = 0; 2807 chan->tx_state = L2CAP_TX_STATE_XMIT; 2808 BT_DBG("recv fbit tx_state 0x2.2%x", chan->tx_state); 2809 } 2810 break; 2811 case L2CAP_EV_EXPLICIT_POLL: 2812 /* Ignore */ 2813 break; 2814 case L2CAP_EV_MONITOR_TO: 2815 if (chan->max_tx == 0 || chan->retry_count < chan->max_tx) { 2816 l2cap_send_rr_or_rnr(chan, 1); 2817 __set_monitor_timer(chan); 2818 chan->retry_count++; 2819 } else { 2820 l2cap_send_disconn_req(chan, ECONNABORTED); 2821 } 2822 break; 2823 default: 2824 break; 2825 } 2826 } 2827 2828 static void l2cap_tx(struct l2cap_chan *chan, struct l2cap_ctrl *control, 2829 struct sk_buff_head *skbs, u8 event) 2830 { 2831 BT_DBG("chan %p, control %p, skbs %p, event %d, state %d", 2832 chan, control, skbs, event, chan->tx_state); 2833 2834 switch (chan->tx_state) { 2835 case L2CAP_TX_STATE_XMIT: 2836 l2cap_tx_state_xmit(chan, control, skbs, event); 2837 break; 2838 case L2CAP_TX_STATE_WAIT_F: 2839 l2cap_tx_state_wait_f(chan, control, skbs, event); 2840 break; 2841 default: 2842 /* Ignore event */ 2843 break; 2844 } 2845 } 2846 2847 static void l2cap_pass_to_tx(struct l2cap_chan *chan, 2848 struct l2cap_ctrl *control) 2849 { 2850 BT_DBG("chan %p, control %p", chan, control); 2851 l2cap_tx(chan, control, NULL, L2CAP_EV_RECV_REQSEQ_AND_FBIT); 2852 } 2853 2854 static void l2cap_pass_to_tx_fbit(struct l2cap_chan *chan, 2855 struct l2cap_ctrl *control) 2856 { 2857 BT_DBG("chan %p, control %p", chan, control); 2858 l2cap_tx(chan, control, NULL, L2CAP_EV_RECV_FBIT); 2859 } 2860 2861 /* Copy frame to all raw sockets on that connection */ 2862 static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb) 2863 { 2864 struct sk_buff *nskb; 2865 struct l2cap_chan *chan; 2866 2867 BT_DBG("conn %p", conn); 2868 2869 mutex_lock(&conn->chan_lock); 2870 2871 list_for_each_entry(chan, &conn->chan_l, list) { 2872 if (chan->chan_type != L2CAP_CHAN_RAW) 2873 continue; 2874 2875 /* Don't send frame to the channel it came from */ 2876 if (bt_cb(skb)->l2cap.chan == chan) 2877 continue; 2878 2879 nskb = skb_clone(skb, GFP_KERNEL); 2880 if (!nskb) 2881 continue; 2882 if (chan->ops->recv(chan, nskb)) 2883 kfree_skb(nskb); 2884 } 2885 2886 mutex_unlock(&conn->chan_lock); 2887 } 2888 2889 /* ---- L2CAP signalling commands ---- */ 2890 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn, u8 code, 2891 u8 ident, u16 dlen, void *data) 2892 { 2893 struct sk_buff *skb, **frag; 2894 struct l2cap_cmd_hdr *cmd; 2895 struct l2cap_hdr *lh; 2896 int len, count; 2897 2898 BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %u", 2899 conn, code, ident, dlen); 2900 2901 if (conn->mtu < L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE) 2902 return NULL; 2903 2904 len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen; 2905 count = min_t(unsigned int, conn->mtu, len); 2906 2907 skb = bt_skb_alloc(count, GFP_KERNEL); 2908 if (!skb) 2909 return NULL; 2910 2911 lh = skb_put(skb, L2CAP_HDR_SIZE); 2912 lh->len = cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen); 2913 2914 if (conn->hcon->type == LE_LINK) 2915 lh->cid = cpu_to_le16(L2CAP_CID_LE_SIGNALING); 2916 else 2917 lh->cid = cpu_to_le16(L2CAP_CID_SIGNALING); 2918 2919 cmd = skb_put(skb, L2CAP_CMD_HDR_SIZE); 2920 cmd->code = code; 2921 cmd->ident = ident; 2922 cmd->len = cpu_to_le16(dlen); 2923 2924 if (dlen) { 2925 count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE; 2926 skb_put_data(skb, data, count); 2927 data += count; 2928 } 2929 2930 len -= skb->len; 2931 2932 /* Continuation fragments (no L2CAP header) */ 2933 frag = &skb_shinfo(skb)->frag_list; 2934 while (len) { 2935 count = min_t(unsigned int, conn->mtu, len); 2936 2937 *frag = bt_skb_alloc(count, GFP_KERNEL); 2938 if (!*frag) 2939 goto fail; 2940 2941 skb_put_data(*frag, data, count); 2942 2943 len -= count; 2944 data += count; 2945 2946 frag = &(*frag)->next; 2947 } 2948 2949 return skb; 2950 2951 fail: 2952 kfree_skb(skb); 2953 return NULL; 2954 } 2955 2956 static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, 2957 unsigned long *val) 2958 { 2959 struct l2cap_conf_opt *opt = *ptr; 2960 int len; 2961 2962 len = L2CAP_CONF_OPT_SIZE + opt->len; 2963 *ptr += len; 2964 2965 *type = opt->type; 2966 *olen = opt->len; 2967 2968 switch (opt->len) { 2969 case 1: 2970 *val = *((u8 *) opt->val); 2971 break; 2972 2973 case 2: 2974 *val = get_unaligned_le16(opt->val); 2975 break; 2976 2977 case 4: 2978 *val = get_unaligned_le32(opt->val); 2979 break; 2980 2981 default: 2982 *val = (unsigned long) opt->val; 2983 break; 2984 } 2985 2986 BT_DBG("type 0x%2.2x len %u val 0x%lx", *type, opt->len, *val); 2987 return len; 2988 } 2989 2990 static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val, size_t size) 2991 { 2992 struct l2cap_conf_opt *opt = *ptr; 2993 2994 BT_DBG("type 0x%2.2x len %u val 0x%lx", type, len, val); 2995 2996 if (size < L2CAP_CONF_OPT_SIZE + len) 2997 return; 2998 2999 opt->type = type; 3000 opt->len = len; 3001 3002 switch (len) { 3003 case 1: 3004 *((u8 *) opt->val) = val; 3005 break; 3006 3007 case 2: 3008 put_unaligned_le16(val, opt->val); 3009 break; 3010 3011 case 4: 3012 put_unaligned_le32(val, opt->val); 3013 break; 3014 3015 default: 3016 memcpy(opt->val, (void *) val, len); 3017 break; 3018 } 3019 3020 *ptr += L2CAP_CONF_OPT_SIZE + len; 3021 } 3022 3023 static void l2cap_add_opt_efs(void **ptr, struct l2cap_chan *chan, size_t size) 3024 { 3025 struct l2cap_conf_efs efs; 3026 3027 switch (chan->mode) { 3028 case L2CAP_MODE_ERTM: 3029 efs.id = chan->local_id; 3030 efs.stype = chan->local_stype; 3031 efs.msdu = cpu_to_le16(chan->local_msdu); 3032 efs.sdu_itime = cpu_to_le32(chan->local_sdu_itime); 3033 efs.acc_lat = cpu_to_le32(L2CAP_DEFAULT_ACC_LAT); 3034 efs.flush_to = cpu_to_le32(L2CAP_EFS_DEFAULT_FLUSH_TO); 3035 break; 3036 3037 case L2CAP_MODE_STREAMING: 3038 efs.id = 1; 3039 efs.stype = L2CAP_SERV_BESTEFFORT; 3040 efs.msdu = cpu_to_le16(chan->local_msdu); 3041 efs.sdu_itime = cpu_to_le32(chan->local_sdu_itime); 3042 efs.acc_lat = 0; 3043 efs.flush_to = 0; 3044 break; 3045 3046 default: 3047 return; 3048 } 3049 3050 l2cap_add_conf_opt(ptr, L2CAP_CONF_EFS, sizeof(efs), 3051 (unsigned long) &efs, size); 3052 } 3053 3054 static void l2cap_ack_timeout(struct work_struct *work) 3055 { 3056 struct l2cap_chan *chan = container_of(work, struct l2cap_chan, 3057 ack_timer.work); 3058 u16 frames_to_ack; 3059 3060 BT_DBG("chan %p", chan); 3061 3062 l2cap_chan_lock(chan); 3063 3064 frames_to_ack = __seq_offset(chan, chan->buffer_seq, 3065 chan->last_acked_seq); 3066 3067 if (frames_to_ack) 3068 l2cap_send_rr_or_rnr(chan, 0); 3069 3070 l2cap_chan_unlock(chan); 3071 l2cap_chan_put(chan); 3072 } 3073 3074 int l2cap_ertm_init(struct l2cap_chan *chan) 3075 { 3076 int err; 3077 3078 chan->next_tx_seq = 0; 3079 chan->expected_tx_seq = 0; 3080 chan->expected_ack_seq = 0; 3081 chan->unacked_frames = 0; 3082 chan->buffer_seq = 0; 3083 chan->frames_sent = 0; 3084 chan->last_acked_seq = 0; 3085 chan->sdu = NULL; 3086 chan->sdu_last_frag = NULL; 3087 chan->sdu_len = 0; 3088 3089 skb_queue_head_init(&chan->tx_q); 3090 3091 chan->local_amp_id = AMP_ID_BREDR; 3092 chan->move_id = AMP_ID_BREDR; 3093 chan->move_state = L2CAP_MOVE_STABLE; 3094 chan->move_role = L2CAP_MOVE_ROLE_NONE; 3095 3096 if (chan->mode != L2CAP_MODE_ERTM) 3097 return 0; 3098 3099 chan->rx_state = L2CAP_RX_STATE_RECV; 3100 chan->tx_state = L2CAP_TX_STATE_XMIT; 3101 3102 INIT_DELAYED_WORK(&chan->retrans_timer, l2cap_retrans_timeout); 3103 INIT_DELAYED_WORK(&chan->monitor_timer, l2cap_monitor_timeout); 3104 INIT_DELAYED_WORK(&chan->ack_timer, l2cap_ack_timeout); 3105 3106 skb_queue_head_init(&chan->srej_q); 3107 3108 err = l2cap_seq_list_init(&chan->srej_list, chan->tx_win); 3109 if (err < 0) 3110 return err; 3111 3112 err = l2cap_seq_list_init(&chan->retrans_list, chan->remote_tx_win); 3113 if (err < 0) 3114 l2cap_seq_list_free(&chan->srej_list); 3115 3116 return err; 3117 } 3118 3119 static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask) 3120 { 3121 switch (mode) { 3122 case L2CAP_MODE_STREAMING: 3123 case L2CAP_MODE_ERTM: 3124 if (l2cap_mode_supported(mode, remote_feat_mask)) 3125 return mode; 3126 /* fall through */ 3127 default: 3128 return L2CAP_MODE_BASIC; 3129 } 3130 } 3131 3132 static inline bool __l2cap_ews_supported(struct l2cap_conn *conn) 3133 { 3134 return ((conn->local_fixed_chan & L2CAP_FC_A2MP) && 3135 (conn->feat_mask & L2CAP_FEAT_EXT_WINDOW)); 3136 } 3137 3138 static inline bool __l2cap_efs_supported(struct l2cap_conn *conn) 3139 { 3140 return ((conn->local_fixed_chan & L2CAP_FC_A2MP) && 3141 (conn->feat_mask & L2CAP_FEAT_EXT_FLOW)); 3142 } 3143 3144 static void __l2cap_set_ertm_timeouts(struct l2cap_chan *chan, 3145 struct l2cap_conf_rfc *rfc) 3146 { 3147 if (chan->local_amp_id != AMP_ID_BREDR && chan->hs_hcon) { 3148 u64 ertm_to = chan->hs_hcon->hdev->amp_be_flush_to; 3149 3150 /* Class 1 devices have must have ERTM timeouts 3151 * exceeding the Link Supervision Timeout. The 3152 * default Link Supervision Timeout for AMP 3153 * controllers is 10 seconds. 3154 * 3155 * Class 1 devices use 0xffffffff for their 3156 * best-effort flush timeout, so the clamping logic 3157 * will result in a timeout that meets the above 3158 * requirement. ERTM timeouts are 16-bit values, so 3159 * the maximum timeout is 65.535 seconds. 3160 */ 3161 3162 /* Convert timeout to milliseconds and round */ 3163 ertm_to = DIV_ROUND_UP_ULL(ertm_to, 1000); 3164 3165 /* This is the recommended formula for class 2 devices 3166 * that start ERTM timers when packets are sent to the 3167 * controller. 3168 */ 3169 ertm_to = 3 * ertm_to + 500; 3170 3171 if (ertm_to > 0xffff) 3172 ertm_to = 0xffff; 3173 3174 rfc->retrans_timeout = cpu_to_le16((u16) ertm_to); 3175 rfc->monitor_timeout = rfc->retrans_timeout; 3176 } else { 3177 rfc->retrans_timeout = cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO); 3178 rfc->monitor_timeout = cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO); 3179 } 3180 } 3181 3182 static inline void l2cap_txwin_setup(struct l2cap_chan *chan) 3183 { 3184 if (chan->tx_win > L2CAP_DEFAULT_TX_WINDOW && 3185 __l2cap_ews_supported(chan->conn)) { 3186 /* use extended control field */ 3187 set_bit(FLAG_EXT_CTRL, &chan->flags); 3188 chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW; 3189 } else { 3190 chan->tx_win = min_t(u16, chan->tx_win, 3191 L2CAP_DEFAULT_TX_WINDOW); 3192 chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW; 3193 } 3194 chan->ack_win = chan->tx_win; 3195 } 3196 3197 static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data, size_t data_size) 3198 { 3199 struct l2cap_conf_req *req = data; 3200 struct l2cap_conf_rfc rfc = { .mode = chan->mode }; 3201 void *ptr = req->data; 3202 void *endptr = data + data_size; 3203 u16 size; 3204 3205 BT_DBG("chan %p", chan); 3206 3207 if (chan->num_conf_req || chan->num_conf_rsp) 3208 goto done; 3209 3210 switch (chan->mode) { 3211 case L2CAP_MODE_STREAMING: 3212 case L2CAP_MODE_ERTM: 3213 if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state)) 3214 break; 3215 3216 if (__l2cap_efs_supported(chan->conn)) 3217 set_bit(FLAG_EFS_ENABLE, &chan->flags); 3218 3219 /* fall through */ 3220 default: 3221 chan->mode = l2cap_select_mode(rfc.mode, chan->conn->feat_mask); 3222 break; 3223 } 3224 3225 done: 3226 if (chan->imtu != L2CAP_DEFAULT_MTU) 3227 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu, endptr - ptr); 3228 3229 switch (chan->mode) { 3230 case L2CAP_MODE_BASIC: 3231 if (disable_ertm) 3232 break; 3233 3234 if (!(chan->conn->feat_mask & L2CAP_FEAT_ERTM) && 3235 !(chan->conn->feat_mask & L2CAP_FEAT_STREAMING)) 3236 break; 3237 3238 rfc.mode = L2CAP_MODE_BASIC; 3239 rfc.txwin_size = 0; 3240 rfc.max_transmit = 0; 3241 rfc.retrans_timeout = 0; 3242 rfc.monitor_timeout = 0; 3243 rfc.max_pdu_size = 0; 3244 3245 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), 3246 (unsigned long) &rfc, endptr - ptr); 3247 break; 3248 3249 case L2CAP_MODE_ERTM: 3250 rfc.mode = L2CAP_MODE_ERTM; 3251 rfc.max_transmit = chan->max_tx; 3252 3253 __l2cap_set_ertm_timeouts(chan, &rfc); 3254 3255 size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu - 3256 L2CAP_EXT_HDR_SIZE - L2CAP_SDULEN_SIZE - 3257 L2CAP_FCS_SIZE); 3258 rfc.max_pdu_size = cpu_to_le16(size); 3259 3260 l2cap_txwin_setup(chan); 3261 3262 rfc.txwin_size = min_t(u16, chan->tx_win, 3263 L2CAP_DEFAULT_TX_WINDOW); 3264 3265 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), 3266 (unsigned long) &rfc, endptr - ptr); 3267 3268 if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) 3269 l2cap_add_opt_efs(&ptr, chan, endptr - ptr); 3270 3271 if (test_bit(FLAG_EXT_CTRL, &chan->flags)) 3272 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2, 3273 chan->tx_win, endptr - ptr); 3274 3275 if (chan->conn->feat_mask & L2CAP_FEAT_FCS) 3276 if (chan->fcs == L2CAP_FCS_NONE || 3277 test_bit(CONF_RECV_NO_FCS, &chan->conf_state)) { 3278 chan->fcs = L2CAP_FCS_NONE; 3279 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, 3280 chan->fcs, endptr - ptr); 3281 } 3282 break; 3283 3284 case L2CAP_MODE_STREAMING: 3285 l2cap_txwin_setup(chan); 3286 rfc.mode = L2CAP_MODE_STREAMING; 3287 rfc.txwin_size = 0; 3288 rfc.max_transmit = 0; 3289 rfc.retrans_timeout = 0; 3290 rfc.monitor_timeout = 0; 3291 3292 size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu - 3293 L2CAP_EXT_HDR_SIZE - L2CAP_SDULEN_SIZE - 3294 L2CAP_FCS_SIZE); 3295 rfc.max_pdu_size = cpu_to_le16(size); 3296 3297 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), 3298 (unsigned long) &rfc, endptr - ptr); 3299 3300 if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) 3301 l2cap_add_opt_efs(&ptr, chan, endptr - ptr); 3302 3303 if (chan->conn->feat_mask & L2CAP_FEAT_FCS) 3304 if (chan->fcs == L2CAP_FCS_NONE || 3305 test_bit(CONF_RECV_NO_FCS, &chan->conf_state)) { 3306 chan->fcs = L2CAP_FCS_NONE; 3307 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, 3308 chan->fcs, endptr - ptr); 3309 } 3310 break; 3311 } 3312 3313 req->dcid = cpu_to_le16(chan->dcid); 3314 req->flags = cpu_to_le16(0); 3315 3316 return ptr - data; 3317 } 3318 3319 static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data, size_t data_size) 3320 { 3321 struct l2cap_conf_rsp *rsp = data; 3322 void *ptr = rsp->data; 3323 void *endptr = data + data_size; 3324 void *req = chan->conf_req; 3325 int len = chan->conf_len; 3326 int type, hint, olen; 3327 unsigned long val; 3328 struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC }; 3329 struct l2cap_conf_efs efs; 3330 u8 remote_efs = 0; 3331 u16 mtu = L2CAP_DEFAULT_MTU; 3332 u16 result = L2CAP_CONF_SUCCESS; 3333 u16 size; 3334 3335 BT_DBG("chan %p", chan); 3336 3337 while (len >= L2CAP_CONF_OPT_SIZE) { 3338 len -= l2cap_get_conf_opt(&req, &type, &olen, &val); 3339 3340 hint = type & L2CAP_CONF_HINT; 3341 type &= L2CAP_CONF_MASK; 3342 3343 switch (type) { 3344 case L2CAP_CONF_MTU: 3345 mtu = val; 3346 break; 3347 3348 case L2CAP_CONF_FLUSH_TO: 3349 chan->flush_to = val; 3350 break; 3351 3352 case L2CAP_CONF_QOS: 3353 break; 3354 3355 case L2CAP_CONF_RFC: 3356 if (olen == sizeof(rfc)) 3357 memcpy(&rfc, (void *) val, olen); 3358 break; 3359 3360 case L2CAP_CONF_FCS: 3361 if (val == L2CAP_FCS_NONE) 3362 set_bit(CONF_RECV_NO_FCS, &chan->conf_state); 3363 break; 3364 3365 case L2CAP_CONF_EFS: 3366 if (olen == sizeof(efs)) { 3367 remote_efs = 1; 3368 memcpy(&efs, (void *) val, olen); 3369 } 3370 break; 3371 3372 case L2CAP_CONF_EWS: 3373 if (!(chan->conn->local_fixed_chan & L2CAP_FC_A2MP)) 3374 return -ECONNREFUSED; 3375 3376 set_bit(FLAG_EXT_CTRL, &chan->flags); 3377 set_bit(CONF_EWS_RECV, &chan->conf_state); 3378 chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW; 3379 chan->remote_tx_win = val; 3380 break; 3381 3382 default: 3383 if (hint) 3384 break; 3385 3386 result = L2CAP_CONF_UNKNOWN; 3387 *((u8 *) ptr++) = type; 3388 break; 3389 } 3390 } 3391 3392 if (chan->num_conf_rsp || chan->num_conf_req > 1) 3393 goto done; 3394 3395 switch (chan->mode) { 3396 case L2CAP_MODE_STREAMING: 3397 case L2CAP_MODE_ERTM: 3398 if (!test_bit(CONF_STATE2_DEVICE, &chan->conf_state)) { 3399 chan->mode = l2cap_select_mode(rfc.mode, 3400 chan->conn->feat_mask); 3401 break; 3402 } 3403 3404 if (remote_efs) { 3405 if (__l2cap_efs_supported(chan->conn)) 3406 set_bit(FLAG_EFS_ENABLE, &chan->flags); 3407 else 3408 return -ECONNREFUSED; 3409 } 3410 3411 if (chan->mode != rfc.mode) 3412 return -ECONNREFUSED; 3413 3414 break; 3415 } 3416 3417 done: 3418 if (chan->mode != rfc.mode) { 3419 result = L2CAP_CONF_UNACCEPT; 3420 rfc.mode = chan->mode; 3421 3422 if (chan->num_conf_rsp == 1) 3423 return -ECONNREFUSED; 3424 3425 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), 3426 (unsigned long) &rfc, endptr - ptr); 3427 } 3428 3429 if (result == L2CAP_CONF_SUCCESS) { 3430 /* Configure output options and let the other side know 3431 * which ones we don't like. */ 3432 3433 if (mtu < L2CAP_DEFAULT_MIN_MTU) 3434 result = L2CAP_CONF_UNACCEPT; 3435 else { 3436 chan->omtu = mtu; 3437 set_bit(CONF_MTU_DONE, &chan->conf_state); 3438 } 3439 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->omtu, endptr - ptr); 3440 3441 if (remote_efs) { 3442 if (chan->local_stype != L2CAP_SERV_NOTRAFIC && 3443 efs.stype != L2CAP_SERV_NOTRAFIC && 3444 efs.stype != chan->local_stype) { 3445 3446 result = L2CAP_CONF_UNACCEPT; 3447 3448 if (chan->num_conf_req >= 1) 3449 return -ECONNREFUSED; 3450 3451 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS, 3452 sizeof(efs), 3453 (unsigned long) &efs, endptr - ptr); 3454 } else { 3455 /* Send PENDING Conf Rsp */ 3456 result = L2CAP_CONF_PENDING; 3457 set_bit(CONF_LOC_CONF_PEND, &chan->conf_state); 3458 } 3459 } 3460 3461 switch (rfc.mode) { 3462 case L2CAP_MODE_BASIC: 3463 chan->fcs = L2CAP_FCS_NONE; 3464 set_bit(CONF_MODE_DONE, &chan->conf_state); 3465 break; 3466 3467 case L2CAP_MODE_ERTM: 3468 if (!test_bit(CONF_EWS_RECV, &chan->conf_state)) 3469 chan->remote_tx_win = rfc.txwin_size; 3470 else 3471 rfc.txwin_size = L2CAP_DEFAULT_TX_WINDOW; 3472 3473 chan->remote_max_tx = rfc.max_transmit; 3474 3475 size = min_t(u16, le16_to_cpu(rfc.max_pdu_size), 3476 chan->conn->mtu - L2CAP_EXT_HDR_SIZE - 3477 L2CAP_SDULEN_SIZE - L2CAP_FCS_SIZE); 3478 rfc.max_pdu_size = cpu_to_le16(size); 3479 chan->remote_mps = size; 3480 3481 __l2cap_set_ertm_timeouts(chan, &rfc); 3482 3483 set_bit(CONF_MODE_DONE, &chan->conf_state); 3484 3485 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, 3486 sizeof(rfc), (unsigned long) &rfc, endptr - ptr); 3487 3488 if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) { 3489 chan->remote_id = efs.id; 3490 chan->remote_stype = efs.stype; 3491 chan->remote_msdu = le16_to_cpu(efs.msdu); 3492 chan->remote_flush_to = 3493 le32_to_cpu(efs.flush_to); 3494 chan->remote_acc_lat = 3495 le32_to_cpu(efs.acc_lat); 3496 chan->remote_sdu_itime = 3497 le32_to_cpu(efs.sdu_itime); 3498 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS, 3499 sizeof(efs), 3500 (unsigned long) &efs, endptr - ptr); 3501 } 3502 break; 3503 3504 case L2CAP_MODE_STREAMING: 3505 size = min_t(u16, le16_to_cpu(rfc.max_pdu_size), 3506 chan->conn->mtu - L2CAP_EXT_HDR_SIZE - 3507 L2CAP_SDULEN_SIZE - L2CAP_FCS_SIZE); 3508 rfc.max_pdu_size = cpu_to_le16(size); 3509 chan->remote_mps = size; 3510 3511 set_bit(CONF_MODE_DONE, &chan->conf_state); 3512 3513 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), 3514 (unsigned long) &rfc, endptr - ptr); 3515 3516 break; 3517 3518 default: 3519 result = L2CAP_CONF_UNACCEPT; 3520 3521 memset(&rfc, 0, sizeof(rfc)); 3522 rfc.mode = chan->mode; 3523 } 3524 3525 if (result == L2CAP_CONF_SUCCESS) 3526 set_bit(CONF_OUTPUT_DONE, &chan->conf_state); 3527 } 3528 rsp->scid = cpu_to_le16(chan->dcid); 3529 rsp->result = cpu_to_le16(result); 3530 rsp->flags = cpu_to_le16(0); 3531 3532 return ptr - data; 3533 } 3534 3535 static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, 3536 void *data, size_t size, u16 *result) 3537 { 3538 struct l2cap_conf_req *req = data; 3539 void *ptr = req->data; 3540 void *endptr = data + size; 3541 int type, olen; 3542 unsigned long val; 3543 struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC }; 3544 struct l2cap_conf_efs efs; 3545 3546 BT_DBG("chan %p, rsp %p, len %d, req %p", chan, rsp, len, data); 3547 3548 while (len >= L2CAP_CONF_OPT_SIZE) { 3549 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val); 3550 3551 switch (type) { 3552 case L2CAP_CONF_MTU: 3553 if (val < L2CAP_DEFAULT_MIN_MTU) { 3554 *result = L2CAP_CONF_UNACCEPT; 3555 chan->imtu = L2CAP_DEFAULT_MIN_MTU; 3556 } else 3557 chan->imtu = val; 3558 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu, endptr - ptr); 3559 break; 3560 3561 case L2CAP_CONF_FLUSH_TO: 3562 chan->flush_to = val; 3563 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO, 3564 2, chan->flush_to, endptr - ptr); 3565 break; 3566 3567 case L2CAP_CONF_RFC: 3568 if (olen == sizeof(rfc)) 3569 memcpy(&rfc, (void *)val, olen); 3570 3571 if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state) && 3572 rfc.mode != chan->mode) 3573 return -ECONNREFUSED; 3574 3575 chan->fcs = 0; 3576 3577 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, 3578 sizeof(rfc), (unsigned long) &rfc, endptr - ptr); 3579 break; 3580 3581 case L2CAP_CONF_EWS: 3582 chan->ack_win = min_t(u16, val, chan->ack_win); 3583 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2, 3584 chan->tx_win, endptr - ptr); 3585 break; 3586 3587 case L2CAP_CONF_EFS: 3588 if (olen == sizeof(efs)) { 3589 memcpy(&efs, (void *)val, olen); 3590 3591 if (chan->local_stype != L2CAP_SERV_NOTRAFIC && 3592 efs.stype != L2CAP_SERV_NOTRAFIC && 3593 efs.stype != chan->local_stype) 3594 return -ECONNREFUSED; 3595 3596 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS, sizeof(efs), 3597 (unsigned long) &efs, endptr - ptr); 3598 } 3599 break; 3600 3601 case L2CAP_CONF_FCS: 3602 if (*result == L2CAP_CONF_PENDING) 3603 if (val == L2CAP_FCS_NONE) 3604 set_bit(CONF_RECV_NO_FCS, 3605 &chan->conf_state); 3606 break; 3607 } 3608 } 3609 3610 if (chan->mode == L2CAP_MODE_BASIC && chan->mode != rfc.mode) 3611 return -ECONNREFUSED; 3612 3613 chan->mode = rfc.mode; 3614 3615 if (*result == L2CAP_CONF_SUCCESS || *result == L2CAP_CONF_PENDING) { 3616 switch (rfc.mode) { 3617 case L2CAP_MODE_ERTM: 3618 chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout); 3619 chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout); 3620 chan->mps = le16_to_cpu(rfc.max_pdu_size); 3621 if (!test_bit(FLAG_EXT_CTRL, &chan->flags)) 3622 chan->ack_win = min_t(u16, chan->ack_win, 3623 rfc.txwin_size); 3624 3625 if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) { 3626 chan->local_msdu = le16_to_cpu(efs.msdu); 3627 chan->local_sdu_itime = 3628 le32_to_cpu(efs.sdu_itime); 3629 chan->local_acc_lat = le32_to_cpu(efs.acc_lat); 3630 chan->local_flush_to = 3631 le32_to_cpu(efs.flush_to); 3632 } 3633 break; 3634 3635 case L2CAP_MODE_STREAMING: 3636 chan->mps = le16_to_cpu(rfc.max_pdu_size); 3637 } 3638 } 3639 3640 req->dcid = cpu_to_le16(chan->dcid); 3641 req->flags = cpu_to_le16(0); 3642 3643 return ptr - data; 3644 } 3645 3646 static int l2cap_build_conf_rsp(struct l2cap_chan *chan, void *data, 3647 u16 result, u16 flags) 3648 { 3649 struct l2cap_conf_rsp *rsp = data; 3650 void *ptr = rsp->data; 3651 3652 BT_DBG("chan %p", chan); 3653 3654 rsp->scid = cpu_to_le16(chan->dcid); 3655 rsp->result = cpu_to_le16(result); 3656 rsp->flags = cpu_to_le16(flags); 3657 3658 return ptr - data; 3659 } 3660 3661 void __l2cap_le_connect_rsp_defer(struct l2cap_chan *chan) 3662 { 3663 struct l2cap_le_conn_rsp rsp; 3664 struct l2cap_conn *conn = chan->conn; 3665 3666 BT_DBG("chan %p", chan); 3667 3668 rsp.dcid = cpu_to_le16(chan->scid); 3669 rsp.mtu = cpu_to_le16(chan->imtu); 3670 rsp.mps = cpu_to_le16(chan->mps); 3671 rsp.credits = cpu_to_le16(chan->rx_credits); 3672 rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS); 3673 3674 l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CONN_RSP, sizeof(rsp), 3675 &rsp); 3676 } 3677 3678 void __l2cap_connect_rsp_defer(struct l2cap_chan *chan) 3679 { 3680 struct l2cap_conn_rsp rsp; 3681 struct l2cap_conn *conn = chan->conn; 3682 u8 buf[128]; 3683 u8 rsp_code; 3684 3685 rsp.scid = cpu_to_le16(chan->dcid); 3686 rsp.dcid = cpu_to_le16(chan->scid); 3687 rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS); 3688 rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); 3689 3690 if (chan->hs_hcon) 3691 rsp_code = L2CAP_CREATE_CHAN_RSP; 3692 else 3693 rsp_code = L2CAP_CONN_RSP; 3694 3695 BT_DBG("chan %p rsp_code %u", chan, rsp_code); 3696 3697 l2cap_send_cmd(conn, chan->ident, rsp_code, sizeof(rsp), &rsp); 3698 3699 if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) 3700 return; 3701 3702 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, 3703 l2cap_build_conf_req(chan, buf, sizeof(buf)), buf); 3704 chan->num_conf_req++; 3705 } 3706 3707 static void l2cap_conf_rfc_get(struct l2cap_chan *chan, void *rsp, int len) 3708 { 3709 int type, olen; 3710 unsigned long val; 3711 /* Use sane default values in case a misbehaving remote device 3712 * did not send an RFC or extended window size option. 3713 */ 3714 u16 txwin_ext = chan->ack_win; 3715 struct l2cap_conf_rfc rfc = { 3716 .mode = chan->mode, 3717 .retrans_timeout = cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO), 3718 .monitor_timeout = cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO), 3719 .max_pdu_size = cpu_to_le16(chan->imtu), 3720 .txwin_size = min_t(u16, chan->ack_win, L2CAP_DEFAULT_TX_WINDOW), 3721 }; 3722 3723 BT_DBG("chan %p, rsp %p, len %d", chan, rsp, len); 3724 3725 if ((chan->mode != L2CAP_MODE_ERTM) && (chan->mode != L2CAP_MODE_STREAMING)) 3726 return; 3727 3728 while (len >= L2CAP_CONF_OPT_SIZE) { 3729 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val); 3730 3731 switch (type) { 3732 case L2CAP_CONF_RFC: 3733 if (olen == sizeof(rfc)) 3734 memcpy(&rfc, (void *)val, olen); 3735 break; 3736 case L2CAP_CONF_EWS: 3737 txwin_ext = val; 3738 break; 3739 } 3740 } 3741 3742 switch (rfc.mode) { 3743 case L2CAP_MODE_ERTM: 3744 chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout); 3745 chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout); 3746 chan->mps = le16_to_cpu(rfc.max_pdu_size); 3747 if (test_bit(FLAG_EXT_CTRL, &chan->flags)) 3748 chan->ack_win = min_t(u16, chan->ack_win, txwin_ext); 3749 else 3750 chan->ack_win = min_t(u16, chan->ack_win, 3751 rfc.txwin_size); 3752 break; 3753 case L2CAP_MODE_STREAMING: 3754 chan->mps = le16_to_cpu(rfc.max_pdu_size); 3755 } 3756 } 3757 3758 static inline int l2cap_command_rej(struct l2cap_conn *conn, 3759 struct l2cap_cmd_hdr *cmd, u16 cmd_len, 3760 u8 *data) 3761 { 3762 struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data; 3763 3764 if (cmd_len < sizeof(*rej)) 3765 return -EPROTO; 3766 3767 if (rej->reason != L2CAP_REJ_NOT_UNDERSTOOD) 3768 return 0; 3769 3770 if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) && 3771 cmd->ident == conn->info_ident) { 3772 cancel_delayed_work(&conn->info_timer); 3773 3774 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; 3775 conn->info_ident = 0; 3776 3777 l2cap_conn_start(conn); 3778 } 3779 3780 return 0; 3781 } 3782 3783 static struct l2cap_chan *l2cap_connect(struct l2cap_conn *conn, 3784 struct l2cap_cmd_hdr *cmd, 3785 u8 *data, u8 rsp_code, u8 amp_id) 3786 { 3787 struct l2cap_conn_req *req = (struct l2cap_conn_req *) data; 3788 struct l2cap_conn_rsp rsp; 3789 struct l2cap_chan *chan = NULL, *pchan; 3790 int result, status = L2CAP_CS_NO_INFO; 3791 3792 u16 dcid = 0, scid = __le16_to_cpu(req->scid); 3793 __le16 psm = req->psm; 3794 3795 BT_DBG("psm 0x%2.2x scid 0x%4.4x", __le16_to_cpu(psm), scid); 3796 3797 /* Check if we have socket listening on psm */ 3798 pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, &conn->hcon->src, 3799 &conn->hcon->dst, ACL_LINK); 3800 if (!pchan) { 3801 result = L2CAP_CR_BAD_PSM; 3802 goto sendresp; 3803 } 3804 3805 mutex_lock(&conn->chan_lock); 3806 l2cap_chan_lock(pchan); 3807 3808 /* Check if the ACL is secure enough (if not SDP) */ 3809 if (psm != cpu_to_le16(L2CAP_PSM_SDP) && 3810 !hci_conn_check_link_mode(conn->hcon)) { 3811 conn->disc_reason = HCI_ERROR_AUTH_FAILURE; 3812 result = L2CAP_CR_SEC_BLOCK; 3813 goto response; 3814 } 3815 3816 result = L2CAP_CR_NO_MEM; 3817 3818 /* Check if we already have channel with that dcid */ 3819 if (__l2cap_get_chan_by_dcid(conn, scid)) 3820 goto response; 3821 3822 chan = pchan->ops->new_connection(pchan); 3823 if (!chan) 3824 goto response; 3825 3826 /* For certain devices (ex: HID mouse), support for authentication, 3827 * pairing and bonding is optional. For such devices, inorder to avoid 3828 * the ACL alive for too long after L2CAP disconnection, reset the ACL 3829 * disc_timeout back to HCI_DISCONN_TIMEOUT during L2CAP connect. 3830 */ 3831 conn->hcon->disc_timeout = HCI_DISCONN_TIMEOUT; 3832 3833 bacpy(&chan->src, &conn->hcon->src); 3834 bacpy(&chan->dst, &conn->hcon->dst); 3835 chan->src_type = bdaddr_src_type(conn->hcon); 3836 chan->dst_type = bdaddr_dst_type(conn->hcon); 3837 chan->psm = psm; 3838 chan->dcid = scid; 3839 chan->local_amp_id = amp_id; 3840 3841 __l2cap_chan_add(conn, chan); 3842 3843 dcid = chan->scid; 3844 3845 __set_chan_timer(chan, chan->ops->get_sndtimeo(chan)); 3846 3847 chan->ident = cmd->ident; 3848 3849 if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) { 3850 if (l2cap_chan_check_security(chan, false)) { 3851 if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) { 3852 l2cap_state_change(chan, BT_CONNECT2); 3853 result = L2CAP_CR_PEND; 3854 status = L2CAP_CS_AUTHOR_PEND; 3855 chan->ops->defer(chan); 3856 } else { 3857 /* Force pending result for AMP controllers. 3858 * The connection will succeed after the 3859 * physical link is up. 3860 */ 3861 if (amp_id == AMP_ID_BREDR) { 3862 l2cap_state_change(chan, BT_CONFIG); 3863 result = L2CAP_CR_SUCCESS; 3864 } else { 3865 l2cap_state_change(chan, BT_CONNECT2); 3866 result = L2CAP_CR_PEND; 3867 } 3868 status = L2CAP_CS_NO_INFO; 3869 } 3870 } else { 3871 l2cap_state_change(chan, BT_CONNECT2); 3872 result = L2CAP_CR_PEND; 3873 status = L2CAP_CS_AUTHEN_PEND; 3874 } 3875 } else { 3876 l2cap_state_change(chan, BT_CONNECT2); 3877 result = L2CAP_CR_PEND; 3878 status = L2CAP_CS_NO_INFO; 3879 } 3880 3881 response: 3882 l2cap_chan_unlock(pchan); 3883 mutex_unlock(&conn->chan_lock); 3884 l2cap_chan_put(pchan); 3885 3886 sendresp: 3887 rsp.scid = cpu_to_le16(scid); 3888 rsp.dcid = cpu_to_le16(dcid); 3889 rsp.result = cpu_to_le16(result); 3890 rsp.status = cpu_to_le16(status); 3891 l2cap_send_cmd(conn, cmd->ident, rsp_code, sizeof(rsp), &rsp); 3892 3893 if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) { 3894 struct l2cap_info_req info; 3895 info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK); 3896 3897 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT; 3898 conn->info_ident = l2cap_get_ident(conn); 3899 3900 schedule_delayed_work(&conn->info_timer, L2CAP_INFO_TIMEOUT); 3901 3902 l2cap_send_cmd(conn, conn->info_ident, L2CAP_INFO_REQ, 3903 sizeof(info), &info); 3904 } 3905 3906 if (chan && !test_bit(CONF_REQ_SENT, &chan->conf_state) && 3907 result == L2CAP_CR_SUCCESS) { 3908 u8 buf[128]; 3909 set_bit(CONF_REQ_SENT, &chan->conf_state); 3910 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, 3911 l2cap_build_conf_req(chan, buf, sizeof(buf)), buf); 3912 chan->num_conf_req++; 3913 } 3914 3915 return chan; 3916 } 3917 3918 static int l2cap_connect_req(struct l2cap_conn *conn, 3919 struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data) 3920 { 3921 struct hci_dev *hdev = conn->hcon->hdev; 3922 struct hci_conn *hcon = conn->hcon; 3923 3924 if (cmd_len < sizeof(struct l2cap_conn_req)) 3925 return -EPROTO; 3926 3927 hci_dev_lock(hdev); 3928 if (hci_dev_test_flag(hdev, HCI_MGMT) && 3929 !test_and_set_bit(HCI_CONN_MGMT_CONNECTED, &hcon->flags)) 3930 mgmt_device_connected(hdev, hcon, 0, NULL, 0); 3931 hci_dev_unlock(hdev); 3932 3933 l2cap_connect(conn, cmd, data, L2CAP_CONN_RSP, 0); 3934 return 0; 3935 } 3936 3937 static int l2cap_connect_create_rsp(struct l2cap_conn *conn, 3938 struct l2cap_cmd_hdr *cmd, u16 cmd_len, 3939 u8 *data) 3940 { 3941 struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data; 3942 u16 scid, dcid, result, status; 3943 struct l2cap_chan *chan; 3944 u8 req[128]; 3945 int err; 3946 3947 if (cmd_len < sizeof(*rsp)) 3948 return -EPROTO; 3949 3950 scid = __le16_to_cpu(rsp->scid); 3951 dcid = __le16_to_cpu(rsp->dcid); 3952 result = __le16_to_cpu(rsp->result); 3953 status = __le16_to_cpu(rsp->status); 3954 3955 BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", 3956 dcid, scid, result, status); 3957 3958 mutex_lock(&conn->chan_lock); 3959 3960 if (scid) { 3961 chan = __l2cap_get_chan_by_scid(conn, scid); 3962 if (!chan) { 3963 err = -EBADSLT; 3964 goto unlock; 3965 } 3966 } else { 3967 chan = __l2cap_get_chan_by_ident(conn, cmd->ident); 3968 if (!chan) { 3969 err = -EBADSLT; 3970 goto unlock; 3971 } 3972 } 3973 3974 err = 0; 3975 3976 l2cap_chan_lock(chan); 3977 3978 switch (result) { 3979 case L2CAP_CR_SUCCESS: 3980 l2cap_state_change(chan, BT_CONFIG); 3981 chan->ident = 0; 3982 chan->dcid = dcid; 3983 clear_bit(CONF_CONNECT_PEND, &chan->conf_state); 3984 3985 if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) 3986 break; 3987 3988 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, 3989 l2cap_build_conf_req(chan, req, sizeof(req)), req); 3990 chan->num_conf_req++; 3991 break; 3992 3993 case L2CAP_CR_PEND: 3994 set_bit(CONF_CONNECT_PEND, &chan->conf_state); 3995 break; 3996 3997 default: 3998 l2cap_chan_del(chan, ECONNREFUSED); 3999 break; 4000 } 4001 4002 l2cap_chan_unlock(chan); 4003 4004 unlock: 4005 mutex_unlock(&conn->chan_lock); 4006 4007 return err; 4008 } 4009 4010 static inline void set_default_fcs(struct l2cap_chan *chan) 4011 { 4012 /* FCS is enabled only in ERTM or streaming mode, if one or both 4013 * sides request it. 4014 */ 4015 if (chan->mode != L2CAP_MODE_ERTM && chan->mode != L2CAP_MODE_STREAMING) 4016 chan->fcs = L2CAP_FCS_NONE; 4017 else if (!test_bit(CONF_RECV_NO_FCS, &chan->conf_state)) 4018 chan->fcs = L2CAP_FCS_CRC16; 4019 } 4020 4021 static void l2cap_send_efs_conf_rsp(struct l2cap_chan *chan, void *data, 4022 u8 ident, u16 flags) 4023 { 4024 struct l2cap_conn *conn = chan->conn; 4025 4026 BT_DBG("conn %p chan %p ident %d flags 0x%4.4x", conn, chan, ident, 4027 flags); 4028 4029 clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state); 4030 set_bit(CONF_OUTPUT_DONE, &chan->conf_state); 4031 4032 l2cap_send_cmd(conn, ident, L2CAP_CONF_RSP, 4033 l2cap_build_conf_rsp(chan, data, 4034 L2CAP_CONF_SUCCESS, flags), data); 4035 } 4036 4037 static void cmd_reject_invalid_cid(struct l2cap_conn *conn, u8 ident, 4038 u16 scid, u16 dcid) 4039 { 4040 struct l2cap_cmd_rej_cid rej; 4041 4042 rej.reason = cpu_to_le16(L2CAP_REJ_INVALID_CID); 4043 rej.scid = __cpu_to_le16(scid); 4044 rej.dcid = __cpu_to_le16(dcid); 4045 4046 l2cap_send_cmd(conn, ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej); 4047 } 4048 4049 static inline int l2cap_config_req(struct l2cap_conn *conn, 4050 struct l2cap_cmd_hdr *cmd, u16 cmd_len, 4051 u8 *data) 4052 { 4053 struct l2cap_conf_req *req = (struct l2cap_conf_req *) data; 4054 u16 dcid, flags; 4055 u8 rsp[64]; 4056 struct l2cap_chan *chan; 4057 int len, err = 0; 4058 4059 if (cmd_len < sizeof(*req)) 4060 return -EPROTO; 4061 4062 dcid = __le16_to_cpu(req->dcid); 4063 flags = __le16_to_cpu(req->flags); 4064 4065 BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags); 4066 4067 chan = l2cap_get_chan_by_scid(conn, dcid); 4068 if (!chan) { 4069 cmd_reject_invalid_cid(conn, cmd->ident, dcid, 0); 4070 return 0; 4071 } 4072 4073 if (chan->state != BT_CONFIG && chan->state != BT_CONNECT2) { 4074 cmd_reject_invalid_cid(conn, cmd->ident, chan->scid, 4075 chan->dcid); 4076 goto unlock; 4077 } 4078 4079 /* Reject if config buffer is too small. */ 4080 len = cmd_len - sizeof(*req); 4081 if (chan->conf_len + len > sizeof(chan->conf_req)) { 4082 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, 4083 l2cap_build_conf_rsp(chan, rsp, 4084 L2CAP_CONF_REJECT, flags), rsp); 4085 goto unlock; 4086 } 4087 4088 /* Store config. */ 4089 memcpy(chan->conf_req + chan->conf_len, req->data, len); 4090 chan->conf_len += len; 4091 4092 if (flags & L2CAP_CONF_FLAG_CONTINUATION) { 4093 /* Incomplete config. Send empty response. */ 4094 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, 4095 l2cap_build_conf_rsp(chan, rsp, 4096 L2CAP_CONF_SUCCESS, flags), rsp); 4097 goto unlock; 4098 } 4099 4100 /* Complete config. */ 4101 len = l2cap_parse_conf_req(chan, rsp, sizeof(rsp)); 4102 if (len < 0) { 4103 l2cap_send_disconn_req(chan, ECONNRESET); 4104 goto unlock; 4105 } 4106 4107 chan->ident = cmd->ident; 4108 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp); 4109 chan->num_conf_rsp++; 4110 4111 /* Reset config buffer. */ 4112 chan->conf_len = 0; 4113 4114 if (!test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) 4115 goto unlock; 4116 4117 if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) { 4118 set_default_fcs(chan); 4119 4120 if (chan->mode == L2CAP_MODE_ERTM || 4121 chan->mode == L2CAP_MODE_STREAMING) 4122 err = l2cap_ertm_init(chan); 4123 4124 if (err < 0) 4125 l2cap_send_disconn_req(chan, -err); 4126 else 4127 l2cap_chan_ready(chan); 4128 4129 goto unlock; 4130 } 4131 4132 if (!test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) { 4133 u8 buf[64]; 4134 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, 4135 l2cap_build_conf_req(chan, buf, sizeof(buf)), buf); 4136 chan->num_conf_req++; 4137 } 4138 4139 /* Got Conf Rsp PENDING from remote side and assume we sent 4140 Conf Rsp PENDING in the code above */ 4141 if (test_bit(CONF_REM_CONF_PEND, &chan->conf_state) && 4142 test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) { 4143 4144 /* check compatibility */ 4145 4146 /* Send rsp for BR/EDR channel */ 4147 if (!chan->hs_hcon) 4148 l2cap_send_efs_conf_rsp(chan, rsp, cmd->ident, flags); 4149 else 4150 chan->ident = cmd->ident; 4151 } 4152 4153 unlock: 4154 l2cap_chan_unlock(chan); 4155 return err; 4156 } 4157 4158 static inline int l2cap_config_rsp(struct l2cap_conn *conn, 4159 struct l2cap_cmd_hdr *cmd, u16 cmd_len, 4160 u8 *data) 4161 { 4162 struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data; 4163 u16 scid, flags, result; 4164 struct l2cap_chan *chan; 4165 int len = cmd_len - sizeof(*rsp); 4166 int err = 0; 4167 4168 if (cmd_len < sizeof(*rsp)) 4169 return -EPROTO; 4170 4171 scid = __le16_to_cpu(rsp->scid); 4172 flags = __le16_to_cpu(rsp->flags); 4173 result = __le16_to_cpu(rsp->result); 4174 4175 BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x len %d", scid, flags, 4176 result, len); 4177 4178 chan = l2cap_get_chan_by_scid(conn, scid); 4179 if (!chan) 4180 return 0; 4181 4182 switch (result) { 4183 case L2CAP_CONF_SUCCESS: 4184 l2cap_conf_rfc_get(chan, rsp->data, len); 4185 clear_bit(CONF_REM_CONF_PEND, &chan->conf_state); 4186 break; 4187 4188 case L2CAP_CONF_PENDING: 4189 set_bit(CONF_REM_CONF_PEND, &chan->conf_state); 4190 4191 if (test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) { 4192 char buf[64]; 4193 4194 len = l2cap_parse_conf_rsp(chan, rsp->data, len, 4195 buf, sizeof(buf), &result); 4196 if (len < 0) { 4197 l2cap_send_disconn_req(chan, ECONNRESET); 4198 goto done; 4199 } 4200 4201 if (!chan->hs_hcon) { 4202 l2cap_send_efs_conf_rsp(chan, buf, cmd->ident, 4203 0); 4204 } else { 4205 if (l2cap_check_efs(chan)) { 4206 amp_create_logical_link(chan); 4207 chan->ident = cmd->ident; 4208 } 4209 } 4210 } 4211 goto done; 4212 4213 case L2CAP_CONF_UNACCEPT: 4214 if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) { 4215 char req[64]; 4216 4217 if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) { 4218 l2cap_send_disconn_req(chan, ECONNRESET); 4219 goto done; 4220 } 4221 4222 /* throw out any old stored conf requests */ 4223 result = L2CAP_CONF_SUCCESS; 4224 len = l2cap_parse_conf_rsp(chan, rsp->data, len, 4225 req, sizeof(req), &result); 4226 if (len < 0) { 4227 l2cap_send_disconn_req(chan, ECONNRESET); 4228 goto done; 4229 } 4230 4231 l2cap_send_cmd(conn, l2cap_get_ident(conn), 4232 L2CAP_CONF_REQ, len, req); 4233 chan->num_conf_req++; 4234 if (result != L2CAP_CONF_SUCCESS) 4235 goto done; 4236 break; 4237 } 4238 4239 default: 4240 l2cap_chan_set_err(chan, ECONNRESET); 4241 4242 __set_chan_timer(chan, L2CAP_DISC_REJ_TIMEOUT); 4243 l2cap_send_disconn_req(chan, ECONNRESET); 4244 goto done; 4245 } 4246 4247 if (flags & L2CAP_CONF_FLAG_CONTINUATION) 4248 goto done; 4249 4250 set_bit(CONF_INPUT_DONE, &chan->conf_state); 4251 4252 if (test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) { 4253 set_default_fcs(chan); 4254 4255 if (chan->mode == L2CAP_MODE_ERTM || 4256 chan->mode == L2CAP_MODE_STREAMING) 4257 err = l2cap_ertm_init(chan); 4258 4259 if (err < 0) 4260 l2cap_send_disconn_req(chan, -err); 4261 else 4262 l2cap_chan_ready(chan); 4263 } 4264 4265 done: 4266 l2cap_chan_unlock(chan); 4267 return err; 4268 } 4269 4270 static inline int l2cap_disconnect_req(struct l2cap_conn *conn, 4271 struct l2cap_cmd_hdr *cmd, u16 cmd_len, 4272 u8 *data) 4273 { 4274 struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data; 4275 struct l2cap_disconn_rsp rsp; 4276 u16 dcid, scid; 4277 struct l2cap_chan *chan; 4278 4279 if (cmd_len != sizeof(*req)) 4280 return -EPROTO; 4281 4282 scid = __le16_to_cpu(req->scid); 4283 dcid = __le16_to_cpu(req->dcid); 4284 4285 BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid); 4286 4287 mutex_lock(&conn->chan_lock); 4288 4289 chan = __l2cap_get_chan_by_scid(conn, dcid); 4290 if (!chan) { 4291 mutex_unlock(&conn->chan_lock); 4292 cmd_reject_invalid_cid(conn, cmd->ident, dcid, scid); 4293 return 0; 4294 } 4295 4296 l2cap_chan_lock(chan); 4297 4298 rsp.dcid = cpu_to_le16(chan->scid); 4299 rsp.scid = cpu_to_le16(chan->dcid); 4300 l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp); 4301 4302 chan->ops->set_shutdown(chan); 4303 4304 l2cap_chan_hold(chan); 4305 l2cap_chan_del(chan, ECONNRESET); 4306 4307 l2cap_chan_unlock(chan); 4308 4309 chan->ops->close(chan); 4310 l2cap_chan_put(chan); 4311 4312 mutex_unlock(&conn->chan_lock); 4313 4314 return 0; 4315 } 4316 4317 static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, 4318 struct l2cap_cmd_hdr *cmd, u16 cmd_len, 4319 u8 *data) 4320 { 4321 struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data; 4322 u16 dcid, scid; 4323 struct l2cap_chan *chan; 4324 4325 if (cmd_len != sizeof(*rsp)) 4326 return -EPROTO; 4327 4328 scid = __le16_to_cpu(rsp->scid); 4329 dcid = __le16_to_cpu(rsp->dcid); 4330 4331 BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid); 4332 4333 mutex_lock(&conn->chan_lock); 4334 4335 chan = __l2cap_get_chan_by_scid(conn, scid); 4336 if (!chan) { 4337 mutex_unlock(&conn->chan_lock); 4338 return 0; 4339 } 4340 4341 l2cap_chan_lock(chan); 4342 4343 l2cap_chan_hold(chan); 4344 l2cap_chan_del(chan, 0); 4345 4346 l2cap_chan_unlock(chan); 4347 4348 chan->ops->close(chan); 4349 l2cap_chan_put(chan); 4350 4351 mutex_unlock(&conn->chan_lock); 4352 4353 return 0; 4354 } 4355 4356 static inline int l2cap_information_req(struct l2cap_conn *conn, 4357 struct l2cap_cmd_hdr *cmd, u16 cmd_len, 4358 u8 *data) 4359 { 4360 struct l2cap_info_req *req = (struct l2cap_info_req *) data; 4361 u16 type; 4362 4363 if (cmd_len != sizeof(*req)) 4364 return -EPROTO; 4365 4366 type = __le16_to_cpu(req->type); 4367 4368 BT_DBG("type 0x%4.4x", type); 4369 4370 if (type == L2CAP_IT_FEAT_MASK) { 4371 u8 buf[8]; 4372 u32 feat_mask = l2cap_feat_mask; 4373 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf; 4374 rsp->type = cpu_to_le16(L2CAP_IT_FEAT_MASK); 4375 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS); 4376 if (!disable_ertm) 4377 feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING 4378 | L2CAP_FEAT_FCS; 4379 if (conn->local_fixed_chan & L2CAP_FC_A2MP) 4380 feat_mask |= L2CAP_FEAT_EXT_FLOW 4381 | L2CAP_FEAT_EXT_WINDOW; 4382 4383 put_unaligned_le32(feat_mask, rsp->data); 4384 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf), 4385 buf); 4386 } else if (type == L2CAP_IT_FIXED_CHAN) { 4387 u8 buf[12]; 4388 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf; 4389 4390 rsp->type = cpu_to_le16(L2CAP_IT_FIXED_CHAN); 4391 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS); 4392 rsp->data[0] = conn->local_fixed_chan; 4393 memset(rsp->data + 1, 0, 7); 4394 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf), 4395 buf); 4396 } else { 4397 struct l2cap_info_rsp rsp; 4398 rsp.type = cpu_to_le16(type); 4399 rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP); 4400 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(rsp), 4401 &rsp); 4402 } 4403 4404 return 0; 4405 } 4406 4407 static inline int l2cap_information_rsp(struct l2cap_conn *conn, 4408 struct l2cap_cmd_hdr *cmd, u16 cmd_len, 4409 u8 *data) 4410 { 4411 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data; 4412 u16 type, result; 4413 4414 if (cmd_len < sizeof(*rsp)) 4415 return -EPROTO; 4416 4417 type = __le16_to_cpu(rsp->type); 4418 result = __le16_to_cpu(rsp->result); 4419 4420 BT_DBG("type 0x%4.4x result 0x%2.2x", type, result); 4421 4422 /* L2CAP Info req/rsp are unbound to channels, add extra checks */ 4423 if (cmd->ident != conn->info_ident || 4424 conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) 4425 return 0; 4426 4427 cancel_delayed_work(&conn->info_timer); 4428 4429 if (result != L2CAP_IR_SUCCESS) { 4430 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; 4431 conn->info_ident = 0; 4432 4433 l2cap_conn_start(conn); 4434 4435 return 0; 4436 } 4437 4438 switch (type) { 4439 case L2CAP_IT_FEAT_MASK: 4440 conn->feat_mask = get_unaligned_le32(rsp->data); 4441 4442 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) { 4443 struct l2cap_info_req req; 4444 req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN); 4445 4446 conn->info_ident = l2cap_get_ident(conn); 4447 4448 l2cap_send_cmd(conn, conn->info_ident, 4449 L2CAP_INFO_REQ, sizeof(req), &req); 4450 } else { 4451 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; 4452 conn->info_ident = 0; 4453 4454 l2cap_conn_start(conn); 4455 } 4456 break; 4457 4458 case L2CAP_IT_FIXED_CHAN: 4459 conn->remote_fixed_chan = rsp->data[0]; 4460 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; 4461 conn->info_ident = 0; 4462 4463 l2cap_conn_start(conn); 4464 break; 4465 } 4466 4467 return 0; 4468 } 4469 4470 static int l2cap_create_channel_req(struct l2cap_conn *conn, 4471 struct l2cap_cmd_hdr *cmd, 4472 u16 cmd_len, void *data) 4473 { 4474 struct l2cap_create_chan_req *req = data; 4475 struct l2cap_create_chan_rsp rsp; 4476 struct l2cap_chan *chan; 4477 struct hci_dev *hdev; 4478 u16 psm, scid; 4479 4480 if (cmd_len != sizeof(*req)) 4481 return -EPROTO; 4482 4483 if (!(conn->local_fixed_chan & L2CAP_FC_A2MP)) 4484 return -EINVAL; 4485 4486 psm = le16_to_cpu(req->psm); 4487 scid = le16_to_cpu(req->scid); 4488 4489 BT_DBG("psm 0x%2.2x, scid 0x%4.4x, amp_id %d", psm, scid, req->amp_id); 4490 4491 /* For controller id 0 make BR/EDR connection */ 4492 if (req->amp_id == AMP_ID_BREDR) { 4493 l2cap_connect(conn, cmd, data, L2CAP_CREATE_CHAN_RSP, 4494 req->amp_id); 4495 return 0; 4496 } 4497 4498 /* Validate AMP controller id */ 4499 hdev = hci_dev_get(req->amp_id); 4500 if (!hdev) 4501 goto error; 4502 4503 if (hdev->dev_type != HCI_AMP || !test_bit(HCI_UP, &hdev->flags)) { 4504 hci_dev_put(hdev); 4505 goto error; 4506 } 4507 4508 chan = l2cap_connect(conn, cmd, data, L2CAP_CREATE_CHAN_RSP, 4509 req->amp_id); 4510 if (chan) { 4511 struct amp_mgr *mgr = conn->hcon->amp_mgr; 4512 struct hci_conn *hs_hcon; 4513 4514 hs_hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK, 4515 &conn->hcon->dst); 4516 if (!hs_hcon) { 4517 hci_dev_put(hdev); 4518 cmd_reject_invalid_cid(conn, cmd->ident, chan->scid, 4519 chan->dcid); 4520 return 0; 4521 } 4522 4523 BT_DBG("mgr %p bredr_chan %p hs_hcon %p", mgr, chan, hs_hcon); 4524 4525 mgr->bredr_chan = chan; 4526 chan->hs_hcon = hs_hcon; 4527 chan->fcs = L2CAP_FCS_NONE; 4528 conn->mtu = hdev->block_mtu; 4529 } 4530 4531 hci_dev_put(hdev); 4532 4533 return 0; 4534 4535 error: 4536 rsp.dcid = 0; 4537 rsp.scid = cpu_to_le16(scid); 4538 rsp.result = cpu_to_le16(L2CAP_CR_BAD_AMP); 4539 rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); 4540 4541 l2cap_send_cmd(conn, cmd->ident, L2CAP_CREATE_CHAN_RSP, 4542 sizeof(rsp), &rsp); 4543 4544 return 0; 4545 } 4546 4547 static void l2cap_send_move_chan_req(struct l2cap_chan *chan, u8 dest_amp_id) 4548 { 4549 struct l2cap_move_chan_req req; 4550 u8 ident; 4551 4552 BT_DBG("chan %p, dest_amp_id %d", chan, dest_amp_id); 4553 4554 ident = l2cap_get_ident(chan->conn); 4555 chan->ident = ident; 4556 4557 req.icid = cpu_to_le16(chan->scid); 4558 req.dest_amp_id = dest_amp_id; 4559 4560 l2cap_send_cmd(chan->conn, ident, L2CAP_MOVE_CHAN_REQ, sizeof(req), 4561 &req); 4562 4563 __set_chan_timer(chan, L2CAP_MOVE_TIMEOUT); 4564 } 4565 4566 static void l2cap_send_move_chan_rsp(struct l2cap_chan *chan, u16 result) 4567 { 4568 struct l2cap_move_chan_rsp rsp; 4569 4570 BT_DBG("chan %p, result 0x%4.4x", chan, result); 4571 4572 rsp.icid = cpu_to_le16(chan->dcid); 4573 rsp.result = cpu_to_le16(result); 4574 4575 l2cap_send_cmd(chan->conn, chan->ident, L2CAP_MOVE_CHAN_RSP, 4576 sizeof(rsp), &rsp); 4577 } 4578 4579 static void l2cap_send_move_chan_cfm(struct l2cap_chan *chan, u16 result) 4580 { 4581 struct l2cap_move_chan_cfm cfm; 4582 4583 BT_DBG("chan %p, result 0x%4.4x", chan, result); 4584 4585 chan->ident = l2cap_get_ident(chan->conn); 4586 4587 cfm.icid = cpu_to_le16(chan->scid); 4588 cfm.result = cpu_to_le16(result); 4589 4590 l2cap_send_cmd(chan->conn, chan->ident, L2CAP_MOVE_CHAN_CFM, 4591 sizeof(cfm), &cfm); 4592 4593 __set_chan_timer(chan, L2CAP_MOVE_TIMEOUT); 4594 } 4595 4596 static void l2cap_send_move_chan_cfm_icid(struct l2cap_conn *conn, u16 icid) 4597 { 4598 struct l2cap_move_chan_cfm cfm; 4599 4600 BT_DBG("conn %p, icid 0x%4.4x", conn, icid); 4601 4602 cfm.icid = cpu_to_le16(icid); 4603 cfm.result = cpu_to_le16(L2CAP_MC_UNCONFIRMED); 4604 4605 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_MOVE_CHAN_CFM, 4606 sizeof(cfm), &cfm); 4607 } 4608 4609 static void l2cap_send_move_chan_cfm_rsp(struct l2cap_conn *conn, u8 ident, 4610 u16 icid) 4611 { 4612 struct l2cap_move_chan_cfm_rsp rsp; 4613 4614 BT_DBG("icid 0x%4.4x", icid); 4615 4616 rsp.icid = cpu_to_le16(icid); 4617 l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM_RSP, sizeof(rsp), &rsp); 4618 } 4619 4620 static void __release_logical_link(struct l2cap_chan *chan) 4621 { 4622 chan->hs_hchan = NULL; 4623 chan->hs_hcon = NULL; 4624 4625 /* Placeholder - release the logical link */ 4626 } 4627 4628 static void l2cap_logical_fail(struct l2cap_chan *chan) 4629 { 4630 /* Logical link setup failed */ 4631 if (chan->state != BT_CONNECTED) { 4632 /* Create channel failure, disconnect */ 4633 l2cap_send_disconn_req(chan, ECONNRESET); 4634 return; 4635 } 4636 4637 switch (chan->move_role) { 4638 case L2CAP_MOVE_ROLE_RESPONDER: 4639 l2cap_move_done(chan); 4640 l2cap_send_move_chan_rsp(chan, L2CAP_MR_NOT_SUPP); 4641 break; 4642 case L2CAP_MOVE_ROLE_INITIATOR: 4643 if (chan->move_state == L2CAP_MOVE_WAIT_LOGICAL_COMP || 4644 chan->move_state == L2CAP_MOVE_WAIT_LOGICAL_CFM) { 4645 /* Remote has only sent pending or 4646 * success responses, clean up 4647 */ 4648 l2cap_move_done(chan); 4649 } 4650 4651 /* Other amp move states imply that the move 4652 * has already aborted 4653 */ 4654 l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED); 4655 break; 4656 } 4657 } 4658 4659 static void l2cap_logical_finish_create(struct l2cap_chan *chan, 4660 struct hci_chan *hchan) 4661 { 4662 struct l2cap_conf_rsp rsp; 4663 4664 chan->hs_hchan = hchan; 4665 chan->hs_hcon->l2cap_data = chan->conn; 4666 4667 l2cap_send_efs_conf_rsp(chan, &rsp, chan->ident, 0); 4668 4669 if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) { 4670 int err; 4671 4672 set_default_fcs(chan); 4673 4674 err = l2cap_ertm_init(chan); 4675 if (err < 0) 4676 l2cap_send_disconn_req(chan, -err); 4677 else 4678 l2cap_chan_ready(chan); 4679 } 4680 } 4681 4682 static void l2cap_logical_finish_move(struct l2cap_chan *chan, 4683 struct hci_chan *hchan) 4684 { 4685 chan->hs_hcon = hchan->conn; 4686 chan->hs_hcon->l2cap_data = chan->conn; 4687 4688 BT_DBG("move_state %d", chan->move_state); 4689 4690 switch (chan->move_state) { 4691 case L2CAP_MOVE_WAIT_LOGICAL_COMP: 4692 /* Move confirm will be sent after a success 4693 * response is received 4694 */ 4695 chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS; 4696 break; 4697 case L2CAP_MOVE_WAIT_LOGICAL_CFM: 4698 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { 4699 chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY; 4700 } else if (chan->move_role == L2CAP_MOVE_ROLE_INITIATOR) { 4701 chan->move_state = L2CAP_MOVE_WAIT_CONFIRM_RSP; 4702 l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED); 4703 } else if (chan->move_role == L2CAP_MOVE_ROLE_RESPONDER) { 4704 chan->move_state = L2CAP_MOVE_WAIT_CONFIRM; 4705 l2cap_send_move_chan_rsp(chan, L2CAP_MR_SUCCESS); 4706 } 4707 break; 4708 default: 4709 /* Move was not in expected state, free the channel */ 4710 __release_logical_link(chan); 4711 4712 chan->move_state = L2CAP_MOVE_STABLE; 4713 } 4714 } 4715 4716 /* Call with chan locked */ 4717 void l2cap_logical_cfm(struct l2cap_chan *chan, struct hci_chan *hchan, 4718 u8 status) 4719 { 4720 BT_DBG("chan %p, hchan %p, status %d", chan, hchan, status); 4721 4722 if (status) { 4723 l2cap_logical_fail(chan); 4724 __release_logical_link(chan); 4725 return; 4726 } 4727 4728 if (chan->state != BT_CONNECTED) { 4729 /* Ignore logical link if channel is on BR/EDR */ 4730 if (chan->local_amp_id != AMP_ID_BREDR) 4731 l2cap_logical_finish_create(chan, hchan); 4732 } else { 4733 l2cap_logical_finish_move(chan, hchan); 4734 } 4735 } 4736 4737 void l2cap_move_start(struct l2cap_chan *chan) 4738 { 4739 BT_DBG("chan %p", chan); 4740 4741 if (chan->local_amp_id == AMP_ID_BREDR) { 4742 if (chan->chan_policy != BT_CHANNEL_POLICY_AMP_PREFERRED) 4743 return; 4744 chan->move_role = L2CAP_MOVE_ROLE_INITIATOR; 4745 chan->move_state = L2CAP_MOVE_WAIT_PREPARE; 4746 /* Placeholder - start physical link setup */ 4747 } else { 4748 chan->move_role = L2CAP_MOVE_ROLE_INITIATOR; 4749 chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS; 4750 chan->move_id = 0; 4751 l2cap_move_setup(chan); 4752 l2cap_send_move_chan_req(chan, 0); 4753 } 4754 } 4755 4756 static void l2cap_do_create(struct l2cap_chan *chan, int result, 4757 u8 local_amp_id, u8 remote_amp_id) 4758 { 4759 BT_DBG("chan %p state %s %u -> %u", chan, state_to_string(chan->state), 4760 local_amp_id, remote_amp_id); 4761 4762 chan->fcs = L2CAP_FCS_NONE; 4763 4764 /* Outgoing channel on AMP */ 4765 if (chan->state == BT_CONNECT) { 4766 if (result == L2CAP_CR_SUCCESS) { 4767 chan->local_amp_id = local_amp_id; 4768 l2cap_send_create_chan_req(chan, remote_amp_id); 4769 } else { 4770 /* Revert to BR/EDR connect */ 4771 l2cap_send_conn_req(chan); 4772 } 4773 4774 return; 4775 } 4776 4777 /* Incoming channel on AMP */ 4778 if (__l2cap_no_conn_pending(chan)) { 4779 struct l2cap_conn_rsp rsp; 4780 char buf[128]; 4781 rsp.scid = cpu_to_le16(chan->dcid); 4782 rsp.dcid = cpu_to_le16(chan->scid); 4783 4784 if (result == L2CAP_CR_SUCCESS) { 4785 /* Send successful response */ 4786 rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS); 4787 rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); 4788 } else { 4789 /* Send negative response */ 4790 rsp.result = cpu_to_le16(L2CAP_CR_NO_MEM); 4791 rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); 4792 } 4793 4794 l2cap_send_cmd(chan->conn, chan->ident, L2CAP_CREATE_CHAN_RSP, 4795 sizeof(rsp), &rsp); 4796 4797 if (result == L2CAP_CR_SUCCESS) { 4798 l2cap_state_change(chan, BT_CONFIG); 4799 set_bit(CONF_REQ_SENT, &chan->conf_state); 4800 l2cap_send_cmd(chan->conn, l2cap_get_ident(chan->conn), 4801 L2CAP_CONF_REQ, 4802 l2cap_build_conf_req(chan, buf, sizeof(buf)), buf); 4803 chan->num_conf_req++; 4804 } 4805 } 4806 } 4807 4808 static void l2cap_do_move_initiate(struct l2cap_chan *chan, u8 local_amp_id, 4809 u8 remote_amp_id) 4810 { 4811 l2cap_move_setup(chan); 4812 chan->move_id = local_amp_id; 4813 chan->move_state = L2CAP_MOVE_WAIT_RSP; 4814 4815 l2cap_send_move_chan_req(chan, remote_amp_id); 4816 } 4817 4818 static void l2cap_do_move_respond(struct l2cap_chan *chan, int result) 4819 { 4820 struct hci_chan *hchan = NULL; 4821 4822 /* Placeholder - get hci_chan for logical link */ 4823 4824 if (hchan) { 4825 if (hchan->state == BT_CONNECTED) { 4826 /* Logical link is ready to go */ 4827 chan->hs_hcon = hchan->conn; 4828 chan->hs_hcon->l2cap_data = chan->conn; 4829 chan->move_state = L2CAP_MOVE_WAIT_CONFIRM; 4830 l2cap_send_move_chan_rsp(chan, L2CAP_MR_SUCCESS); 4831 4832 l2cap_logical_cfm(chan, hchan, L2CAP_MR_SUCCESS); 4833 } else { 4834 /* Wait for logical link to be ready */ 4835 chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM; 4836 } 4837 } else { 4838 /* Logical link not available */ 4839 l2cap_send_move_chan_rsp(chan, L2CAP_MR_NOT_ALLOWED); 4840 } 4841 } 4842 4843 static void l2cap_do_move_cancel(struct l2cap_chan *chan, int result) 4844 { 4845 if (chan->move_role == L2CAP_MOVE_ROLE_RESPONDER) { 4846 u8 rsp_result; 4847 if (result == -EINVAL) 4848 rsp_result = L2CAP_MR_BAD_ID; 4849 else 4850 rsp_result = L2CAP_MR_NOT_ALLOWED; 4851 4852 l2cap_send_move_chan_rsp(chan, rsp_result); 4853 } 4854 4855 chan->move_role = L2CAP_MOVE_ROLE_NONE; 4856 chan->move_state = L2CAP_MOVE_STABLE; 4857 4858 /* Restart data transmission */ 4859 l2cap_ertm_send(chan); 4860 } 4861 4862 /* Invoke with locked chan */ 4863 void __l2cap_physical_cfm(struct l2cap_chan *chan, int result) 4864 { 4865 u8 local_amp_id = chan->local_amp_id; 4866 u8 remote_amp_id = chan->remote_amp_id; 4867 4868 BT_DBG("chan %p, result %d, local_amp_id %d, remote_amp_id %d", 4869 chan, result, local_amp_id, remote_amp_id); 4870 4871 if (chan->state == BT_DISCONN || chan->state == BT_CLOSED) { 4872 l2cap_chan_unlock(chan); 4873 return; 4874 } 4875 4876 if (chan->state != BT_CONNECTED) { 4877 l2cap_do_create(chan, result, local_amp_id, remote_amp_id); 4878 } else if (result != L2CAP_MR_SUCCESS) { 4879 l2cap_do_move_cancel(chan, result); 4880 } else { 4881 switch (chan->move_role) { 4882 case L2CAP_MOVE_ROLE_INITIATOR: 4883 l2cap_do_move_initiate(chan, local_amp_id, 4884 remote_amp_id); 4885 break; 4886 case L2CAP_MOVE_ROLE_RESPONDER: 4887 l2cap_do_move_respond(chan, result); 4888 break; 4889 default: 4890 l2cap_do_move_cancel(chan, result); 4891 break; 4892 } 4893 } 4894 } 4895 4896 static inline int l2cap_move_channel_req(struct l2cap_conn *conn, 4897 struct l2cap_cmd_hdr *cmd, 4898 u16 cmd_len, void *data) 4899 { 4900 struct l2cap_move_chan_req *req = data; 4901 struct l2cap_move_chan_rsp rsp; 4902 struct l2cap_chan *chan; 4903 u16 icid = 0; 4904 u16 result = L2CAP_MR_NOT_ALLOWED; 4905 4906 if (cmd_len != sizeof(*req)) 4907 return -EPROTO; 4908 4909 icid = le16_to_cpu(req->icid); 4910 4911 BT_DBG("icid 0x%4.4x, dest_amp_id %d", icid, req->dest_amp_id); 4912 4913 if (!(conn->local_fixed_chan & L2CAP_FC_A2MP)) 4914 return -EINVAL; 4915 4916 chan = l2cap_get_chan_by_dcid(conn, icid); 4917 if (!chan) { 4918 rsp.icid = cpu_to_le16(icid); 4919 rsp.result = cpu_to_le16(L2CAP_MR_NOT_ALLOWED); 4920 l2cap_send_cmd(conn, cmd->ident, L2CAP_MOVE_CHAN_RSP, 4921 sizeof(rsp), &rsp); 4922 return 0; 4923 } 4924 4925 chan->ident = cmd->ident; 4926 4927 if (chan->scid < L2CAP_CID_DYN_START || 4928 chan->chan_policy == BT_CHANNEL_POLICY_BREDR_ONLY || 4929 (chan->mode != L2CAP_MODE_ERTM && 4930 chan->mode != L2CAP_MODE_STREAMING)) { 4931 result = L2CAP_MR_NOT_ALLOWED; 4932 goto send_move_response; 4933 } 4934 4935 if (chan->local_amp_id == req->dest_amp_id) { 4936 result = L2CAP_MR_SAME_ID; 4937 goto send_move_response; 4938 } 4939 4940 if (req->dest_amp_id != AMP_ID_BREDR) { 4941 struct hci_dev *hdev; 4942 hdev = hci_dev_get(req->dest_amp_id); 4943 if (!hdev || hdev->dev_type != HCI_AMP || 4944 !test_bit(HCI_UP, &hdev->flags)) { 4945 if (hdev) 4946 hci_dev_put(hdev); 4947 4948 result = L2CAP_MR_BAD_ID; 4949 goto send_move_response; 4950 } 4951 hci_dev_put(hdev); 4952 } 4953 4954 /* Detect a move collision. Only send a collision response 4955 * if this side has "lost", otherwise proceed with the move. 4956 * The winner has the larger bd_addr. 4957 */ 4958 if ((__chan_is_moving(chan) || 4959 chan->move_role != L2CAP_MOVE_ROLE_NONE) && 4960 bacmp(&conn->hcon->src, &conn->hcon->dst) > 0) { 4961 result = L2CAP_MR_COLLISION; 4962 goto send_move_response; 4963 } 4964 4965 chan->move_role = L2CAP_MOVE_ROLE_RESPONDER; 4966 l2cap_move_setup(chan); 4967 chan->move_id = req->dest_amp_id; 4968 icid = chan->dcid; 4969 4970 if (req->dest_amp_id == AMP_ID_BREDR) { 4971 /* Moving to BR/EDR */ 4972 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { 4973 chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY; 4974 result = L2CAP_MR_PEND; 4975 } else { 4976 chan->move_state = L2CAP_MOVE_WAIT_CONFIRM; 4977 result = L2CAP_MR_SUCCESS; 4978 } 4979 } else { 4980 chan->move_state = L2CAP_MOVE_WAIT_PREPARE; 4981 /* Placeholder - uncomment when amp functions are available */ 4982 /*amp_accept_physical(chan, req->dest_amp_id);*/ 4983 result = L2CAP_MR_PEND; 4984 } 4985 4986 send_move_response: 4987 l2cap_send_move_chan_rsp(chan, result); 4988 4989 l2cap_chan_unlock(chan); 4990 4991 return 0; 4992 } 4993 4994 static void l2cap_move_continue(struct l2cap_conn *conn, u16 icid, u16 result) 4995 { 4996 struct l2cap_chan *chan; 4997 struct hci_chan *hchan = NULL; 4998 4999 chan = l2cap_get_chan_by_scid(conn, icid); 5000 if (!chan) { 5001 l2cap_send_move_chan_cfm_icid(conn, icid); 5002 return; 5003 } 5004 5005 __clear_chan_timer(chan); 5006 if (result == L2CAP_MR_PEND) 5007 __set_chan_timer(chan, L2CAP_MOVE_ERTX_TIMEOUT); 5008 5009 switch (chan->move_state) { 5010 case L2CAP_MOVE_WAIT_LOGICAL_COMP: 5011 /* Move confirm will be sent when logical link 5012 * is complete. 5013 */ 5014 chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM; 5015 break; 5016 case L2CAP_MOVE_WAIT_RSP_SUCCESS: 5017 if (result == L2CAP_MR_PEND) { 5018 break; 5019 } else if (test_bit(CONN_LOCAL_BUSY, 5020 &chan->conn_state)) { 5021 chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY; 5022 } else { 5023 /* Logical link is up or moving to BR/EDR, 5024 * proceed with move 5025 */ 5026 chan->move_state = L2CAP_MOVE_WAIT_CONFIRM_RSP; 5027 l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED); 5028 } 5029 break; 5030 case L2CAP_MOVE_WAIT_RSP: 5031 /* Moving to AMP */ 5032 if (result == L2CAP_MR_SUCCESS) { 5033 /* Remote is ready, send confirm immediately 5034 * after logical link is ready 5035 */ 5036 chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM; 5037 } else { 5038 /* Both logical link and move success 5039 * are required to confirm 5040 */ 5041 chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_COMP; 5042 } 5043 5044 /* Placeholder - get hci_chan for logical link */ 5045 if (!hchan) { 5046 /* Logical link not available */ 5047 l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED); 5048 break; 5049 } 5050 5051 /* If the logical link is not yet connected, do not 5052 * send confirmation. 5053 */ 5054 if (hchan->state != BT_CONNECTED) 5055 break; 5056 5057 /* Logical link is already ready to go */ 5058 5059 chan->hs_hcon = hchan->conn; 5060 chan->hs_hcon->l2cap_data = chan->conn; 5061 5062 if (result == L2CAP_MR_SUCCESS) { 5063 /* Can confirm now */ 5064 l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED); 5065 } else { 5066 /* Now only need move success 5067 * to confirm 5068 */ 5069 chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS; 5070 } 5071 5072 l2cap_logical_cfm(chan, hchan, L2CAP_MR_SUCCESS); 5073 break; 5074 default: 5075 /* Any other amp move state means the move failed. */ 5076 chan->move_id = chan->local_amp_id; 5077 l2cap_move_done(chan); 5078 l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED); 5079 } 5080 5081 l2cap_chan_unlock(chan); 5082 } 5083 5084 static void l2cap_move_fail(struct l2cap_conn *conn, u8 ident, u16 icid, 5085 u16 result) 5086 { 5087 struct l2cap_chan *chan; 5088 5089 chan = l2cap_get_chan_by_ident(conn, ident); 5090 if (!chan) { 5091 /* Could not locate channel, icid is best guess */ 5092 l2cap_send_move_chan_cfm_icid(conn, icid); 5093 return; 5094 } 5095 5096 __clear_chan_timer(chan); 5097 5098 if (chan->move_role == L2CAP_MOVE_ROLE_INITIATOR) { 5099 if (result == L2CAP_MR_COLLISION) { 5100 chan->move_role = L2CAP_MOVE_ROLE_RESPONDER; 5101 } else { 5102 /* Cleanup - cancel move */ 5103 chan->move_id = chan->local_amp_id; 5104 l2cap_move_done(chan); 5105 } 5106 } 5107 5108 l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED); 5109 5110 l2cap_chan_unlock(chan); 5111 } 5112 5113 static int l2cap_move_channel_rsp(struct l2cap_conn *conn, 5114 struct l2cap_cmd_hdr *cmd, 5115 u16 cmd_len, void *data) 5116 { 5117 struct l2cap_move_chan_rsp *rsp = data; 5118 u16 icid, result; 5119 5120 if (cmd_len != sizeof(*rsp)) 5121 return -EPROTO; 5122 5123 icid = le16_to_cpu(rsp->icid); 5124 result = le16_to_cpu(rsp->result); 5125 5126 BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result); 5127 5128 if (result == L2CAP_MR_SUCCESS || result == L2CAP_MR_PEND) 5129 l2cap_move_continue(conn, icid, result); 5130 else 5131 l2cap_move_fail(conn, cmd->ident, icid, result); 5132 5133 return 0; 5134 } 5135 5136 static int l2cap_move_channel_confirm(struct l2cap_conn *conn, 5137 struct l2cap_cmd_hdr *cmd, 5138 u16 cmd_len, void *data) 5139 { 5140 struct l2cap_move_chan_cfm *cfm = data; 5141 struct l2cap_chan *chan; 5142 u16 icid, result; 5143 5144 if (cmd_len != sizeof(*cfm)) 5145 return -EPROTO; 5146 5147 icid = le16_to_cpu(cfm->icid); 5148 result = le16_to_cpu(cfm->result); 5149 5150 BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result); 5151 5152 chan = l2cap_get_chan_by_dcid(conn, icid); 5153 if (!chan) { 5154 /* Spec requires a response even if the icid was not found */ 5155 l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid); 5156 return 0; 5157 } 5158 5159 if (chan->move_state == L2CAP_MOVE_WAIT_CONFIRM) { 5160 if (result == L2CAP_MC_CONFIRMED) { 5161 chan->local_amp_id = chan->move_id; 5162 if (chan->local_amp_id == AMP_ID_BREDR) 5163 __release_logical_link(chan); 5164 } else { 5165 chan->move_id = chan->local_amp_id; 5166 } 5167 5168 l2cap_move_done(chan); 5169 } 5170 5171 l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid); 5172 5173 l2cap_chan_unlock(chan); 5174 5175 return 0; 5176 } 5177 5178 static inline int l2cap_move_channel_confirm_rsp(struct l2cap_conn *conn, 5179 struct l2cap_cmd_hdr *cmd, 5180 u16 cmd_len, void *data) 5181 { 5182 struct l2cap_move_chan_cfm_rsp *rsp = data; 5183 struct l2cap_chan *chan; 5184 u16 icid; 5185 5186 if (cmd_len != sizeof(*rsp)) 5187 return -EPROTO; 5188 5189 icid = le16_to_cpu(rsp->icid); 5190 5191 BT_DBG("icid 0x%4.4x", icid); 5192 5193 chan = l2cap_get_chan_by_scid(conn, icid); 5194 if (!chan) 5195 return 0; 5196 5197 __clear_chan_timer(chan); 5198 5199 if (chan->move_state == L2CAP_MOVE_WAIT_CONFIRM_RSP) { 5200 chan->local_amp_id = chan->move_id; 5201 5202 if (chan->local_amp_id == AMP_ID_BREDR && chan->hs_hchan) 5203 __release_logical_link(chan); 5204 5205 l2cap_move_done(chan); 5206 } 5207 5208 l2cap_chan_unlock(chan); 5209 5210 return 0; 5211 } 5212 5213 static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn, 5214 struct l2cap_cmd_hdr *cmd, 5215 u16 cmd_len, u8 *data) 5216 { 5217 struct hci_conn *hcon = conn->hcon; 5218 struct l2cap_conn_param_update_req *req; 5219 struct l2cap_conn_param_update_rsp rsp; 5220 u16 min, max, latency, to_multiplier; 5221 int err; 5222 5223 if (hcon->role != HCI_ROLE_MASTER) 5224 return -EINVAL; 5225 5226 if (cmd_len != sizeof(struct l2cap_conn_param_update_req)) 5227 return -EPROTO; 5228 5229 req = (struct l2cap_conn_param_update_req *) data; 5230 min = __le16_to_cpu(req->min); 5231 max = __le16_to_cpu(req->max); 5232 latency = __le16_to_cpu(req->latency); 5233 to_multiplier = __le16_to_cpu(req->to_multiplier); 5234 5235 BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x", 5236 min, max, latency, to_multiplier); 5237 5238 memset(&rsp, 0, sizeof(rsp)); 5239 5240 err = hci_check_conn_params(min, max, latency, to_multiplier); 5241 if (err) 5242 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_REJECTED); 5243 else 5244 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED); 5245 5246 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP, 5247 sizeof(rsp), &rsp); 5248 5249 if (!err) { 5250 u8 store_hint; 5251 5252 store_hint = hci_le_conn_update(hcon, min, max, latency, 5253 to_multiplier); 5254 mgmt_new_conn_param(hcon->hdev, &hcon->dst, hcon->dst_type, 5255 store_hint, min, max, latency, 5256 to_multiplier); 5257 5258 } 5259 5260 return 0; 5261 } 5262 5263 static int l2cap_le_connect_rsp(struct l2cap_conn *conn, 5264 struct l2cap_cmd_hdr *cmd, u16 cmd_len, 5265 u8 *data) 5266 { 5267 struct l2cap_le_conn_rsp *rsp = (struct l2cap_le_conn_rsp *) data; 5268 struct hci_conn *hcon = conn->hcon; 5269 u16 dcid, mtu, mps, credits, result; 5270 struct l2cap_chan *chan; 5271 int err, sec_level; 5272 5273 if (cmd_len < sizeof(*rsp)) 5274 return -EPROTO; 5275 5276 dcid = __le16_to_cpu(rsp->dcid); 5277 mtu = __le16_to_cpu(rsp->mtu); 5278 mps = __le16_to_cpu(rsp->mps); 5279 credits = __le16_to_cpu(rsp->credits); 5280 result = __le16_to_cpu(rsp->result); 5281 5282 if (result == L2CAP_CR_SUCCESS && (mtu < 23 || mps < 23 || 5283 dcid < L2CAP_CID_DYN_START || 5284 dcid > L2CAP_CID_LE_DYN_END)) 5285 return -EPROTO; 5286 5287 BT_DBG("dcid 0x%4.4x mtu %u mps %u credits %u result 0x%2.2x", 5288 dcid, mtu, mps, credits, result); 5289 5290 mutex_lock(&conn->chan_lock); 5291 5292 chan = __l2cap_get_chan_by_ident(conn, cmd->ident); 5293 if (!chan) { 5294 err = -EBADSLT; 5295 goto unlock; 5296 } 5297 5298 err = 0; 5299 5300 l2cap_chan_lock(chan); 5301 5302 switch (result) { 5303 case L2CAP_CR_SUCCESS: 5304 if (__l2cap_get_chan_by_dcid(conn, dcid)) { 5305 err = -EBADSLT; 5306 break; 5307 } 5308 5309 chan->ident = 0; 5310 chan->dcid = dcid; 5311 chan->omtu = mtu; 5312 chan->remote_mps = mps; 5313 chan->tx_credits = credits; 5314 l2cap_chan_ready(chan); 5315 break; 5316 5317 case L2CAP_CR_AUTHENTICATION: 5318 case L2CAP_CR_ENCRYPTION: 5319 /* If we already have MITM protection we can't do 5320 * anything. 5321 */ 5322 if (hcon->sec_level > BT_SECURITY_MEDIUM) { 5323 l2cap_chan_del(chan, ECONNREFUSED); 5324 break; 5325 } 5326 5327 sec_level = hcon->sec_level + 1; 5328 if (chan->sec_level < sec_level) 5329 chan->sec_level = sec_level; 5330 5331 /* We'll need to send a new Connect Request */ 5332 clear_bit(FLAG_LE_CONN_REQ_SENT, &chan->flags); 5333 5334 smp_conn_security(hcon, chan->sec_level); 5335 break; 5336 5337 default: 5338 l2cap_chan_del(chan, ECONNREFUSED); 5339 break; 5340 } 5341 5342 l2cap_chan_unlock(chan); 5343 5344 unlock: 5345 mutex_unlock(&conn->chan_lock); 5346 5347 return err; 5348 } 5349 5350 static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn, 5351 struct l2cap_cmd_hdr *cmd, u16 cmd_len, 5352 u8 *data) 5353 { 5354 int err = 0; 5355 5356 switch (cmd->code) { 5357 case L2CAP_COMMAND_REJ: 5358 l2cap_command_rej(conn, cmd, cmd_len, data); 5359 break; 5360 5361 case L2CAP_CONN_REQ: 5362 err = l2cap_connect_req(conn, cmd, cmd_len, data); 5363 break; 5364 5365 case L2CAP_CONN_RSP: 5366 case L2CAP_CREATE_CHAN_RSP: 5367 l2cap_connect_create_rsp(conn, cmd, cmd_len, data); 5368 break; 5369 5370 case L2CAP_CONF_REQ: 5371 err = l2cap_config_req(conn, cmd, cmd_len, data); 5372 break; 5373 5374 case L2CAP_CONF_RSP: 5375 l2cap_config_rsp(conn, cmd, cmd_len, data); 5376 break; 5377 5378 case L2CAP_DISCONN_REQ: 5379 err = l2cap_disconnect_req(conn, cmd, cmd_len, data); 5380 break; 5381 5382 case L2CAP_DISCONN_RSP: 5383 l2cap_disconnect_rsp(conn, cmd, cmd_len, data); 5384 break; 5385 5386 case L2CAP_ECHO_REQ: 5387 l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data); 5388 break; 5389 5390 case L2CAP_ECHO_RSP: 5391 break; 5392 5393 case L2CAP_INFO_REQ: 5394 err = l2cap_information_req(conn, cmd, cmd_len, data); 5395 break; 5396 5397 case L2CAP_INFO_RSP: 5398 l2cap_information_rsp(conn, cmd, cmd_len, data); 5399 break; 5400 5401 case L2CAP_CREATE_CHAN_REQ: 5402 err = l2cap_create_channel_req(conn, cmd, cmd_len, data); 5403 break; 5404 5405 case L2CAP_MOVE_CHAN_REQ: 5406 err = l2cap_move_channel_req(conn, cmd, cmd_len, data); 5407 break; 5408 5409 case L2CAP_MOVE_CHAN_RSP: 5410 l2cap_move_channel_rsp(conn, cmd, cmd_len, data); 5411 break; 5412 5413 case L2CAP_MOVE_CHAN_CFM: 5414 err = l2cap_move_channel_confirm(conn, cmd, cmd_len, data); 5415 break; 5416 5417 case L2CAP_MOVE_CHAN_CFM_RSP: 5418 l2cap_move_channel_confirm_rsp(conn, cmd, cmd_len, data); 5419 break; 5420 5421 default: 5422 BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code); 5423 err = -EINVAL; 5424 break; 5425 } 5426 5427 return err; 5428 } 5429 5430 static int l2cap_le_connect_req(struct l2cap_conn *conn, 5431 struct l2cap_cmd_hdr *cmd, u16 cmd_len, 5432 u8 *data) 5433 { 5434 struct l2cap_le_conn_req *req = (struct l2cap_le_conn_req *) data; 5435 struct l2cap_le_conn_rsp rsp; 5436 struct l2cap_chan *chan, *pchan; 5437 u16 dcid, scid, credits, mtu, mps; 5438 __le16 psm; 5439 u8 result; 5440 5441 if (cmd_len != sizeof(*req)) 5442 return -EPROTO; 5443 5444 scid = __le16_to_cpu(req->scid); 5445 mtu = __le16_to_cpu(req->mtu); 5446 mps = __le16_to_cpu(req->mps); 5447 psm = req->psm; 5448 dcid = 0; 5449 credits = 0; 5450 5451 if (mtu < 23 || mps < 23) 5452 return -EPROTO; 5453 5454 BT_DBG("psm 0x%2.2x scid 0x%4.4x mtu %u mps %u", __le16_to_cpu(psm), 5455 scid, mtu, mps); 5456 5457 /* Check if we have socket listening on psm */ 5458 pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, &conn->hcon->src, 5459 &conn->hcon->dst, LE_LINK); 5460 if (!pchan) { 5461 result = L2CAP_CR_BAD_PSM; 5462 chan = NULL; 5463 goto response; 5464 } 5465 5466 mutex_lock(&conn->chan_lock); 5467 l2cap_chan_lock(pchan); 5468 5469 if (!smp_sufficient_security(conn->hcon, pchan->sec_level, 5470 SMP_ALLOW_STK)) { 5471 result = L2CAP_CR_AUTHENTICATION; 5472 chan = NULL; 5473 goto response_unlock; 5474 } 5475 5476 /* Check for valid dynamic CID range */ 5477 if (scid < L2CAP_CID_DYN_START || scid > L2CAP_CID_LE_DYN_END) { 5478 result = L2CAP_CR_INVALID_SCID; 5479 chan = NULL; 5480 goto response_unlock; 5481 } 5482 5483 /* Check if we already have channel with that dcid */ 5484 if (__l2cap_get_chan_by_dcid(conn, scid)) { 5485 result = L2CAP_CR_SCID_IN_USE; 5486 chan = NULL; 5487 goto response_unlock; 5488 } 5489 5490 chan = pchan->ops->new_connection(pchan); 5491 if (!chan) { 5492 result = L2CAP_CR_NO_MEM; 5493 goto response_unlock; 5494 } 5495 5496 l2cap_le_flowctl_init(chan); 5497 5498 bacpy(&chan->src, &conn->hcon->src); 5499 bacpy(&chan->dst, &conn->hcon->dst); 5500 chan->src_type = bdaddr_src_type(conn->hcon); 5501 chan->dst_type = bdaddr_dst_type(conn->hcon); 5502 chan->psm = psm; 5503 chan->dcid = scid; 5504 chan->omtu = mtu; 5505 chan->remote_mps = mps; 5506 chan->tx_credits = __le16_to_cpu(req->credits); 5507 5508 __l2cap_chan_add(conn, chan); 5509 dcid = chan->scid; 5510 credits = chan->rx_credits; 5511 5512 __set_chan_timer(chan, chan->ops->get_sndtimeo(chan)); 5513 5514 chan->ident = cmd->ident; 5515 5516 if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) { 5517 l2cap_state_change(chan, BT_CONNECT2); 5518 /* The following result value is actually not defined 5519 * for LE CoC but we use it to let the function know 5520 * that it should bail out after doing its cleanup 5521 * instead of sending a response. 5522 */ 5523 result = L2CAP_CR_PEND; 5524 chan->ops->defer(chan); 5525 } else { 5526 l2cap_chan_ready(chan); 5527 result = L2CAP_CR_SUCCESS; 5528 } 5529 5530 response_unlock: 5531 l2cap_chan_unlock(pchan); 5532 mutex_unlock(&conn->chan_lock); 5533 l2cap_chan_put(pchan); 5534 5535 if (result == L2CAP_CR_PEND) 5536 return 0; 5537 5538 response: 5539 if (chan) { 5540 rsp.mtu = cpu_to_le16(chan->imtu); 5541 rsp.mps = cpu_to_le16(chan->mps); 5542 } else { 5543 rsp.mtu = 0; 5544 rsp.mps = 0; 5545 } 5546 5547 rsp.dcid = cpu_to_le16(dcid); 5548 rsp.credits = cpu_to_le16(credits); 5549 rsp.result = cpu_to_le16(result); 5550 5551 l2cap_send_cmd(conn, cmd->ident, L2CAP_LE_CONN_RSP, sizeof(rsp), &rsp); 5552 5553 return 0; 5554 } 5555 5556 static inline int l2cap_le_credits(struct l2cap_conn *conn, 5557 struct l2cap_cmd_hdr *cmd, u16 cmd_len, 5558 u8 *data) 5559 { 5560 struct l2cap_le_credits *pkt; 5561 struct l2cap_chan *chan; 5562 u16 cid, credits, max_credits; 5563 5564 if (cmd_len != sizeof(*pkt)) 5565 return -EPROTO; 5566 5567 pkt = (struct l2cap_le_credits *) data; 5568 cid = __le16_to_cpu(pkt->cid); 5569 credits = __le16_to_cpu(pkt->credits); 5570 5571 BT_DBG("cid 0x%4.4x credits 0x%4.4x", cid, credits); 5572 5573 chan = l2cap_get_chan_by_dcid(conn, cid); 5574 if (!chan) 5575 return -EBADSLT; 5576 5577 max_credits = LE_FLOWCTL_MAX_CREDITS - chan->tx_credits; 5578 if (credits > max_credits) { 5579 BT_ERR("LE credits overflow"); 5580 l2cap_send_disconn_req(chan, ECONNRESET); 5581 l2cap_chan_unlock(chan); 5582 5583 /* Return 0 so that we don't trigger an unnecessary 5584 * command reject packet. 5585 */ 5586 return 0; 5587 } 5588 5589 chan->tx_credits += credits; 5590 5591 /* Resume sending */ 5592 l2cap_le_flowctl_send(chan); 5593 5594 if (chan->tx_credits) 5595 chan->ops->resume(chan); 5596 5597 l2cap_chan_unlock(chan); 5598 5599 return 0; 5600 } 5601 5602 static inline int l2cap_le_command_rej(struct l2cap_conn *conn, 5603 struct l2cap_cmd_hdr *cmd, u16 cmd_len, 5604 u8 *data) 5605 { 5606 struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data; 5607 struct l2cap_chan *chan; 5608 5609 if (cmd_len < sizeof(*rej)) 5610 return -EPROTO; 5611 5612 mutex_lock(&conn->chan_lock); 5613 5614 chan = __l2cap_get_chan_by_ident(conn, cmd->ident); 5615 if (!chan) 5616 goto done; 5617 5618 l2cap_chan_lock(chan); 5619 l2cap_chan_del(chan, ECONNREFUSED); 5620 l2cap_chan_unlock(chan); 5621 5622 done: 5623 mutex_unlock(&conn->chan_lock); 5624 return 0; 5625 } 5626 5627 static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn, 5628 struct l2cap_cmd_hdr *cmd, u16 cmd_len, 5629 u8 *data) 5630 { 5631 int err = 0; 5632 5633 switch (cmd->code) { 5634 case L2CAP_COMMAND_REJ: 5635 l2cap_le_command_rej(conn, cmd, cmd_len, data); 5636 break; 5637 5638 case L2CAP_CONN_PARAM_UPDATE_REQ: 5639 err = l2cap_conn_param_update_req(conn, cmd, cmd_len, data); 5640 break; 5641 5642 case L2CAP_CONN_PARAM_UPDATE_RSP: 5643 break; 5644 5645 case L2CAP_LE_CONN_RSP: 5646 l2cap_le_connect_rsp(conn, cmd, cmd_len, data); 5647 break; 5648 5649 case L2CAP_LE_CONN_REQ: 5650 err = l2cap_le_connect_req(conn, cmd, cmd_len, data); 5651 break; 5652 5653 case L2CAP_LE_CREDITS: 5654 err = l2cap_le_credits(conn, cmd, cmd_len, data); 5655 break; 5656 5657 case L2CAP_DISCONN_REQ: 5658 err = l2cap_disconnect_req(conn, cmd, cmd_len, data); 5659 break; 5660 5661 case L2CAP_DISCONN_RSP: 5662 l2cap_disconnect_rsp(conn, cmd, cmd_len, data); 5663 break; 5664 5665 default: 5666 BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code); 5667 err = -EINVAL; 5668 break; 5669 } 5670 5671 return err; 5672 } 5673 5674 static inline void l2cap_le_sig_channel(struct l2cap_conn *conn, 5675 struct sk_buff *skb) 5676 { 5677 struct hci_conn *hcon = conn->hcon; 5678 struct l2cap_cmd_hdr *cmd; 5679 u16 len; 5680 int err; 5681 5682 if (hcon->type != LE_LINK) 5683 goto drop; 5684 5685 if (skb->len < L2CAP_CMD_HDR_SIZE) 5686 goto drop; 5687 5688 cmd = (void *) skb->data; 5689 skb_pull(skb, L2CAP_CMD_HDR_SIZE); 5690 5691 len = le16_to_cpu(cmd->len); 5692 5693 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd->code, len, cmd->ident); 5694 5695 if (len != skb->len || !cmd->ident) { 5696 BT_DBG("corrupted command"); 5697 goto drop; 5698 } 5699 5700 err = l2cap_le_sig_cmd(conn, cmd, len, skb->data); 5701 if (err) { 5702 struct l2cap_cmd_rej_unk rej; 5703 5704 BT_ERR("Wrong link type (%d)", err); 5705 5706 rej.reason = cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD); 5707 l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ, 5708 sizeof(rej), &rej); 5709 } 5710 5711 drop: 5712 kfree_skb(skb); 5713 } 5714 5715 static inline void l2cap_sig_channel(struct l2cap_conn *conn, 5716 struct sk_buff *skb) 5717 { 5718 struct hci_conn *hcon = conn->hcon; 5719 u8 *data = skb->data; 5720 int len = skb->len; 5721 struct l2cap_cmd_hdr cmd; 5722 int err; 5723 5724 l2cap_raw_recv(conn, skb); 5725 5726 if (hcon->type != ACL_LINK) 5727 goto drop; 5728 5729 while (len >= L2CAP_CMD_HDR_SIZE) { 5730 u16 cmd_len; 5731 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE); 5732 data += L2CAP_CMD_HDR_SIZE; 5733 len -= L2CAP_CMD_HDR_SIZE; 5734 5735 cmd_len = le16_to_cpu(cmd.len); 5736 5737 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, 5738 cmd.ident); 5739 5740 if (cmd_len > len || !cmd.ident) { 5741 BT_DBG("corrupted command"); 5742 break; 5743 } 5744 5745 err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data); 5746 if (err) { 5747 struct l2cap_cmd_rej_unk rej; 5748 5749 BT_ERR("Wrong link type (%d)", err); 5750 5751 rej.reason = cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD); 5752 l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, 5753 sizeof(rej), &rej); 5754 } 5755 5756 data += cmd_len; 5757 len -= cmd_len; 5758 } 5759 5760 drop: 5761 kfree_skb(skb); 5762 } 5763 5764 static int l2cap_check_fcs(struct l2cap_chan *chan, struct sk_buff *skb) 5765 { 5766 u16 our_fcs, rcv_fcs; 5767 int hdr_size; 5768 5769 if (test_bit(FLAG_EXT_CTRL, &chan->flags)) 5770 hdr_size = L2CAP_EXT_HDR_SIZE; 5771 else 5772 hdr_size = L2CAP_ENH_HDR_SIZE; 5773 5774 if (chan->fcs == L2CAP_FCS_CRC16) { 5775 skb_trim(skb, skb->len - L2CAP_FCS_SIZE); 5776 rcv_fcs = get_unaligned_le16(skb->data + skb->len); 5777 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size); 5778 5779 if (our_fcs != rcv_fcs) 5780 return -EBADMSG; 5781 } 5782 return 0; 5783 } 5784 5785 static void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan) 5786 { 5787 struct l2cap_ctrl control; 5788 5789 BT_DBG("chan %p", chan); 5790 5791 memset(&control, 0, sizeof(control)); 5792 control.sframe = 1; 5793 control.final = 1; 5794 control.reqseq = chan->buffer_seq; 5795 set_bit(CONN_SEND_FBIT, &chan->conn_state); 5796 5797 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { 5798 control.super = L2CAP_SUPER_RNR; 5799 l2cap_send_sframe(chan, &control); 5800 } 5801 5802 if (test_and_clear_bit(CONN_REMOTE_BUSY, &chan->conn_state) && 5803 chan->unacked_frames > 0) 5804 __set_retrans_timer(chan); 5805 5806 /* Send pending iframes */ 5807 l2cap_ertm_send(chan); 5808 5809 if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state) && 5810 test_bit(CONN_SEND_FBIT, &chan->conn_state)) { 5811 /* F-bit wasn't sent in an s-frame or i-frame yet, so 5812 * send it now. 5813 */ 5814 control.super = L2CAP_SUPER_RR; 5815 l2cap_send_sframe(chan, &control); 5816 } 5817 } 5818 5819 static void append_skb_frag(struct sk_buff *skb, struct sk_buff *new_frag, 5820 struct sk_buff **last_frag) 5821 { 5822 /* skb->len reflects data in skb as well as all fragments 5823 * skb->data_len reflects only data in fragments 5824 */ 5825 if (!skb_has_frag_list(skb)) 5826 skb_shinfo(skb)->frag_list = new_frag; 5827 5828 new_frag->next = NULL; 5829 5830 (*last_frag)->next = new_frag; 5831 *last_frag = new_frag; 5832 5833 skb->len += new_frag->len; 5834 skb->data_len += new_frag->len; 5835 skb->truesize += new_frag->truesize; 5836 } 5837 5838 static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, 5839 struct l2cap_ctrl *control) 5840 { 5841 int err = -EINVAL; 5842 5843 switch (control->sar) { 5844 case L2CAP_SAR_UNSEGMENTED: 5845 if (chan->sdu) 5846 break; 5847 5848 err = chan->ops->recv(chan, skb); 5849 break; 5850 5851 case L2CAP_SAR_START: 5852 if (chan->sdu) 5853 break; 5854 5855 if (!pskb_may_pull(skb, L2CAP_SDULEN_SIZE)) 5856 break; 5857 5858 chan->sdu_len = get_unaligned_le16(skb->data); 5859 skb_pull(skb, L2CAP_SDULEN_SIZE); 5860 5861 if (chan->sdu_len > chan->imtu) { 5862 err = -EMSGSIZE; 5863 break; 5864 } 5865 5866 if (skb->len >= chan->sdu_len) 5867 break; 5868 5869 chan->sdu = skb; 5870 chan->sdu_last_frag = skb; 5871 5872 skb = NULL; 5873 err = 0; 5874 break; 5875 5876 case L2CAP_SAR_CONTINUE: 5877 if (!chan->sdu) 5878 break; 5879 5880 append_skb_frag(chan->sdu, skb, 5881 &chan->sdu_last_frag); 5882 skb = NULL; 5883 5884 if (chan->sdu->len >= chan->sdu_len) 5885 break; 5886 5887 err = 0; 5888 break; 5889 5890 case L2CAP_SAR_END: 5891 if (!chan->sdu) 5892 break; 5893 5894 append_skb_frag(chan->sdu, skb, 5895 &chan->sdu_last_frag); 5896 skb = NULL; 5897 5898 if (chan->sdu->len != chan->sdu_len) 5899 break; 5900 5901 err = chan->ops->recv(chan, chan->sdu); 5902 5903 if (!err) { 5904 /* Reassembly complete */ 5905 chan->sdu = NULL; 5906 chan->sdu_last_frag = NULL; 5907 chan->sdu_len = 0; 5908 } 5909 break; 5910 } 5911 5912 if (err) { 5913 kfree_skb(skb); 5914 kfree_skb(chan->sdu); 5915 chan->sdu = NULL; 5916 chan->sdu_last_frag = NULL; 5917 chan->sdu_len = 0; 5918 } 5919 5920 return err; 5921 } 5922 5923 static int l2cap_resegment(struct l2cap_chan *chan) 5924 { 5925 /* Placeholder */ 5926 return 0; 5927 } 5928 5929 void l2cap_chan_busy(struct l2cap_chan *chan, int busy) 5930 { 5931 u8 event; 5932 5933 if (chan->mode != L2CAP_MODE_ERTM) 5934 return; 5935 5936 event = busy ? L2CAP_EV_LOCAL_BUSY_DETECTED : L2CAP_EV_LOCAL_BUSY_CLEAR; 5937 l2cap_tx(chan, NULL, NULL, event); 5938 } 5939 5940 static int l2cap_rx_queued_iframes(struct l2cap_chan *chan) 5941 { 5942 int err = 0; 5943 /* Pass sequential frames to l2cap_reassemble_sdu() 5944 * until a gap is encountered. 5945 */ 5946 5947 BT_DBG("chan %p", chan); 5948 5949 while (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { 5950 struct sk_buff *skb; 5951 BT_DBG("Searching for skb with txseq %d (queue len %d)", 5952 chan->buffer_seq, skb_queue_len(&chan->srej_q)); 5953 5954 skb = l2cap_ertm_seq_in_queue(&chan->srej_q, chan->buffer_seq); 5955 5956 if (!skb) 5957 break; 5958 5959 skb_unlink(skb, &chan->srej_q); 5960 chan->buffer_seq = __next_seq(chan, chan->buffer_seq); 5961 err = l2cap_reassemble_sdu(chan, skb, &bt_cb(skb)->l2cap); 5962 if (err) 5963 break; 5964 } 5965 5966 if (skb_queue_empty(&chan->srej_q)) { 5967 chan->rx_state = L2CAP_RX_STATE_RECV; 5968 l2cap_send_ack(chan); 5969 } 5970 5971 return err; 5972 } 5973 5974 static void l2cap_handle_srej(struct l2cap_chan *chan, 5975 struct l2cap_ctrl *control) 5976 { 5977 struct sk_buff *skb; 5978 5979 BT_DBG("chan %p, control %p", chan, control); 5980 5981 if (control->reqseq == chan->next_tx_seq) { 5982 BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq); 5983 l2cap_send_disconn_req(chan, ECONNRESET); 5984 return; 5985 } 5986 5987 skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq); 5988 5989 if (skb == NULL) { 5990 BT_DBG("Seq %d not available for retransmission", 5991 control->reqseq); 5992 return; 5993 } 5994 5995 if (chan->max_tx != 0 && bt_cb(skb)->l2cap.retries >= chan->max_tx) { 5996 BT_DBG("Retry limit exceeded (%d)", chan->max_tx); 5997 l2cap_send_disconn_req(chan, ECONNRESET); 5998 return; 5999 } 6000 6001 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); 6002 6003 if (control->poll) { 6004 l2cap_pass_to_tx(chan, control); 6005 6006 set_bit(CONN_SEND_FBIT, &chan->conn_state); 6007 l2cap_retransmit(chan, control); 6008 l2cap_ertm_send(chan); 6009 6010 if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) { 6011 set_bit(CONN_SREJ_ACT, &chan->conn_state); 6012 chan->srej_save_reqseq = control->reqseq; 6013 } 6014 } else { 6015 l2cap_pass_to_tx_fbit(chan, control); 6016 6017 if (control->final) { 6018 if (chan->srej_save_reqseq != control->reqseq || 6019 !test_and_clear_bit(CONN_SREJ_ACT, 6020 &chan->conn_state)) 6021 l2cap_retransmit(chan, control); 6022 } else { 6023 l2cap_retransmit(chan, control); 6024 if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) { 6025 set_bit(CONN_SREJ_ACT, &chan->conn_state); 6026 chan->srej_save_reqseq = control->reqseq; 6027 } 6028 } 6029 } 6030 } 6031 6032 static void l2cap_handle_rej(struct l2cap_chan *chan, 6033 struct l2cap_ctrl *control) 6034 { 6035 struct sk_buff *skb; 6036 6037 BT_DBG("chan %p, control %p", chan, control); 6038 6039 if (control->reqseq == chan->next_tx_seq) { 6040 BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq); 6041 l2cap_send_disconn_req(chan, ECONNRESET); 6042 return; 6043 } 6044 6045 skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq); 6046 6047 if (chan->max_tx && skb && 6048 bt_cb(skb)->l2cap.retries >= chan->max_tx) { 6049 BT_DBG("Retry limit exceeded (%d)", chan->max_tx); 6050 l2cap_send_disconn_req(chan, ECONNRESET); 6051 return; 6052 } 6053 6054 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); 6055 6056 l2cap_pass_to_tx(chan, control); 6057 6058 if (control->final) { 6059 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state)) 6060 l2cap_retransmit_all(chan, control); 6061 } else { 6062 l2cap_retransmit_all(chan, control); 6063 l2cap_ertm_send(chan); 6064 if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) 6065 set_bit(CONN_REJ_ACT, &chan->conn_state); 6066 } 6067 } 6068 6069 static u8 l2cap_classify_txseq(struct l2cap_chan *chan, u16 txseq) 6070 { 6071 BT_DBG("chan %p, txseq %d", chan, txseq); 6072 6073 BT_DBG("last_acked_seq %d, expected_tx_seq %d", chan->last_acked_seq, 6074 chan->expected_tx_seq); 6075 6076 if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) { 6077 if (__seq_offset(chan, txseq, chan->last_acked_seq) >= 6078 chan->tx_win) { 6079 /* See notes below regarding "double poll" and 6080 * invalid packets. 6081 */ 6082 if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) { 6083 BT_DBG("Invalid/Ignore - after SREJ"); 6084 return L2CAP_TXSEQ_INVALID_IGNORE; 6085 } else { 6086 BT_DBG("Invalid - in window after SREJ sent"); 6087 return L2CAP_TXSEQ_INVALID; 6088 } 6089 } 6090 6091 if (chan->srej_list.head == txseq) { 6092 BT_DBG("Expected SREJ"); 6093 return L2CAP_TXSEQ_EXPECTED_SREJ; 6094 } 6095 6096 if (l2cap_ertm_seq_in_queue(&chan->srej_q, txseq)) { 6097 BT_DBG("Duplicate SREJ - txseq already stored"); 6098 return L2CAP_TXSEQ_DUPLICATE_SREJ; 6099 } 6100 6101 if (l2cap_seq_list_contains(&chan->srej_list, txseq)) { 6102 BT_DBG("Unexpected SREJ - not requested"); 6103 return L2CAP_TXSEQ_UNEXPECTED_SREJ; 6104 } 6105 } 6106 6107 if (chan->expected_tx_seq == txseq) { 6108 if (__seq_offset(chan, txseq, chan->last_acked_seq) >= 6109 chan->tx_win) { 6110 BT_DBG("Invalid - txseq outside tx window"); 6111 return L2CAP_TXSEQ_INVALID; 6112 } else { 6113 BT_DBG("Expected"); 6114 return L2CAP_TXSEQ_EXPECTED; 6115 } 6116 } 6117 6118 if (__seq_offset(chan, txseq, chan->last_acked_seq) < 6119 __seq_offset(chan, chan->expected_tx_seq, chan->last_acked_seq)) { 6120 BT_DBG("Duplicate - expected_tx_seq later than txseq"); 6121 return L2CAP_TXSEQ_DUPLICATE; 6122 } 6123 6124 if (__seq_offset(chan, txseq, chan->last_acked_seq) >= chan->tx_win) { 6125 /* A source of invalid packets is a "double poll" condition, 6126 * where delays cause us to send multiple poll packets. If 6127 * the remote stack receives and processes both polls, 6128 * sequence numbers can wrap around in such a way that a 6129 * resent frame has a sequence number that looks like new data 6130 * with a sequence gap. This would trigger an erroneous SREJ 6131 * request. 6132 * 6133 * Fortunately, this is impossible with a tx window that's 6134 * less than half of the maximum sequence number, which allows 6135 * invalid frames to be safely ignored. 6136 * 6137 * With tx window sizes greater than half of the tx window 6138 * maximum, the frame is invalid and cannot be ignored. This 6139 * causes a disconnect. 6140 */ 6141 6142 if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) { 6143 BT_DBG("Invalid/Ignore - txseq outside tx window"); 6144 return L2CAP_TXSEQ_INVALID_IGNORE; 6145 } else { 6146 BT_DBG("Invalid - txseq outside tx window"); 6147 return L2CAP_TXSEQ_INVALID; 6148 } 6149 } else { 6150 BT_DBG("Unexpected - txseq indicates missing frames"); 6151 return L2CAP_TXSEQ_UNEXPECTED; 6152 } 6153 } 6154 6155 static int l2cap_rx_state_recv(struct l2cap_chan *chan, 6156 struct l2cap_ctrl *control, 6157 struct sk_buff *skb, u8 event) 6158 { 6159 int err = 0; 6160 bool skb_in_use = false; 6161 6162 BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb, 6163 event); 6164 6165 switch (event) { 6166 case L2CAP_EV_RECV_IFRAME: 6167 switch (l2cap_classify_txseq(chan, control->txseq)) { 6168 case L2CAP_TXSEQ_EXPECTED: 6169 l2cap_pass_to_tx(chan, control); 6170 6171 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { 6172 BT_DBG("Busy, discarding expected seq %d", 6173 control->txseq); 6174 break; 6175 } 6176 6177 chan->expected_tx_seq = __next_seq(chan, 6178 control->txseq); 6179 6180 chan->buffer_seq = chan->expected_tx_seq; 6181 skb_in_use = true; 6182 6183 err = l2cap_reassemble_sdu(chan, skb, control); 6184 if (err) 6185 break; 6186 6187 if (control->final) { 6188 if (!test_and_clear_bit(CONN_REJ_ACT, 6189 &chan->conn_state)) { 6190 control->final = 0; 6191 l2cap_retransmit_all(chan, control); 6192 l2cap_ertm_send(chan); 6193 } 6194 } 6195 6196 if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) 6197 l2cap_send_ack(chan); 6198 break; 6199 case L2CAP_TXSEQ_UNEXPECTED: 6200 l2cap_pass_to_tx(chan, control); 6201 6202 /* Can't issue SREJ frames in the local busy state. 6203 * Drop this frame, it will be seen as missing 6204 * when local busy is exited. 6205 */ 6206 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { 6207 BT_DBG("Busy, discarding unexpected seq %d", 6208 control->txseq); 6209 break; 6210 } 6211 6212 /* There was a gap in the sequence, so an SREJ 6213 * must be sent for each missing frame. The 6214 * current frame is stored for later use. 6215 */ 6216 skb_queue_tail(&chan->srej_q, skb); 6217 skb_in_use = true; 6218 BT_DBG("Queued %p (queue len %d)", skb, 6219 skb_queue_len(&chan->srej_q)); 6220 6221 clear_bit(CONN_SREJ_ACT, &chan->conn_state); 6222 l2cap_seq_list_clear(&chan->srej_list); 6223 l2cap_send_srej(chan, control->txseq); 6224 6225 chan->rx_state = L2CAP_RX_STATE_SREJ_SENT; 6226 break; 6227 case L2CAP_TXSEQ_DUPLICATE: 6228 l2cap_pass_to_tx(chan, control); 6229 break; 6230 case L2CAP_TXSEQ_INVALID_IGNORE: 6231 break; 6232 case L2CAP_TXSEQ_INVALID: 6233 default: 6234 l2cap_send_disconn_req(chan, ECONNRESET); 6235 break; 6236 } 6237 break; 6238 case L2CAP_EV_RECV_RR: 6239 l2cap_pass_to_tx(chan, control); 6240 if (control->final) { 6241 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); 6242 6243 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state) && 6244 !__chan_is_moving(chan)) { 6245 control->final = 0; 6246 l2cap_retransmit_all(chan, control); 6247 } 6248 6249 l2cap_ertm_send(chan); 6250 } else if (control->poll) { 6251 l2cap_send_i_or_rr_or_rnr(chan); 6252 } else { 6253 if (test_and_clear_bit(CONN_REMOTE_BUSY, 6254 &chan->conn_state) && 6255 chan->unacked_frames) 6256 __set_retrans_timer(chan); 6257 6258 l2cap_ertm_send(chan); 6259 } 6260 break; 6261 case L2CAP_EV_RECV_RNR: 6262 set_bit(CONN_REMOTE_BUSY, &chan->conn_state); 6263 l2cap_pass_to_tx(chan, control); 6264 if (control && control->poll) { 6265 set_bit(CONN_SEND_FBIT, &chan->conn_state); 6266 l2cap_send_rr_or_rnr(chan, 0); 6267 } 6268 __clear_retrans_timer(chan); 6269 l2cap_seq_list_clear(&chan->retrans_list); 6270 break; 6271 case L2CAP_EV_RECV_REJ: 6272 l2cap_handle_rej(chan, control); 6273 break; 6274 case L2CAP_EV_RECV_SREJ: 6275 l2cap_handle_srej(chan, control); 6276 break; 6277 default: 6278 break; 6279 } 6280 6281 if (skb && !skb_in_use) { 6282 BT_DBG("Freeing %p", skb); 6283 kfree_skb(skb); 6284 } 6285 6286 return err; 6287 } 6288 6289 static int l2cap_rx_state_srej_sent(struct l2cap_chan *chan, 6290 struct l2cap_ctrl *control, 6291 struct sk_buff *skb, u8 event) 6292 { 6293 int err = 0; 6294 u16 txseq = control->txseq; 6295 bool skb_in_use = false; 6296 6297 BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb, 6298 event); 6299 6300 switch (event) { 6301 case L2CAP_EV_RECV_IFRAME: 6302 switch (l2cap_classify_txseq(chan, txseq)) { 6303 case L2CAP_TXSEQ_EXPECTED: 6304 /* Keep frame for reassembly later */ 6305 l2cap_pass_to_tx(chan, control); 6306 skb_queue_tail(&chan->srej_q, skb); 6307 skb_in_use = true; 6308 BT_DBG("Queued %p (queue len %d)", skb, 6309 skb_queue_len(&chan->srej_q)); 6310 6311 chan->expected_tx_seq = __next_seq(chan, txseq); 6312 break; 6313 case L2CAP_TXSEQ_EXPECTED_SREJ: 6314 l2cap_seq_list_pop(&chan->srej_list); 6315 6316 l2cap_pass_to_tx(chan, control); 6317 skb_queue_tail(&chan->srej_q, skb); 6318 skb_in_use = true; 6319 BT_DBG("Queued %p (queue len %d)", skb, 6320 skb_queue_len(&chan->srej_q)); 6321 6322 err = l2cap_rx_queued_iframes(chan); 6323 if (err) 6324 break; 6325 6326 break; 6327 case L2CAP_TXSEQ_UNEXPECTED: 6328 /* Got a frame that can't be reassembled yet. 6329 * Save it for later, and send SREJs to cover 6330 * the missing frames. 6331 */ 6332 skb_queue_tail(&chan->srej_q, skb); 6333 skb_in_use = true; 6334 BT_DBG("Queued %p (queue len %d)", skb, 6335 skb_queue_len(&chan->srej_q)); 6336 6337 l2cap_pass_to_tx(chan, control); 6338 l2cap_send_srej(chan, control->txseq); 6339 break; 6340 case L2CAP_TXSEQ_UNEXPECTED_SREJ: 6341 /* This frame was requested with an SREJ, but 6342 * some expected retransmitted frames are 6343 * missing. Request retransmission of missing 6344 * SREJ'd frames. 6345 */ 6346 skb_queue_tail(&chan->srej_q, skb); 6347 skb_in_use = true; 6348 BT_DBG("Queued %p (queue len %d)", skb, 6349 skb_queue_len(&chan->srej_q)); 6350 6351 l2cap_pass_to_tx(chan, control); 6352 l2cap_send_srej_list(chan, control->txseq); 6353 break; 6354 case L2CAP_TXSEQ_DUPLICATE_SREJ: 6355 /* We've already queued this frame. Drop this copy. */ 6356 l2cap_pass_to_tx(chan, control); 6357 break; 6358 case L2CAP_TXSEQ_DUPLICATE: 6359 /* Expecting a later sequence number, so this frame 6360 * was already received. Ignore it completely. 6361 */ 6362 break; 6363 case L2CAP_TXSEQ_INVALID_IGNORE: 6364 break; 6365 case L2CAP_TXSEQ_INVALID: 6366 default: 6367 l2cap_send_disconn_req(chan, ECONNRESET); 6368 break; 6369 } 6370 break; 6371 case L2CAP_EV_RECV_RR: 6372 l2cap_pass_to_tx(chan, control); 6373 if (control->final) { 6374 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); 6375 6376 if (!test_and_clear_bit(CONN_REJ_ACT, 6377 &chan->conn_state)) { 6378 control->final = 0; 6379 l2cap_retransmit_all(chan, control); 6380 } 6381 6382 l2cap_ertm_send(chan); 6383 } else if (control->poll) { 6384 if (test_and_clear_bit(CONN_REMOTE_BUSY, 6385 &chan->conn_state) && 6386 chan->unacked_frames) { 6387 __set_retrans_timer(chan); 6388 } 6389 6390 set_bit(CONN_SEND_FBIT, &chan->conn_state); 6391 l2cap_send_srej_tail(chan); 6392 } else { 6393 if (test_and_clear_bit(CONN_REMOTE_BUSY, 6394 &chan->conn_state) && 6395 chan->unacked_frames) 6396 __set_retrans_timer(chan); 6397 6398 l2cap_send_ack(chan); 6399 } 6400 break; 6401 case L2CAP_EV_RECV_RNR: 6402 set_bit(CONN_REMOTE_BUSY, &chan->conn_state); 6403 l2cap_pass_to_tx(chan, control); 6404 if (control->poll) { 6405 l2cap_send_srej_tail(chan); 6406 } else { 6407 struct l2cap_ctrl rr_control; 6408 memset(&rr_control, 0, sizeof(rr_control)); 6409 rr_control.sframe = 1; 6410 rr_control.super = L2CAP_SUPER_RR; 6411 rr_control.reqseq = chan->buffer_seq; 6412 l2cap_send_sframe(chan, &rr_control); 6413 } 6414 6415 break; 6416 case L2CAP_EV_RECV_REJ: 6417 l2cap_handle_rej(chan, control); 6418 break; 6419 case L2CAP_EV_RECV_SREJ: 6420 l2cap_handle_srej(chan, control); 6421 break; 6422 } 6423 6424 if (skb && !skb_in_use) { 6425 BT_DBG("Freeing %p", skb); 6426 kfree_skb(skb); 6427 } 6428 6429 return err; 6430 } 6431 6432 static int l2cap_finish_move(struct l2cap_chan *chan) 6433 { 6434 BT_DBG("chan %p", chan); 6435 6436 chan->rx_state = L2CAP_RX_STATE_RECV; 6437 6438 if (chan->hs_hcon) 6439 chan->conn->mtu = chan->hs_hcon->hdev->block_mtu; 6440 else 6441 chan->conn->mtu = chan->conn->hcon->hdev->acl_mtu; 6442 6443 return l2cap_resegment(chan); 6444 } 6445 6446 static int l2cap_rx_state_wait_p(struct l2cap_chan *chan, 6447 struct l2cap_ctrl *control, 6448 struct sk_buff *skb, u8 event) 6449 { 6450 int err; 6451 6452 BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb, 6453 event); 6454 6455 if (!control->poll) 6456 return -EPROTO; 6457 6458 l2cap_process_reqseq(chan, control->reqseq); 6459 6460 if (!skb_queue_empty(&chan->tx_q)) 6461 chan->tx_send_head = skb_peek(&chan->tx_q); 6462 else 6463 chan->tx_send_head = NULL; 6464 6465 /* Rewind next_tx_seq to the point expected 6466 * by the receiver. 6467 */ 6468 chan->next_tx_seq = control->reqseq; 6469 chan->unacked_frames = 0; 6470 6471 err = l2cap_finish_move(chan); 6472 if (err) 6473 return err; 6474 6475 set_bit(CONN_SEND_FBIT, &chan->conn_state); 6476 l2cap_send_i_or_rr_or_rnr(chan); 6477 6478 if (event == L2CAP_EV_RECV_IFRAME) 6479 return -EPROTO; 6480 6481 return l2cap_rx_state_recv(chan, control, NULL, event); 6482 } 6483 6484 static int l2cap_rx_state_wait_f(struct l2cap_chan *chan, 6485 struct l2cap_ctrl *control, 6486 struct sk_buff *skb, u8 event) 6487 { 6488 int err; 6489 6490 if (!control->final) 6491 return -EPROTO; 6492 6493 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); 6494 6495 chan->rx_state = L2CAP_RX_STATE_RECV; 6496 l2cap_process_reqseq(chan, control->reqseq); 6497 6498 if (!skb_queue_empty(&chan->tx_q)) 6499 chan->tx_send_head = skb_peek(&chan->tx_q); 6500 else 6501 chan->tx_send_head = NULL; 6502 6503 /* Rewind next_tx_seq to the point expected 6504 * by the receiver. 6505 */ 6506 chan->next_tx_seq = control->reqseq; 6507 chan->unacked_frames = 0; 6508 6509 if (chan->hs_hcon) 6510 chan->conn->mtu = chan->hs_hcon->hdev->block_mtu; 6511 else 6512 chan->conn->mtu = chan->conn->hcon->hdev->acl_mtu; 6513 6514 err = l2cap_resegment(chan); 6515 6516 if (!err) 6517 err = l2cap_rx_state_recv(chan, control, skb, event); 6518 6519 return err; 6520 } 6521 6522 static bool __valid_reqseq(struct l2cap_chan *chan, u16 reqseq) 6523 { 6524 /* Make sure reqseq is for a packet that has been sent but not acked */ 6525 u16 unacked; 6526 6527 unacked = __seq_offset(chan, chan->next_tx_seq, chan->expected_ack_seq); 6528 return __seq_offset(chan, chan->next_tx_seq, reqseq) <= unacked; 6529 } 6530 6531 static int l2cap_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control, 6532 struct sk_buff *skb, u8 event) 6533 { 6534 int err = 0; 6535 6536 BT_DBG("chan %p, control %p, skb %p, event %d, state %d", chan, 6537 control, skb, event, chan->rx_state); 6538 6539 if (__valid_reqseq(chan, control->reqseq)) { 6540 switch (chan->rx_state) { 6541 case L2CAP_RX_STATE_RECV: 6542 err = l2cap_rx_state_recv(chan, control, skb, event); 6543 break; 6544 case L2CAP_RX_STATE_SREJ_SENT: 6545 err = l2cap_rx_state_srej_sent(chan, control, skb, 6546 event); 6547 break; 6548 case L2CAP_RX_STATE_WAIT_P: 6549 err = l2cap_rx_state_wait_p(chan, control, skb, event); 6550 break; 6551 case L2CAP_RX_STATE_WAIT_F: 6552 err = l2cap_rx_state_wait_f(chan, control, skb, event); 6553 break; 6554 default: 6555 /* shut it down */ 6556 break; 6557 } 6558 } else { 6559 BT_DBG("Invalid reqseq %d (next_tx_seq %d, expected_ack_seq %d", 6560 control->reqseq, chan->next_tx_seq, 6561 chan->expected_ack_seq); 6562 l2cap_send_disconn_req(chan, ECONNRESET); 6563 } 6564 6565 return err; 6566 } 6567 6568 static int l2cap_stream_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control, 6569 struct sk_buff *skb) 6570 { 6571 BT_DBG("chan %p, control %p, skb %p, state %d", chan, control, skb, 6572 chan->rx_state); 6573 6574 if (l2cap_classify_txseq(chan, control->txseq) == 6575 L2CAP_TXSEQ_EXPECTED) { 6576 l2cap_pass_to_tx(chan, control); 6577 6578 BT_DBG("buffer_seq %d->%d", chan->buffer_seq, 6579 __next_seq(chan, chan->buffer_seq)); 6580 6581 chan->buffer_seq = __next_seq(chan, chan->buffer_seq); 6582 6583 l2cap_reassemble_sdu(chan, skb, control); 6584 } else { 6585 if (chan->sdu) { 6586 kfree_skb(chan->sdu); 6587 chan->sdu = NULL; 6588 } 6589 chan->sdu_last_frag = NULL; 6590 chan->sdu_len = 0; 6591 6592 if (skb) { 6593 BT_DBG("Freeing %p", skb); 6594 kfree_skb(skb); 6595 } 6596 } 6597 6598 chan->last_acked_seq = control->txseq; 6599 chan->expected_tx_seq = __next_seq(chan, control->txseq); 6600 6601 return 0; 6602 } 6603 6604 static int l2cap_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb) 6605 { 6606 struct l2cap_ctrl *control = &bt_cb(skb)->l2cap; 6607 u16 len; 6608 u8 event; 6609 6610 __unpack_control(chan, skb); 6611 6612 len = skb->len; 6613 6614 /* 6615 * We can just drop the corrupted I-frame here. 6616 * Receiver will miss it and start proper recovery 6617 * procedures and ask for retransmission. 6618 */ 6619 if (l2cap_check_fcs(chan, skb)) 6620 goto drop; 6621 6622 if (!control->sframe && control->sar == L2CAP_SAR_START) 6623 len -= L2CAP_SDULEN_SIZE; 6624 6625 if (chan->fcs == L2CAP_FCS_CRC16) 6626 len -= L2CAP_FCS_SIZE; 6627 6628 if (len > chan->mps) { 6629 l2cap_send_disconn_req(chan, ECONNRESET); 6630 goto drop; 6631 } 6632 6633 if ((chan->mode == L2CAP_MODE_ERTM || 6634 chan->mode == L2CAP_MODE_STREAMING) && sk_filter(chan->data, skb)) 6635 goto drop; 6636 6637 if (!control->sframe) { 6638 int err; 6639 6640 BT_DBG("iframe sar %d, reqseq %d, final %d, txseq %d", 6641 control->sar, control->reqseq, control->final, 6642 control->txseq); 6643 6644 /* Validate F-bit - F=0 always valid, F=1 only 6645 * valid in TX WAIT_F 6646 */ 6647 if (control->final && chan->tx_state != L2CAP_TX_STATE_WAIT_F) 6648 goto drop; 6649 6650 if (chan->mode != L2CAP_MODE_STREAMING) { 6651 event = L2CAP_EV_RECV_IFRAME; 6652 err = l2cap_rx(chan, control, skb, event); 6653 } else { 6654 err = l2cap_stream_rx(chan, control, skb); 6655 } 6656 6657 if (err) 6658 l2cap_send_disconn_req(chan, ECONNRESET); 6659 } else { 6660 const u8 rx_func_to_event[4] = { 6661 L2CAP_EV_RECV_RR, L2CAP_EV_RECV_REJ, 6662 L2CAP_EV_RECV_RNR, L2CAP_EV_RECV_SREJ 6663 }; 6664 6665 /* Only I-frames are expected in streaming mode */ 6666 if (chan->mode == L2CAP_MODE_STREAMING) 6667 goto drop; 6668 6669 BT_DBG("sframe reqseq %d, final %d, poll %d, super %d", 6670 control->reqseq, control->final, control->poll, 6671 control->super); 6672 6673 if (len != 0) { 6674 BT_ERR("Trailing bytes: %d in sframe", len); 6675 l2cap_send_disconn_req(chan, ECONNRESET); 6676 goto drop; 6677 } 6678 6679 /* Validate F and P bits */ 6680 if (control->final && (control->poll || 6681 chan->tx_state != L2CAP_TX_STATE_WAIT_F)) 6682 goto drop; 6683 6684 event = rx_func_to_event[control->super]; 6685 if (l2cap_rx(chan, control, skb, event)) 6686 l2cap_send_disconn_req(chan, ECONNRESET); 6687 } 6688 6689 return 0; 6690 6691 drop: 6692 kfree_skb(skb); 6693 return 0; 6694 } 6695 6696 static void l2cap_chan_le_send_credits(struct l2cap_chan *chan) 6697 { 6698 struct l2cap_conn *conn = chan->conn; 6699 struct l2cap_le_credits pkt; 6700 u16 return_credits; 6701 6702 /* We return more credits to the sender only after the amount of 6703 * credits falls below half of the initial amount. 6704 */ 6705 if (chan->rx_credits >= (le_max_credits + 1) / 2) 6706 return; 6707 6708 return_credits = le_max_credits - chan->rx_credits; 6709 6710 BT_DBG("chan %p returning %u credits to sender", chan, return_credits); 6711 6712 chan->rx_credits += return_credits; 6713 6714 pkt.cid = cpu_to_le16(chan->scid); 6715 pkt.credits = cpu_to_le16(return_credits); 6716 6717 chan->ident = l2cap_get_ident(conn); 6718 6719 l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CREDITS, sizeof(pkt), &pkt); 6720 } 6721 6722 static int l2cap_le_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb) 6723 { 6724 int err; 6725 6726 if (!chan->rx_credits) { 6727 BT_ERR("No credits to receive LE L2CAP data"); 6728 l2cap_send_disconn_req(chan, ECONNRESET); 6729 return -ENOBUFS; 6730 } 6731 6732 if (chan->imtu < skb->len) { 6733 BT_ERR("Too big LE L2CAP PDU"); 6734 return -ENOBUFS; 6735 } 6736 6737 chan->rx_credits--; 6738 BT_DBG("rx_credits %u -> %u", chan->rx_credits + 1, chan->rx_credits); 6739 6740 l2cap_chan_le_send_credits(chan); 6741 6742 err = 0; 6743 6744 if (!chan->sdu) { 6745 u16 sdu_len; 6746 6747 sdu_len = get_unaligned_le16(skb->data); 6748 skb_pull(skb, L2CAP_SDULEN_SIZE); 6749 6750 BT_DBG("Start of new SDU. sdu_len %u skb->len %u imtu %u", 6751 sdu_len, skb->len, chan->imtu); 6752 6753 if (sdu_len > chan->imtu) { 6754 BT_ERR("Too big LE L2CAP SDU length received"); 6755 err = -EMSGSIZE; 6756 goto failed; 6757 } 6758 6759 if (skb->len > sdu_len) { 6760 BT_ERR("Too much LE L2CAP data received"); 6761 err = -EINVAL; 6762 goto failed; 6763 } 6764 6765 if (skb->len == sdu_len) 6766 return chan->ops->recv(chan, skb); 6767 6768 chan->sdu = skb; 6769 chan->sdu_len = sdu_len; 6770 chan->sdu_last_frag = skb; 6771 6772 return 0; 6773 } 6774 6775 BT_DBG("SDU fragment. chan->sdu->len %u skb->len %u chan->sdu_len %u", 6776 chan->sdu->len, skb->len, chan->sdu_len); 6777 6778 if (chan->sdu->len + skb->len > chan->sdu_len) { 6779 BT_ERR("Too much LE L2CAP data received"); 6780 err = -EINVAL; 6781 goto failed; 6782 } 6783 6784 append_skb_frag(chan->sdu, skb, &chan->sdu_last_frag); 6785 skb = NULL; 6786 6787 if (chan->sdu->len == chan->sdu_len) { 6788 err = chan->ops->recv(chan, chan->sdu); 6789 if (!err) { 6790 chan->sdu = NULL; 6791 chan->sdu_last_frag = NULL; 6792 chan->sdu_len = 0; 6793 } 6794 } 6795 6796 failed: 6797 if (err) { 6798 kfree_skb(skb); 6799 kfree_skb(chan->sdu); 6800 chan->sdu = NULL; 6801 chan->sdu_last_frag = NULL; 6802 chan->sdu_len = 0; 6803 } 6804 6805 /* We can't return an error here since we took care of the skb 6806 * freeing internally. An error return would cause the caller to 6807 * do a double-free of the skb. 6808 */ 6809 return 0; 6810 } 6811 6812 static void l2cap_data_channel(struct l2cap_conn *conn, u16 cid, 6813 struct sk_buff *skb) 6814 { 6815 struct l2cap_chan *chan; 6816 6817 chan = l2cap_get_chan_by_scid(conn, cid); 6818 if (!chan) { 6819 if (cid == L2CAP_CID_A2MP) { 6820 chan = a2mp_channel_create(conn, skb); 6821 if (!chan) { 6822 kfree_skb(skb); 6823 return; 6824 } 6825 6826 l2cap_chan_lock(chan); 6827 } else { 6828 BT_DBG("unknown cid 0x%4.4x", cid); 6829 /* Drop packet and return */ 6830 kfree_skb(skb); 6831 return; 6832 } 6833 } 6834 6835 BT_DBG("chan %p, len %d", chan, skb->len); 6836 6837 /* If we receive data on a fixed channel before the info req/rsp 6838 * procdure is done simply assume that the channel is supported 6839 * and mark it as ready. 6840 */ 6841 if (chan->chan_type == L2CAP_CHAN_FIXED) 6842 l2cap_chan_ready(chan); 6843 6844 if (chan->state != BT_CONNECTED) 6845 goto drop; 6846 6847 switch (chan->mode) { 6848 case L2CAP_MODE_LE_FLOWCTL: 6849 if (l2cap_le_data_rcv(chan, skb) < 0) 6850 goto drop; 6851 6852 goto done; 6853 6854 case L2CAP_MODE_BASIC: 6855 /* If socket recv buffers overflows we drop data here 6856 * which is *bad* because L2CAP has to be reliable. 6857 * But we don't have any other choice. L2CAP doesn't 6858 * provide flow control mechanism. */ 6859 6860 if (chan->imtu < skb->len) { 6861 BT_ERR("Dropping L2CAP data: receive buffer overflow"); 6862 goto drop; 6863 } 6864 6865 if (!chan->ops->recv(chan, skb)) 6866 goto done; 6867 break; 6868 6869 case L2CAP_MODE_ERTM: 6870 case L2CAP_MODE_STREAMING: 6871 l2cap_data_rcv(chan, skb); 6872 goto done; 6873 6874 default: 6875 BT_DBG("chan %p: bad mode 0x%2.2x", chan, chan->mode); 6876 break; 6877 } 6878 6879 drop: 6880 kfree_skb(skb); 6881 6882 done: 6883 l2cap_chan_unlock(chan); 6884 } 6885 6886 static void l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, 6887 struct sk_buff *skb) 6888 { 6889 struct hci_conn *hcon = conn->hcon; 6890 struct l2cap_chan *chan; 6891 6892 if (hcon->type != ACL_LINK) 6893 goto free_skb; 6894 6895 chan = l2cap_global_chan_by_psm(0, psm, &hcon->src, &hcon->dst, 6896 ACL_LINK); 6897 if (!chan) 6898 goto free_skb; 6899 6900 BT_DBG("chan %p, len %d", chan, skb->len); 6901 6902 if (chan->state != BT_BOUND && chan->state != BT_CONNECTED) 6903 goto drop; 6904 6905 if (chan->imtu < skb->len) 6906 goto drop; 6907 6908 /* Store remote BD_ADDR and PSM for msg_name */ 6909 bacpy(&bt_cb(skb)->l2cap.bdaddr, &hcon->dst); 6910 bt_cb(skb)->l2cap.psm = psm; 6911 6912 if (!chan->ops->recv(chan, skb)) { 6913 l2cap_chan_put(chan); 6914 return; 6915 } 6916 6917 drop: 6918 l2cap_chan_put(chan); 6919 free_skb: 6920 kfree_skb(skb); 6921 } 6922 6923 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb) 6924 { 6925 struct l2cap_hdr *lh = (void *) skb->data; 6926 struct hci_conn *hcon = conn->hcon; 6927 u16 cid, len; 6928 __le16 psm; 6929 6930 if (hcon->state != BT_CONNECTED) { 6931 BT_DBG("queueing pending rx skb"); 6932 skb_queue_tail(&conn->pending_rx, skb); 6933 return; 6934 } 6935 6936 skb_pull(skb, L2CAP_HDR_SIZE); 6937 cid = __le16_to_cpu(lh->cid); 6938 len = __le16_to_cpu(lh->len); 6939 6940 if (len != skb->len) { 6941 kfree_skb(skb); 6942 return; 6943 } 6944 6945 /* Since we can't actively block incoming LE connections we must 6946 * at least ensure that we ignore incoming data from them. 6947 */ 6948 if (hcon->type == LE_LINK && 6949 hci_bdaddr_list_lookup(&hcon->hdev->blacklist, &hcon->dst, 6950 bdaddr_dst_type(hcon))) { 6951 kfree_skb(skb); 6952 return; 6953 } 6954 6955 BT_DBG("len %d, cid 0x%4.4x", len, cid); 6956 6957 switch (cid) { 6958 case L2CAP_CID_SIGNALING: 6959 l2cap_sig_channel(conn, skb); 6960 break; 6961 6962 case L2CAP_CID_CONN_LESS: 6963 psm = get_unaligned((__le16 *) skb->data); 6964 skb_pull(skb, L2CAP_PSMLEN_SIZE); 6965 l2cap_conless_channel(conn, psm, skb); 6966 break; 6967 6968 case L2CAP_CID_LE_SIGNALING: 6969 l2cap_le_sig_channel(conn, skb); 6970 break; 6971 6972 default: 6973 l2cap_data_channel(conn, cid, skb); 6974 break; 6975 } 6976 } 6977 6978 static void process_pending_rx(struct work_struct *work) 6979 { 6980 struct l2cap_conn *conn = container_of(work, struct l2cap_conn, 6981 pending_rx_work); 6982 struct sk_buff *skb; 6983 6984 BT_DBG(""); 6985 6986 while ((skb = skb_dequeue(&conn->pending_rx))) 6987 l2cap_recv_frame(conn, skb); 6988 } 6989 6990 static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon) 6991 { 6992 struct l2cap_conn *conn = hcon->l2cap_data; 6993 struct hci_chan *hchan; 6994 6995 if (conn) 6996 return conn; 6997 6998 hchan = hci_chan_create(hcon); 6999 if (!hchan) 7000 return NULL; 7001 7002 conn = kzalloc(sizeof(*conn), GFP_KERNEL); 7003 if (!conn) { 7004 hci_chan_del(hchan); 7005 return NULL; 7006 } 7007 7008 kref_init(&conn->ref); 7009 hcon->l2cap_data = conn; 7010 conn->hcon = hci_conn_get(hcon); 7011 conn->hchan = hchan; 7012 7013 BT_DBG("hcon %p conn %p hchan %p", hcon, conn, hchan); 7014 7015 switch (hcon->type) { 7016 case LE_LINK: 7017 if (hcon->hdev->le_mtu) { 7018 conn->mtu = hcon->hdev->le_mtu; 7019 break; 7020 } 7021 /* fall through */ 7022 default: 7023 conn->mtu = hcon->hdev->acl_mtu; 7024 break; 7025 } 7026 7027 conn->feat_mask = 0; 7028 7029 conn->local_fixed_chan = L2CAP_FC_SIG_BREDR | L2CAP_FC_CONNLESS; 7030 7031 if (hcon->type == ACL_LINK && 7032 hci_dev_test_flag(hcon->hdev, HCI_HS_ENABLED)) 7033 conn->local_fixed_chan |= L2CAP_FC_A2MP; 7034 7035 if (hci_dev_test_flag(hcon->hdev, HCI_LE_ENABLED) && 7036 (bredr_sc_enabled(hcon->hdev) || 7037 hci_dev_test_flag(hcon->hdev, HCI_FORCE_BREDR_SMP))) 7038 conn->local_fixed_chan |= L2CAP_FC_SMP_BREDR; 7039 7040 mutex_init(&conn->ident_lock); 7041 mutex_init(&conn->chan_lock); 7042 7043 INIT_LIST_HEAD(&conn->chan_l); 7044 INIT_LIST_HEAD(&conn->users); 7045 7046 INIT_DELAYED_WORK(&conn->info_timer, l2cap_info_timeout); 7047 7048 skb_queue_head_init(&conn->pending_rx); 7049 INIT_WORK(&conn->pending_rx_work, process_pending_rx); 7050 INIT_WORK(&conn->id_addr_update_work, l2cap_conn_update_id_addr); 7051 7052 conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM; 7053 7054 return conn; 7055 } 7056 7057 static bool is_valid_psm(u16 psm, u8 dst_type) { 7058 if (!psm) 7059 return false; 7060 7061 if (bdaddr_type_is_le(dst_type)) 7062 return (psm <= 0x00ff); 7063 7064 /* PSM must be odd and lsb of upper byte must be 0 */ 7065 return ((psm & 0x0101) == 0x0001); 7066 } 7067 7068 int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid, 7069 bdaddr_t *dst, u8 dst_type) 7070 { 7071 struct l2cap_conn *conn; 7072 struct hci_conn *hcon; 7073 struct hci_dev *hdev; 7074 int err; 7075 7076 BT_DBG("%pMR -> %pMR (type %u) psm 0x%2.2x", &chan->src, dst, 7077 dst_type, __le16_to_cpu(psm)); 7078 7079 hdev = hci_get_route(dst, &chan->src, chan->src_type); 7080 if (!hdev) 7081 return -EHOSTUNREACH; 7082 7083 hci_dev_lock(hdev); 7084 7085 if (!is_valid_psm(__le16_to_cpu(psm), dst_type) && !cid && 7086 chan->chan_type != L2CAP_CHAN_RAW) { 7087 err = -EINVAL; 7088 goto done; 7089 } 7090 7091 if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED && !psm) { 7092 err = -EINVAL; 7093 goto done; 7094 } 7095 7096 if (chan->chan_type == L2CAP_CHAN_FIXED && !cid) { 7097 err = -EINVAL; 7098 goto done; 7099 } 7100 7101 switch (chan->mode) { 7102 case L2CAP_MODE_BASIC: 7103 break; 7104 case L2CAP_MODE_LE_FLOWCTL: 7105 l2cap_le_flowctl_init(chan); 7106 break; 7107 case L2CAP_MODE_ERTM: 7108 case L2CAP_MODE_STREAMING: 7109 if (!disable_ertm) 7110 break; 7111 /* fall through */ 7112 default: 7113 err = -EOPNOTSUPP; 7114 goto done; 7115 } 7116 7117 switch (chan->state) { 7118 case BT_CONNECT: 7119 case BT_CONNECT2: 7120 case BT_CONFIG: 7121 /* Already connecting */ 7122 err = 0; 7123 goto done; 7124 7125 case BT_CONNECTED: 7126 /* Already connected */ 7127 err = -EISCONN; 7128 goto done; 7129 7130 case BT_OPEN: 7131 case BT_BOUND: 7132 /* Can connect */ 7133 break; 7134 7135 default: 7136 err = -EBADFD; 7137 goto done; 7138 } 7139 7140 /* Set destination address and psm */ 7141 bacpy(&chan->dst, dst); 7142 chan->dst_type = dst_type; 7143 7144 chan->psm = psm; 7145 chan->dcid = cid; 7146 7147 if (bdaddr_type_is_le(dst_type)) { 7148 /* Convert from L2CAP channel address type to HCI address type 7149 */ 7150 if (dst_type == BDADDR_LE_PUBLIC) 7151 dst_type = ADDR_LE_DEV_PUBLIC; 7152 else 7153 dst_type = ADDR_LE_DEV_RANDOM; 7154 7155 if (hci_dev_test_flag(hdev, HCI_ADVERTISING)) 7156 hcon = hci_connect_le(hdev, dst, dst_type, 7157 chan->sec_level, 7158 HCI_LE_CONN_TIMEOUT, 7159 HCI_ROLE_SLAVE); 7160 else 7161 hcon = hci_connect_le_scan(hdev, dst, dst_type, 7162 chan->sec_level, 7163 HCI_LE_CONN_TIMEOUT); 7164 7165 } else { 7166 u8 auth_type = l2cap_get_auth_type(chan); 7167 hcon = hci_connect_acl(hdev, dst, chan->sec_level, auth_type); 7168 } 7169 7170 if (IS_ERR(hcon)) { 7171 err = PTR_ERR(hcon); 7172 goto done; 7173 } 7174 7175 conn = l2cap_conn_add(hcon); 7176 if (!conn) { 7177 hci_conn_drop(hcon); 7178 err = -ENOMEM; 7179 goto done; 7180 } 7181 7182 mutex_lock(&conn->chan_lock); 7183 l2cap_chan_lock(chan); 7184 7185 if (cid && __l2cap_get_chan_by_dcid(conn, cid)) { 7186 hci_conn_drop(hcon); 7187 err = -EBUSY; 7188 goto chan_unlock; 7189 } 7190 7191 /* Update source addr of the socket */ 7192 bacpy(&chan->src, &hcon->src); 7193 chan->src_type = bdaddr_src_type(hcon); 7194 7195 __l2cap_chan_add(conn, chan); 7196 7197 /* l2cap_chan_add takes its own ref so we can drop this one */ 7198 hci_conn_drop(hcon); 7199 7200 l2cap_state_change(chan, BT_CONNECT); 7201 __set_chan_timer(chan, chan->ops->get_sndtimeo(chan)); 7202 7203 /* Release chan->sport so that it can be reused by other 7204 * sockets (as it's only used for listening sockets). 7205 */ 7206 write_lock(&chan_list_lock); 7207 chan->sport = 0; 7208 write_unlock(&chan_list_lock); 7209 7210 if (hcon->state == BT_CONNECTED) { 7211 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) { 7212 __clear_chan_timer(chan); 7213 if (l2cap_chan_check_security(chan, true)) 7214 l2cap_state_change(chan, BT_CONNECTED); 7215 } else 7216 l2cap_do_start(chan); 7217 } 7218 7219 err = 0; 7220 7221 chan_unlock: 7222 l2cap_chan_unlock(chan); 7223 mutex_unlock(&conn->chan_lock); 7224 done: 7225 hci_dev_unlock(hdev); 7226 hci_dev_put(hdev); 7227 return err; 7228 } 7229 EXPORT_SYMBOL_GPL(l2cap_chan_connect); 7230 7231 /* ---- L2CAP interface with lower layer (HCI) ---- */ 7232 7233 int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr) 7234 { 7235 int exact = 0, lm1 = 0, lm2 = 0; 7236 struct l2cap_chan *c; 7237 7238 BT_DBG("hdev %s, bdaddr %pMR", hdev->name, bdaddr); 7239 7240 /* Find listening sockets and check their link_mode */ 7241 read_lock(&chan_list_lock); 7242 list_for_each_entry(c, &chan_list, global_l) { 7243 if (c->state != BT_LISTEN) 7244 continue; 7245 7246 if (!bacmp(&c->src, &hdev->bdaddr)) { 7247 lm1 |= HCI_LM_ACCEPT; 7248 if (test_bit(FLAG_ROLE_SWITCH, &c->flags)) 7249 lm1 |= HCI_LM_MASTER; 7250 exact++; 7251 } else if (!bacmp(&c->src, BDADDR_ANY)) { 7252 lm2 |= HCI_LM_ACCEPT; 7253 if (test_bit(FLAG_ROLE_SWITCH, &c->flags)) 7254 lm2 |= HCI_LM_MASTER; 7255 } 7256 } 7257 read_unlock(&chan_list_lock); 7258 7259 return exact ? lm1 : lm2; 7260 } 7261 7262 /* Find the next fixed channel in BT_LISTEN state, continue iteration 7263 * from an existing channel in the list or from the beginning of the 7264 * global list (by passing NULL as first parameter). 7265 */ 7266 static struct l2cap_chan *l2cap_global_fixed_chan(struct l2cap_chan *c, 7267 struct hci_conn *hcon) 7268 { 7269 u8 src_type = bdaddr_src_type(hcon); 7270 7271 read_lock(&chan_list_lock); 7272 7273 if (c) 7274 c = list_next_entry(c, global_l); 7275 else 7276 c = list_entry(chan_list.next, typeof(*c), global_l); 7277 7278 list_for_each_entry_from(c, &chan_list, global_l) { 7279 if (c->chan_type != L2CAP_CHAN_FIXED) 7280 continue; 7281 if (c->state != BT_LISTEN) 7282 continue; 7283 if (bacmp(&c->src, &hcon->src) && bacmp(&c->src, BDADDR_ANY)) 7284 continue; 7285 if (src_type != c->src_type) 7286 continue; 7287 7288 l2cap_chan_hold(c); 7289 read_unlock(&chan_list_lock); 7290 return c; 7291 } 7292 7293 read_unlock(&chan_list_lock); 7294 7295 return NULL; 7296 } 7297 7298 static void l2cap_connect_cfm(struct hci_conn *hcon, u8 status) 7299 { 7300 struct hci_dev *hdev = hcon->hdev; 7301 struct l2cap_conn *conn; 7302 struct l2cap_chan *pchan; 7303 u8 dst_type; 7304 7305 if (hcon->type != ACL_LINK && hcon->type != LE_LINK) 7306 return; 7307 7308 BT_DBG("hcon %p bdaddr %pMR status %d", hcon, &hcon->dst, status); 7309 7310 if (status) { 7311 l2cap_conn_del(hcon, bt_to_errno(status)); 7312 return; 7313 } 7314 7315 conn = l2cap_conn_add(hcon); 7316 if (!conn) 7317 return; 7318 7319 dst_type = bdaddr_dst_type(hcon); 7320 7321 /* If device is blocked, do not create channels for it */ 7322 if (hci_bdaddr_list_lookup(&hdev->blacklist, &hcon->dst, dst_type)) 7323 return; 7324 7325 /* Find fixed channels and notify them of the new connection. We 7326 * use multiple individual lookups, continuing each time where 7327 * we left off, because the list lock would prevent calling the 7328 * potentially sleeping l2cap_chan_lock() function. 7329 */ 7330 pchan = l2cap_global_fixed_chan(NULL, hcon); 7331 while (pchan) { 7332 struct l2cap_chan *chan, *next; 7333 7334 /* Client fixed channels should override server ones */ 7335 if (__l2cap_get_chan_by_dcid(conn, pchan->scid)) 7336 goto next; 7337 7338 l2cap_chan_lock(pchan); 7339 chan = pchan->ops->new_connection(pchan); 7340 if (chan) { 7341 bacpy(&chan->src, &hcon->src); 7342 bacpy(&chan->dst, &hcon->dst); 7343 chan->src_type = bdaddr_src_type(hcon); 7344 chan->dst_type = dst_type; 7345 7346 __l2cap_chan_add(conn, chan); 7347 } 7348 7349 l2cap_chan_unlock(pchan); 7350 next: 7351 next = l2cap_global_fixed_chan(pchan, hcon); 7352 l2cap_chan_put(pchan); 7353 pchan = next; 7354 } 7355 7356 l2cap_conn_ready(conn); 7357 } 7358 7359 int l2cap_disconn_ind(struct hci_conn *hcon) 7360 { 7361 struct l2cap_conn *conn = hcon->l2cap_data; 7362 7363 BT_DBG("hcon %p", hcon); 7364 7365 if (!conn) 7366 return HCI_ERROR_REMOTE_USER_TERM; 7367 return conn->disc_reason; 7368 } 7369 7370 static void l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason) 7371 { 7372 if (hcon->type != ACL_LINK && hcon->type != LE_LINK) 7373 return; 7374 7375 BT_DBG("hcon %p reason %d", hcon, reason); 7376 7377 l2cap_conn_del(hcon, bt_to_errno(reason)); 7378 } 7379 7380 static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt) 7381 { 7382 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) 7383 return; 7384 7385 if (encrypt == 0x00) { 7386 if (chan->sec_level == BT_SECURITY_MEDIUM) { 7387 __set_chan_timer(chan, L2CAP_ENC_TIMEOUT); 7388 } else if (chan->sec_level == BT_SECURITY_HIGH || 7389 chan->sec_level == BT_SECURITY_FIPS) 7390 l2cap_chan_close(chan, ECONNREFUSED); 7391 } else { 7392 if (chan->sec_level == BT_SECURITY_MEDIUM) 7393 __clear_chan_timer(chan); 7394 } 7395 } 7396 7397 static void l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt) 7398 { 7399 struct l2cap_conn *conn = hcon->l2cap_data; 7400 struct l2cap_chan *chan; 7401 7402 if (!conn) 7403 return; 7404 7405 BT_DBG("conn %p status 0x%2.2x encrypt %u", conn, status, encrypt); 7406 7407 mutex_lock(&conn->chan_lock); 7408 7409 list_for_each_entry(chan, &conn->chan_l, list) { 7410 l2cap_chan_lock(chan); 7411 7412 BT_DBG("chan %p scid 0x%4.4x state %s", chan, chan->scid, 7413 state_to_string(chan->state)); 7414 7415 if (chan->scid == L2CAP_CID_A2MP) { 7416 l2cap_chan_unlock(chan); 7417 continue; 7418 } 7419 7420 if (!status && encrypt) 7421 chan->sec_level = hcon->sec_level; 7422 7423 if (!__l2cap_no_conn_pending(chan)) { 7424 l2cap_chan_unlock(chan); 7425 continue; 7426 } 7427 7428 if (!status && (chan->state == BT_CONNECTED || 7429 chan->state == BT_CONFIG)) { 7430 chan->ops->resume(chan); 7431 l2cap_check_encryption(chan, encrypt); 7432 l2cap_chan_unlock(chan); 7433 continue; 7434 } 7435 7436 if (chan->state == BT_CONNECT) { 7437 if (!status) 7438 l2cap_start_connection(chan); 7439 else 7440 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT); 7441 } else if (chan->state == BT_CONNECT2 && 7442 chan->mode != L2CAP_MODE_LE_FLOWCTL) { 7443 struct l2cap_conn_rsp rsp; 7444 __u16 res, stat; 7445 7446 if (!status) { 7447 if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) { 7448 res = L2CAP_CR_PEND; 7449 stat = L2CAP_CS_AUTHOR_PEND; 7450 chan->ops->defer(chan); 7451 } else { 7452 l2cap_state_change(chan, BT_CONFIG); 7453 res = L2CAP_CR_SUCCESS; 7454 stat = L2CAP_CS_NO_INFO; 7455 } 7456 } else { 7457 l2cap_state_change(chan, BT_DISCONN); 7458 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT); 7459 res = L2CAP_CR_SEC_BLOCK; 7460 stat = L2CAP_CS_NO_INFO; 7461 } 7462 7463 rsp.scid = cpu_to_le16(chan->dcid); 7464 rsp.dcid = cpu_to_le16(chan->scid); 7465 rsp.result = cpu_to_le16(res); 7466 rsp.status = cpu_to_le16(stat); 7467 l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP, 7468 sizeof(rsp), &rsp); 7469 7470 if (!test_bit(CONF_REQ_SENT, &chan->conf_state) && 7471 res == L2CAP_CR_SUCCESS) { 7472 char buf[128]; 7473 set_bit(CONF_REQ_SENT, &chan->conf_state); 7474 l2cap_send_cmd(conn, l2cap_get_ident(conn), 7475 L2CAP_CONF_REQ, 7476 l2cap_build_conf_req(chan, buf, sizeof(buf)), 7477 buf); 7478 chan->num_conf_req++; 7479 } 7480 } 7481 7482 l2cap_chan_unlock(chan); 7483 } 7484 7485 mutex_unlock(&conn->chan_lock); 7486 } 7487 7488 void l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags) 7489 { 7490 struct l2cap_conn *conn = hcon->l2cap_data; 7491 struct l2cap_hdr *hdr; 7492 int len; 7493 7494 /* For AMP controller do not create l2cap conn */ 7495 if (!conn && hcon->hdev->dev_type != HCI_PRIMARY) 7496 goto drop; 7497 7498 if (!conn) 7499 conn = l2cap_conn_add(hcon); 7500 7501 if (!conn) 7502 goto drop; 7503 7504 BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags); 7505 7506 switch (flags) { 7507 case ACL_START: 7508 case ACL_START_NO_FLUSH: 7509 case ACL_COMPLETE: 7510 if (conn->rx_len) { 7511 BT_ERR("Unexpected start frame (len %d)", skb->len); 7512 kfree_skb(conn->rx_skb); 7513 conn->rx_skb = NULL; 7514 conn->rx_len = 0; 7515 l2cap_conn_unreliable(conn, ECOMM); 7516 } 7517 7518 /* Start fragment always begin with Basic L2CAP header */ 7519 if (skb->len < L2CAP_HDR_SIZE) { 7520 BT_ERR("Frame is too short (len %d)", skb->len); 7521 l2cap_conn_unreliable(conn, ECOMM); 7522 goto drop; 7523 } 7524 7525 hdr = (struct l2cap_hdr *) skb->data; 7526 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE; 7527 7528 if (len == skb->len) { 7529 /* Complete frame received */ 7530 l2cap_recv_frame(conn, skb); 7531 return; 7532 } 7533 7534 BT_DBG("Start: total len %d, frag len %d", len, skb->len); 7535 7536 if (skb->len > len) { 7537 BT_ERR("Frame is too long (len %d, expected len %d)", 7538 skb->len, len); 7539 l2cap_conn_unreliable(conn, ECOMM); 7540 goto drop; 7541 } 7542 7543 /* Allocate skb for the complete frame (with header) */ 7544 conn->rx_skb = bt_skb_alloc(len, GFP_KERNEL); 7545 if (!conn->rx_skb) 7546 goto drop; 7547 7548 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len), 7549 skb->len); 7550 conn->rx_len = len - skb->len; 7551 break; 7552 7553 case ACL_CONT: 7554 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len); 7555 7556 if (!conn->rx_len) { 7557 BT_ERR("Unexpected continuation frame (len %d)", skb->len); 7558 l2cap_conn_unreliable(conn, ECOMM); 7559 goto drop; 7560 } 7561 7562 if (skb->len > conn->rx_len) { 7563 BT_ERR("Fragment is too long (len %d, expected %d)", 7564 skb->len, conn->rx_len); 7565 kfree_skb(conn->rx_skb); 7566 conn->rx_skb = NULL; 7567 conn->rx_len = 0; 7568 l2cap_conn_unreliable(conn, ECOMM); 7569 goto drop; 7570 } 7571 7572 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len), 7573 skb->len); 7574 conn->rx_len -= skb->len; 7575 7576 if (!conn->rx_len) { 7577 /* Complete frame received. l2cap_recv_frame 7578 * takes ownership of the skb so set the global 7579 * rx_skb pointer to NULL first. 7580 */ 7581 struct sk_buff *rx_skb = conn->rx_skb; 7582 conn->rx_skb = NULL; 7583 l2cap_recv_frame(conn, rx_skb); 7584 } 7585 break; 7586 } 7587 7588 drop: 7589 kfree_skb(skb); 7590 } 7591 7592 static struct hci_cb l2cap_cb = { 7593 .name = "L2CAP", 7594 .connect_cfm = l2cap_connect_cfm, 7595 .disconn_cfm = l2cap_disconn_cfm, 7596 .security_cfm = l2cap_security_cfm, 7597 }; 7598 7599 static int l2cap_debugfs_show(struct seq_file *f, void *p) 7600 { 7601 struct l2cap_chan *c; 7602 7603 read_lock(&chan_list_lock); 7604 7605 list_for_each_entry(c, &chan_list, global_l) { 7606 seq_printf(f, "%pMR (%u) %pMR (%u) %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n", 7607 &c->src, c->src_type, &c->dst, c->dst_type, 7608 c->state, __le16_to_cpu(c->psm), 7609 c->scid, c->dcid, c->imtu, c->omtu, 7610 c->sec_level, c->mode); 7611 } 7612 7613 read_unlock(&chan_list_lock); 7614 7615 return 0; 7616 } 7617 7618 static int l2cap_debugfs_open(struct inode *inode, struct file *file) 7619 { 7620 return single_open(file, l2cap_debugfs_show, inode->i_private); 7621 } 7622 7623 static const struct file_operations l2cap_debugfs_fops = { 7624 .open = l2cap_debugfs_open, 7625 .read = seq_read, 7626 .llseek = seq_lseek, 7627 .release = single_release, 7628 }; 7629 7630 static struct dentry *l2cap_debugfs; 7631 7632 int __init l2cap_init(void) 7633 { 7634 int err; 7635 7636 err = l2cap_init_sockets(); 7637 if (err < 0) 7638 return err; 7639 7640 hci_register_cb(&l2cap_cb); 7641 7642 if (IS_ERR_OR_NULL(bt_debugfs)) 7643 return 0; 7644 7645 l2cap_debugfs = debugfs_create_file("l2cap", 0444, bt_debugfs, 7646 NULL, &l2cap_debugfs_fops); 7647 7648 debugfs_create_u16("l2cap_le_max_credits", 0644, bt_debugfs, 7649 &le_max_credits); 7650 debugfs_create_u16("l2cap_le_default_mps", 0644, bt_debugfs, 7651 &le_default_mps); 7652 7653 return 0; 7654 } 7655 7656 void l2cap_exit(void) 7657 { 7658 debugfs_remove(l2cap_debugfs); 7659 hci_unregister_cb(&l2cap_cb); 7660 l2cap_cleanup_sockets(); 7661 } 7662 7663 module_param(disable_ertm, bool, 0644); 7664 MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode"); 7665