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