Part one of undating the bluetooth code to the newest version

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

53 files changed, 7187 insertions, 1472 deletions

diff --git a/sys/netgraph/bluetooth/common/ng_bluetooth.c b/sys/netgraph/bluetooth/common/ng_bluetooth.c
index d6604b5..bf4b066 100644
--- a/sys/netgraph/bluetooth/common/ng_bluetooth.c
+++ b/sys/netgraph/bluetooth/common/ng_bluetooth.c
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_bluetooth.c,v 1.1.1.1 2002/09/04 21:47:41 max Exp $
+ * $Id: ng_bluetooth.c,v 1.3 2003/04/26 22:37:31 max Exp $
  * $FreeBSD$
  */
 
@@ -43,7 +43,6 @@
 
 static u_int32_t	bluetooth_hci_command_timeout_value  = 5;   /* sec */
 static u_int32_t	bluetooth_hci_connect_timeout_value  = 60;  /* sec */
-static u_int32_t	bluetooth_hci_watchdog_timeout_value = 60;  /* sec */
 static u_int32_t	bluetooth_hci_max_neighbor_age_value = 600; /* sec */
 static u_int32_t	bluetooth_l2cap_rtx_timeout_value    = 60;  /* sec */
 static u_int32_t	bluetooth_l2cap_ertx_timeout_value   = 300; /* sec */
@@ -111,10 +110,6 @@ SYSCTL_PROC(_net_bluetooth_hci, OID_AUTO, connection_timeout,
 	bluetooth_set_hci_connect_timeout_value,
 	"I", "HCI connect timeout (sec)");
 
-SYSCTL_INT(_net_bluetooth_hci, OID_AUTO, watchdog_timeout, CTLFLAG_RW,
-	&bluetooth_hci_watchdog_timeout_value, 60,
-	"HCI connection watchdog timeout (sec)");
-
 SYSCTL_INT(_net_bluetooth_hci, OID_AUTO, max_neighbor_age, CTLFLAG_RW,
 	&bluetooth_hci_max_neighbor_age_value, 600,
 	"Maximal HCI neighbor cache entry age (sec)");
@@ -192,12 +187,6 @@ bluetooth_hci_connect_timeout(void)
 } /* bluetooth_hci_connect_timeout */
 
 u_int32_t
-bluetooth_hci_watchdog_timeout(void)
-{
-	return (bluetooth_hci_watchdog_timeout_value * hz);
-} /* bluetooth_hci_watchdog_timeout */ 
-
-u_int32_t
 bluetooth_hci_max_neighbor_age(void)
 {
 	return (bluetooth_hci_max_neighbor_age_value);
@@ -215,6 +204,13 @@ bluetooth_l2cap_ertx_timeout(void)
 	return (bluetooth_l2cap_ertx_timeout_value * hz);
 } /* bluetooth_l2cap_ertx_timeout */
 
+/* 
+ * RFCOMM
+ */
+
+SYSCTL_NODE(_net_bluetooth, OID_AUTO, rfcomm, CTLFLAG_RW,
+	0, "Bluetooth RFCOMM family");
+
 /*
  * Handle loading and unloading for this code.
  */
diff --git a/sys/netgraph/bluetooth/drivers/bt3c/ng_bt3c_pccard.c b/sys/netgraph/bluetooth/drivers/bt3c/ng_bt3c_pccard.c
index 443c18a..fe0a241 100644
--- a/sys/netgraph/bluetooth/drivers/bt3c/ng_bt3c_pccard.c
+++ b/sys/netgraph/bluetooth/drivers/bt3c/ng_bt3c_pccard.c
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_bt3c_pccard.c,v 1.2 2002/11/12 00:51:45 max Exp $
+ * $Id: ng_bt3c_pccard.c,v 1.5 2003/04/01 18:15:21 max Exp $
  * $FreeBSD$
  *
  * XXX XXX XX XXX XXX XXX XXX XXX XXX XXX XXX XXX XXX XXX XXX XXX XXX XXX XXX 
@@ -86,7 +86,6 @@ static int	bt3c_pccard_detach	(device_t);
 
 static void	bt3c_intr		(void *);
 static void	bt3c_receive		(bt3c_softc_p);
-static int	bt3c_append		(struct mbuf *, int);
 
 static void	bt3c_swi_intr		(void *);
 static void	bt3c_forward		(node_p, hook_p, void *, int);
@@ -241,8 +240,6 @@ static struct ng_type	typestruct = {
 	ng_bt3c_disconnect,	/* disconnect hook */
         ng_bt3c_cmdlist		/* node command list */
 };
-NETGRAPH_INIT(bt3c, &typestruct);
-MODULE_VERSION(ng_bt3c, NG_BLUETOOTH_VERSION);
 
 /*
  * Netgraph node constructor. Do not allow to create node of this type.
@@ -840,12 +837,24 @@ bt3c_receive(bt3c_softc_p sc)
 				break; /* XXX lost of sync */
 			}
 
+			MCLGET(sc->m, M_DONTWAIT);
+			if (!(sc->m->m_flags & M_EXT)) {
+				NG_FREE_M(sc->m);
+
+				NG_BT3C_ERR(sc->dev, "Could not get cluster\n");
+				NG_BT3C_STAT_IERROR(sc->stat);
+
+				break; /* XXX lost of sync */
+			}
+
 			sc->m->m_len = sc->m->m_pkthdr.len = 0;
 		}
 
 		/* Read and append character to mbuf */
 		bt3c_read_data(sc, c);
-		if (bt3c_append(sc->m, c) != 0) {
+		if (sc->m->m_pkthdr.len >= MCLBYTES) {
+			NG_BT3C_ERR(sc->dev, "Oversized frame\n");
+	
 			NG_FREE_M(sc->m);
 			sc->state = NG_BT3C_W4_PKT_IND;
 			sc->want = 1;
@@ -853,6 +862,9 @@ bt3c_receive(bt3c_softc_p sc)
 			break; /* XXX lost of sync */
 		}
 
+		mtod(sc->m, u_int8_t *)[sc->m->m_len ++] = (u_int8_t) c;
+		sc->m->m_pkthdr.len ++;
+
 		NG_BT3C_INFO(sc->dev,
 "Got char %#x, want=%d, got=%d\n", c, sc->want, sc->m->m_pkthdr.len);
 
@@ -974,43 +986,6 @@ bt3c_receive(bt3c_softc_p sc)
 } /* bt3c_receive */
 
 /*
- * Append character to the mbuf.
- * XXX assumes mbuf has header
- * XXX does not handle external mbuf's
- * XXX always appends char to the end of chain
- */
-
-static int
-bt3c_append(struct mbuf *m0, int c)
-{
-	struct mbuf	*m = m0;
-	int		 len;
-
-	if (m0->m_next == NULL)
-		len = MHLEN;
-	else {
-		len = MLEN;
-
-		while (m->m_next != NULL)
-			m = m->m_next;
-	}
-
-	if (m->m_len >= len) {
-		MGET(m->m_next, M_DONTWAIT, m0->m_type);
-		if (m->m_next == NULL)
-			return (ENOBUFS);
- 
-		m = m->m_next;
-		m->m_len = 0;
-	}
-
-	m->m_data[m->m_len ++] = (char) c;
-	m0->m_pkthdr.len ++;
-
-	return (0);
-} /* bt3c_append */
-
-/*
  * SWI interrupt handler
  * Netgraph part is handled via ng_send_fn() to avoid race with hook
  * connection/disconnection
@@ -1243,5 +1218,37 @@ static driver_t		bt3c_pccard_driver = {
 
 static devclass_t	bt3c_devclass;
 
-DRIVER_MODULE(bt3c, pccard, bt3c_pccard_driver, bt3c_devclass, 0, 0);
+ 
+/*
+ * Load/Unload the driver module
+ */
+ 
+static int
+bt3c_modevent(module_t mod, int event, void *data)
+{
+	int	error;
+ 
+	switch (event) {
+	case MOD_LOAD:
+		error = ng_newtype(&typestruct);
+		if (error != 0)
+			printf("%s: Could not register Netgraph node type, " \
+				"error=%d\n", NG_BT3C_NODE_TYPE, error);
+		break;
+
+	case MOD_UNLOAD:
+		error = ng_rmtype(&typestruct);
+		break;
+
+	default:
+		error = EOPNOTSUPP;
+		break;
+	}
+
+	return (error);
+} /* bt3c_modevent */
+
+DRIVER_MODULE(bt3c, pccard, bt3c_pccard_driver, bt3c_devclass, bt3c_modevent,0);
+MODULE_VERSION(ng_bt3c, NG_BLUETOOTH_VERSION);
+MODULE_DEPEND(ng_bt3c, netgraph, NG_ABI_VERSION, NG_ABI_VERSION,NG_ABI_VERSION);
 
diff --git a/sys/netgraph/bluetooth/drivers/bt3c/ng_bt3c_var.h b/sys/netgraph/bluetooth/drivers/bt3c/ng_bt3c_var.h
index 89ac57c..12ad770 100644
--- a/sys/netgraph/bluetooth/drivers/bt3c/ng_bt3c_var.h
+++ b/sys/netgraph/bluetooth/drivers/bt3c/ng_bt3c_var.h
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_bt3c_var.h,v 1.1 2002/11/09 19:07:56 max Exp $
+ * $Id: ng_bt3c_var.h,v 1.1 2002/11/24 19:46:54 max Exp $
  * $FreeBSD$
  *
  * XXX XXX XX XXX XXX XXX XXX XXX XXX XXX XXX XXX XXX XXX XXX XXX XXX XXX XXX 
diff --git a/sys/netgraph/bluetooth/drivers/h4/TODO b/sys/netgraph/bluetooth/drivers/h4/TODO
index 67cfbba..d106659 100644
--- a/sys/netgraph/bluetooth/drivers/h4/TODO
+++ b/sys/netgraph/bluetooth/drivers/h4/TODO
@@ -1,5 +1,5 @@
-# $FreeBSD$
-$Id: TODO,v 1.6 2002/06/27 09:50:17 max Exp $
+$Id: TODO,v 1.1 2002/11/24 19:46:55 max Exp $
+$FreeBSD$
 
 FIXME/TODO list
 
diff --git a/sys/netgraph/bluetooth/drivers/h4/ng_h4.c b/sys/netgraph/bluetooth/drivers/h4/ng_h4.c
index 650c6f4..6afde39 100644
--- a/sys/netgraph/bluetooth/drivers/h4/ng_h4.c
+++ b/sys/netgraph/bluetooth/drivers/h4/ng_h4.c
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_h4.c,v 1.25 2002/11/03 02:17:31 max Exp $
+ * $Id: ng_h4.c,v 1.4 2003/04/01 18:15:23 max Exp $
  * $FreeBSD$
  * 
  * Based on:
@@ -170,7 +170,7 @@ ng_h4_open(dev_t dev, struct tty *tp)
 	}
 
 	/* Initialize private struct */
-	MALLOC(sc, ng_h4_info_p, sizeof(*sc), M_NETGRAPH_H4, M_WAITOK | M_ZERO);
+	MALLOC(sc, ng_h4_info_p, sizeof(*sc), M_NETGRAPH_H4, M_ZERO);
 	if (sc == NULL) {
 		error = ENOMEM;
 		goto out;
diff --git a/sys/netgraph/bluetooth/drivers/h4/ng_h4_prse.h b/sys/netgraph/bluetooth/drivers/h4/ng_h4_prse.h
index a4ba75d..7ed646e 100644
--- a/sys/netgraph/bluetooth/drivers/h4/ng_h4_prse.h
+++ b/sys/netgraph/bluetooth/drivers/h4/ng_h4_prse.h
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_h4_prse.h,v 1.6 2002/09/04 21:35:01 max Exp $
+ * $Id: ng_h4_prse.h,v 1.1 2002/11/24 19:46:55 max Exp $
  * $FreeBSD$
  */
 
diff --git a/sys/netgraph/bluetooth/drivers/h4/ng_h4_var.h b/sys/netgraph/bluetooth/drivers/h4/ng_h4_var.h
index 1b0d5b8..f6ac7fe 100644
--- a/sys/netgraph/bluetooth/drivers/h4/ng_h4_var.h
+++ b/sys/netgraph/bluetooth/drivers/h4/ng_h4_var.h
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_h4_var.h,v 1.14 2002/11/03 02:16:31 max Exp $
+ * $Id: ng_h4_var.h,v 1.1 2002/11/24 19:46:55 max Exp $
  * $FreeBSD$
  * 
  * Based on:
diff --git a/sys/netgraph/bluetooth/drivers/ubt/TODO b/sys/netgraph/bluetooth/drivers/ubt/TODO
index aba45ed..90f9d1d 100644
--- a/sys/netgraph/bluetooth/drivers/ubt/TODO
+++ b/sys/netgraph/bluetooth/drivers/ubt/TODO
@@ -1,5 +1,5 @@
-# $FreeBSD$
-$Id: TODO,v 1.1.1.1 2002/06/09 20:21:47 max Exp $
+$Id: TODO,v 1.1 2002/11/24 19:46:56 max Exp $
+$FreeBSD$
 
 1) SMP/Locking
 
diff --git a/sys/netgraph/bluetooth/drivers/ubt/ng_ubt.c b/sys/netgraph/bluetooth/drivers/ubt/ng_ubt.c
index 603d722..d2ef0f1 100644
--- a/sys/netgraph/bluetooth/drivers/ubt/ng_ubt.c
+++ b/sys/netgraph/bluetooth/drivers/ubt/ng_ubt.c
@@ -25,22 +25,22 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_ubt.c,v 1.1 2002/11/09 19:09:02 max Exp $
+ * $Id: ng_ubt.c,v 1.14 2003/04/14 23:00:50 max Exp $
  * $FreeBSD$
  */
 
 #include <sys/param.h>
 #include <sys/systm.h>
 #include <sys/bus.h>
+#include <sys/conf.h>
 #include <sys/endian.h>
-#include <sys/interrupt.h>
+#include <sys/filio.h>
+#include <sys/fcntl.h>
 #include <sys/mbuf.h>
 #include <sys/malloc.h>
 #include <sys/kernel.h>
-
-#include <sys/socket.h>
-#include <net/if.h>
-#include <net/if_var.h>
+#include <sys/poll.h>
+#include <sys/vnode.h>
 
 #include <dev/usb/usb.h>
 #include <dev/usb/usbdi.h>
@@ -62,33 +62,39 @@
 
 USB_DECLARE_DRIVER(ubt);
 
-Static int         ubt_modevent		 (module_t, int, void *);
+Static int         ubt_modevent		  (module_t, int, void *);
 
-Static usbd_status ubt_request_start     (ubt_softc_p, struct mbuf *);
-Static void        ubt_request_complete  (usbd_xfer_handle, 
-					  usbd_private_handle, usbd_status);
+Static usbd_status ubt_request_start      (ubt_softc_p);
+Static void        ubt_request_complete   (usbd_xfer_handle, 
+					   usbd_private_handle, usbd_status);
+Static void        ubt_request_complete2  (node_p, hook_p, void *, int);
 
-Static usbd_status ubt_intr_start	 (ubt_softc_p);
-Static void        ubt_intr_complete     (usbd_xfer_handle, 
-					  usbd_private_handle, usbd_status);
+Static usbd_status ubt_intr_start	  (ubt_softc_p);
+Static void        ubt_intr_complete      (usbd_xfer_handle, 
+					   usbd_private_handle, usbd_status);
+Static void        ubt_intr_complete2     (node_p, hook_p, void *, int); 
 
-Static usbd_status ubt_bulk_in_start	 (ubt_softc_p);
-Static void        ubt_bulk_in_complete  (usbd_xfer_handle, 
-					  usbd_private_handle, usbd_status);
+Static usbd_status ubt_bulk_in_start	  (ubt_softc_p);
+Static void        ubt_bulk_in_complete   (usbd_xfer_handle, 
+					   usbd_private_handle, usbd_status);
+Static void        ubt_bulk_in_complete2  (node_p, hook_p, void *, int);
 
-Static usbd_status ubt_bulk_out_start    (ubt_softc_p, struct mbuf *);
-Static void        ubt_bulk_out_complete (usbd_xfer_handle, 
-					  usbd_private_handle, usbd_status);
+Static usbd_status ubt_bulk_out_start     (ubt_softc_p);
+Static void        ubt_bulk_out_complete  (usbd_xfer_handle, 
+					   usbd_private_handle, usbd_status);
+Static void        ubt_bulk_out_complete2 (node_p, hook_p, void *, int); 
 
-Static usbd_status ubt_isoc_in_start     (ubt_softc_p);
-Static void        ubt_isoc_in_complete  (usbd_xfer_handle, 
-					  usbd_private_handle, usbd_status);
+Static usbd_status ubt_isoc_in_start      (ubt_softc_p);
+Static void        ubt_isoc_in_complete   (usbd_xfer_handle, 
+					   usbd_private_handle, usbd_status);
+Static void        ubt_isoc_in_complete2  (node_p, hook_p, void *, int);
 
-Static usbd_status ubt_isoc_out_start    (ubt_softc_p, struct mbuf *);
-Static void        ubt_isoc_out_complete (usbd_xfer_handle, 
-					  usbd_private_handle, usbd_status);
+Static usbd_status ubt_isoc_out_start     (ubt_softc_p);
+Static void        ubt_isoc_out_complete  (usbd_xfer_handle, 
+					   usbd_private_handle, usbd_status);
+Static void        ubt_isoc_out_complete2 (node_p, hook_p, void *, int);
 
-Static void        ubt_swi_intr          (void *);
+Static void        ubt_reset              (ubt_softc_p);
 
 /*
  * Netgraph methods
@@ -101,7 +107,6 @@ Static ng_connect_t	ng_ubt_connect;
 Static ng_disconnect_t	ng_ubt_disconnect;
 Static ng_rcvmsg_t	ng_ubt_rcvmsg;
 Static ng_rcvdata_t	ng_ubt_rcvdata;
-Static void		ng_ubt_reset	(ubt_softc_p);
 
 /* Queue length */
 Static const struct ng_parse_struct_field	ng_ubt_node_qlen_type_fields[] =
@@ -175,6 +180,13 @@ Static const struct ng_cmdlist	ng_ubt_cmdlist[] = {
         NULL,
 	NULL
 },
+{
+	NGM_UBT_COOKIE,
+	NGM_UBT_NODE_DEV_NODES,
+	"dev_nodes",
+        &ng_parse_uint16_type,
+	NULL
+},
 { 0, }
 };
 
@@ -191,7 +203,42 @@ Static struct ng_type	typestruct = {
 	ng_ubt_connect,		/* connect hook */
 	ng_ubt_rcvdata,		/* data */
 	ng_ubt_disconnect,	/* disconnect hook */
-	ng_ubt_cmdlist		/* node command list */
+	ng_ubt_cmdlist,		/* node command list */
+};
+
+/*
+ * Device methods
+ */
+
+#define UBT_UNIT(n)	((minor(n) >> 4) & 0xf)
+#define UBT_ENDPOINT(n)	(minor(n) & 0xf)
+#define UBT_MINOR(u, e)	(((u) << 4) | (e))
+#define UBT_BSIZE	1024
+
+Static d_open_t		ubt_open;
+Static d_close_t	ubt_close;
+Static d_read_t		ubt_read;
+Static d_write_t	ubt_write;
+Static d_ioctl_t	ubt_ioctl;
+Static d_poll_t		ubt_poll;
+Static void		ubt_create_device_nodes  (ubt_softc_p);
+Static void		ubt_destroy_device_nodes (ubt_softc_p);
+
+#if __FreeBSD_version < 500104
+#define CDEV_MAJOR	222
+#else
+#define CDEV_MAJOR	MAJOR_AUTO
+#endif
+
+Static struct cdevsw	ubt_cdevsw = {
+	.d_open =	ubt_open,
+	.d_close =	ubt_close,
+	.d_read =	ubt_read,
+	.d_write =	ubt_write,
+	.d_ioctl =	ubt_ioctl,
+	.d_poll =	ubt_poll,
+	.d_name =	"ubt",
+	.d_maj =	CDEV_MAJOR,
 };
 
 /*
@@ -221,8 +268,9 @@ ubt_modevent(module_t mod, int event, void *data)
 	case MOD_LOAD:
 		error = ng_newtype(&typestruct);
 		if (error != 0)
-			printf("%s: Could not register Netgraph node type, " \
-				"error=%d\n", NG_UBT_NODE_TYPE, error);
+			printf(
+"%s: Could not register Netgraph node type, error=%d\n",
+				NG_UBT_NODE_TYPE, error);
 		else
 			error = usbd_driver_load(mod, event, data);
 		break;
@@ -247,23 +295,48 @@ ubt_modevent(module_t mod, int event, void *data)
 
 USB_MATCH(ubt)
 {
-	Static struct usb_devno const	ubt_devices[] = {
-		{ USB_VENDOR_3COM,     USB_PRODUCT_3COM_3CREB96 },
-		{ USB_VENDOR_MITSUMI,  USB_PRODUCT_MITSUMI_BT_DONGLE },
-		{ USB_VENDOR_TDK,      USB_PRODUCT_TDK_BT_DONGLE },
-		{ USB_VENDOR_MSI,      USB_PRODUCT_MSI_BT_DONGLE },
-		{ USB_VENDOR_BROADCOM, USB_PRODUCT_DBW_120M_BT_DONGLE },
-		{ USB_VENDOR_EPOX,     USB_PRODUCT_BT_DG02_DONGLE },
-		{ 0, 0 }
+	/*
+	 * If for some reason device should not be attached then put
+	 * VendorID/ProductID pair into the list below. Currently I
+	 * do not know of any such devices. The format is as follows:
+	 *
+	 *	{ VENDOR_ID, PRODUCT_ID },
+	 *
+	 * where VENDOR_ID and PRODUCT_ID are hex numbers.
+	 */
+
+	Static struct usb_devno const	ubt_ignored_devices[] = {
+		{ 0, 0 } /* This should be the last item in the list */
+	};
+
+	/*
+	 * If device violates Bluetooth specification and has bDeviceClass,
+	 * bDeviceSubClass and bDeviceProtocol set to wrong values then you
+	 * could try to put VendorID/ProductID pair into the list below. 
+	 * Currently I do not know of any such devices.
+	 */
+
+	Static struct usb_devno const	ubt_broken_devices[] = {
+		{ 0, 0 } /* This should be the last item in the list */
 	};
 
 	USB_MATCH_START(ubt, uaa);
 
-	if (uaa->iface == NULL || 
-	    usb_lookup(ubt_devices, uaa->vendor, uaa->product) == NULL)
+	usb_device_descriptor_t	*dd = usbd_get_device_descriptor(uaa->device);
+
+	if (uaa->iface == NULL ||
+	    usb_lookup(ubt_ignored_devices, uaa->vendor, uaa->product))
 		return (UMATCH_NONE);
+	
+	if (dd->bDeviceClass == UDCLASS_WIRELESS &&
+	    dd->bDeviceSubClass == UDSUBCLASS_RF &&
+	    dd->bDeviceProtocol == UDPROTO_BLUETOOTH)
+		return (UMATCH_DEVCLASS_DEVSUBCLASS);
+
+	if (usb_lookup(ubt_broken_devices, uaa->vendor, uaa->product))
+		return (UMATCH_VENDOR_PRODUCT);
 
-	return (UMATCH_VENDOR_PRODUCT);
+	return (UMATCH_NONE);
 } /* USB_MATCH(ubt) */
 
 /*
@@ -276,7 +349,7 @@ USB_ATTACH(ubt)
 	usb_config_descriptor_t		*cd = NULL;
 	usb_interface_descriptor_t	*id = NULL;
 	usb_endpoint_descriptor_t	*ed = NULL;
-	char				 devinfo[1024];
+	char				 devinfo[UBT_BSIZE];
 	usbd_status			 error;
 	int				 i, ai, alt_no, isoc_in, isoc_out,
 					 isoc_isize, isoc_osize;
@@ -299,11 +372,6 @@ USB_ATTACH(ubt)
 	/* Interfaces */
 	sc->sc_iface0 = sc->sc_iface1 = NULL;
 
-	/* Input queue */
-	bzero(&sc->sc_inq, sizeof(sc->sc_inq));
-	sc->sc_inq.ifq_maxlen = UBT_DEFAULT_QLEN;
-	mtx_init(&sc->sc_inq.ifq_mtx, "UBT inq", NULL, MTX_DEF);
-
 	/* Interrupt pipe */
 	sc->sc_intr_ep = -1;
 	sc->sc_intr_pipe = NULL;
@@ -313,9 +381,7 @@ USB_ATTACH(ubt)
 	/* Control pipe */
 	sc->sc_ctrl_xfer = NULL;
 	sc->sc_ctrl_buffer = NULL;
-	bzero(&sc->sc_cmdq, sizeof(sc->sc_cmdq));
-	sc->sc_cmdq.ifq_maxlen = UBT_DEFAULT_QLEN;
-	mtx_init(&sc->sc_cmdq.ifq_mtx, "UBT cmdq", NULL, MTX_DEF);
+	NG_BT_MBUFQ_INIT(&sc->sc_cmdq, UBT_DEFAULT_QLEN);
 
 	/* Bulk-in pipe */
 	sc->sc_bulk_in_ep = -1;
@@ -328,9 +394,7 @@ USB_ATTACH(ubt)
 	sc->sc_bulk_out_pipe = NULL;
 	sc->sc_bulk_out_xfer = NULL;
 	sc->sc_bulk_out_buffer = NULL;
-	bzero(&sc->sc_aclq, sizeof(sc->sc_aclq));
-	sc->sc_aclq.ifq_maxlen = UBT_DEFAULT_QLEN;
-	mtx_init(&sc->sc_aclq.ifq_mtx, "UBT aclq", NULL, MTX_DEF);
+	NG_BT_MBUFQ_INIT(&sc->sc_aclq, UBT_DEFAULT_QLEN);
 
 	/* Isoc-in pipe */
 	sc->sc_isoc_in_ep = -1;
@@ -342,21 +406,15 @@ USB_ATTACH(ubt)
 	sc->sc_isoc_out_pipe = NULL;
 	sc->sc_isoc_out_xfer = NULL;
 	sc->sc_isoc_size = -1;
-	bzero(&sc->sc_scoq, sizeof(sc->sc_scoq));
-	sc->sc_scoq.ifq_maxlen = UBT_DEFAULT_QLEN;
-	mtx_init(&sc->sc_scoq.ifq_mtx, "UBT scoq", NULL, MTX_DEF);
+	NG_BT_MBUFQ_INIT(&sc->sc_scoq, UBT_DEFAULT_QLEN);
 
 	/* Netgraph part */
 	sc->sc_node = NULL;
 	sc->sc_hook = NULL;
 
