1 /* 2 * ng_l2cap_llpi.c 3 */ 4 5 /*- 6 * SPDX-License-Identifier: BSD-2-Clause 7 * 8 * Copyright (c) Maksim Yevmenkin <m_evmenkin@yahoo.com> 9 * All rights reserved. 10 * 11 * Redistribution and use in source and binary forms, with or without 12 * modification, are permitted provided that the following conditions 13 * are met: 14 * 1. Redistributions of source code must retain the above copyright 15 * notice, this list of conditions and the following disclaimer. 16 * 2. Redistributions in binary form must reproduce the above copyright 17 * notice, this list of conditions and the following disclaimer in the 18 * documentation and/or other materials provided with the distribution. 19 * 20 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 21 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 24 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 26 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 27 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 29 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 30 * SUCH DAMAGE. 31 * 32 * $Id: ng_l2cap_llpi.c,v 1.5 2003/09/08 19:11:45 max Exp $ 33 */ 34 35 #include <sys/param.h> 36 #include <sys/systm.h> 37 #include <sys/kernel.h> 38 #include <sys/endian.h> 39 #include <sys/malloc.h> 40 #include <sys/mbuf.h> 41 #include <sys/queue.h> 42 #include <netgraph/ng_message.h> 43 #include <netgraph/netgraph.h> 44 #include <netgraph/bluetooth/include/ng_bluetooth.h> 45 #include <netgraph/bluetooth/include/ng_hci.h> 46 #include <netgraph/bluetooth/include/ng_l2cap.h> 47 #include <netgraph/bluetooth/l2cap/ng_l2cap_var.h> 48 #include <netgraph/bluetooth/l2cap/ng_l2cap_cmds.h> 49 #include <netgraph/bluetooth/l2cap/ng_l2cap_evnt.h> 50 #include <netgraph/bluetooth/l2cap/ng_l2cap_llpi.h> 51 #include <netgraph/bluetooth/l2cap/ng_l2cap_ulpi.h> 52 #include <netgraph/bluetooth/l2cap/ng_l2cap_misc.h> 53 54 /****************************************************************************** 55 ****************************************************************************** 56 ** Lower Layer Protocol (HCI) Interface module 57 ****************************************************************************** 58 ******************************************************************************/ 59 60 /* 61 * Send LP_ConnectReq event to the lower layer protocol. Create new connection 62 * descriptor and initialize it. Create LP_ConnectReq event and send it to the 63 * lower layer, then adjust connection state and start timer. The function WILL 64 * FAIL if connection to the remote unit already exists. 65 */ 66 67 int 68 ng_l2cap_lp_con_req(ng_l2cap_p l2cap, bdaddr_p bdaddr, int type) 69 { 70 struct ng_mesg *msg = NULL; 71 ng_hci_lp_con_req_ep *ep = NULL; 72 ng_l2cap_con_p con = NULL; 73 int error = 0; 74 75 /* Verify that we DO NOT have connection to the remote unit */ 76 con = ng_l2cap_con_by_addr(l2cap, bdaddr, type); 77 if (con != NULL) { 78 NG_L2CAP_ALERT( 79 "%s: %s - unexpected LP_ConnectReq event. " \ 80 "Connection already exists, state=%d, con_handle=%d\n", 81 __func__, NG_NODE_NAME(l2cap->node), con->state, 82 con->con_handle); 83 84 return (EEXIST); 85 } 86 87 /* Check if lower layer protocol is still connected */ 88 if (l2cap->hci == NULL || NG_HOOK_NOT_VALID(l2cap->hci)) { 89 NG_L2CAP_ERR( 90 "%s: %s - hook \"%s\" is not connected or valid\n", 91 __func__, NG_NODE_NAME(l2cap->node), NG_L2CAP_HOOK_HCI); 92 93 return (ENOTCONN); 94 } 95 96 /* Create and intialize new connection descriptor */ 97 con = ng_l2cap_new_con(l2cap, bdaddr, type); 98 if (con == NULL) 99 return (ENOMEM); 100 101 /* Create and send LP_ConnectReq event */ 102 NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_CON_REQ, 103 sizeof(*ep), M_NOWAIT); 104 if (msg == NULL) { 105 ng_l2cap_free_con(con); 106 107 return (ENOMEM); 108 } 109 110 ep = (ng_hci_lp_con_req_ep *) (msg->data); 111 bcopy(bdaddr, &ep->bdaddr, sizeof(ep->bdaddr)); 112 ep->link_type = type; 113 114 con->flags |= NG_L2CAP_CON_OUTGOING; 115 con->state = NG_L2CAP_W4_LP_CON_CFM; 116 ng_l2cap_lp_timeout(con); 117 118 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->hci, 0); 119 if (error != 0) { 120 if (ng_l2cap_lp_untimeout(con) == 0) 121 ng_l2cap_free_con(con); 122 123 /* 124 * Do not free connection if ng_l2cap_lp_untimeout() failed 125 * let timeout handler deal with it. Always return error to 126 * the caller. 127 */ 128 } 129 130 return (error); 131 } /* ng_l2cap_lp_con_req */ 132 133 /* 134 * Process LP_ConnectCfm event from the lower layer protocol. It could be 135 * positive or negative. Verify remote unit address then stop the timer and 136 * process event. 137 */ 138 139 int 140 ng_l2cap_lp_con_cfm(ng_l2cap_p l2cap, struct ng_mesg *msg) 141 { 142 ng_hci_lp_con_cfm_ep *ep = NULL; 143 ng_l2cap_con_p con = NULL; 144 int error = 0; 145 146 /* Check message */ 147 if (msg->header.arglen != sizeof(*ep)) { 148 NG_L2CAP_ALERT( 149 "%s: %s - invalid LP_ConnectCfm[Neg] message size\n", 150 __func__, NG_NODE_NAME(l2cap->node)); 151 error = EMSGSIZE; 152 goto out; 153 } 154 155 ep = (ng_hci_lp_con_cfm_ep *) (msg->data); 156 /* Check if we have requested/accepted this connection */ 157 con = ng_l2cap_con_by_addr(l2cap, &ep->bdaddr, ep->link_type); 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, ep->link_type); 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, ep->link_type); 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 int 486 ng_l2cap_lp_enc_change(ng_l2cap_p l2cap, struct ng_mesg *msg) 487 { 488 ng_hci_lp_enc_change_ep *ep = NULL; 489 ng_l2cap_con_p con = NULL; 490 int error = 0; 491 ng_l2cap_chan_p ch = NULL; 492 /* Check message */ 493 if (msg->header.arglen != sizeof(*ep)) { 494 NG_L2CAP_ALERT( 495 "%s: %s - invalid LP_ENCChange message size\n", 496 __func__, NG_NODE_NAME(l2cap->node)); 497 error = EMSGSIZE; 498 goto out; 499 } 500 501 ep = (ng_hci_lp_enc_change_ep *) (msg->data); 502 503 /* Check if we have this connection */ 504 con = ng_l2cap_con_by_handle(l2cap, ep->con_handle); 505 if (con == NULL) { 506 NG_L2CAP_ERR( 507 "%s: %s - unexpected LP_Enc Change Event. " \ 508 "Connection does not exist, con_handle=%d\n", 509 __func__, NG_NODE_NAME(l2cap->node), ep->con_handle); 510 error = ENOENT; 511 goto out; 512 } 513 514 /* Verify connection state */ 515 if (con->state != NG_L2CAP_CON_OPEN) { 516 NG_L2CAP_ERR( 517 "%s: %s - unexpected ENC_CHANGE event. " \ 518 "Invalid connection state, state=%d, con_handle=%d\n", 519 __func__, NG_NODE_NAME(l2cap->node), con->state, 520 con->con_handle); 521 error = EINVAL; 522 goto out; 523 } 524 525 con->encryption = ep->status; 526 527 LIST_FOREACH(ch, &l2cap->chan_list, next){ 528 if((ch->con->con_handle == ep->con_handle) && 529 (ch->con->linktype == ep->link_type)) 530 ng_l2cap_l2ca_encryption_change(ch, ep->status); 531 } 532 533 out: 534 return (error); 535 } /* ng_l2cap_enc_change */ 536 537 /* 538 * Prepare L2CAP packet. Prepend packet with L2CAP packet header and then 539 * segment it according to HCI MTU. 540 */ 541 542 int 543 ng_l2cap_lp_send(ng_l2cap_con_p con, u_int16_t dcid, struct mbuf *m0) 544 { 545 ng_l2cap_p l2cap = con->l2cap; 546 ng_l2cap_hdr_t *l2cap_hdr = NULL; 547 ng_hci_acldata_pkt_t *acl_hdr = NULL; 548 struct mbuf *m_last = NULL, *m = NULL; 549 int len, flag = (con->linktype == NG_HCI_LINK_ACL) ? NG_HCI_PACKET_START : NG_HCI_LE_PACKET_START; 550 551 KASSERT((con->tx_pkt == NULL), 552 ("%s: %s - another packet pending?!\n", __func__, NG_NODE_NAME(l2cap->node))); 553 KASSERT((l2cap->pkt_size > 0), 554 ("%s: %s - invalid l2cap->pkt_size?!\n", __func__, NG_NODE_NAME(l2cap->node))); 555 556 /* Prepend mbuf with L2CAP header */ 557 m0 = ng_l2cap_prepend(m0, sizeof(*l2cap_hdr)); 558 if (m0 == NULL) { 559 NG_L2CAP_ALERT( 560 "%s: %s - ng_l2cap_prepend(%zd) failed\n", 561 __func__, NG_NODE_NAME(l2cap->node), 562 sizeof(*l2cap_hdr)); 563 564 goto fail; 565 } 566 567 l2cap_hdr = mtod(m0, ng_l2cap_hdr_t *); 568 l2cap_hdr->length = htole16(m0->m_pkthdr.len - sizeof(*l2cap_hdr)); 569 l2cap_hdr->dcid = htole16(dcid); 570 571 /* 572 * Segment single L2CAP packet according to the HCI layer MTU. Convert 573 * each segment into ACL data packet and prepend it with ACL data packet 574 * header. Link all segments together via m_nextpkt link. 575 * 576 * XXX BC (Broadcast flag) will always be 0 (zero). 577 */ 578 579 while (m0 != NULL) { 580 /* Check length of the packet against HCI MTU */ 581 len = m0->m_pkthdr.len; 582 if (len > l2cap->pkt_size) { 583 m = m_split(m0, l2cap->pkt_size, M_NOWAIT); 584 if (m == NULL) { 585 NG_L2CAP_ALERT( 586 "%s: %s - m_split(%d) failed\n", __func__, NG_NODE_NAME(l2cap->node), 587 l2cap->pkt_size); 588 goto fail; 589 } 590 591 len = l2cap->pkt_size; 592 } 593 594 /* Convert packet fragment into ACL data packet */ 595 m0 = ng_l2cap_prepend(m0, sizeof(*acl_hdr)); 596 if (m0 == NULL) { 597 NG_L2CAP_ALERT( 598 "%s: %s - ng_l2cap_prepend(%zd) failed\n", 599 __func__, NG_NODE_NAME(l2cap->node), 600 sizeof(*acl_hdr)); 601 goto fail; 602 } 603 604 acl_hdr = mtod(m0, ng_hci_acldata_pkt_t *); 605 acl_hdr->type = NG_HCI_ACL_DATA_PKT; 606 acl_hdr->length = htole16(len); 607 acl_hdr->con_handle = htole16(NG_HCI_MK_CON_HANDLE( 608 con->con_handle, flag, 0)); 609 610 /* Add fragment to the chain */ 611 m0->m_nextpkt = NULL; 612 613 if (con->tx_pkt == NULL) 614 con->tx_pkt = m_last = m0; 615 else { 616 m_last->m_nextpkt = m0; 617 m_last = m0; 618 } 619 620 NG_L2CAP_INFO( 621 "%s: %s - attaching ACL packet, con_handle=%d, PB=%#x, length=%d\n", 622 __func__, NG_NODE_NAME(l2cap->node), con->con_handle, 623 flag, len); 624 625 m0 = m; 626 m = NULL; 627 flag = NG_HCI_PACKET_FRAGMENT; 628 } 629 630 return (0); 631 fail: 632 NG_FREE_M(m0); 633 NG_FREE_M(m); 634 635 while (con->tx_pkt != NULL) { 636 m = con->tx_pkt->m_nextpkt; 637 m_freem(con->tx_pkt); 638 con->tx_pkt = m; 639 } 640 641 return (ENOBUFS); 642 } /* ng_l2cap_lp_send */ 643 644 /* 645 * Receive ACL data packet from the HCI layer. First strip ACL packet header 646 * and get connection handle, PB (Packet Boundary) flag and payload length. 647 * Then find connection descriptor and verify its state. Then process ACL 648 * packet as follows. 649 * 650 * 1) If we got first segment (pb == NG_HCI_PACKET_START) then extract L2CAP 651 * header and get total length of the L2CAP packet. Then start new L2CAP 652 * packet. 653 * 654 * 2) If we got other (then first :) segment (pb == NG_HCI_PACKET_FRAGMENT) 655 * then add segment to the packet. 656 */ 657 658 int 659 ng_l2cap_lp_receive(ng_l2cap_p l2cap, struct mbuf *m) 660 { 661 ng_hci_acldata_pkt_t *acl_hdr = NULL; 662 ng_l2cap_hdr_t *l2cap_hdr = NULL; 663 ng_l2cap_con_p con = NULL; 664 u_int16_t con_handle, length, pb; 665 int error = 0; 666 667 /* Check ACL data packet */ 668 if (m->m_pkthdr.len < sizeof(*acl_hdr)) { 669 NG_L2CAP_ERR( 670 "%s: %s - invalid ACL data packet. Packet too small, length=%d\n", 671 __func__, NG_NODE_NAME(l2cap->node), m->m_pkthdr.len); 672 error = EMSGSIZE; 673 goto drop; 674 } 675 676 /* Strip ACL data packet header */ 677 NG_L2CAP_M_PULLUP(m, sizeof(*acl_hdr)); 678 if (m == NULL) 679 return (ENOBUFS); 680 681 acl_hdr = mtod(m, ng_hci_acldata_pkt_t *); 682 m_adj(m, sizeof(*acl_hdr)); 683 684 /* Get ACL connection handle, PB flag and payload length */ 685 acl_hdr->con_handle = le16toh(acl_hdr->con_handle); 686 con_handle = NG_HCI_CON_HANDLE(acl_hdr->con_handle); 687 pb = NG_HCI_PB_FLAG(acl_hdr->con_handle); 688 length = le16toh(acl_hdr->length); 689 690 NG_L2CAP_INFO( 691 "%s: %s - got ACL data packet, con_handle=%d, PB=%#x, length=%d\n", 692 __func__, NG_NODE_NAME(l2cap->node), con_handle, pb, length); 693 694 /* Get connection descriptor */ 695 con = ng_l2cap_con_by_handle(l2cap, con_handle); 696 if (con == NULL) { 697 NG_L2CAP_ERR( 698 "%s: %s - unexpected ACL data packet. " \ 699 "Connection does not exist, con_handle=%d\n", 700 __func__, NG_NODE_NAME(l2cap->node), con_handle); 701 error = ENOENT; 702 goto drop; 703 } 704 705 /* Verify connection state */ 706 if (con->state != NG_L2CAP_CON_OPEN) { 707 NG_L2CAP_ERR( 708 "%s: %s - unexpected ACL data packet. Invalid connection state=%d\n", 709 __func__, NG_NODE_NAME(l2cap->node), con->state); 710 error = EHOSTDOWN; 711 goto drop; 712 } 713 714 /* Process packet */ 715 if ((pb == NG_HCI_PACKET_START) || (pb == NG_HCI_LE_PACKET_START)) 716 { 717 if (con->rx_pkt != NULL) { 718 NG_L2CAP_ERR( 719 "%s: %s - dropping incomplete L2CAP packet, got %d bytes, want %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 } 725 726 /* Get L2CAP header */ 727 if (m->m_pkthdr.len < sizeof(*l2cap_hdr)) { 728 NG_L2CAP_ERR( 729 "%s: %s - invalid L2CAP packet start fragment. Packet too small, length=%d\n", 730 __func__, NG_NODE_NAME(l2cap->node), 731 m->m_pkthdr.len); 732 error = EMSGSIZE; 733 goto drop; 734 } 735 736 NG_L2CAP_M_PULLUP(m, sizeof(*l2cap_hdr)); 737 if (m == NULL) 738 return (ENOBUFS); 739 740 l2cap_hdr = mtod(m, ng_l2cap_hdr_t *); 741 742 NG_L2CAP_INFO( 743 "%s: %s - staring new L2CAP packet, con_handle=%d, length=%d\n", 744 __func__, NG_NODE_NAME(l2cap->node), con_handle, 745 le16toh(l2cap_hdr->length)); 746 747 /* Start new L2CAP packet */ 748 con->rx_pkt = m; 749 con->rx_pkt_len = le16toh(l2cap_hdr->length)+sizeof(*l2cap_hdr); 750 } else if (pb == NG_HCI_PACKET_FRAGMENT) { 751 if (con->rx_pkt == NULL) { 752 NG_L2CAP_ERR( 753 "%s: %s - unexpected ACL data packet fragment, con_handle=%d\n", 754 __func__, NG_NODE_NAME(l2cap->node), 755 con->con_handle); 756 goto drop; 757 } 758 759 /* Add fragment to the L2CAP packet */ 760 m_cat(con->rx_pkt, m); 761 con->rx_pkt->m_pkthdr.len += length; 762 } else { 763 NG_L2CAP_ERR( 764 "%s: %s - invalid ACL data packet. Invalid PB flag=%#x\n", 765 __func__, NG_NODE_NAME(l2cap->node), pb); 766 error = EINVAL; 767 goto drop; 768 } 769 770 con->rx_pkt_len -= length; 771 if (con->rx_pkt_len < 0) { 772 NG_L2CAP_ALERT( 773 "%s: %s - packet length mismatch. Got %d bytes, offset %d bytes\n", 774 __func__, NG_NODE_NAME(l2cap->node), 775 con->rx_pkt->m_pkthdr.len, con->rx_pkt_len); 776 NG_FREE_M(con->rx_pkt); 777 con->rx_pkt_len = 0; 778 } else if (con->rx_pkt_len == 0) { 779 /* OK, we have got complete L2CAP packet, so process it */ 780 error = ng_l2cap_receive(con); 781 con->rx_pkt = NULL; 782 con->rx_pkt_len = 0; 783 } 784 785 return (error); 786 787 drop: 788 NG_FREE_M(m); 789 790 return (error); 791 } /* ng_l2cap_lp_receive */ 792 793 /* 794 * Send queued ACL packets to the HCI layer 795 */ 796 797 void 798 ng_l2cap_lp_deliver(ng_l2cap_con_p con) 799 { 800 ng_l2cap_p l2cap = con->l2cap; 801 struct mbuf *m = NULL; 802 int error; 803 804 /* Check connection */ 805 if (con->state != NG_L2CAP_CON_OPEN) 806 return; 807 808 if (con->tx_pkt == NULL) 809 ng_l2cap_con_wakeup(con); 810 811 if (con->tx_pkt == NULL) 812 return; 813 814 /* Check if lower layer protocol is still connected */ 815 if (l2cap->hci == NULL || NG_HOOK_NOT_VALID(l2cap->hci)) { 816 NG_L2CAP_ERR( 817 "%s: %s - hook \"%s\" is not connected or valid", 818 __func__, NG_NODE_NAME(l2cap->node), NG_L2CAP_HOOK_HCI); 819 820 goto drop; /* XXX what to do with "pending"? */ 821 } 822 823 /* Send ACL data packets */ 824 while (con->pending < con->l2cap->num_pkts && con->tx_pkt != NULL) { 825 m = con->tx_pkt; 826 con->tx_pkt = con->tx_pkt->m_nextpkt; 827 m->m_nextpkt = NULL; 828 829 if(m->m_flags &M_PROTO2){ 830 ng_l2cap_lp_receive(con->l2cap, m); 831 continue; 832 } 833 NG_L2CAP_INFO( 834 "%s: %s - sending ACL packet, con_handle=%d, len=%d\n", 835 __func__, NG_NODE_NAME(l2cap->node), con->con_handle, 836 m->m_pkthdr.len); 837 838 NG_SEND_DATA_ONLY(error, l2cap->hci, m); 839 if (error != 0) { 840 NG_L2CAP_ERR( 841 "%s: %s - could not send ACL data packet, con_handle=%d, error=%d\n", 842 __func__, NG_NODE_NAME(l2cap->node), 843 con->con_handle, error); 844 845 goto drop; /* XXX what to do with "pending"? */ 846 } 847 848 con->pending ++; 849 } 850 851 NG_L2CAP_INFO( 852 "%s: %s - %d ACL packets have been sent, con_handle=%d\n", 853 __func__, NG_NODE_NAME(l2cap->node), con->pending, 854 con->con_handle); 855 856 return; 857 858 drop: 859 while (con->tx_pkt != NULL) { 860 m = con->tx_pkt->m_nextpkt; 861 m_freem(con->tx_pkt); 862 con->tx_pkt = m; 863 } 864 } /* ng_l2cap_lp_deliver */ 865 866 /* 867 * Process connection timeout. Remove connection from the list. If there 868 * are any channels that wait for the connection then notify them. Free 869 * connection descriptor. 870 */ 871 872 void 873 ng_l2cap_process_lp_timeout(node_p node, hook_p hook, void *arg1, int con_handle) 874 { 875 ng_l2cap_p l2cap = NULL; 876 ng_l2cap_con_p con = NULL; 877 878 if (NG_NODE_NOT_VALID(node)) { 879 printf("%s: Netgraph node is not valid\n", __func__); 880 return; 881 } 882 883 l2cap = (ng_l2cap_p) NG_NODE_PRIVATE(node); 884 con = ng_l2cap_con_by_handle(l2cap, con_handle); 885 886 if (con == NULL) { 887 NG_L2CAP_ALERT( 888 "%s: %s - could not find connection, con_handle=%d\n", 889 __func__, NG_NODE_NAME(node), con_handle); 890 return; 891 } 892 893 if (!(con->flags & NG_L2CAP_CON_LP_TIMO)) { 894 NG_L2CAP_ALERT( 895 "%s: %s - no pending LP timeout, con_handle=%d, state=%d, flags=%#x\n", 896 __func__, NG_NODE_NAME(node), con_handle, con->state, 897 con->flags); 898 return; 899 } 900 901 /* 902 * Notify channels that connection has timed out. This will remove 903 * connection, channels and pending commands. 904 */ 905 906 con->flags &= ~NG_L2CAP_CON_LP_TIMO; 907 ng_l2cap_con_fail(con, NG_L2CAP_TIMEOUT); 908 } /* ng_l2cap_process_lp_timeout */ 909 910 /* 911 * Process auto disconnect timeout and send LP_DisconReq event to the 912 * lower layer protocol 913 */ 914 915 void 916 ng_l2cap_process_discon_timeout(node_p node, hook_p hook, void *arg1, int con_handle) 917 { 918 ng_l2cap_p l2cap = NULL; 919 ng_l2cap_con_p con = NULL; 920 struct ng_mesg *msg = NULL; 921 ng_hci_lp_discon_req_ep *ep = NULL; 922 int error; 923 924 if (NG_NODE_NOT_VALID(node)) { 925 printf("%s: Netgraph node is not valid\n", __func__); 926 return; 927 } 928 929 l2cap = (ng_l2cap_p) NG_NODE_PRIVATE(node); 930 con = ng_l2cap_con_by_handle(l2cap, con_handle); 931 932 if (con == NULL) { 933 NG_L2CAP_ALERT( 934 "%s: %s - could not find connection, con_handle=%d\n", 935 __func__, NG_NODE_NAME(node), con_handle); 936 return; 937 } 938 939 if (!(con->flags & NG_L2CAP_CON_AUTO_DISCON_TIMO)) { 940 NG_L2CAP_ALERT( 941 "%s: %s - no pending disconnect timeout, con_handle=%d, state=%d, flags=%#x\n", 942 __func__, NG_NODE_NAME(node), con_handle, con->state, 943 con->flags); 944 return; 945 } 946 947 con->flags &= ~NG_L2CAP_CON_AUTO_DISCON_TIMO; 948 949 /* Check if lower layer protocol is still connected */ 950 if (l2cap->hci == NULL || NG_HOOK_NOT_VALID(l2cap->hci)) { 951 NG_L2CAP_ERR( 952 "%s: %s - hook \"%s\" is not connected or valid\n", 953 __func__, NG_NODE_NAME(l2cap->node), NG_L2CAP_HOOK_HCI); 954 return; 955 } 956 957 /* Create and send LP_DisconReq event */ 958 NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_DISCON_REQ, 959 sizeof(*ep), M_NOWAIT); 960 if (msg == NULL) 961 return; 962 963 ep = (ng_hci_lp_discon_req_ep *) (msg->data); 964 ep->con_handle = con->con_handle; 965 ep->reason = 0x13; /* User Ended Connection */ 966 967 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->hci, 0); 968 } /* ng_l2cap_process_discon_timeout */ 969