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