SDP works, but it needs to be redone (and non-blocking)

With this in bt.conf, the HID driver should work:

 # Enable all interfaces.
 interface * "OpenBSD BT"
 # Attach a keyboard and/or mouse, or a mobile phone emulating such things.
 attach 00:02:76:08:1c:37 type hid pin "0000"

Then start btd and run btctl -f bt.conf.

9 files changed, 1995 insertions, 25 deletions

diff --git a/usr.sbin/btd/Makefile b/usr.sbin/btd/Makefile
index 35ca5020fb8..0a9cf592838 100644
--- a/usr.sbin/btd/Makefile
+++ b/usr.sbin/btd/Makefile
@@ -1,8 +1,9 @@
-#	$OpenBSD: Makefile,v 1.4 2008/11/26 15:32:56 uwe Exp $
+#	$OpenBSD: Makefile,v 1.5 2008/11/26 21:48:30 uwe Exp $
 
 PROG=		btd
 SRCS=		atomicio.c bt.c bt_subr.c btd.c conf.c control.c db.c \
-		devinfo.c fdpass.c hci.c log.c sdp.c util.c
+		devinfo.c fdpass.c hci.c log.c sdp.c sdp_query.c \
+		sdp_search.c util.c
 NOMAN=
 
 LDADD+=		-levent -lusbhid
diff --git a/usr.sbin/btd/bt.c b/usr.sbin/btd/bt.c
index 57da5e3ee4c..7ea19bcbb97 100644
--- a/usr.sbin/btd/bt.c
+++ b/usr.sbin/btd/bt.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: bt.c,v 1.4 2008/11/26 15:32:56 uwe Exp $ */
+/*	$OpenBSD: bt.c,v 1.5 2008/11/26 21:48:30 uwe Exp $ */
 
 /*
  * Copyright (c) 2008 Uwe Stuehler <uwe@openbsd.org>
@@ -160,7 +160,7 @@ bt_find_inquiry_interface(struct btd *env)
 {
 	struct bt_interface *iface;
 
-	TAILQ_FOREACH(iface, &env->interfaces, entry) {
+	TAILQ_FOREACH_REVERSE(iface, &env->interfaces, interfaces, entry) {
 		if (iface->physif != NULL)
 			return iface;
 	}
@@ -173,8 +173,7 @@ bt_find_attach_interface(struct bt_device *btdev)
 {
 	struct bt_interface *iface;
 
-	TAILQ_FOREACH_REVERSE(iface, &btdev->env->interfaces,
-	    interfaces, entry) {
+	TAILQ_FOREACH(iface, &btdev->env->interfaces, entry) {
 		if (iface->physif != NULL)
 			return iface;
 	}
@@ -188,6 +187,8 @@ bt_devices_changed(void)
 	struct timeval tv;
 
 	memset(&tv, 0, sizeof(tv));
+	tv.tv_sec = 1;
+
 	evtimer_add(&ev_check_devices, &tv);
 }
 
@@ -198,7 +199,8 @@ bt_check_devices(int fd, short evflags, void *arg)
 	struct bt_device *btdev;
 
 	for (btdev = TAILQ_FIRST(&env->devices); btdev != NULL;) {
-		if ((btdev->flags & (BTDF_ATTACH|BTDF_ATTACH_DONE)) ==
+		if (!bdaddr_same(&btdev->addr, BDADDR_ANY) &&
+		    (btdev->flags & (BTDF_ATTACH|BTDF_ATTACH_DONE)) ==
 		    BTDF_ATTACH_DONE) {
 			if (bt_device_detach(btdev) < 0) {
 				btdev = TAILQ_NEXT(btdev, entry);
@@ -219,7 +221,8 @@ bt_check_devices(int fd, short evflags, void *arg)
 			continue;
 		}
 
-		if ((btdev->flags & (BTDF_ATTACH|BTDF_ATTACH_DONE)) ==
+		if (!bdaddr_same(&btdev->addr, BDADDR_ANY) &&
+		    (btdev->flags & (BTDF_ATTACH|BTDF_ATTACH_DONE)) ==
 		    BTDF_ATTACH) {
 			if (bt_device_attach(btdev) < 0) {
 				btdev = TAILQ_NEXT(btdev, entry);
@@ -245,11 +248,32 @@ bt_get_devinfo(struct bt_device *btdev)
 	if (res < 0)
 		return -1;
 	else if (res != 0) {
-		/* try sdp query */
-		return 0;
+		struct bt_interface *iface;
+
+		if ((iface = bt_find_attach_interface(btdev)) == NULL)
+			return 0;
+
+		res = sdp_get_devinfo(iface, btdev);
+		if (res < 0) {
+			btdev->flags &= ~BTDF_SDP_STARTED;
+			return -1;
+		} else if (res == 1) {
+			btdev->flags &= ~BTDF_SDP_STARTED;
+			log_info("%s: SDP query failed",
+			    bt_ntoa(&btdev->addr, NULL));
+			return 0;
+		} else if (res == 2) {
+			if (!(btdev->flags & BTDF_SDP_STARTED)) {
+				btdev->flags |= BTDF_SDP_STARTED;
+				log_info("%s: SDP query started",
+				    bt_ntoa(&btdev->addr, NULL));
+			}
+			return 0;
+		} else if (res != 0)
+			fatalx("bt_get_devinfo: sdp_get_devinfo");
 	}
 
-	bdaddr_copy(&btdev->info.baa.bd_raddr, &btdev->addr);
+	bdaddr_copy(devinfo_raddr(&btdev->info), &btdev->addr);
 	btdev->flags |= BTDF_DEVINFO_VALID;
 	return 0;
 }
@@ -272,11 +296,11 @@ bt_device_attach(struct bt_device *btdev)
 			return 0;
 	}
 
-	if (bdaddr_any(&btdev->info.baa.bd_laddr)) {
+	if (bdaddr_any(devinfo_laddr(&btdev->info))) {
 		if ((iface = bt_find_attach_interface(btdev)) == NULL)
 			return 0;
 
-		bdaddr_copy(&btdev->info.baa.bd_laddr, &iface->addr);
+		bdaddr_copy(devinfo_laddr(&btdev->info), &iface->addr);
 	}
 
 	if (devinfo_store(&btdev->info, &buf, &n) < 0)
@@ -319,11 +343,11 @@ bt_device_detach(struct bt_device *btdev)
 			return 0;
 	}
 
