1 /* $NetBSD: l2cap_signal.c,v 1.20 2020/02/29 11:40:06 maxv Exp $ */ 2 3 /*- 4 * Copyright (c) 2005 Iain Hibbert. 5 * Copyright (c) 2006 Itronix Inc. 6 * All rights reserved. 7 * 8 * Redistribution and use in source and binary forms, with or without 9 * modification, are permitted provided that the following conditions 10 * are met: 11 * 1. Redistributions of source code must retain the above copyright 12 * notice, this list of conditions and the following disclaimer. 13 * 2. Redistributions in binary form must reproduce the above copyright 14 * notice, this list of conditions and the following disclaimer in the 15 * documentation and/or other materials provided with the distribution. 16 * 3. The name of Itronix Inc. may not be used to endorse 17 * or promote products derived from this software without specific 18 * prior written permission. 19 * 20 * THIS SOFTWARE IS PROVIDED BY ITRONIX INC. ``AS IS'' AND 21 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 22 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 23 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL ITRONIX INC. BE LIABLE FOR ANY 24 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 25 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 27 * ON ANY THEORY OF LIABILITY, WHETHER IN 28 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 * POSSIBILITY OF SUCH DAMAGE. 31 */ 32 33 #include <sys/cdefs.h> 34 __KERNEL_RCSID(0, "$NetBSD: l2cap_signal.c,v 1.20 2020/02/29 11:40:06 maxv Exp $"); 35 36 #include <sys/param.h> 37 #include <sys/kernel.h> 38 #include <sys/mbuf.h> 39 #include <sys/proc.h> 40 #include <sys/queue.h> 41 #include <sys/systm.h> 42 43 #include <netbt/bluetooth.h> 44 #include <netbt/hci.h> 45 #include <netbt/l2cap.h> 46 47 /******************************************************************************* 48 * 49 * L2CAP Signal processing 50 */ 51 52 static void l2cap_recv_command_rej(struct mbuf *, struct hci_link *); 53 static void l2cap_recv_connect_req(struct mbuf *, struct hci_link *); 54 static void l2cap_recv_connect_rsp(struct mbuf *, struct hci_link *); 55 static void l2cap_recv_config_req(struct mbuf *, struct hci_link *); 56 static void l2cap_recv_config_rsp(struct mbuf *, struct hci_link *); 57 static void l2cap_recv_disconnect_req(struct mbuf *, struct hci_link *); 58 static void l2cap_recv_disconnect_rsp(struct mbuf *, struct hci_link *); 59 static void l2cap_recv_info_req(struct mbuf *, struct hci_link *); 60 static int l2cap_send_signal(struct hci_link *, uint8_t, uint8_t, uint16_t, void *); 61 static int l2cap_send_command_rej(struct hci_link *, uint8_t, int, ...); 62 static void l2cap_qos_btoh(l2cap_qos_t *, void *); 63 static void l2cap_qos_htob(void *, l2cap_qos_t *); 64 65 /* 66 * process incoming signal packets (CID 0x0001). Can contain multiple 67 * requests/responses. The signal hander should clear the command from 68 * the mbuf before returning. 69 */ 70 void 71 l2cap_recv_signal(struct mbuf *m, struct hci_link *link) 72 { 73 l2cap_cmd_hdr_t cmd; 74 75 for(;;) { 76 if (m->m_pkthdr.len < sizeof(cmd)) 77 goto finish; 78 79 m_copydata(m, 0, sizeof(cmd), &cmd); 80 cmd.length = le16toh(cmd.length); 81 82 if (m->m_pkthdr.len < sizeof(cmd) + cmd.length) 83 goto reject; 84 85 DPRINTFN(2, "(%s) code %d, ident %d, len %d\n", 86 device_xname(link->hl_unit->hci_dev), 87 cmd.code, cmd.ident, cmd.length); 88 89 switch (cmd.code) { 90 case L2CAP_COMMAND_REJ: 91 if (cmd.length > sizeof(l2cap_cmd_rej_cp)) 92 goto finish; 93 94 l2cap_recv_command_rej(m, link); 95 break; 96 97 case L2CAP_CONNECT_REQ: 98 if (cmd.length != sizeof(l2cap_con_req_cp)) 99 goto reject; 100 101 l2cap_recv_connect_req(m, link); 102 break; 103 104 case L2CAP_CONNECT_RSP: 105 if (cmd.length != sizeof(l2cap_con_rsp_cp)) 106 goto finish; 107 108 l2cap_recv_connect_rsp(m, link); 109 break; 110 111 case L2CAP_CONFIG_REQ: 112 l2cap_recv_config_req(m, link); 113 break; 114 115 case L2CAP_CONFIG_RSP: 116 l2cap_recv_config_rsp(m, link); 117 break; 118 119 case L2CAP_DISCONNECT_REQ: 120 if (cmd.length != sizeof(l2cap_discon_req_cp)) 121 goto reject; 122 123 l2cap_recv_disconnect_req(m, link); 124 break; 125 126 case L2CAP_DISCONNECT_RSP: 127 if (cmd.length != sizeof(l2cap_discon_rsp_cp)) 128 goto finish; 129 130 l2cap_recv_disconnect_rsp(m, link); 131 break; 132 133 case L2CAP_ECHO_REQ: 134 m_adj(m, sizeof(cmd) + cmd.length); 135 l2cap_send_signal(link, L2CAP_ECHO_RSP, cmd.ident, 136 0, NULL); 137 break; 138 139 case L2CAP_ECHO_RSP: 140 m_adj(m, sizeof(cmd) + cmd.length); 141 break; 142 143 case L2CAP_INFO_REQ: 144 if (cmd.length != sizeof(l2cap_info_req_cp)) 145 goto reject; 146 147 l2cap_recv_info_req(m, link); 148 break; 149 150 case L2CAP_INFO_RSP: 151 m_adj(m, sizeof(cmd) + cmd.length); 152 break; 153 154 default: 155 goto reject; 156 } 157 } 158 panic("impossible!"); 159 160 reject: 161 l2cap_send_command_rej(link, cmd.ident, L2CAP_REJ_NOT_UNDERSTOOD); 162 finish: 163 m_freem(m); 164 } 165 166 /* 167 * Process Received Command Reject. For now we dont try to recover gracefully 168 * from this, it probably means that the link is garbled or the other end is 169 * insufficiently capable of handling normal traffic. (not *my* fault, no way!) 170 */ 171 static void 172 l2cap_recv_command_rej(struct mbuf *m, struct hci_link *link) 173 { 174 struct l2cap_req *req; 175 struct l2cap_channel *chan; 176 l2cap_cmd_hdr_t cmd; 177 l2cap_cmd_rej_cp cp; 178 179 m_copydata(m, 0, sizeof(cmd), &cmd); 180 m_adj(m, sizeof(cmd)); 181 182 cmd.length = le16toh(cmd.length); 183 184 /* The length here must contain the reason (2 octets) plus 185 * any data (0 or more octets) but we already know it is not 186 * bigger than l2cap_cmd_rej_cp 187 */ 188 m_copydata(m, 0, cmd.length, &cp); 189 m_adj(m, cmd.length); 190 191 if (cmd.length < 2) 192 return; 193 194 req = l2cap_request_lookup(link, cmd.ident); 195 if (req == NULL) 196 return; 197 198 switch (le16toh(cp.reason)) { 199 case L2CAP_REJ_NOT_UNDERSTOOD: /* data length = 0 octets */ 200 /* 201 * I dont know what to do, just move up the timeout 202 */ 203 callout_schedule(&req->lr_rtx, 0); 204 break; 205 206 case L2CAP_REJ_MTU_EXCEEDED: /* data length = 2 octets */ 207 /* 208 * I didnt send any commands over L2CAP_MTU_MINIMUM size, but.. 209 * 210 * XXX maybe we should resend this, instead? 211 */ 212 if (cmd.length != 4) 213 return; 214 215 link->hl_mtu = le16toh(cp.data[0]); 216 callout_schedule(&req->lr_rtx, 0); 217 break; 218 219 case L2CAP_REJ_INVALID_CID: /* data length = 4 octets */ 220 /* 221 * Well, if they dont have such a channel then our channel is 222 * most likely closed. Make it so. 223 */ 224 chan = req->lr_chan; 225 l2cap_request_free(req); 226 if (chan != NULL && chan->lc_state != L2CAP_CLOSED) 227 l2cap_close(chan, ECONNABORTED); 228 229 break; 230 231 default: 232 UNKNOWN(le16toh(cp.reason)); 233 break; 234 } 235 } 236 237 /* 238 * Process Received Connect Request. Find listening channel matching 239 * psm & addr and ask upper layer for a new channel. 240 */ 241 static void 242 l2cap_recv_connect_req(struct mbuf *m, struct hci_link *link) 243 { 244 struct sockaddr_bt laddr, raddr; 245 struct l2cap_channel *chan, *new; 246 l2cap_cmd_hdr_t cmd; 247 l2cap_con_req_cp cp; 248 int err; 249 250 /* extract cmd */ 251 m_copydata(m, 0, sizeof(cmd), &cmd); 252 m_adj(m, sizeof(cmd)); 253 254 /* extract request */ 255 m_copydata(m, 0, sizeof(cp), &cp); 256 m_adj(m, sizeof(cp)); 257 258 cp.scid = le16toh(cp.scid); 259 cp.psm = le16toh(cp.psm); 260 261 memset(&laddr, 0, sizeof(struct sockaddr_bt)); 262 laddr.bt_len = sizeof(struct sockaddr_bt); 263 laddr.bt_family = AF_BLUETOOTH; 264 laddr.bt_psm = cp.psm; 265 bdaddr_copy(&laddr.bt_bdaddr, &link->hl_unit->hci_bdaddr); 266 267 memset(&raddr, 0, sizeof(struct sockaddr_bt)); 268 raddr.bt_len = sizeof(struct sockaddr_bt); 269 raddr.bt_family = AF_BLUETOOTH; 270 raddr.bt_psm = cp.psm; 271 bdaddr_copy(&raddr.bt_bdaddr, &link->hl_bdaddr); 272 273 LIST_FOREACH(chan, &l2cap_listen_list, lc_ncid) { 274 if (chan->lc_laddr.bt_psm != laddr.bt_psm) 275 continue; 276 277 if (!bdaddr_same(&laddr.bt_bdaddr, &chan->lc_laddr.bt_bdaddr) 278 && bdaddr_any(&chan->lc_laddr.bt_bdaddr) == 0) 279 continue; 280 281 new= (*chan->lc_proto->newconn)(chan->lc_upper, &laddr, &raddr); 282 if (new == NULL) 283 continue; 284 285 err = l2cap_cid_alloc(new); 286 if (err) { 287 l2cap_send_connect_rsp(link, cmd.ident, 288 0, cp.scid, 289 L2CAP_NO_RESOURCES); 290 291 (*new->lc_proto->disconnected)(new->lc_upper, err); 292 return; 293 } 294 295 new->lc_link = hci_acl_open(link->hl_unit, &link->hl_bdaddr); 296 KASSERT(new->lc_link == link); 297 298 new->lc_rcid = cp.scid; 299 new->lc_ident = cmd.ident; 300 301 memcpy(&new->lc_laddr, &laddr, sizeof(struct sockaddr_bt)); 302 memcpy(&new->lc_raddr, &raddr, sizeof(struct sockaddr_bt)); 303 304 new->lc_mode = chan->lc_mode; 305 306 err = l2cap_setmode(new); 307 if (err == EINPROGRESS) { 308 new->lc_state = L2CAP_WAIT_SEND_CONNECT_RSP; 309 (*new->lc_proto->connecting)(new->lc_upper); 310 return; 311 } 312 if (err) { 313 new->lc_state = L2CAP_CLOSED; 314 hci_acl_close(link, err); 315 new->lc_link = NULL; 316 317 l2cap_send_connect_rsp(link, cmd.ident, 318 0, cp.scid, 319 L2CAP_NO_RESOURCES); 320 321 (*new->lc_proto->disconnected)(new->lc_upper, err); 322 return; 323 } 324 325 err = l2cap_send_connect_rsp(link, cmd.ident, 326 new->lc_lcid, new->lc_rcid, 327 L2CAP_SUCCESS); 328 if (err) { 329 l2cap_close(new, err); 330 return; 331 } 332 333 new->lc_state = L2CAP_WAIT_CONFIG; 334 new->lc_flags |= (L2CAP_WAIT_CONFIG_REQ | L2CAP_WAIT_CONFIG_RSP); 335 err = l2cap_send_config_req(new); 336 if (err) 337 l2cap_close(new, err); 338 339 return; 340 } 341 342 l2cap_send_connect_rsp(link, cmd.ident, 343 0, cp.scid, 344 L2CAP_PSM_NOT_SUPPORTED); 345 } 346 347 /* 348 * Process Received Connect Response. 349 */ 350 static void 351 l2cap_recv_connect_rsp(struct mbuf *m, struct hci_link *link) 352 { 353 l2cap_cmd_hdr_t cmd; 354 l2cap_con_rsp_cp cp; 355 struct l2cap_req *req; 356 struct l2cap_channel *chan; 357 358 m_copydata(m, 0, sizeof(cmd), &cmd); 359 m_adj(m, sizeof(cmd)); 360 361 m_copydata(m, 0, sizeof(cp), &cp); 362 m_adj(m, sizeof(cp)); 363 364 cp.scid = le16toh(cp.scid); 365 cp.dcid = le16toh(cp.dcid); 366 cp.result = le16toh(cp.result); 367 368 req = l2cap_request_lookup(link, cmd.ident); 369 if (req == NULL || req->lr_code != L2CAP_CONNECT_REQ) 370 return; 371 372 chan = req->lr_chan; 373 if (chan != NULL && chan->lc_lcid != cp.scid) 374 return; 375 376 if (chan == NULL || chan->lc_state != L2CAP_WAIT_RECV_CONNECT_RSP) { 377 l2cap_request_free(req); 378 return; 379 } 380 381 switch (cp.result) { 382 case L2CAP_SUCCESS: 383 /* 384 * Ok, at this point we have a connection to the other party. We 385 * could indicate upstream that we are ready for business and 386 * wait for a "Configure Channel Request" but I'm not so sure 387 * that is required in our case - we will proceed directly to 388 * sending our config request. We set two state bits because in 389 * the config state we are waiting for requests and responses. 390 */ 391 l2cap_request_free(req); 392 chan->lc_rcid = cp.dcid; 393 chan->lc_state = L2CAP_WAIT_CONFIG; 394 chan->lc_flags |= (L2CAP_WAIT_CONFIG_REQ | L2CAP_WAIT_CONFIG_RSP); 395 l2cap_send_config_req(chan); 396 break; 397 398 case L2CAP_PENDING: 399 /* XXX dont release request, should start eRTX timeout? */ 400 (*chan->lc_proto->connecting)(chan->lc_upper); 401 break; 402 403 case L2CAP_PSM_NOT_SUPPORTED: 404 case L2CAP_SECURITY_BLOCK: 405 case L2CAP_NO_RESOURCES: 406 default: 407 l2cap_request_free(req); 408 l2cap_close(chan, ECONNREFUSED); 409 break; 410 } 411 } 412 413 /* 414 * Process Received Config Reqest. 415 */ 416 static void 417 l2cap_recv_config_req(struct mbuf *m, struct hci_link *link) 418 { 419 uint8_t buf[L2CAP_MTU_MINIMUM]; 420 l2cap_cmd_hdr_t cmd; 421 l2cap_cfg_req_cp cp; 422 l2cap_cfg_opt_t opt; 423 l2cap_cfg_opt_val_t val; 424 l2cap_cfg_rsp_cp rp; 425 struct l2cap_channel *chan; 426 int left, len; 427 428 m_copydata(m, 0, sizeof(cmd), &cmd); 429 m_adj(m, sizeof(cmd)); 430 left = le16toh(cmd.length); 431 432 if (left < sizeof(cp)) 433 goto reject; 434 435 m_copydata(m, 0, sizeof(cp), &cp); 436 m_adj(m, sizeof(cp)); 437 left -= sizeof(cp); 438 439 cp.dcid = le16toh(cp.dcid); 440 cp.flags = le16toh(cp.flags); 441 442 chan = l2cap_cid_lookup(cp.dcid); 443 if (chan == NULL || chan->lc_link != link 444 || chan->lc_state != L2CAP_WAIT_CONFIG 445 || (chan->lc_flags & L2CAP_WAIT_CONFIG_REQ) == 0) { 446 /* XXX we should really accept reconfiguration requests */ 447 l2cap_send_command_rej(link, cmd.ident, L2CAP_REJ_INVALID_CID, 448 L2CAP_NULL_CID, cp.dcid); 449 goto out; 450 } 451 452 /* ready our response packet */ 453 rp.scid = htole16(chan->lc_rcid); 454 rp.flags = 0; /* "No Continuation" */ 455 rp.result = L2CAP_SUCCESS; 456 len = sizeof(rp); 457 458 /* 459 * Process the packet. We build the return packet on the fly adding any 460 * unacceptable parameters as we go. As we can only return one result, 461 * unknown option takes precedence so we start our return packet anew 462 * and ignore option values thereafter as they will be re-sent. 463 * 464 * Since we do not support enough options to make overflowing the min 465 * MTU size an issue in normal use, we just reject config requests that 466 * make that happen. This could be because options are repeated or the 467 * packet is corrupted in some way. 468 * 469 * If unknown option types threaten to overflow the packet, we just 470 * ignore them. We can deny them next time. 471 */ 472 while (left > 0) { 473 if (left < sizeof(opt)) 474 goto reject; 475 476 m_copydata(m, 0, sizeof(opt), &opt); 477 m_adj(m, sizeof(opt)); 478 left -= sizeof(opt); 479 480 if (left < opt.length) 481 goto reject; 482 483 switch(opt.type & L2CAP_OPT_HINT_MASK) { 484 case L2CAP_OPT_MTU: 485 if (rp.result == L2CAP_UNKNOWN_OPTION) 486 break; 487 488 if (opt.length != L2CAP_OPT_MTU_SIZE) 489 goto reject; 490 491 m_copydata(m, 0, L2CAP_OPT_MTU_SIZE, &val.mtu); 492 val.mtu = le16toh(val.mtu); 493 494 /* 495 * XXX how do we know what the minimum acceptable MTU is 496 * for a channel? Spec says some profiles have a higher 497 * minimum but I have no way to find that out at this 498 * juncture.. 499 */ 500 if (val.mtu < L2CAP_MTU_MINIMUM) { 501 if (len + sizeof(opt) + L2CAP_OPT_MTU_SIZE > sizeof(buf)) 502 goto reject; 503 504 rp.result = L2CAP_UNACCEPTABLE_PARAMS; 505 memcpy(buf + len, &opt, sizeof(opt)); 506 len += sizeof(opt); 507 val.mtu = htole16(L2CAP_MTU_MINIMUM); 508 memcpy(buf + len, &val, L2CAP_OPT_MTU_SIZE); 509 len += L2CAP_OPT_MTU_SIZE; 510 } else 511 chan->lc_omtu = val.mtu; 512 513 break; 514 515 case L2CAP_OPT_FLUSH_TIMO: 516 if (rp.result == L2CAP_UNKNOWN_OPTION) 517 break; 518 519 if (opt.length != L2CAP_OPT_FLUSH_TIMO_SIZE) 520 goto reject; 521 522 /* 523 * I think that this is informational only - he is 524 * informing us of the flush timeout he will be using. 525 * I dont think this affects us in any significant way, 526 * so just ignore this value for now. 527 */ 528 break; 529 530 case L2CAP_OPT_QOS: 531 if (rp.result == L2CAP_UNKNOWN_OPTION) 532 break; 533 534 if (opt.length != L2CAP_OPT_QOS_SIZE) 535 goto reject; 536 537 /* 538 * We don't actually support QoS, but an incoming 539 * config request is merely advising us of their 540 * outgoing traffic flow, so be nice. 541 */ 542 m_copydata(m, 0, L2CAP_OPT_QOS_SIZE, &val.qos); 543 switch (val.qos.service_type) { 544 case L2CAP_QOS_NO_TRAFFIC: 545 /* 546 * "No traffic" means they don't plan to send 547 * any data and the fields should be ignored. 548 */ 549 chan->lc_iqos = l2cap_default_qos; 550 chan->lc_iqos.service_type = L2CAP_QOS_NO_TRAFFIC; 551 break; 552 553 case L2CAP_QOS_BEST_EFFORT: 554 /* 555 * "Best effort" is the default, and we may 556 * choose to ignore the fields, try to satisfy 557 * the parameters while giving no response, or 558 * respond with the settings we will try to 559 * meet. 560 */ 561 l2cap_qos_btoh(&chan->lc_iqos, &val.qos); 562 break; 563 564 case L2CAP_QOS_GUARANTEED: 565 default: 566 /* 567 * Anything else we don't support, so make a 568 * counter-offer with the current settings. 569 */ 570 if (len + sizeof(opt) + L2CAP_OPT_QOS_SIZE > sizeof(buf)) 571 goto reject; 572 573 rp.result = L2CAP_UNACCEPTABLE_PARAMS; 574 memcpy(buf + len, &opt, sizeof(opt)); 575 len += sizeof(opt); 576 l2cap_qos_htob(buf + len, &chan->lc_iqos); 577 len += L2CAP_OPT_QOS_SIZE; 578 break; 579 } 580 break; 581 582 default: 583 /* ignore hints */ 584 if (opt.type & L2CAP_OPT_HINT_BIT) 585 break; 586 587 /* unknown options supercede all else */ 588 if (rp.result != L2CAP_UNKNOWN_OPTION) { 589 rp.result = L2CAP_UNKNOWN_OPTION; 590 len = sizeof(rp); 591 } 592 593 /* ignore if it don't fit */ 594 if (len + sizeof(opt) > sizeof(buf)) 595 break; 596 597 /* return unknown option type, but no data */ 598 buf[len++] = opt.type; 599 buf[len++] = 0; 600 break; 601 } 602 603 m_adj(m, opt.length); 604 left -= opt.length; 605 } 606 607 rp.result = htole16(rp.result); 608 memcpy(buf, &rp, sizeof(rp)); 609 l2cap_send_signal(link, L2CAP_CONFIG_RSP, cmd.ident, len, buf); 610 611 if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0 612 && rp.result == le16toh(L2CAP_SUCCESS)) { 613 614 chan->lc_flags &= ~L2CAP_WAIT_CONFIG_REQ; 615 616 if ((chan->lc_flags & L2CAP_WAIT_CONFIG_RSP) == 0) { 617 chan->lc_state = L2CAP_OPEN; 618 /* XXX how to distinguish REconfiguration? */ 619 (*chan->lc_proto->connected)(chan->lc_upper); 620 } 621 } 622 return; 623 624 reject: 625 l2cap_send_command_rej(link, cmd.ident, L2CAP_REJ_NOT_UNDERSTOOD); 626 out: 627 m_adj(m, left); 628 } 629 630 /* 631 * Process Received Config Response. 632 */ 633 static void 634 l2cap_recv_config_rsp(struct mbuf *m, struct hci_link *link) 635 { 636 l2cap_cmd_hdr_t cmd; 637 l2cap_cfg_rsp_cp cp; 638 l2cap_cfg_opt_t opt; 639 l2cap_cfg_opt_val_t val; 640 struct l2cap_req *req; 641 struct l2cap_channel *chan; 642 int left; 643 644 m_copydata(m, 0, sizeof(cmd), &cmd); 645 m_adj(m, sizeof(cmd)); 646 left = le16toh(cmd.length); 647 648 if (left < sizeof(cp)) 649 goto out; 650 651 m_copydata(m, 0, sizeof(cp), &cp); 652 m_adj(m, sizeof(cp)); 653 left -= sizeof(cp); 654 655 cp.scid = le16toh(cp.scid); 656 cp.flags = le16toh(cp.flags); 657 cp.result = le16toh(cp.result); 658 659 req = l2cap_request_lookup(link, cmd.ident); 660 if (req == NULL || req->lr_code != L2CAP_CONFIG_REQ) 661 goto out; 662 663 chan = req->lr_chan; 664 if (chan != NULL && chan->lc_lcid != cp.scid) 665 goto out; 666 667 l2cap_request_free(req); 668 669 if (chan == NULL || chan->lc_state != L2CAP_WAIT_CONFIG 670 || (chan->lc_flags & L2CAP_WAIT_CONFIG_RSP) == 0) 671 goto out; 672 673 if ((cp.flags & L2CAP_OPT_CFLAG_BIT)) { 674 l2cap_cfg_req_cp rp; 675 676 /* 677 * They have more to tell us and want another ID to 678 * use, so send an empty config request 679 */ 680 if (l2cap_request_alloc(chan, L2CAP_CONFIG_REQ)) 681 goto discon; 682 683 rp.dcid = htole16(cp.scid); 684 rp.flags = 0; 685 686 if (l2cap_send_signal(link, L2CAP_CONFIG_REQ, link->hl_lastid, 687 sizeof(rp), &rp)) 688 goto discon; 689 } 690 691 switch(cp.result) { 692 case L2CAP_SUCCESS: 693 /* 694 * If continuation flag was not set, our config request was 695 * accepted. We may have to wait for their config request to 696 * complete, so check that but otherwise we are open 697 * 698 * There may be 'advisory' values in the packet but we just 699 * ignore those.. 700 */ 701 if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0) { 702 chan->lc_flags &= ~L2CAP_WAIT_CONFIG_RSP; 703 704 if ((chan->lc_flags & L2CAP_WAIT_CONFIG_REQ) == 0) { 705 chan->lc_state = L2CAP_OPEN; 706 /* XXX how to distinguish REconfiguration? */ 707 (*chan->lc_proto->connected)(chan->lc_upper); 708 } 709 } 710 goto out; 711 712 case L2CAP_UNACCEPTABLE_PARAMS: 713 /* 714 * Packet contains unacceptable parameters with preferred values 715 */ 716 while (left > 0) { 717 if (left < sizeof(opt)) 718 goto discon; 719 720 m_copydata(m, 0, sizeof(opt), &opt); 721 m_adj(m, sizeof(opt)); 722 left -= sizeof(opt); 723 724 if (left < opt.length) 725 goto discon; 726 727 switch (opt.type) { 728 case L2CAP_OPT_MTU: 729 if (opt.length != L2CAP_OPT_MTU_SIZE) 730 goto discon; 731 732 m_copydata(m, 0, L2CAP_OPT_MTU_SIZE, &val.mtu); 733 chan->lc_imtu = le16toh(val.mtu); 734 if (chan->lc_imtu < L2CAP_MTU_MINIMUM) 735 chan->lc_imtu = L2CAP_MTU_DEFAULT; 736 break; 737 738 case L2CAP_OPT_FLUSH_TIMO: 739 if (opt.length != L2CAP_OPT_FLUSH_TIMO_SIZE) 740 goto discon; 741 742 /* 743 * Spec says: If we cannot honor proposed value, 744 * either disconnect or try again with original 745 * value. I can't really see why they want to 746 * interfere with OUR flush timeout in any case 747 * so we just punt for now. 748 */ 749 goto discon; 750 751 case L2CAP_OPT_QOS: 752 if (opt.length != L2CAP_OPT_QOS_SIZE) 753 goto discon; 754 755 /* 756 * This may happen even if we haven't sent a 757 * QoS request, where they need to state their 758 * preferred incoming traffic flow. 759 * We don't support anything, but copy in the 760 * parameters if no action is good enough. 761 */ 762 m_copydata(m, 0, L2CAP_OPT_QOS_SIZE, &val.qos); 763 switch (val.qos.service_type) { 764 case L2CAP_QOS_NO_TRAFFIC: 765 case L2CAP_QOS_BEST_EFFORT: 766 l2cap_qos_btoh(&chan->lc_oqos, &val.qos); 767 break; 768 769 case L2CAP_QOS_GUARANTEED: 770 default: 771 goto discon; 772 } 773 break; 774 775 default: 776 UNKNOWN(opt.type); 777 goto discon; 778 } 779 780 m_adj(m, opt.length); 781 left -= opt.length; 782 } 783 784 if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0) 785 l2cap_send_config_req(chan); /* no state change */ 786 787 goto out; 788 789 case L2CAP_REJECT: 790 goto discon; 791 792 case L2CAP_UNKNOWN_OPTION: 793 /* 794 * Packet contains options not understood. Turn off unknown 795 * options by setting them to default values (means they will 796 * not be requested again). 797 * 798 * If our option was already off then fail (paranoia?) 799 * 800 * XXX Should we consider that options were set for a reason? 801 */ 802 while (left > 0) { 803 if (left < sizeof(opt)) 804 goto discon; 805 806 m_copydata(m, 0, sizeof(opt), &opt); 807 m_adj(m, sizeof(opt)); 808 left -= sizeof(opt); 809 810 if (left < opt.length) 811 goto discon; 812 813 m_adj(m, opt.length); 814 left -= opt.length; 815 816 switch(opt.type) { 817 case L2CAP_OPT_MTU: 818 if (chan->lc_imtu == L2CAP_MTU_DEFAULT) 819 goto discon; 820 821 chan->lc_imtu = L2CAP_MTU_DEFAULT; 822 break; 823 824 case L2CAP_OPT_FLUSH_TIMO: 825 if (chan->lc_flush == L2CAP_FLUSH_TIMO_DEFAULT) 826 goto discon; 827 828 chan->lc_flush = L2CAP_FLUSH_TIMO_DEFAULT; 829 break; 830 831 case L2CAP_OPT_QOS: 832 break; 833 834 default: 835 UNKNOWN(opt.type); 836 goto discon; 837 } 838 } 839 840 if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0) 841 l2cap_send_config_req(chan); /* no state change */ 842 843 goto out; 844 845 default: 846 UNKNOWN(cp.result); 847 goto discon; 848 } 849 850 DPRINTF("how did I get here!?\n"); 851 852 discon: 853 l2cap_send_disconnect_req(chan); 854 l2cap_close(chan, ECONNABORTED); 855 856 out: 857 m_adj(m, left); 858 } 859 860 /* 861 * Process Received Disconnect Request. We must validate scid and dcid 862 * just in case but otherwise this connection is finished. 863 */ 864 static void 865 l2cap_recv_disconnect_req(struct mbuf *m, struct hci_link *link) 866 { 867 l2cap_cmd_hdr_t cmd; 868 l2cap_discon_req_cp cp; 869 l2cap_discon_rsp_cp rp; 870 struct l2cap_channel *chan; 871 872 m_copydata(m, 0, sizeof(cmd), &cmd); 873 m_adj(m, sizeof(cmd)); 874 875 m_copydata(m, 0, sizeof(cp), &cp); 876 m_adj(m, sizeof(cp)); 877 878 cp.scid = le16toh(cp.scid); 879 cp.dcid = le16toh(cp.dcid); 880 881 chan = l2cap_cid_lookup(cp.dcid); 882 if (chan == NULL || chan->lc_link != link || chan->lc_rcid != cp.scid) { 883 l2cap_send_command_rej(link, cmd.ident, L2CAP_REJ_INVALID_CID, 884 cp.dcid, cp.scid); 885 return; 886 } 887 888 rp.dcid = htole16(chan->lc_lcid); 889 rp.scid = htole16(chan->lc_rcid); 890 l2cap_send_signal(link, L2CAP_DISCONNECT_RSP, cmd.ident, 891 sizeof(rp), &rp); 892 893 if (chan->lc_state != L2CAP_CLOSED) 894 l2cap_close(chan, 0); 895 } 896 897 /* 898 * Process Received Disconnect Response. We must validate scid and dcid but 899 * unless we were waiting for this signal, ignore it. 900 */ 901 static void 902 l2cap_recv_disconnect_rsp(struct mbuf *m, struct hci_link *link) 903 { 904 l2cap_cmd_hdr_t cmd; 905 l2cap_discon_rsp_cp cp; 906 struct l2cap_req *req; 907 struct l2cap_channel *chan; 908 909 m_copydata(m, 0, sizeof(cmd), &cmd); 910 m_adj(m, sizeof(cmd)); 911 912 m_copydata(m, 0, sizeof(cp), &cp); 913 m_adj(m, sizeof(cp)); 914 915 cp.scid = le16toh(cp.scid); 916 cp.dcid = le16toh(cp.dcid); 917 918 req = l2cap_request_lookup(link, cmd.ident); 919 if (req == NULL || req->lr_code != L2CAP_DISCONNECT_REQ) 920 return; 921 922 chan = req->lr_chan; 923 if (chan == NULL 924 || chan->lc_lcid != cp.scid 925 || chan->lc_rcid != cp.dcid) 926 return; 927 928 l2cap_request_free(req); 929 930 if (chan->lc_state != L2CAP_WAIT_DISCONNECT) 931 return; 932 933 l2cap_close(chan, 0); 934 } 935 936 /* 937 * Process Received Info Request. 938 */ 939 static void 940 l2cap_recv_info_req(struct mbuf *m, struct hci_link *link) 941 { 942 l2cap_cmd_hdr_t cmd; 943 l2cap_info_req_cp cp; 944 uint8_t rsp[12]; 945 946 m_copydata(m, 0, sizeof(cmd), &cmd); 947 m_adj(m, sizeof(cmd)); 948 949 m_copydata(m, 0, sizeof(cp), &cp); 950 m_adj(m, sizeof(cp)); 951 952 cp.type = le16toh(cp.type); 953 switch(cp.type) { 954 case L2CAP_EXTENDED_FEATURES: 955 /* 956 * 32-bit data field, unused bits set to zero 957 * 958 * octet bit feature 959 * 0 0 Flow control mode 960 * 0 1 Retransmission mode 961 * 0 2 Bi-directional QoS 962 * 0 3 Enhanced retransmission mode 963 * 0 4 Streaming mode 964 * 0 5 FCS option 965 * 0 6 Extended flow specification for BR/EDR 966 * 0 7 Fixed channels (SET) 967 * 1 0 Extended window size 968 * 1 1 Unicast connectionless data reception 969 */ 970 le16enc(rsp + 0, cp.type); 971 le16enc(rsp + 2, L2CAP_SUCCESS); 972 le32enc(rsp + 4, 0x00000080); 973 l2cap_send_signal(link, L2CAP_INFO_RSP, cmd.ident, 8, rsp); 974 break; 975 976 case L2CAP_FIXED_CHANNELS: 977 /* 978 * 64-bit data field, unused bits set to zero 979 * 980 * octet bit channel 981 * 0 0 0x0000 Null 982 * 0 1 0x0001 L2CAP Signalling Channel (SET) 983 * 0 2 0x0002 Connectionless Reception 984 * 0 3 0x0003 AMP Manager Protocol Channel 985 * 0 7 0x0007 BR/EDR Security Manager 986 * 7 7 0x003f AMP Test Manager 987 */ 988 le16enc(rsp + 0, cp.type); 989 le16enc(rsp + 2, L2CAP_SUCCESS); 990 le64enc(rsp + 4, 0x0000000000000002); 991 l2cap_send_signal(link, L2CAP_INFO_RSP, cmd.ident, 12, rsp); 992 break; 993 994 case L2CAP_CONNLESS_MTU: 995 default: 996 le16enc(rsp + 0, cp.type); 997 le16enc(rsp + 2, L2CAP_NOT_SUPPORTED); 998 l2cap_send_signal(link, L2CAP_INFO_RSP, cmd.ident, 4, rsp); 999 break; 1000 } 1001 } 1002 1003 /* 1004 * Construct signal and wrap in C-Frame for link. 1005 */ 1006 static int 1007 l2cap_send_signal(struct hci_link *link, uint8_t code, uint8_t ident, 1008 uint16_t length, void *data) 1009 { 1010 struct mbuf *m; 1011 l2cap_hdr_t *hdr; 1012 l2cap_cmd_hdr_t *cmd; 1013 1014 KASSERT(link != NULL); 1015 KASSERT(sizeof(l2cap_cmd_hdr_t) + length <= link->hl_mtu); 1016 1017 m = m_gethdr(M_DONTWAIT, MT_DATA); 1018 if (m == NULL) 1019 return ENOMEM; 1020 1021 hdr = mtod(m, l2cap_hdr_t *); 1022 cmd = (l2cap_cmd_hdr_t *)(hdr + 1); 1023 1024 m->m_len = m->m_pkthdr.len = MHLEN; 1025 1026 /* Command Data */ 1027 if (length > 0) 1028 m_copyback(m, sizeof(*hdr) + sizeof(*cmd), length, data); 1029 1030 /* Command Header */ 1031 cmd->code = code; 1032 cmd->ident = ident; 1033 cmd->length = htole16(length); 1034 length += sizeof(*cmd); 1035 1036 /* C-Frame Header */ 1037 hdr->length = htole16(length); 1038 hdr->dcid = htole16(L2CAP_SIGNAL_CID); 1039 length += sizeof(*hdr); 1040 1041 if (m->m_pkthdr.len != MAX(MHLEN, length)) { 1042 m_freem(m); 1043 return ENOMEM; 1044 } 1045 1046 m->m_pkthdr.len = length; 1047 m->m_len = MIN(length, MHLEN); 1048 1049 DPRINTFN(2, "(%s) code %d, ident %d, len %d\n", 1050 device_xname(link->hl_unit->hci_dev), code, ident, length); 1051 1052 return hci_acl_send(m, link, NULL); 1053 } 1054 1055 /* 1056 * Send Command Reject packet. 1057 */ 1058 static int 1059 l2cap_send_command_rej(struct hci_link *link, uint8_t ident, 1060 int reason, ...) 1061 { 1062 l2cap_cmd_rej_cp cp; 1063 int len = 0; 1064 va_list ap; 1065 1066 va_start(ap, reason); 1067 1068 cp.reason = htole16(reason); 1069 1070 switch (reason) { 1071 case L2CAP_REJ_NOT_UNDERSTOOD: 1072 len = 2; 1073 break; 1074 1075 case L2CAP_REJ_MTU_EXCEEDED: 1076 len = 4; 1077 cp.data[0] = va_arg(ap, int); /* SigMTU */ 1078 cp.data[0] = htole16(cp.data[0]); 1079 break; 1080 1081 case L2CAP_REJ_INVALID_CID: 1082 len = 6; 1083 cp.data[0] = va_arg(ap, int); /* dcid */ 1084 cp.data[0] = htole16(cp.data[0]); 1085 cp.data[1] = va_arg(ap, int); /* scid */ 1086 cp.data[1] = htole16(cp.data[1]); 1087 break; 1088 1089 default: 1090 UNKNOWN(reason); 1091 va_end(ap); 1092 return EINVAL; 1093 } 1094 1095 va_end(ap); 1096 1097 return l2cap_send_signal(link, L2CAP_COMMAND_REJ, ident, len, &cp); 1098 } 1099 1100 /* 1101 * Send Connect Request 1102 */ 1103 int 1104 l2cap_send_connect_req(struct l2cap_channel *chan) 1105 { 1106 l2cap_con_req_cp cp; 1107 int err; 1108 1109 err = l2cap_request_alloc(chan, L2CAP_CONNECT_REQ); 1110 if (err) 1111 return err; 1112 1113 cp.psm = htole16(chan->lc_raddr.bt_psm); 1114 cp.scid = htole16(chan->lc_lcid); 1115 1116 return l2cap_send_signal(chan->lc_link, L2CAP_CONNECT_REQ, 1117 chan->lc_link->hl_lastid, sizeof(cp), &cp); 1118 } 1119 1120 /* 1121 * Send Config Request 1122 * 1123 * For outgoing config request, we only put options in the packet if they 1124 * differ from the default and would have to be actioned. We dont support 1125 * enough option types to make overflowing SigMTU an issue so it can all 1126 * go in one packet. 1127 */ 1128 int 1129 l2cap_send_config_req(struct l2cap_channel *chan) 1130 { 1131 l2cap_cfg_req_cp *cp; 1132 l2cap_cfg_opt_t *opt; 1133 l2cap_cfg_opt_val_t *val; 1134 uint8_t *next, buf[L2CAP_MTU_MINIMUM]; 1135 int err; 1136 1137 err = l2cap_request_alloc(chan, L2CAP_CONFIG_REQ); 1138 if (err) 1139 return err; 1140 1141 /* Config Header (4 octets) */ 1142 cp = (l2cap_cfg_req_cp *)buf; 1143 cp->dcid = htole16(chan->lc_rcid); 1144 cp->flags = 0; /* "No Continuation" */ 1145 1146 next = buf + sizeof(l2cap_cfg_req_cp); 1147 1148 /* Incoming MTU (4 octets) */ 1149 if (chan->lc_imtu != L2CAP_MTU_DEFAULT) { 1150 opt = (l2cap_cfg_opt_t *)next; 1151 opt->type = L2CAP_OPT_MTU; 1152 opt->length = L2CAP_OPT_MTU_SIZE; 1153 1154 val = (l2cap_cfg_opt_val_t *)(opt + 1); 1155 val->mtu = htole16(chan->lc_imtu); 1156 1157 next += sizeof(l2cap_cfg_opt_t) + L2CAP_OPT_MTU_SIZE; 1158 } 1159 1160 /* Flush Timeout (4 octets) */ 1161 if (chan->lc_flush != L2CAP_FLUSH_TIMO_DEFAULT) { 1162 opt = (l2cap_cfg_opt_t *)next; 1163 opt->type = L2CAP_OPT_FLUSH_TIMO; 1164 opt->length = L2CAP_OPT_FLUSH_TIMO_SIZE; 1165 1166 val = (l2cap_cfg_opt_val_t *)(opt + 1); 1167 val->flush_timo = htole16(chan->lc_flush); 1168 1169 next += sizeof(l2cap_cfg_opt_t) + L2CAP_OPT_FLUSH_TIMO_SIZE; 1170 } 1171 1172 /* Outgoing QoS Flow (24 octets) */ 1173 /* Retransmission & Flow Control (11 octets) */ 1174 /* 1175 * From here we need to start paying attention to SigMTU as we have 1176 * possibly overflowed the minimum supported.. 1177 */ 1178 1179 return l2cap_send_signal(chan->lc_link, L2CAP_CONFIG_REQ, 1180 chan->lc_link->hl_lastid, (int)(next - buf), buf); 1181 } 1182 1183 /* 1184 * Send Disconnect Request 1185 */ 1186 int 1187 l2cap_send_disconnect_req(struct l2cap_channel *chan) 1188 { 1189 l2cap_discon_req_cp cp; 1190 int err; 1191 1192 err = l2cap_request_alloc(chan, L2CAP_DISCONNECT_REQ); 1193 if (err) 1194 return err; 1195 1196 cp.dcid = htole16(chan->lc_rcid); 1197 cp.scid = htole16(chan->lc_lcid); 1198 1199 return l2cap_send_signal(chan->lc_link, L2CAP_DISCONNECT_REQ, 1200 chan->lc_link->hl_lastid, sizeof(cp), &cp); 1201 } 1202 1203 /* 1204 * Send Connect Response 1205 */ 1206 int 1207 l2cap_send_connect_rsp(struct hci_link *link, uint8_t ident, uint16_t dcid, uint16_t scid, uint16_t result) 1208 { 1209 l2cap_con_rsp_cp cp; 1210 1211 memset(&cp, 0, sizeof(cp)); 1212 cp.dcid = htole16(dcid); 1213 cp.scid = htole16(scid); 1214 cp.result = htole16(result); 1215 1216 return l2cap_send_signal(link, L2CAP_CONNECT_RSP, ident, sizeof(cp), &cp); 1217 } 1218 1219 /* 1220 * copy in QoS buffer to host 1221 */ 1222 static void 1223 l2cap_qos_btoh(l2cap_qos_t *qos, void *buf) 1224 { 1225 l2cap_qos_t *src = buf; 1226 1227 qos->flags = src->flags; 1228 qos->service_type = src->service_type; 1229 qos->token_rate = le32toh(src->token_rate); 1230 qos->token_bucket_size = le32toh(src->token_bucket_size); 1231 qos->peak_bandwidth = le32toh(src->peak_bandwidth); 1232 qos->latency = le32toh(src->latency); 1233 qos->delay_variation = le32toh(src->delay_variation); 1234 } 1235 1236 /* 1237 * copy out host QoS to buffer 1238 */ 1239 static void 1240 l2cap_qos_htob(void *buf, l2cap_qos_t *qos) 1241 { 1242 l2cap_qos_t *dst = buf; 1243 1244 dst->flags = qos->flags; 1245 dst->service_type = qos->service_type; 1246 dst->token_rate = htole32(qos->token_rate); 1247 dst->token_bucket_size = htole32(qos->token_bucket_size); 1248 dst->peak_bandwidth = htole32(qos->peak_bandwidth); 1249 dst->latency = htole32(qos->latency); 1250 dst->delay_variation = htole32(qos->delay_variation); 1251 } 1252