1 /* 2 * ng_l2cap_llpi.c 3 * 4 * Copyright (c) Maksim Yevmenkin <m_evmenkin@yahoo.com> 5 * All rights reserved. 6 * 7 * Redistribution and use in source and binary forms, with or without 8 * modification, are permitted provided that the following conditions 9 * are met: 10 * 1. Redistributions of source code must retain the above copyright 11 * notice, this list of conditions and the following disclaimer. 12 * 2. Redistributions in binary form must reproduce the above copyright 13 * notice, this list of conditions and the following disclaimer in the 14 * documentation and/or other materials provided with the distribution. 15 * 16 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 17 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 20 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 22 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 23 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 24 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 25 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 26 * SUCH DAMAGE. 27 * 28 * $Id: ng_l2cap_llpi.c,v 1.16 2002/09/04 21:38:38 max Exp $ 29 * $FreeBSD$ 30 */ 31 32 #include <sys/param.h> 33 #include <sys/systm.h> 34 #include <sys/kernel.h> 35 #include <sys/endian.h> 36 #include <sys/malloc.h> 37 #include <sys/mbuf.h> 38 #include <sys/queue.h> 39 #include <netgraph/ng_message.h> 40 #include <netgraph/netgraph.h> 41 #include "ng_bluetooth.h" 42 #include "ng_hci.h" 43 #include "ng_l2cap.h" 44 #include "ng_l2cap_var.h" 45 #include "ng_l2cap_cmds.h" 46 #include "ng_l2cap_evnt.h" 47 #include "ng_l2cap_llpi.h" 48 #include "ng_l2cap_ulpi.h" 49 #include "ng_l2cap_misc.h" 50 51 /****************************************************************************** 52 ****************************************************************************** 53 ** Lower Layer Protocol (HCI) Interface module 54 ****************************************************************************** 55 ******************************************************************************/ 56 57 /* 58 * Send LP_ConnectReq event to the lower layer protocol. Create new connection 59 * descriptor and initialize it. Create LP_ConnectReq event and send it to the 60 * lower layer, then adjust connection state and start timer. The function WILL 61 * FAIL if connection to the remote unit already exists. 62 */ 63 64 int 65 ng_l2cap_lp_con_req(ng_l2cap_p l2cap, bdaddr_p bdaddr) 66 { 67 struct ng_mesg *msg = NULL; 68 ng_hci_lp_con_req_ep *ep = NULL; 69 ng_l2cap_con_p con = NULL; 70 int error = 0; 71 72 /* Verify that we DO NOT have connection to the remote unit */ 73 con = ng_l2cap_con_by_addr(l2cap, bdaddr); 74 if (con != NULL) { 75 NG_L2CAP_ALERT( 76 "%s: %s - unexpected LP_ConnectReq event. " \ 77 "Connection already exists, state=%d, con_handle=%d\n", 78 __func__, NG_NODE_NAME(l2cap->node), con->state, 79 con->con_handle); 80 81 return (EEXIST); 82 } 83 84 /* Check if lower layer protocol is still connected */ 85 if (l2cap->hci == NULL || NG_HOOK_NOT_VALID(l2cap->hci)) { 86 NG_L2CAP_ERR( 87 "%s: %s - hook \"%s\" is not connected or valid\n", 88 __func__, NG_NODE_NAME(l2cap->node), NG_L2CAP_HOOK_HCI); 89 90 return (ENOTCONN); 91 } 92 93 /* Create and intialize new connection descriptor */ 94 con = ng_l2cap_new_con(l2cap, bdaddr); 95 if (con == NULL) 96 return (ENOMEM); 97 98 /* Create and send LP_ConnectReq event */ 99 NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_CON_REQ, 100 sizeof(*ep), M_NOWAIT); 101 if (msg == NULL) { 102 ng_l2cap_free_con(con); 103 104 return (ENOMEM); 105 } 106 107 ep = (ng_hci_lp_con_req_ep *) (msg->data); 108 bcopy(bdaddr, &ep->bdaddr, sizeof(ep->bdaddr)); 109 ep->link_type = NG_HCI_LINK_ACL; 110 111 con->state = NG_L2CAP_W4_LP_CON_CFM; 112 ng_l2cap_lp_timeout(con); 113 114 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->hci, NULL); 115 if (error != 0) 116 ng_l2cap_free_con(con); /* will remove timeout */ 117 118 return (error); 119 } /* ng_l2cap_lp_con_req */ 120 121 /* 122 * Process LP_ConnectCfm event from the lower layer protocol. It could be 123 * positive or negative. Verify remote unit address then stop the timer and 124 * process event. 125 */ 126 127 int 128 ng_l2cap_lp_con_cfm(ng_l2cap_p l2cap, struct ng_mesg *msg) 129 { 130 ng_hci_lp_con_cfm_ep *ep = NULL; 131 ng_l2cap_con_p con = NULL; 132 int error = 0; 133 134 /* Check message */ 135 if (msg->header.arglen != sizeof(*ep)) { 136 NG_L2CAP_ALERT( 137 "%s: %s - invalid LP_ConnectCfm[Neg] message size\n", 138 __func__, NG_NODE_NAME(l2cap->node)); 139 error = EMSGSIZE; 140 goto out; 141 } 142 143 ep = (ng_hci_lp_con_cfm_ep *) (msg->data); 144 145 /* Check if we have requested/accepted this connection */ 146 con = ng_l2cap_con_by_addr(l2cap, &ep->bdaddr); 147 if (con == NULL) { 148 NG_L2CAP_ERR( 149 "%s: %s - unexpected LP_ConnectCfm event. Connection does not exist\n", 150 __func__, NG_NODE_NAME(l2cap->node)); 151 error = ENOENT; 152 goto out; 153 } 154 155 /* Check connection state */ 156 if (con->state != NG_L2CAP_W4_LP_CON_CFM) { 157 NG_L2CAP_ALERT( 158 "%s: %s - unexpected LP_ConnectCfm event. " \ 159 "Invalid connection state, state=%d, con_handle=%d\n", 160 __func__, NG_NODE_NAME(l2cap->node), con->state, 161 con->con_handle); 162 error = EINVAL; 163 goto out; 164 } 165 166 /* 167 * Looks like it is our confirmation. It is safe now to cancel 168 * connection timer and notify upper layer. 169 */ 170 171 ng_l2cap_lp_untimeout(con); 172 173 if (ep->status == 0) { 174 con->state = NG_L2CAP_CON_OPEN; 175 con->con_handle = ep->con_handle; 176 ng_l2cap_lp_deliver(con); 177 } else { 178 /* Negative confirmation - remove connection descriptor */ 179 con->state = NG_L2CAP_CON_CLOSED; 180 ng_l2cap_con_fail(con, ep->status); 181 } 182 out: 183 return (error); 184 } /* ng_l2cap_lp_con_cfm */ 185 186 /* 187 * Process LP_ConnectInd event from the lower layer protocol. This is a good 188 * place to put some extra check on remote unit address and/or class. We could 189 * even forward this information to control hook (or check against internal 190 * black list) and thus implement some kind of firewall. But for now be simple 191 * and create new connection descriptor, start timer and send LP_ConnectRsp 192 * event (i.e. accept connection). 193 */ 194 195 int 196 ng_l2cap_lp_con_ind(ng_l2cap_p l2cap, struct ng_mesg *msg) 197 { 198 ng_hci_lp_con_ind_ep *ep = NULL; 199 ng_hci_lp_con_rsp_ep *rp = NULL; 200 struct ng_mesg *rsp = NULL; 201 ng_l2cap_con_p con = NULL; 202 int error = 0; 203 204 /* Check message */ 205 if (msg->header.arglen != sizeof(*ep)) { 206 NG_L2CAP_ALERT( 207 "%s: %s - invalid LP_ConnectInd message size\n", 208 __func__, NG_NODE_NAME(l2cap->node)); 209 error = EMSGSIZE; 210 goto out; 211 } 212 213 ep = (ng_hci_lp_con_ind_ep *) (msg->data); 214 215 /* Make sure we have only one connection to the remote unit */ 216 con = ng_l2cap_con_by_addr(l2cap, &ep->bdaddr); 217 if (con != NULL) { 218 NG_L2CAP_ALERT( 219 "%s: %s - unexpected LP_ConnectInd event. " \ 220 "Connection already exists, state=%d, con_handle=%d\n", 221 __func__, NG_NODE_NAME(l2cap->node), con->state, 222 con->con_handle); 223 error = EEXIST; 224 goto out; 225 } 226 227 /* Check if lower layer protocol is still connected */ 228 if (l2cap->hci == NULL || NG_HOOK_NOT_VALID(l2cap->hci)) { 229 NG_L2CAP_ERR( 230 "%s: %s - hook \"%s\" is not connected or valid", 231 __func__, NG_NODE_NAME(l2cap->node), NG_L2CAP_HOOK_HCI); 232 error = ENOTCONN; 233 goto out; 234 } 235 236 /* Create and intialize new connection descriptor */ 237 con = ng_l2cap_new_con(l2cap, &ep->bdaddr); 238 if (con == NULL) { 239 error = ENOMEM; 240 goto out; 241 } 242 243 /* Create and send LP_ConnectRsp event */ 244 NG_MKMESSAGE(rsp, NGM_HCI_COOKIE, NGM_HCI_LP_CON_RSP, 245 sizeof(*rp), M_NOWAIT); 246 if (msg == NULL) { 247 ng_l2cap_free_con(con); 248 error = ENOMEM; 249 goto out; 250 } 251 252 rp = (ng_hci_lp_con_rsp_ep *)(rsp->data); 253 rp->status = 0x00; /* accept connection */ 254 rp->link_type = NG_HCI_LINK_ACL; 255 bcopy(&ep->bdaddr, &rp->bdaddr, sizeof(rp->bdaddr)); 256 257 con->state = NG_L2CAP_W4_LP_CON_CFM; 258 ng_l2cap_lp_timeout(con); 259 260 NG_SEND_MSG_HOOK(error, l2cap->node, rsp, l2cap->hci, NULL); 261 if (error != 0) 262 ng_l2cap_free_con(con); /* will remove timeout */ 263 out: 264 return (error); 265 } /* ng_hci_lp_con_ind */ 266 267 /* 268 * Process LP_DisconnectInd event from the lower layer protocol. We have been 269 * disconnected from the remote unit. So notify the upper layer protocol. 270 */ 271 272 int 273 ng_l2cap_lp_discon_ind(ng_l2cap_p l2cap, struct ng_mesg *msg) 274 { 275 ng_hci_lp_discon_ind_ep *ep = NULL; 276 ng_l2cap_con_p con = NULL; 277 int error = 0; 278 279 /* Check message */ 280 if (msg->header.arglen != sizeof(*ep)) { 281 NG_L2CAP_ALERT( 282 "%s: %s - invalid LP_DisconnectInd message size\n", 283 __func__, NG_NODE_NAME(l2cap->node)); 284 error = EMSGSIZE; 285 goto out; 286 } 287 288 ep = (ng_hci_lp_discon_ind_ep *) (msg->data); 289 290 /* Check if we have this connection */ 291 con = ng_l2cap_con_by_handle(l2cap, ep->con_handle); 292 if (con == NULL) { 293 NG_L2CAP_ERR( 294 "%s: %s - unexpected LP_DisconnectInd event. " \ 295 "Connection does not exist, con_handle=%d\n", 296 __func__, NG_NODE_NAME(l2cap->node), ep->con_handle); 297 error = ENOENT; 298 goto out; 299 } 300 301 /* XXX Verify connection state -- do we need to check this? */ 302 if (con->state != NG_L2CAP_CON_OPEN) { 303 NG_L2CAP_ERR( 304 "%s: %s - unexpected LP_DisconnectInd event. " \ 305 "Invalid connection state, state=%d, con_handle=%d\n", 306 __func__, NG_NODE_NAME(l2cap->node), con->state, 307 con->con_handle); 308 error = EINVAL; 309 goto out; 310 } 311 312 /* Notify upper layer and remove connection */ 313 con->state = NG_L2CAP_CON_CLOSED; 314 ng_l2cap_con_fail(con, ep->reason); 315 out: 316 return (error); 317 } /* ng_l2cap_lp_discon_ind */ 318 319 /* 320 * Send LP_QoSSetupReq event to the lower layer protocol 321 */ 322 323 int 324 ng_l2cap_lp_qos_req(ng_l2cap_p l2cap, u_int16_t con_handle, 325 ng_l2cap_flow_p flow) 326 { 327 struct ng_mesg *msg = NULL; 328 ng_hci_lp_qos_req_ep *ep = NULL; 329 ng_l2cap_con_p con = NULL; 330 int error = 0; 331 332 /* Verify that we have this connection */ 333 con = ng_l2cap_con_by_handle(l2cap, con_handle); 334 if (con == NULL) { 335 NG_L2CAP_ERR( 336 "%s: %s - unexpected LP_QoSSetupReq event. " \ 337 "Connection does not exist, con_handle=%d\n", 338 __func__, NG_NODE_NAME(l2cap->node), con_handle); 339 340 return (ENOENT); 341 } 342 343 /* Verify connection state */ 344 if (con->state != NG_L2CAP_CON_OPEN) { 345 NG_L2CAP_ERR( 346 "%s: %s - unexpected LP_QoSSetupReq event. " \ 347 "Invalid connection state, state=%d, con_handle=%d\n", 348 __func__, NG_NODE_NAME(l2cap->node), con->state, 349 con->con_handle); 350 351 return (EINVAL); 352 } 353 354 /* Check if lower layer protocol is still connected */ 355 if (l2cap->hci == NULL || NG_HOOK_NOT_VALID(l2cap->hci)) { 356 NG_L2CAP_ERR( 357 "%s: %s - hook \"%s\" is not connected or valid", 358 __func__, NG_NODE_NAME(l2cap->node), NG_L2CAP_HOOK_HCI); 359 360 return (ENOTCONN); 361 } 362 363 /* Create and send LP_QoSSetupReq event */ 364 NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_QOS_REQ, 365 sizeof(*ep), M_NOWAIT); 366 if (msg == NULL) 367 return (ENOMEM); 368 369 ep = (ng_hci_lp_qos_req_ep *) (msg->data); 370 ep->con_handle = con_handle; 371 ep->flags = flow->flags; 372 ep->service_type = flow->service_type; 373 ep->token_rate = flow->token_rate; 374 ep->peak_bandwidth = flow->peak_bandwidth; 375 ep->latency = flow->latency; 376 ep->delay_variation = flow->delay_variation; 377 378 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->hci, NULL); 379 380 return (error); 381 } /* ng_l2cap_lp_con_req */ 382 383 /* 384 * Process LP_QoSSetupCfm from the lower layer protocol 385 */ 386 387 int 388 ng_l2cap_lp_qos_cfm(ng_l2cap_p l2cap, struct ng_mesg *msg) 389 { 390 ng_hci_lp_qos_cfm_ep *ep = NULL; 391 int error = 0; 392 393 /* Check message */ 394 if (msg->header.arglen != sizeof(*ep)) { 395 NG_L2CAP_ALERT( 396 "%s: %s - invalid LP_QoSSetupCfm[Neg] message size\n", 397 __func__, NG_NODE_NAME(l2cap->node)); 398 error = EMSGSIZE; 399 goto out; 400 } 401 402 ep = (ng_hci_lp_qos_cfm_ep *) (msg->data); 403 /* XXX FIXME do something */ 404 out: 405 return (error); 406 } /* ng_l2cap_lp_qos_cfm */ 407 408 /* 409 * Process LP_QoSViolationInd event from the lower layer protocol. Lower 410 * layer protocol has detected QoS Violation, so we MUST notify the 411 * upper layer. 412 */ 413 414 int 415 ng_l2cap_lp_qos_ind(ng_l2cap_p l2cap, struct ng_mesg *msg) 416 { 417 ng_hci_lp_qos_ind_ep *ep = NULL; 418 ng_l2cap_con_p con = NULL; 419 int error = 0; 420 421 /* Check message */ 422 if (msg->header.arglen != sizeof(*ep)) { 423 NG_L2CAP_ALERT( 424 "%s: %s - invalid LP_QoSViolation message size\n", 425 __func__, NG_NODE_NAME(l2cap->node)); 426 error = EMSGSIZE; 427 goto out; 428 } 429 430 ep = (ng_hci_lp_qos_ind_ep *) (msg->data); 431 432 /* Check if we have this connection */ 433 con = ng_l2cap_con_by_handle(l2cap, ep->con_handle); 434 if (con == NULL) { 435 NG_L2CAP_ERR( 436 "%s: %s - unexpected LP_QoSViolationInd event. " \ 437 "Connection does not exist, con_handle=%d\n", 438 __func__, NG_NODE_NAME(l2cap->node), ep->con_handle); 439 error = ENOENT; 440 goto out; 441 } 442 443 /* Verify connection state */ 444 if (con->state != NG_L2CAP_CON_OPEN) { 445 NG_L2CAP_ERR( 446 "%s: %s - unexpected LP_QoSViolationInd event. " \ 447 "Invalid connection state, state=%d, con_handle=%d\n", 448 __func__, NG_NODE_NAME(l2cap->node), con->state, 449 con->con_handle); 450 error = EINVAL; 451 goto out; 452 } 453 454 /* XXX FIXME Notify upper layer and terminate channels if required */ 455 out: 456 return (error); 457 } /* ng_l2cap_qos_ind */ 458 459 /* 460 * Prepare L2CAP packet. Prepend packet with L2CAP packet header and then 461 * segment it according to HCI MTU. 462 */ 463 464 int 465 ng_l2cap_lp_send(ng_l2cap_con_p con, u_int16_t dcid, struct mbuf *m0) 466 { 467 ng_l2cap_p l2cap = con->l2cap; 468 ng_l2cap_hdr_t *l2cap_hdr = NULL; 469 ng_hci_acldata_pkt_t *acl_hdr = NULL; 470 struct mbuf *m_last = NULL, *m = NULL; 471 int len, flag = NG_HCI_PACKET_START; 472 473 KASSERT((con->tx_pkt == NULL), 474 ("%s: %s - another packet pending?!\n", __func__, NG_NODE_NAME(l2cap->node))); 475 KASSERT((l2cap->pkt_size > 0), 476 ("%s: %s - invalid l2cap->pkt_size?!\n", __func__, NG_NODE_NAME(l2cap->node))); 477 478 /* Prepend mbuf with L2CAP header */ 479 m0 = ng_l2cap_prepend(m0, sizeof(*l2cap_hdr)); 480 if (m0 == NULL) { 481 NG_L2CAP_ALERT( 482 "%s: %s - ng_l2cap_prepend(%d) failed\n", 483 __func__, NG_NODE_NAME(l2cap->node), 484 sizeof(*l2cap_hdr)); 485 486 goto fail; 487 } 488 489 l2cap_hdr = mtod(m0, ng_l2cap_hdr_t *); 490 l2cap_hdr->length = htole16(m0->m_pkthdr.len - sizeof(*l2cap_hdr)); 491 l2cap_hdr->dcid = htole16(dcid); 492 493 /* 494 * Segment single L2CAP packet according to the HCI layer MTU. Convert 495 * each segment into ACL data packet and prepend it with ACL data packet 496 * header. Link all segments together via m_nextpkt link. 497 * 498 * XXX BC (Broadcast flag) will always be 0 (zero). 499 */ 500 501 while (m0 != NULL) { 502 /* Check length of the packet against HCI MTU */ 503 len = m0->m_pkthdr.len; 504 if (len > l2cap->pkt_size) { 505 m = m_split(m0, l2cap->pkt_size, M_DONTWAIT); 506 if (m == NULL) { 507 NG_L2CAP_ALERT( 508 "%s: %s - m_split(%d) failed\n", __func__, NG_NODE_NAME(l2cap->node), 509 l2cap->pkt_size); 510 goto fail; 511 } 512 513 len = l2cap->pkt_size; 514 } 515 516 /* Convert packet fragment into ACL data packet */ 517 m0 = ng_l2cap_prepend(m0, sizeof(*acl_hdr)); 518 if (m0 == NULL) { 519 NG_L2CAP_ALERT( 520 "%s: %s - ng_l2cap_prepend(%d) failed\n", 521 __func__, NG_NODE_NAME(l2cap->node), 522 sizeof(*acl_hdr)); 523 goto fail; 524 } 525 526 acl_hdr = mtod(m0, ng_hci_acldata_pkt_t *); 527 acl_hdr->type = NG_HCI_ACL_DATA_PKT; 528 acl_hdr->length = htole16(len); 529 acl_hdr->con_handle = htole16(NG_HCI_MK_CON_HANDLE( 530 con->con_handle, flag, 0)); 531 532 /* Add fragment to the chain */ 533 m0->m_nextpkt = NULL; 534 535 if (con->tx_pkt == NULL) 536 con->tx_pkt = m_last = m0; 537 else { 538 m_last->m_nextpkt = m0; 539 m_last = m0; 540 } 541 542 NG_L2CAP_INFO( 543 "%s: %s - attaching ACL packet, con_handle=%d, PB=%#x, length=%d\n", 544 __func__, NG_NODE_NAME(l2cap->node), con->con_handle, 545 flag, len); 546 547 m0 = m; 548 m = NULL; 549 flag = NG_HCI_PACKET_FRAGMENT; 550 } 551 552 return (0); 553 fail: 554 NG_FREE_M(m0); 555 NG_FREE_M(m); 556 557 while (con->tx_pkt != NULL) { 558 m = con->tx_pkt->m_nextpkt; 559 m_freem(con->tx_pkt); 560 con->tx_pkt = m; 561 } 562 563 return (ENOBUFS); 564 } /* ng_l2cap_lp_send */ 565 566 /* 567 * Receive ACL data packet from the HCI layer. First strip ACL packet header 568 * and get connection handle, PB (Packet Boundary) flag and payload length. 569 * Then find connection descriptor and verify its state. Then process ACL 570 * packet as follows. 571 * 572 * 1) If we got first segment (pb == NG_HCI_PACKET_START) then extract L2CAP 573 * header and get total length of the L2CAP packet. Then start new L2CAP 574 * packet. 575 * 576 * 2) If we got other (then first :) segment (pb == NG_HCI_PACKET_FRAGMENT) 577 * then add segment to the packet. 578 */ 579 580 int 581 ng_l2cap_lp_receive(ng_l2cap_p l2cap, struct mbuf *m) 582 { 583 ng_hci_acldata_pkt_t *acl_hdr = NULL; 584 ng_l2cap_hdr_t *l2cap_hdr = NULL; 585 ng_l2cap_con_p con = NULL; 586 u_int16_t con_handle, length, pb; 587 int error = 0; 588 589 /* Check ACL data packet */ 590 if (m->m_pkthdr.len < sizeof(*acl_hdr)) { 591 NG_L2CAP_ERR( 592 "%s: %s - invalid ACL data packet. Packet too small, length=%d\n", 593 __func__, NG_NODE_NAME(l2cap->node), m->m_pkthdr.len); 594 error = EMSGSIZE; 595 goto drop; 596 } 597 598 /* Strip ACL data packet header */ 599 NG_L2CAP_M_PULLUP(m, sizeof(*acl_hdr)); 600 if (m == NULL) 601 return (ENOBUFS); 602 603 acl_hdr = mtod(m, ng_hci_acldata_pkt_t *); 604 m_adj(m, sizeof(*acl_hdr)); 605 606 /* Get ACL connection handle, PB flag and payload length */ 607 acl_hdr->con_handle = le16toh(acl_hdr->con_handle); 608 con_handle = NG_HCI_CON_HANDLE(acl_hdr->con_handle); 609 pb = NG_HCI_PB_FLAG(acl_hdr->con_handle); 610 length = le16toh(acl_hdr->length); 611 612 NG_L2CAP_INFO( 613 "%s: %s - got ACL data packet, con_handle=%d, PB=%#x, length=%d\n", 614 __func__, NG_NODE_NAME(l2cap->node), con_handle, pb, length); 615 616 /* Get connection descriptor */ 617 con = ng_l2cap_con_by_handle(l2cap, con_handle); 618 if (con == NULL) { 619 NG_L2CAP_ERR( 620 "%s: %s - unexpected ACL data packet. " \ 621 "Connection does not exist, con_handle=%d\n", 622 __func__, NG_NODE_NAME(l2cap->node), con_handle); 623 error = ENOENT; 624 goto drop; 625 } 626 627 /* Verify connection state */ 628 if (con->state != NG_L2CAP_CON_OPEN) { 629 NG_L2CAP_ERR( 630 "%s: %s - unexpected ACL data packet. Invalid connection state=%d\n", 631 __func__, NG_NODE_NAME(l2cap->node), con->state); 632 error = EHOSTDOWN; 633 goto drop; 634 } 635 636 /* Process packet */ 637 if (pb == NG_HCI_PACKET_START) { 638 if (con->rx_pkt != NULL) { 639 NG_L2CAP_ERR( 640 "%s: %s - dropping incomplete L2CAP packet, got %d bytes, want %d bytes\n", 641 __func__, NG_NODE_NAME(l2cap->node), 642 con->rx_pkt->m_pkthdr.len, con->rx_pkt_len); 643 NG_FREE_M(con->rx_pkt); 644 con->rx_pkt_len = 0; 645 } 646 647 /* Get L2CAP header */ 648 if (m->m_pkthdr.len < sizeof(*l2cap_hdr)) { 649 NG_L2CAP_ERR( 650 "%s: %s - invalid L2CAP packet start fragment. Packet too small, length=%d\n", 651 __func__, NG_NODE_NAME(l2cap->node), 652 m->m_pkthdr.len); 653 error = EMSGSIZE; 654 goto drop; 655 } 656 657 NG_L2CAP_M_PULLUP(m, sizeof(*l2cap_hdr)); 658 if (m == NULL) 659 return (ENOBUFS); 660 661 l2cap_hdr = mtod(m, ng_l2cap_hdr_t *); 662 663 NG_L2CAP_INFO( 664 "%s: %s - staring new L2CAP packet, con_handle=%d, length=%d\n", 665 __func__, NG_NODE_NAME(l2cap->node), con_handle, 666 le16toh(l2cap_hdr->length)); 667 668 /* Start new L2CAP packet */ 669 con->rx_pkt = m; 670 con->rx_pkt_len = le16toh(l2cap_hdr->length)+sizeof(*l2cap_hdr); 671 } else if (pb == NG_HCI_PACKET_FRAGMENT) { 672 if (con->rx_pkt == NULL) { 673 NG_L2CAP_ERR( 674 "%s: %s - unexpected ACL data packet fragment, con_handle=%d\n", 675 __func__, NG_NODE_NAME(l2cap->node), 676 con->con_handle); 677 goto drop; 678 } 679 680 /* Add fragment to the L2CAP packet */ 681 m_cat(con->rx_pkt, m); 682 con->rx_pkt->m_pkthdr.len += length; 683 } else { 684 NG_L2CAP_ERR( 685 "%s: %s - invalid ACL data packet. Invalid PB flag=%#x\n", 686 __func__, NG_NODE_NAME(l2cap->node), pb); 687 error = EINVAL; 688 goto drop; 689 } 690 691 con->rx_pkt_len -= length; 692 if (con->rx_pkt_len < 0) { 693 NG_L2CAP_ALERT( 694 "%s: %s - packet length mismatch. Got %d bytes, offset %d bytes\n", 695 __func__, NG_NODE_NAME(l2cap->node), 696 con->rx_pkt->m_pkthdr.len, con->rx_pkt_len); 697 NG_FREE_M(con->rx_pkt); 698 con->rx_pkt_len = 0; 699 } else if (con->rx_pkt_len == 0) { 700 /* OK, we have got complete L2CAP packet, so process it */ 701 error = ng_l2cap_receive(con); 702 con->rx_pkt = NULL; 703 con->rx_pkt_len = 0; 704 } 705 706 return (error); 707 708 drop: 709 NG_FREE_M(m); 710 711 return (error); 712 } /* ng_l2cap_lp_receive */ 713 714 /* 715 * Send queued ACL packets to the HCI layer 716 */ 717 718 void 719 ng_l2cap_lp_deliver(ng_l2cap_con_p con) 720 { 721 ng_l2cap_p l2cap = con->l2cap; 722 struct mbuf *m = NULL; 723 int error; 724 725 /* Check connection */ 726 if (con->state != NG_L2CAP_CON_OPEN) 727 return; 728 729 if (con->tx_pkt == NULL) 730 ng_l2cap_con_wakeup(con); 731 732 if (con->tx_pkt == NULL) 733 return; 734 735 /* Check if lower layer protocol is still connected */ 736 if (l2cap->hci == NULL || NG_HOOK_NOT_VALID(l2cap->hci)) { 737 NG_L2CAP_ERR( 738 "%s: %s - hook \"%s\" is not connected or valid", 739 __func__, NG_NODE_NAME(l2cap->node), NG_L2CAP_HOOK_HCI); 740 741 goto drop; /* XXX what to do with "pending"? */ 742 } 743 744 /* Send ACL data packets */ 745 while (con->pending < con->l2cap->num_pkts && con->tx_pkt != NULL) { 746 m = con->tx_pkt; 747 con->tx_pkt = con->tx_pkt->m_nextpkt; 748 m->m_nextpkt = NULL; 749 750 NG_L2CAP_INFO( 751 "%s: %s - sending ACL packet, con_handle=%d, len=%d\n", 752 __func__, NG_NODE_NAME(l2cap->node), con->con_handle, 753 m->m_pkthdr.len); 754 755 NG_SEND_DATA_ONLY(error, l2cap->hci, m); 756 if (error != 0) { 757 NG_L2CAP_ERR( 758 "%s: %s - could not send ACL data packet, con_handle=%d, error=%d\n", 759 __func__, NG_NODE_NAME(l2cap->node), 760 con->con_handle, error); 761 762 goto drop; /* XXX what to do with "pending"? */ 763 } 764 765 con->pending ++; 766 } 767 768 NG_L2CAP_INFO( 769 "%s: %s - %d ACL packets have been sent, con_handle=%d\n", 770 __func__, NG_NODE_NAME(l2cap->node), con->pending, 771 con->con_handle); 772 773 return; 774 775 drop: 776 while (con->tx_pkt != NULL) { 777 m = con->tx_pkt->m_nextpkt; 778 m_freem(con->tx_pkt); 779 con->tx_pkt = m; 780 } 781 } /* ng_l2cap_lp_deliver */ 782 783 /* 784 * Process connection timeout. Remove connection from the list. If there 785 * are any channels that wait for the connection then notify them. Free 786 * connection descriptor. 787 */ 788 789 void 790 ng_l2cap_process_lp_timeout(node_p node, hook_p hook, void *arg1, int arg2) 791 { 792 ng_l2cap_con_p con = (ng_l2cap_con_p) arg1; 793 ng_l2cap_p l2cap = con->l2cap; 794 795 NG_L2CAP_ERR( 796 "%s: %s - ACL connection timeout\n", __func__, NG_NODE_NAME(l2cap->node)); 797 798 /* 799 * Notify channels that connection has timed out. This will remove 800 * connection, channels and pending commands. 801 */ 802 803 con->state = NG_L2CAP_CON_CLOSED; 804 ng_l2cap_con_fail(con, NG_L2CAP_TIMEOUT); 805 } /* ng_l2cap_process_lp_timeout */ 806 807