Part one of undating the bluetooth code to the newest version

Submitted by:   Maksim Yevmenkin <m_evmenkin@yahoo.com>
Approved by: re@

6 files changed, 4611 insertions, 523 deletions

diff --git a/sys/netgraph/bluetooth/socket/TODO b/sys/netgraph/bluetooth/socket/TODO
index 97ba9ed..c1aa3b2 100644
--- a/sys/netgraph/bluetooth/socket/TODO
+++ b/sys/netgraph/bluetooth/socket/TODO
@@ -1,4 +1,4 @@
-$Id: TODO,v 1.4 2002/09/06 21:03:56 max Exp $
+$Id: TODO,v 1.1 2002/11/24 19:47:07 max Exp $
 $FreeBSD$
 
 FIXME/TODO list
diff --git a/sys/netgraph/bluetooth/socket/ng_btsocket.c b/sys/netgraph/bluetooth/socket/ng_btsocket.c
index f3eb8ff..269e9a2 100644
--- a/sys/netgraph/bluetooth/socket/ng_btsocket.c
+++ b/sys/netgraph/bluetooth/socket/ng_btsocket.c
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_btsocket.c,v 1.20 2002/09/13 17:56:58 max Exp $
+ * $Id: ng_btsocket.c,v 1.3 2003/01/19 00:19:04 max Exp $
  * $FreeBSD$
  */
 
@@ -40,6 +40,7 @@
 #include <sys/socket.h>
 #include <sys/socketvar.h>
 #include <sys/sysctl.h>
+#include <sys/taskqueue.h>
 #include <bitstring.h>
 #include <netgraph/ng_message.h>
 #include <netgraph/netgraph.h>
@@ -49,6 +50,7 @@
 #include "ng_btsocket.h"
 #include "ng_btsocket_hci_raw.h"
 #include "ng_btsocket_l2cap.h"
+#include "ng_btsocket_rfcomm.h"
 
 static int			ng_btsocket_modevent (module_t, int, void *);
 extern struct domain		ng_btsocket_domain;
@@ -134,6 +136,33 @@ static struct pr_usrreqs	ng_btsocket_l2cap_usrreqs = {
 	sopoll
 };
 
+/*
+ * Bluetooth STREAM RFCOMM sockets
+ */
+
+static struct pr_usrreqs	ng_btsocket_rfcomm_usrreqs = {
+	ng_btsocket_rfcomm_abort,	/* abort */
+	ng_btsocket_rfcomm_accept,	/* accept */
+	ng_btsocket_rfcomm_attach,	/* attach */
+	ng_btsocket_rfcomm_bind,	/* bind */
+	ng_btsocket_rfcomm_connect,	/* connect */
+	pru_connect2_notsupp,		/* connect2 */
+	ng_btsocket_rfcomm_control,	/* control */
+	ng_btsocket_rfcomm_detach,	/* detach */
+	ng_btsocket_rfcomm_disconnect,	/* disconnect */
+        ng_btsocket_rfcomm_listen,	/* listen */
+	ng_btsocket_rfcomm_peeraddr,	/* peeraddr */
+	pru_rcvd_notsupp,		/* rcvd */
+	pru_rcvoob_notsupp,		/* rcvoob */
+	ng_btsocket_rfcomm_send,	/* send */
+	pru_sense_null,			/* send */
+	NULL,				/* shutdown */
+	ng_btsocket_rfcomm_sockaddr,	/* sockaddr */
+	sosend,
+	soreceive,
+	sopoll
+};
+
 /* 
  * Definitions of protocols supported in the BLUETOOTH domain 
  */
@@ -177,6 +206,19 @@ static struct protosw		ng_btsocket_protosw[] = {
 	NULL, NULL, NULL,		/* fasttimeo, slowtimo, drain */
 	&ng_btsocket_l2cap_usrreqs,	/* usrreq table (above) */
 	/* { NULL } */			/* pfh (protocol filter head?) */
+},
+{
+	SOCK_STREAM,			/* protocol type */
+	&ng_btsocket_domain,		/* backpointer to domain */
+	BLUETOOTH_PROTO_RFCOMM,		/* protocol */
+	PR_ATOMIC | PR_CONNREQUIRED,	/* flags */
+	NULL, NULL, NULL,		/* input, output, ctlinput */
+	ng_btsocket_rfcomm_ctloutput,	/* ctloutput */
+	NULL,				/* ousrreq() */
+	ng_btsocket_rfcomm_init,	/* init */
+	NULL, NULL, NULL,		/* fasttimeo, slowtimo, drain */
+	&ng_btsocket_rfcomm_usrreqs,	/* usrreq table (above) */
+	/* { NULL } */			/* pfh (protocol filter head?) */
 }
 };
 #define ng_btsocket_protosw_size \
@@ -210,6 +252,8 @@ SYSCTL_NODE(_net_bluetooth_hci, OID_AUTO, sockets, CTLFLAG_RW,
 	0, "Bluetooth HCI sockets family");
 SYSCTL_NODE(_net_bluetooth_l2cap, OID_AUTO, sockets, CTLFLAG_RW,
 	0, "Bluetooth L2CAP sockets family");
+SYSCTL_NODE(_net_bluetooth_rfcomm, OID_AUTO, sockets, CTLFLAG_RW,
+	0, "Bluetooth RFCOMM sockets family");
 
 /* 
  * Module 
diff --git a/sys/netgraph/bluetooth/socket/ng_btsocket_hci_raw.c b/sys/netgraph/bluetooth/socket/ng_btsocket_hci_raw.c
index d8da877..562f377 100644
--- a/sys/netgraph/bluetooth/socket/ng_btsocket_hci_raw.c
+++ b/sys/netgraph/bluetooth/socket/ng_btsocket_hci_raw.c
@@ -25,13 +25,14 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_btsocket_hci_raw.c,v 1.3 2002/11/12 22:31:39 max Exp $
+ * $Id: ng_btsocket_hci_raw.c,v 1.13 2003/04/01 18:15:27 max Exp $
  * $FreeBSD$
  */
 
 #include <sys/param.h>
 #include <sys/systm.h>
 #include <sys/domain.h>
+#include <sys/endian.h>
 #include <sys/errno.h>
 #include <sys/filedesc.h>
 #include <sys/ioccom.h>
@@ -77,6 +78,17 @@ static void 		ng_btsocket_hci_raw_output(node_p, hook_p, void *, int);
 static void		ng_btsocket_hci_raw_savctl(ng_btsocket_hci_raw_pcb_p, 
 						   struct mbuf **,
 						   struct mbuf *); 