-	if (bdaddr_any(&btdev->info.baa.bd_laddr)) {
+	if (bdaddr_any(devinfo_laddr(&btdev->info))) {
 		if ((iface = bt_find_attach_interface(btdev)) == NULL)
 			return 0;
 
-		bdaddr_copy(&btdev->info.baa.bd_laddr, &iface->addr);
+		bdaddr_copy(devinfo_laddr(&btdev->info), &iface->addr);
 	}
 
 	if (devinfo_store(&btdev->info, &buf, &n) < 0)
diff --git a/usr.sbin/btd/btd.h b/usr.sbin/btd/btd.h
index 3b390d8abc1..aef9d24fc98 100644
--- a/usr.sbin/btd/btd.h
+++ b/usr.sbin/btd/btd.h
@@ -1,4 +1,4 @@
-/*	$OpenBSD: btd.h,v 1.4 2008/11/26 15:32:56 uwe Exp $	*/
+/*	$OpenBSD: btd.h,v 1.5 2008/11/26 21:48:30 uwe Exp $	*/
 
 /*
  * Copyright (c) 2008 Uwe Stuehler <uwe@openbsd.org>
@@ -155,6 +155,8 @@ int db_get_devinfo(struct btd_db *, const bdaddr_t *, struct bt_devinfo *);
 void db_dump(struct btd_db *);
 
 /* devinfo.c */
+bdaddr_t *devinfo_laddr(struct bt_devinfo *);
+bdaddr_t *devinfo_raddr(struct bt_devinfo *);
 int devinfo_load(struct bt_devinfo *, void *, size_t);
 void devinfo_unload(struct bt_devinfo *);
 int devinfo_store(const struct bt_devinfo *, void **, size_t *);
@@ -184,7 +186,7 @@ void fatalx(const char *);
 
 /* sdp.c */
 void sdp_init(struct btd *);
-int sdp_query(struct bt_interface *, struct bt_device *);
+int sdp_get_devinfo(struct bt_interface *, struct bt_device *);
 
 /* util.c */
 time_t getmonotime(void);
diff --git a/usr.sbin/btd/devinfo.c b/usr.sbin/btd/devinfo.c
index 08b54bc66bb..521b9bf9122 100644
--- a/usr.sbin/btd/devinfo.c
+++ b/usr.sbin/btd/devinfo.c
@@ -3,6 +3,18 @@
 
 #include "btd.h"
 
+bdaddr_t *
+devinfo_laddr(struct bt_devinfo *info)
+{
+	return &info->baa.bd_laddr;
+}
+
+bdaddr_t *
+devinfo_raddr(struct bt_devinfo *info)
+{
+	return &info->baa.bd_raddr;
+}
+
 int
 devinfo_store(const struct bt_devinfo *info, void **data, size_t *datalen)
 {
diff --git a/usr.sbin/btd/hci.c b/usr.sbin/btd/hci.c
index a8070bcf84d..88e7fbd8d4b 100644
--- a/usr.sbin/btd/hci.c
+++ b/usr.sbin/btd/hci.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: hci.c,v 1.4 2008/11/26 15:32:56 uwe Exp $	*/
+/*	$OpenBSD: hci.c,v 1.5 2008/11/26 21:48:30 uwe Exp $	*/
 /*	$NetBSD: btconfig.c,v 1.13 2008/07/21 13:36:57 lukem Exp $	*/
 
 /*-
@@ -395,7 +395,8 @@ hci_interface_open(struct bt_interface *iface)
 		return -1;
 	}
 
-	log_info("listening on %s", bt_ntoa(&physif->addr, NULL));
+	log_info("listening on %s (%s)", bt_ntoa(&physif->addr, NULL),
+	    physif->xname);
 	return 0;
 }
 
diff --git a/usr.sbin/btd/sdp.c b/usr.sbin/btd/sdp.c
index 9aae4eb7ef7..0445bf1b26b 100644
--- a/usr.sbin/btd/sdp.c
+++ b/usr.sbin/btd/sdp.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: sdp.c,v 1.3 2008/11/26 15:32:56 uwe Exp $	*/
+/*	$OpenBSD: sdp.c,v 1.4 2008/11/26 21:48:30 uwe Exp $	*/
 
 /*
  * Copyright (c) 2008 Uwe Stuehler <uwe@openbsd.org>
@@ -16,18 +16,26 @@
  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
 
+#include <netbt/l2cap.h>
+
 #include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
 
 #include "btd.h"
-
-struct sdp_session {
-	TAILQ_ENTRY(sdp_session) entry;
-};
+#include "sdp.h"
 
 struct sdp_state {
 	TAILQ_HEAD(, sdp_session) sessions;
 };
 
+struct sdp_session *sdp_new_session(struct sdp_state *, const bdaddr_t *);
+struct sdp_session *sdp_find_session(struct sdp_state *, const bdaddr_t *);
+int sdp_open_session(struct sdp_session *, const bdaddr_t *);
+void sdp_close_session(struct sdp_session *);
+void sdp_delete_session(struct sdp_session *);
+void sdp_eventcb(int, short, void *);
+
 void
 sdp_init(struct btd *env)
 {
@@ -38,8 +46,214 @@ sdp_init(struct btd *env)
 	TAILQ_INIT(&sdp->sessions);
 }
 
+/* return 0 on success, 2 if the SDP query is still in progress, 1 if
+ * the device was not found, -1 on error */
+int
+sdp_get_devinfo(struct bt_interface *iface, struct bt_device *btdev)
+{
+	struct sdp_state *sdp = iface->env->sdp;
+	struct sdp_session *sess;
+	const char *service;
+
+	log_debug("sdp_get_devinfo: raddr %s", bt_ntoa(&btdev->addr, NULL));
+
+	switch (btdev->type) {
+	case BTDEV_NONE:
+		return 1;
+	case BTDEV_HID:
+		service = "HID";
+		break;
+	case BTDEV_HSET:
+		service = "HSET";
+		break;
+	case BTDEV_HF:
+		service = "HF";
+		break;
+	default:
+		log_warnx("sdp_get_devinfo: invalid device type %#x",
+		    btdev->type);
+		return -1;
+	}
+
+	if ((sess = sdp_find_session(sdp, &btdev->addr)) == NULL &&
+	    (sess = sdp_new_session(sdp, &btdev->addr)) == NULL)
+		return -1;
+
+	if (sdp_open_session(sess, &iface->addr) < 0)
+		return -1;
+
+	if (sdp_query(sess, &btdev->info.baa, &iface->addr, &btdev->addr,
+	    service) < 0) {
+		log_warnx("sdp_query failed");
+		sdp_close_session(sess);
+		return -1;
+	}
+
+	sdp_close_session(sess);
+	return 0;
+}
+
+struct sdp_session *
+sdp_new_session(struct sdp_state *sdp, const bdaddr_t *raddr)
+{
+	struct sdp_session *sess;
+
+	if ((sess = calloc(1, sizeof(*sess))) == NULL) {
+		log_warn("sdp_new_session");
+		return NULL;
+	}
+
+	sess->sdp = sdp;
+	sess->state = SDP_SESSION_CLOSED;
+	bdaddr_copy(&sess->raddr, raddr);
+
+	TAILQ_INSERT_TAIL(&sdp->sessions, sess, entry);
+
+	return sess;
+}
+
+struct sdp_session *
+sdp_find_session(struct sdp_state *sdp, const bdaddr_t *raddr)
+{
+	struct sdp_session *sess;
+
+	TAILQ_FOREACH(sess, &sdp->sessions, entry)
+		if (bdaddr_same(&sess->raddr, raddr))
+			return sess;
+
+	return NULL;
+}
+
 int
-sdp_query(struct bt_interface *iface, struct bt_device *btdev)
+sdp_open_session(struct sdp_session *sess, const bdaddr_t *laddr)
 {
+	struct sockaddr_bt sa;
+	socklen_t size;
+
+	if (sess->state != SDP_SESSION_CLOSED) {
+		if (bdaddr_same(&sess->laddr, laddr))
+			return 0;
+
+		sdp_close_session(sess);
+	}
+
+	bdaddr_copy(&sess->laddr, laddr);
+
+	sess->fd = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
+	if (sess->fd == -1) {
+		log_warn("sdp_open_session: socket");
+		goto fail;
+	}
+
+	memset(&sa, 0, sizeof(sa));
+	sa.bt_len = sizeof(sa);
+	sa.bt_family = AF_BLUETOOTH;
+	bdaddr_copy(&sa.bt_bdaddr, &sess->laddr);
+
+	if (bind(sess->fd, (struct sockaddr *)&sa, sizeof(sa)) < 0) {
+		log_warn("sdp_open_session: bind");
+		goto fail;
+	}
+
+	sa.bt_psm = L2CAP_PSM_SDP;
+	bdaddr_copy(&sa.bt_bdaddr, &sess->raddr);
+
+	if (connect(sess->fd, (struct sockaddr *)&sa, sizeof(sa)) < 0) {
+		log_warn("sdp_open_session: connect");
+		goto fail;
+	}
+
+	size = sizeof(sess->omtu);
+	if (getsockopt(sess->fd, BTPROTO_L2CAP, SO_L2CAP_OMTU, &sess->omtu,
+	    &size) < 0) {
+		log_warn("sdp_open_session: getsockopt");
+		goto fail;
+	}
+
+	size = sizeof(sess->imtu);
+	if (getsockopt(sess->fd, BTPROTO_L2CAP, SO_L2CAP_IMTU, &sess->imtu,
+	    &size) < 0) {
+		log_warn("sdp_open_session: getsockopt");
+		goto fail;
+	}
+
+	if ((sess->req = malloc((size_t)sess->omtu)) == NULL) {
+		log_warn("sdp_open_session: malloc req");
+		goto fail;
+	}
+	sess->req_e = sess->req + sess->omtu;
+
+	if ((sess->rsp = malloc((size_t)sess->imtu)) == NULL) {
+		log_warn("sdp_open_session: malloc rsp");
+		goto fail;
+	}
+	sess->rsp_e = sess->rsp + sess->imtu;
+
+	event_set(&sess->ev, sess->fd, EV_READ | EV_PERSIST,
+	    sdp_eventcb, sess);
+	if (event_add(&sess->ev, NULL)) {
+		log_warnx("event_add failed");
+		goto fail;
+	}
+
+	sess->state = SDP_SESSION_OPEN;
 	return 0;
+
+fail:
+	if (sess->fd != -1) {
+		close(sess->fd);
+		sess->fd = -1;
+	}
+
+	if (sess->req != NULL) {
+		free(sess->req);
+		sess->req = NULL;
+	}
+
+	if (sess->rsp != NULL) {
+		free(sess->rsp);
+		sess->rsp = NULL;
+	}
+
+	return -1;
+}
+
+void
+sdp_close_session(struct sdp_session *sess)
+{
+	if (sess->state == SDP_SESSION_CLOSED)
+		return;
+
+	log_debug("sdp_close_session: laddr %s", bt_ntoa(&sess->laddr, NULL));
+
+	if (sess->req != NULL) {
+		free(sess->req);
+		sess->req = NULL;
+	}
+
+	if (sess->rsp != NULL) {
+		free(sess->rsp);
+		sess->rsp = NULL;
+	}
+
+	close(sess->fd);
+	sess->fd = -1;
+	sess->state = SDP_SESSION_CLOSED;
+}
+
+void
+sdp_delete_session(struct sdp_session *sess)
+{
+	struct sdp_state *sdp = sess->sdp;
+
+	sdp_close_session(sess);
+
+	TAILQ_REMOVE(&sdp->sessions, sess, entry);
+
+	free(sess);
+}
+
+void
+sdp_eventcb(int fd, short evflags, void *arg)
+{
 }
diff --git a/usr.sbin/btd/sdp.h b/usr.sbin/btd/sdp.h
new file mode 100644
index 00000000000..93d41cb2d29
--- /dev/null
+++ b/usr.sbin/btd/sdp.h
@@ -0,0 +1,564 @@
+/*	$NetBSD: sdp.h,v 1.4 2008/08/06 14:21:33 plunky Exp $	*/
+
+/*-
+ * Copyright (c) 2006 Itronix Inc.
+ * All rights reserved.
+ *
+ * Written by Iain Hibbert for Itronix Inc.
+ *
+ * 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.
+ * 3. The name of Itronix Inc. may not be used to endorse
+ *    or promote products derived from this software without specific
+ *    prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY ITRONIX INC. ``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 ITRONIX INC. 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.
+ */
+/*
+ * sdp.h
+ *
+ * Copyright (c) 2001-2003, 2008 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: sdp.h,v 1.1 2008/11/26 21:48:30 uwe Exp $
+ * $FreeBSD: src/lib/libsdp/sdp.h,v 1.5 2005/05/27 19:11:33 emax Exp $
+ */
+
+#ifndef _SDP_H_
+#define _SDP_H_
+
+#include <sys/queue.h>
+#include <event.h>
+#include <string.h>
+
+__BEGIN_DECLS
+
+/*
+ * Data representation (page 349)
+ */
+
+/* Nil, the null type */
+#define SDP_DATA_NIL					0x00
+
+/* Unsigned integer */
+#define SDP_DATA_UINT8					0x08
+#define SDP_DATA_UINT16					0x09
+#define SDP_DATA_UINT32					0x0A
+#define SDP_DATA_UINT64					0x0B
+#define SDP_DATA_UINT128				0x0C
+
+/* Signed two's-complement integer */
+#define SDP_DATA_INT8					0x10
+#define SDP_DATA_INT16					0x11
+#define SDP_DATA_INT32					0x12
+#define SDP_DATA_INT64					0x13
+#define SDP_DATA_INT128					0x14
+
+/* UUID, a universally unique identifier */
+#define SDP_DATA_UUID16					0x19
+#define SDP_DATA_UUID32					0x1A
+#define SDP_DATA_UUID128				0x1C
+
+/* Text string */
+#define SDP_DATA_STR8					0x25
+#define SDP_DATA_STR16					0x26
+#define SDP_DATA_STR32					0x27
+
+/* Boolean */
+#define SDP_DATA_BOOL					0x28
+
+/*
+ * Data element sequence.
+ * A data element whose data field is a sequence of data elements
+ */
+#define SDP_DATA_SEQ8					0x35
+#define SDP_DATA_SEQ16					0x36
+#define SDP_DATA_SEQ32					0x37
+
+/*
+ * Data element alternative.
+ * A data element whose data field is a sequence of data elements from
+ * which one data element is to be selected.
+ */
+#define SDP_DATA_ALT8					0x3D
+#define SDP_DATA_ALT16					0x3E
+#define SDP_DATA_ALT32					0x3F
+
+/* URL, a uniform resource locator */
+#define SDP_DATA_URL8					0x45
+#define SDP_DATA_URL16					0x46
+#define SDP_DATA_URL32					0x47
+
+/*
+ * Protocols UUID (short) http://www.bluetoothsig.org/assigned-numbers/sdp.htm
+ * BASE UUID 00000000-0000-1000-8000-00805F9B34FB
+ */
+
+#define SDP_UUID_PROTOCOL_SDP				0x0001
+#define SDP_UUID_PROTOCOL_UDP				0x0002
+#define SDP_UUID_PROTOCOL_RFCOMM			0x0003
+#define SDP_UUID_PROTOCOL_TCP				0x0004
+#define SDP_UUID_PROTOCOL_TCS_BIN			0x0005
+#define SDP_UUID_PROTOCOL_TCS_AT			0x0006
+#define SDP_UUID_PROTOCOL_OBEX				0x0008
+#define SDP_UUID_PROTOCOL_IP				0x0009
+#define SDP_UUID_PROTOCOL_FTP				0x000A
+#define SDP_UUID_PROTOCOL_HTTP				0x000C
+#define SDP_UUID_PROTOCOL_WSP				0x000E
+#define SDP_UUID_PROTOCOL_BNEP				0x000F
+#define SDP_UUID_PROTOCOL_UPNP				0x0010
+#define SDP_UUID_PROTOCOL_HIDP				0x0011
+#define SDP_UUID_PROTOCOL_HARDCOPY_CONTROL_CHANNEL	0x0012
+#define SDP_UUID_PROTOCOL_HARDCOPY_DATA_CHANNEL		0x0014
+#define SDP_UUID_PROTOCOL_HARDCOPY_NOTIFICATION		0x0016
+#define SDP_UUID_PROTOCOL_AVCTP				0x0017
+#define SDP_UUID_PROTOCOL_AVDTP				0x0019
+#define SDP_UUID_PROTOCOL_CMPT				0x001B
+#define SDP_UUID_PROTOCOL_UDI_C_PLANE			0x001D
+#define SDP_UUID_PROTOCOL_L2CAP				0x0100
+
+/*
+ * Service class IDs http://www.bluetoothsig.org/assigned-numbers/sdp.htm
+ */
+
+#define SDP_SERVICE_CLASS_SERVICE_DISCOVERY_SERVER	0x1000
+#define SDP_SERVICE_CLASS_BROWSE_GROUP_DESCRIPTOR	0x1001
+#define SDP_SERVICE_CLASS_PUBLIC_BROWSE_GROUP		0x1002
+#define SDP_SERVICE_CLASS_SERIAL_PORT			0x1101
+#define SDP_SERVICE_CLASS_LAN_ACCESS_USING_PPP		0x1102
+#define SDP_SERVICE_CLASS_DIALUP_NETWORKING		0x1103
+#define SDP_SERVICE_CLASS_IR_MC_SYNC			0x1104
+#define SDP_SERVICE_CLASS_OBEX_OBJECT_PUSH		0x1105
+#define SDP_SERVICE_CLASS_OBEX_FILE_TRANSFER		0x1106
+#define SDP_SERVICE_CLASS_IR_MC_SYNC_COMMAND		0x1107
+#define SDP_SERVICE_CLASS_HEADSET			0x1108
+#define SDP_SERVICE_CLASS_CORDLESS_TELEPHONY		0x1109
+#define SDP_SERVICE_CLASS_AUDIO_SOURCE			0x110A
+#define SDP_SERVICE_CLASS_AUDIO_SINK			0x110B
+#define SDP_SERVICE_CLASS_AV_REMOTE_CONTROL_TARGET	0x110C
+#define SDP_SERVICE_CLASS_ADVANCED_AUDIO_DISTRIBUTION	0x110D
+#define SDP_SERVICE_CLASS_AV_REMOTE_CONTROL		0x110E
+#define SDP_SERVICE_CLASS_VIDEO_CONFERENCING		0x110F
+#define SDP_SERVICE_CLASS_INTERCOM			0x1110
+#define SDP_SERVICE_CLASS_FAX				0x1111
+#define SDP_SERVICE_CLASS_HEADSET_AUDIO_GATEWAY		0x1112
+#define SDP_SERVICE_CLASS_WAP				0x1113
+#define SDP_SERVICE_CLASS_WAP_CLIENT			0x1114
+#define SDP_SERVICE_CLASS_PANU				0x1115
+#define SDP_SERVICE_CLASS_NAP				0x1116
+#define SDP_SERVICE_CLASS_GN				0x1117
+#define SDP_SERVICE_CLASS_DIRECT_PRINTING		0x1118
+#define SDP_SERVICE_CLASS_REFERENCE_PRINTING		0x1119
+#define SDP_SERVICE_CLASS_IMAGING			0x111A
+#define SDP_SERVICE_CLASS_IMAGING_RESPONDER		0x111B
+#define SDP_SERVICE_CLASS_IMAGING_AUTOMATIC_ARCHIVE	0x111C
+#define SDP_SERVICE_CLASS_IMAGING_REFERENCED_OBJECTS	0x111D
+#define SDP_SERVICE_CLASS_HANDSFREE			0x111E
+#define SDP_SERVICE_CLASS_HANDSFREE_AUDIO_GATEWAY	0x111F
+#define SDP_SERVICE_CLASS_DIRECT_PRINTING_REFERENCE_OBJECTS	0x1120
+#define SDP_SERVICE_CLASS_REFLECTED_UI			0x1121
+#define SDP_SERVICE_CLASS_BASIC_PRINTING		0x1122
+#define SDP_SERVICE_CLASS_PRINTING_STATUS		0x1123
+#define SDP_SERVICE_CLASS_HUMAN_INTERFACE_DEVICE	0x1124
+#define SDP_SERVICE_CLASS_HARDCOPY_CABLE_REPLACEMENT	0x1125
+#define SDP_SERVICE_CLASS_HCR_PRINT			0x1126
+#define SDP_SERVICE_CLASS_HCR_SCAN			0x1127
+#define SDP_SERVICE_CLASS_COMMON_ISDN_ACCESS		0x1128
+#define SDP_SERVICE_CLASS_VIDEO_CONFERENCING_GW		0x1129
+#define SDP_SERVICE_CLASS_UDI_MT			0x112A
+#define SDP_SERVICE_CLASS_UDI_TA			0x112B
+#define SDP_SERVICE_CLASS_AUDIO_VIDEO			0x112C
+#define SDP_SERVICE_CLASS_SIM_ACCESS			0x112D
+#define SDP_SERVICE_CLASS_PNP_INFORMATION		0x1200
+#define SDP_SERVICE_CLASS_GENERIC_NETWORKING		0x1201
+#define SDP_SERVICE_CLASS_GENERIC_FILE_TRANSFER		0x1202
+#define SDP_SERVICE_CLASS_GENERIC_AUDIO			0x1203
+#define SDP_SERVICE_CLASS_GENERIC_TELEPHONY		0x1204
+#define SDP_SERVICE_CLASS_UPNP				0x1205
+#define SDP_SERVICE_CLASS_UPNP_IP			0x1206
+#define SDP_SERVICE_CLASS_ESDP_UPNP_IP_PAN		0x1300
+#define SDP_SERVICE_CLASS_ESDP_UPNP_IP_LAP		0x1301
+#define SDP_SERVICE_CLASS_ESDP_UPNP_L2CAP		0x1302
+
+/*
+ * Universal attribute definitions (page 366) and
+ * http://www.bluetoothsig.org/assigned-numbers/sdp.htm
+ */
+
+#define SDP_ATTR_RANGE(lo, hi) \
+	(uint32_t)(((uint16_t)(lo) << 16) | ((uint16_t)(hi)))
+
+#define SDP_ATTR_SERVICE_RECORD_HANDLE			0x0000
+#define SDP_ATTR_SERVICE_CLASS_ID_LIST			0x0001
+#define SDP_ATTR_SERVICE_RECORD_STATE			0x0002
+#define SDP_ATTR_SERVICE_ID				0x0003
+#define SDP_ATTR_PROTOCOL_DESCRIPTOR_LIST		0x0004
+#define SDP_ATTR_BROWSE_GROUP_LIST			0x0005
+#define SDP_ATTR_LANGUAGE_BASE_ATTRIBUTE_ID_LIST	0x0006
+#define SDP_ATTR_SERVICE_INFO_TIME_TO_LIVE		0x0007
+#define SDP_ATTR_SERVICE_AVAILABILITY			0x0008
+#define SDP_ATTR_BLUETOOTH_PROFILE_DESCRIPTOR_LIST	0x0009
+#define SDP_ATTR_DOCUMENTATION_URL			0x000A
+#define SDP_ATTR_CLIENT_EXECUTABLE_URL			0x000B
+#define SDP_ATTR_ICON_URL				0x000C
+#define SDP_ATTR_ADDITIONAL_PROTOCOL_DESCRIPTOR_LISTS	0x000D
+#define SDP_ATTR_GROUP_ID				0x0200
+#define SDP_ATTR_IP_SUBNET				0x0200
+#define SDP_ATTR_VERSION_NUMBER_LIST			0x0200
+#define SDP_ATTR_SERVICE_DATABASE_STATE			0x0201
+#define SDP_ATTR_SERVICE_VERSION			0x0300
+#define SDP_ATTR_EXTERNAL_NETWORK			0x0301
+#define SDP_ATTR_NETWORK				0x0301
+#define SDP_ATTR_SUPPORTED_DATA_STORES_LIST		0x0301
+#define SDP_ATTR_FAX_CLASS1_SUPPORT			0x0302
+#define SDP_ATTR_REMOTE_AUDIO_VOLUME_CONTROL		0x0302
+#define SDP_ATTR_FAX_CLASS20_SUPPORT			0x0303
+#define SDP_ATTR_SUPPORTED_FORMATS_LIST			0x0303
+#define SDP_ATTR_FAX_CLASS2_SUPPORT			0x0304
+#define SDP_ATTR_AUDIO_FEEDBACK_SUPPORT			0x0305
+#define SDP_ATTR_NETWORK_ADDRESS			0x0306
+#define SDP_ATTR_WAP_GATEWAY				0x0307
+#define SDP_ATTR_HOME_PAGE_URL				0x0308
+#define SDP_ATTR_WAP_STACK_TYPE				0x0309
+#define SDP_ATTR_SECURITY_DESCRIPTION			0x030A
+#define SDP_ATTR_NET_ACCESS_TYPE			0x030B
+#define SDP_ATTR_MAX_NET_ACCESS_RATE			0x030C
+#define SDP_ATTR_IPV4_SUBNET				0x030D
+#define SDP_ATTR_IPV6_SUBNET				0x030E
+#define SDP_ATTR_SUPPORTED_CAPABALITIES			0x0310
+#define SDP_ATTR_SUPPORTED_FEATURES			0x0311
+#define SDP_ATTR_SUPPORTED_FUNCTIONS			0x0312
+#define SDP_ATTR_TOTAL_IMAGING_DATA_CAPACITY		0x0313
+
+/*
+ * The offset must be added to the attribute ID base (contained in the
+ * LANGUAGE_BASE_ATTRIBUTE_ID_LIST attribute) in order to compute the
+ * attribute ID for these attributes.
+ */
+
+#define SDP_ATTR_PRIMARY_LANGUAGE_BASE_ID		0x0100
+#define SDP_ATTR_SERVICE_NAME_OFFSET			0x0000
+#define SDP_ATTR_SERVICE_DESCRIPTION_OFFSET		0x0001
+#define SDP_ATTR_PROVIDER_NAME_OFFSET			0x0002
+
+/*
+ * Protocol data unit (PDU) format (page 352)
+ */
+
+#define SDP_PDU_ERROR_RESPONSE				0x01
+#define SDP_PDU_SERVICE_SEARCH_REQUEST			0x02
+#define SDP_PDU_SERVICE_SEARCH_RESPONSE			0x03
+#define SDP_PDU_SERVICE_ATTRIBUTE_REQUEST		0x04
+#define SDP_PDU_SERVICE_ATTRIBUTE_RESPONSE		0x05
+#define SDP_PDU_SERVICE_SEARCH_ATTRIBUTE_REQUEST	0x06
+#define SDP_PDU_SERVICE_SEARCH_ATTRIBUTE_RESPONSE	0x07
+
+struct sdp_pdu {
+	uint8_t		pid;	/* PDU ID - SDP_PDU_xxx */
+	uint16_t	tid;	/* transaction ID */
+	uint16_t	len;	/* parameters length (in bytes) */
+} __attribute__ ((packed));
+typedef struct sdp_pdu		sdp_pdu_t;
+typedef struct sdp_pdu *	sdp_pdu_p;
+
+/*
+ * Error codes for SDP_PDU_ERROR_RESPONSE
+ */
+
+#define SDP_ERROR_CODE_INVALID_SDP_VERSION		0x0001
+#define SDP_ERROR_CODE_INVALID_SERVICE_RECORD_HANDLE	0x0002
+#define SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX		0x0003
+#define SDP_ERROR_CODE_INVALID_PDU_SIZE			0x0004
+#define SDP_ERROR_CODE_INVALID_CONTINUATION_STATE	0x0005
+#define SDP_ERROR_CODE_INSUFFICIENT_RESOURCES		0x0006
+
+/*
+ * SDP int128/uint128 parameter
+ */
+
+struct int128 {
+	int8_t	b[16];
+};
+typedef struct int128	int128_t;
+typedef struct int128	uint128_t;
+
+/*
+ * SDP attribute
+ */
+
+struct sdp_attr {
+	uint16_t	 flags;
+#define SDP_ATTR_OK		(0 << 0)
+#define SDP_ATTR_INVALID	(1 << 0)
+#define SDP_ATTR_TRUNCATED	(1 << 1)
+	uint16_t	 attr;  /* SDP_ATTR_xxx */
+	uint32_t	 vlen;	/* length of the value[] in bytes */
+	uint8_t		*value;	/* base pointer */
+};
+typedef struct sdp_attr		sdp_attr_t;
+typedef struct sdp_attr *	sdp_attr_p;
+
+/******************************************************************************
+ * User interface
+ *****************************************************************************/
+
+/* Inline versions of get/put byte/short/long. Pointer is advanced */
+#define SDP_GET8(b, cp)		do {			\
+	(b) = *(const uint8_t *)(cp);			\
+	(cp) += sizeof(uint8_t);			\
+} while (/* CONSTCOND */0)
+
+#define SDP_GET16(s, cp)	do {			\
+	(s) = be16dec(cp);				\
+	(cp) += sizeof(uint16_t);			\
+} while (/* CONSTCOND */0)
+
+#define SDP_GET32(l, cp)	do {			\
+	(l) = be32dec(cp);				\
+	(cp) += sizeof(uint32_t);			\
+} while (/* CONSTCOND */0)
+
+#define SDP_GET64(l, cp)	do {			\
+	(l) = be64dec(cp);				\
+	(cp) += sizeof(uint64_t);			\
+} while (/* CONSTCOND */0)
+
+#if BYTE_ORDER == LITTLE_ENDIAN
+#define SDP_GET128(l, cp)	do {			\
+	register const uint8_t *t_cp = (const uint8_t *)(cp);	\
+	(l)->b[15] = *t_cp++;				\
+	(l)->b[14] = *t_cp++;				\
+	(l)->b[13] = *t_cp++;				\
+	(l)->b[12] = *t_cp++;				\
+	(l)->b[11] = *t_cp++;				\
+	(l)->b[10] = *t_cp++;				\
+	(l)->b[9]  = *t_cp++;				\
+	(l)->b[8]  = *t_cp++;				\
+	(l)->b[7]  = *t_cp++;				\
+	(l)->b[6]  = *t_cp++;				\
+	(l)->b[5]  = *t_cp++;				\
+	(l)->b[4]  = *t_cp++;				\
+	(l)->b[3]  = *t_cp++;				\
+	(l)->b[2]  = *t_cp++;				\
+	(l)->b[1]  = *t_cp++;				\
+	(l)->b[0]  = *t_cp++;				\
+	(cp) += 16;					\
+} while (/* CONSTCOND */0)
+
+#define SDP_GET_UUID128(l, cp)	do {			\
+	memcpy(&((l)->b), (cp), 16);			\
+	(cp) += 16;					\
+} while (/* CONSTCOND */0)
+#elif BYTE_ORDER == BIG_ENDIAN
+#define SDP_GET128(l, cp)	do {			\
+	memcpy(&((l)->b), (cp), 16);			\
+	(cp) += 16;					\
+} while (/* CONSTCOND */0)
+
+#define	SDP_GET_UUID128(l, cp)	SDP_GET128(l, cp)
+#else
+#error	"Unsupported BYTE_ORDER"
+#endif /* BYTE_ORDER */
+
+#define SDP_PUT8(b, cp)		do {			\
+	*(uint8_t *)(cp) = (b);				\
+	(cp) += sizeof(uint8_t);			\
+} while (/* CONSTCOND */0)
+
+#define SDP_PUT16(s, cp)	do {			\
+	be16enc((cp), (s));				\
+	(cp) += sizeof(uint16_t);			\
+} while (/* CONSTCOND */0)
+
+#define SDP_PUT32(s, cp)	do {			\
+	be32enc((cp), (s));				\
+	(cp) += sizeof(uint32_t);			\
+} while (/* CONSTCOND */0)
+
+#define SDP_PUT64(s, cp)	do {			\
+	be64enc((cp), (s));				\
+	(cp) += sizeof(uint64_t);			\
+} while (/* CONSTCOND */0)
+
+#if BYTE_ORDER == LITTLE_ENDIAN
+#define SDP_PUT128(l, cp)	do {			\
+	register const uint8_t *t_cp = (const uint8_t *)(cp);	\
+	*t_cp++ = (l)->b[15];				\
+	*t_cp++ = (l)->b[14];				\
+	*t_cp++ = (l)->b[13];				\
+	*t_cp++ = (l)->b[12];				\
+	*t_cp++ = (l)->b[11];				\
+	*t_cp++ = (l)->b[10];				\
+	*t_cp++ = (l)->b[9];				\
+	*t_cp++ = (l)->b[8];				\
+	*t_cp++ = (l)->b[7];				\
+	*t_cp++ = (l)->b[6];				\
+	*t_cp++ = (l)->b[5];				\
+	*t_cp++ = (l)->b[4];				\
+	*t_cp++ = (l)->b[3];				\
+	*t_cp++ = (l)->b[2];				\
+	*t_cp++ = (l)->b[1];				\
+	*t_cp   = (l)->b[0];				\
+	(cp) += 16;					\
+} while (/* CONSTCOND */0)
+
+#define SDP_PUT_UUID128(l, cp)	do {			\
+	memcpy((cp), &((l)->b), 16);			\
+	(cp) += 16;					\
+} while (/* CONSTCOND */0)
+#elif BYTE_ORDER == BIG_ENDIAN
+#define SDP_PUT128(l, cp)	do {			\
+	memcpy((cp), &((l)->b), 16);			\
+	(cp) += 16;					\
+} while (/* CONSTCOND */0)
+
+#define SDP_PUT_UUID128(l, cp)	SDP_PUT128(l, cp)
+#else
+#error	"Unsupported BYTE_ORDER"
+#endif /* BYTE_ORDER */
+
+void *             sdp_open       (bdaddr_t const *l, bdaddr_t const *r);
+void *             sdp_open_local (char const *control);
+int32_t            sdp_close      (void *xs);
+int32_t            sdp_error      (void *xs);
+
+int32_t            sdp_search     (void *xs,
+                                   uint32_t plen, uint16_t const *pp,
+                                   uint32_t alen, uint32_t const *ap,
+                                   uint32_t vlen, sdp_attr_t *vp);
+
+char const *       sdp_attr2desc  (uint16_t attr);
+char const *       sdp_uuid2desc  (uint16_t uuid);
+void               sdp_print      (uint32_t level, uint8_t *start,
+                                   uint8_t const *end);
+
+struct sdp_session;
+struct btdev_attach_args;
+
+int sdp_query(struct sdp_session *, struct btdev_attach_args *, bdaddr_t *,
+    bdaddr_t *, const char *);
+
+/******************************************************************************
+ * internal interface and utility functions
+ *****************************************************************************/
+
+enum sdp_session_state {
+	SDP_SESSION_CLOSED,
+	SDP_SESSION_OPEN,
+	SDP_SESSION_FINISHED
+};
+
+struct sdp_session {
+	TAILQ_ENTRY(sdp_session) entry;
+	struct sdp_state *sdp;
+	enum sdp_session_state state;
+	bdaddr_t laddr;
+	bdaddr_t raddr;
+	uint16_t omtu;
+	uint16_t imtu;
+	uint8_t *req;
+	uint8_t *rsp;
+	int fd;
+	struct event ev;
+	/* for legacy SDP code */
+	uint8_t *req_e;
+	uint8_t *rsp_e;
+	uint16_t tid;
+	uint8_t cs[16];
+	uint32_t cslen;
+	int error;
+};
+
+static __inline void
+be16enc(void *buf, uint16_t u)
+{
+	uint8_t *p = (uint8_t *)buf;
+
+	p[0] = ((unsigned)u >> 8) & 0xff;
+	p[1] = u & 0xff;
+}
+
+static __inline uint16_t
+be16dec(const void *buf)
+{
+	const uint8_t *p = (const uint8_t *)buf;
+
+	return ((p[0] << 8) | p[1]);
+}
+
+static __inline void
+be32enc(void *buf, uint32_t u)
+{
+	uint8_t *p = (uint8_t *)buf;
+
+	p[0] = (u >> 24) & 0xff;
+	p[1] = (u >> 16) & 0xff;
+	p[2] = (u >> 8) & 0xff;
+	p[3] = u & 0xff;
+}
+
+static __inline uint32_t
+be32dec(const void *buf)
+{
+	const uint8_t *p = (const uint8_t *)buf;
+
+	return ((p[0] << 24) | (p[1] << 16) | (p[2] << 8) | p[3]);
+}
+
+static __inline void
+be64enc(void *buf, uint64_t u)
+{
+	uint8_t *p = (uint8_t *)buf;
+
+	be32enc(p, (uint32_t)(u >> 32));
+	be32enc(p + 4, (uint32_t)(u & 0xffffffffULL));
+}
+
+static __inline uint64_t
+be64dec(const void *buf)
+{
+	const uint8_t *p = (const uint8_t *)buf;
+
+	return (((uint64_t)be32dec(p) << 32) | be32dec(p + 4));
+}
+
+__END_DECLS
+
+#endif /* ndef _SDP_H_ */
diff --git a/usr.sbin/btd/sdp_query.c b/usr.sbin/btd/sdp_query.c
new file mode 100644
index 00000000000..e6bde48e28e
--- /dev/null
+++ b/usr.sbin/btd/sdp_query.c
@@ -0,0 +1,730 @@
+/*	$OpenBSD: sdp_query.c,v 1.1 2008/11/26 21:48:30 uwe Exp $	*/
+/*	$NetBSD: sdp.c,v 1.5 2008/04/20 19:34:23 plunky Exp $	*/
+
+/*-
+ * Copyright (c) 2006 Itronix Inc.
+ * 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.
+ * 3. The name of Itronix Inc. may not be used to endorse
+ *    or promote products derived from this software without specific
+ *    prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY ITRONIX INC. ``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 ITRONIX INC. 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.
+ */
+/*
+ * Copyright (c) 2004 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.
+ */
+
+#include <sys/types.h>
+
+#include <dev/bluetooth/btdev.h>
+#include <dev/usb/usb.h>
+#include <dev/usb/usbhid.h>
+
+#include <err.h>
+#include <errno.h>
+#include <sdp.h>
+#include <stdlib.h>
+#include <usbhid.h>
+
+#include "btd.h"
+
+static int hid_mode(uint8_t *, int32_t);
+
+static int32_t parse_l2cap_psm(sdp_attr_t *);
+static int32_t parse_rfcomm_channel(sdp_attr_t *);
+static int32_t parse_hid_descriptor(sdp_attr_t *);
+static int32_t parse_boolean(sdp_attr_t *);
+
+static int config_hid(struct btdev_attach_args *);
+static int config_hset(struct btdev_attach_args *);
+static int config_hf(struct btdev_attach_args *);
+
+uint16_t hid_services[] = {
+	SDP_SERVICE_CLASS_HUMAN_INTERFACE_DEVICE
+};
+
+uint32_t hid_attrs[] = {
+	SDP_ATTR_RANGE(	SDP_ATTR_PROTOCOL_DESCRIPTOR_LIST,
+			SDP_ATTR_PROTOCOL_DESCRIPTOR_LIST),
+	SDP_ATTR_RANGE( SDP_ATTR_ADDITIONAL_PROTOCOL_DESCRIPTOR_LISTS,
+			SDP_ATTR_ADDITIONAL_PROTOCOL_DESCRIPTOR_LISTS),
+	SDP_ATTR_RANGE(	0x0205,		/* HIDReconnectInitiate */
+			0x0206),	/* HIDDescriptorList */
+	SDP_ATTR_RANGE(	0x0209,		/* HIDBatteryPower */
+			0x0209),
+	SDP_ATTR_RANGE(	0x020d,		/* HIDNormallyConnectable */
+			0x020d)
+};
+
+uint16_t hset_services[] = {
+	SDP_SERVICE_CLASS_HEADSET
+};
+
+uint32_t hset_attrs[] = {
+	SDP_ATTR_RANGE(	SDP_ATTR_PROTOCOL_DESCRIPTOR_LIST,
+			SDP_ATTR_PROTOCOL_DESCRIPTOR_LIST),
+};
+
+uint16_t hf_services[] = {
+	SDP_SERVICE_CLASS_HANDSFREE_AUDIO_GATEWAY
+};
+
+uint32_t hf_attrs[] = {
+	SDP_ATTR_RANGE(	SDP_ATTR_PROTOCOL_DESCRIPTOR_LIST,
+			SDP_ATTR_PROTOCOL_DESCRIPTOR_LIST),
+};
+
+#define NUM(v)		(sizeof(v) / sizeof(v[0]))
+
+static struct {
+	const char		*name;
+	int			(*handler)(struct btdev_attach_args *);
+	const char		*description;
+	uint16_t		*services;
+	int			nservices;
+	uint32_t		*attrs;
+	int			nattrs;
+} cfgtype[] = {
+    {
+	"HID",		config_hid,	"Human Interface Device",
+	hid_services,	NUM(hid_services),
+	hid_attrs,	NUM(hid_attrs),
+    },
+    {
+	"HSET",		config_hset,	"Headset",
+	hset_services,	NUM(hset_services),
+	hset_attrs,	NUM(hset_attrs),
+    },
+    {
+	"HF",		config_hf,	"Handsfree",
+	hf_services,	NUM(hf_services),
+	hf_attrs,	NUM(hf_attrs),
+    },
+};
+
+static sdp_attr_t	values[8];
+static uint8_t		buffer[NUM(values)][512];
+
+int
+sdp_query(struct sdp_session *sess, struct btdev_attach_args *dict,
+    bdaddr_t *laddr, bdaddr_t *raddr, const char *service)
+{
+	int rv, i;
+
+	for (i = 0 ; i < NUM(values) ; i++) {
+		values[i].flags = SDP_ATTR_INVALID;
+		values[i].attr = 0;
+		values[i].vlen = sizeof(buffer[i]);
+		values[i].value = buffer[i];
+	}
+
+	for (i = 0 ; i < NUM(cfgtype) ; i++) {
+		if (strcasecmp(service, cfgtype[i].name) == 0) {
+			rv = sdp_search(sess,
+				cfgtype[i].nservices, cfgtype[i].services,
+				cfgtype[i].nattrs, cfgtype[i].attrs,
+				NUM(values), values);
+
+			if (rv != 0) {
+				errno = sess->error;
+				return -1;
+			}
+
+			rv = (*cfgtype[i].handler)(dict);
+			if (rv != 0)
+				return -1;
+
+			return 0;
+		}
+	}
+
+	fatalx("bad device type");
+	/* NOTREACHED */
+	return -1;
+}
+
+/*
+ * Configure HID results
+ */
+static int
+config_hid(struct btdev_attach_args *dict)
+{
+	int32_t control_psm, interrupt_psm,
+		reconnect_initiate, battery_power,
+		normally_connectable, hid_length;
+	uint8_t *hid_descriptor;
+	int i;
+
+	control_psm = -1;
+	interrupt_psm = -1;
+	reconnect_initiate = -1;
+	normally_connectable = 0;
+	battery_power = 0;
+	hid_descriptor = NULL;
+	hid_length = -1;
+
+	for (i = 0; i < NUM(values) ; i++) {
+		if (values[i].flags != SDP_ATTR_OK)
+			continue;
+
+		switch (values[i].attr) {
+		case SDP_ATTR_PROTOCOL_DESCRIPTOR_LIST:
+			control_psm = parse_l2cap_psm(&values[i]);
+			break;
+
+		case SDP_ATTR_ADDITIONAL_PROTOCOL_DESCRIPTOR_LISTS:
+			interrupt_psm = parse_l2cap_psm(&values[i]);
+			break;
+
+		case 0x0205: /* HIDReconnectInitiate */
+			reconnect_initiate = parse_boolean(&values[i]);
+			break;
+
+		case 0x0206: /* HIDDescriptorList */
+			if (parse_hid_descriptor(&values[i]) == 0) {
+				hid_descriptor = values[i].value;
+				hid_length = values[i].vlen;
+			}
+			break;
+
+		case 0x0209: /* HIDBatteryPower */
+			battery_power = parse_boolean(&values[i]);
+			break;
+
+		case 0x020d: /* HIDNormallyConnectable */
+			normally_connectable = parse_boolean(&values[i]);
+			break;
+		}
+	}
+
+	if (control_psm == -1
+	    || interrupt_psm == -1
+	    || reconnect_initiate == -1
+	    || hid_descriptor == NULL
+	    || hid_length == -1)
+		return ENOATTR;
+
+	dict->bd_type = BTDEV_HID;
+	dict->bd_hid.hid_ctl = control_psm;
+	dict->bd_hid.hid_int = interrupt_psm;
+	dict->bd_hid.hid_desc = hid_descriptor;
+	dict->bd_hid.hid_dlen = hid_length;
+	dict->bd_mode = hid_mode(hid_descriptor, hid_length);
+
+	if (!reconnect_initiate)
+		dict->bd_hid.hid_flags |= BTHID_INITIATE;
+
+	return 0;
+}
+
+/*
+ * Configure HSET results
+ */
+static int
+config_hset(struct btdev_attach_args *dict)
+{
+	uint32_t channel;
+	int i;
+
+	channel = -1;
+
+	for (i = 0; i < NUM(values) ; i++) {
+		if (values[i].flags != SDP_ATTR_OK)
+			continue;
+
+		switch (values[i].attr) {
+		case SDP_ATTR_PROTOCOL_DESCRIPTOR_LIST:
+			channel = parse_rfcomm_channel(&values[i]);
+			break;
+		}
+	}
+
+	if (channel == -1)
+		return ENOATTR;
+
+	dict->bd_type = BTDEV_HSET;
+	dict->bd_hset.hset_channel = channel;
+
+	return 0;
+}
+
+/*
+ * Configure HF results
+ */
+static int
+config_hf(struct btdev_attach_args *dict)
+{
+	uint32_t channel;
+	int i;
+
+	channel = -1;
+
+	for (i = 0 ; i < NUM(values) ; i++) {
+		if (values[i].flags != SDP_ATTR_OK)
+			continue;
+
+		switch (values[i].attr) {
+		case SDP_ATTR_PROTOCOL_DESCRIPTOR_LIST:
+			channel = parse_rfcomm_channel(&values[i]);
+			break;
+		}
+	}
+
+	if (channel == -1)
+		return ENOATTR;
+
+	dict->bd_type = BTDEV_HF;
+	dict->bd_hf.hf_listen = 1;
+	dict->bd_hf.hf_channel = channel;
+	return 0;
+}
+
+/*
+ * Parse [additional] protocol descriptor list for L2CAP PSM
+ *
+ * seq8 len8				2
+ *	seq8 len8			2
+ *		uuid16 value16		3	L2CAP
+ *		uint16 value16		3	PSM
+ *	seq8 len8			2
+ *		uuid16 value16		3	HID Protocol
+ *				      ===
+ *				       15
+ */
+
+static int32_t
+parse_l2cap_psm(sdp_attr_t *a)
+{
+	uint8_t	*ptr = a->value;
+	uint8_t	*end = a->value + a->vlen;
+	int32_t	 type, len, uuid, psm;
+
+	if (end - ptr < 15)
+		return (-1);
+
+	if (a->attr == SDP_ATTR_ADDITIONAL_PROTOCOL_DESCRIPTOR_LISTS) {
+		SDP_GET8(type, ptr);
+		switch (type) {
+		case SDP_DATA_SEQ8:
+			SDP_GET8(len, ptr);
+			break;
+
+		case SDP_DATA_SEQ16:
+			SDP_GET16(len, ptr);
+			break;
+
+		case SDP_DATA_SEQ32:
+			SDP_GET32(len, ptr);
+			break;
+
+		default:
+			return (-1);
+		}
+		if (ptr + len > end)
+			return (-1);
+	}
+
+	SDP_GET8(type, ptr);
+	switch (type) {
+	case SDP_DATA_SEQ8:
+		SDP_GET8(len, ptr);
+		break;
+
+	case SDP_DATA_SEQ16:
+		SDP_GET16(len, ptr);
+		break;
+
+	case SDP_DATA_SEQ32:
+		SDP_GET32(len, ptr);
+		break;
+
+	default:
+		return (-1);
+	}
+	if (ptr + len > end)
+		return (-1);
+
+	/* Protocol */
+	SDP_GET8(type, ptr);
+	switch (type) {
+	case SDP_DATA_SEQ8:
+		SDP_GET8(len, ptr);
+		break;
+
+	case SDP_DATA_SEQ16:
+		SDP_GET16(len, ptr);
+		break;
+
+	case SDP_DATA_SEQ32:
+		SDP_GET32(len, ptr);
+		break;
+
+	default:
+		return (-1);
+	}
+	if (ptr + len > end)
+		return (-1);
+
+	/* UUID */
+	if (ptr + 3 > end)
+		return (-1);
+	SDP_GET8(type, ptr);
+	switch (type) {
+	case SDP_DATA_UUID16:
+		SDP_GET16(uuid, ptr);
+		if (uuid != SDP_UUID_PROTOCOL_L2CAP)
+			return (-1);
+		break;
+
+	case SDP_DATA_UUID32:  /* XXX FIXME can we have 32-bit UUID */
+	case SDP_DATA_UUID128: /* XXX FIXME can we have 128-bit UUID */
+	default:
+		return (-1);
+	}
+
+	/* PSM */
+	if (ptr + 3 > end)
+		return (-1);
+	SDP_GET8(type, ptr);
+	if (type != SDP_DATA_UINT16)
+		return (-1);
+	SDP_GET16(psm, ptr);
+
+	return (psm);
+}
+
+/*
+ * Parse HID descriptor string
+ *
+ * seq8 len8			2
+ *	seq8 len8		2
+ *		uint8 value8	2
+ *		str value	3
+ *			      ===
+ *			        9
+ */
+
+static int32_t
+parse_hid_descriptor(sdp_attr_t *a)
+{
+	uint8_t	*ptr = a->value;
+	uint8_t	*end = a->value + a->vlen;
+	int32_t	 type, len, descriptor_type;
+
+	if (end - ptr < 9)
+		return (-1);
+
+	SDP_GET8(type, ptr);
+	switch (type) {
+	case SDP_DATA_SEQ8:
+		SDP_GET8(len, ptr);
+		break;
+
+	case SDP_DATA_SEQ16:
+		SDP_GET16(len, ptr);
+		break;
+
+	case SDP_DATA_SEQ32:
+		SDP_GET32(len, ptr);
+		break;
+
+	default:
+		return (-1);
+	}
+	if (ptr + len > end)
+		return (-1);
+
+	while (ptr < end) {
+		/* Descriptor */
+		SDP_GET8(type, ptr);
+		switch (type) {
+		case SDP_DATA_SEQ8:
+			if (ptr + 1 > end)
+				return (-1);
+			SDP_GET8(len, ptr);
+			break;
+
+		case SDP_DATA_SEQ16:
+			if (ptr + 2 > end)
+				return (-1);
+			SDP_GET16(len, ptr);
+			break;
+
+		case SDP_DATA_SEQ32:
+			if (ptr + 4 > end)
+				return (-1);
+			SDP_GET32(len, ptr);
+			break;
+
+		default:
+			return (-1);
+		}
+
+		/* Descripor type */
+		if (ptr + 1 > end)
+			return (-1);
+		SDP_GET8(type, ptr);
+		if (type != SDP_DATA_UINT8 || ptr + 1 > end)
+			return (-1);
+		SDP_GET8(descriptor_type, ptr);
+
+		/* Descriptor value */
+		if (ptr + 1 > end)
+			return (-1);
+		SDP_GET8(type, ptr);
+		switch (type) {
+		case SDP_DATA_STR8:
+			if (ptr + 1 > end)
+				return (-1);
+			SDP_GET8(len, ptr);
+			break;
+
+		case SDP_DATA_STR16:
+			if (ptr + 2 > end)
+				return (-1);
+			SDP_GET16(len, ptr);
+			break;
+
+		case SDP_DATA_STR32:
+			if (ptr + 4 > end)
+				return (-1);
+			SDP_GET32(len, ptr);
+			break;
+
+		default:
+			return (-1);
+		}
+		if (ptr + len > end)
+			return (-1);
+
+		if (descriptor_type == UDESC_REPORT && len > 0) {
+			a->value = ptr;
+			a->vlen = len;
+
+			return (0);
+		}
+
+		ptr += len;
+	}
+
+	return (-1);
+}
+
+/*
+ * Parse boolean value
+ *
+ * bool8 int8
+ */
+
+static int32_t
+parse_boolean(sdp_attr_t *a)
+{
+	if (a->vlen != 2 || a->value[0] != SDP_DATA_BOOL)
+		return (-1);
+
+	return (a->value[1]);
+}
+
+/*
+ * Parse protocol descriptor list for the RFCOMM channel
+ *
+ * seq8 len8				2
+ *	seq8 len8			2
+ *		uuid16 value16		3	L2CAP
+ *	seq8 len8			2
+ *		uuid16 value16		3	RFCOMM
+ *		uint8 value8		2	channel
+ *				      ===
+ *				       14
+ */
+
+static int32_t
+parse_rfcomm_channel(sdp_attr_t *a)
+{
+	uint8_t	*ptr = a->value;
+	uint8_t	*end = a->value + a->vlen;
+	int32_t	 type, len, uuid, channel;
+
+	if (end - ptr < 14)
+		return (-1);
+
+	SDP_GET8(type, ptr);
+	switch (type) {
+	case SDP_DATA_SEQ8:
+		SDP_GET8(len, ptr);
+		break;
+
+	case SDP_DATA_SEQ16:
+		SDP_GET16(len, ptr);
+		break;
+
+	case SDP_DATA_SEQ32:
+		SDP_GET32(len, ptr);
+		break;
+
+	default:
+		return (-1);
+	}
+	if (ptr + len > end)
+		return (-1);
+
+	/* Protocol */
+	SDP_GET8(type, ptr);
+	switch (type) {
+	case SDP_DATA_SEQ8:
+		SDP_GET8(len, ptr);
+		break;
+
+	case SDP_DATA_SEQ16:
+		SDP_GET16(len, ptr);
+		break;
+
+	case SDP_DATA_SEQ32:
+		SDP_GET32(len, ptr);
+		break;
+
+	default:
+		return (-1);
+	}
+	if (ptr + len > end)
+		return (-1);
+
+	/* UUID */
+	if (ptr + 3 > end)
+		return (-1);
+	SDP_GET8(type, ptr);
+	switch (type) {
+	case SDP_DATA_UUID16:
+		SDP_GET16(uuid, ptr);
+		if (uuid != SDP_UUID_PROTOCOL_L2CAP)
+			return (-1);
+		break;
+
+	case SDP_DATA_UUID32:  /* XXX FIXME can we have 32-bit UUID */
+	case SDP_DATA_UUID128: /* XXX FIXME can we have 128-bit UUID */
+	default:
+		return (-1);
+	}
+
+	/* Protocol */
+	SDP_GET8(type, ptr);
+	switch (type) {
+	case SDP_DATA_SEQ8:
+		SDP_GET8(len, ptr);
+		break;
+
+	case SDP_DATA_SEQ16:
+		SDP_GET16(len, ptr);
+		break;
+
+	case SDP_DATA_SEQ32:
+		SDP_GET32(len, ptr);
+		break;
+
+	default:
+		return (-1);
+	}
+	if (ptr + len > end)
+		return (-1);
+
+	/* UUID */
+	if (ptr + 3 > end)
+		return (-1);
+	SDP_GET8(type, ptr);
+	switch (type) {
+	case SDP_DATA_UUID16:
+		SDP_GET16(uuid, ptr);
+		if (uuid != SDP_UUID_PROTOCOL_RFCOMM)
+			return (-1);
+		break;
+
+	case SDP_DATA_UUID32:  /* XXX FIXME can we have 32-bit UUID */
+	case SDP_DATA_UUID128: /* XXX FIXME can we have 128-bit UUID */
+	default:
+		return (-1);
+	}
+
+	/* channel */
+	if (ptr + 2 > end)
+		return (-1);
+
+	SDP_GET8(type, ptr);
+	if (type != SDP_DATA_UINT8)
+		return (-1);
+
+	SDP_GET8(channel, ptr);
+
+	return (channel);
+}
+
+/*
+ * return appropriate mode for HID descriptor
+ */
+static int
+hid_mode(uint8_t *desc, int32_t dlen)
+{
+	report_desc_t r;
+	hid_data_t d;
+	struct hid_item h;
+	int mode;
+
+	mode = BTDEV_MODE_AUTH;	/* default */
+
+	r = hid_use_report_desc(desc, dlen);
+	if (r == NULL)
+		err(EXIT_FAILURE, "hid_use_report_desc");
+
+	d = hid_start_parse(r, ~0, -1);
+	while (hid_get_item(d, &h) > 0) {
+		if (h.kind == hid_collection
+		    && HID_PAGE(h.usage) == HUP_GENERIC_DESKTOP
+		    && HID_USAGE(h.usage) == HUG_KEYBOARD)
+			mode = BTDEV_MODE_ENCRYPT;
+	}
+
+	hid_end_parse(d);
+	hid_dispose_report_desc(r);
+
+	return mode;
+}
diff --git a/usr.sbin/btd/sdp_search.c b/usr.sbin/btd/sdp_search.c
new file mode 100644
index 00000000000..a0ca443510f
--- /dev/null
+++ b/usr.sbin/btd/sdp_search.c
@@ -0,0 +1,422 @@
+/*	$NetBSD: search.c,v 1.3 2007/12/15 16:03:29 perry Exp $	*/
+
+/*
+ * search.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: sdp_search.c,v 1.1 2008/11/26 21:48:30 uwe Exp $
+ * $FreeBSD: src/lib/libsdp/search.c,v 1.8 2007/11/16 15:13:12 emax Exp $
+ */
+
+#include <sys/types.h>
+#include <sys/uio.h>
+#include <netbt/bluetooth.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <errno.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "sdp.h"
+
+int32_t
+sdp_search(void *xss,
+		uint32_t plen, uint16_t const *pp,
+		uint32_t alen, uint32_t const *ap,
+		uint32_t vlen, sdp_attr_t *vp)
+{
+	struct sdp_xpdu {
+		sdp_pdu_t		 pdu;
+		uint16_t		 len;
+	} __packed	 xpdu;
+
+	struct sdp_session		*ss = xss;
+	uint8_t				*req = NULL, *rsp = NULL, *rsp_tmp = NULL;
+	int32_t				 t, len;
+	uint16_t			 lo, hi;
+
+	if (ss == NULL)
+		return (-1);
+
+	if (ss->req == NULL || ss->rsp == NULL ||
+	    plen == 0 || pp == NULL || alen == 0 || ap == NULL) {
+		ss->error = EINVAL;
+		return (-1);
+	}
+
+	req = ss->req;
+
+	/* Calculate ServiceSearchPattern length */
+	plen = plen * (sizeof(pp[0]) + 1);
+
+	/* Calculate AttributeIDList length */
+	for (len = 0, t = 0; t < alen; t ++) {
+		lo = (uint16_t) (ap[t] >> 16);
+		hi = (uint16_t) (ap[t]);
+
+		if (lo > hi) {
+			ss->error = EINVAL;
+			return (-1);
+		}
+
+		if (lo != hi)
+			len += (sizeof(ap[t]) + 1);
+		else
+			len += (sizeof(lo) + 1);
+	}
+	alen = len;
+
+	/* Calculate length of the request */
+	len =	plen + sizeof(uint8_t) + sizeof(uint16_t) +
+			/* ServiceSearchPattern */
+		sizeof(uint16_t) +
+			/* MaximumAttributeByteCount */
+		alen + sizeof(uint8_t) + sizeof(uint16_t);
+			/* AttributeIDList */
+
+	if (ss->req_e - req < len) {
+		ss->error = ENOBUFS;
+		return (-1);
+	}
+
+	/* Put ServiceSearchPattern */
+	SDP_PUT8(SDP_DATA_SEQ16, req);
+	SDP_PUT16(plen, req);
+	for (; plen > 0; pp ++, plen -= (sizeof(pp[0]) + 1)) {
+		SDP_PUT8(SDP_DATA_UUID16, req);
+		SDP_PUT16(*pp, req);
+	}
+
+	/* Put MaximumAttributeByteCount */
+	SDP_PUT16(0xffff, req);
+
+	/* Put AttributeIDList */
+	SDP_PUT8(SDP_DATA_SEQ16, req);
+	SDP_PUT16(alen, req);
+	for (; alen > 0; ap ++) {
+		lo = (uint16_t) (*ap >> 16);
+		hi = (uint16_t) (*ap);
+
+		if (lo != hi) {
+			/* Put attribute range */
+			SDP_PUT8(SDP_DATA_UINT32, req);
+			SDP_PUT32(*ap, req);
+			alen -= (sizeof(ap[0]) + 1);
+		} else {
+			/* Put attribute */
+			SDP_PUT8(SDP_DATA_UINT16, req);
+			SDP_PUT16(lo, req);
+			alen -= (sizeof(lo) + 1);
+		}
+	}
+
+	/* Submit ServiceSearchAttributeRequest and wait for response */
+	ss->cslen = 0;
+	rsp = ss->rsp;
+
+	do {
+		struct iovec	 iov[2];
+		uint8_t		*req_cs = req;
+
+		/* Add continuation state (if any) */
+		if (ss->req_e - req_cs < ss->cslen + 1) {
+			ss->error = ENOBUFS;
+			return (-1);
+		}
+
+		SDP_PUT8(ss->cslen, req_cs);
+		if (ss->cslen > 0) {
+			memcpy(req_cs, ss->cs, ss->cslen);
+			req_cs += ss->cslen;
+		}
+
+		/* Prepare SDP PDU header */
+		xpdu.pdu.pid = SDP_PDU_SERVICE_SEARCH_ATTRIBUTE_REQUEST;
+		xpdu.pdu.tid = htons(ss->tid);
+		xpdu.pdu.len = htons(req_cs - ss->req);
+
+		/* Submit request */
+		iov[0].iov_base = (void *) &xpdu;
+		iov[0].iov_len = sizeof(xpdu.pdu);
+		iov[1].iov_base = (void *) ss->req;
+		iov[1].iov_len = req_cs - ss->req;
+
+		do {
+			len = writev(ss->fd, iov, sizeof(iov)/sizeof(iov[0]));
+		} while (len < 0 && errno == EINTR);
+
+		if (len < 0) {
+			ss->error = errno;
+			return (-1);
+		}
+
+		/* Read response */
+		iov[0].iov_base = (void *) &xpdu;
+		iov[0].iov_len = sizeof(xpdu);
+		iov[1].iov_base = (void *) rsp;
+		iov[1].iov_len = ss->imtu;
+
+		do {
+			len = readv(ss->fd, iov, sizeof(iov)/sizeof(iov[0]));
+		} while (len < 0 && errno == EINTR);
+
+		if (len < 0) {
+			ss->error = errno;
+			return (-1);
+		}
+		if (len < sizeof(xpdu)) {
+			ss->error = ENOMSG;
+			return (-1);
+		}
+
+		xpdu.pdu.tid = ntohs(xpdu.pdu.tid);
+		xpdu.pdu.len = ntohs(xpdu.pdu.len);
+		xpdu.len = ntohs(xpdu.len);
+
+		if (xpdu.pdu.pid == SDP_PDU_ERROR_RESPONSE ||
+		    xpdu.pdu.tid != ss->tid ||
+		    xpdu.pdu.len > len ||
+		    xpdu.len > xpdu.pdu.len) {
+			ss->error = EIO;
+			return (-1);
+		}
+
+		rsp += xpdu.len;
+		ss->tid ++;
+
+		/* Save continuation state (if any) */
+		ss->cslen = rsp[0];
+		if (ss->cslen > 0) {
+			if (ss->cslen > sizeof(ss->cs)) {
+				ss->error = ENOBUFS;
+				return (-1);
+			}
+
+			memcpy(ss->cs, rsp + 1, ss->cslen);
+
+			/*
+			 * Ensure that we always have ss->imtu bytes
+			 * available in the ss->rsp buffer
+			 */
+
+			if (ss->rsp_e - rsp <= ss->imtu) {
+				uint32_t	 size, offset;
+
+				size = ss->rsp_e - ss->rsp + ss->imtu;
+				offset = rsp - ss->rsp;
+
+				rsp_tmp = realloc(ss->rsp, size);
+				if (rsp_tmp == NULL) {
+					ss->error = ENOMEM;
+					return (-1);
+				}
+
+				ss->rsp = rsp_tmp;
+				ss->rsp_e = ss->rsp + size;
+				rsp = ss->rsp + offset;
+			}
+		}
+	} while (ss->cslen > 0);
+
+	/*
+	 * If we got here then we have completed SDP transaction and now
+	 * we must populate attribute values into vp array. At this point
+	 * ss->rsp points to the beginning of the response and rsp points
+	 * to the end of the response.
+	 *
+	 * From Bluetooth v1.1 spec page 364
+	 *
+	 * The AttributeLists is a data element sequence where each element
+	 * in turn is a data element sequence representing an attribute list.
+	 * Each attribute list contains attribute IDs and attribute values
+	 * from one service record. The first element in each attribute list
+	 * contains the attribute ID of the first attribute to be returned for
+	 * that service record. The second element in each attribute list
+	 * contains the corresponding attribute value. Successive pairs of
+	 * elements in each attribute list contain additional attribute ID
+	 * and value pairs. Only attributes that have non-null values within
+	 * the service record and whose attribute IDs were specified in the
+	 * SDP_ServiceSearchAttributeRequest are contained in the AttributeLists
+	 * Neither an attribute ID nor attribute value is placed in
+	 * AttributeLists for attributes in the service record that have no
+	 * value. Within each attribute list, the attributes are listed in
+	 * ascending order of attribute ID value.
+	 */
+
+	if (vp == NULL)
+		goto done;
+
+	rsp_tmp = ss->rsp;
+
+	/* Skip the first SEQ */
+	SDP_GET8(t, rsp_tmp);
+	switch (t) {
+	case SDP_DATA_SEQ8:
+		SDP_GET8(len, rsp_tmp);
+		break;
+
+	case SDP_DATA_SEQ16:
+		SDP_GET16(len, rsp_tmp);
+		break;
+
+	case SDP_DATA_SEQ32:
+		SDP_GET32(len, rsp_tmp);
+		break;
+
+	default:
+		ss->error = ENOATTR;
+		return (-1);
+		/* NOT REACHED */
+	}
+
+	for (; rsp_tmp < rsp && vlen > 0; ) {
+		/* Get set of attributes for the next record */
+		SDP_GET8(t, rsp_tmp);
+		switch (t) {
+		case SDP_DATA_SEQ8:
+			SDP_GET8(len, rsp_tmp);
+			break;
+
+		case SDP_DATA_SEQ16:
+			SDP_GET16(len, rsp_tmp);
+			break;
+
+		case SDP_DATA_SEQ32:
+			SDP_GET32(len, rsp_tmp);
+			break;
+
+		default:
+			ss->error = ENOATTR;
+			return (-1);
+			/* NOT REACHED */
+		}
+
+		/* Now rsp_tmp points to list of (attr,value) pairs */
+		for (; len > 0 && vlen > 0; vp ++, vlen --) {
+			/* Attribute */
+			SDP_GET8(t, rsp_tmp);
+			if (t != SDP_DATA_UINT16) {
+				ss->error = ENOATTR;
+				return (-1);
+			}
+			SDP_GET16(vp->attr, rsp_tmp);
+
+			/* Attribute value */
+			switch (rsp_tmp[0]) {
+			case SDP_DATA_NIL:
+				alen = 0;
+				break;
+
+			case SDP_DATA_UINT8:
+			case SDP_DATA_INT8:
+			case SDP_DATA_BOOL:
+				alen = sizeof(uint8_t);
+				break;
+
+			case SDP_DATA_UINT16:
+			case SDP_DATA_INT16:
+			case SDP_DATA_UUID16:
+				alen = sizeof(uint16_t);
+				break;
+
+			case SDP_DATA_UINT32:
+			case SDP_DATA_INT32:
+			case SDP_DATA_UUID32:
+				alen = sizeof(uint32_t);
+				break;
+
+			case SDP_DATA_UINT64:
+			case SDP_DATA_INT64:
+				alen = sizeof(uint64_t);
+				break;
+
+			case SDP_DATA_UINT128:
+			case SDP_DATA_INT128:
+			case SDP_DATA_UUID128:
+				alen = sizeof(uint128_t);
+				break;
+
+			case SDP_DATA_STR8:
+			case SDP_DATA_URL8:
+			case SDP_DATA_SEQ8:
+			case SDP_DATA_ALT8:
+				alen = rsp_tmp[1] + sizeof(uint8_t);
+				break;
+
+			case SDP_DATA_STR16:
+			case SDP_DATA_URL16:
+			case SDP_DATA_SEQ16:
+			case SDP_DATA_ALT16:
+				alen =	  ((uint16_t)rsp_tmp[1] << 8)
+					| ((uint16_t)rsp_tmp[2]);
+				alen += sizeof(uint16_t);
+				break;
+
+			case SDP_DATA_STR32:
+			case SDP_DATA_URL32:
+			case SDP_DATA_SEQ32:
+			case SDP_DATA_ALT32:
+				alen =    ((uint32_t)rsp_tmp[1] << 24)
+					| ((uint32_t)rsp_tmp[2] << 16)
+					| ((uint32_t)rsp_tmp[3] <<  8)
+					| ((uint32_t)rsp_tmp[4]);
+				alen += sizeof(uint32_t);
+				break;
+
+			default:
+				ss->error = ENOATTR;
+				return (-1);
+				/* NOT REACHED */
+			}
+
+			alen += sizeof(uint8_t);
+
+			if (vp->value != NULL) {
+				if (alen <= vp->vlen) {
+					vp->flags = SDP_ATTR_OK;
+					vp->vlen = alen;
+				} else
+					vp->flags = SDP_ATTR_TRUNCATED;
+
+				memcpy(vp->value, rsp_tmp, vp->vlen);
+			} else
+				vp->flags = SDP_ATTR_INVALID;
+
+			len -=	(
+				sizeof(uint8_t) + sizeof(uint16_t) +
+				alen
+				);
+
+			rsp_tmp += alen;
+		}
+	}
+done:
+	ss->error = 0;
+
+	return (0);
+}