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