diff options
Diffstat (limited to 'sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.c')
| -rw-r--r-- | sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.c | 1337 |
1 files changed, 1337 insertions, 0 deletions
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.c b/sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.c new file mode 100644 index 0000000..3779b91 --- /dev/null +++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.c @@ -0,0 +1,1337 @@ +/* + * ng_l2cap_evnt.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_evnt.c,v 1.18 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" + +/****************************************************************************** + ****************************************************************************** + ** L2CAP events processing module + ****************************************************************************** + ******************************************************************************/ + +static int ng_l2cap_process_signal_cmd (ng_l2cap_con_p); +static int ng_l2cap_process_cmd_rej (ng_l2cap_con_p, u_int8_t); +static int ng_l2cap_process_con_req (ng_l2cap_con_p, u_int8_t); +static int ng_l2cap_process_con_rsp (ng_l2cap_con_p, u_int8_t); +static int ng_l2cap_process_cfg_req (ng_l2cap_con_p, u_int8_t); +static int ng_l2cap_process_cfg_rsp (ng_l2cap_con_p, u_int8_t); +static int ng_l2cap_process_discon_req (ng_l2cap_con_p, u_int8_t); +static int ng_l2cap_process_discon_rsp (ng_l2cap_con_p, u_int8_t); +static int ng_l2cap_process_echo_req (ng_l2cap_con_p, u_int8_t); +static int ng_l2cap_process_echo_rsp (ng_l2cap_con_p, u_int8_t); +static int ng_l2cap_process_info_req (ng_l2cap_con_p, u_int8_t); +static int ng_l2cap_process_info_rsp (ng_l2cap_con_p, u_int8_t); +static int send_l2cap_reject + (ng_l2cap_con_p, u_int8_t, u_int16_t, u_int16_t, u_int16_t, u_int16_t); +static int send_l2cap_con_rej + (ng_l2cap_con_p, u_int8_t, u_int16_t, u_int16_t, u_int16_t); +static int send_l2cap_cfg_rsp + (ng_l2cap_con_p, u_int8_t, u_int16_t, u_int16_t, struct mbuf *); +static int get_next_l2cap_opt + (struct mbuf *, int *, ng_l2cap_cfg_opt_p, ng_l2cap_cfg_opt_val_p); + +/* + * Receive L2CAP packet. First get L2CAP header and verify packet. Than + * get destination channel and process packet. + */ + +int +ng_l2cap_receive(ng_l2cap_con_p con) +{ + ng_l2cap_p l2cap = con->l2cap; + ng_l2cap_hdr_t *hdr = NULL; + int error = 0; + + /* Check packet */ + if (con->rx_pkt->m_pkthdr.len < sizeof(*hdr)) { + NG_L2CAP_ERR( +"%s: %s - invalid L2CAP packet. Packet too small, len=%d\n", + __func__, NG_NODE_NAME(l2cap->node), + con->rx_pkt->m_pkthdr.len); + error = EMSGSIZE; + goto drop; + } + + /* Get L2CAP header */ + NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr)); + if (con->rx_pkt == NULL) + return (ENOBUFS); + + hdr = mtod(con->rx_pkt, ng_l2cap_hdr_t *); + hdr->length = le16toh(hdr->length); + hdr->dcid = le16toh(hdr->dcid); + + /* Check payload size */ + if (hdr->length != con->rx_pkt->m_pkthdr.len - sizeof(*hdr)) { + NG_L2CAP_ERR( +"%s: %s - invalid L2CAP packet. Payload length mismatch, length=%d, len=%d\n", + __func__, NG_NODE_NAME(l2cap->node), hdr->length, + con->rx_pkt->m_pkthdr.len - sizeof(*hdr)); + error = EMSGSIZE; + goto drop; + } + + /* Process packet */ + switch (hdr->dcid) { + case NG_L2CAP_SIGNAL_CID: /* L2CAP command */ + m_adj(con->rx_pkt, sizeof(*hdr)); + error = ng_l2cap_process_signal_cmd(con); + break; + + case NG_L2CAP_CLT_CID: /* Connectionless packet */ + error = ng_l2cap_l2ca_clt_receive(con); + break; + + default: /* Data packet */ + error = ng_l2cap_l2ca_receive(con); + break; + } + + return (error); +drop: + NG_FREE_M(con->rx_pkt); + + return (error); +} /* ng_l2cap_receive */ + +/* + * Process L2CAP signaling command. We already know that destination channel ID + * is 0x1 that means we have received signaling command from peer's L2CAP layer. + * So get command header, decode and process it. + * + * XXX do we need to check signaling MTU here? + */ + +static int +ng_l2cap_process_signal_cmd(ng_l2cap_con_p con) +{ + ng_l2cap_p l2cap = con->l2cap; + ng_l2cap_cmd_hdr_t *hdr = NULL; + struct mbuf *m = NULL; + + while (con->rx_pkt != NULL) { + /* Verify packet length */ + if (con->rx_pkt->m_pkthdr.len < sizeof(*hdr)) { + NG_L2CAP_ERR( +"%s: %s - invalid L2CAP signaling command. Packet too small, len=%d\n", + __func__, NG_NODE_NAME(l2cap->node), + con->rx_pkt->m_pkthdr.len); + NG_FREE_M(con->rx_pkt); + + return (EMSGSIZE); + } + + /* Get signaling command */ + NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr)); + if (con->rx_pkt == NULL) + return (ENOBUFS); + + hdr = mtod(con->rx_pkt, ng_l2cap_cmd_hdr_t *); + hdr->length = le16toh(hdr->length); + m_adj(con->rx_pkt, sizeof(*hdr)); + + /* Verify command length */ + if (con->rx_pkt->m_pkthdr.len < hdr->length) { + NG_L2CAP_ERR( +"%s: %s - invalid L2CAP signaling command, code=%#x, ident=%d. " \ +"Invalid command length=%d, m_pkthdr.len=%d\n", + __func__, NG_NODE_NAME(l2cap->node), + hdr->code, hdr->ident, hdr->length, + con->rx_pkt->m_pkthdr.len); + NG_FREE_M(con->rx_pkt); + + return (EMSGSIZE); + } + + /* Get the command, save the rest (if any) */ + if (con->rx_pkt->m_pkthdr.len > hdr->length) + m = m_split(con->rx_pkt, hdr->length, M_DONTWAIT); + else + m = NULL; + + /* Process command */ + switch (hdr->code) { + case NG_L2CAP_CMD_REJ: + ng_l2cap_process_cmd_rej(con, hdr->ident); + break; + + case NG_L2CAP_CON_REQ: + ng_l2cap_process_con_req(con, hdr->ident); + break; + + case NG_L2CAP_CON_RSP: + ng_l2cap_process_con_rsp(con, hdr->ident); + break; + + case NG_L2CAP_CFG_REQ: + ng_l2cap_process_cfg_req(con, hdr->ident); + break; + + case NG_L2CAP_CFG_RSP: + ng_l2cap_process_cfg_rsp(con, hdr->ident); + break; + + case NG_L2CAP_DISCON_REQ: + ng_l2cap_process_discon_req(con, hdr->ident); + break; + + case NG_L2CAP_DISCON_RSP: + ng_l2cap_process_discon_rsp(con, hdr->ident); + break; + + case NG_L2CAP_ECHO_REQ: + ng_l2cap_process_echo_req(con, hdr->ident); + break; + + case NG_L2CAP_ECHO_RSP: + ng_l2cap_process_echo_rsp(con, hdr->ident); + break; + + case NG_L2CAP_INFO_REQ: + ng_l2cap_process_info_req(con, hdr->ident); + break; + + case NG_L2CAP_INFO_RSP: + ng_l2cap_process_info_rsp(con, hdr->ident); + break; + + default: + NG_L2CAP_ERR( +"%s: %s - unknown L2CAP signaling command, code=%#x, ident=%d, length=%d\n", + __func__, NG_NODE_NAME(l2cap->node), + hdr->code, hdr->ident, hdr->length); + + /* + * Send L2CAP_CommandRej. Do not really care + * about the result + */ + + send_l2cap_reject(con, hdr->ident, + NG_L2CAP_REJ_NOT_UNDERSTOOD, 0, 0, 0); + NG_FREE_M(con->rx_pkt); + break; + } + + con->rx_pkt = m; + } + + return (0); +} /* ng_l2cap_process_signal_cmd */ + +/* + * Process L2CAP_CommandRej command + */ + +static int +ng_l2cap_process_cmd_rej(ng_l2cap_con_p con, u_int8_t ident) +{ + ng_l2cap_p l2cap = con->l2cap; + ng_l2cap_cmd_rej_cp *cp = NULL; + ng_l2cap_cmd_p cmd = NULL; + + /* Get command parameters */ + NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); + if (con->rx_pkt == NULL) + return (ENOBUFS); + + cp = mtod(con->rx_pkt, ng_l2cap_cmd_rej_cp *); + cp->reason = le16toh(cp->reason); + + /* Check if we have pending command descriptor */ + cmd = ng_l2cap_cmd_by_ident(con, ident); + if (cmd != NULL) { + KASSERT((cmd->con == con), +("%s: %s - invalid connection pointer!\n", + __func__, NG_NODE_NAME(con->l2cap->node))); + KASSERT((cmd->flags & NG_L2CAP_CMD_PENDING), +("%s: %s - invalid command state, flags=%#x\n", + __func__, NG_NODE_NAME(con->l2cap->node), cmd->flags)); + + ng_l2cap_command_untimeout(cmd); + ng_l2cap_unlink_cmd(cmd); + + switch (cmd->code) { + case NG_L2CAP_CON_REQ: + ng_l2cap_l2ca_con_rsp(cmd->ch,cmd->token,cp->reason,0); + ng_l2cap_free_chan(cmd->ch); + break; + + case NG_L2CAP_CFG_REQ: + ng_l2cap_l2ca_cfg_rsp(cmd->ch, cmd->token, cp->reason); + break; + + case NG_L2CAP_DISCON_REQ: + ng_l2cap_l2ca_discon_rsp(cmd->ch,cmd->token,cp->reason); + ng_l2cap_free_chan(cmd->ch); /* XXX free channel */ + break; + + case NG_L2CAP_ECHO_REQ: + ng_l2cap_l2ca_ping_rsp(cmd->con, cmd->token, + cp->reason, NULL); + break; + + case NG_L2CAP_INFO_REQ: + ng_l2cap_l2ca_get_info_rsp(cmd->con, cmd->token, + cp->reason, NULL); + break; + + default: + NG_L2CAP_ALERT( +"%s: %s - unexpected L2CAP_CommandRej. Unexpected L2CAP command opcode=%d\n", + __func__, NG_NODE_NAME(l2cap->node), cmd->code); + break; + } + + ng_l2cap_free_cmd(cmd); + } else + NG_L2CAP_ERR( +"%s: %s - unexpected L2CAP_CommandRej command. " \ +"Requested ident does not exist, ident=%d\n", + __func__, NG_NODE_NAME(l2cap->node), ident); + + NG_FREE_M(con->rx_pkt); + + return (0); +} /* ng_l2cap_process_cmd_rej */ + +/* + * Process L2CAP_ConnectReq command + */ + +static int +ng_l2cap_process_con_req(ng_l2cap_con_p con, u_int8_t ident) +{ + ng_l2cap_p l2cap = con->l2cap; + struct mbuf *m = con->rx_pkt; + ng_l2cap_con_req_cp *cp = NULL; + ng_l2cap_chan_p ch = NULL; + int error = 0; + u_int16_t dcid, psm; + + /* Get command parameters */ + NG_L2CAP_M_PULLUP(m, sizeof(*cp)); + if (m == NULL) + return (ENOBUFS); + + cp = mtod(m, ng_l2cap_con_req_cp *); + psm = le16toh(cp->psm); + dcid = le16toh(cp->scid); + + NG_FREE_M(m); + con->rx_pkt = NULL; + + /* + * Create new channel and send L2CA_ConnectInd notification + * to the upper layer protocol. + */ + + ch = ng_l2cap_new_chan(l2cap, con, psm); + if (ch == NULL) + return (send_l2cap_con_rej(con, ident, 0, dcid, + NG_L2CAP_NO_RESOURCES)); + + /* Update channel IDs */ + ch->dcid = dcid; + + /* Sent L2CA_ConnectInd notification to the upper layer */ + ch->ident = ident; + ch->state = NG_L2CAP_W4_L2CA_CON_RSP; + + error = ng_l2cap_l2ca_con_ind(ch); + if (error != 0) { + send_l2cap_con_rej(con, ident, ch->scid, dcid, + (error == ENOMEM)? NG_L2CAP_NO_RESOURCES : + NG_L2CAP_PSM_NOT_SUPPORTED); + ng_l2cap_free_chan(ch); + } + + return (error); +} /* ng_l2cap_process_con_req */ + +/* + * Process L2CAP_ConnectRsp command + */ + +static int +ng_l2cap_process_con_rsp(ng_l2cap_con_p con, u_int8_t ident) +{ + ng_l2cap_p l2cap = con->l2cap; + struct mbuf *m = con->rx_pkt; + ng_l2cap_con_rsp_cp *cp = NULL; + ng_l2cap_cmd_p cmd = NULL; + u_int16_t scid, dcid, result, status; + int error = 0; + + /* Get command parameters */ + NG_L2CAP_M_PULLUP(m, sizeof(*cp)); + if (m == NULL) + return (ENOBUFS); + + cp = mtod(m, ng_l2cap_con_rsp_cp *); + dcid = le16toh(cp->dcid); + scid = le16toh(cp->scid); + result = le16toh(cp->result); + status = le16toh(cp->status); + + NG_FREE_M(m); + con->rx_pkt = NULL; + + /* Check if we have pending command descriptor */ + cmd = ng_l2cap_cmd_by_ident(con, ident); + if (cmd == NULL) { + NG_L2CAP_ERR( +"%s: %s - unexpected L2CAP_ConnectRsp command. ident=%d, con_handle=%d\n", + __func__, NG_NODE_NAME(l2cap->node), ident, + con->con_handle); + + return (ENOENT); + } + + KASSERT((cmd->flags & NG_L2CAP_CMD_PENDING), +("%s: %s - invalid command state, flags=%#x\n", + __func__, NG_NODE_NAME(l2cap->node), cmd->flags)); + + /* Verify channel state, if invalid - do nothing */ + if (cmd->ch->state != NG_L2CAP_W4_L2CAP_CON_RSP) { + NG_L2CAP_ERR( +"%s: %s - unexpected L2CAP_ConnectRsp. " \ +"Invalid channel state, cid=%d, state=%d\n", + __func__, NG_NODE_NAME(l2cap->node), scid, + cmd->ch->state); + goto reject; + } + + /* Verify CIDs and send reject if does not match */ + if (cmd->ch->scid != scid) { + NG_L2CAP_ERR( +"%s: %s - unexpected L2CAP_ConnectRsp. Channel IDs do not match, scid=%d(%d)\n", + __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid, + scid); + goto reject; + } + + /* + * Looks good. We got confirmation from our peer. Now process + * it. First disable RTX timer. Then check the result and send + * notification to the upper layer. + */ + + ng_l2cap_command_untimeout(cmd); + + if (result == NG_L2CAP_PENDING) { + /* + * Our peer wants more time to complete connection. We shall + * start ERTX timer and wait. Keep command in the list. + */ + + cmd->ch->dcid = dcid; + ng_l2cap_command_timeout(cmd, bluetooth_l2cap_ertx_timeout()); + + error = ng_l2cap_l2ca_con_rsp(cmd->ch, cmd->token, + result, status); + if (error != 0) + ng_l2cap_free_chan(cmd->ch); + } else { + ng_l2cap_unlink_cmd(cmd); + + if (result == NG_L2CAP_SUCCESS) { + /* + * Channel is open. Complete command and move to CONFIG + * state. Since we have sent positive confirmation we + * expect to receive L2CA_Config request from the upper + * layer protocol. + */ + + cmd->ch->dcid = dcid; + cmd->ch->state = NG_L2CAP_CONFIG; + } else + /* There was an error, so close the channel */ + NG_L2CAP_INFO( +"%s: %s - failed to open L2CAP channel, result=%d, status=%d\n", + __func__, NG_NODE_NAME(l2cap->node), result, + status); + + error = ng_l2cap_l2ca_con_rsp(cmd->ch, cmd->token, + result, status); + + /* XXX do we have to remove the channel on error? */ + if (error != 0 || result != NG_L2CAP_SUCCESS) + ng_l2cap_free_chan(cmd->ch); + + ng_l2cap_free_cmd(cmd); + } + + return (error); + +reject: + /* Send reject. Do not really care about the result */ + send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, dcid); + + return (0); +} /* ng_l2cap_process_con_rsp */ + +/* + * Process L2CAP_ConfigReq command + */ + +static int +ng_l2cap_process_cfg_req(ng_l2cap_con_p con, u_int8_t ident) +{ + ng_l2cap_p l2cap = con->l2cap; + struct mbuf *m = con->rx_pkt; + ng_l2cap_cfg_req_cp *cp = NULL; + ng_l2cap_chan_p ch = NULL; + u_int16_t dcid, respond, result; + ng_l2cap_cfg_opt_t hdr; + ng_l2cap_cfg_opt_val_t val; + int off, error = 0; + + /* Get command parameters */ + con->rx_pkt = NULL; + NG_L2CAP_M_PULLUP(m, sizeof(*cp)); + if (m == NULL) + return (ENOBUFS); + + cp = mtod(m, ng_l2cap_cfg_req_cp *); + dcid = le16toh(cp->dcid); + respond = NG_L2CAP_OPT_CFLAG(le16toh(cp->flags)); + m_adj(m, sizeof(*cp)); + + /* Check if we have this channel and it is in valid state */ + ch = ng_l2cap_chan_by_scid(l2cap, dcid); + if (ch == NULL) { + NG_L2CAP_ERR( +"%s: %s - unexpected L2CAP_ConfigReq command. " \ +"Channel does not exist, cid=%d\n", + __func__, NG_NODE_NAME(l2cap->node), dcid); + goto reject; + } + + /* Verify channel state */ + if (ch->state != NG_L2CAP_CONFIG && ch->state != NG_L2CAP_OPEN) { + NG_L2CAP_ERR( +"%s: %s - unexpected L2CAP_ConfigReq. " \ +"Invalid channel state, cid=%d, state=%d\n", + __func__, NG_NODE_NAME(l2cap->node), dcid, ch->state); + goto reject; + } + + if (ch->state == NG_L2CAP_OPEN) { /* Re-configuration */ + ch->cfg_state = 0; + ch->state = NG_L2CAP_CONFIG; + } + + for (result = 0, off = 0; ; ) { + error = get_next_l2cap_opt(m, &off, &hdr, &val); + if (error == 0) { /* We done with this packet */ + NG_FREE_M(m); + break; + } else if (error > 0) { /* Got option */ + switch (hdr.type) { + case NG_L2CAP_OPT_MTU: + ch->omtu = val.mtu; + break; + + case NG_L2CAP_OPT_FLUSH_TIMO: + ch->flush_timo = val.flush_timo; + break; + + case NG_L2CAP_OPT_QOS: + bcopy(&val.flow, &ch->iflow, sizeof(ch->iflow)); + break; + + default: + KASSERT(0, +("%s: %s - unknown option: %d\n", __func__, NG_NODE_NAME(l2cap->node), + hdr.type)); + break; + } + } else { /* Oops, something is wrong */ + respond = 1; + + if (error == -3) { + + /* + * Adjust mbuf so we can get to the start + * of the first option we did not like. + */ + + m_adj(m, off - sizeof(hdr)); + m->m_pkthdr.len = sizeof(hdr) + hdr.length; + + result = NG_L2CAP_UNKNOWN_OPTION; + } else { + /* XXX FIXME Send other reject codes? */ + NG_FREE_M(m); + result = NG_L2CAP_REJECT; + } + + break; + } + } + + /* + * Now check and see if we have to respond. If everything was OK then + * respond contain "C flag" and (if set) we will respond with empty + * packet and will wait for more options. + * + * Other case is that we did not like peer's options and will respond + * with L2CAP_Config response command with Reject error code. + * + * When "respond == 0" than we have received all options and we will + * sent L2CA_ConfigInd event to the upper layer protocol. + */ + + if (respond) { + error = send_l2cap_cfg_rsp(con, ident, ch->dcid, result, m); + if (error != 0) { + ng_l2cap_l2ca_discon_ind(ch); + ng_l2cap_free_chan(ch); + } + } else { + /* Send L2CA_ConfigInd event to the upper layer protocol */ + ch->ident = ident; + error = ng_l2cap_l2ca_cfg_ind(ch); + if (error != 0) + ng_l2cap_free_chan(ch); + } + + return (error); + +reject: + /* Send reject. Do not really care about the result */ + NG_FREE_M(m); + + send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, 0, dcid); + + return (0); +} /* ng_l2cap_process_cfg_req */ + +/* + * Process L2CAP_ConfigRsp command + */ + +static int +ng_l2cap_process_cfg_rsp(ng_l2cap_con_p con, u_int8_t ident) +{ + ng_l2cap_p l2cap = con->l2cap; + struct mbuf *m = con->rx_pkt; + ng_l2cap_cfg_rsp_cp *cp = NULL; + ng_l2cap_cmd_p cmd = NULL; + u_int16_t scid, cflag, result; + ng_l2cap_cfg_opt_t hdr; + ng_l2cap_cfg_opt_val_t val; + int off, error = 0; + + /* Get command parameters */ + con->rx_pkt = NULL; + NG_L2CAP_M_PULLUP(m, sizeof(*cp)); + if (m == NULL) + return (ENOBUFS); + + cp = mtod(m, ng_l2cap_cfg_rsp_cp *); + scid = le16toh(cp->scid); + cflag = NG_L2CAP_OPT_CFLAG(le16toh(cp->flags)); + result = le16toh(cp->result); + m_adj(m, sizeof(*cp)); + + /* Check if we have this command */ + cmd = ng_l2cap_cmd_by_ident(con, ident); + if (cmd == NULL) { + NG_L2CAP_ERR( +"%s: %s - unexpected L2CAP_ConfigRsp command. ident=%d, con_handle=%d\n", + __func__, NG_NODE_NAME(l2cap->node), ident, + con->con_handle); + NG_FREE_M(m); + + return (ENOENT); + } + + KASSERT((cmd->flags & NG_L2CAP_CMD_PENDING), +("%s: %s - invalid command state, flags=%#x\n", + __func__, NG_NODE_NAME(l2cap->node), cmd->flags)); + + /* Verify CIDs and send reject if does not match */ + if (cmd->ch->scid != scid) { + NG_L2CAP_ERR( +"%s: %s - unexpected L2CAP_ConfigRsp. " \ +"Channel ID does not match, scid=%d(%d)\n", + __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid, + scid); + goto reject; + } + + /* Verify channel state and reject if invalid */ + if (cmd->ch->state != NG_L2CAP_CONFIG) { + NG_L2CAP_ERR( +"%s: %s - unexpected L2CAP_ConfigRsp. " \ +"Invalid channel state, scid=%d, state=%d\n", + __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid, + cmd->ch->state); + goto reject; + } + + /* + * Looks like it is our response, so process it. First parse options, + * then verify C flag. If it is set then we shall expect more + * configuration options from the peer and we will wait. Otherwise we + * have received all options and we will send L2CA_ConfigRsp event to + * the upper layer protocol. + */ + + ng_l2cap_command_untimeout(cmd); + + for (off = 0; ; ) { + error = get_next_l2cap_opt(m, &off, &hdr, &val); + if (error == 0) /* We done with this packet */ + break; + else if (error > 0) { /* Got option */ + switch (hdr.type) { + case NG_L2CAP_OPT_MTU: + cmd->ch->imtu = val.mtu; + break; + + case NG_L2CAP_OPT_FLUSH_TIMO: + cmd->ch->flush_timo = val.flush_timo; + break; + + case NG_L2CAP_OPT_QOS: + bcopy(&val.flow, &cmd->ch->oflow, + sizeof(cmd->ch->oflow)); + break; + + default: + KASSERT(0, +("%s: %s - unknown option: %d\n", __func__, NG_NODE_NAME(l2cap->node), + hdr.type)); + break; + } + } else { + /* + * XXX FIXME What to do here? + * + * This is really BAD :( options packet was broken, + * so let upper layer know and do not wait for more + * options + */ + + NG_L2CAP_ALERT( +"%s: %s - failed to parse configuration options, error=%d\n", + __func__, NG_NODE_NAME(l2cap->node), error); + + result = NG_L2CAP_UNKNOWN; + cflag = 0; + + break; + } + } + + NG_FREE_M(m); + + if (cflag) /* Restart timer and wait for more options */ + ng_l2cap_command_timeout(cmd, bluetooth_l2cap_rtx_timeout()); + else { + ng_l2cap_unlink_cmd(cmd); + + /* Send L2CA_Config response to the upper layer protocol */ + error = ng_l2cap_l2ca_cfg_rsp(cmd->ch, cmd->token, result); + if (error != 0) { + /* + * XXX FIXME what to do here? we were not able to send + * response to the upper layer protocol, so for now + * just close the channel. Send L2CAP_Disconnect to + * remote peer? + */ + + NG_L2CAP_ERR( +"%s: %s - failed to send L2CA_Config response, error=%d\n", + __func__, NG_NODE_NAME(l2cap->node), error); + + ng_l2cap_free_chan(cmd->ch); + } + + ng_l2cap_free_cmd(cmd); + } + + return (error); + +reject: + /* Send reject. Do not really care about the result */ + NG_FREE_M(m); + + send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, 0); + + return (0); +} /* ng_l2cap_process_cfg_rsp */ + +/* + * Process L2CAP_DisconnectReq command + */ + +static int +ng_l2cap_process_discon_req(ng_l2cap_con_p con, u_int8_t ident) +{ + ng_l2cap_p l2cap = con->l2cap; + ng_l2cap_discon_req_cp *cp = NULL; + ng_l2cap_chan_p ch = NULL; + ng_l2cap_cmd_p cmd = NULL; + u_int16_t scid, dcid; + + /* Get command parameters */ + NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); + if (con->rx_pkt == NULL) + return (ENOBUFS); + + cp = mtod(con->rx_pkt, ng_l2cap_discon_req_cp *); + dcid = le16toh(cp->dcid); + scid = le16toh(cp->scid); + + NG_FREE_M(con->rx_pkt); + + /* Check if we have this channel and it is in valid state */ + ch = ng_l2cap_chan_by_scid(l2cap, dcid); + if (ch == NULL) { + NG_L2CAP_ERR( +"%s: %s - unexpected L2CAP_DisconnectReq message. " \ +"Channel does not exist, cid=%d\n", + __func__, NG_NODE_NAME(l2cap->node), dcid); + goto reject; + } + + /* XXX Verify channel state and reject if invalid -- is that true? */ + if (ch->state != NG_L2CAP_OPEN && ch->state != NG_L2CAP_CONFIG) { + NG_L2CAP_ERR( +"%s: %s - unexpected L2CAP_DisconnectReq. " \ +"Invalid channel state, cid=%d, state=%d\n", + __func__, NG_NODE_NAME(l2cap->node), dcid, ch->state); + goto reject; + } + + /* Match destination channel ID */ + if (ch->dcid != scid || ch->scid != dcid) { + NG_L2CAP_ERR( +"%s: %s - unexpected L2CAP_DisconnectReq. " \ +"Channel IDs does not match, channel: scid=%d, dcid=%d, " \ +"request: scid=%d, dcid=%d\n", + __func__, NG_NODE_NAME(l2cap->node), ch->scid, ch->dcid, + scid, dcid); + goto reject; + } + + /* + * Looks good, so notify upper layer protocol that channel is about + * to be disconnected and send L2CA_DisconnectInd message. Then respond + * with L2CAP_DisconnectRsp. + */ + + ng_l2cap_l2ca_discon_ind(ch); /* do not care about result */ + ng_l2cap_free_chan(ch); + + /* Send L2CAP_DisconnectRsp */ + cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_DISCON_RSP, 0); + if (cmd == NULL) + return (ENOMEM); + + _ng_l2cap_discon_rsp(cmd->aux, ident, dcid, scid); + if (cmd->aux == NULL) { + ng_l2cap_free_cmd(cmd); + + return (ENOBUFS); + } + + /* Link command to the queue */ + ng_l2cap_link_cmd(con, cmd); + ng_l2cap_lp_deliver(con); + + return (0); + +reject: + /* Send reject. Do not really care about the result */ + send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, dcid); + + return (0); +} /* ng_l2cap_process_discon_req */ + +/* + * Process L2CAP_DisconnectRsp command + */ + +static int +ng_l2cap_process_discon_rsp(ng_l2cap_con_p con, u_int8_t ident) +{ + ng_l2cap_p l2cap = con->l2cap; + ng_l2cap_discon_rsp_cp *cp = NULL; + ng_l2cap_cmd_p cmd = NULL; + u_int16_t scid, dcid; + int error = 0; + + /* Get command parameters */ + NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); + if (con->rx_pkt == NULL) + return (ENOBUFS); + + cp = mtod(con->rx_pkt, ng_l2cap_discon_rsp_cp *); + dcid = le16toh(cp->dcid); + scid = le16toh(cp->scid); + + NG_FREE_M(con->rx_pkt); + + /* Check if we have pending command descriptor */ + cmd = ng_l2cap_cmd_by_ident(con, ident); + if (cmd == NULL) { + NG_L2CAP_ERR( +"%s: %s - unexpected L2CAP_DisconnectRsp command. ident=%d, con_handle=%d\n", + __func__, NG_NODE_NAME(l2cap->node), ident, + con->con_handle); + goto out; + } + + KASSERT((cmd->flags & NG_L2CAP_CMD_PENDING), +("%s: %s - invalid command state, flags=%#x\n", + __func__, NG_NODE_NAME(l2cap->node), cmd->flags)); + + /* Verify channel state, do nothing if invalid */ + if (cmd->ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) { + NG_L2CAP_ERR( +"%s: %s - unexpected L2CAP_DisconnectRsp. " \ +"Invalid channel state, cid=%d, state=%d\n", + __func__, NG_NODE_NAME(l2cap->node), scid, + cmd->ch->state); + goto out; + } + + /* Verify CIDs and send reject if does not match */ + if (cmd->ch->scid != scid || cmd->ch->dcid != dcid) { + NG_L2CAP_ERR( +"%s: %s - unexpected L2CAP_DisconnectRsp. " \ +"Channel IDs do not match, scid=%d(%d), dcid=%d(%d)\n", + __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid, + scid, cmd->ch->dcid, dcid); + goto out; + } + + /* + * Looks like we have successfuly disconnected channel, + * so notify upper layer. + */ + + ng_l2cap_command_untimeout(cmd); + error = ng_l2cap_l2ca_discon_rsp(cmd->ch, cmd->token, NG_L2CAP_SUCCESS); + ng_l2cap_free_chan(cmd->ch); /* this will free commands too */ +out: + return (error); +} /* ng_l2cap_process_discon_rsp */ + +/* + * Process L2CAP_EchoReq command + */ + +static int +ng_l2cap_process_echo_req(ng_l2cap_con_p con, u_int8_t ident) +{ + ng_l2cap_p l2cap = con->l2cap; + ng_l2cap_cmd_hdr_t *hdr = NULL; + ng_l2cap_cmd_p cmd = NULL; + + con->rx_pkt = ng_l2cap_prepend(con->rx_pkt, sizeof(*hdr)); + if (con->rx_pkt == NULL) { + NG_L2CAP_ALERT( +"%s: %s - ng_l2cap_prepend() failed, size=%d\n", + __func__, NG_NODE_NAME(l2cap->node), sizeof(*hdr)); + + return (ENOBUFS); + } + + hdr = mtod(con->rx_pkt, ng_l2cap_cmd_hdr_t *); + hdr->code = NG_L2CAP_ECHO_RSP; + hdr->ident = ident; + hdr->length = htole16(con->rx_pkt->m_pkthdr.len - sizeof(*hdr)); + + cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_ECHO_RSP, 0); + if (cmd == NULL) { + NG_FREE_M(con->rx_pkt); + + return (ENOBUFS); + } + + /* Attach data and link command to the queue */ + cmd->aux = con->rx_pkt; + con->rx_pkt = NULL; + ng_l2cap_link_cmd(con, cmd); + ng_l2cap_lp_deliver(con); + + return (0); +} /* ng_l2cap_process_echo_req */ + +/* + * Process L2CAP_EchoRsp command + */ + +static int +ng_l2cap_process_echo_rsp(ng_l2cap_con_p con, u_int8_t ident) +{ + ng_l2cap_p l2cap = con->l2cap; + ng_l2cap_cmd_p cmd = NULL; + int error = 0; + + /* Check if we have this command */ + cmd = ng_l2cap_cmd_by_ident(con, ident); + if (cmd != NULL) { + KASSERT((cmd->con == con), +("%s: %s - invalid connection pointer!\n", + __func__, NG_NODE_NAME(l2cap->node))); + KASSERT((cmd->flags & NG_L2CAP_CMD_PENDING), +("%s: %s - invalid command state, flags=%#x\n", + __func__, NG_NODE_NAME(l2cap->node), cmd->flags)); + + ng_l2cap_command_untimeout(cmd); + ng_l2cap_unlink_cmd(cmd); + + error = ng_l2cap_l2ca_ping_rsp(cmd->con, cmd->token, + NG_L2CAP_SUCCESS, con->rx_pkt); + + ng_l2cap_free_cmd(cmd); + con->rx_pkt = NULL; + } else { + NG_L2CAP_ERR( +"%s: %s - unexpected L2CAP_EchoRsp command. " \ +"Requested ident does not exist, ident=%d\n", + __func__, NG_NODE_NAME(l2cap->node), ident); + NG_FREE_M(con->rx_pkt); + } + + return (error); +} /* ng_l2cap_process_echo_rsp */ + +/* + * Process L2CAP_InfoReq command + */ + +static int +ng_l2cap_process_info_req(ng_l2cap_con_p con, u_int8_t ident) +{ + ng_l2cap_p l2cap = con->l2cap; + ng_l2cap_cmd_p cmd = NULL; + u_int16_t type; + + /* Get command parameters */ + NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(ng_l2cap_info_req_cp)); + if (con->rx_pkt == NULL) + return (ENOBUFS); + + type = le16toh(mtod(con->rx_pkt, ng_l2cap_info_req_cp *)->type); + NG_FREE_M(con->rx_pkt); + + cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_INFO_RSP, 0); + if (cmd == NULL) + return (ENOMEM); + + switch (type) { + case NG_L2CAP_CONNLESS_MTU: + _ng_l2cap_info_rsp(cmd->aux, ident, NG_L2CAP_CONNLESS_MTU, + NG_L2CAP_SUCCESS, NG_L2CAP_MTU_DEFAULT); + break; + + default: + _ng_l2cap_info_rsp(cmd->aux, ident, type, + NG_L2CAP_NOT_SUPPORTED, 0); + break; + } + + if (cmd->aux == NULL) { + ng_l2cap_free_cmd(cmd); + + return (ENOBUFS); + } + + /* Link command to the queue */ + ng_l2cap_link_cmd(con, cmd); + ng_l2cap_lp_deliver(con); + + return (0); +} /* ng_l2cap_process_info_req */ + +/* + * Process L2CAP_InfoRsp command + */ + +static int +ng_l2cap_process_info_rsp(ng_l2cap_con_p con, u_int8_t ident) +{ + ng_l2cap_p l2cap = con->l2cap; + ng_l2cap_info_rsp_cp *cp = NULL; + ng_l2cap_cmd_p cmd = NULL; + int error = 0; + + /* Get command parameters */ + NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); + if (con->rx_pkt == NULL) + return (ENOBUFS); + + cp = mtod(con->rx_pkt, ng_l2cap_info_rsp_cp *); + cp->type = le16toh(cp->type); + cp->result = le16toh(cp->result); + m_adj(con->rx_pkt, sizeof(*cp)); + + /* Check if we have pending command descriptor */ + cmd = ng_l2cap_cmd_by_ident(con, ident); + if (cmd == NULL) { + NG_L2CAP_ERR( +"%s: %s - unexpected L2CAP_InfoRsp command. " \ +"Requested ident does not exist, ident=%d\n", + __func__, NG_NODE_NAME(l2cap->node), ident); + NG_FREE_M(con->rx_pkt); + + return (ENOENT); + } + + KASSERT((cmd->con == con), +("%s: %s - invalid connection pointer!\n", + __func__, NG_NODE_NAME(l2cap->node))); + KASSERT((cmd->flags & NG_L2CAP_CMD_PENDING), +("%s: %s - invalid command state, flags=%#x\n", + __func__, NG_NODE_NAME(l2cap->node), cmd->flags)); + + ng_l2cap_command_untimeout(cmd); + ng_l2cap_unlink_cmd(cmd); + + if (cp->result == NG_L2CAP_SUCCESS) { + switch (cp->type) { + case NG_L2CAP_CONNLESS_MTU: + if (con->rx_pkt->m_pkthdr.len == sizeof(u_int16_t)) + *mtod(con->rx_pkt, u_int16_t *) = + le16toh(*mtod(con->rx_pkt,u_int16_t *)); + else { + cp->result = NG_L2CAP_UNKNOWN; /* XXX */ + + NG_L2CAP_ERR( +"%s: %s - invalid L2CAP_InfoRsp command. " \ +"Bad connectionless MTU parameter, len=%d\n", + __func__, NG_NODE_NAME(l2cap->node), + con->rx_pkt->m_pkthdr.len); + } + break; + + default: + NG_L2CAP_WARN( +"%s: %s - invalid L2CAP_InfoRsp command. Unknown info type=%d\n", + __func__, NG_NODE_NAME(l2cap->node), cp->type); + break; + } + } + + error = ng_l2cap_l2ca_get_info_rsp(cmd->con, cmd->token, + cp->result, con->rx_pkt); + + ng_l2cap_free_cmd(cmd); + con->rx_pkt = NULL; + + return (error); +} /* ng_l2cap_process_info_rsp */ + +/* + * Send L2CAP reject + */ + +static int +send_l2cap_reject(ng_l2cap_con_p con, u_int8_t ident, u_int16_t reason, + u_int16_t mtu, u_int16_t scid, u_int16_t dcid) +{ + ng_l2cap_cmd_p cmd = NULL; + + cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CMD_REJ, 0); + if (cmd == NULL) + return (ENOMEM); + + _ng_l2cap_cmd_rej(cmd->aux, cmd->ident, reason, mtu, scid, dcid); + if (cmd->aux == NULL) { + ng_l2cap_free_cmd(cmd); + + return (ENOBUFS); + } + + /* Link command to the queue */ + ng_l2cap_link_cmd(con, cmd); + ng_l2cap_lp_deliver(con); + + return (0); +} /* send_l2cap_reject */ + +/* + * Send L2CAP connection reject + */ + +static int +send_l2cap_con_rej(ng_l2cap_con_p con, u_int8_t ident, u_int16_t scid, + u_int16_t dcid, u_int16_t result) +{ + ng_l2cap_cmd_p cmd = NULL; + + cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CON_RSP, 0); + if (cmd == NULL) + return (ENOMEM); + + _ng_l2cap_con_rsp(cmd->aux, cmd->ident, scid, dcid, result, 0); + if (cmd->aux == NULL) { + ng_l2cap_free_cmd(cmd); + + return (ENOBUFS); + } + + /* Link command to the queue */ + ng_l2cap_link_cmd(con, cmd); + ng_l2cap_lp_deliver(con); + + return (0); +} /* send_l2cap_con_rej */ + +/* + * Send L2CAP config response + */ + +static int +send_l2cap_cfg_rsp(ng_l2cap_con_p con, u_int8_t ident, u_int16_t scid, + u_int16_t result, struct mbuf *opt) +{ + ng_l2cap_cmd_p cmd = NULL; + + cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CFG_RSP, 0); + if (cmd == NULL) { + NG_FREE_M(opt); + + return (ENOMEM); + } + + _ng_l2cap_cfg_rsp(cmd->aux, cmd->ident, scid, 0, result, opt); + if (cmd->aux == NULL) { + ng_l2cap_free_cmd(cmd); + + return (ENOBUFS); + } + + /* Link command to the queue */ + ng_l2cap_link_cmd(con, cmd); + ng_l2cap_lp_deliver(con); + + return (0); +} /* send_l2cap_cfg_rsp */ + +/* + * Get next L2CAP configuration option + * + * Return codes: + * 0 no option + * 1 we have got option + * -1 header too short + * -2 bad option value or length + * -3 unknown option + */ + +static int +get_next_l2cap_opt(struct mbuf *m, int *off, ng_l2cap_cfg_opt_p hdr, + ng_l2cap_cfg_opt_val_p val) +{ + int hint, len = m->m_pkthdr.len - (*off); + + if (len == 0) + return (0); + if (len < 0 || len < sizeof(*hdr)) + return (-1); + + m_copydata(m, *off, sizeof(*hdr), (caddr_t) hdr); + *off += sizeof(*hdr); + len -= sizeof(*hdr); + + hint = NG_L2CAP_OPT_HINT(hdr->type); + hdr->type &= NG_L2CAP_OPT_HINT_MASK; + + switch (hdr->type) { + case NG_L2CAP_OPT_MTU: + if (hdr->length != NG_L2CAP_OPT_MTU_SIZE || len < hdr->length) + return (-2); + + m_copydata(m, *off, NG_L2CAP_OPT_MTU_SIZE, (caddr_t) val); + val->mtu = le16toh(val->mtu); + *off += NG_L2CAP_OPT_MTU_SIZE; + break; + + case NG_L2CAP_OPT_FLUSH_TIMO: + if (hdr->length != NG_L2CAP_OPT_FLUSH_TIMO_SIZE || + len < hdr->length) + return (-2); + + m_copydata(m, *off, NG_L2CAP_OPT_FLUSH_TIMO_SIZE, (caddr_t)val); + val->flush_timo = le16toh(val->flush_timo); + *off += NG_L2CAP_OPT_FLUSH_TIMO_SIZE; + break; + + case NG_L2CAP_OPT_QOS: + if (hdr->length != NG_L2CAP_OPT_QOS_SIZE || len < hdr->length) + return (-2); + + m_copydata(m, *off, NG_L2CAP_OPT_QOS_SIZE, (caddr_t) val); + val->flow.token_rate = le32toh(val->flow.token_rate); + val->flow.token_bucket_size = + le32toh(val->flow.token_bucket_size); + val->flow.peak_bandwidth = le32toh(val->flow.peak_bandwidth); + val->flow.latency = le32toh(val->flow.latency); + val->flow.delay_variation = le32toh(val->flow.delay_variation); + *off += NG_L2CAP_OPT_QOS_SIZE; + break; + + default: + if (hint) + *off += hdr->length; + else + return (-3); + break; + } + + return (1); +} /* get_next_l2cap_opt */ + |