-	/* Attach SWI handler to TTY SWI thread */
-	sc->sc_ith = NULL;
-	if (swi_add(&tty_ithd, USBDEVNAME(sc->sc_dev),
-			ubt_swi_intr, sc, SWI_TTY, 0, &sc->sc_ith) < 0) {  
-		printf("%s: Could not setup SWI ISR\n", USBDEVNAME(sc->sc_dev));
-		goto bad;
-	}
+	/* Device part */
+	sc->sc_ctrl_dev = sc->sc_intr_dev = sc->sc_bulk_dev = NODEV;
+	sc->sc_refcnt = sc->sc_dying = 0;
 
 	/*
 	 * XXX set configuration?
@@ -396,16 +454,6 @@ USB_ATTACH(ubt)
 			USBDEVNAME(sc->sc_dev));
 		goto bad;
 	}
-	if (id->bInterfaceClass != UICLASS_WIRELESS_CONTROLLER ||
-	    id->bInterfaceSubClass != UISUBCLASS_RF_CONTROLLER ||
-	    id->bInterfaceProtocol != UIPROTO_BLUETOOTH) {
-		printf("%s: Interface 0 is not supported, " \
-			"bInterfaceClass=%#x, bInterfaceSubClass=%#x, "\
-			"bInterfaceProtocol=%#x\n", USBDEVNAME(sc->sc_dev),
-			id->bInterfaceClass, id->bInterfaceSubClass, 
-			id->bInterfaceProtocol);
-		goto bad;
-	}
 
 	for (i = 0; i < id->bNumEndpoints; i ++) {
 		ed = usbd_interface2endpoint_descriptor(sc->sc_iface0, i);
@@ -476,16 +524,6 @@ USB_ATTACH(ubt)
 			USBDEVNAME(sc->sc_dev));
 		goto bad;
 	}
-	if (id->bInterfaceClass != UICLASS_WIRELESS_CONTROLLER ||
-	    id->bInterfaceSubClass != UISUBCLASS_RF_CONTROLLER ||
-	    id->bInterfaceProtocol != UIPROTO_BLUETOOTH) {
-		printf("%s: Interface 1 is not supported, " \
-			"bInterfaceClass=%#x, bInterfaceSubClass=%#x, "\
-			"bInterfaceProtocol=%#x\n", USBDEVNAME(sc->sc_dev),
-			id->bInterfaceClass, id->bInterfaceSubClass, 
-			id->bInterfaceProtocol);
-		goto bad;
-	}
 
 	/*
 	 * Scan all alternate configurations for interface 1
@@ -596,13 +634,6 @@ USB_ATTACH(ubt)
 			USBDEVNAME(sc->sc_dev));
 		goto bad;
 	}
-	sc->sc_intr_buffer = usbd_alloc_buffer(sc->sc_intr_xfer, 
-						UBT_INTR_BUFFER_SIZE);
-	if (sc->sc_intr_buffer == NULL) {
-		printf("%s: Could not allocate interrupt buffer\n",
-			USBDEVNAME(sc->sc_dev));
-		goto bad;
-	}
 
 	sc->sc_bulk_in_xfer = usbd_alloc_xfer(sc->sc_udev);
 	if (sc->sc_bulk_in_xfer == NULL) {
@@ -610,13 +641,6 @@ USB_ATTACH(ubt)
 			USBDEVNAME(sc->sc_dev));
 		goto bad;
 	}
-	sc->sc_bulk_in_buffer = usbd_alloc_buffer(sc->sc_bulk_in_xfer,
-						UBT_BULK_BUFFER_SIZE);
-	if (sc->sc_bulk_in_buffer == NULL) {
-		printf("%s: Could not allocate bulk-in buffer\n",
-			USBDEVNAME(sc->sc_dev));
-		goto bad;
-	}
 
 	sc->sc_bulk_out_xfer = usbd_alloc_xfer(sc->sc_udev);
 	if (sc->sc_bulk_out_xfer == NULL) {
@@ -686,6 +710,62 @@ USB_ATTACH(ubt)
 		sc->sc_isoc_out_ep, sc->sc_isoc_size, sc->sc_isoc_nframes, 
 		(sc->sc_isoc_nframes * sc->sc_isoc_size));
 
+	/*
+	 * Open pipes
+	 */
+
+	/* Interrupt */	
+	error = usbd_open_pipe(sc->sc_iface0, sc->sc_intr_ep,
+			USBD_EXCLUSIVE_USE, &sc->sc_intr_pipe);
+	if (error != USBD_NORMAL_COMPLETION) {
+		printf("%s: %s - Could not open interrupt pipe. %s (%d)\n",
+			__func__, USBDEVNAME(sc->sc_dev), usbd_errstr(error),
+			error);
+		goto bad;
+	}
+
+	/* Bulk-in */
+	error = usbd_open_pipe(sc->sc_iface0, sc->sc_bulk_in_ep,
+			USBD_EXCLUSIVE_USE, &sc->sc_bulk_in_pipe);
+	if (error != USBD_NORMAL_COMPLETION) {
+		printf("%s: %s - Could not open bulk-in pipe. %s (%d)\n",
+			__func__,  USBDEVNAME(sc->sc_dev), usbd_errstr(error),
+			error);
+		goto bad;
+	}
+
+	/* Bulk-out */
+	error = usbd_open_pipe(sc->sc_iface0, sc->sc_bulk_out_ep,
+			USBD_EXCLUSIVE_USE, &sc->sc_bulk_out_pipe);
+	if (error != USBD_NORMAL_COMPLETION) {
+		printf("%s: %s - Could not open bulk-out pipe. %s (%d)\n",
+			__func__, USBDEVNAME(sc->sc_dev), usbd_errstr(error),
+			error);
+		goto bad;
+	}
+
+#if __broken__ /* XXX FIXME */
+	/* Isoc-in */
+	error = usbd_open_pipe(sc->sc_iface1, sc->sc_isoc_in_ep,
+			USBD_EXCLUSIVE_USE, &sc->sc_isoc_in_pipe);
+	if (error != USBD_NORMAL_COMPLETION) {
+		printf("%s: %s - Could not open isoc-in pipe. %s (%d)\n",
+			__func__, USBDEVNAME(sc->sc_dev), usbd_errstr(error),
+			error);
+		goto bad;
+	}
+
+	/* Isoc-out */
+	error = usbd_open_pipe(sc->sc_iface1, sc->sc_isoc_out_ep, 
+			USBD_EXCLUSIVE_USE, &sc->sc_isoc_out_pipe);
+	if (error != USBD_NORMAL_COMPLETION) {
+		printf("%s: %s - Could not open isoc-out pipe. %s (%d)\n",
+			__func__, USBDEVNAME(sc->sc_dev), usbd_errstr(error),
+			error);
+		goto bad;
+	}
+#endif /* __broken__ */
+
 	/* Create Netgraph node */
 	if (ng_make_node_common(&typestruct, &sc->sc_node) != 0) {
 		printf("%s: Could not create Netgraph node\n",
@@ -704,15 +784,15 @@ USB_ATTACH(ubt)
 	}
 
 	NG_NODE_SET_PRIVATE(sc->sc_node, sc);
+	NG_NODE_FORCE_WRITER(sc->sc_node);
 
-	/*
-	 * XXX Is that correct?
-	 * Claim all interfaces on the device
-	 */
-
+	/* Claim all interfaces on the device */
 	for (i = 0; i < uaa->nifaces; i++)
 		uaa->ifaces[i] = NULL;
 
+	usbd_add_drv_event(USB_EVENT_DRIVER_ATTACH, sc->sc_udev,
+		USBDEV(sc->sc_dev));
+
 	USB_ATTACH_SUCCESS_RETURN;
 bad:
 	ubt_detach(self);
@@ -728,7 +808,9 @@ USB_DETACH(ubt)
 {
 	USB_DETACH_START(ubt, sc);
 
-	ng_ubt_reset(sc);
+	sc->sc_dying = 1;
+
+	ubt_destroy_device_nodes(sc); /* XXX FIXME locking? */
 
 	/* Destroy Netgraph node */
 	if (sc->sc_node != NULL) {
@@ -737,6 +819,30 @@ USB_DETACH(ubt)
 		sc->sc_node = NULL;
 	}
 
+	/* Close pipes */
+	if (sc->sc_intr_pipe != NULL) {
+		usbd_close_pipe(sc->sc_intr_pipe);
+		sc->sc_intr_pipe = NULL;
+	}
+
+	if (sc->sc_bulk_in_pipe != NULL) {
+		usbd_close_pipe(sc->sc_bulk_in_pipe);
+		sc->sc_bulk_in_pipe = NULL;
+	}
+	if (sc->sc_bulk_out_pipe != NULL) {
+		usbd_close_pipe(sc->sc_bulk_out_pipe);
+		sc->sc_bulk_out_pipe = NULL;
+	}
+
+	if (sc->sc_isoc_in_pipe != NULL) {
+		usbd_close_pipe(sc->sc_isoc_in_pipe);
+		sc->sc_isoc_in_pipe = NULL;
+	}
+	if (sc->sc_isoc_out_pipe != NULL) {
+		usbd_close_pipe(sc->sc_isoc_out_pipe);
+		sc->sc_isoc_out_pipe = NULL;
+	}
+
 	/* Destroy USB transfer handles */
 	if (sc->sc_ctrl_xfer != NULL) {
 		usbd_free_xfer(sc->sc_ctrl_xfer);
@@ -776,70 +882,43 @@ USB_DETACH(ubt)
 		sc->sc_isoc_out_frlen = NULL;
 	}
 
-	if (sc->sc_ith != NULL) {
-		ithread_remove_handler(sc->sc_ith);
-		sc->sc_ith = NULL;
-	}
-
 	/* Destroy queues */
-	IF_DRAIN(&sc->sc_cmdq);
-	IF_DRAIN(&sc->sc_aclq);
-	IF_DRAIN(&sc->sc_scoq);
-	IF_DRAIN(&sc->sc_inq);
+	NG_BT_MBUFQ_DRAIN(&sc->sc_cmdq);
+	NG_BT_MBUFQ_DRAIN(&sc->sc_aclq);
+	NG_BT_MBUFQ_DRAIN(&sc->sc_scoq);
 
-	mtx_destroy(&sc->sc_cmdq.ifq_mtx);
-	mtx_destroy(&sc->sc_aclq.ifq_mtx);
-	mtx_destroy(&sc->sc_scoq.ifq_mtx);
-	mtx_destroy(&sc->sc_inq.ifq_mtx);
+	usbd_add_drv_event(USB_EVENT_DRIVER_DETACH, sc->sc_udev,
+			USBDEV(sc->sc_dev));
 
 	return (0);
 } /* USB_DETACH(ubt) */
 
 /*
- * Start USB control request (HCI command)
+ * Start USB control request (HCI command). Must be called with node locked
  */
 
 Static usbd_status
-ubt_request_start(ubt_softc_p sc, struct mbuf *m)
+ubt_request_start(ubt_softc_p sc)
 {
-	usb_device_request_t	req;
-	usbd_status		status = USBD_NORMAL_COMPLETION;
-
-	IF_LOCK(&sc->sc_cmdq);
-
-	if (m != NULL) {
-		if (_IF_QFULL(&sc->sc_cmdq)) {
-			NG_UBT_ERR(
-"%s: %s - Dropping HCI command frame, len=%d. Queue full\n",
-				__func__, USBDEVNAME(sc->sc_dev), 
-				m->m_pkthdr.len);
-
-			_IF_DROP(&sc->sc_cmdq);
-			NG_UBT_STAT_OERROR(sc->sc_stat);
-
-			NG_FREE_M(m);
-		} else
-			_IF_ENQUEUE(&sc->sc_cmdq, m);
-	} else
-		sc->sc_flags &= ~UBT_CMD_XMIT;
+	usb_device_request_t	 req;
+	struct mbuf		*m = NULL;
+	usbd_status		 status;
 
-	if (sc->sc_flags & UBT_CMD_XMIT) {
-		NG_UBT_INFO(
+	KASSERT(!(sc->sc_flags & UBT_CMD_XMIT), (
 "%s: %s - Another control request is pending\n",
-			__func__, USBDEVNAME(sc->sc_dev));
-		goto done;
-	}
+		__func__, USBDEVNAME(sc->sc_dev)));
 
-	_IF_DEQUEUE(&sc->sc_cmdq, m);
+	NG_BT_MBUFQ_DEQUEUE(&sc->sc_cmdq, m);
 	if (m == NULL) {
 		NG_UBT_INFO(
 "%s: %s - HCI command queue is empty\n", __func__, USBDEVNAME(sc->sc_dev));
-		goto done;
+
+		return (USBD_NORMAL_COMPLETION);
 	}
 
 	/*
-	 * Check HCI command frame size and copy it back
-	 *  to linear USB transfer buffer.
+	 * Check HCI command frame size and copy it back to 
+	 * linear USB transfer buffer.
 	 */ 
 
 	if (m->m_pkthdr.len > UBT_CTRL_BUFFER_SIZE)
@@ -851,7 +930,6 @@ ubt_request_start(ubt_softc_p sc, struct mbuf *m)
 	m_copydata(m, 0, m->m_pkthdr.len, sc->sc_ctrl_buffer);
 
 	/* Initialize a USB control request and then schedule it */
-
 	bzero(&req, sizeof(req));
 	req.bmRequestType = UBT_HCI_REQUEST;
 	USETW(req.wLength, m->m_pkthdr.len);
@@ -864,38 +942,39 @@ ubt_request_start(ubt_softc_p sc, struct mbuf *m)
 	usbd_setup_default_xfer(
 		sc->sc_ctrl_xfer,
 		sc->sc_udev,
-		(usbd_private_handle) sc,
-		USBD_DEFAULT_TIMEOUT,	/* XXX */
+		(usbd_private_handle) sc->sc_node,
+		USBD_DEFAULT_TIMEOUT, /* XXX */
 		&req,
 		sc->sc_ctrl_buffer,
 		m->m_pkthdr.len,
 		USBD_NO_COPY,
 		ubt_request_complete);
 
+	NG_NODE_REF(sc->sc_node);
+
 	status = usbd_transfer(sc->sc_ctrl_xfer);
-	if (status && status != USBD_IN_PROGRESS) {
+	if (status != USBD_NORMAL_COMPLETION && status != USBD_IN_PROGRESS) {
 		NG_UBT_ERR(
 "%s: %s - Could not start control request. %s (%d)\n",
 			__func__, USBDEVNAME(sc->sc_dev),
 			usbd_errstr(status), status);
 
-			_IF_DROP(&sc->sc_cmdq); /* XXX */
-			NG_UBT_STAT_OERROR(sc->sc_stat);
+		NG_NODE_UNREF(sc->sc_node);
 
-		/* XXX FIXME: should we try to resubmit another request? */
+		NG_BT_MBUFQ_DROP(&sc->sc_cmdq);
+		NG_UBT_STAT_OERROR(sc->sc_stat);
+
+		/* XXX FIXME should we try to resubmit another request? */
 	} else {
 		NG_UBT_INFO(
 "%s: %s - Control request has been started\n",
 			__func__, USBDEVNAME(sc->sc_dev));
 
 		sc->sc_flags |= UBT_CMD_XMIT;
-
 		status = USBD_NORMAL_COMPLETION;
 	}
 
 	NG_FREE_M(m);
-done:
-	IF_UNLOCK(&sc->sc_cmdq);
 
 	return (status);
 } /* ubt_request_start */
@@ -907,7 +986,24 @@ done:
 Static void
 ubt_request_complete(usbd_xfer_handle h, usbd_private_handle p, usbd_status s)
 {
-	ubt_softc_p	sc = (ubt_softc_p) p;
+	ng_send_fn((node_p) p, NULL, ubt_request_complete2, (void *) h, s);
+	NG_NODE_UNREF((node_p) p);
+} /* ubt_request_complete */
+
+Static void
+ubt_request_complete2(node_p node, hook_p hook, void *arg1, int arg2)
+{ 
+	ubt_softc_p		sc = (ubt_softc_p) NG_NODE_PRIVATE(node);
+	usbd_xfer_handle	h = (usbd_xfer_handle) arg1;
+	usbd_status		s = (usbd_status) arg2;
+
+	if (sc == NULL)
+		return;
+
+	KASSERT((sc->sc_flags & UBT_CMD_XMIT), (
+"%s: %s - No control request is pending\n", __func__, USBDEVNAME(sc->sc_dev)));
+
+	sc->sc_flags &= ~UBT_CMD_XMIT;
 
 	if (s == USBD_CANCELLED) {
 		NG_UBT_INFO(
@@ -934,39 +1030,71 @@ ubt_request_complete(usbd_xfer_handle h, usbd_private_handle p, usbd_status s)
 		NG_UBT_STAT_PCKTS_SENT(sc->sc_stat);
 	}
 
-	ubt_request_start(sc, NULL /* completed request */);
-} /* ubt_request_complete */
+	if (NG_BT_MBUFQ_LEN(&sc->sc_cmdq) > 0)
+		ubt_request_start(sc);
+} /* ubt_request_complete2 */
 
 /*
- * Start interrupt transfer
+ * Start interrupt transfer. Must be called when node is locked
  */
 
 Static usbd_status
 ubt_intr_start(ubt_softc_p sc)
 {
-	usbd_status	status;
+	struct mbuf	*m = NULL;
+	usbd_status	 status;
 
+	KASSERT(!(sc->sc_flags & UBT_EVT_RECV), (
+"%s: %s - Another interrupt request is pending\n",
+		__func__, USBDEVNAME(sc->sc_dev)));
+
+	/* Allocate new mbuf cluster */
+	MGETHDR(m, M_DONTWAIT, MT_DATA);
+	if (m == NULL)
+		return (USBD_NOMEM);
+
+	MCLGET(m, M_DONTWAIT);
+	if (!(m->m_flags & M_EXT)) {
+		NG_FREE_M(m);
+		return (USBD_NOMEM);
+	}
+
+	if (!(sc->sc_flags & UBT_HAVE_FRAME_TYPE)) {
+		*mtod(m, u_int8_t *) = NG_HCI_EVENT_PKT;
+		m->m_pkthdr.len = m->m_len = 1;
+	} else
+		m->m_pkthdr.len = m->m_len = 0;
+	
 	/* Initialize a USB transfer and then schedule it */
 	usbd_setup_xfer(
 			sc->sc_intr_xfer,
 			sc->sc_intr_pipe,
-			(usbd_private_handle) sc,
-			sc->sc_intr_buffer,
-			UBT_INTR_BUFFER_SIZE,
-			USBD_SHORT_XFER_OK | USBD_NO_COPY,
+			(usbd_private_handle) sc->sc_node,
+			(void *)(mtod(m, u_int8_t *) + m->m_len),
+			MCLBYTES - m->m_len,
+			USBD_SHORT_XFER_OK,
 			USBD_NO_TIMEOUT,
 			ubt_intr_complete);
 
+	NG_NODE_REF(sc->sc_node);
+
 	status = usbd_transfer(sc->sc_intr_xfer);
-	if (status && status != USBD_IN_PROGRESS) {
+	if (status != USBD_NORMAL_COMPLETION && status != USBD_IN_PROGRESS) {
 		NG_UBT_ERR(
 "%s: %s - Failed to start intrerrupt transfer. %s (%d)\n",
 			__func__, USBDEVNAME(sc->sc_dev), usbd_errstr(status),
 			status);
 
+		NG_NODE_UNREF(sc->sc_node);
+
+		NG_FREE_M(m);
+
 		return (status);
 	}
 
+	sc->sc_flags |= UBT_EVT_RECV;
+	sc->sc_intr_buffer = m;
+
 	return (USBD_NORMAL_COMPLETION);
 } /* ubt_intr_start */
 
@@ -977,65 +1105,77 @@ ubt_intr_start(ubt_softc_p sc)
 Static void
 ubt_intr_complete(usbd_xfer_handle h, usbd_private_handle p, usbd_status s)
 {
-	ubt_softc_p		 sc = (ubt_softc_p) p;
+	ng_send_fn((node_p) p, NULL, ubt_intr_complete2, (void *) h, s);
+	NG_NODE_UNREF((node_p) p);
+} /* ubt_intr_complete */
+
+Static void
+ubt_intr_complete2(node_p node, hook_p hook, void *arg1, int arg2)
+{
+	ubt_softc_p		 sc = (ubt_softc_p) NG_NODE_PRIVATE(node);
+	usbd_xfer_handle	 h = (usbd_xfer_handle) arg1;
+	usbd_status		 s = (usbd_status) arg2;
 	struct mbuf		*m = NULL;
 	ng_hci_event_pkt_t	*hdr = NULL;
-	int			 off;
+	int			 error;
+
+	if (sc == NULL)
+		return;
+
+	KASSERT((sc->sc_flags & UBT_EVT_RECV), (
+"%s: %s - No interrupt request is pending\n",
+		__func__, USBDEVNAME(sc->sc_dev)));
+
+	sc->sc_flags &= ~UBT_EVT_RECV;
+
+	m = sc->sc_intr_buffer;
+	sc->sc_intr_buffer = NULL;
+
+	hdr = mtod(m, ng_hci_event_pkt_t *);
+
+	if (sc->sc_hook == NULL || NG_HOOK_NOT_VALID(sc->sc_hook)) {
+		NG_UBT_INFO(
+"%s: %s - No upstream hook\n", __func__, USBDEVNAME(sc->sc_dev));
+
+		NG_FREE_M(m);
+		return;
+	}
 
 	if (s == USBD_CANCELLED) {
 		NG_UBT_INFO(
 "%s: %s - Interrupt xfer cancelled\n", __func__, USBDEVNAME(sc->sc_dev));
 
+		NG_FREE_M(m);
 		return;
 	}
 		
 	if (s != USBD_NORMAL_COMPLETION) {    
 		NG_UBT_WARN(
-"%s: %s - Interrupt xfer failed. %s (%d)\n",
+"%s: %s - Interrupt xfer failed, %s (%d). No new xfer will be submitted!\n",
 			__func__, USBDEVNAME(sc->sc_dev), usbd_errstr(s), s);
 
 		if (s == USBD_STALLED)
 			usbd_clear_endpoint_stall_async(sc->sc_intr_pipe);
 
 		NG_UBT_STAT_IERROR(sc->sc_stat);
-		goto done;
+		NG_FREE_M(m);
+
+		return; /* XXX FIXME we should restart after some delay */
 	}
 
 	NG_UBT_STAT_BYTES_RECV(sc->sc_stat, h->actlen);
+	m->m_pkthdr.len += h->actlen;
+	m->m_len += h->actlen;
 
 	NG_UBT_INFO(
 "%s: %s - Got %d bytes from interrupt pipe\n",
 		__func__, USBDEVNAME(sc->sc_dev), h->actlen);
 
-	if (h->actlen < sizeof(*hdr))
-		goto done;
-
-	/* Copy HCI event frame to mbuf */
-
-	MGETHDR(m, M_DONTWAIT, MT_DATA);
-	if (m == NULL) {
-		NG_UBT_ALERT(
-"%s: %s - Could not allocate mbuf\n", __func__, USBDEVNAME(sc->sc_dev));
-			NG_UBT_STAT_IERROR(sc->sc_stat);
+	if (m->m_pkthdr.len < sizeof(*hdr)) {
+		NG_FREE_M(m);
 		goto done;
 	}
 
-	/*
-	 * Copy data from USB buffer into mbuf and check if we got 
-	 * full HCI event frame. Fix HCI event frame if required.
-	 */
-
-	if (!(sc->sc_flags & UBT_HAVE_FRAME_TYPE)) {
-		off = 1;
-		*mtod(m, u_int8_t *) = NG_HCI_EVENT_PKT;
-	} else
-		off = 0;
-	
-	m->m_pkthdr.len = 0;
-	m->m_len = min(MHLEN, h->actlen + off); /* XXX m_copyback is stupid */
-	m_copyback(m, off, h->actlen, sc->sc_intr_buffer);
-
-	hdr = mtod(m, ng_hci_event_pkt_t *);
 	if (hdr->length == m->m_pkthdr.len - sizeof(*hdr)) {
 		NG_UBT_INFO(
 "%s: %s - Got complete HCI event frame, pktlen=%d, length=%d\n",
@@ -1044,23 +1184,9 @@ ubt_intr_complete(usbd_xfer_handle h, usbd_private_handle p, usbd_status s)
 
 		NG_UBT_STAT_PCKTS_RECV(sc->sc_stat);
 
-		IF_LOCK(&sc->sc_inq);
-		if (_IF_QFULL(&sc->sc_inq)) {
-			NG_UBT_ERR(
-"%s: %s -Incoming queue is full. Dropping mbuf, len=%d\n",
-				__func__,  USBDEVNAME(sc->sc_dev),
-				m->m_pkthdr.len);
-
-			_IF_DROP(&sc->sc_inq);
+		NG_SEND_DATA_ONLY(error, sc->sc_hook, m);
+		if (error != 0)
 			NG_UBT_STAT_IERROR(sc->sc_stat);
-
-			NG_FREE_M(m);
-		} else
-			_IF_ENQUEUE(&sc->sc_inq, m);
-		IF_UNLOCK(&sc->sc_inq);
-
-		/* Schedule SWI */
-	        swi_sched(sc->sc_ith, 0);
 	} else {
 		NG_UBT_ERR(
 "%s: %s - Invalid HCI event frame size, length=%d, pktlen=%d\n",
@@ -1072,38 +1198,69 @@ ubt_intr_complete(usbd_xfer_handle h, usbd_private_handle p, usbd_status s)
 	}
 done:
 	ubt_intr_start(sc);
-} /* ubt_intr_complete */
+} /* ubt_intr_complete2 */
 
 /*
- * Start bulk-in USB transfer (ACL data)
+ * Start bulk-in USB transfer (ACL data). Must be called when node is locked
  */
 
 Static usbd_status
 ubt_bulk_in_start(ubt_softc_p sc)
 {
-	usbd_status	status;
+	struct mbuf	*m = NULL;
+	usbd_status	 status;
+
+	KASSERT(!(sc->sc_flags & UBT_ACL_RECV), (
+"%s: %s - Another bulk-in request is pending\n",
+		__func__, USBDEVNAME(sc->sc_dev)));
+
+	/* Allocate new mbuf cluster */
+	MGETHDR(m, M_DONTWAIT, MT_DATA);
+	if (m == NULL)
+		return (USBD_NOMEM);
 
+	MCLGET(m, M_DONTWAIT);
+	if (!(m->m_flags & M_EXT)) {
+		NG_FREE_M(m);
+		return (USBD_NOMEM);
+	}
+
+	if (!(sc->sc_flags & UBT_HAVE_FRAME_TYPE)) {
+		*mtod(m, u_int8_t *) = NG_HCI_ACL_DATA_PKT;
+		m->m_pkthdr.len = m->m_len = 1;
+	} else
+		m->m_pkthdr.len = m->m_len = 0;
+	
 	/* Initialize a bulk-in USB transfer and then schedule it */
 	usbd_setup_xfer(
 			sc->sc_bulk_in_xfer,
 			sc->sc_bulk_in_pipe,
-			(usbd_private_handle) sc,
-			sc->sc_bulk_in_buffer,
-			UBT_BULK_BUFFER_SIZE,
-			USBD_SHORT_XFER_OK | USBD_NO_COPY,
+			(usbd_private_handle) sc->sc_node,
+			(void *)(mtod(m, u_int8_t *) + m->m_len),
+			MCLBYTES - m->m_len,
+			USBD_SHORT_XFER_OK,
 			USBD_NO_TIMEOUT,
 			ubt_bulk_in_complete);
 
+	NG_NODE_REF(sc->sc_node);
+
 	status = usbd_transfer(sc->sc_bulk_in_xfer);
-	if (status && status != USBD_IN_PROGRESS) {
+	if (status != USBD_NORMAL_COMPLETION && status != USBD_IN_PROGRESS) {
 		NG_UBT_ERR(
 "%s: %s - Failed to start bulk-in transfer. %s (%d)\n",
 			__func__, USBDEVNAME(sc->sc_dev), usbd_errstr(status),
 			status);
 
+		NG_NODE_UNREF(sc->sc_node);
+
+		NG_FREE_M(m);
+
 		return (status);
 	}
 
+	sc->sc_flags |= UBT_ACL_RECV;
+	sc->sc_bulk_in_buffer = m;
+
 	return (USBD_NORMAL_COMPLETION);
 } /* ubt_bulk_in_start */
 
@@ -1114,92 +1271,88 @@ ubt_bulk_in_start(ubt_softc_p sc)
 Static void
 ubt_bulk_in_complete(usbd_xfer_handle h, usbd_private_handle p, usbd_status s)
 {
-	ubt_softc_p		 sc = (ubt_softc_p) p;
+	ng_send_fn((node_p) p, NULL, ubt_bulk_in_complete2, (void *) h, s);
+	NG_NODE_UNREF((node_p) p);
+} /* ubt_bulk_in_complete */
+
+Static void
+ubt_bulk_in_complete2(node_p node, hook_p hook, void *arg1, int arg2)
+{
+	ubt_softc_p		 sc = (ubt_softc_p) NG_NODE_PRIVATE(node);
+	usbd_xfer_handle	 h = (usbd_xfer_handle) arg1;
+	usbd_status		 s = (usbd_status) arg2;
 	struct mbuf		*m = NULL;
 	ng_hci_acldata_pkt_t	*hdr = NULL;
 	int			 len;
 
+	if (sc == NULL)
+		return;
+
+	KASSERT((sc->sc_flags & UBT_ACL_RECV), (
+"%s: %s - No bulk-in request is pending\n", __func__, USBDEVNAME(sc->sc_dev)));
+
+	sc->sc_flags &= ~UBT_ACL_RECV;
+
+	m = sc->sc_bulk_in_buffer;
+	sc->sc_bulk_in_buffer = NULL;
+
+	hdr = mtod(m, ng_hci_acldata_pkt_t *);
+
+	if (sc->sc_hook == NULL || NG_HOOK_NOT_VALID(sc->sc_hook)) {
+		NG_UBT_INFO(
+"%s: %s - No upstream hook\n", __func__, USBDEVNAME(sc->sc_dev));
+
+		NG_FREE_M(m);
+		return;
+	}
+
 	if (s == USBD_CANCELLED) {
 		NG_UBT_INFO(
 "%s: %s - Bulk-in xfer cancelled, pipe=%p\n",
 			__func__, USBDEVNAME(sc->sc_dev), sc->sc_bulk_in_pipe);
 
+		NG_FREE_M(m);
 		return;
 	}
 
 	if (s != USBD_NORMAL_COMPLETION) {    
 		NG_UBT_WARN(
-"%s: %s - Bulk-in xfer failed. %s (%d)\n", 
+"%s: %s - Bulk-in xfer failed, %s (%d). No new xfer will be submitted!\n",
 			__func__, USBDEVNAME(sc->sc_dev), usbd_errstr(s), s);
 
 		if (s == USBD_STALLED)
 			usbd_clear_endpoint_stall_async(sc->sc_bulk_in_pipe);
 
 		NG_UBT_STAT_IERROR(sc->sc_stat);
-		goto done;
+		NG_FREE_M(m);
+
+		return; /* XXX FIXME we should restart after some delay */
 	}
 
 	NG_UBT_STAT_BYTES_RECV(sc->sc_stat, h->actlen);
+	m->m_pkthdr.len += h->actlen;
+	m->m_len += h->actlen;
 
 	NG_UBT_INFO(
 "%s: %s - Got %d bytes from bulk-in pipe\n",
 		__func__, USBDEVNAME(sc->sc_dev), h->actlen);
 
-	if (h->actlen < sizeof(*hdr))
-		goto done;
-
-	MGETHDR(m, M_DONTWAIT, MT_DATA);
-	if (m == NULL) {
-		NG_UBT_ALERT(
-"%s: %s - Could not allocate mbuf\n", __func__, USBDEVNAME(sc->sc_dev));
-
-		NG_UBT_STAT_IERROR(sc->sc_stat);
+	if (m->m_pkthdr.len < sizeof(*hdr)) {
+		NG_FREE_M(m);
 		goto done;
 	}
 
-	/*
-	 * Copy data from linear USB buffer into mbuf and check if we got 
-	 * full ACL data frame. Fix ACL data frame header if required.
-	 */
-
-	if (!(sc->sc_flags & UBT_HAVE_FRAME_TYPE)) {
-		len = 1;
-		*mtod(m, u_int8_t *) = NG_HCI_ACL_DATA_PKT;
-	} else
-		len  = 0;
-
-	m->m_pkthdr.len = 0;
-	m->m_len = min(MHLEN, h->actlen + len); /* XXX m_copyback is stupid */
-	m_copyback(m, len, h->actlen, sc->sc_bulk_in_buffer);
-
-	hdr = mtod(m, ng_hci_acldata_pkt_t *);
 	len = le16toh(hdr->length);
-
 	if (len == m->m_pkthdr.len - sizeof(*hdr)) {
 		NG_UBT_INFO(
 "%s: %s - Got complete ACL data frame, pktlen=%d, length=%d\n",
-			__func__, USBDEVNAME(sc->sc_dev), m->m_pkthdr.len,
-			len);
+			__func__, USBDEVNAME(sc->sc_dev), m->m_pkthdr.len, len);
 
 		NG_UBT_STAT_PCKTS_RECV(sc->sc_stat);
 
-		IF_LOCK(&sc->sc_inq);
-		if (_IF_QFULL(&sc->sc_inq)) {
-			NG_UBT_ERR(
-"%s: %s -Incoming queue is full. Dropping mbuf, len=%d\n",
-				__func__,  USBDEVNAME(sc->sc_dev),
-				m->m_pkthdr.len);
-
-			_IF_DROP(&sc->sc_inq);
+		NG_SEND_DATA_ONLY(len, sc->sc_hook, m);
+		if (len != 0)
 			NG_UBT_STAT_IERROR(sc->sc_stat);
-
-			NG_FREE_M(m);
-		} else
-			_IF_ENQUEUE(&sc->sc_inq, m);
-		IF_UNLOCK(&sc->sc_inq);
-
-		/* Schedule SWI */
-	        swi_sched(sc->sc_ith, 0);
 	} else {
 		NG_UBT_ERR(
 "%s: %s - Invalid ACL frame size, length=%d, pktlen=%d\n",
@@ -1211,47 +1364,28 @@ ubt_bulk_in_complete(usbd_xfer_handle h, usbd_private_handle p, usbd_status s)
 	}
 done:
 	ubt_bulk_in_start(sc);
-} /* ubt_bulk_in_complete */
+} /* ubt_bulk_in_complete2 */
 
 /*
- * Start bulk-out USB transfer
+ * Start bulk-out USB transfer. Must be called with node locked
  */
 
 Static usbd_status
-ubt_bulk_out_start(ubt_softc_p sc, struct mbuf *m)
+ubt_bulk_out_start(ubt_softc_p sc)
 {
-	usbd_status	status = USBD_NORMAL_COMPLETION;
-
-	IF_LOCK(&sc->sc_aclq);
-
-	if (m != NULL) {
-		if (_IF_QFULL(&sc->sc_aclq)) {
-			NG_UBT_ERR(
-"%s: %s - Dropping HCI ACL frame, len=%d. Queue full\n",
-				__func__, USBDEVNAME(sc->sc_dev),
-				m->m_pkthdr.len);
-
-			_IF_DROP(&sc->sc_aclq);
-			NG_UBT_STAT_OERROR(sc->sc_stat);
+	struct mbuf	*m = NULL;
+	usbd_status	status;
 
-			NG_FREE_M(m);
-		} else
-			_IF_ENQUEUE(&sc->sc_aclq, m);
-	} else
-		sc->sc_flags &= ~UBT_ACL_XMIT;
+	KASSERT(!(sc->sc_flags & UBT_ACL_XMIT), (
+"%s: %s - Another bulk-out request is pending\n",
+		__func__, USBDEVNAME(sc->sc_dev)));
 
-	if (sc->sc_flags & UBT_ACL_XMIT) {
-		NG_UBT_INFO(
-"%s: %s - Another bulk-out transfer is pending\n",
-			__func__, USBDEVNAME(sc->sc_dev));
-		goto done;
-	}
-
-	_IF_DEQUEUE(&sc->sc_aclq, m);
+	NG_BT_MBUFQ_DEQUEUE(&sc->sc_aclq, m);
 	if (m == NULL) {
 		NG_UBT_INFO(
 "%s: %s - ACL data queue is empty\n", __func__, USBDEVNAME(sc->sc_dev));
-		goto done;
+
+ 		return (USBD_NORMAL_COMPLETION);
 	}
 
 	/*
@@ -1271,37 +1405,38 @@ ubt_bulk_out_start(ubt_softc_p sc, struct mbuf *m)
 	usbd_setup_xfer(
 			sc->sc_bulk_out_xfer,
 			sc->sc_bulk_out_pipe,
-			(usbd_private_handle) sc,
+			(usbd_private_handle) sc->sc_node,
 			sc->sc_bulk_out_buffer,
 			m->m_pkthdr.len,
 			USBD_NO_COPY,
 			USBD_DEFAULT_TIMEOUT, /* XXX */
 			ubt_bulk_out_complete);
 
+	NG_NODE_REF(sc->sc_node);
+
 	status = usbd_transfer(sc->sc_bulk_out_xfer);
-	if (status && status != USBD_IN_PROGRESS) {
+	if (status != USBD_NORMAL_COMPLETION && status != USBD_IN_PROGRESS) {
 		NG_UBT_ERR(
 "%s: %s - Could not start bulk-out transfer. %s (%d)\n",
 			__func__, USBDEVNAME(sc->sc_dev), usbd_errstr(status),
 			status);
 
-		_IF_DROP(&sc->sc_aclq); /* XXX */
+		NG_NODE_UNREF(sc->sc_node);
+
+		NG_BT_MBUFQ_DROP(&sc->sc_aclq);
 		NG_UBT_STAT_OERROR(sc->sc_stat);
 
-		/* XXX FIXME: should we try to start another transfer? */
+		/* XXX FIXME should we try to start another transfer? */
 	} else {
 		NG_UBT_INFO(
 "%s: %s - Bulk-out transfer has been started, len=%d\n",
 			__func__, USBDEVNAME(sc->sc_dev), m->m_pkthdr.len);
 
 		sc->sc_flags |= UBT_ACL_XMIT;
-
 		status = USBD_NORMAL_COMPLETION;
 	}
 
 	NG_FREE_M(m);
-done:
-	IF_UNLOCK(&sc->sc_aclq);
 
 	return (status);
 } /* ubt_bulk_out_start */
@@ -1313,13 +1448,29 @@ done:
 Static void
 ubt_bulk_out_complete(usbd_xfer_handle h, usbd_private_handle p, usbd_status s)
 {
-	ubt_softc_p	sc = (ubt_softc_p) p;
+	ng_send_fn((node_p) p,  NULL, ubt_bulk_out_complete2, (void *) h, s);
+	NG_NODE_UNREF((node_p) p);
+} /* ubt_bulk_out_complete */
+
+Static void
+ubt_bulk_out_complete2(node_p node, hook_p hook, void *arg1, int arg2)
+{
+	ubt_softc_p		sc = (ubt_softc_p) NG_NODE_PRIVATE(node);
+	usbd_xfer_handle	h = (usbd_xfer_handle) arg1;
+	usbd_status		s = (usbd_status) arg2;
+
+	if (sc == NULL)
+		return;
+
+	KASSERT((sc->sc_flags & UBT_ACL_XMIT), (
+"%s: %s - No bulk-out request is pending\n", __func__, USBDEVNAME(sc->sc_dev)));
+
+	sc->sc_flags &= ~UBT_ACL_XMIT;
 
 	if (s == USBD_CANCELLED) {
 		NG_UBT_INFO(
 "%s: %s - Bulk-out xfer cancelled, pipe=%p\n",
-			__func__, USBDEVNAME(sc->sc_dev),
-			sc->sc_bulk_out_pipe);
+			__func__, USBDEVNAME(sc->sc_dev), sc->sc_bulk_out_pipe);
 
 		return;
 	}
@@ -1342,11 +1493,12 @@ ubt_bulk_out_complete(usbd_xfer_handle h, usbd_private_handle p, usbd_status s)
 		NG_UBT_STAT_PCKTS_SENT(sc->sc_stat); 
 	}
 
-	ubt_bulk_out_start(sc, NULL /* completed request */);
-} /* ubt_bulk_out_complete */
+	if (NG_BT_MBUFQ_LEN(&sc->sc_aclq) > 0)
+		ubt_bulk_out_start(sc);
+} /* ubt_bulk_out_complete2 */
 
 /*
- * Start Isochronous-in USB transfer
+ * Start Isochronous-in USB transfer. Must be called with node locked
  */
 
 Static usbd_status
@@ -1355,30 +1507,39 @@ ubt_isoc_in_start(ubt_softc_p sc)
 	usbd_status	status;
 	int		i;
 
-	/* Initialize an isoc-in USB transfer and then schedule it. */
+	KASSERT(!(sc->sc_flags & UBT_SCO_RECV), (
+"%s: %s - Another isoc-in request is pending\n",
+                __func__, USBDEVNAME(sc->sc_dev)));
 
+	/* Initialize a isoc-in USB transfer and then schedule it */
 	for (i = 0; i < sc->sc_isoc_nframes; i++)
 		sc->sc_isoc_in_frlen[i] = sc->sc_isoc_size;
 
 	usbd_setup_isoc_xfer(
 			sc->sc_isoc_in_xfer,
 			sc->sc_isoc_in_pipe,
-			(usbd_private_handle) sc,
+			(usbd_private_handle) sc->sc_node,
 			sc->sc_isoc_in_frlen,
 			sc->sc_isoc_nframes,
 			USBD_NO_COPY, /* XXX flags */
 			ubt_isoc_in_complete);
 
+	NG_NODE_REF(sc->sc_node);
+
 	status = usbd_transfer(sc->sc_isoc_in_xfer);
-	if (status && status != USBD_IN_PROGRESS) {
+	if (status != USBD_NORMAL_COMPLETION && status != USBD_IN_PROGRESS) {
 		NG_UBT_ERR(
 "%s: %s - Failed to start isoc-in transfer. %s (%d)\n",
 			__func__, USBDEVNAME(sc->sc_dev),
 			usbd_errstr(status), status);
 
+		NG_NODE_UNREF(sc->sc_node);
+
 		return (status);
 	}
 
+	sc->sc_flags |= UBT_SCO_RECV;
+
 	return (USBD_NORMAL_COMPLETION);
 } /* ubt_isoc_in_start */
 
@@ -1389,12 +1550,36 @@ ubt_isoc_in_start(ubt_softc_p sc)
 Static void
 ubt_isoc_in_complete(usbd_xfer_handle h, usbd_private_handle p, usbd_status s)
 {
-	ubt_softc_p		 sc = (ubt_softc_p) p;
+	ng_send_fn((node_p) p, NULL, ubt_isoc_in_complete2, (void *) h, s);
+	NG_NODE_UNREF((node_p) p);
+} /* ubt_isoc_in_complete */
+
+Static void
+ubt_isoc_in_complete2(node_p node, hook_p hook, void *arg1, int arg2)
+{
+	ubt_softc_p		 sc = (ubt_softc_p) NG_NODE_PRIVATE(node);
+	usbd_xfer_handle	 h = (usbd_xfer_handle) arg1;
+	usbd_status		 s = (usbd_status) arg2;
 	struct mbuf		*m = NULL;
 	ng_hci_scodata_pkt_t	*hdr = NULL;
 	u_int8_t		*b = NULL;
 	int			 i;
 
+	if (sc == NULL)
+		return;
+
+	KASSERT((sc->sc_flags & UBT_SCO_RECV), (
+"%s: %s - No isoc-in request is pending\n", __func__, USBDEVNAME(sc->sc_dev)));
+
+	sc->sc_flags &= ~UBT_SCO_RECV;
+
+	if (sc->sc_hook == NULL || NG_HOOK_NOT_VALID(sc->sc_hook)) {
+		NG_UBT_INFO(
+"%s: %s - No upstream hook\n", __func__, USBDEVNAME(sc->sc_dev));
+
+		return;
+	}
+
 	if (s == USBD_CANCELLED) {
 		NG_UBT_INFO(
 "%s: %s - Isoc-in xfer cancelled, pipe=%p\n",
@@ -1405,14 +1590,15 @@ ubt_isoc_in_complete(usbd_xfer_handle h, usbd_private_handle p, usbd_status s)
 
 	if (s != USBD_NORMAL_COMPLETION) {    
 		NG_UBT_WARN(
-"%s: %s - Isoc-in xfer failed. %s (%d)\n",
+"%s: %s - Isoc-in xfer failed, %s (%d). No new xfer will be submitted!\n",
 			__func__, USBDEVNAME(sc->sc_dev), usbd_errstr(s), s);
 
 		if (s == USBD_STALLED)
 			usbd_clear_endpoint_stall_async(sc->sc_isoc_in_pipe);
 
 		NG_UBT_STAT_IERROR(sc->sc_stat);
-		goto done;
+
+		return; /* XXX FIXME we should restart after some delay */
 	}
 
 	NG_UBT_STAT_BYTES_RECV(sc->sc_stat, h->actlen);
@@ -1421,11 +1607,7 @@ ubt_isoc_in_complete(usbd_xfer_handle h, usbd_private_handle p, usbd_status s)
 "%s: %s - Got %d bytes from isoc-in pipe\n",
 		__func__, USBDEVNAME(sc->sc_dev), h->actlen);
 
-	if (h->actlen < sizeof(*hdr))
-		goto done;
-
 	/* Copy SCO data frame to mbuf */
-
 	MGETHDR(m, M_DONTWAIT, MT_DATA);
 	if (m == NULL) {
 		NG_UBT_ALERT(
@@ -1437,18 +1619,18 @@ ubt_isoc_in_complete(usbd_xfer_handle h, usbd_private_handle p, usbd_status s)
 	}
 
 	/* Fix SCO data frame header if required */
-
-	if (sc->sc_flags & UBT_HAVE_FRAME_TYPE) {
-		m->m_pkthdr.len = m->m_len = 0;
-	} else {
+	if (!(sc->sc_flags & UBT_HAVE_FRAME_TYPE)) {
 		*mtod(m, u_int8_t *) = NG_HCI_SCO_DATA_PKT;
-		m->m_pkthdr.len = m->m_len = 1;
+		m->m_pkthdr.len = 1;
+		m->m_len = min(MHLEN, h->actlen + 1); /* XXX m_copyback */
+	} else {
+		m->m_pkthdr.len = 0;
+		m->m_len = min(MHLEN, h->actlen); /* XXX m_copyback */
 	}
 
 	/*
-	 * XXX FIXME: how do we know how many frames we have received?
-	 * XXX use frlen for now.
-	 * XXX is that correct?
+	 * XXX FIXME how do we know how many frames we have received?
+	 * XXX use frlen for now. is that correct?
 	 */
 
 	b = (u_int8_t *) sc->sc_isoc_in_buffer;
@@ -1461,11 +1643,8 @@ ubt_isoc_in_complete(usbd_xfer_handle h, usbd_private_handle p, usbd_status s)
 				sc->sc_isoc_in_frlen[i], b);
 	}
 
-	NG_UBT_M_PULLUP(m, sizeof(*hdr));
-	if (m == NULL) {
-		NG_UBT_STAT_IERROR(sc->sc_stat);
+	if (m->m_pkthdr.len < sizeof(*hdr))
 		goto done;
-	}
 
 	hdr = mtod(m, ng_hci_scodata_pkt_t *);
 
@@ -1477,23 +1656,9 @@ ubt_isoc_in_complete(usbd_xfer_handle h, usbd_private_handle p, usbd_status s)
 
 		NG_UBT_STAT_PCKTS_RECV(sc->sc_stat);
 
-		IF_LOCK(&sc->sc_inq);
-		if (_IF_QFULL(&sc->sc_inq)) {
-			NG_UBT_ERR(
-"%s: %s -Incoming queue is full. Dropping mbuf, len=%d\n",
-				__func__,  USBDEVNAME(sc->sc_dev),
-				m->m_pkthdr.len);
-
-			_IF_DROP(&sc->sc_inq);
+		NG_SEND_DATA_ONLY(i, sc->sc_hook, m);
+		if (i != 0)
 			NG_UBT_STAT_IERROR(sc->sc_stat);
-
-			NG_FREE_M(m);
-		} else
-			_IF_ENQUEUE(&sc->sc_inq, m);
-		IF_UNLOCK(&sc->sc_inq);
-
-		/* Schedule SWI */
-	        swi_sched(sc->sc_ith, 0);
 	} else {
 		NG_UBT_ERR(
 "%s: %s - Invalid SCO frame size, length=%d, pktlen=%d\n",
@@ -1505,53 +1670,33 @@ ubt_isoc_in_complete(usbd_xfer_handle h, usbd_private_handle p, usbd_status s)
 	}
 done:
 	ubt_isoc_in_start(sc);
-} /* ubt_bulk_in_complete */
+} /* ubt_isoc_in_complete2 */
 
 /*
- * Start isochronous-out USB transfer
+ * Start isochronous-out USB transfer. Must be called with node locked
  */
 
 Static usbd_status
-ubt_isoc_out_start(ubt_softc_p sc, struct mbuf *m)
+ubt_isoc_out_start(ubt_softc_p sc)
 {
+	struct mbuf	*m = NULL;
 	u_int8_t	*b = NULL;
 	int		 i, len, nframes;
-	usbd_status	 status = USBD_NORMAL_COMPLETION;
-
-	IF_LOCK(&sc->sc_scoq);
+	usbd_status	 status;
 
-	if (m != NULL) {
-		if (_IF_QFULL(&sc->sc_scoq)) {
-			NG_UBT_ERR(
-"%s: %s - Dropping HCI SCO frame, len=%d. Queue full\n",
-				__func__, USBDEVNAME(sc->sc_dev), 
-				m->m_pkthdr.len);
+	KASSERT(!(sc->sc_flags & UBT_SCO_XMIT), (
+"%s: %s - Another isoc-out request is pending\n",
+		__func__, USBDEVNAME(sc->sc_dev)));
 
-			_IF_DROP(&sc->sc_scoq);
-			NG_UBT_STAT_OERROR(sc->sc_stat);
-
-			NG_FREE_M(m);
-		} else
-			_IF_ENQUEUE(&sc->sc_scoq, m);
-	} else
-		sc->sc_flags &= ~UBT_SCO_XMIT;
-
-	if (sc->sc_flags & UBT_SCO_XMIT) {
-		NG_UBT_INFO(
-"%s: %s - Another isoc-out transfer is pending\n",
-			__func__, USBDEVNAME(sc->sc_dev));
-		goto done;
-	}
-
-	_IF_DEQUEUE(&sc->sc_scoq, m);
+	NG_BT_MBUFQ_DEQUEUE(&sc->sc_scoq, m);
 	if (m == NULL) {
 		NG_UBT_INFO(
 "%s: %s - SCO data queue is empty\n", __func__, USBDEVNAME(sc->sc_dev));
-		goto done;
+
+ 		return (USBD_NORMAL_COMPLETION);
 	}
 
 	/* Copy entire SCO frame into USB transfer buffer and start transfer */ 
-
 	b = (u_int8_t *) sc->sc_isoc_out_buffer;
 	nframes = 0;
 
@@ -1576,25 +1721,28 @@ ubt_isoc_out_start(ubt_softc_p sc, struct mbuf *m)
 
 	NG_FREE_M(m);
 
-	/* Initialize an isoc-out USB transfer and then schedule it. */
-
+	/* Initialize a isoc-out USB transfer and then schedule it */
 	usbd_setup_isoc_xfer(
 			sc->sc_isoc_out_xfer,
 			sc->sc_isoc_out_pipe,
-			(usbd_private_handle) sc,
+			(usbd_private_handle) sc->sc_node,
 			sc->sc_isoc_out_frlen,
 			nframes,
 			USBD_NO_COPY,
 			ubt_isoc_out_complete);
 
+	NG_NODE_REF(sc->sc_node);
+
 	status = usbd_transfer(sc->sc_isoc_out_xfer);
-	if (status && status != USBD_IN_PROGRESS) {
+	if (status != USBD_NORMAL_COMPLETION && status != USBD_IN_PROGRESS) {
 		NG_UBT_ERR(
 "%s: %s - Could not start isoc-out transfer. %s (%d)\n",
 			__func__, USBDEVNAME(sc->sc_dev), usbd_errstr(status),
 			status);
 
-		_IF_DROP(&sc->sc_scoq); /* XXX */
+		NG_NODE_UNREF(sc->sc_node);
+
+		NG_BT_MBUFQ_DROP(&sc->sc_scoq);
 		NG_UBT_STAT_OERROR(sc->sc_stat);
 	} else {
 		NG_UBT_INFO(
@@ -1603,11 +1751,8 @@ ubt_isoc_out_start(ubt_softc_p sc, struct mbuf *m)
 			sc->sc_isoc_size);
 
 		sc->sc_flags |= UBT_SCO_XMIT;
-
 		status = USBD_NORMAL_COMPLETION;
 	}
-done:
-	IF_UNLOCK(&sc->sc_scoq);
 
 	return (status);
 } /* ubt_isoc_out_start */
@@ -1619,7 +1764,24 @@ done:
 Static void
 ubt_isoc_out_complete(usbd_xfer_handle h, usbd_private_handle p, usbd_status s)
 {
-	ubt_softc_p	sc = (ubt_softc_p) p;
+	ng_send_fn((node_p) p, NULL, ubt_isoc_out_complete2, (void *) h, s);
+	NG_NODE_UNREF((node_p) p);
+} /* ubt_isoc_out_complete */
+
+Static void
+ubt_isoc_out_complete2(node_p node, hook_p hook, void *arg1, int arg2)
+{
+	ubt_softc_p		sc = (ubt_softc_p) NG_NODE_PRIVATE(node);
+	usbd_xfer_handle	h = (usbd_xfer_handle) arg1;
+	usbd_status		s = (usbd_status) arg2;
+
+	if (sc == NULL)
+		return;
+
+	KASSERT((sc->sc_flags & UBT_SCO_XMIT), (
+"%s: %s - No isoc-out request is pending\n", __func__, USBDEVNAME(sc->sc_dev)));
+
+	sc->sc_flags &= ~UBT_SCO_XMIT;
 
 	if (s == USBD_CANCELLED) {
 		NG_UBT_INFO(
@@ -1648,43 +1810,38 @@ ubt_isoc_out_complete(usbd_xfer_handle h, usbd_private_handle p, usbd_status s)
 		NG_UBT_STAT_PCKTS_SENT(sc->sc_stat);
 	}
 
-	ubt_isoc_out_start(sc, NULL /* completed request */);
-} /* ubt_isoc_out_complete */
+	if (NG_BT_MBUFQ_LEN(&sc->sc_scoq) > 0)
+		ubt_isoc_out_start(sc);
+} /* ubt_isoc_out_complete2 */
 
 /*
- * SWI interrupt handler
+ * Abort transfers on all USB pipes
  */
 
 Static void
-ubt_swi_intr(void *context)
+ubt_reset(ubt_softc_p sc)
 {
-	ubt_softc_p	 sc = (ubt_softc_p) context;
-	struct mbuf	*m = NULL;
-	int		 error;
+	/* Interrupt */
+	if (sc->sc_intr_pipe != NULL)
+		usbd_abort_pipe(sc->sc_intr_pipe);
 
-	if (sc->sc_hook != NULL && NG_HOOK_IS_VALID(sc->sc_hook)) {
-		for (;;) {
-			IF_DEQUEUE(&sc->sc_inq, m);
-			if (m == NULL)
-				break;
+	/* Bulk-in/out */
+	if (sc->sc_bulk_in_pipe != NULL)
+		usbd_abort_pipe(sc->sc_bulk_in_pipe);
+	if (sc->sc_bulk_out_pipe != NULL)
+		usbd_abort_pipe(sc->sc_bulk_out_pipe);
 
-			NG_SEND_DATA_ONLY(error, sc->sc_hook, m);
-			if (error != 0)
-				NG_UBT_STAT_IERROR(sc->sc_stat);
-		}
-	} else {
-		IF_LOCK(&sc->sc_inq);
-		for (;;) {
-			_IF_DEQUEUE(&sc->sc_inq, m);
-			if (m == NULL)
-				break;
+	/* Isoc-in/out */
+	if (sc->sc_isoc_in_pipe != NULL)
+		usbd_abort_pipe(sc->sc_isoc_in_pipe);
+	if (sc->sc_isoc_out_pipe != NULL)
+		usbd_abort_pipe(sc->sc_isoc_out_pipe);
 
-			NG_UBT_STAT_IERROR(sc->sc_stat);
-			NG_FREE_M(m);
-		}
-		IF_UNLOCK(&sc->sc_inq);
-	}
-} /* ubt_swi_intr */
+	/* Cleanup queues */
+	NG_BT_MBUFQ_DRAIN(&sc->sc_cmdq);
+	NG_BT_MBUFQ_DRAIN(&sc->sc_aclq);
+	NG_BT_MBUFQ_DRAIN(&sc->sc_scoq);
+} /* ubt_reset */
 
 /****************************************************************************
  ****************************************************************************
@@ -1736,6 +1893,7 @@ ng_ubt_shutdown(node_p node)
 	}
 
 	NG_NODE_SET_PRIVATE(sc->sc_node, sc);
+	NG_NODE_FORCE_WRITER(sc->sc_node);
 done:
 	return (0);
 } /* ng_ubt_shutdown */
@@ -1748,7 +1906,11 @@ Static int
 ng_ubt_newhook(node_p node, hook_p hook, char const *name)
 {
 	ubt_softc_p	sc = (ubt_softc_p) NG_NODE_PRIVATE(node);
-	usbd_status	status;
+
+	/* Refuse to create new hook if device interface is active */
+	if (sc->sc_ctrl_dev != NODEV || sc->sc_intr_dev != NODEV ||
+	    sc->sc_bulk_dev != NODEV)
+		return (EBUSY);
 
 	if (strcmp(name, NG_UBT_HOOK) != 0)
 		return (EINVAL);
@@ -1758,69 +1920,7 @@ ng_ubt_newhook(node_p node, hook_p hook, char const *name)
 
 	sc->sc_hook = hook;
 
-	/* Interrupt */	
-	status = usbd_open_pipe(sc->sc_iface0, sc->sc_intr_ep,
-			USBD_EXCLUSIVE_USE, &sc->sc_intr_pipe);
-	if (status) {
-		NG_UBT_ALERT(
-"%s: %s - Could not open interrupt pipe. %s (%d)\n",
-			__func__, USBDEVNAME(sc->sc_dev), usbd_errstr(status),
-			status);
-		goto fail;
-	}
-
-	/* Bulk-in */
-	status = usbd_open_pipe(sc->sc_iface0, sc->sc_bulk_in_ep,
-			USBD_EXCLUSIVE_USE, &sc->sc_bulk_in_pipe);
-	if (status) {
-		NG_UBT_ALERT(
-"%s: %s - Could not open bulk-in pipe. %s (%d)\n",
-			__func__,  USBDEVNAME(sc->sc_dev), usbd_errstr(status),
-			status);
-		goto fail;
-	}
-
-	/* Bulk-out */
-	status = usbd_open_pipe(sc->sc_iface0, sc->sc_bulk_out_ep,
-			USBD_EXCLUSIVE_USE, &sc->sc_bulk_out_pipe);
-	if (status) {
-		NG_UBT_ALERT(
-"%s: %s - Could not open bulk-out pipe. %s (%d)\n",
-			__func__, USBDEVNAME(sc->sc_dev), usbd_errstr(status),
-			status);
-		goto fail;
-	}
-
-#if __broken__ /* XXX FIXME */
-	/* Isoc-in */
-	status = usbd_open_pipe(sc->sc_iface1, sc->sc_isoc_in_ep,
-			USBD_EXCLUSIVE_USE, &sc->sc_isoc_in_pipe);
-	if (status) {
-		NG_UBT_ALERT(
-"%s: %s - Could not open isoc-in pipe. %s (%d)\n",
-			__func__, USBDEVNAME(sc->sc_dev), usbd_errstr(status),
-			status);
-		goto fail;
-	}
-
-	/* Isoc-out */
-	status = usbd_open_pipe(sc->sc_iface1, sc->sc_isoc_out_ep, 
-			USBD_EXCLUSIVE_USE, &sc->sc_isoc_out_pipe);
-	if (status) {
-		NG_UBT_ALERT(
-"%s: %s - Could not open isoc-out pipe. %s (%d)\n",
-			__func__, USBDEVNAME(sc->sc_dev), usbd_errstr(status),
-			status);
-		goto fail;
-	}
-#endif /* __broken__ */
-
 	return (0);
-fail:
-	ng_ubt_reset(sc);
-	sc->sc_hook = NULL;
-
-	return (ENXIO);
 } /* ng_ubt_newhook */
 
 /*
@@ -1833,6 +1933,13 @@ ng_ubt_connect(hook_p hook)
 	ubt_softc_p	sc = (ubt_softc_p) NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
 	usbd_status	status;
 
+	/* Refuse to connect hook if device interface is active */
+	if (sc->sc_ctrl_dev != NODEV || sc->sc_intr_dev != NODEV ||
+	    sc->sc_bulk_dev != NODEV)
+		return (EBUSY);
+
+	NG_HOOK_FORCE_QUEUE(NG_HOOK_PEER(hook));
+
 	/* Start intr transfer */
 	status = ubt_intr_start(sc);
 	if (status != USBD_NORMAL_COMPLETION) {
@@ -1865,11 +1972,9 @@ ng_ubt_connect(hook_p hook)
 	}
 #endif /* __broken__ */
 
-	NG_HOOK_FORCE_QUEUE(NG_HOOK_PEER(hook));
-
 	return (0);
 fail:
-	ng_ubt_reset(sc);
+	ubt_reset(sc);
 	sc->sc_hook = NULL;
 
 	return (ENXIO);
@@ -1888,7 +1993,7 @@ ng_ubt_disconnect(hook_p hook)
 		if (hook != sc->sc_hook)
 			return (EINVAL);
 
-		ng_ubt_reset(sc);
+		ubt_reset(sc);
 		sc->sc_hook = NULL;
 	}
 
@@ -1902,10 +2007,10 @@ ng_ubt_disconnect(hook_p hook)
 Static int
 ng_ubt_rcvmsg(node_p node, item_p item, hook_p lasthook)
 {
-	ubt_softc_p	 sc = (ubt_softc_p) NG_NODE_PRIVATE(node);
-	struct ng_mesg	*msg = NULL, *rsp = NULL;
-	int		 error = 0, queue, qlen;
-	struct ifqueue	*q = NULL;
+	ubt_softc_p		 sc = (ubt_softc_p) NG_NODE_PRIVATE(node);
+	struct ng_mesg		*msg = NULL, *rsp = NULL;
+	struct ng_bt_mbufq	*q = NULL;
+	int			 error = 0, queue, qlen;
 
 	if (sc == NULL) {
 		NG_FREE_ITEM(item);
@@ -1928,19 +2033,16 @@ ng_ubt_rcvmsg(node_p node, item_p item, hook_p lasthook)
 					"Debug: %d\n"  \
 					"CMD queue: [have:%d,max:%d]\n" \
 					"ACL queue: [have:%d,max:%d]\n" \
-					"SCO queue: [have:%d,max:%d]\n" \
-					"Inp queue: [have:%d,max:%d]",
+					"SCO queue: [have:%d,max:%d]",
 					(sc->sc_hook != NULL)? NG_UBT_HOOK : "",
 					sc->sc_flags,
 					sc->sc_debug,
-					_IF_QLEN(&sc->sc_cmdq), /* XXX */
-					sc->sc_cmdq.ifq_maxlen, /* XXX */
-					_IF_QLEN(&sc->sc_aclq), /* XXX */
-					sc->sc_aclq.ifq_maxlen, /* XXX */
-					_IF_QLEN(&sc->sc_scoq), /* XXX */
-					sc->sc_scoq.ifq_maxlen, /* XXX */
-					_IF_QLEN(&sc->sc_inq),  /* XXX */
-					sc->sc_inq.ifq_maxlen   /* XXX */ );
+					NG_BT_MBUFQ_LEN(&sc->sc_cmdq),
+					sc->sc_cmdq.maxlen,
+					NG_BT_MBUFQ_LEN(&sc->sc_aclq),
+					sc->sc_aclq.maxlen,
+					NG_BT_MBUFQ_LEN(&sc->sc_scoq),
+					sc->sc_scoq.maxlen);
 			break;
 
 		default:
@@ -1984,10 +2086,6 @@ ng_ubt_rcvmsg(node_p node, item_p item, hook_p lasthook)
 				}
 
 				switch (queue) {
-				case NGM_UBT_NODE_QUEUE_IN:
-					q = &sc->sc_inq;
-					break;
-
 				case NGM_UBT_NODE_QUEUE_CMD:
 					q = &sc->sc_cmdq;
 					break;
@@ -2007,7 +2105,7 @@ ng_ubt_rcvmsg(node_p node, item_p item, hook_p lasthook)
 				}
 
 				if (q != NULL)
-					q->ifq_maxlen = qlen; /* XXX */
+					q->maxlen = qlen;
 			}
 			break;
 
