Introduce support for Bluetooth SCO sockets. This is based on older

code that was revisted.

MFC after:	3 months

3 files changed, 2132 insertions, 1 deletions

diff --git a/sys/netgraph/bluetooth/include/ng_btsocket_sco.h b/sys/netgraph/bluetooth/include/ng_btsocket_sco.h
new file mode 100644
index 000000000000..25aef6bac91e
--- /dev/null
+++ b/sys/netgraph/bluetooth/include/ng_btsocket_sco.h
@@ -0,0 +1,130 @@
+/*
+ * ng_btsocket_sco.h
+ */
+
+/*-
+ * Copyright (c) 2001-2002 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_sco.h,v 1.3 2005/10/31 18:08:52 max Exp $
+ * $FreeBSD$
+ */
+
+#ifndef _NETGRAPH_BTSOCKET_SCO_H_
+#define _NETGRAPH_BTSOCKET_SCO_H_
+
+/*
+ * SCO routing entry
+ */
+
+struct ng_hook;
+struct ng_message;
+
+struct ng_btsocket_sco_rtentry {
+	bdaddr_t				 src;  /* source BD_ADDR */
+	u_int16_t				 pkt_size; /* mtu */
+	u_int16_t				 num_pkts; /* buffer size */
+	int32_t					 pending; /* pending packets */
+	struct ng_hook				*hook; /* downstream hook */
+	LIST_ENTRY(ng_btsocket_sco_rtentry)	 next; /* link to next */
+};
+typedef struct ng_btsocket_sco_rtentry		ng_btsocket_sco_rtentry_t;
+typedef struct ng_btsocket_sco_rtentry *	ng_btsocket_sco_rtentry_p;
+
+/*****************************************************************************
+ *****************************************************************************
+ **                      SOCK_SEQPACKET SCO sockets                         **
+ *****************************************************************************
+ *****************************************************************************/
+
+#define NG_BTSOCKET_SCO_SENDSPACE	1024
+#define NG_BTSOCKET_SCO_RECVSPACE	(64 * 1024)
+
+/*
+ * Bluetooth SCO socket PCB
+ */
+
+struct ng_btsocket_sco_pcb {
+	struct socket			*so;	     /* Pointer to socket */
+
+	bdaddr_t			 src;	     /* Source address */
+	bdaddr_t			 dst;	     /* Destination address */
+
+	u_int16_t			 con_handle; /* connection handle */
+
+	u_int16_t			 flags;      /* socket flags */
+#define NG_BTSOCKET_SCO_CLIENT		(1 << 0)     /* socket is client */
+#define NG_BTSOCKET_SCO_TIMO		(1 << 1)     /* timeout pending */
+
+	u_int8_t			 state;      /* socket state */
+#define NG_BTSOCKET_SCO_CLOSED		0            /* socket closed */
+#define NG_BTSOCKET_SCO_CONNECTING	1            /* wait for connect */
+#define NG_BTSOCKET_SCO_OPEN		2            /* socket open */
+#define NG_BTSOCKET_SCO_DISCONNECTING	3            /* wait for disconnect */
+
+	struct callout			 timo;       /* timeout */
+
+	ng_btsocket_sco_rtentry_p	 rt;         /* routing info */
+
+	struct mtx			 pcb_mtx;    /* pcb mutex */
+
+	LIST_ENTRY(ng_btsocket_sco_pcb)	 next;       /* link to next PCB */
+};
+typedef struct ng_btsocket_sco_pcb	ng_btsocket_sco_pcb_t;
+typedef struct ng_btsocket_sco_pcb *	ng_btsocket_sco_pcb_p;
+
+#define	so2sco_pcb(so) \
+	((struct ng_btsocket_sco_pcb *)((so)->so_pcb))
+
+/*
+ * Bluetooth SCO socket methods
+ */
+
+#ifdef _KERNEL
+
+void ng_btsocket_sco_init       (void);
+void ng_btsocket_sco_abort      (struct socket *);
+void ng_btsocket_sco_close      (struct socket *);
+int  ng_btsocket_sco_accept     (struct socket *, struct sockaddr **);
+int  ng_btsocket_sco_attach     (struct socket *, int, struct thread *);
+int  ng_btsocket_sco_bind       (struct socket *, struct sockaddr *,
+                                   struct thread *);
+int  ng_btsocket_sco_connect    (struct socket *, struct sockaddr *,
+                                   struct thread *);
+int  ng_btsocket_sco_control    (struct socket *, u_long, caddr_t,
+                                   struct ifnet *, struct thread *);
+int  ng_btsocket_sco_ctloutput  (struct socket *, struct sockopt *);
+void ng_btsocket_sco_detach     (struct socket *);
+int  ng_btsocket_sco_disconnect (struct socket *);
+int  ng_btsocket_sco_listen     (struct socket *, int, struct thread *);
+int  ng_btsocket_sco_peeraddr   (struct socket *, struct sockaddr **);
+int  ng_btsocket_sco_send       (struct socket *, int, struct mbuf *,
+                                   struct sockaddr *, struct mbuf *,
+                                   struct thread *);
+int  ng_btsocket_sco_sockaddr   (struct socket *, struct sockaddr **);
+
+#endif /* _KERNEL */
+
+#endif /* _NETGRAPH_BTSOCKET_SCO_H_ */
+
diff --git a/sys/netgraph/bluetooth/socket/ng_btsocket.c b/sys/netgraph/bluetooth/socket/ng_btsocket.c
index 3029ffd4c187..6c6b1f3f701c 100644
--- a/sys/netgraph/bluetooth/socket/ng_btsocket.c
+++ b/sys/netgraph/bluetooth/socket/ng_btsocket.c
@@ -54,6 +54,7 @@
 #include <netgraph/bluetooth/include/ng_btsocket_hci_raw.h>
 #include <netgraph/bluetooth/include/ng_btsocket_l2cap.h>
 #include <netgraph/bluetooth/include/ng_btsocket_rfcomm.h>
+#include <netgraph/bluetooth/include/ng_btsocket_sco.h>
 
 static int			ng_btsocket_modevent (module_t, int, void *);
 extern struct domain		ng_btsocket_domain;
@@ -138,6 +139,27 @@ static struct pr_usrreqs	ng_btsocket_rfcomm_usrreqs = {
 	.pru_close =		ng_btsocket_rfcomm_close,
 };
 
+/*
+ * Bluetooth SEQPACKET SCO sockets
+ */
+
+static struct pr_usrreqs	ng_btsocket_sco_usrreqs = {
+	.pru_abort =		ng_btsocket_sco_abort,
+	.pru_accept =		ng_btsocket_sco_accept,
+	.pru_attach =		ng_btsocket_sco_attach,
+	.pru_bind =		ng_btsocket_sco_bind,
+	.pru_connect =		ng_btsocket_sco_connect,
+	.pru_control =		ng_btsocket_sco_control,
+	.pru_detach =		ng_btsocket_sco_detach,
+	.pru_disconnect =	ng_btsocket_sco_disconnect,
+	.pru_listen =		ng_btsocket_sco_listen,
+	.pru_peeraddr =		ng_btsocket_sco_peeraddr,
+	.pru_send =		ng_btsocket_sco_send,
+	.pru_shutdown =		NULL,
+	.pru_sockaddr =		ng_btsocket_sco_sockaddr,
+	.pru_close =		ng_btsocket_sco_close,
+};
+
 /* 
  * Definitions of protocols supported in the BLUETOOTH domain 
  */
@@ -177,7 +199,16 @@ static struct protosw		ng_btsocket_protosw[] = {
 	.pr_ctloutput =		ng_btsocket_rfcomm_ctloutput,
 	.pr_init =		ng_btsocket_rfcomm_init,
 	.pr_usrreqs =		&ng_btsocket_rfcomm_usrreqs,
-}
+},
+{
+	.pr_type =		SOCK_SEQPACKET,
+	.pr_domain =		&ng_btsocket_domain,
+	.pr_protocol =		BLUETOOTH_PROTO_SCO,
+	.pr_flags =		PR_ATOMIC|PR_CONNREQUIRED,
+	.pr_ctloutput =		ng_btsocket_sco_ctloutput,
+	.pr_init =		ng_btsocket_sco_init,
+	.pr_usrreqs =		&ng_btsocket_sco_usrreqs,
+},
 };
 #define ng_btsocket_protosw_size \
 	(sizeof(ng_btsocket_protosw)/sizeof(ng_btsocket_protosw[0]))
