freebsd-skq/sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.c

/*
 * 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.5 2003/09/08 19:11:45 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 <netgraph/bluetooth/include/ng_bluetooth.h>
#include <netgraph/bluetooth/include/ng_hci.h>
#include <netgraph/bluetooth/include/ng_l2cap.h>
#include <netgraph/bluetooth/l2cap/ng_l2cap_var.h>
#include <netgraph/bluetooth/l2cap/ng_l2cap_cmds.h>
#include <netgraph/bluetooth/l2cap/ng_l2cap_evnt.h>
#include <netgraph/bluetooth/l2cap/ng_l2cap_llpi.h>
#include <netgraph/bluetooth/l2cap/ng_l2cap_ulpi.h>
#include <netgraph/bluetooth/l2cap/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=%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_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) {
		/* 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 */