@@ -2019,10 +2117,6 @@ ng_ubt_rcvmsg(node_p node, item_p item, hook_p lasthook)
 
 			queue = ((ng_ubt_node_qlen_ep *)(msg->data))->queue;
 			switch (queue) {
-			case NGM_UBT_NODE_QUEUE_IN:
-				q = &sc->sc_inq;
-				break;
-
 			case NGM_UBT_NODE_QUEUE_CMD:
 				q = &sc->sc_cmdq;
 				break;
@@ -2052,7 +2146,7 @@ ng_ubt_rcvmsg(node_p node, item_p item, hook_p lasthook)
 				((ng_ubt_node_qlen_ep *)(rsp->data))->queue =
 					queue;
 				((ng_ubt_node_qlen_ep *)(rsp->data))->qlen =
-					q->ifq_maxlen; /* XXX */
+					q->maxlen;
 			}
 			break;
 
@@ -2070,6 +2164,25 @@ ng_ubt_rcvmsg(node_p node, item_p item, hook_p lasthook)
 			NG_UBT_STAT_RESET(sc->sc_stat);
 			break;
 
+		case NGM_UBT_NODE_DEV_NODES:
+			if (msg->header.arglen !=
+					sizeof(ng_ubt_node_dev_nodes_ep)) {
+				error = EMSGSIZE;
+				break;
+			}
+
+			if ((sc->sc_flags & UBT_ANY_DEV) || 
+			    sc->sc_hook != NULL) {
+				error = EBUSY;
+				break;
+			}
+
+			if (*((ng_ubt_node_dev_nodes_ep *)(msg->data)))
+				ubt_create_device_nodes(sc);
+			else
+				ubt_destroy_device_nodes(sc);
+			break;
+
 		default:
 			error = EINVAL;
 			break;