+static int		ng_btsocket_hci_raw_filter(ng_btsocket_hci_raw_pcb_p,
+						   struct mbuf *, int);
+
+#define ng_btsocket_hci_raw_wakeup_input_task() \
+	taskqueue_enqueue(taskqueue_swi_giant, &ng_btsocket_hci_raw_task)
+
+/* Security filter */
+struct ng_btsocket_hci_raw_sec_filter {
+	bitstr_t	bit_decl(events, 0xff);
+	bitstr_t	bit_decl(commands[0x3f], 0x3ff);
+};
 
 /* Netgraph type descriptor */
 static struct ng_type   typestruct = {
@@ -106,7 +118,8 @@ static LIST_HEAD(, ng_btsocket_hci_raw_pcb)	ng_btsocket_hci_raw_sockets;
 static struct mtx				ng_btsocket_hci_raw_sockets_mtx;
 static u_int32_t				ng_btsocket_hci_raw_token;
 static struct mtx				ng_btsocket_hci_raw_token_mtx;
-
+static struct ng_btsocket_hci_raw_sec_filter	*ng_btsocket_hci_raw_sec_filter;
+ 
 /* Sysctl tree */
 SYSCTL_DECL(_net_bluetooth_hci_sockets);
 SYSCTL_NODE(_net_bluetooth_hci_sockets, OID_AUTO, raw, CTLFLAG_RW,
@@ -236,11 +249,25 @@ static int
 ng_btsocket_hci_raw_node_rcvmsg(node_p node, item_p item, hook_p lasthook) 
 {
 	struct ng_mesg	*msg = NGI_MSG(item); /* item still has message */
-	int		 error = 0;
+	int		 empty, error = 0;
+
+	mtx_lock(&ng_btsocket_hci_raw_sockets_mtx);
+	empty = LIST_EMPTY(&ng_btsocket_hci_raw_sockets);
+	mtx_unlock(&ng_btsocket_hci_raw_sockets_mtx);
+
+	if (empty) {
+		NG_FREE_ITEM(item);
+		return (0);
+	}
 
 	if (msg != NULL &&
 	    msg->header.typecookie == NGM_HCI_COOKIE &&
 	    msg->header.flags & NGF_RESP) {
+		if (msg->header.token == 0) {
+			NG_FREE_ITEM(item);
+			return (0);
+		}
+
 		mtx_lock(&ng_btsocket_hci_raw_queue_mtx);
 		if (NG_BT_ITEMQ_FULL(&ng_btsocket_hci_raw_queue)) {
 			NG_BTSOCKET_HCI_RAW_ERR(
@@ -251,8 +278,7 @@ ng_btsocket_hci_raw_node_rcvmsg(node_p node, item_p item, hook_p lasthook)
 			error = ENOBUFS;
 		} else {
 			NG_BT_ITEMQ_ENQUEUE(&ng_btsocket_hci_raw_queue, item);
-			error = taskqueue_enqueue(taskqueue_swi_giant,
-						&ng_btsocket_hci_raw_task);
+			error = ng_btsocket_hci_raw_wakeup_input_task();
 		}
 		mtx_unlock(&ng_btsocket_hci_raw_queue_mtx);
 	} else {
@@ -273,7 +299,16 @@ static int
 ng_btsocket_hci_raw_node_rcvdata(hook_p hook, item_p item)
 {
 	struct mbuf	*nam = NULL;
-	int		 error;
+	int		 empty, error;
+
+	mtx_lock(&ng_btsocket_hci_raw_sockets_mtx);
+	empty = LIST_EMPTY(&ng_btsocket_hci_raw_sockets);
+	mtx_unlock(&ng_btsocket_hci_raw_sockets_mtx);
+
+	if (empty) {
+		NG_FREE_ITEM(item);
+		return (0);
+	}
 
 	MGET(nam, M_DONTWAIT, MT_SONAME);
 	if (nam != NULL) {
@@ -283,9 +318,8 @@ ng_btsocket_hci_raw_node_rcvdata(hook_p hook, item_p item)
 
 		sa->hci_len = sizeof(*sa);
 		sa->hci_family = AF_BLUETOOTH;
-		strncpy(sa->hci_node, NG_PEER_NODE_NAME(hook),
+		strlcpy(sa->hci_node, NG_PEER_NODE_NAME(hook),
 			sizeof(sa->hci_node));
-		sa->hci_node[sizeof(sa->hci_node) - 1] = 0; /* sanity */
 
 		NGI_GET_M(item, nam->m_next);
 		NGI_M(item) = nam;
@@ -300,8 +334,7 @@ ng_btsocket_hci_raw_node_rcvdata(hook_p hook, item_p item)
 			error = ENOBUFS;
 		} else {
 			NG_BT_ITEMQ_ENQUEUE(&ng_btsocket_hci_raw_queue, item);
-			error = taskqueue_enqueue(taskqueue_swi_giant,
-						&ng_btsocket_hci_raw_task);
+			error = ng_btsocket_hci_raw_wakeup_input_task();
 		}
 		mtx_unlock(&ng_btsocket_hci_raw_queue_mtx);
 	} else {
@@ -322,33 +355,35 @@ ng_btsocket_hci_raw_node_rcvdata(hook_p hook, item_p item)
  ****************************************************************************/
 
 /*
- * Get next token
+ * Get next token. We need token to avoid theoretical race where process
+ * submits ioctl() message then interrupts ioctl() and re-submits another
+ * ioctl() on the same socket *before* first ioctl() complete.
  */
-
+ 
 static void
 ng_btsocket_hci_raw_get_token(u_int32_t *token)
 {
 	mtx_lock(&ng_btsocket_hci_raw_token_mtx);
-
+  
 	if (++ ng_btsocket_hci_raw_token == 0)
 		ng_btsocket_hci_raw_token = 1;
-
+ 
 	*token = ng_btsocket_hci_raw_token;
-	
+ 
 	mtx_unlock(&ng_btsocket_hci_raw_token_mtx);
-} /* ng_btsocket_hci_raw_token */
+} /* ng_btsocket_hci_raw_get_token */
 
 /*
  * Send Netgraph message to the node - do not expect reply
  */
 
 static int
-ng_btsocket_raw_send_ngmsg(char *path, int cmd, void *arg, int arglen)
+ng_btsocket_hci_raw_send_ngmsg(char *path, int cmd, void *arg, int arglen)
 {
 	struct ng_mesg	*msg = NULL;
 	int		 error = 0;
 
-	NG_MKMESSAGE(msg, NGM_HCI_COOKIE, cmd, arglen, M_WAITOK);
+	NG_MKMESSAGE(msg, NGM_HCI_COOKIE, cmd, arglen, M_NOWAIT);
 	if (msg == NULL)
 		return (ENOMEM);
 
@@ -358,28 +393,28 @@ ng_btsocket_raw_send_ngmsg(char *path, int cmd, void *arg, int arglen)
 	NG_SEND_MSG_PATH(error, ng_btsocket_hci_raw_node, msg, path, NULL);
 
 	return (error);
-} /* ng_btsocket_raw_send_ngmsg */
+} /* ng_btsocket_hci_raw_send_ngmsg */
 
 /*
  * Send Netgraph message to the node (no data) and wait for reply 
  */
 
 static int
-ng_btsocket_raw_send_sync_ngmsg(ng_btsocket_hci_raw_pcb_p pcb, char *path,
+ng_btsocket_hci_raw_send_sync_ngmsg(ng_btsocket_hci_raw_pcb_p pcb, char *path,
 		int cmd, void *rsp, int rsplen)
 {
 	struct ng_mesg	*msg = NULL;
 	int		 error = 0;
 
-	ng_btsocket_hci_raw_get_token(&pcb->token);
-	pcb->msg = NULL;
+	mtx_assert(&pcb->pcb_mtx, MA_OWNED);
 
-	NG_MKMESSAGE(msg, NGM_HCI_COOKIE, cmd, 0, M_WAITOK);
-	if (msg == NULL) {
-		pcb->token = 0;
+	NG_MKMESSAGE(msg, NGM_HCI_COOKIE, cmd, 0, M_NOWAIT);
+	if (msg == NULL)
 		return (ENOMEM);
-	}
-	msg->header.token = pcb->token;
+
+	ng_btsocket_hci_raw_get_token(&msg->header.token);
+	pcb->token = msg->header.token;
+	pcb->msg = NULL;
 
 	NG_SEND_MSG_PATH(error, ng_btsocket_hci_raw_node, msg, path, NULL);
 	if (error != 0) {
@@ -387,12 +422,12 @@ ng_btsocket_raw_send_sync_ngmsg(ng_btsocket_hci_raw_pcb_p pcb, char *path,
 		return (error);
 	}
 
-	error = tsleep(&pcb->msg, PZERO|PCATCH, "hcictl", 
+	error = msleep(&pcb->msg, &pcb->pcb_mtx, PZERO|PCATCH, "hcictl", 
 			ng_btsocket_hci_raw_ioctl_timeout * hz);
-	if (error != 0) {
-		pcb->token = 0;
+	pcb->token = 0;
+
+	if (error != 0)
 		return (error);
-	}
 
 	if (pcb->msg != NULL && pcb->msg->header.cmd == cmd)
 		bcopy(pcb->msg->data, rsp, rsplen);
@@ -400,10 +435,9 @@ ng_btsocket_raw_send_sync_ngmsg(ng_btsocket_hci_raw_pcb_p pcb, char *path,
 		error = EINVAL;
 
 	NG_FREE_MSG(pcb->msg); /* checks for != NULL */
-	pcb->token = 0;
 
 	return (0);
-} /* ng_btsocket_raw_send_sync_ngmsg */
+} /* ng_btsocket_hci_raw_send_sync_ngmsg */
 
 /*
  * Create control information for the packet
@@ -416,6 +450,8 @@ ng_btsocket_hci_raw_savctl(ng_btsocket_hci_raw_pcb_p pcb, struct mbuf **ctl,
 	int		dir;
 	struct timeval	tv;
 
+	mtx_assert(&pcb->pcb_mtx, MA_OWNED);
+
 	if (pcb->flags & NG_BTSOCKET_HCI_RAW_DIRECTION) {
 		dir = (m->m_flags & M_PROTO1)? 1 : 0;
 		*ctl = sbcreatecontrol((caddr_t) &dir, sizeof(dir),
@@ -443,21 +479,23 @@ ng_btsocket_hci_raw_data_input(struct mbuf *nam)
 	ng_btsocket_hci_raw_pcb_p	 pcb = NULL;
 	struct mbuf			*m0 = NULL, *m = NULL;
 	struct sockaddr_hci		*sa = NULL;
-	bitstr_t			*mask = NULL;
-	int				 bit;
 
 	m0 = nam->m_next;
 	nam->m_next = NULL;
 
 	KASSERT((nam->m_type == MT_SONAME),
 		("%s: m_type=%d\n", __func__, nam->m_type));
-	M_ASSERTPKTHDR(m0);
+	KASSERT((m0->m_flags & M_PKTHDR),
+		("%s: m_flags=%#x\n", __func__, m0->m_flags));
 
 	sa = mtod(nam, struct sockaddr_hci *);
 
 	mtx_lock(&ng_btsocket_hci_raw_sockets_mtx);
 
 	LIST_FOREACH(pcb, &ng_btsocket_hci_raw_sockets, next) {
+
+		mtx_lock(&pcb->pcb_mtx);
+
 		/*
 		 * If socket was bound then check address and
 		 *  make sure it matches.
@@ -465,37 +503,15 @@ ng_btsocket_hci_raw_data_input(struct mbuf *nam)
 
 		if (pcb->addr.hci_node[0] != 0 &&
 		    strcmp(sa->hci_node, pcb->addr.hci_node) != 0)
-			continue;
+			goto next;
 
 		/*
-		 * Check packet agains socket filter
+		 * Check packet against filters
 		 * XXX do we have to call m_pullup() here?
 		 */
 
-		switch (*mtod(m0, u_int8_t *)) {
-		case NG_HCI_CMD_PKT:
-		case NG_HCI_ACL_DATA_PKT:
-		case NG_HCI_SCO_DATA_PKT:
-			mask = pcb->filter.packet_mask;
-			bit = *mtod(m0, u_int8_t *) - 1;
-			break;
-
-		case NG_HCI_EVENT_PKT:
-			mask = pcb->filter.event_mask;
-			bit = mtod(m0, ng_hci_event_pkt_t *)->event - 1;
-			break;
-
-		default:
-			KASSERT(0, 
-("%s: invalid packet type=%#x\n", __func__, *mtod(m0, u_int8_t *)));
-
-			mask = NULL;
-			bit = 0;
-			break;
-		}
-
-		if (mask == NULL || !bit_test(mask, bit))
-			continue;
+		if (ng_btsocket_hci_raw_filter(pcb, m0, 1) != 0)
+			goto next;
 
 		/*
 		 * Make a copy of the packet, append to the socket's
@@ -513,13 +529,15 @@ ng_btsocket_hci_raw_data_input(struct mbuf *nam)
 					(struct sockaddr *) sa, m, ctl))
 				sorwakeup(pcb->so);
 			else {
-				NG_BTSOCKET_HCI_RAW_WARN(
-"%s: sbappendadd() failed\n", __func__);
+				NG_BTSOCKET_HCI_RAW_INFO(
+"%s: sbappendaddr() failed\n", __func__);
 
 				NG_FREE_M(m);
 				NG_FREE_M(ctl);
 			}
 		}
+next:
+		mtx_unlock(&pcb->pcb_mtx);
 	}
 
 	mtx_unlock(&ng_btsocket_hci_raw_sockets_mtx);
@@ -537,21 +555,26 @@ ng_btsocket_hci_raw_msg_input(struct ng_mesg *msg)
 {
 	ng_btsocket_hci_raw_pcb_p	 pcb = NULL;
 
-	if (msg->header.token != 0) {
-		mtx_lock(&ng_btsocket_hci_raw_sockets_mtx);
+	mtx_lock(&ng_btsocket_hci_raw_sockets_mtx);
+
+	LIST_FOREACH(pcb, &ng_btsocket_hci_raw_sockets, next) {
+		mtx_lock(&pcb->pcb_mtx);
 
-		LIST_FOREACH(pcb, &ng_btsocket_hci_raw_sockets, next) {
-			if (msg->header.token == pcb->token) {
-				pcb->msg = msg;
-				msg = NULL;
-				wakeup(&pcb->msg);
-				break;
-			}
+		if (msg->header.token == pcb->token) {
+			pcb->msg = msg;
+			wakeup(&pcb->msg);
+
+			mtx_unlock(&pcb->pcb_mtx);
+			mtx_unlock(&ng_btsocket_hci_raw_sockets_mtx);
+
+			return;
 		}
 
-		mtx_unlock(&ng_btsocket_hci_raw_sockets_mtx);
+		mtx_unlock(&pcb->pcb_mtx);
 	}
 
+	mtx_unlock(&ng_btsocket_hci_raw_sockets_mtx);
+
 	NG_FREE_MSG(msg); /* checks for != NULL */
 } /* ng_btsocket_hci_raw_msg_input */
 
@@ -613,7 +636,8 @@ ng_btsocket_hci_raw_output(node_p node, hook_p hook, void *arg1, int arg2)
 
 	KASSERT((nam->m_type == MT_SONAME),
 		("%s: m_type=%d\n", __func__, nam->m_type));
-	M_ASSERTPKTHDR(m);
+	KASSERT((m->m_flags & M_PKTHDR),
+		("%s: m_flags=%#x\n", __func__, m->m_flags));
 
 	sa = mtod(nam, struct sockaddr_hci *);
 
@@ -639,13 +663,70 @@ ng_btsocket_hci_raw_output(node_p node, hook_p hook, void *arg1, int arg2)
 } /* ng_btsocket_hci_raw_output */
 
 /*
+ * Check frame against security and socket filters. 
+ * d (direction bit) == 1 means incoming frame.
+ */
+
+static int
+ng_btsocket_hci_raw_filter(ng_btsocket_hci_raw_pcb_p pcb, struct mbuf *m, int d)
+{
+	int	type, event, opcode;
+
+	mtx_assert(&pcb->pcb_mtx, MA_OWNED);
+
+	switch ((type = *mtod(m, u_int8_t *))) {
+	case NG_HCI_CMD_PKT:
+		if (!(pcb->flags & NG_BTSOCKET_HCI_RAW_PRIVILEGED)) {
+			opcode = le16toh(mtod(m, ng_hci_cmd_pkt_t *)->opcode);
+		
+			if (!bit_test(
+ng_btsocket_hci_raw_sec_filter->commands[NG_HCI_OGF(opcode) - 1],
+NG_HCI_OCF(opcode) - 1))
+				return (EPERM);
+		}
+
+		if (d && !bit_test(pcb->filter.packet_mask, NG_HCI_CMD_PKT - 1))
+			return (EPERM);
+		break;
+
+	case NG_HCI_ACL_DATA_PKT:
+	case NG_HCI_SCO_DATA_PKT:
+		if (!(pcb->flags & NG_BTSOCKET_HCI_RAW_PRIVILEGED) ||
+		    !bit_test(pcb->filter.packet_mask, type - 1) ||
+		    !d)
+			return (EPERM);
+		break;
+
+	case NG_HCI_EVENT_PKT:
+		if (!d)
+			return (EINVAL);
+
+		event = mtod(m, ng_hci_event_pkt_t *)->event - 1;
+
+		if (!(pcb->flags & NG_BTSOCKET_HCI_RAW_PRIVILEGED))
+			if (!bit_test(ng_btsocket_hci_raw_sec_filter->events, event))
+				return (EPERM);
+
+		if (!bit_test(pcb->filter.event_mask, event))
+			return (EPERM);
+		break;
+
+	default:
+		return (EINVAL);
+	}
+
+	return (0);
+} /* ng_btsocket_hci_raw_filter */
+
+/*
  * Initialize everything
  */
 
 void
 ng_btsocket_hci_raw_init(void)
 {
-	int	error = 0;
+	bitstr_t	*f = NULL;
+	int		 error = 0;
 
 	ng_btsocket_hci_raw_node = NULL;
 	ng_btsocket_hci_raw_debug_level = NG_BTSOCKET_WARN_LEVEL;
@@ -699,6 +780,99 @@ ng_btsocket_hci_raw_init(void)
 	ng_btsocket_hci_raw_token = 0;
 	mtx_init(&ng_btsocket_hci_raw_token_mtx,
 		"btsocks_hci_raw_token_mtx", NULL, MTX_DEF);
+
+	/* 
+	 * Security filter
+	 * XXX never FREE()ed
+	 */
+
+	ng_btsocket_hci_raw_sec_filter = NULL;
+
+	MALLOC(ng_btsocket_hci_raw_sec_filter, 
+		struct ng_btsocket_hci_raw_sec_filter *,
+		sizeof(struct ng_btsocket_hci_raw_sec_filter), 
+		M_NETGRAPH_BTSOCKET_HCI_RAW, M_NOWAIT|M_ZERO);
+	if (ng_btsocket_hci_raw_sec_filter == NULL) {
+		printf("%s: Could not allocate security filter!\n", __func__);
+		return;
+	}
+
+	/*
+	 * XXX How paranoid can we get? 
+	 *
+	 * Initialize security filter. If bit is set in the mask then
+	 * unprivileged socket is allowed to send (receive) this command
+	 * (event).
+	 */
+
+	/* Enable all events */
+	memset(&ng_btsocket_hci_raw_sec_filter->events, 0xff,
+		sizeof(ng_btsocket_hci_raw_sec_filter->events)/
+			sizeof(ng_btsocket_hci_raw_sec_filter->events[0]));
+
+	/* Disable some critical events */
+	f = ng_btsocket_hci_raw_sec_filter->events;
+	bit_clear(f, NG_HCI_EVENT_RETURN_LINK_KEYS - 1);
+	bit_clear(f, NG_HCI_EVENT_LINK_KEY_NOTIFICATION - 1);
+	bit_clear(f, NG_HCI_EVENT_VENDOR - 1);
+
+	/* Commands - Link control */
+	f = ng_btsocket_hci_raw_sec_filter->commands[NG_HCI_OGF_LINK_CONTROL-1];
+	bit_set(f, NG_HCI_OCF_INQUIRY - 1);
+	bit_set(f, NG_HCI_OCF_INQUIRY_CANCEL - 1);
+	bit_set(f, NG_HCI_OCF_PERIODIC_INQUIRY - 1);
+	bit_set(f, NG_HCI_OCF_EXIT_PERIODIC_INQUIRY - 1);
+	bit_set(f, NG_HCI_OCF_REMOTE_NAME_REQ - 1);
+	bit_set(f, NG_HCI_OCF_READ_REMOTE_FEATURES - 1);
+	bit_set(f, NG_HCI_OCF_READ_REMOTE_VER_INFO - 1);
+	bit_set(f, NG_HCI_OCF_READ_CLOCK_OFFSET - 1);
+
+	/* Commands - Link policy */
+	f = ng_btsocket_hci_raw_sec_filter->commands[NG_HCI_OGF_LINK_POLICY-1];
+	bit_set(f, NG_HCI_OCF_ROLE_DISCOVERY - 1);
+	bit_set(f, NG_HCI_OCF_READ_LINK_POLICY_SETTINGS - 1);
+
+	/* Commands - Host controller and baseband */
+	f = ng_btsocket_hci_raw_sec_filter->commands[NG_HCI_OGF_HC_BASEBAND-1];
+	bit_set(f, NG_HCI_OCF_READ_PIN_TYPE - 1);
+	bit_set(f, NG_HCI_OCF_READ_LOCAL_NAME - 1);
+	bit_set(f, NG_HCI_OCF_READ_CON_ACCEPT_TIMO - 1);
+	bit_set(f, NG_HCI_OCF_READ_PAGE_TIMO - 1);
+	bit_set(f, NG_HCI_OCF_READ_SCAN_ENABLE - 1);
+	bit_set(f, NG_HCI_OCF_READ_PAGE_SCAN_ACTIVITY - 1);
+	bit_set(f, NG_HCI_OCF_READ_INQUIRY_SCAN_ACTIVITY - 1);
+	bit_set(f, NG_HCI_OCF_READ_AUTH_ENABLE - 1);
+	bit_set(f, NG_HCI_OCF_READ_ENCRYPTION_MODE - 1);
+	bit_set(f, NG_HCI_OCF_READ_UNIT_CLASS - 1);
+	bit_set(f, NG_HCI_OCF_READ_VOICE_SETTINGS - 1);
+	bit_set(f, NG_HCI_OCF_READ_AUTO_FLUSH_TIMO - 1);
+	bit_set(f, NG_HCI_OCF_READ_NUM_BROADCAST_RETRANS - 1);
+	bit_set(f, NG_HCI_OCF_READ_HOLD_MODE_ACTIVITY - 1);
+	bit_set(f, NG_HCI_OCF_READ_XMIT_LEVEL - 1);
+	bit_set(f, NG_HCI_OCF_READ_SCO_FLOW_CONTROL - 1);
+	bit_set(f, NG_HCI_OCF_READ_LINK_SUPERVISION_TIMO - 1);
+	bit_set(f, NG_HCI_OCF_READ_SUPPORTED_IAC_NUM - 1);
+	bit_set(f, NG_HCI_OCF_READ_IAC_LAP - 1);
+	bit_set(f, NG_HCI_OCF_READ_PAGE_SCAN_PERIOD - 1);
+	bit_set(f, NG_HCI_OCF_READ_PAGE_SCAN - 1);
+
+	/* Commands - Informational */
+	f = ng_btsocket_hci_raw_sec_filter->commands[NG_HCI_OGF_INFO - 1];
+	bit_set(f, NG_HCI_OCF_READ_LOCAL_VER - 1);
+	bit_set(f, NG_HCI_OCF_READ_LOCAL_FEATURES - 1);
+	bit_set(f, NG_HCI_OCF_READ_BUFFER_SIZE - 1);
+	bit_set(f, NG_HCI_OCF_READ_COUNTRY_CODE - 1);
+	bit_set(f, NG_HCI_OCF_READ_BDADDR - 1);
+
+	/* Commands - Status */
+	f = ng_btsocket_hci_raw_sec_filter->commands[NG_HCI_OGF_STATUS - 1];
+	bit_set(f, NG_HCI_OCF_READ_FAILED_CONTACT_CNTR - 1);
+	bit_set(f, NG_HCI_OCF_GET_LINK_QUALITY - 1);
+	bit_set(f, NG_HCI_OCF_READ_RSSI - 1);
+
+	/* Commands - Testing */
+	f = ng_btsocket_hci_raw_sec_filter->commands[NG_HCI_OGF_TESTING - 1];
+	bit_set(f, NG_HCI_OCF_READ_LOOPBACK_MODE - 1);
 } /* ng_btsocket_hci_raw_init */
 
 /*
@@ -708,8 +882,6 @@ ng_btsocket_hci_raw_init(void)
 int
 ng_btsocket_hci_raw_abort(struct socket *so)
 {
-	soisdisconnected(so);
-
 	return (ng_btsocket_hci_raw_detach(so));
 } /* ng_btsocket_hci_raw_abort */
 
@@ -732,8 +904,6 @@ ng_btsocket_hci_raw_attach(struct socket *so, int proto, struct thread *td)
 		return (EPROTONOSUPPORT);
 	if (so->so_type != SOCK_RAW)
 		return (ESOCKTNOSUPPORT);
-	if ((error = suser(td)) != 0)
-		return (error);
 
 	error = soreserve(so, NG_BTSOCKET_HCI_RAW_SENDSPACE,
 				NG_BTSOCKET_HCI_RAW_RECVSPACE);
@@ -741,13 +911,16 @@ ng_btsocket_hci_raw_attach(struct socket *so, int proto, struct thread *td)
 		return (error);
 
 	MALLOC(pcb, ng_btsocket_hci_raw_pcb_p, sizeof(*pcb), 
-		M_NETGRAPH_BTSOCKET_HCI_RAW, M_WAITOK | M_ZERO);
+		M_NETGRAPH_BTSOCKET_HCI_RAW, M_NOWAIT|M_ZERO);
 	if (pcb == NULL)
 		return (ENOMEM);
 
 	so->so_pcb = (caddr_t) pcb;
 	pcb->so = so;
 
+	if (suser(td) == 0)
+		pcb->flags |= NG_BTSOCKET_HCI_RAW_PRIVILEGED;
+
 	/*
 	 * Set default socket filter. By default socket only accepts HCI
 	 * Command_Complete and Command_Status event packets.
@@ -756,6 +929,8 @@ ng_btsocket_hci_raw_attach(struct socket *so, int proto, struct thread *td)
 	bit_set(pcb->filter.event_mask, NG_HCI_EVENT_COMMAND_COMPL - 1);
 	bit_set(pcb->filter.event_mask, NG_HCI_EVENT_COMMAND_STATUS - 1);
 
+	mtx_init(&pcb->pcb_mtx, "btsocks_hci_raw_pcb_mtx", NULL, MTX_DEF);
+
 	mtx_lock(&ng_btsocket_hci_raw_sockets_mtx);
 	LIST_INSERT_HEAD(&ng_btsocket_hci_raw_sockets, pcb, next);
 	mtx_unlock(&ng_btsocket_hci_raw_sockets_mtx);
@@ -834,8 +1009,7 @@ ng_btsocket_hci_raw_control(struct socket *so, u_long cmd, caddr_t data,
 		struct ifnet *ifp, struct thread *td)
 {
 	ng_btsocket_hci_raw_pcb_p	 pcb = so2hci_raw_pcb(so);
-	char				 path[NG_NODELEN + 2], 
-					*hci_node = (char *) data;
+	char				 path[NG_NODELEN + 2];
 	struct ng_mesg			*msg = NULL;
 	int				 error = 0;
 
@@ -844,41 +1018,45 @@ ng_btsocket_hci_raw_control(struct socket *so, u_long cmd, caddr_t data,
 	if (ng_btsocket_hci_raw_node == NULL)
 		return (EINVAL);
 
-	/*
-	 * Make sure caller has provided HCI node name, if not try to
-	 * use addr from socket (if socket was bound)
-	 */
+	mtx_lock(&pcb->pcb_mtx);
 
-	if (hci_node[0] == 0) {
-		if (pcb->addr.hci_node[0] == 0)
-			return (EINVAL);
+	/* Check if we have device name */
+	if (pcb->addr.hci_node[0] == 0) {
+		mtx_unlock(&pcb->pcb_mtx);
+		return (EHOSTUNREACH);
+	}
 
-		bzero(hci_node, sizeof(pcb->addr.hci_node));
-		strncpy(hci_node,pcb->addr.hci_node,sizeof(pcb->addr.hci_node));
+	/* Check if we have pending ioctl() */
+	if (pcb->token != 0) {
+		mtx_unlock(&pcb->pcb_mtx);
+		return (EBUSY);
 	}
 
-	snprintf(path, sizeof(path), "%s:", hci_node);
+	snprintf(path, sizeof(path), "%s:", pcb->addr.hci_node);
 
 	switch (cmd) {
 	case SIOC_HCI_RAW_NODE_GET_STATE: {
 		struct ng_btsocket_hci_raw_node_state	*p =
 			(struct ng_btsocket_hci_raw_node_state *) data;
 
-		error = ng_btsocket_raw_send_sync_ngmsg(pcb, path, 
+		error = ng_btsocket_hci_raw_send_sync_ngmsg(pcb, path, 
 				NGM_HCI_NODE_GET_STATE,
 				&p->state, sizeof(p->state));
 		} break;
 
 	case SIOC_HCI_RAW_NODE_INIT:
-		error = ng_btsocket_raw_send_ngmsg(path, NGM_HCI_NODE_INIT,
-				NULL, 0);
+		if (pcb->flags & NG_BTSOCKET_HCI_RAW_PRIVILEGED)
+			error = ng_btsocket_hci_raw_send_ngmsg(path,
+					NGM_HCI_NODE_INIT, NULL, 0);
+		else
+			error = EPERM;
 		break;
 
 	case SIOC_HCI_RAW_NODE_GET_DEBUG: {
 		struct ng_btsocket_hci_raw_node_debug	*p = 
 			(struct ng_btsocket_hci_raw_node_debug *) data;
 
-		error = ng_btsocket_raw_send_sync_ngmsg(pcb, path,
+		error = ng_btsocket_hci_raw_send_sync_ngmsg(pcb, path,
 				NGM_HCI_NODE_GET_DEBUG,
 				&p->debug, sizeof(p->debug));
 		} break;
@@ -887,15 +1065,19 @@ ng_btsocket_hci_raw_control(struct socket *so, u_long cmd, caddr_t data,
 		struct ng_btsocket_hci_raw_node_debug	*p = 
 			(struct ng_btsocket_hci_raw_node_debug *) data;
 
-		error = ng_btsocket_raw_send_ngmsg(path, NGM_HCI_NODE_SET_DEBUG,
-				&p->debug, sizeof(p->debug));
+		if (pcb->flags & NG_BTSOCKET_HCI_RAW_PRIVILEGED)
+			error = ng_btsocket_hci_raw_send_ngmsg(path,
+					NGM_HCI_NODE_SET_DEBUG, &p->debug,
+					sizeof(p->debug));
+		else
+			error = EPERM;
 		} break;
 
 	case SIOC_HCI_RAW_NODE_GET_BUFFER: {
 		struct ng_btsocket_hci_raw_node_buffer	*p = 
 			(struct ng_btsocket_hci_raw_node_buffer *) data;
 
-		error = ng_btsocket_raw_send_sync_ngmsg(pcb, path,
+		error = ng_btsocket_hci_raw_send_sync_ngmsg(pcb, path,
 				NGM_HCI_NODE_GET_BUFFER,
 				&p->buffer, sizeof(p->buffer));
 		} break;
@@ -904,7 +1086,7 @@ ng_btsocket_hci_raw_control(struct socket *so, u_long cmd, caddr_t data,
 		struct ng_btsocket_hci_raw_node_bdaddr	*p = 
 			(struct ng_btsocket_hci_raw_node_bdaddr *) data;
 
-		error = ng_btsocket_raw_send_sync_ngmsg(pcb, path,
+		error = ng_btsocket_hci_raw_send_sync_ngmsg(pcb, path,
 				NGM_HCI_NODE_GET_BDADDR,
 				&p->bdaddr, sizeof(p->bdaddr));
 		} break;
@@ -913,7 +1095,7 @@ ng_btsocket_hci_raw_control(struct socket *so, u_long cmd, caddr_t data,
 		struct ng_btsocket_hci_raw_node_features	*p = 
 			(struct ng_btsocket_hci_raw_node_features *) data;
 
-		error = ng_btsocket_raw_send_sync_ngmsg(pcb, path,
+		error = ng_btsocket_hci_raw_send_sync_ngmsg(pcb, path,
 				NGM_HCI_NODE_GET_FEATURES,
 				&p->features, sizeof(p->features));
 		} break;
@@ -922,19 +1104,26 @@ ng_btsocket_hci_raw_control(struct socket *so, u_long cmd, caddr_t data,
 		struct ng_btsocket_hci_raw_node_stat	*p = 
 			(struct ng_btsocket_hci_raw_node_stat *) data;
 
-		error = ng_btsocket_raw_send_sync_ngmsg(pcb, path,
+		error = ng_btsocket_hci_raw_send_sync_ngmsg(pcb, path,
 				NGM_HCI_NODE_GET_STAT,
 				&p->stat, sizeof(p->stat));
 		} break;
 
 	case SIOC_HCI_RAW_NODE_RESET_STAT:
-		error = ng_btsocket_raw_send_ngmsg(path,
-				NGM_HCI_NODE_RESET_STAT, NULL, 0);
+		if (pcb->flags & NG_BTSOCKET_HCI_RAW_PRIVILEGED)
+			error = ng_btsocket_hci_raw_send_ngmsg(path,
+					NGM_HCI_NODE_RESET_STAT, NULL, 0);
+		else
+			error = EPERM;
 		break;
 
 	case SIOC_HCI_RAW_NODE_FLUSH_NEIGHBOR_CACHE:
-		error = ng_btsocket_raw_send_ngmsg(path,
-				NGM_HCI_NODE_FLUSH_NEIGHBOR_CACHE, NULL, 0);
+		if (pcb->flags & NG_BTSOCKET_HCI_RAW_PRIVILEGED)
+			error = ng_btsocket_hci_raw_send_ngmsg(path,
+					NGM_HCI_NODE_FLUSH_NEIGHBOR_CACHE,
+					NULL, 0);
+		else
+			error = EPERM;
 		break;
 
 	case SIOC_HCI_RAW_NODE_GET_NEIGHBOR_CACHE:  {
@@ -950,17 +1139,15 @@ ng_btsocket_hci_raw_control(struct socket *so, u_long cmd, caddr_t data,
 			break;
 		}
 
-		ng_btsocket_hci_raw_get_token(&pcb->token);
-		pcb->msg = NULL;
-
 		NG_MKMESSAGE(msg, NGM_HCI_COOKIE,
-			NGM_HCI_NODE_GET_NEIGHBOR_CACHE, 0, M_WAITOK);
+			NGM_HCI_NODE_GET_NEIGHBOR_CACHE, 0, M_NOWAIT);
 		if (msg == NULL) {
-			pcb->token = 0;
 			error = ENOMEM;
 			break;
 		}
-		msg->header.token = pcb->token;
+		ng_btsocket_hci_raw_get_token(&msg->header.token);
+		pcb->token = msg->header.token;
+		pcb->msg = NULL;
 
 		NG_SEND_MSG_PATH(error,ng_btsocket_hci_raw_node,msg,path,NULL);
 		if (error != 0) {
@@ -968,12 +1155,13 @@ ng_btsocket_hci_raw_control(struct socket *so, u_long cmd, caddr_t data,
 			break;
 		}
 
-		error = tsleep(&pcb->msg, PZERO|PCATCH, "hcictl", 
+		error = msleep(&pcb->msg, &pcb->pcb_mtx,
+				PZERO|PCATCH, "hcictl", 
 				ng_btsocket_hci_raw_ioctl_timeout * hz);
-		if (error != 0) {
-			pcb->token = 0;
+		pcb->token = 0;
+
+		if (error != 0)
 			break;
-		}
 
 		if (pcb->msg != NULL &&
 		    pcb->msg->header.cmd == NGM_HCI_NODE_GET_NEIGHBOR_CACHE) {
@@ -992,7 +1180,6 @@ ng_btsocket_hci_raw_control(struct socket *so, u_long cmd, caddr_t data,
 			error = EINVAL;
 
 		NG_FREE_MSG(pcb->msg); /* checks for != NULL */
-		pcb->token = 0;
 		}break;
 
 	case SIOC_HCI_RAW_NODE_GET_CON_LIST: {
@@ -1008,17 +1195,15 @@ ng_btsocket_hci_raw_control(struct socket *so, u_long cmd, caddr_t data,
 			break;
 		}
 
-		ng_btsocket_hci_raw_get_token(&pcb->token);
-		pcb->msg = NULL;
-
 		NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_NODE_GET_CON_LIST,
-			0, M_WAITOK);
+			0, M_NOWAIT);
 		if (msg == NULL) {
-			pcb->token = 0;
 			error = ENOMEM;
 			break;
 		}
-		msg->header.token = pcb->token;
+		ng_btsocket_hci_raw_get_token(&msg->header.token);
+		pcb->token = msg->header.token;
+		pcb->msg = NULL;
 
 		NG_SEND_MSG_PATH(error,ng_btsocket_hci_raw_node,msg,path,NULL);
 		if (error != 0) {
@@ -1026,12 +1211,13 @@ ng_btsocket_hci_raw_control(struct socket *so, u_long cmd, caddr_t data,
 			break;
 		}
 
-		error = tsleep(&pcb->msg, PZERO|PCATCH, "hcictl", 
+		error = msleep(&pcb->msg, &pcb->pcb_mtx,
+				PZERO|PCATCH, "hcictl", 
 				ng_btsocket_hci_raw_ioctl_timeout * hz);
-		if (error != 0) {
-			pcb->token = 0;
+		pcb->token = 0;
+
+		if (error != 0)
 			break;
-		}
 
 		if (pcb->msg != NULL &&
 		    pcb->msg->header.cmd == NGM_HCI_NODE_GET_CON_LIST) {
@@ -1049,7 +1235,6 @@ ng_btsocket_hci_raw_control(struct socket *so, u_long cmd, caddr_t data,
 			error = EINVAL;
 
 		NG_FREE_MSG(pcb->msg); /* checks for != NULL */
-		pcb->token = 0;
 		} break;
 
 	case SIOC_HCI_RAW_NODE_GET_LINK_POLICY_MASK: {
@@ -1057,7 +1242,7 @@ ng_btsocket_hci_raw_control(struct socket *so, u_long cmd, caddr_t data,
 			(struct ng_btsocket_hci_raw_node_link_policy_mask *) 
 				data;
 
-		error = ng_btsocket_raw_send_sync_ngmsg(pcb, path,
+		error = ng_btsocket_hci_raw_send_sync_ngmsg(pcb, path,
 				NGM_HCI_NODE_GET_LINK_POLICY_SETTINGS_MASK,
 				&p->policy_mask, sizeof(p->policy_mask));
 		} break;
@@ -1067,16 +1252,20 @@ ng_btsocket_hci_raw_control(struct socket *so, u_long cmd, caddr_t data,
 			(struct ng_btsocket_hci_raw_node_link_policy_mask *) 
 				data;
 
-		error = ng_btsocket_raw_send_ngmsg(path,
-				NGM_HCI_NODE_SET_LINK_POLICY_SETTINGS_MASK,
-				&p->policy_mask, sizeof(p->policy_mask));
+		if (pcb->flags & NG_BTSOCKET_HCI_RAW_PRIVILEGED)
+			error = ng_btsocket_hci_raw_send_ngmsg(path,
+					NGM_HCI_NODE_SET_LINK_POLICY_SETTINGS_MASK,
+					&p->policy_mask,
+					sizeof(p->policy_mask));
+		else
+			error = EPERM;
 		} break;
 
 	case SIOC_HCI_RAW_NODE_GET_PACKET_MASK: {
 		struct ng_btsocket_hci_raw_node_packet_mask	*p = 
 			(struct ng_btsocket_hci_raw_node_packet_mask *) data;
 
-		error = ng_btsocket_raw_send_sync_ngmsg(pcb, path,
+		error = ng_btsocket_hci_raw_send_sync_ngmsg(pcb, path,
 				NGM_HCI_NODE_GET_PACKET_MASK,
 				&p->packet_mask, sizeof(p->packet_mask));
 		} break;
@@ -1085,10 +1274,35 @@ ng_btsocket_hci_raw_control(struct socket *so, u_long cmd, caddr_t data,
 		struct ng_btsocket_hci_raw_node_packet_mask	*p = 
 			(struct ng_btsocket_hci_raw_node_packet_mask *) data;
 
-		error = ng_btsocket_raw_send_ngmsg(path,
-				NGM_HCI_NODE_SET_PACKET_MASK,
-				&p->packet_mask, sizeof(p->packet_mask));
+		if (pcb->flags & NG_BTSOCKET_HCI_RAW_PRIVILEGED)
+			error = ng_btsocket_hci_raw_send_ngmsg(path,
+					NGM_HCI_NODE_SET_PACKET_MASK,
+					&p->packet_mask,
+					sizeof(p->packet_mask));
+		else
+			error = EPERM;
+		} break;
+
+	case SIOC_HCI_RAW_NODE_GET_ROLE_SWITCH: {
+		struct ng_btsocket_hci_raw_node_role_switch	*p = 
+			(struct ng_btsocket_hci_raw_node_role_switch *) data;
 
+		error = ng_btsocket_hci_raw_send_sync_ngmsg(pcb, path,
+				NGM_HCI_NODE_GET_ROLE_SWITCH,
+				&p->role_switch, sizeof(p->role_switch));
+		} break;
+
+	case SIOC_HCI_RAW_NODE_SET_ROLE_SWITCH: {
+		struct ng_btsocket_hci_raw_node_role_switch	*p = 
+			(struct ng_btsocket_hci_raw_node_role_switch *) data;
+
+		if (pcb->flags & NG_BTSOCKET_HCI_RAW_PRIVILEGED)
+			error = ng_btsocket_hci_raw_send_ngmsg(path,
+					NGM_HCI_NODE_SET_ROLE_SWITCH,
+					&p->role_switch,
+					sizeof(p->role_switch));
+		else
+			error = EPERM;
 		} break;
 
 	default:
@@ -1096,6 +1310,8 @@ ng_btsocket_hci_raw_control(struct socket *so, u_long cmd, caddr_t data,
 		break;
 	}
 
+	mtx_unlock(&pcb->pcb_mtx);
+
 	return (error);
 } /* ng_btsocket_hci_raw_control */
 
@@ -1118,6 +1334,8 @@ ng_btsocket_hci_raw_ctloutput(struct socket *so, struct sockopt *sopt)
 	if (sopt->sopt_level != SOL_HCI_RAW)
 		return (0);
 
+	mtx_lock(&pcb->pcb_mtx);
+
 	switch (sopt->sopt_dir) {
 	case SOPT_GET:
 		switch (sopt->sopt_name) {
@@ -1169,6 +1387,8 @@ ng_btsocket_hci_raw_ctloutput(struct socket *so, struct sockopt *sopt)
 		error = EINVAL;
 		break;
 	}
+
+	mtx_unlock(&pcb->pcb_mtx);
 	
 	return (error);
 } /* ng_btsocket_hci_raw_ctloutput */
@@ -1187,16 +1407,22 @@ ng_btsocket_hci_raw_detach(struct socket *so)
 	if (ng_btsocket_hci_raw_node == NULL)
 		return (EINVAL);
 
-	so->so_pcb = NULL;
-	sotryfree(so);
-
 	mtx_lock(&ng_btsocket_hci_raw_sockets_mtx);
+	mtx_lock(&pcb->pcb_mtx);
+
 	LIST_REMOVE(pcb, next);
+
+	mtx_unlock(&pcb->pcb_mtx);
 	mtx_unlock(&ng_btsocket_hci_raw_sockets_mtx);
 
+	mtx_destroy(&pcb->pcb_mtx);
+
 	bzero(pcb, sizeof(*pcb));
 	FREE(pcb, M_NETGRAPH_BTSOCKET_HCI_RAW);
 
+	so->so_pcb = NULL;
+	sotryfree(so);
+
 	return (0);
 } /* ng_btsocket_hci_raw_detach */
 
@@ -1226,7 +1452,7 @@ ng_btsocket_hci_raw_disconnect(struct socket *so)
 int
 ng_btsocket_hci_raw_peeraddr(struct socket *so, struct sockaddr **nam)
 {
-	return (EOPNOTSUPP);
+	return (ng_btsocket_hci_raw_sockaddr(so, nam));
 } /* ng_btsocket_hci_raw_peeraddr */
 
 /*
@@ -1260,8 +1486,28 @@ ng_btsocket_hci_raw_send(struct socket *so, int flags, struct mbuf *m,
 		goto drop;
 	}
 
+	if (m->m_len < sizeof(ng_hci_cmd_pkt_t)) {
+		if ((m = m_pullup(m, sizeof(ng_hci_cmd_pkt_t))) == NULL) {
+			error = ENOBUFS;
+			goto drop;
+		}
+	}
+	if (*mtod(m, u_int8_t *) != NG_HCI_CMD_PKT) {
+		error = ENOTSUP;
+		goto drop;
+	}
+
+	mtx_lock(&pcb->pcb_mtx);
+
+	error = ng_btsocket_hci_raw_filter(pcb, m, 0);
+	if (error != 0) {
+		mtx_unlock(&pcb->pcb_mtx);
+		goto drop;
+	}
+
 	if (sa == NULL) {
 		if (pcb->addr.hci_node[0] == 0) {
+			mtx_unlock(&pcb->pcb_mtx);
 			error = EDESTADDRREQ;
 			goto drop;
 		}
@@ -1269,8 +1515,9 @@ ng_btsocket_hci_raw_send(struct socket *so, int flags, struct mbuf *m,
 		sa = (struct sockaddr *) &pcb->addr;
 	}
 
-	MGET(nam, M_TRYWAIT, MT_SONAME);
+	MGET(nam, M_DONTWAIT, MT_SONAME);
 	if (nam == NULL) {
+		mtx_unlock(&pcb->pcb_mtx);
 		error = ENOBUFS;
 		goto drop;
 	}
@@ -1281,6 +1528,8 @@ ng_btsocket_hci_raw_send(struct socket *so, int flags, struct mbuf *m,
 	nam->m_next = m;
 	m = NULL;
 
+	mtx_unlock(&pcb->pcb_mtx);
+
 	return (ng_send_fn(ng_btsocket_hci_raw_node, NULL, 
 				ng_btsocket_hci_raw_output, nam, 0));
 drop:
@@ -1309,7 +1558,7 @@ ng_btsocket_hci_raw_sockaddr(struct socket *so, struct sockaddr **nam)
 	bzero(&sa, sizeof(sa));
 	sa.hci_len = sizeof(sa);
 	sa.hci_family = AF_BLUETOOTH;
-	strncpy(sa.hci_node, pcb->addr.hci_node, sizeof(sa.hci_node));
+	strlcpy(sa.hci_node, pcb->addr.hci_node, sizeof(sa.hci_node));
 
 	*nam = dup_sockaddr((struct sockaddr *) &sa, 0);
 
diff --git a/sys/netgraph/bluetooth/socket/ng_btsocket_l2cap.c b/sys/netgraph/bluetooth/socket/ng_btsocket_l2cap.c
index f4c99f7..7cf45d6 100644
--- a/sys/netgraph/bluetooth/socket/ng_btsocket_l2cap.c
+++ b/sys/netgraph/bluetooth/socket/ng_btsocket_l2cap.c
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_btsocket_l2cap.c,v 1.5 2002/10/26 03:34:37 max Exp $
+ * $Id: ng_btsocket_l2cap.c,v 1.14 2003/04/06 22:53:18 max Exp $
  * $FreeBSD$
  */
 
@@ -95,15 +95,12 @@ static struct ng_type	typestruct = {
 /* Globals */
 extern int					ifqmaxlen;
 static u_int32_t				ng_btsocket_l2cap_debug_level;
-static u_int32_t				ng_btsocket_l2cap_ioctl_timeout;
 static node_p					ng_btsocket_l2cap_node;
 static struct ng_bt_itemq			ng_btsocket_l2cap_queue;
 static struct mtx				ng_btsocket_l2cap_queue_mtx;
 static struct task				ng_btsocket_l2cap_queue_task;
 static LIST_HEAD(, ng_btsocket_l2cap_pcb)	ng_btsocket_l2cap_sockets;
 static struct mtx				ng_btsocket_l2cap_sockets_mtx;
-static u_int32_t				ng_btsocket_l2cap_token;
-static struct mtx				ng_btsocket_l2cap_token_mtx;
 static LIST_HEAD(, ng_btsocket_l2cap_rtentry)	ng_btsocket_l2cap_rt;
 static struct mtx				ng_btsocket_l2cap_rt_mtx;
 static struct task				ng_btsocket_l2cap_rt_task;
@@ -116,10 +113,6 @@ SYSCTL_INT(_net_bluetooth_l2cap_sockets_seq, OID_AUTO, debug_level,
 	CTLFLAG_RW,
 	&ng_btsocket_l2cap_debug_level, NG_BTSOCKET_WARN_LEVEL,
 	"Bluetooth SEQPACKET L2CAP sockets debug level");
-SYSCTL_INT(_net_bluetooth_l2cap_sockets_seq, OID_AUTO, ioctl_timeout,
-	CTLFLAG_RW,
-	&ng_btsocket_l2cap_ioctl_timeout, 5,
-	"Bluetooth SEQPACKET L2CAP sockets ioctl timeout");
 SYSCTL_INT(_net_bluetooth_l2cap_sockets_seq, OID_AUTO, queue_len, 
 	CTLFLAG_RD,
 	&ng_btsocket_l2cap_queue.len, 0,
@@ -155,26 +148,26 @@ SYSCTL_INT(_net_bluetooth_l2cap_sockets_seq, OID_AUTO, queue_drops,
  */
 
 static int ng_btsocket_l2cap_process_l2ca_con_req_rsp
-	(struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
+	(struct ng_mesg *, ng_btsocket_l2cap_rtentry_p, struct socket **);
 static int ng_btsocket_l2cap_process_l2ca_con_rsp_rsp
-	(struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
+	(struct ng_mesg *, ng_btsocket_l2cap_rtentry_p, struct socket **);
 static int ng_btsocket_l2cap_process_l2ca_con_ind
-	(struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
+	(struct ng_mesg *, ng_btsocket_l2cap_rtentry_p, struct socket **);
 
 static int ng_btsocket_l2cap_process_l2ca_cfg_req_rsp
-	(struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
+	(struct ng_mesg *, ng_btsocket_l2cap_rtentry_p, struct socket **);
 static int ng_btsocket_l2cap_process_l2ca_cfg_rsp_rsp
-	(struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
+	(struct ng_mesg *, ng_btsocket_l2cap_rtentry_p, struct socket **);
 static int ng_btsocket_l2cap_process_l2ca_cfg_ind
-	(struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
+	(struct ng_mesg *, ng_btsocket_l2cap_rtentry_p, struct socket **);
 
 static int ng_btsocket_l2cap_process_l2ca_discon_rsp
-	(struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
+	(struct ng_mesg *, ng_btsocket_l2cap_rtentry_p, struct socket **);
 static int ng_btsocket_l2cap_process_l2ca_discon_ind
-	(struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
+	(struct ng_mesg *, ng_btsocket_l2cap_rtentry_p, struct socket **);
 
 static int ng_btsocket_l2cap_process_l2ca_write_rsp
-	(struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
+	(struct ng_mesg *, ng_btsocket_l2cap_rtentry_p, struct socket **);
 
 /*
  * Send L2CA_xxx messages to the lower layer
@@ -209,9 +202,14 @@ static void ng_btsocket_l2cap_process_timeout (void *);
 static ng_btsocket_l2cap_pcb_p     ng_btsocket_l2cap_pcb_by_addr(bdaddr_p, int);
 static ng_btsocket_l2cap_pcb_p     ng_btsocket_l2cap_pcb_by_token(u_int32_t);
 static ng_btsocket_l2cap_pcb_p     ng_btsocket_l2cap_pcb_by_cid (bdaddr_p, int);
-static void                        ng_btsocket_l2cap_get_token  (u_int32_t *);
 static int                         ng_btsocket_l2cap_result2errno(int);
 
+#define ng_btsocket_l2cap_wakeup_input_task() \
+	taskqueue_enqueue(taskqueue_swi_giant, &ng_btsocket_l2cap_queue_task)
+
+#define ng_btsocket_l2cap_wakeup_route_task() \
+	taskqueue_enqueue(taskqueue_swi_giant, &ng_btsocket_l2cap_rt_task)
+
 /*****************************************************************************
  *****************************************************************************
  **                        Netgraph node interface
@@ -309,8 +307,7 @@ ng_btsocket_l2cap_node_disconnect(hook_p hook)
 	 */
 
 	if (NG_HOOK_PRIVATE(hook) != NULL)
-		return (taskqueue_enqueue(taskqueue_swi_giant, 
-				&ng_btsocket_l2cap_rt_task));
+		return (ng_btsocket_l2cap_wakeup_route_task());
 
 	NG_HOOK_UNREF(hook); /* Remove extra reference */
 
@@ -343,8 +340,7 @@ ng_btsocket_l2cap_node_rcvmsg(node_p node, item_p item, hook_p hook)
 			}
 
 			NG_BT_ITEMQ_ENQUEUE(&ng_btsocket_l2cap_queue, item);
-			error = taskqueue_enqueue(taskqueue_swi_giant,
-						&ng_btsocket_l2cap_queue_task);
+			error = ng_btsocket_l2cap_wakeup_input_task();
 		}
 		mtx_unlock(&ng_btsocket_l2cap_queue_mtx);
 	} else {
@@ -377,8 +373,7 @@ ng_btsocket_l2cap_node_rcvdata(hook_p hook, item_p item)
 		NGI_SET_HOOK(item, hook);
 
 		NG_BT_ITEMQ_ENQUEUE(&ng_btsocket_l2cap_queue, item);
-		error = taskqueue_enqueue(taskqueue_swi_giant,
-						&ng_btsocket_l2cap_queue_task);
+		error = ng_btsocket_l2cap_wakeup_input_task();
 	}
 	mtx_unlock(&ng_btsocket_l2cap_queue_mtx);
 
@@ -392,7 +387,7 @@ ng_btsocket_l2cap_node_rcvdata(hook_p hook, item_p item)
 
 static int
 ng_btsocket_l2cap_process_l2ca_con_req_rsp(struct ng_mesg *msg,
-		ng_btsocket_l2cap_rtentry_p rt)
+		ng_btsocket_l2cap_rtentry_p rt, struct socket **sop)
 {
 	ng_l2cap_l2ca_con_op	*op = NULL;
 	ng_btsocket_l2cap_pcb_t	*pcb = NULL;
@@ -403,10 +398,14 @@ ng_btsocket_l2cap_process_l2ca_con_req_rsp(struct ng_mesg *msg,
 
 	op = (ng_l2cap_l2ca_con_op *)(msg->data);
 
+	mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
+
 	/* Look for the socket with the token */
 	pcb = ng_btsocket_l2cap_pcb_by_token(msg->header.token);
-	if (pcb == NULL) 
+	if (pcb == NULL) {
+		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
 		return (ENOENT);
+	}
 
 	mtx_lock(&pcb->pcb_mtx);
 
@@ -423,6 +422,8 @@ ng_btsocket_l2cap_process_l2ca_con_req_rsp(struct ng_mesg *msg,
 
 	if (pcb->state != NG_BTSOCKET_L2CAP_CONNECTING) {
 		mtx_unlock(&pcb->pcb_mtx);
+		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
+
 		return (ENOENT);
 	}
 
@@ -431,6 +432,8 @@ ng_btsocket_l2cap_process_l2ca_con_req_rsp(struct ng_mesg *msg,
 	if (op->result == NG_L2CAP_PENDING) {
 		ng_btsocket_l2cap_timeout(pcb);
 		mtx_unlock(&pcb->pcb_mtx);
+		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
+
 		return (0);
 	}
 
@@ -451,6 +454,9 @@ ng_btsocket_l2cap_process_l2ca_con_req_rsp(struct ng_mesg *msg,
 			/* ... and close the socket */
 			pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
 			soisdisconnected(pcb->so);
+
+			if (pcb->so->so_state & SS_NOFDREF)
+				*sop = pcb->so;
 		} else {
 			pcb->cfg_state = NG_BTSOCKET_L2CAP_CFG_IN_SENT;
 			pcb->state = NG_BTSOCKET_L2CAP_CONFIGURING;
@@ -467,9 +473,13 @@ ng_btsocket_l2cap_process_l2ca_con_req_rsp(struct ng_mesg *msg,
 		pcb->so->so_error = ng_btsocket_l2cap_result2errno(op->result);
 		pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
 		soisdisconnected(pcb->so); 
+
+		if (pcb->so->so_state & SS_NOFDREF)
+			*sop = pcb->so;
 	}
 
 	mtx_unlock(&pcb->pcb_mtx);
+	mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
 
 	return (error);
 } /* ng_btsocket_l2cap_process_l2ca_con_req_rsp */
@@ -480,7 +490,7 @@ ng_btsocket_l2cap_process_l2ca_con_req_rsp(struct ng_mesg *msg,
 
 static int
 ng_btsocket_l2cap_process_l2ca_con_rsp_rsp(struct ng_mesg *msg,
-		ng_btsocket_l2cap_rtentry_p rt)
+		ng_btsocket_l2cap_rtentry_p rt, struct socket **sop)
 {
 	ng_l2cap_l2ca_con_rsp_op	*op = NULL;
 	ng_btsocket_l2cap_pcb_t		*pcb = NULL;
@@ -490,10 +500,14 @@ ng_btsocket_l2cap_process_l2ca_con_rsp_rsp(struct ng_mesg *msg,
 
 	op = (ng_l2cap_l2ca_con_rsp_op *)(msg->data);
 
+	mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
+
 	/* Look for the socket with the token */
 	pcb = ng_btsocket_l2cap_pcb_by_token(msg->header.token);
-	if (pcb == NULL)
+	if (pcb == NULL) {
+		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
 		return (ENOENT);
+	}
 
 	mtx_lock(&pcb->pcb_mtx);
 
@@ -509,6 +523,8 @@ ng_btsocket_l2cap_process_l2ca_con_rsp_rsp(struct ng_mesg *msg,
 
 	if (pcb->state != NG_BTSOCKET_L2CAP_CONNECTING) {
 		mtx_unlock(&pcb->pcb_mtx);
+		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
+
 		return (ENOENT);
 	}
 
@@ -520,6 +536,9 @@ ng_btsocket_l2cap_process_l2ca_con_rsp_rsp(struct ng_mesg *msg,
 		pcb->so->so_error = ng_btsocket_l2cap_result2errno(op->result);
 		pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
 		soisdisconnected(pcb->so);
+
+		if (pcb->so->so_state & SS_NOFDREF)
+			*sop = pcb->so;
 	} else {
 		/* Move to CONFIGURING state and wait for CONFIG_IND */
 		pcb->cfg_state = 0;
@@ -528,6 +547,7 @@ ng_btsocket_l2cap_process_l2ca_con_rsp_rsp(struct ng_mesg *msg,
 	}
 
 	mtx_unlock(&pcb->pcb_mtx);
+	mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
 
 	return (0);
 } /* ng_btsocket_process_l2ca_con_rsp_rsp */
@@ -540,7 +560,7 @@ ng_btsocket_l2cap_process_l2ca_con_rsp_rsp(struct ng_mesg *msg,
 
 static int
 ng_btsocket_l2cap_process_l2ca_con_ind(struct ng_mesg *msg,
-		ng_btsocket_l2cap_rtentry_p rt)
+		ng_btsocket_l2cap_rtentry_p rt, struct socket **sop)
 {
 	ng_l2cap_l2ca_con_ind_ip	*ip = NULL;
 	ng_btsocket_l2cap_pcb_t		*pcb = NULL, *pcb1 = NULL;
@@ -562,6 +582,8 @@ ng_btsocket_l2cap_process_l2ca_con_ind(struct ng_mesg *msg,
 		ip->bdaddr.b[5], ip->bdaddr.b[4], ip->bdaddr.b[3],
 		ip->bdaddr.b[2], ip->bdaddr.b[1], ip->bdaddr.b[0],
 		ip->psm, ip->lcid, ip->ident);
+
+	mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
 	
 	pcb = ng_btsocket_l2cap_pcb_by_addr(&rt->src, ip->psm);
 	if (pcb != NULL) {
@@ -593,7 +615,7 @@ ng_btsocket_l2cap_process_l2ca_con_ind(struct ng_mesg *msg,
 		KASSERT((pcb1 != NULL),
 ("%s: pcb1 == NULL\n", __func__));
 
-		mtx_lock(&pcb1->pcb_mtx);
+ 		mtx_lock(&pcb1->pcb_mtx);
 
 		if (bcmp(&pcb->src, NG_HCI_BDADDR_ANY, sizeof(pcb->src)) != 0)
 			bcopy(&pcb->src, &pcb1->src, sizeof(pcb1->src));
@@ -625,6 +647,9 @@ respond:
 			pcb1->so->so_error = error;
 			pcb1->state = NG_BTSOCKET_L2CAP_CLOSED;
 			soisdisconnected(pcb1->so);
+
+			if (pcb1->so->so_state & SS_NOFDREF)
+				*sop = pcb1->so;
 		} else {
 			pcb1->state = NG_BTSOCKET_L2CAP_CONNECTING;
 			soisconnecting(pcb1->so);
@@ -638,6 +663,8 @@ respond:
 	if (pcb != NULL)
 		mtx_unlock(&pcb->pcb_mtx);
 
+	mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
+
 	return (error);
 } /* ng_btsocket_l2cap_process_l2ca_con_ind */
 
@@ -647,7 +674,7 @@ respond:
 
 static int
 ng_btsocket_l2cap_process_l2ca_cfg_req_rsp(struct ng_mesg *msg,
-		ng_btsocket_l2cap_rtentry_p rt)
+		ng_btsocket_l2cap_rtentry_p rt, struct socket **sop)
 {
 	ng_l2cap_l2ca_cfg_op	*op = NULL;
 	ng_btsocket_l2cap_pcb_p	 pcb = NULL;
@@ -657,6 +684,8 @@ ng_btsocket_l2cap_process_l2ca_cfg_req_rsp(struct ng_mesg *msg,
 
 	op = (ng_l2cap_l2ca_cfg_op *)(msg->data);
 
+	mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
+
 	/* 
 	 * Socket must have issued a Configure request, so we must have a 
 	 * socket that wants to be configured. Use Netgraph message token 
@@ -671,6 +700,7 @@ ng_btsocket_l2cap_process_l2ca_cfg_req_rsp(struct ng_mesg *msg,
 		 * Disconnect, because we do not know channel ID
 		 */
 
+		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
 		return (ENOENT);
 	}
 
@@ -689,6 +719,8 @@ ng_btsocket_l2cap_process_l2ca_cfg_req_rsp(struct ng_mesg *msg,
 
 	if (pcb->state != NG_BTSOCKET_L2CAP_CONFIGURING) {
 		mtx_unlock(&pcb->pcb_mtx);
+		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
+
 		return (ENOENT);
 	}
 
@@ -745,9 +777,13 @@ ng_btsocket_l2cap_process_l2ca_cfg_req_rsp(struct ng_mesg *msg,
 		/* ... and close the socket */
 		pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
 		soisdisconnected(pcb->so);
+
+		if (pcb->so->so_state & SS_NOFDREF)
+			*sop = pcb->so;
 	}
 
 	mtx_unlock(&pcb->pcb_mtx);
+	mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
 
 	return (0);
 } /* ng_btsocket_l2cap_process_l2ca_cfg_req_rsp */
@@ -758,7 +794,7 @@ ng_btsocket_l2cap_process_l2ca_cfg_req_rsp(struct ng_mesg *msg,
 
 static int
 ng_btsocket_l2cap_process_l2ca_cfg_rsp_rsp(struct ng_mesg *msg,
-		ng_btsocket_l2cap_rtentry_p rt)
+		ng_btsocket_l2cap_rtentry_p rt, struct socket **sop)
 {
 	ng_l2cap_l2ca_cfg_rsp_op	*op = NULL;
 	ng_btsocket_l2cap_pcb_t		*pcb = NULL;
@@ -769,10 +805,14 @@ ng_btsocket_l2cap_process_l2ca_cfg_rsp_rsp(struct ng_mesg *msg,
 
 	op = (ng_l2cap_l2ca_cfg_rsp_op *)(msg->data);
 
+	mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
+
 	/* Look for the socket with the token */
 	pcb = ng_btsocket_l2cap_pcb_by_token(msg->header.token);
-	if (pcb == NULL)
+	if (pcb == NULL) {
+		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
 		return (ENOENT);
+	}
 
 	mtx_lock(&pcb->pcb_mtx);
 
@@ -789,6 +829,8 @@ ng_btsocket_l2cap_process_l2ca_cfg_rsp_rsp(struct ng_mesg *msg,
 
 	if (pcb->state != NG_BTSOCKET_L2CAP_CONFIGURING) {
 		mtx_unlock(&pcb->pcb_mtx);
+		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
+
 		return (ENOENT);
 	}
 
@@ -821,6 +863,7 @@ ng_btsocket_l2cap_process_l2ca_cfg_rsp_rsp(struct ng_mesg *msg,
 	}
 
 	mtx_unlock(&pcb->pcb_mtx);
+	mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
 
 	return (error);
 
@@ -834,7 +877,11 @@ disconnect:
 	pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
 	soisdisconnected(pcb->so);
 
+	if (pcb->so->so_state & SS_NOFDREF)
+		*sop = pcb->so;
+
 	mtx_unlock(&pcb->pcb_mtx);
+	mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
 
 	return (error);
 } /* ng_btsocket_l2cap_process_l2ca_cfg_rsp_rsp */
@@ -845,7 +892,7 @@ disconnect:
 
 static int
 ng_btsocket_l2cap_process_l2ca_cfg_ind(struct ng_mesg *msg,
-		ng_btsocket_l2cap_rtentry_p rt)
+		ng_btsocket_l2cap_rtentry_p rt, struct socket **sop)
 {
 	ng_l2cap_l2ca_cfg_ind_ip	*ip = NULL;
 	ng_btsocket_l2cap_pcb_t		*pcb = NULL;
@@ -856,10 +903,14 @@ ng_btsocket_l2cap_process_l2ca_cfg_ind(struct ng_mesg *msg,
 
 	ip = (ng_l2cap_l2ca_cfg_ind_ip *)(msg->data);
 
+	mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
+
 	/* Check for the open socket that has given channel ID */
 	pcb = ng_btsocket_l2cap_pcb_by_cid(&rt->src, ip->lcid);
-	if (pcb == NULL)
+	if (pcb == NULL) {
+		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
 		return (ENOENT);
+	}
 
 	mtx_lock(&pcb->pcb_mtx);
 
@@ -876,6 +927,8 @@ ng_btsocket_l2cap_process_l2ca_cfg_ind(struct ng_mesg *msg,
 	/* XXX FIXME re-configuration on open socket */
  	if (pcb->state != NG_BTSOCKET_L2CAP_CONFIGURING) {
 		mtx_unlock(&pcb->pcb_mtx);
+		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
+
 		return (ENOENT);
 	}
 
@@ -894,29 +947,28 @@ ng_btsocket_l2cap_process_l2ca_cfg_ind(struct ng_mesg *msg,
 	 * if any send disconnect to close the channel. 
 	 */
 
-	if (sbreserve(&pcb->so->so_snd, ip->omtu, pcb->so, curthread)) {
-		if (!(pcb->cfg_state & NG_BTSOCKET_L2CAP_CFG_OUT_SENT)) {
-			error = ng_btsocket_l2cap_send_l2ca_cfg_rsp(pcb);
-			if (error == 0)
-				pcb->cfg_state |= NG_BTSOCKET_L2CAP_CFG_OUT_SENT;
-		}
-	} else
-		error = ENOBUFS;
+	if (!(pcb->cfg_state & NG_BTSOCKET_L2CAP_CFG_OUT_SENT)) {
+		error = ng_btsocket_l2cap_send_l2ca_cfg_rsp(pcb);
+		if (error != 0) {
+			ng_btsocket_l2cap_untimeout(pcb);
 
-	if (error != 0) {
-		ng_btsocket_l2cap_untimeout(pcb);
+			pcb->so->so_error = error;
 
-		pcb->so->so_error = error;
+			/* Send disconnect with "zero" token */
+			ng_btsocket_l2cap_send_l2ca_discon_req(0, pcb);
 
-		/* Send disconnect with "zero" token */
-		ng_btsocket_l2cap_send_l2ca_discon_req(0, pcb);
+			/* ... and close the socket */
+			pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
+			soisdisconnected(pcb->so);
 
-		/* ... and close the socket */
-		pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
-		soisdisconnected(pcb->so);
+			if (pcb->so->so_state & SS_NOFDREF)
+				*sop = pcb->so;
+		} else
+			pcb->cfg_state |= NG_BTSOCKET_L2CAP_CFG_OUT_SENT;
 	}
 
 	mtx_unlock(&pcb->pcb_mtx);
+	mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
 
 	return (error);
 } /* ng_btsocket_l2cap_process_l2cap_cfg_ind */
@@ -927,7 +979,7 @@ ng_btsocket_l2cap_process_l2ca_cfg_ind(struct ng_mesg *msg,
 
 static int
 ng_btsocket_l2cap_process_l2ca_discon_rsp(struct ng_mesg *msg,
-		ng_btsocket_l2cap_rtentry_p rt)
+		ng_btsocket_l2cap_rtentry_p rt, struct socket **sop)
 {
 	ng_l2cap_l2ca_discon_op	*op = NULL;
 	ng_btsocket_l2cap_pcb_t	*pcb = NULL;
@@ -938,6 +990,8 @@ ng_btsocket_l2cap_process_l2ca_discon_rsp(struct ng_mesg *msg,
 
 	op = (ng_l2cap_l2ca_discon_op *)(msg->data);
 
+	mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
+
 	/*
 	 * Socket layer must have issued L2CA_Disconnect request, so there 
 	 * must be a socket that wants to be disconnected. Use Netgraph 
@@ -945,8 +999,10 @@ ng_btsocket_l2cap_process_l2ca_discon_rsp(struct ng_mesg *msg,
 	 */
 
 	pcb = ng_btsocket_l2cap_pcb_by_token(msg->header.token);
-	if (pcb == NULL)
+	if (pcb == NULL) {
+		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
 		return (0);
+	}
 
 	mtx_lock(&pcb->pcb_mtx);
 
@@ -966,9 +1022,13 @@ ng_btsocket_l2cap_process_l2ca_discon_rsp(struct ng_mesg *msg,
 
 		pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
 		soisdisconnected(pcb->so);
+
+		if (pcb->so->so_state & SS_NOFDREF)
+			*sop = pcb->so;
 	}
 
 	mtx_unlock(&pcb->pcb_mtx);
+	mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
 
 	return (0);
 } /* ng_btsocket_l2cap_process_l2ca_discon_rsp */
@@ -979,7 +1039,7 @@ ng_btsocket_l2cap_process_l2ca_discon_rsp(struct ng_mesg *msg,
 
 static int
 ng_btsocket_l2cap_process_l2ca_discon_ind(struct ng_mesg *msg,
-		ng_btsocket_l2cap_rtentry_p rt)
+		ng_btsocket_l2cap_rtentry_p rt, struct socket **sop)
 {
 	ng_l2cap_l2ca_discon_ind_ip	*ip = NULL;
 	ng_btsocket_l2cap_pcb_t		*pcb = NULL;
@@ -990,10 +1050,14 @@ ng_btsocket_l2cap_process_l2ca_discon_ind(struct ng_mesg *msg,
 
 	ip = (ng_l2cap_l2ca_discon_ind_ip *)(msg->data);
 
+	mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
+
 	/* Look for the socket with given channel ID */
 	pcb = ng_btsocket_l2cap_pcb_by_cid(&rt->src, ip->lcid);
-	if (pcb == NULL)
+	if (pcb == NULL) {
+		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
 		return (0);
+	}
 
 	/*
 	 * Channel has already been destroyed, so disconnect the socket 
@@ -1019,7 +1083,11 @@ ng_btsocket_l2cap_process_l2ca_discon_ind(struct ng_mesg *msg,
 	pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
 	soisdisconnected(pcb->so);
 
+	if (pcb->so->so_state & SS_NOFDREF)
+		*sop = pcb->so;
+
 	mtx_unlock(&pcb->pcb_mtx);
+	mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
 
 	return (0);
 } /* ng_btsocket_l2cap_process_l2ca_discon_ind */
@@ -1030,7 +1098,7 @@ ng_btsocket_l2cap_process_l2ca_discon_ind(struct ng_mesg *msg,
 
 static int 
 ng_btsocket_l2cap_process_l2ca_write_rsp(struct ng_mesg *msg,
-		ng_btsocket_l2cap_rtentry_p rt)
+		ng_btsocket_l2cap_rtentry_p rt, struct socket **sop)
 {
 	ng_l2cap_l2ca_write_op	*op = NULL;
 	ng_btsocket_l2cap_pcb_t	*pcb = NULL;
@@ -1041,10 +1109,14 @@ ng_btsocket_l2cap_process_l2ca_write_rsp(struct ng_mesg *msg,
 
 	op = (ng_l2cap_l2ca_write_op *)(msg->data);
 
+	mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
+
 	/* Look for the socket with given token */
 	pcb = ng_btsocket_l2cap_pcb_by_token(msg->header.token);
-	if (pcb == NULL)
+	if (pcb == NULL) {
+		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
 		return (ENOENT);
+	}
 
 	mtx_lock(&pcb->pcb_mtx);
 
@@ -1061,6 +1133,8 @@ ng_btsocket_l2cap_process_l2ca_write_rsp(struct ng_mesg *msg,
 
 	if (pcb->state != NG_BTSOCKET_L2CAP_OPEN) {
 		mtx_unlock(&pcb->pcb_mtx);
+		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
+
 		return (ENOENT);
 	}
 	
@@ -1087,6 +1161,7 @@ ng_btsocket_l2cap_process_l2ca_write_rsp(struct ng_mesg *msg,
 	sowwakeup(pcb->so);
 
 	mtx_unlock(&pcb->pcb_mtx);
+	mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
 
 	return (0);
 } /* ng_btsocket_l2cap_process_l2ca_write_rsp */
@@ -1329,7 +1404,7 @@ ng_btsocket_l2cap_data_input(struct mbuf *m, hook_p hook)
 	 */
 
 	NG_BTSOCKET_L2CAP_INFO(
-"%s: Received L2CAP datat packet: src bdaddr=%x:%x:%x:%x:%x:%x, " \
+"%s: Received L2CAP data packet: src bdaddr=%x:%x:%x:%x:%x:%x, " \
 "dcid=%d, length=%d\n",
 		__func__, 
 		rt->src.b[5], rt->src.b[4], rt->src.b[3],
@@ -1337,10 +1412,15 @@ ng_btsocket_l2cap_data_input(struct mbuf *m, hook_p hook)
 		hdr->dcid, hdr->length);
 
 	if (NG_L2CAP_FIRST_CID <= hdr->dcid && hdr->dcid <= NG_L2CAP_LAST_CID) {
+
+		mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
+
 		/* Normal packet: find connected socket */
 		pcb = ng_btsocket_l2cap_pcb_by_cid(&rt->src, hdr->dcid);
-		if (pcb == NULL)
+		if (pcb == NULL) {
+			mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
 			goto drop;
+		}
 
 		mtx_lock(&pcb->pcb_mtx);
 
@@ -1353,6 +1433,7 @@ ng_btsocket_l2cap_data_input(struct mbuf *m, hook_p hook)
 				hdr->dcid, pcb->state);
 
 			mtx_unlock(&pcb->pcb_mtx);
+			mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
 			goto drop;
 		}
 
@@ -1367,6 +1448,7 @@ ng_btsocket_l2cap_data_input(struct mbuf *m, hook_p hook)
 				hdr->dcid, hdr->length, pcb->imtu);
 
 			mtx_unlock(&pcb->pcb_mtx);
+			mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
 			goto drop;
 		}
 
@@ -1390,6 +1472,7 @@ ng_btsocket_l2cap_data_input(struct mbuf *m, hook_p hook)
 				sbspace(&pcb->so->so_rcv));
 
 			mtx_unlock(&pcb->pcb_mtx);
+			mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
 			goto drop;
 		}
 
@@ -1398,8 +1481,10 @@ ng_btsocket_l2cap_data_input(struct mbuf *m, hook_p hook)
 		m = NULL;
 
 		sorwakeup(pcb->so);
+
 		mtx_unlock(&pcb->pcb_mtx);
-	} else if (hdr->dcid == NG_L2CAP_CLT_CID) { 
+		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
+	} else if (hdr->dcid == NG_L2CAP_CLT_CID) {
 		/* Broadcast packet: give packet to all sockets  */
 
 		/* Check packet size against connectionless MTU */
@@ -1472,7 +1557,7 @@ ng_btsocket_l2cap_data_input(struct mbuf *m, hook_p hook)
 next:
 			mtx_unlock(&pcb->pcb_mtx);
 		}
-		
+
 		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
 	}
 drop:
@@ -1496,23 +1581,27 @@ ng_btsocket_l2cap_default_msg_input(struct ng_mesg *msg, hook_p hook)
 		if (bcmp(msg->data, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0)
 			break;
 
+		mtx_lock(&ng_btsocket_l2cap_rt_mtx);
+
 		rt = (ng_btsocket_l2cap_rtentry_t *) NG_HOOK_PRIVATE(hook);
 		if (rt == NULL) {
 			MALLOC(rt, ng_btsocket_l2cap_rtentry_p, sizeof(*rt),
 				M_NETGRAPH_BTSOCKET_L2CAP, M_NOWAIT|M_ZERO);
-			if (rt == NULL)
+			if (rt == NULL) {
+				mtx_unlock(&ng_btsocket_l2cap_rt_mtx);
 				break;
+			}
 
-			mtx_lock(&ng_btsocket_l2cap_rt_mtx);
 			LIST_INSERT_HEAD(&ng_btsocket_l2cap_rt, rt, next);
-			mtx_unlock(&ng_btsocket_l2cap_rt_mtx);
 
 			NG_HOOK_SET_PRIVATE(hook, rt);
 		}
-		
+
 		bcopy(msg->data, &rt->src, sizeof(rt->src));
 		rt->hook = hook;
 
+		mtx_unlock(&ng_btsocket_l2cap_rt_mtx);
+
 		NG_BTSOCKET_L2CAP_INFO(
 "%s: Updating hook \"%s\", src bdaddr=%x:%x:%x:%x:%x:%x\n",
 			__func__, NG_HOOK_NAME(hook), 
@@ -1536,7 +1625,8 @@ ng_btsocket_l2cap_default_msg_input(struct ng_mesg *msg, hook_p hook)
 static void
 ng_btsocket_l2cap_l2ca_msg_input(struct ng_mesg *msg, hook_p hook)
 {
-	ng_btsocket_l2cap_rtentry_p	rt = NULL;
+	ng_btsocket_l2cap_rtentry_p	 rt = NULL;
+	struct socket			*so = NULL;
 
 	if (hook == NULL) {
 		NG_BTSOCKET_L2CAP_ALERT(
@@ -1553,39 +1643,39 @@ ng_btsocket_l2cap_l2ca_msg_input(struct ng_mesg *msg, hook_p hook)
 
 	switch (msg->header.cmd) {
 	case NGM_L2CAP_L2CA_CON: /* L2CA_Connect response */
-		ng_btsocket_l2cap_process_l2ca_con_req_rsp(msg, rt);
+		ng_btsocket_l2cap_process_l2ca_con_req_rsp(msg, rt, &so);
 		break;
 
 	case NGM_L2CAP_L2CA_CON_RSP: /* L2CA_ConnectRsp response */
-		ng_btsocket_l2cap_process_l2ca_con_rsp_rsp(msg, rt);
+		ng_btsocket_l2cap_process_l2ca_con_rsp_rsp(msg, rt, &so);
 		break;
 
 	case NGM_L2CAP_L2CA_CON_IND: /* L2CA_Connect indicator */
-		ng_btsocket_l2cap_process_l2ca_con_ind(msg, rt);
+		ng_btsocket_l2cap_process_l2ca_con_ind(msg, rt, &so);
 		break;
 
 	case NGM_L2CAP_L2CA_CFG: /* L2CA_Config response */
-		ng_btsocket_l2cap_process_l2ca_cfg_req_rsp(msg, rt);
+		ng_btsocket_l2cap_process_l2ca_cfg_req_rsp(msg, rt, &so);
 		break;
 
 	case NGM_L2CAP_L2CA_CFG_RSP: /* L2CA_ConfigRsp response */
-		ng_btsocket_l2cap_process_l2ca_cfg_rsp_rsp(msg, rt);
+		ng_btsocket_l2cap_process_l2ca_cfg_rsp_rsp(msg, rt, &so);
 		break;
 
 	case NGM_L2CAP_L2CA_CFG_IND: /* L2CA_Config indicator */
-		ng_btsocket_l2cap_process_l2ca_cfg_ind(msg, rt);
+		ng_btsocket_l2cap_process_l2ca_cfg_ind(msg, rt, &so);
 		break;
 
 	case NGM_L2CAP_L2CA_DISCON: /* L2CA_Disconnect response */
-		ng_btsocket_l2cap_process_l2ca_discon_rsp(msg, rt);
+		ng_btsocket_l2cap_process_l2ca_discon_rsp(msg, rt, &so);
 		break;
 
 	case NGM_L2CAP_L2CA_DISCON_IND: /* L2CA_Disconnect indicator */
-		ng_btsocket_l2cap_process_l2ca_discon_ind(msg, rt);
+		ng_btsocket_l2cap_process_l2ca_discon_ind(msg, rt, &so);
 		break;
 
 	case NGM_L2CAP_L2CA_WRITE: /* L2CA_Write response */
-		ng_btsocket_l2cap_process_l2ca_write_rsp(msg, rt);
+		ng_btsocket_l2cap_process_l2ca_write_rsp(msg, rt, &so);
 		break;
 
 	/* XXX FIXME add other L2CA messages */
@@ -1597,6 +1687,9 @@ ng_btsocket_l2cap_l2ca_msg_input(struct ng_mesg *msg, hook_p hook)
 	}
 drop:
 	NG_FREE_MSG(msg);
+
+	if (so != NULL)
+		ng_btsocket_l2cap_detach(so);
 } /* ng_btsocket_l2cap_l2ca_msg_input */
 
 /*
@@ -1675,7 +1768,7 @@ drop:
 static void
 ng_btsocket_l2cap_rtclean(void *context, int pending)
 {
-	ng_btsocket_l2cap_pcb_p		pcb = NULL;
+	ng_btsocket_l2cap_pcb_p		pcb = NULL, pcb_next = NULL;
 	ng_btsocket_l2cap_rtentry_p	rt = NULL;
 
 	mtx_lock(&ng_btsocket_l2cap_rt_mtx);
@@ -1685,8 +1778,9 @@ ng_btsocket_l2cap_rtclean(void *context, int pending)
 	 * First disconnect all sockets that use "invalid" hook
 	 */
 
-	LIST_FOREACH(pcb, &ng_btsocket_l2cap_sockets, next) {
+	for (pcb = LIST_FIRST(&ng_btsocket_l2cap_sockets); pcb != NULL; ) {
 		mtx_lock(&pcb->pcb_mtx);
+		pcb_next = LIST_NEXT(pcb, next);
 
 		if (pcb->rt != NULL &&
 		    pcb->rt->hook != NULL && NG_HOOK_NOT_VALID(pcb->rt->hook)) {
@@ -1698,18 +1792,38 @@ ng_btsocket_l2cap_rtclean(void *context, int pending)
 			soisdisconnected(pcb->so);
 
 			pcb->token = 0;
+			pcb->cid = 0;
 			pcb->rt = NULL;
+
+			if (pcb->so->so_state & SS_NOFDREF) {
+				struct socket	*so = pcb->so;
+
+				LIST_REMOVE(pcb, next);
+
+				mtx_unlock(&pcb->pcb_mtx);
+
+				mtx_destroy(&pcb->pcb_mtx);
+				bzero(pcb, sizeof(*pcb));
+				FREE(pcb, M_NETGRAPH_BTSOCKET_L2CAP);
+
+				soisdisconnected(so);
+				so->so_pcb = NULL;
+				sotryfree(so);
+
+				goto next;
+			}
 		}
 
 		mtx_unlock(&pcb->pcb_mtx);
+next:
+		pcb = pcb_next;
 	}
 
 	/*
 	 * Now cleanup routing table
 	 */
 
-	rt = LIST_FIRST(&ng_btsocket_l2cap_rt);
-	while (rt != NULL) {
+	for (rt = LIST_FIRST(&ng_btsocket_l2cap_rt); rt != NULL; ) {
 		ng_btsocket_l2cap_rtentry_p	rt_next = LIST_NEXT(rt, next);
 
 		if (rt->hook != NULL && NG_HOOK_NOT_VALID(rt->hook)) {
@@ -1740,7 +1854,6 @@ ng_btsocket_l2cap_init(void)
 
 	ng_btsocket_l2cap_node = NULL;
 	ng_btsocket_l2cap_debug_level = NG_BTSOCKET_WARN_LEVEL;
-	ng_btsocket_l2cap_ioctl_timeout = 5;
 
 	/* Register Netgraph node type */
 	error = ng_newtype(&typestruct);
@@ -1786,11 +1899,6 @@ ng_btsocket_l2cap_init(void)
 	mtx_init(&ng_btsocket_l2cap_sockets_mtx,
 		"btsocks_l2cap_sockets_mtx", NULL, MTX_DEF);
 
-	/* Tokens */
-	ng_btsocket_l2cap_token = 0;
-	mtx_init(&ng_btsocket_l2cap_token_mtx,
-		"btsocks_l2cap_token_mtx", NULL, MTX_DEF);
-
 	/* Routing table */
 	LIST_INIT(&ng_btsocket_l2cap_rt);
 	mtx_init(&ng_btsocket_l2cap_rt_mtx,
@@ -1832,6 +1940,7 @@ ng_btsocket_l2cap_accept(struct socket *so, struct sockaddr **nam)
 int
 ng_btsocket_l2cap_attach(struct socket *so, int proto, struct thread *td)
 {
+	static u_int32_t	token = 0;
 	ng_btsocket_l2cap_pcb_p	pcb = so2l2cap_pcb(so);
 	int			error;
 
@@ -1886,15 +1995,52 @@ ng_btsocket_l2cap_attach(struct socket *so, int proto, struct thread *td)
 	pcb->flush_timo = NG_L2CAP_FLUSH_TIMO_DEFAULT;
 	pcb->link_timo = NG_L2CAP_LINK_TIMO_DEFAULT;
 
-	ng_btsocket_l2cap_get_token(&pcb->token);
-
 	callout_handle_init(&pcb->timo);
-	mtx_init(&pcb->pcb_mtx, "btsocket_pcb_mtx", NULL, MTX_DEF);
 
-        /* Add the PCB to the list */
-	mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
+	/*
+	 * XXX Mark PCB mutex as DUPOK to prevent "duplicated lock of
+	 * the same type" message. When accepting new L2CAP connection 
+	 * ng_btsocket_l2cap_process_l2ca_con_ind() holds both PCB mutexes 
+	 * for "old" (accepting) PCB and "new" (created) PCB.
+	 */
+		
+	mtx_init(&pcb->pcb_mtx, "btsocks_l2cap_pcb_mtx", NULL,
+		MTX_DEF|MTX_DUPOK);
+
+        /*
+	 * Add the PCB to the list
+	 * 
+	 * XXX FIXME VERY IMPORTANT!
+	 *
+	 * This is totally FUBAR. We could get here in two cases:
+	 *
+	 * 1) When user calls socket()
+	 * 2) When we need to accept new incomming connection and call 
+	 *    sonewconn()
+	 *
+	 * In the first case we must aquire ng_btsocket_l2cap_sockets_mtx.
+	 * In the second case we hold ng_btsocket_l2cap_sockets_mtx already.
+	 * So we now need to distinguish between these cases. From reading
+	 * /sys/kern/uipc_socket2.c we can find out that sonewconn() calls
+	 * pru_attach with proto == 0 and td == NULL. For now use this fact
+	 * to figure out if we were called from socket() or from sonewconn().
+	 */
+
+	if (td != NULL)
+		mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
+	else
+		mtx_assert(&ng_btsocket_l2cap_sockets_mtx, MA_OWNED);
+	
+	/* Set PCB token. Use ng_btsocket_l2cap_sockets_mtx for protection */
+	if (++ token == 0)
+		token ++;
+
+	pcb->token = token;
+
 	LIST_INSERT_HEAD(&ng_btsocket_l2cap_sockets, pcb, next);
-	mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
+
+	if (td != NULL)
+		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
 
         return (0);
 } /* ng_btsocket_l2cap_attach */
@@ -1989,10 +2135,6 @@ ng_btsocket_l2cap_connect(struct socket *so, struct sockaddr *nam,
 	if (pcb->psm != 0 && pcb->psm != le16toh(sa->l2cap_psm))
 		return (EINVAL);
 
-	/* Send destination address and PSM */
-	bcopy(&sa->l2cap_bdaddr, &pcb->dst, sizeof(pcb->dst));
-	pcb->psm = le16toh(sa->l2cap_psm);
-
 	/*
 	 * Routing. Socket should be bound to some source address. The source
 	 * address can be ANY. Destination address must be set and it must not
@@ -2000,11 +2142,17 @@ ng_btsocket_l2cap_connect(struct socket *so, struct sockaddr *nam,
 	 * src != dst.
 	 */
 
+	mtx_lock(&ng_btsocket_l2cap_rt_mtx);
+	mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
+	mtx_lock(&pcb->pcb_mtx);
+
+	/* Send destination address and PSM */
+	bcopy(&sa->l2cap_bdaddr, &pcb->dst, sizeof(pcb->dst));
+	pcb->psm = le16toh(sa->l2cap_psm);
+
 	pcb->rt = NULL;
 	have_src = bcmp(&pcb->src, NG_HCI_BDADDR_ANY, sizeof(pcb->src));
 
-	mtx_lock(&ng_btsocket_l2cap_rt_mtx);
-
 	LIST_FOREACH(rt, &ng_btsocket_l2cap_rt, next) {
 		if (rt->hook == NULL || NG_HOOK_NOT_VALID(rt->hook))
 			continue;
@@ -2027,14 +2175,11 @@ ng_btsocket_l2cap_connect(struct socket *so, struct sockaddr *nam,
 	} else
 		error = EHOSTUNREACH;
 
-	mtx_unlock(&ng_btsocket_l2cap_rt_mtx);
-
 	/*
 	 * Send L2CA_Connect request 
 	 */
 
 	if (error == 0) {	
-		mtx_lock(&pcb->pcb_mtx);
 		error = ng_btsocket_l2cap_send_l2ca_con_req(pcb);
 		if (error == 0) {
 			pcb->flags |= NG_BTSOCKET_L2CAP_CLIENT;
@@ -2043,9 +2188,12 @@ ng_btsocket_l2cap_connect(struct socket *so, struct sockaddr *nam,
 
 			ng_btsocket_l2cap_timeout(pcb);
 		}
-		mtx_unlock(&pcb->pcb_mtx);
 	}
 
+	mtx_unlock(&pcb->pcb_mtx);
+	mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
+	mtx_unlock(&ng_btsocket_l2cap_rt_mtx);
+
 	return (error);
 } /* ng_btsocket_l2cap_connect */
 
@@ -2117,12 +2265,11 @@ ng_btsocket_l2cap_ctloutput(struct socket *so, struct sockopt *sopt)
 
 	case SOPT_SET:
 		/*
-		 * We do not allow to change these parameters while 
-		 * socket is connected or we are in the process of 
-		 * creating a connection
-		 *
-		 * XXX may be this should indicate re-configuration of the
-		 * open channel?
+		 * XXX
+		 * We do not allow to change these parameters while socket is 
+		 * connected or we are in the process of creating a connection.
+		 * May be this should indicate re-configuration of the open 
+		 * channel?
 		 */
 
 		if (pcb->state != NG_BTSOCKET_L2CAP_CLOSED)
@@ -2178,6 +2325,7 @@ ng_btsocket_l2cap_detach(struct socket *so)
 	if (ng_btsocket_l2cap_node == NULL) 
 		return (EINVAL);
 
+	mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
 	mtx_lock(&pcb->pcb_mtx);
 
 	/* XXX what to do with pending request? */
@@ -2190,21 +2338,20 @@ ng_btsocket_l2cap_detach(struct socket *so)
 		ng_btsocket_l2cap_send_l2ca_discon_req(0, pcb);
 
 	pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
-	soisdisconnected(so);
 
-	so->so_pcb = NULL;
-	sotryfree(so);
+	LIST_REMOVE(pcb, next);
 
 	mtx_unlock(&pcb->pcb_mtx);
-
-	mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
-	LIST_REMOVE(pcb, next);
 	mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
 
 	mtx_destroy(&pcb->pcb_mtx);
 	bzero(pcb, sizeof(*pcb));
 	FREE(pcb, M_NETGRAPH_BTSOCKET_L2CAP);
 
+	soisdisconnected(so);
+	so->so_pcb = NULL;
+	sotryfree(so);
+
 	return (0);
 } /* ng_btsocket_l2cap_detach */
 
@@ -2389,7 +2536,7 @@ ng_btsocket_l2cap_send2(ng_btsocket_l2cap_pcb_p pcb)
 		return (ENOBUFS);
 
 	/* Create L2CA packet header */
-	M_PREPEND(m, sizeof(*hdr), M_NOWAIT);
+	M_PREPEND(m, sizeof(*hdr), M_DONTWAIT);
 	if (m != NULL)
 		if (m->m_len < sizeof(*hdr))
 			m = m_pullup(m, sizeof(*hdr));
@@ -2454,7 +2601,7 @@ ng_btsocket_l2cap_sockaddr(struct socket *so, struct sockaddr **nam)
 
 /*
  * Look for the socket that listens on given PSM and bdaddr. Returns exact or
- * close match (if any).
+ * close match (if any). Caller must hold ng_btsocket_l2cap_sockets_mtx.
  */
 
 static ng_btsocket_l2cap_pcb_p
@@ -2462,7 +2609,7 @@ ng_btsocket_l2cap_pcb_by_addr(bdaddr_p bdaddr, int psm)
 {
 	ng_btsocket_l2cap_pcb_p	p = NULL, p1 = NULL;
 
-	mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
+	mtx_assert(&ng_btsocket_l2cap_sockets_mtx, MA_OWNED);
 
 	LIST_FOREACH(p, &ng_btsocket_l2cap_sockets, next) {
 		if (p->so == NULL || !(p->so->so_options & SO_ACCEPTCONN) || 
@@ -2476,13 +2623,12 @@ ng_btsocket_l2cap_pcb_by_addr(bdaddr_p bdaddr, int psm)
 			p1 = p;
 	}
 
-	mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
-
 	return ((p != NULL)? p : p1);
 } /* ng_btsocket_l2cap_pcb_by_addr */
 
 /*
- * Look for the socket that has given token
+ * Look for the socket that has given token.
+ * Caller must hold ng_btsocket_l2cap_sockets_mtx.
  */
 
 static ng_btsocket_l2cap_pcb_p
@@ -2490,21 +2636,21 @@ ng_btsocket_l2cap_pcb_by_token(u_int32_t token)
 {
 	ng_btsocket_l2cap_pcb_p	p = NULL;
 
-	if (token != 0) {
-		mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
+	if (token == 0)
+		return (NULL);
 
-		LIST_FOREACH(p, &ng_btsocket_l2cap_sockets, next)
-			if (p->token == token)
-				break;
+	mtx_assert(&ng_btsocket_l2cap_sockets_mtx, MA_OWNED);
 
-		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
-	}
+	LIST_FOREACH(p, &ng_btsocket_l2cap_sockets, next)
+		if (p->token == token)
+			break;
 
 	return (p);
 } /* ng_btsocket_l2cap_pcb_by_token */
 
 /*
- * Look for the socket that assigned to given source address and channel ID
+ * Look for the socket that assigned to given source address and channel ID.
+ * Caller must hold ng_btsocket_l2cap_sockets_mtx
  */
 
 static ng_btsocket_l2cap_pcb_p
@@ -2512,14 +2658,12 @@ ng_btsocket_l2cap_pcb_by_cid(bdaddr_p src, int cid)
 {
 	ng_btsocket_l2cap_pcb_p	p = NULL;
 
-	mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
+	mtx_assert(&ng_btsocket_l2cap_sockets_mtx, MA_OWNED);
 
 	LIST_FOREACH(p, &ng_btsocket_l2cap_sockets, next)
 		if (p->cid == cid && bcmp(src, &p->src, sizeof(p->src)) == 0)
 			break;
 
-	mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
-
 	return (p);
 } /* ng_btsocket_l2cap_pcb_by_cid */
 
@@ -2565,7 +2709,8 @@ ng_btsocket_l2cap_untimeout(ng_btsocket_l2cap_pcb_p pcb)
 static void
 ng_btsocket_l2cap_process_timeout(void *xpcb)
 {
-	ng_btsocket_l2cap_pcb_p	pcb = (ng_btsocket_l2cap_pcb_p) xpcb;
+	ng_btsocket_l2cap_pcb_p	 pcb = (ng_btsocket_l2cap_pcb_p) xpcb;
+	struct socket		*so = NULL;
 
 	mtx_lock(&pcb->pcb_mtx);
 
@@ -2582,6 +2727,9 @@ ng_btsocket_l2cap_process_timeout(void *xpcb)
 		/* ... and close the socket */
 		pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
 		soisdisconnected(pcb->so);
+
+		if (pcb->so->so_state & SS_NOFDREF)
+			so = pcb->so;
 		break;
 
 	case NG_BTSOCKET_L2CAP_OPEN:
@@ -2594,6 +2742,9 @@ ng_btsocket_l2cap_process_timeout(void *xpcb)
 		/* Disconnect timeout - disconnect the socket anyway */
 		pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
 		soisdisconnected(pcb->so);
+
+		if (pcb->so->so_state & SS_NOFDREF)
+			so = pcb->so;
 		break;
 
 	default:
@@ -2603,24 +2754,10 @@ ng_btsocket_l2cap_process_timeout(void *xpcb)
 	}
 
 	mtx_unlock(&pcb->pcb_mtx);
-} /* ng_btsocket_l2cap_process_timeout */
 
-/*
- * Get next token
- */
-
-static void
-ng_btsocket_l2cap_get_token(u_int32_t *token)
-{
-	mtx_lock(&ng_btsocket_l2cap_token_mtx);
-  
-	if (++ ng_btsocket_l2cap_token == 0)
-		ng_btsocket_l2cap_token = 1;
- 
-	*token = ng_btsocket_l2cap_token;
- 
-	mtx_unlock(&ng_btsocket_l2cap_token_mtx);
-} /* ng_btsocket_l2cap_get_token */
+	if (so != NULL)
+		ng_btsocket_l2cap_detach(so);
+} /* ng_btsocket_l2cap_process_timeout */
 
 /*
  * Translate HCI/L2CAP error code into "errno" code
diff --git a/sys/netgraph/bluetooth/socket/ng_btsocket_l2cap_raw.c b/sys/netgraph/bluetooth/socket/ng_btsocket_l2cap_raw.c
index 56a53b9..0e52d30 100644
--- a/sys/netgraph/bluetooth/socket/ng_btsocket_l2cap_raw.c
+++ b/sys/netgraph/bluetooth/socket/ng_btsocket_l2cap_raw.c
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_btsocket_l2cap_raw.c,v 1.1 2002/09/04 21:44:00 max Exp $
+ * $Id: ng_btsocket_l2cap_raw.c,v 1.11 2003/04/27 19:52:14 max Exp $
  * $FreeBSD$
  */
 
@@ -76,6 +76,17 @@ static void		ng_btsocket_l2cap_raw_input     (void *, int);
 static void		ng_btsocket_l2cap_raw_rtclean   (void *, int);
 static void		ng_btsocket_l2cap_raw_get_token (u_int32_t *);
 
+static int		ng_btsocket_l2cap_raw_send_ngmsg
+				(hook_p, int, void *, int);
+static int		ng_btsocket_l2cap_raw_send_sync_ngmsg
+				(ng_btsocket_l2cap_raw_pcb_p, int, void *, int);
+
+#define ng_btsocket_l2cap_raw_wakeup_input_task() \
+	taskqueue_enqueue(taskqueue_swi_giant, &ng_btsocket_l2cap_raw_queue_task)
+
+#define ng_btsocket_l2cap_raw_wakeup_route_task() \
+	taskqueue_enqueue(taskqueue_swi_giant, &ng_btsocket_l2cap_raw_rt_task)
+
 /* Netgraph type descriptor */
 static struct ng_type	typestruct = {
 	NG_ABI_VERSION,
@@ -242,8 +253,7 @@ ng_btsocket_l2cap_raw_node_disconnect(hook_p hook)
 	 */
 
 	if (NG_HOOK_PRIVATE(hook) != NULL)
-		return (taskqueue_enqueue(taskqueue_swi_giant, 
-				&ng_btsocket_l2cap_raw_rt_task));
+		return (ng_btsocket_l2cap_raw_wakeup_route_task());
 
 	NG_HOOK_UNREF(hook); /* Remove extra reference */
 
@@ -261,6 +271,21 @@ ng_btsocket_l2cap_raw_node_rcvmsg(node_p node, item_p item, hook_p hook)
 	int		 error = 0;
 
 	if (msg != NULL && msg->header.typecookie == NGM_L2CAP_COOKIE) {
+
+		/*
+		 * NGM_L2CAP_NODE_HOOK_INFO is special message initiated by
+		 * L2CAP layer. Ignore all other messages if they are not
+		 * replies or token is zero 
+		 */
+
+		if (msg->header.cmd != NGM_L2CAP_NODE_HOOK_INFO) {
+			if (msg->header.token == 0 ||
+			    !(msg->header.flags & NGF_RESP)) {
+				NG_FREE_ITEM(item);
+				return (0);
+			}
+		}
+
 		mtx_lock(&ng_btsocket_l2cap_raw_queue_mtx);
 		if (NG_BT_ITEMQ_FULL(&ng_btsocket_l2cap_raw_queue)) {
 			NG_BTSOCKET_L2CAP_RAW_ERR(
@@ -276,8 +301,7 @@ ng_btsocket_l2cap_raw_node_rcvmsg(node_p node, item_p item, hook_p hook)
 			}
 
 			NG_BT_ITEMQ_ENQUEUE(&ng_btsocket_l2cap_raw_queue, item);
-			error = taskqueue_enqueue(taskqueue_swi_giant,
-					&ng_btsocket_l2cap_raw_queue_task);
+			error = ng_btsocket_l2cap_raw_wakeup_input_task();
 		}
 		mtx_unlock(&ng_btsocket_l2cap_raw_queue_mtx);
 	} else {
@@ -344,6 +368,8 @@ ng_btsocket_l2cap_raw_input(void *context, int pending)
 					sizeof(bdaddr_t)) == 0)
 				break;
 
+			mtx_lock(&ng_btsocket_l2cap_raw_rt_mtx);
+
 			rt = (ng_btsocket_l2cap_rtentry_t *) 
 				NG_HOOK_PRIVATE(hook);
 			if (rt == NULL) {
@@ -351,13 +377,13 @@ ng_btsocket_l2cap_raw_input(void *context, int pending)
 					sizeof(*rt),
 					M_NETGRAPH_BTSOCKET_L2CAP_RAW,
 					M_NOWAIT|M_ZERO);
-				if (rt == NULL)
+				if (rt == NULL) {
+					mtx_unlock(&ng_btsocket_l2cap_raw_rt_mtx);
 					break;
+				}
 
-				mtx_lock(&ng_btsocket_l2cap_raw_rt_mtx);
 				LIST_INSERT_HEAD(&ng_btsocket_l2cap_raw_rt, 
 					rt, next);
-				mtx_unlock(&ng_btsocket_l2cap_raw_rt_mtx);
 
 				NG_HOOK_SET_PRIVATE(hook, rt);
 			}
@@ -365,6 +391,8 @@ ng_btsocket_l2cap_raw_input(void *context, int pending)
 			bcopy(msg->data, &rt->src, sizeof(rt->src));
 			rt->hook = hook;
 
+			mtx_unlock(&ng_btsocket_l2cap_raw_rt_mtx);
+
 			NG_BTSOCKET_L2CAP_RAW_INFO(
 "%s: Updating hook \"%s\", src bdaddr=%x:%x:%x:%x:%x:%x\n",
 				__func__, NG_HOOK_NAME(hook), 
@@ -376,24 +404,27 @@ ng_btsocket_l2cap_raw_input(void *context, int pending)
 		case NGM_L2CAP_NODE_GET_DEBUG:
 		case NGM_L2CAP_NODE_GET_CON_LIST:
 		case NGM_L2CAP_NODE_GET_CHAN_LIST:
+		case NGM_L2CAP_NODE_GET_AUTO_DISCON_TIMO:
 		case NGM_L2CAP_L2CA_PING:
 		case NGM_L2CAP_L2CA_GET_INFO: {
 			ng_btsocket_l2cap_raw_pcb_p	pcb = NULL;
 
-			if (msg->header.token == 0 ||
-			    !(msg->header.flags & NGF_RESP))
-				break;
-
 			mtx_lock(&ng_btsocket_l2cap_raw_sockets_mtx);
 
-			LIST_FOREACH(pcb, &ng_btsocket_l2cap_raw_sockets, next)
+			LIST_FOREACH(pcb,&ng_btsocket_l2cap_raw_sockets,next) {
+				mtx_lock(&pcb->pcb_mtx);
+
 				if (pcb->token == msg->header.token) {
 					pcb->msg = msg;
 					msg = NULL;
 					wakeup(&pcb->msg);
+					mtx_unlock(&pcb->pcb_mtx);
 					break;
 				}
 
+				mtx_unlock(&pcb->pcb_mtx);
+			}
+
 			mtx_unlock(&ng_btsocket_l2cap_raw_sockets_mtx);
 			} break;
 
@@ -408,7 +439,7 @@ ng_btsocket_l2cap_raw_input(void *context, int pending)
 
 		NG_FREE_MSG(msg); /* Checks for msg != NULL */
 	}
-} /* ng_btsocket_l2cap_raw_default_msg_input */
+} /* ng_btsocket_l2cap_raw_input */
 
 /*
  * Route cleanup task. Gets scheduled when hook is disconnected. Here we 
@@ -428,7 +459,9 @@ ng_btsocket_l2cap_raw_rtclean(void *context, int pending)
 	 * First disconnect all sockets that use "invalid" hook
 	 */
 
-	LIST_FOREACH(pcb, &ng_btsocket_l2cap_raw_sockets, next)
+	LIST_FOREACH(pcb, &ng_btsocket_l2cap_raw_sockets, next) {
+		mtx_lock(&pcb->pcb_mtx);
+
 		if (pcb->rt != NULL &&
 		    pcb->rt->hook != NULL && NG_HOOK_NOT_VALID(pcb->rt->hook)) {
 			if (pcb->so != NULL &&
@@ -438,12 +471,14 @@ ng_btsocket_l2cap_raw_rtclean(void *context, int pending)
 			pcb->rt = NULL;
 		}
 
+		mtx_unlock(&pcb->pcb_mtx);
+	}
+
 	/*
 	 * Now cleanup routing table
 	 */
 
-	rt = LIST_FIRST(&ng_btsocket_l2cap_raw_rt);
-	while (rt != NULL) {
+	for (rt = LIST_FIRST(&ng_btsocket_l2cap_raw_rt); rt != NULL; ) {
 		ng_btsocket_l2cap_rtentry_p	rt_next = LIST_NEXT(rt, next);
 
 		if (rt->hook != NULL && NG_HOOK_NOT_VALID(rt->hook)) {
@@ -560,8 +595,6 @@ ng_btsocket_l2cap_raw_attach(struct socket *so, int proto, struct thread *td)
 		return (EPROTONOSUPPORT);
 	if (so->so_type != SOCK_RAW)
 		return (ESOCKTNOSUPPORT);
-	if ((error = suser(td)) != 0)
-		return (error);
 
 	/* Reserve send and receive space if it is not reserved yet */
 	error = soreserve(so, NG_BTSOCKET_L2CAP_RAW_SENDSPACE,
@@ -571,7 +604,7 @@ ng_btsocket_l2cap_raw_attach(struct socket *so, int proto, struct thread *td)
 
 	/* Allocate the PCB */
         MALLOC(pcb, ng_btsocket_l2cap_raw_pcb_p, sizeof(*pcb),
-		M_NETGRAPH_BTSOCKET_L2CAP_RAW, M_WAITOK | M_ZERO);
+		M_NETGRAPH_BTSOCKET_L2CAP_RAW, M_NOWAIT|M_ZERO);
         if (pcb == NULL)
                 return (ENOMEM);
 
@@ -579,6 +612,11 @@ ng_btsocket_l2cap_raw_attach(struct socket *so, int proto, struct thread *td)
 	so->so_pcb = (caddr_t) pcb;
 	pcb->so = so;
 
+	if (suser(td) == 0)
+		pcb->flags |= NG_BTSOCKET_L2CAP_RAW_PRIVILEGED;
+
+	mtx_init(&pcb->pcb_mtx, "btsocks_l2cap_raw_pcb_mtx", NULL, MTX_DEF);
+
         /* Add the PCB to the list */
 	mtx_lock(&ng_btsocket_l2cap_raw_sockets_mtx);
 	LIST_INSERT_HEAD(&ng_btsocket_l2cap_raw_sockets, pcb, next);
@@ -597,6 +635,8 @@ ng_btsocket_l2cap_raw_bind(struct socket *so, struct sockaddr *nam,
 {
 	ng_btsocket_l2cap_raw_pcb_t	*pcb = so2l2cap_raw_pcb(so);
 	struct sockaddr_l2cap		*sa = (struct sockaddr_l2cap *) nam;
+	ng_btsocket_l2cap_rtentry_t	*rt = NULL;
+	int				 error;
 
 	if (pcb == NULL)
 		return (EINVAL);
@@ -609,12 +649,41 @@ ng_btsocket_l2cap_raw_bind(struct socket *so, struct sockaddr *nam,
 		return (EAFNOSUPPORT);
 	if (sa->l2cap_len != sizeof(*sa))
 		return (EINVAL);
-	if (bcmp(&sa->l2cap_bdaddr, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0)
-		return (EINVAL);
 
+	mtx_lock(&ng_btsocket_l2cap_raw_rt_mtx);
+	mtx_lock(&ng_btsocket_l2cap_raw_sockets_mtx);
+	mtx_lock(&pcb->pcb_mtx);
+
+	pcb->rt = NULL;
 	bcopy(&sa->l2cap_bdaddr, &pcb->src, sizeof(pcb->src));
 
-	return (0);
+	if (bcmp(&pcb->src, NG_HCI_BDADDR_ANY, sizeof(pcb->src)) == 0) {
+		mtx_unlock(&pcb->pcb_mtx);
+		mtx_unlock(&ng_btsocket_l2cap_raw_sockets_mtx);
+		mtx_unlock(&ng_btsocket_l2cap_raw_rt_mtx);
+
+		return (0);
+	}
+
+	LIST_FOREACH(rt, &ng_btsocket_l2cap_raw_rt, next) {
+		if (rt->hook == NULL || NG_HOOK_NOT_VALID(rt->hook))
+			continue;
+
+		if (bcmp(&pcb->src, &rt->src, sizeof(rt->src)) == 0)
+			break;
+	}
+
+	if (rt != NULL) {
+		pcb->rt = rt;
+		error = 0;
+	} else
+		error = ENETDOWN;
+
+	mtx_unlock(&pcb->pcb_mtx);
+	mtx_unlock(&ng_btsocket_l2cap_raw_sockets_mtx);
+	mtx_unlock(&ng_btsocket_l2cap_raw_rt_mtx);
+
+	return (error);
 } /* ng_btsocket_l2cap_raw_bind */
 
 /*
@@ -628,6 +697,7 @@ ng_btsocket_l2cap_raw_connect(struct socket *so, struct sockaddr *nam,
 	ng_btsocket_l2cap_raw_pcb_t	*pcb = so2l2cap_raw_pcb(so);
 	struct sockaddr_l2cap		*sa = (struct sockaddr_l2cap *) nam;
 	ng_btsocket_l2cap_rtentry_t	*rt = NULL;
+	int				 error;
 
 	if (pcb == NULL)
 		return (EINVAL);
@@ -642,80 +712,63 @@ ng_btsocket_l2cap_raw_connect(struct socket *so, struct sockaddr *nam,
 		return (EINVAL);
 	if (bcmp(&sa->l2cap_bdaddr, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0)
 		return (EINVAL);
-	if (bcmp(&sa->l2cap_bdaddr, &pcb->src, sizeof(pcb->src)) != 0)
+
+	mtx_lock(&ng_btsocket_l2cap_raw_rt_mtx);
+	mtx_lock(&ng_btsocket_l2cap_raw_sockets_mtx);
+	mtx_lock(&pcb->pcb_mtx);
+
+	bcopy(&sa->l2cap_bdaddr, &pcb->dst, sizeof(pcb->dst));
+
+	if (bcmp(&pcb->src, &pcb->dst, sizeof(pcb->src)) == 0) {
+		mtx_unlock(&pcb->pcb_mtx);
+		mtx_unlock(&ng_btsocket_l2cap_raw_sockets_mtx);
+		mtx_unlock(&ng_btsocket_l2cap_raw_rt_mtx);
+
 		return (EADDRNOTAVAIL);
+	}
 
 	/*
-	 * Find hook with specified source address
+	 * If there is route already - use it
 	 */
 
-	pcb->rt = NULL;
+	if (pcb->rt != NULL) {
+		soisconnected(so);
 
-	mtx_lock(&ng_btsocket_l2cap_raw_rt_mtx);
+		mtx_unlock(&pcb->pcb_mtx);
+		mtx_unlock(&ng_btsocket_l2cap_raw_sockets_mtx);
+		mtx_unlock(&ng_btsocket_l2cap_raw_rt_mtx);
+
+		return (0);
+	}
+	
+	/*
+	 * Find the first hook that does not match specified destination address
+	 */
 
 	LIST_FOREACH(rt, &ng_btsocket_l2cap_raw_rt, next) {
 		if (rt->hook == NULL || NG_HOOK_NOT_VALID(rt->hook))
 			continue;
 
-		if (bcmp(&pcb->src, &rt->src, sizeof(rt->src)) == 0)
+		if (bcmp(&pcb->dst, &rt->src, sizeof(rt->src)) != 0)
 			break;
 	}
 
 	if (rt != NULL) {
-		pcb->rt = rt;
 		soisconnected(so);
-	}
 
-	mtx_unlock(&ng_btsocket_l2cap_raw_rt_mtx);
-
-	return  ((pcb->rt != NULL)? 0 : ENETDOWN);
-} /* ng_btsocket_l2cap_raw_connect */
-
-/*
- * Find hook that matches source address
- */
-
-static ng_btsocket_l2cap_rtentry_p
-ng_btsocket_l2cap_raw_find_src_route(bdaddr_p src)
-{
-	ng_btsocket_l2cap_rtentry_p	rt = NULL;
-
-	if (bcmp(src, NG_HCI_BDADDR_ANY, sizeof(*src)) != 0) {
-		mtx_lock(&ng_btsocket_l2cap_raw_rt_mtx);
-
-		LIST_FOREACH(rt, &ng_btsocket_l2cap_raw_rt, next)
-			if (rt->hook != NULL && NG_HOOK_IS_VALID(rt->hook) &&
-			    bcmp(src, &rt->src, sizeof(*src)) == 0)
-				break;
-
-		mtx_unlock(&ng_btsocket_l2cap_raw_rt_mtx);
-	}
-
-	return (rt);
-} /* ng_btsocket_l2cap_raw_find_src_route */
-
-/*
- * Find first hook does does not match destination address
- */
-
-static ng_btsocket_l2cap_rtentry_p
-ng_btsocket_l2cap_raw_find_dst_route(bdaddr_p dst)
-{
-	ng_btsocket_l2cap_rtentry_p	rt = NULL;
-
-	if (bcmp(dst, NG_HCI_BDADDR_ANY, sizeof(*dst)) != 0) {
-		mtx_lock(&ng_btsocket_l2cap_raw_rt_mtx);
+		pcb->rt = rt;
+		bcopy(&rt->src, &pcb->src, sizeof(pcb->src));
 
-		LIST_FOREACH(rt, &ng_btsocket_l2cap_raw_rt, next)
-			if (rt->hook != NULL && NG_HOOK_IS_VALID(rt->hook) &&
-			    bcmp(dst, &rt->src, sizeof(*dst)) != 0)
-				break;
+		error = 0;
+	} else
+		error = ENETDOWN;
 
-		mtx_unlock(&ng_btsocket_l2cap_raw_rt_mtx);
-	}
+	mtx_unlock(&pcb->pcb_mtx);
+	mtx_unlock(&ng_btsocket_l2cap_raw_sockets_mtx);
+	mtx_unlock(&ng_btsocket_l2cap_raw_rt_mtx);
 
-	return (rt);
-} /* ng_btsocket_l2cap_raw_find_dst_route */
+	return  (error);
+} /* ng_btsocket_l2cap_raw_connect */
 
 /*
  * Process ioctl's calls on socket
@@ -726,8 +779,6 @@ ng_btsocket_l2cap_raw_control(struct socket *so, u_long cmd, caddr_t data,
 		struct ifnet *ifp, struct thread *td)
 {
 	ng_btsocket_l2cap_raw_pcb_p	 pcb = so2l2cap_raw_pcb(so);
-	bdaddr_t			*src = (bdaddr_t *) data;
-	ng_btsocket_l2cap_rtentry_p	 rt = pcb->rt;
 	struct ng_mesg			*msg = NULL;
 	int				 error = 0;
 
@@ -736,116 +787,49 @@ ng_btsocket_l2cap_raw_control(struct socket *so, u_long cmd, caddr_t data,
 	if (ng_btsocket_l2cap_raw_node == NULL)
 		return (EINVAL);
 
-	if (rt == NULL || rt->hook == NULL || NG_HOOK_NOT_VALID(rt->hook)) {
-		if (cmd == SIOC_L2CAP_L2CA_PING ||
-		    cmd == SIOC_L2CAP_L2CA_GET_INFO)
-			rt = ng_btsocket_l2cap_raw_find_dst_route(src + 1);
-		else
-			rt = ng_btsocket_l2cap_raw_find_src_route(src);
+	mtx_lock(&pcb->pcb_mtx);
 
-		if (rt == NULL) 
-			return (EHOSTUNREACH);
+	/* Check if we route info */
+	if (pcb->rt == NULL) {
+		mtx_unlock(&pcb->pcb_mtx);
+		return (EHOSTUNREACH);
 	}
 
-	bcopy(&rt->src, src, sizeof(*src));
+	/* Check if we have pending ioctl() */
+	if (pcb->token != 0) {
+		mtx_unlock(&pcb->pcb_mtx);
+		return (EBUSY);
+	}  
 
 	switch (cmd) {
 	case SIOC_L2CAP_NODE_GET_FLAGS: {
 		struct ng_btsocket_l2cap_raw_node_flags	*p =
 			(struct ng_btsocket_l2cap_raw_node_flags *) data;
 
-		ng_btsocket_l2cap_raw_get_token(&pcb->token);
-		pcb->msg = NULL;
-
-		NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_NODE_GET_FLAGS,
-			0, M_WAITOK);
-		if (msg == NULL) {
-			pcb->token = 0;
-			error = ENOMEM;
-			break;
-		}
-		msg->header.token = pcb->token;
-
-		NG_SEND_MSG_HOOK(error, ng_btsocket_l2cap_raw_node, msg,
-			rt->hook, NULL);
-		if (error != 0) {
-			pcb->token = 0;
-			break;
-		}
-
-		error = tsleep(&pcb->msg, PZERO|PCATCH, "l2ctl",
-				ng_btsocket_l2cap_raw_ioctl_timeout * hz);
-		if (error != 0) {
-			pcb->token = 0;
-			break;
-		}
-
-		if (pcb->msg != NULL &&
-		    pcb->msg->header.cmd == NGM_L2CAP_NODE_GET_FLAGS)
-			p->flags = *((ng_l2cap_node_flags_ep *)
-							(pcb->msg->data));
-		else
-			error = EINVAL;
-
-		NG_FREE_MSG(pcb->msg); /* checks for != NULL */
-		pcb->token = 0;
+		error = ng_btsocket_l2cap_raw_send_sync_ngmsg(pcb,
+				NGM_L2CAP_NODE_GET_FLAGS,
+				&p->flags, sizeof(p->flags));
 		} break;
 
 	case SIOC_L2CAP_NODE_GET_DEBUG: {
 		struct ng_btsocket_l2cap_raw_node_debug	*p =
 			(struct ng_btsocket_l2cap_raw_node_debug *) data;
 
-		ng_btsocket_l2cap_raw_get_token(&pcb->token);
-		pcb->msg = NULL;
-
-		NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_NODE_GET_DEBUG,
-			0, M_WAITOK);
-		if (msg == NULL) {
-			pcb->token = 0;
-			error = ENOMEM;
-			break;
-		}
-		msg->header.token = pcb->token;
-
-		NG_SEND_MSG_HOOK(error, ng_btsocket_l2cap_raw_node, msg,
-			rt->hook, NULL);
-		if (error != 0) {
-			pcb->token = 0;
-			break;
-		}
-
-		error = tsleep(&pcb->msg, PZERO|PCATCH, "l2ctl",
-				ng_btsocket_l2cap_raw_ioctl_timeout * hz);
-		if (error != 0) {
-			pcb->token = 0;
-			break;
-		}
-
-		if (pcb->msg != NULL &&
-		    pcb->msg->header.cmd == NGM_L2CAP_NODE_GET_DEBUG)
-			p->debug = *((ng_l2cap_node_debug_ep *)
-							(pcb->msg->data));
-		else
-			error = EINVAL;
-
-		NG_FREE_MSG(pcb->msg); /* checks for != NULL */
-		pcb->token = 0;
+		error = ng_btsocket_l2cap_raw_send_sync_ngmsg(pcb,
+				NGM_L2CAP_NODE_GET_DEBUG,
+				&p->debug, sizeof(p->debug));
 		} break;
 
 	case SIOC_L2CAP_NODE_SET_DEBUG: {
 		struct ng_btsocket_l2cap_raw_node_debug	*p = 
 			(struct ng_btsocket_l2cap_raw_node_debug *) data;
 
-		NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_NODE_SET_DEBUG,
-			sizeof(ng_l2cap_node_debug_ep), M_WAITOK);
-		if (msg == NULL) {
-			error = ENOMEM;
-			break;
-		}
-
-		*((ng_l2cap_node_debug_ep *)(msg->data)) = p->debug;
-		NG_SEND_MSG_HOOK(error, ng_btsocket_l2cap_raw_node,
-			msg, rt->hook, NULL);
+		if (pcb->flags & NG_BTSOCKET_L2CAP_RAW_PRIVILEGED)
+			error = ng_btsocket_l2cap_raw_send_ngmsg(pcb->rt->hook,
+					NGM_L2CAP_NODE_SET_DEBUG,
+					&p->debug, sizeof(p->debug));
+		else
+			error = EPERM;
 		} break;
 
 	case SIOC_L2CAP_NODE_GET_CON_LIST: {
@@ -860,32 +844,30 @@ ng_btsocket_l2cap_raw_control(struct socket *so, u_long cmd, caddr_t data,
 			error = EINVAL;
 			break;
 		}
- 
-		ng_btsocket_l2cap_raw_get_token(&pcb->token);
-		pcb->msg = NULL;
 
 		NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_NODE_GET_CON_LIST,
-			0, M_WAITOK);
+			0, M_NOWAIT);
 		if (msg == NULL) {
-			pcb->token = 0;
 			error = ENOMEM;
 			break;
 		}
-		msg->header.token = pcb->token;
+		ng_btsocket_l2cap_raw_get_token(&msg->header.token);
+		pcb->token = msg->header.token;
+		pcb->msg = NULL;
 
 		NG_SEND_MSG_HOOK(error, ng_btsocket_l2cap_raw_node, msg,
-			rt->hook, NULL);
+			pcb->rt->hook, NULL);
 		if (error != 0) {
 			pcb->token = 0;
 			break;
 		}
 
-		error = tsleep(&pcb->msg, PZERO|PCATCH, "l2ctl",
+		error = msleep(&pcb->msg, &pcb->pcb_mtx, PZERO|PCATCH, "l2ctl",
 				ng_btsocket_l2cap_raw_ioctl_timeout * hz);
-		if (error != 0) {
-			pcb->token = 0;
+		pcb->token = 0;
+
+		if (error != 0)
 			break;
-		}
 
 		if (pcb->msg != NULL &&
 		    pcb->msg->header.cmd == NGM_L2CAP_NODE_GET_CON_LIST) {
@@ -903,7 +885,6 @@ ng_btsocket_l2cap_raw_control(struct socket *so, u_long cmd, caddr_t data,
 			error = EINVAL;
 
 		NG_FREE_MSG(pcb->msg); /* checks for != NULL */
-		pcb->token = 0;
 		} break;
 
 	case SIOC_L2CAP_NODE_GET_CHAN_LIST: {
@@ -919,31 +900,29 @@ ng_btsocket_l2cap_raw_control(struct socket *so, u_long cmd, caddr_t data,
 			break;
 		}
  
-		ng_btsocket_l2cap_raw_get_token(&pcb->token);
-		pcb->msg = NULL;
-
 		NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE,
-			NGM_L2CAP_NODE_GET_CHAN_LIST, 0, M_WAITOK);
+			NGM_L2CAP_NODE_GET_CHAN_LIST, 0, M_NOWAIT);
 		if (msg == NULL) {
-			pcb->token = 0;
 			error = ENOMEM;
 			break;
 		}
-		msg->header.token = pcb->token;
+		ng_btsocket_l2cap_raw_get_token(&msg->header.token);
+		pcb->token = msg->header.token;
+		pcb->msg = NULL;
 
 		NG_SEND_MSG_HOOK(error, ng_btsocket_l2cap_raw_node, msg,
-			rt->hook, NULL);
+			pcb->rt->hook, NULL);
 		if (error != 0) {
 			pcb->token = 0;
 			break;
 		}
 
-		error = tsleep(&pcb->msg, PZERO|PCATCH, "l2ctl",
+		error = msleep(&pcb->msg, &pcb->pcb_mtx, PZERO|PCATCH, "l2ctl",
 				ng_btsocket_l2cap_raw_ioctl_timeout * hz);
-		if (error != 0) {
-			pcb->token = 0;
+		pcb->token = 0;
+
+		if (error != 0)
 			break;
-		}
 
 		if (pcb->msg != NULL &&
 		    pcb->msg->header.cmd == NGM_L2CAP_NODE_GET_CHAN_LIST) {
@@ -961,7 +940,6 @@ ng_btsocket_l2cap_raw_control(struct socket *so, u_long cmd, caddr_t data,
 			error = EINVAL;
 
 		NG_FREE_MSG(pcb->msg); /* checks for != NULL */
-		pcb->token = 0;
 		} break;
 
 	case SIOC_L2CAP_L2CA_PING: {
@@ -970,33 +948,30 @@ ng_btsocket_l2cap_raw_control(struct socket *so, u_long cmd, caddr_t data,
 		ng_l2cap_l2ca_ping_ip			*ip = NULL;
 		ng_l2cap_l2ca_ping_op			*op = NULL;
 
-		if ((p->echo_size != 0 && p->echo_data == NULL) ||
-		     p->echo_size > NG_L2CAP_MAX_ECHO_SIZE) {
-			error = EINVAL;
+		if (!(pcb->flags & NG_BTSOCKET_L2CAP_RAW_PRIVILEGED)) {
+			error = EPERM;
 			break;
 		}
 
-		/* Loop back local ping */
-		if (bcmp(&p->echo_dst, &rt->src, sizeof(rt->src)) == 0) {
-			p->result = 0;
+		if ((p->echo_size != 0 && p->echo_data == NULL) ||
+		     p->echo_size > NG_L2CAP_MAX_ECHO_SIZE) {
+			error = EINVAL;
 			break;
 		}
 
-		ng_btsocket_l2cap_raw_get_token(&pcb->token);
-		pcb->msg = NULL;
-
 		NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE,
 			NGM_L2CAP_L2CA_PING, sizeof(*ip) + p->echo_size,
-			M_WAITOK);
+			M_NOWAIT);
 		if (msg == NULL) {
-			pcb->token = 0;
 			error = ENOMEM;
 			break;
 		}
-		msg->header.token = pcb->token;
+		ng_btsocket_l2cap_raw_get_token(&msg->header.token);
+		pcb->token = msg->header.token;
+		pcb->msg = NULL;
 
 		ip = (ng_l2cap_l2ca_ping_ip *)(msg->data);
-		bcopy(&p->echo_dst, &ip->bdaddr, sizeof(ip->bdaddr));
+		bcopy(&pcb->dst, &ip->bdaddr, sizeof(ip->bdaddr));
 		ip->echo_size = p->echo_size;
 
 		if (ip->echo_size > 0) {
@@ -1009,18 +984,18 @@ ng_btsocket_l2cap_raw_control(struct socket *so, u_long cmd, caddr_t data,
 		}
 
 		NG_SEND_MSG_HOOK(error, ng_btsocket_l2cap_raw_node, msg,
-			rt->hook, NULL);
+			pcb->rt->hook, NULL);
 		if (error != 0) {
 			pcb->token = 0;
 			break;
 		}
 
-		error = tsleep(&pcb->msg, PZERO|PCATCH, "l2ctl",
+		error = msleep(&pcb->msg, &pcb->pcb_mtx, PZERO|PCATCH, "l2ctl",
 				bluetooth_l2cap_rtx_timeout());
-		if (error != 0) {
-			pcb->token = 0;
+		pcb->token = 0;
+
+		if (error != 0)
 			break;
-		}
 
 		if (pcb->msg != NULL &&
 		    pcb->msg->header.cmd == NGM_L2CAP_L2CA_PING) {
@@ -1036,7 +1011,6 @@ ng_btsocket_l2cap_raw_control(struct socket *so, u_long cmd, caddr_t data,
 			error = EINVAL;
 
 		NG_FREE_MSG(pcb->msg); /* checks for != NULL */
-		pcb->token = 0;
 		} break;
 
 	case SIOC_L2CAP_L2CA_GET_INFO: {
@@ -1045,42 +1019,44 @@ ng_btsocket_l2cap_raw_control(struct socket *so, u_long cmd, caddr_t data,
 		ng_l2cap_l2ca_get_info_ip		*ip = NULL;
 		ng_l2cap_l2ca_get_info_op		*op = NULL;
 
-		if ((p->info_size != 0 && p->info_data == NULL) ||
-		    bcmp(&p->info_dst, &rt->src, sizeof(rt->src)) == 0) {
-			error = EINVAL;
+		if (!(pcb->flags & NG_BTSOCKET_L2CAP_RAW_PRIVILEGED)) {
+			error = EPERM;
 			break;
 		}
 
-		ng_btsocket_l2cap_raw_get_token(&pcb->token);
-		pcb->msg = NULL;
+		if (p->info_size != 0 && p->info_data == NULL) {
+			error = EINVAL;
+			break;
+		}
 
 		NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE,
 			NGM_L2CAP_L2CA_GET_INFO, sizeof(*ip) + p->info_size,
-			M_WAITOK);
+			M_NOWAIT);
 		if (msg == NULL) {
-			pcb->token = 0;
 			error = ENOMEM;
 			break;
 		}
-		msg->header.token = pcb->token;
+		ng_btsocket_l2cap_raw_get_token(&msg->header.token);
+		pcb->token = msg->header.token;
+		pcb->msg = NULL;
 
 		ip = (ng_l2cap_l2ca_get_info_ip *)(msg->data);
-		bcopy(&p->info_dst, &ip->bdaddr, sizeof(ip->bdaddr));
+		bcopy(&pcb->dst, &ip->bdaddr, sizeof(ip->bdaddr));
 		ip->info_type = p->info_type;
 
 		NG_SEND_MSG_HOOK(error, ng_btsocket_l2cap_raw_node, msg,
-			rt->hook, NULL);
+			pcb->rt->hook, NULL);
 		if (error != 0) {
 			pcb->token = 0;
 			break;
 		}
 
-		error = tsleep(&pcb->msg, PZERO|PCATCH, "l2ctl",
+		error = msleep(&pcb->msg, &pcb->pcb_mtx, PZERO|PCATCH, "l2ctl",
 				bluetooth_l2cap_rtx_timeout());
-		if (error != 0) {
-			pcb->token = 0;
+		pcb->token = 0;
+
+		if (error != 0)
 			break;
-		}
 
 		if (pcb->msg != NULL &&
 		    pcb->msg->header.cmd == NGM_L2CAP_L2CA_GET_INFO) {
@@ -1096,7 +1072,27 @@ ng_btsocket_l2cap_raw_control(struct socket *so, u_long cmd, caddr_t data,
 			error = EINVAL;
 
 		NG_FREE_MSG(pcb->msg); /* checks for != NULL */
-		pcb->token = 0;
+		} break;
+
+	case SIOC_L2CAP_NODE_GET_AUTO_DISCON_TIMO: {
+		struct ng_btsocket_l2cap_raw_auto_discon_timo	*p =
+			(struct ng_btsocket_l2cap_raw_auto_discon_timo *) data;
+
+		error = ng_btsocket_l2cap_raw_send_sync_ngmsg(pcb,
+				NGM_L2CAP_NODE_GET_AUTO_DISCON_TIMO,
+				&p->timeout, sizeof(p->timeout));
+		} break;
+
+	case SIOC_L2CAP_NODE_SET_AUTO_DISCON_TIMO: {
+		struct ng_btsocket_l2cap_raw_auto_discon_timo	*p =
+			(struct ng_btsocket_l2cap_raw_auto_discon_timo *) data;
+
+		if (pcb->flags & NG_BTSOCKET_L2CAP_RAW_PRIVILEGED)
+			error = ng_btsocket_l2cap_raw_send_ngmsg(pcb->rt->hook,
+					NGM_L2CAP_NODE_SET_AUTO_DISCON_TIMO,
+					&p->timeout, sizeof(p->timeout));
+		else
+			error = EPERM;
 		} break;
 
 	default:
@@ -1104,6 +1100,8 @@ ng_btsocket_l2cap_raw_control(struct socket *so, u_long cmd, caddr_t data,
 		break;
 	}
 
+	mtx_unlock(&pcb->pcb_mtx);
+
 	return (error);
 } /* ng_btsocket_l2cap_raw_control */
 
@@ -1121,16 +1119,22 @@ ng_btsocket_l2cap_raw_detach(struct socket *so)
 	if (ng_btsocket_l2cap_raw_node == NULL) 
 		return (EINVAL);
 
-	so->so_pcb = NULL;
-	sotryfree(so);
-
 	mtx_lock(&ng_btsocket_l2cap_raw_sockets_mtx);
+	mtx_lock(&pcb->pcb_mtx);
+
 	LIST_REMOVE(pcb, next);
+
+	mtx_unlock(&pcb->pcb_mtx);
 	mtx_unlock(&ng_btsocket_l2cap_raw_sockets_mtx);
 
+	mtx_destroy(&pcb->pcb_mtx);
+
 	bzero(pcb, sizeof(*pcb));
 	FREE(pcb, M_NETGRAPH_BTSOCKET_L2CAP_RAW);
 
+	so->so_pcb = NULL;
+	sotryfree(so);
+
 	return (0);
 } /* ng_btsocket_l2cap_raw_detach */
 
@@ -1148,8 +1152,10 @@ ng_btsocket_l2cap_raw_disconnect(struct socket *so)
 	if (ng_btsocket_l2cap_raw_node == NULL) 
 		return (EINVAL);
 
+	mtx_lock(&pcb->pcb_mtx);
 	pcb->rt = NULL;
 	soisdisconnected(so);
+	mtx_unlock(&pcb->pcb_mtx);
 
 	return (0);
 } /* ng_btsocket_l2cap_raw_disconnect */
@@ -1161,7 +1167,22 @@ ng_btsocket_l2cap_raw_disconnect(struct socket *so)
 int
 ng_btsocket_l2cap_raw_peeraddr(struct socket *so, struct sockaddr **nam)
 {
-	return (EOPNOTSUPP);
+	ng_btsocket_l2cap_raw_pcb_p	pcb = so2l2cap_raw_pcb(so);
+	struct sockaddr_l2cap		sa;
+
+	if (pcb == NULL)
+		return (EINVAL);
+	if (ng_btsocket_l2cap_raw_node == NULL) 
+		return (EINVAL);
+
+	bcopy(&pcb->dst, &sa.l2cap_bdaddr, sizeof(sa.l2cap_bdaddr));
+	sa.l2cap_psm = 0;
+	sa.l2cap_len = sizeof(sa);
+	sa.l2cap_family = AF_BLUETOOTH;
+
+	*nam = dup_sockaddr((struct sockaddr *) &sa, 0);
+
+	return ((*nam == NULL)? ENOMEM : 0);
 } /* ng_btsocket_l2cap_raw_peeraddr */
 
 /*
@@ -1220,3 +1241,70 @@ ng_btsocket_l2cap_raw_get_token(u_int32_t *token)
 	mtx_unlock(&ng_btsocket_l2cap_raw_token_mtx);
 } /* ng_btsocket_l2cap_raw_get_token */
 
+/*
+ * Send Netgraph message to the node - do not expect reply
+ */
+
+static int
+ng_btsocket_l2cap_raw_send_ngmsg(hook_p hook, int cmd, void *arg, int arglen)
+{
+	struct ng_mesg	*msg = NULL;
+	int		 error = 0;
+
+	NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, cmd, arglen, M_NOWAIT);
+	if (msg == NULL)
+		return (ENOMEM);
+
+	if (arg != NULL && arglen > 0)
+		bcopy(arg, msg->data, arglen);
+
+	NG_SEND_MSG_HOOK(error, ng_btsocket_l2cap_raw_node, msg, hook, NULL);
+
+	return (error);
+} /* ng_btsocket_l2cap_raw_send_ngmsg */
+
+/*
+ * Send Netgraph message to the node (no data) and wait for reply
+ */
+
+static int
+ng_btsocket_l2cap_raw_send_sync_ngmsg(ng_btsocket_l2cap_raw_pcb_p pcb,
+		int cmd, void *rsp, int rsplen)
+{
+	struct ng_mesg	*msg = NULL;
+	int		 error = 0;
+
+	mtx_assert(&pcb->pcb_mtx, MA_OWNED);
+
+	NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, cmd, 0, M_NOWAIT);
+	if (msg == NULL)
+		return (ENOMEM);
+
+	ng_btsocket_l2cap_raw_get_token(&msg->header.token);
+	pcb->token = msg->header.token;
+	pcb->msg = NULL;
+
+	NG_SEND_MSG_HOOK(error, ng_btsocket_l2cap_raw_node, msg, 
+		pcb->rt->hook, NULL);
+	if (error != 0) {
+		pcb->token = 0;
+		return (error);
+	}
+
+	error = msleep(&pcb->msg, &pcb->pcb_mtx, PZERO|PCATCH, "l2ctl",
+			ng_btsocket_l2cap_raw_ioctl_timeout * hz);
+	pcb->token = 0;
+
+	if (error != 0)
+		return (error);
+
+	if (pcb->msg != NULL && pcb->msg->header.cmd == cmd)
+		bcopy(pcb->msg->data, rsp, rsplen);
+	else
+		error = EINVAL;
+
+	NG_FREE_MSG(pcb->msg); /* checks for != NULL */
+
+	return (0);
+} /* ng_btsocket_l2cap_raw_send_sync_ngmsg */
+
diff --git a/sys/netgraph/bluetooth/socket/ng_btsocket_rfcomm.c b/sys/netgraph/bluetooth/socket/ng_btsocket_rfcomm.c
new file mode 100644
index 0000000..4565e91
--- /dev/null
+++ b/sys/netgraph/bluetooth/socket/ng_btsocket_rfcomm.c
@@ -0,0 +1,3570 @@
+/*
+ * ng_btsocket_rfcomm.c
+ *
+ * Copyright (c) 2001-2003 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_btsocket_rfcomm.c,v 1.24 2003/04/07 01:37:05 max Exp $
+ * $FreeBSD$
+ */
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/domain.h>
+#include <sys/endian.h>
+#include <sys/errno.h>
+#include <sys/filedesc.h>
+#include <sys/ioccom.h>
+#include <sys/kernel.h>
+#include <sys/lock.h>
+#include <sys/malloc.h>
+#include <sys/mbuf.h>
+#include <sys/mutex.h>
+#include <sys/proc.h>
+#include <sys/protosw.h>
+#include <sys/queue.h>
+#include <sys/socket.h>
+#include <sys/socketvar.h>
+#include <sys/sysctl.h>
+#include <sys/taskqueue.h>
+#include <sys/uio.h>
+#include <netgraph/ng_message.h>
+#include <netgraph/netgraph.h>
+#include <bitstring.h>
+#include "ng_bluetooth.h"
+#include "ng_hci.h"
+#include "ng_l2cap.h"
+#include "ng_btsocket.h"
+#include "ng_btsocket_l2cap.h"
+#include "ng_btsocket_rfcomm.h"
+
+/* MALLOC define */
+#ifdef NG_SEPARATE_MALLOC
+MALLOC_DEFINE(M_NETGRAPH_BTSOCKET_RFCOMM, "netgraph_btsocks_rfcomm",
+		"Netgraph Bluetooth RFCOMM sockets");
+#else
+#define M_NETGRAPH_BTSOCKET_RFCOMM M_NETGRAPH
+#endif /* NG_SEPARATE_MALLOC */
+
+/* Debug */
+#define NG_BTSOCKET_RFCOMM_INFO \
+	if (ng_btsocket_rfcomm_debug_level >= NG_BTSOCKET_INFO_LEVEL) \
+		printf
+
+#define NG_BTSOCKET_RFCOMM_WARN \
+	if (ng_btsocket_rfcomm_debug_level >= NG_BTSOCKET_WARN_LEVEL) \
+		printf
+
+#define NG_BTSOCKET_RFCOMM_ERR \
+	if (ng_btsocket_rfcomm_debug_level >= NG_BTSOCKET_ERR_LEVEL) \
+		printf
+
+#define NG_BTSOCKET_RFCOMM_ALERT \
+	if (ng_btsocket_rfcomm_debug_level >= NG_BTSOCKET_ALERT_LEVEL) \
+		printf
+
+#define	ALOT	0x7fff
+
+/* Local prototypes */
+static void ng_btsocket_rfcomm_upcall
+	(struct socket *so, void *arg, int waitflag);
+static void ng_btsocket_rfcomm_sessions_task
+	(void *ctx, int pending);
+static void ng_btsocket_rfcomm_session_task
+	(ng_btsocket_rfcomm_session_p s);
+#define ng_btsocket_rfcomm_task_wakeup() \
+	taskqueue_enqueue(taskqueue_swi_giant, &ng_btsocket_rfcomm_task)
+
+static ng_btsocket_rfcomm_pcb_p ng_btsocket_rfcomm_connect_ind
+	(ng_btsocket_rfcomm_session_p s, int channel);
+static void ng_btsocket_rfcomm_connect_cfm
+	(ng_btsocket_rfcomm_session_p s);
+
+static int ng_btsocket_rfcomm_session_create
+	(ng_btsocket_rfcomm_session_p *sp, struct socket *l2so,
+	 bdaddr_p src, bdaddr_p dst, struct thread *td);
+static int ng_btsocket_rfcomm_session_accept
+	(ng_btsocket_rfcomm_session_p s0);
+static int ng_btsocket_rfcomm_session_connect
+	(ng_btsocket_rfcomm_session_p s);
+static int ng_btsocket_rfcomm_session_receive
+	(ng_btsocket_rfcomm_session_p s);
+static int ng_btsocket_rfcomm_session_send
+	(ng_btsocket_rfcomm_session_p s);
+static void ng_btsocket_rfcomm_session_clean
+	(ng_btsocket_rfcomm_session_p s);
+static void ng_btsocket_rfcomm_session_process_pcb
+	(ng_btsocket_rfcomm_session_p s);
+static ng_btsocket_rfcomm_session_p ng_btsocket_rfcomm_session_by_addr
+	(bdaddr_p src, bdaddr_p dst);
+
+static int ng_btsocket_rfcomm_receive_frame
+	(ng_btsocket_rfcomm_session_p s, struct mbuf *m0);
+static int ng_btsocket_rfcomm_receive_sabm
+	(ng_btsocket_rfcomm_session_p s, int dlci);
+static int ng_btsocket_rfcomm_receive_disc
+	(ng_btsocket_rfcomm_session_p s, int dlci);
+static int ng_btsocket_rfcomm_receive_ua
+	(ng_btsocket_rfcomm_session_p s, int dlci);
+static int ng_btsocket_rfcomm_receive_dm
+	(ng_btsocket_rfcomm_session_p s, int dlci);
+static int ng_btsocket_rfcomm_receive_uih
+	(ng_btsocket_rfcomm_session_p s, int dlci, int pf, struct mbuf *m0);
+static int ng_btsocket_rfcomm_receive_mcc
+	(ng_btsocket_rfcomm_session_p s, struct mbuf *m0);
+static int ng_btsocket_rfcomm_receive_test
+	(ng_btsocket_rfcomm_session_p s, struct mbuf *m0);
+static int ng_btsocket_rfcomm_receive_fc
+	(ng_btsocket_rfcomm_session_p s, struct mbuf *m0);
+static int ng_btsocket_rfcomm_receive_msc
+	(ng_btsocket_rfcomm_session_p s, struct mbuf *m0);
+static int ng_btsocket_rfcomm_receive_rpn
+	(ng_btsocket_rfcomm_session_p s, struct mbuf *m0);
+static int ng_btsocket_rfcomm_receive_rls
+	(ng_btsocket_rfcomm_session_p s, struct mbuf *m0);
+static int ng_btsocket_rfcomm_receive_pn
+	(ng_btsocket_rfcomm_session_p s, struct mbuf *m0);
+static void ng_btsocket_rfcomm_set_pn
+	(ng_btsocket_rfcomm_pcb_p pcb, u_int8_t cr, u_int8_t flow_control, 
+	 u_int8_t credits, u_int16_t mtu);
+
+static int ng_btsocket_rfcomm_send_command
+	(ng_btsocket_rfcomm_session_p s, u_int8_t type, u_int8_t dlci);
+static int ng_btsocket_rfcomm_send_uih
+	(ng_btsocket_rfcomm_session_p s, u_int8_t address, u_int8_t pf, 
+	 u_int8_t credits, struct mbuf *data);
+static int ng_btsocket_rfcomm_send_msc
+	(ng_btsocket_rfcomm_pcb_p pcb);
+static int ng_btsocket_rfcomm_send_pn
+	(ng_btsocket_rfcomm_pcb_p pcb);
+static int ng_btsocket_rfcomm_send_credits
+	(ng_btsocket_rfcomm_pcb_p pcb);
+
+static int ng_btsocket_rfcomm_pcb_send
+	(ng_btsocket_rfcomm_pcb_p pcb, int limit);
+static int ng_btsocket_rfcomm_pcb_kill
+	(ng_btsocket_rfcomm_pcb_p pcb, int error);
+static ng_btsocket_rfcomm_pcb_p ng_btsocket_rfcomm_pcb_by_channel
+	(bdaddr_p src, int channel);
+static ng_btsocket_rfcomm_pcb_p ng_btsocket_rfcomm_pcb_by_dlci
+	(ng_btsocket_rfcomm_session_p s, int dlci);
+static ng_btsocket_rfcomm_pcb_p ng_btsocket_rfcomm_pcb_listener
+	(bdaddr_p src, int channel);
+
+static void ng_btsocket_rfcomm_timeout
+	(ng_btsocket_rfcomm_pcb_p pcb);
+static void ng_btsocket_rfcomm_untimeout
+	(ng_btsocket_rfcomm_pcb_p pcb);
+static void ng_btsocket_rfcomm_process_timeout
+	(void *xpcb);
+
+static struct mbuf * ng_btsocket_rfcomm_prepare_packet
+	(struct sockbuf *sb, int length);
+
+/* Globals */
+extern int					ifqmaxlen;
+static u_int32_t				ng_btsocket_rfcomm_debug_level;
+static u_int32_t				ng_btsocket_rfcomm_timo;
+struct task					ng_btsocket_rfcomm_task;
+static LIST_HEAD(, ng_btsocket_rfcomm_session)	ng_btsocket_rfcomm_sessions;
+static struct mtx				ng_btsocket_rfcomm_sessions_mtx;
+static LIST_HEAD(, ng_btsocket_rfcomm_pcb)	ng_btsocket_rfcomm_sockets;
+static struct mtx				ng_btsocket_rfcomm_sockets_mtx;
+
+/* Sysctl tree */
+SYSCTL_DECL(_net_bluetooth_rfcomm_sockets);
+SYSCTL_NODE(_net_bluetooth_rfcomm_sockets, OID_AUTO, stream, CTLFLAG_RW,
+	0, "Bluetooth STREAM RFCOMM sockets family");
+SYSCTL_INT(_net_bluetooth_rfcomm_sockets_stream, OID_AUTO, debug_level,
+	CTLFLAG_RW,
+	&ng_btsocket_rfcomm_debug_level, NG_BTSOCKET_INFO_LEVEL,
+	"Bluetooth STREAM RFCOMM sockets debug level");
+SYSCTL_INT(_net_bluetooth_rfcomm_sockets_stream, OID_AUTO, timeout,
+	CTLFLAG_RW,
+	&ng_btsocket_rfcomm_timo, 60,
+	"Bluetooth STREAM RFCOMM sockets timeout");
+
+/*****************************************************************************
+ *****************************************************************************
+ **                              RFCOMM CRC
+ *****************************************************************************
+ *****************************************************************************/
+
+static u_int8_t	ng_btsocket_rfcomm_crc_table[256] = {
+	0x00, 0x91, 0xe3, 0x72, 0x07, 0x96, 0xe4, 0x75,
+	0x0e, 0x9f, 0xed, 0x7c, 0x09, 0x98, 0xea, 0x7b,
+	0x1c, 0x8d, 0xff, 0x6e, 0x1b, 0x8a, 0xf8, 0x69,
+	0x12, 0x83, 0xf1, 0x60, 0x15, 0x84, 0xf6, 0x67,
+
+	0x38, 0xa9, 0xdb, 0x4a, 0x3f, 0xae, 0xdc, 0x4d,
+	0x36, 0xa7, 0xd5, 0x44, 0x31, 0xa0, 0xd2, 0x43,
+	0x24, 0xb5, 0xc7, 0x56, 0x23, 0xb2, 0xc0, 0x51,
+	0x2a, 0xbb, 0xc9, 0x58, 0x2d, 0xbc, 0xce, 0x5f,
+
+	0x70, 0xe1, 0x93, 0x02, 0x77, 0xe6, 0x94, 0x05,
+	0x7e, 0xef, 0x9d, 0x0c, 0x79, 0xe8, 0x9a, 0x0b,
+	0x6c, 0xfd, 0x8f, 0x1e, 0x6b, 0xfa, 0x88, 0x19,
+	0x62, 0xf3, 0x81, 0x10, 0x65, 0xf4, 0x86, 0x17,
+
+	0x48, 0xd9, 0xab, 0x3a, 0x4f, 0xde, 0xac, 0x3d,
+	0x46, 0xd7, 0xa5, 0x34, 0x41, 0xd0, 0xa2, 0x33,
+	0x54, 0xc5, 0xb7, 0x26, 0x53, 0xc2, 0xb0, 0x21,
+	0x5a, 0xcb, 0xb9, 0x28, 0x5d, 0xcc, 0xbe, 0x2f,
+
+	0xe0, 0x71, 0x03, 0x92, 0xe7, 0x76, 0x04, 0x95,
+	0xee, 0x7f, 0x0d, 0x9c, 0xe9, 0x78, 0x0a, 0x9b,
+	0xfc, 0x6d, 0x1f, 0x8e, 0xfb, 0x6a, 0x18, 0x89,
+	0xf2, 0x63, 0x11, 0x80, 0xf5, 0x64, 0x16, 0x87,
+
+	0xd8, 0x49, 0x3b, 0xaa, 0xdf, 0x4e, 0x3c, 0xad,
+	0xd6, 0x47, 0x35, 0xa4, 0xd1, 0x40, 0x32, 0xa3,
+	0xc4, 0x55, 0x27, 0xb6, 0xc3, 0x52, 0x20, 0xb1,
+	0xca, 0x5b, 0x29, 0xb8, 0xcd, 0x5c, 0x2e, 0xbf,
+
+	0x90, 0x01, 0x73, 0xe2, 0x97, 0x06, 0x74, 0xe5,
+	0x9e, 0x0f, 0x7d, 0xec, 0x99, 0x08, 0x7a, 0xeb,
+	0x8c, 0x1d, 0x6f, 0xfe, 0x8b, 0x1a, 0x68, 0xf9,
+	0x82, 0x13, 0x61, 0xf0, 0x85, 0x14, 0x66, 0xf7,
+
+	0xa8, 0x39, 0x4b, 0xda, 0xaf, 0x3e, 0x4c, 0xdd,
+	0xa6, 0x37, 0x45, 0xd4, 0xa1, 0x30, 0x42, 0xd3,
+	0xb4, 0x25, 0x57, 0xc6, 0xb3, 0x22, 0x50, 0xc1,
+	0xba, 0x2b, 0x59, 0xc8, 0xbd, 0x2c, 0x5e, 0xcf
+};
+
+/* CRC */
+static u_int8_t
+ng_btsocket_rfcomm_crc(u_int8_t *data, int length)
+{
+	u_int8_t	crc = 0xff;
+
+	while (length --)
+		crc = ng_btsocket_rfcomm_crc_table[crc ^ *data++];
+
+	return (crc);
+} /* ng_btsocket_rfcomm_crc */
+
+/* FCS on 2 bytes */
+static u_int8_t
+ng_btsocket_rfcomm_fcs2(u_int8_t *data)
+{
+	return (0xff - ng_btsocket_rfcomm_crc(data, 2));
+} /* ng_btsocket_rfcomm_fcs2 */
+  
+/* FCS on 3 bytes */
+static u_int8_t
+ng_btsocket_rfcomm_fcs3(u_int8_t *data)
+{
+	return (0xff - ng_btsocket_rfcomm_crc(data, 3));
+} /* ng_btsocket_rfcomm_fcs3 */
+
+/* 
+ * Check FCS
+ *
+ * From Bluetooth spec
+ *
+ * "... In 07.10, the frame check sequence (FCS) is calculated on different 
+ * sets of fields for different frame types. These are the fields that the 
+ * FCS are calculated on:
+ *
+ * For SABM, DISC, UA, DM frames: on Address, Control and length field.
+ * For UIH frames: on Address and Control field.
+ *
+ * (This is stated here for clarification, and to set the standard for RFCOMM;
+ * the fields included in FCS calculation have actually changed in version
+ * 7.0.0 of TS 07.10, but RFCOMM will not change the FCS calculation scheme
+ * from the one above.) ..."
+ */
+
+static int
+ng_btsocket_rfcomm_check_fcs(u_int8_t *data, int type, u_int8_t fcs)
+{
+	if (type != RFCOMM_FRAME_UIH)
+		return (ng_btsocket_rfcomm_fcs3(data) != fcs);
+
+	return (ng_btsocket_rfcomm_fcs2(data) != fcs);
+} /* ng_btsocket_rfcomm_check_fcs */
+
+/*****************************************************************************
+ *****************************************************************************
+ **                              Socket interface
+ *****************************************************************************
+ *****************************************************************************/
+
+/* 
+ * Initialize everything
+ */
+
+void
+ng_btsocket_rfcomm_init(void)
+{
+	ng_btsocket_rfcomm_debug_level = NG_BTSOCKET_WARN_LEVEL;
+	ng_btsocket_rfcomm_timo = 60;
+
+	/* RFCOMM task */
+	TASK_INIT(&ng_btsocket_rfcomm_task, 0,
+		ng_btsocket_rfcomm_sessions_task, NULL);
+
+	/* RFCOMM sessions list */
+	LIST_INIT(&ng_btsocket_rfcomm_sessions);
+	mtx_init(&ng_btsocket_rfcomm_sessions_mtx,
+		"btsocks_rfcomm_sessions_mtx", NULL, MTX_DEF);
+
+	/* RFCOMM sockets list */
+	LIST_INIT(&ng_btsocket_rfcomm_sockets);
+	mtx_init(&ng_btsocket_rfcomm_sockets_mtx,
+		"btsocks_rfcomm_sockets_mtx", NULL, MTX_DEF);
+} /* ng_btsocket_rfcomm_init */
+
+/*
+ * Abort connection on socket
+ */
+
+int
+ng_btsocket_rfcomm_abort(struct socket *so)
+{
+	so->so_error = ECONNABORTED;
+
+	return (ng_btsocket_rfcomm_detach(so));
+} /* ng_btsocket_rfcomm_abort */
+
+/*
+ * Accept connection on socket. Nothing to do here, socket must be connected
+ * and ready, so just return peer address and be done with it.
+ */
+
+int
+ng_btsocket_rfcomm_accept(struct socket *so, struct sockaddr **nam)
+{
+	return (ng_btsocket_rfcomm_peeraddr(so, nam));
+} /* ng_btsocket_rfcomm_accept */
+
+/*
+ * Create and attach new socket
+ */
+
+int
+ng_btsocket_rfcomm_attach(struct socket *so, int proto, struct thread *td)
+{
+	ng_btsocket_rfcomm_pcb_p	pcb = so2rfcomm_pcb(so);
+	int				error;
+
+	/* Check socket and protocol */
+	if (so->so_type != SOCK_STREAM)
+		return (ESOCKTNOSUPPORT);
+
+#if 0 /* XXX sonewconn() calls "pru_attach" with proto == 0 */
+	if (proto != 0) 
+		if (proto != BLUETOOTH_PROTO_RFCOMM)
+			return (EPROTONOSUPPORT);
+#endif /* XXX */
+
+	if (pcb != NULL)
+		return (EISCONN);
+
+	/* Reserve send and receive space if it is not reserved yet */
+	if ((so->so_snd.sb_hiwat == 0) || (so->so_rcv.sb_hiwat == 0)) {
+		error = soreserve(so, NG_BTSOCKET_RFCOMM_SENDSPACE,
+					NG_BTSOCKET_RFCOMM_RECVSPACE);
+		if (error != 0)
+			return (error);
+	}
+
+	/* Allocate the PCB */
+        MALLOC(pcb, ng_btsocket_rfcomm_pcb_p, sizeof(*pcb),
+		M_NETGRAPH_BTSOCKET_RFCOMM, M_NOWAIT | M_ZERO);
+        if (pcb == NULL)
+                return (ENOMEM);
+
+	/* Link the PCB and the socket */
+	so->so_pcb = (caddr_t) pcb;
+	pcb->so = so;
+
+	/* Initialize PCB */
+	pcb->state = NG_BTSOCKET_RFCOMM_DLC_CLOSED;
+	pcb->flags = NG_BTSOCKET_RFCOMM_DLC_CFC;
+
+	pcb->lmodem =
+	pcb->rmodem = (RFCOMM_MODEM_RTC | RFCOMM_MODEM_RTR | RFCOMM_MODEM_DV);
+
+	pcb->mtu = RFCOMM_DEFAULT_MTU;
+	pcb->tx_cred = 0;
+	pcb->rx_cred = RFCOMM_DEFAULT_CREDITS;
+
+	mtx_init(&pcb->pcb_mtx, "btsocks_rfcomm_pcb_mtx", NULL, MTX_DEF);
+	callout_handle_init(&pcb->timo);
+
+	/* Add the PCB to the list */
+	mtx_lock(&ng_btsocket_rfcomm_sockets_mtx);
+	LIST_INSERT_HEAD(&ng_btsocket_rfcomm_sockets, pcb, next);
+	mtx_unlock(&ng_btsocket_rfcomm_sockets_mtx);
+
+        return (0);
+} /* ng_btsocket_rfcomm_attach */
+
+/*
+ * Bind socket
+ */
+
+int
+ng_btsocket_rfcomm_bind(struct socket *so, struct sockaddr *nam, 
+		struct thread *td)
+{
+	ng_btsocket_rfcomm_pcb_t	*pcb = so2rfcomm_pcb(so);
+	struct sockaddr_rfcomm		*sa = (struct sockaddr_rfcomm *) nam;
+
+	if (pcb == NULL)
+		return (EINVAL);
+
+	/* Verify address */
+	if (sa == NULL)
+		return (EINVAL);
+	if (sa->rfcomm_family != AF_BLUETOOTH)
+		return (EAFNOSUPPORT);
+	if (sa->rfcomm_len != sizeof(*sa))
+		return (EINVAL);
+	if (sa->rfcomm_channel > 30)
+		return (EINVAL);
+	if (sa->rfcomm_channel != 0 &&
+	    ng_btsocket_rfcomm_pcb_by_channel(&sa->rfcomm_bdaddr, sa->rfcomm_channel) != NULL)
+		return (EADDRINUSE);
+
+	bcopy(&sa->rfcomm_bdaddr, &pcb->src, sizeof(pcb->src));
+	pcb->channel = sa->rfcomm_channel;
+
+	return (0);
+} /* ng_btsocket_rfcomm_bind */
+
+/*
+ * Connect socket
+ */
+
+int
+ng_btsocket_rfcomm_connect(struct socket *so, struct sockaddr *nam, 
+		struct thread *td)
+{
+	ng_btsocket_rfcomm_pcb_t	*pcb = so2rfcomm_pcb(so);
+	struct sockaddr_rfcomm		*sa = (struct sockaddr_rfcomm *) nam;
+	ng_btsocket_rfcomm_session_t	*s = NULL;
+	struct socket			*l2so = NULL;
+	int				 dlci, error = 0;
+
+	if (pcb == NULL)
+		return (EINVAL);
+
+	/* Verify address */
+	if (sa == NULL)
+		return (EINVAL);
+	if (sa->rfcomm_family != AF_BLUETOOTH)
+		return (EAFNOSUPPORT);
+	if (sa->rfcomm_len != sizeof(*sa))
+		return (EINVAL);
+	if (sa->rfcomm_channel > 30)
+		return (EINVAL);
+	if (sa->rfcomm_channel == 0 ||
+	    bcmp(&sa->rfcomm_bdaddr, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0)
+		return (EDESTADDRREQ);
+
+	/*
+	 * XXX FIXME - This is FUBAR. socreate() will call soalloc(1), i.e.
+	 * soalloc() is allowed to sleep in MALLOC. This creates "could sleep"
+	 * WITNESS warnings. To work around this problem we will create L2CAP
+	 * socket first and then check if we actually need it. Note that we 
+	 * will not check for errors in socreate() because if we failed to 
+	 * create L2CAP socket at this point we still might have already open
+	 * session.
+	 */
+
+	error = socreate(PF_BLUETOOTH, &l2so, SOCK_SEQPACKET,
+			BLUETOOTH_PROTO_L2CAP, td->td_ucred, td);
+
+	/* 
+	 * Look for session between "pcb->src" and "sa->rfcomm_bdaddr" (dst)
+	 */
+
+	mtx_lock(&ng_btsocket_rfcomm_sessions_mtx);
+
+	s = ng_btsocket_rfcomm_session_by_addr(&pcb->src, &sa->rfcomm_bdaddr);
+	if (s == NULL) {
+		/*
+		 * We need to create new RFCOMM session. Check if we have L2CAP
+		 * socket. If l2so == NULL then error has the error code from
+		 * socreate()
+		 */
+
+		if (l2so == NULL) {
+			mtx_unlock(&ng_btsocket_rfcomm_sessions_mtx);
+			return (error);
+		}
+
+		error = ng_btsocket_rfcomm_session_create(&s, l2so,
+				&pcb->src, &sa->rfcomm_bdaddr, td);
+		if (error != 0) {
+			mtx_unlock(&ng_btsocket_rfcomm_sessions_mtx);
+			soclose(l2so);
+
+			return (error);
+		}
+	} else if (l2so != NULL)
+		soclose(l2so); /* we don't need new L2CAP socket */
+
+	/*
+	 * Check if we already have the same DLCI the the same session
+	 */
+
+	mtx_lock(&s->session_mtx);
+	mtx_lock(&pcb->pcb_mtx);
+
+	dlci = RFCOMM_MKDLCI(!INITIATOR(s), sa->rfcomm_channel);
+
+	if (ng_btsocket_rfcomm_pcb_by_dlci(s, dlci) != NULL) {
+		mtx_unlock(&pcb->pcb_mtx);
+		mtx_unlock(&s->session_mtx);
+		mtx_unlock(&ng_btsocket_rfcomm_sessions_mtx);
+
+		return (EBUSY);
+	}
+
+	/*
+	 * Check session state and if its not acceptable then refuse connection
+	 */
+
+	switch (s->state) {
+	case NG_BTSOCKET_RFCOMM_SESSION_CONNECTING:
+	case NG_BTSOCKET_RFCOMM_SESSION_CONNECTED:
+	case NG_BTSOCKET_RFCOMM_SESSION_OPEN:
+		/*
+		 * Update destination address and channel and attach 
+		 * DLC to the session
+		 */
+
+		bcopy(&sa->rfcomm_bdaddr, &pcb->dst, sizeof(pcb->dst));
+		pcb->channel = sa->rfcomm_channel;
+		pcb->dlci = dlci;
+
+		LIST_INSERT_HEAD(&s->dlcs, pcb, session_next);
+		pcb->session = s;
+
+		ng_btsocket_rfcomm_timeout(pcb);
+		soisconnecting(pcb->so);
+
+		if (s->state == NG_BTSOCKET_RFCOMM_SESSION_OPEN) {
+			pcb->mtu = s->mtu;
+			bcopy(&so2l2cap_pcb(s->l2so)->src, &pcb->src,
+				sizeof(pcb->src));
+
+			pcb->state = NG_BTSOCKET_RFCOMM_DLC_CONFIGURING;
+
+			error = ng_btsocket_rfcomm_send_pn(pcb);
+			if (error == 0)
+				error = ng_btsocket_rfcomm_task_wakeup();
+		} else
+			pcb->state = NG_BTSOCKET_RFCOMM_DLC_W4_CONNECT;
+		break;
+
+	default:
+		error = ECONNRESET;
+		break;
+	}
+
+	mtx_unlock(&pcb->pcb_mtx);
+	mtx_unlock(&s->session_mtx);
+	mtx_unlock(&ng_btsocket_rfcomm_sessions_mtx);
+
+	return (error);
+} /* ng_btsocket_rfcomm_connect */
+
+/*
+ * Process ioctl's calls on socket.
+ * XXX FIXME this should provide interface to the RFCOMM multiplexor channel
+ */
+
+int
+ng_btsocket_rfcomm_control(struct socket *so, u_long cmd, caddr_t data,
+		struct ifnet *ifp, struct thread *td)
+{
+	return (EINVAL);
+} /* ng_btsocket_rfcomm_control */
+
+/*
+ * Process getsockopt/setsockopt system calls
+ */
+
+int
+ng_btsocket_rfcomm_ctloutput(struct socket *so, struct sockopt *sopt)
+{
+	ng_btsocket_rfcomm_pcb_p		pcb = so2rfcomm_pcb(so);
+	struct ng_btsocket_rfcomm_fc_info	fcinfo;
+	int					error = 0;
+
+	if (pcb == NULL)
+		return (EINVAL);
+	if (sopt->sopt_level != SOL_RFCOMM)
+		return (0);
+
+	mtx_lock(&pcb->pcb_mtx);
+
+	switch (sopt->sopt_dir) {
+	case SOPT_GET:
+		switch (sopt->sopt_name) {
+		case SO_RFCOMM_MTU:
+			error = sooptcopyout(sopt, &pcb->mtu, sizeof(pcb->mtu));
+			break;
+
+		case SO_RFCOMM_FC_INFO:
+			fcinfo.lmodem = pcb->lmodem;
+			fcinfo.rmodem = pcb->rmodem;
+			fcinfo.tx_cred = pcb->tx_cred;
+			fcinfo.rx_cred = pcb->rx_cred;
+			fcinfo.cfc = (pcb->flags & NG_BTSOCKET_RFCOMM_DLC_CFC)?
+				1 : 0;
+			fcinfo.reserved = 0;
+
+			error = sooptcopyout(sopt, &fcinfo, sizeof(fcinfo));
+			break;
+
+		default:
+			error = ENOPROTOOPT;
+			break;
+		}
+		break;
+
+	case SOPT_SET:
+		switch (sopt->sopt_name) {
+		default:
+			error = ENOPROTOOPT;
+			break;
+		}
+		break;
+
+	default:
+		error = EINVAL;
+		break;
+	}
+
+	mtx_unlock(&pcb->pcb_mtx);
+
+	return (error);
+} /* ng_btsocket_rfcomm_ctloutput */
+
+/*
+ * Detach and destroy socket
+ */
+
+int
+ng_btsocket_rfcomm_detach(struct socket *so)
+{
+	ng_btsocket_rfcomm_pcb_p	pcb = so2rfcomm_pcb(so);
+
+	if (pcb == NULL)
+		return (EINVAL);
+
+	mtx_lock(&pcb->pcb_mtx);
+
+	switch (pcb->state) {
+	case NG_BTSOCKET_RFCOMM_DLC_W4_CONNECT:
+	case NG_BTSOCKET_RFCOMM_DLC_CONFIGURING:
+	case NG_BTSOCKET_RFCOMM_DLC_CONNECTING:
+	case NG_BTSOCKET_RFCOMM_DLC_CONNECTED:
+		/* XXX What to do with pending request? */
+		if (pcb->flags & NG_BTSOCKET_RFCOMM_DLC_TIMO)
+			ng_btsocket_rfcomm_untimeout(pcb);
+
+		if (pcb->state == NG_BTSOCKET_RFCOMM_DLC_W4_CONNECT)
+			pcb->flags |= NG_BTSOCKET_RFCOMM_DLC_DETACHED;
+		else
+			pcb->state = NG_BTSOCKET_RFCOMM_DLC_DISCONNECTING;
+
+		ng_btsocket_rfcomm_task_wakeup();
+		break;
+
+	case NG_BTSOCKET_RFCOMM_DLC_DISCONNECTING:
+		ng_btsocket_rfcomm_task_wakeup();
+		break;
+	}
+	
+	while (pcb->state != NG_BTSOCKET_RFCOMM_DLC_CLOSED)
+		msleep(&pcb->state, &pcb->pcb_mtx, PZERO, "rf_det", 0);
+
+	if (pcb->session != NULL)
+		panic("%s: pcb->session != NULL\n", __func__);
+	if (pcb->flags & NG_BTSOCKET_RFCOMM_DLC_TIMO)
+		panic("%s: timeout on closed DLC, flags=%#x\n",
+			__func__, pcb->flags);
+
+	mtx_lock(&ng_btsocket_rfcomm_sockets_mtx);
+	LIST_REMOVE(pcb, next);
+	mtx_unlock(&ng_btsocket_rfcomm_sockets_mtx);
+
+	mtx_unlock(&pcb->pcb_mtx);
+
+	mtx_destroy(&pcb->pcb_mtx);
+	bzero(pcb, sizeof(*pcb));
+	FREE(pcb, M_NETGRAPH_BTSOCKET_RFCOMM);
+
+	soisdisconnected(so);
+	so->so_pcb = NULL;
+	sotryfree(so);
+
+	return (0);
+} /* ng_btsocket_rfcomm_detach */
+
+/*
+ * Disconnect socket
+ */
+
+int
+ng_btsocket_rfcomm_disconnect(struct socket *so)
+{
+	ng_btsocket_rfcomm_pcb_p	pcb = so2rfcomm_pcb(so);
+
+	if (pcb == NULL)
+		return (EINVAL);
+
+	mtx_lock(&pcb->pcb_mtx);
+
+	if (pcb->state == NG_BTSOCKET_RFCOMM_DLC_DISCONNECTING) {
+		mtx_unlock(&pcb->pcb_mtx);
+		return (EINPROGRESS);
+	}
+
+	/* XXX What to do with pending request? */
+	if (pcb->flags & NG_BTSOCKET_RFCOMM_DLC_TIMO)
+		ng_btsocket_rfcomm_untimeout(pcb);
+
+	switch (pcb->state) {
+	case NG_BTSOCKET_RFCOMM_DLC_CONFIGURING: /* XXX can we get here? */
+	case NG_BTSOCKET_RFCOMM_DLC_CONNECTING: /* XXX can we get here? */
+	case NG_BTSOCKET_RFCOMM_DLC_CONNECTED:
+
+		/*
+		 * Just change DLC state and enqueue RFCOMM task. It will
+		 * queue and send DISC on the DLC.
+		 */ 
+
+		pcb->state = NG_BTSOCKET_RFCOMM_DLC_DISCONNECTING;
+		soisdisconnecting(so);
+
+		ng_btsocket_rfcomm_task_wakeup();
+		break;
+
+/*
+ * 	case NG_BTSOCKET_RFCOMM_DLC_CLOSED:
+ *	case NG_BTSOCKET_RFCOMM_DLC_W4_CONNECT:
+ *	case NG_BTSOCKET_RFCOMM_DLC_DISCONNECTING:
+ */
+	default:
+		panic("%s: Invalid DLC state=%d, flags=%#x\n",
+			__func__, pcb->state, pcb->flags);
+		break;
+	}
+
+	mtx_unlock(&pcb->pcb_mtx);
+
+	return (0);
+} /* ng_btsocket_rfcomm_disconnect */
+
+/*
+ * Listen on socket. First call to listen() will create listening RFCOMM session
+ */
+
+int
+ng_btsocket_rfcomm_listen(struct socket *so, struct thread *td)
+{
+	ng_btsocket_rfcomm_pcb_p	 pcb = so2rfcomm_pcb(so);
+	ng_btsocket_rfcomm_session_p	 s = NULL;
+	struct socket			*l2so = NULL;
+	int				 error;
+
+	if (pcb == NULL)
+		return (EINVAL);
+	if (pcb->channel < 1 || pcb->channel > 30)
+		return (EDESTADDRREQ);
+
+	/*
+	 * XXX FIXME - This is FUBAR. socreate() will call soalloc(1), i.e.
+	 * soalloc() is allowed to sleep in MALLOC. This creates "could sleep"
+	 * WITNESS warnings. To work around this problem we will create L2CAP
+	 * socket first and then check if we actually need it. Note that we 
+	 * will not check for errors in socreate() because if we failed to 
+	 * create L2CAP socket at this point we still might have already open
+	 * session.
+	 */
+
+	error = socreate(PF_BLUETOOTH, &l2so, SOCK_SEQPACKET,
+			BLUETOOTH_PROTO_L2CAP, td->td_ucred, td);
+
+	/* 
+	 * Look for the session in LISTENING state. There can only be one.
+	 */
+
+	mtx_lock(&ng_btsocket_rfcomm_sessions_mtx);
+
+	LIST_FOREACH(s, &ng_btsocket_rfcomm_sessions, next)
+		if (s->state == NG_BTSOCKET_RFCOMM_SESSION_LISTENING)
+			break;
+
+	if (s == NULL) {
+		/*
+		 * We need to create default RFCOMM session. Check if we have 
+		 * L2CAP socket. If l2so == NULL then error has the error code 
+		 * from socreate()
+		 */
+
+		if (l2so == NULL) {
+			mtx_unlock(&ng_btsocket_rfcomm_sessions_mtx);
+			return (error);
+		}
+
+		/* 
+		 * Create default listen RFCOMM session. The default RFCOMM 
+		 * session will listen on ANY address.
+		 *
+		 * XXX FIXME Note that currently there is no way to adjust MTU
+		 * for the default session.
+		 */
+
+		error = ng_btsocket_rfcomm_session_create(&s, l2so,
+					NG_HCI_BDADDR_ANY, NULL, td);
+		if (error != 0) {
+			mtx_unlock(&ng_btsocket_rfcomm_sessions_mtx);
+			soclose(l2so);
+
+			return (error);
+		}
+	} else if (l2so != NULL)
+		soclose(l2so); /* we don't need new L2CAP socket */
+
+	mtx_unlock(&ng_btsocket_rfcomm_sessions_mtx);
+
+	return (0);
+} /* ng_btsocket_listen */
+
+/*
+ * Get peer address
+ */
+
+int
+ng_btsocket_rfcomm_peeraddr(struct socket *so, struct sockaddr **nam)
+{
+	ng_btsocket_rfcomm_pcb_p	pcb = so2rfcomm_pcb(so);
+	struct sockaddr_rfcomm		sa;
+
+	if (pcb == NULL)
+		return (EINVAL);
+
+	bcopy(&pcb->dst, &sa.rfcomm_bdaddr, sizeof(sa.rfcomm_bdaddr));
+	sa.rfcomm_channel = pcb->channel;
+	sa.rfcomm_len = sizeof(sa);
+	sa.rfcomm_family = AF_BLUETOOTH;
+
+	*nam = dup_sockaddr((struct sockaddr *) &sa, 0);
+
+	return ((*nam == NULL)? ENOMEM : 0);
+} /* ng_btsocket_rfcomm_peeraddr */
+
+/*
+ * Send data to socket
+ */
+
+int
+ng_btsocket_rfcomm_send(struct socket *so, int flags, struct mbuf *m,
+		struct sockaddr *nam, struct mbuf *control, struct thread *td)
+{
+	ng_btsocket_rfcomm_pcb_t	*pcb = so2rfcomm_pcb(so);
+	int				 error = 0;
+
+	/* Check socket and input */
+	if (pcb == NULL || m == NULL || control != NULL) {
+		error = EINVAL;
+		goto drop;
+	}
+
+	mtx_lock(&pcb->pcb_mtx);
+
+	/* Make sure DLC is connected */
+	if (pcb->state != NG_BTSOCKET_RFCOMM_DLC_CONNECTED) {
+		mtx_unlock(&pcb->pcb_mtx);
+		error = ENOTCONN;
+		goto drop;
+	}
+
+	/* Put the packet on the socket's send queue and wakeup RFCOMM task */
+	sbappend(&pcb->so->so_snd, m);
+	m = NULL;
+	
+	if (!(pcb->flags & NG_BTSOCKET_RFCOMM_DLC_SENDING)) {
+		pcb->flags |= NG_BTSOCKET_RFCOMM_DLC_SENDING;
+		error = ng_btsocket_rfcomm_task_wakeup();
+	}
+
+	mtx_unlock(&pcb->pcb_mtx);
+drop:
+	NG_FREE_M(m); /* checks for != NULL */
+	NG_FREE_M(control);
+
+	return (error);
+} /* ng_btsocket_rfcomm_send */
+
+/*
+ * Get socket address
+ */
+
+int
+ng_btsocket_rfcomm_sockaddr(struct socket *so, struct sockaddr **nam)
+{
+	ng_btsocket_rfcomm_pcb_p	pcb = so2rfcomm_pcb(so);
+	struct sockaddr_rfcomm		sa;
+
+	if (pcb == NULL)
+		return (EINVAL);
+
+	bcopy(&pcb->src, &sa.rfcomm_bdaddr, sizeof(sa.rfcomm_bdaddr));
+	sa.rfcomm_channel = pcb->channel;
+	sa.rfcomm_len = sizeof(sa);
+	sa.rfcomm_family = AF_BLUETOOTH;
+
+	*nam = dup_sockaddr((struct sockaddr *) &sa, 0);
+
+	return ((*nam == NULL)? ENOMEM : 0);
+} /* ng_btsocket_rfcomm_sockaddr */
+
+/*
+ * Upcall function for L2CAP sockets. Enqueue RFCOMM task.
+ */
+
+static void
+ng_btsocket_rfcomm_upcall(struct socket *so, void *arg, int waitflag)
+{
+	int	error;
+
+	if (so == NULL)
+		panic("%s: so == NULL\n", __func__);
+
+	if ((error = ng_btsocket_rfcomm_task_wakeup()) != 0)
+		NG_BTSOCKET_RFCOMM_ALERT(
+"%s: Could not enqueue RFCOMM task, error=%d\n", __func__, error);
+} /* ng_btsocket_rfcomm_upcall */
+
+/*
+ * RFCOMM task. Will handle all RFCOMM sessions in one pass.
+ * XXX FIXME does not scale very well
+ */
+
+static void
+ng_btsocket_rfcomm_sessions_task(void *ctx, int pending)
+{
+	ng_btsocket_rfcomm_session_p	s = NULL, s_next = NULL;
+
+	mtx_lock(&ng_btsocket_rfcomm_sessions_mtx);
+
+	for (s = LIST_FIRST(&ng_btsocket_rfcomm_sessions); s != NULL; ) {
+		mtx_lock(&s->session_mtx);
+		s_next = LIST_NEXT(s, next);
+
+		ng_btsocket_rfcomm_session_task(s);
+
+		if (s->state == NG_BTSOCKET_RFCOMM_SESSION_CLOSED) {
+			/* Unlink and clean the session */
+			LIST_REMOVE(s, next);
+
+			NG_BT_MBUFQ_DRAIN(&s->outq);
+			if (!LIST_EMPTY(&s->dlcs))
+				panic("%s: DLC list is not empty\n", __func__);
+
+			/* Close L2CAP socket */
+			s->l2so->so_upcallarg = NULL;
+			s->l2so->so_upcall = NULL;
+			s->l2so->so_rcv.sb_flags &= ~SB_UPCALL;
+			s->l2so->so_snd.sb_flags &= ~SB_UPCALL;
+			soclose(s->l2so);
+
+			mtx_unlock(&s->session_mtx);
+
+			mtx_destroy(&s->session_mtx);
+			bzero(s, sizeof(*s));
+			FREE(s, M_NETGRAPH_BTSOCKET_RFCOMM);
+		} else
+			mtx_unlock(&s->session_mtx);
+
+		s = s_next;
+	}
+
+	mtx_unlock(&ng_btsocket_rfcomm_sessions_mtx);
+} /* ng_btsocket_rfcomm_sessions_task */
+
+/*
+ * Process RFCOMM session. Will handle all RFCOMM sockets in one pass.
+ */
+
+static void
+ng_btsocket_rfcomm_session_task(ng_btsocket_rfcomm_session_p s)
+{
+	mtx_assert(&s->session_mtx, MA_OWNED);
+
+	if (s->l2so->so_state & SS_CANTRCVMORE) {
+		NG_BTSOCKET_RFCOMM_INFO(
+"%s: L2CAP connection has been terminated, so=%p, so_state=%#x, so_count=%d, " \
+"state=%d, flags=%#x\n", __func__, s->l2so, s->l2so->so_state, 
+			s->l2so->so_count, s->state, s->flags);
+
+		s->state = NG_BTSOCKET_RFCOMM_SESSION_CLOSED;
+		ng_btsocket_rfcomm_session_clean(s);
+	}
+
+	/* Now process upcall */
+	switch (s->state) {
+	/* Try to accept new L2CAP connection(s) */
+	case NG_BTSOCKET_RFCOMM_SESSION_LISTENING:
+		while (ng_btsocket_rfcomm_session_accept(s) == 0)
+			;
+		break;
+
+	/* Process the results of the L2CAP connect */
+	case NG_BTSOCKET_RFCOMM_SESSION_CONNECTING:
+		ng_btsocket_rfcomm_session_process_pcb(s);
+
+		if (ng_btsocket_rfcomm_session_connect(s) != 0) {
+			s->state = NG_BTSOCKET_RFCOMM_SESSION_CLOSED;
+			ng_btsocket_rfcomm_session_clean(s);
+		} 
+		break;
+
+	/* Try to receive/send more data */
+	case NG_BTSOCKET_RFCOMM_SESSION_CONNECTED:
+	case NG_BTSOCKET_RFCOMM_SESSION_OPEN:
+	case NG_BTSOCKET_RFCOMM_SESSION_DISCONNECTING:
+		ng_btsocket_rfcomm_session_process_pcb(s);
+
+		if (ng_btsocket_rfcomm_session_receive(s) != 0) {
+			s->state = NG_BTSOCKET_RFCOMM_SESSION_CLOSED;
+			ng_btsocket_rfcomm_session_clean(s);
+		} else if (ng_btsocket_rfcomm_session_send(s) != 0) {
+			s->state = NG_BTSOCKET_RFCOMM_SESSION_CLOSED;
+			ng_btsocket_rfcomm_session_clean(s);
+		}
+		break;
+
+	case NG_BTSOCKET_RFCOMM_SESSION_CLOSED:
+		break;
+
+	default:
+		panic("%s: Invalid session state=%d, flags=%#x\n",
+			__func__, s->state, s->flags);
+		break;
+	}
+} /* ng_btsocket_rfcomm_session_task */
+
+/*
+ * Process RFCOMM connection indicator. Caller must hold s->session_mtx
+ */
+
+static ng_btsocket_rfcomm_pcb_p
+ng_btsocket_rfcomm_connect_ind(ng_btsocket_rfcomm_session_p s, int channel)
+{
+	ng_btsocket_rfcomm_pcb_p	 pcb = NULL, pcb1 = NULL;
+	ng_btsocket_l2cap_pcb_p		 l2pcb = NULL;
+	struct socket			*so1 = NULL;
+
+	mtx_assert(&s->session_mtx, MA_OWNED);
+
+	/*
+	 * Try to find RFCOMM socket that listens on given source address 
+	 * and channel. This will return the best possible match.
+	 */
+
+	l2pcb = so2l2cap_pcb(s->l2so);
+	pcb = ng_btsocket_rfcomm_pcb_listener(&l2pcb->src, channel);
+	if (pcb == NULL)
+		return (NULL);
+
+	/*
+	 * Check the pending connections queue and if we have space then 
+	 * create new socket and set proper source and destination address,
+	 * and channel.
+	 */
+
+	mtx_lock(&pcb->pcb_mtx);
+
+	if (pcb->so->so_qlen <= pcb->so->so_qlimit)
+		so1 = sonewconn(pcb->so, 0);
+
+	mtx_unlock(&pcb->pcb_mtx);
+
+	if (so1 == NULL)
+		return (NULL);
+
+	/*
+	 * If we got here than we have created new socket. So complete the 
+	 * connection. Set source and destination address from the session.
+	 */
+
+	pcb1 = so2rfcomm_pcb(so1);
+	if (pcb1 == NULL)
+		panic("%s: pcb1 == NULL\n", __func__);
+
+	mtx_lock(&pcb1->pcb_mtx);
+
+	bcopy(&l2pcb->src, &pcb1->src, sizeof(pcb1->src));
+	bcopy(&l2pcb->dst, &pcb1->dst, sizeof(pcb1->dst));
+	pcb1->channel = channel;
+
+	/* Link new DLC to the session. We already hold s->session_mtx */
+	LIST_INSERT_HEAD(&s->dlcs, pcb1, session_next);
+	pcb1->session = s;
+			
+	mtx_unlock(&pcb1->pcb_mtx);
+
+	return (pcb1);
+} /* ng_btsocket_rfcomm_connect_ind */
+
+/*
+ * Process RFCOMM connect confirmation. Caller must hold s->session_mtx.
+ */
+
+static void
+ng_btsocket_rfcomm_connect_cfm(ng_btsocket_rfcomm_session_p s)
+{
+	ng_btsocket_rfcomm_pcb_p	 pcb = NULL, pcb_next = NULL;
+	struct socket			*so = NULL;
+	int				 error;
+
+	mtx_assert(&s->session_mtx, MA_OWNED);
+
+	/*
+	 * Wake up all waiting sockets and send PN request for each of them. 
+	 * Note that timeout already been set in ng_btsocket_rfcomm_connect()
+	 *
+	 * Note: cannot use LIST_FOREACH because ng_btsocket_rfcomm_pcb_kill
+	 * will unlink DLC from the session
+	 */
+
+	for (pcb = LIST_FIRST(&s->dlcs); pcb != NULL; ) {
+		mtx_lock(&pcb->pcb_mtx);
+		pcb_next = LIST_NEXT(pcb, session_next);
+
+		if (pcb->state == NG_BTSOCKET_RFCOMM_DLC_W4_CONNECT) {
+			pcb->mtu = s->mtu;
+			bcopy(&so2l2cap_pcb(s->l2so)->src, &pcb->src,
+				sizeof(pcb->src));
+
+			error = ng_btsocket_rfcomm_send_pn(pcb);
+			if (error == 0)
+				pcb->state = NG_BTSOCKET_RFCOMM_DLC_CONFIGURING;
+			else if (ng_btsocket_rfcomm_pcb_kill(pcb, error))
+				so = pcb->so;
+			else 
+				so = NULL;
+		}
+
+		mtx_unlock(&pcb->pcb_mtx);
+		pcb = pcb_next;
+
+		if (so != NULL)
+			ng_btsocket_rfcomm_detach(so);
+	}
+} /* ng_btsocket_rfcomm_connect_cfm */
+
+/*****************************************************************************
+ *****************************************************************************
+ **                              RFCOMM sessions
+ *****************************************************************************
+ *****************************************************************************/
+
+/*
+ * Create new RFCOMM session. That function WILL NOT take ownership over l2so.
+ * Caller MUST free l2so if function failed.
+ */
+
+static int
+ng_btsocket_rfcomm_session_create(ng_btsocket_rfcomm_session_p *sp,
+		struct socket *l2so, bdaddr_p src, bdaddr_p dst,
+		struct thread *td)
+{
+	ng_btsocket_rfcomm_session_p	s = NULL;
+	struct sockaddr_l2cap		l2sa;
+	struct sockopt			l2sopt;
+	int				mtu, error;
+
+	mtx_assert(&ng_btsocket_rfcomm_sessions_mtx, MA_OWNED);
+
+	/* Allocate the RFCOMM session */
+        MALLOC(s, ng_btsocket_rfcomm_session_p, sizeof(*s),
+		M_NETGRAPH_BTSOCKET_RFCOMM, M_NOWAIT | M_ZERO);
+        if (s == NULL)
+                return (ENOMEM);
+
+	/* Set defaults */
+	s->mtu = RFCOMM_DEFAULT_MTU;
+	s->flags = 0;
+	s->state = NG_BTSOCKET_RFCOMM_SESSION_CLOSED;
+	NG_BT_MBUFQ_INIT(&s->outq, ifqmaxlen);
+
+	/*
+	 * XXX Mark session mutex as DUPOK to prevent "duplicated lock of 
+	 * the same type" message. When accepting new L2CAP connection
+	 * ng_btsocket_rfcomm_session_accept() holds both session mutexes 
+	 * for "old" (accepting) session and "new" (created) session.
+	 */
+
+	mtx_init(&s->session_mtx, "btsocks_rfcomm_session_mtx", NULL,
+		MTX_DEF|MTX_DUPOK);
+
+	LIST_INIT(&s->dlcs);
+
+	/* Prepare L2CAP socket */
+	l2so->so_upcallarg = NULL;
+	l2so->so_upcall = ng_btsocket_rfcomm_upcall;
+	l2so->so_rcv.sb_flags |= SB_UPCALL;
+	l2so->so_snd.sb_flags |= SB_UPCALL;
+	l2so->so_state |= SS_NBIO;
+	s->l2so = l2so;
+
+	mtx_lock(&s->session_mtx);
+
+	/*
+	 * "src" == NULL and "dst" == NULL means just create session.
+	 * caller must do the rest
+	 */
+
+	if (src == NULL && dst == NULL)
+		goto done;
+
+	/*
+	 * Set incoming MTU on L2CAP socket. It is RFCOMM session default MTU 
+	 * plus 5 bytes: RFCOMM frame header, one extra byte for length and one
+	 * extra byte for credits.
+	 */
+
+	mtu = s->mtu + sizeof(struct rfcomm_frame_hdr) + 1 + 1;
+
+	l2sopt.sopt_dir = SOPT_SET;
+	l2sopt.sopt_level = SOL_L2CAP;
+	l2sopt.sopt_name = SO_L2CAP_IMTU;
+	l2sopt.sopt_val = (void *) &mtu;
+	l2sopt.sopt_valsize = sizeof(mtu);
+	l2sopt.sopt_td = NULL;
+
+	error = sosetopt(s->l2so, &l2sopt);
+	if (error != 0)
+		goto bad;
+
+	/* Bind socket to "src" address */
+	l2sa.l2cap_len = sizeof(l2sa);
+	l2sa.l2cap_family = AF_BLUETOOTH;
+	l2sa.l2cap_psm = (dst == NULL)? htole16(NG_L2CAP_PSM_RFCOMM) : 0;
+	bcopy(src, &l2sa.l2cap_bdaddr, sizeof(l2sa.l2cap_bdaddr));
+
+	error = sobind(s->l2so, (struct sockaddr *) &l2sa, td);
+	if (error != 0)
+		goto bad;
+
+	/* If "dst" is not NULL then initiate connect(), otherwise listen() */
+	if (dst == NULL) {
+		s->flags = 0;
+		s->state = NG_BTSOCKET_RFCOMM_SESSION_LISTENING;
+
+		error = solisten(s->l2so, 10, td);
+		if (error != 0)
+			goto bad;
+	} else {
+		s->flags = NG_BTSOCKET_RFCOMM_SESSION_INITIATOR;
+		s->state = NG_BTSOCKET_RFCOMM_SESSION_CONNECTING;
+
+		l2sa.l2cap_len = sizeof(l2sa);   
+		l2sa.l2cap_family = AF_BLUETOOTH;
+		l2sa.l2cap_psm = htole16(NG_L2CAP_PSM_RFCOMM);
+	        bcopy(dst, &l2sa.l2cap_bdaddr, sizeof(l2sa.l2cap_bdaddr));
+
+		error = soconnect(s->l2so, (struct sockaddr *) &l2sa, td);
+		if (error != 0)
+			goto bad;
+	}
+
+done:
+	LIST_INSERT_HEAD(&ng_btsocket_rfcomm_sessions, s, next);
+	*sp = s;
+
+	mtx_unlock(&s->session_mtx);
+
+	return (0);
+
+bad:
+	mtx_unlock(&s->session_mtx);
+
+	/* Return L2CAP socket back to its original state */
+	l2so->so_upcallarg = NULL;
+	l2so->so_upcall = NULL;
+	l2so->so_rcv.sb_flags &= ~SB_UPCALL;
+	l2so->so_snd.sb_flags &= ~SB_UPCALL;
+	l2so->so_state &= ~SS_NBIO;
+
+	mtx_destroy(&s->session_mtx);
+	bzero(s, sizeof(*s));
+	FREE(s, M_NETGRAPH_BTSOCKET_RFCOMM);
+
+	return (error);
+} /* ng_btsocket_rfcomm_session_create */
+
+/*
+ * Process accept() on RFCOMM session
+ * XXX FIXME locking for "l2so"?
+ */
+
+static int
+ng_btsocket_rfcomm_session_accept(ng_btsocket_rfcomm_session_p s0)
+{
+	struct socket			*l2so = NULL;
+	struct sockaddr_l2cap		*l2sa = NULL;
+	ng_btsocket_l2cap_pcb_t		*l2pcb = NULL;
+	ng_btsocket_rfcomm_session_p	 s = NULL;
+	int				 error = 0;
+
+	mtx_assert(&ng_btsocket_rfcomm_sessions_mtx, MA_OWNED);
+	mtx_assert(&s0->session_mtx, MA_OWNED);
+
+	/* Check if there is a complete L2CAP connection in the queue */
+	if ((error = s0->l2so->so_error) != 0) {
+		NG_BTSOCKET_RFCOMM_ERR(
+"%s: Could not accept connection on L2CAP socket, error=%d\n", __func__, error);
+		s0->l2so->so_error = 0;
+
+		return (error);
+	}
+
+	if (TAILQ_EMPTY(&s0->l2so->so_comp)) {
+		if (s0->l2so->so_state & SS_CANTRCVMORE)
+			return (ECONNABORTED);
+
+		return (EWOULDBLOCK);
+	}
+
+	/* Accept incoming L2CAP connection */
+	l2so = TAILQ_FIRST(&s0->l2so->so_comp);
+	if (l2so == NULL)
+		panic("%s: l2so == NULL\n", __func__);
+
+	TAILQ_REMOVE(&s0->l2so->so_comp, l2so, so_list);
+	s0->l2so->so_qlen --;
+
+	soref(l2so);
+	l2so->so_state &= ~SS_COMP;
+	l2so->so_state |= SS_NBIO;
+	l2so->so_head = NULL;
+
+	error = soaccept(l2so, (struct sockaddr **) &l2sa);
+	if (error != 0) {
+		NG_BTSOCKET_RFCOMM_ERR(
+"%s: soaccept() on L2CAP socket failed, error=%d\n", __func__, error);
+		soclose(l2so);
+
+		return (error);
+	}
+
+	/*
+	 * Check if there is already active RFCOMM session between two devices.
+	 * If so then close L2CAP connection. We only support one RFCOMM session
+	 * between each pair of devices. Note that here we assume session in any
+	 * state. The session even could be in the middle of disconnecting.
+	 */
+
+	l2pcb = so2l2cap_pcb(l2so);
+	s = ng_btsocket_rfcomm_session_by_addr(&l2pcb->src, &l2pcb->dst);
+	if (s == NULL) {
+		/* Create a new RFCOMM session */
+		error = ng_btsocket_rfcomm_session_create(&s, l2so, NULL, NULL,
+				curthread /* XXX */);
+		if (error == 0) {
+			mtx_lock(&s->session_mtx);
+
+			s->flags = 0;
+			s->state = NG_BTSOCKET_RFCOMM_SESSION_CONNECTED;
+
+			/*
+			 * Adjust MTU on incomming connection. Reserve 5 bytes:
+			 * RFCOMM frame header, one extra byte for length and 
+			 * one extra byte for credits.
+			 */
+
+			s->mtu = min(l2pcb->imtu, l2pcb->omtu) -
+					sizeof(struct rfcomm_frame_hdr) - 1 - 1;
+
+			mtx_unlock(&s->session_mtx);
+		} else {
+			NG_BTSOCKET_RFCOMM_ALERT(
+"%s: Failed to create new RFCOMM session, error=%d\n", __func__, error);
+
+			soclose(l2so);
+		}
+	} else {
+		NG_BTSOCKET_RFCOMM_WARN(
+"%s: Rejecting duplicating RFCOMM session between src=%x:%x:%x:%x:%x:%x and " \
+"dst=%x:%x:%x:%x:%x:%x, state=%d, flags=%#x\n",	__func__,
+			l2pcb->src.b[5], l2pcb->src.b[4], l2pcb->src.b[3],
+			l2pcb->src.b[2], l2pcb->src.b[1], l2pcb->src.b[0],
+			l2pcb->dst.b[5], l2pcb->dst.b[4], l2pcb->dst.b[3],
+			l2pcb->dst.b[2], l2pcb->dst.b[1], l2pcb->dst.b[0],
+			s->state, s->flags);
+
+		error = EBUSY;
+		soclose(l2so);
+	}
+
+	return (error);
+} /* ng_btsocket_rfcomm_session_accept */
+
+/*
+ * Process connect() on RFCOMM session
+ * XXX FIXME locking for "l2so"?
+ */
+
+static int
+ng_btsocket_rfcomm_session_connect(ng_btsocket_rfcomm_session_p s)
+{
+	ng_btsocket_l2cap_pcb_p	l2pcb = so2l2cap_pcb(s->l2so);
+	int			error;
+
+	mtx_assert(&s->session_mtx, MA_OWNED);
+
+	/* First check if connection has failed */
+	if ((error = s->l2so->so_error) != 0) {
+		s->l2so->so_error = 0;
+
+		NG_BTSOCKET_RFCOMM_ERR(
+"%s: Could not connect RFCOMM session, error=%d, state=%d, flags=%#x\n",
+			__func__, error, s->state, s->flags);
+
+		return (error);
+	}
+
+	/* Is connection still in progress? */
+	if (s->l2so->so_state & SS_ISCONNECTING)
+		return (0); 
+
+	/* 
+	 * If we got here then we are connected. Send SABM on DLCI 0 to 
+	 * open multiplexor channel.
+	 */
+
+	if (error == 0) {
+		s->state = NG_BTSOCKET_RFCOMM_SESSION_CONNECTED;
+
+		/*
+		 * Adjust MTU on outgoing connection. Reserve 5 bytes: RFCOMM 
+		 * frame header, one extra byte for length and one extra byte 
+		 * for credits.
+		 */
+
+		s->mtu = min(l2pcb->imtu, l2pcb->omtu) -
+				sizeof(struct rfcomm_frame_hdr) - 1 - 1;
+
+		error = ng_btsocket_rfcomm_send_command(s,RFCOMM_FRAME_SABM,0);
+		if (error == 0)
+			error = ng_btsocket_rfcomm_task_wakeup();
+	}
+
+	return (error);
+}/* ng_btsocket_rfcomm_session_connect */
+
+/*
+ * Receive data on RFCOMM session
+ * XXX FIXME locking for "l2so"?
+ */
+
+static int
+ng_btsocket_rfcomm_session_receive(ng_btsocket_rfcomm_session_p s)
+{
+	struct mbuf	*m = NULL;
+	struct uio	 uio;
+	int		 more, flags, error;
+
+	mtx_assert(&s->session_mtx, MA_OWNED);
+
+	/* Can we read from the L2CAP socket? */
+	if (!soreadable(s->l2so))
+		return (0);
+
+	/* First check for error on L2CAP socket */
+	if ((error = s->l2so->so_error) != 0) {
+		s->l2so->so_error = 0;
+
+		NG_BTSOCKET_RFCOMM_ERR(
+"%s: Could not receive data from L2CAP socket, error=%d, state=%d, flags=%#x\n",
+			__func__, error, s->state, s->flags);
+
+		return (error);
+	}
+
+	/*
+	 * Read all packets from the L2CAP socket. 
+	 * XXX FIXME/VERIFY is that correct? For now use m->m_nextpkt as
+	 * indication that there is more packets on the socket's buffer.
+	 * Also what should we use in uio.uio_resid?
+	 * May be s->mtu + sizeof(struct rfcomm_frame_hdr) + 1 + 1?
+	 */
+
+	for (more = 1; more; ) {
+		/* Try to get next packet from socket */
+		bzero(&uio, sizeof(uio));
+/*		uio.uio_td = NULL; */
+		uio.uio_resid = 1000000000;
+		flags = MSG_DONTWAIT;
+
+		m = NULL;
+		error = (*s->l2so->so_proto->pr_usrreqs->pru_soreceive)(s->l2so,
+				NULL, &uio, &m, (struct mbuf **) NULL, &flags);
+		if (error != 0) {
+			if (error == EWOULDBLOCK)
+				return (0); /* XXX can happen? */
+
+			NG_BTSOCKET_RFCOMM_ERR(
+"%s: Could not receive data from L2CAP socket, error=%d\n", __func__, error);
+
+			return (error);
+		}
+	
+		more = (m->m_nextpkt != NULL);
+		m->m_nextpkt = NULL;
+
+		ng_btsocket_rfcomm_receive_frame(s, m);
+	}
+
+	return (0);
+} /* ng_btsocket_rfcomm_session_receive */
+
+/*
+ * Send data on RFCOMM session
+ * XXX FIXME locking for "l2so"?
+ */
+
+static int
+ng_btsocket_rfcomm_session_send(ng_btsocket_rfcomm_session_p s)
+{
+	struct mbuf	*m = NULL;
+	int		 error;
+
+	mtx_assert(&s->session_mtx, MA_OWNED);
+
+	/* Send as much as we can from the session queue */
+	while (sowriteable(s->l2so)) {
+		/* Check if socket still OK */
+		if ((error = s->l2so->so_error) != 0) {
+			s->l2so->so_error = 0;
+
+			NG_BTSOCKET_RFCOMM_ERR(
+"%s: Detected error=%d on L2CAP socket, state=%d, flags=%#x\n",
+				__func__, error, s->state, s->flags);
+
+			return (error);
+		}
+
+		NG_BT_MBUFQ_DEQUEUE(&s->outq, m);
+		if (m == NULL)
+			return (0); /* we are done */
+
+		/* Call send function on the L2CAP socket */
+		error = (*s->l2so->so_proto->pr_usrreqs->pru_sosend)
+				(s->l2so, NULL, NULL, m, NULL, 0,
+				 curthread /* XXX */);
+		if (error != 0) {
+			NG_BTSOCKET_RFCOMM_ERR(
+"%s: Could not send data to L2CAP socket, error=%d\n", __func__, error);
+
+			return (error);
+		}
+	}
+
+	return (0);
+} /* ng_btsocket_rfcomm_session_send */
+
+/*
+ * Close and disconnect all DLCs for the given session. Caller must hold 
+ * s->sesson_mtx. Will wakeup session.
+ */
+
+static void
+ng_btsocket_rfcomm_session_clean(ng_btsocket_rfcomm_session_p s)
+{
+	ng_btsocket_rfcomm_pcb_p	 pcb = NULL, pcb_next = NULL;
+	struct socket			*so = NULL;
+	int				 error;
+
+	mtx_assert(&s->session_mtx, MA_OWNED);
+
+	/*
+	 * Note: cannot use LIST_FOREACH because ng_btsocket_rfcomm_pcb_kill
+	 * will unlink DLC from the session
+	 */
+
+	for (pcb = LIST_FIRST(&s->dlcs); pcb != NULL; ) {
+		mtx_lock(&pcb->pcb_mtx);
+		pcb_next = LIST_NEXT(pcb, session_next);
+
+		NG_BTSOCKET_RFCOMM_INFO(
+"%s: Disconnecting dlci=%d, state=%d, flags=%#x\n",
+			__func__, pcb->dlci, pcb->state, pcb->flags);
+
+		if (pcb->state == NG_BTSOCKET_RFCOMM_DLC_CONNECTED)
+			error = ECONNRESET;
+		else
+			error = ECONNREFUSED;
+
+		if (ng_btsocket_rfcomm_pcb_kill(pcb, error))
+			so = pcb->so;
+		else
+			so = NULL;
+
+		mtx_unlock(&pcb->pcb_mtx);
+		pcb = pcb_next;
+
+		if (so != NULL)
+			ng_btsocket_rfcomm_detach(so);
+	}
+} /* ng_btsocket_rfcomm_session_clean */
+
+/*
+ * Process all DLCs on the session. Caller MUST hold s->session_mtx.
+ */
+
+static void
+ng_btsocket_rfcomm_session_process_pcb(ng_btsocket_rfcomm_session_p s)
+{
+	ng_btsocket_rfcomm_pcb_p	 pcb = NULL, pcb_next = NULL;
+	struct socket			*so = NULL;
+	int				 error;
+
+	mtx_assert(&s->session_mtx, MA_OWNED);
+
+	/*
+	 * Note: cannot use LIST_FOREACH because ng_btsocket_rfcomm_pcb_kill
+	 * will unlink DLC from the session
+	 */
+
+	for (pcb = LIST_FIRST(&s->dlcs); pcb != NULL; ) {
+		so = NULL;
+
+		mtx_lock(&pcb->pcb_mtx);
+		pcb_next = LIST_NEXT(pcb, session_next);
+
+		switch (pcb->state) {
+
+		/*
+		 * If DLC in W4_CONNECT state then we should check for both
+		 * timeout and detach.
+		 */
+
+		case NG_BTSOCKET_RFCOMM_DLC_W4_CONNECT:
+			if (pcb->flags & NG_BTSOCKET_RFCOMM_DLC_DETACHED) {
+				if (ng_btsocket_rfcomm_pcb_kill(pcb, 0))
+					so = pcb->so;
+			} else if (pcb->flags & NG_BTSOCKET_RFCOMM_DLC_TIMEDOUT)
+				if (ng_btsocket_rfcomm_pcb_kill(pcb, ETIMEDOUT))
+					so = pcb->so;
+			break;
+
+		/*
+		 * If DLC in CONFIGURING or CONNECTING state then we only
+		 * should check for timeout. If detach() was called then
+		 * DLC will be moved into DISCONNECTING state.
+		 */
+
+		case NG_BTSOCKET_RFCOMM_DLC_CONFIGURING:
+		case NG_BTSOCKET_RFCOMM_DLC_CONNECTING:
+			if (pcb->flags & NG_BTSOCKET_RFCOMM_DLC_TIMEDOUT)
+				if (ng_btsocket_rfcomm_pcb_kill(pcb, ETIMEDOUT))
+					so = pcb->so;
+			break;
+
+		/*
+		 * If DLC in CONNECTED state then we need to send data (if any)
+		 * from the socket's send queue. Note that we will send data
+		 * from either all sockets or none. This may overload session's
+		 * outgoing queue (but we do not check for that).
+		 *
+ 		 * XXX FIXME need scheduler for RFCOMM sockets
+		 */
+
+		case NG_BTSOCKET_RFCOMM_DLC_CONNECTED:
+			error = ng_btsocket_rfcomm_pcb_send(pcb, ALOT);
+			if (error != 0)
+				if (ng_btsocket_rfcomm_pcb_kill(pcb, error))
+					so = pcb->so;
+			break;
+
+		/*
+		 * If DLC in DISCONNECTING state then we must send DISC frame.
+		 * Note that if DLC has timeout set then we do not need to 
+		 * resend DISC frame.
+		 *
+		 * XXX FIXME need to drain all data from the socket's queue
+		 * if LINGER option was set
+		 */
+
+		case NG_BTSOCKET_RFCOMM_DLC_DISCONNECTING:
+			if (!(pcb->flags & NG_BTSOCKET_RFCOMM_DLC_TIMO)) {
+				error = ng_btsocket_rfcomm_send_command(
+						pcb->session, RFCOMM_FRAME_DISC,
+						pcb->dlci);
+				if (error == 0)
+					ng_btsocket_rfcomm_timeout(pcb);
+				else if (ng_btsocket_rfcomm_pcb_kill(pcb,error))
+					so = pcb->so;
+			} else if (pcb->flags & NG_BTSOCKET_RFCOMM_DLC_TIMEDOUT)
+				if (ng_btsocket_rfcomm_pcb_kill(pcb, ETIMEDOUT))
+					so = pcb->so;
+			break;
+		
+/*		case NG_BTSOCKET_RFCOMM_DLC_CLOSED: */
+		default:
+			panic("%s: Invalid DLC state=%d, flags=%#x\n",
+				__func__, pcb->state, pcb->flags);
+			break;
+		}
+
+		mtx_unlock(&pcb->pcb_mtx);
+		pcb = pcb_next;
+
+		if (so != NULL)
+			ng_btsocket_rfcomm_detach(so);
+	}
+} /* ng_btsocket_rfcomm_session_process_pcb */
+
+/*
+ * Find RFCOMM session between "src" and "dst".
+ * Caller MUST hold ng_btsocket_rfcomm_sessions_mtx.
+ */
+
+static ng_btsocket_rfcomm_session_p
+ng_btsocket_rfcomm_session_by_addr(bdaddr_p src, bdaddr_p dst)
+{
+	ng_btsocket_rfcomm_session_p	s = NULL;
+	ng_btsocket_l2cap_pcb_p		l2pcb = NULL;
+	int				any_src;
+
+	mtx_assert(&ng_btsocket_rfcomm_sessions_mtx, MA_OWNED);
+
+	any_src = (bcmp(src, NG_HCI_BDADDR_ANY, sizeof(*src)) == 0);
+
+	LIST_FOREACH(s, &ng_btsocket_rfcomm_sessions, next) {
+		l2pcb = so2l2cap_pcb(s->l2so);
+
+		if ((any_src || bcmp(&l2pcb->src, src, sizeof(*src)) == 0) &&
+		    bcmp(&l2pcb->dst, dst, sizeof(*dst)) == 0)
+			break;
+	}
+
+	return (s);
+} /* ng_btsocket_rfcomm_session_by_addr */
+
+/*****************************************************************************
+ *****************************************************************************
+ **                                  RFCOMM 
+ *****************************************************************************
+ *****************************************************************************/
+
+/*
+ * Process incoming RFCOMM frame. Caller must hold s->session_mtx.
+ * XXX FIXME check frame length
+ */
+
+static int
+ng_btsocket_rfcomm_receive_frame(ng_btsocket_rfcomm_session_p s,
+		struct mbuf *m0)
+{
+	struct rfcomm_frame_hdr	*hdr = NULL;
+	struct mbuf		*m = NULL;
+	u_int16_t		 length;
+	u_int8_t		 dlci, type;
+	int			 error = 0;
+
+	mtx_assert(&s->session_mtx, MA_OWNED);
+
+	/* Pullup as much as we can into first mbuf (for direct access) */
+	length = min(m0->m_pkthdr.len, MHLEN);
+	if (m0->m_len < length) {
+		if ((m0 = m_pullup(m0, length)) == NULL) {
+			NG_BTSOCKET_RFCOMM_ALERT(
+"%s: m_pullup(%d) failed\n", __func__, length);
+
+			return (ENOBUFS);
+		}
+	}
+
+	hdr = mtod(m0, struct rfcomm_frame_hdr *);
+	dlci = RFCOMM_DLCI(hdr->address);
+	type = RFCOMM_TYPE(hdr->control);
+
+	/* Test EA bit in length. If not set then we have 2 bytes of length */
+	if (!RFCOMM_EA(hdr->length)) {
+		bcopy(&hdr->length, &length, sizeof(length));
+		length = le16toh(length);
+		m_adj(m0, sizeof(*hdr) + 1);
+	} else {
+		length = hdr->length >> 1;
+		m_adj(m0, sizeof(*hdr));
+	}
+
+	NG_BTSOCKET_RFCOMM_INFO(
+"%s: Got frame type=%#x, dlci=%d, length=%d, cr=%d, pf=%d, len=%d\n",
+		__func__, type, dlci, length, RFCOMM_CR(hdr->address),
+		RFCOMM_PF(hdr->control), m0->m_pkthdr.len);
+
+	/*
+	 * Get FCS (the last byte in the frame)
+	 * XXX this will not work if mbuf chain ends with empty mbuf.
+	 * XXX let's hope it never happens :)
+	 */
+
+	for (m = m0; m->m_next != NULL; m = m->m_next)
+		;
+	if (m->m_len <= 0)
+		panic("%s: Empty mbuf at the end of the chain, len=%d\n",
+			__func__, m->m_len);
+
+	/*
+	 * Check FCS. We only need to calculate FCS on first 2 or 3 bytes
+	 * and already m_pullup'ed mbuf chain, so it should be safe.
+	 */
+
+	if (ng_btsocket_rfcomm_check_fcs((u_int8_t *) hdr, type, m->m_data[m->m_len - 1])) {
+		NG_BTSOCKET_RFCOMM_ERR(
+"%s: Invalid RFCOMM packet. Bad checksum\n", __func__);
+		NG_FREE_M(m0);
+
+		return (EINVAL);
+	}
+
+	m_adj(m0, -1); /* Trim FCS byte */
+
+	/*
+	 * Process RFCOMM frame.
+	 *
+	 * From TS 07.10 spec
+	 * 
+	 * "... In the case where a SABM or DISC command with the P bit set
+	 * to 0 is received then the received frame shall be discarded..."
+ 	 *
+	 * "... If a unsolicited DM response is received then the frame shall
+	 * be processed irrespective of the P/F setting... "
+	 *
+	 * "... The station may transmit response frames with the F bit set 
+	 * to 0 at any opportunity on an asynchronous basis. However, in the 
+	 * case where a UA response is received with the F bit set to 0 then 
+	 * the received frame shall be discarded..."
+	 *
+	 * From Bluetooth spec
+	 *
+	 * "... When credit based flow control is being used, the meaning of
+	 * the P/F bit in the control field of the RFCOMM header is redefined
+	 * for UIH frames..."
+	 */
+
+	switch (type) {
+	case RFCOMM_FRAME_SABM:
+		if (RFCOMM_PF(hdr->control))
+			error = ng_btsocket_rfcomm_receive_sabm(s, dlci);
+		break;
+
+	case RFCOMM_FRAME_DISC:
+		if (RFCOMM_PF(hdr->control))
+			error = ng_btsocket_rfcomm_receive_disc(s, dlci);
+		break;
+
+	case RFCOMM_FRAME_UA:
+		if (RFCOMM_PF(hdr->control))
+			error = ng_btsocket_rfcomm_receive_ua(s, dlci);
+		break;
+
+	case RFCOMM_FRAME_DM:
+		error = ng_btsocket_rfcomm_receive_dm(s, dlci);
+		break;
+
+	case RFCOMM_FRAME_UIH:
+		if (dlci == 0)
+			error = ng_btsocket_rfcomm_receive_mcc(s, m0);
+		else
+			error = ng_btsocket_rfcomm_receive_uih(s, dlci,
+					RFCOMM_PF(hdr->control), m0);
+
+		return (error);
+		/* NOT REACHED */
+
+	default:
+		NG_BTSOCKET_RFCOMM_ERR(
+"%s: Invalid RFCOMM packet. Unknown type=%#x\n", __func__, type);
+		error = EINVAL;
+		break;
+	}
+
+	NG_FREE_M(m0);
+
+	return (error);
+} /* ng_btsocket_rfcomm_receive_frame */
+
+/*
+ * Process RFCOMM SABM frame
+ */
+
+static int
+ng_btsocket_rfcomm_receive_sabm(ng_btsocket_rfcomm_session_p s, int dlci)
+{
+	ng_btsocket_rfcomm_pcb_p	 pcb = NULL;
+	struct socket			*so = NULL;
+	int				 error = 0;
+
+	mtx_assert(&s->session_mtx, MA_OWNED);
+
+	NG_BTSOCKET_RFCOMM_INFO(
+"%s: Got SABM, session state=%d, flags=%#x, mtu=%d, dlci=%d\n",
+		__func__, s->state, s->flags, s->mtu, dlci);
+
+	/* DLCI == 0 means open multiplexor channel */
+	if (dlci == 0) {
+		switch (s->state) {
+		case NG_BTSOCKET_RFCOMM_SESSION_CONNECTED:
+		case NG_BTSOCKET_RFCOMM_SESSION_OPEN:
+			error = ng_btsocket_rfcomm_send_command(s,
+					RFCOMM_FRAME_UA, dlci);
+			if (error == 0) {
+				s->state = NG_BTSOCKET_RFCOMM_SESSION_OPEN;
+				ng_btsocket_rfcomm_connect_cfm(s);
+			} else {
+				s->state = NG_BTSOCKET_RFCOMM_SESSION_CLOSED;
+				ng_btsocket_rfcomm_session_clean(s);
+			}
+			break;
+
+		default:
+			NG_BTSOCKET_RFCOMM_WARN(
+"%s: Got SABM for session in invalid state state=%d, flags=%#x\n",
+				__func__, s->state, s->flags);
+			error = EINVAL;
+			break;
+		}
+
+		return (error);
+	}
+
+	/* Make sure multiplexor channel is open */
+	if (s->state != NG_BTSOCKET_RFCOMM_SESSION_OPEN) {
+		NG_BTSOCKET_RFCOMM_ERR(
+"%s: Got SABM for dlci=%d with mulitplexor channel closed, state=%d, " \
+"flags=%#x\n",		__func__, dlci, s->state, s->flags);
+
+		return (EINVAL);
+	}
+
+	/*
+	 * Check if we have this DLCI. This might happen when remote
+	 * peer uses PN command before actual open (SABM) happens.
+	 */
+
+	pcb = ng_btsocket_rfcomm_pcb_by_dlci(s, dlci);
+	if (pcb != NULL) {
+		mtx_lock(&pcb->pcb_mtx);
+
+		if (pcb->state != NG_BTSOCKET_RFCOMM_DLC_CONNECTING) {
+			NG_BTSOCKET_RFCOMM_ERR(
+"%s: Got SABM for dlci=%d in invalid state=%d, flags=%#x\n",
+				__func__, dlci, pcb->state, pcb->flags);
+			mtx_unlock(&pcb->pcb_mtx);
+
+			return (ENOENT);
+		}
+
+		ng_btsocket_rfcomm_untimeout(pcb);
+
+		error = ng_btsocket_rfcomm_send_command(s,RFCOMM_FRAME_UA,dlci);
+		if (error == 0)
+			error = ng_btsocket_rfcomm_send_msc(pcb);
+
+		if (error == 0) {
+			pcb->state = NG_BTSOCKET_RFCOMM_DLC_CONNECTED;
+			soisconnected(pcb->so);
+		} else if (ng_btsocket_rfcomm_pcb_kill(pcb, error))
+			so = pcb->so;
+
+		mtx_unlock(&pcb->pcb_mtx);
+
+		if (so != NULL)
+			ng_btsocket_rfcomm_detach(so);
+
+		return (error);
+	}
+
+	/*
+	 * We do not have requested DLCI, so it must be an incoming connection
+	 * with default parameters. Try to accept it.
+	 */ 
+
+	pcb = ng_btsocket_rfcomm_connect_ind(s, RFCOMM_SRVCHANNEL(dlci));
+	if (pcb != NULL) {
+		mtx_lock(&pcb->pcb_mtx);
+
+		pcb->dlci = dlci;
+
+		error = ng_btsocket_rfcomm_send_command(s,RFCOMM_FRAME_UA,dlci);
+		if (error == 0) {
+			pcb->state = NG_BTSOCKET_RFCOMM_DLC_CONNECTED;
+			soisconnected(pcb->so);
+		} else if (ng_btsocket_rfcomm_pcb_kill(pcb, error))
+			so = pcb->so;
+
+		mtx_unlock(&pcb->pcb_mtx);
+
+		if (so != NULL)
+			ng_btsocket_rfcomm_detach(so);
+	} else
+		/* Nobody is listen()ing on the requested DLCI */
+		error = ng_btsocket_rfcomm_send_command(s,RFCOMM_FRAME_DM,dlci);
+
+	return (error);
+} /* ng_btsocket_rfcomm_receive_sabm */
+
+/*
+ * Process RFCOMM DISC frame
+ */
+
+static int
+ng_btsocket_rfcomm_receive_disc(ng_btsocket_rfcomm_session_p s, int dlci)
+{
+	ng_btsocket_rfcomm_pcb_p	pcb = NULL;
+	int				error = 0;
+
+	mtx_assert(&s->session_mtx, MA_OWNED);
+
+	NG_BTSOCKET_RFCOMM_INFO(
+"%s: Got DISC, session state=%d, flags=%#x, mtu=%d, dlci=%d\n",
+		__func__, s->state, s->flags, s->mtu, dlci);
+
+	/* DLCI == 0 means close multiplexor channel */
+	if (dlci == 0) {
+		/* XXX FIXME assume that remote side will close the socket */
+		error = ng_btsocket_rfcomm_send_command(s, RFCOMM_FRAME_UA, 0);
+		if (error == 0)
+			s->state = NG_BTSOCKET_RFCOMM_SESSION_DISCONNECTING;
+		else
+			s->state = NG_BTSOCKET_RFCOMM_SESSION_CLOSED; /* XXX */
+
+		ng_btsocket_rfcomm_session_clean(s);
+	} else {
+		pcb = ng_btsocket_rfcomm_pcb_by_dlci(s, dlci);
+		if (pcb != NULL) {
+			struct socket	*so = NULL;
+			int		 err;
+
+			mtx_lock(&pcb->pcb_mtx);
+
+			NG_BTSOCKET_RFCOMM_INFO(
+"%s: Got DISC for dlci=%d, state=%d, flags=%#x\n",
+				__func__, dlci, pcb->state, pcb->flags);
+
+			error = ng_btsocket_rfcomm_send_command(s,
+					RFCOMM_FRAME_UA, dlci);
+
+			if (pcb->state == NG_BTSOCKET_RFCOMM_DLC_CONNECTED)
+				err = 0;
+			else
+				err = ECONNREFUSED;
+
+			if (ng_btsocket_rfcomm_pcb_kill(pcb, err))
+				so = pcb->so;
+
+			mtx_unlock(&pcb->pcb_mtx);
+
+			if (so != NULL)
+				ng_btsocket_rfcomm_detach(so);
+		} else {
+			NG_BTSOCKET_RFCOMM_WARN(
+"%s: Got DISC for non-existing dlci=%d\n", __func__, dlci);
+
+			error = ng_btsocket_rfcomm_send_command(s,
+					RFCOMM_FRAME_DM, dlci);
+		}
+	}
+
+	return (error);
+} /* ng_btsocket_rfcomm_receive_disc */
+
+/*
+ * Process RFCOMM UA frame
+ */
+
+static int
+ng_btsocket_rfcomm_receive_ua(ng_btsocket_rfcomm_session_p s, int dlci)
+{
+	ng_btsocket_rfcomm_pcb_p	pcb = NULL;
+	int				error = 0;
+
+	mtx_assert(&s->session_mtx, MA_OWNED);
+
+	NG_BTSOCKET_RFCOMM_INFO(
+"%s: Got UA, session state=%d, flags=%#x, mtu=%d, dlci=%d\n",
+		__func__, s->state, s->flags, s->mtu, dlci);
+
+	/* dlci == 0 means multiplexor channel */
+	if (dlci == 0) {
+		switch (s->state) {
+		case NG_BTSOCKET_RFCOMM_SESSION_CONNECTED:
+			s->state = NG_BTSOCKET_RFCOMM_SESSION_OPEN;
+			ng_btsocket_rfcomm_connect_cfm(s);
+			break;
+
+		case NG_BTSOCKET_RFCOMM_SESSION_DISCONNECTING:
+			s->state = NG_BTSOCKET_RFCOMM_SESSION_CLOSED;
+			ng_btsocket_rfcomm_session_clean(s);
+			break;
+
+		default:
+			NG_BTSOCKET_RFCOMM_WARN(
+"%s: Got UA for session in invalid state=%d(%d), flags=%#x, mtu=%d\n",
+				__func__, s->state, INITIATOR(s), s->flags,
+				s->mtu);
+			error = ENOENT;
+			break;
+		}
+
+		return (error);
+	}
+
+	/* Check if we have this DLCI */
+	pcb = ng_btsocket_rfcomm_pcb_by_dlci(s, dlci);
+	if (pcb != NULL) {
+		struct socket	*so = NULL;
+
+		mtx_lock(&pcb->pcb_mtx);
+
+		NG_BTSOCKET_RFCOMM_INFO(
+"%s: Got UA for dlci=%d, state=%d, flags=%#x\n",
+			__func__, dlci, pcb->state, pcb->flags);
+
+		switch (pcb->state) {
+		case NG_BTSOCKET_RFCOMM_DLC_CONNECTING:
+			ng_btsocket_rfcomm_untimeout(pcb);
+
+			error = ng_btsocket_rfcomm_send_msc(pcb);
+			if (error == 0) {
+				pcb->state = NG_BTSOCKET_RFCOMM_DLC_CONNECTED;
+				soisconnected(pcb->so);
+			}
+			break;
+
+		case NG_BTSOCKET_RFCOMM_DLC_DISCONNECTING:
+			if (ng_btsocket_rfcomm_pcb_kill(pcb, 0))
+				so = pcb->so;
+			break;
+
+		default:
+			NG_BTSOCKET_RFCOMM_WARN(
+"%s: Got UA for dlci=%d in invalid state=%d, flags=%#x\n",
+				__func__, dlci, pcb->state, pcb->flags);
+			error = ENOENT;
+			break;
+		}
+
+		mtx_unlock(&pcb->pcb_mtx);
+
+		if (so != NULL)
+			ng_btsocket_rfcomm_detach(so);
+	} else {
+		NG_BTSOCKET_RFCOMM_WARN(
+"%s: Got UA for non-existing dlci=%d\n", __func__, dlci);
+
+		error = ng_btsocket_rfcomm_send_command(s,RFCOMM_FRAME_DM,dlci);
+	}
+
+	return (error);
+} /* ng_btsocket_rfcomm_receive_ua */
+
+/*
+ * Process RFCOMM DM frame
+ */
+
+static int
+ng_btsocket_rfcomm_receive_dm(ng_btsocket_rfcomm_session_p s, int dlci)
+{
+	ng_btsocket_rfcomm_pcb_p	pcb = NULL;
+	int				error;
+
+	mtx_assert(&s->session_mtx, MA_OWNED);
+
+	NG_BTSOCKET_RFCOMM_INFO(
+"%s: Got DM, session state=%d, flags=%#x, mtu=%d, dlci=%d\n",
+		__func__, s->state, s->flags, s->mtu, dlci);
+
+	/* DLCI == 0 means multiplexor channel */
+	if (dlci == 0) {
+		/* Disconnect all dlc's on the session */
+		s->state = NG_BTSOCKET_RFCOMM_SESSION_CLOSED;
+		ng_btsocket_rfcomm_session_clean(s);
+	} else {
+		pcb = ng_btsocket_rfcomm_pcb_by_dlci(s, dlci);
+		if (pcb != NULL) {
+			struct socket	*so = NULL;
+
+			mtx_lock(&pcb->pcb_mtx);
+
+			NG_BTSOCKET_RFCOMM_INFO(
+"%s: Got DM for dlci=%d, state=%d, flags=%#x\n",
+				__func__, dlci, pcb->state, pcb->flags);
+
+			if (pcb->state == NG_BTSOCKET_RFCOMM_DLC_CONNECTED)
+				error = ECONNRESET;
+			else
+				error = ECONNREFUSED;
+
+			if (ng_btsocket_rfcomm_pcb_kill(pcb, error))
+				so = pcb->so;
+
+			mtx_unlock(&pcb->pcb_mtx);
+
+			if (so != NULL)
+				ng_btsocket_rfcomm_detach(so);
+		} else
+			NG_BTSOCKET_RFCOMM_WARN(
+"%s: Got DM for non-existing dlci=%d\n", __func__, dlci);
+	}
+
+	return (0);
+} /* ng_btsocket_rfcomm_receive_dm */
+
+/*
+ * Process RFCOMM UIH frame (data)
+ */
+
+static int
+ng_btsocket_rfcomm_receive_uih(ng_btsocket_rfcomm_session_p s, int dlci,
+		int pf, struct mbuf *m0)
+{
+	ng_btsocket_rfcomm_pcb_p	pcb = NULL;
+	int				error = 0;
+
+	mtx_assert(&s->session_mtx, MA_OWNED);
+
+	NG_BTSOCKET_RFCOMM_INFO(
+"%s: Got UIH, session state=%d, flags=%#x, mtu=%d, dlci=%d, pf=%d, len=%d\n",
+		__func__, s->state, s->flags, s->mtu, dlci, pf,
+		m0->m_pkthdr.len);
+
+	/* XXX should we do it here? Check for session flow control */
+	if (s->flags & NG_BTSOCKET_RFCOMM_SESSION_LFC) {
+		NG_BTSOCKET_RFCOMM_WARN(
+"%s: Got UIH with session flow control asserted, state=%d, flags=%#x\n",
+			__func__, s->state, s->flags);
+		goto drop;
+	}
+
+	/* Check if we have this dlci */
+	pcb = ng_btsocket_rfcomm_pcb_by_dlci(s, dlci);
+	if (pcb == NULL) {
+		NG_BTSOCKET_RFCOMM_WARN(
+"%s: Got UIH for non-existing dlci=%d\n", __func__, dlci);
+		error = ng_btsocket_rfcomm_send_command(s,RFCOMM_FRAME_DM,dlci);
+		goto drop;
+	}
+
+	mtx_lock(&pcb->pcb_mtx);
+
+	/* Check dlci state */	
+	if (pcb->state != NG_BTSOCKET_RFCOMM_DLC_CONNECTED) {
+		NG_BTSOCKET_RFCOMM_WARN(
+"%s: Got UIH for dlci=%d in invalid state=%d, flags=%#x\n",
+			__func__, dlci, pcb->state, pcb->flags);
+		error = EINVAL;
+		goto drop1;
+	}
+
+	/* Check dlci flow control */
+	if (((pcb->flags & NG_BTSOCKET_RFCOMM_DLC_CFC) && pcb->rx_cred <= 0) ||
+	     (pcb->lmodem & RFCOMM_MODEM_FC)) {
+		NG_BTSOCKET_RFCOMM_ERR(
+"%s: Got UIH for dlci=%d with asserted flow control, state=%d, " \
+"flags=%#x, rx_cred=%d, lmodem=%#x\n",
+			__func__, dlci, pcb->state, pcb->flags,
+			pcb->rx_cred, pcb->lmodem);
+		goto drop1;
+	}
+
+	/* Did we get any credits? */
+	if ((pcb->flags & NG_BTSOCKET_RFCOMM_DLC_CFC) && pf) {
+		NG_BTSOCKET_RFCOMM_INFO(
+"%s: Got %d more credits for dlci=%d, state=%d, flags=%#x, " \
+"rx_cred=%d, tx_cred=%d\n",
+			__func__, *mtod(m0, u_int8_t *), dlci, pcb->state, 
+			pcb->flags, pcb->rx_cred, pcb->tx_cred);
+
+		pcb->tx_cred += *mtod(m0, u_int8_t *);
+		m_adj(m0, 1);
+
+		/* Send more from the DLC. XXX check for errors? */
+		ng_btsocket_rfcomm_pcb_send(pcb, ALOT);
+	} 
+
+	/* OK the of the rest of the mbuf is the data */
+	if (m0->m_pkthdr.len > 0) {
+		/* If we are using credit flow control decrease rx_cred here */
+		if (pcb->flags & NG_BTSOCKET_RFCOMM_DLC_CFC) {
+			/* Give remote peer more credits (if needed) */
+			if (-- pcb->rx_cred <= RFCOMM_MAX_CREDITS / 2)
+				ng_btsocket_rfcomm_send_credits(pcb);
+			else
+				NG_BTSOCKET_RFCOMM_INFO(
+"%s: Remote side still has credits, dlci=%d, state=%d, flags=%#x, " \
+"rx_cred=%d, tx_cred=%d\n",		__func__, dlci, pcb->state, pcb->flags,
+					pcb->rx_cred, pcb->tx_cred);
+		}
+		
+		/* Check packet against mtu on dlci */
+		if (m0->m_pkthdr.len > pcb->mtu) {
+			NG_BTSOCKET_RFCOMM_ERR(
+"%s: Got oversized UIH for dlci=%d, state=%d, flags=%#x, mtu=%d, len=%d\n",
+				__func__, dlci, pcb->state, pcb->flags,
+				pcb->mtu, m0->m_pkthdr.len);
+
+			error = EMSGSIZE;
+		} else if (m0->m_pkthdr.len > sbspace(&pcb->so->so_rcv)) {
+ 
+			/*
+			 * This is really bad. Receive queue on socket does
+			 * not have enough space for the packet. We do not
+			 * have any other choice but drop the packet. 
+			 */
+ 
+			NG_BTSOCKET_RFCOMM_ERR(
+"%s: Not enough space in socket receive queue. Dropping UIH for dlci=%d, " \
+"state=%d, flags=%#x, len=%d, space=%ld\n",
+				__func__, dlci, pcb->state, pcb->flags,
+				m0->m_pkthdr.len, sbspace(&pcb->so->so_rcv));
+
+			error = ENOBUFS;
+		} else {
+			/* Append packet to the socket receive queue */
+			sbappend(&pcb->so->so_rcv, m0);
+			m0 = NULL;
+
+			sorwakeup(pcb->so);
+		}
+	}
+drop1:
+	mtx_unlock(&pcb->pcb_mtx);
+drop:
+	NG_FREE_M(m0); /* checks for != NULL */
+
+	return (error);
+} /* ng_btsocket_rfcomm_receive_uih */
+
+/*
+ * Process RFCOMM MCC command (Multiplexor)
+ * 
+ * From TS 07.10 spec
+ *
+ * "5.4.3.1 Information Data
+ * 
+ *  ...The frames (UIH) sent by the initiating station have the C/R bit set 
+ *  to 1 and those sent by the responding station have the C/R bit set to 0..."
+ *
+ * "5.4.6.2 Operating procedures
+ *
+ *  Messages always exist in pairs; a command message and a corresponding 
+ *  response message. If the C/R bit is set to 1 the message is a command, 
+ *  if it is set to 0 the message is a response...
+ *
+ *  ...
+ * 
+ *  NOTE: Notice that when UIH frames are used to convey information on DLCI 0
+ *  there are at least two different fields that contain a C/R bit, and the 
+ *  bits are set of different form. The C/R bit in the Type field shall be set
+ *  as it is stated above, while the C/R bit in the Address field (see subclause
+ *  5.2.1.2) shall be set as it is described in subclause 5.4.3.1."
+ */
+
+static int
+ng_btsocket_rfcomm_receive_mcc(ng_btsocket_rfcomm_session_p s, struct mbuf *m0)
+{
+	struct rfcomm_mcc_hdr	*hdr = NULL;
+	u_int8_t		 cr, type, length;
+
+	mtx_assert(&s->session_mtx, MA_OWNED);
+
+	/*
+	 * We can access data directly in the first mbuf, because we have
+	 * m_pullup()'ed mbuf chain in ng_btsocket_rfcomm_receive_frame().
+	 * All MCC commands should fit into single mbuf (except probably TEST).
+	 */
+
+	hdr = mtod(m0, struct rfcomm_mcc_hdr *);
+	cr = RFCOMM_CR(hdr->type);
+	type = RFCOMM_MCC_TYPE(hdr->type);
+	length = RFCOMM_MCC_LENGTH(hdr->length);
+
+	/* Check MCC frame length */
+	if (sizeof(*hdr) + length != m0->m_pkthdr.len) {
+		NG_BTSOCKET_RFCOMM_ERR(
+"%s: Invalid MCC frame length=%d, len=%d\n",
+			__func__, length, m0->m_pkthdr.len);
+		NG_FREE_M(m0);
+
+		return (EMSGSIZE);
+	}
+
+	switch (type) {
+	case RFCOMM_MCC_TEST:
+		return (ng_btsocket_rfcomm_receive_test(s, m0));
+		/* NOT REACHED */
+
+	case RFCOMM_MCC_FCON:
+	case RFCOMM_MCC_FCOFF:
+		return (ng_btsocket_rfcomm_receive_fc(s, m0));
+		/* NOT REACHED */
+
+	case RFCOMM_MCC_MSC:
+		return (ng_btsocket_rfcomm_receive_msc(s, m0));
+		/* NOT REACHED */
+
+	case RFCOMM_MCC_RPN:
+		return (ng_btsocket_rfcomm_receive_rpn(s, m0));
+		/* NOT REACHED */
+
+	case RFCOMM_MCC_RLS:
+		return (ng_btsocket_rfcomm_receive_rls(s, m0));
+		/* NOT REACHED */
+
+	case RFCOMM_MCC_PN:
+		return (ng_btsocket_rfcomm_receive_pn(s, m0));
+		/* NOT REACHED */
+
+	case RFCOMM_MCC_NSC:
+		NG_BTSOCKET_RFCOMM_ERR(
+"%s: Got MCC NSC, type=%#x, cr=%d, length=%d, session state=%d, flags=%#x, " \
+"mtu=%d, len=%d\n",	__func__, RFCOMM_MCC_TYPE(*((u_int8_t *)(hdr + 1))), cr,
+			 length, s->state, s->flags, s->mtu, m0->m_pkthdr.len);
+		NG_FREE_M(m0);
+		break;
+
+	default:
+		NG_BTSOCKET_RFCOMM_ERR(
+"%s: Got unknown MCC, type=%#x, cr=%d, length=%d, session state=%d, " \
+"flags=%#x, mtu=%d, len=%d\n",
+			__func__, type, cr, length, s->state, s->flags,
+			s->mtu, m0->m_pkthdr.len);
+
+		/* Reuse mbuf to send NSC */
+		hdr = mtod(m0, struct rfcomm_mcc_hdr *);
+		m0->m_pkthdr.len = m0->m_len = sizeof(*hdr);
+
+		/* Create MCC NSC header */
+		hdr->type = RFCOMM_MKMCC_TYPE(0, RFCOMM_MCC_NSC);
+		hdr->length = RFCOMM_MKLEN8(1);
+
+		/* Put back MCC command type we did not like */
+		m0->m_data[m0->m_len] = RFCOMM_MKMCC_TYPE(cr, type);
+		m0->m_pkthdr.len ++;
+		m0->m_len ++;
+
+		/* Send UIH frame */
+		return (ng_btsocket_rfcomm_send_uih(s,
+				RFCOMM_MKADDRESS(INITIATOR(s), 0), 0, 0, m0));
+		/* NOT REACHED */
+	}
+
+	return (0);
+} /* ng_btsocket_rfcomm_receive_mcc */
+
+/*
+ * Receive RFCOMM TEST MCC command
+ */
+
+static int
+ng_btsocket_rfcomm_receive_test(ng_btsocket_rfcomm_session_p s, struct mbuf *m0)
+{
+	struct rfcomm_mcc_hdr	*hdr = mtod(m0, struct rfcomm_mcc_hdr *);
+	int			 error = 0;
+
+	mtx_assert(&s->session_mtx, MA_OWNED);
+
+	NG_BTSOCKET_RFCOMM_INFO(
+"%s: Got MCC TEST, cr=%d, length=%d, session state=%d, flags=%#x, mtu=%d, " \
+"len=%d\n",	__func__, RFCOMM_CR(hdr->type), RFCOMM_MCC_LENGTH(hdr->length),
+		s->state, s->flags, s->mtu, m0->m_pkthdr.len);
+
+	if (RFCOMM_CR(hdr->type)) {
+		hdr->type = RFCOMM_MKMCC_TYPE(0, RFCOMM_MCC_TEST);
+		error = ng_btsocket_rfcomm_send_uih(s,
+				RFCOMM_MKADDRESS(INITIATOR(s), 0), 0, 0, m0);
+	} else
+		NG_FREE_M(m0); /* XXX ignore response */
+
+	return (error);
+} /* ng_btsocket_rfcomm_receive_test */
+
+/*
+ * Receive RFCOMM FCON/FCOFF MCC command
+ */
+
+static int
+ng_btsocket_rfcomm_receive_fc(ng_btsocket_rfcomm_session_p s, struct mbuf *m0)
+{
+	struct rfcomm_mcc_hdr	*hdr = mtod(m0, struct rfcomm_mcc_hdr *);
+	u_int8_t		 type = RFCOMM_MCC_TYPE(hdr->type);
+	int			 error = 0;
+
+	mtx_assert(&s->session_mtx, MA_OWNED);
+
+	/*
+	 * Turn ON/OFF aggregate flow on the entire session. When remote peer 
+	 * asserted flow control no transmission shall occur except on dlci 0
+	 * (control channel).
+	 */
+
+	NG_BTSOCKET_RFCOMM_INFO(
+"%s: Got MCC FC%s, cr=%d, length=%d, session state=%d, flags=%#x, mtu=%d, " \
+"len=%d\n",	__func__, (type == RFCOMM_MCC_FCON)? "ON" : "OFF",
+		RFCOMM_CR(hdr->type), RFCOMM_MCC_LENGTH(hdr->length),
+		s->state, s->flags, s->mtu, m0->m_pkthdr.len);
+
+	if (RFCOMM_CR(hdr->type)) {
+		if (type == RFCOMM_MCC_FCON)
+			s->flags &= ~NG_BTSOCKET_RFCOMM_SESSION_RFC;
+		else
+			s->flags |= NG_BTSOCKET_RFCOMM_SESSION_RFC;
+
+		hdr->type = RFCOMM_MKMCC_TYPE(0, type);
+		error = ng_btsocket_rfcomm_send_uih(s,
+				RFCOMM_MKADDRESS(INITIATOR(s), 0), 0, 0, m0);
+	} else
+		NG_FREE_M(m0); /* XXX ignore response */
+
+	return (error);
+} /* ng_btsocket_rfcomm_receive_fc  */
+
+/*
+ * Receive RFCOMM MSC MCC command
+ */
+
+static int
+ng_btsocket_rfcomm_receive_msc(ng_btsocket_rfcomm_session_p s, struct mbuf *m0)
+{
+	struct rfcomm_mcc_hdr		*hdr = mtod(m0, struct rfcomm_mcc_hdr*);
+	struct rfcomm_mcc_msc		*msc = (struct rfcomm_mcc_msc *)(hdr+1);
+	ng_btsocket_rfcomm_pcb_t	*pcb = NULL;
+	int				 error = 0;
+
+	mtx_assert(&s->session_mtx, MA_OWNED);
+
+	NG_BTSOCKET_RFCOMM_INFO(
+"%s: Got MCC MSC, dlci=%d, cr=%d, length=%d, session state=%d, flags=%#x, " \
+"mtu=%d, len=%d\n",
+		__func__,  RFCOMM_DLCI(msc->address), RFCOMM_CR(hdr->type),
+		RFCOMM_MCC_LENGTH(hdr->length), s->state, s->flags,
+		s->mtu, m0->m_pkthdr.len);
+
+	if (RFCOMM_CR(hdr->type)) {
+		pcb = ng_btsocket_rfcomm_pcb_by_dlci(s, RFCOMM_DLCI(msc->address));
+		if (pcb == NULL) {
+			NG_BTSOCKET_RFCOMM_WARN(
+"%s: Got MSC command for non-existing dlci=%d\n",
+				__func__, RFCOMM_DLCI(msc->address));
+			NG_FREE_M(m0);
+
+			return (ENOENT);
+		}
+
+		mtx_lock(&pcb->pcb_mtx);
+
+		if (pcb->state != NG_BTSOCKET_RFCOMM_DLC_CONNECTING &&
+		    pcb->state != NG_BTSOCKET_RFCOMM_DLC_CONNECTED) {
+			NG_BTSOCKET_RFCOMM_WARN(
+"%s: Got MSC on dlci=%d in invalid state=%d\n",
+				__func__, RFCOMM_DLCI(msc->address),
+				pcb->state);
+
+			mtx_unlock(&pcb->pcb_mtx);
+			NG_FREE_M(m0);
+
+			return (EINVAL);
+		}
+
+		pcb->rmodem = msc->modem; /* Update remote port signals */
+
+		hdr->type = RFCOMM_MKMCC_TYPE(0, RFCOMM_MCC_MSC);
+		error = ng_btsocket_rfcomm_send_uih(s,
+				RFCOMM_MKADDRESS(INITIATOR(s), 0), 0, 0, m0);
+
+#if 0 /* YYY */
+		/* Send more data from DLC. XXX check for errors? */
+		if (!(pcb->rmodem & RFCOMM_MODEM_FC) &&
+		    !(pcb->flags & NG_BTSOCKET_RFCOMM_DLC_CFC))
+			ng_btsocket_rfcomm_pcb_send(pcb, ALOT);
+#endif /* YYY */
+
+		mtx_unlock(&pcb->pcb_mtx);
+	} else
+		NG_FREE_M(m0); /* XXX ignore response */
+
+	return (error);
+} /* ng_btsocket_rfcomm_receive_msc */
+
+/*
+ * Receive RFCOMM RPN MCC command
+ * XXX FIXME do we need htole16/le16toh for RPN param_mask?
+ */
+
+static int
+ng_btsocket_rfcomm_receive_rpn(ng_btsocket_rfcomm_session_p s, struct mbuf *m0)
+{
+	struct rfcomm_mcc_hdr	*hdr = mtod(m0, struct rfcomm_mcc_hdr *);
+	struct rfcomm_mcc_rpn	*rpn = (struct rfcomm_mcc_rpn *)(hdr + 1);
+	int			 error = 0;
+	u_int16_t		 param_mask;
+	u_int8_t		 bit_rate, data_bits, stop_bits, parity,
+				 flow_control, xon_char, xoff_char;
+
+	mtx_assert(&s->session_mtx, MA_OWNED);
+
+	NG_BTSOCKET_RFCOMM_INFO(
+"%s: Got MCC RPN, dlci=%d, cr=%d, length=%d, session state=%d, flags=%#x, " \
+"mtu=%d, len=%d\n",
+		__func__, RFCOMM_DLCI(rpn->dlci), RFCOMM_CR(hdr->type),
+		RFCOMM_MCC_LENGTH(hdr->length), s->state, s->flags,
+		s->mtu, m0->m_pkthdr.len);
+
+	if (RFCOMM_CR(hdr->type)) {
+		param_mask = RFCOMM_RPN_PM_ALL;
+
+		if (RFCOMM_MCC_LENGTH(hdr->length) == 1) {
+			/* Request - return default setting */
+			bit_rate = RFCOMM_RPN_BR_115200;
+			data_bits = RFCOMM_RPN_DATA_8;
+			stop_bits = RFCOMM_RPN_STOP_1;
+			parity = RFCOMM_RPN_PARITY_NONE;
+			flow_control = RFCOMM_RPN_FLOW_NONE;
+			xon_char = RFCOMM_RPN_XON_CHAR;
+			xoff_char = RFCOMM_RPN_XOFF_CHAR;
+                } else {
+			/*
+			 * Ignore/accept bit_rate, 8 bits, 1 stop bit, no 
+			 * parity, no flow control lines, default XON/XOFF 
+			 * chars.
+			 */
+
+			bit_rate = rpn->bit_rate;
+			rpn->param_mask = le16toh(rpn->param_mask); /* XXX */
+
+			data_bits = RFCOMM_RPN_DATA_BITS(rpn->line_settings);
+			if (rpn->param_mask & RFCOMM_RPN_PM_DATA &&
+			    data_bits != RFCOMM_RPN_DATA_8) {
+				data_bits = RFCOMM_RPN_DATA_8;
+				param_mask ^= RFCOMM_RPN_PM_DATA;
+			}
+
+			stop_bits = RFCOMM_RPN_STOP_BITS(rpn->line_settings);
+			if (rpn->param_mask & RFCOMM_RPN_PM_STOP &&
+			    stop_bits != RFCOMM_RPN_STOP_1) {
+				stop_bits = RFCOMM_RPN_STOP_1;
+				param_mask ^= RFCOMM_RPN_PM_STOP;
+			}
+
+			parity = RFCOMM_RPN_PARITY(rpn->line_settings);
+			if (rpn->param_mask & RFCOMM_RPN_PM_PARITY &&
+			    parity != RFCOMM_RPN_PARITY_NONE) {
+				parity = RFCOMM_RPN_PARITY_NONE;
+				param_mask ^= RFCOMM_RPN_PM_PARITY;
+			}
+
+			flow_control = rpn->flow_control;
+			if (rpn->param_mask & RFCOMM_RPN_PM_FLOW &&
+			    flow_control != RFCOMM_RPN_FLOW_NONE) {
+				flow_control = RFCOMM_RPN_FLOW_NONE;
+				param_mask ^= RFCOMM_RPN_PM_FLOW;
+			}
+
+			xon_char = rpn->xon_char;
+			if (rpn->param_mask & RFCOMM_RPN_PM_XON &&
+			    xon_char != RFCOMM_RPN_XON_CHAR) {
+				xon_char = RFCOMM_RPN_XON_CHAR;
+				param_mask ^= RFCOMM_RPN_PM_XON;
+			}
+
+			xoff_char = rpn->xoff_char;
+			if (rpn->param_mask & RFCOMM_RPN_PM_XOFF &&
+			    xoff_char != RFCOMM_RPN_XOFF_CHAR) {
+				xoff_char = RFCOMM_RPN_XOFF_CHAR;
+				param_mask ^= RFCOMM_RPN_PM_XOFF;
+			}
+		}
+
+		rpn->bit_rate = bit_rate;
+		rpn->line_settings = RFCOMM_MKRPN_LINE_SETTINGS(data_bits, 
+						stop_bits, parity);
+		rpn->flow_control = flow_control;
+		rpn->xon_char = xon_char;
+		rpn->xoff_char = xoff_char;
+		rpn->param_mask = htole16(param_mask); /* XXX */
+
+		m0->m_pkthdr.len = m0->m_len = sizeof(*hdr) + sizeof(*rpn);
+
+		hdr->type = RFCOMM_MKMCC_TYPE(0, RFCOMM_MCC_RPN);
+		error = ng_btsocket_rfcomm_send_uih(s,
+				RFCOMM_MKADDRESS(INITIATOR(s), 0), 0, 0, m0);
+	} else
+		NG_FREE_M(m0); /* XXX ignore response */
+
+	return (error);
+} /* ng_btsocket_rfcomm_receive_rpn */
+
+/*
+ * Receive RFCOMM RLS MCC command
+ */
+
+static int
+ng_btsocket_rfcomm_receive_rls(ng_btsocket_rfcomm_session_p s, struct mbuf *m0)
+{
+	struct rfcomm_mcc_hdr	*hdr = mtod(m0, struct rfcomm_mcc_hdr *);
+	struct rfcomm_mcc_rls	*rls = (struct rfcomm_mcc_rls *)(hdr + 1);
+	int			 error = 0;
+
+	mtx_assert(&s->session_mtx, MA_OWNED);
+
+	/*
+	 * XXX FIXME Do we have to do anything else here? Remote peer tries to 
+	 * tell us something about DLCI. Just report what we have received and
+	 * return back received values as required by TS 07.10 spec.
+	 */
+
+	NG_BTSOCKET_RFCOMM_INFO(
+"%s: Got MCC RLS, dlci=%d, status=%#x, cr=%d, length=%d, session state=%d, " \
+"flags=%#x, mtu=%d, len=%d\n",
+		__func__, RFCOMM_DLCI(rls->address), rls->status,
+		RFCOMM_CR(hdr->type), RFCOMM_MCC_LENGTH(hdr->length),
+		s->state, s->flags, s->mtu, m0->m_pkthdr.len);
+
+	if (RFCOMM_CR(hdr->type)) {
+		if (rls->status & 0x1)
+			NG_BTSOCKET_RFCOMM_ERR(
+"%s: Got RLS dlci=%d, error=%#x\n", __func__, RFCOMM_DLCI(rls->address),
+				rls->status >> 1);
+
+		hdr->type = RFCOMM_MKMCC_TYPE(0, RFCOMM_MCC_RLS);
+		error = ng_btsocket_rfcomm_send_uih(s,
+				RFCOMM_MKADDRESS(INITIATOR(s), 0), 0, 0, m0);
+	} else
+		NG_FREE_M(m0); /* XXX ignore responses */
+
+	return (error);
+} /* ng_btsocket_rfcomm_receive_rls */
+
+/*
+ * Receive RFCOMM PN MCC command
+ */
+
+static int
+ng_btsocket_rfcomm_receive_pn(ng_btsocket_rfcomm_session_p s, struct mbuf *m0)
+{
+	struct rfcomm_mcc_hdr		*hdr = mtod(m0, struct rfcomm_mcc_hdr*);
+	struct rfcomm_mcc_pn		*pn = (struct rfcomm_mcc_pn *)(hdr+1);
+	ng_btsocket_rfcomm_pcb_t	*pcb = NULL;
+	int				 error = 0;
+
+	mtx_assert(&s->session_mtx, MA_OWNED);
+
+	NG_BTSOCKET_RFCOMM_INFO(
+"%s: Got MCC PN, dlci=%d, cr=%d, length=%d, flow_control=%#x, priority=%d, " \
+"ack_timer=%d, mtu=%d, max_retrans=%d, credits=%d, session state=%d, " \
+"flags=%#x, session mtu=%d, len=%d\n",
+		__func__, pn->dlci, RFCOMM_CR(hdr->type),
+		RFCOMM_MCC_LENGTH(hdr->length), pn->flow_control, pn->priority,
+		pn->ack_timer, le16toh(pn->mtu), pn->max_retrans, pn->credits,
+		s->state, s->flags, s->mtu, m0->m_pkthdr.len);
+
+	if (pn->dlci == 0) {
+		NG_BTSOCKET_RFCOMM_ERR("%s: Zero dlci in MCC PN\n", __func__);
+		NG_FREE_M(m0);
+
+		return (EINVAL);
+	}
+
+	/* Check if we have this dlci */
+	pcb = ng_btsocket_rfcomm_pcb_by_dlci(s, pn->dlci);
+	if (pcb != NULL) {
+		mtx_lock(&pcb->pcb_mtx);
+
+		if (RFCOMM_CR(hdr->type)) {
+			/* PN Request */
+			ng_btsocket_rfcomm_set_pn(pcb, 1, pn->flow_control,
+				pn->credits, pn->mtu);
+
+			if (pcb->flags & NG_BTSOCKET_RFCOMM_DLC_CFC) {
+				pn->flow_control = 0xe0;
+				pn->credits = RFCOMM_DEFAULT_CREDITS;
+			} else {
+				pn->flow_control = 0;
+				pn->credits = 0;
+			}
+
+			hdr->type = RFCOMM_MKMCC_TYPE(0, RFCOMM_MCC_PN);
+			error = ng_btsocket_rfcomm_send_uih(s, 
+					RFCOMM_MKADDRESS(INITIATOR(s), 0),
+					0, 0, m0);
+		} else {
+			/* PN Response - proceed with SABM. Timeout still set */
+			if (pcb->state == NG_BTSOCKET_RFCOMM_DLC_CONFIGURING) {
+				ng_btsocket_rfcomm_set_pn(pcb, 0,
+					pn->flow_control, pn->credits, pn->mtu);
+
+				pcb->state = NG_BTSOCKET_RFCOMM_DLC_CONNECTING;
+				error = ng_btsocket_rfcomm_send_command(s,
+						RFCOMM_FRAME_SABM, pn->dlci);
+			} else
+				NG_BTSOCKET_RFCOMM_WARN(
+"%s: Got PN response for dlci=%d in invalid state=%d\n",
+					__func__, pn->dlci, pcb->state);
+
+			NG_FREE_M(m0);
+		}
+
+		mtx_unlock(&pcb->pcb_mtx);
+	} else if (RFCOMM_CR(hdr->type)) {
+		/* PN request to non-existing dlci - incomming connection */
+		pcb = ng_btsocket_rfcomm_connect_ind(s,
+				RFCOMM_SRVCHANNEL(pn->dlci));
+		if (pcb != NULL) {
+			struct socket	*so = NULL;
+
+			mtx_lock(&pcb->pcb_mtx);
+
+			pcb->dlci = pn->dlci;
+
+			ng_btsocket_rfcomm_set_pn(pcb, 1, pn->flow_control,
+				pn->credits, pn->mtu);
+
+			if (pcb->flags & NG_BTSOCKET_RFCOMM_DLC_CFC) {
+				pn->flow_control = 0xe0;
+				pn->credits = RFCOMM_DEFAULT_CREDITS;
+			} else {
+				pn->flow_control = 0;
+				pn->credits = 0;
+			}
+
+			hdr->type = RFCOMM_MKMCC_TYPE(0, RFCOMM_MCC_PN);
+			error = ng_btsocket_rfcomm_send_uih(s, 
+					RFCOMM_MKADDRESS(INITIATOR(s), 0),
+					0, 0, m0);
+
+			if (error == 0) {
+				ng_btsocket_rfcomm_timeout(pcb);
+				pcb->state = NG_BTSOCKET_RFCOMM_DLC_CONNECTING;
+				soisconnecting(pcb->so);
+			} else if (ng_btsocket_rfcomm_pcb_kill(pcb, error))
+				so = pcb->so;
+
+			mtx_unlock(&pcb->pcb_mtx);
+
+			if (so != NULL)
+				ng_btsocket_rfcomm_detach(so);
+		} else {
+			/* Nobody is listen()ing on this channel */
+			error = ng_btsocket_rfcomm_send_command(s,
+					RFCOMM_FRAME_DM, pn->dlci);
+			NG_FREE_M(m0);
+		}
+	} else
+		NG_FREE_M(m0); /* XXX ignore response to non-existing dlci */
+
+	return (error);
+} /* ng_btsocket_rfcomm_receive_pn */
+
+/*
+ * Set PN parameters for dlci. Caller must hold pcb->pcb_mtx.
+ * 
+ * From Bluetooth spec.
+ * 
+ * "... The CL1 - CL4 field is completely redefined. (In TS07.10 this defines 
+ *  the convergence layer to use, which is not applicable to RFCOMM. In RFCOMM,
+ *  in Bluetooth versions up to 1.0B, this field was forced to 0).
+ *
+ *  In the PN request sent prior to a DLC establishment, this field must contain
+ *  the value 15 (0xF), indicating support of credit based flow control in the 
+ *  sender. See Table 5.3 below. If the PN response contains any other value 
+ *  than 14 (0xE) in this field, it is inferred that the peer RFCOMM entity is 
+ *  not supporting the credit based flow control feature. (This is only possible
+ *  if the peer RFCOMM implementation is only conforming to Bluetooth version 
+ *  1.0B.) If a PN request is sent on an already open DLC, then this field must
+ *  contain the value zero; it is not possible to set initial credits  more 
+ *  than once per DLC activation. A responding implementation must set this 
+ *  field in the PN response to 14 (0xE), if (and only if) the value in the PN 
+ *  request was 15..."
+ */
+
+static void
+ng_btsocket_rfcomm_set_pn(ng_btsocket_rfcomm_pcb_p pcb, u_int8_t cr,
+		u_int8_t flow_control, u_int8_t credits, u_int16_t mtu)
+{
+	mtx_assert(&pcb->pcb_mtx, MA_OWNED);
+
+	pcb->mtu = le16toh(mtu);
+
+	if (cr) {
+		if (flow_control == 0xf0) {
+			pcb->flags |= NG_BTSOCKET_RFCOMM_DLC_CFC;
+			pcb->tx_cred = credits;
+		} else {
+			pcb->flags &= ~NG_BTSOCKET_RFCOMM_DLC_CFC;
+			pcb->tx_cred = 0;
+		}
+	} else {
+		if (flow_control == 0xe0) {
+			pcb->flags |= NG_BTSOCKET_RFCOMM_DLC_CFC;
+			pcb->tx_cred = credits;
+		} else {
+			pcb->flags &= ~NG_BTSOCKET_RFCOMM_DLC_CFC;
+			pcb->tx_cred = 0;
+		}
+	}
+
+	NG_BTSOCKET_RFCOMM_INFO(
+"%s: cr=%d, dlci=%d, state=%d, flags=%#x, mtu=%d, rx_cred=%d, tx_cred=%d\n",
+		__func__, cr, pcb->dlci, pcb->state, pcb->flags, pcb->mtu,
+		pcb->rx_cred, pcb->tx_cred);
+} /* ng_btsocket_rfcomm_set_pn */
+
+/*
+ * Send RFCOMM SABM/DISC/UA/DM frames. Caller must hold s->session_mtx
+ */
+
+static int
+ng_btsocket_rfcomm_send_command(ng_btsocket_rfcomm_session_p s,
+		u_int8_t type, u_int8_t dlci)
+{
+	struct rfcomm_cmd_hdr	*hdr = NULL;
+	struct mbuf		*m = NULL;
+	int			 cr;
+
+	mtx_assert(&s->session_mtx, MA_OWNED);
+
+	NG_BTSOCKET_RFCOMM_INFO(
+"%s: Sending command type %#x, session state=%d, flags=%#x, mtu=%d, dlci=%d\n",
+		__func__, type, s->state, s->flags, s->mtu, dlci);
+
+	switch (type) {
+	case RFCOMM_FRAME_SABM:
+	case RFCOMM_FRAME_DISC:
+		cr = INITIATOR(s);
+		break;
+
+	case RFCOMM_FRAME_UA:
+	case RFCOMM_FRAME_DM:
+		cr = !INITIATOR(s);
+		break;
+
+	default:
+		panic("%s: Invalid frame type=%#x\n", __func__, type);
+		return (EINVAL);
+		/* NOT REACHED */
+	}
+
+	MGETHDR(m, M_DONTWAIT, MT_DATA);
+	if (m == NULL)
+		return (ENOBUFS);
+
+	m->m_pkthdr.len = m->m_len = sizeof(*hdr);
+
+	hdr = mtod(m, struct rfcomm_cmd_hdr *);
+	hdr->address = RFCOMM_MKADDRESS(cr, dlci);
+	hdr->control = RFCOMM_MKCONTROL(type, 1);
+	hdr->length = RFCOMM_MKLEN8(0);
+	hdr->fcs = ng_btsocket_rfcomm_fcs3((u_int8_t *) hdr);
+
+	NG_BT_MBUFQ_ENQUEUE(&s->outq, m);
+
+	return (0);
+} /* ng_btsocket_rfcomm_send_command */
+
+/*
+ * Send RFCOMM UIH frame. Caller must hold s->session_mtx
+ */
+
+static int
+ng_btsocket_rfcomm_send_uih(ng_btsocket_rfcomm_session_p s, u_int8_t address,
+		u_int8_t pf, u_int8_t credits, struct mbuf *data)
+{
+	struct rfcomm_frame_hdr	*hdr = NULL;
+	struct mbuf		*m = NULL, *mcrc = NULL;
+	u_int16_t		 length;
+
+	mtx_assert(&s->session_mtx, MA_OWNED);
+
+	MGETHDR(m, M_DONTWAIT, MT_DATA);
+	if (m == NULL) {
+		NG_FREE_M(data);
+		return (ENOBUFS);
+	}
+	m->m_pkthdr.len = m->m_len = sizeof(*hdr);
+
+	MGET(mcrc, M_DONTWAIT, MT_DATA);
+	if (mcrc == NULL) {
+		NG_FREE_M(data);
+		return (ENOBUFS);
+	}
+	mcrc->m_len = 1;
+
+	/* Fill UIH frame header */
+	hdr = mtod(m, struct rfcomm_frame_hdr *);
+	hdr->address = address;
+	hdr->control = RFCOMM_MKCONTROL(RFCOMM_FRAME_UIH, pf);
+
+	/* Calculate FCS */
+	mcrc->m_data[0] = ng_btsocket_rfcomm_fcs2((u_int8_t *) hdr);
+
+	/* Put length back */
+	length = (data != NULL)? data->m_pkthdr.len : 0;
+	if (length > 127) {
+		u_int16_t	l = htole16(RFCOMM_MKLEN16(length));
+
+		bcopy(&l, &hdr->length, sizeof(l));
+		m->m_pkthdr.len ++;
+		m->m_len ++;
+	} else
+		hdr->length = RFCOMM_MKLEN8(length);
+
+	if (pf) {
+		m->m_data[m->m_len] = credits;
+		m->m_pkthdr.len ++;
+		m->m_len ++;
+	}
+
+	/* Add payload */
+	if (data != NULL) {
+		m_cat(m, data);
+		m->m_pkthdr.len += length;
+	}
+
+	/* Put FCS back */
+	m_cat(m, mcrc);
+	m->m_pkthdr.len ++;
+
+	NG_BTSOCKET_RFCOMM_INFO(
+"%s: Sending UIH state=%d, flags=%#x, address=%d, length=%d, pf=%d, " \
+"credits=%d, len=%d\n",
+		__func__, s->state, s->flags, address, length, pf, credits,
+		m->m_pkthdr.len);
+
+	NG_BT_MBUFQ_ENQUEUE(&s->outq, m);
+
+	return (0);
+} /* ng_btsocket_rfcomm_send_uih */
+
+/*
+ * Send MSC request. Caller must hold pcb->pcb_mtx and pcb->session->session_mtx
+ */
+
+static int
+ng_btsocket_rfcomm_send_msc(ng_btsocket_rfcomm_pcb_p pcb)
+{
+	struct mbuf		*m = NULL;
+	struct rfcomm_mcc_hdr	*hdr = NULL;
+	struct rfcomm_mcc_msc	*msc = NULL;
+
+	mtx_assert(&pcb->session->session_mtx, MA_OWNED);
+	mtx_assert(&pcb->pcb_mtx, MA_OWNED);
+
+	MGETHDR(m, M_DONTWAIT, MT_DATA);
+	if (m == NULL)
+		return (ENOBUFS);
+
+	m->m_pkthdr.len = m->m_len = sizeof(*hdr) + sizeof(*msc);
+
+	hdr = mtod(m, struct rfcomm_mcc_hdr *);
+	msc = (struct rfcomm_mcc_msc *)(hdr + 1);
+
+	hdr->type = RFCOMM_MKMCC_TYPE(1, RFCOMM_MCC_MSC);
+	hdr->length = RFCOMM_MKLEN8(sizeof(*msc));
+
+	msc->address = RFCOMM_MKADDRESS(1, pcb->dlci);
+	msc->modem = pcb->lmodem;
+
+	NG_BTSOCKET_RFCOMM_INFO(
+"%s: Sending MSC dlci=%d, state=%d, flags=%#x, address=%d, modem=%#x\n",
+		__func__, pcb->dlci, pcb->state, pcb->flags, msc->address,
+		msc->modem);
+
+	return (ng_btsocket_rfcomm_send_uih(pcb->session,
+			RFCOMM_MKADDRESS(INITIATOR(pcb->session), 0), 0, 0, m));
+} /* ng_btsocket_rfcomm_send_msc */
+
+/*
+ * Send PN request. Caller must hold pcb->pcb_mtx and pcb->session->session_mtx
+ */
+
+static int
+ng_btsocket_rfcomm_send_pn(ng_btsocket_rfcomm_pcb_p pcb)
+{
+	struct mbuf		*m = NULL;
+	struct rfcomm_mcc_hdr	*hdr = NULL;
+	struct rfcomm_mcc_pn	*pn = NULL;
+
+	mtx_assert(&pcb->session->session_mtx, MA_OWNED);
+	mtx_assert(&pcb->pcb_mtx, MA_OWNED);
+
+	MGETHDR(m, M_DONTWAIT, MT_DATA);
+	if (m == NULL)
+		return (ENOBUFS);
+
+	m->m_pkthdr.len = m->m_len = sizeof(*hdr) + sizeof(*pn);
+
+	hdr = mtod(m, struct rfcomm_mcc_hdr *);
+	pn = (struct rfcomm_mcc_pn *)(hdr + 1);
+
+	hdr->type = RFCOMM_MKMCC_TYPE(1, RFCOMM_MCC_PN);
+	hdr->length = RFCOMM_MKLEN8(sizeof(*pn));
+
+	pn->dlci = pcb->dlci;
+	pn->priority = 0;
+	pn->ack_timer = 0;
+	pn->mtu = htole16(pcb->mtu);
+	pn->max_retrans = 0;
+
+	if (pcb->flags & NG_BTSOCKET_RFCOMM_DLC_CFC) {
+		pn->flow_control = 0xf0;
+		pn->credits = pcb->rx_cred;
+	} else {
+		pn->flow_control = 0;
+		pn->credits = 0;
+	}
+
+	NG_BTSOCKET_RFCOMM_INFO(
+"%s: Sending PN dlci=%d, state=%d, flags=%#x, mtu=%d, flow_control=%#x, " \
+"credits=%d\n",	__func__, pcb->dlci, pcb->state, pcb->flags, pcb->mtu,
+		pn->flow_control, pn->credits);
+
+	return (ng_btsocket_rfcomm_send_uih(pcb->session,
+			RFCOMM_MKADDRESS(INITIATOR(pcb->session), 0), 0, 0, m));
+} /* ng_btsocket_rfcomm_send_pn */
+
+/*
+ * Calculate and send credits based on available space in receive buffer
+ */
+
+static int
+ng_btsocket_rfcomm_send_credits(ng_btsocket_rfcomm_pcb_p pcb)
+{
+	int		error = 0;
+	u_int8_t	credits;
+
+	mtx_assert(&pcb->pcb_mtx, MA_OWNED);
+	mtx_assert(&pcb->session->session_mtx, MA_OWNED);
+
+	NG_BTSOCKET_RFCOMM_INFO(
+"%s: Sending more credits, dlci=%d, state=%d, flags=%#x, mtu=%d, " \
+"space=%ld, tx_cred=%d, rx_cred=%d\n",
+		__func__, pcb->dlci, pcb->state, pcb->flags, pcb->mtu,
+		sbspace(&pcb->so->so_rcv), pcb->tx_cred, pcb->rx_cred);
+
+	credits = sbspace(&pcb->so->so_rcv) / pcb->mtu;
+	if (credits > 0) {
+		if (pcb->rx_cred + credits > RFCOMM_MAX_CREDITS)
+			credits = RFCOMM_MAX_CREDITS - pcb->rx_cred;
+
+		error = ng_btsocket_rfcomm_send_uih(
+				pcb->session,
+				RFCOMM_MKADDRESS(INITIATOR(pcb->session),
+					pcb->dlci), 1, credits, NULL);
+		if (error == 0) {
+			pcb->rx_cred += credits;
+
+			NG_BTSOCKET_RFCOMM_INFO(
+"%s: Gave remote side %d more credits, dlci=%d, state=%d, flags=%#x, " \
+"rx_cred=%d, tx_cred=%d\n",	__func__, credits, pcb->dlci, pcb->state,
+				pcb->flags, pcb->rx_cred, pcb->tx_cred);
+		} else
+			NG_BTSOCKET_RFCOMM_ERR(
+"%s: Could not send credits, error=%d, dlci=%d, state=%d, flags=%#x, " \
+"mtu=%d, space=%ld, tx_cred=%d, rx_cred=%d\n",
+				__func__, error, pcb->dlci, pcb->state,
+				pcb->flags, pcb->mtu, sbspace(&pcb->so->so_rcv),
+				pcb->tx_cred, pcb->rx_cred);
+	}
+
+	return (error);
+} /* ng_btsocket_rfcomm_send_credits */
+
+/*****************************************************************************
+ *****************************************************************************
+ **                              RFCOMM DLCs
+ *****************************************************************************
+ *****************************************************************************/
+
+/*
+ * Send data from socket send buffer
+ * Caller must hold pcb->pcb_mtx and pcb->session->session_mtx
+ */
+
+static int
+ng_btsocket_rfcomm_pcb_send(ng_btsocket_rfcomm_pcb_p pcb, int limit)
+{
+	struct mbuf	*m = NULL;
+	int		 sent, length, error;
+
+	mtx_assert(&pcb->session->session_mtx, MA_OWNED);
+	mtx_assert(&pcb->pcb_mtx, MA_OWNED);
+
+	if (pcb->flags & NG_BTSOCKET_RFCOMM_DLC_CFC)
+		limit = min(limit, pcb->tx_cred);
+	else if (!(pcb->rmodem & RFCOMM_MODEM_FC))
+		limit = min(limit, RFCOMM_MAX_CREDITS); /* XXX ??? */
+	else
+		limit = 0;
+
+	if (limit == 0) {
+		NG_BTSOCKET_RFCOMM_INFO(
+"%s: Could not send - remote flow control asserted, dlci=%d, flags=%#x, " \
+"rmodem=%#x, tx_cred=%d\n",
+			__func__, pcb->dlci, pcb->flags, pcb->rmodem,
+			pcb->tx_cred);
+
+		return (0);
+	}
+
+	for (error = 0, sent = 0; sent < limit; sent ++) { 
+		length = min(pcb->mtu, pcb->so->so_snd.sb_cc);
+		if (length == 0)
+			break;
+
+		/* Get the chunk from the socket's send buffer */
+		m = ng_btsocket_rfcomm_prepare_packet(&pcb->so->so_snd, length);
+		if (m == NULL) {
+			error = ENOBUFS;
+			break;
+		}
+
+		sbdrop(&pcb->so->so_snd, length);
+
+		error = ng_btsocket_rfcomm_send_uih(pcb->session,
+				RFCOMM_MKADDRESS(INITIATOR(pcb->session),
+					pcb->dlci), 0, 0, m);
+		if (error != 0)
+			break;
+	}
+
+	if (pcb->flags & NG_BTSOCKET_RFCOMM_DLC_CFC)
+		pcb->tx_cred -= sent;
+
+	if (error == 0 && sent > 0) {
+		pcb->flags &= ~NG_BTSOCKET_RFCOMM_DLC_SENDING;
+		sowwakeup(pcb->so);
+	}
+
+	return (error);
+} /* ng_btsocket_rfcomm_pcb_send */
+
+/*
+ * Unlink and disconnect DLC. If ng_btsocket_rfcomm_pcb_kill() returns
+ * non zero value than socket has no reference and has to be detached.
+ * Caller must hold pcb->pcb_mtx and pcb->session->session_mtx
+ */
+
+static int
+ng_btsocket_rfcomm_pcb_kill(ng_btsocket_rfcomm_pcb_p pcb, int error)
+{
+	ng_btsocket_rfcomm_session_p	s = pcb->session;
+
+	mtx_assert(&pcb->session->session_mtx, MA_OWNED);
+	mtx_assert(&pcb->pcb_mtx, MA_OWNED);
+
+	NG_BTSOCKET_RFCOMM_INFO(
+"%s: Killing DLC, so=%p, dlci=%d, state=%d, flags=%#x, error=%d\n",
+		__func__, pcb->so, pcb->dlci, pcb->state, pcb->flags, error);
+
+	if (pcb->session == NULL)
+		panic("%s: DLC without session, pcb=%p, state=%d, flags=%#x\n",
+			__func__, pcb, pcb->state, pcb->flags);
+
+	if (pcb->flags & NG_BTSOCKET_RFCOMM_DLC_TIMO)
+		ng_btsocket_rfcomm_untimeout(pcb);
+
+	/* Detach DLC from the session. Does not matter which state DLC in */
+	LIST_REMOVE(pcb, session_next);
+	pcb->session = NULL;
+
+	/* Change DLC state and wakeup all sleepers */
+	pcb->state = NG_BTSOCKET_RFCOMM_DLC_CLOSED;
+	pcb->so->so_error = error;
+	soisdisconnected(pcb->so);
+	wakeup(&pcb->state);
+
+	/* Check if we have any DLCs left on the session */
+	if (LIST_EMPTY(&s->dlcs) && INITIATOR(s)) {
+		NG_BTSOCKET_RFCOMM_INFO(
+"%s: Disconnecting session, state=%d, flags=%#x, mtu=%d\n",
+			__func__, s->state, s->flags, s->mtu);
+
+		switch (s->state) {
+		case NG_BTSOCKET_RFCOMM_SESSION_CLOSED:
+		case NG_BTSOCKET_RFCOMM_SESSION_DISCONNECTING:
+			/*
+			 * Do not have to do anything here. We can get here
+			 * when L2CAP connection was terminated or we have 
+			 * received DISC on multiplexor channel
+			 */
+			break;
+
+		case NG_BTSOCKET_RFCOMM_SESSION_OPEN:
+			/* Send DISC on multiplexor channel */
+			error = ng_btsocket_rfcomm_send_command(s,
+					RFCOMM_FRAME_DISC, 0);
+			if (error == 0) {
+				s->state = NG_BTSOCKET_RFCOMM_SESSION_DISCONNECTING;
+				break;
+			}
+			/* FALL THROUGH */
+
+		case NG_BTSOCKET_RFCOMM_SESSION_CONNECTING:
+		case NG_BTSOCKET_RFCOMM_SESSION_CONNECTED:
+			s->state = NG_BTSOCKET_RFCOMM_SESSION_CLOSED;
+			break;
+
+/*		case NG_BTSOCKET_RFCOMM_SESSION_LISTENING: */
+		default:
+			panic("%s: Invalid session state=%d, flags=%#x\n",
+				__func__, s->state, s->flags);
+			break;
+		}
+
+		ng_btsocket_rfcomm_task_wakeup();
+	}
+
+	return (pcb->so->so_state & SS_NOFDREF);
+} /* ng_btsocket_rfcomm_pcb_kill */
+
+/*
+ * Look for RFCOMM socket with given channel and source address
+ */
+
+static ng_btsocket_rfcomm_pcb_p
+ng_btsocket_rfcomm_pcb_by_channel(bdaddr_p src, int channel)
+{
+	ng_btsocket_rfcomm_pcb_p	pcb = NULL;
+
+	mtx_lock(&ng_btsocket_rfcomm_sockets_mtx);
+
+	LIST_FOREACH(pcb, &ng_btsocket_rfcomm_sockets, next)
+		if (pcb->channel == channel &&
+		    bcmp(&pcb->src, src, sizeof(*src)) == 0)
+			break;
+
+	mtx_unlock(&ng_btsocket_rfcomm_sockets_mtx);
+
+	return (pcb);
+} /* ng_btsocket_rfcomm_pcb_by_channel */
+
+/*
+ * Look for given dlci for given RFCOMM session. Caller must hold s->session_mtx
+ */
+
+static ng_btsocket_rfcomm_pcb_p
+ng_btsocket_rfcomm_pcb_by_dlci(ng_btsocket_rfcomm_session_p s, int dlci)
+{
+	ng_btsocket_rfcomm_pcb_p	pcb = NULL;
+
+	mtx_assert(&s->session_mtx, MA_OWNED);
+
+	LIST_FOREACH(pcb, &s->dlcs, session_next)
+		if (pcb->dlci == dlci)
+			break;
+
+	return (pcb);
+} /* ng_btsocket_rfcomm_pcb_by_dlci */
+
+/*
+ * Look for socket that listens on given src address and given channel
+ */
+
+static ng_btsocket_rfcomm_pcb_p
+ng_btsocket_rfcomm_pcb_listener(bdaddr_p src, int channel)
+{
+	ng_btsocket_rfcomm_pcb_p	pcb = NULL, pcb1 = NULL;
+
+	mtx_lock(&ng_btsocket_rfcomm_sockets_mtx);
+
+	LIST_FOREACH(pcb, &ng_btsocket_rfcomm_sockets, next) {
+		if (pcb->channel != channel ||
+		    !(pcb->so->so_options & SO_ACCEPTCONN))
+			continue;
+
+		if (bcmp(&pcb->src, src, sizeof(*src)) == 0)
+			break;
+
+		if (bcmp(&pcb->src, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0)
+			pcb1 = pcb;
+	}
+
+	mtx_unlock(&ng_btsocket_rfcomm_sockets_mtx);
+
+	return ((pcb != NULL)? pcb : pcb1);
+} /* ng_btsocket_rfcomm_pcb_listener */
+
+/*****************************************************************************
+ *****************************************************************************
+ **                              Misc. functions 
+ *****************************************************************************
+ *****************************************************************************/
+
+/*
+ *  Set timeout. Caller MUST hold pcb_mtx
+ */
+
+static void
+ng_btsocket_rfcomm_timeout(ng_btsocket_rfcomm_pcb_p pcb)
+{
+	mtx_assert(&pcb->pcb_mtx, MA_OWNED);
+
+	if (!(pcb->flags & NG_BTSOCKET_RFCOMM_DLC_TIMO)) {
+		pcb->flags |= NG_BTSOCKET_RFCOMM_DLC_TIMO;
+		pcb->flags &= ~NG_BTSOCKET_RFCOMM_DLC_TIMEDOUT;
+		pcb->timo = timeout(ng_btsocket_rfcomm_process_timeout, pcb,
+					ng_btsocket_rfcomm_timo * hz);
+	} else
+		panic("%s: Duplicated socket timeout?!\n", __func__);
+} /* ng_btsocket_rfcomm_timeout */
+
+/*
+ *  Unset pcb timeout. Caller MUST hold pcb_mtx
+ */
+
+static void
+ng_btsocket_rfcomm_untimeout(ng_btsocket_rfcomm_pcb_p pcb)
+{
+	mtx_assert(&pcb->pcb_mtx, MA_OWNED);
+
+	if (pcb->flags & NG_BTSOCKET_RFCOMM_DLC_TIMO) {
+		untimeout(ng_btsocket_rfcomm_process_timeout, pcb, pcb->timo);
+		pcb->flags &= ~NG_BTSOCKET_RFCOMM_DLC_TIMO;
+		pcb->flags &= ~NG_BTSOCKET_RFCOMM_DLC_TIMEDOUT;
+	} else
+		panic("%s: No socket timeout?!\n", __func__);
+} /* ng_btsocket_rfcomm_timeout */
+
+/*
+ * Process pcb timeout
+ */
+
+static void
+ng_btsocket_rfcomm_process_timeout(void *xpcb)
+{
+	ng_btsocket_rfcomm_pcb_p	pcb = (ng_btsocket_rfcomm_pcb_p) xpcb;
+
+	mtx_lock(&pcb->pcb_mtx);
+
+	NG_BTSOCKET_RFCOMM_INFO(
+"%s: Timeout, so=%p, dlci=%d, state=%d, flags=%#x\n",
+		__func__, pcb->so, pcb->dlci, pcb->state, pcb->flags);
+
+	pcb->flags &= ~NG_BTSOCKET_RFCOMM_DLC_TIMO;
+	pcb->flags |= NG_BTSOCKET_RFCOMM_DLC_TIMEDOUT;
+
+	switch (pcb->state) {
+	case NG_BTSOCKET_RFCOMM_DLC_CONFIGURING:
+	case NG_BTSOCKET_RFCOMM_DLC_CONNECTING:
+		pcb->state = NG_BTSOCKET_RFCOMM_DLC_DISCONNECTING;
+		break;
+
+	case NG_BTSOCKET_RFCOMM_DLC_W4_CONNECT:
+	case NG_BTSOCKET_RFCOMM_DLC_DISCONNECTING:
+		break;
+
+	default:
+		panic(
+"%s: DLC timeout in invalid state, dlci=%d, state=%d, flags=%#x\n",
+			__func__, pcb->dlci, pcb->state, pcb->flags);
+		break;
+	}
+
+	ng_btsocket_rfcomm_task_wakeup();
+
+	mtx_unlock(&pcb->pcb_mtx);
+} /* ng_btsocket_rfcomm_process_timeout */
+
+/*
+ * Get up to length bytes from the socket buffer
+ */
+
+static struct mbuf *
+ng_btsocket_rfcomm_prepare_packet(struct sockbuf *sb, int length)
+{
+	struct mbuf	*top = NULL, *m = NULL, *n = NULL, *nextpkt = NULL;
+	int		 mlen, noff, len;
+
+	MGETHDR(top, M_DONTWAIT, MT_DATA);
+	if (top == NULL)
+		return (NULL);
+
+	top->m_pkthdr.len = length;
+	top->m_len = 0;
+	mlen = MHLEN;
+
+	m = top;
+	n = sb->sb_mb;
+	nextpkt = n->m_nextpkt;
+	noff = 0;
+
+	while (length > 0 && n != NULL) {
+		len = min(mlen - m->m_len, n->m_len - noff);
+		if (len > length)
+			len = length;
+
+		bcopy(mtod(n, caddr_t)+noff, mtod(m, caddr_t)+m->m_len, len);
+		m->m_len += len;
+		noff += len;
+		length -= len;
+
+		if (length > 0 && m->m_len == mlen) {
+			MGET(m->m_next, M_DONTWAIT, MT_DATA);
+			if (m->m_next == NULL) {
+				NG_FREE_M(top);
+				return (NULL);
+			}
+
+			m = m->m_next;
+			m->m_len = 0;
+			mlen = MLEN;
+		}
+
+		if (noff == n->m_len) {
+			noff = 0;
+			n = n->m_next;
+
+			if (n == NULL)
+				n = nextpkt;
+
+			nextpkt = (n != NULL)? n->m_nextpkt : NULL;
+		}
+	}
+
+	if (length < 0)
+		panic("%s: length=%d\n", __func__, length);
+	if (length > 0 && n == NULL)
+		panic("%s: bogus length=%d, n=%p\n", __func__, length, n);
+
+	return (top);
+} /* ng_btsocket_rfcomm_prepare_packet */
+