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