@@ -2094,10 +2207,11 @@ ng_ubt_rcvmsg(node_p node, item_p item, hook_p lasthook)
 Static int
 ng_ubt_rcvdata(hook_p hook, item_p item)
 {
-	ubt_softc_p	 sc = (ubt_softc_p) NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
-	struct mbuf	*m = NULL;
-	usbd_status	(*f)(ubt_softc_p, struct mbuf *) = NULL;
-	int		 error = 0;
+	ubt_softc_p		 sc = (ubt_softc_p) NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
+	struct mbuf		*m = NULL;
+	usbd_status		(*f)(ubt_softc_p) = NULL;
+	struct ng_bt_mbufq	*q = NULL;
+	int			 b, error = 0;
 
 	if (sc == NULL) {
 		error = EHOSTDOWN;
@@ -2116,15 +2230,21 @@ ng_ubt_rcvdata(hook_p hook, item_p item)
 	switch (*mtod(m, u_int8_t *)) { /* XXX call m_pullup ? */
 	case NG_HCI_CMD_PKT:
 		f = ubt_request_start;
+		q = &sc->sc_cmdq;
+		b = UBT_CMD_XMIT;
 		break;
 
 	case NG_HCI_ACL_DATA_PKT:
 		f = ubt_bulk_out_start;
+		q = &sc->sc_aclq;
+		b = UBT_ACL_XMIT;
 		break;
 
 #if __broken__ /* XXX FIXME */
 	case NG_HCI_SCO_DATA_PKT:
 		f = ubt_isoc_out_start;
+		q = &sc->sc_scoq;
+		b = UBT_SCO_XMIT;
 		break;
 #endif /* __broken__ */
 
@@ -2145,58 +2265,485 @@ ng_ubt_rcvdata(hook_p hook, item_p item)
 	if (!(sc->sc_flags & UBT_NEED_FRAME_TYPE))
 		m_adj(m, sizeof(u_int8_t)); 
 
-	if ((*f)(sc, m) != USBD_NORMAL_COMPLETION)
-		error = EIO;
+	if (NG_BT_MBUFQ_FULL(q)) {
+		NG_UBT_ERR(
+"%s: %s - Dropping HCI frame %#x, len=%d. Queue full\n",
+			__func__, USBDEVNAME(sc->sc_dev),
+			*mtod(m, u_int8_t *), m->m_pkthdr.len);
+
+		NG_FREE_M(m);
+	} else
+		NG_BT_MBUFQ_ENQUEUE(q, m);
+
+	if (!(sc->sc_flags & b))
+		if ((*f)(sc) != USBD_NORMAL_COMPLETION)
+			error = EIO;
 done:
 	NG_FREE_ITEM(item);
 
 	return (error);
 } /* ng_ubt_rcvdata */
 
+/****************************************************************************
+ ****************************************************************************
+ **                              Device specific
+ ****************************************************************************
+ ****************************************************************************/
+
+/*
+ * Open endpoint device
+ * XXX FIXME softc locking
+ */
+
+Static int
+ubt_open(dev_t dev, int flag, int mode, usb_proc_ptr p)
+{
+	ubt_softc_p	sc = NULL;
+	int		ep = UBT_ENDPOINT(dev);
+
+	USB_GET_SC_OPEN(ubt, UBT_UNIT(dev), sc); /* check for sc != NULL */
+	if (sc->sc_dying)
+		return (ENXIO);
+
+	if (ep == USB_CONTROL_ENDPOINT) {
+		if (sc->sc_flags & UBT_CTRL_DEV)
+			return (EBUSY);
+
+		sc->sc_flags |= UBT_CTRL_DEV;
+	} else if (ep == UE_GET_ADDR(sc->sc_intr_ep)) {
+		if (sc->sc_flags & UBT_INTR_DEV)
+			return (EBUSY);
+		if (sc->sc_intr_pipe == NULL)
+			return (ENXIO);
+
+		sc->sc_flags |= UBT_INTR_DEV;
+	} else if (ep == UE_GET_ADDR(sc->sc_bulk_in_ep)) {
+		if (sc->sc_flags & UBT_BULK_DEV)
+			return (EBUSY);
+		if (sc->sc_bulk_in_pipe == NULL || sc->sc_bulk_out_pipe == NULL)
+			return (ENXIO);
+
+		sc->sc_flags |= UBT_BULK_DEV;
+	} else
+		return (EINVAL);
+
+	return (0);
+} /* ubt_open */
+
+/*
+ * Close endpoint device
+ * XXX FIXME softc locking
+ */
+
+Static int
+ubt_close(dev_t dev, int flag, int mode, usb_proc_ptr p)
+{
+	ubt_softc_p	sc = NULL;
+	int		ep = UBT_ENDPOINT(dev);
+
+	USB_GET_SC(ubt, UBT_UNIT(dev), sc);
+	if (sc == NULL)
+		return (ENXIO);
+
+	if (ep == USB_CONTROL_ENDPOINT)
+		sc->sc_flags &= ~UBT_CTRL_DEV;
+	else if (ep == UE_GET_ADDR(sc->sc_intr_ep)) {
+		if (sc->sc_intr_pipe != NULL)
+			usbd_abort_pipe(sc->sc_intr_pipe);
+
+		sc->sc_flags &= ~UBT_INTR_DEV;
+	} else if (ep == UE_GET_ADDR(sc->sc_bulk_in_ep)) {
+		/* Close both in and out bulk pipes */
+		if (sc->sc_bulk_in_pipe != NULL)
+			usbd_abort_pipe(sc->sc_bulk_in_pipe);
+
+		if (sc->sc_bulk_out_pipe != NULL)
+			usbd_abort_pipe(sc->sc_bulk_out_pipe);
+
+		sc->sc_flags &= ~UBT_BULK_DEV;
+	} else
+		return (EINVAL);
+
+	return (0);
+} /* ubt_close */
+
+/*
+ * Read from the endpoint device
+ * XXX FIXME softc locking
+ */
+
+Static int
+ubt_read(dev_t dev, struct uio *uio, int flag)
+{
+	ubt_softc_p		sc = NULL;
+	int			error = 0, n, tn, ep = UBT_ENDPOINT(dev);
+	usbd_status		status;
+	usbd_pipe_handle	pipe = NULL;
+	usbd_xfer_handle	xfer = NULL;
+	u_int8_t		buf[UBT_BSIZE];
+
+	USB_GET_SC(ubt, UBT_UNIT(dev), sc);
+	if (sc == NULL || sc->sc_dying)
+		return (ENXIO);
+
+	if (ep == USB_CONTROL_ENDPOINT)
+		return (EOPNOTSUPP);
+
+	if (ep == UE_GET_ADDR(sc->sc_intr_ep)) {
+		pipe = sc->sc_intr_pipe;
+		xfer = sc->sc_intr_xfer;
+	} else if (ep == UE_GET_ADDR(sc->sc_bulk_in_ep)) {
+		pipe = sc->sc_bulk_in_pipe;
+		xfer = sc->sc_bulk_in_xfer;
+	} else
+		return (EINVAL);
+
+	if (pipe == NULL || xfer == NULL)
+		return (ENXIO);
+
+	sc->sc_refcnt ++;
+
+	while ((n = min(sizeof(buf), uio->uio_resid)) != 0) {
+		tn = n;
+		status = usbd_bulk_transfer(xfer, pipe, USBD_SHORT_XFER_OK,
+				USBD_DEFAULT_TIMEOUT, buf, &tn, "ubtrd");
+		switch (status) {
+		case USBD_NORMAL_COMPLETION:
+			error = uiomove(buf, tn, uio);
+			break;
+
+		case USBD_INTERRUPTED:
+			error = EINTR;
+			break;
+
+		case USBD_TIMEOUT:
+			error = ETIMEDOUT;
+			break;
+
+		default:
+			error = EIO;
+			break;
+		}
+
+		if (error != 0 || tn < n)
+			break;
+	}
+
+	if (-- sc->sc_refcnt < 0)
+		usb_detach_wakeup(USBDEV(sc->sc_dev));
+
+	return (error);
+} /* ubt_read */
+
+/*
+ * Write into the endpoint device
+ * XXX FIXME softc locking
+ */
+
+Static int
+ubt_write(dev_t dev, struct uio *uio, int flag)
+{
+	ubt_softc_p	sc = NULL;
+	int		error = 0, n, ep = UBT_ENDPOINT(dev);
+	usbd_status	status;
+	u_int8_t	buf[UBT_BSIZE];
+
+	USB_GET_SC(ubt, UBT_UNIT(dev), sc);
+	if (sc == NULL || sc->sc_dying)
+		return (ENXIO);
+
+	if (ep == USB_CONTROL_ENDPOINT || ep == UE_GET_ADDR(sc->sc_intr_ep))
+		return (EOPNOTSUPP);
+	if (ep != UE_GET_ADDR(sc->sc_bulk_in_ep))
+		return (EINVAL);
+
+	sc->sc_refcnt ++;
+
+	while ((n = min(sizeof(buf), uio->uio_resid)) != 0) {
+		error = uiomove(buf, n, uio);
+		if (error != 0)
+			break;
+
+		status = usbd_bulk_transfer(sc->sc_bulk_out_xfer,
+				sc->sc_bulk_out_pipe, 0, USBD_DEFAULT_TIMEOUT,
+				buf, &n,"ubtwr");
+		switch (status) {
+		case USBD_NORMAL_COMPLETION:
+			break;
+
+		case USBD_INTERRUPTED:
+			error = EINTR;
+			break;
+
+		case USBD_TIMEOUT:
+			error = ETIMEDOUT;
+			break;
+
+		default:
+			error = EIO;
+			break;
+		}
+
+		if (error != 0)
+			break;
+	}
+
+	if (-- sc->sc_refcnt < 0)
+		usb_detach_wakeup(USBDEV(sc->sc_dev));
+
+	return (error);
+} /* ubt_write */
+
+/*
+ * Process ioctl on the endpoint device. Mostly stolen from ugen(4)
+ * XXX FIXME softc locking
+ */
+
+Static int
+ubt_ioctl(dev_t dev, u_long cmd, caddr_t data, int flag, usb_proc_ptr p)
+{
+	ubt_softc_p		 sc = NULL;
+	int			 len, error = 0, ep = UBT_ENDPOINT(dev);
+	usbd_status		 status;
+	struct usb_string_desc	*si = NULL;
+	struct usb_ctl_request	*ur = NULL;
+	void			*ptr = NULL;
+	struct iovec		 iov;
+	struct uio		 uio;
+
+	USB_GET_SC(ubt, UBT_UNIT(dev), sc);
+	if (sc == NULL || sc->sc_dying)
+		return (ENXIO);
+
+	if (ep != USB_CONTROL_ENDPOINT)
+		return (EOPNOTSUPP);
+
+	sc->sc_refcnt ++;
+
+	switch (cmd) {
+	case USB_GET_DEVICE_DESC:
+		*(usb_device_descriptor_t *) data =
+				*usbd_get_device_descriptor(sc->sc_udev);
+		break;
+
+	case USB_GET_STRING_DESC:
+		si = (struct usb_string_desc *) data;
+		status = usbd_get_string_desc(sc->sc_udev, si->usd_string_index,
+				si->usd_language_id, &si->usd_desc);
+		if (status != USBD_NORMAL_COMPLETION)
+			error = EINVAL;
+		break;
+
+	case USB_DO_REQUEST:
+		ur = (void *) data;
+		len = UGETW(ur->ucr_request.wLength);
+
+		if (!(flag & FWRITE)) {
+			error = EPERM;
+			break;
+		}
+
+		/* Avoid requests that would damage the bus integrity. */
+		if ((ur->ucr_request.bmRequestType == UT_WRITE_DEVICE &&
+		     ur->ucr_request.bRequest == UR_SET_ADDRESS) ||
+		    (ur->ucr_request.bmRequestType == UT_WRITE_DEVICE &&
+		     ur->ucr_request.bRequest == UR_SET_CONFIG) ||
+		    (ur->ucr_request.bmRequestType == UT_WRITE_INTERFACE &&
+		     ur->ucr_request.bRequest == UR_SET_INTERFACE) ||
+		    len < 0 || len > 32767) {
+			error = EINVAL;
+			break;
+		}
+
+		if (len != 0) {
+			iov.iov_base = (caddr_t) ur->ucr_data;
+			iov.iov_len = len;
+
+			uio.uio_iov = &iov;
+			uio.uio_iovcnt = 1;
+			uio.uio_resid = len;
+			uio.uio_offset = 0;
+			uio.uio_segflg = UIO_USERSPACE;
+			uio.uio_rw = ur->ucr_request.bmRequestType & UT_READ ?
+						UIO_READ : UIO_WRITE;
+			uio.uio_procp = p;
+
+			ptr = malloc(len, M_TEMP, M_WAITOK);
+			if (uio.uio_rw == UIO_WRITE) {
+				error = uiomove(ptr, len, &uio);
+				if (error != 0)
+					goto ret;
+			}
+		}
+
+		status = usbd_do_request_flags(sc->sc_udev, &ur->ucr_request,
+				ptr, ur->ucr_flags, &ur->ucr_actlen,
+				USBD_DEFAULT_TIMEOUT);
+		if (status != USBD_NORMAL_COMPLETION) {
+			error = EIO;
+			goto ret;
+		}
+
+		if (len != 0) {
+			if (uio.uio_rw == UIO_READ) {
+				error = uiomove(ptr, len, &uio);
+				if (error != 0)
+					goto ret;
+			}
+		}
+ret:
+		if (ptr != NULL)
+			free(ptr, M_TEMP);
+		break;
+
+	case USB_GET_DEVICEINFO:
+		usbd_fill_deviceinfo(sc->sc_udev,
+			(struct usb_device_info *) data, 1);
+		break;
+
+	default:
+		error = EINVAL;
+		break;
+	}
+
+	if (-- sc->sc_refcnt < 0)
+		usb_detach_wakeup(USBDEV(sc->sc_dev));
+
+	return (error);
+} /* ubt_ioctl */
+
 /*
- * Abort transfers and close all USB pipes.
+ * Poll the endpoint device
+ * XXX FIXME softc locking
+ */
+
+Static int
+ubt_poll(dev_t dev, int events, usb_proc_ptr p)
+{
+	ubt_softc_p	sc = NULL;
+	int		revents = 0, ep = UBT_ENDPOINT(dev);
+
+	USB_GET_SC(ubt, UBT_UNIT(dev), sc);
+	if (sc == NULL || sc->sc_dying)
+		return (ENXIO);
+
+	if (ep == USB_CONTROL_ENDPOINT)
+		return (EOPNOTSUPP);
+
+	if (ep == UE_GET_ADDR(sc->sc_intr_ep)) {
+		if (sc->sc_intr_pipe != NULL)
+			revents |= events & (POLLIN | POLLRDNORM);
+		else
+			revents = EIO;
+	} else if (ep == UE_GET_ADDR(sc->sc_bulk_in_ep)) {
+		if (sc->sc_bulk_in_pipe != NULL) 
+			revents |= events & (POLLIN | POLLRDNORM);
+
+		if (sc->sc_bulk_out_pipe != NULL)
+			revents |= events & (POLLOUT | POLLWRNORM);
+
+		if (revents == 0)
+			revents = EIO; /* both pipes closed */
+	} else
+		revents = EINVAL;
+
+	return (revents);
+} /* ubt_poll */
+
+/*
+ * Create device nodes for all endpoints. Must be called with node locked.
  */
 
 Static void
-ng_ubt_reset(ubt_softc_p sc)
+ubt_create_device_nodes(ubt_softc_p sc)
 {
-	/* Abort transfers and close all USB pipes */
+	int	ep;
 
-	/* Interrupt */
-	if (sc->sc_intr_pipe != NULL) {
-		usbd_abort_pipe(sc->sc_intr_pipe);
-		usbd_close_pipe(sc->sc_intr_pipe);
-		sc->sc_intr_pipe = NULL;
+	KASSERT((sc->sc_hook == NULL), (
+"%s: %s - hook != NULL!\n", __func__, USBDEVNAME(sc->sc_dev)));
+
+	/* Control device */
+	if (sc->sc_ctrl_dev == NODEV)
+		sc->sc_ctrl_dev = make_dev(&ubt_cdevsw,
+					UBT_MINOR(USBDEVUNIT(sc->sc_dev), 0),
+					UID_ROOT, GID_OPERATOR, 0644,
+					"%s", USBDEVNAME(sc->sc_dev));
+
+	/* Interrupt device */
+	if (sc->sc_intr_dev == NODEV && sc->sc_intr_ep != -1) {
+		ep = UE_GET_ADDR(sc->sc_intr_ep);
+		sc->sc_intr_dev = make_dev(&ubt_cdevsw,
+					UBT_MINOR(USBDEVUNIT(sc->sc_dev), ep),
+					UID_ROOT, GID_OPERATOR, 0644,
+					"%s.%d", USBDEVNAME(sc->sc_dev), ep);
 	}
 
-	/* Bulk-in/out */
-	if (sc->sc_bulk_in_pipe != NULL) {
-		usbd_abort_pipe(sc->sc_bulk_in_pipe);
-		usbd_close_pipe(sc->sc_bulk_in_pipe);
-		sc->sc_bulk_in_pipe = NULL;
+	/* 
+	 * Bulk-in and bulk-out device
+	 * XXX will create one device for both in and out endpoints.
+	 * XXX note that address of the in and out endpoint should be the same
+	 */
+
+	if (sc->sc_bulk_dev == NODEV && 
+	    sc->sc_bulk_in_ep != -1 && sc->sc_bulk_out_ep != -1 &&
+	    UE_GET_ADDR(sc->sc_bulk_in_ep) == UE_GET_ADDR(sc->sc_bulk_out_ep)) {
+		ep = UE_GET_ADDR(sc->sc_bulk_in_ep);
+		sc->sc_bulk_dev = make_dev(&ubt_cdevsw,
+					UBT_MINOR(USBDEVUNIT(sc->sc_dev), ep),
+					UID_ROOT, GID_OPERATOR, 0644,
+					"%s.%d", USBDEVNAME(sc->sc_dev), ep);
 	}
-	if (sc->sc_bulk_out_pipe != NULL) {
-		usbd_abort_pipe(sc->sc_bulk_out_pipe);
-		usbd_close_pipe(sc->sc_bulk_out_pipe);
-		sc->sc_bulk_out_pipe = NULL;
+} /* ubt_create_device_nodes */
+
+/*
+ * Destroy device nodes for all endpoints
+ * XXX FIXME locking
+ */
+
+Static void
+ubt_destroy_device_nodes(ubt_softc_p sc)
+{
+	struct vnode	*vp = NULL;
+
+	/*
+	 * Wait for processes to go away. This should be safe as we will not
+	 * call ubt_destroy_device_nodes() from Netgraph unless all devices
+	 * were closed (and thus no active processes).
+	 */
+
+	if (-- sc->sc_refcnt >= 0) {
+		ubt_reset(sc);
+		usb_detach_wait(USBDEV(sc->sc_dev));
 	}
 
-	/* Isoc-in/out */
-	if (sc->sc_isoc_in_pipe != NULL) {
-		usbd_abort_pipe(sc->sc_isoc_in_pipe);
-		usbd_close_pipe(sc->sc_isoc_in_pipe);
-		sc->sc_isoc_in_pipe = NULL;
+	sc->sc_refcnt = 0;
+
+	/* Destroy device nodes */
+	if (sc->sc_bulk_dev != NODEV) {
+		vp = SLIST_FIRST(&sc->sc_bulk_dev->si_hlist);
+		if (vp != NULL) 
+			VOP_REVOKE(vp, REVOKEALL);
+
+		destroy_dev(sc->sc_bulk_dev);
+		sc->sc_bulk_dev = NODEV;
 	}
-	if (sc->sc_isoc_out_pipe != NULL) {
-		usbd_abort_pipe(sc->sc_isoc_out_pipe);
-		usbd_close_pipe(sc->sc_isoc_out_pipe);
-		sc->sc_isoc_out_pipe = NULL;
+
+	if (sc->sc_intr_dev != NODEV) {
+		vp = SLIST_FIRST(&sc->sc_intr_dev->si_hlist);
+		if (vp != NULL) 
+			VOP_REVOKE(vp, REVOKEALL);
+
+		destroy_dev(sc->sc_intr_dev);
+		sc->sc_intr_dev = NODEV;
 	}
 
-	/* Cleanup queues */
-	IF_DRAIN(&sc->sc_cmdq);
-	IF_DRAIN(&sc->sc_aclq);
-	IF_DRAIN(&sc->sc_scoq);
-	IF_DRAIN(&sc->sc_inq);
-} /* ng_ubt_reset */
+	if (sc->sc_ctrl_dev != NODEV) {
+		vp = SLIST_FIRST(&sc->sc_ctrl_dev->si_hlist);
+		if (vp != NULL) 
+			VOP_REVOKE(vp, REVOKEALL);
+
+		destroy_dev(sc->sc_ctrl_dev);
+		sc->sc_ctrl_dev = NODEV;
+	}
+} /* ubt_destroy_device_nodes */
 
diff --git a/sys/netgraph/bluetooth/drivers/ubt/ng_ubt_var.h b/sys/netgraph/bluetooth/drivers/ubt/ng_ubt_var.h
index 10b8911..3fc3322 100644
--- a/sys/netgraph/bluetooth/drivers/ubt/ng_ubt_var.h
+++ b/sys/netgraph/bluetooth/drivers/ubt/ng_ubt_var.h
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_ubt_var.h,v 1.1 2002/11/09 19:09:02 max Exp $
+ * $Id: ng_ubt_var.h,v 1.2 2003/03/22 23:44:36 max Exp $
  * $FreeBSD$
  */
 
@@ -62,6 +62,13 @@ struct ubt_softc {
 #define UBT_CMD_XMIT		(1 << 1)	/* CMD xmit in progress */
 #define UBT_ACL_XMIT		(1 << 2)	/* ACL xmit in progress */
 #define UBT_SCO_XMIT		(1 << 3)	/* SCO xmit in progress */
+#define UBT_EVT_RECV		(1 << 4)	/* EVN recv in progress */
+#define UBT_ACL_RECV		(1 << 5)	/* ACL recv in progress */
+#define UBT_SCO_RECV		(1 << 6)	/* SCO recv in progress */
+#define UBT_CTRL_DEV		(1 << 7)	/* ctrl device is open */
+#define UBT_INTR_DEV		(1 << 8)	/* intr device is open */
+#define UBT_BULK_DEV		(1 << 9)	/* bulk device is open */
+#define UBT_ANY_DEV		(UBT_CTRL_DEV|UBT_INTR_DEV|UBT_BULK_DEV)
 
 	ng_ubt_node_stat_ep	 sc_stat;	/* statistic */
 #define NG_UBT_STAT_PCKTS_SENT(s)	(s).pckts_sent ++
@@ -79,21 +86,16 @@ struct ubt_softc {
 	usbd_interface_handle	 sc_iface0;	/* USB interface 0 */
 	usbd_interface_handle	 sc_iface1;	/* USB interface 1 */
 
-	struct ifqueue		 sc_inq;	/* incoming queue */
-	void			*sc_ith;	/* SWI interrupt handler */
-
 	/* Interrupt pipe (HCI events) */
 	int			 sc_intr_ep;	/* interrupt endpoint */
 	usbd_pipe_handle	 sc_intr_pipe;	/* interrupt pipe handle */
 	usbd_xfer_handle	 sc_intr_xfer;	/* intr xfer */
-	u_int8_t		*sc_intr_buffer; /* interrupt buffer */
-#define UBT_INTR_BUFFER_SIZE \
-		(sizeof(ng_hci_event_pkt_t) + NG_HCI_EVENT_PKT_SIZE)
+	struct mbuf		*sc_intr_buffer; /* interrupt buffer */
 
 	/* Control pipe (HCI commands) */
 	usbd_xfer_handle	 sc_ctrl_xfer;	/* control xfer handle */
 	void			*sc_ctrl_buffer; /* control buffer */
-	struct ifqueue		 sc_cmdq;	/* HCI command queue */
+	struct ng_bt_mbufq	 sc_cmdq;	/* HCI command queue */
 #define UBT_CTRL_BUFFER_SIZE \
 		(sizeof(ng_hci_cmd_pkt_t) + NG_HCI_CMD_PKT_SIZE)
 
@@ -101,16 +103,16 @@ struct ubt_softc {
 	int			 sc_bulk_in_ep;	/* bulk-in enpoint */
 	usbd_pipe_handle	 sc_bulk_in_pipe; /* bulk-in pipe */
 	usbd_xfer_handle	 sc_bulk_in_xfer; /* bulk-in xfer */
-	void			*sc_bulk_in_buffer; /* bulk-in buffer */
+	struct mbuf		*sc_bulk_in_buffer; /* bulk-in buffer */
 
 	/* Bulk out pipe (ACL data) */
 	int			 sc_bulk_out_ep; /* bulk-out endpoint */
 	usbd_pipe_handle	 sc_bulk_out_pipe; /* bulk-out pipe */
 	usbd_xfer_handle	 sc_bulk_out_xfer; /* bulk-out xfer */
 	void			*sc_bulk_out_buffer; /* bulk-out buffer */
-	struct ifqueue		 sc_aclq;	/* ACL data queue */
+	struct ng_bt_mbufq	 sc_aclq;	/* ACL data queue */
 #define UBT_BULK_BUFFER_SIZE \
-		512	/* XXX should be big enough to hold one frame */
+		MCLBYTES /* XXX should be big enough to hold one frame */
 
 	/* Isoc. in pipe (SCO data) */
 	int			 sc_isoc_in_ep; /* isoc-in endpoint */
@@ -125,7 +127,7 @@ struct ubt_softc {
 	usbd_xfer_handle	 sc_isoc_out_xfer; /* isoc-out xfer */
 	void			*sc_isoc_out_buffer; /* isoc-in buffer */
 	u_int16_t		*sc_isoc_out_frlen; /* isoc-out. frame length */
-	struct ifqueue		 sc_scoq;	/* SCO data queue */
+	struct ng_bt_mbufq	 sc_scoq;	/* SCO data queue */
 
 	int			 sc_isoc_size; /* max. size of isoc. packet */
 	u_int32_t		 sc_isoc_nframes; /* num. isoc. frames */
@@ -135,6 +137,14 @@ struct ubt_softc {
 	/* Netgraph specific */
 	node_p			 sc_node;	/* pointer back to node */
 	hook_p			 sc_hook;	/* upstream hook */
+
+	/* Device specific */
+	dev_t			 sc_ctrl_dev;	/* control device */
+	dev_t			 sc_intr_dev;	/* interrupt device */
+	dev_t			 sc_bulk_dev;	/* bulk device */
+
+	int			 sc_refcnt;	/* device ref. count */
+	int			 sc_dying;
 };
 typedef struct ubt_softc	ubt_softc_t;
 typedef struct ubt_softc *	ubt_softc_p;
diff --git a/sys/netgraph/bluetooth/drivers/ubtbcmfw/ubtbcmfw.c b/sys/netgraph/bluetooth/drivers/ubtbcmfw/ubtbcmfw.c
new file mode 100644
index 0000000..acd51ce
--- /dev/null
+++ b/sys/netgraph/bluetooth/drivers/ubtbcmfw/ubtbcmfw.c
@@ -0,0 +1,571 @@
+/*
+ * ubtbcmfw.c
+ *
+ * Copyright (c) 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: ubtbcmfw.c,v 1.1 2003/04/27 00:20:15 max Exp $
+ * $FreeBSD$
+ */
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/bus.h>
+#include <sys/conf.h>
+#include <sys/filio.h>
+#include <sys/fcntl.h>
+#include <sys/kernel.h>
+#include <sys/poll.h>
+#include <sys/proc.h>
+#include <sys/sysctl.h>
+#include <sys/vnode.h>
+
+#include <dev/usb/usb.h>
+#include <dev/usb/usbdi.h>
+#include <dev/usb/usbdi_util.h>
+#include <dev/usb/usbdevs.h>
+
+/*
+ * Download firmware to BCM2033.
+ */
+
+#define UBTBCMFW_CONFIG_NO	1	/* Config number */
+#define UBTBCMFW_IFACE_IDX	0 	/* Control interface */
+#define UBTBCMFW_INTR_IN_EP	0x81	/* Fixed endpoint */
+#define UBTBCMFW_BULK_OUT_EP	0x02	/* Fixed endpoint */
+#define UBTBCMFW_INTR_IN	UE_GET_ADDR(UBTBCMFW_INTR_IN_EP)
+#define UBTBCMFW_BULK_OUT	UE_GET_ADDR(UBTBCMFW_BULK_OUT_EP)
+
+struct ubtbcmfw_softc {
+	USBBASEDEVICE		sc_dev;			/* base device */
+	usbd_device_handle	sc_udev;		/* USB device handle */
+	dev_t			sc_ctrl_dev;		/* control device */
+	dev_t			sc_intr_in_dev;		/* interrupt device */
+	dev_t			sc_bulk_out_dev;	/* bulk device */
+	usbd_pipe_handle	sc_intr_in_pipe;	/* interrupt pipe */
+	usbd_pipe_handle	sc_bulk_out_pipe;	/* bulk out pipe */
+	int			sc_flags;
+#define UBTBCMFW_CTRL_DEV	(1 << 0)
+#define UBTBCMFW_INTR_IN_DEV	(1 << 1)
+#define UBTBCMFW_BULK_OUT_DEV	(1 << 2)
+	int			sc_refcnt;
+	int			sc_dying;
+};
+
+typedef struct ubtbcmfw_softc	*ubtbcmfw_softc_p;
+
+/*
+ * Device methods
+ */
+
+#define UBTBCMFW_UNIT(n)	((minor(n) >> 4) & 0xf)
+#define UBTBCMFW_ENDPOINT(n)	(minor(n) & 0xf)
+#define UBTBCMFW_MINOR(u, e)	(((u) << 4) | (e))
+#define UBTBCMFW_BSIZE		1024
+
+Static d_open_t		ubtbcmfw_open;
+Static d_close_t	ubtbcmfw_close;
+Static d_read_t		ubtbcmfw_read;
+Static d_write_t	ubtbcmfw_write;
+Static d_ioctl_t	ubtbcmfw_ioctl;
+Static d_poll_t		ubtbcmfw_poll;
+
+#if __FreeBSD_version < 500104
+#define CDEV_MAJOR	223
+#else
+#define CDEV_MAJOR	MAJOR_AUTO
+#endif
+
+Static struct cdevsw	ubtbcmfw_cdevsw = {
+	.d_open =	ubtbcmfw_open,
+	.d_close =	ubtbcmfw_close,
+	.d_read =	ubtbcmfw_read,
+	.d_write =	ubtbcmfw_write,
+	.d_ioctl =	ubtbcmfw_ioctl,
+	.d_poll =	ubtbcmfw_poll,
+	.d_name =	"ubtbcmfw",
+	.d_maj =	CDEV_MAJOR,
+};
+
+/*
+ * Module
+ */
+
+USB_DECLARE_DRIVER(ubtbcmfw);
+DRIVER_MODULE(ubtbcmfw, uhub, ubtbcmfw_driver, ubtbcmfw_devclass,
+	      usbd_driver_load, 0);
+
+/*
+ * Probe for a USB Bluetooth device
+ */
+
+USB_MATCH(ubtbcmfw)
+{
+#define	USB_PRODUCT_BROADCOM_BCM2033NF	0x2033
+
+	USB_MATCH_START(ubtbcmfw, uaa);
+
+	if (uaa->iface != NULL)
+		return (UMATCH_NONE);
+
+	/* Match the boot device. */
+	if (uaa->vendor == USB_VENDOR_BROADCOM &&
+	    uaa->product == USB_PRODUCT_BROADCOM_BCM2033NF)
+		return (UMATCH_VENDOR_PRODUCT);
+
+	return (UMATCH_NONE);
+}
+
+/*
+ * Attach the device
+ */
+
+USB_ATTACH(ubtbcmfw)
+{
+	USB_ATTACH_START(ubtbcmfw, sc, uaa);
+	usbd_interface_handle	iface;
+	usbd_status		err;
+	char			devinfo[1024];
+
+	sc->sc_udev = uaa->device;
+	usbd_devinfo(sc->sc_udev, 0, devinfo);
+	USB_ATTACH_SETUP;
+	printf("%s: %s\n", USBDEVNAME(sc->sc_dev), devinfo);
+
+	sc->sc_ctrl_dev = sc->sc_intr_in_dev = sc->sc_bulk_out_dev = NODEV;
+	sc->sc_intr_in_pipe = sc->sc_bulk_out_pipe = NULL;
+	sc->sc_flags = sc->sc_refcnt = sc->sc_dying = 0;
+
+	err = usbd_set_config_no(sc->sc_udev, UBTBCMFW_CONFIG_NO, 1);
+	if (err) {
+		printf("%s: setting config no failed. %s\n",
+			USBDEVNAME(sc->sc_dev), usbd_errstr(err));
+		goto bad;
+	}
+
+	err = usbd_device2interface_handle(sc->sc_udev, UBTBCMFW_IFACE_IDX,
+			&iface);
+	if (err) {
+		printf("%s: getting interface handle failed. %s\n",
+			USBDEVNAME(sc->sc_dev), usbd_errstr(err));
+		goto bad;
+	}
+
+	/* Will be used as a bulk pipe */
+	err = usbd_open_pipe(iface, UBTBCMFW_INTR_IN_EP, 0,
+			&sc->sc_intr_in_pipe);
+	if (err) {
+		printf("%s: open intr in failed. %s\n",
+			USBDEVNAME(sc->sc_dev), usbd_errstr(err));
+		goto bad;
+	}
+
+	err = usbd_open_pipe(iface, UBTBCMFW_BULK_OUT_EP, 0,
+			&sc->sc_bulk_out_pipe);
+	if (err) {
+		printf("%s: open bulk out failed. %s\n",
+			USBDEVNAME(sc->sc_dev), usbd_errstr(err));
+		goto bad;
+	}
+
+	/* Create device nodes */
+	sc->sc_ctrl_dev = make_dev(&ubtbcmfw_cdevsw,
+		UBTBCMFW_MINOR(USBDEVUNIT(sc->sc_dev), 0),
+		UID_ROOT, GID_OPERATOR, 0644,
+		"%s", USBDEVNAME(sc->sc_dev));
+
+	sc->sc_intr_in_dev = make_dev(&ubtbcmfw_cdevsw,
+		UBTBCMFW_MINOR(USBDEVUNIT(sc->sc_dev), UBTBCMFW_INTR_IN),
+		UID_ROOT, GID_OPERATOR, 0644,
+		"%s.%d", USBDEVNAME(sc->sc_dev), UBTBCMFW_INTR_IN);
+
+	sc->sc_bulk_out_dev = make_dev(&ubtbcmfw_cdevsw,
+		UBTBCMFW_MINOR(USBDEVUNIT(sc->sc_dev), UBTBCMFW_BULK_OUT),
+		UID_ROOT, GID_OPERATOR, 0644,
+		"%s.%d", USBDEVNAME(sc->sc_dev), UBTBCMFW_BULK_OUT);
+
+	USB_ATTACH_SUCCESS_RETURN;
+bad:
+	ubtbcmfw_detach(self);  
+        
+        USB_ATTACH_ERROR_RETURN;
+}
+
+/*
+ * Detach the device
+ */
+
+USB_DETACH(ubtbcmfw)
+{
+	USB_DETACH_START(ubtbcmfw, sc);
+
+	struct vnode	*vp = NULL;
+
+	sc->sc_dying = 1;
+
+	if (-- sc->sc_refcnt >= 0) {
+		if (sc->sc_intr_in_pipe != NULL) 
+			usbd_abort_pipe(sc->sc_intr_in_pipe);
+
+		if (sc->sc_bulk_out_pipe != NULL) 
+			usbd_abort_pipe(sc->sc_bulk_out_pipe);
+
+		usb_detach_wait(USBDEV(sc->sc_dev));
+	}
+
+	/* Destroy device nodes */
+	if (sc->sc_bulk_out_dev != NODEV) {
+		vp = SLIST_FIRST(&sc->sc_bulk_out_dev->si_hlist);
+		if (vp != NULL)
+			VOP_REVOKE(vp, REVOKEALL);
+
+		destroy_dev(sc->sc_bulk_out_dev);
+		sc->sc_bulk_out_dev = NODEV;
+	}
+
+	if (sc->sc_intr_in_dev != NODEV) {
+		vp = SLIST_FIRST(&sc->sc_intr_in_dev->si_hlist);
+		if (vp != NULL)
+			VOP_REVOKE(vp, REVOKEALL);
+
+		destroy_dev(sc->sc_intr_in_dev);
+		sc->sc_intr_in_dev = NODEV;
+	}
+
+	if (sc->sc_ctrl_dev != NODEV) {
+		vp = SLIST_FIRST(&sc->sc_ctrl_dev->si_hlist);
+		if (vp != NULL)
+			VOP_REVOKE(vp, REVOKEALL);
+
+		destroy_dev(sc->sc_ctrl_dev);
+		sc->sc_ctrl_dev = NODEV;
+	}
+
+	/* Close pipes */
+	if (sc->sc_intr_in_pipe != NULL) {
+		usbd_close_pipe(sc->sc_intr_in_pipe);
+		sc->sc_intr_in_pipe = NULL;
+	}
+
+	if (sc->sc_bulk_out_pipe != NULL) {
+		usbd_close_pipe(sc->sc_bulk_out_pipe);
+		sc->sc_intr_in_pipe = NULL;
+	}
+
+	return (0);
+}
+
+/*
+ * Open endpoint device
+ * XXX FIXME softc locking
+ */
+
+Static int
+ubtbcmfw_open(dev_t dev, int flag, int mode, usb_proc_ptr p)
+{
+	ubtbcmfw_softc_p	sc = NULL;
+	int			error = 0;
+
+	/* checks for sc != NULL */
+	USB_GET_SC_OPEN(ubtbcmfw, UBTBCMFW_UNIT(dev), sc);
+	if (sc->sc_dying)
+		return (ENXIO);
+
+	switch (UBTBCMFW_ENDPOINT(dev)) {
+	case USB_CONTROL_ENDPOINT:
+		if (!(sc->sc_flags & UBTBCMFW_CTRL_DEV))
+			sc->sc_flags |= UBTBCMFW_CTRL_DEV;
+		else
+			error = EBUSY;
+		break;
+
+	case UBTBCMFW_INTR_IN:
+		if (!(sc->sc_flags & UBTBCMFW_INTR_IN_DEV)) {
+			if (sc->sc_intr_in_pipe != NULL)
+				sc->sc_flags |= UBTBCMFW_INTR_IN_DEV;
+			else
+				error = ENXIO;
+		} else
+			error = EBUSY;
+		break;
+
+	case UBTBCMFW_BULK_OUT:
+		if (!(sc->sc_flags & UBTBCMFW_BULK_OUT_DEV)) {
+			if (sc->sc_bulk_out_pipe != NULL)
+				sc->sc_flags |= UBTBCMFW_BULK_OUT_DEV;
+			else
+				error = ENXIO;
+		} else
+			error = EBUSY;
+		break;
+
+	default:
+		error = ENXIO;
+		break;
+	}
+
+	return (error);
+}
+
+/*
+ * Close endpoint device
+ * XXX FIXME softc locking
+ */
+
+Static int
+ubtbcmfw_close(dev_t dev, int flag, int mode, usb_proc_ptr p)
+{
+	ubtbcmfw_softc_p	sc = NULL;
+
+	USB_GET_SC(ubtbcmfw, UBTBCMFW_UNIT(dev), sc);
+	if (sc == NULL)
+		return (ENXIO);
+
+	switch (UBTBCMFW_ENDPOINT(dev)) {
+	case USB_CONTROL_ENDPOINT:
+		sc->sc_flags &= ~UBTBCMFW_CTRL_DEV;
+		break;
+
+	case UBTBCMFW_INTR_IN:
+		if (sc->sc_intr_in_pipe != NULL)
+			usbd_abort_pipe(sc->sc_intr_in_pipe);
+
+		sc->sc_flags &= ~UBTBCMFW_INTR_IN_DEV;
+		break;
+
+	case UBTBCMFW_BULK_OUT:
+		if (sc->sc_bulk_out_pipe != NULL)
+			usbd_abort_pipe(sc->sc_bulk_out_pipe);
+
+		sc->sc_flags &= ~UBTBCMFW_BULK_OUT_DEV;
+		break;
+	}
+
+	return (0);
+}
+
+/*
+ * Read from the endpoint device
+ * XXX FIXME softc locking
+ */
+
+Static int
+ubtbcmfw_read(dev_t dev, struct uio *uio, int flag)
+{
+	ubtbcmfw_softc_p	sc = NULL;
+	u_int8_t		buf[UBTBCMFW_BSIZE];
+	usbd_xfer_handle	xfer;
+	usbd_status		err;
+	int			n, tn, error = 0;
+
+	USB_GET_SC(ubtbcmfw, UBTBCMFW_UNIT(dev), sc);
+	if (sc == NULL || sc->sc_dying)
+		return (ENXIO);
+
+	if (UBTBCMFW_ENDPOINT(dev) != UBTBCMFW_INTR_IN)
+		return (EOPNOTSUPP);
+	if (sc->sc_intr_in_pipe == NULL)
+		return (ENXIO);
+
+	xfer = usbd_alloc_xfer(sc->sc_udev);
+	if (xfer == NULL)
+		return (ENOMEM);
+
+	sc->sc_refcnt ++;
+
+	while ((n = min(sizeof(buf), uio->uio_resid)) != 0) {
+		tn = n;
+		err = usbd_bulk_transfer(xfer, sc->sc_intr_in_pipe,
+				USBD_SHORT_XFER_OK, USBD_DEFAULT_TIMEOUT,
+				buf, &tn, "bcmrd");
+		switch (err) {
+		case USBD_NORMAL_COMPLETION:
+			error = uiomove(buf, tn, uio);
+			break;
+
+		case USBD_INTERRUPTED:
+			error = EINTR;
+			break;
+
+		case USBD_TIMEOUT:
+			error = ETIMEDOUT;
+			break;
+
+		default:
+			error = EIO;
+			break;
+		}
+
+		if (error != 0 || tn < n)
+			break;
+	}
+
+	usbd_free_xfer(xfer);
+
+	if (-- sc->sc_refcnt < 0)
+		usb_detach_wakeup(USBDEV(sc->sc_dev));
+
+	return (error);
+}
+
+/*
+ * Write into the endpoint device
+ * XXX FIXME softc locking
+ */
+
+Static int
+ubtbcmfw_write(dev_t dev, struct uio *uio, int flag)
+{
+	ubtbcmfw_softc_p	sc = NULL;
+	u_int8_t		buf[UBTBCMFW_BSIZE];
+	usbd_xfer_handle	xfer;
+	usbd_status		err;
+	int			n, error = 0;
+
+	USB_GET_SC(ubtbcmfw, UBTBCMFW_UNIT(dev), sc);
+	if (sc == NULL || sc->sc_dying)
+		return (ENXIO);
+
+	if (UBTBCMFW_ENDPOINT(dev) != UBTBCMFW_BULK_OUT)
+		return (EOPNOTSUPP);
+	if (sc->sc_bulk_out_pipe == NULL)
+		return (ENXIO);
+
+	xfer = usbd_alloc_xfer(sc->sc_udev);
+	if (xfer == NULL)
+		return (ENOMEM);
+
+	sc->sc_refcnt ++;
+
+	while ((n = min(sizeof(buf), uio->uio_resid)) != 0) {
+		error = uiomove(buf, n, uio);
+		if (error != 0)
+			break;
+
+		err = usbd_bulk_transfer(xfer, sc->sc_bulk_out_pipe,
+				0, USBD_DEFAULT_TIMEOUT, buf, &n, "bcmwr");
+		switch (err) {
+		case USBD_NORMAL_COMPLETION:
+			break;
+
+		case USBD_INTERRUPTED:
+			error = EINTR;
+			break;
+
+		case USBD_TIMEOUT:
+			error = ETIMEDOUT;
+			break;
+
+		default:
+			error = EIO;
+			break;
+		}
+
+		if (error != 0)
+			break;
+	}
+
+	usbd_free_xfer(xfer);
+
+	if (-- sc->sc_refcnt < 0)
+		usb_detach_wakeup(USBDEV(sc->sc_dev));
+
+	return (error);
+}
+
+/*
+ * Process ioctl on the endpoint device
+ * XXX FIXME softc locking
+ */
+
+Static int
+ubtbcmfw_ioctl(dev_t dev, u_long cmd, caddr_t data, int flag, usb_proc_ptr p)
+{
+	ubtbcmfw_softc_p	sc = NULL;
+	int			error = 0;
+
+	USB_GET_SC(ubtbcmfw, UBTBCMFW_UNIT(dev), sc);
+	if (sc == NULL || sc->sc_dying)
+		return (ENXIO);
+
+	if (UBTBCMFW_ENDPOINT(dev) != USB_CONTROL_ENDPOINT)
+		return (EOPNOTSUPP);
+
+	sc->sc_refcnt ++;
+
+	switch (cmd) {
+	case USB_GET_DEVICE_DESC:
+		*(usb_device_descriptor_t *) data =
+				*usbd_get_device_descriptor(sc->sc_udev);
+		break;
+
+	default:
+		error = EINVAL;
+		break;
+	}
+
+	if (-- sc->sc_refcnt < 0)
+		usb_detach_wakeup(USBDEV(sc->sc_dev));
+
+	return (error);
+}
+
+/*
+ * Poll the endpoint device
+ * XXX FIXME softc locking
+ */
+
+Static int
+ubtbcmfw_poll(dev_t dev, int events, usb_proc_ptr p)
+{
+	ubtbcmfw_softc_p	sc = NULL;
+	int			revents = 0;
+
+	USB_GET_SC(ubtbcmfw, UBTBCMFW_UNIT(dev), sc);
+	if (sc == NULL)
+		return (ENXIO);
+
+	switch (UBTBCMFW_ENDPOINT(dev)) {
+	case UBTBCMFW_INTR_IN:
+		if (sc->sc_intr_in_pipe != NULL)
+			revents |= events & (POLLIN | POLLRDNORM);
+		else
+			revents = ENXIO;
+		break;
+
+	case UBTBCMFW_BULK_OUT:
+		if (sc->sc_bulk_out_pipe != NULL)
+			revents |= events & (POLLOUT | POLLWRNORM);
+		else
+			revents = ENXIO;
+		break;
+
+	default:
+		revents = EOPNOTSUPP;
+		break;
+	}
+
+	return (revents);
+}
+
diff --git a/sys/netgraph/bluetooth/hci/TODO b/sys/netgraph/bluetooth/hci/TODO
index f277e7e..6f66d7b 100644
--- a/sys/netgraph/bluetooth/hci/TODO
+++ b/sys/netgraph/bluetooth/hci/TODO
@@ -1,6 +1,5 @@
-# $FreeBSD$
-
-$Id: TODO,v 1.10 2002/09/06 21:03:57 max Exp $
+$Id: TODO,v 1.2 2003/04/26 22:36:29 max Exp $
+$FreeBSD$
 
 FIXME/TODO list
 
@@ -12,30 +11,18 @@ This is a list of open issues for HCI node
 	it should be fine as long as Netgraph is SMP safe. Just need to 
 	verify it.
 
-3) HCI QoS handling
+2) HCI QoS handling
 
 	Some code exists, but i have no idea how it should work. Will 
 	understand and fix later. I only have CSR based hardware and
 	it does not support QoS.
 
-4) Add proper handling for some HCI commands
+3) Add proper handling for some HCI commands
 
 	HCI testing commands is one example. Also might implement Host to 
 	Host Controller flow control (not sure if it is required).
 
-5) Link security
-
-	Manage link keys and PINs. Options:
-
-	1) manage keys inside HCI node/unit itself
-	2) use user space daemon.
-	3) Mix option 1 and option 2.
-
-6) Implement watchdog routine for HCI connections
-
-	Some code exists, but it is not used
-
-7) Code cleanup
+4) Code cleanup
 
 	Verify return codes from functions
 	Remove some waringns/errors
