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