The second try a committing the bluetooth code

Has been seen to work on several cards and communicating with
several mobile phones to use them as modems etc.

We are still talking with 3com to try get them to allow us to include
the firmware for their pccard in the driver but the driver is here..
In the mean time
it can be downloaded from the 3com website and loaded using the utility
bt3cfw(8) (supplied) (instructions in the man page)

Not yet linked to the build

Submitted by:	Maksim Yevmenkin <myevmenk@exodus.net>
Approved by:	re

14 files changed, 6363 insertions, 0 deletions

diff --git a/sys/netgraph/bluetooth/l2cap/TODO b/sys/netgraph/bluetooth/l2cap/TODO
new file mode 100644
index 0000000..98d0440
--- /dev/null
+++ b/sys/netgraph/bluetooth/l2cap/TODO
@@ -0,0 +1,43 @@
+$Id: TODO,v 1.8 2002/09/06 21:03:58 max Exp $
+$FreeBSD$
+
+FIXME/TODO list
+
+0) Ping itself. Should L2CAP layer loopback data?
+
+1) Locking/SMP
+
+	External code now uses ng_send_fn to inject data into Netgraph, so
+	it should be fine as long as Netgraph is SMP safe. Just need to
+	verify it.
+
+2) Understand and implement L2CAP QoS
+
+	Will fix later. I only have CSR based hardware and it does not
+	support QoS.
+
+3) Better functions to manage CIDs and command ident's.
+
+	Resource manager is not good because it uses MTX_DEF mutexes,
+	(i.e. could block/sleep)
+
+4) Implement group channels (multicast)
+
+	Will fix later
+
+5) Add bytes/packets counters and commands to get/reset them
+
+	Will fix later. What to count?
+
+6) Better way to get information about channels
+
+	L2CAP can support about 65000 channels. Need define some good way
+	to get data from kernel to user space. For example if we need to pass
+	1K of information for every channel, then worst case is that we need
+	to pass 65Mbytes of data from kernel to user space. Not good.
+
+7) Deal properly with "shutdown"s and hook "disconnect"s
+
+	For now we destroy all channels when upstream hook is disconnected.
+	Is there a better way to handle this?
+
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_cmds.c b/sys/netgraph/bluetooth/l2cap/ng_l2cap_cmds.c
new file mode 100644
index 0000000..0d28f1a
--- /dev/null
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_cmds.c
@@ -0,0 +1,365 @@
+/*
+ * ng_l2cap_cmds.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_cmds.c,v 1.14 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 commands processing module
+ ******************************************************************************
+ ******************************************************************************/
+
+/*
+ * Process L2CAP command queue on connection
+ */
+
+void
+ng_l2cap_con_wakeup(ng_l2cap_con_p con)
+{
+	ng_l2cap_cmd_p	 cmd = NULL;
+	struct mbuf	*m = NULL;
+	int		 error = 0;
+
+	/* Find first non-pending command in the queue */
+	TAILQ_FOREACH(cmd, &con->cmd_list, next) {
+		KASSERT((cmd->con == con),
+("%s: %s - invalid connection pointer!\n",
+		__func__, NG_NODE_NAME(con->l2cap->node)));
+
+		if (!(cmd->flags & NG_L2CAP_CMD_PENDING))
+			break;
+	}
+
+	if (cmd == NULL)
+		return;
+
+	/* Detach command packet */
+	m = cmd->aux;
+	cmd->aux = NULL;
+
+	/* Process command */
+	switch (cmd->code) {
+	case NG_L2CAP_CMD_REJ:
+	case NG_L2CAP_DISCON_RSP:
+	case NG_L2CAP_ECHO_RSP:
+	case NG_L2CAP_INFO_RSP:
+		ng_l2cap_lp_send(con, NG_L2CAP_SIGNAL_CID, m);
+		ng_l2cap_unlink_cmd(cmd);
+		ng_l2cap_free_cmd(cmd);
+		break;
+
+	case NG_L2CAP_CON_REQ:
+		error = ng_l2cap_lp_send(con, NG_L2CAP_SIGNAL_CID, m);
+		if (error != 0) {
+			ng_l2cap_l2ca_con_rsp(cmd->ch, cmd->token,
+				NG_L2CAP_NO_RESOURCES, 0);
+			ng_l2cap_free_chan(cmd->ch); /* will free commands */
+		} else
+			ng_l2cap_command_timeout(cmd,
+				bluetooth_l2cap_rtx_timeout());
+		break;
+
+	case NG_L2CAP_CON_RSP:
+		error = ng_l2cap_lp_send(con, NG_L2CAP_SIGNAL_CID, m);
+		ng_l2cap_unlink_cmd(cmd);
+		if (cmd->ch != NULL) {
+			ng_l2cap_l2ca_con_rsp_rsp(cmd->ch, cmd->token,
+				(error == 0)? NG_L2CAP_SUCCESS : 
+					NG_L2CAP_NO_RESOURCES);
+			if (error != 0)
+				ng_l2cap_free_chan(cmd->ch);
+		}
+		ng_l2cap_free_cmd(cmd);
+		break;
+
+	case NG_L2CAP_CFG_REQ:
+		error = ng_l2cap_lp_send(con, NG_L2CAP_SIGNAL_CID, m);
+		if (error != 0) {
+			ng_l2cap_l2ca_cfg_rsp(cmd->ch, cmd->token,
+				NG_L2CAP_NO_RESOURCES);
+			ng_l2cap_unlink_cmd(cmd);
+			ng_l2cap_free_cmd(cmd);
+		} else
+			ng_l2cap_command_timeout(cmd,
+				bluetooth_l2cap_rtx_timeout());
+		break;
+
+	case NG_L2CAP_CFG_RSP:
+		error = ng_l2cap_lp_send(con, NG_L2CAP_SIGNAL_CID, m);
+		ng_l2cap_unlink_cmd(cmd);
+		if (cmd->ch != NULL)
+			ng_l2cap_l2ca_cfg_rsp_rsp(cmd->ch, cmd->token,
+				(error == 0)? NG_L2CAP_SUCCESS :
+					NG_L2CAP_NO_RESOURCES);
+		ng_l2cap_free_cmd(cmd);
+		break;
+
+	case NG_L2CAP_DISCON_REQ:
+		error = ng_l2cap_lp_send(con, NG_L2CAP_SIGNAL_CID, m);
+		ng_l2cap_l2ca_discon_rsp(cmd->ch, cmd->token,
+			(error == 0)? NG_L2CAP_SUCCESS : NG_L2CAP_NO_RESOURCES);
+		if (error != 0)
+			ng_l2cap_free_chan(cmd->ch); /* XXX free channel */
+		else
+			ng_l2cap_command_timeout(cmd,
+				bluetooth_l2cap_rtx_timeout());
+		break;
+
+	case NG_L2CAP_ECHO_REQ:
+		error = ng_l2cap_lp_send(con, NG_L2CAP_SIGNAL_CID, m);
+		if (error != 0) {
+			ng_l2cap_l2ca_ping_rsp(con, cmd->token,
+					NG_L2CAP_NO_RESOURCES, NULL);
+			ng_l2cap_unlink_cmd(cmd);
+			ng_l2cap_free_cmd(cmd);
+		} else
+			ng_l2cap_command_timeout(cmd, 
+				bluetooth_l2cap_rtx_timeout());
+		break;
+
+	case NG_L2CAP_INFO_REQ:
+		error = ng_l2cap_lp_send(con, NG_L2CAP_SIGNAL_CID, m);
+		if (error != 0) {
+			ng_l2cap_l2ca_get_info_rsp(con, cmd->token, 
+				NG_L2CAP_NO_RESOURCES, NULL);
+			ng_l2cap_unlink_cmd(cmd);
+			ng_l2cap_free_cmd(cmd);
+		} else
+			ng_l2cap_command_timeout(cmd, 
+				bluetooth_l2cap_rtx_timeout());
+		break;
+
+	case NGM_L2CAP_L2CA_WRITE: {
+		int	length = m->m_pkthdr.len;
+
+		if (cmd->ch->dcid == NG_L2CAP_CLT_CID) {
+			m = ng_l2cap_prepend(m, sizeof(ng_l2cap_clt_hdr_t));
+			if (m == NULL)
+				error = ENOBUFS;
+			else
+                		mtod(m, ng_l2cap_clt_hdr_t *)->psm =
+							htole16(cmd->ch->psm);
+		}
+
+		if (error == 0)
+			error = ng_l2cap_lp_send(con, cmd->ch->dcid, m);
+
+		ng_l2cap_l2ca_write_rsp(cmd->ch, cmd->token,
+			(error == 0)? NG_L2CAP_SUCCESS : NG_L2CAP_NO_RESOURCES,
+			length);
+
+		ng_l2cap_unlink_cmd(cmd);
+		ng_l2cap_free_cmd(cmd);
+		} break;
+
+	/* XXX FIXME add other commands */
+
+	default:
+		KASSERT(0,
+("%s: %s - unknown command code=%d\n",
+			__func__, NG_NODE_NAME(con->l2cap->node), cmd->code));
+
+		ng_l2cap_unlink_cmd(cmd);
+		ng_l2cap_free_cmd(cmd);
+		break;
+	}
+} /* ng_l2cap_con_wakeup */
+
+/*
+ * We have failed to open ACL connection to the remote unit. Could be negative
+ * confirmation or timeout. So fail any "delayed" commands, notify upper layer,
+ * remove all channels and remove connection descriptor.
+ */
+
+void
+ng_l2cap_con_fail(ng_l2cap_con_p con, u_int16_t result)
+{
+	ng_l2cap_p	l2cap = con->l2cap;
+	ng_l2cap_cmd_p	cmd = NULL;
+	ng_l2cap_chan_p	ch = NULL;
+
+	NG_L2CAP_INFO(
+"%s: %s - ACL connection failed, result=%d\n",
+		__func__, NG_NODE_NAME(l2cap->node), result);
+
+	/* Clean command queue */
+	while (!TAILQ_EMPTY(&con->cmd_list)) {
+		cmd = TAILQ_FIRST(&con->cmd_list);
+
+		ng_l2cap_unlink_cmd(cmd);
+		if(cmd->flags & NG_L2CAP_CMD_PENDING)
+			ng_l2cap_command_untimeout(cmd);
+
+		KASSERT((cmd->con == con),
+("%s: %s - invalid connection pointer!\n",
+			__func__, NG_NODE_NAME(con->l2cap->node)));
+
+		switch (cmd->code) {
+		case NG_L2CAP_CMD_REJ:
+		case NG_L2CAP_DISCON_RSP:
+		case NG_L2CAP_ECHO_RSP:
+		case NG_L2CAP_INFO_RSP:
+			break;
+
+		case NG_L2CAP_CON_REQ:
+			ng_l2cap_l2ca_con_rsp(cmd->ch, cmd->token, result, 0);
+			break;
+
+		case NG_L2CAP_CON_RSP:
+			if (cmd->ch != NULL)
+				ng_l2cap_l2ca_con_rsp_rsp(cmd->ch, cmd->token,
+					result);
+			break;
+
+		case NG_L2CAP_CFG_REQ:
+		case NG_L2CAP_CFG_RSP:
+		case NGM_L2CAP_L2CA_WRITE:
+			ng_l2cap_l2ca_discon_ind(cmd->ch);
+			break;
+
+		case NG_L2CAP_DISCON_REQ:
+			ng_l2cap_l2ca_discon_rsp(cmd->ch, cmd->token,
+				NG_L2CAP_SUCCESS);
+			break;
+
+		case NG_L2CAP_ECHO_REQ:
+			ng_l2cap_l2ca_ping_rsp(cmd->con, cmd->token,
+				result, NULL);
+			break;
+
+		case NG_L2CAP_INFO_REQ:
+			ng_l2cap_l2ca_get_info_rsp(cmd->con, cmd->token,
+				result, NULL);
+			break;
+
+		/* XXX FIXME add other commands */
+
+		default:
+			KASSERT(0,
+("%s: %s - unexpected command code=%d\n",
+				__func__, NG_NODE_NAME(con->l2cap->node),
+				cmd->code));
+			break;
+		}
+
+		if (cmd->ch != NULL)
+			ng_l2cap_free_chan(cmd->ch);
+
+		ng_l2cap_free_cmd(cmd);
+	}
+
+	/*
+	 * There still might be channels (in OPEN state?) that
+	 * did not submit any commands, so diconnect them
+	 */
+
+	LIST_FOREACH(ch, &l2cap->chan_list, next)
+		if (ch->con == con)
+			ng_l2cap_l2ca_discon_ind(ch);
+
+	/* Free connection descriptor */
+	ng_l2cap_free_con(con);
+} /* ng_l2cap_con_fail */
+
+/*
+ * Process L2CAP command timeout. In general - notify upper layer and destroy
+ * channel. Do not pay much attension to return code, just do our best.
+ */
+
+void
+ng_l2cap_process_command_timeout(node_p node, hook_p hook, void *arg1, int arg2)
+{
+	ng_l2cap_cmd_p	cmd = (ng_l2cap_cmd_p) arg1;
+
+	KASSERT((cmd->flags & NG_L2CAP_CMD_PENDING),
+("%s: %s - invalid command flags flags=%#x!\n",
+		__func__, NG_NODE_NAME(cmd->con->l2cap->node), cmd->flags));
+
+	cmd->flags &= ~NG_L2CAP_CMD_PENDING;
+	ng_l2cap_unlink_cmd(cmd);
+
+	switch (cmd->code) {
+ 	case NG_L2CAP_CON_REQ:
+		ng_l2cap_l2ca_con_rsp(cmd->ch, cmd->token, NG_L2CAP_TIMEOUT, 0);
+		ng_l2cap_free_chan(cmd->ch); 
+		break;
+
+	case NG_L2CAP_CFG_REQ:
+		ng_l2cap_l2ca_cfg_rsp(cmd->ch, cmd->token, NG_L2CAP_TIMEOUT);
+		break;
+
+ 	case NG_L2CAP_DISCON_REQ:
+		ng_l2cap_l2ca_discon_rsp(cmd->ch, cmd->token, NG_L2CAP_TIMEOUT);
+		ng_l2cap_free_chan(cmd->ch); /* XXX free channel */
+		break;
+
+	case NG_L2CAP_ECHO_REQ:
+		/* Echo request timed out. Let the upper layer know */
+		ng_l2cap_l2ca_ping_rsp(cmd->con, cmd->token, 
+			NG_L2CAP_TIMEOUT, NULL);
+		break;
+
+	case NG_L2CAP_INFO_REQ:
+		/* Info request timed out. Let the upper layer know */
+		ng_l2cap_l2ca_get_info_rsp(cmd->con, cmd->token,
+			NG_L2CAP_TIMEOUT, NULL);
+		break;
+
+	/* XXX FIXME add other commands */
+
+	default:
+		KASSERT(0,
+("%s: %s - unexpected command code=%d\n",
+			__func__, NG_NODE_NAME(cmd->con->l2cap->node),
+			cmd->code));
+		break;
+	}
+
+	ng_l2cap_free_cmd(cmd);
+} /* ng_l2cap_process_command_timeout */
+
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_cmds.h b/sys/netgraph/bluetooth/l2cap/ng_l2cap_cmds.h
new file mode 100644
index 0000000..d107ad6
--- /dev/null
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_cmds.h
@@ -0,0 +1,403 @@
+/*
+ * ng_l2cap_cmds.h
+ *
+ * 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_cmds.h,v 1.9 2002/04/16 00:43:56 max Exp $
+ * $FreeBSD$
+ */
+
+#ifndef _NETGRAPH_L2CAP_CMDS_H_
+#define _NETGRAPH_L2CAP_CMDS_H_
+
+/******************************************************************************
+ ******************************************************************************
+ **                L2CAP to L2CAP signaling command macros
+ ******************************************************************************
+ ******************************************************************************/
+
+/*
+ * Note: All L2CAP implementations are required to support minimal signaling
+ *       MTU of 48 bytes. In order to simplify things we will send one command
+ *       per one L2CAP packet. Given evrything above we can assume that one
+ *       signaling packet will fit into single mbuf.
+ */
+
+/* L2CAP_CommandRej */
+#define	_ng_l2cap_cmd_rej(_m, _ident, _reason, _mtu, _scid, _dcid)	\
+do {									\
+	struct _cmd_rej {						\
+		ng_l2cap_cmd_hdr_t	 hdr;				\
+		ng_l2cap_cmd_rej_cp	 param;				\
+		ng_l2cap_cmd_rej_data_t	 data;				\
+	} __attribute__ ((packed))	*c = NULL;			\
+									\
+	MGETHDR((_m), M_DONTWAIT, MT_DATA);				\
+	if ((_m) == NULL) 						\
+		break;							\
+									\
+	c = mtod((_m), struct _cmd_rej *);				\
+	c->hdr.code = NG_L2CAP_CMD_REJ;					\
+	c->hdr.ident = (_ident);					\
+	c->hdr.length = sizeof(c->param);				\
+									\
+	c->param.reason = htole16((_reason));				\
+									\
+	if ((_reason) == NG_L2CAP_REJ_MTU_EXCEEDED) {			\
+		c->data.mtu.mtu = htole16((_mtu));			\
+		c->hdr.length += sizeof(c->data.mtu);			\
+	} else if ((_reason) == NG_L2CAP_REJ_INVALID_CID) {		\
+		c->data.cid.scid = htole16((_scid));			\
+		c->data.cid.dcid = htole16((_dcid));			\
+		c->hdr.length += sizeof(c->data.cid);			\
+	}								\
+									\
+	(_m)->m_pkthdr.len = (_m)->m_len = sizeof(c->hdr) + 		\
+					c->hdr.length;			\
+									\
+	c->hdr.length = htole16(c->hdr.length);				\
+} while (0)
+
+/* L2CAP_ConnectReq */
+#define	_ng_l2cap_con_req(_m, _ident, _psm, _scid)			\
+do {									\
+	struct _con_req {						\
+		ng_l2cap_cmd_hdr_t	 hdr;				\
+		ng_l2cap_con_req_cp	 param;				\
+	} __attribute__ ((packed)) 	*c = NULL;			\
+									\
+	MGETHDR((_m), M_DONTWAIT, MT_DATA);				\
+	if ((_m) == NULL) 						\
+		break;							\
+									\
+	(_m)->m_pkthdr.len = (_m)->m_len = sizeof(*c);			\
+									\
+	c = mtod((_m), struct _con_req *);				\
+	c->hdr.code = NG_L2CAP_CON_REQ;					\
+	c->hdr.ident = (_ident);					\
+	c->hdr.length = htole16(sizeof(c->param));			\
+									\
+	c->param.psm = htole16((_psm));					\
+	c->param.scid = htole16((_scid));				\
+} while (0)
+
+/* L2CAP_ConnectRsp */
+#define _ng_l2cap_con_rsp(_m, _ident, _dcid, _scid, _result, _status)	\
+do {									\
+	struct _con_rsp {						\
+		ng_l2cap_cmd_hdr_t	 hdr;				\
+		ng_l2cap_con_rsp_cp	 param;				\
+	} __attribute__ ((packed))	*c = NULL;			\
+									\
+	MGETHDR((_m), M_DONTWAIT, MT_DATA);				\
+	if ((_m) == NULL) 						\
+		break;							\
+									\
+	(_m)->m_pkthdr.len = (_m)->m_len = sizeof(*c);			\
+									\
+	c = mtod((_m), struct _con_rsp *);				\
+	c->hdr.code = NG_L2CAP_CON_RSP;					\
+	c->hdr.ident = (_ident);					\
+	c->hdr.length = htole16(sizeof(c->param));			\
+									\
+	c->param.dcid = htole16((_dcid));				\
+	c->param.scid = htole16((_scid));				\
+	c->param.result = htole16((_result));				\
+	c->param.status = htole16((_status));				\
+} while (0)
+
+/* L2CAP_ConfigReq */
+#define	_ng_l2cap_cfg_req(_m, _ident, _dcid, _flags, _data)		\
+do {									\
+	struct _cfg_req {						\
+		ng_l2cap_cmd_hdr_t	 hdr;				\
+		ng_l2cap_cfg_req_cp	 param;				\
+	} __attribute__ ((packed))	*c = NULL;			\
+									\
+	MGETHDR((_m), M_DONTWAIT, MT_DATA);				\
+	if ((_m) == NULL) { 						\
+		NG_FREE_M((_data));					\
+		break;							\
+	}								\
+									\
+	(_m)->m_pkthdr.len = (_m)->m_len = sizeof(*c);			\
+									\
+	c = mtod((_m), struct _cfg_req *);				\
+	c->hdr.code = NG_L2CAP_CFG_REQ;					\
+	c->hdr.ident = (_ident);					\
+	c->hdr.length = sizeof(c->param);				\
+									\
+	c->param.dcid = htole16((_dcid));				\
+	c->param.flags = htole16((_flags));				\
+	if ((_data) != NULL) {						\
+		m_cat((_m), (_data));					\
+		c->hdr.length += (_data)->m_pkthdr.len;			\
+		(_m)->m_pkthdr.len += (_data)->m_pkthdr.len;		\
+	}								\
+									\
+	c->hdr.length = htole16(c->hdr.length);				\
+} while (0)
+
+/* L2CAP_ConfigRsp */
+#define _ng_l2cap_cfg_rsp(_m, _ident, _scid, _flags, _result, _data)	\
+do {									\
+	struct _cfg_rsp {						\
+		ng_l2cap_cmd_hdr_t	 hdr;				\
+		ng_l2cap_cfg_rsp_cp	 param;				\
+	} __attribute__ ((packed))	*c = NULL;			\
+									\
+	MGETHDR((_m), M_DONTWAIT, MT_DATA);				\
+	if ((_m) == NULL) { 						\
+		NG_FREE_M((_data));					\
+		break;							\
+	}								\
+									\
+	(_m)->m_pkthdr.len = (_m)->m_len = sizeof(*c);			\
+									\
+	c = mtod((_m), struct _cfg_rsp *);				\
+	c->hdr.code = NG_L2CAP_CFG_RSP;					\
+	c->hdr.ident = (_ident);					\
+	c->hdr.length = sizeof(c->param);				\
+									\
+	c->param.scid = htole16((_scid));				\
+	c->param.flags = htole16((_flags));				\
+	c->param.result = htole16((_result));				\
+	if ((_data) != NULL) {						\
+		m_cat((_m), (_data));					\
+		c->hdr.length += (_data)->m_pkthdr.len;			\
+		(_m)->m_pkthdr.len += (_data)->m_pkthdr.len;		\
+	}								\
+									\
+	c->hdr.length = htole16(c->hdr.length);				\
+} while (0)
+
+/* Build configuration options */
+#define _ng_l2cap_build_cfg_options(_m, _mtu, _flush_timo, _flow)	\
+do {									\
+	u_int8_t	*p = NULL;					\
+									\
+	MGETHDR((_m), M_DONTWAIT, MT_DATA);				\
+	if ((_m) == NULL)						\
+		break;							\
+									\
+	(_m)->m_pkthdr.len = (_m)->m_len = 0;				\
+	p = mtod((_m), u_int8_t *);					\
+									\
+	if ((_mtu) != NULL) {						\
+		struct _cfg_opt_mtu {					\
+			ng_l2cap_cfg_opt_t	 hdr;			\
+			u_int16_t		 val;			\
+		} __attribute__ ((packed))	*o = NULL;		\
+									\
+		o = (struct _cfg_opt_mtu *) p;				\
+		o->hdr.type = NG_L2CAP_OPT_MTU;				\
+		o->hdr.length = sizeof(o->val);				\
+		o->val = htole16(*(u_int16_t *)(_mtu));			\
+									\
+		(_m)->m_pkthdr.len += sizeof(*o);			\
+		p += sizeof(*o);					\
+	}								\
+									\
+	if ((_flush_timo) != NULL) {					\
+		struct _cfg_opt_flush {					\
+			ng_l2cap_cfg_opt_t	 hdr;			\
+			u_int16_t		 val;			\
+		} __attribute__ ((packed))	*o = NULL;		\
+									\
+		o = (struct _cfg_opt_flush *) p;			\
+		o->hdr.type = NG_L2CAP_OPT_FLUSH_TIMO;			\
+		o->hdr.length = sizeof(o->val);				\
+		o->val = htole16(*(u_int16_t *)(_flush_timo));		\
+									\
+		(_m)->m_pkthdr.len += sizeof(*o);			\
+		p += sizeof(*o);					\
+	}								\
+									\
+	if ((_flow) != NULL) {						\
+		struct _cfg_opt_flow {					\
+			ng_l2cap_cfg_opt_t	 hdr;			\
+			ng_l2cap_flow_t		 val;			\
+		} __attribute__ ((packed))	*o = NULL;		\
+									\
+		o = (struct _cfg_opt_flow *) p;				\
+		o->hdr.type = NG_L2CAP_OPT_QOS;				\
+		o->hdr.length = sizeof(o->val);				\
+		o->val.flags = ((ng_l2cap_flow_p)(_flow))->flags;	\
+		o->val.service_type = ((ng_l2cap_flow_p)		\
+				(_flow))->service_type;			\
+		o->val.token_rate =					\
+			htole32(((ng_l2cap_flow_p)(_flow))->token_rate);\
+		o->val.token_bucket_size =				\
+			htole32(((ng_l2cap_flow_p)			\
+				(_flow))->token_bucket_size);		\
+		o->val.peak_bandwidth = 				\
+			htole32(((ng_l2cap_flow_p)			\
+				(_flow))->peak_bandwidth);		\
+		o->val.latency = htole32(((ng_l2cap_flow_p)		\
+				(_flow))->latency);			\
+		o->val.delay_variation = 				\
+			htole32(((ng_l2cap_flow_p)			\
+				(_flow))->delay_variation);		\
+									\
+		(_m)->m_pkthdr.len += sizeof(*o);			\
+	}								\
+									\
+	(_m)->m_len = (_m)->m_pkthdr.len;				\
+} while (0)
+
+/* L2CAP_DisconnectReq */
+#define	_ng_l2cap_discon_req(_m, _ident, _dcid, _scid)			\
+do {									\
+	struct _discon_req {						\
+		ng_l2cap_cmd_hdr_t	 hdr;				\
+		ng_l2cap_discon_req_cp	 param;				\
+	} __attribute__ ((packed))	*c = NULL;			\
+									\
+	MGETHDR((_m), M_DONTWAIT, MT_DATA);				\
+	if ((_m) == NULL)						\
+		break;							\
+									\
+	(_m)->m_pkthdr.len = (_m)->m_len = sizeof(*c);			\
+									\
+	c = mtod((_m), struct _discon_req *);				\
+	c->hdr.code = NG_L2CAP_DISCON_REQ;				\
+	c->hdr.ident = (_ident);					\
+	c->hdr.length = htole16(sizeof(c->param));			\
+									\
+	c->param.dcid = htole16((_dcid));				\
+	c->param.scid = htole16((_scid));				\
+} while (0)
+
+/* L2CA_DisconnectRsp */
+#define	_ng_l2cap_discon_rsp(_m, _ident, _dcid, _scid)			\
+do {									\
+	struct _discon_rsp {						\
+		ng_l2cap_cmd_hdr_t	 hdr;				\
+		ng_l2cap_discon_rsp_cp	 param;				\
+	} __attribute__ ((packed))	*c = NULL;			\
+									\
+	MGETHDR((_m), M_DONTWAIT, MT_DATA);				\
+	if ((_m) == NULL)						\
+		break;							\
+									\
+	(_m)->m_pkthdr.len = (_m)->m_len = sizeof(*c);			\
+									\
+	c = mtod((_m), struct _discon_rsp *);				\
+	c->hdr.code = NG_L2CAP_DISCON_RSP;				\
+	c->hdr.ident = (_ident);					\
+	c->hdr.length = htole16(sizeof(c->param));			\
+									\
+	c->param.dcid = htole16((_dcid));				\
+	c->param.scid = htole16((_scid));				\
+} while (0)
+
+/* L2CAP_EchoReq */
+#define	_ng_l2cap_echo_req(_m, _ident, _data, _size)			\
+do {									\
+	ng_l2cap_cmd_hdr_t	*c = NULL;				\
+									\
+	MGETHDR((_m), M_DONTWAIT, MT_DATA);				\
+	if ((_m) == NULL) 						\
+		break;							\
+									\
+	(_m)->m_pkthdr.len = (_m)->m_len = sizeof(*c);			\
+									\
+	c = mtod((_m), ng_l2cap_cmd_hdr_t *);				\
+	c->code = NG_L2CAP_ECHO_REQ;					\
+	c->ident = (_ident);						\
+	c->length = 0;							\
+									\
+	if ((_data) != NULL) {						\
+		m_copyback((_m), sizeof(*c), (_size), (_data));		\
+		c->length += (_size);					\
+	}								\
+									\
+	c->length = htole16(c->length);					\
+} while (0)
+
+/* L2CAP_InfoReq */
+#define	_ng_l2cap_info_req(_m, _ident, _type)				\
+do {									\
+	struct _info_req {						\
+		ng_l2cap_cmd_hdr_t	 hdr;				\
+		ng_l2cap_info_req_cp	 param;				\
+	} __attribute__ ((packed))	*c = NULL;			\
+									\
+	MGETHDR((_m), M_DONTWAIT, MT_DATA);				\
+	if ((_m) == NULL)						\
+		break;							\
+									\
+	(_m)->m_pkthdr.len = (_m)->m_len = sizeof(*c);			\
+									\
+	c = mtod((_m), struct _info_req *);				\
+	c->hdr.code = NG_L2CAP_INFO_REQ;				\
+	c->hdr.ident = (_ident);					\
+	c->hdr.length = htole16(sizeof(c->param));			\
+									\
+	c->param.type = htole16((_type));				\
+} while (0)
+
+/* L2CAP_InfoRsp */
+#define	_ng_l2cap_info_rsp(_m, _ident, _type, _result, _mtu)		\
+do {									\
+	struct _info_rsp {						\
+		ng_l2cap_cmd_hdr_t	 hdr;				\
+		ng_l2cap_info_rsp_cp	 param;				\
+		ng_l2cap_info_rsp_data_t data;				\
+	} __attribute__ ((packed))	*c = NULL;			\
+									\
+	MGETHDR((_m), M_DONTWAIT, MT_DATA);				\
+	if ((_m) == NULL) 						\
+		break;							\
+									\
+	c = mtod((_m), struct _info_rsp *);				\
+	c->hdr.code = NG_L2CAP_INFO_REQ;				\
+	c->hdr.ident = (_ident);					\
+	c->hdr.length = sizeof(c->param);				\
+									\
+	c->param.type = htole16((_type));				\
+	c->param.result = htole16((_result));				\
+									\
+	if ((_result) == NG_L2CAP_SUCCESS) {				\
+		switch ((_type)) {					\
+		case NG_L2CAP_CONNLESS_MTU:				\
+			c->data.mtu.mtu = htole16((_mtu));		\
+			c->hdr.length += sizeof((c->data.mtu.mtu));	\
+			break;						\
+		}							\
+	}								\
+									\
+	(_m)->m_pkthdr.len = (_m)->m_len = sizeof(c->hdr) +		\
+					c->hdr.length;			\
+									\
+	c->hdr.length = htole16(c->hdr.length);		 		\
+} while (0)
+
+void ng_l2cap_con_wakeup              (ng_l2cap_con_p);
+void ng_l2cap_con_fail                (ng_l2cap_con_p, u_int16_t);
+void ng_l2cap_process_command_timeout (node_p, hook_p, void *, int);
+
+#endif /* ndef _NETGRAPH_L2CAP_CMDS_H_ */
+
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 */
+
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.h b/sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.h
new file mode 100644
index 0000000..26fcac5
--- /dev/null
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.h
@@ -0,0 +1,38 @@
+/*
+ * ng_l2cap_evnt.h
+ *
+ * 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.h,v 1.2 2002/04/16 00:43:56 max Exp $
+ * $FreeBSD$
+ */
+
+#ifndef _NETGRAPH_L2CAP_EVNT_H_
+#define _NETGRAPH_L2CAP_EVNT_H_
+
+int ng_l2cap_receive (ng_l2cap_con_p);
+
+#endif /* ndef _NETGRAPH_L2CAP_EVNT_H_ */
+
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.c b/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.c
new file mode 100644
index 0000000..f505158
--- /dev/null
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.c
@@ -0,0 +1,806 @@
+/*
+ * ng_l2cap_llpi.c
+ *
+ * Copyright (c) Maksim Yevmenkin <m_evmenkin@yahoo.com>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ * $Id: ng_l2cap_llpi.c,v 1.16 2002/09/04 21:38:38 max Exp $
+ * $FreeBSD$
+ */
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/kernel.h>
+#include <sys/endian.h>
+#include <sys/malloc.h>
+#include <sys/mbuf.h>
+#include <sys/queue.h>
+#include <netgraph/ng_message.h>
+#include <netgraph/netgraph.h>
+#include "ng_bluetooth.h"
+#include "ng_hci.h"
+#include "ng_l2cap.h"
+#include "ng_l2cap_var.h"
+#include "ng_l2cap_cmds.h"
+#include "ng_l2cap_evnt.h"
+#include "ng_l2cap_llpi.h"
+#include "ng_l2cap_ulpi.h"
+#include "ng_l2cap_misc.h"
+
+/******************************************************************************
+ ******************************************************************************
+ **                 Lower Layer Protocol (HCI) Interface module
+ ******************************************************************************
+ ******************************************************************************/
+
+/*
+ * Send LP_ConnectReq event to the lower layer protocol. Create new connection
+ * descriptor and initialize it. Create LP_ConnectReq event and send it to the
+ * lower layer, then adjust connection state and start timer. The function WILL
+ * FAIL if connection to the remote unit already exists.
+ */
+
+int
+ng_l2cap_lp_con_req(ng_l2cap_p l2cap, bdaddr_p bdaddr)
+{
+	struct ng_mesg		*msg = NULL;
+	ng_hci_lp_con_req_ep	*ep = NULL;
+	ng_l2cap_con_p		 con = NULL;
+	int			 error = 0;
+
+	/* Verify that we DO NOT have connection to the remote unit */
+	con = ng_l2cap_con_by_addr(l2cap, bdaddr);
+	if (con != NULL) {
+		NG_L2CAP_ALERT(
+"%s: %s - unexpected LP_ConnectReq event. " \
+"Connection already exists, state=%d, con_handle=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), con->state, 
+			con->con_handle);
+
+		return (EEXIST);
+	}
+
+	/* Check if lower layer protocol is still connected */
+	if (l2cap->hci == NULL || NG_HOOK_NOT_VALID(l2cap->hci)) {
+		NG_L2CAP_ERR(
+"%s: %s - hook \"%s\" is not connected or valid\n",
+			__func__, NG_NODE_NAME(l2cap->node), NG_L2CAP_HOOK_HCI);
+
+		return (ENOTCONN);
+	}
+
+	/* Create and intialize new connection descriptor */
+	con = ng_l2cap_new_con(l2cap, bdaddr);
+	if (con == NULL)
+		return (ENOMEM);
+
+	/* Create and send LP_ConnectReq event */
+	NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_CON_REQ,
+		sizeof(*ep), M_NOWAIT);
+	if (msg == NULL) {
+		ng_l2cap_free_con(con);
+
+		return (ENOMEM);
+	}
+
+	ep = (ng_hci_lp_con_req_ep *) (msg->data);
+	bcopy(bdaddr, &ep->bdaddr, sizeof(ep->bdaddr));
+	ep->link_type = NG_HCI_LINK_ACL;
+
+	con->state = NG_L2CAP_W4_LP_CON_CFM;
+	ng_l2cap_lp_timeout(con);
+
+	NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->hci, NULL);
+	if (error != 0)
+		ng_l2cap_free_con(con); /* will remove timeout */
+	
+	return (error);
+} /* ng_l2cap_lp_con_req */
+
+/*
+ * Process LP_ConnectCfm event from the lower layer protocol. It could be 
+ * positive or negative. Verify remote unit address then stop the timer and 
+ * process event.
+ */
+
+int
+ng_l2cap_lp_con_cfm(ng_l2cap_p l2cap, struct ng_mesg *msg)
+{
+	ng_hci_lp_con_cfm_ep	*ep = NULL;
+	ng_l2cap_con_p		 con = NULL;
+	int			 error = 0;
+
+	/* Check message */
+	if (msg->header.arglen != sizeof(*ep)) {
+		NG_L2CAP_ALERT(
+"%s: %s - invalid LP_ConnectCfm[Neg] message size\n",
+			__func__, NG_NODE_NAME(l2cap->node));
+		error = EMSGSIZE;
+		goto out;
+	}
+
+	ep = (ng_hci_lp_con_cfm_ep *) (msg->data);
+
+	/* Check if we have requested/accepted this connection */
+	con = ng_l2cap_con_by_addr(l2cap, &ep->bdaddr);
+	if (con == NULL) {
+		NG_L2CAP_ERR(
+"%s: %s - unexpected LP_ConnectCfm event. Connection does not exist\n",
+			__func__, NG_NODE_NAME(l2cap->node));
+		error = ENOENT;
+		goto out;
+	}
+
+	/* Check connection state */
+	if (con->state != NG_L2CAP_W4_LP_CON_CFM) {
+		NG_L2CAP_ALERT(
+"%s: %s - unexpected LP_ConnectCfm event. " \
+"Invalid connection state, state=%d, con_handle=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), con->state, 
+			con->con_handle);
+		error = EINVAL;
+		goto out;
+	}
+
+	/*
+	 * Looks like it is our confirmation. It is safe now to cancel 
+	 * connection timer and notify upper layer.
+ 	 */
+
+	ng_l2cap_lp_untimeout(con);
+
+	if (ep->status == 0) {
+		con->state = NG_L2CAP_CON_OPEN;
+		con->con_handle = ep->con_handle;
+		ng_l2cap_lp_deliver(con);
+	} else {
+		/* Negative confirmation - remove connection descriptor */
+		con->state = NG_L2CAP_CON_CLOSED;
+		ng_l2cap_con_fail(con, ep->status);
+	}
+out:
+	return (error);
+} /* ng_l2cap_lp_con_cfm */
+
+/*
+ * Process LP_ConnectInd event from the lower layer protocol. This is a good 
+ * place to put some extra check on remote unit address and/or class. We could
+ * even forward this information to control hook (or check against internal
+ * black list) and thus implement some kind of firewall. But for now be simple 
+ * and create new connection descriptor, start timer and send LP_ConnectRsp 
+ * event (i.e. accept connection).
+ */
+
+int
+ng_l2cap_lp_con_ind(ng_l2cap_p l2cap, struct ng_mesg *msg)
+{
+	ng_hci_lp_con_ind_ep	*ep = NULL;
+	ng_hci_lp_con_rsp_ep	*rp = NULL;
+	struct ng_mesg		*rsp = NULL;
+	ng_l2cap_con_p		 con = NULL;
+	int			 error = 0;
+
+	/* Check message */
+	if (msg->header.arglen != sizeof(*ep)) {
+		NG_L2CAP_ALERT(
+"%s: %s - invalid LP_ConnectInd message size\n",
+			__func__, NG_NODE_NAME(l2cap->node));
+		error = EMSGSIZE;
+		goto out;
+	}
+
+ 	ep = (ng_hci_lp_con_ind_ep *) (msg->data);
+
+	/* Make sure we have only one connection to the remote unit */
+	con = ng_l2cap_con_by_addr(l2cap, &ep->bdaddr);
+	if (con != NULL) {
+		NG_L2CAP_ALERT(
+"%s: %s - unexpected LP_ConnectInd event. " \
+"Connection already exists, state=%d, con_handle=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), con->state, 
+			con->con_handle);
+		error = EEXIST;
+		goto out;
+	}
+
+	/* Check if lower layer protocol is still connected */
+	if (l2cap->hci == NULL || NG_HOOK_NOT_VALID(l2cap->hci)) {
+		NG_L2CAP_ERR(
+"%s: %s - hook \"%s\" is not connected or valid",
+			__func__, NG_NODE_NAME(l2cap->node), NG_L2CAP_HOOK_HCI);
+		error = ENOTCONN;
+		goto out;
+	}
+
+	/* Create and intialize new connection descriptor */
+	con = ng_l2cap_new_con(l2cap, &ep->bdaddr);
+	if (con == NULL) {
+		error = ENOMEM;
+		goto out;
+	}
+
+	/* Create and send LP_ConnectRsp event */
+	NG_MKMESSAGE(rsp, NGM_HCI_COOKIE, NGM_HCI_LP_CON_RSP,
+		sizeof(*rp), M_NOWAIT);
+	if (msg == NULL) {
+		ng_l2cap_free_con(con);
+		error = ENOMEM;
+		goto out;
+	}
+
+	rp = (ng_hci_lp_con_rsp_ep *)(rsp->data);
+	rp->status = 0x00; /* accept connection */
+	rp->link_type = NG_HCI_LINK_ACL;
+	bcopy(&ep->bdaddr, &rp->bdaddr, sizeof(rp->bdaddr));
+
+	con->state = NG_L2CAP_W4_LP_CON_CFM;
+	ng_l2cap_lp_timeout(con);
+
+	NG_SEND_MSG_HOOK(error, l2cap->node, rsp, l2cap->hci, NULL);
+	if (error != 0)
+		ng_l2cap_free_con(con); /* will remove timeout */
+out:
+	return (error);
+} /* ng_hci_lp_con_ind */
+
+/*
+ * Process LP_DisconnectInd event from the lower layer protocol. We have been
+ * disconnected from the remote unit. So notify the upper layer protocol.
+ */
+
+int
+ng_l2cap_lp_discon_ind(ng_l2cap_p l2cap, struct ng_mesg *msg)
+{
+	ng_hci_lp_discon_ind_ep	*ep = NULL;
+	ng_l2cap_con_p		 con = NULL;
+	int			 error = 0;
+
+	/* Check message */
+	if (msg->header.arglen != sizeof(*ep)) {
+		NG_L2CAP_ALERT(
+"%s: %s - invalid LP_DisconnectInd message size\n",
+			__func__, NG_NODE_NAME(l2cap->node));
+		error = EMSGSIZE;
+		goto out;
+	}
+
+	ep = (ng_hci_lp_discon_ind_ep *) (msg->data);
+
+	/* Check if we have this connection */
+	con = ng_l2cap_con_by_handle(l2cap, ep->con_handle);
+	if (con == NULL) {
+		NG_L2CAP_ERR(
+"%s: %s - unexpected LP_DisconnectInd event. " \
+"Connection does not exist, con_handle=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), ep->con_handle);
+		error = ENOENT;
+		goto out;
+	}
+
+	/* XXX Verify connection state -- do we need to check this? */
+	if (con->state != NG_L2CAP_CON_OPEN) {
+		NG_L2CAP_ERR(
+"%s: %s - unexpected LP_DisconnectInd event. " \
+"Invalid connection state, state=%d, con_handle=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), con->state, 
+			con->con_handle);
+		error = EINVAL;
+		goto out;
+	}
+
+	/* Notify upper layer and remove connection */
+	con->state = NG_L2CAP_CON_CLOSED;
+	ng_l2cap_con_fail(con, ep->reason);
+out:
+	return (error);
+} /* ng_l2cap_lp_discon_ind */
+
+/*
+ * Send LP_QoSSetupReq event to the lower layer protocol
+ */
+
+int
+ng_l2cap_lp_qos_req(ng_l2cap_p l2cap, u_int16_t con_handle,
+		ng_l2cap_flow_p flow)
+{
+	struct ng_mesg		*msg = NULL;
+	ng_hci_lp_qos_req_ep	*ep = NULL;
+	ng_l2cap_con_p		 con = NULL;
+	int			 error = 0;
+
+	/* Verify that we have this connection */
+	con = ng_l2cap_con_by_handle(l2cap, con_handle);
+	if (con == NULL) {
+		NG_L2CAP_ERR(
+"%s: %s - unexpected LP_QoSSetupReq event. " \
+"Connection does not exist, con_handle=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), con_handle);
+
+		return (ENOENT);
+	}
+
+	/* Verify connection state */
+	if (con->state != NG_L2CAP_CON_OPEN) {
+		NG_L2CAP_ERR(
+"%s: %s - unexpected LP_QoSSetupReq event. " \
+"Invalid connection state, state=%d, con_handle=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), con->state,
+			con->con_handle);
+
+		return (EINVAL);
+	}
+
+	/* Check if lower layer protocol is still connected */
+	if (l2cap->hci == NULL || NG_HOOK_NOT_VALID(l2cap->hci)) {
+		NG_L2CAP_ERR(
+"%s: %s - hook \"%s\" is not connected or valid",
+			__func__, NG_NODE_NAME(l2cap->node), NG_L2CAP_HOOK_HCI);
+
+		return (ENOTCONN);
+	}
+
+	/* Create and send LP_QoSSetupReq event */
+	NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_QOS_REQ,
+		sizeof(*ep), M_NOWAIT);
+	if (msg == NULL)
+		return (ENOMEM);
+
+	ep = (ng_hci_lp_qos_req_ep *) (msg->data);
+	ep->con_handle = con_handle;
+	ep->flags = flow->flags;
+	ep->service_type = flow->service_type;
+	ep->token_rate = flow->token_rate;
+	ep->peak_bandwidth = flow->peak_bandwidth;
+	ep->latency = flow->latency;
+	ep->delay_variation = flow->delay_variation;
+
+	NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->hci, NULL);
+	
+	return (error);
+} /* ng_l2cap_lp_con_req */
+
+/*
+ * Process LP_QoSSetupCfm from the lower layer protocol
+ */
+
+int
+ng_l2cap_lp_qos_cfm(ng_l2cap_p l2cap, struct ng_mesg *msg)
+{
+	ng_hci_lp_qos_cfm_ep	*ep = NULL;
+	int			 error = 0;
+
+	/* Check message */
+	if (msg->header.arglen != sizeof(*ep)) {
+		NG_L2CAP_ALERT(
+"%s: %s - invalid LP_QoSSetupCfm[Neg] message size\n",
+			__func__, NG_NODE_NAME(l2cap->node));
+		error = EMSGSIZE;
+		goto out;
+	}
+
+	ep = (ng_hci_lp_qos_cfm_ep *) (msg->data);
+	/* XXX FIXME do something */
+out:
+	return (error);
+} /* ng_l2cap_lp_qos_cfm */
+
+/*
+ * Process LP_QoSViolationInd event from the lower layer protocol. Lower 
+ * layer protocol has detected QoS Violation, so we MUST notify the 
+ * upper layer.
+ */
+
+int
+ng_l2cap_lp_qos_ind(ng_l2cap_p l2cap, struct ng_mesg *msg)
+{
+	ng_hci_lp_qos_ind_ep	*ep = NULL;
+	ng_l2cap_con_p		 con = NULL;
+	int			 error = 0;
+
+	/* Check message */
+	if (msg->header.arglen != sizeof(*ep)) {
+		NG_L2CAP_ALERT(
+"%s: %s - invalid LP_QoSViolation message size\n",
+			__func__, NG_NODE_NAME(l2cap->node));
+		error = EMSGSIZE;
+		goto out;
+	}
+
+	ep = (ng_hci_lp_qos_ind_ep *) (msg->data);
+
+	/* Check if we have this connection */
+	con = ng_l2cap_con_by_handle(l2cap, ep->con_handle);
+	if (con == NULL) {
+		NG_L2CAP_ERR(
+"%s: %s - unexpected LP_QoSViolationInd event. " \
+"Connection does not exist, con_handle=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), ep->con_handle);
+		error = ENOENT;
+		goto out;
+	}
+
+	/* Verify connection state */
+	if (con->state != NG_L2CAP_CON_OPEN) {
+		NG_L2CAP_ERR(
+"%s: %s - unexpected LP_QoSViolationInd event. " \
+"Invalid connection state, state=%d, con_handle=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), con->state, 
+			con->con_handle);
+		error = EINVAL;
+		goto out;
+	}
+
+	/* XXX FIXME Notify upper layer and terminate channels if required */
+out:
+	return (error);
+} /* ng_l2cap_qos_ind */
+
+/*
+ * Prepare L2CAP packet. Prepend packet with L2CAP packet header and then 
+ * segment it according to HCI MTU.
+ */
+
+int
+ng_l2cap_lp_send(ng_l2cap_con_p con, u_int16_t dcid, struct mbuf *m0)
+{
+	ng_l2cap_p		 l2cap = con->l2cap;
+	ng_l2cap_hdr_t		*l2cap_hdr = NULL;
+        ng_hci_acldata_pkt_t	*acl_hdr = NULL;
+        struct mbuf		*m_last = NULL, *m = NULL;
+        int			 len, flag = NG_HCI_PACKET_START;
+
+	KASSERT((con->tx_pkt == NULL),
+("%s: %s - another packet pending?!\n", __func__, NG_NODE_NAME(l2cap->node)));
+	KASSERT((l2cap->pkt_size > 0),
+("%s: %s - invalid l2cap->pkt_size?!\n", __func__, NG_NODE_NAME(l2cap->node)));
+
+	/* Prepend mbuf with L2CAP header */
+	m0 = ng_l2cap_prepend(m0, sizeof(*l2cap_hdr));
+	if (m0 == NULL) {
+		NG_L2CAP_ALERT(
+"%s: %s - ng_l2cap_prepend(%d) failed\n",
+			__func__, NG_NODE_NAME(l2cap->node),
+			sizeof(*l2cap_hdr));
+
+		goto fail;
+	}
+
+	l2cap_hdr = mtod(m0, ng_l2cap_hdr_t *);
+	l2cap_hdr->length = htole16(m0->m_pkthdr.len - sizeof(*l2cap_hdr));
+	l2cap_hdr->dcid = htole16(dcid);
+
+	/*
+	 * Segment single L2CAP packet according to the HCI layer MTU. Convert 
+	 * each segment into ACL data packet and prepend it with ACL data packet
+	 * header. Link all segments together via m_nextpkt link. 
+ 	 *
+	 * XXX BC (Broadcast flag) will always be 0 (zero).
+	 */
+
+	while (m0 != NULL) {
+		/* Check length of the packet against HCI MTU */
+		len = m0->m_pkthdr.len;
+		if (len > l2cap->pkt_size) {
+			m = m_split(m0, l2cap->pkt_size, M_DONTWAIT);
+			if (m == NULL) {
+				NG_L2CAP_ALERT(
+"%s: %s - m_split(%d) failed\n",	__func__, NG_NODE_NAME(l2cap->node),
+					l2cap->pkt_size);
+				goto fail;
+			}
+
+			len = l2cap->pkt_size;
+		}
+
+		/* Convert packet fragment into ACL data packet */
+		m0 = ng_l2cap_prepend(m0, sizeof(*acl_hdr));
+		if (m0 == NULL) {
+			NG_L2CAP_ALERT(
+"%s: %s - ng_l2cap_prepend(%d) failed\n",
+				__func__, NG_NODE_NAME(l2cap->node),
+				sizeof(*acl_hdr));
+			goto fail;
+		}
+
+		acl_hdr = mtod(m0, ng_hci_acldata_pkt_t *);
+		acl_hdr->type = NG_HCI_ACL_DATA_PKT;
+		acl_hdr->length = htole16(len);
+		acl_hdr->con_handle = htole16(NG_HCI_MK_CON_HANDLE(
+					con->con_handle, flag, 0));
+
+		/* Add fragment to the chain */
+		m0->m_nextpkt = NULL;
+
+		if (con->tx_pkt == NULL)
+			con->tx_pkt = m_last = m0;
+		else {
+			m_last->m_nextpkt = m0;
+			m_last = m0;
+		}
+
+		NG_L2CAP_INFO(
+"%s: %s - attaching ACL packet, con_handle=%d, PB=%#x, length=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), con->con_handle,
+			flag, len);
+
+		m0 = m;
+		m = NULL;
+		flag = NG_HCI_PACKET_FRAGMENT;
+	}
+
+	return (0);
+fail:
+	NG_FREE_M(m0);
+	NG_FREE_M(m);
+
+	while (con->tx_pkt != NULL) {
+		m = con->tx_pkt->m_nextpkt;
+		m_freem(con->tx_pkt);
+		con->tx_pkt = m;
+	}
+
+	return (ENOBUFS);
+} /* ng_l2cap_lp_send */
+
+/*
+ * Receive ACL data packet from the HCI layer. First strip ACL packet header
+ * and get connection handle, PB (Packet Boundary) flag and payload length.
+ * Then find connection descriptor and verify its state. Then process ACL 
+ * packet as follows.
+ * 
+ * 1) If we got first segment (pb == NG_HCI_PACKET_START) then extract L2CAP 
+ *    header and get total length of the L2CAP packet. Then start new L2CAP 
+ *    packet.
+ *
+ * 2) If we got other (then first :) segment (pb == NG_HCI_PACKET_FRAGMENT)
+ *    then add segment to the packet.
+ */
+
+int
+ng_l2cap_lp_receive(ng_l2cap_p l2cap, struct mbuf *m)
+{
+	ng_hci_acldata_pkt_t	*acl_hdr = NULL;
+	ng_l2cap_hdr_t		*l2cap_hdr = NULL;
+	ng_l2cap_con_p		 con = NULL;
+	u_int16_t		 con_handle, length, pb;
+	int			 error = 0;
+
+	/* Check ACL data packet */
+	if (m->m_pkthdr.len < sizeof(*acl_hdr)) {
+		NG_L2CAP_ERR(
+"%s: %s - invalid ACL data packet. Packet too small, length=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), m->m_pkthdr.len);
+		error = EMSGSIZE;
+		goto drop;
+	}
+
+	/* Strip ACL data packet header */
+	NG_L2CAP_M_PULLUP(m, sizeof(*acl_hdr));
+	if (m == NULL)
+		return (ENOBUFS);
+
+	acl_hdr = mtod(m, ng_hci_acldata_pkt_t *);
+	m_adj(m, sizeof(*acl_hdr));
+
+	/* Get ACL connection handle, PB flag and payload length */
+	acl_hdr->con_handle = le16toh(acl_hdr->con_handle);
+	con_handle = NG_HCI_CON_HANDLE(acl_hdr->con_handle);
+	pb = NG_HCI_PB_FLAG(acl_hdr->con_handle);
+	length = le16toh(acl_hdr->length);
+
+	NG_L2CAP_INFO(
+"%s: %s - got ACL data packet, con_handle=%d, PB=%#x, length=%d\n",
+		__func__, NG_NODE_NAME(l2cap->node), con_handle, pb, length);
+
+	/* Get connection descriptor */
+	con = ng_l2cap_con_by_handle(l2cap, con_handle);
+	if (con == NULL) {
+		NG_L2CAP_ERR(
+"%s: %s - unexpected ACL data packet. " \
+"Connection does not exist, con_handle=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), con_handle);
+		error = ENOENT;
+		goto drop;
+	}
+
+	/* Verify connection state */
+	if (con->state != NG_L2CAP_CON_OPEN) {
+		NG_L2CAP_ERR(
+"%s: %s - unexpected ACL data packet. Invalid connection state=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), con->state);
+		error = EHOSTDOWN;
+		goto drop;
+	}
+
+	/* Process packet */
+	if (pb == NG_HCI_PACKET_START) {
+		if (con->rx_pkt != NULL) {
+			NG_L2CAP_ERR(
+"%s: %s - dropping incomplete L2CAP packet, got %d bytes, want %d bytes\n",
+				__func__, NG_NODE_NAME(l2cap->node),
+				con->rx_pkt->m_pkthdr.len, con->rx_pkt_len);
+			NG_FREE_M(con->rx_pkt);
+			con->rx_pkt_len = 0;
+		}
+
+		/* Get L2CAP header */
+		if (m->m_pkthdr.len < sizeof(*l2cap_hdr)) {
+			NG_L2CAP_ERR(
+"%s: %s - invalid L2CAP packet start fragment. Packet too small, length=%d\n",
+				__func__, NG_NODE_NAME(l2cap->node),
+				m->m_pkthdr.len);
+			error = EMSGSIZE;
+			goto drop;
+		}
+
+		NG_L2CAP_M_PULLUP(m, sizeof(*l2cap_hdr));
+		if (m == NULL)
+			return (ENOBUFS);
+
+		l2cap_hdr = mtod(m, ng_l2cap_hdr_t *);
+
+		NG_L2CAP_INFO(
+"%s: %s - staring new L2CAP packet, con_handle=%d, length=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), con_handle,
+			le16toh(l2cap_hdr->length));
+
+		/* Start new L2CAP packet */
+		con->rx_pkt = m;
+		con->rx_pkt_len = le16toh(l2cap_hdr->length)+sizeof(*l2cap_hdr);
+	} else if (pb == NG_HCI_PACKET_FRAGMENT) {
+		if (con->rx_pkt == NULL) {
+			NG_L2CAP_ERR(
+"%s: %s - unexpected ACL data packet fragment, con_handle=%d\n",
+				__func__, NG_NODE_NAME(l2cap->node), 
+				con->con_handle);
+			goto drop;
+		}
+
+		/* Add fragment to the L2CAP packet */
+		m_cat(con->rx_pkt, m);
+		con->rx_pkt->m_pkthdr.len += length;
+	} else {
+		NG_L2CAP_ERR(
+"%s: %s - invalid ACL data packet. Invalid PB flag=%#x\n",
+			__func__, NG_NODE_NAME(l2cap->node), pb);
+		error = EINVAL;
+		goto drop;
+	}
+
+	con->rx_pkt_len -= length;
+	if (con->rx_pkt_len < 0) {
+		NG_L2CAP_ALERT(
+"%s: %s - packet length mismatch. Got %d bytes, offset %d bytes\n",
+			__func__, NG_NODE_NAME(l2cap->node), 
+			con->rx_pkt->m_pkthdr.len, con->rx_pkt_len);
+		NG_FREE_M(con->rx_pkt);
+		con->rx_pkt_len = 0;
+	} else if (con->rx_pkt_len == 0) {
+		/* OK, we have got complete L2CAP packet, so process it */
+		error = ng_l2cap_receive(con);
+		con->rx_pkt = NULL;
+		con->rx_pkt_len = 0;
+	}
+
+	return (error);
+
+drop:
+	NG_FREE_M(m);
+
+	return (error);
+} /* ng_l2cap_lp_receive */
+
+/*
+ * Send queued ACL packets to the HCI layer
+ */
+
+void
+ng_l2cap_lp_deliver(ng_l2cap_con_p con)
+{
+	ng_l2cap_p	 l2cap = con->l2cap;
+	struct mbuf	*m = NULL;
+	int		 error;
+
+	/* Check connection */
+	if (con->state != NG_L2CAP_CON_OPEN)
+		return;
+
+	if (con->tx_pkt == NULL)
+		ng_l2cap_con_wakeup(con);
+
+	if (con->tx_pkt == NULL)
+		return;
+
+	/* Check if lower layer protocol is still connected */
+	if (l2cap->hci == NULL || NG_HOOK_NOT_VALID(l2cap->hci)) {
+		NG_L2CAP_ERR(
+"%s: %s - hook \"%s\" is not connected or valid",
+			__func__, NG_NODE_NAME(l2cap->node), NG_L2CAP_HOOK_HCI);
+
+		goto drop; /* XXX what to do with "pending"? */
+	}
+
+	/* Send ACL data packets */
+	while (con->pending < con->l2cap->num_pkts && con->tx_pkt != NULL) {
+		m = con->tx_pkt;
+		con->tx_pkt = con->tx_pkt->m_nextpkt;
+		m->m_nextpkt = NULL;
+
+		NG_L2CAP_INFO(
+"%s: %s - sending ACL packet, con_handle=%d, len=%d\n", 
+			__func__, NG_NODE_NAME(l2cap->node), con->con_handle, 
+			m->m_pkthdr.len);
+
+		NG_SEND_DATA_ONLY(error, l2cap->hci, m);
+		if (error != 0) {
+			NG_L2CAP_ERR(
+"%s: %s - could not send ACL data packet, con_handle=%d, error=%d\n",
+				__func__, NG_NODE_NAME(l2cap->node), 
+				con->con_handle, error);
+
+			goto drop; /* XXX what to do with "pending"? */
+		}
+
+		con->pending ++;
+	}
+
+	NG_L2CAP_INFO(
+"%s: %s - %d ACL packets have been sent, con_handle=%d\n",
+		__func__, NG_NODE_NAME(l2cap->node), con->pending, 
+		con->con_handle);
+
+	return;
+
+drop:
+	while (con->tx_pkt != NULL) {
+		m = con->tx_pkt->m_nextpkt;
+		m_freem(con->tx_pkt);
+		con->tx_pkt = m;
+	}
+} /* ng_l2cap_lp_deliver */
+
+/*
+ * Process connection timeout. Remove connection from the list. If there
+ * are any channels that wait for the connection then notify them. Free 
+ * connection descriptor.
+ */
+
+void
+ng_l2cap_process_lp_timeout(node_p node, hook_p hook, void *arg1, int arg2)
+{
+	ng_l2cap_con_p	con = (ng_l2cap_con_p) arg1;
+	ng_l2cap_p	l2cap = con->l2cap;
+
+	NG_L2CAP_ERR(
+"%s: %s - ACL connection timeout\n", __func__, NG_NODE_NAME(l2cap->node));
+
+	/*
+	 * Notify channels that connection has timed out. This will remove 
+	 * connection, channels and pending commands.
+	 */
+
+	con->state = NG_L2CAP_CON_CLOSED;
+	ng_l2cap_con_fail(con, NG_L2CAP_TIMEOUT);
+} /* ng_l2cap_process_lp_timeout */
+
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.h b/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.h
new file mode 100644
index 0000000..161761e
--- /dev/null
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.h
@@ -0,0 +1,48 @@
+/*
+ * ng_l2cap_llpi.h
+ *
+ * Copyright (c) Maksim Yevmenkin <m_evmenkin@yahoo.com>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ * $Id: ng_l2cap_llpi.h,v 1.6 2002/04/16 00:43:57 max Exp $
+ * $FreeBSD$
+ */
+
+#ifndef _NETGRAPH_L2CAP_LLPI_H_
+#define _NETGRAPH_L2CAP_LLPI_H_
+
+int  ng_l2cap_lp_con_req         (ng_l2cap_p, bdaddr_p);
+int  ng_l2cap_lp_con_cfm         (ng_l2cap_p, struct ng_mesg *);
+int  ng_l2cap_lp_con_ind         (ng_l2cap_p, struct ng_mesg *);
+int  ng_l2cap_lp_discon_ind      (ng_l2cap_p, struct ng_mesg *);
+int  ng_l2cap_lp_qos_req         (ng_l2cap_p, u_int16_t, ng_l2cap_flow_p);
+int  ng_l2cap_lp_qos_cfm         (ng_l2cap_p, struct ng_mesg *);
+int  ng_l2cap_lp_qos_ind         (ng_l2cap_p, struct ng_mesg *);
+int  ng_l2cap_lp_send            (ng_l2cap_con_p, u_int16_t,struct mbuf *);
+int  ng_l2cap_lp_receive         (ng_l2cap_p, struct mbuf *);
+void ng_l2cap_lp_deliver         (ng_l2cap_con_p);
+void ng_l2cap_process_lp_timeout (node_p, hook_p, void *, int);
+
+#endif /* ndef _NETGRAPH_L2CAP_LLPI_H_ */
+
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_main.c b/sys/netgraph/bluetooth/l2cap/ng_l2cap_main.c
new file mode 100644
index 0000000..1dfc4c0
--- /dev/null
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_main.c
@@ -0,0 +1,736 @@
+/*
+ * ng_l2cap_main.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_main.c,v 1.24 2002/09/04 21:38:38 max Exp $
+ * $FreeBSD$
+ */
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/kernel.h>
+#include <sys/malloc.h>
+#include <sys/mbuf.h>
+#include <sys/queue.h>
+#include <netgraph/ng_message.h>
+#include <netgraph/netgraph.h>
+#include <netgraph/ng_parse.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"
+#include "ng_l2cap_prse.h"
+
+/******************************************************************************
+ ******************************************************************************
+ **  This node implements Link Layer Control and Adaptation Protocol (L2CAP)
+ ******************************************************************************
+ ******************************************************************************/
+
+/* MALLOC define */
+#ifdef NG_SEPARATE_MALLOC
+MALLOC_DEFINE(M_NETGRAPH_L2CAP, "netgraph_l2cap", 
+	"Netgraph Bluetooth L2CAP node");
+#else
+#define M_NETGRAPH_L2CAP M_NETGRAPH
+#endif /* NG_SEPARATE_MALLOC */
+
+/* Netgraph node methods */
+static	ng_constructor_t	ng_l2cap_constructor;
+static	ng_shutdown_t		ng_l2cap_shutdown;
+static	ng_newhook_t		ng_l2cap_newhook;
+static	ng_connect_t		ng_l2cap_connect;
+static	ng_disconnect_t		ng_l2cap_disconnect;
+static	ng_rcvmsg_t		ng_l2cap_lower_rcvmsg;
+static	ng_rcvmsg_t		ng_l2cap_upper_rcvmsg;
+static	ng_rcvmsg_t		ng_l2cap_default_rcvmsg;
+static	ng_rcvdata_t		ng_l2cap_rcvdata;
+
+/* Netgraph node type descriptor */
+static	struct ng_type		typestruct = {
+	NG_ABI_VERSION,
+	NG_L2CAP_NODE_TYPE,	/* typename */
+	NULL,			/* modevent */
+	ng_l2cap_constructor,	/* constructor */
+	ng_l2cap_default_rcvmsg,/* control message */
+	ng_l2cap_shutdown,	/* destructor */
+	ng_l2cap_newhook,	/* new hook */
+	NULL,			/* findhook */
+	ng_l2cap_connect,	/* connect hook */
+	ng_l2cap_rcvdata,	/* data */
+	ng_l2cap_disconnect,	/* disconnect hook */
+	ng_l2cap_cmdlist	/* node command list */
+};
+NETGRAPH_INIT(l2cap, &typestruct);
+MODULE_VERSION(ng_l2cap, NG_BLUETOOTH_VERSION);
+MODULE_DEPEND(ng_l2cap, ng_bluetooth, NG_BLUETOOTH_VERSION,
+        NG_BLUETOOTH_VERSION, NG_BLUETOOTH_VERSION);
+
+/*****************************************************************************
+ *****************************************************************************
+ **                   Netgraph methods implementation
+ *****************************************************************************
+ *****************************************************************************/
+
+static void ng_l2cap_cleanup          (ng_l2cap_p);
+static void ng_l2cap_destroy_channels (ng_l2cap_p);
+
+/*
+ * Create new instance of L2CAP node
+ */
+
+static int
+ng_l2cap_constructor(node_p node)
+{
+	ng_l2cap_p	l2cap = NULL;
+
+	/* Create new L2CAP node */
+	MALLOC(l2cap, ng_l2cap_p, sizeof(*l2cap),
+		M_NETGRAPH_L2CAP, M_NOWAIT|M_ZERO);
+	if (l2cap == NULL)
+		return (ENOMEM);
+
+	l2cap->node = node;
+	l2cap->debug = NG_L2CAP_WARN_LEVEL;
+
+	LIST_INIT(&l2cap->con_list);
+	LIST_INIT(&l2cap->chan_list);
+
+	NG_NODE_SET_PRIVATE(node, l2cap);
+	NG_NODE_FORCE_WRITER(node);
+
+	return (0);
+} /* ng_l2cap_constructor */
+
+/*
+ * Shutdown L2CAP node
+ */
+
+static int
+ng_l2cap_shutdown(node_p node)
+{
+	ng_l2cap_p	l2cap = (ng_l2cap_p) NG_NODE_PRIVATE(node);
+
+	NG_NODE_SET_PRIVATE(node, NULL);
+	NG_NODE_UNREF(node);
+
+	/* Clean up L2CAP node. Delete all connection, channels and commands */
+	l2cap->node = NULL;
+	ng_l2cap_cleanup(l2cap);
+
+	bzero(l2cap, sizeof(*l2cap));
+	FREE(l2cap, M_NETGRAPH_L2CAP);
+
+	return (0);
+} /* ng_l2cap_shutdown */
+
+/*
+ * Give our OK for a hook to be added. HCI layer is connected to the HCI 
+ * (NG_L2CAP_HOOK_HCI) hook. As per specification L2CAP layer MUST provide
+ * Procol/Service Multiplexing, so the L2CAP node provides separate hooks 
+ * for SDP (NG_L2CAP_HOOK_SDP), RFCOMM (NG_L2CAP_HOOK_RFCOMM) and TCP 
+ * (NG_L2CAP_HOOK_TCP) protcols. Unknown PSM will be forwarded to 
+ * NG_L2CAP_HOOK_ORPHAN hook. Control node/application is connected to 
+ * control (NG_L2CAP_HOOK_CTL) hook. 
+ */
+
+static int
+ng_l2cap_newhook(node_p node, hook_p hook, char const *name)
+{
+	ng_l2cap_p	 l2cap = (ng_l2cap_p) NG_NODE_PRIVATE(node);
+	hook_p		*h = NULL;
+
+	if (strcmp(name, NG_L2CAP_HOOK_HCI) == 0)
+		h = &l2cap->hci;
+	else if (strcmp(name, NG_L2CAP_HOOK_L2C) == 0)
+		h = &l2cap->l2c;
+	else if (strcmp(name, NG_L2CAP_HOOK_CTL) == 0)
+		h = &l2cap->ctl;
+	else
+		return (EINVAL);
+
+	if (*h != NULL)
+		return (EISCONN);
+
+	*h = hook;
+
+	return (0);
+} /* ng_l2cap_newhook */
+
+/*
+ * Give our final OK to connect hook. Nothing to do here.
+ */
+
+static int
+ng_l2cap_connect(hook_p hook)
+{
+	ng_l2cap_p	l2cap = (ng_l2cap_p) NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
+	int		error = 0;
+
+	if (hook == l2cap->hci)
+		NG_HOOK_SET_RCVMSG(hook, ng_l2cap_lower_rcvmsg);
+	else
+	if (hook == l2cap->l2c || hook == l2cap->ctl) {
+		NG_HOOK_SET_RCVMSG(hook, ng_l2cap_upper_rcvmsg);
+
+		/* Send delayed notification to the upper layer */
+		error = ng_send_fn(l2cap->node, hook, ng_l2cap_send_hook_info,
+				NULL, 0);
+	} else
+		error = EINVAL;
+
+	return (error);
+} /* ng_l2cap_connect */
+
+/*
+ * Disconnect the hook. For downstream hook we must notify upper layers.
+ * 
+ * XXX For upstream hooks this is really ugly :( Hook was disconnected and it 
+ * XXX is now too late to do anything. For now we just clean up our own mess
+ * XXX and remove all channels that use disconnected upstream hook. If we don't 
+ * XXX do that then L2CAP node can get out of sync with upper layers.
+ * XXX No notification will be sent to remote peer. 
+ */
+
+static int
+ng_l2cap_disconnect(hook_p hook)
+{
+	ng_l2cap_p	 l2cap = (ng_l2cap_p) NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
+	hook_p		*h = NULL;
+
+	if (hook == l2cap->hci) {
+		ng_l2cap_cleanup(l2cap);
+		h = &l2cap->hci;
+	} else
+	if (hook == l2cap->l2c) {
+		ng_l2cap_destroy_channels(l2cap);
+		h = &l2cap->l2c;
+	} else
+	if (hook == l2cap->ctl)
+		h = &l2cap->ctl;
+	else
+		return (EINVAL);
+
+	*h = NULL;
+
+	/* Shutdown when all hooks are disconnected */
+	if (NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) == 0 &&
+	    NG_NODE_IS_VALID(NG_HOOK_NODE(hook)))
+		ng_rmnode_self(NG_HOOK_NODE(hook));
+
+	return (0);
+} /* ng_l2cap_disconnect */
+
+/*
+ * Process control message from lower layer
+ */
+
+static int
+ng_l2cap_lower_rcvmsg(node_p node, item_p item, hook_p lasthook)
+{
+	ng_l2cap_p	 l2cap = (ng_l2cap_p) NG_NODE_PRIVATE(node);
+	struct ng_mesg	*msg = NGI_MSG(item); /* item still has message */
+	int		 error = 0;
+
+	switch (msg->header.typecookie) {
+	case NGM_HCI_COOKIE:
+		switch (msg->header.cmd) {
+		/* HCI node is ready */
+		case NGM_HCI_NODE_UP: {
+			ng_hci_node_up_ep	*ep = NULL;
+
+			if (msg->header.arglen != sizeof(*ep))
+				error = EMSGSIZE;
+			else {
+				ep = (ng_hci_node_up_ep *)(msg->data);
+
+				NG_L2CAP_INFO(
+"%s: %s - HCI node is up, bdaddr: %x:%x:%x:%x:%x:%x, " \
+"pkt_size=%d bytes, num_pkts=%d\n",	__func__, NG_NODE_NAME(l2cap->node),
+					ep->bdaddr.b[5], ep->bdaddr.b[4],
+					ep->bdaddr.b[3], ep->bdaddr.b[2],
+					ep->bdaddr.b[1], ep->bdaddr.b[0],
+					ep->pkt_size, ep->num_pkts);
+
+				bcopy(&ep->bdaddr, &l2cap->bdaddr,
+					sizeof(l2cap->bdaddr));
+				l2cap->pkt_size = ep->pkt_size;
+				l2cap->num_pkts = ep->num_pkts;
+
+				/* Notify upper layers */
+				ng_l2cap_send_hook_info(l2cap->node,
+					l2cap->l2c, NULL, 0);
+				ng_l2cap_send_hook_info(l2cap->node,
+					l2cap->ctl, NULL, 0);
+			}
+			} break;
+
+		case NGM_HCI_SYNC_CON_QUEUE: {
+			ng_hci_sync_con_queue_ep	*ep = NULL;
+			ng_l2cap_con_p			 con = NULL;
+
+			if (msg->header.arglen != sizeof(*ep))
+				error = EMSGSIZE;
+			else {
+				ep = (ng_hci_sync_con_queue_ep *)(msg->data);
+				con = ng_l2cap_con_by_handle(l2cap,
+							ep->con_handle);
+				if (con == NULL)
+					break;
+
+				NG_L2CAP_INFO(
+"%s: %s - sync HCI connection queue, con_handle=%d, pending=%d, completed=%d\n",
+					 __func__, NG_NODE_NAME(l2cap->node),
+					ep->con_handle, con->pending,
+					ep->completed);
+
+				con->pending -= ep->completed;
+				if (con->pending < 0) {
+					NG_L2CAP_WARN(
+"%s: %s - pending packet counter is out of sync! " \
+"con_handle=%d, pending=%d, completed=%d\n",	__func__, 
+						NG_NODE_NAME(l2cap->node),
+						con->con_handle, con->pending, 
+						ep->completed);
+
+					con->pending = 0;
+				}
+
+				ng_l2cap_lp_deliver(con);
+			}
+			} break;
+
+		/* LP_ConnectCfm[Neg] */
+		case NGM_HCI_LP_CON_CFM:
+			error = ng_l2cap_lp_con_cfm(l2cap, msg);
+			break;
+
+		/* LP_ConnectInd */
+		case NGM_HCI_LP_CON_IND:
+			error = ng_l2cap_lp_con_ind(l2cap, msg);
+			break;
+
+		/* LP_DisconnectInd */
+		case NGM_HCI_LP_DISCON_IND:
+			error = ng_l2cap_lp_discon_ind(l2cap, msg);
+			break;
+
+		/* LP_QoSSetupCfm[Neg] */
+		case NGM_HCI_LP_QOS_CFM:
+			error = ng_l2cap_lp_qos_cfm(l2cap, msg);
+			break;
+
+		/* LP_OoSViolationInd */
+		case NGM_HCI_LP_QOS_IND:
+			error = ng_l2cap_lp_qos_ind(l2cap, msg);
+			break;
+
+		default:
+			error = EINVAL;
+			break;
+		}
+		break;
+
+	default:
+		return (ng_l2cap_default_rcvmsg(node, item, lasthook));
+		/* NOT REACHED */
+	}
+
+	NG_FREE_ITEM(item);
+
+	return (error);
+} /* ng_l2cap_lower_rcvmsg */
+
+/*
+ * Process control message from upper layer
+ */
+
+static int
+ng_l2cap_upper_rcvmsg(node_p node, item_p item, hook_p lasthook)
+{
+	ng_l2cap_p	 l2cap = (ng_l2cap_p) NG_NODE_PRIVATE(node);
+	struct ng_mesg	*msg = NGI_MSG(item); /* item still has message */
+	int		 error = 0;
+
+	switch (msg->header.typecookie) {
+	case NGM_L2CAP_COOKIE:
+		switch (msg->header.cmd) {
+		/* L2CA_Connect */
+		case NGM_L2CAP_L2CA_CON:
+			error = ng_l2cap_l2ca_con_req(l2cap, msg);
+			break;
+
+		/* L2CA_ConnectRsp */
+		case NGM_L2CAP_L2CA_CON_RSP:
+			error = ng_l2cap_l2ca_con_rsp_req(l2cap, msg);
+			break;
+
+		/* L2CA_Config */
+		case NGM_L2CAP_L2CA_CFG:
+			error = ng_l2cap_l2ca_cfg_req(l2cap, msg);
+			break;
+
+		/* L2CA_ConfigRsp */
+		case NGM_L2CAP_L2CA_CFG_RSP:
+			error = ng_l2cap_l2ca_cfg_rsp_req(l2cap, msg);
+			break;
+
+		/* L2CA_Disconnect */
+		case NGM_L2CAP_L2CA_DISCON:
+			error = ng_l2cap_l2ca_discon_req(l2cap, msg);
+			break;
+
+		/* L2CA_GroupCreate */
+		case NGM_L2CAP_L2CA_GRP_CREATE:
+			error = ng_l2cap_l2ca_grp_create(l2cap, msg);
+			break;
+
+		/* L2CA_GroupClose */
+		case NGM_L2CAP_L2CA_GRP_CLOSE:
+			error = ng_l2cap_l2ca_grp_close(l2cap, msg);
+			break;
+
+		/* L2CA_GroupAddMember */
+		case NGM_L2CAP_L2CA_GRP_ADD_MEMBER:
+			error = ng_l2cap_l2ca_grp_add_member_req(l2cap, msg);
+			break;
+
+		/* L2CA_GroupDeleteMember */
+		case NGM_L2CAP_L2CA_GRP_REM_MEMBER:
+			error = ng_l2cap_l2ca_grp_rem_member(l2cap, msg);
+			break;
+
+		/* L2CA_GroupMembership */
+		case NGM_L2CAP_L2CA_GRP_MEMBERSHIP:
+			error = ng_l2cap_l2ca_grp_get_members(l2cap, msg);
+			break;
+
+		/* L2CA_Ping */
+		case NGM_L2CAP_L2CA_PING:
+			error = ng_l2cap_l2ca_ping_req(l2cap, msg);
+			break;
+
+		/* L2CA_GetInfo */
+		case NGM_L2CAP_L2CA_GET_INFO:
+			error = ng_l2cap_l2ca_get_info_req(l2cap, msg);
+			break;
+
+		/* L2CA_EnableCLT */
+		case NGM_L2CAP_L2CA_ENABLE_CLT:
+			error = ng_l2cap_l2ca_enable_clt(l2cap, msg);
+			break;
+
+		default:
+			return (ng_l2cap_default_rcvmsg(node, item, lasthook));
+			/* NOT REACHED */
+		}
+		break;
+
+	default:
+		return (ng_l2cap_default_rcvmsg(node, item, lasthook));
+		/* NOT REACHED */
+	}
+
+	NG_FREE_ITEM(item);
+
+	return (error);
+} /* ng_l2cap_upper_rcvmsg */
+
+/*
+ * Default control message processing routine
+ */
+
+static int
+ng_l2cap_default_rcvmsg(node_p node, item_p item, hook_p lasthook)
+{
+	ng_l2cap_p	 l2cap = (ng_l2cap_p) NG_NODE_PRIVATE(node);
+	struct ng_mesg	*msg = NULL, *rsp = NULL;
+	int		 error = 0;
+
+	/* Detach and process message */
+	NGI_GET_MSG(item, msg);
+
+	switch (msg->header.typecookie) {
+	case NGM_GENERIC_COOKIE:
+		switch (msg->header.cmd) {
+		case NGM_TEXT_STATUS:
+			NG_MKRESPONSE(rsp, msg, NG_TEXTRESPONSE, M_NOWAIT);
+			if (rsp == NULL)
+				error = ENOMEM;
+			else
+				snprintf(rsp->data, NG_TEXTRESPONSE,
+					"bdaddr %x:%x:%x:%x:%x:%x, " \
+					"pkt_size %d\n" \
+					"Hooks %s %s %s\n" \
+					"Flags %#x\n",
+					l2cap->bdaddr.b[5], l2cap->bdaddr.b[4],
+					l2cap->bdaddr.b[3], l2cap->bdaddr.b[2],
+					l2cap->bdaddr.b[1], l2cap->bdaddr.b[0],
+					l2cap->pkt_size,
+					(l2cap->hci != NULL)?
+						NG_L2CAP_HOOK_HCI : "",
+					(l2cap->l2c != NULL)?
+						NG_L2CAP_HOOK_L2C : "", 
+					(l2cap->ctl != NULL)?
+						NG_L2CAP_HOOK_CTL : "", 
+					l2cap->flags);
+			break;
+
+		default:
+			error = EINVAL;
+			break;
+		}
+		break;
+
+	/* Messages from the upper layer or directed to the local node */
+	case NGM_L2CAP_COOKIE:
+		switch (msg->header.cmd) {
+		/* Get node flags */
+		case NGM_L2CAP_NODE_GET_FLAGS:
+			NG_MKRESPONSE(rsp, msg, sizeof(ng_l2cap_node_flags_ep),
+				M_NOWAIT);
+			if (rsp == NULL)
+				error = ENOMEM;
+			else
+				*((ng_l2cap_node_flags_ep *)(rsp->data)) =
+					l2cap->flags;
+			break;
+
+		/* Get node debug */
+		case NGM_L2CAP_NODE_GET_DEBUG:
+			NG_MKRESPONSE(rsp, msg, sizeof(ng_l2cap_node_debug_ep),
+				M_NOWAIT);
+			if (rsp == NULL)
+				error = ENOMEM;
+			else
+				*((ng_l2cap_node_debug_ep *)(rsp->data)) =
+					l2cap->debug;
+			break;
+
+		/* Set node debug */
+		case NGM_L2CAP_NODE_SET_DEBUG:
+			if (msg->header.arglen !=
+					sizeof(ng_l2cap_node_debug_ep))
+				error = EMSGSIZE;
+			else
+				l2cap->debug =
+					*((ng_l2cap_node_debug_ep *)(msg->data));
+			break;
+
+		/* Get connection list */
+		case NGM_L2CAP_NODE_GET_CON_LIST: {
+			ng_l2cap_con_p			 con = NULL;
+			ng_l2cap_node_con_list_ep	*e1 = NULL;
+			ng_l2cap_node_con_ep		*e2 = NULL;
+			int				 n = 0;
+
+			/* Count number of connections */
+			LIST_FOREACH(con, &l2cap->con_list, next)
+				n++;
+			if (n > NG_L2CAP_MAX_CON_NUM)
+				n = NG_L2CAP_MAX_CON_NUM;
+
+			/* Prepare response */
+			NG_MKRESPONSE(rsp, msg,
+				sizeof(*e1) + n * sizeof(*e2), M_NOWAIT);
+			if (rsp == NULL) {
+				error = ENOMEM;
+				break;
+			}
+
+			e1 = (ng_l2cap_node_con_list_ep *)(rsp->data);
+			e2 = (ng_l2cap_node_con_ep *)(e1 + 1);
+
+			e1->num_connections = n;
+
+			LIST_FOREACH(con, &l2cap->con_list, next) {
+				e2->state = con->state;
+
+				if (con->tx_pkt != NULL)
+					e2->flags |= NG_L2CAP_CON_TX;
+				if (con->rx_pkt != NULL)
+					e2->flags |= NG_L2CAP_CON_RX;
+
+				e2->pending = con->pending;
+
+				e2->con_handle = con->con_handle;
+				bcopy(&con->remote, &e2->remote,
+					sizeof(e2->remote));
+
+				e2 ++;
+				if (--n <= 0)
+					break;
+			}
+			} break;
+
+		/* Get channel list */
+		case NGM_L2CAP_NODE_GET_CHAN_LIST: {
+			ng_l2cap_chan_p			 ch = NULL;
+			ng_l2cap_node_chan_list_ep	*e1 = NULL;
+			ng_l2cap_node_chan_ep		*e2 = NULL;
+			int				 n = 0;
+
+			/* Count number of channels */
+			LIST_FOREACH(ch, &l2cap->chan_list, next)
+				n ++;
+			if (n > NG_L2CAP_MAX_CHAN_NUM)
+				n = NG_L2CAP_MAX_CHAN_NUM;
+
+			/* Prepare response */
+			NG_MKRESPONSE(rsp, msg,
+				sizeof(ng_l2cap_node_chan_list_ep) +
+				n * sizeof(ng_l2cap_node_chan_ep), M_NOWAIT);
+			if (rsp == NULL) {
+				error = ENOMEM;
+				break;
+			}
+
+			e1 = (ng_l2cap_node_chan_list_ep *)(rsp->data);
+			e2 = (ng_l2cap_node_chan_ep *)(e1 + 1);
+
+			e1->num_channels = n;
+
+			LIST_FOREACH(ch, &l2cap->chan_list, next) {
+				e2->state = ch->state;
+
+				e2->scid = ch->scid;
+				e2->dcid = ch->dcid;
+
+				e2->imtu = ch->imtu;
+				e2->omtu = ch->omtu;
+
+				e2->psm = ch->psm;
+				bcopy(&ch->con->remote, &e2->remote,
+					sizeof(e2->remote));
+
+				e2 ++;
+				if (--n <= 0)
+					break;
+			}
+			} break;
+
+		default:
+			error = EINVAL;
+			break;
+		}
+		break;
+
+	default:
+		error = EINVAL;
+		break;
+	}
+
+	NG_RESPOND_MSG(error, node, item, rsp);
+	NG_FREE_MSG(msg);
+
+	return (error);
+} /* ng_l2cap_rcvmsg */
+
+/*
+ * Process data packet from one of our hooks.
+ *
+ * From the HCI hook we expect to receive ACL data packets. ACL data packets 
+ * gets re-assembled into one L2CAP packet (according to length) and then gets 
+ * processed.
+ *
+ * NOTE: We expect to receive L2CAP packet header in the first fragment. 
+ *       Otherwise we WILL NOT be able to get length of the L2CAP packet.
+ * 
+ * Signaling L2CAP packets (destination channel ID == 0x1) are processed within
+ * the node. Connectionless data packets (destination channel ID == 0x2) will 
+ * be forwarded to appropriate upstream hook unless it is not connected or 
+ * connectionless traffic for the specified PSM was disabled.
+ *
+ * From the upstream hooks we expect to receive data packets. These data 
+ * packets will be converted into L2CAP data packets. The length of each
+ * L2CAP packet must not exceed channel's omtu (our peer's imtu). Then
+ * these L2CAP packets will be converted to ACL data packets (according to 
+ * HCI layer MTU) and sent to lower layer.
+ *
+ * No data is expected from the control hook.
+ */
+
+static int
+ng_l2cap_rcvdata(hook_p hook, item_p item)
+{
+	ng_l2cap_p	 l2cap = (ng_l2cap_p) NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
+	struct mbuf	*m = NULL;
+	int		 error = 0;
+
+	/* Detach mbuf, discard item and process data */
+	NGI_GET_M(item, m);
+	NG_FREE_ITEM(item);
+
+	if (hook == l2cap->hci)
+		error = ng_l2cap_lp_receive(l2cap, m);
+	else if (hook == l2cap->l2c)
+		error = ng_l2cap_l2ca_write_req(l2cap, m);
+	else {
+		NG_FREE_M(m);
+		error = EINVAL;
+	}
+
+	return (error);
+} /* ng_l2cap_rcvdata */
+
+/*
+ * Clean all connections, channels and commands for the L2CAP node
+ */
+
+static void
+ng_l2cap_cleanup(ng_l2cap_p l2cap)
+{
+	ng_l2cap_con_p	con = NULL;
+
+	/* Clean up connection and channels */
+	while (!LIST_EMPTY(&l2cap->con_list)) {
+		con = LIST_FIRST(&l2cap->con_list);
+
+		if (con->state == NG_L2CAP_W4_LP_CON_CFM)
+			ng_l2cap_lp_untimeout(con);
+
+		con->state = NG_L2CAP_CON_CLOSED;
+		ng_l2cap_con_fail(con, 0x16);
+				/* Connection terminated by local host */
+	}
+} /* ng_l2cap_cleanup */
+
+/*
+ * Destroy all channels that use specified upstream hook
+ */
+
+static void
+ng_l2cap_destroy_channels(ng_l2cap_p l2cap)
+{
+	while (!LIST_EMPTY(&l2cap->chan_list))
+		ng_l2cap_free_chan(LIST_FIRST(&l2cap->chan_list));
+} /* ng_l2cap_destroy_channels_by_hook */
+
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_misc.c b/sys/netgraph/bluetooth/l2cap/ng_l2cap_misc.c
new file mode 100644
index 0000000..9fcbc6e
--- /dev/null
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_misc.c
@@ -0,0 +1,517 @@
+/*
+ * ng_l2cap_misc.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_misc.c,v 1.16 2002/09/04 21:38:38 max Exp $
+ * $FreeBSD$
+ */
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/kernel.h>
+#include <sys/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"
+
+static u_int16_t	ng_l2cap_get_cid		(ng_l2cap_p);
+static void		ng_l2cap_queue_lp_timeout	(void *);
+static void		ng_l2cap_queue_command_timeout	(void *);
+
+/******************************************************************************
+ ******************************************************************************
+ **                              Utility routines
+ ******************************************************************************
+ ******************************************************************************/
+
+/*
+ * Send hook information to the upper layer
+ */
+
+void
+ng_l2cap_send_hook_info(node_p node, hook_p hook, void *arg1, int arg2)
+{
+	ng_l2cap_p	 l2cap = NULL;
+	struct ng_mesg	*msg = NULL;
+	int		 error = 0;
+
+	if (node == NULL || NG_NODE_NOT_VALID(node) ||
+	    hook == NULL || NG_HOOK_NOT_VALID(hook))
+		return;
+
+	l2cap = (ng_l2cap_p) NG_NODE_PRIVATE(node);
+	if (l2cap->hci == NULL || NG_HOOK_NOT_VALID(l2cap->hci) ||
+	    bcmp(&l2cap->bdaddr, NG_HCI_BDADDR_ANY, sizeof(l2cap->bdaddr)) == 0)
+		return;
+
+	NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_NODE_HOOK_INFO,
+		sizeof(bdaddr_t), M_NOWAIT);
+	if (msg != NULL) {
+		bcopy(&l2cap->bdaddr, msg->data, sizeof(bdaddr_t));
+		NG_SEND_MSG_HOOK(error, node, msg, hook, NULL);
+	} else
+		error = ENOMEM;
+
+	if (error != 0)
+		NG_L2CAP_INFO(
+"%s: %s - failed to send HOOK_INFO message to hook \"%s\", error=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), NG_HOOK_NAME(hook),
+			error);
+} /* ng_l2cap_send_hook_info */
+
+/*
+ * Create new connection descriptor for the "remote" unit. Will create new
+ * connection descriptor and signal channel. Will link both connection and
+ * channel to the l2cap node.
+ */
+
+ng_l2cap_con_p
+ng_l2cap_new_con(ng_l2cap_p l2cap, bdaddr_p bdaddr)
+{
+	ng_l2cap_con_p	con = NULL;
+
+	/* Create new connection descriptor */
+	MALLOC(con, ng_l2cap_con_p, sizeof(*con), M_NETGRAPH_L2CAP,
+		M_NOWAIT|M_ZERO);
+	if (con == NULL)
+		return (NULL);
+
+	con->l2cap = l2cap;
+	con->state = NG_L2CAP_CON_CLOSED;
+
+	bcopy(bdaddr, &con->remote, sizeof(con->remote));
+	callout_handle_init(&con->con_timo);
+
+	con->ident = NG_L2CAP_FIRST_IDENT - 1;
+	TAILQ_INIT(&con->cmd_list);
+
+	/* Link connection */
+	LIST_INSERT_HEAD(&l2cap->con_list, con, next); 
+
+	return (con);
+} /* ng_l2cap_new_con */
+
+/*
+ * Free connection descriptor. Will unlink connection and free everything.
+ */
+
+void
+ng_l2cap_free_con(ng_l2cap_con_p con)
+{
+	ng_l2cap_chan_p f = NULL, n = NULL;
+
+	if (con->state == NG_L2CAP_W4_LP_CON_CFM)
+		ng_l2cap_lp_untimeout(con);
+
+	if (con->tx_pkt != NULL) {
+		while (con->tx_pkt != NULL) {
+			struct mbuf	*m = con->tx_pkt->m_nextpkt;
+
+			m_freem(con->tx_pkt);
+			con->tx_pkt = m;
+		}
+	}
+
+	NG_FREE_M(con->rx_pkt);
+
+	for (f = LIST_FIRST(&con->l2cap->chan_list); f != NULL; ) {
+		n = LIST_NEXT(f, next);
+
+		if (f->con == con)
+			ng_l2cap_free_chan(f);
+
+		f = n;
+	}
+
+	while (!TAILQ_EMPTY(&con->cmd_list)) {
+		ng_l2cap_cmd_p	cmd = TAILQ_FIRST(&con->cmd_list);
+
+		ng_l2cap_unlink_cmd(cmd);
+		ng_l2cap_free_cmd(cmd);
+	}
+
+	LIST_REMOVE(con, next);
+	bzero(con, sizeof(*con));
+	FREE(con, M_NETGRAPH_L2CAP);
+} /* ng_l2cap_free_con */
+
+/*
+ * Get connection by "remote" address
+ */
+
+ng_l2cap_con_p
+ng_l2cap_con_by_addr(ng_l2cap_p l2cap, bdaddr_p bdaddr)
+{
+	ng_l2cap_con_p	con = NULL;
+
+	LIST_FOREACH(con, &l2cap->con_list, next)
+		if (bcmp(bdaddr, &con->remote, sizeof(con->remote)) == 0)
+			break;
+
+	return (con);
+} /* ng_l2cap_con_by_addr */
+
+/*
+ * Get connection by "handle" 
+ */
+
+ng_l2cap_con_p
+ng_l2cap_con_by_handle(ng_l2cap_p l2cap, u_int16_t con_handle)
+{
+	ng_l2cap_con_p	con = NULL;
+
+	LIST_FOREACH(con, &l2cap->con_list, next)
+		if (con->con_handle == con_handle)
+			break;
+
+	return (con);
+} /* ng_l2cap_con_by_handle */
+
+/*
+ * Allocate new L2CAP channel descriptor on "con" conection with "psm".
+ * Will link the channel to the l2cap node
+ */
+
+ng_l2cap_chan_p
+ng_l2cap_new_chan(ng_l2cap_p l2cap, ng_l2cap_con_p con, u_int16_t psm)
+{
+	ng_l2cap_chan_p	ch = NULL;
+
+	MALLOC(ch, ng_l2cap_chan_p, sizeof(*ch), M_NETGRAPH_L2CAP,
+		M_NOWAIT|M_ZERO);
+	if (ch == NULL)
+		return (NULL);
+
+	ch->scid = ng_l2cap_get_cid(l2cap);
+
+	if (ch->scid != NG_L2CAP_NULL_CID) {
+		/* Initialize channel */
+		ch->psm = psm;
+		ch->con = con;
+		ch->state = NG_L2CAP_CLOSED;
+
+		/* Set MTU and flow control settings to defaults */
+		ch->imtu = NG_L2CAP_MTU_DEFAULT;
+		bcopy(ng_l2cap_default_flow(), &ch->iflow, sizeof(ch->iflow));
+
+		ch->omtu = NG_L2CAP_MTU_DEFAULT;
+		bcopy(ng_l2cap_default_flow(), &ch->oflow, sizeof(ch->oflow));
+
+		ch->flush_timo = NG_L2CAP_FLUSH_TIMO_DEFAULT;
+		ch->link_timo = NG_L2CAP_LINK_TIMO_DEFAULT;
+
+		LIST_INSERT_HEAD(&l2cap->chan_list, ch, next);
+	} else {
+		bzero(ch, sizeof(*ch));
+		FREE(ch, M_NETGRAPH_L2CAP);
+		ch = NULL;
+	}
+
+	return (ch);
+} /* ng_l2cap_new_chan */
+
+/*
+ * Get channel by source (local) channel ID
+ */
+
+ng_l2cap_chan_p
+ng_l2cap_chan_by_scid(ng_l2cap_p l2cap, u_int16_t scid)
+{
+	ng_l2cap_chan_p	ch = NULL;
+
+	LIST_FOREACH(ch, &l2cap->chan_list, next)
+		if (ch->scid == scid)
+			break;
+
+	return (ch);
+} /* ng_l2cap_chan_by_scid */
+
+/*
+ * Free channel descriptor.
+ */
+
+void
+ng_l2cap_free_chan(ng_l2cap_chan_p ch)
+{
+	ng_l2cap_cmd_p	f = NULL, n = NULL;
+
+	f = TAILQ_FIRST(&ch->con->cmd_list);
+	while (f != NULL) {
+		n = TAILQ_NEXT(f, next);
+
+		if (f->ch == ch) {
+			ng_l2cap_unlink_cmd(f);
+			ng_l2cap_free_cmd(f);
+		}
+
+		f = n;
+	}
+
+	LIST_REMOVE(ch, next);
+	bzero(ch, sizeof(*ch));
+	FREE(ch, M_NETGRAPH_L2CAP);
+} /* ng_l2cap_free_chan */
+
+/*
+ * Create new L2CAP command descriptor. WILL NOT add command to the queue.
+ */
+
+ng_l2cap_cmd_p
+ng_l2cap_new_cmd(ng_l2cap_con_p con, ng_l2cap_chan_p ch, u_int8_t ident,
+		u_int8_t code, u_int32_t token)
+{
+	ng_l2cap_cmd_p	cmd = NULL;
+
+	KASSERT((ch == NULL || ch->con == con),
+("%s: %s - invalid channel pointer!\n",
+		__func__, NG_NODE_NAME(con->l2cap->node)));
+
+	MALLOC(cmd, ng_l2cap_cmd_p, sizeof(*cmd), M_NETGRAPH_L2CAP,
+		M_NOWAIT|M_ZERO);
+	if (cmd == NULL)
+		return (NULL);
+
+	cmd->con = con;
+	cmd->ch = ch;
+	cmd->ident = ident;
+	cmd->code = code;
+	cmd->token = token;
+	callout_handle_init(&cmd->timo);
+
+	return (cmd);
+} /* ng_l2cap_new_cmd */
+ 
+/*
+ * Get L2CAP command descriptor by ident
+ */
+
+ng_l2cap_cmd_p
+ng_l2cap_cmd_by_ident(ng_l2cap_con_p con, u_int8_t ident)
+{
+	ng_l2cap_cmd_p	cmd = NULL;
+
+	TAILQ_FOREACH(cmd, &con->cmd_list, next)
+		if (cmd->ident == ident)
+			break;
+
+	return (cmd);
+} /* ng_l2cap_cmd_by_ident */
+
+/*
+ * Set LP timeout
+ */
+
+void
+ng_l2cap_lp_timeout(ng_l2cap_con_p con)
+{
+	NG_NODE_REF(con->l2cap->node);
+	con->con_timo = timeout(ng_l2cap_queue_lp_timeout, con,
+				bluetooth_hci_connect_timeout());
+} /* ng_l2cap_lp_timeout */
+
+/*
+ * Unset LP timeout
+ */
+
+void
+ng_l2cap_lp_untimeout(ng_l2cap_con_p con)
+{
+	untimeout(ng_l2cap_queue_lp_timeout, con, con->con_timo);
+	NG_NODE_UNREF(con->l2cap->node);
+} /* ng_l2cap_lp_untimeout */
+
+/*
+ * OK, timeout has happend so queue LP timeout processing function
+ */
+
+static void
+ng_l2cap_queue_lp_timeout(void *context)
+{
+	ng_l2cap_con_p	con = (ng_l2cap_con_p) context;
+	node_p		node = con->l2cap->node;
+
+	/*
+	 * We need to save node pointer here, because ng_send_fn()
+	 * can execute ng_l2cap_process_lp_timeout() without putting
+	 * item into node's queue (if node can be locked). Once 
+	 * ng_l2cap_process_lp_timeout() executed the con pointer 
+	 * is no longer valid.
+	 */
+
+	if (NG_NODE_IS_VALID(node))
+		ng_send_fn(node, NULL, &ng_l2cap_process_lp_timeout, con, 0);
+
+	NG_NODE_UNREF(node);
+} /* ng_l2cap_queue_lp_timeout */
+
+/*
+ * Set L2CAP command timeout
+ */
+
+void
+ng_l2cap_command_timeout(ng_l2cap_cmd_p cmd, int timo)
+{
+	NG_NODE_REF(cmd->con->l2cap->node);
+	cmd->flags |= NG_L2CAP_CMD_PENDING;
+	cmd->timo = timeout(ng_l2cap_queue_command_timeout, cmd, timo);
+} /* ng_l2cap_command_timeout */
+
+/*
+ * Unset L2CAP command timeout
+ */
+
+void
+ng_l2cap_command_untimeout(ng_l2cap_cmd_p cmd)
+{
+	cmd->flags &= ~NG_L2CAP_CMD_PENDING;
+	untimeout(ng_l2cap_queue_command_timeout, cmd, cmd->timo);
+	NG_NODE_UNREF(cmd->con->l2cap->node);
+} /* ng_l2cap_command_untimeout */
+
+/*
+ * OK, timeout has happend so queue L2CAP command timeout processing function
+ */
+
+static void
+ng_l2cap_queue_command_timeout(void *context)
+{
+	ng_l2cap_cmd_p	cmd = (ng_l2cap_cmd_p) context;
+	node_p		node = cmd->con->l2cap->node;
+
+	/*
+	 * We need to save node pointer here, because ng_send_fn()
+	 * can execute ng_l2cap_process_command_timeout() without
+	 * putting item into node's queue (if node can be locked).
+	 * Once ng_l2cap_process_command_timeout() executed the
+	 * cmd pointer is no longer valid.
+	 */
+
+	if (NG_NODE_IS_VALID(node))
+		ng_send_fn(node,NULL,&ng_l2cap_process_command_timeout,cmd,0);
+
+	NG_NODE_UNREF(node);
+} /* ng_l2cap_queue_command_timeout */
+
+/*
+ * Prepend "m"buf with "size" bytes
+ */
+
+struct mbuf *
+ng_l2cap_prepend(struct mbuf *m, int size)
+{
+	M_PREPEND(m, size, M_DONTWAIT);
+	if (m == NULL || (m->m_len < size && (m = m_pullup(m, size)) == NULL))
+		return (NULL);
+
+	return (m);
+} /* ng_l2cap_prepend */
+
+/*
+ * Default flow settings
+ */
+
+ng_l2cap_flow_p
+ng_l2cap_default_flow(void)
+{
+	static ng_l2cap_flow_t	default_flow = {
+		/* flags */		0x0,
+		/* service_type */	NG_HCI_SERVICE_TYPE_BEST_EFFORT,
+		/* token_rate */	0xffffffff, /* maximum */
+		/* token_bucket_size */	0xffffffff, /* maximum */
+		/* peak_bandwidth */	0x00000000, /* maximum */
+		/* latency */		0xffffffff, /* don't care */
+		/* delay_variation */	0xffffffff  /* don't care */
+	};
+
+	return (&default_flow);
+} /* ng_l2cap_default_flow */
+
+/*
+ * Get next available channel ID
+ * XXX FIXME this is *UGLY* but will do for now
+ */
+
+static u_int16_t
+ng_l2cap_get_cid(ng_l2cap_p l2cap)
+{
+	u_int16_t	cid = l2cap->cid + 1;
+
+	if (cid < NG_L2CAP_FIRST_CID)
+		cid = NG_L2CAP_FIRST_CID;
+
+	while (cid != l2cap->cid) {
+		if (ng_l2cap_chan_by_scid(l2cap, cid) == NULL) {
+			l2cap->cid = cid;
+
+			return (cid);
+		}
+
+		cid ++;
+		if (cid < NG_L2CAP_FIRST_CID)
+			cid = NG_L2CAP_FIRST_CID;
+	}
+		
+	return (NG_L2CAP_NULL_CID);
+} /* ng_l2cap_get_cid */
+
+/*
+ * Get next available command ident
+ * XXX FIXME this is *UGLY* but will do for now
+ */
+
+u_int8_t
+ng_l2cap_get_ident(ng_l2cap_con_p con)
+{
+	u_int8_t	ident = con->ident + 1;
+
+	if (ident < NG_L2CAP_FIRST_IDENT)
+		ident = NG_L2CAP_FIRST_IDENT;
+
+	while (ident != con->ident) {
+		if (ng_l2cap_cmd_by_ident(con, ident) == NULL) {
+			con->ident = ident;
+
+			return (ident);
+		}
+
+		ident ++;
+		if (ident < NG_L2CAP_FIRST_IDENT)
+			ident = NG_L2CAP_FIRST_IDENT;
+	}
+
+	return (NG_L2CAP_NULL_IDENT);
+} /* ng_l2cap_get_ident */
+
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_misc.h b/sys/netgraph/bluetooth/l2cap/ng_l2cap_misc.h
new file mode 100644
index 0000000..f0f72a3
--- /dev/null
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_misc.h
@@ -0,0 +1,100 @@
+/*
+ * ng_l2cap_misc.h
+ *
+ * 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_misc.h,v 1.6 2002/04/16 00:43:57 max Exp $
+ * $FreeBSD$
+ */
+
+#ifndef _NETGRAPH_L2CAP_MISC_H_
+#define _NETGRAPH_L2CAP_MISC_H_ 1
+
+void           ng_l2cap_send_hook_info (node_p, hook_p, void *, int);
+
+/*
+ * ACL Connections
+ */
+
+ng_l2cap_con_p ng_l2cap_new_con       (ng_l2cap_p, bdaddr_p);
+ng_l2cap_con_p ng_l2cap_con_by_addr   (ng_l2cap_p, bdaddr_p);
+ng_l2cap_con_p ng_l2cap_con_by_handle (ng_l2cap_p, u_int16_t);
+void           ng_l2cap_free_con      (ng_l2cap_con_p);
+
+/*
+ * L2CAP channels
+ */
+
+ng_l2cap_chan_p ng_l2cap_new_chan     (ng_l2cap_p, ng_l2cap_con_p, u_int16_t);
+ng_l2cap_chan_p ng_l2cap_chan_by_scid (ng_l2cap_p, u_int16_t);
+void            ng_l2cap_free_chan    (ng_l2cap_chan_p);
+
+/*
+ * L2CAP command descriptors
+ */
+
+#define ng_l2cap_link_cmd(con, cmd) \
+do { \
+	TAILQ_INSERT_TAIL(&(con)->cmd_list, (cmd), next); \
+} while (0)
+
+#define ng_l2cap_unlink_cmd(cmd) \
+do { \
+	TAILQ_REMOVE(&((cmd)->con->cmd_list), (cmd), next); \
+} while (0)
+
+#define ng_l2cap_free_cmd(cmd) \
+do { \
+	if ((cmd)->flags & NG_L2CAP_CMD_PENDING) \
+		ng_l2cap_command_untimeout((cmd)); \
+	\
+	NG_FREE_M((cmd)->aux); \
+	bzero((cmd), sizeof(*(cmd))); \
+	FREE((cmd), M_NETGRAPH_L2CAP); \
+} while (0)
+
+ng_l2cap_cmd_p ng_l2cap_new_cmd      (ng_l2cap_con_p, ng_l2cap_chan_p,
+						u_int8_t, u_int8_t, u_int32_t);
+ng_l2cap_cmd_p ng_l2cap_cmd_by_ident (ng_l2cap_con_p, u_int8_t);
+u_int8_t       ng_l2cap_get_ident    (ng_l2cap_con_p);
+
+/*
+ * Timeout
+ */
+
+void ng_l2cap_lp_timeout        (ng_l2cap_con_p);
+void ng_l2cap_lp_untimeout      (ng_l2cap_con_p);
+void ng_l2cap_command_timeout   (ng_l2cap_cmd_p, int);
+void ng_l2cap_command_untimeout (ng_l2cap_cmd_p);
+
+/*
+ * Other stuff
+ */
+
+struct mbuf *   ng_l2cap_prepend      (struct mbuf *, int);
+ng_l2cap_flow_p ng_l2cap_default_flow (void);
+
+#endif /* ndef _NETGRAPH_L2CAP_MISC_H_ */
+
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_prse.h b/sys/netgraph/bluetooth/l2cap/ng_l2cap_prse.h
new file mode 100644
index 0000000..5209c24
--- /dev/null
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_prse.h
@@ -0,0 +1,71 @@
+/*
+ * ng_l2cap_prse.h
+ *
+ * Copyright (c) 2001 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_prse.h,v 1.2 2002/09/04 21:38:38 max Exp $
+ * $FreeBSD$
+ */
+
+/***************************************************************************
+ ***************************************************************************
+ **                ng_parse definitions for the L2CAP node
+ ***************************************************************************
+ ***************************************************************************/
+
+#ifndef _NETGRAPH_L2CAP_PRSE_H_
+#define _NETGRAPH_L2CAP_PRSE_H_ 1
+
+/* 
+ * L2CAP node command list
+ */
+
+static const struct ng_cmdlist	ng_l2cap_cmdlist[] = {
+	{
+		NGM_L2CAP_COOKIE,
+		NGM_L2CAP_NODE_GET_FLAGS,
+		"get_flags",
+		NULL,
+		&ng_parse_uint16_type
+	},
+	{
+		NGM_L2CAP_COOKIE,
+		NGM_L2CAP_NODE_GET_DEBUG,
+		"get_debug",
+		NULL,
+		&ng_parse_uint16_type
+	},
+	{
+		NGM_L2CAP_COOKIE,
+		NGM_L2CAP_NODE_SET_DEBUG,
+		"set_debug",
+		&ng_parse_uint16_type,
+		NULL
+	},
+	{ 0, }
+};
+
+#endif /* ndef _NETGRAPH_L2CAP_PRSE_H_ */
+
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.c b/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.c
new file mode 100644
index 0000000..c913d1c
--- /dev/null
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.c
@@ -0,0 +1,1640 @@
+/*
+ * ng_l2cap_ulpi.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_ulpi.c,v 1.22 2002/09/08 23:35:51 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_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"
+
+/******************************************************************************
+ ******************************************************************************
+ **                 Upper Layer Protocol Interface module
+ ******************************************************************************
+ ******************************************************************************/
+
+/*
+ * Process L2CA_Connect request from the upper layer protocol.
+ */
+
+int
+ng_l2cap_l2ca_con_req(ng_l2cap_p l2cap, struct ng_mesg *msg)
+{
+	ng_l2cap_l2ca_con_ip	*ip = NULL;
+	ng_l2cap_con_p		 con = NULL;
+	ng_l2cap_chan_p		 ch = NULL;
+	ng_l2cap_cmd_p		 cmd = NULL;
+	int			 error = 0;
+
+	/* Check message */
+	if (msg->header.arglen != sizeof(*ip)) {
+		NG_L2CAP_ALERT(
+"%s: %s - invalid L2CA_Connect request message size, size=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node),
+			msg->header.arglen);
+		error = EMSGSIZE;
+		goto out;
+	}
+
+	ip = (ng_l2cap_l2ca_con_ip *)(msg->data);
+
+	/* Check if we have connection to the remote unit */
+	con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr);
+	if (con == NULL) {
+		/* Submit LP_ConnectReq to the lower layer */
+		error = ng_l2cap_lp_con_req(l2cap, &ip->bdaddr);
+		if (error != 0) {
+			NG_L2CAP_ERR(
+"%s: %s - unable to send LP_ConnectReq message, error=%d\n",
+				__func__, NG_NODE_NAME(l2cap->node), error);
+			goto out;
+		}
+
+		/* This should not fail */
+		con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr);
+		KASSERT((con != NULL),
+("%s: %s - could not find connection!\n", __func__, NG_NODE_NAME(l2cap->node)));
+	}
+
+	/*
+	 * Create new empty channel descriptor. In case of any failure do 
+	 * not touch connection descriptor.
+	 */
+
+	ch = ng_l2cap_new_chan(l2cap, con, ip->psm);
+	if (ch == NULL) {
+		error = ENOMEM;
+		goto out;
+	}
+
+	/* Now create L2CAP_ConnectReq command */
+	cmd = ng_l2cap_new_cmd(ch->con, ch, ng_l2cap_get_ident(con),
+			NG_L2CAP_CON_REQ, msg->header.token);
+	if (cmd == NULL) {
+		ng_l2cap_free_chan(ch);
+		error = ENOMEM;
+		goto out;
+	}
+
+	if (cmd->ident == NG_L2CAP_NULL_IDENT) {
+		ng_l2cap_free_cmd(cmd);
+		ng_l2cap_free_chan(ch);
+		error = EIO;
+		goto out;
+	}
+
+	/* Create L2CAP command packet */
+	_ng_l2cap_con_req(cmd->aux, cmd->ident, ch->psm, ch->scid);
+	if (cmd->aux == NULL) {
+		ng_l2cap_free_cmd(cmd);
+		ng_l2cap_free_chan(ch);
+		error = ENOBUFS;
+		goto out;
+	}
+
+	ch->state = NG_L2CAP_W4_L2CAP_CON_RSP;
+
+	/* Link command to the queue */
+	ng_l2cap_link_cmd(ch->con, cmd);
+	ng_l2cap_lp_deliver(ch->con);
+out:
+	return (error);
+} /* ng_l2cap_l2ca_con_req */
+
+/*
+ * Send L2CA_Connect response to the upper layer protocol.
+ */
+
+int
+ng_l2cap_l2ca_con_rsp(ng_l2cap_chan_p ch, u_int32_t token, u_int16_t result,
+		u_int16_t status)
+{
+	ng_l2cap_p		 l2cap = ch->con->l2cap;
+	struct ng_mesg		*msg = NULL;
+	ng_l2cap_l2ca_con_op	*op = NULL;
+	int			 error = 0;
+
+	/* Check if upstream hook is connected and valid */
+	if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) {
+		NG_L2CAP_ERR(
+"%s: %s - unable to send L2CA_Connect response message. " \
+"Hook is not connected or valid, psm=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), ch->psm);
+
+		return (ENOTCONN);
+	}
+
+	/* Create and send L2CA_Connect response message */
+	NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_CON,
+		sizeof(*op), M_NOWAIT);
+	if (msg == NULL)
+		error = ENOMEM;
+	else {
+		msg->header.token = token;
+		msg->header.flags |= NGF_RESP;
+
+		op = (ng_l2cap_l2ca_con_op *)(msg->data);
+		
+		/*
+		 * XXX Spec. says we should only populate LCID when result == 0
+		 * What about PENDING? What the heck, for now always populate
+		 * LCID :)
+		 */
+
+		op->lcid = ch->scid;
+		op->result = result;
+		op->status = status;
+
+		NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, NULL);
+	}
+
+	return (error);
+} /* ng_l2cap_l2ca_con_rsp */
+
+/*
+ * Process L2CA_ConnectRsp request from the upper layer protocol.
+ */
+
+int
+ng_l2cap_l2ca_con_rsp_req(ng_l2cap_p l2cap, struct ng_mesg *msg)
+{
+	ng_l2cap_l2ca_con_rsp_ip	*ip = NULL;
+	ng_l2cap_con_p			 con = NULL;
+	ng_l2cap_chan_p			 ch = NULL;
+	ng_l2cap_cmd_p			 cmd = NULL;
+	u_int16_t			 dcid;
+	int				 error = 0;
+
+	/* Check message */
+	if (msg->header.arglen != sizeof(*ip)) {
+		NG_L2CAP_ALERT(
+"%s: %s - invalid L2CA_ConnectRsp request message size, size=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node),
+			msg->header.arglen);
+		error = EMSGSIZE;
+		goto out;
+	}
+
+	ip = (ng_l2cap_l2ca_con_rsp_ip *)(msg->data);
+
+	/* Check if we have this channel */
+	ch = ng_l2cap_chan_by_scid(l2cap, ip->lcid);
+	if (ch == NULL) {
+		NG_L2CAP_ALERT(
+"%s: %s - unexpected L2CA_ConnectRsp request message. " \
+"Channel does not exist, lcid=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), ip->lcid);
+		error = ENOENT;
+		goto out;
+	}
+
+	/* Check channel state */
+	if (ch->state != NG_L2CAP_W4_L2CA_CON_RSP) {
+		NG_L2CAP_ERR(
+"%s: %s - unexpected L2CA_ConnectRsp request message. " \
+"Invalid channel state, state=%d, lcid=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), ch->state,
+			ip->lcid);
+		error = EINVAL;
+		goto out;
+	}
+
+	dcid = ch->dcid;
+	con = ch->con;
+
+	/*
+	 * Now we are pretty much sure it is our response. So create and send 
+	 * L2CAP_ConnectRsp message to our peer.
+	 */
+
+	if (ch->ident != ip->ident)
+		NG_L2CAP_WARN(
+"%s: %s - channel ident and response ident do not match, scid=%d, ident=%d. " \
+"Will use response ident=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), ch->scid, 
+			ch->ident, ip->ident);
+
+	/* Check result */
+	switch (ip->result) {
+	case NG_L2CAP_SUCCESS:
+		ch->state = NG_L2CAP_CONFIG;
+		ch->cfg_state = 0;
+		break;
+
+	case NG_L2CAP_PENDING:
+		break;
+
+	default:
+		ng_l2cap_free_chan(ch);
+		ch = NULL;
+		break;
+	}
+
+	/* Create L2CAP command */
+	cmd = ng_l2cap_new_cmd(con, ch, ip->ident, NG_L2CAP_CON_RSP,
+			msg->header.token);
+	if (cmd == NULL) {
+		if (ch != NULL)
+			ng_l2cap_free_chan(ch);
+
+		error = ENOMEM;
+		goto out;
+	}
+
+	_ng_l2cap_con_rsp(cmd->aux, cmd->ident, ip->lcid, dcid, 
+		ip->result, ip->status);
+	if (cmd->aux == NULL) {
+		if (ch != NULL)
+			ng_l2cap_free_chan(ch);
+
+		ng_l2cap_free_cmd(cmd);
+		error = ENOBUFS;
+		goto out;
+	} 
+
+	/* Link command to the queue */
+	ng_l2cap_link_cmd(con, cmd);
+	ng_l2cap_lp_deliver(con);
+out:
+	return (error);
+} /* ng_l2cap_l2ca_con_rsp_req */
+
+/*
+ * Send L2CAP_ConnectRsp response to the upper layer
+ */
+ 
+int
+ng_l2cap_l2ca_con_rsp_rsp(ng_l2cap_chan_p ch, u_int32_t token, u_int16_t result)
+{
+	ng_l2cap_p			 l2cap = ch->con->l2cap;
+	struct ng_mesg			*msg = NULL;
+	ng_l2cap_l2ca_con_rsp_op	*op = NULL;
+	int				 error = 0;
+
+	/* Check if upstream hook is connected and valid */
+	if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) {
+		NG_L2CAP_ERR(
+"%s: %s - unable to send L2CA_ConnectRsp response message. " \
+"Hook is not connected or valid, psm=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), ch->psm);
+
+		return (ENOTCONN);
+	}
+
+	/* Create and send L2CA_ConnectRsp response message */
+	NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_CON_RSP,
+		sizeof(*op), M_NOWAIT);
+	if (msg == NULL)
+		error = ENOMEM;
+	else {
+		msg->header.token = token;
+		msg->header.flags |= NGF_RESP;
+
+		op = (ng_l2cap_l2ca_con_rsp_op *)(msg->data);
+		op->result = result;
+
+		NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, NULL);
+	}
+
+	return (error);
+} /* ng_l2cap_l2ca_con_rsp_rsp */
+
+/*
+ * Send L2CA_ConnectInd message to the upper layer protocol. 
+ */
+
+int
+ng_l2cap_l2ca_con_ind(ng_l2cap_chan_p ch)
+{
+	ng_l2cap_p			 l2cap = ch->con->l2cap;
+	struct ng_mesg			*msg = NULL;
+	ng_l2cap_l2ca_con_ind_ip	*ip = NULL;
+	int				 error = 0;
+
+	/* Check if upstream hook is connected and valid */
+	if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) {
+		NG_L2CAP_ERR(
+"%s: %s - unable to send L2CA_ConnectInd message. " \
+"Hook is not connected or valid, psm=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), ch->psm);
+
+		return (ENOTCONN);
+	}
+
+	/* Create and send L2CA_ConnectInd message */
+	NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_CON_IND,
+		sizeof(*ip), M_NOWAIT);
+	if (msg == NULL)
+		error = ENOMEM;
+	else {
+		ip = (ng_l2cap_l2ca_con_ind_ip *)(msg->data);
+
+		bcopy(&ch->con->remote, &ip->bdaddr, sizeof(ip->bdaddr));
+		ip->lcid = ch->scid;
+		ip->psm = ch->psm;
+		ip->ident = ch->ident;
+
+		NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, NULL);
+	}
+
+	return (error);
+} /* ng_l2cap_l2ca_con_ind */
+
+/*
+ * Process L2CA_Config request from the upper layer protocol
+ */
+
+int
+ng_l2cap_l2ca_cfg_req(ng_l2cap_p l2cap, struct ng_mesg *msg)
+{
+	ng_l2cap_l2ca_cfg_ip	*ip = NULL;
+	ng_l2cap_chan_p		 ch = NULL;
+	ng_l2cap_cmd_p		 cmd = NULL;
+	struct mbuf		*opt = NULL;
+        u_int16_t		*mtu = NULL, *flush_timo = NULL;
+        ng_l2cap_flow_p		 flow = NULL;
+	int			 error = 0;
+
+	/* Check message */
+	if (msg->header.arglen != sizeof(*ip)) {
+		NG_L2CAP_ALERT(
+"%s: %s - Invalid L2CA_Config request message size, size=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node),
+			msg->header.arglen);
+		error = EMSGSIZE;
+		goto out;
+	}
+
+	ip = (ng_l2cap_l2ca_cfg_ip *)(msg->data);
+
+	/* Check if we have this channel */
+	ch = ng_l2cap_chan_by_scid(l2cap, ip->lcid);
+	if (ch == NULL) {
+		NG_L2CAP_ERR(
+"%s: %s - unexpected L2CA_Config request message. " \
+"Channel does not exist, lcid=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), ip->lcid);
+		error = ENOENT;
+		goto out;
+	}
+
+	/* Check channel state */
+	if (ch->state != NG_L2CAP_OPEN && ch->state != NG_L2CAP_CONFIG) {
+		NG_L2CAP_ERR(
+"%s: %s - unexpected L2CA_Config request message. " \
+"Invalid channel state, state=%d, lcid=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), ch->state,
+			ch->scid);
+		error = EINVAL;
+		goto out;
+	}
+
+	/* Set requested channel configuration options */
+	ch->imtu = ip->imtu;
+	bcopy(&ip->oflow, &ch->oflow, sizeof(ch->oflow));
+	ch->flush_timo = ip->flush_timo;
+	ch->link_timo = ip->link_timo;
+
+	/* Compare channel settings with defaults */
+	if (ch->imtu != NG_L2CAP_MTU_DEFAULT)
+		mtu = &ch->imtu;
+	if (ch->flush_timo != NG_L2CAP_FLUSH_TIMO_DEFAULT)
+		flush_timo = &ch->flush_timo;
+	if (bcmp(ng_l2cap_default_flow(), &ch->oflow, sizeof(ch->oflow)) != 0)
+		flow = &ch->oflow;
+
+	/* Create configuration options */
+	_ng_l2cap_build_cfg_options(opt, mtu, flush_timo, flow);
+	if (opt == NULL) {
+                error = ENOBUFS;
+		goto out;
+	}
+
+	/* Create L2CAP command descriptor */
+	cmd = ng_l2cap_new_cmd(ch->con, ch, ng_l2cap_get_ident(ch->con),
+			NG_L2CAP_CFG_REQ, msg->header.token);
+	if (cmd == NULL) {
+		NG_FREE_M(opt);
+		error = ENOMEM;
+		goto out;
+	}
+
+	if (cmd->ident == NG_L2CAP_NULL_IDENT) {
+		ng_l2cap_free_cmd(cmd);
+		NG_FREE_M(opt);
+		error = EIO;
+		goto out;
+	}
+
+	/* Create L2CAP command packet */
+	_ng_l2cap_cfg_req(cmd->aux, cmd->ident, ch->dcid, 0, opt);
+	if (cmd->aux == NULL) {
+		ng_l2cap_free_cmd(cmd);
+		error =  ENOBUFS;
+		goto out;
+	}
+
+	/* Adjust channel state for re-configuration */
+	if (ch->state == NG_L2CAP_OPEN) {
+		ch->state = NG_L2CAP_CONFIG;
+		ch->cfg_state = 0;
+	}
+
+        /* Link command to the queue */
+	ng_l2cap_link_cmd(ch->con, cmd);
+	ng_l2cap_lp_deliver(ch->con);
+out:
+	return (error);
+} /* ng_l2cap_l2ca_cfg_req */
+
+/*
+ * Send L2CA_Config response to the upper layer protocol
+ */
+
+int
+ng_l2cap_l2ca_cfg_rsp(ng_l2cap_chan_p ch, u_int32_t token, u_int16_t result)
+{
+	ng_l2cap_p		 l2cap = ch->con->l2cap;
+	struct ng_mesg		*msg = NULL;
+	ng_l2cap_l2ca_cfg_op	*op = NULL;
+	int			 error = 0;
+
+	/* Check if upstream hook is connected and valid */
+	if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) {
+		NG_L2CAP_ERR(
+"%s: %s - unable to send L2CA_Config response message. " \
+"Hook is not connected or valid, psm=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), ch->psm);
+
+		return (ENOTCONN);
+	}
+
+	/* Create and send L2CA_Config response message */
+	NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_CFG,
+		sizeof(*op), M_NOWAIT);
+	if (msg == NULL)
+		error = ENOMEM;
+	else {
+		msg->header.token = token;
+		msg->header.flags |= NGF_RESP;
+
+		op = (ng_l2cap_l2ca_cfg_op *)(msg->data);
+		op->result = result;
+		op->imtu = ch->imtu;
+		bcopy(&ch->oflow, &op->oflow, sizeof(op->oflow));
+		op->flush_timo = ch->flush_timo;
+
+		NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, NULL);
+
+		if (error == 0 && result == NG_L2CAP_SUCCESS) {
+			ch->cfg_state |= NG_L2CAP_CFG_IN;
+
+			if (ch->cfg_state == NG_L2CAP_CFG_BOTH)
+				ch->state = NG_L2CAP_OPEN;
+		}
+	}
+
+	return (error);
+} /* ng_l2cap_l2ca_cfg_rsp */
+
+/*
+ * Process L2CA_ConfigRsp request from the upper layer protocol
+ *
+ * XXX XXX XXX
+ *
+ * NOTE: The Bluetooth specification says that Configuration_Response 
+ * (L2CA_ConfigRsp) should be used to issue response to configuration request
+ * indication. The minor problem here is L2CAP command ident. We should use 
+ * ident from original L2CAP request to make sure our peer can match request
+ * and response. For some reason Bluetooth specification does not include
+ * ident field into L2CA_ConfigInd and L2CA_ConfigRsp messages. This seems
+ * strange to me, because L2CA_ConnectInd and L2CA_ConnectRsp do have ident
+ * field. So we should store last known L2CAP request command ident in channel.
+ * Also it seems that upper layer can not reject configuration request, as
+ * Configuration_Response message does not have status/reason field.
+ */
+
+int
+ng_l2cap_l2ca_cfg_rsp_req(ng_l2cap_p l2cap, struct ng_mesg *msg)
+{
+	ng_l2cap_l2ca_cfg_rsp_ip	*ip = NULL;
+	ng_l2cap_chan_p			 ch = NULL;
+	ng_l2cap_cmd_p			 cmd = NULL;
+	struct mbuf			*opt = NULL;
+	u_int16_t			*mtu = NULL;
+	ng_l2cap_flow_p			 flow = NULL;
+	int				 error = 0;
+
+	/* Check message */
+	if (msg->header.arglen != sizeof(*ip)) {
+		NG_L2CAP_ALERT(
+"%s: %s - invalid L2CA_ConfigRsp request message size, size=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node),
+			msg->header.arglen);
+		error = EMSGSIZE;
+		goto out;
+	}
+
+	ip = (ng_l2cap_l2ca_cfg_rsp_ip *)(msg->data);
+
+	/* Check if we have this channel */
+	ch = ng_l2cap_chan_by_scid(l2cap, ip->lcid);
+	if (ch == NULL) {
+		NG_L2CAP_ERR(
+"%s: %s - unexpected L2CA_ConfigRsp request message. " \
+"Channel does not exist, lcid=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), ip->lcid);
+		error = ENOENT;
+		goto out;
+	}
+
+	/* Check channel state */
+	if (ch->state != NG_L2CAP_CONFIG) {
+		NG_L2CAP_ERR(
+"%s: %s - unexpected L2CA_ConfigRsp request message. " \
+"Invalid channel state, state=%d, lcid=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), ch->state,
+			ch->scid);
+		error = EINVAL;
+		goto out;
+	}
+
+	/* Set channel settings */
+	if (ip->omtu != ch->omtu) {
+		ch->omtu = ip->omtu;
+		mtu = &ch->omtu;
+	}
+
+	if (bcmp(&ip->iflow, &ch->iflow, sizeof(ch->iflow)) != 0) { 
+		bcopy(&ip->iflow, &ch->iflow, sizeof(ch->iflow));
+		flow = &ch->iflow;
+	}
+
+	if (mtu != NULL || flow != NULL) {
+		_ng_l2cap_build_cfg_options(opt, mtu, NULL, flow);
+		if (opt == NULL) {
+			error = ENOBUFS;
+			goto out;
+		}
+	}
+
+	/* Create L2CAP command */
+	cmd = ng_l2cap_new_cmd(ch->con, ch, ch->ident, NG_L2CAP_CFG_RSP,
+			msg->header.token);
+	if (cmd == NULL) {
+		NG_FREE_M(opt);
+		error = ENOMEM;
+		goto out;
+	}
+
+	_ng_l2cap_cfg_rsp(cmd->aux,cmd->ident,ch->dcid,0,NG_L2CAP_SUCCESS,opt);
+	if (cmd->aux == NULL) {
+		ng_l2cap_free_cmd(cmd);
+		error = ENOBUFS;
+		goto out;
+	}
+
+	/* XXX FIXME - not here ??? */
+	ch->cfg_state |= NG_L2CAP_CFG_OUT;
+	if (ch->cfg_state == NG_L2CAP_CFG_BOTH)
+		ch->state = NG_L2CAP_OPEN;
+
+	/* Link command to the queue */
+	ng_l2cap_link_cmd(ch->con, cmd);
+	ng_l2cap_lp_deliver(ch->con);
+out:
+	return (error);
+} /* ng_l2cap_l2ca_cfg_rsp_req */
+
+/*
+ * Send L2CA_ConfigRsp response to the upper layer protocol
+ */
+
+int
+ng_l2cap_l2ca_cfg_rsp_rsp(ng_l2cap_chan_p ch, u_int32_t token, u_int16_t result)
+{
+	ng_l2cap_p			 l2cap = ch->con->l2cap;
+	struct ng_mesg			*msg = NULL;
+	ng_l2cap_l2ca_cfg_rsp_op	*op = NULL;
+	int				 error = 0;
+
+	/* Check if upstream hook is connected and valid */
+	if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) {
+		NG_L2CAP_ERR(
+"%s: %s - unable to send L2CA_ConfigRsp response message. " \
+"Hook is not connected or valid, psm=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), ch->psm);
+
+		return (ENOTCONN);
+	}
+
+	/* Create and send L2CA_ConfigRsp response message */
+	NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_CFG_RSP,
+		sizeof(*op), M_NOWAIT);
+	if (msg == NULL)
+		error = ENOMEM;
+	else {
+		msg->header.token = token;
+		msg->header.flags |= NGF_RESP;
+
+		op = (ng_l2cap_l2ca_cfg_rsp_op *)(msg->data);
+		op->result = result;
+
+		NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, NULL);
+	}
+
+	return (error);
+} /* ng_l2cap_l2ca_cfg_rsp_rsp */
+
+/*
+ * Send L2CA_ConfigInd message to the upper layer protocol
+ *
+ * XXX XXX XXX
+ *
+ * NOTE: The Bluetooth specification says that Configuration_Response 
+ * (L2CA_ConfigRsp) should be used to issue response to configuration request
+ * indication. The minor problem here is L2CAP command ident. We should use 
+ * ident from original L2CAP request to make sure our peer can match request
+ * and response. For some reason Bluetooth specification does not include
+ * ident field into L2CA_ConfigInd and L2CA_ConfigRsp messages. This seems
+ * strange to me, because L2CA_ConnectInd and L2CA_ConnectRsp do have ident
+ * field. So we should store last known L2CAP request command ident in channel.
+ * Also it seems that upper layer can not reject configuration request, as
+ * Configuration_Response message does not have status/reason field.
+ */
+
+int
+ng_l2cap_l2ca_cfg_ind(ng_l2cap_chan_p ch)
+{
+	ng_l2cap_p			 l2cap = ch->con->l2cap;
+	struct ng_mesg			*msg = NULL;
+	ng_l2cap_l2ca_cfg_ind_ip	*ip = NULL;
+	int				 error = 0;
+
+	/* Check if upstream hook is connected and valid */
+	if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) {
+		NG_L2CAP_ERR(
+"%s: %s - Unable to send L2CA_ConfigInd message. " \
+"Hook is not connected or valid, psm=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), ch->psm);
+
+		return (ENOTCONN);
+	}
+
+	/* Create and send L2CA_ConnectInd message */
+	NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_CFG_IND,
+			sizeof(*ip), M_NOWAIT);
+	if (msg == NULL)
+		error = ENOMEM;
+	else {
+		ip = (ng_l2cap_l2ca_cfg_ind_ip *)(msg->data);
+		ip->lcid = ch->scid;
+		ip->omtu = ch->omtu;
+		bcopy(&ch->iflow, &ip->iflow, sizeof(ip->iflow));
+		ip->flush_timo = ch->flush_timo;
+
+		NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, NULL);
+	}
+
+	return (error);
+} /* ng_l2cap_l2ca_cfg_ind */
+
+/*
+ * Process L2CA_Write event
+ */
+
+int
+ng_l2cap_l2ca_write_req(ng_l2cap_p l2cap, struct mbuf *m)
+{
+	ng_l2cap_l2ca_hdr_t	*l2ca_hdr = NULL;
+	ng_l2cap_chan_p		 ch = NULL;
+	ng_l2cap_cmd_p		 cmd = NULL;
+	int			 error = 0;
+	u_int32_t		 token = 0;
+
+	/* Make sure we can access L2CA data packet header */
+	if (m->m_pkthdr.len < sizeof(*l2ca_hdr)) {
+		NG_L2CAP_ERR(
+"%s: %s - L2CA Data packet too small, len=%d\n",
+			__func__,NG_NODE_NAME(l2cap->node),m->m_pkthdr.len);
+		error = EMSGSIZE;
+		goto drop;
+	}
+
+	/* Get L2CA data packet header */
+	NG_L2CAP_M_PULLUP(m, sizeof(*l2ca_hdr));
+	if (m == NULL)
+		return (ENOBUFS);
+
+	l2ca_hdr = mtod(m, ng_l2cap_l2ca_hdr_t *);
+	token = l2ca_hdr->token;
+	m_adj(m, sizeof(*l2ca_hdr));
+
+	/* Verify payload size */
+	if (l2ca_hdr->length != m->m_pkthdr.len) {
+		NG_L2CAP_ERR(
+"%s: %s - invalid L2CA Data packet. " \
+"Payload length does not match, length=%d, len=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), l2ca_hdr->length,
+			m->m_pkthdr.len);
+		error = EMSGSIZE;
+		goto drop;
+	}
+
+	/* Check channel ID */
+	if (l2ca_hdr->lcid < NG_L2CAP_FIRST_CID) {
+		NG_L2CAP_ERR(
+"%s: %s - invalid L2CA Data packet. Inavlid channel ID, cid=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), l2ca_hdr->lcid);
+		error = EINVAL;
+		goto drop;
+	}
+
+	/* Verify that we have the channel and make sure it is open */
+	ch = ng_l2cap_chan_by_scid(l2cap, l2ca_hdr->lcid);
+	if (ch == NULL) {
+		NG_L2CAP_ERR(
+"%s: %s - invalid L2CA Data packet. Channel does not exist, cid=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), l2ca_hdr->lcid);
+		error = ENOENT;
+		goto drop;
+	}
+
+	if (ch->state != NG_L2CAP_OPEN) {
+		NG_L2CAP_ERR(
+"%s: %s - invalid L2CA Data packet. Invalid channel state, scid=%d, state=%d\n",
+			 __func__, NG_NODE_NAME(l2cap->node), ch->scid, 
+			ch->state);
+		error = EHOSTDOWN;
+		goto drop; /* XXX not always - re-configure */
+	}
+
+	/* Create L2CAP command descriptor */
+	cmd = ng_l2cap_new_cmd(ch->con, ch, 0, NGM_L2CAP_L2CA_WRITE, token);
+	if (cmd == NULL) {
+		error = ENOMEM;
+		goto drop;
+	}
+
+	/* Attach data packet and link command to the queue */
+	cmd->aux = m;
+	ng_l2cap_link_cmd(ch->con, cmd);
+	ng_l2cap_lp_deliver(ch->con);
+
+	return (error);
+drop:
+	NG_FREE_M(m);
+
+	return (error);
+} /* ng_l2cap_l2ca_write_req */
+
+/*
+ * Send L2CA_Write response
+ */
+
+int
+ng_l2cap_l2ca_write_rsp(ng_l2cap_chan_p ch, u_int32_t token, u_int16_t result,
+		u_int16_t length)
+{
+	ng_l2cap_p		 l2cap = ch->con->l2cap;
+	struct ng_mesg		*msg = NULL;
+	ng_l2cap_l2ca_write_op	*op = NULL;
+	int			 error = 0;
+
+	/* Check if upstream hook is connected and valid */
+	if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) {
+		NG_L2CAP_ERR(
+"%s: %s - unable to send L2CA_WriteRsp message. " \
+"Hook is not connected or valid, psm=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), ch->psm);
+
+		return (ENOTCONN);
+	}
+
+	/* Create and send L2CA_WriteRsp message */
+	NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_WRITE,
+			sizeof(*op), M_NOWAIT);
+	if (msg == NULL)
+		error = ENOMEM;
+	else {
+		msg->header.token = token;
+		msg->header.flags |= NGF_RESP;
+
+		op = (ng_l2cap_l2ca_write_op *)(msg->data);
+		op->result = result;
+		op->length = length;
+		op->lcid   = ch->scid;
+
+		NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, NULL);
+	}
+
+	return (error);
+} /* ng_l2cap_l2ca_write_rsp */
+
+/*
+ * Receive packet from the lower layer protocol and send it to the upper
+ * layer protocol (L2CAP_Read)
+ */
+
+int
+ng_l2cap_l2ca_receive(ng_l2cap_con_p con)
+{
+	ng_l2cap_p	 l2cap = con->l2cap;
+	ng_l2cap_hdr_t	*hdr = NULL;
+	ng_l2cap_chan_p  ch = NULL;
+	int		 error = 0;
+
+	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 *);
+
+	/* Check channel */
+	ch = ng_l2cap_chan_by_scid(l2cap, hdr->dcid);
+	if (ch == NULL) {
+		NG_L2CAP_ERR(
+"%s: %s - unexpected L2CAP data packet. Channel does not exist, cid=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), hdr->dcid);
+		error = ENOENT;
+		goto drop;
+	}
+
+	/* Check channel state */
+	if (ch->state != NG_L2CAP_OPEN) {
+		NG_L2CAP_WARN(
+"%s: %s - unexpected L2CAP data packet. " \
+"Invalid channel state, cid=%d, state=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), ch->scid,
+			ch->state);
+		error = EHOSTDOWN; /* XXX not always - re-configuration */
+		goto drop;
+	}
+
+	/* Check payload size and channel's MTU */
+	if (hdr->length > ch->imtu) {
+		NG_L2CAP_ERR(
+"%s: %s - invalid L2CAP data packet. " \
+"Packet too big, length=%d, imtu=%d, cid=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), hdr->length, 
+			ch->imtu, ch->scid);
+		error = EMSGSIZE;
+		goto drop;
+	}
+
+	/*
+	 * If we got here then everything looks good and we can sent packet
+	 * to the upper layer protocol.
+	 */
+
+	/* Check if upstream hook is connected and valid */
+	if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) {
+		NG_L2CAP_ERR(
+"%s: %s - unable to send L2CAP data packet. " \
+"Hook is not connected or valid, psm=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), ch->psm);
+		error = ENOTCONN;
+		goto drop;
+	}
+
+	NG_SEND_DATA_ONLY(error, l2cap->l2c, con->rx_pkt);
+	con->rx_pkt = NULL;
+drop:
+	NG_FREE_M(con->rx_pkt); /* checks for != NULL */
+
+	return (error);
+} /* ng_l2cap_receive */
+
+/*
+ * Receive connectioless (multicast) packet from the lower layer protocol and 
+ * send it to the upper layer protocol
+ */
+
+int
+ng_l2cap_l2ca_clt_receive(ng_l2cap_con_p con)
+{
+	struct _clt_pkt {
+		ng_l2cap_hdr_t		 h;
+		ng_l2cap_clt_hdr_t	 c_h;
+	} __attribute__ ((packed))	*hdr = NULL;
+	ng_l2cap_p			 l2cap = con->l2cap;
+	int				 length, error = 0;
+
+	NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr));
+	if (con->rx_pkt == NULL)
+		return (ENOBUFS);
+
+	hdr = mtod(con->rx_pkt, struct _clt_pkt *);
+
+	/* Check packet */
+	length = con->rx_pkt->m_pkthdr.len - sizeof(*hdr);
+	if (length < 0) {
+		NG_L2CAP_ERR(
+"%s: %s - invalid L2CAP CLT data packet. Packet too small, length=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), length);
+		error = EMSGSIZE;
+		goto drop;
+	}
+
+	/* Check payload size against CLT MTU */
+	if (length > NG_L2CAP_MTU_DEFAULT) {
+		NG_L2CAP_ERR(
+"%s: %s - invalid L2CAP CLT data packet. Packet too big, length=%d, mtu=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), length,
+			NG_L2CAP_MTU_DEFAULT);
+		error = EMSGSIZE;
+		goto drop;
+	}
+
+	hdr->c_h.psm = le16toh(hdr->c_h.psm);
+
+	/*
+	 * If we got here then everything looks good and we can sent packet
+	 * to the upper layer protocol.
+	 */
+
+	/* Select upstream hook based on PSM */
+	switch (hdr->c_h.psm) {
+	case NG_L2CAP_PSM_SDP:
+		if (l2cap->flags & NG_L2CAP_CLT_SDP_DISABLED)
+			goto drop;
+		break;
+
+	case NG_L2CAP_PSM_RFCOMM:
+		if (l2cap->flags & NG_L2CAP_CLT_RFCOMM_DISABLED)
+			goto drop;
+		break;
+
+	case NG_L2CAP_PSM_TCP:
+		if (l2cap->flags & NG_L2CAP_CLT_TCP_DISABLED)
+			goto drop;
+		break;
+        }
+
+	/* Check if upstream hook is connected and valid */
+	if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) {
+		NG_L2CAP_ERR(
+"%s: %s - unable to send L2CAP CLT data packet. " \
+"Hook is not connected or valid, psm=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), hdr->c_h.psm);
+		error = ENOTCONN;
+		goto drop;
+	}
+
+	NG_SEND_DATA_ONLY(error, l2cap->l2c, con->rx_pkt);
+	con->rx_pkt = NULL;
+drop:
+	NG_FREE_M(con->rx_pkt); /* checks for != NULL */
+
+	return (error);
+} /* ng_l2cap_l2ca_clt_receive */
+
+/*
+ * Send L2CA_QoSViolationInd to the upper layer protocol
+ */
+
+int
+ng_l2cap_l2ca_qos_ind(ng_l2cap_chan_p ch)
+{
+	ng_l2cap_p			 l2cap = ch->con->l2cap;
+	struct ng_mesg			*msg = NULL;
+	ng_l2cap_l2ca_qos_ind_ip	*ip = NULL;
+	int				 error = 0;
+
+	/* Check if upstream hook is connected and valid */
+	if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) {
+		NG_L2CAP_ERR(
+"%s: %s - unable to send L2CA_QoSViolationInd message. " \
+"Hook is not connected or valid, psm=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), ch->psm);
+
+		return (ENOTCONN);
+	}
+
+	/* Create and send L2CA_QoSViolationInd message */
+	NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_QOS_IND,
+		sizeof(*ip), M_NOWAIT);
+	if (msg == NULL)
+		error = ENOMEM;
+	else {
+		ip = (ng_l2cap_l2ca_qos_ind_ip *)(msg->data);
+		bcopy(&ch->con->remote, &ip->bdaddr, sizeof(ip->bdaddr));
+		NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, NULL);
+	}
+
+	return (error);
+} /* ng_l2cap_l2ca_qos_ind */
+
+/*
+ * Process L2CA_Disconnect request from the upper layer protocol.
+ */
+
+int
+ng_l2cap_l2ca_discon_req(ng_l2cap_p l2cap, struct ng_mesg *msg)
+{
+	ng_l2cap_l2ca_discon_ip	*ip = NULL;
+	ng_l2cap_chan_p		 ch = NULL;
+	ng_l2cap_cmd_p		 cmd = NULL;
+	int			 error = 0;
+
+	/* Check message */
+	if (msg->header.arglen != sizeof(*ip)) {
+		NG_L2CAP_ALERT(
+"%s: %s - invalid L2CA_Disconnect request message size, size=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node),
+			msg->header.arglen);
+		error = EMSGSIZE;
+		goto out;
+	}
+
+	ip = (ng_l2cap_l2ca_discon_ip *)(msg->data);
+
+	/* Check if we have this channel */
+	ch = ng_l2cap_chan_by_scid(l2cap, ip->lcid);
+	if (ch == NULL) {
+		NG_L2CAP_ERR(
+"%s: %s - unexpected L2CA_Disconnect request message. " \
+"Channel does not exist, lcid=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), ip->lcid);
+		error = ENOENT;
+		goto out;
+	}
+
+	/* Check channel state */
+	if (ch->state != NG_L2CAP_CONFIG && ch->state != NG_L2CAP_OPEN && 
+	    ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) {
+		NG_L2CAP_ERR(
+"%s: %s - unexpected L2CA_Disconnect request message. " \
+"Invalid channel state, state=%d, lcid=%d\n", 
+			__func__, NG_NODE_NAME(l2cap->node), ch->state,
+			ch->scid);
+		error = EINVAL;
+		goto out;
+	}
+
+	/* Create and send L2CAP_DisconReq message */
+	cmd = ng_l2cap_new_cmd(ch->con, ch, ng_l2cap_get_ident(ch->con),
+			NG_L2CAP_DISCON_REQ, msg->header.token);
+	if (cmd == NULL) {
+		ng_l2cap_free_chan(ch);
+		error = ENOMEM;
+		goto out;
+	}
+
+	if (cmd->ident == NG_L2CAP_NULL_IDENT) {
+		ng_l2cap_free_chan(ch);
+		ng_l2cap_free_cmd(cmd);
+		error = EIO;
+		goto out;
+	}
+
+	_ng_l2cap_discon_req(cmd->aux, cmd->ident, ch->dcid, ch->scid);
+	if (cmd->aux == NULL) {
+		ng_l2cap_free_chan(ch);
+		ng_l2cap_free_cmd(cmd);
+		error = ENOBUFS;
+		goto out;
+	}
+
+	ch->state = NG_L2CAP_W4_L2CAP_DISCON_RSP;
+
+	/* Link command to the queue */
+	ng_l2cap_link_cmd(ch->con, cmd);
+	ng_l2cap_lp_deliver(ch->con);
+out:
+	return (error);
+} /* ng_l2cap_l2ca_discon_req */
+
+/*
+ * Send L2CA_Disconnect response to the upper layer protocol
+ */
+
+int
+ng_l2cap_l2ca_discon_rsp(ng_l2cap_chan_p ch, u_int32_t token, u_int16_t result)
+{
+	ng_l2cap_p		 l2cap = ch->con->l2cap;
+	struct ng_mesg		*msg = NULL;
+	ng_l2cap_l2ca_discon_op	*op = NULL;
+	int			 error = 0;
+
+	/* Check if upstream hook is connected and valid */
+	if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) {
+		NG_L2CAP_ERR(
+"%s: %s - unable to send L2CA_Disconnect response message. " \
+"Hook is not connected or valid, psm=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), ch->psm);
+
+		return (ENOTCONN);
+	}
+
+	/* Create and send L2CA_Disconnect response message */
+	NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_DISCON,
+		sizeof(*op), M_NOWAIT);
+	if (msg == NULL)
+		error = ENOMEM;
+	else {
+		msg->header.token = token;
+		msg->header.flags |= NGF_RESP;
+
+		op = (ng_l2cap_l2ca_discon_op *)(msg->data);
+		op->result = result;
+
+		NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, NULL);
+	}
+
+	return (error);
+} /* ng_l2cap_l2ca_discon_rsp */
+
+/*
+ * Send L2CA_DisconnectInd message to the upper layer protocol.
+ */
+
+int
+ng_l2cap_l2ca_discon_ind(ng_l2cap_chan_p ch)
+{
+	ng_l2cap_p			 l2cap = ch->con->l2cap;
+	struct ng_mesg			*msg = NULL;
+	ng_l2cap_l2ca_discon_ind_ip	*ip = NULL;
+	int				 error = 0;
+
+	/* Check if upstream hook is connected and valid */
+	if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) {
+		NG_L2CAP_ERR(
+"%s: %s - unable to send L2CA_DisconnectInd message. " \
+"Hook is not connected or valid, psm=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), ch->psm);
+
+		return (ENOTCONN);
+	}
+
+	/* Create and send L2CA_DisconnectInd message */
+	NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_DISCON_IND,
+		sizeof(*ip), M_NOWAIT);
+	if (msg == NULL)
+		error = ENOMEM;
+	else {
+		ip = (ng_l2cap_l2ca_discon_ind_ip *)(msg->data);
+		ip->lcid = ch->scid;
+		NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, NULL);
+	} 
+
+	return (error);
+} /* ng_l2cap_l2ca_discon_ind */
+
+/*
+ * Process L2CA_GroupCreate request from the upper layer protocol.
+ * XXX FIXME
+ */
+
+int
+ng_l2cap_l2ca_grp_create(ng_l2cap_p l2cap, struct ng_mesg *msg)
+{
+	return (ENOTSUP);
+} /* ng_l2cap_l2ca_grp_create */
+
+/*
+ * Process L2CA_GroupClose request from the upper layer protocol
+ * XXX FIXME
+ */
+
+int
+ng_l2cap_l2ca_grp_close(ng_l2cap_p l2cap, struct ng_mesg *msg)
+{
+	return (ENOTSUP);
+} /* ng_l2cap_l2ca_grp_close */
+
+/*
+ * Process L2CA_GroupAddMember request from the upper layer protocol.
+ * XXX FIXME
+ */
+
+int
+ng_l2cap_l2ca_grp_add_member_req(ng_l2cap_p l2cap, struct ng_mesg *msg)
+{
+	return (ENOTSUP);
+} /* ng_l2cap_l2ca_grp_add_member_req */
+
+/*
+ * Send L2CA_GroupAddMember response to the upper layer protocol.
+ * XXX FIXME
+ */
+
+int
+ng_l2cap_l2ca_grp_add_member_rsp(ng_l2cap_chan_p ch, u_int32_t token,
+		u_int16_t result)
+{
+	return (0);
+} /* ng_l2cap_l2ca_grp_add_member_rsp */
+
+/*
+ * Process L2CA_GroupDeleteMember request from the upper layer protocol
+ * XXX FIXME
+ */
+
+int
+ng_l2cap_l2ca_grp_rem_member(ng_l2cap_p l2cap, struct ng_mesg *msg)
+{
+	return (ENOTSUP);
+} /* ng_l2cap_l2ca_grp_rem_member */
+
+/*
+ * Process L2CA_GroupGetMembers request from the upper layer protocol
+ * XXX FIXME
+ */
+
+int
+ng_l2cap_l2ca_grp_get_members(ng_l2cap_p l2cap, struct ng_mesg *msg)
+{
+	return (ENOTSUP);
+} /* ng_l2cap_l2ca_grp_get_members */
+
+/*
+ * Process L2CA_Ping request from the upper layer protocol
+ */
+
+int
+ng_l2cap_l2ca_ping_req(ng_l2cap_p l2cap, struct ng_mesg *msg)
+{
+	ng_l2cap_l2ca_ping_ip	*ip = NULL;
+	ng_l2cap_con_p		 con = NULL;
+	ng_l2cap_cmd_p		 cmd = NULL;
+	int			 error = 0;
+
+	/* Verify message */
+	if (msg->header.arglen < sizeof(*ip)) {
+		NG_L2CAP_ALERT(
+"%s: %s - invalid L2CA_Ping request message size, size=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node),
+			msg->header.arglen);
+		error = EMSGSIZE;
+		goto out;
+	}
+
+	ip = (ng_l2cap_l2ca_ping_ip *)(msg->data);
+	if (ip->echo_size > NG_L2CAP_MAX_ECHO_SIZE) {
+		NG_L2CAP_WARN(
+"%s: %s - invalid L2CA_Ping request. Echo size is too big, echo_size=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node), ip->echo_size);
+		error = EMSGSIZE;
+		goto out;
+	}
+
+	/* Check if we have connection to the unit */
+	con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr);
+	if (con == NULL) {
+		/* Submit LP_ConnectReq to the lower layer */
+		error = ng_l2cap_lp_con_req(l2cap, &ip->bdaddr);
+		if (error != 0) {
+			NG_L2CAP_ERR(
+"%s: %s - unable to send LP_ConnectReq message, error=%d\n",
+				__func__, NG_NODE_NAME(l2cap->node), error);
+			goto out;
+		}
+
+		/* This should not fail */
+		con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr);
+		KASSERT((con != NULL),
+("%s: %s - could not find connection!\n", __func__, NG_NODE_NAME(l2cap->node)));
+	}
+
+	/* Create L2CAP command descriptor */
+	cmd = ng_l2cap_new_cmd(con, NULL, ng_l2cap_get_ident(con),
+			NG_L2CAP_ECHO_REQ, msg->header.token);
+	if (cmd == NULL) {
+		error = ENOMEM;
+		goto out;
+	}
+
+	if (cmd->ident == NG_L2CAP_NULL_IDENT) {
+		ng_l2cap_free_cmd(cmd);
+                error = EIO;
+		goto out;
+	}
+
+	/* Create L2CAP command packet */
+	_ng_l2cap_echo_req(cmd->aux, cmd->ident, 
+			msg->data + sizeof(*ip), ip->echo_size);
+	if (cmd->aux == NULL) {
+		ng_l2cap_free_cmd(cmd);
+                error = ENOBUFS;
+		goto out;
+	}
+
+        /* Link command to the queue */
+        ng_l2cap_link_cmd(con, cmd);
+	ng_l2cap_lp_deliver(con);
+out:
+	return (error);
+} /* ng_l2cap_l2ca_ping_req */
+
+/*
+ * Send L2CA_Ping response to the upper layer protocol
+ */
+
+int
+ng_l2cap_l2ca_ping_rsp(ng_l2cap_con_p con, u_int32_t token, u_int16_t result,
+		struct mbuf *data)
+{
+	ng_l2cap_p		 l2cap = con->l2cap;
+	struct ng_mesg		*msg = NULL;
+	ng_l2cap_l2ca_ping_op	*op = NULL;
+	int			 error = 0, size = 0;
+
+	/* Check if control hook is connected and valid */
+	if (l2cap->ctl == NULL || NG_HOOK_NOT_VALID(l2cap->ctl)) {
+		NG_L2CAP_WARN(
+"%s: %s - unable to send L2CA_Ping response message. " \
+"Hook is not connected or valid\n",
+			__func__, NG_NODE_NAME(l2cap->node));
+		error = ENOTCONN;
+		goto out;
+	}
+
+	size = (data == NULL)? 0 : data->m_pkthdr.len;
+
+	/* Create and send L2CA_Ping response message */
+	NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_PING,
+		sizeof(*op) + size, M_NOWAIT);
+	if (msg == NULL)
+		error = ENOMEM;
+	else {
+		msg->header.token = token;
+		msg->header.flags |= NGF_RESP;
+
+		op = (ng_l2cap_l2ca_ping_op *)(msg->data);
+		op->result = result;
+		bcopy(&con->remote, &op->bdaddr, sizeof(op->bdaddr));
+		if (data != NULL && size > 0) {
+			op->echo_size = size;
+			m_copydata(data, 0, size, (caddr_t) op + sizeof(*op));
+		}
+
+		NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->ctl, NULL);
+	}
+out:
+	NG_FREE_M(data);
+
+	return (error);
+} /* ng_l2cap_l2ca_ping_rsp */
+
+/*
+ * Process L2CA_GetInfo request from the upper layer protocol
+ */
+
+int
+ng_l2cap_l2ca_get_info_req(ng_l2cap_p l2cap, struct ng_mesg *msg)
+{
+	ng_l2cap_l2ca_get_info_ip	*ip = NULL;
+	ng_l2cap_con_p			 con = NULL;
+	ng_l2cap_cmd_p			 cmd = NULL;
+	int				 error = 0;
+
+	/* Verify message */
+	if (msg->header.arglen != sizeof(*ip)) {
+		NG_L2CAP_ALERT(
+"%s: %s - invalid L2CA_GetInfo request message size, size=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node),
+			msg->header.arglen);
+		error = EMSGSIZE;
+		goto out;
+	}
+
+	ip = (ng_l2cap_l2ca_get_info_ip *)(msg->data);
+
+	/* Check if we have connection to the unit */
+	con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr);
+	if (con == NULL) {
+		/* Submit LP_ConnectReq to the lower layer */
+		error = ng_l2cap_lp_con_req(l2cap, &ip->bdaddr);
+		if (error != 0) {
+			NG_L2CAP_ERR(
+"%s: %s - unable to send LP_ConnectReq message, error=%d\n",
+				__func__, NG_NODE_NAME(l2cap->node), error);
+			goto out;
+		}
+
+		/* This should not fail */
+		con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr);
+		KASSERT((con != NULL),
+("%s: %s - could not find connection!\n", __func__, NG_NODE_NAME(l2cap->node)));
+	}
+
+	/* Create L2CAP command descriptor */
+	cmd = ng_l2cap_new_cmd(con, NULL, ng_l2cap_get_ident(con),
+			NG_L2CAP_INFO_REQ, msg->header.token);
+	if (cmd == NULL) {
+		error = ENOMEM;
+		goto out;
+	}
+
+	if (cmd->ident == NG_L2CAP_NULL_IDENT) {
+		ng_l2cap_free_cmd(cmd);
+		error = EIO;
+		goto out;
+	}
+
+	/* Create L2CAP command packet */
+	_ng_l2cap_info_req(cmd->aux, cmd->ident, ip->info_type);
+	if (cmd->aux == NULL) {
+		ng_l2cap_free_cmd(cmd);
+		error = ENOBUFS;
+		goto out;
+	}
+
+        /* Link command to the queue */
+	ng_l2cap_link_cmd(con, cmd);
+	ng_l2cap_lp_deliver(con);
+out:
+	return (error);
+} /* ng_l2cap_l2ca_get_info_req */
+
+/*
+ * Send L2CA_GetInfo response to the upper layer protocol
+ */
+
+int
+ng_l2cap_l2ca_get_info_rsp(ng_l2cap_con_p con, u_int32_t token, 
+		u_int16_t result, struct mbuf *data)
+{
+	ng_l2cap_p			 l2cap = con->l2cap;
+	struct ng_mesg			*msg = NULL;
+	ng_l2cap_l2ca_get_info_op	*op = NULL;
+	int				 error = 0, size;
+
+	/* Check if control hook is connected and valid */
+	if (l2cap->ctl == NULL || NG_HOOK_NOT_VALID(l2cap->ctl)) {
+		NG_L2CAP_WARN(
+"%s: %s - unable to send L2CA_GetInfo response message. " \
+"Hook is not connected or valid\n",
+			__func__, NG_NODE_NAME(l2cap->node));
+		error = ENOTCONN;
+		goto out;
+	}
+
+	size = (data == NULL)? 0 : data->m_pkthdr.len;
+
+	/* Create and send L2CA_GetInfo response message */
+	NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_GET_INFO,
+		sizeof(*op) + size, M_NOWAIT);
+	if (msg == NULL)
+		error = ENOMEM;
+	else {
+		msg->header.token = token;
+		msg->header.flags |= NGF_RESP;
+
+		op = (ng_l2cap_l2ca_get_info_op *)(msg->data);
+		op->result = result;
+		if (data != NULL && size > 0) {
+			op->info_size = size;
+			m_copydata(data, 0, size, (caddr_t) op + sizeof(*op));
+		}
+
+		NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->ctl, NULL);
+	}
+out:
+	NG_FREE_M(data);
+
+	return (error);
+} /* ng_l2cap_l2ca_get_info_rsp */
+	
+/*
+ * Process L2CA_EnableCLT message from the upper layer protocol
+ * XXX convert to NGN_L2CAP_NODE_SET_FLAGS?
+ */
+
+int
+ng_l2cap_l2ca_enable_clt(ng_l2cap_p l2cap, struct ng_mesg *msg)
+{
+	ng_l2cap_l2ca_enable_clt_ip	*ip = NULL;
+	int				 error = 0;
+#if 0
+ *	ng_l2cap_l2ca_enable_clt_op	*op = NULL;
+ *	u_int16_t			 result; 
+ * 	u_int32_t			 token;
+#endif
+
+	/* Check message */
+	if (msg->header.arglen != sizeof(*ip)) {
+		NG_L2CAP_ALERT(
+"%s: %s - invalid L2CA_EnableCLT message size, size=%d\n",
+			__func__, NG_NODE_NAME(l2cap->node),
+			msg->header.arglen);
+
+		return (EMSGSIZE);
+	}
+
+	/* Process request */
+	ip = (ng_l2cap_l2ca_enable_clt_ip *) (msg->data);
+#if 0
+ *	result = NG_L2CAP_SUCCESS;
+#endif
+
+	switch (ip->psm) 
+	{
+	case 0:
+		/* Special case: disable/enable all PSM */
+		if (ip->enable)
+			l2cap->flags &= ~(NG_L2CAP_CLT_SDP_DISABLED    |
+					  NG_L2CAP_CLT_RFCOMM_DISABLED |
+					  NG_L2CAP_CLT_TCP_DISABLED);
+		else
+			l2cap->flags |= (NG_L2CAP_CLT_SDP_DISABLED    |
+					 NG_L2CAP_CLT_RFCOMM_DISABLED |
+					 NG_L2CAP_CLT_TCP_DISABLED);
+		break;
+
+	case NG_L2CAP_PSM_SDP:
+		if (ip->enable)
+			l2cap->flags &= ~NG_L2CAP_CLT_SDP_DISABLED;
+		else
+			l2cap->flags |= NG_L2CAP_CLT_SDP_DISABLED;
+		break;
+
+	case NG_L2CAP_PSM_RFCOMM:
+		if (ip->enable)
+			l2cap->flags &= ~NG_L2CAP_CLT_RFCOMM_DISABLED;
+		else
+			l2cap->flags |= NG_L2CAP_CLT_RFCOMM_DISABLED;
+		break;
+
+	case NG_L2CAP_PSM_TCP:
+		if (ip->enable)
+			l2cap->flags &= ~NG_L2CAP_CLT_TCP_DISABLED;
+		else
+			l2cap->flags |= NG_L2CAP_CLT_TCP_DISABLED;
+		break;
+	
+	default:
+		NG_L2CAP_ERR(
+"%s: %s - unsupported PSM=%d\n", __func__, NG_NODE_NAME(l2cap->node), ip->psm);
+#if 0
+ *		result = NG_L2CAP_PSM_NOT_SUPPORTED;
+#endif
+		error = ENOTSUP;
+		break;
+	}
+
+#if 0
+ *	/* Create and send response message */
+ * 	token = msg->header.token;
+ * 	NG_FREE_MSG(msg);
+ * 	NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_ENABLE_CLT,
+ * 		sizeof(*op), M_NOWAIT);
+ * 	if (msg == NULL)
+ * 		error = ENOMEM;
+ * 	else {
+ * 		msg->header.token = token;
+ * 		msg->header.flags |= NGF_RESP;
+ * 
+ * 		op = (ng_l2cap_l2ca_enable_clt_op *)(msg->data);
+ * 		op->result = result;
+ * 	}
+ * 
+ * 	/* Send response to control hook */
+ * 	if (l2cap->ctl != NULL && NG_HOOK_IS_VALID(l2cap->ctl))
+ * 		NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->ctl, NULL);
+#endif
+
+	return (error);
+} /* ng_l2cap_l2ca_enable_clt */
+
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.h b/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.h
new file mode 100644
index 0000000..6c464e6
--- /dev/null
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.h
@@ -0,0 +1,77 @@
+/*
+ * ng_l2cap_ulpi.h
+ *
+ * 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_ulpi.h,v 1.5 2002/07/04 21:48:53 max Exp $
+ * $FreeBSD$
+ */
+
+#ifndef _NETGRAPH_L2CAP_ULPI_H_
+#define _NETGRAPH_L2CAP_ULPI_H_
+
+int ng_l2cap_l2ca_con_req     (ng_l2cap_p, struct ng_mesg *);
+int ng_l2cap_l2ca_con_rsp    (ng_l2cap_chan_p, u_int32_t, u_int16_t, u_int16_t);
+int ng_l2cap_l2ca_con_rsp_req (ng_l2cap_p, struct ng_mesg *);
+int ng_l2cap_l2ca_con_rsp_rsp (ng_l2cap_chan_p, u_int32_t, u_int16_t);
+int ng_l2cap_l2ca_con_ind     (ng_l2cap_chan_p);
+
+int ng_l2cap_l2ca_cfg_req     (ng_l2cap_p, struct ng_mesg *);
+int ng_l2cap_l2ca_cfg_rsp     (ng_l2cap_chan_p, u_int32_t, u_int16_t);
+int ng_l2cap_l2ca_cfg_rsp_req (ng_l2cap_p, struct ng_mesg *);
+int ng_l2cap_l2ca_cfg_rsp_rsp (ng_l2cap_chan_p, u_int32_t, u_int16_t);
+int ng_l2cap_l2ca_cfg_ind     (ng_l2cap_chan_p);
+
+int ng_l2cap_l2ca_write_req   (ng_l2cap_p, struct mbuf *);
+int ng_l2cap_l2ca_write_rsp  (ng_l2cap_chan_p, u_int32_t, u_int16_t, u_int16_t);
+
+int ng_l2cap_l2ca_receive     (ng_l2cap_con_p);
+int ng_l2cap_l2ca_clt_receive (ng_l2cap_con_p);
+
+int ng_l2cap_l2ca_qos_ind     (ng_l2cap_chan_p);
+
+int ng_l2cap_l2ca_discon_req  (ng_l2cap_p, struct ng_mesg *);
+int ng_l2cap_l2ca_discon_rsp  (ng_l2cap_chan_p, u_int32_t, u_int16_t);
+int ng_l2cap_l2ca_discon_ind  (ng_l2cap_chan_p);
+
+int ng_l2cap_l2ca_grp_create  (ng_l2cap_p, struct ng_mesg *);
+int ng_l2cap_l2ca_grp_close   (ng_l2cap_p, struct ng_mesg *);
+int ng_l2cap_l2ca_grp_add_member_req (ng_l2cap_p, struct ng_mesg *);
+int ng_l2cap_l2ca_grp_add_member_rsp (ng_l2cap_chan_p, u_int32_t, u_int16_t);
+int ng_l2cap_l2ca_grp_rem_member  (ng_l2cap_p, struct ng_mesg *);
+int ng_l2cap_l2ca_grp_get_members (ng_l2cap_p, struct ng_mesg *);
+
+int ng_l2cap_l2ca_ping_req     (ng_l2cap_p, struct ng_mesg *);
+int ng_l2cap_l2ca_ping_rsp     (ng_l2cap_con_p, u_int32_t, u_int16_t, 
+					struct mbuf *);
+
+int ng_l2cap_l2ca_get_info_req (ng_l2cap_p, struct ng_mesg *);
+int ng_l2cap_l2ca_get_info_rsp (ng_l2cap_con_p, u_int32_t, u_int16_t, 
+					struct mbuf *);
+
+int ng_l2cap_l2ca_enable_clt   (ng_l2cap_p, struct ng_mesg *);
+
+#endif /* ndef _NETGRAPH_L2CAP_ULPI_H_ */
+
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_var.h b/sys/netgraph/bluetooth/l2cap/ng_l2cap_var.h
new file mode 100644
index 0000000..057fa28
--- /dev/null
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_var.h
@@ -0,0 +1,182 @@
+/*
+ * ng_l2cap_var.h
+ *
+ * Copyright (c) 2001 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_var.h,v 1.13 2002/09/04 21:38:38 max Exp $
+ * $FreeBSD$
+ */
+
+#ifndef _NETGRAPH_L2CAP_VAR_H_
+#define _NETGRAPH_L2CAP_VAR_H_ 1
+
+/* MALLOC decalation */
+#ifdef NG_SEPARATE_MALLOC
+MALLOC_DECLARE(M_NETGRAPH_L2CAP);
+#else
+#define M_NETGRAPH_L2CAP M_NETGRAPH
+#endif /* NG_SEPARATE_MALLOC */
+
+/* Debug */
+#define	NG_L2CAP_ALERT	if (l2cap->debug >= NG_L2CAP_ALERT_LEVEL) printf
+#define	NG_L2CAP_ERR	if (l2cap->debug >= NG_L2CAP_ERR_LEVEL)   printf
+#define	NG_L2CAP_WARN	if (l2cap->debug >= NG_L2CAP_WARN_LEVEL)  printf
+#define	NG_L2CAP_INFO	if (l2cap->debug >= NG_L2CAP_INFO_LEVEL)  printf
+
+/* Wrapper around m_pullup */
+#define NG_L2CAP_M_PULLUP(m, s) \
+	do { \
+		if ((m)->m_len < (s)) \
+			(m) = m_pullup((m), (s)); \
+		if ((m) == NULL) \
+			NG_L2CAP_ALERT("%s: %s - m_pullup(%d) failed\n", \
+				__func__, NG_NODE_NAME(l2cap->node), (s)); \
+	} while (0)
+
+/*
+ * L2CAP signaling command ident's are assigned relative to the connection,
+ * because there is only one signaling channel (cid == 0x01) for every 
+ * connection. So up to 254 (0xff - 0x01) L2CAP commands can be pending at the 
+ * same time for the same connection.
+ */
+
+#define NG_L2CAP_NULL_IDENT	0x00        /* DO NOT USE THIS IDENT */ 
+#define NG_L2CAP_FIRST_IDENT	0x01        /* dynamically alloc. (start) */ 
+#define NG_L2CAP_LAST_IDENT	0xff        /* dynamically alloc. (end) */ 
+
+/* 
+ * L2CAP (Node private)
+ */
+
+struct ng_l2cap_con;
+struct ng_l2cap_chan;
+
+typedef struct ng_l2cap {
+	node_p				node;         /* node ptr */
+
+	ng_l2cap_node_debug_ep		debug;        /* debug level */
+	ng_l2cap_node_flags_ep		flags;        /* L2CAP node flags */
+
+	bdaddr_t			bdaddr;       /* unit BDADDR */
+	u_int16_t			pkt_size;     /* max. ACL packet size */
+	u_int16_t			num_pkts;     /* out queue size */
+
+	hook_p				hci;          /* HCI downstream hook */
+	hook_p				l2c;          /* L2CAP upstream hook */
+	hook_p				ctl;          /* control hook */
+
+	LIST_HEAD(, ng_l2cap_con)	con_list;     /* ACL connections */
+
+	u_int16_t			cid;          /* last allocated CID */
+	LIST_HEAD(, ng_l2cap_chan)	chan_list;    /* L2CAP channels */
+} ng_l2cap_t;
+typedef ng_l2cap_t *			ng_l2cap_p;
+
+/* 
+ * L2CAP connection descriptor
+ */
+
+struct ng_l2cap_cmd;
+
+typedef struct ng_l2cap_con {
+	ng_l2cap_p			 l2cap;      /* pointer to L2CAP */
+
+	u_int16_t			 state;      /* ACL connection state */
+
+	bdaddr_t			 remote;     /* remote unit address */
+	u_int16_t			 con_handle; /* ACL connection handle */
+	struct callout_handle		 con_timo;   /* connection timeout */
+
+	u_int8_t			 ident;      /* last allocated ident */
+	TAILQ_HEAD(, ng_l2cap_cmd)	 cmd_list;   /* pending L2CAP cmds */
+
+	struct mbuf			*tx_pkt;     /* xmitted L2CAP packet */
+	int				 pending;    /* num. of pending pkts */
+
+	struct mbuf			*rx_pkt;     /* received L2CAP packet */
+	int				 rx_pkt_len; /* packet len. so far */
+
+	LIST_ENTRY(ng_l2cap_con)	 next;       /* link */
+} ng_l2cap_con_t;
+typedef ng_l2cap_con_t *		ng_l2cap_con_p;
+
+/*
+ * L2CAP channel descriptor
+ */
+
+typedef struct ng_l2cap_chan {
+	ng_l2cap_con_p			con;        /* pointer to connection */
+
+	u_int16_t			state;      /* channel state */
+
+	u_int8_t			cfg_state;  /* configuration state */
+#define NG_L2CAP_CFG_IN			(1 << 0)    /* incoming cfg path done */
+#define NG_L2CAP_CFG_OUT		(1 << 1)    /* outgoing cfg path done */
+#define NG_L2CAP_CFG_BOTH		(NG_L2CAP_CFG_IN|NG_L2CAP_CFG_OUT)
+
+	u_int8_t			ident;      /* last L2CAP req. ident */
+
+	u_int16_t			psm;        /* channel PSM */
+	u_int16_t			scid;       /* source channel ID */
+	u_int16_t			dcid;       /* destination channel ID */
+
+	u_int16_t			imtu;       /* incoming channel MTU */
+	ng_l2cap_flow_t			iflow;      /* incoming flow control */
+
+	u_int16_t			omtu;       /* outgoing channel MTU */
+	ng_l2cap_flow_t			oflow;      /* outgoing flow control */
+
+	u_int16_t			flush_timo; /* flush timeout */
+	u_int16_t			link_timo;  /* link timeout */
+
+	LIST_ENTRY(ng_l2cap_chan)	next;       /* link */
+} ng_l2cap_chan_t;
+typedef ng_l2cap_chan_t *		ng_l2cap_chan_p;
+
+/*
+ * L2CAP command descriptor
+ */
+
+typedef struct ng_l2cap_cmd {
+	ng_l2cap_con_p			 con;       /* L2CAP connection */
+	ng_l2cap_chan_p			 ch;        /* L2CAP channel */
+
+	u_int16_t 			 flags;     /* command flags */
+#define NG_L2CAP_CMD_PENDING		 (1 << 0)   /* command is pending */
+
+	u_int8_t 			 code;      /* L2CAP command opcode */
+	u_int8_t			 ident;     /* L2CAP command ident */
+	u_int32_t			 token;     /* L2CA message token */
+
+	struct callout_handle		 timo;      /* RTX/ERTX timeout */
+
+	struct mbuf			*aux;       /* optional data */
+
+	TAILQ_ENTRY(ng_l2cap_cmd)	 next;      /* link */
+} ng_l2cap_cmd_t;
+typedef ng_l2cap_cmd_t *		ng_l2cap_cmd_p;
+
+#endif /* ndef _NETGRAPH_L2CAP_VAR_H_ */
+