diff --git a/sys/netgraph/bluetooth/hci/ng_hci_cmds.c b/sys/netgraph/bluetooth/hci/ng_hci_cmds.c
index 5769ee5..83b640b 100644
--- a/sys/netgraph/bluetooth/hci/ng_hci_cmds.c
+++ b/sys/netgraph/bluetooth/hci/ng_hci_cmds.c
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_hci_cmds.c,v 1.22 2002/10/30 00:18:18 max Exp $
+ * $Id: ng_hci_cmds.c,v 1.3 2003/04/01 18:15:25 max Exp $
  * $FreeBSD$
  */
 
diff --git a/sys/netgraph/bluetooth/hci/ng_hci_cmds.h b/sys/netgraph/bluetooth/hci/ng_hci_cmds.h
index 1123802..6cf75d6 100644
--- a/sys/netgraph/bluetooth/hci/ng_hci_cmds.h
+++ b/sys/netgraph/bluetooth/hci/ng_hci_cmds.h
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_hci_cmds.h,v 1.6 2002/09/04 21:36:51 max Exp $
+ * $Id: ng_hci_cmds.h,v 1.1 2002/11/24 19:46:58 max Exp $
  * $FreeBSD$
  */
 
diff --git a/sys/netgraph/bluetooth/hci/ng_hci_evnt.c b/sys/netgraph/bluetooth/hci/ng_hci_evnt.c
index 9d81380..1f1c74a 100644
--- a/sys/netgraph/bluetooth/hci/ng_hci_evnt.c
+++ b/sys/netgraph/bluetooth/hci/ng_hci_evnt.c
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_hci_evnt.c,v 1.18 2002/11/12 22:35:39 max Exp $
+ * $Id: ng_hci_evnt.c,v 1.5 2003/04/01 18:15:25 max Exp $
  * $FreeBSD$
  */
 
@@ -60,6 +60,7 @@ static int inquiry_result             (ng_hci_unit_p, struct mbuf *);
 static int con_compl                  (ng_hci_unit_p, struct mbuf *);
 static int con_req                    (ng_hci_unit_p, struct mbuf *);
 static int discon_compl               (ng_hci_unit_p, struct mbuf *);
+static int encryption_change          (ng_hci_unit_p, struct mbuf *);
 static int read_remote_features_compl (ng_hci_unit_p, struct mbuf *);
 static int qos_setup_compl            (ng_hci_unit_p, struct mbuf *);
 static int hardware_error             (ng_hci_unit_p, struct mbuf *);
@@ -100,18 +101,15 @@ ng_hci_process_event(ng_hci_unit_p unit, struct mbuf *event)
 
 	switch (hdr->event) {
 	case NG_HCI_EVENT_INQUIRY_COMPL:
-/*	case NG_HCI_EVENT_MODE_CHANGE: */
 	case NG_HCI_EVENT_RETURN_LINK_KEYS:
 	case NG_HCI_EVENT_PIN_CODE_REQ:
 	case NG_HCI_EVENT_LINK_KEY_REQ:
 	case NG_HCI_EVENT_LINK_KEY_NOTIFICATION:
 	case NG_HCI_EVENT_LOOPBACK_COMMAND:
 	case NG_HCI_EVENT_AUTH_COMPL:
-	case NG_HCI_EVENT_ENCRYPTION_CHANGE:
 	case NG_HCI_EVENT_CHANGE_CON_LINK_KEY_COMPL:
 	case NG_HCI_EVENT_MASTER_LINK_KEY_COMPL:
 	case NG_HCI_EVENT_FLUSH_OCCUR:	/* XXX Do we have to handle it? */
-/*	case NG_HCI_EVENT_ROLE_CHANGE: */
 	case NG_HCI_EVENT_MAX_SLOT_CHANGE:
 	case NG_HCI_EVENT_CON_PKT_TYPE_CHANGED:
 	case NG_HCI_EVENT_BT_LOGO:
@@ -138,6 +136,10 @@ ng_hci_process_event(ng_hci_unit_p unit, struct mbuf *event)
 		error = discon_compl(unit, event);
 		break;
 
+	case NG_HCI_EVENT_ENCRYPTION_CHANGE:
+		error = encryption_change(unit, event);
+		break;
+
 	case NG_HCI_EVENT_READ_REMOTE_FEATURES_COMPL:
 		error = read_remote_features_compl(unit, event);
 		break;
@@ -518,7 +520,8 @@ con_compl(ng_hci_unit_p unit, struct mbuf *event)
 				lp->cp.con_handle = ep->con_handle;
 
 				lp->cp.settings = 0;
-				if (unit->features[0] & NG_HCI_LMP_SWITCH)
+				if ((unit->features[0] & NG_HCI_LMP_SWITCH) &&
+				    unit->role_switch)
 					lp->cp.settings |= 0x1;
 				if (unit->features[0] & NG_HCI_LMP_HOLD_MODE)
 					lp->cp.settings |= 0x2;
@@ -666,6 +669,50 @@ discon_compl(ng_hci_unit_p unit, struct mbuf *event)
 	return (error);
 } /* discon_compl */
 
+/* Encryption change event */
+static int
+encryption_change(ng_hci_unit_p unit, struct mbuf *event)
+{
+	ng_hci_encryption_change_ep	*ep = NULL;
+	ng_hci_unit_con_p		 con = NULL;
+	int				 error = 0;
+
+	NG_HCI_M_PULLUP(event, sizeof(*ep));
+	if (event == NULL)
+		return (ENOBUFS);
+
+	ep = mtod(event, ng_hci_encryption_change_ep *);
+
+	if (ep->status == 0) {
+		u_int16_t	h = NG_HCI_CON_HANDLE(le16toh(ep->con_handle));
+
+		con = ng_hci_con_by_handle(unit, h);
+		if (con == NULL) {
+			NG_HCI_ALERT(
+"%s: %s - invalid connection handle=%d\n",
+				__func__, NG_NODE_NAME(unit->node), h);
+			error = ENOENT;
+		} else if (con->link_type != NG_HCI_LINK_ACL) {
+			NG_HCI_ALERT(
+"%s: %s - invalid link type=%d\n",
+				__func__, NG_NODE_NAME(unit->node), 
+				con->link_type);
+			error = EINVAL;
+		} else if (ep->encryption_enable)
+			/* XXX is that true? */
+			con->encryption_mode = NG_HCI_ENCRYPTION_MODE_P2P;
+		else
+			con->encryption_mode = NG_HCI_ENCRYPTION_MODE_NONE;
+	} else
+		NG_HCI_ERR(
+"%s: %s - failed to change encryption mode, status=%d\n",
+			__func__, NG_NODE_NAME(unit->node), ep->status);
+
+	NG_FREE_M(event);
+
+	return (error);
+} /* encryption_change */
+
 /* Read remote feature complete event */
 static int
 read_remote_features_compl(ng_hci_unit_p unit, struct mbuf *event)
diff --git a/sys/netgraph/bluetooth/hci/ng_hci_evnt.h b/sys/netgraph/bluetooth/hci/ng_hci_evnt.h
index 706d7b0..497dda0 100644
--- a/sys/netgraph/bluetooth/hci/ng_hci_evnt.h
+++ b/sys/netgraph/bluetooth/hci/ng_hci_evnt.h
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_hci_evnt.h,v 1.4 2002/09/04 21:36:51 max Exp $
+ * $Id: ng_hci_evnt.h,v 1.1 2002/11/24 19:46:58 max Exp $
  * $FreeBSD$
  */
 
diff --git a/sys/netgraph/bluetooth/hci/ng_hci_main.c b/sys/netgraph/bluetooth/hci/ng_hci_main.c
index c3c77fc..4a6a62d 100644
--- a/sys/netgraph/bluetooth/hci/ng_hci_main.c
+++ b/sys/netgraph/bluetooth/hci/ng_hci_main.c
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_hci_main.c,v 1.28 2002/11/12 22:35:40 max Exp $
+ * $Id: ng_hci_main.c,v 1.2 2003/03/18 00:09:36 max Exp $
  * $FreeBSD$
  */
 
@@ -119,6 +119,7 @@ ng_hci_constructor(node_p node)
 
 	unit->link_policy_mask = 0xffff; /* Enable all supported modes */
 	unit->packet_mask = 0xffff; /* Enable all packet types */
+	unit->role_switch = 1; /* Enable role switch (if device supports it) */
 
 	/*
 	 * Set default buffer info
@@ -535,7 +536,6 @@ ng_hci_default_rcvmsg(node_p node, item_p item, hook_p lasthook)
 				bcopy(&c->bdaddr, &e2->bdaddr, 
 					sizeof(e2->bdaddr));
 
-
 				e2 ++;
 				if (--s <= 0)
 					break;
@@ -593,6 +593,31 @@ ng_hci_default_rcvmsg(node_p node, item_p item, hook_p lasthook)
 				*((ng_hci_node_packet_mask_ep *)(msg->data));
 			break;
 
+		/* Get role switch */
+		case NGM_HCI_NODE_GET_ROLE_SWITCH:
+			NG_MKRESPONSE(rsp, msg, sizeof(unit->role_switch),
+				M_NOWAIT);
+			if (rsp == NULL) {
+				error = ENOMEM;
+				break;
+			}
+
+			*((ng_hci_node_role_switch_ep *)(rsp->data)) = 
+				unit->role_switch;
+			break;
+
+		/* Set role switch */
+		case NGM_HCI_NODE_SET_ROLE_SWITCH:
+			if (msg->header.arglen !=
+					sizeof(ng_hci_node_role_switch_ep)) {
+				error = EMSGSIZE;
+				break;
+			}
+
+			unit->role_switch = 
+				*((ng_hci_node_role_switch_ep *)(msg->data));
+			break;
+
 		default:
 			error = EINVAL;
 			break;
