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