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