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