@@ -205,6 +236,8 @@ 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");
+SYSCTL_NODE(_net_bluetooth_sco, OID_AUTO, sockets, CTLFLAG_RW,
+	0, "Bluetooth SCO sockets family");
 
 /* 
  * Module 
diff --git a/sys/netgraph/bluetooth/socket/ng_btsocket_sco.c b/sys/netgraph/bluetooth/socket/ng_btsocket_sco.c
new file mode 100644
index 000000000000..88ea8e0c40a6
--- /dev/null
+++ b/sys/netgraph/bluetooth/socket/ng_btsocket_sco.c
@@ -0,0 +1,1968 @@
+/*
+ * ng_btsocket_sco.c
+ */
+
+/*-
+ * Copyright (c) 2001-2002 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_sco.c,v 1.2 2005/10/31 18:08:51 max Exp $
+ * $FreeBSD$
+ */
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/bitstring.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/protosw.h>
+#include <sys/queue.h>
+#include <sys/socket.h>
+#include <sys/socketvar.h>
+#include <sys/sysctl.h>
+#include <sys/taskqueue.h>
+#include <netgraph/ng_message.h>
+#include <netgraph/netgraph.h>
+#include <netgraph/bluetooth/include/ng_bluetooth.h>
+#include <netgraph/bluetooth/include/ng_hci.h>
+#include <netgraph/bluetooth/include/ng_l2cap.h>
+#include <netgraph/bluetooth/include/ng_btsocket.h>
+#include <netgraph/bluetooth/include/ng_btsocket_sco.h>
+
+/* MALLOC define */
+#ifdef NG_SEPARATE_MALLOC
+MALLOC_DEFINE(M_NETGRAPH_BTSOCKET_SCO, "netgraph_btsocks_sco",
+		"Netgraph Bluetooth SCO sockets");
+#else
+#define M_NETGRAPH_BTSOCKET_SCO M_NETGRAPH
+#endif /* NG_SEPARATE_MALLOC */
+
+/* Netgraph node methods */
+static ng_constructor_t	ng_btsocket_sco_node_constructor;
+static ng_rcvmsg_t	ng_btsocket_sco_node_rcvmsg;
+static ng_shutdown_t	ng_btsocket_sco_node_shutdown;
+static ng_newhook_t	ng_btsocket_sco_node_newhook;
+static ng_connect_t	ng_btsocket_sco_node_connect;
+static ng_rcvdata_t	ng_btsocket_sco_node_rcvdata;
+static ng_disconnect_t	ng_btsocket_sco_node_disconnect;
+
+static void		ng_btsocket_sco_input   (void *, int);
+static void		ng_btsocket_sco_rtclean (void *, int);
+
+/* Netgraph type descriptor */
+static struct ng_type	typestruct = {
+	.version =	NG_ABI_VERSION,
+	.name =		NG_BTSOCKET_SCO_NODE_TYPE,
+	.constructor =	ng_btsocket_sco_node_constructor,
+	.rcvmsg =	ng_btsocket_sco_node_rcvmsg,
+	.shutdown =	ng_btsocket_sco_node_shutdown,
+	.newhook =	ng_btsocket_sco_node_newhook,
+	.connect =	ng_btsocket_sco_node_connect,
+	.rcvdata =	ng_btsocket_sco_node_rcvdata,
+	.disconnect =	ng_btsocket_sco_node_disconnect,
+};
+
+/* Globals */
+static u_int32_t				ng_btsocket_sco_debug_level;
+static node_p					ng_btsocket_sco_node;
+static struct ng_bt_itemq			ng_btsocket_sco_queue;
+static struct mtx				ng_btsocket_sco_queue_mtx;
+static struct task				ng_btsocket_sco_queue_task;
+static struct mtx				ng_btsocket_sco_sockets_mtx;
+static LIST_HEAD(, ng_btsocket_sco_pcb)		ng_btsocket_sco_sockets;
+static LIST_HEAD(, ng_btsocket_sco_rtentry)	ng_btsocket_sco_rt;
+static struct mtx				ng_btsocket_sco_rt_mtx;
+static struct task				ng_btsocket_sco_rt_task;
+
+/* Sysctl tree */
+SYSCTL_DECL(_net_bluetooth_sco_sockets);
+SYSCTL_NODE(_net_bluetooth_sco_sockets, OID_AUTO, seq, CTLFLAG_RW,
+	0, "Bluetooth SEQPACKET SCO sockets family");
+SYSCTL_INT(_net_bluetooth_sco_sockets_seq, OID_AUTO, debug_level,
+	CTLFLAG_RW,
+	&ng_btsocket_sco_debug_level, NG_BTSOCKET_WARN_LEVEL,
+	"Bluetooth SEQPACKET SCO sockets debug level");
+SYSCTL_INT(_net_bluetooth_sco_sockets_seq, OID_AUTO, queue_len, 
+	CTLFLAG_RD,
+	&ng_btsocket_sco_queue.len, 0,
+	"Bluetooth SEQPACKET SCO sockets input queue length");
+SYSCTL_INT(_net_bluetooth_sco_sockets_seq, OID_AUTO, queue_maxlen, 
+	CTLFLAG_RD,
+	&ng_btsocket_sco_queue.maxlen, 0,
+	"Bluetooth SEQPACKET SCO sockets input queue max. length");
+SYSCTL_INT(_net_bluetooth_sco_sockets_seq, OID_AUTO, queue_drops, 
+	CTLFLAG_RD,
+	&ng_btsocket_sco_queue.drops, 0,
+	"Bluetooth SEQPACKET SCO sockets input queue drops");
+
+/* Debug */
+#define NG_BTSOCKET_SCO_INFO \
+	if (ng_btsocket_sco_debug_level >= NG_BTSOCKET_INFO_LEVEL) \
+		printf
+
+#define NG_BTSOCKET_SCO_WARN \
+	if (ng_btsocket_sco_debug_level >= NG_BTSOCKET_WARN_LEVEL) \
+		printf
+
+#define NG_BTSOCKET_SCO_ERR \
+	if (ng_btsocket_sco_debug_level >= NG_BTSOCKET_ERR_LEVEL) \
+		printf
+
+#define NG_BTSOCKET_SCO_ALERT \
+	if (ng_btsocket_sco_debug_level >= NG_BTSOCKET_ALERT_LEVEL) \
+		printf
+
+/* 
+ * Netgraph message processing routines
+ */
+
+static int ng_btsocket_sco_process_lp_con_cfm
+	(struct ng_mesg *, ng_btsocket_sco_rtentry_p);
+static int ng_btsocket_sco_process_lp_con_ind
+	(struct ng_mesg *, ng_btsocket_sco_rtentry_p);
+static int ng_btsocket_sco_process_lp_discon_ind
+	(struct ng_mesg *, ng_btsocket_sco_rtentry_p);
+
+/*
+ * Send LP messages to the lower layer
+ */
+
+static int  ng_btsocket_sco_send_lp_con_req
+	(ng_btsocket_sco_pcb_p);
+static int  ng_btsocket_sco_send_lp_con_rsp
+	(ng_btsocket_sco_rtentry_p, bdaddr_p, int);
+static int  ng_btsocket_sco_send_lp_discon_req
+	(ng_btsocket_sco_pcb_p);
+
+static int ng_btsocket_sco_send2
+	(ng_btsocket_sco_pcb_p);
+
+/* 
+ * Timeout processing routines
+ */
+
+static void ng_btsocket_sco_timeout         (ng_btsocket_sco_pcb_p);
+static void ng_btsocket_sco_untimeout       (ng_btsocket_sco_pcb_p);
+static void ng_btsocket_sco_process_timeout (void *);
+
+/* 
+ * Other stuff 
+ */
+
+static ng_btsocket_sco_pcb_p	ng_btsocket_sco_pcb_by_addr(bdaddr_p);
+static ng_btsocket_sco_pcb_p	ng_btsocket_sco_pcb_by_handle(bdaddr_p, int);
+static ng_btsocket_sco_pcb_p	ng_btsocket_sco_pcb_by_addrs(bdaddr_p, bdaddr_p);
+
+#define ng_btsocket_sco_wakeup_input_task() \
+	taskqueue_enqueue(taskqueue_swi, &ng_btsocket_sco_queue_task)
+
+#define ng_btsocket_sco_wakeup_route_task() \
+	taskqueue_enqueue(taskqueue_swi, &ng_btsocket_sco_rt_task)
+
+/*****************************************************************************
+ *****************************************************************************
+ **                        Netgraph node interface
+ *****************************************************************************
+ *****************************************************************************/
+
+/*
+ * Netgraph node constructor. Do not allow to create node of this type.
+ */
+
+static int
+ng_btsocket_sco_node_constructor(node_p node)
+{
+	return (EINVAL);
+} /* ng_btsocket_sco_node_constructor */
+
+/*
+ * Do local shutdown processing. Let old node go and create new fresh one.
+ */
+
+static int
+ng_btsocket_sco_node_shutdown(node_p node)
+{
+	int	error = 0;
+
+	NG_NODE_UNREF(node);
+
+	/* Create new node */
+	error = ng_make_node_common(&typestruct, &ng_btsocket_sco_node);
+	if (error != 0) {
+		NG_BTSOCKET_SCO_ALERT(
+"%s: Could not create Netgraph node, error=%d\n", __func__, error);
+
+		ng_btsocket_sco_node = NULL;
+
+		return (error);
+	}
+
+	error = ng_name_node(ng_btsocket_sco_node,
+				NG_BTSOCKET_SCO_NODE_TYPE);
+	if (error != 0) {
+		NG_BTSOCKET_SCO_ALERT(
+"%s: Could not name Netgraph node, error=%d\n", __func__, error);
+
+		NG_NODE_UNREF(ng_btsocket_sco_node);
+		ng_btsocket_sco_node = NULL;
+
+		return (error);
+	}
+		
+	return (0);
+} /* ng_btsocket_sco_node_shutdown */
+
+/*
+ * We allow any hook to be connected to the node.
+ */
+
+static int
+ng_btsocket_sco_node_newhook(node_p node, hook_p hook, char const *name)
+{
+	return (0);
+} /* ng_btsocket_sco_node_newhook */
+
+/* 
+ * Just say "YEP, that's OK by me!"
+ */
+
+static int
+ng_btsocket_sco_node_connect(hook_p hook)
+{
+	NG_HOOK_SET_PRIVATE(hook, NULL);
+	NG_HOOK_REF(hook); /* Keep extra reference to the hook */
+
+#if 0
+	NG_HOOK_FORCE_QUEUE(NG_HOOK_PEER(hook));
+	NG_HOOK_FORCE_QUEUE(hook);
+#endif
+
+	return (0);
+} /* ng_btsocket_sco_node_connect */
+
+/*
+ * Hook disconnection. Schedule route cleanup task
+ */
+
+static int
+ng_btsocket_sco_node_disconnect(hook_p hook)
+{
+	/*
+	 * If hook has private information than we must have this hook in
+	 * the routing table and must schedule cleaning for the routing table.
+	 * Otherwise hook was connected but we never got "hook_info" message,
+	 * so we have never added this hook to the routing table and it save
+	 * to just delete it.
+	 */
+
+	if (NG_HOOK_PRIVATE(hook) != NULL)
+		return (ng_btsocket_sco_wakeup_route_task());
+
+	NG_HOOK_UNREF(hook); /* Remove extra reference */
+
+	return (0);
+} /* ng_btsocket_sco_node_disconnect */
+
+/*
+ * Process incoming messages 
+ */
+
+static int
+ng_btsocket_sco_node_rcvmsg(node_p node, item_p item, hook_p hook)
+{
+	struct ng_mesg	*msg = NGI_MSG(item); /* item still has message */
+	int		 error = 0;
+
+	if (msg != NULL && msg->header.typecookie == NGM_HCI_COOKIE) {
+		mtx_lock(&ng_btsocket_sco_queue_mtx);
+		if (NG_BT_ITEMQ_FULL(&ng_btsocket_sco_queue)) {
+			NG_BTSOCKET_SCO_ERR(
+"%s: Input queue is full (msg)\n", __func__);
+
+			NG_BT_ITEMQ_DROP(&ng_btsocket_sco_queue);
+			NG_FREE_ITEM(item);
+			error = ENOBUFS;
+		} else {
+			if (hook != NULL) {
+				NG_HOOK_REF(hook);
+				NGI_SET_HOOK(item, hook);
+			}
+
+			NG_BT_ITEMQ_ENQUEUE(&ng_btsocket_sco_queue, item);
+			error = ng_btsocket_sco_wakeup_input_task();
+		}
+		mtx_unlock(&ng_btsocket_sco_queue_mtx);
+	} else {
+		NG_FREE_ITEM(item);
+		error = EINVAL;
+	}
+
+	return (error);
+} /* ng_btsocket_sco_node_rcvmsg */
+
+/*
+ * Receive data on a hook
+ */
+
+static int
+ng_btsocket_sco_node_rcvdata(hook_p hook, item_p item)
+{
+	int	error = 0;
+
+	mtx_lock(&ng_btsocket_sco_queue_mtx);
+	if (NG_BT_ITEMQ_FULL(&ng_btsocket_sco_queue)) {
+		NG_BTSOCKET_SCO_ERR(
+"%s: Input queue is full (data)\n", __func__);
+
+		NG_BT_ITEMQ_DROP(&ng_btsocket_sco_queue);
+		NG_FREE_ITEM(item);
+		error = ENOBUFS;
+	} else {
+		NG_HOOK_REF(hook);
+		NGI_SET_HOOK(item, hook);
+
+		NG_BT_ITEMQ_ENQUEUE(&ng_btsocket_sco_queue, item);
+		error = ng_btsocket_sco_wakeup_input_task();
+	}
+	mtx_unlock(&ng_btsocket_sco_queue_mtx);
+
+	return (error);
+} /* ng_btsocket_sco_node_rcvdata */
+
+/*
+ * Process LP_ConnectCfm event from the lower layer protocol
+ */
+
+static int
+ng_btsocket_sco_process_lp_con_cfm(struct ng_mesg *msg,
+		ng_btsocket_sco_rtentry_p rt)
+{
+	ng_hci_lp_con_cfm_ep	*ep = NULL;
+	ng_btsocket_sco_pcb_t	*pcb = NULL;
+	int			 error = 0;
+
+	if (msg->header.arglen != sizeof(*ep))
+		return (EMSGSIZE);
+
+	ep = (ng_hci_lp_con_cfm_ep *)(msg->data);
+
+	mtx_lock(&ng_btsocket_sco_sockets_mtx);
+
+	/* Look for the socket with the token */
+	pcb = ng_btsocket_sco_pcb_by_addrs(&rt->src, &ep->bdaddr);
+	if (pcb == NULL) {
+		mtx_unlock(&ng_btsocket_sco_sockets_mtx);
+		return (ENOENT);
+	}
+
+	/* pcb is locked */
+
+	NG_BTSOCKET_SCO_INFO(
+"%s: Got LP_ConnectCfm response, src bdaddr=%x:%x:%x:%x:%x:%x, " \
+"dst bdaddr=%x:%x:%x:%x:%x:%x, status=%d, handle=%d, state=%d\n",
+		__func__,
+		pcb->src.b[5], pcb->src.b[4], pcb->src.b[3],
+		pcb->src.b[2], pcb->src.b[1], pcb->src.b[0],
+		pcb->dst.b[5], pcb->dst.b[4], pcb->dst.b[3],
+		pcb->dst.b[2], pcb->dst.b[1], pcb->dst.b[0],
+		ep->status, ep->con_handle, pcb->state);
+
+	if (pcb->state != NG_BTSOCKET_SCO_CONNECTING) {
+		mtx_unlock(&pcb->pcb_mtx);
+		mtx_unlock(&ng_btsocket_sco_sockets_mtx);
+
+		return (ENOENT);
+	}
+
+	ng_btsocket_sco_untimeout(pcb);
+
+	if (ep->status == 0) {
+		/*
+		 * Connection is open. Update connection handle and
+		 * socket state
+		 */
+
+		pcb->con_handle = ep->con_handle; 
+		pcb->state = NG_BTSOCKET_SCO_OPEN;
+		soisconnected(pcb->so); 
+	} else {
+		/*
+		 * We have failed to open connection, so disconnect the socket
+		 */
+
+		pcb->so->so_error = ECONNREFUSED; /* XXX convert status ??? */
+		pcb->state = NG_BTSOCKET_SCO_CLOSED;
+		soisdisconnected(pcb->so); 
+	}
+
+	mtx_unlock(&pcb->pcb_mtx);
+	mtx_unlock(&ng_btsocket_sco_sockets_mtx);
+
+	return (error);
+} /* ng_btsocket_sco_process_lp_con_cfm */
+
+/*
+ * Process LP_ConnectInd indicator. Find socket that listens on address.
+ * Find exact or closest match.
+ */
+
+static int
+ng_btsocket_sco_process_lp_con_ind(struct ng_mesg *msg,
+		ng_btsocket_sco_rtentry_p rt)
+{
+	ng_hci_lp_con_ind_ep	*ep = NULL;
+	ng_btsocket_sco_pcb_t	*pcb = NULL, *pcb1 = NULL;
+	int			 error = 0;
+	u_int16_t		 status = 0;
+
+	if (msg->header.arglen != sizeof(*ep))
+		return (EMSGSIZE);
+
+	ep = (ng_hci_lp_con_ind_ep *)(msg->data);
+
+	NG_BTSOCKET_SCO_INFO(
+"%s: Got LP_ConnectInd indicator, src bdaddr=%x:%x:%x:%x:%x:%x, " \
+"dst bdaddr=%x:%x:%x:%x:%x:%x\n",
+		__func__,
+		rt->src.b[5], rt->src.b[4], rt->src.b[3],
+		rt->src.b[2], rt->src.b[1], rt->src.b[0],
+		ep->bdaddr.b[5], ep->bdaddr.b[4], ep->bdaddr.b[3],
+		ep->bdaddr.b[2], ep->bdaddr.b[1], ep->bdaddr.b[0]);
+
+	mtx_lock(&ng_btsocket_sco_sockets_mtx);
+
+	pcb = ng_btsocket_sco_pcb_by_addr(&rt->src);
+	if (pcb != NULL) {
+		struct socket	*so1 = NULL;
+
+		/* pcb is locked */
+
+		/*
+		 * First check the pending connections queue and if we have
+		 * space then create new socket and set proper source address.
+		 */
+
+		if (pcb->so->so_qlen <= pcb->so->so_qlimit)
+			so1 = sonewconn(pcb->so, 0);
+
+		if (so1 == NULL) {
+			status = 0x0d; /* Rejected due to limited resources */
+			goto respond;
+		}
+
+		/*
+		 * If we got here than we have created new socket. So complete 
+		 * connection. If we we listening on specific address then copy 
+		 * source address from listening socket, otherwise copy source 
+		 * address from hook's routing information.
+		 */
+
+		pcb1 = so2sco_pcb(so1);
+		KASSERT((pcb1 != NULL),
+("%s: pcb1 == NULL\n", __func__));
+
+ 		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));
+		else
+			bcopy(&rt->src, &pcb1->src, sizeof(pcb1->src));
+
+		pcb1->flags &= ~NG_BTSOCKET_SCO_CLIENT;
+
+		bcopy(&ep->bdaddr, &pcb1->dst, sizeof(pcb1->dst));
+		pcb1->rt = rt;
+	} else
+		/* Nobody listens on requested BDADDR */
+		status = 0x1f; /* Unspecified Error */
+
+respond:
+	error = ng_btsocket_sco_send_lp_con_rsp(rt, &ep->bdaddr, status);
+	if (pcb1 != NULL) {
+		if (error != 0) {
+			pcb1->so->so_error = error;
+			pcb1->state = NG_BTSOCKET_SCO_CLOSED;
+			soisdisconnected(pcb1->so);
+		} else {
+			pcb1->state = NG_BTSOCKET_SCO_CONNECTING;
+			soisconnecting(pcb1->so);
+
+			ng_btsocket_sco_timeout(pcb1);
+		}
+
+		mtx_unlock(&pcb1->pcb_mtx);
+	}
+
+	if (pcb != NULL)
+		mtx_unlock(&pcb->pcb_mtx);
+
+	mtx_unlock(&ng_btsocket_sco_sockets_mtx);
+
+	return (error);
+} /* ng_btsocket_sco_process_lp_con_ind */
+
+/*
+ * Process LP_DisconnectInd indicator
+ */
+
+static int
+ng_btsocket_sco_process_lp_discon_ind(struct ng_mesg *msg,
+		ng_btsocket_sco_rtentry_p rt)
+{
+	ng_hci_lp_discon_ind_ep	*ep = NULL;
+	ng_btsocket_sco_pcb_t	*pcb = NULL;
+
+	/* Check message */
+	if (msg->header.arglen != sizeof(*ep))
+		return (EMSGSIZE);
+
+	ep = (ng_hci_lp_discon_ind_ep *)(msg->data);
+
+	mtx_lock(&ng_btsocket_sco_sockets_mtx);
+
+	/* Look for the socket with given channel ID */
+	pcb = ng_btsocket_sco_pcb_by_handle(&rt->src, ep->con_handle);
+	if (pcb == NULL) {
+		mtx_unlock(&ng_btsocket_sco_sockets_mtx);
+		return (0);
+	}
+
+	/*
+	 * Disconnect the socket. If there was any pending request we can
+	 * not do anything here anyway.
+	 */
+
+	/* pcb is locked */
+
+       	NG_BTSOCKET_SCO_INFO(
+"%s: Got LP_DisconnectInd indicator, src bdaddr=%x:%x:%x:%x:%x:%x, " \
+"dst bdaddr=%x:%x:%x:%x:%x:%x, handle=%d, state=%d\n",
+		__func__,
+		pcb->src.b[5], pcb->src.b[4], pcb->src.b[3],
+		pcb->src.b[2], pcb->src.b[1], pcb->src.b[0],
+		pcb->dst.b[5], pcb->dst.b[4], pcb->dst.b[3],
+		pcb->dst.b[2], pcb->dst.b[1], pcb->dst.b[0],
+		pcb->con_handle, pcb->state);
+
+	if (pcb->flags & NG_BTSOCKET_SCO_TIMO)
+		ng_btsocket_sco_untimeout(pcb);
+
+	pcb->state = NG_BTSOCKET_SCO_CLOSED;
+	soisdisconnected(pcb->so);
+
+	mtx_unlock(&pcb->pcb_mtx);
+	mtx_unlock(&ng_btsocket_sco_sockets_mtx);
+
+	return (0);
+} /* ng_btsocket_sco_process_lp_discon_ind */
+
+/*
+ * Send LP_ConnectReq request
+ */
+
+static int
+ng_btsocket_sco_send_lp_con_req(ng_btsocket_sco_pcb_p pcb)
+{
+	struct ng_mesg		*msg = NULL;
+	ng_hci_lp_con_req_ep	*ep = NULL;
+	int			 error = 0;
+
+	mtx_assert(&pcb->pcb_mtx, MA_OWNED);
+
+	if (pcb->rt == NULL || 
+	    pcb->rt->hook == NULL || NG_HOOK_NOT_VALID(pcb->rt->hook))
+		return (ENETDOWN); 
+
+	NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_CON_REQ,
+		sizeof(*ep), M_NOWAIT);
+	if (msg == NULL)
+		return (ENOMEM);
+
+	ep = (ng_hci_lp_con_req_ep *)(msg->data);
+	ep->link_type = NG_HCI_LINK_SCO;
+	bcopy(&pcb->dst, &ep->bdaddr, sizeof(ep->bdaddr));
+
+	NG_SEND_MSG_HOOK(error, ng_btsocket_sco_node, msg, pcb->rt->hook, 0);
+
+	return (error);
+} /* ng_btsocket_sco_send_lp_con_req */
+
+/*
+ * Send LP_ConnectRsp response
+ */
+
+static int
+ng_btsocket_sco_send_lp_con_rsp(ng_btsocket_sco_rtentry_p rt, bdaddr_p dst, int status)
+{
+	struct ng_mesg		*msg = NULL;
+	ng_hci_lp_con_rsp_ep	*ep = NULL;
+	int			 error = 0;
+
+	if (rt == NULL || rt->hook == NULL || NG_HOOK_NOT_VALID(rt->hook))
+		return (ENETDOWN); 
+
+	NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_CON_RSP,
+		sizeof(*ep), M_NOWAIT);
+	if (msg == NULL)
+		return (ENOMEM);
+
+	ep = (ng_hci_lp_con_rsp_ep *)(msg->data);
+	ep->status = status;
+	ep->link_type = NG_HCI_LINK_SCO;
+	bcopy(dst, &ep->bdaddr, sizeof(ep->bdaddr));
+
+	NG_SEND_MSG_HOOK(error, ng_btsocket_sco_node, msg, rt->hook, 0);
+
+	return (error);
+} /* ng_btsocket_sco_send_lp_con_rsp */
+
+/*
+ * Send LP_DisconReq request
+ */
+
+static int
+ng_btsocket_sco_send_lp_discon_req(ng_btsocket_sco_pcb_p pcb)
+{
+	struct ng_mesg		*msg = NULL;
+	ng_hci_lp_discon_req_ep	*ep = NULL;
+	int			 error = 0;
+
+	mtx_assert(&pcb->pcb_mtx, MA_OWNED);
+
+	if (pcb->rt == NULL || 
+	    pcb->rt->hook == NULL || NG_HOOK_NOT_VALID(pcb->rt->hook))
+		return (ENETDOWN); 
+
+	NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_DISCON_REQ,
+		sizeof(*ep), M_NOWAIT);
+	if (msg == NULL)
+		return (ENOMEM);
+
+	ep = (ng_hci_lp_discon_req_ep *)(msg->data);
+	ep->con_handle = pcb->con_handle;
+	ep->reason = 0x13; /* User Ended Connection */
+
+	NG_SEND_MSG_HOOK(error, ng_btsocket_sco_node, msg, pcb->rt->hook, 0);
+
+	return (error);
+} /* ng_btsocket_sco_send_lp_discon_req */
+
+/*****************************************************************************
+ *****************************************************************************
+ **                              Socket interface
+ *****************************************************************************
+ *****************************************************************************/
+
+/*
+ * SCO sockets data input routine
+ */
+
+static void
+ng_btsocket_sco_data_input(struct mbuf *m, hook_p hook)
+{
+	ng_hci_scodata_pkt_t		*hdr = NULL;
+	ng_btsocket_sco_pcb_t		*pcb = NULL;
+	ng_btsocket_sco_rtentry_t	*rt = NULL;
+	u_int16_t			 con_handle;
+
+	if (hook == NULL) {
+		NG_BTSOCKET_SCO_ALERT(
+"%s: Invalid source hook for SCO data packet\n", __func__);
+		goto drop;
+	}
+
+	rt = (ng_btsocket_sco_rtentry_t *) NG_HOOK_PRIVATE(hook);
+	if (rt == NULL) {
+		NG_BTSOCKET_SCO_ALERT(
+"%s: Could not find out source bdaddr for SCO data packet\n", __func__);
+		goto drop;
+	}
+
+	/* Make sure we can access header */
+	if (m->m_pkthdr.len < sizeof(*hdr)) {
+		NG_BTSOCKET_SCO_ERR(
+"%s: SCO data packet too small, len=%d\n", __func__, m->m_pkthdr.len);
+		goto drop;
+	}
+
+	if (m->m_len < sizeof(*hdr)) { 
+		m = m_pullup(m, sizeof(*hdr));
+		if (m == NULL)
+			goto drop;
+	}
+
+	/* Strip SCO packet header and verify packet length */
+	hdr = mtod(m, ng_hci_scodata_pkt_t *);
+	m_adj(m, sizeof(*hdr));
+
+	if (hdr->length != m->m_pkthdr.len) {
+		NG_BTSOCKET_SCO_ERR(
+"%s: Bad SCO data packet length, len=%d, length=%d\n",
+			__func__, m->m_pkthdr.len, hdr->length);
+		goto drop;
+	}
+
+	/*
+	 * Now process packet
+	 */
+
+	con_handle = NG_HCI_CON_HANDLE(le16toh(hdr->con_handle));
+
+	NG_BTSOCKET_SCO_INFO(
+"%s: Received SCO data packet: src bdaddr=%x:%x:%x:%x:%x:%x, handle=%d, " \
+"length=%d\n",	__func__,
+		rt->src.b[5], rt->src.b[4], rt->src.b[3],
+		rt->src.b[2], rt->src.b[1], rt->src.b[0],
+		con_handle, hdr->length);
+
+	mtx_lock(&ng_btsocket_sco_sockets_mtx);
+
+	/* Find socket */
+	pcb = ng_btsocket_sco_pcb_by_handle(&rt->src, con_handle);
+	if (pcb == NULL) {
+		mtx_unlock(&ng_btsocket_sco_sockets_mtx);
+		goto drop;
+	}
+
+	/* pcb is locked */
+
+	if (pcb->state != NG_BTSOCKET_SCO_OPEN) {
+		NG_BTSOCKET_SCO_ERR(
+"%s: No connected socket found, src bdaddr=%x:%x:%x:%x:%x:%x, state=%d\n",
+			__func__,
+			rt->src.b[5], rt->src.b[4], rt->src.b[3],
+			rt->src.b[2], rt->src.b[1], rt->src.b[0],
+			pcb->state);
+
+		mtx_unlock(&pcb->pcb_mtx);
+		mtx_unlock(&ng_btsocket_sco_sockets_mtx);
+		goto drop;
+	}
+
+	/* Check if we have enough space in socket receive queue */
+	if (m->m_pkthdr.len > sbspace(&pcb->so->so_rcv)) {
+		NG_BTSOCKET_SCO_ERR(
+"%s: Not enough space in socket receive queue. Dropping SCO data packet, " \
+"src bdaddr=%x:%x:%x:%x:%x:%x, len=%d, space=%ld\n",
+			__func__,
+			rt->src.b[5], rt->src.b[4], rt->src.b[3],
+			rt->src.b[2], rt->src.b[1], rt->src.b[0],
+			m->m_pkthdr.len,
+			sbspace(&pcb->so->so_rcv));
+
+		mtx_unlock(&pcb->pcb_mtx);
+		mtx_unlock(&ng_btsocket_sco_sockets_mtx);
+		goto drop;
+	}
+
+	/* Append packet to the socket receive queue and wakeup */
+	sbappendrecord(&pcb->so->so_rcv, m);
+	m = NULL;
+
+	sorwakeup(pcb->so);
+
+	mtx_unlock(&pcb->pcb_mtx);
+	mtx_unlock(&ng_btsocket_sco_sockets_mtx);
+drop:
+	NG_FREE_M(m); /* checks for m != NULL */
+} /* ng_btsocket_sco_data_input */
+
+/*
+ * SCO sockets default message input routine
+ */
+
+static void
+ng_btsocket_sco_default_msg_input(struct ng_mesg *msg, hook_p hook)
+{
+	ng_btsocket_sco_rtentry_t	*rt = NULL;
+
+	if (hook == NULL || NG_HOOK_NOT_VALID(hook))
+		return;
+
+	rt = (ng_btsocket_sco_rtentry_t *) NG_HOOK_PRIVATE(hook);
+
+	switch (msg->header.cmd) {
+	case NGM_HCI_NODE_UP: {
+		ng_hci_node_up_ep	*ep = NULL;
+
+		if (msg->header.arglen != sizeof(*ep))
+			break;
+
+		ep = (ng_hci_node_up_ep *)(msg->data);
+		if (bcmp(&ep->bdaddr, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0)
+			break;
+
+		if (rt == NULL) {
+			MALLOC(rt, ng_btsocket_sco_rtentry_p, sizeof(*rt),
+				M_NETGRAPH_BTSOCKET_SCO, M_NOWAIT|M_ZERO);
+			if (rt == NULL)
+				break;
+
+			NG_HOOK_SET_PRIVATE(hook, rt);
+
+			mtx_lock(&ng_btsocket_sco_rt_mtx);
+
+			LIST_INSERT_HEAD(&ng_btsocket_sco_rt, rt, next);
+		} else
+			mtx_lock(&ng_btsocket_sco_rt_mtx);
+
+		bcopy(&ep->bdaddr, &rt->src, sizeof(rt->src));
+		rt->pkt_size = (ep->pkt_size == 0)? 60 : ep->pkt_size;
+		rt->num_pkts = ep->num_pkts;
+		rt->hook = hook;
+
+		mtx_unlock(&ng_btsocket_sco_rt_mtx);
+
+		NG_BTSOCKET_SCO_INFO(
+"%s: Updating hook \"%s\", src bdaddr=%x:%x:%x:%x:%x:%x, pkt_size=%d, " \
+"num_pkts=%d\n",	__func__, NG_HOOK_NAME(hook), 
+			rt->src.b[5], rt->src.b[4], rt->src.b[3], 
+			rt->src.b[2], rt->src.b[1], rt->src.b[0],
+			rt->pkt_size, rt->num_pkts);
+		} break;
+
+	case NGM_HCI_SYNC_CON_QUEUE: {
+		ng_hci_sync_con_queue_ep	*ep = NULL;
+		ng_btsocket_sco_pcb_t		*pcb = NULL;
+
+		if (rt == NULL || msg->header.arglen != sizeof(*ep))
+			break;
+
+		ep = (ng_hci_sync_con_queue_ep *)(msg->data);
+
+		rt->pending -= ep->completed;
+		if (rt->pending < 0) {
+			NG_BTSOCKET_SCO_WARN(
+"%s: Pending packet counter is out of sync! bdaddr=%x:%x:%x:%x:%x:%x, " \
+"handle=%d, pending=%d, completed=%d\n",
+				__func__,
+				rt->src.b[5], rt->src.b[4], rt->src.b[3],
+				rt->src.b[2], rt->src.b[1], rt->src.b[0],
+				ep->con_handle, rt->pending,
+				ep->completed);
+
+			rt->pending = 0;
+		}
+
+		mtx_lock(&ng_btsocket_sco_sockets_mtx);
+
+		/* Find socket */
+		pcb = ng_btsocket_sco_pcb_by_handle(&rt->src, ep->con_handle);
+		if (pcb == NULL) {
+			mtx_unlock(&ng_btsocket_sco_sockets_mtx);
+			break;
+		}
+
+		/* pcb is locked */
+
+		/* Check state */
+		if (pcb->state == NG_BTSOCKET_SCO_OPEN) {
+			/* Remove timeout */
+			ng_btsocket_sco_untimeout(pcb);
+			
+			/* Drop completed packets from the send queue */
+			for (; ep->completed > 0; ep->completed --)
+				sbdroprecord(&pcb->so->so_snd);
+
+			/* Send more if we have any */
+			if (pcb->so->so_snd.sb_cc > 0)
+				if (ng_btsocket_sco_send2(pcb) == 0)
+					ng_btsocket_sco_timeout(pcb);
+
+			/* Wake up writers */
+			sowwakeup(pcb->so);
+		}
+
+		mtx_unlock(&pcb->pcb_mtx);
+		mtx_unlock(&ng_btsocket_sco_sockets_mtx);
+	} break;
+
+	default:
+		NG_BTSOCKET_SCO_WARN(
+"%s: Unknown message, cmd=%d\n", __func__, msg->header.cmd);
+		break;
+	}
+
+	NG_FREE_MSG(msg); /* Checks for msg != NULL */
+} /* ng_btsocket_sco_default_msg_input */
+
+/*
+ * SCO sockets LP message input routine
+ */
+
+static void
+ng_btsocket_sco_lp_msg_input(struct ng_mesg *msg, hook_p hook)
+{
+	ng_btsocket_sco_rtentry_p	 rt = NULL;
+
+	if (hook == NULL) {
+		NG_BTSOCKET_SCO_ALERT(
+"%s: Invalid source hook for LP message\n", __func__);
+		goto drop;
+	}
+
+	rt = (ng_btsocket_sco_rtentry_p) NG_HOOK_PRIVATE(hook);
+	if (rt == NULL) {
+		NG_BTSOCKET_SCO_ALERT(
+"%s: Could not find out source bdaddr for LP message\n", __func__);
+		goto drop;
+	}
+
+	switch (msg->header.cmd) {
+	case NGM_HCI_LP_CON_CFM: /* Connection Confirmation Event */
+		ng_btsocket_sco_process_lp_con_cfm(msg, rt);
+		break;
+
+	case NGM_HCI_LP_CON_IND: /* Connection Indication Event */
+		ng_btsocket_sco_process_lp_con_ind(msg, rt);
+		break;
+
+	case NGM_HCI_LP_DISCON_IND: /* Disconnection Indication Event */
+		ng_btsocket_sco_process_lp_discon_ind(msg, rt);
+		break;
+
+	/* XXX FIXME add other LP messages */
+
+	default:
+		NG_BTSOCKET_SCO_WARN(
+"%s: Unknown LP message, cmd=%d\n", __func__, msg->header.cmd);
+		break;
+	}
+drop:
+	NG_FREE_MSG(msg);
+} /* ng_btsocket_sco_lp_msg_input */
+
+/*
+ * SCO sockets input routine
+ */
+
+static void
+ng_btsocket_sco_input(void *context, int pending)
+{
+	item_p	item = NULL;
+	hook_p	hook = NULL;
+
+	for (;;) {
+		mtx_lock(&ng_btsocket_sco_queue_mtx);
+		NG_BT_ITEMQ_DEQUEUE(&ng_btsocket_sco_queue, item);
+		mtx_unlock(&ng_btsocket_sco_queue_mtx);
+
+		if (item == NULL)
+			break;
+
+		NGI_GET_HOOK(item, hook);
+		if (hook != NULL && NG_HOOK_NOT_VALID(hook))
+			goto drop;
+
+		switch(item->el_flags & NGQF_TYPE) {
+		case NGQF_DATA: {
+			struct mbuf     *m = NULL;
+
+			NGI_GET_M(item, m);
+			ng_btsocket_sco_data_input(m, hook);
+			} break;
+
+		case NGQF_MESG: {
+			struct ng_mesg  *msg = NULL;
+
+			NGI_GET_MSG(item, msg);
+
+			switch (msg->header.cmd) {
+			case NGM_HCI_LP_CON_CFM:
+			case NGM_HCI_LP_CON_IND:
+			case NGM_HCI_LP_DISCON_IND:
+			/* XXX FIXME add other LP messages */
+				ng_btsocket_sco_lp_msg_input(msg, hook);
+				break;
+
+			default:
+				ng_btsocket_sco_default_msg_input(msg, hook);
+				break;
+			}
+			} break;
+
+		default:
+			KASSERT(0,
+("%s: invalid item type=%ld\n", __func__, (item->el_flags & NGQF_TYPE)));
+			break;
+		}
+drop:
+		if (hook != NULL)
+			NG_HOOK_UNREF(hook);
+
+		NG_FREE_ITEM(item);
+	}
+} /* ng_btsocket_sco_input */
+
+/*
+ * Route cleanup task. Gets scheduled when hook is disconnected. Here we 
+ * will find all sockets that use "invalid" hook and disconnect them.
+ */
+
+static void
+ng_btsocket_sco_rtclean(void *context, int pending)
+{
+	ng_btsocket_sco_pcb_p		pcb = NULL, pcb_next = NULL;
+	ng_btsocket_sco_rtentry_p	rt = NULL;
+
+	/*
+	 * First disconnect all sockets that use "invalid" hook
+	 */
+
+	mtx_lock(&ng_btsocket_sco_sockets_mtx);
+
+	for(pcb = LIST_FIRST(&ng_btsocket_sco_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)) {
+			if (pcb->flags & NG_BTSOCKET_SCO_TIMO)
+				ng_btsocket_sco_untimeout(pcb);
+
+			pcb->rt = NULL;
+			pcb->so->so_error = ENETDOWN;
+			pcb->state = NG_BTSOCKET_SCO_CLOSED;
+			soisdisconnected(pcb->so);
+		}
+
+		mtx_unlock(&pcb->pcb_mtx);
+		pcb = pcb_next;
+	}
+
+	mtx_unlock(&ng_btsocket_sco_sockets_mtx);
+
+	/*
+	 * Now cleanup routing table
+	 */
+
+	mtx_lock(&ng_btsocket_sco_rt_mtx);
+
+	for (rt = LIST_FIRST(&ng_btsocket_sco_rt); rt != NULL; ) {
+		ng_btsocket_sco_rtentry_p	rt_next = LIST_NEXT(rt, next);
+
+		if (rt->hook != NULL && NG_HOOK_NOT_VALID(rt->hook)) {
+			LIST_REMOVE(rt, next);
+
+			NG_HOOK_SET_PRIVATE(rt->hook, NULL);
+			NG_HOOK_UNREF(rt->hook); /* Remove extra reference */
+
+			bzero(rt, sizeof(*rt));
+			FREE(rt, M_NETGRAPH_BTSOCKET_SCO);
+		}
+
+		rt = rt_next;
+	}
+
+	mtx_unlock(&ng_btsocket_sco_rt_mtx);
+} /* ng_btsocket_sco_rtclean */
+
+/*
+ * Initialize everything
+ */
+
+void
+ng_btsocket_sco_init(void)
+{
+	int	error = 0;
+
+	ng_btsocket_sco_node = NULL;
+	ng_btsocket_sco_debug_level = NG_BTSOCKET_WARN_LEVEL;
+
+	/* Register Netgraph node type */
+	error = ng_newtype(&typestruct);
+	if (error != 0) {
+		NG_BTSOCKET_SCO_ALERT(
+"%s: Could not register Netgraph node type, error=%d\n", __func__, error);
+
+                return;
+	}
+
+	/* Create Netgrapg node */
+	error = ng_make_node_common(&typestruct, &ng_btsocket_sco_node);
+	if (error != 0) {
+		NG_BTSOCKET_SCO_ALERT(
+"%s: Could not create Netgraph node, error=%d\n", __func__, error);
+
+		ng_btsocket_sco_node = NULL;
+
+		return;
+	}
+
+	error = ng_name_node(ng_btsocket_sco_node, NG_BTSOCKET_SCO_NODE_TYPE);
+	if (error != 0) {
+		NG_BTSOCKET_SCO_ALERT(
+"%s: Could not name Netgraph node, error=%d\n", __func__, error);
+
+		NG_NODE_UNREF(ng_btsocket_sco_node);
+		ng_btsocket_sco_node = NULL;
+
+		return;
+	}
+
+	/* Create input queue */
+	NG_BT_ITEMQ_INIT(&ng_btsocket_sco_queue, 300);
+	mtx_init(&ng_btsocket_sco_queue_mtx,
+		"btsocks_sco_queue_mtx", NULL, MTX_DEF);
+	TASK_INIT(&ng_btsocket_sco_queue_task, 0,
+		ng_btsocket_sco_input, NULL);
+
+	/* Create list of sockets */
+	LIST_INIT(&ng_btsocket_sco_sockets);
+	mtx_init(&ng_btsocket_sco_sockets_mtx,
+		"btsocks_sco_sockets_mtx", NULL, MTX_DEF);
+
+	/* Routing table */
+	LIST_INIT(&ng_btsocket_sco_rt);
+	mtx_init(&ng_btsocket_sco_rt_mtx,
+		"btsocks_sco_rt_mtx", NULL, MTX_DEF);
+	TASK_INIT(&ng_btsocket_sco_rt_task, 0,
+		ng_btsocket_sco_rtclean, NULL);
+} /* ng_btsocket_sco_init */
+
+/*
+ * Abort connection on socket
+ */
+
+void
+ng_btsocket_sco_abort(struct socket *so)
+{
+	so->so_error = ECONNABORTED;
+
+	(void) ng_btsocket_sco_disconnect(so);
+} /* ng_btsocket_sco_abort */
+
+void
+ng_btsocket_sco_close(struct socket *so)
+{
+	(void) ng_btsocket_sco_disconnect(so);
+} /* ng_btsocket_sco_close */
+
+/*
+ * 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_sco_accept(struct socket *so, struct sockaddr **nam)
+{
+	if (ng_btsocket_sco_node == NULL) 
+		return (EINVAL);
+
+	return (ng_btsocket_sco_peeraddr(so, nam));
+} /* ng_btsocket_sco_accept */
+
+/*
+ * Create and attach new socket
+ */
+
+int
+ng_btsocket_sco_attach(struct socket *so, int proto, struct thread *td)
+{
+	ng_btsocket_sco_pcb_p	pcb = so2sco_pcb(so);
+	int			error;
+
+	/* Check socket and protocol */
+	if (ng_btsocket_sco_node == NULL) 
+		return (EPROTONOSUPPORT);
+	if (so->so_type != SOCK_SEQPACKET)
+		return (ESOCKTNOSUPPORT);
+
+#if 0 /* XXX sonewconn() calls "pru_attach" with proto == 0 */
+	if (proto != 0) 
+		if (proto != BLUETOOTH_PROTO_SCO)
+			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_SCO_SENDSPACE,
+					NG_BTSOCKET_SCO_RECVSPACE);
+		if (error != 0)
+			return (error);
+	}
+
+	/* Allocate the PCB */
+        MALLOC(pcb, ng_btsocket_sco_pcb_p, sizeof(*pcb),
+		M_NETGRAPH_BTSOCKET_SCO, M_NOWAIT | M_ZERO);
+        if (pcb == NULL)
+                return (ENOMEM);
+
+	/* Link the PCB and the socket */
+	so->so_pcb = (caddr_t) pcb;
+	pcb->so = so;
+	pcb->state = NG_BTSOCKET_SCO_CLOSED;
+
+	callout_init(&pcb->timo, 1);
+
+	/*
+	 * Mark PCB mutex as DUPOK to prevent "duplicated lock of
+	 * the same type" message. When accepting new SCO connection 
+	 * ng_btsocket_sco_process_lp_con_ind() holds both PCB mutexes 
+	 * for "old" (accepting) PCB and "new" (created) PCB.
+	 */
+		
+	mtx_init(&pcb->pcb_mtx, "btsocks_sco_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_sco_sockets_mtx.
+	 * In the second case we hold ng_btsocket_sco_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_sco_sockets_mtx);
+	else
+		mtx_assert(&ng_btsocket_sco_sockets_mtx, MA_OWNED);
+
+	LIST_INSERT_HEAD(&ng_btsocket_sco_sockets, pcb, next);
+
+	if (td != NULL)
+		mtx_unlock(&ng_btsocket_sco_sockets_mtx);
+
+        return (0);
+} /* ng_btsocket_sco_attach */
+
+/*
+ * Bind socket
+ */
+
+int
+ng_btsocket_sco_bind(struct socket *so, struct sockaddr *nam, 
+		struct thread *td)
+{
+	ng_btsocket_sco_pcb_t	*pcb = NULL;
+	struct sockaddr_sco	*sa = (struct sockaddr_sco *) nam;
+
+	if (ng_btsocket_sco_node == NULL) 
+		return (EINVAL);
+
+	/* Verify address */
+	if (sa == NULL)
+		return (EINVAL);
+	if (sa->sco_family != AF_BLUETOOTH)
+		return (EAFNOSUPPORT);
+	if (sa->sco_len != sizeof(*sa))
+		return (EINVAL);
+
+	mtx_lock(&ng_btsocket_sco_sockets_mtx);
+
+	/* 
+	 * Check if other socket has this address already (look for exact
+	 * match in bdaddr) and assign socket address if it's available.
+	 */
+
+	if (bcmp(&sa->sco_bdaddr, NG_HCI_BDADDR_ANY, sizeof(sa->sco_bdaddr)) != 0) {
+ 		LIST_FOREACH(pcb, &ng_btsocket_sco_sockets, next) {
+			mtx_lock(&pcb->pcb_mtx);
+
+			if (bcmp(&pcb->src, &sa->sco_bdaddr, sizeof(bdaddr_t)) == 0) {
+				mtx_unlock(&pcb->pcb_mtx);
+				mtx_unlock(&ng_btsocket_sco_sockets_mtx);
+
+				return (EADDRINUSE);
+			}
+
+			mtx_unlock(&pcb->pcb_mtx);
+		}
+
+	}
+
+	pcb = so2sco_pcb(so);
+	if (pcb == NULL) {
+		mtx_unlock(&ng_btsocket_sco_sockets_mtx);
+		return (EINVAL);
+	}
+
+	mtx_lock(&pcb->pcb_mtx);
+	bcopy(&sa->sco_bdaddr, &pcb->src, sizeof(pcb->src));
+	mtx_unlock(&pcb->pcb_mtx);
+
+	mtx_unlock(&ng_btsocket_sco_sockets_mtx);
+
+	return (0);
+} /* ng_btsocket_sco_bind */
+
+/*
+ * Connect socket
+ */
+
+int
+ng_btsocket_sco_connect(struct socket *so, struct sockaddr *nam, 
+		struct thread *td)
+{
+	ng_btsocket_sco_pcb_t		*pcb = so2sco_pcb(so);
+	struct sockaddr_sco		*sa = (struct sockaddr_sco *) nam;
+	ng_btsocket_sco_rtentry_t	*rt = NULL;
+	int				 have_src, error = 0;
+
+	/* Check socket */
+	if (pcb == NULL)
+		return (EINVAL);
+	if (ng_btsocket_sco_node == NULL) 
+		return (EINVAL);
+
+	/* Verify address */
+	if (sa == NULL)
+		return (EINVAL);
+	if (sa->sco_family != AF_BLUETOOTH)
+		return (EAFNOSUPPORT);
+	if (sa->sco_len != sizeof(*sa))
+		return (EINVAL);
+	if (bcmp(&sa->sco_bdaddr, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0)
+		return (EDESTADDRREQ);
+
+	/*
+	 * Routing. Socket should be bound to some source address. The source
+	 * address can be ANY. Destination address must be set and it must not
+	 * be ANY. If source address is ANY then find first rtentry that has
+	 * src != dst.
+	 */
+
+	mtx_lock(&ng_btsocket_sco_rt_mtx);
+	mtx_lock(&pcb->pcb_mtx);
+
+	if (pcb->state == NG_BTSOCKET_SCO_CONNECTING) {
+		mtx_unlock(&pcb->pcb_mtx);
+		mtx_unlock(&ng_btsocket_sco_rt_mtx);
+
+		return (EINPROGRESS);
+	}
+
+	if (bcmp(&sa->sco_bdaddr, &pcb->src, sizeof(pcb->src)) == 0) {
+		mtx_unlock(&pcb->pcb_mtx);
+		mtx_unlock(&ng_btsocket_sco_rt_mtx);
+
+		return (EINVAL);
+	}
+
+	/* Send destination address and PSM */
+	bcopy(&sa->sco_bdaddr, &pcb->dst, sizeof(pcb->dst));
+
+	pcb->rt = NULL;
+	have_src = bcmp(&pcb->src, NG_HCI_BDADDR_ANY, sizeof(pcb->src));
+
+	LIST_FOREACH(rt, &ng_btsocket_sco_rt, next) {
+		if (rt->hook == NULL || NG_HOOK_NOT_VALID(rt->hook))
+			continue;
+
+		/* Match src and dst */
+		if (have_src) {
+			if (bcmp(&pcb->src, &rt->src, sizeof(rt->src)) == 0)
+				break;
+		} else {
+			if (bcmp(&pcb->dst, &rt->src, sizeof(rt->src)) != 0)
+				break;
+		}
+	}
+
+	if (rt != NULL) {
+		pcb->rt = rt;
+
+		if (!have_src)
+			bcopy(&rt->src, &pcb->src, sizeof(pcb->src));
+	} else
+		error = EHOSTUNREACH;
+
+	/*
+	 * Send LP_Connect request 
+	 */
+
+	if (error == 0) {	
+		error = ng_btsocket_sco_send_lp_con_req(pcb);
+		if (error == 0) {
+			pcb->flags |= NG_BTSOCKET_SCO_CLIENT;
+			pcb->state = NG_BTSOCKET_SCO_CONNECTING;
+			soisconnecting(pcb->so);
+
+			ng_btsocket_sco_timeout(pcb);
+		}
+	}
+
+	mtx_unlock(&pcb->pcb_mtx);
+	mtx_unlock(&ng_btsocket_sco_rt_mtx);
+
+	return (error);
+} /* ng_btsocket_sco_connect */
+
+/*
+ * Process ioctl's calls on socket
+ */
+
+int
+ng_btsocket_sco_control(struct socket *so, u_long cmd, caddr_t data,
+		struct ifnet *ifp, struct thread *td)
+{
+	return (EINVAL);
+} /* ng_btsocket_sco_control */
+
+/*
+ * Process getsockopt/setsockopt system calls
+ */
+
+int
+ng_btsocket_sco_ctloutput(struct socket *so, struct sockopt *sopt)
+{
+	ng_btsocket_sco_pcb_p	pcb = so2sco_pcb(so);
+        int			error, tmp;
+
+	if (ng_btsocket_sco_node == NULL) 
+		return (EINVAL);
+	if (pcb == NULL)
+		return (EINVAL);
+
+	if (sopt->sopt_level != SOL_SCO)
+		return (0);
+
+	mtx_lock(&pcb->pcb_mtx);
+
+	switch (sopt->sopt_dir) {
+	case SOPT_GET:
+		if (pcb->state != NG_BTSOCKET_SCO_OPEN) {
+			error = ENOTCONN;
+			break;
+		}
+		
+		switch (sopt->sopt_name) {
+		case SO_SCO_MTU:
+			tmp = pcb->rt->pkt_size;
+			error = sooptcopyout(sopt, &tmp, sizeof(tmp));
+			break;
+
+		case SO_SCO_CONNINFO:
+			tmp = pcb->con_handle;
+			error = sooptcopyout(sopt, &tmp, sizeof(tmp));
+			break;
+
+		default:
+			error = EINVAL;
+			break;
+		}
+		break;
+
+	case SOPT_SET:
+		error = ENOPROTOOPT;
+		break;
+
+	default:
+		error = EINVAL;
+		break;
+	}
+
+	mtx_unlock(&pcb->pcb_mtx);
+	
+	return (error);
+} /* ng_btsocket_sco_ctloutput */
+
+/*
+ * Detach and destroy socket
+ */
+
+void
+ng_btsocket_sco_detach(struct socket *so)
+{
+	ng_btsocket_sco_pcb_p	pcb = so2sco_pcb(so);
+
+	KASSERT(pcb != NULL, ("ng_btsocket_sco_detach: pcb == NULL"));
+
+	if (ng_btsocket_sco_node == NULL) 
+		return;
+
+	mtx_lock(&ng_btsocket_sco_sockets_mtx);
+	mtx_lock(&pcb->pcb_mtx);
+
+	if (pcb->flags & NG_BTSOCKET_SCO_TIMO)
+		ng_btsocket_sco_untimeout(pcb);
+
+	if (pcb->state == NG_BTSOCKET_SCO_OPEN)
+		ng_btsocket_sco_send_lp_discon_req(pcb);
+
+	pcb->state = NG_BTSOCKET_SCO_CLOSED;
+
+	LIST_REMOVE(pcb, next);
+
+	mtx_unlock(&pcb->pcb_mtx);
+	mtx_unlock(&ng_btsocket_sco_sockets_mtx);
+
+	mtx_destroy(&pcb->pcb_mtx);
+	bzero(pcb, sizeof(*pcb));
+	FREE(pcb, M_NETGRAPH_BTSOCKET_SCO);
+
+	soisdisconnected(so);
+	so->so_pcb = NULL;
+} /* ng_btsocket_sco_detach */
+
+/*
+ * Disconnect socket
+ */
+
+int
+ng_btsocket_sco_disconnect(struct socket *so)
+{
+	ng_btsocket_sco_pcb_p	pcb = so2sco_pcb(so);
+
+	if (pcb == NULL)
+		return (EINVAL);
+	if (ng_btsocket_sco_node == NULL) 
+		return (EINVAL);
+
+	mtx_lock(&pcb->pcb_mtx);
+
+	if (pcb->state == NG_BTSOCKET_SCO_DISCONNECTING) {
+		mtx_unlock(&pcb->pcb_mtx);
+
+		return (EINPROGRESS);
+	}
+
+	if (pcb->flags & NG_BTSOCKET_SCO_TIMO)
+		ng_btsocket_sco_untimeout(pcb);
+
+	if (pcb->state == NG_BTSOCKET_SCO_OPEN) {
+		ng_btsocket_sco_send_lp_discon_req(pcb);
+
+		pcb->state = NG_BTSOCKET_SCO_DISCONNECTING;
+		soisdisconnecting(so);
+
+		ng_btsocket_sco_timeout(pcb);
+	} else {
+		pcb->state = NG_BTSOCKET_SCO_CLOSED;
+		soisdisconnected(so);
+	}
+
+	mtx_unlock(&pcb->pcb_mtx);
+
+	return (0);
+} /* ng_btsocket_sco_disconnect */
+
+/*
+ * Listen on socket
+ */
+
+int
+ng_btsocket_sco_listen(struct socket *so, int backlog, struct thread *td)
+{
+	ng_btsocket_sco_pcb_p	pcb = so2sco_pcb(so);
+	int			error;
+
+	if (pcb == NULL)
+		return (EINVAL);
+	if (ng_btsocket_sco_node == NULL)
+		return (EINVAL);
+
+	SOCK_LOCK(so);
+	mtx_lock(&pcb->pcb_mtx);
+
+	error = solisten_proto_check(so);
+	if (error != 0)
+		goto out;
+#if 0
+	if (bcmp(&pcb->src, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0) {
+		error = EDESTADDRREQ;
+		goto out;
+	}
+#endif
+	solisten_proto(so, backlog);
+out:
+	mtx_unlock(&pcb->pcb_mtx);
+	SOCK_UNLOCK(so);
+
+	return (error);
+} /* ng_btsocket_listen */
+
+/*
+ * Get peer address
+ */
+
+int
+ng_btsocket_sco_peeraddr(struct socket *so, struct sockaddr **nam)
+{
+	ng_btsocket_sco_pcb_p	pcb = so2sco_pcb(so);
+	struct sockaddr_sco	sa;
+
+	if (pcb == NULL)
+		return (EINVAL);
+	if (ng_btsocket_sco_node == NULL) 
+		return (EINVAL);
+
+	mtx_lock(&pcb->pcb_mtx);
+	bcopy(&pcb->dst, &sa.sco_bdaddr, sizeof(sa.sco_bdaddr));
+	mtx_unlock(&pcb->pcb_mtx);
+
+	sa.sco_len = sizeof(sa);
+	sa.sco_family = AF_BLUETOOTH;
+
+	*nam = sodupsockaddr((struct sockaddr *) &sa, M_NOWAIT);
+
+	return ((*nam == NULL)? ENOMEM : 0);
+} /* ng_btsocket_sco_peeraddr */
+
+/*
+ * Send data to socket
+ */
+
+int
+ng_btsocket_sco_send(struct socket *so, int flags, struct mbuf *m,
+		struct sockaddr *nam, struct mbuf *control, struct thread *td)
+{
+	ng_btsocket_sco_pcb_t	*pcb = so2sco_pcb(so);
+	int			 error = 0;
+                        
+	if (ng_btsocket_sco_node == NULL) {
+		error = ENETDOWN;
+		goto drop;
+	}
+
+	/* Check socket and input */
+	if (pcb == NULL || m == NULL || control != NULL) {
+		error = EINVAL;
+		goto drop;
+	}
+                 
+	mtx_lock(&pcb->pcb_mtx);
+                  
+	/* Make sure socket is connected */
+	if (pcb->state != NG_BTSOCKET_SCO_OPEN) {
+		mtx_unlock(&pcb->pcb_mtx); 
+		error = ENOTCONN;
+		goto drop;
+	}
+
+	/* Check route */
+	if (pcb->rt == NULL ||
+	    pcb->rt->hook == NULL || NG_HOOK_NOT_VALID(pcb->rt->hook)) {
+		mtx_unlock(&pcb->pcb_mtx);
+		error = ENETDOWN;
+		goto drop;
+	}
+
+	/* Check packet size */
+	if (m->m_pkthdr.len > pcb->rt->pkt_size) {
+		NG_BTSOCKET_SCO_ERR(
+"%s: Packet too big, len=%d, pkt_size=%d\n",
+			__func__, m->m_pkthdr.len, pcb->rt->pkt_size);
+
+		mtx_unlock(&pcb->pcb_mtx);
+		error = EMSGSIZE;
+		goto drop;
+	}
+
+	/*
+	 * First put packet on socket send queue. Then check if we have
+	 * pending timeout. If we do not have timeout then we must send
+	 * packet and schedule timeout. Otherwise do nothing and wait for
+	 * NGM_HCI_SYNC_CON_QUEUE message.
+	 */
+
+	sbappendrecord(&pcb->so->so_snd, m);
+	m = NULL;
+
+	if (!(pcb->flags & NG_BTSOCKET_SCO_TIMO)) {
+		error = ng_btsocket_sco_send2(pcb);
+		if (error == 0)
+			ng_btsocket_sco_timeout(pcb);
+		else
+			sbdroprecord(&pcb->so->so_snd); /* XXX */
+	}
+
+	mtx_unlock(&pcb->pcb_mtx);
+drop:
+	NG_FREE_M(m); /* checks for != NULL */
+	NG_FREE_M(control);
+
+	return (error);
+} /* ng_btsocket_sco_send */
+
+/*
+ * Send first packet in the socket queue to the SCO layer
+ */
+
+static int
+ng_btsocket_sco_send2(ng_btsocket_sco_pcb_p pcb)
+{
+	struct  mbuf		*m = NULL;
+	ng_hci_scodata_pkt_t	*hdr = NULL;
+	int			 error = 0;
+
+	mtx_assert(&pcb->pcb_mtx, MA_OWNED);
+
+	while (pcb->rt->pending < pcb->rt->num_pkts &&
+	       pcb->so->so_snd.sb_cc > 0) {
+		/* Get a copy of the first packet on send queue */
+		m = m_dup(pcb->so->so_snd.sb_mb, M_DONTWAIT);
+		if (m == NULL) {
+			error = ENOBUFS;
+			break;
+		}
+
+		/* Create SCO packet header */
+		M_PREPEND(m, sizeof(*hdr), M_DONTWAIT);
+		if (m != NULL)
+			if (m->m_len < sizeof(*hdr))
+				m = m_pullup(m, sizeof(*hdr));
+
+		if (m == NULL) {
+			error = ENOBUFS;
+			break;
+		}
+
+		/* Fill in the header */
+		hdr = mtod(m, ng_hci_scodata_pkt_t *);
+		hdr->type = NG_HCI_SCO_DATA_PKT;
+		hdr->con_handle = htole16(NG_HCI_MK_CON_HANDLE(pcb->con_handle, 0, 0));
+		hdr->length = m->m_pkthdr.len - sizeof(*hdr);
+
+		/* Send packet */
+		NG_SEND_DATA_ONLY(error, pcb->rt->hook, m);
+		if (error != 0)
+			break;
+
+		pcb->rt->pending ++;
+	}
+
+	return ((pcb->rt->pending > 0)? 0 : error);
+} /* ng_btsocket_sco_send2 */
+
+/*
+ * Get socket address
+ */
+
+int
+ng_btsocket_sco_sockaddr(struct socket *so, struct sockaddr **nam)
+{
+	ng_btsocket_sco_pcb_p	pcb = so2sco_pcb(so);
+	struct sockaddr_sco	sa;
+
+	if (pcb == NULL)
+		return (EINVAL);
+	if (ng_btsocket_sco_node == NULL) 
+		return (EINVAL);
+
+	mtx_lock(&pcb->pcb_mtx);
+	bcopy(&pcb->src, &sa.sco_bdaddr, sizeof(sa.sco_bdaddr));
+	mtx_unlock(&pcb->pcb_mtx);
+
+	sa.sco_len = sizeof(sa);
+	sa.sco_family = AF_BLUETOOTH;
+
+	*nam = sodupsockaddr((struct sockaddr *) &sa, M_NOWAIT);
+
+	return ((*nam == NULL)? ENOMEM : 0);
+} /* ng_btsocket_sco_sockaddr */
+
+/*****************************************************************************
+ *****************************************************************************
+ **                              Misc. functions
+ *****************************************************************************
+ *****************************************************************************/
+
+/*
+ * Look for the socket that listens on given bdaddr.
+ * Returns exact or close match (if any).
+ * Caller must hold ng_btsocket_sco_sockets_mtx.
+ * Returns with locked pcb.
+ */
+
+static ng_btsocket_sco_pcb_p
+ng_btsocket_sco_pcb_by_addr(bdaddr_p bdaddr)
+{
+	ng_btsocket_sco_pcb_p	p = NULL, p1 = NULL;
+
+	mtx_assert(&ng_btsocket_sco_sockets_mtx, MA_OWNED);
+
+	LIST_FOREACH(p, &ng_btsocket_sco_sockets, next) {
+		mtx_lock(&p->pcb_mtx);
+
+		if (p->so == NULL || !(p->so->so_options & SO_ACCEPTCONN)) {
+			mtx_unlock(&p->pcb_mtx);
+			continue;
+		}
+
+		if (bcmp(&p->src, bdaddr, sizeof(p->src)) == 0)
+			return (p); /* return with locked pcb */
+
+		if (bcmp(&p->src, NG_HCI_BDADDR_ANY, sizeof(p->src)) == 0)
+			p1 = p;
+
+		mtx_unlock(&p->pcb_mtx);
+	}
+
+	return (p1);
+} /* ng_btsocket_sco_pcb_by_addr */
+
+/*
+ * Look for the socket that assigned to given source address and handle.
+ * Caller must hold ng_btsocket_sco_sockets_mtx.
+ * Returns with locked pcb.
+ */
+
+static ng_btsocket_sco_pcb_p
+ng_btsocket_sco_pcb_by_handle(bdaddr_p src, int con_handle)
+{
+	ng_btsocket_sco_pcb_p	p = NULL;
+
+	mtx_assert(&ng_btsocket_sco_sockets_mtx, MA_OWNED);
+
+	LIST_FOREACH(p, &ng_btsocket_sco_sockets, next) {
+		mtx_lock(&p->pcb_mtx);
+
+		if (p->con_handle == con_handle &&
+		    bcmp(src, &p->src, sizeof(p->src)) == 0)
+			return (p); /* return with locked pcb */
+
+		mtx_unlock(&p->pcb_mtx);
+	}
+
+	return (NULL);
+} /* ng_btsocket_sco_pcb_by_handle */
+
+/*
+ * Look for the socket in CONNECTING state with given source and destination
+ * addresses. Caller must hold ng_btsocket_sco_sockets_mtx.
+ * Returns with locked pcb.
+ */
+
+static ng_btsocket_sco_pcb_p
+ng_btsocket_sco_pcb_by_addrs(bdaddr_p src, bdaddr_p dst)
+{
+	ng_btsocket_sco_pcb_p	p = NULL;
+
+	mtx_assert(&ng_btsocket_sco_sockets_mtx, MA_OWNED);
+
+	LIST_FOREACH(p, &ng_btsocket_sco_sockets, next) {
+		mtx_lock(&p->pcb_mtx);
+
+		if (p->state == NG_BTSOCKET_SCO_CONNECTING &&
+		    bcmp(src, &p->src, sizeof(p->src)) == 0 &&
+		    bcmp(dst, &p->dst, sizeof(p->dst)) == 0)
+			return (p); /* return with locked pcb */
+
+		mtx_unlock(&p->pcb_mtx);
+	}
+
+	return (NULL);
+} /* ng_btsocket_sco_pcb_by_addrs */
+
+/*
+ * Set timeout on socket
+ */
+
+static void
+ng_btsocket_sco_timeout(ng_btsocket_sco_pcb_p pcb)
+{
+	mtx_assert(&pcb->pcb_mtx, MA_OWNED);
+
+	if (!(pcb->flags & NG_BTSOCKET_SCO_TIMO)) {
+		pcb->flags |= NG_BTSOCKET_SCO_TIMO;
+		callout_reset(&pcb->timo, bluetooth_sco_rtx_timeout(),
+					ng_btsocket_sco_process_timeout, pcb);
+	} else
+		KASSERT(0,
+("%s: Duplicated socket timeout?!\n", __func__));
+} /* ng_btsocket_sco_timeout */
+
+/*
+ * Unset timeout on socket
+ */
+
+static void
+ng_btsocket_sco_untimeout(ng_btsocket_sco_pcb_p pcb)
+{
+	mtx_assert(&pcb->pcb_mtx, MA_OWNED);
+
+	if (pcb->flags & NG_BTSOCKET_SCO_TIMO) {
+		callout_stop(&pcb->timo);
+		pcb->flags &= ~NG_BTSOCKET_SCO_TIMO;
+	} else
+		KASSERT(0,
+("%s: No socket timeout?!\n", __func__));
+} /* ng_btsocket_sco_untimeout */
+
+/*
+ * Process timeout on socket
+ */
+
+static void
+ng_btsocket_sco_process_timeout(void *xpcb)
+{
+	ng_btsocket_sco_pcb_p	 pcb = (ng_btsocket_sco_pcb_p) xpcb;
+
+	mtx_lock(&pcb->pcb_mtx);
+
+	pcb->flags &= ~NG_BTSOCKET_SCO_TIMO;
+	pcb->so->so_error = ETIMEDOUT;
+
+	switch (pcb->state) {
+	case NG_BTSOCKET_SCO_CONNECTING:
+		/* Connect timeout - close the socket */
+		pcb->state = NG_BTSOCKET_SCO_CLOSED;
+		soisdisconnected(pcb->so);
+		break;
+
+	case NG_BTSOCKET_SCO_OPEN:
+		/* Send timeout - did not get NGM_HCI_SYNC_CON_QUEUE */
+		sbdroprecord(&pcb->so->so_snd);
+		sowwakeup(pcb->so);
+		/* XXX FIXME what to do with pcb->rt->pending??? */
+		break;
+
+	case NG_BTSOCKET_SCO_DISCONNECTING:
+		/* Disconnect timeout - disconnect the socket anyway */
+		pcb->state = NG_BTSOCKET_SCO_CLOSED;
+		soisdisconnected(pcb->so);
+		break;
+
+	default:
+		NG_BTSOCKET_SCO_ERR(
+"%s: Invalid socket state=%d\n", __func__, pcb->state);
+		break;
+	}
+
+	mtx_unlock(&pcb->pcb_mtx);
+} /* ng_btsocket_sco_process_timeout */
+