diff --git a/sys/netgraph/bluetooth/hci/ng_hci_misc.c b/sys/netgraph/bluetooth/hci/ng_hci_misc.c
index ba92f89..fb7c661 100644
--- a/sys/netgraph/bluetooth/hci/ng_hci_misc.c
+++ b/sys/netgraph/bluetooth/hci/ng_hci_misc.c
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_hci_misc.c,v 1.18 2002/10/30 00:18:19 max Exp $
+ * $Id: ng_hci_misc.c,v 1.4 2003/04/26 22:35:21 max Exp $
  * $FreeBSD$
  */
 
@@ -53,7 +53,6 @@
 
 static void	ng_hci_command_queue_timeout		(void *);
 static void	ng_hci_con_queue_timeout		(void *);
-static void	ng_hci_con_queue_watchdog_timeout	(void *);
 
 /*
  * Give packet to RAW hook
@@ -270,7 +269,6 @@ ng_hci_new_con(ng_hci_unit_p unit, int link_type)
 		NG_BT_ITEMQ_INIT(&con->conq, num_pkts);
 
 		callout_handle_init(&con->con_timo);
-		callout_handle_init(&con->watchdog_timo);
 
 		LIST_INSERT_HEAD(&unit->con_list, con, next);
 	}
@@ -291,9 +289,6 @@ ng_hci_free_con(ng_hci_unit_con_p con)
 	if (con->flags & NG_HCI_CON_TIMEOUT_PENDING)
 		ng_hci_con_untimeout(con);
 
-	if (con->flags & NG_HCI_CON_WATCHDOG_TIMEOUT_PENDING)
-		ng_hci_con_watchdog_untimeout(con);
-
 	/*
 	 * If we have pending packets then assume that Host Controller has 
 	 * flushed these packets and we can free them too
@@ -442,59 +437,6 @@ ng_hci_con_queue_timeout(void *context)
 	NG_NODE_UNREF(node);
 } /* ng_hci_con_queue_timeout */
 
-/*
- * Set HCI connection watchdog timeout
- */
-
-void
-ng_hci_con_watchdog_timeout(ng_hci_unit_con_p con)
-{
-	if (!(con->flags & NG_HCI_CON_WATCHDOG_TIMEOUT_PENDING)) {
-		NG_NODE_REF(con->unit->node);
-		con->flags |= NG_HCI_CON_WATCHDOG_TIMEOUT_PENDING;
-		con->watchdog_timo = timeout(ng_hci_con_queue_watchdog_timeout,
-					con, bluetooth_hci_watchdog_timeout());
-	} else
-		KASSERT(0,
-("%s: %s - Duplicated connection watchdog timeout!\n",
-			__func__, NG_NODE_NAME(con->unit->node)));
-} /* ng_hci_con_watchdog_timeout */
-
-/*
- * Unset HCI connection watchdog timeout
- */
-
-void
-ng_hci_con_watchdog_untimeout(ng_hci_unit_con_p con)
-{
-	if (con->flags & NG_HCI_CON_WATCHDOG_TIMEOUT_PENDING) {
-		con->flags &= ~NG_HCI_CON_WATCHDOG_TIMEOUT_PENDING;
-		untimeout(ng_hci_con_queue_watchdog_timeout, con, 
-			con->watchdog_timo);
-		NG_NODE_UNREF(con->unit->node);
-	} else
-		KASSERT(0,
-("%s: %s - No connection watchdog timeout!\n",
-			 __func__, NG_NODE_NAME(con->unit->node)));
-} /* ng_hci_con_watchdog_untimeout */
-
-/*
- * OK timeout has happend, so queue timeout processing function
- */
-
-static void
-ng_hci_con_queue_watchdog_timeout(void *context)
-{
-	ng_hci_unit_con_p	con = (ng_hci_unit_con_p) context;
-	node_p			node = con->unit->node;
-
-	if (NG_NODE_IS_VALID(node))
-		ng_send_fn(node, NULL, &ng_hci_process_con_watchdog_timeout, 
-			con, 0);
-
-	NG_NODE_UNREF(node);
-} /* ng_hci_con_queue_watchdog_timeout */
-
 #if 0
 /*
  * Convert numeric error code/reason to a string
diff --git a/sys/netgraph/bluetooth/hci/ng_hci_misc.h b/sys/netgraph/bluetooth/hci/ng_hci_misc.h
index 862d7db..b9f29cd 100644
--- a/sys/netgraph/bluetooth/hci/ng_hci_misc.h
+++ b/sys/netgraph/bluetooth/hci/ng_hci_misc.h
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_hci_misc.h,v 1.8 2002/09/04 21:36:52 max Exp $
+ * $Id: ng_hci_misc.h,v 1.2 2003/04/26 22:35:21 max Exp $
  * $FreeBSD$
  */
 
@@ -51,8 +51,6 @@ void              ng_hci_command_timeout        (ng_hci_unit_p);
 void              ng_hci_command_untimeout      (ng_hci_unit_p);
 void              ng_hci_con_timeout            (ng_hci_unit_con_p);
 void              ng_hci_con_untimeout          (ng_hci_unit_con_p);
-void              ng_hci_con_watchdog_timeout   (ng_hci_unit_con_p);
-void              ng_hci_con_watchdog_untimeout (ng_hci_unit_con_p);
 
 #endif /* ndef _NETGRAPH_HCI_MISC_H_ */
 
diff --git a/sys/netgraph/bluetooth/hci/ng_hci_prse.h b/sys/netgraph/bluetooth/hci/ng_hci_prse.h
index 4c4bd6e..dd8ff06 100644
--- a/sys/netgraph/bluetooth/hci/ng_hci_prse.h
+++ b/sys/netgraph/bluetooth/hci/ng_hci_prse.h
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_hci_prse.h,v 1.9 2002/11/12 22:35:40 max Exp $
+ * $Id: ng_hci_prse.h,v 1.2 2003/03/18 00:09:36 max Exp $
  * $FreeBSD$
  */
 
@@ -196,6 +196,20 @@ static const struct ng_cmdlist	ng_hci_cmdlist[] = {
 		&ng_parse_uint16_type,
 		NULL
 	},
+	{
+		NGM_HCI_COOKIE,
+		NGM_HCI_NODE_GET_ROLE_SWITCH,
+		"get_role_sw",
+		NULL,
+		&ng_parse_uint16_type
+	},
+	{
+		NGM_HCI_COOKIE,
+		NGM_HCI_NODE_SET_ROLE_SWITCH,
+		"set_role_sw",
+		&ng_parse_uint16_type,
+		NULL
+	},
 	{ 0, }
 };
 
diff --git a/sys/netgraph/bluetooth/hci/ng_hci_ulpi.c b/sys/netgraph/bluetooth/hci/ng_hci_ulpi.c
index 97a0ea8..927cfe1 100644
--- a/sys/netgraph/bluetooth/hci/ng_hci_ulpi.c
+++ b/sys/netgraph/bluetooth/hci/ng_hci_ulpi.c
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_hci_ulpi.c,v 1.14 2002/11/12 22:35:40 max Exp $
+ * $Id: ng_hci_ulpi.c,v 1.6 2003/04/26 22:35:21 max Exp $
  * $FreeBSD$
  */
 
@@ -132,30 +132,22 @@ ng_hci_lp_acl_con_req(ng_hci_unit_p unit, item_p item, hook_p hook)
 	 * 2) We do have connection descriptor. We need to check connection
 	 *    state:
 	 * 
-	 * 2.1) NG_HCI_CON_CLOSED mean we are in the process of closing
-	 *      connection to the remote unit. We will reject connection
-	 *      request until connection is closed.
-	 *
-	 * 2.2) NG_HCI_CON_W4_LP_CON_RSP means that we are in the middle of
+	 * 2.1) NG_HCI_CON_W4_LP_CON_RSP means that we are in the middle of
 	 *      accepting connection from the remote unit. This is a race
 	 *      condition. We will ignore this message.
 	 *
-	 * 2.3) NG_HCI_CON_W4_CONN_COMPLETE means that upper layer already
+	 * 2.2) NG_HCI_CON_W4_CONN_COMPLETE means that upper layer already
 	 *      requested connection or we just accepted it. In any case
 	 *      all we need to do here is set appropriate notification bit
 	 *      and wait.
 	 *	
-	 * 2.4) NG_HCI_CON_OPEN means connection is open. Just reply back
+	 * 2.3) NG_HCI_CON_OPEN means connection is open. Just reply back
 	 *      and let upper layer know that we have connection already.
 	 */
 
 	con = ng_hci_con_by_bdaddr(unit, &ep->bdaddr, NG_HCI_LINK_ACL);
 	if (con != NULL) {
 		switch (con->state) {
-		case NG_HCI_CON_CLOSED:
-			error = EBUSY;
-			break;
-
 		case NG_HCI_CON_W4_LP_CON_RSP: /* XXX */
 			error = EALREADY;
 			break;
@@ -255,12 +247,14 @@ ng_hci_lp_acl_con_req(ng_hci_unit_p unit, item_p item, hook_p hook)
 		req->cp.pkt_type |= (NG_HCI_PKT_DM5|NG_HCI_PKT_DH5);
 
 	req->cp.pkt_type &= unit->packet_mask;
-	if (req->cp.pkt_type == 0)
+	if ((req->cp.pkt_type & (NG_HCI_PKT_DM1|NG_HCI_PKT_DH1|
+				 NG_HCI_PKT_DM3|NG_HCI_PKT_DH3|
+				 NG_HCI_PKT_DM5|NG_HCI_PKT_DH5)) == 0)
 		req->cp.pkt_type = (NG_HCI_PKT_DM1|NG_HCI_PKT_DH1);
 
 	req->cp.pkt_type = htole16(req->cp.pkt_type);
 
-	if (unit->features[0] & NG_HCI_LMP_SWITCH)
+	if ((unit->features[0] & NG_HCI_LMP_SWITCH) && unit->role_switch)
 		req->cp.accept_role_switch = 1;
 	else
 		req->cp.accept_role_switch = 0;
@@ -376,8 +370,6 @@ ng_hci_lp_sco_con_req(ng_hci_unit_p unit, item_p item, hook_p hook)
 	 *
 	 * 2.2) NG_HCI_CON_W4_CONN_COMPLETE means upper layer already requested
 	 *      connection or we just accepted it.
-	 *
-	 * XXX FIXME what to do with connection(s) in CLOSED state?
 	 */
 
 	LIST_FOREACH(sco_con, &unit->con_list, next)
@@ -450,7 +442,9 @@ ng_hci_lp_sco_con_req(ng_hci_unit_p unit, item_p item, hook_p hook)
 		req->cp.pkt_type |= NG_HCI_PKT_HV3;
 
 	req->cp.pkt_type &= unit->packet_mask;
-	if (req->cp.pkt_type == 0)
+	if ((req->cp.pkt_type & (NG_HCI_PKT_HV1|
+				 NG_HCI_PKT_HV2|
+				 NG_HCI_PKT_HV3)) == 0)
 		req->cp.pkt_type = NG_HCI_PKT_HV1;
 
 	req->cp.pkt_type = htole16(req->cp.pkt_type);
@@ -479,12 +473,6 @@ out:
 
 /*
  * Process LP_DisconnectReq event from the upper layer protocol
- * 
- * XXX XXX XXX
- *
- * NOTE: This is NOT defined by Bluetooth specification (why?) But i think
- * this might be useful (at least for testing), so please do not depend on
- * this interface.
  */
 
 int
@@ -562,13 +550,6 @@ ng_hci_lp_discon_req(ng_hci_unit_p unit, item_p item, hook_p hook)
 	req->cp.reason = ep->reason;
 
 	/* 
-	 * Adjust connection state
-	 */
-
-	con->state = NG_HCI_CON_CLOSED;
-	ng_hci_con_timeout(con);
-
-	/* 
 	 * Queue and send HCI command 
 	 */
 
@@ -813,16 +794,15 @@ ng_hci_lp_con_rsp(ng_hci_unit_p unit, item_p item, hook_p hook)
 				sizeof(req->cp.acc.bdaddr));
 
 			/*
-			 * XXX should be configurable?
-			 *
 			 * We are accepting connection, so if we support role 
-			 * switch then set role to NG_HCI_ROLE_MASTER and let 
-			 * LM peform role switch. Otherwise it is probably 
-			 * makes sense to remain slave. In this case LM WILL 
-			 * NOT perform role switch.
+			 * switch and role switch was enabled then set role to 
+			 * NG_HCI_ROLE_MASTER and let LM peform role switch.
+			 * Otherwise we remain slave. In this case LM WILL NOT
+			 * perform role switch.
 			 */
 
-			if (unit->features[0] & NG_HCI_LMP_SWITCH)
+			if ((unit->features[0] & NG_HCI_LMP_SWITCH) &&
+			    unit->role_switch)
 				req->cp.acc.role = NG_HCI_ROLE_MASTER;
 			else
 				req->cp.acc.role = NG_HCI_ROLE_SLAVE;
@@ -1183,24 +1163,16 @@ ng_hci_process_con_timeout(node_p node, hook_p hook, void *arg1, int arg2)
 	 * We expect to receive connection timeout in one of the following
 	 * states:
 	 *
-	 * 1) NG_HCI_CON_CLOSED means that upper layer has requested disconnect
-	 *    via LP_DISCON_REQ and we have not received Disconnect_Complete
-	 *    event. In this case we will send LP_DISCON_IND to upper layer.
-	 *
-	 * 2) NG_HCI_CON_W4_LP_CON_RSP means that upper layer has not responded
+	 * 1) NG_HCI_CON_W4_LP_CON_RSP means that upper layer has not responded
 	 *    to our LP_CON_IND. Do nothing and destroy connection. Remote peer
 	 *    most likely already gave up on us.
 	 * 
-	 * 3) NG_HCI_CON_W4_CONN_COMPLETE means upper layer requested connection
+	 * 2) NG_HCI_CON_W4_CONN_COMPLETE means upper layer requested connection
 	 *    (or we in the process of accepting it) and baseband has timedout
 	 *    on us. Inform upper layers and send LP_CON_CFM.
 	 */
 
 	switch (con->state) {
-	case NG_HCI_CON_CLOSED:
-		ng_hci_lp_discon_ind(con, 0x16);
-		break;
-
 	case NG_HCI_CON_W4_LP_CON_RSP:
 		break;
 
@@ -1218,63 +1190,3 @@ ng_hci_process_con_timeout(node_p node, hook_p hook, void *arg1, int arg2)
 	ng_hci_free_con(con);
 } /* ng_hci_process_con_timeout */
 
-/*
- * Process connection watchdog timeout
- */
-
-void
-ng_hci_process_con_watchdog_timeout(node_p node, hook_p hook, 
-		void *arg1, int arg2)
-{
-	ng_hci_unit_con_p		 con = (ng_hci_unit_con_p) arg1;
-	struct discon_req {
-		ng_hci_cmd_pkt_t	 hdr;
-		ng_hci_discon_cp	 cp;
-	} __attribute__ ((packed))	*req = NULL;
-	struct mbuf			*m = NULL;
-
-	KASSERT((con->state == NG_HCI_CON_OPEN), 
-("%s: %s - invalid connection state=%d, handle=%d\n",
-		__func__, NG_NODE_NAME(node), con->state, con->con_handle));
-
-	KASSERT((con->flags & NG_HCI_CON_WATCHDOG_TIMEOUT_PENDING),
-("%s: %s - No connection watchdog timeout!\n",
-		__func__, NG_NODE_NAME(node)));
-
-	con->flags &= ~NG_HCI_CON_WATCHDOG_TIMEOUT_PENDING;
-
-	/* 
-	 * Create HCI command
-	 */
-
-	MGETHDR(m, M_DONTWAIT, MT_DATA);
-	if (m == NULL)
-		return; /* XXX this is bad */
-
-	m->m_pkthdr.len = m->m_len = sizeof(*req);
-	req = mtod(m, struct discon_req *);
-	req->hdr.type = NG_HCI_CMD_PKT;
-	req->hdr.length = sizeof(req->cp);
-	req->hdr.opcode = htole16(NG_HCI_OPCODE(NG_HCI_OGF_LINK_CONTROL,
-							NG_HCI_OCF_DISCON));
-
-	req->cp.con_handle = htole16(con->con_handle);
-	req->cp.reason = 0x13; /* User ended connection */
-
-	/* 
-	 * Queue and send HCI command 
-	 */
-
-	NG_BT_MBUFQ_ENQUEUE(&con->unit->cmdq, m);
-	if (!(con->unit->state & NG_HCI_UNIT_COMMAND_PENDING))
-		ng_hci_send_command(con->unit);
-
-	/* 
-	 * Send LP_DISCON_IND to the upper layers
-	 * Connection terminated by local host
-	 */
-
-	ng_hci_lp_discon_ind(con, 0x16);
-	ng_hci_free_con(con);
-} /* ng_hci_process_con_watchdog_timeout */
-
diff --git a/sys/netgraph/bluetooth/hci/ng_hci_ulpi.h b/sys/netgraph/bluetooth/hci/ng_hci_ulpi.h
index 9f4e0ef..28c3ade 100644
--- a/sys/netgraph/bluetooth/hci/ng_hci_ulpi.h
+++ b/sys/netgraph/bluetooth/hci/ng_hci_ulpi.h
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_hci_ulpi.h,v 1.4 2002/09/04 21:36:52 max Exp $
+ * $Id: ng_hci_ulpi.h,v 1.2 2003/04/26 22:35:21 max Exp $
  * $FreeBSD$
  */
 
@@ -47,7 +47,6 @@ int  ng_hci_lp_qos_cfm                   (ng_hci_unit_con_p, int);
 int  ng_hci_lp_qos_ind                   (ng_hci_unit_con_p);
 
 void ng_hci_process_con_timeout          (node_p, hook_p, void *, int);
-void ng_hci_process_con_watchdog_timeout (node_p, hook_p, void *, int);
 
 #endif /* ndef _NETGRAPH_HCI_ULPI_H_ */
 
diff --git a/sys/netgraph/bluetooth/hci/ng_hci_var.h b/sys/netgraph/bluetooth/hci/ng_hci_var.h
index 43940a9..c0adccc 100644
--- a/sys/netgraph/bluetooth/hci/ng_hci_var.h
+++ b/sys/netgraph/bluetooth/hci/ng_hci_var.h
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_hci_var.h,v 1.14 2002/11/12 22:35:40 max Exp $
+ * $Id: ng_hci_var.h,v 1.3 2003/04/26 22:35:21 max Exp $
  * $FreeBSD$
  */
 
