diff options
Diffstat (limited to 'sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.c')
| -rw-r--r-- | sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.c | 806 |
1 files changed, 806 insertions, 0 deletions
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.c b/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.c new file mode 100644 index 0000000..f505158 --- /dev/null +++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.c @@ -0,0 +1,806 @@ +/* + * ng_l2cap_llpi.c + * + * Copyright (c) Maksim Yevmenkin <m_evmenkin@yahoo.com> + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + * SUCH DAMAGE. + * + * $Id: ng_l2cap_llpi.c,v 1.16 2002/09/04 21:38:38 max Exp $ + * $FreeBSD$ + */ + +#include <sys/param.h> +#include <sys/systm.h> +#include <sys/kernel.h> +#include <sys/endian.h> +#include <sys/malloc.h> +#include <sys/mbuf.h> +#include <sys/queue.h> +#include <netgraph/ng_message.h> +#include <netgraph/netgraph.h> +#include "ng_bluetooth.h" +#include "ng_hci.h" +#include "ng_l2cap.h" +#include "ng_l2cap_var.h" +#include "ng_l2cap_cmds.h" +#include "ng_l2cap_evnt.h" +#include "ng_l2cap_llpi.h" +#include "ng_l2cap_ulpi.h" +#include "ng_l2cap_misc.h" + +/****************************************************************************** + ****************************************************************************** + ** Lower Layer Protocol (HCI) Interface module + ****************************************************************************** + ******************************************************************************/ + +/* + * Send LP_ConnectReq event to the lower layer protocol. Create new connection + * descriptor and initialize it. Create LP_ConnectReq event and send it to the + * lower layer, then adjust connection state and start timer. The function WILL + * FAIL if connection to the remote unit already exists. + */ + +int +ng_l2cap_lp_con_req(ng_l2cap_p l2cap, bdaddr_p bdaddr) +{ + struct ng_mesg *msg = NULL; + ng_hci_lp_con_req_ep *ep = NULL; + ng_l2cap_con_p con = NULL; + int error = 0; + + /* Verify that we DO NOT have connection to the remote unit */ + con = ng_l2cap_con_by_addr(l2cap, bdaddr); + if (con != NULL) { + NG_L2CAP_ALERT( +"%s: %s - unexpected LP_ConnectReq event. " \ +"Connection already exists, state=%d, con_handle=%d\n", + __func__, NG_NODE_NAME(l2cap->node), con->state, + con->con_handle); + + return (EEXIST); + } + + /* Check if lower layer protocol is still connected */ + if (l2cap->hci == NULL || NG_HOOK_NOT_VALID(l2cap->hci)) { + NG_L2CAP_ERR( +"%s: %s - hook \"%s\" is not connected or valid\n", + __func__, NG_NODE_NAME(l2cap->node), NG_L2CAP_HOOK_HCI); + + return (ENOTCONN); + } + + /* Create and intialize new connection descriptor */ + con = ng_l2cap_new_con(l2cap, bdaddr); + if (con == NULL) + return (ENOMEM); + + /* Create and send LP_ConnectReq event */ + NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_CON_REQ, + sizeof(*ep), M_NOWAIT); + if (msg == NULL) { + ng_l2cap_free_con(con); + + return (ENOMEM); + } + + ep = (ng_hci_lp_con_req_ep *) (msg->data); + bcopy(bdaddr, &ep->bdaddr, sizeof(ep->bdaddr)); + ep->link_type = NG_HCI_LINK_ACL; + + con->state = NG_L2CAP_W4_LP_CON_CFM; + ng_l2cap_lp_timeout(con); + + NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->hci, NULL); + if (error != 0) + ng_l2cap_free_con(con); /* will remove timeout */ + + return (error); +} /* ng_l2cap_lp_con_req */ + +/* + * Process LP_ConnectCfm event from the lower layer protocol. It could be + * positive or negative. Verify remote unit address then stop the timer and + * process event. + */ + +int +ng_l2cap_lp_con_cfm(ng_l2cap_p l2cap, struct ng_mesg *msg) +{ + ng_hci_lp_con_cfm_ep *ep = NULL; + ng_l2cap_con_p con = NULL; + int error = 0; + + /* Check message */ + if (msg->header.arglen != sizeof(*ep)) { + NG_L2CAP_ALERT( +"%s: %s - invalid LP_ConnectCfm[Neg] message size\n", + __func__, NG_NODE_NAME(l2cap->node)); + error = EMSGSIZE; + goto out; + } + + ep = (ng_hci_lp_con_cfm_ep *) (msg->data); + + /* Check if we have requested/accepted this connection */ + con = ng_l2cap_con_by_addr(l2cap, &ep->bdaddr); + if (con == NULL) { + NG_L2CAP_ERR( +"%s: %s - unexpected LP_ConnectCfm event. Connection does not exist\n", + __func__, NG_NODE_NAME(l2cap->node)); + error = ENOENT; + goto out; + } + + /* Check connection state */ + if (con->state != NG_L2CAP_W4_LP_CON_CFM) { + NG_L2CAP_ALERT( +"%s: %s - unexpected LP_ConnectCfm event. " \ +"Invalid connection state, state=%d, con_handle=%d\n", + __func__, NG_NODE_NAME(l2cap->node), con->state, + con->con_handle); + error = EINVAL; + goto out; + } + + /* + * Looks like it is our confirmation. It is safe now to cancel + * connection timer and notify upper layer. + */ + + ng_l2cap_lp_untimeout(con); + + if (ep->status == 0) { + con->state = NG_L2CAP_CON_OPEN; + con->con_handle = ep->con_handle; + ng_l2cap_lp_deliver(con); + } else { + /* Negative confirmation - remove connection descriptor */ + con->state = NG_L2CAP_CON_CLOSED; + ng_l2cap_con_fail(con, ep->status); + } +out: + return (error); +} /* ng_l2cap_lp_con_cfm */ + +/* + * Process LP_ConnectInd event from the lower layer protocol. This is a good + * place to put some extra check on remote unit address and/or class. We could + * even forward this information to control hook (or check against internal + * black list) and thus implement some kind of firewall. But for now be simple + * and create new connection descriptor, start timer and send LP_ConnectRsp + * event (i.e. accept connection). + */ + +int +ng_l2cap_lp_con_ind(ng_l2cap_p l2cap, struct ng_mesg *msg) +{ + ng_hci_lp_con_ind_ep *ep = NULL; + ng_hci_lp_con_rsp_ep *rp = NULL; + struct ng_mesg *rsp = NULL; + ng_l2cap_con_p con = NULL; + int error = 0; + + /* Check message */ + if (msg->header.arglen != sizeof(*ep)) { + NG_L2CAP_ALERT( +"%s: %s - invalid LP_ConnectInd message size\n", + __func__, NG_NODE_NAME(l2cap->node)); + error = EMSGSIZE; + goto out; + } + + ep = (ng_hci_lp_con_ind_ep *) (msg->data); + + /* Make sure we have only one connection to the remote unit */ + con = ng_l2cap_con_by_addr(l2cap, &ep->bdaddr); + if (con != NULL) { + NG_L2CAP_ALERT( +"%s: %s - unexpected LP_ConnectInd event. " \ +"Connection already exists, state=%d, con_handle=%d\n", + __func__, NG_NODE_NAME(l2cap->node), con->state, + con->con_handle); + error = EEXIST; + goto out; + } + + /* Check if lower layer protocol is still connected */ + if (l2cap->hci == NULL || NG_HOOK_NOT_VALID(l2cap->hci)) { + NG_L2CAP_ERR( +"%s: %s - hook \"%s\" is not connected or valid", + __func__, NG_NODE_NAME(l2cap->node), NG_L2CAP_HOOK_HCI); + error = ENOTCONN; + goto out; + } + + /* Create and intialize new connection descriptor */ + con = ng_l2cap_new_con(l2cap, &ep->bdaddr); + if (con == NULL) { + error = ENOMEM; + goto out; + } + + /* Create and send LP_ConnectRsp event */ + NG_MKMESSAGE(rsp, NGM_HCI_COOKIE, NGM_HCI_LP_CON_RSP, + sizeof(*rp), M_NOWAIT); + if (msg == NULL) { + ng_l2cap_free_con(con); + error = ENOMEM; + goto out; + } + + rp = (ng_hci_lp_con_rsp_ep *)(rsp->data); + rp->status = 0x00; /* accept connection */ + rp->link_type = NG_HCI_LINK_ACL; + bcopy(&ep->bdaddr, &rp->bdaddr, sizeof(rp->bdaddr)); + + con->state = NG_L2CAP_W4_LP_CON_CFM; + ng_l2cap_lp_timeout(con); + + NG_SEND_MSG_HOOK(error, l2cap->node, rsp, l2cap->hci, NULL); + if (error != 0) + ng_l2cap_free_con(con); /* will remove timeout */ +out: + return (error); +} /* ng_hci_lp_con_ind */ + +/* + * Process LP_DisconnectInd event from the lower layer protocol. We have been + * disconnected from the remote unit. So notify the upper layer protocol. + */ + +int +ng_l2cap_lp_discon_ind(ng_l2cap_p l2cap, struct ng_mesg *msg) +{ + ng_hci_lp_discon_ind_ep *ep = NULL; + ng_l2cap_con_p con = NULL; + int error = 0; + + /* Check message */ + if (msg->header.arglen != sizeof(*ep)) { + NG_L2CAP_ALERT( +"%s: %s - invalid LP_DisconnectInd message size\n", + __func__, NG_NODE_NAME(l2cap->node)); + error = EMSGSIZE; + goto out; + } + + ep = (ng_hci_lp_discon_ind_ep *) (msg->data); + + /* Check if we have this connection */ + con = ng_l2cap_con_by_handle(l2cap, ep->con_handle); + if (con == NULL) { + NG_L2CAP_ERR( +"%s: %s - unexpected LP_DisconnectInd event. " \ +"Connection does not exist, con_handle=%d\n", + __func__, NG_NODE_NAME(l2cap->node), ep->con_handle); + error = ENOENT; + goto out; + } + + /* XXX Verify connection state -- do we need to check this? */ + if (con->state != NG_L2CAP_CON_OPEN) { + NG_L2CAP_ERR( +"%s: %s - unexpected LP_DisconnectInd event. " \ +"Invalid connection state, state=%d, con_handle=%d\n", + __func__, NG_NODE_NAME(l2cap->node), con->state, + con->con_handle); + error = EINVAL; + goto out; + } + + /* Notify upper layer and remove connection */ + con->state = NG_L2CAP_CON_CLOSED; + ng_l2cap_con_fail(con, ep->reason); +out: + return (error); +} /* ng_l2cap_lp_discon_ind */ + +/* + * Send LP_QoSSetupReq event to the lower layer protocol + */ + +int +ng_l2cap_lp_qos_req(ng_l2cap_p l2cap, u_int16_t con_handle, + ng_l2cap_flow_p flow) +{ + struct ng_mesg *msg = NULL; + ng_hci_lp_qos_req_ep *ep = NULL; + ng_l2cap_con_p con = NULL; + int error = 0; + + /* Verify that we have this connection */ + con = ng_l2cap_con_by_handle(l2cap, con_handle); + if (con == NULL) { + NG_L2CAP_ERR( +"%s: %s - unexpected LP_QoSSetupReq event. " \ +"Connection does not exist, con_handle=%d\n", + __func__, NG_NODE_NAME(l2cap->node), con_handle); + + return (ENOENT); + } + + /* Verify connection state */ + if (con->state != NG_L2CAP_CON_OPEN) { + NG_L2CAP_ERR( +"%s: %s - unexpected LP_QoSSetupReq event. " \ +"Invalid connection state, state=%d, con_handle=%d\n", + __func__, NG_NODE_NAME(l2cap->node), con->state, + con->con_handle); + + return (EINVAL); + } + + /* Check if lower layer protocol is still connected */ + if (l2cap->hci == NULL || NG_HOOK_NOT_VALID(l2cap->hci)) { + NG_L2CAP_ERR( +"%s: %s - hook \"%s\" is not connected or valid", + __func__, NG_NODE_NAME(l2cap->node), NG_L2CAP_HOOK_HCI); + + return (ENOTCONN); + } + + /* Create and send LP_QoSSetupReq event */ + NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_QOS_REQ, + sizeof(*ep), M_NOWAIT); + if (msg == NULL) + return (ENOMEM); + + ep = (ng_hci_lp_qos_req_ep *) (msg->data); + ep->con_handle = con_handle; + ep->flags = flow->flags; + ep->service_type = flow->service_type; + ep->token_rate = flow->token_rate; + ep->peak_bandwidth = flow->peak_bandwidth; + ep->latency = flow->latency; + ep->delay_variation = flow->delay_variation; + + NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->hci, NULL); + + return (error); +} /* ng_l2cap_lp_con_req */ + +/* + * Process LP_QoSSetupCfm from the lower layer protocol + */ + +int +ng_l2cap_lp_qos_cfm(ng_l2cap_p l2cap, struct ng_mesg *msg) +{ + ng_hci_lp_qos_cfm_ep *ep = NULL; + int error = 0; + + /* Check message */ + if (msg->header.arglen != sizeof(*ep)) { + NG_L2CAP_ALERT( +"%s: %s - invalid LP_QoSSetupCfm[Neg] message size\n", + __func__, NG_NODE_NAME(l2cap->node)); + error = EMSGSIZE; + goto out; + } + + ep = (ng_hci_lp_qos_cfm_ep *) (msg->data); + /* XXX FIXME do something */ +out: + return (error); +} /* ng_l2cap_lp_qos_cfm */ + +/* + * Process LP_QoSViolationInd event from the lower layer protocol. Lower + * layer protocol has detected QoS Violation, so we MUST notify the + * upper layer. + */ + +int +ng_l2cap_lp_qos_ind(ng_l2cap_p l2cap, struct ng_mesg *msg) +{ + ng_hci_lp_qos_ind_ep *ep = NULL; + ng_l2cap_con_p con = NULL; + int error = 0; + + /* Check message */ + if (msg->header.arglen != sizeof(*ep)) { + NG_L2CAP_ALERT( +"%s: %s - invalid LP_QoSViolation message size\n", + __func__, NG_NODE_NAME(l2cap->node)); + error = EMSGSIZE; + goto out; + } + + ep = (ng_hci_lp_qos_ind_ep *) (msg->data); + + /* Check if we have this connection */ + con = ng_l2cap_con_by_handle(l2cap, ep->con_handle); + if (con == NULL) { + NG_L2CAP_ERR( +"%s: %s - unexpected LP_QoSViolationInd event. " \ +"Connection does not exist, con_handle=%d\n", + __func__, NG_NODE_NAME(l2cap->node), ep->con_handle); + error = ENOENT; + goto out; + } + + /* Verify connection state */ + if (con->state != NG_L2CAP_CON_OPEN) { + NG_L2CAP_ERR( +"%s: %s - unexpected LP_QoSViolationInd event. " \ +"Invalid connection state, state=%d, con_handle=%d\n", + __func__, NG_NODE_NAME(l2cap->node), con->state, + con->con_handle); + error = EINVAL; + goto out; + } + + /* XXX FIXME Notify upper layer and terminate channels if required */ +out: + return (error); +} /* ng_l2cap_qos_ind */ + +/* + * Prepare L2CAP packet. Prepend packet with L2CAP packet header and then + * segment it according to HCI MTU. + */ + +int +ng_l2cap_lp_send(ng_l2cap_con_p con, u_int16_t dcid, struct mbuf *m0) +{ + ng_l2cap_p l2cap = con->l2cap; + ng_l2cap_hdr_t *l2cap_hdr = NULL; + ng_hci_acldata_pkt_t *acl_hdr = NULL; + struct mbuf *m_last = NULL, *m = NULL; + int len, flag = NG_HCI_PACKET_START; + + KASSERT((con->tx_pkt == NULL), +("%s: %s - another packet pending?!\n", __func__, NG_NODE_NAME(l2cap->node))); + KASSERT((l2cap->pkt_size > 0), +("%s: %s - invalid l2cap->pkt_size?!\n", __func__, NG_NODE_NAME(l2cap->node))); + + /* Prepend mbuf with L2CAP header */ + m0 = ng_l2cap_prepend(m0, sizeof(*l2cap_hdr)); + if (m0 == NULL) { + NG_L2CAP_ALERT( +"%s: %s - ng_l2cap_prepend(%d) failed\n", + __func__, NG_NODE_NAME(l2cap->node), + sizeof(*l2cap_hdr)); + + goto fail; + } + + l2cap_hdr = mtod(m0, ng_l2cap_hdr_t *); + l2cap_hdr->length = htole16(m0->m_pkthdr.len - sizeof(*l2cap_hdr)); + l2cap_hdr->dcid = htole16(dcid); + + /* + * Segment single L2CAP packet according to the HCI layer MTU. Convert + * each segment into ACL data packet and prepend it with ACL data packet + * header. Link all segments together via m_nextpkt link. + * + * XXX BC (Broadcast flag) will always be 0 (zero). + */ + + while (m0 != NULL) { + /* Check length of the packet against HCI MTU */ + len = m0->m_pkthdr.len; + if (len > l2cap->pkt_size) { + m = m_split(m0, l2cap->pkt_size, M_DONTWAIT); + if (m == NULL) { + NG_L2CAP_ALERT( +"%s: %s - m_split(%d) failed\n", __func__, NG_NODE_NAME(l2cap->node), + l2cap->pkt_size); + goto fail; + } + + len = l2cap->pkt_size; + } + + /* Convert packet fragment into ACL data packet */ + m0 = ng_l2cap_prepend(m0, sizeof(*acl_hdr)); + if (m0 == NULL) { + NG_L2CAP_ALERT( +"%s: %s - ng_l2cap_prepend(%d) failed\n", + __func__, NG_NODE_NAME(l2cap->node), + sizeof(*acl_hdr)); + goto fail; + } + + acl_hdr = mtod(m0, ng_hci_acldata_pkt_t *); + acl_hdr->type = NG_HCI_ACL_DATA_PKT; + acl_hdr->length = htole16(len); + acl_hdr->con_handle = htole16(NG_HCI_MK_CON_HANDLE( + con->con_handle, flag, 0)); + + /* Add fragment to the chain */ + m0->m_nextpkt = NULL; + + if (con->tx_pkt == NULL) + con->tx_pkt = m_last = m0; + else { + m_last->m_nextpkt = m0; + m_last = m0; + } + + NG_L2CAP_INFO( +"%s: %s - attaching ACL packet, con_handle=%d, PB=%#x, length=%d\n", + __func__, NG_NODE_NAME(l2cap->node), con->con_handle, + flag, len); + + m0 = m; + m = NULL; + flag = NG_HCI_PACKET_FRAGMENT; + } + + return (0); +fail: + NG_FREE_M(m0); + NG_FREE_M(m); + + while (con->tx_pkt != NULL) { + m = con->tx_pkt->m_nextpkt; + m_freem(con->tx_pkt); + con->tx_pkt = m; + } + + return (ENOBUFS); +} /* ng_l2cap_lp_send */ + +/* + * Receive ACL data packet from the HCI layer. First strip ACL packet header + * and get connection handle, PB (Packet Boundary) flag and payload length. + * Then find connection descriptor and verify its state. Then process ACL + * packet as follows. + * + * 1) If we got first segment (pb == NG_HCI_PACKET_START) then extract L2CAP + * header and get total length of the L2CAP packet. Then start new L2CAP + * packet. + * + * 2) If we got other (then first :) segment (pb == NG_HCI_PACKET_FRAGMENT) + * then add segment to the packet. + */ + +int +ng_l2cap_lp_receive(ng_l2cap_p l2cap, struct mbuf *m) +{ + ng_hci_acldata_pkt_t *acl_hdr = NULL; + ng_l2cap_hdr_t *l2cap_hdr = NULL; + ng_l2cap_con_p con = NULL; + u_int16_t con_handle, length, pb; + int error = 0; + + /* Check ACL data packet */ + if (m->m_pkthdr.len < sizeof(*acl_hdr)) { + NG_L2CAP_ERR( +"%s: %s - invalid ACL data packet. Packet too small, length=%d\n", + __func__, NG_NODE_NAME(l2cap->node), m->m_pkthdr.len); + error = EMSGSIZE; + goto drop; + } + + /* Strip ACL data packet header */ + NG_L2CAP_M_PULLUP(m, sizeof(*acl_hdr)); + if (m == NULL) + return (ENOBUFS); + + acl_hdr = mtod(m, ng_hci_acldata_pkt_t *); + m_adj(m, sizeof(*acl_hdr)); + + /* Get ACL connection handle, PB flag and payload length */ + acl_hdr->con_handle = le16toh(acl_hdr->con_handle); + con_handle = NG_HCI_CON_HANDLE(acl_hdr->con_handle); + pb = NG_HCI_PB_FLAG(acl_hdr->con_handle); + length = le16toh(acl_hdr->length); + + NG_L2CAP_INFO( +"%s: %s - got ACL data packet, con_handle=%d, PB=%#x, length=%d\n", + __func__, NG_NODE_NAME(l2cap->node), con_handle, pb, length); + + /* Get connection descriptor */ + con = ng_l2cap_con_by_handle(l2cap, con_handle); + if (con == NULL) { + NG_L2CAP_ERR( +"%s: %s - unexpected ACL data packet. " \ +"Connection does not exist, con_handle=%d\n", + __func__, NG_NODE_NAME(l2cap->node), con_handle); + error = ENOENT; + goto drop; + } + + /* Verify connection state */ + if (con->state != NG_L2CAP_CON_OPEN) { + NG_L2CAP_ERR( +"%s: %s - unexpected ACL data packet. Invalid connection state=%d\n", + __func__, NG_NODE_NAME(l2cap->node), con->state); + error = EHOSTDOWN; + goto drop; + } + + /* Process packet */ + if (pb == NG_HCI_PACKET_START) { + if (con->rx_pkt != NULL) { + NG_L2CAP_ERR( +"%s: %s - dropping incomplete L2CAP packet, got %d bytes, want %d bytes\n", + __func__, NG_NODE_NAME(l2cap->node), + con->rx_pkt->m_pkthdr.len, con->rx_pkt_len); + NG_FREE_M(con->rx_pkt); + con->rx_pkt_len = 0; + } + + /* Get L2CAP header */ + if (m->m_pkthdr.len < sizeof(*l2cap_hdr)) { + NG_L2CAP_ERR( +"%s: %s - invalid L2CAP packet start fragment. Packet too small, length=%d\n", + __func__, NG_NODE_NAME(l2cap->node), + m->m_pkthdr.len); + error = EMSGSIZE; + goto drop; + } + + NG_L2CAP_M_PULLUP(m, sizeof(*l2cap_hdr)); + if (m == NULL) + return (ENOBUFS); + + l2cap_hdr = mtod(m, ng_l2cap_hdr_t *); + + NG_L2CAP_INFO( +"%s: %s - staring new L2CAP packet, con_handle=%d, length=%d\n", + __func__, NG_NODE_NAME(l2cap->node), con_handle, + le16toh(l2cap_hdr->length)); + + /* Start new L2CAP packet */ + con->rx_pkt = m; + con->rx_pkt_len = le16toh(l2cap_hdr->length)+sizeof(*l2cap_hdr); + } else if (pb == NG_HCI_PACKET_FRAGMENT) { + if (con->rx_pkt == NULL) { + NG_L2CAP_ERR( +"%s: %s - unexpected ACL data packet fragment, con_handle=%d\n", + __func__, NG_NODE_NAME(l2cap->node), + con->con_handle); + goto drop; + } + + /* Add fragment to the L2CAP packet */ + m_cat(con->rx_pkt, m); + con->rx_pkt->m_pkthdr.len += length; + } else { + NG_L2CAP_ERR( +"%s: %s - invalid ACL data packet. Invalid PB flag=%#x\n", + __func__, NG_NODE_NAME(l2cap->node), pb); + error = EINVAL; + goto drop; + } + + con->rx_pkt_len -= length; + if (con->rx_pkt_len < 0) { + NG_L2CAP_ALERT( +"%s: %s - packet length mismatch. Got %d bytes, offset %d bytes\n", + __func__, NG_NODE_NAME(l2cap->node), + con->rx_pkt->m_pkthdr.len, con->rx_pkt_len); + NG_FREE_M(con->rx_pkt); + con->rx_pkt_len = 0; + } else if (con->rx_pkt_len == 0) { + /* OK, we have got complete L2CAP packet, so process it */ + error = ng_l2cap_receive(con); + con->rx_pkt = NULL; + con->rx_pkt_len = 0; + } + + return (error); + +drop: + NG_FREE_M(m); + + return (error); +} /* ng_l2cap_lp_receive */ + +/* + * Send queued ACL packets to the HCI layer + */ + +void +ng_l2cap_lp_deliver(ng_l2cap_con_p con) +{ + ng_l2cap_p l2cap = con->l2cap; + struct mbuf *m = NULL; + int error; + + /* Check connection */ + if (con->state != NG_L2CAP_CON_OPEN) + return; + + if (con->tx_pkt == NULL) + ng_l2cap_con_wakeup(con); + + if (con->tx_pkt == NULL) + return; + + /* Check if lower layer protocol is still connected */ + if (l2cap->hci == NULL || NG_HOOK_NOT_VALID(l2cap->hci)) { + NG_L2CAP_ERR( +"%s: %s - hook \"%s\" is not connected or valid", + __func__, NG_NODE_NAME(l2cap->node), NG_L2CAP_HOOK_HCI); + + goto drop; /* XXX what to do with "pending"? */ + } + + /* Send ACL data packets */ + while (con->pending < con->l2cap->num_pkts && con->tx_pkt != NULL) { + m = con->tx_pkt; + con->tx_pkt = con->tx_pkt->m_nextpkt; + m->m_nextpkt = NULL; + + NG_L2CAP_INFO( +"%s: %s - sending ACL packet, con_handle=%d, len=%d\n", + __func__, NG_NODE_NAME(l2cap->node), con->con_handle, + m->m_pkthdr.len); + + NG_SEND_DATA_ONLY(error, l2cap->hci, m); + if (error != 0) { + NG_L2CAP_ERR( +"%s: %s - could not send ACL data packet, con_handle=%d, error=%d\n", + __func__, NG_NODE_NAME(l2cap->node), + con->con_handle, error); + + goto drop; /* XXX what to do with "pending"? */ + } + + con->pending ++; + } + + NG_L2CAP_INFO( +"%s: %s - %d ACL packets have been sent, con_handle=%d\n", + __func__, NG_NODE_NAME(l2cap->node), con->pending, + con->con_handle); + + return; + +drop: + while (con->tx_pkt != NULL) { + m = con->tx_pkt->m_nextpkt; + m_freem(con->tx_pkt); + con->tx_pkt = m; + } +} /* ng_l2cap_lp_deliver */ + +/* + * Process connection timeout. Remove connection from the list. If there + * are any channels that wait for the connection then notify them. Free + * connection descriptor. + */ + +void +ng_l2cap_process_lp_timeout(node_p node, hook_p hook, void *arg1, int arg2) +{ + ng_l2cap_con_p con = (ng_l2cap_con_p) arg1; + ng_l2cap_p l2cap = con->l2cap; + + NG_L2CAP_ERR( +"%s: %s - ACL connection timeout\n", __func__, NG_NODE_NAME(l2cap->node)); + + /* + * Notify channels that connection has timed out. This will remove + * connection, channels and pending commands. + */ + + con->state = NG_L2CAP_CON_CLOSED; + ng_l2cap_con_fail(con, NG_L2CAP_TIMEOUT); +} /* ng_l2cap_process_lp_timeout */ + |
