/* * ng_l2cap_evnt.c */ /*- * Copyright (c) Maksim Yevmenkin * 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.5 2003/09/08 19:11:45 max Exp $ * $FreeBSD$ */ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include /****************************************************************************** ****************************************************************************** ** 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=%zd\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_NOWAIT); 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) { /* If command timeout already happened then ignore reject */ if (ng_l2cap_command_untimeout(cmd) != 0) { NG_FREE_M(con->rx_pkt); return (ETIMEDOUT); } 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); } /* 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. If command timeout already * happened then ignore response. */ if ((error = ng_l2cap_command_untimeout(cmd)) != 0) return (error); 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: /* Ignore unknown hint option */ 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); } /* 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. If command timeout already happened then * ignore response. */ if ((error = ng_l2cap_command_untimeout(cmd)) != 0) { NG_FREE_M(m); return (error); } 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: /* Ignore unknown hint option */ break; } } else { /* * XXX FIXME What to do here? * * This is really BAD :( options packet was broken, or * peer sent us option that we did not understand. 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 && ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) { 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. */ if (ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) { 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; } /* 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. If command timeout already happened then ignore * response. */ if ((error = ng_l2cap_command_untimeout(cmd)) != 0) goto out; 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=%zd\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) { /* If command timeout already happened then ignore response */ if ((error = ng_l2cap_command_untimeout(cmd)) != 0) { NG_FREE_M(con->rx_pkt); return (error); } 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); } /* If command timeout already happened then ignore response */ if ((error = ng_l2cap_command_untimeout(cmd)) != 0) { NG_FREE_M(con->rx_pkt); return (error); } 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 */