1 /* $NetBSD: l2cap_signal.c,v 1.1 2006/06/19 15:44:45 gdamore 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.1 2006/06/19 15:44:45 gdamore 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 <machine/stdarg.h> 44 45 #include <netbt/bluetooth.h> 46 #include <netbt/hci.h> 47 #include <netbt/l2cap.h> 48 49 /******************************************************************************* 50 * 51 * L2CAP Signal processing 52 */ 53 54 static void l2cap_recv_command_rej(struct mbuf *, struct hci_link *); 55 static void l2cap_recv_connect_req(struct mbuf *, struct hci_link *); 56 static void l2cap_recv_connect_rsp(struct mbuf *, struct hci_link *); 57 static void l2cap_recv_config_req(struct mbuf *, struct hci_link *); 58 static void l2cap_recv_config_rsp(struct mbuf *, struct hci_link *); 59 static void l2cap_recv_disconnect_req(struct mbuf *, struct hci_link *); 60 static void l2cap_recv_disconnect_rsp(struct mbuf *, struct hci_link *); 61 static void l2cap_recv_info_req(struct mbuf *, struct hci_link *); 62 static int l2cap_send_signal(struct hci_link *, uint8_t, uint8_t, uint16_t, void *); 63 static int l2cap_send_command_rej(struct hci_link *, uint8_t, uint16_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 link->hl_unit->hci_devname, 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 161 #ifdef DIAGNOSTIC 162 panic("impossible!"); 163 #endif 164 165 reject: 166 l2cap_send_command_rej(link, cmd.ident, L2CAP_REJ_NOT_UNDERSTOOD); 167 finish: 168 m_freem(m); 169 } 170 171 /* 172 * Process Received Command Reject. For now we dont try to recover gracefully 173 * from this, it probably means that the link is garbled or the other end is 174 * insufficiently capable of handling normal traffic. (not *my* fault, no way!) 175 */ 176 static void 177 l2cap_recv_command_rej(struct mbuf *m, struct hci_link *link) 178 { 179 struct l2cap_req *req; 180 struct l2cap_channel *chan; 181 l2cap_cmd_hdr_t cmd; 182 l2cap_cmd_rej_cp cp; 183 184 m_copydata(m, 0, sizeof(cmd), &cmd); 185 m_adj(m, sizeof(cmd)); 186 187 cmd.length = le16toh(cmd.length); 188 189 m_copydata(m, 0, cmd.length, &cp); 190 m_adj(m, cmd.length); 191 192 req = l2cap_request_lookup(link, cmd.ident); 193 if (req == NULL) 194 return; 195 196 switch (le16toh(cp.reason)) { 197 case L2CAP_REJ_NOT_UNDERSTOOD: 198 /* 199 * I dont know what to do, just move up the timeout 200 */ 201 callout_schedule(&req->lr_rtx, 0); 202 break; 203 204 case L2CAP_REJ_MTU_EXCEEDED: 205 /* 206 * I didnt send any commands over L2CAP_MTU_MINIMUM size, but.. 207 */ 208 link->hl_mtu = le16toh(cp.data[0]); 209 callout_schedule(&req->lr_rtx, 0); // XX maybe resend instead? 210 break; 211 212 case L2CAP_REJ_INVALID_CID: 213 /* 214 * Well, if they dont have such a channel then our channel is 215 * most likely closed. Make it so. 216 */ 217 chan = req->lr_chan; 218 l2cap_request_free(req); 219 if (chan != NULL && chan->lc_state != L2CAP_CLOSED) 220 l2cap_close(chan, ECONNABORTED); 221 222 break; 223 224 default: 225 UNKNOWN(le16toh(cp.reason)); 226 break; 227 } 228 } 229 230 /* 231 * Process Received Connect Request. Find listening channel matching 232 * psm & addr and ask upper layer for a new channel. 233 */ 234 static void 235 l2cap_recv_connect_req(struct mbuf *m, struct hci_link *link) 236 { 237 struct sockaddr_bt laddr, raddr; 238 struct l2cap_channel *chan, *new; 239 l2cap_cmd_hdr_t cmd; 240 l2cap_con_req_cp cp; 241 l2cap_con_rsp_cp rp; 242 int err; 243 244 /* extract cmd */ 245 m_copydata(m, 0, sizeof(cmd), &cmd); 246 m_adj(m, sizeof(cmd)); 247 248 /* extract request */ 249 m_copydata(m, 0, sizeof(cp), &cp); 250 m_adj(m, sizeof(cp)); 251 252 /* init response */ 253 memset(&rp, 0, sizeof(rp)); 254 rp.scid = cp.scid; 255 256 memset(&laddr, 0, sizeof(struct sockaddr_bt)); 257 laddr.bt_len = sizeof(struct sockaddr_bt); 258 laddr.bt_family = AF_BLUETOOTH; 259 laddr.bt_psm = le16toh(cp.psm); 260 bdaddr_copy(&laddr.bt_bdaddr, &link->hl_unit->hci_bdaddr); 261 262 memset(&raddr, 0, sizeof(struct sockaddr_bt)); 263 raddr.bt_len = sizeof(struct sockaddr_bt); 264 raddr.bt_family = AF_BLUETOOTH; 265 raddr.bt_psm = le16toh(cp.psm); 266 bdaddr_copy(&raddr.bt_bdaddr, &link->hl_bdaddr); 267 268 LIST_FOREACH(chan, &l2cap_listen_list, lc_ncid) { 269 if (chan->lc_laddr.bt_psm != laddr.bt_psm 270 && chan->lc_laddr.bt_psm != L2CAP_PSM_ANY) 271 continue; 272 273 if (!bdaddr_same(&laddr.bt_bdaddr, &chan->lc_laddr.bt_bdaddr) 274 && bdaddr_any(&chan->lc_laddr.bt_bdaddr) == 0) 275 continue; 276 277 new= (*chan->lc_proto->newconn)(chan->lc_upper, &laddr, &raddr); 278 if (new == NULL) 279 continue; 280 281 err = l2cap_cid_alloc(new); 282 if (err) { 283 rp.result = htole16(L2CAP_NO_RESOURCES); 284 l2cap_send_signal(link, L2CAP_CONNECT_RSP, 285 cmd.ident, sizeof(rp), &rp); 286 (*new->lc_proto->disconnected)(new->lc_upper, err); 287 return; 288 } 289 290 new->lc_link = hci_acl_open(link->hl_unit, &link->hl_bdaddr); 291 KASSERT(new->lc_link == link); 292 293 new->lc_rcid = le16toh(cp.scid); 294 295 memcpy(&new->lc_laddr, &laddr, sizeof(struct sockaddr_bt)); 296 memcpy(&new->lc_raddr, &raddr, sizeof(struct sockaddr_bt)); 297 298 rp.dcid = htole16(new->lc_lcid); 299 rp.result = htole16(L2CAP_SUCCESS); 300 l2cap_send_signal(link, L2CAP_CONNECT_RSP, cmd.ident, 301 sizeof(rp), &rp); 302 303 new->lc_state = L2CAP_WAIT_CONFIG_REQ | L2CAP_WAIT_CONFIG_RSP; 304 l2cap_send_config_req(new); 305 return; 306 } 307 308 rp.result = htole16(L2CAP_PSM_NOT_SUPPORTED); 309 l2cap_send_signal(link, L2CAP_CONNECT_RSP, cmd.ident, sizeof(rp), &rp); 310 } 311 312 /* 313 * Process Received Connect Response. 314 */ 315 static void 316 l2cap_recv_connect_rsp(struct mbuf *m, struct hci_link *link) 317 { 318 l2cap_cmd_hdr_t cmd; 319 l2cap_con_rsp_cp cp; 320 struct l2cap_req *req; 321 struct l2cap_channel *chan; 322 323 m_copydata(m, 0, sizeof(cmd), &cmd); 324 m_adj(m, sizeof(cmd)); 325 326 m_copydata(m, 0, sizeof(cp), &cp); 327 m_adj(m, sizeof(cp)); 328 329 cp.scid = le16toh(cp.scid); 330 cp.dcid = le16toh(cp.dcid); 331 cp.result = le16toh(cp.result); 332 333 req = l2cap_request_lookup(link, cmd.ident); 334 if (req == NULL || req->lr_code != L2CAP_CONNECT_REQ) 335 return; 336 337 chan = req->lr_chan; 338 if (chan != NULL && chan->lc_lcid != cp.scid) 339 return; 340 341 if (chan == NULL || chan->lc_state != L2CAP_WAIT_CONNECT_RSP) { 342 l2cap_request_free(req); 343 return; 344 } 345 346 switch (cp.result) { 347 case L2CAP_SUCCESS: 348 /* 349 * Ok, at this point we have a connection to the other party. We 350 * could indicate upstream that we are ready for business and 351 * wait for a "Configure Channel Request" but I'm not so sure 352 * that is required in our case - we will proceed directly to 353 * sending our config request. We set two state bits because in 354 * the config state we are waiting for requests and responses. 355 */ 356 l2cap_request_free(req); 357 chan->lc_rcid = cp.dcid; 358 chan->lc_state = L2CAP_WAIT_CONFIG_REQ | L2CAP_WAIT_CONFIG_RSP; 359 l2cap_send_config_req(chan); 360 break; 361 362 case L2CAP_PENDING: 363 // dont release request, should start eRTX timeout? 364 (*chan->lc_proto->connecting)(chan->lc_upper); 365 break; 366 367 case L2CAP_PSM_NOT_SUPPORTED: 368 case L2CAP_SECURITY_BLOCK: 369 case L2CAP_NO_RESOURCES: 370 default: 371 l2cap_request_free(req); 372 l2cap_close(chan, ECONNREFUSED); 373 break; 374 } 375 } 376 377 /* 378 * Process Received Config Reqest. 379 */ 380 static void 381 l2cap_recv_config_req(struct mbuf *m, struct hci_link *link) 382 { 383 uint8_t buf[L2CAP_MTU_MINIMUM]; 384 l2cap_cmd_hdr_t cmd; 385 l2cap_cfg_req_cp cp; 386 l2cap_cfg_opt_t *opt; 387 l2cap_cfg_opt_val_t *val; 388 l2cap_cfg_rsp_cp *rp; 389 struct l2cap_channel *chan; 390 int left, len; 391 392 m_copydata(m, 0, sizeof(cmd), &cmd); 393 m_adj(m, sizeof(cmd)); 394 left = le16toh(cmd.length); 395 396 if (left < sizeof(cp)) 397 goto reject; 398 399 m_copydata(m, 0, sizeof(cp), &cp); 400 m_adj(m, sizeof(cp)); 401 left -= sizeof(cp); 402 403 cp.dcid = le16toh(cp.dcid); 404 cp.flags = le16toh(cp.flags); 405 406 chan = l2cap_cid_lookup(cp.dcid); 407 if (chan == NULL || (chan->lc_state & L2CAP_WAIT_CONFIG_REQ) == 0) { 408 l2cap_send_command_rej(link, cmd.ident, L2CAP_REJ_INVALID_CID, 409 L2CAP_NULL_CID, cp.dcid); 410 goto out; 411 } 412 413 /* ready our response packet */ 414 rp = (l2cap_cfg_rsp_cp *)buf; 415 rp->scid = chan->lc_rcid; 416 rp->flags = 0; /* "No Continuation" */ 417 rp->result = L2CAP_SUCCESS; 418 len = sizeof(*rp); 419 420 /* 421 * Process the packet. We build the return packet on the fly adding any 422 * unacceptable parameters as we go. As we can only return one result, 423 * unknown option takes precedence so we start our return packet anew 424 * and ignore option values thereafter as they will be re-sent. 425 * 426 * Since we do not support enough options to make overflowing the min 427 * MTU size an issue in normal use, we just reject config requests that 428 * would make that happen. This could be if: 429 * 430 * 1. too many unknown option types (>40!) 431 * 2. repeated options we found unacceptable 432 * 3. corrupted packet looks like above 433 */ 434 while (left > 0) { 435 if (left < sizeof(*opt) || len + sizeof(*opt) > sizeof(buf)) 436 goto reject; 437 438 opt = (l2cap_cfg_opt_t *)&buf[len]; 439 m_copydata(m, 0, sizeof(*opt), opt); 440 m_adj(m, sizeof(*opt)); 441 left -= sizeof(*opt); 442 443 if (left < opt->length) 444 goto reject; 445 446 val = (l2cap_cfg_opt_val_t *)&buf[len + sizeof(*opt)]; 447 448 switch(opt->type & L2CAP_OPT_HINT_MASK) { 449 case L2CAP_OPT_MTU: 450 if (rp->result == L2CAP_UNKNOWN_OPTION) 451 break; 452 453 if (opt->length != L2CAP_OPT_MTU_SIZE 454 || len + sizeof(*opt) + L2CAP_OPT_MTU_SIZE > sizeof(buf)) 455 goto reject; 456 457 m_copydata(m, 0, L2CAP_OPT_MTU_SIZE, val); 458 val->mtu = le16toh(val->mtu); 459 460 /* 461 * XXX how do we know what the minimum acceptable MTU is 462 * for a channel? Spec says some profiles have a higher 463 * minimum but I have no way to find that out at this 464 * juncture.. 465 */ 466 if (val->mtu < L2CAP_MTU_MINIMUM) { 467 rp->result = L2CAP_UNACCEPTABLE_PARAMS; 468 val->mtu = htole16(L2CAP_MTU_MINIMUM); 469 len += sizeof(*opt) + L2CAP_OPT_MTU_SIZE; 470 } else 471 chan->lc_omtu = val->mtu; 472 473 break; 474 475 case L2CAP_OPT_FLUSH_TIMO: 476 if (rp->result == L2CAP_UNKNOWN_OPTION) 477 break; 478 479 if (opt->length != L2CAP_OPT_FLUSH_TIMO_SIZE 480 || len + sizeof(*opt) + L2CAP_OPT_FLUSH_TIMO_SIZE > sizeof(buf)) 481 goto reject; 482 483 /* 484 * I think that this is informational only - he is 485 * informing us of the flush timeout he will be using. 486 * I dont think this affects us in any significant way, 487 * so just ignore this value for now. 488 */ 489 break; 490 491 case L2CAP_OPT_QOS: 492 default: 493 /* ignore hints */ 494 if (opt->type & L2CAP_OPT_HINT_BIT) 495 break; 496 497 /* unknown options supercede all else */ 498 if (rp->result != L2CAP_UNKNOWN_OPTION) { 499 rp->result = L2CAP_UNKNOWN_OPTION; 500 len = sizeof(*rp); 501 } 502 503 buf[len++] = opt->type; 504 buf[len++] = 0; // no need to return his data 505 break; 506 } 507 508 m_adj(m, opt->length); 509 left -= opt->length; 510 } 511 512 rp->result = htole16(rp->result); 513 l2cap_send_signal(link, L2CAP_CONFIG_RSP, cmd.ident, len, buf); 514 515 if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0 516 && rp->result == L2CAP_SUCCESS) { 517 518 chan->lc_state &= ~L2CAP_WAIT_CONFIG_REQ; 519 520 if ((chan->lc_state & L2CAP_WAIT_CONFIG_RSP) == 0) { 521 chan->lc_state = L2CAP_OPEN; 522 // XXX how to distinguish REconfiguration? 523 (*chan->lc_proto->connected)(chan->lc_upper); 524 } 525 } 526 return; 527 528 reject: 529 l2cap_send_command_rej(link, cmd.ident, L2CAP_REJ_NOT_UNDERSTOOD); 530 out: 531 m_adj(m, left); 532 } 533 534 /* 535 * Process Received Config Response. 536 */ 537 static void 538 l2cap_recv_config_rsp(struct mbuf *m, struct hci_link *link) 539 { 540 l2cap_cmd_hdr_t cmd; 541 l2cap_cfg_rsp_cp cp; 542 l2cap_cfg_opt_t opt; 543 l2cap_cfg_opt_val_t val; 544 struct l2cap_req *req; 545 struct l2cap_channel *chan; 546 int left; 547 548 m_copydata(m, 0, sizeof(cmd), &cmd); 549 m_adj(m, sizeof(cmd)); 550 left = le16toh(cmd.length); 551 552 if (left < sizeof(cp)) 553 goto out; 554 555 m_copydata(m, 0, sizeof(cp), &cp); 556 m_adj(m, sizeof(cp)); 557 left -= sizeof(cp); 558 559 cp.scid = le16toh(cp.scid); 560 cp.flags = le16toh(cp.flags); 561 cp.result = le16toh(cp.result); 562 563 req = l2cap_request_lookup(link, cmd.ident); 564 if (req == NULL || req->lr_code != L2CAP_CONFIG_REQ) 565 goto out; 566 567 chan = req->lr_chan; 568 if (chan != NULL && chan->lc_lcid != cp.scid) 569 goto out; 570 571 l2cap_request_free(req); 572 573 if (chan == NULL || (chan->lc_state & L2CAP_WAIT_CONFIG_RSP) == 0) 574 goto out; 575 576 if ((cp.flags & L2CAP_OPT_CFLAG_BIT)) { 577 l2cap_cfg_req_cp rp; 578 579 /* 580 * They have more to tell us and want another ID to 581 * use, so send an empty config request 582 */ 583 if (l2cap_request_alloc(chan, L2CAP_CONFIG_REQ)) 584 goto discon; 585 586 rp.dcid = htole16(cp.scid); 587 rp.flags = 0; 588 589 if (l2cap_send_signal(link, L2CAP_CONFIG_REQ, link->hl_lastid, 590 sizeof(rp), &rp)) 591 goto discon; 592 } 593 594 switch(cp.result) { 595 case L2CAP_SUCCESS: 596 /* 597 * If continuation flag was not set, our config request was 598 * accepted. We may have to wait for their config request to 599 * complete, so check that but otherwise we are open 600 * 601 * There may be 'advisory' values in the packet but we just 602 * ignore those.. 603 */ 604 if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0) { 605 chan->lc_state &= ~L2CAP_WAIT_CONFIG_RSP; 606 607 if ((chan->lc_state & L2CAP_WAIT_CONFIG_REQ) == 0) { 608 chan->lc_state = L2CAP_OPEN; 609 // XXX how to distinguish REconfiguration? 610 (*chan->lc_proto->connected)(chan->lc_upper); 611 } 612 } 613 goto out; 614 615 case L2CAP_UNACCEPTABLE_PARAMS: 616 /* 617 * Packet contains unacceptable parameters with preferred values 618 */ 619 while (left > 0) { 620 if (left < sizeof(opt)) 621 goto discon; 622 623 m_copydata(m, 0, sizeof(opt), &opt); 624 m_adj(m, sizeof(opt)); 625 left -= sizeof(opt); 626 627 if (left < opt.length) 628 goto discon; 629 630 switch (opt.type) { 631 case L2CAP_OPT_MTU: 632 if (opt.length != L2CAP_OPT_MTU_SIZE) 633 goto discon; 634 635 m_copydata(m, 0, L2CAP_OPT_MTU_SIZE, &val); 636 chan->lc_imtu = le16toh(val.mtu); 637 if (chan->lc_imtu < L2CAP_MTU_MINIMUM) 638 chan->lc_imtu = L2CAP_MTU_DEFAULT; 639 break; 640 641 case L2CAP_OPT_FLUSH_TIMO: 642 if (opt.length != L2CAP_OPT_FLUSH_TIMO_SIZE) 643 goto discon; 644 645 /* 646 * Spec says: If we cannot honor proposed value, 647 * either disconnect or try again with original 648 * value. I can't really see why they want to 649 * interfere with OUR flush timeout in any case 650 * so we just punt for now. 651 */ 652 goto discon; 653 654 case L2CAP_OPT_QOS: 655 break; 656 657 default: 658 UNKNOWN(opt.type); 659 goto discon; 660 } 661 662 m_adj(m, opt.length); 663 left -= opt.length; 664 } 665 666 if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0) 667 l2cap_send_config_req(chan); // no state change 668 669 goto out; 670 671 case L2CAP_REJECT: 672 goto discon; 673 674 case L2CAP_UNKNOWN_OPTION: 675 /* 676 * Packet contains options not understood. Turn off unknown 677 * options by setting them to default values (means they will 678 * not be requested again). 679 * 680 * If our option was already off then fail (paranoia?) 681 * 682 * XXX Should we consider that options were set for a reason? 683 */ 684 while (left > 0) { 685 if (left < sizeof(opt)) 686 goto discon; 687 688 m_copydata(m, 0, sizeof(opt), &opt); 689 m_adj(m, sizeof(opt)); 690 left -= sizeof(opt); 691 692 if (left < opt.length) 693 goto discon; 694 695 m_adj(m, opt.length); 696 left -= opt.length; 697 698 switch(opt.type) { 699 case L2CAP_OPT_MTU: 700 if (chan->lc_imtu == L2CAP_MTU_DEFAULT) 701 goto discon; 702 703 chan->lc_imtu = L2CAP_MTU_DEFAULT; 704 break; 705 706 case L2CAP_OPT_FLUSH_TIMO: 707 if (chan->lc_flush == L2CAP_FLUSH_TIMO_DEFAULT) 708 goto discon; 709 710 chan->lc_flush = L2CAP_FLUSH_TIMO_DEFAULT; 711 break; 712 713 case L2CAP_OPT_QOS: 714 break; 715 716 default: 717 UNKNOWN(opt.type); 718 goto discon; 719 } 720 } 721 722 if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0) 723 l2cap_send_config_req(chan); /* no state change */ 724 725 goto out; 726 727 default: 728 UNKNOWN(cp.result); 729 goto discon; 730 } 731 732 DPRINTF("how did I get here!?\n"); 733 734 discon: 735 l2cap_send_disconnect_req(chan); 736 l2cap_close(chan, ECONNABORTED); 737 738 out: 739 m_adj(m, left); 740 } 741 742 /* 743 * Process Received Disconnect Request. We must validate scid and dcid 744 * just in case but otherwise this connection is finished. 745 */ 746 static void 747 l2cap_recv_disconnect_req(struct mbuf *m, struct hci_link *link) 748 { 749 l2cap_cmd_hdr_t cmd; 750 l2cap_discon_req_cp cp; 751 l2cap_discon_rsp_cp rp; 752 struct l2cap_channel *chan; 753 754 m_copydata(m, 0, sizeof(cmd), &cmd); 755 m_adj(m, sizeof(cmd)); 756 757 m_copydata(m, 0, sizeof(cp), &cp); 758 m_adj(m, sizeof(cp)); 759 760 cp.scid = le16toh(cp.scid); 761 cp.dcid = le16toh(cp.dcid); 762 763 chan = l2cap_cid_lookup(cp.dcid); 764 if (chan == NULL || chan->lc_rcid != cp.scid) { 765 l2cap_send_command_rej(link, cmd.ident, L2CAP_REJ_INVALID_CID, 766 cp.dcid, cp.scid); 767 return; 768 } 769 770 rp.dcid = htole16(chan->lc_lcid); 771 rp.scid = htole16(chan->lc_rcid); 772 l2cap_send_signal(link, L2CAP_DISCONNECT_RSP, cmd.ident, 773 sizeof(rp), &rp); 774 775 if (chan->lc_state != L2CAP_CLOSED) 776 l2cap_close(chan, ECONNRESET); 777 } 778 779 /* 780 * Process Received Disconnect Response. We must validate scid and dcid but 781 * unless we were waiting for this signal, ignore it. 782 */ 783 static void 784 l2cap_recv_disconnect_rsp(struct mbuf *m, struct hci_link *link) 785 { 786 l2cap_cmd_hdr_t cmd; 787 l2cap_discon_rsp_cp cp; 788 struct l2cap_req *req; 789 struct l2cap_channel *chan; 790 791 m_copydata(m, 0, sizeof(cmd), &cmd); 792 m_adj(m, sizeof(cmd)); 793 794 m_copydata(m, 0, sizeof(cp), &cp); 795 m_adj(m, sizeof(cp)); 796 797 cp.scid = le16toh(cp.scid); 798 cp.dcid = le16toh(cp.dcid); 799 800 req = l2cap_request_lookup(link, cmd.ident); 801 if (req == NULL || req->lr_code != L2CAP_DISCONNECT_REQ) 802 return; 803 804 chan = req->lr_chan; 805 if (chan == NULL 806 || chan->lc_lcid != cp.scid 807 || chan->lc_rcid != cp.dcid) 808 return; 809 810 l2cap_request_free(req); 811 812 if (chan->lc_state != L2CAP_WAIT_DISCONNECT) 813 return; 814 815 l2cap_close(chan, 0); 816 } 817 818 /* 819 * Process Received Info Request. We must respond but alas dont 820 * support anything as yet so thats easy. 821 */ 822 static void 823 l2cap_recv_info_req(struct mbuf *m, struct hci_link *link) 824 { 825 l2cap_cmd_hdr_t cmd; 826 l2cap_info_req_cp cp; 827 l2cap_info_rsp_cp rp; 828 829 m_copydata(m, 0, sizeof(cmd), &cmd); 830 m_adj(m, sizeof(cmd)); 831 832 m_copydata(m, 0, sizeof(cp), &cp); 833 m_adj(m, sizeof(cp)); 834 835 switch(le16toh(cp.type)) { 836 case L2CAP_CONNLESS_MTU: 837 case L2CAP_EXTENDED_FEATURES: 838 default: 839 rp.type = cp.type; 840 rp.result = htole16(L2CAP_NOT_SUPPORTED); 841 842 l2cap_send_signal(link, L2CAP_INFO_RSP, cmd.ident, 843 sizeof(rp), &rp); 844 break; 845 } 846 } 847 848 /* 849 * Construct signal and wrap in C-Frame for link. 850 */ 851 static int 852 l2cap_send_signal(struct hci_link *link, uint8_t code, uint8_t ident, 853 uint16_t length, void *data) 854 { 855 struct mbuf *m; 856 l2cap_hdr_t *hdr; 857 l2cap_cmd_hdr_t *cmd; 858 859 #ifdef DIAGNOSTIC 860 if (link == NULL) 861 return ENETDOWN; 862 863 if (sizeof(l2cap_cmd_hdr_t) + length > link->hl_mtu) 864 printf("(%s) exceeding L2CAP Signal MTU for link!\n", 865 link->hl_unit->hci_devname); 866 #endif 867 868 m = m_gethdr(M_DONTWAIT, MT_DATA); 869 if (m == NULL) 870 return ENOMEM; 871 872 hdr = mtod(m, l2cap_hdr_t *); 873 cmd = (l2cap_cmd_hdr_t *)(hdr + 1); 874 875 m->m_len = m->m_pkthdr.len = MHLEN; 876 877 /* Command Data */ 878 if (length > 0) 879 m_copyback(m, sizeof(hdr) + sizeof(cmd), length, data); 880 881 /* Command Header */ 882 cmd->code = code; 883 cmd->ident = ident; 884 cmd->length = htole16(length); 885 length += sizeof(cmd); 886 887 /* C-Frame Header */ 888 hdr->length = htole16(length); 889 hdr->dcid = htole16(L2CAP_SIGNAL_CID); 890 length += sizeof(hdr); 891 892 if (m->m_pkthdr.len != MAX(MHLEN, length)) { 893 m_freem(m); 894 return ENOMEM; 895 } 896 897 m->m_pkthdr.len = length; 898 m->m_len = MIN(length, MHLEN); 899 900 DPRINTFN(2, "(%s) code %d, ident %d, len %d\n", 901 link->hl_unit->hci_devname, code, ident, length); 902 903 return hci_acl_send(m, link, NULL); 904 } 905 906 /* 907 * Send Command Reject packet. 908 */ 909 static int 910 l2cap_send_command_rej(struct hci_link *link, uint8_t ident, 911 uint16_t reason, ...) 912 { 913 l2cap_cmd_rej_cp cp; 914 int len = 0; 915 va_list ap; 916 917 va_start(ap, reason); 918 919 cp.reason = htole16(reason); 920 921 switch (reason) { 922 case L2CAP_REJ_NOT_UNDERSTOOD: 923 len = 2; 924 break; 925 926 case L2CAP_REJ_MTU_EXCEEDED: 927 len = 4; 928 cp.data[0] = va_arg(ap, int); /* SigMTU */ 929 cp.data[0] = htole16(cp.data[0]); 930 break; 931 932 case L2CAP_REJ_INVALID_CID: 933 len = 6; 934 cp.data[0] = va_arg(ap, int); /* dcid */ 935 cp.data[0] = htole16(cp.data[0]); 936 cp.data[1] = va_arg(ap, int); /* scid */ 937 cp.data[1] = htole16(cp.data[1]); 938 break; 939 940 default: 941 UNKNOWN(reason); 942 return EINVAL; 943 } 944 945 va_end(ap); 946 947 return l2cap_send_signal(link, L2CAP_COMMAND_REJ, ident, len, &cp); 948 } 949 950 /* 951 * Send Connect Request 952 */ 953 int 954 l2cap_send_connect_req(struct l2cap_channel *chan) 955 { 956 l2cap_con_req_cp cp; 957 int err; 958 959 err = l2cap_request_alloc(chan, L2CAP_CONNECT_REQ); 960 if (err) 961 return err; 962 963 cp.psm = htole16(chan->lc_raddr.bt_psm); 964 cp.scid = htole16(chan->lc_lcid); 965 966 return l2cap_send_signal(chan->lc_link, L2CAP_CONNECT_REQ, 967 chan->lc_link->hl_lastid, sizeof(cp), &cp); 968 } 969 970 /* 971 * Send Config Request 972 * 973 * For outgoing config request, we only put options in the packet if they 974 * differ from the default and would have to be actioned. We dont support 975 * enough option types to make overflowing SigMTU an issue so it can all 976 * go in one packet. 977 */ 978 int 979 l2cap_send_config_req(struct l2cap_channel *chan) 980 { 981 l2cap_cfg_req_cp *cp; 982 l2cap_cfg_opt_t *opt; 983 l2cap_cfg_opt_val_t *val; 984 uint8_t *next, buf[L2CAP_MTU_MINIMUM]; 985 int err; 986 987 err = l2cap_request_alloc(chan, L2CAP_CONFIG_REQ); 988 if (err) 989 return err; 990 991 /* Config Header (4 octets) */ 992 cp = (l2cap_cfg_req_cp *)buf; 993 cp->dcid = htole16(chan->lc_rcid); 994 cp->flags = 0; /* "No Continuation" */ 995 996 next = buf + sizeof(l2cap_cfg_req_cp); 997 998 /* Incoming MTU (4 octets) */ 999 if (chan->lc_imtu != L2CAP_MTU_DEFAULT) { 1000 opt = (l2cap_cfg_opt_t *)next; 1001 opt->type = L2CAP_OPT_MTU; 1002 opt->length = L2CAP_OPT_MTU_SIZE; 1003 1004 val = (l2cap_cfg_opt_val_t *)(opt + 1); 1005 val->mtu = htole16(chan->lc_imtu); 1006 1007 next += sizeof(l2cap_cfg_opt_t) + L2CAP_OPT_MTU_SIZE; 1008 } 1009 1010 /* Flush Timeout (4 octets) */ 1011 if (chan->lc_flush != L2CAP_FLUSH_TIMO_DEFAULT) { 1012 opt = (l2cap_cfg_opt_t *)next; 1013 opt->type = L2CAP_OPT_FLUSH_TIMO; 1014 opt->length = L2CAP_OPT_FLUSH_TIMO_SIZE; 1015 1016 val = (l2cap_cfg_opt_val_t *)(opt + 1); 1017 val->flush_timo = htole16(chan->lc_flush); 1018 1019 next += sizeof(l2cap_cfg_opt_t) + L2CAP_OPT_FLUSH_TIMO_SIZE; 1020 } 1021 1022 /* Outgoing QoS Flow (24 octets) */ 1023 /* Retransmission & Flow Control (11 octets) */ 1024 /* 1025 * From here we need to start paying attention to SigMTU as we have 1026 * possibly overflowed the minimum supported.. 1027 */ 1028 1029 return l2cap_send_signal(chan->lc_link, L2CAP_CONFIG_REQ, 1030 chan->lc_link->hl_lastid, (int)(next - buf), buf); 1031 } 1032 1033 /* 1034 * Send Disconnect Request 1035 */ 1036 int 1037 l2cap_send_disconnect_req(struct l2cap_channel *chan) 1038 { 1039 l2cap_discon_req_cp cp; 1040 int err; 1041 1042 err = l2cap_request_alloc(chan, L2CAP_DISCONNECT_REQ); 1043 if (err) 1044 return err; 1045 1046 cp.dcid = htole16(chan->lc_rcid); 1047 cp.scid = htole16(chan->lc_lcid); 1048 1049 return l2cap_send_signal(chan->lc_link, L2CAP_DISCONNECT_REQ, 1050 chan->lc_link->hl_lastid, sizeof(cp), &cp); 1051 } 1052