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 int 485 ng_l2cap_lp_enc_change(ng_l2cap_p l2cap, struct ng_mesg *msg) 486 { 487 ng_hci_lp_enc_change_ep *ep = NULL; 488 ng_l2cap_con_p con = NULL; 489 int error = 0; 490 ng_l2cap_chan_p ch = NULL; 491 /* Check message */ 492 if (msg->header.arglen != sizeof(*ep)) { 493 NG_L2CAP_ALERT( 494 "%s: %s - invalid LP_ENCChange message size\n", 495 __func__, NG_NODE_NAME(l2cap->node)); 496 error = EMSGSIZE; 497 goto out; 498 } 499 500 ep = (ng_hci_lp_enc_change_ep *) (msg->data); 501 502 /* Check if we have this connection */ 503 con = ng_l2cap_con_by_handle(l2cap, ep->con_handle); 504 if (con == NULL) { 505 NG_L2CAP_ERR( 506 "%s: %s - unexpected LP_Enc Change Event. " \ 507 "Connection does not exist, con_handle=%d\n", 508 __func__, NG_NODE_NAME(l2cap->node), ep->con_handle); 509 error = ENOENT; 510 goto out; 511 } 512 513 /* Verify connection state */ 514 if (con->state != NG_L2CAP_CON_OPEN) { 515 NG_L2CAP_ERR( 516 "%s: %s - unexpected ENC_CHANGE event. " \ 517 "Invalid connection state, state=%d, con_handle=%d\n", 518 __func__, NG_NODE_NAME(l2cap->node), con->state, 519 con->con_handle); 520 error = EINVAL; 521 goto out; 522 } 523 524 con->encryption = ep->status; 525 526 LIST_FOREACH(ch, &l2cap->chan_list, next){ 527 if((ch->con->con_handle == ep->con_handle) && 528 (ch->con->linktype == ep->link_type)) 529 ng_l2cap_l2ca_encryption_change(ch, ep->status); 530 } 531 532 out: 533 return (error); 534 } /* ng_l2cap_enc_change */ 535 536 /* 537 * Prepare L2CAP packet. Prepend packet with L2CAP packet header and then 538 * segment it according to HCI MTU. 539 */ 540 541 int 542 ng_l2cap_lp_send(ng_l2cap_con_p con, u_int16_t dcid, struct mbuf *m0) 543 { 544 ng_l2cap_p l2cap = con->l2cap; 545 ng_l2cap_hdr_t *l2cap_hdr = NULL; 546 ng_hci_acldata_pkt_t *acl_hdr = NULL; 547 struct mbuf *m_last = NULL, *m = NULL; 548 int len, flag = NG_HCI_PACKET_START; 549 550 KASSERT((con->tx_pkt == NULL), 551 ("%s: %s - another packet pending?!\n", __func__, NG_NODE_NAME(l2cap->node))); 552 KASSERT((l2cap->pkt_size > 0), 553 ("%s: %s - invalid l2cap->pkt_size?!\n", __func__, NG_NODE_NAME(l2cap->node))); 554 555 /* Prepend mbuf with L2CAP header */ 556 m0 = ng_l2cap_prepend(m0, sizeof(*l2cap_hdr)); 557 if (m0 == NULL) { 558 NG_L2CAP_ALERT( 559 "%s: %s - ng_l2cap_prepend(%zd) failed\n", 560 __func__, NG_NODE_NAME(l2cap->node), 561 sizeof(*l2cap_hdr)); 562 563 goto fail; 564 } 565 566 l2cap_hdr = mtod(m0, ng_l2cap_hdr_t *); 567 l2cap_hdr->length = htole16(m0->m_pkthdr.len - sizeof(*l2cap_hdr)); 568 l2cap_hdr->dcid = htole16(dcid); 569 570 /* 571 * Segment single L2CAP packet according to the HCI layer MTU. Convert 572 * each segment into ACL data packet and prepend it with ACL data packet 573 * header. Link all segments together via m_nextpkt link. 574 * 575 * XXX BC (Broadcast flag) will always be 0 (zero). 576 */ 577 578 while (m0 != NULL) { 579 /* Check length of the packet against HCI MTU */ 580 len = m0->m_pkthdr.len; 581 if (len > l2cap->pkt_size) { 582 m = m_split(m0, l2cap->pkt_size, M_NOWAIT); 583 if (m == NULL) { 584 NG_L2CAP_ALERT( 585 "%s: %s - m_split(%d) failed\n", __func__, NG_NODE_NAME(l2cap->node), 586 l2cap->pkt_size); 587 goto fail; 588 } 589 590 len = l2cap->pkt_size; 591 } 592 593 /* Convert packet fragment into ACL data packet */ 594 m0 = ng_l2cap_prepend(m0, sizeof(*acl_hdr)); 595 if (m0 == NULL) { 596 NG_L2CAP_ALERT( 597 "%s: %s - ng_l2cap_prepend(%zd) failed\n", 598 __func__, NG_NODE_NAME(l2cap->node), 599 sizeof(*acl_hdr)); 600 goto fail; 601 } 602 603 acl_hdr = mtod(m0, ng_hci_acldata_pkt_t *); 604 acl_hdr->type = NG_HCI_ACL_DATA_PKT; 605 acl_hdr->length = htole16(len); 606 acl_hdr->con_handle = htole16(NG_HCI_MK_CON_HANDLE( 607 con->con_handle, flag, 0)); 608 609 /* Add fragment to the chain */ 610 m0->m_nextpkt = NULL; 611 612 if (con->tx_pkt == NULL) 613 con->tx_pkt = m_last = m0; 614 else { 615 m_last->m_nextpkt = m0; 616 m_last = m0; 617 } 618 619 NG_L2CAP_INFO( 620 "%s: %s - attaching ACL packet, con_handle=%d, PB=%#x, length=%d\n", 621 __func__, NG_NODE_NAME(l2cap->node), con->con_handle, 622 flag, len); 623 624 m0 = m; 625 m = NULL; 626 flag = NG_HCI_PACKET_FRAGMENT; 627 } 628 629 return (0); 630 fail: 631 NG_FREE_M(m0); 632 NG_FREE_M(m); 633 634 while (con->tx_pkt != NULL) { 635 m = con->tx_pkt->m_nextpkt; 636 m_freem(con->tx_pkt); 637 con->tx_pkt = m; 638 } 639 640 return (ENOBUFS); 641 } /* ng_l2cap_lp_send */ 642 643 /* 644 * Receive ACL data packet from the HCI layer. First strip ACL packet header 645 * and get connection handle, PB (Packet Boundary) flag and payload length. 646 * Then find connection descriptor and verify its state. Then process ACL 647 * packet as follows. 648 * 649 * 1) If we got first segment (pb == NG_HCI_PACKET_START) then extract L2CAP 650 * header and get total length of the L2CAP packet. Then start new L2CAP 651 * packet. 652 * 653 * 2) If we got other (then first :) segment (pb == NG_HCI_PACKET_FRAGMENT) 654 * then add segment to the packet. 655 */ 656 657 int 658 ng_l2cap_lp_receive(ng_l2cap_p l2cap, struct mbuf *m) 659 { 660 ng_hci_acldata_pkt_t *acl_hdr = NULL; 661 ng_l2cap_hdr_t *l2cap_hdr = NULL; 662 ng_l2cap_con_p con = NULL; 663 u_int16_t con_handle, length, pb; 664 int error = 0; 665 666 /* Check ACL data packet */ 667 if (m->m_pkthdr.len < sizeof(*acl_hdr)) { 668 NG_L2CAP_ERR( 669 "%s: %s - invalid ACL data packet. Packet too small, length=%d\n", 670 __func__, NG_NODE_NAME(l2cap->node), m->m_pkthdr.len); 671 error = EMSGSIZE; 672 goto drop; 673 } 674 675 /* Strip ACL data packet header */ 676 NG_L2CAP_M_PULLUP(m, sizeof(*acl_hdr)); 677 if (m == NULL) 678 return (ENOBUFS); 679 680 acl_hdr = mtod(m, ng_hci_acldata_pkt_t *); 681 m_adj(m, sizeof(*acl_hdr)); 682 683 /* Get ACL connection handle, PB flag and payload length */ 684 acl_hdr->con_handle = le16toh(acl_hdr->con_handle); 685 con_handle = NG_HCI_CON_HANDLE(acl_hdr->con_handle); 686 pb = NG_HCI_PB_FLAG(acl_hdr->con_handle); 687 length = le16toh(acl_hdr->length); 688 689 NG_L2CAP_INFO( 690 "%s: %s - got ACL data packet, con_handle=%d, PB=%#x, length=%d\n", 691 __func__, NG_NODE_NAME(l2cap->node), con_handle, pb, length); 692 693 /* Get connection descriptor */ 694 con = ng_l2cap_con_by_handle(l2cap, con_handle); 695 if (con == NULL) { 696 NG_L2CAP_ERR( 697 "%s: %s - unexpected ACL data packet. " \ 698 "Connection does not exist, con_handle=%d\n", 699 __func__, NG_NODE_NAME(l2cap->node), con_handle); 700 error = ENOENT; 701 goto drop; 702 } 703 704 /* Verify connection state */ 705 if (con->state != NG_L2CAP_CON_OPEN) { 706 NG_L2CAP_ERR( 707 "%s: %s - unexpected ACL data packet. Invalid connection state=%d\n", 708 __func__, NG_NODE_NAME(l2cap->node), con->state); 709 error = EHOSTDOWN; 710 goto drop; 711 } 712 713 /* Process packet */ 714 if (pb == NG_HCI_PACKET_START) { 715 if (con->rx_pkt != NULL) { 716 NG_L2CAP_ERR( 717 "%s: %s - dropping incomplete L2CAP packet, got %d bytes, want %d bytes\n", 718 __func__, NG_NODE_NAME(l2cap->node), 719 con->rx_pkt->m_pkthdr.len, con->rx_pkt_len); 720 NG_FREE_M(con->rx_pkt); 721 con->rx_pkt_len = 0; 722 } 723 724 /* Get L2CAP header */ 725 if (m->m_pkthdr.len < sizeof(*l2cap_hdr)) { 726 NG_L2CAP_ERR( 727 "%s: %s - invalid L2CAP packet start fragment. Packet too small, length=%d\n", 728 __func__, NG_NODE_NAME(l2cap->node), 729 m->m_pkthdr.len); 730 error = EMSGSIZE; 731 goto drop; 732 } 733 734 NG_L2CAP_M_PULLUP(m, sizeof(*l2cap_hdr)); 735 if (m == NULL) 736 return (ENOBUFS); 737 738 l2cap_hdr = mtod(m, ng_l2cap_hdr_t *); 739 740 NG_L2CAP_INFO( 741 "%s: %s - staring new L2CAP packet, con_handle=%d, length=%d\n", 742 __func__, NG_NODE_NAME(l2cap->node), con_handle, 743 le16toh(l2cap_hdr->length)); 744 745 /* Start new L2CAP packet */ 746 con->rx_pkt = m; 747 con->rx_pkt_len = le16toh(l2cap_hdr->length)+sizeof(*l2cap_hdr); 748 } else if (pb == NG_HCI_PACKET_FRAGMENT) { 749 if (con->rx_pkt == NULL) { 750 NG_L2CAP_ERR( 751 "%s: %s - unexpected ACL data packet fragment, con_handle=%d\n", 752 __func__, NG_NODE_NAME(l2cap->node), 753 con->con_handle); 754 goto drop; 755 } 756 757 /* Add fragment to the L2CAP packet */ 758 m_cat(con->rx_pkt, m); 759 con->rx_pkt->m_pkthdr.len += length; 760 } else { 761 NG_L2CAP_ERR( 762 "%s: %s - invalid ACL data packet. Invalid PB flag=%#x\n", 763 __func__, NG_NODE_NAME(l2cap->node), pb); 764 error = EINVAL; 765 goto drop; 766 } 767 768 con->rx_pkt_len -= length; 769 if (con->rx_pkt_len < 0) { 770 NG_L2CAP_ALERT( 771 "%s: %s - packet length mismatch. Got %d bytes, offset %d bytes\n", 772 __func__, NG_NODE_NAME(l2cap->node), 773 con->rx_pkt->m_pkthdr.len, con->rx_pkt_len); 774 NG_FREE_M(con->rx_pkt); 775 con->rx_pkt_len = 0; 776 } else if (con->rx_pkt_len == 0) { 777 /* OK, we have got complete L2CAP packet, so process it */ 778 error = ng_l2cap_receive(con); 779 con->rx_pkt = NULL; 780 con->rx_pkt_len = 0; 781 } 782 783 return (error); 784 785 drop: 786 NG_FREE_M(m); 787 788 return (error); 789 } /* ng_l2cap_lp_receive */ 790 791 /* 792 * Send queued ACL packets to the HCI layer 793 */ 794 795 void 796 ng_l2cap_lp_deliver(ng_l2cap_con_p con) 797 { 798 ng_l2cap_p l2cap = con->l2cap; 799 struct mbuf *m = NULL; 800 int error; 801 802 /* Check connection */ 803 if (con->state != NG_L2CAP_CON_OPEN) 804 return; 805 806 if (con->tx_pkt == NULL) 807 ng_l2cap_con_wakeup(con); 808 809 if (con->tx_pkt == NULL) 810 return; 811 812 /* Check if lower layer protocol is still connected */ 813 if (l2cap->hci == NULL || NG_HOOK_NOT_VALID(l2cap->hci)) { 814 NG_L2CAP_ERR( 815 "%s: %s - hook \"%s\" is not connected or valid", 816 __func__, NG_NODE_NAME(l2cap->node), NG_L2CAP_HOOK_HCI); 817 818 goto drop; /* XXX what to do with "pending"? */ 819 } 820 821 /* Send ACL data packets */ 822 while (con->pending < con->l2cap->num_pkts && con->tx_pkt != NULL) { 823 m = con->tx_pkt; 824 con->tx_pkt = con->tx_pkt->m_nextpkt; 825 m->m_nextpkt = NULL; 826 827 if(m->m_flags &M_PROTO2){ 828 ng_l2cap_lp_receive(con->l2cap, m); 829 continue; 830 } 831 NG_L2CAP_INFO( 832 "%s: %s - sending ACL packet, con_handle=%d, len=%d\n", 833 __func__, NG_NODE_NAME(l2cap->node), con->con_handle, 834 m->m_pkthdr.len); 835 836 NG_SEND_DATA_ONLY(error, l2cap->hci, m); 837 if (error != 0) { 838 NG_L2CAP_ERR( 839 "%s: %s - could not send ACL data packet, con_handle=%d, error=%d\n", 840 __func__, NG_NODE_NAME(l2cap->node), 841 con->con_handle, error); 842 843 goto drop; /* XXX what to do with "pending"? */ 844 } 845 846 con->pending ++; 847 } 848 849 NG_L2CAP_INFO( 850 "%s: %s - %d ACL packets have been sent, con_handle=%d\n", 851 __func__, NG_NODE_NAME(l2cap->node), con->pending, 852 con->con_handle); 853 854 return; 855 856 drop: 857 while (con->tx_pkt != NULL) { 858 m = con->tx_pkt->m_nextpkt; 859 m_freem(con->tx_pkt); 860 con->tx_pkt = m; 861 } 862 } /* ng_l2cap_lp_deliver */ 863 864 /* 865 * Process connection timeout. Remove connection from the list. If there 866 * are any channels that wait for the connection then notify them. Free 867 * connection descriptor. 868 */ 869 870 void 871 ng_l2cap_process_lp_timeout(node_p node, hook_p hook, void *arg1, int con_handle) 872 { 873 ng_l2cap_p l2cap = NULL; 874 ng_l2cap_con_p con = NULL; 875 876 if (NG_NODE_NOT_VALID(node)) { 877 printf("%s: Netgraph node is not valid\n", __func__); 878 return; 879 } 880 881 l2cap = (ng_l2cap_p) NG_NODE_PRIVATE(node); 882 con = ng_l2cap_con_by_handle(l2cap, con_handle); 883 884 if (con == NULL) { 885 NG_L2CAP_ALERT( 886 "%s: %s - could not find connection, con_handle=%d\n", 887 __func__, NG_NODE_NAME(node), con_handle); 888 return; 889 } 890 891 if (!(con->flags & NG_L2CAP_CON_LP_TIMO)) { 892 NG_L2CAP_ALERT( 893 "%s: %s - no pending LP timeout, con_handle=%d, state=%d, flags=%#x\n", 894 __func__, NG_NODE_NAME(node), con_handle, con->state, 895 con->flags); 896 return; 897 } 898 899 /* 900 * Notify channels that connection has timed out. This will remove 901 * connection, channels and pending commands. 902 */ 903 904 con->flags &= ~NG_L2CAP_CON_LP_TIMO; 905 ng_l2cap_con_fail(con, NG_L2CAP_TIMEOUT); 906 } /* ng_l2cap_process_lp_timeout */ 907 908 /* 909 * Process auto disconnect timeout and send LP_DisconReq event to the 910 * lower layer protocol 911 */ 912 913 void 914 ng_l2cap_process_discon_timeout(node_p node, hook_p hook, void *arg1, int con_handle) 915 { 916 ng_l2cap_p l2cap = NULL; 917 ng_l2cap_con_p con = NULL; 918 struct ng_mesg *msg = NULL; 919 ng_hci_lp_discon_req_ep *ep = NULL; 920 int error; 921 922 if (NG_NODE_NOT_VALID(node)) { 923 printf("%s: Netgraph node is not valid\n", __func__); 924 return; 925 } 926 927 l2cap = (ng_l2cap_p) NG_NODE_PRIVATE(node); 928 con = ng_l2cap_con_by_handle(l2cap, con_handle); 929 930 if (con == NULL) { 931 NG_L2CAP_ALERT( 932 "%s: %s - could not find connection, con_handle=%d\n", 933 __func__, NG_NODE_NAME(node), con_handle); 934 return; 935 } 936 937 if (!(con->flags & NG_L2CAP_CON_AUTO_DISCON_TIMO)) { 938 NG_L2CAP_ALERT( 939 "%s: %s - no pending disconnect timeout, con_handle=%d, state=%d, flags=%#x\n", 940 __func__, NG_NODE_NAME(node), con_handle, con->state, 941 con->flags); 942 return; 943 } 944 945 con->flags &= ~NG_L2CAP_CON_AUTO_DISCON_TIMO; 946 947 /* Check if lower layer protocol is still connected */ 948 if (l2cap->hci == NULL || NG_HOOK_NOT_VALID(l2cap->hci)) { 949 NG_L2CAP_ERR( 950 "%s: %s - hook \"%s\" is not connected or valid\n", 951 __func__, NG_NODE_NAME(l2cap->node), NG_L2CAP_HOOK_HCI); 952 return; 953 } 954 955 /* Create and send LP_DisconReq event */ 956 NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_DISCON_REQ, 957 sizeof(*ep), M_NOWAIT); 958 if (msg == NULL) 959 return; 960 961 ep = (ng_hci_lp_discon_req_ep *) (msg->data); 962 ep->con_handle = con->con_handle; 963 ep->reason = 0x13; /* User Ended Connection */ 964 965 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->hci, 0); 966 } /* ng_l2cap_process_discon_timeout */ 967 968