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