@@ -133,6 +133,7 @@ typedef struct ng_hci_unit {
 
 	ng_hci_node_link_policy_mask_ep	link_policy_mask; /* link policy mask */
 	ng_hci_node_packet_mask_ep	packet_mask;	/* packet mask */
+	ng_hci_node_role_switch_ep	role_switch;	/* role switch */
 
 	ng_hci_node_stat_ep		stat;           /* statistic */
 #define NG_HCI_STAT_CMD_SENT(s)		(s).cmd_sent ++
@@ -171,9 +172,8 @@ typedef struct ng_hci_unit_con {
 	u_int16_t			state;           /* con. state */
 	u_int16_t			flags;           /* con. flags */
 #define NG_HCI_CON_TIMEOUT_PENDING		(1 << 0)
-#define NG_HCI_CON_WATCHDOG_TIMEOUT_PENDING	(1 << 1)
-#define NG_HCI_CON_NOTIFY_ACL			(1 << 2)
-#define NG_HCI_CON_NOTIFY_SCO			(1 << 3)
+#define NG_HCI_CON_NOTIFY_ACL			(1 << 1)
+#define NG_HCI_CON_NOTIFY_SCO			(1 << 2)
 
 	bdaddr_t			bdaddr;          /* remote address */
 	u_int16_t			con_handle;      /* con. handle */
@@ -184,7 +184,6 @@ typedef struct ng_hci_unit_con {
 	u_int8_t			role;            /* MASTER/SLAVE */
 
 	struct callout_handle		con_timo;        /* con. timeout */
-	struct callout_handle		watchdog_timo;   /* watch dog */
 
 	int				pending;         /* # of data pkts */
 	ng_bt_itemq_t			conq;            /* con. queue */
diff --git a/sys/netgraph/bluetooth/include/ng_bluetooth.h b/sys/netgraph/bluetooth/include/ng_bluetooth.h
index 238da42..e4b4846 100644
--- a/sys/netgraph/bluetooth/include/ng_bluetooth.h
+++ b/sys/netgraph/bluetooth/include/ng_bluetooth.h
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_bluetooth.h,v 1.1.1.1 2002/09/04 21:47:41 max Exp $
+ * $Id: ng_bluetooth.h,v 1.4 2003/04/26 22:32:34 max Exp $
  * $FreeBSD$
  */
 
@@ -47,6 +47,7 @@
 SYSCTL_DECL(_net_bluetooth);
 SYSCTL_DECL(_net_bluetooth_hci);
 SYSCTL_DECL(_net_bluetooth_l2cap);
+SYSCTL_DECL(_net_bluetooth_rfcomm);
 #endif /* SYSCTL_DECL */
 
 /*
@@ -84,7 +85,7 @@ typedef struct ng_bt_mbufq *	ng_bt_mbufq_p;
 
 #define NG_BT_MBUFQ_LEN(q)	(q)->len
 
-#define NG_BT_MBUFQ_FULL(q)	(q)->len >= (q)->maxlen
+#define NG_BT_MBUFQ_FULL(q)	((q)->len >= (q)->maxlen)
 
 #define NG_BT_MBUFQ_DROP(q)	(q)->drops ++
 
@@ -223,7 +224,6 @@ typedef struct ng_bt_itemq *	ng_bt_itemq_p;
 
 u_int32_t	bluetooth_hci_command_timeout	(void);
 u_int32_t	bluetooth_hci_connect_timeout	(void);
-u_int32_t	bluetooth_hci_watchdog_timeout	(void);
 u_int32_t	bluetooth_hci_max_neighbor_age	(void);
 u_int32_t	bluetooth_l2cap_rtx_timeout	(void);
 u_int32_t	bluetooth_l2cap_ertx_timeout	(void);
diff --git a/sys/netgraph/bluetooth/include/ng_bt3c.h b/sys/netgraph/bluetooth/include/ng_bt3c.h
index 6030b81..88084a2 100644
--- a/sys/netgraph/bluetooth/include/ng_bt3c.h
+++ b/sys/netgraph/bluetooth/include/ng_bt3c.h
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_bt3c.h,v 1.2 2002/11/12 00:51:45 max Exp $
+ * $Id: ng_bt3c.h,v 1.1 2002/11/24 19:47:05 max Exp $
  * $FreeBSD$
  *
  * XXX XXX XX XXX XXX XXX XXX XXX XXX XXX XXX XXX XXX XXX XXX XXX XXX XXX XXX 
diff --git a/sys/netgraph/bluetooth/include/ng_btsocket.h b/sys/netgraph/bluetooth/include/ng_btsocket.h
index 5ace3fa..2a133a3 100644
--- a/sys/netgraph/bluetooth/include/ng_btsocket.h
+++ b/sys/netgraph/bluetooth/include/ng_btsocket.h
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_btsocket.h,v 1.7 2002/11/12 22:31:39 max Exp $
+ * $Id: ng_btsocket.h,v 1.8 2003/04/26 22:32:10 max Exp $
  * $FreeBSD$
  */
 
@@ -45,6 +45,7 @@
 
 #define BLUETOOTH_PROTO_HCI	134	/* HCI protocol number */
 #define BLUETOOTH_PROTO_L2CAP	135	/* L2CAP protocol number */
+#define BLUETOOTH_PROTO_RFCOMM	136	/* RFCOMM protocol number */
 
 /*
  * XXX FIXME: probably does not belong here
@@ -82,7 +83,6 @@ struct ng_btsocket_hci_raw_filter {
 
 /* Get state */
 struct ng_btsocket_hci_raw_node_state {
-	char			hci_node[16];
 	ng_hci_node_state_ep	state;
 };
 #define SIOC_HCI_RAW_NODE_GET_STATE \
@@ -90,16 +90,11 @@ struct ng_btsocket_hci_raw_node_state {
 		struct ng_btsocket_hci_raw_node_state)
 
 /* Initialize */
-struct ng_btsocket_hci_raw_node_init {
-	char	hci_node[16];
-};
 #define SIOC_HCI_RAW_NODE_INIT \
-	_IOWR('b', NGM_HCI_NODE_INIT, \
-		struct ng_btsocket_hci_raw_node_init)
+	_IO('b', NGM_HCI_NODE_INIT)
 
 /* Get/Set debug level */
 struct ng_btsocket_hci_raw_node_debug {
-	char			hci_node[16];
 	ng_hci_node_debug_ep	debug;
 };
 #define SIOC_HCI_RAW_NODE_GET_DEBUG \
@@ -111,7 +106,6 @@ struct ng_btsocket_hci_raw_node_debug {
 
 /* Get buffer info */
 struct ng_btsocket_hci_raw_node_buffer {
-	char			hci_node[16];
 	ng_hci_node_buffer_ep	buffer;
 };
 #define SIOC_HCI_RAW_NODE_GET_BUFFER \
@@ -120,7 +114,6 @@ struct ng_btsocket_hci_raw_node_buffer {
 
 /* Get BD_ADDR */
 struct ng_btsocket_hci_raw_node_bdaddr {
-	char		hci_node[16];
 	bdaddr_t	bdaddr;
 };
 #define SIOC_HCI_RAW_NODE_GET_BDADDR \
@@ -129,7 +122,6 @@ struct ng_btsocket_hci_raw_node_bdaddr {
 
 /* Get features */
 struct ng_btsocket_hci_raw_node_features {
-	char		hci_node [16];
 	u_int8_t	features[NG_HCI_FEATURES_SIZE];
 };
 #define SIOC_HCI_RAW_NODE_GET_FEATURES \
@@ -138,7 +130,6 @@ struct ng_btsocket_hci_raw_node_features {
 
 /* Get stat */
 struct ng_btsocket_hci_raw_node_stat {
-	char			hci_node[16];
 	ng_hci_node_stat_ep	stat;
 };
 #define SIOC_HCI_RAW_NODE_GET_STAT \
@@ -146,24 +137,15 @@ struct ng_btsocket_hci_raw_node_stat {
 		struct ng_btsocket_hci_raw_node_stat)
 
 /* Reset stat */
-struct ng_btsocket_hci_raw_node_reset_stat {
-	char	hci_node[16];
-};
 #define SIOC_HCI_RAW_NODE_RESET_STAT \
-	_IOWR('b', NGM_HCI_NODE_RESET_STAT, \
-		struct ng_btsocket_hci_raw_node_reset_stat)
+	_IO('b', NGM_HCI_NODE_RESET_STAT)
 
 /* Flush neighbor cache */
-struct ng_btsocket_hci_raw_node_flush_neighbor_cache {
-	char	hci_node[16];
-};
 #define SIOC_HCI_RAW_NODE_FLUSH_NEIGHBOR_CACHE \
-	_IOWR('b', NGM_HCI_NODE_FLUSH_NEIGHBOR_CACHE, \
-		struct ng_btsocket_hci_raw_node_flush_neighbor_cache)
+	_IO('b', NGM_HCI_NODE_FLUSH_NEIGHBOR_CACHE)
 
 /* Get neighbor cache */
 struct ng_btsocket_hci_raw_node_neighbor_cache {
-	char					 hci_node[16];
 	u_int32_t				 num_entries;
 	ng_hci_node_neighbor_cache_entry_ep	*entries;
 };
@@ -173,7 +155,6 @@ struct ng_btsocket_hci_raw_node_neighbor_cache {
 
 /* Get connection list */
 struct ng_btsocket_hci_raw_con_list {
-	char			 hci_node[16];
 	u_int32_t		 num_connections;
 	ng_hci_node_con_ep	*connections;
 };
@@ -183,7 +164,6 @@ struct ng_btsocket_hci_raw_con_list {
 
 /* Get/Set link policy settings mask */
 struct ng_btsocket_hci_raw_node_link_policy_mask {
-	char				hci_node[16];
 	ng_hci_node_link_policy_mask_ep	policy_mask;
 };
 #define SIOC_HCI_RAW_NODE_GET_LINK_POLICY_MASK \
@@ -195,7 +175,6 @@ struct ng_btsocket_hci_raw_node_link_policy_mask {
 
 /* Get/Set packet mask */
 struct ng_btsocket_hci_raw_node_packet_mask {
-	char				hci_node[16];
 	ng_hci_node_packet_mask_ep	packet_mask;
 };
 #define SIOC_HCI_RAW_NODE_GET_PACKET_MASK \
@@ -205,6 +184,17 @@ struct ng_btsocket_hci_raw_node_packet_mask {
 	_IOWR('b', NGM_HCI_NODE_SET_PACKET_MASK, \
 		struct ng_btsocket_hci_raw_node_packet_mask)
 
+/* Get/Set role switch */
+struct ng_btsocket_hci_raw_node_role_switch {
+	ng_hci_node_role_switch_ep	role_switch;
+};
+#define SIOC_HCI_RAW_NODE_GET_ROLE_SWITCH \
+	_IOWR('b', NGM_HCI_NODE_GET_ROLE_SWITCH, \
+		struct ng_btsocket_hci_raw_node_role_switch)
+#define SIOC_HCI_RAW_NODE_SET_ROLE_SWITCH \
+	_IOWR('b', NGM_HCI_NODE_SET_ROLE_SWITCH, \
+		struct ng_btsocket_hci_raw_node_role_switch)
+
 /*
  * XXX FIXME: probably does not belong here
  * Bluetooth version of struct sockaddr for L2CAP sockets (RAW and SEQPACKET)
@@ -232,9 +222,6 @@ struct sockaddr_l2cap {
 
 /* Ping */
 struct ng_btsocket_l2cap_raw_ping {
-	bdaddr_t		 bdaddr[2];
-#define echo_src		 bdaddr[0]
-#define echo_dst		 bdaddr[1]
 	u_int32_t		 result;
 	u_int32_t		 echo_size;
 	u_int8_t		*echo_data;
@@ -245,9 +232,6 @@ struct ng_btsocket_l2cap_raw_ping {
 
 /* Get info */
 struct ng_btsocket_l2cap_raw_get_info {
-	bdaddr_t		 bdaddr[2];
-#define info_src		 bdaddr[0]
-#define info_dst		 bdaddr[1]
 	u_int32_t		 result;
 	u_int32_t		 info_type;
 	u_int32_t		 info_size;
@@ -259,7 +243,6 @@ struct ng_btsocket_l2cap_raw_get_info {
 
 /* Get flags */
 struct ng_btsocket_l2cap_raw_node_flags {
-	bdaddr_t		src;
 	ng_l2cap_node_flags_ep	flags;
 };
 #define SIOC_L2CAP_NODE_GET_FLAGS \
@@ -268,7 +251,6 @@ struct ng_btsocket_l2cap_raw_node_flags {
 
 /* Get/Set debug level */
 struct ng_btsocket_l2cap_raw_node_debug {
-	bdaddr_t		src;
 	ng_l2cap_node_debug_ep	debug;
 };
 #define SIOC_L2CAP_NODE_GET_DEBUG \
@@ -280,7 +262,6 @@ struct ng_btsocket_l2cap_raw_node_debug {
 
 /* Get connection list */
 struct ng_btsocket_l2cap_raw_con_list {
-	bdaddr_t		 src;
 	u_int32_t		 num_connections;
 	ng_l2cap_node_con_ep	*connections;
 };
@@ -290,7 +271,6 @@ struct ng_btsocket_l2cap_raw_con_list {
 
 /* Get channel list */
 struct ng_btsocket_l2cap_raw_chan_list {
-	bdaddr_t		 src;
 	u_int32_t		 num_channels;
 	ng_l2cap_node_chan_ep	*channels;
 };
@@ -298,6 +278,46 @@ struct ng_btsocket_l2cap_raw_chan_list {
 	_IOWR('b', NGM_L2CAP_NODE_GET_CHAN_LIST, \
 		struct ng_btsocket_l2cap_raw_chan_list)
 
+/* Get/Set auto disconnect timeout */
+struct ng_btsocket_l2cap_raw_auto_discon_timo
+{
+	ng_l2cap_node_auto_discon_ep	timeout;
+};
+#define SIOC_L2CAP_NODE_GET_AUTO_DISCON_TIMO \
+	_IOWR('b', NGM_L2CAP_NODE_GET_AUTO_DISCON_TIMO, \
+		struct ng_btsocket_l2cap_raw_auto_discon_timo)
+#define SIOC_L2CAP_NODE_SET_AUTO_DISCON_TIMO \
+	_IOWR('b', NGM_L2CAP_NODE_SET_AUTO_DISCON_TIMO, \
+		struct ng_btsocket_l2cap_raw_auto_discon_timo)
+
+/*
+ * XXX FIXME: probably does not belong here
+ * Bluetooth version of struct sockaddr for RFCOMM sockets (STREAM)
+ */
+
+struct sockaddr_rfcomm {
+	u_char		rfcomm_len;	/* total length */
+	u_char		rfcomm_family;	/* address family */
+	bdaddr_t	rfcomm_bdaddr;	/* address */
+	u_int8_t	rfcomm_channel;	/* channel */
+};
+
+/* Flow control information */
+struct ng_btsocket_rfcomm_fc_info {
+	u_int8_t	lmodem;		/* modem signals (local) */
+	u_int8_t	rmodem;		/* modem signals (remote) */
+	u_int8_t	tx_cred;	/* TX credits */
+	u_int8_t	rx_cred;	/* RX credits */
+	u_int8_t	cfc;		/* credit flow control */
+	u_int8_t	reserved;
+};
+
+/* STREAM RFCOMM socket options */
+#define SOL_RFCOMM		0x0816	/* socket options level */
+
+#define SO_RFCOMM_MTU		1	/* get channel MTU */
+#define SO_RFCOMM_FC_INFO	2	/* get flow control information */
+
 /* 
  * Netgraph node type name and cookie 
  */
diff --git a/sys/netgraph/bluetooth/include/ng_btsocket_hci_raw.h b/sys/netgraph/bluetooth/include/ng_btsocket_hci_raw.h
index 399e8ac..c3af9e9 100644
--- a/sys/netgraph/bluetooth/include/ng_btsocket_hci_raw.h
+++ b/sys/netgraph/bluetooth/include/ng_btsocket_hci_raw.h
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_btsocket_hci_raw.h,v 1.2 2002/09/16 19:46:02 max Exp $
+ * $Id: ng_btsocket_hci_raw.h,v 1.3 2003/03/25 23:53:32 max Exp $
  * $FreeBSD$
  */
 
@@ -41,13 +41,15 @@
 
 struct ng_btsocket_hci_raw_pcb {
 	struct socket				*so;     /* socket */
-	int					 flags;  /* flags */
+	u_int32_t				 flags;  /* flags */
 #define NG_BTSOCKET_HCI_RAW_DIRECTION	(1 << 0)
+#define NG_BTSOCKET_HCI_RAW_PRIVILEGED	(1 << 1)
 	struct sockaddr_hci			 addr;   /* local address */
 	struct ng_btsocket_hci_raw_filter	 filter; /* filter */
 	u_int32_t				 token;  /* message token */
 	struct ng_mesg				*msg;    /* message */
 	LIST_ENTRY(ng_btsocket_hci_raw_pcb)	 next;   /* link to next */
+	struct mtx				 pcb_mtx; /* pcb mutex */
 };
 typedef struct ng_btsocket_hci_raw_pcb		ng_btsocket_hci_raw_pcb_t;
 typedef struct ng_btsocket_hci_raw_pcb *	ng_btsocket_hci_raw_pcb_p;
diff --git a/sys/netgraph/bluetooth/include/ng_btsocket_l2cap.h b/sys/netgraph/bluetooth/include/ng_btsocket_l2cap.h
index cfae52e..093b22a 100644
--- a/sys/netgraph/bluetooth/include/ng_btsocket_l2cap.h
+++ b/sys/netgraph/bluetooth/include/ng_btsocket_l2cap.h
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_btsocket_l2cap.h,v 1.3 2002/09/22 18:23:31 max Exp $
+ * $Id: ng_btsocket_l2cap.h,v 1.4 2003/03/25 23:53:33 max Exp $
  * $FreeBSD$
  */
 
@@ -42,7 +42,7 @@ struct ng_message;
 struct ng_btsocket_l2cap_rtentry {
 	bdaddr_t				 src;  /* source BD_ADDR */
 	struct ng_hook				*hook; /* downstream hook */
-	LIST_ENTRY(ng_btsocket_l2cap_rtentry)	 next; /* linkt to next */
+	LIST_ENTRY(ng_btsocket_l2cap_rtentry)	 next; /* link to next */
 };
 typedef struct ng_btsocket_l2cap_rtentry	ng_btsocket_l2cap_rtentry_t;
 typedef struct ng_btsocket_l2cap_rtentry *	ng_btsocket_l2cap_rtentry_p;
@@ -63,12 +63,18 @@ typedef struct ng_btsocket_l2cap_rtentry *	ng_btsocket_l2cap_rtentry_p;
 struct ng_btsocket_l2cap_raw_pcb {
 	struct socket				*so;	/* socket */
 
+	u_int32_t				 flags; /* flags */
+#define NG_BTSOCKET_L2CAP_RAW_PRIVILEGED	(1 << 0)
+
 	bdaddr_t				 src;	/* source address */
+	bdaddr_t				 dst;	/* dest address */
 	ng_btsocket_l2cap_rtentry_p		 rt;    /* routing info */
 
 	u_int32_t				 token;	/* message token */
 	struct ng_mesg				*msg;   /* message */
 
+	struct mtx				 pcb_mtx; /* pcb mutex */
+
 	LIST_ENTRY(ng_btsocket_l2cap_raw_pcb)	 next;  /* link to next PCB */
 };
 typedef struct ng_btsocket_l2cap_raw_pcb	ng_btsocket_l2cap_raw_pcb_t;
diff --git a/sys/netgraph/bluetooth/include/ng_btsocket_rfcomm.h b/sys/netgraph/bluetooth/include/ng_btsocket_rfcomm.h
new file mode 100644
index 0000000..21bfb3e
--- /dev/null
+++ b/sys/netgraph/bluetooth/include/ng_btsocket_rfcomm.h
@@ -0,0 +1,337 @@
+/*
+ * ng_btsocket_rfcomm.h
+ *
+ * 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.h,v 1.10 2003/03/29 22:27:42 max Exp $
+ * $FreeBSD$
+ */
+
+#ifndef _NETGRAPH_BTSOCKET_RFCOMM_H_
+#define _NETGRAPH_BTSOCKET_RFCOMM_H_ 1
+
+/*****************************************************************************
+ *****************************************************************************
+ **                              RFCOMM                                     **
+ *****************************************************************************
+ *****************************************************************************/
+
+/* XXX FIXME this does not belong here */
+
+#define RFCOMM_DEFAULT_MTU		127
+#define RFCOMM_MAX_MTU			1024
+
+#define RFCOMM_DEFAULT_CREDITS		7
+#define RFCOMM_MAX_CREDITS		40
+
+/* RFCOMM frame types */
+#define RFCOMM_FRAME_SABM		0x2f
+#define RFCOMM_FRAME_DISC		0x43
+#define RFCOMM_FRAME_UA			0x63
+#define RFCOMM_FRAME_DM			0x0f
+#define RFCOMM_FRAME_UIH		0xef
+
+/* RFCOMM MCC commands */
+#define RFCOMM_MCC_TEST			0x08 /* Test */
+#define RFCOMM_MCC_FCON			0x28 /* Flow Control on */
+#define RFCOMM_MCC_FCOFF		0x18 /* Flow Control off */
+#define RFCOMM_MCC_MSC			0x38 /* Modem Status Command */
+#define RFCOMM_MCC_RPN			0x24 /* Remote Port Negotiation */
+#define RFCOMM_MCC_RLS			0x14 /* Remote Line Status */
+#define RFCOMM_MCC_PN			0x20 /* Port Negotiation */
+#define RFCOMM_MCC_NSC			0x04 /* Non Supported Command */
+
+/* RFCOMM modem signals */
+#define RFCOMM_MODEM_FC			0x02 /* Flow Control asserted */
+#define RFCOMM_MODEM_RTC		0x04 /* Ready To Communicate */
+#define RFCOMM_MODEM_RTR		0x08 /* Ready To Receive */
+#define RFCOMM_MODEM_IC			0x40 /* Incomming Call */
+#define RFCOMM_MODEM_DV			0x80 /* Data Valid */
+
+/* RPN parameters - baud rate */
+#define RFCOMM_RPN_BR_2400		0x0
+#define RFCOMM_RPN_BR_4800		0x1
+#define RFCOMM_RPN_BR_7200		0x2
+#define RFCOMM_RPN_BR_9600		0x3
+#define RFCOMM_RPN_BR_19200		0x4
+#define RFCOMM_RPN_BR_38400		0x5
+#define RFCOMM_RPN_BR_57600		0x6
+#define RFCOMM_RPN_BR_115200		0x7
+#define RFCOMM_RPN_BR_230400		0x8
+
+/* RPN parameters - data bits */
+#define RFCOMM_RPN_DATA_5		0x0
+#define RFCOMM_RPN_DATA_6		0x1
+#define RFCOMM_RPN_DATA_7		0x2
+#define RFCOMM_RPN_DATA_8		0x3
+
+/* RPN parameters - stop bit */
+#define RFCOMM_RPN_STOP_1		0
+#define RFCOMM_RPN_STOP_15		1
+
+/* RPN parameters - parity */
+#define RFCOMM_RPN_PARITY_NONE		0x0
+#define RFCOMM_RPN_PARITY_ODD		0x4
+#define RFCOMM_RPN_PARITY_EVEN		0x5
+#define RFCOMM_RPN_PARITY_MARK		0x6
+#define RFCOMM_RPN_PARITY_SPACE		0x7
+
+/* RPN parameters - flow control */
+#define RFCOMM_RPN_FLOW_NONE		0x00
+#define RFCOMM_RPN_XON_CHAR		0x11
+#define RFCOMM_RPN_XOFF_CHAR		0x13
+
+/* RPN parameters - mask */
+#define RFCOMM_RPN_PM_BITRATE		0x0001
+#define RFCOMM_RPN_PM_DATA		0x0002
+#define RFCOMM_RPN_PM_STOP		0x0004
+#define RFCOMM_RPN_PM_PARITY		0x0008
+#define RFCOMM_RPN_PM_PARITY_TYPE	0x0010
+#define RFCOMM_RPN_PM_XON		0x0020
+#define RFCOMM_RPN_PM_XOFF		0x0040
+#define RFCOMM_RPN_PM_FLOW		0x3F00
+#define RFCOMM_RPN_PM_ALL		0x3F7F
+
+/* RFCOMM frame header */
+struct rfcomm_frame_hdr 
+{
+	u_int8_t	address;
+	u_int8_t	control;
+	u_int8_t	length;	/* Actual size could be 2 bytes */
+} __attribute__ ((packed));
+
+/* RFCOMM command frame header */
+struct rfcomm_cmd_hdr
+{
+	u_int8_t	address;
+	u_int8_t	control;
+	u_int8_t	length;
+	u_int8_t	fcs;
+} __attribute__ ((packed));
+                
+/* RFCOMM MCC command header */
+struct rfcomm_mcc_hdr
+{
+	u_int8_t	type;
+	u_int8_t	length; /* XXX FIXME Can actual size be 2 bytes?? */
+} __attribute__ ((packed));
+
+/* RFCOMM MSC command */
+struct rfcomm_mcc_msc
+{
+	u_int8_t	address;
+	u_int8_t	modem;
+} __attribute__ ((packed));
+
+/* RFCOMM RPN command */
+struct rfcomm_mcc_rpn
+{
+	u_int8_t	dlci;
+	u_int8_t	bit_rate;
+	u_int8_t	line_settings;
+	u_int8_t	flow_control;
+	u_int8_t	xon_char;
+	u_int8_t	xoff_char;
+	u_int16_t	param_mask;
+} __attribute__ ((packed));
+
+/* RFCOMM RLS command */
+struct rfcomm_mcc_rls
+{
+	u_int8_t	address;
+	u_int8_t	status;
+} __attribute__ ((packed));
+
+/* RFCOMM PN command */
+struct rfcomm_mcc_pn
+{
+	u_int8_t	dlci;
+	u_int8_t	flow_control;
+	u_int8_t	priority;
+	u_int8_t	ack_timer;
+	u_int16_t	mtu;
+	u_int8_t	max_retrans;
+	u_int8_t	credits;
+} __attribute__ ((packed));
+
+/* RFCOMM frame parsing macros */
+#define RFCOMM_DLCI(b)			(((b) & 0xfc) >> 2)
+#define RFCOMM_CHANNEL(b)		(((b) & 0xf8) >> 3)
+#define RFCOMM_DIRECTION(b)		(((b) & 0x04) >> 2)
+#define RFCOMM_TYPE(b)			(((b) & 0xef))
+  
+#define RFCOMM_EA(b)			(((b) & 0x01))
+#define RFCOMM_CR(b)			(((b) & 0x02) >> 1)
+#define RFCOMM_PF(b)			(((b) & 0x10) >> 4)
+
+#define RFCOMM_SRVCHANNEL(dlci)		((dlci) >> 1)
+  
+#define RFCOMM_MKADDRESS(cr, dlci) \
+	((((dlci) & 0x3f) << 2) | ((cr) << 1) | 0x01)
+
+#define RFCOMM_MKCONTROL(type, pf)	((((type) & 0xef) | ((pf) << 4)))
+#define RFCOMM_MKDLCI(dir, channel)	((((channel) & 0x1f) << 1) | (dir))
+
+#define RFCOMM_MKLEN8(len)		(((len) << 1) | 1)
+#define RFCOMM_MKLEN16(len)		((len) << 1)
+
+/* RFCOMM MCC macros */
+#define RFCOMM_MCC_TYPE(b)		(((b) & 0xfc) >> 2)
+#define RFCOMM_MCC_LENGTH(b)		(((b) & 0xfe) >> 1)
+#define RFCOMM_MKMCC_TYPE(cr, type)	((((type) << 2) | ((cr) << 1) | 0x01))
+   
+/* RPN macros */
+#define RFCOMM_RPN_DATA_BITS(line)	((line) & 0x3)
+#define RFCOMM_RPN_STOP_BITS(line)	(((line) >> 2) & 0x1)
+#define RFCOMM_RPN_PARITY(line)		(((line) >> 3) & 0x3)
+#define RFCOMM_MKRPN_LINE_SETTINGS(data, stop, parity) \
+	(((data) & 0x3) | (((stop) & 0x1) << 2) | (((parity) & 0x3) << 3))
+
+/*****************************************************************************
+ *****************************************************************************
+ **                      SOCK_STREAM RFCOMM sockets                         **
+ *****************************************************************************
+ *****************************************************************************/
+
+#define NG_BTSOCKET_RFCOMM_SENDSPACE \
+	(RFCOMM_MAX_CREDITS * RFCOMM_DEFAULT_MTU * 10)
+#define NG_BTSOCKET_RFCOMM_RECVSPACE \
+	(RFCOMM_MAX_CREDITS * RFCOMM_DEFAULT_MTU * 10)
+
+/*
+ * Bluetooth RFCOMM session. One L2CAP connection == one RFCOMM session
+ */
+
+struct ng_btsocket_rfcomm_pcb;
+struct ng_btsocket_rfcomm_session;
+
+struct ng_btsocket_rfcomm_session {
+	struct socket				*l2so;	 /* L2CAP socket */
+
+	u_int16_t				 state;  /* session state */
+#define NG_BTSOCKET_RFCOMM_SESSION_CLOSED	 0
+#define NG_BTSOCKET_RFCOMM_SESSION_LISTENING	 1
+#define NG_BTSOCKET_RFCOMM_SESSION_CONNECTING	 2 
+#define NG_BTSOCKET_RFCOMM_SESSION_CONNECTED	 3
+#define NG_BTSOCKET_RFCOMM_SESSION_OPEN		 4
+#define NG_BTSOCKET_RFCOMM_SESSION_DISCONNECTING 5
+
+	u_int16_t				 flags;  /* session flags */
+#define NG_BTSOCKET_RFCOMM_SESSION_INITIATOR	(1 << 0) /* initiator */
+#define NG_BTSOCKET_RFCOMM_SESSION_LFC		(1 << 1) /* local flow */
+#define NG_BTSOCKET_RFCOMM_SESSION_RFC		(1 << 2) /* remote flow */
+
+#define INITIATOR(s) \
+	(((s)->flags & NG_BTSOCKET_RFCOMM_SESSION_INITIATOR)? 1 : 0)
+
+	u_int16_t				 mtu;    /* default MTU */
+	struct ng_bt_mbufq			 outq;   /* outgoing queue */
+
+	struct mtx				 session_mtx; /* session lock */
+	LIST_HEAD(, ng_btsocket_rfcomm_pcb)	 dlcs;	 /* active DLC */
+
+	LIST_ENTRY(ng_btsocket_rfcomm_session)	 next;	 /* link to next */
+};
+typedef struct ng_btsocket_rfcomm_session	ng_btsocket_rfcomm_session_t;
+typedef struct ng_btsocket_rfcomm_session *	ng_btsocket_rfcomm_session_p;
+
+/*
+ * Bluetooth RFCOMM socket PCB (DLC)
+ */
+
+struct ng_btsocket_rfcomm_pcb {
+	struct socket				*so;	  /* RFCOMM socket */
+	struct ng_btsocket_rfcomm_session	*session; /* RFCOMM session */
+
+	u_int16_t				 flags;   /* DLC flags */
+#define NG_BTSOCKET_RFCOMM_DLC_TIMO		(1 << 0)  /* timeout pending */
+#define NG_BTSOCKET_RFCOMM_DLC_CFC		(1 << 1)  /* credit flow ctrl */
+#define NG_BTSOCKET_RFCOMM_DLC_TIMEDOUT		(1 << 2)  /* timeout happend */
+#define NG_BTSOCKET_RFCOMM_DLC_DETACHED		(1 << 3)  /* DLC detached */
+#define NG_BTSOCKET_RFCOMM_DLC_SENDING		(1 << 4)  /* send pending */
+
+	u_int16_t				 state;   /* DLC state */
+#define NG_BTSOCKET_RFCOMM_DLC_CLOSED		0
+#define NG_BTSOCKET_RFCOMM_DLC_W4_CONNECT	1
+#define NG_BTSOCKET_RFCOMM_DLC_CONFIGURING	2
+#define NG_BTSOCKET_RFCOMM_DLC_CONNECTING	3
+#define NG_BTSOCKET_RFCOMM_DLC_CONNECTED	4
+#define NG_BTSOCKET_RFCOMM_DLC_DISCONNECTING	5
+
+	bdaddr_t				 src;     /* source address */
+	bdaddr_t				 dst;     /* dest. address */
+
+	u_int8_t				 channel; /* RFCOMM channel */
+	u_int8_t				 dlci;    /* RFCOMM DLCI */
+
+	u_int8_t				 lmodem;  /* local mdm signls */
+	u_int8_t				 rmodem;  /* remote -/- */
+
+	u_int16_t				 mtu;	  /* MTU */
+	int16_t					 rx_cred; /* RX credits */
+	int16_t					 tx_cred; /* TX credits */
+
+	struct mtx				 pcb_mtx; /* PCB lock */
+	struct callout_handle			 timo;    /* timeout */
+
+	LIST_ENTRY(ng_btsocket_rfcomm_pcb)	 session_next;/* link to next */
+	LIST_ENTRY(ng_btsocket_rfcomm_pcb)	 next;	  /* link to next */
+};
+typedef struct ng_btsocket_rfcomm_pcb	ng_btsocket_rfcomm_pcb_t;
+typedef struct ng_btsocket_rfcomm_pcb *	ng_btsocket_rfcomm_pcb_p;
+
+#define	so2rfcomm_pcb(so) \
+	((struct ng_btsocket_rfcomm_pcb *)((so)->so_pcb))
+
+/*
+ * Bluetooth RFCOMM socket methods
+ */
+
+#ifdef _KERNEL
+
+void ng_btsocket_rfcomm_init       (void);
+int  ng_btsocket_rfcomm_abort      (struct socket *);
+int  ng_btsocket_rfcomm_accept     (struct socket *, struct sockaddr **);
+int  ng_btsocket_rfcomm_attach     (struct socket *, int, struct thread *);
+int  ng_btsocket_rfcomm_bind       (struct socket *, struct sockaddr *,
+                                    struct thread *);
+int  ng_btsocket_rfcomm_connect    (struct socket *, struct sockaddr *,
+                                    struct thread *);
+int  ng_btsocket_rfcomm_control    (struct socket *, u_long, caddr_t,
+                                    struct ifnet *, struct thread *);
+int  ng_btsocket_rfcomm_ctloutput  (struct socket *, struct sockopt *);
+int  ng_btsocket_rfcomm_detach     (struct socket *);
+int  ng_btsocket_rfcomm_disconnect (struct socket *);
+int  ng_btsocket_rfcomm_listen     (struct socket *, struct thread *);
+int  ng_btsocket_rfcomm_peeraddr   (struct socket *, struct sockaddr **);
+int  ng_btsocket_rfcomm_send       (struct socket *, int, struct mbuf *,
+                                    struct sockaddr *, struct mbuf *,
+                                    struct thread *);
+int  ng_btsocket_rfcomm_sockaddr   (struct socket *, struct sockaddr **);
+
+#endif /* _KERNEL */
+
+#endif /* _NETGRAPH_BTSOCKET_RFCOMM_H_ */
+
diff --git a/sys/netgraph/bluetooth/include/ng_h4.h b/sys/netgraph/bluetooth/include/ng_h4.h
index 8f6f849..f590277 100644
--- a/sys/netgraph/bluetooth/include/ng_h4.h
+++ b/sys/netgraph/bluetooth/include/ng_h4.h
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_h4.h,v 1.5 2002/06/27 09:50:18 max Exp $
+ * $Id: ng_h4.h,v 1.1 2002/11/24 19:47:05 max Exp $
  * $FreeBSD$
  * 
  * Based on:
diff --git a/sys/netgraph/bluetooth/include/ng_hci.h b/sys/netgraph/bluetooth/include/ng_hci.h
index c4b1aba..3bea725 100644
--- a/sys/netgraph/bluetooth/include/ng_hci.h
+++ b/sys/netgraph/bluetooth/include/ng_hci.h
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_hci.h,v 1.13 2002/11/12 22:35:39 max Exp $
+ * $Id: ng_hci.h,v 1.2 2003/03/18 00:09:37 max Exp $
  * $FreeBSD$
  */
 
@@ -598,6 +598,10 @@ typedef u_int16_t	ng_hci_node_link_policy_mask_ep;
 #define NGM_HCI_NODE_SET_PACKET_MASK		117 /* User -> HCI */
 typedef u_int16_t	ng_hci_node_packet_mask_ep;
 
+#define NGM_HCI_NODE_GET_ROLE_SWITCH		118 /* HCI -> User */
+#define NGM_HCI_NODE_SET_ROLE_SWITCH		119 /* User -> HCI */
+typedef u_int16_t	ng_hci_node_role_switch_ep;
+
 /**************************************************************************
  **************************************************************************
  **             Link control commands and return parameters
diff --git a/sys/netgraph/bluetooth/include/ng_l2cap.h b/sys/netgraph/bluetooth/include/ng_l2cap.h
index e277c21..5759389 100644
--- a/sys/netgraph/bluetooth/include/ng_l2cap.h
+++ b/sys/netgraph/bluetooth/include/ng_l2cap.h
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_l2cap.h,v 1.13 2002/09/08 23:35:51 max Exp $
+ * $Id: ng_l2cap.h,v 1.2 2003/04/27 00:52:26 max Exp $
  * $FreeBSD$
  */
 
@@ -610,14 +610,17 @@ typedef u_int16_t	ng_l2cap_node_debug_ep;
 #define NGM_L2CAP_NODE_HOOK_INFO	0x409	/* L2CAP -> Upper */
 /* bdaddr_t bdaddr; -- local (source BDADDR) */
 
-#define NGM_L2CAP_NODE_GET_CON_LIST	0x40a	/* User -> L2CAP */
+#define NGM_L2CAP_NODE_GET_CON_LIST	0x40a	/* L2CAP -> User */
 typedef struct {
 	u_int32_t	num_connections; /* number of connections */
 } ng_l2cap_node_con_list_ep;
 
 /* Connection flags */
-#define NG_L2CAP_CON_TX	(1 << 0)    /* sending data */
-#define NG_L2CAP_CON_RX	(1 << 1)    /* receiving data */
+#define NG_L2CAP_CON_TX			(1 << 0) /* sending data */
+#define NG_L2CAP_CON_RX			(1 << 1) /* receiving data */
+#define NG_L2CAP_CON_OUTGOING		(1 << 2) /* outgoing connection */
+#define NG_L2CAP_CON_LP_TIMO		(1 << 3) /* LP timeout */
+#define NG_L2CAP_CON_AUTO_DISCON_TIMO	(1 << 4) /* auto discon. timeout */
 
 typedef struct {
 	u_int8_t	state;      /* connection state */
@@ -630,7 +633,7 @@ typedef struct {
 #define NG_L2CAP_MAX_CON_NUM \
 	((0xffff - sizeof(ng_l2cap_node_con_list_ep))/sizeof(ng_l2cap_node_con_ep))
 
-#define NGM_L2CAP_NODE_GET_CHAN_LIST	0x40b	/* User -> L2CAP */
+#define NGM_L2CAP_NODE_GET_CHAN_LIST	0x40b	/* L2CAP -> User */
 typedef struct {
 	u_int32_t	num_channels;	/* number of channels */
 } ng_l2cap_node_chan_list_ep;
@@ -651,5 +654,9 @@ typedef struct {
 #define NG_L2CAP_MAX_CHAN_NUM \
 	((0xffff - sizeof(ng_l2cap_node_chan_list_ep))/sizeof(ng_l2cap_node_chan_ep))
 
+#define NGM_L2CAP_NODE_GET_AUTO_DISCON_TIMO 0x40c /* L2CAP -> User */
+#define NGM_L2CAP_NODE_SET_AUTO_DISCON_TIMO 0x40d /* User -> L2CAP */
+typedef u_int16_t	ng_l2cap_node_auto_discon_ep;
+
 #endif /* ndef _NETGRAPH_L2CAP_H_ */
 
diff --git a/sys/netgraph/bluetooth/include/ng_ubt.h b/sys/netgraph/bluetooth/include/ng_ubt.h
index 1c55ad4..bdfc47a 100644
--- a/sys/netgraph/bluetooth/include/ng_ubt.h
+++ b/sys/netgraph/bluetooth/include/ng_ubt.h
@@ -25,29 +25,13 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_ubt.h,v 1.1 2002/11/09 19:09:02 max Exp $
+ * $Id: ng_ubt.h,v 1.6 2003/04/13 21:34:42 max Exp $
  * $FreeBSD$
  */
 
 #ifndef _NG_UBT_H_
 #define _NG_UBT_H_
 
-/* XXX FIXME Does not belong here. Move to usbdevs.h later */
-#define USB_VENDOR_MSI			0x0db0 /* MSI www.msi.com.tw */
-#define USB_VENDOR_EPOX			0x0a12 /* EPoX www.epox.com */
-
-#define USB_PRODUCT_3COM_3CREB96	0x00a0 /* 3Com Bluetooth USB dongle */
-#define USB_PRODUCT_MITSUMI_BT_DONGLE	0x641f /* Mitsumi Bluetooth USB dongle*/
-#define USB_PRODUCT_TDK_BT_DONGLE	0x0309 /* TDK Bluetooth USB dongle */
-#define USB_PRODUCT_MSI_BT_DONGLE	0x1967 /* MSI Bluetooth USB dongle */
-#define USB_PRODUCT_DBW_120M_BT_DONGLE	0x2033 /* D-Link DBW-120M */
-#define USB_PRODUCT_BT_DG02_DONGLE	0x0001 /* EPoX BT-DG02 USB dongle */
-
-/* XXX FIXME Does not belong here. Move to usb.h later */
-#define UICLASS_WIRELESS_CONTROLLER	0xe0	/* Wireless Controller */
-#define UISUBCLASS_RF_CONTROLLER	0x01	/* RF Controller */
-#define UIPROTO_BLUETOOTH		0x01	/* Bluetooth programming */
-
 /**************************************************************************
  **************************************************************************
  **     Netgraph node hook name, type name and type cookie and commands 
@@ -79,10 +63,9 @@ typedef u_int16_t		ng_ubt_node_debug_ep;
 #define NGM_UBT_NODE_GET_QLEN	4		/* get queue length */ 
 typedef struct {
 	int32_t		queue;			/* queue index */
-#define	NGM_UBT_NODE_QUEUE_IN	1		/* incoming queue */
-#define	NGM_UBT_NODE_QUEUE_CMD	2		/* commands */
-#define	NGM_UBT_NODE_QUEUE_ACL	3		/* ACL data */
-#define	NGM_UBT_NODE_QUEUE_SCO	4		/* SCO data */
+#define	NGM_UBT_NODE_QUEUE_CMD	1		/* commands */
+#define	NGM_UBT_NODE_QUEUE_ACL	2		/* ACL data */
+#define	NGM_UBT_NODE_QUEUE_SCO	3		/* SCO data */
 
 	int32_t		qlen;			/* queue length */
 } ng_ubt_node_qlen_ep;
@@ -99,5 +82,8 @@ typedef struct {
 
 #define NGM_UBT_NODE_RESET_STAT	6		/* reset statistic */
 
+#define NGM_UBT_NODE_DEV_NODES	7		/* on/off device interface */
+typedef u_int16_t	ng_ubt_node_dev_nodes_ep;
+
 #endif /* ndef _NG_UBT_H_ */
 
diff --git a/sys/netgraph/bluetooth/l2cap/TODO b/sys/netgraph/bluetooth/l2cap/TODO
index 98d0440..7c7bb27 100644
--- a/sys/netgraph/bluetooth/l2cap/TODO
+++ b/sys/netgraph/bluetooth/l2cap/TODO
@@ -1,4 +1,4 @@
-$Id: TODO,v 1.8 2002/09/06 21:03:58 max Exp $
+$Id: TODO,v 1.1 2002/11/24 19:47:06 max Exp $
 $FreeBSD$
 
 FIXME/TODO list
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_cmds.c b/sys/netgraph/bluetooth/l2cap/ng_l2cap_cmds.c
index 0d28f1a..844c5a6 100644
--- a/sys/netgraph/bluetooth/l2cap/ng_l2cap_cmds.c
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_cmds.c
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_l2cap_cmds.c,v 1.14 2002/09/04 21:38:38 max Exp $
+ * $Id: ng_l2cap_cmds.c,v 1.1 2002/11/24 19:47:06 max Exp $
  * $FreeBSD$
  */
 
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_cmds.h b/sys/netgraph/bluetooth/l2cap/ng_l2cap_cmds.h
index d107ad6..ed8e84f 100644
--- a/sys/netgraph/bluetooth/l2cap/ng_l2cap_cmds.h
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_cmds.h
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_l2cap_cmds.h,v 1.9 2002/04/16 00:43:56 max Exp $
+ * $Id: ng_l2cap_cmds.h,v 1.4 2003/04/01 18:15:26 max Exp $
  * $FreeBSD$
  */
 
@@ -152,9 +152,11 @@ do {									\
 	c->param.dcid = htole16((_dcid));				\
 	c->param.flags = htole16((_flags));				\
 	if ((_data) != NULL) {						\
+		int	l = (_data)->m_pkthdr.len;			\
+									\
 		m_cat((_m), (_data));					\
-		c->hdr.length += (_data)->m_pkthdr.len;			\
-		(_m)->m_pkthdr.len += (_data)->m_pkthdr.len;		\
+		c->hdr.length += l;					\
+		(_m)->m_pkthdr.len += l;				\
 	}								\
 									\
 	c->hdr.length = htole16(c->hdr.length);				\
@@ -185,9 +187,11 @@ do {									\
 	c->param.flags = htole16((_flags));				\
 	c->param.result = htole16((_result));				\
 	if ((_data) != NULL) {						\
+		int	l = (_data)->m_pkthdr.len;			\
+									\
 		m_cat((_m), (_data));					\
-		c->hdr.length += (_data)->m_pkthdr.len;			\
-		(_m)->m_pkthdr.len += (_data)->m_pkthdr.len;		\
+		c->hdr.length += l;					\
+		(_m)->m_pkthdr.len += l;				\
 	}								\
 									\
 	c->hdr.length = htole16(c->hdr.length);				\
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.c b/sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.c
index 3779b91..4a9c6fd 100644
--- a/sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.c
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.c
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_l2cap_evnt.c,v 1.18 2002/09/04 21:38:38 max Exp $
+ * $Id: ng_l2cap_evnt.c,v 1.4 2003/04/01 18:15:26 max Exp $
  * $FreeBSD$
  */
 
@@ -844,7 +844,8 @@ ng_l2cap_process_discon_req(ng_l2cap_con_p con, u_int8_t ident)
 	}
 
 	/* XXX Verify channel state and reject if invalid -- is that true? */
-	if (ch->state != NG_L2CAP_OPEN && ch->state != NG_L2CAP_CONFIG) {
+	if (ch->state != NG_L2CAP_OPEN && ch->state != NG_L2CAP_CONFIG &&
+	    ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) {
 		NG_L2CAP_ERR(
 "%s: %s - unexpected L2CAP_DisconnectReq. " \
 "Invalid channel state, cid=%d, state=%d\n",
@@ -869,8 +870,10 @@ ng_l2cap_process_discon_req(ng_l2cap_con_p con, u_int8_t ident)
 	 * with L2CAP_DisconnectRsp.
 	 */
 
-	ng_l2cap_l2ca_discon_ind(ch); /* do not care about result */
-	ng_l2cap_free_chan(ch);
+	if (ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) {
+		ng_l2cap_l2ca_discon_ind(ch); /* do not care about result */
+		ng_l2cap_free_chan(ch);
+	}
 
 	/* Send L2CAP_DisconnectRsp */
 	cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_DISCON_RSP, 0);
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.h b/sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.h
index 26fcac5..c7cf8ac 100644
--- a/sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.h
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.h
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_l2cap_evnt.h,v 1.2 2002/04/16 00:43:56 max Exp $
+ * $Id: ng_l2cap_evnt.h,v 1.1 2002/11/24 19:47:06 max Exp $
  * $FreeBSD$
  */
 
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.c b/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.c
index f505158..bc110ad 100644
--- a/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.c
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.c
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_l2cap_llpi.c,v 1.16 2002/09/04 21:38:38 max Exp $
+ * $Id: ng_l2cap_llpi.c,v 1.4 2003/04/28 21:44:59 max Exp $
  * $FreeBSD$
  */
 
@@ -108,12 +108,15 @@ ng_l2cap_lp_con_req(ng_l2cap_p l2cap, bdaddr_p bdaddr)
 	bcopy(bdaddr, &ep->bdaddr, sizeof(ep->bdaddr));
 	ep->link_type = NG_HCI_LINK_ACL;
 
+	con->flags |= NG_L2CAP_CON_OUTGOING;
 	con->state = NG_L2CAP_W4_LP_CON_CFM;
 	ng_l2cap_lp_timeout(con);
 
 	NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->hci, NULL);
-	if (error != 0)
-		ng_l2cap_free_con(con); /* will remove timeout */
+	if (error != 0) {
+		ng_l2cap_lp_untimeout(con);
+		ng_l2cap_free_con(con);
+	}
 	
 	return (error);
 } /* ng_l2cap_lp_con_req */
@@ -174,11 +177,8 @@ ng_l2cap_lp_con_cfm(ng_l2cap_p l2cap, struct ng_mesg *msg)
 		con->state = NG_L2CAP_CON_OPEN;
 		con->con_handle = ep->con_handle;
 		ng_l2cap_lp_deliver(con);
-	} else {
-		/* Negative confirmation - remove connection descriptor */
-		con->state = NG_L2CAP_CON_CLOSED;
+	} else /* Negative confirmation - remove connection descriptor */
 		ng_l2cap_con_fail(con, ep->status);
-	}
 out:
 	return (error);
 } /* ng_l2cap_lp_con_cfm */
@@ -258,8 +258,10 @@ ng_l2cap_lp_con_ind(ng_l2cap_p l2cap, struct ng_mesg *msg)
 	ng_l2cap_lp_timeout(con);
 
 	NG_SEND_MSG_HOOK(error, l2cap->node, rsp, l2cap->hci, NULL);
-	if (error != 0)
-		ng_l2cap_free_con(con); /* will remove timeout */
+	if (error != 0) {
+		ng_l2cap_lp_untimeout(con);
+		ng_l2cap_free_con(con);
+	}
 out:
 	return (error);
 } /* ng_hci_lp_con_ind */
@@ -310,7 +312,9 @@ ng_l2cap_lp_discon_ind(ng_l2cap_p l2cap, struct ng_mesg *msg)
 	}
 
 	/* Notify upper layer and remove connection */
-	con->state = NG_L2CAP_CON_CLOSED;
+	if (con->flags & NG_L2CAP_CON_AUTO_DISCON_TIMO)
+		ng_l2cap_discon_untimeout(con);
+
 	ng_l2cap_con_fail(con, ep->reason);
 out:
 	return (error);
@@ -800,7 +804,45 @@ ng_l2cap_process_lp_timeout(node_p node, hook_p hook, void *arg1, int arg2)
 	 * connection, channels and pending commands.
 	 */
 
-	con->state = NG_L2CAP_CON_CLOSED;
+	con->flags &= ~NG_L2CAP_CON_LP_TIMO;
 	ng_l2cap_con_fail(con, NG_L2CAP_TIMEOUT);
 } /* ng_l2cap_process_lp_timeout */
 
+/*
+ * Process auto disconnect timeout and send LP_DisconReq event to the 
+ * lower layer protocol
+ */
+
+void
+ng_l2cap_process_discon_timeout(node_p node, hook_p hook, void *arg1, int arg2)
+{
+	ng_l2cap_con_p		 con = (ng_l2cap_con_p) arg1;
+	ng_l2cap_p		 l2cap = con->l2cap;
+	struct ng_mesg		*msg = NULL;
+	ng_hci_lp_discon_req_ep	*ep = NULL;
+	int			 error;
+
+	con->flags &= ~NG_L2CAP_CON_AUTO_DISCON_TIMO;
+
+	/* Check if lower layer protocol is still connected */
+	if (l2cap->hci == NULL || NG_HOOK_NOT_VALID(l2cap->hci)) {
+		NG_L2CAP_ERR(
+"%s: %s - hook \"%s\" is not connected or valid\n",
+			__func__, NG_NODE_NAME(l2cap->node), NG_L2CAP_HOOK_HCI);
+
+		return;
+	}
+
+	/* Create and send LP_DisconReq event */
+	NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_DISCON_REQ,
+		sizeof(*ep), M_NOWAIT);
+	if (msg == NULL)
+		return;
+
+	ep = (ng_hci_lp_discon_req_ep *) (msg->data);
+	ep->con_handle = con->con_handle;
+	ep->reason = 0x13; /* User Ended Connection */
+
+	NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->hci, NULL);
+} /* ng_l2cap_process_discon_timeout */
+
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.h b/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.h
index 161761e..9efc835 100644
--- a/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.h
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.h
@@ -25,24 +25,25 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_l2cap_llpi.h,v 1.6 2002/04/16 00:43:57 max Exp $
+ * $Id: ng_l2cap_llpi.h,v 1.2 2003/04/28 21:44:59 max Exp $
  * $FreeBSD$
  */
 
 #ifndef _NETGRAPH_L2CAP_LLPI_H_
 #define _NETGRAPH_L2CAP_LLPI_H_
 
-int  ng_l2cap_lp_con_req         (ng_l2cap_p, bdaddr_p);
-int  ng_l2cap_lp_con_cfm         (ng_l2cap_p, struct ng_mesg *);
-int  ng_l2cap_lp_con_ind         (ng_l2cap_p, struct ng_mesg *);
-int  ng_l2cap_lp_discon_ind      (ng_l2cap_p, struct ng_mesg *);
-int  ng_l2cap_lp_qos_req         (ng_l2cap_p, u_int16_t, ng_l2cap_flow_p);
-int  ng_l2cap_lp_qos_cfm         (ng_l2cap_p, struct ng_mesg *);
-int  ng_l2cap_lp_qos_ind         (ng_l2cap_p, struct ng_mesg *);
-int  ng_l2cap_lp_send            (ng_l2cap_con_p, u_int16_t,struct mbuf *);
-int  ng_l2cap_lp_receive         (ng_l2cap_p, struct mbuf *);
-void ng_l2cap_lp_deliver         (ng_l2cap_con_p);
-void ng_l2cap_process_lp_timeout (node_p, hook_p, void *, int);
+int  ng_l2cap_lp_con_req             (ng_l2cap_p, bdaddr_p);
+int  ng_l2cap_lp_con_cfm             (ng_l2cap_p, struct ng_mesg *);
+int  ng_l2cap_lp_con_ind             (ng_l2cap_p, struct ng_mesg *);
+int  ng_l2cap_lp_discon_ind          (ng_l2cap_p, struct ng_mesg *);
+int  ng_l2cap_lp_qos_req             (ng_l2cap_p, u_int16_t, ng_l2cap_flow_p);
+int  ng_l2cap_lp_qos_cfm             (ng_l2cap_p, struct ng_mesg *);
+int  ng_l2cap_lp_qos_ind             (ng_l2cap_p, struct ng_mesg *);
+int  ng_l2cap_lp_send                (ng_l2cap_con_p, u_int16_t,struct mbuf *);
+int  ng_l2cap_lp_receive             (ng_l2cap_p, struct mbuf *);
+void ng_l2cap_lp_deliver             (ng_l2cap_con_p);
+void ng_l2cap_process_lp_timeout     (node_p, hook_p, void *, int);
+void ng_l2cap_process_discon_timeout (node_p, hook_p, void *, int);
 
 #endif /* ndef _NETGRAPH_L2CAP_LLPI_H_ */
 
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_main.c b/sys/netgraph/bluetooth/l2cap/ng_l2cap_main.c
index 1dfc4c0..3117c29 100644
--- a/sys/netgraph/bluetooth/l2cap/ng_l2cap_main.c
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_main.c
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_l2cap_main.c,v 1.24 2002/09/04 21:38:38 max Exp $
+ * $Id: ng_l2cap_main.c,v 1.2 2003/04/28 21:44:59 max Exp $
  * $FreeBSD$
  */
 
@@ -120,6 +120,7 @@ ng_l2cap_constructor(node_p node)
 
 	l2cap->node = node;
 	l2cap->debug = NG_L2CAP_WARN_LEVEL;
+	l2cap->discon_timo = 5; /* sec */
 
 	LIST_INIT(&l2cap->con_list);
 	LIST_INIT(&l2cap->chan_list);
@@ -574,6 +575,7 @@ ng_l2cap_default_rcvmsg(node_p node, item_p item, hook_p lasthook)
 			LIST_FOREACH(con, &l2cap->con_list, next) {
 				e2->state = con->state;
 
+				e2->flags = con->flags;
 				if (con->tx_pkt != NULL)
 					e2->flags |= NG_L2CAP_CON_TX;
 				if (con->rx_pkt != NULL)
@@ -637,6 +639,26 @@ ng_l2cap_default_rcvmsg(node_p node, item_p item, hook_p lasthook)
 			}
 			} break;
 
+		case NGM_L2CAP_NODE_GET_AUTO_DISCON_TIMO:
+			NG_MKRESPONSE(rsp, msg,
+				sizeof(ng_l2cap_node_auto_discon_ep), M_NOWAIT);
+			if (rsp == NULL)
+				error = ENOMEM;
+			else
+				*((ng_l2cap_node_auto_discon_ep *)(rsp->data)) =
+					l2cap->discon_timo;
+			break;
+
+		case NGM_L2CAP_NODE_SET_AUTO_DISCON_TIMO:
+			if (msg->header.arglen !=
+					sizeof(ng_l2cap_node_auto_discon_ep))
+				error = EMSGSIZE;
+			else
+				l2cap->discon_timo =
+					*((ng_l2cap_node_auto_discon_ep *)
+							(msg->data));
+			break;
+
 		default:
 			error = EINVAL;
 			break;
@@ -714,12 +736,13 @@ ng_l2cap_cleanup(ng_l2cap_p l2cap)
 	while (!LIST_EMPTY(&l2cap->con_list)) {
 		con = LIST_FIRST(&l2cap->con_list);
 
-		if (con->state == NG_L2CAP_W4_LP_CON_CFM)
+		if (con->flags & NG_L2CAP_CON_LP_TIMO)
 			ng_l2cap_lp_untimeout(con);
+		else if (con->flags & NG_L2CAP_CON_AUTO_DISCON_TIMO)
+			ng_l2cap_discon_untimeout(con);
 
-		con->state = NG_L2CAP_CON_CLOSED;
+		/* Connection terminated by local host */
 		ng_l2cap_con_fail(con, 0x16);
-				/* Connection terminated by local host */
 	}
 } /* ng_l2cap_cleanup */
 
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_misc.c b/sys/netgraph/bluetooth/l2cap/ng_l2cap_misc.c
index 9fcbc6e..43a6d13 100644
--- a/sys/netgraph/bluetooth/l2cap/ng_l2cap_misc.c
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_misc.c
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_l2cap_misc.c,v 1.16 2002/09/04 21:38:38 max Exp $
+ * $Id: ng_l2cap_misc.c,v 1.4 2003/04/28 21:44:59 max Exp $
  * $FreeBSD$
  */
 
@@ -48,6 +48,7 @@
 #include "ng_l2cap_misc.h"
 
 static u_int16_t	ng_l2cap_get_cid		(ng_l2cap_p);
+static void		ng_l2cap_queue_discon_timeout	(void *);
 static void		ng_l2cap_queue_lp_timeout	(void *);
 static void		ng_l2cap_queue_command_timeout	(void *);
 
@@ -93,9 +94,8 @@ ng_l2cap_send_hook_info(node_p node, hook_p hook, void *arg1, int arg2)
 } /* ng_l2cap_send_hook_info */
 
 /*
- * Create new connection descriptor for the "remote" unit. Will create new
- * connection descriptor and signal channel. Will link both connection and
- * channel to the l2cap node.
+ * Create new connection descriptor for the "remote" unit. 
+ * Will link connection descriptor to the l2cap node.
  */
 
 ng_l2cap_con_p
@@ -125,6 +125,104 @@ ng_l2cap_new_con(ng_l2cap_p l2cap, bdaddr_p bdaddr)
 } /* ng_l2cap_new_con */
 
 /*
+ * Add reference to the connection descriptor
+ */
+
+void
+ng_l2cap_con_ref(ng_l2cap_con_p con)
+{
+	con->refcnt ++;
+
+	if (con->flags & NG_L2CAP_CON_AUTO_DISCON_TIMO) {
+		if ((con->state != NG_L2CAP_CON_OPEN) ||
+		    (con->flags & NG_L2CAP_CON_OUTGOING) == 0)
+			panic("%s: %s - bad auto disconnect timeout\n",
+				__func__, NG_NODE_NAME(con->l2cap->node));
+
+		ng_l2cap_discon_untimeout(con);
+	}
+} /* ng_l2cap_con_ref */
+
+/*
+ * Remove reference from the connection descriptor
+ */
+
+void
+ng_l2cap_con_unref(ng_l2cap_con_p con)
+{
+	con->refcnt --;
+
+	if (con->refcnt < 0)
+		panic("%s: %s - con->refcnt < 0\n",
+			__func__, NG_NODE_NAME(con->l2cap->node));
+
+	/*
+	 * Set auto disconnect timer only if the following conditions are met:
+	 * 1) we have no reference on the connection
+	 * 2) connection is in OPEN state
+	 * 3) it is an outgoing connection
+	 * 4) disconnect timeout > 0
+	 */
+
+	if ((con->refcnt == 0) &&
+	    (con->state == NG_L2CAP_CON_OPEN) &&
+	    (con->flags & NG_L2CAP_CON_OUTGOING) && 
+	    (con->l2cap->discon_timo > 0)) {
+		if (con->flags & NG_L2CAP_CON_AUTO_DISCON_TIMO)
+			panic("%s: %s - duplicated auto disconnect timeout\n",
+				__func__, NG_NODE_NAME(con->l2cap->node));
+
+		ng_l2cap_discon_timeout(con);
+	}
+} /* ng_l2cap_con_unref */
+
+/*
+ * Set auto disconnect timeout
+ */
+
+void
+ng_l2cap_discon_timeout(ng_l2cap_con_p con)
+{
+	if (con->flags & (NG_L2CAP_CON_LP_TIMO|NG_L2CAP_CON_AUTO_DISCON_TIMO))
+		panic("%s: %s - invalid timeout, state=%d, flags=%#x\n",
+			__func__, NG_NODE_NAME(con->l2cap->node),
+			con->state, con->flags);
+
+	NG_NODE_REF(con->l2cap->node);
+	con->flags |= NG_L2CAP_CON_AUTO_DISCON_TIMO;
+	con->con_timo = timeout(ng_l2cap_queue_discon_timeout, con,
+					con->l2cap->discon_timo * hz);
+} /* ng_l2cap_discon_timeout */
+
+/*
+ * Unset auto disconnect timeout
+ */
+
+void
+ng_l2cap_discon_untimeout(ng_l2cap_con_p con)
+{
+	untimeout(ng_l2cap_queue_discon_timeout, con, con->con_timo);
+	con->flags &= ~NG_L2CAP_CON_AUTO_DISCON_TIMO;
+	NG_NODE_UNREF(con->l2cap->node);
+} /* ng_l2cap_discon_untimeout */
+
+/*
+ *  Queue auto disconnect timeout
+ */
+
+static void
+ng_l2cap_queue_discon_timeout(void *context)
+{
+	ng_l2cap_con_p	con = (ng_l2cap_con_p) context;
+	node_p		node = con->l2cap->node;
+
+	if (NG_NODE_IS_VALID(node))
+		ng_send_fn(node,NULL,&ng_l2cap_process_discon_timeout,con,0);
+
+	NG_NODE_UNREF(node);
+} /* ng_l2cap_queue_discon_timeout */
+
+/*
  * Free connection descriptor. Will unlink connection and free everything.
  */
 
@@ -133,8 +231,12 @@ ng_l2cap_free_con(ng_l2cap_con_p con)
 {
 	ng_l2cap_chan_p f = NULL, n = NULL;
 
-	if (con->state == NG_L2CAP_W4_LP_CON_CFM)
+	if (con->flags & NG_L2CAP_CON_LP_TIMO)
 		ng_l2cap_lp_untimeout(con);
+	else if (con->flags & NG_L2CAP_CON_AUTO_DISCON_TIMO)
+		ng_l2cap_discon_untimeout(con);
+
+	con->state = NG_L2CAP_CON_CLOSED;
 
 	if (con->tx_pkt != NULL) {
 		while (con->tx_pkt != NULL) {
@@ -234,6 +336,8 @@ ng_l2cap_new_chan(ng_l2cap_p l2cap, ng_l2cap_con_p con, u_int16_t psm)
 		ch->link_timo = NG_L2CAP_LINK_TIMO_DEFAULT;
 
 		LIST_INSERT_HEAD(&l2cap->chan_list, ch, next);
+
+		ng_l2cap_con_ref(con);
 	} else {
 		bzero(ch, sizeof(*ch));
 		FREE(ch, M_NETGRAPH_L2CAP);
@@ -281,6 +385,9 @@ ng_l2cap_free_chan(ng_l2cap_chan_p ch)
 	}
 
 	LIST_REMOVE(ch, next);
+
+	ng_l2cap_con_unref(ch->con);
+
 	bzero(ch, sizeof(*ch));
 	FREE(ch, M_NETGRAPH_L2CAP);
 } /* ng_l2cap_free_chan */
@@ -337,7 +444,13 @@ ng_l2cap_cmd_by_ident(ng_l2cap_con_p con, u_int8_t ident)
 void
 ng_l2cap_lp_timeout(ng_l2cap_con_p con)
 {
+	if (con->flags & (NG_L2CAP_CON_LP_TIMO|NG_L2CAP_CON_AUTO_DISCON_TIMO))
+		panic("%s: %s - invalid timeout, state=%d, flags=%#x\n",
+			__func__, NG_NODE_NAME(con->l2cap->node),
+			con->state, con->flags);
+
 	NG_NODE_REF(con->l2cap->node);
+	con->flags |= NG_L2CAP_CON_LP_TIMO;
 	con->con_timo = timeout(ng_l2cap_queue_lp_timeout, con,
 				bluetooth_hci_connect_timeout());
 } /* ng_l2cap_lp_timeout */
@@ -350,6 +463,7 @@ void
 ng_l2cap_lp_untimeout(ng_l2cap_con_p con)
 {
 	untimeout(ng_l2cap_queue_lp_timeout, con, con->con_timo);
+	con->flags &= ~NG_L2CAP_CON_LP_TIMO;
 	NG_NODE_UNREF(con->l2cap->node);
 } /* ng_l2cap_lp_untimeout */
 
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_misc.h b/sys/netgraph/bluetooth/l2cap/ng_l2cap_misc.h
index f0f72a3..a20d5b9 100644
--- a/sys/netgraph/bluetooth/l2cap/ng_l2cap_misc.h
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_misc.h
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_l2cap_misc.h,v 1.6 2002/04/16 00:43:57 max Exp $
+ * $Id: ng_l2cap_misc.h,v 1.2 2003/04/28 21:44:59 max Exp $
  * $FreeBSD$
  */
 
@@ -39,6 +39,8 @@ void           ng_l2cap_send_hook_info (node_p, hook_p, void *, int);
  */
 
 ng_l2cap_con_p ng_l2cap_new_con       (ng_l2cap_p, bdaddr_p);
+void           ng_l2cap_con_ref       (ng_l2cap_con_p);
+void           ng_l2cap_con_unref     (ng_l2cap_con_p);
 ng_l2cap_con_p ng_l2cap_con_by_addr   (ng_l2cap_p, bdaddr_p);
 ng_l2cap_con_p ng_l2cap_con_by_handle (ng_l2cap_p, u_int16_t);
 void           ng_l2cap_free_con      (ng_l2cap_con_p);
@@ -58,11 +60,13 @@ void            ng_l2cap_free_chan    (ng_l2cap_chan_p);
 #define ng_l2cap_link_cmd(con, cmd) \
 do { \
 	TAILQ_INSERT_TAIL(&(con)->cmd_list, (cmd), next); \
+	ng_l2cap_con_ref((con)); \
 } while (0)
 
 #define ng_l2cap_unlink_cmd(cmd) \
 do { \
 	TAILQ_REMOVE(&((cmd)->con->cmd_list), (cmd), next); \
+	ng_l2cap_con_unref((cmd)->con); \
 } while (0)
 
 #define ng_l2cap_free_cmd(cmd) \
@@ -84,6 +88,8 @@ u_int8_t       ng_l2cap_get_ident    (ng_l2cap_con_p);
  * Timeout
  */
 
+void ng_l2cap_discon_timeout    (ng_l2cap_con_p);
+void ng_l2cap_discon_untimeout  (ng_l2cap_con_p);
 void ng_l2cap_lp_timeout        (ng_l2cap_con_p);
 void ng_l2cap_lp_untimeout      (ng_l2cap_con_p);
 void ng_l2cap_command_timeout   (ng_l2cap_cmd_p, int);
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_prse.h b/sys/netgraph/bluetooth/l2cap/ng_l2cap_prse.h
index 5209c24..7c64e7c 100644
--- a/sys/netgraph/bluetooth/l2cap/ng_l2cap_prse.h
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_prse.h
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_l2cap_prse.h,v 1.2 2002/09/04 21:38:38 max Exp $
+ * $Id: ng_l2cap_prse.h,v 1.2 2003/04/28 21:44:59 max Exp $
  * $FreeBSD$
  */
 
@@ -64,6 +64,20 @@ static const struct ng_cmdlist	ng_l2cap_cmdlist[] = {
 		&ng_parse_uint16_type,
 		NULL
 	},
+	{
+		NGM_L2CAP_COOKIE,
+		NGM_L2CAP_NODE_GET_AUTO_DISCON_TIMO,
+		"get_disc_timo",
+		NULL,
+		&ng_parse_uint16_type
+	},
+	{
+		NGM_L2CAP_COOKIE,
+		NGM_L2CAP_NODE_SET_AUTO_DISCON_TIMO,
+		"set_disc_timo",
+		&ng_parse_uint16_type,
+		NULL
+	},
 	{ 0, }
 };
 
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.c b/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.c
index c913d1c..d928805 100644
--- a/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.c
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.c
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_l2cap_ulpi.c,v 1.22 2002/09/08 23:35:51 max Exp $
+ * $Id: ng_l2cap_ulpi.c,v 1.1 2002/11/24 19:47:06 max Exp $
  * $FreeBSD$
  */
 
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.h b/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.h
index 6c464e6..109b5b7 100644
--- a/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.h
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.h
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_l2cap_ulpi.h,v 1.5 2002/07/04 21:48:53 max Exp $
+ * $Id: ng_l2cap_ulpi.h,v 1.1 2002/11/24 19:47:06 max Exp $
  * $FreeBSD$
  */
 
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_var.h b/sys/netgraph/bluetooth/l2cap/ng_l2cap_var.h
index 057fa28..f643b51 100644
--- a/sys/netgraph/bluetooth/l2cap/ng_l2cap_var.h
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_var.h
@@ -25,7 +25,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- * $Id: ng_l2cap_var.h,v 1.13 2002/09/04 21:38:38 max Exp $
+ * $Id: ng_l2cap_var.h,v 1.2 2003/04/28 21:44:59 max Exp $
  * $FreeBSD$
  */
 
@@ -78,10 +78,11 @@ typedef struct ng_l2cap {
 
 	ng_l2cap_node_debug_ep		debug;        /* debug level */
 	ng_l2cap_node_flags_ep		flags;        /* L2CAP node flags */
+	ng_l2cap_node_auto_discon_ep	discon_timo;  /* auto discon. timeout */
 
-	bdaddr_t			bdaddr;       /* unit BDADDR */
 	u_int16_t			pkt_size;     /* max. ACL packet size */
 	u_int16_t			num_pkts;     /* out queue size */
+	bdaddr_t			bdaddr;       /* unit BDADDR */
 
 	hook_p				hci;          /* HCI downstream hook */
 	hook_p				l2c;          /* L2CAP upstream hook */
@@ -104,6 +105,9 @@ typedef struct ng_l2cap_con {
 	ng_l2cap_p			 l2cap;      /* pointer to L2CAP */
 
 	u_int16_t			 state;      /* ACL connection state */
+	u_int16_t			 flags;      /* ACL connection flags */
+
+	int32_t				 refcnt;     /* reference count */
 
 	bdaddr_t			 remote;     /* remote unit address */
 	u_int16_t			 con_handle; /* ACL connection handle */
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 */
+