Integrate sdpd natively into hcid without any library tricks

6 files changed, 2320 insertions, 10 deletions

diff --git a/hcid/Makefile.am b/hcid/Makefile.am
index 857fff0c..e9d93a30 100644
--- a/hcid/Makefile.am
+++ b/hcid/Makefile.am
@@ -13,9 +13,11 @@ statedir = $(localstatedir)/lib/bluetooth
 state_DATA =
 endif
 
-noinst_LIBRARIES = libhciserver.a
+sbin_PROGRAMS = hcid
 
-libhciserver_a_SOURCES = hcid.h security.c storage.c \
+hcid_SOURCES = main.c hcid.h sdpd.h \
+	sdpd-server.c sdpd-request.c sdpd-service.c \
+	sdpd-database.c security.c storage.c \
 	parser.h parser.y lexer.l kword.c kword.h \
 	server.h server.c manager.h manager.c \
 	adapter.h adapter.c device.h device.c plugin.h plugin.c \
@@ -24,13 +26,7 @@ libhciserver_a_SOURCES = hcid.h security.c storage.c \
 	dbus-hci.h dbus-hci.c \
 	telephony.h telephony.c agent.h agent.c
 
-sbin_PROGRAMS = hcid
-
-hcid_SOURCES = main.c
-
-hcid_LDADD = libhciserver.a \
-	$(top_builddir)/sdpd/libsdpserver.a \
-	$(top_builddir)/common/libhelper.a \
+hcid_LDADD = $(top_builddir)/common/libhelper.a \
 	@GDBUS_LIBS@ @GMODULE_LIBS@ @GLIB_LIBS@ @DBUS_LIBS@ @BLUEZ_LIBS@
 
 if MAINTAINER_MODE
@@ -43,7 +39,7 @@ AM_CFLAGS = @BLUEZ_CFLAGS@ @DBUS_CFLAGS@ \
 		@GLIB_CFLAGS@ @GMODULE_CFLAGS@ @GDBUS_CFLAGS@ \
 					-DPLUGINDIR=\""$(plugindir)"\"
 
-INCLUDES = -I$(top_srcdir)/common -I$(top_srcdir)/sdpd
+INCLUDES = -I$(top_srcdir)/common
 
 BUILT_SOURCES = parser.h
 
diff --git a/hcid/sdpd-database.c b/hcid/sdpd-database.c
new file mode 100644
index 00000000..6cc34bd3
--- /dev/null
+++ b/hcid/sdpd-database.c
@@ -0,0 +1,304 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2001-2002  Nokia Corporation
+ *  Copyright (C) 2002-2003  Maxim Krasnyansky <maxk@qualcomm.com>
+ *  Copyright (C) 2002-2008  Marcel Holtmann <marcel@holtmann.org>
+ *  Copyright (C) 2002-2003  Stephen Crane <steve.crane@rococosoft.com>
+ *
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <stdlib.h>
+#include <sys/socket.h>
+
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/l2cap.h>
+#include <bluetooth/sdp.h>
+#include <bluetooth/sdp_lib.h>
+
+#include "sdpd.h"
+#include "logging.h"
+
+static sdp_list_t *service_db;
+static sdp_list_t *access_db;
+
+typedef struct {
+	uint32_t handle;
+	bdaddr_t device;
+} sdp_access_t;
+
+/*
+ * Ordering function called when inserting a service record.
+ * The service repository is a linked list in sorted order
+ * and the service record handle is the sort key
+ */
+static int record_sort(const void *r1, const void *r2)
+{
+	const sdp_record_t *rec1 = (const sdp_record_t *) r1;
+	const sdp_record_t *rec2 = (const sdp_record_t *) r2;
+
+	if (!rec1 || !rec2) {
+		error("NULL RECORD LIST FATAL");
+		return -1;
+	}
+
+	return rec1->handle - rec2->handle;
+}
+
+static int access_sort(const void *r1, const void *r2)
+{
+	const sdp_access_t *rec1 = (const sdp_access_t *) r1;
+	const sdp_access_t *rec2 = (const sdp_access_t *) r2;
+
+	if (!rec1 || !rec2) {
+		error("NULL RECORD LIST FATAL");
+		return -1;
+	}
+
+	return rec1->handle - rec2->handle;
+}
+
+static void access_free(void *p)
+{
+	free(p);
+}
+
+/*
+ * Reset the service repository by deleting its contents
+ */
+void sdp_svcdb_reset()
+{
+	sdp_list_free(service_db, (sdp_free_func_t) sdp_record_free);
+	sdp_list_free(access_db, access_free);
+}
+
+typedef struct _indexed {
+	int sock;
+	sdp_record_t *record;
+} sdp_indexed_t;
+
+static sdp_list_t *socket_index;
+
+/*
+ * collect all services registered over this socket
+ */
+void sdp_svcdb_collect_all(int sock)
+{
+	sdp_list_t *p, *q;
+
+	for (p = socket_index, q = 0; p; ) {
+		sdp_indexed_t *item = (sdp_indexed_t *) p->data;
+		if (item->sock == sock) {
+			sdp_list_t *next = p->next;
+			sdp_record_remove(item->record->handle);
+			sdp_record_free(item->record);
+			free(item);
+			if (q)
+				q->next = next;
+			else
+				socket_index = next;
+			free(p);
+			p = next;
+		} else if (item->sock > sock)
+			return;
+		else {
+			q = p;
+			p = p->next;
+		}
+	}
+}
+
+void sdp_svcdb_collect(sdp_record_t *rec)
+{
+	sdp_list_t *p, *q;
+
+	for (p = socket_index, q = 0; p; q = p, p = p->next) {
+		sdp_indexed_t *item = (sdp_indexed_t *) p->data;
+		if (rec == item->record) {
+			free(item);
+			if (q)
+				q->next = p->next;
+			else
+				socket_index = p->next;
+			free(p);
+			return;
+		}
+	}
+}
+
+static int compare_indices(const void *i1, const void *i2)
+{
+	const sdp_indexed_t *s1 = (const sdp_indexed_t *) i1;
+	const sdp_indexed_t *s2 = (const sdp_indexed_t *) i2;
+	return s1->sock - s2->sock;
+}
+
+void sdp_svcdb_set_collectable(sdp_record_t *record, int sock)
+{
+	sdp_indexed_t *item = malloc(sizeof(sdp_indexed_t));
+	item->sock = sock;
+	item->record = record;
+	socket_index = sdp_list_insert_sorted(socket_index, item, compare_indices);
+}
+
+/*
+ * Add a service record to the repository
+ */
+void sdp_record_add(bdaddr_t *device, sdp_record_t *rec)
+{
+	sdp_access_t *dev;
+
+	debug("Adding rec : 0x%lx", (long) rec);
+	debug("with handle : 0x%x", rec->handle);
+
+	service_db = sdp_list_insert_sorted(service_db, rec, record_sort);
+
+	dev = malloc(sizeof(*dev));
+	if (!dev)
+		return;
+
+	bacpy(&dev->device, device);
+	dev->handle = rec->handle;
+
+	access_db = sdp_list_insert_sorted(access_db, dev, access_sort);
+}
+
+static sdp_list_t *record_locate(uint32_t handle)
+{
+	if (service_db) {
+		sdp_list_t *p;
+		sdp_record_t r;
+
+		r.handle = handle;
+		p = sdp_list_find(service_db, &r, record_sort);
+		return p;
+	}
+
+	debug("Could not find svcRec for : 0x%x", handle);
+	return NULL;
+}
+
+static sdp_list_t *access_locate(uint32_t handle)
+{
+	if (access_db) {
+		sdp_list_t *p;
+		sdp_access_t a;
+
+		a.handle = handle;
+		p = sdp_list_find(access_db, &a, access_sort);
+		return p;
+	}
+
+	debug("Could not find access data for : 0x%x", handle);
+	return NULL;
+}
+
+/*
+ * Given a service record handle, find the record associated with it.
+ */
+sdp_record_t *sdp_record_find(uint32_t handle)
+{
+	sdp_list_t *p = record_locate(handle);
+
+        if (!p) {
+		debug("Couldn't find record for : 0x%x", handle);
+		return 0;
+	}
+
+	return (sdp_record_t *) p->data;
+}
+
+/*
+ * Given a service record handle, remove its record from the repository
+ */
+int sdp_record_remove(uint32_t handle)
+{
+	sdp_list_t *p = record_locate(handle);
+	sdp_record_t *r;
+	sdp_access_t *a;
+
+	if (!p) {
+		error("Remove : Couldn't find record for : 0x%x", handle);
+		return -1;
+	}
+
+	r = (sdp_record_t *) p->data;
+	if (r)
+		service_db = sdp_list_remove(service_db, r);
+
+	p = access_locate(handle);
+	if (p) {
+		a = (sdp_access_t *) p->data;
+		if (a) {
+			access_db = sdp_list_remove(access_db, a);
+			access_free(a);
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * Return a pointer to the linked list containing the records in sorted order
+ */
+sdp_list_t *sdp_get_record_list(void)
+{
+	return service_db;
+}
+
+sdp_list_t *sdp_get_access_list(void)
+{
+	return access_db;
+}
+
+int sdp_check_access(uint32_t handle, bdaddr_t *device)
+{
+	sdp_list_t *p = access_locate(handle);
+	sdp_access_t *a;
+
+	if (!p)
+		return 1;
+
+	a = (sdp_access_t *) p->data;
+	if (!a)
+		return 1;
+
+	if (bacmp(&a->device, device) &&
+			bacmp(&a->device, BDADDR_ANY) &&
+			bacmp(device, BDADDR_ANY))
+		return 0;
+
+	return 1;
+}
+
+uint32_t sdp_next_handle(void)
+{
+	uint32_t handle = 0x10000;
+
+	while (sdp_record_find(handle))
+		handle++;
+
+	return handle;
+}
diff --git a/hcid/sdpd-request.c b/hcid/sdpd-request.c
new file mode 100644
index 00000000..ece8cd54
--- /dev/null
+++ b/hcid/sdpd-request.c
@@ -0,0 +1,960 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2001-2002  Nokia Corporation
+ *  Copyright (C) 2002-2003  Maxim Krasnyansky <maxk@qualcomm.com>
+ *  Copyright (C) 2002-2008  Marcel Holtmann <marcel@holtmann.org>
+ *  Copyright (C) 2002-2003  Stephen Crane <steve.crane@rococosoft.com>
+ *
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <stdlib.h>
+#include <string.h>
+#include <limits.h>
+#include <sys/socket.h>
+
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/l2cap.h>
+#include <bluetooth/sdp.h>
+#include <bluetooth/sdp_lib.h>
+
+#include <netinet/in.h>
+
+#include "sdpd.h"
+#include "logging.h"
+
+#define MIN(x, y) ((x) < (y)) ? (x): (y)
+
+typedef struct _sdp_cstate_list sdp_cstate_list_t;
+
+struct _sdp_cstate_list {
+	sdp_cstate_list_t *next;
+	uint32_t timestamp;
+	sdp_buf_t buf;
+};
+
+static sdp_cstate_list_t *cstates;
+
+// FIXME: should probably remove it when it's found
+sdp_buf_t *sdp_get_cached_rsp(sdp_cont_state_t *cstate)
+{
+	sdp_cstate_list_t *p;
+
+	for (p = cstates; p; p = p->next)
+		if (p->timestamp == cstate->timestamp)
+			return &p->buf;
+	return 0;
+}
+
+static uint32_t sdp_cstate_alloc_buf(sdp_buf_t *buf)
+{
+	sdp_cstate_list_t *cstate = malloc(sizeof(sdp_cstate_list_t));
+	uint8_t *data = malloc(buf->data_size);
+
+	memcpy(data, buf->data, buf->data_size);
+	memset((char *)cstate, 0, sizeof(sdp_cstate_list_t));
+	cstate->buf.data = data;
+	cstate->buf.data_size = buf->data_size;
+	cstate->buf.buf_size = buf->data_size;
+	cstate->timestamp = sdp_get_time();
+	cstate->next = cstates;
+	cstates = cstate;
+	return cstate->timestamp;
+}
+
+/* Additional values for checking datatype (not in spec) */
+#define SDP_TYPE_UUID	0xfe
+#define SDP_TYPE_ANY	0xff
+
+/*
+ * Generic data element sequence extractor. Builds
+ * a list whose elements are those found in the 
+ * sequence. The data type of elements found in the
+ * sequence is returned in the reference pDataType
+ */
+static int extract_des(uint8_t *buf, int len, sdp_list_t **svcReqSeq, uint8_t *pDataType, uint8_t expectedType)
+{
+	uint8_t seqType;
+	int scanned, data_size = 0;
+	short numberOfElements = 0;
+	int seqlen = 0;
+	sdp_list_t *pSeq = NULL;
+	uint8_t dataType;
+	int status = 0;
+	const uint8_t *p;
+	int bufsize;
+
+	scanned = sdp_extract_seqtype_safe(buf, len, &seqType, &data_size);
+
+	debug("Seq type : %d", seqType);
+	if (!scanned || (seqType != SDP_SEQ8 && seqType != SDP_SEQ16)) {
+		error("Unknown seq type");
+		return -1;
+	}
+	p = buf + scanned;
+	bufsize = len - scanned;
+
+	debug("Data size : %d", data_size);
+
+	for (;;) {
+		char *pElem = NULL;
+		int localSeqLength = 0;
+
+		if (bufsize < sizeof(uint8_t)) {
+			debug("->Unexpected end of buffer");
+			return -1;
+		}
+
+		dataType = *(uint8_t *)p;
+		debug("Data type: 0x%02x", dataType);
+
+		if (expectedType == SDP_TYPE_UUID) {
+			if (dataType != SDP_UUID16 && dataType != SDP_UUID32 && dataType != SDP_UUID128) {
+				debug("->Unexpected Data type (expected UUID_ANY)");
+				return -1;
+			}
+		} else if (expectedType != SDP_TYPE_ANY && dataType != expectedType) {
+			debug("->Unexpected Data type (expected 0x%02x)", expectedType);
+			return -1;
+		}
+
+		switch (dataType) {
+		case SDP_UINT16:
+			p += sizeof(uint8_t);
+			seqlen += sizeof(uint8_t);
+			bufsize -= sizeof(uint8_t);
+			if (bufsize < sizeof(uint16_t)) {
+				debug("->Unexpected end of buffer");
+				return -1;
+			}
+
+			pElem = malloc(sizeof(uint16_t));
+			bt_put_unaligned(ntohs(bt_get_unaligned((uint16_t *)p)), (uint16_t *)pElem);
+			p += sizeof(uint16_t);
+			seqlen += sizeof(uint16_t);
+			bufsize -= sizeof(uint16_t);
+			break;
+		case SDP_UINT32:
+			p += sizeof(uint8_t);
+			seqlen += sizeof(uint8_t);
+			bufsize -= sizeof(uint8_t);
+			if (bufsize < (int)sizeof(uint32_t)) {
+				debug("->Unexpected end of buffer");
+				return -1;
+			}
+
+			pElem = malloc(sizeof(uint32_t));
+			bt_put_unaligned(ntohl(bt_get_unaligned((uint32_t *)p)), (uint32_t *)pElem);
+			p += sizeof(uint32_t);
+			seqlen += sizeof(uint32_t);
+			bufsize -= sizeof(uint32_t);
+			break;
+		case SDP_UUID16:
+		case SDP_UUID32:
+		case SDP_UUID128:
+			pElem = malloc(sizeof(uuid_t));
+			status = sdp_uuid_extract_safe(p, bufsize, (uuid_t *) pElem, &localSeqLength);
+			if (status == 0) {
+				seqlen += localSeqLength;
+				p += localSeqLength;
+				bufsize -= localSeqLength;
+			}
+			break;
+		default:
+			return -1;
+		}
+		if (status == 0) {
+			pSeq = sdp_list_append(pSeq, pElem);
+			numberOfElements++;
+			debug("No of elements : %d", numberOfElements);
+
+			if (seqlen == data_size)
+				break;
+			else if (seqlen > data_size || seqlen > len)
+				return -1;
+		} else
+			free(pElem);
+	}
+	*svcReqSeq = pSeq;
+	scanned += seqlen;
+	*pDataType = dataType;
+	return scanned;
+}
+
+static int sdp_set_cstate_pdu(sdp_buf_t *buf, sdp_cont_state_t *cstate)
+{
+	uint8_t *pdata = buf->data + buf->data_size;
+	int length = 0;
+
+	if (cstate) {
+		debug("Non null sdp_cstate_t id : 0x%lx", cstate->timestamp);
+		*(uint8_t *)pdata = sizeof(sdp_cont_state_t);
+		pdata += sizeof(uint8_t);
+		length += sizeof(uint8_t);
+		memcpy(pdata, cstate, sizeof(sdp_cont_state_t));
+		length += sizeof(sdp_cont_state_t);
+	} else {
+		// set "null" continuation state
+		*(uint8_t *)pdata = 0;
+		pdata += sizeof(uint8_t);
+		length += sizeof(uint8_t);
+	}
+	buf->data_size += length;
+	return length;
+}
+
+static sdp_cont_state_t *sdp_cstate_get(uint8_t *buffer)
+{
+	uint8_t *pdata = buffer;
+	uint8_t cStateSize = *(uint8_t *)pdata;
+
+	/*
+	 * Check if continuation state exists, if yes attempt
+	 * to get response remainder from cache, else send error
+	 */
+	debug("Continuation State size : %d", cStateSize);
+
+	pdata += sizeof(uint8_t);
+	if (cStateSize != 0) {
+		sdp_cont_state_t *cstate = malloc(sizeof(sdp_cont_state_t));
+		if (!cstate)
+			return NULL;
+		memcpy(cstate, (sdp_cont_state_t *)pdata, sizeof(sdp_cont_state_t));
+		debug("Cstate TS : 0x%lx", cstate->timestamp);
+		debug("Bytes sent : %d", cstate->cStateValue.maxBytesSent);
+		return cstate;
+	}
+	return NULL;
+}
+
+/*
+ * The matching process is defined as "each and every UUID
+ * specified in the "search pattern" must be present in the
+ * "target pattern". Here "search pattern" is the set of UUIDs
+ * specified by the service discovery client and "target pattern"
+ * is the set of UUIDs present in a service record. 
+ * 
+ * Return 1 if each and every UUID in the search
+ * pattern exists in the target pattern, 0 if the
+ * match succeeds and -1 on error.
+ */
+static int sdp_match_uuid(sdp_list_t *search, sdp_list_t *pattern)
+{
+	/*
+	 * The target is a sorted list, so we need not look
+	 * at all elements to confirm existence of an element
+	 * from the search pattern
+	 */
+	int patlen = sdp_list_len(pattern);
+
+	if (patlen < sdp_list_len(search))
+		return -1;
+	for (; search; search = search->next) {
+		uuid_t *uuid128;
+		void *data = search->data;
+		sdp_list_t *list;
+		if (data == NULL)
+			return -1;
+
+		// create 128-bit form of the search UUID
+		uuid128 = sdp_uuid_to_uuid128((uuid_t *)data);
+		list = sdp_list_find(pattern, uuid128, sdp_uuid128_cmp);
+		bt_free(uuid128);
+		if (!list)
+			return 0;
+	}
+	return 1;
+}
+
+/*
+ * Service search request PDU. This method extracts the search pattern
+ * (a sequence of UUIDs) and calls the matching function
+ * to find matching services
+ */
+static int service_search_req(sdp_req_t *req, sdp_buf_t *buf)
+{
+	int status = 0, i, plen, mlen, mtu, scanned;
+	sdp_list_t *pattern = NULL;
+	uint16_t expected, actual, rsp_count = 0;
+	uint8_t dtd;
+	sdp_cont_state_t *cstate = NULL;
+	uint8_t *pCacheBuffer = NULL;
+	int handleSize = 0;
+	uint32_t cStateId = 0;
+	short *pTotalRecordCount, *pCurrentRecordCount;
+	uint8_t *pdata = req->buf + sizeof(sdp_pdu_hdr_t);
+
+	scanned = extract_des(pdata, req->len - sizeof(sdp_pdu_hdr_t),
+					&pattern, &dtd, SDP_TYPE_UUID);
+
+	if (scanned == -1) {
+		status = SDP_INVALID_SYNTAX;
+		goto done;
+	}
+	pdata += scanned;
+
+	plen = ntohs(((sdp_pdu_hdr_t *)(req->buf))->plen);
+	mlen = scanned + sizeof(uint16_t) + 1;
+	// ensure we don't read past buffer
+	if (plen < mlen || plen != mlen + *(uint8_t *)(pdata+sizeof(uint16_t))) {
+		status = SDP_INVALID_SYNTAX;
+		goto done;
+	}
+
+	expected = ntohs(bt_get_unaligned((uint16_t *)pdata));
+	
+	debug("Expected count: %d", expected);
+	debug("Bytes scanned : %d", scanned);
+
+	pdata += sizeof(uint16_t);
+
+	/*
+	 * Check if continuation state exists, if yes attempt
+	 * to get rsp remainder from cache, else send error
+	 */
+	cstate = sdp_cstate_get(pdata);
+
+	mtu = req->mtu - sizeof(sdp_pdu_hdr_t) - sizeof(uint16_t) - sizeof(uint16_t) - SDP_CONT_STATE_SIZE;
+	actual = MIN(expected, mtu >> 2);
+
+	/* make space in the rsp buffer for total and current record counts */
+	pdata = buf->data;
+
+	/* total service record count = 0 */
+	pTotalRecordCount = (short *)pdata;
+	bt_put_unaligned(0, (uint16_t *)pdata);
+	pdata += sizeof(uint16_t);
+	buf->data_size += sizeof(uint16_t);
+
+	/* current service record count = 0 */
+	pCurrentRecordCount = (short *)pdata;
+	bt_put_unaligned(0, (uint16_t *)pdata);
+	pdata += sizeof(uint16_t);
+	buf->data_size += sizeof(uint16_t);
+
+	if (cstate == NULL) {
+		/* for every record in the DB, do a pattern search */
+		sdp_list_t *list = sdp_get_record_list();
+
+		handleSize = 0;
+		for (; list && rsp_count < expected; list = list->next) {
+			sdp_record_t *rec = (sdp_record_t *) list->data;
+
+			debug("Checking svcRec : 0x%x", rec->handle);
+				
+			if (sdp_match_uuid(pattern, rec->pattern) > 0 &&
+					sdp_check_access(rec->handle, &req->device)) {
+				rsp_count++;
+				bt_put_unaligned(htonl(rec->handle), (uint32_t *)pdata);
+				pdata += sizeof(uint32_t);
+				handleSize += sizeof(uint32_t);
+			}
+		}
+		
+		debug("Match count: %d", rsp_count);
+
+		buf->data_size += handleSize;
+		bt_put_unaligned(htons(rsp_count), (uint16_t *)pTotalRecordCount);
+		bt_put_unaligned(htons(rsp_count), (uint16_t *)pCurrentRecordCount);
+
+		if (rsp_count > actual) {
+			/* cache the rsp and generate a continuation state */
+			cStateId = sdp_cstate_alloc_buf(buf);
+			/*
+			 * subtract handleSize since we now send only
+			 * a subset of handles
+			 */
+			buf->data_size -= handleSize;
+		} else {
+			/* NULL continuation state */
+			sdp_set_cstate_pdu(buf, NULL);
+		}
+	}
+
+	/* under both the conditions below, the rsp buffer is not built yet */
+	if (cstate || cStateId > 0) {
+		short lastIndex = 0;
+
+		if (cstate) {
+			/*
+			 * Get the previous sdp_cont_state_t and obtain
+			 * the cached rsp
+			 */
+			sdp_buf_t *pCache = sdp_get_cached_rsp(cstate);
+			if (pCache) {
+				pCacheBuffer = pCache->data;
+				/* get the rsp_count from the cached buffer */
+				rsp_count = ntohs(bt_get_unaligned((uint16_t *)pCacheBuffer));
+
+				/* get index of the last sdp_record_t sent */
+				lastIndex = cstate->cStateValue.lastIndexSent;
+			} else {
+				status = SDP_INVALID_CSTATE;
+				goto done;
+			}
+		} else {
+			pCacheBuffer = buf->data;
+			lastIndex = 0;
+		}
+
+		/*
+		 * Set the local buffer pointer to after the
+		 * current record count and increment the cached
+		 * buffer pointer to beyond the counters
+		 */
+		pdata = (uint8_t *) pCurrentRecordCount + sizeof(uint16_t);
+
+		/* increment beyond the totalCount and the currentCount */
+		pCacheBuffer += 2 * sizeof(uint16_t);
+
+		if (cstate) {
+			handleSize = 0;
+			for (i = lastIndex; (i - lastIndex) < actual && i < rsp_count; i++) {
+				bt_put_unaligned(bt_get_unaligned((uint32_t *)(pCacheBuffer + i * sizeof(uint32_t))), (uint32_t *)pdata);
+				pdata += sizeof(uint32_t);
+				handleSize += sizeof(uint32_t);
+			}
+		} else {
+			handleSize = actual << 2;
+			i = actual;
+		}
+
+		buf->data_size += handleSize;
+		bt_put_unaligned(htons(rsp_count), (uint16_t *)pTotalRecordCount);
+		bt_put_unaligned(htons(i - lastIndex), (uint16_t *)pCurrentRecordCount);
+
+		if (i == rsp_count) {
+			/* set "null" continuationState */
+			sdp_set_cstate_pdu(buf, NULL);
+		} else {
+			/*
+			 * there's more: set lastIndexSent to
+			 * the new value and move on
+			 */
+			sdp_cont_state_t newState;
+
+			debug("Setting non-NULL sdp_cstate_t");
+
+			if (cstate)
+				memcpy((char *)&newState, cstate, sizeof(sdp_cont_state_t));
+			else {
+				memset((char *)&newState, 0, sizeof(sdp_cont_state_t));
+				newState.timestamp = cStateId;
+			}
+			newState.cStateValue.lastIndexSent = i;
+			sdp_set_cstate_pdu(buf, &newState);
+		}
+	}
+
+done:	
+	if (cstate)
+		free(cstate);
+	if (pattern)
+		sdp_list_free(pattern, free);
+
+	return status;
+}
+
+/*
+ * Extract attribute identifiers from the request PDU.
+ * Clients could request a subset of attributes (by id)
+ * from a service record, instead of the whole set. The
+ * requested identifiers are present in the PDU form of
+ * the request
+ */
+static int extract_attrs(sdp_record_t *rec, sdp_list_t *seq, uint8_t dtd, sdp_buf_t *buf)
+{
+	if (!rec)
+		return SDP_INVALID_RECORD_HANDLE;
+
+	if (seq)
+		debug("Entries in attr seq : %d", sdp_list_len(seq));
+	else
+		debug("NULL attribute descriptor");
+
+	debug("AttrDataType : %d", dtd);
+
+	if (seq == NULL) {
+		debug("Attribute sequence is NULL");
+		return 0;
+	}
+	if (dtd == SDP_UINT16)
+		for (; seq; seq = seq->next) {
+			uint16_t attr = bt_get_unaligned((uint16_t *)seq->data);
+			sdp_data_t *a = (sdp_data_t *)sdp_data_get(rec, attr);
+			if (a)
+				sdp_append_to_pdu(buf, a);
+		}
+	else if (dtd == SDP_UINT32) {
+		sdp_buf_t pdu;
+		sdp_gen_record_pdu(rec, &pdu);
+		for (; seq; seq = seq->next) {
+			uint32_t range = bt_get_unaligned((uint32_t *)seq->data);
+			uint16_t attr;
+			uint16_t low = (0xffff0000 & range) >> 16;
+			uint16_t high = 0x0000ffff & range;
+			sdp_data_t *data;
+
+			debug("attr range : 0x%x", range);
+			debug("Low id : 0x%x", low);
+			debug("High id : 0x%x", high);
+
+			if (low == 0x0000 && high == 0xffff && pdu.data_size <= buf->buf_size) {
+				/* copy it */
+				memcpy(buf->data, pdu.data, pdu.data_size);
+				buf->data_size = pdu.data_size;
+				break;
+			}
+			/* (else) sub-range of attributes */
+			for (attr = low; attr < high; attr++) {
+				data = sdp_data_get(rec, attr);
+				if (data)
+					sdp_append_to_pdu(buf, data);
+			}
+			data = sdp_data_get(rec, high);
+			if (data)
+				sdp_append_to_pdu(buf, data);
+		}
+		free(pdu.data);
+	} else {
+		error("Unexpected data type : 0x%x", dtd);
+		error("Expect uint16_t or uint32_t");
+		return SDP_INVALID_SYNTAX;
+	}
+	return 0;
+}
+
+/*
+ * A request for the attributes of a service record.
+ * First check if the service record (specified by
+ * service record handle) exists, then call the attribute
+ * streaming function
+ */
+static int service_attr_req(sdp_req_t *req, sdp_buf_t *buf)
+{
+	sdp_cont_state_t *cstate = NULL;
+	uint8_t *pResponse = NULL;
+	short cstate_size = 0;
+	sdp_list_t *seq = NULL;
+	uint8_t dtd = 0;
+	int scanned = 0;
+	int max_rsp_size;
+	int status = 0, plen, mlen;
+	uint8_t *pdata = req->buf + sizeof(sdp_pdu_hdr_t);
+	uint32_t handle = ntohl(bt_get_unaligned((uint32_t *)pdata));
+
+	pdata += sizeof(uint32_t);
+	max_rsp_size = ntohs(bt_get_unaligned((uint16_t *)pdata));
+	pdata += sizeof(uint16_t);
+
+	/* extract the attribute list */
+	scanned = extract_des(pdata, req->len - sizeof(sdp_pdu_hdr_t),
+						&seq, &dtd, SDP_TYPE_ANY);
+	if (scanned == -1) {
+		status = SDP_INVALID_SYNTAX;
+		goto done;
+	}
+	pdata += scanned;
+
+	plen = ntohs(((sdp_pdu_hdr_t *)(req->buf))->plen);
+	mlen = scanned + sizeof(uint32_t) + sizeof(uint16_t) + 1;
+	// ensure we don't read past buffer
+	if (plen < mlen || plen != mlen + *(uint8_t *)pdata) {
+		status = SDP_INVALID_SYNTAX;
+		goto done;
+	}
+
+	/*
+	 * if continuation state exists, attempt
+	 * to get rsp remainder from cache, else send error
+	 */
+	cstate = sdp_cstate_get(pdata);
+
+	debug("SvcRecHandle : 0x%x", handle);
+	debug("max_rsp_size : %d", max_rsp_size);
+
+	/* 
+	 * Calculate Attribute size acording to MTU
+	 * We can send only (MTU - sizeof(sdp_pdu_hdr_t) - sizeof(sdp_cont_state_t))
+	 */
+	max_rsp_size = MIN(max_rsp_size, req->mtu - sizeof(sdp_pdu_hdr_t) - 
+			sizeof(uint32_t) - SDP_CONT_STATE_SIZE - sizeof(uint16_t));
+
+	/* pull header for AttributeList byte count */
+	buf->data += sizeof(uint16_t);
+	buf->buf_size -= sizeof(uint16_t);
+
+	if (cstate) {
+		sdp_buf_t *pCache = sdp_get_cached_rsp(cstate);
+
+		debug("Obtained cached rsp : %p", pCache);
+
+		if (pCache) {
+			short sent = MIN(max_rsp_size, pCache->data_size - cstate->cStateValue.maxBytesSent);
+			pResponse = pCache->data;
+			memcpy(buf->data, pResponse + cstate->cStateValue.maxBytesSent, sent);
+			buf->data_size += sent;
+			cstate->cStateValue.maxBytesSent += sent;
+
+			debug("Response size : %d sending now : %d bytes sent so far : %d",
+				pCache->data_size, sent, cstate->cStateValue.maxBytesSent);
+			if (cstate->cStateValue.maxBytesSent == pCache->data_size)
+				cstate_size = sdp_set_cstate_pdu(buf, NULL);
+			else
+				cstate_size = sdp_set_cstate_pdu(buf, cstate);
+		} else {
+			status = SDP_INVALID_CSTATE;
+			error("NULL cache buffer and non-NULL continuation state");
+		}
+	} else {
+		sdp_record_t *rec = sdp_record_find(handle);
+		status = extract_attrs(rec, seq, dtd, buf);
+		if (buf->data_size > max_rsp_size) {
+			sdp_cont_state_t newState;
+
+			memset((char *)&newState, 0, sizeof(sdp_cont_state_t));
+			newState.timestamp = sdp_cstate_alloc_buf(buf);
+			/*
+			 * Reset the buffer size to the maximum expected and
+			 * set the sdp_cont_state_t
+			 */
+			debug("Creating continuation state of size : %d", buf->data_size);
+			buf->data_size = max_rsp_size;
+			newState.cStateValue.maxBytesSent = max_rsp_size;
+			cstate_size = sdp_set_cstate_pdu(buf, &newState);
+		} else {
+			if (buf->data_size == 0)
+				sdp_append_to_buf(buf, 0, 0);
+			cstate_size = sdp_set_cstate_pdu(buf, NULL);
+		}
+	}
+
+	// push header
+	buf->data -= sizeof(uint16_t);
+	buf->buf_size += sizeof(uint16_t);
+
+done:
+	if (cstate)
+		free(cstate);
+	if (seq)
+                sdp_list_free(seq, free);
+	if (status)
+		return status;
+
+	/* set attribute list byte count */
+	bt_put_unaligned(htons(buf->data_size - cstate_size), (uint16_t *)buf->data);
+	buf->data_size += sizeof(uint16_t);
+	return 0;
+}
+
+/*
+ * combined service search and attribute extraction
+ */
+static int service_search_attr_req(sdp_req_t *req, sdp_buf_t *buf)
+{
+	int status = 0, plen, totscanned;
+	uint8_t *pdata, *pResponse = NULL;
+	int scanned, max, rsp_count = 0;
+	sdp_list_t *pattern = NULL, *seq = NULL, *svcList;
+	sdp_cont_state_t *cstate = NULL;
+	short cstate_size = 0;
+	uint8_t dtd = 0;
+	sdp_buf_t tmpbuf;
+
+	tmpbuf.data = NULL;
+	pdata = req->buf + sizeof(sdp_pdu_hdr_t);
+	scanned = extract_des(pdata, req->len - sizeof(sdp_pdu_hdr_t),
+					&pattern, &dtd, SDP_TYPE_UUID);
+	if (scanned == -1) {
+        	status = SDP_INVALID_SYNTAX;
+		goto done;
+	}
+	totscanned = scanned;
+
+	debug("Bytes scanned: %d", scanned);
+
+	pdata += scanned;
+	max = ntohs(bt_get_unaligned((uint16_t *)pdata));
+	pdata += sizeof(uint16_t);
+
+	debug("Max Attr expected: %d", max);
+
+	/* extract the attribute list */
+	scanned = extract_des(pdata, req->len - sizeof(sdp_pdu_hdr_t),
+						&seq, &dtd, SDP_TYPE_ANY);
+	if (scanned == -1) {
+		status = SDP_INVALID_SYNTAX;
+		goto done;
+	}
+	pdata += scanned;
+	totscanned += scanned + sizeof(uint16_t) + 1;
+
+	plen = ntohs(((sdp_pdu_hdr_t *)(req->buf))->plen);
+	if (plen < totscanned || plen != totscanned + *(uint8_t *)pdata) {
+		status = SDP_INVALID_SYNTAX;
+		goto done;
+	}
+
+	/*
+	 * if continuation state exists attempt
+	 * to get rsp remainder from cache, else send error
+	 */
+	cstate = sdp_cstate_get(pdata);	// continuation information
+
+	svcList = sdp_get_record_list();
+
+	tmpbuf.data = malloc(USHRT_MAX);
+	tmpbuf.data_size = 0;
+	tmpbuf.buf_size = USHRT_MAX;
+	memset(tmpbuf.data, 0, USHRT_MAX);
+
+	/* 
+	 * Calculate Attribute size acording to MTU
+	 * We can send only (MTU - sizeof(sdp_pdu_hdr_t) - sizeof(sdp_cont_state_t))
+	 */
+	max = MIN(max, req->mtu - sizeof(sdp_pdu_hdr_t) - SDP_CONT_STATE_SIZE - sizeof(uint16_t));
+
+	/* pull header for AttributeList byte count */
+	buf->data += sizeof(uint16_t);
+	buf->buf_size -= sizeof(uint16_t);
+
+	if (cstate == NULL) {
+		/* no continuation state -> create new response */
+		sdp_list_t *p;
+		for (p = svcList; p; p = p->next) {
+			sdp_record_t *rec = (sdp_record_t *) p->data;
+			if (sdp_match_uuid(pattern, rec->pattern) > 0 &&
+					sdp_check_access(rec->handle, &req->device)) {
+				rsp_count++;
+				status = extract_attrs(rec, seq, dtd, &tmpbuf);
+
+				debug("Response count : %d", rsp_count);
+				debug("Local PDU size : %d", tmpbuf.data_size);
+				if (status) {
+					debug("Extract attr from record returns err");
+					break;
+				}
+				if (buf->data_size + tmpbuf.data_size < buf->buf_size) {
+					// to be sure no relocations
+					sdp_append_to_buf(buf, tmpbuf.data, tmpbuf.data_size);
+					tmpbuf.data_size = 0;
+					memset(tmpbuf.data, 0, USHRT_MAX);
+				} else {
+					error("Relocation needed");
+					break;
+				}
+				debug("Net PDU size : %d", buf->data_size);
+			}
+		}
+		if (buf->data_size > max) {
+			sdp_cont_state_t newState;
+
+			memset((char *)&newState, 0, sizeof(sdp_cont_state_t));
+			newState.timestamp = sdp_cstate_alloc_buf(buf);
+			/*
+			 * Reset the buffer size to the maximum expected and
+			 * set the sdp_cont_state_t
+			 */
+			buf->data_size = max;
+			newState.cStateValue.maxBytesSent = max;
+			cstate_size = sdp_set_cstate_pdu(buf, &newState);
+		} else
+			cstate_size = sdp_set_cstate_pdu(buf, NULL);
+	} else {
+		/* continuation State exists -> get from cache */
+		sdp_buf_t *pCache = sdp_get_cached_rsp(cstate);
+		if (pCache) {
+			uint16_t sent = MIN(max, pCache->data_size - cstate->cStateValue.maxBytesSent);
+			pResponse = pCache->data;
+			memcpy(buf->data, pResponse + cstate->cStateValue.maxBytesSent, sent);
+			buf->data_size += sent;
+			cstate->cStateValue.maxBytesSent += sent;
+			if (cstate->cStateValue.maxBytesSent == pCache->data_size)
+				cstate_size = sdp_set_cstate_pdu(buf, NULL);
+			else
+				cstate_size = sdp_set_cstate_pdu(buf, cstate);
+		} else {
+			status = SDP_INVALID_CSTATE;
+			debug("Non-null continuation state, but null cache buffer");
+		}
+	}
+
+	if (!rsp_count && !cstate) {
+		// found nothing
+		buf->data_size = 0;
+		sdp_append_to_buf(buf, tmpbuf.data, tmpbuf.data_size);
+		sdp_set_cstate_pdu(buf, NULL);
+	}
+
+	// push header
+	buf->data -= sizeof(uint16_t);
+	buf->buf_size += sizeof(uint16_t);
+
+	if (!status) {
+		/* set attribute list byte count */
+		bt_put_unaligned(htons(buf->data_size - cstate_size), (uint16_t *)buf->data);
+		buf->data_size += sizeof(uint16_t);
+	}
+
+done:
+	if (cstate)
+		free(cstate);
+	if (tmpbuf.data)
+		free(tmpbuf.data);
+	if (pattern)
+		sdp_list_free(pattern, free);
+	if (seq)
+                sdp_list_free(seq, free);
+	return status;
+}
+
+/*
+ * Top level request processor. Calls the appropriate processing
+ * function based on request type. Handles service registration
+ * client requests also.
+ */
+static void process_request(sdp_req_t *req)
+{
+	sdp_pdu_hdr_t *reqhdr = (sdp_pdu_hdr_t *)req->buf;
+	sdp_pdu_hdr_t *rsphdr;
+	sdp_buf_t rsp;
+	uint8_t *buf = malloc(USHRT_MAX);
+	int sent = 0;
+	int status = SDP_INVALID_SYNTAX;
+
+	memset(buf, 0, USHRT_MAX);
+	rsp.data = buf + sizeof(sdp_pdu_hdr_t);
+	rsp.data_size = 0;
+	rsp.buf_size = USHRT_MAX - sizeof(sdp_pdu_hdr_t);
+	rsphdr = (sdp_pdu_hdr_t *)buf;
+
+	if (ntohs(reqhdr->plen) != req->len - sizeof(sdp_pdu_hdr_t)) {
+		status = SDP_INVALID_PDU_SIZE;
+		goto send_rsp;
+	}
+	switch (reqhdr->pdu_id) {
+	case SDP_SVC_SEARCH_REQ:
+		debug("Got a svc srch req");
+		status = service_search_req(req, &rsp);
+		rsphdr->pdu_id = SDP_SVC_SEARCH_RSP;
+		break;
+	case SDP_SVC_ATTR_REQ:
+		debug("Got a svc attr req");
+		status = service_attr_req(req, &rsp);
+		rsphdr->pdu_id = SDP_SVC_ATTR_RSP;
+		break;
+	case SDP_SVC_SEARCH_ATTR_REQ:
+		debug("Got a svc srch attr req");
+		status = service_search_attr_req(req, &rsp);
+		rsphdr->pdu_id = SDP_SVC_SEARCH_ATTR_RSP;
+		break;
+	/* Following requests are allowed only for local connections */
+	case SDP_SVC_REGISTER_REQ:
+		debug("Service register request");
+		if (req->local) {
+			status = service_register_req(req, &rsp);
+			rsphdr->pdu_id = SDP_SVC_REGISTER_RSP;
+		}
+		break;
+	case SDP_SVC_UPDATE_REQ:
+		debug("Service update request");
+		if (req->local) {
+			status = service_update_req(req, &rsp);
+			rsphdr->pdu_id = SDP_SVC_UPDATE_RSP;
+		}
+		break;
+	case SDP_SVC_REMOVE_REQ:
+		debug("Service removal request");
+		if (req->local) {
+			status = service_remove_req(req, &rsp);
+			rsphdr->pdu_id = SDP_SVC_REMOVE_RSP;
+		}
+		break;
+	default:
+		error("Unknown PDU ID : 0x%x received", reqhdr->pdu_id);
+		status = SDP_INVALID_SYNTAX;
+		break;
+	}
+
+send_rsp:
+	if (status) {
+		rsphdr->pdu_id = SDP_ERROR_RSP;
+		bt_put_unaligned(htons(status), (uint16_t *)rsp.data);
+		rsp.data_size = sizeof(uint16_t);
+	}
+	
+	debug("Sending rsp. status %d", status);
+
+	rsphdr->tid  = reqhdr->tid;
+	rsphdr->plen = htons(rsp.data_size);
+
+	/* point back to the real buffer start and set the real rsp length */
+	rsp.data_size += sizeof(sdp_pdu_hdr_t);
+	rsp.data = buf;
+
+	/* stream the rsp PDU */
+	sent = send(req->sock, rsp.data, rsp.data_size, 0);
+
+	debug("Bytes Sent : %d", sent);
+
+	free(rsp.data);
+	free(req->buf);
+}
+
+void handle_request(int sk, uint8_t *data, int len)
+{
+	struct sockaddr_l2 sa;
+	socklen_t size;
+	sdp_req_t req;
+
+	size = sizeof(sa);
+	if (getpeername(sk, (struct sockaddr *) &sa, &size) < 0)
+		return;
+
+	if (sa.l2_family == AF_BLUETOOTH) { 
+		struct l2cap_options lo;
+		memset(&lo, 0, sizeof(lo));
+		size = sizeof(lo);
+		getsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &lo, &size);
+		bacpy(&req.bdaddr, &sa.l2_bdaddr);
+		req.mtu = lo.omtu;
+		req.local = 0;
+		memset(&sa, 0, sizeof(sa));
+		size = sizeof(sa);
+		getsockname(sk, (struct sockaddr *) &sa, &size);
+		bacpy(&req.device, &sa.l2_bdaddr);
+	} else {
+		bacpy(&req.device, BDADDR_ANY);
+		bacpy(&req.bdaddr, BDADDR_LOCAL);
+		req.mtu = 2048;
+		req.local = 1;
+	}
+
+	req.sock = sk;
+	req.buf  = data;
+	req.len  = len;
+
+	process_request(&req);
+}
diff --git a/hcid/sdpd-server.c b/hcid/sdpd-server.c
new file mode 100644
index 00000000..1524d1c0
--- /dev/null
+++ b/hcid/sdpd-server.c
@@ -0,0 +1,280 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2001-2002  Nokia Corporation
+ *  Copyright (C) 2002-2003  Maxim Krasnyansky <maxk@qualcomm.com>
+ *  Copyright (C) 2002-2008  Marcel Holtmann <marcel@holtmann.org>
+ *  Copyright (C) 2002-2003  Stephen Crane <steve.crane@rococosoft.com>
+ *
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <sys/stat.h>
+#include <sys/socket.h>
+
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/l2cap.h>
+#include <bluetooth/sdp.h>
+#include <bluetooth/sdp_lib.h>
+
+#include <sys/un.h>
+#include <netinet/in.h>
+
+#include <glib.h>
+
+#include "logging.h"
+#include "sdpd.h"
+
+static GIOChannel *l2cap_io = NULL, *unix_io = NULL;
+
+static int l2cap_sock, unix_sock;
+
+/*
+ * SDP server initialization on startup includes creating the
+ * l2cap and unix sockets over which discovery and registration clients
+ * access us respectively
+ */
+static int init_server(uint16_t mtu, int master, int compat)
+{
+	struct l2cap_options opts;
+	struct sockaddr_l2 l2addr;
+	struct sockaddr_un unaddr;
+	socklen_t optlen;
+
+	/* Register the public browse group root */
+	register_public_browse_group();
+
+	/* Register the SDP server's service record */
+	register_server_service();
+
+	/* Create L2CAP socket */
+	l2cap_sock = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
+	if (l2cap_sock < 0) {
+		error("opening L2CAP socket: %s", strerror(errno));
+		return -1;
+	}
+
+	memset(&l2addr, 0, sizeof(l2addr));
+	l2addr.l2_family = AF_BLUETOOTH;
+	bacpy(&l2addr.l2_bdaddr, BDADDR_ANY);
+	l2addr.l2_psm = htobs(SDP_PSM);
+
+	if (bind(l2cap_sock, (struct sockaddr *) &l2addr, sizeof(l2addr)) < 0) {
+		error("binding L2CAP socket: %s", strerror(errno));
+		return -1;
+	}
+
+	if (master) {
+		int opt = L2CAP_LM_MASTER;
+		if (setsockopt(l2cap_sock, SOL_L2CAP, L2CAP_LM, &opt, sizeof(opt)) < 0) {
+			error("setsockopt: %s", strerror(errno));
+			return -1;
+		}
+	}
+
+	if (mtu > 0) {
+		memset(&opts, 0, sizeof(opts));
+		optlen = sizeof(opts);
+
+		if (getsockopt(l2cap_sock, SOL_L2CAP, L2CAP_OPTIONS, &opts, &optlen) < 0) {
+			error("getsockopt: %s", strerror(errno));
+			return -1;
+		}
+
+		opts.omtu = mtu;
+		opts.imtu = mtu;
+
+		if (setsockopt(l2cap_sock, SOL_L2CAP, L2CAP_OPTIONS, &opts, sizeof(opts)) < 0) {
+			error("setsockopt: %s", strerror(errno));
+			return -1;
+		}
+	}
+
+	listen(l2cap_sock, 5);
+
+	if (!compat) {
+		unix_sock = -1;
+		return 0;
+	}
+
+	/* Create local Unix socket */
+	unix_sock = socket(PF_UNIX, SOCK_STREAM, 0);
+	if (unix_sock < 0) {
+		error("opening UNIX socket: %s", strerror(errno));
+		return -1;
+	}
+
+	memset(&unaddr, 0, sizeof(unaddr));
+	unaddr.sun_family = AF_UNIX;
+	strcpy(unaddr.sun_path, SDP_UNIX_PATH);
+
+	unlink(unaddr.sun_path);
+
+	if (bind(unix_sock, (struct sockaddr *) &unaddr, sizeof(unaddr)) < 0) {
+		error("binding UNIX socket: %s", strerror(errno));
+		return -1;
+	}
+
+	listen(unix_sock, 5);
+
+	chmod(SDP_UNIX_PATH, S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP | S_IROTH | S_IWOTH);
+
+	return 0;
+}
+
+static gboolean io_session_event(GIOChannel *chan, GIOCondition cond, gpointer data)
+{
+	sdp_pdu_hdr_t hdr;
+	uint8_t *buf;
+	int sk, len, size;
+
+	if (cond & G_IO_NVAL)
+		return FALSE;
+
+	sk = g_io_channel_unix_get_fd(chan);
+
+	if (cond & (G_IO_HUP | G_IO_ERR)) {
+		sdp_svcdb_collect_all(sk);
+		return FALSE;
+	}
+
+	len = recv(sk, &hdr, sizeof(sdp_pdu_hdr_t), MSG_PEEK);
+	if (len <= 0) {
+		sdp_svcdb_collect_all(sk);
+		return FALSE;
+	}
+
+	size = sizeof(sdp_pdu_hdr_t) + ntohs(hdr.plen);
+	buf = malloc(size);
+	if (!buf)
+		return TRUE;
+
+	len = recv(sk, buf, size, 0);
+	if (len <= 0) {
+		sdp_svcdb_collect_all(sk);
+		return FALSE;
+	}
+		
+	handle_request(sk, buf, len);
+
+	return TRUE;
+}
+
+static gboolean io_accept_event(GIOChannel *chan, GIOCondition cond, gpointer data)
+{
+	GIOChannel *io;
+	int nsk;
+
+	if (cond & (G_IO_HUP | G_IO_ERR | G_IO_NVAL)) {
+		g_io_channel_unref(chan);
+		return FALSE;
+	}
+
+	if (data == &l2cap_sock) {
+		struct sockaddr_l2 addr;
+		socklen_t len = sizeof(addr);
+
+		nsk = accept(l2cap_sock, (struct sockaddr *) &addr, &len);
+	} else if (data == &unix_sock) {
+		struct sockaddr_un addr;
+		socklen_t len = sizeof(addr);
+
+		nsk = accept(unix_sock, (struct sockaddr *) &addr, &len);
+	} else
+		return FALSE;
+
+	if (nsk < 0) {
+		error("Can't accept connection: %s", strerror(errno));
+		return TRUE;
+	}
+
+	io = g_io_channel_unix_new(nsk);
+	g_io_channel_set_close_on_unref(io, TRUE);
+
+	g_io_add_watch(io, G_IO_IN | G_IO_ERR | G_IO_HUP | G_IO_NVAL,
+					io_session_event, data);
+
+	g_io_channel_unref(io);
+
+	return TRUE;
+}
+
+int start_sdp_server(uint16_t mtu, const char *did, uint32_t flags)
+{
+	int compat = flags & SDP_SERVER_COMPAT;
+	int master = flags & SDP_SERVER_MASTER;
+
+	info("Starting SDP server");
+
+	if (init_server(mtu, master, compat) < 0) {
+		error("Server initialization failed");
+		return -1;
+	}
+
+	if (did && strlen(did) > 0) {
+		const char *ptr = did;
+		uint16_t vid = 0x0000, pid = 0x0000, ver = 0x0000;
+
+		vid = (uint16_t) strtol(ptr, NULL, 16);
+		ptr = strchr(ptr, ':');
+		if (ptr) {
+			pid = (uint16_t) strtol(ptr + 1, NULL, 16);
+			ptr = strchr(ptr + 1, ':');
+			if (ptr)
+				ver = (uint16_t) strtol(ptr + 1, NULL, 16);
+			register_device_id(vid, pid, ver);
+		}
+	}
+
+	l2cap_io = g_io_channel_unix_new(l2cap_sock);
+	g_io_channel_set_close_on_unref(l2cap_io, TRUE);
+
+	g_io_add_watch(l2cap_io, G_IO_IN | G_IO_ERR | G_IO_HUP | G_IO_NVAL,
+					io_accept_event, &l2cap_sock);
+
+	if (compat && unix_sock > fileno(stderr)) {
+		unix_io = g_io_channel_unix_new(unix_sock);
+		g_io_channel_set_close_on_unref(unix_io, TRUE);
+
+		g_io_add_watch(unix_io, G_IO_IN | G_IO_ERR | G_IO_HUP | G_IO_NVAL,
+					io_accept_event, &unix_sock);
+	}
+
+	return 0;
+}
+
+void stop_sdp_server(void)
+{
+	info("Stopping SDP server");
+
+	sdp_svcdb_reset();
+
+	if (unix_io)
+		g_io_channel_unref(unix_io);
+
+	if (l2cap_io)
+		g_io_channel_unref(l2cap_io);
+}
diff --git a/hcid/sdpd-service.c b/hcid/sdpd-service.c
new file mode 100644
index 00000000..09459f43
--- /dev/null
+++ b/hcid/sdpd-service.c
@@ -0,0 +1,677 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2001-2002  Nokia Corporation
+ *  Copyright (C) 2002-2003  Maxim Krasnyansky <maxk@qualcomm.com>
+ *  Copyright (C) 2002-2008  Marcel Holtmann <marcel@holtmann.org>
+ *  Copyright (C) 2002-2003  Stephen Crane <steve.crane@rococosoft.com>
+ *
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <stdlib.h>
+#include <sys/time.h>
+#include <sys/socket.h>
+
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/sdp.h>
+#include <bluetooth/sdp_lib.h>
+
+#include <netinet/in.h>
+
+#include "sdpd.h"
+#include "logging.h"
+
+static sdp_record_t *server = NULL;
+
+static uint8_t service_classes = 0x00;
+static service_classes_callback_t service_classes_callback = NULL;
+
+static uint16_t did_vendor = 0x0000;
+static uint16_t did_product = 0x0000;
+static uint16_t did_version = 0x0000;
+
+/*
+ * List of version numbers supported by the SDP server.
+ * Add to this list when newer versions are supported.
+ */
+static sdp_version_t sdpVnumArray[1] = {
+	{ 1, 0 }
+};
+static const int sdpServerVnumEntries = 1;
+
+/*
+ * A simple function which returns the time of day in
+ * seconds. Used for updating the service db state
+ * attribute of the service record of the SDP server
+ */
+uint32_t sdp_get_time()
+{
+	/*
+	 * To handle failure in gettimeofday, so an old
+	 * value is returned and service does not fail
+	 */
+	static struct timeval tm;
+
+	gettimeofday(&tm, NULL);
+	return (uint32_t) tm.tv_sec;
+}
+
+/*
+ * The service database state is an attribute of the service record
+ * of the SDP server itself. This attribute is guaranteed to
+ * change if any of the contents of the service repository
+ * changes. This function updates the timestamp of value of
+ * the svcDBState attribute
+ * Set the SDP server DB. Simply a timestamp which is the marker
+ * when the DB was modified.
+ */
+static void update_db_timestamp(void)
+{
+	uint32_t dbts = sdp_get_time();
+	sdp_data_t *d = sdp_data_alloc(SDP_UINT32, &dbts);
+	sdp_attr_replace(server, SDP_ATTR_SVCDB_STATE, d);
+}
+
+static void update_svclass_list(void)
+{
+	sdp_list_t *list = sdp_get_record_list();
+	uint8_t val = 0;
+
+	for (; list; list = list->next) {
+		sdp_record_t *rec = (sdp_record_t *) list->data;
+
+		if (rec->svclass.type != SDP_UUID16)
+			continue;
+
+		switch (rec->svclass.value.uuid16) {
+		case DIALUP_NET_SVCLASS_ID:
+		case CIP_SVCLASS_ID:
+			val |= 0x42;	/* Telephony & Networking */
+			break;
+		case IRMC_SYNC_SVCLASS_ID:
+		case OBEX_OBJPUSH_SVCLASS_ID:
+		case OBEX_FILETRANS_SVCLASS_ID:
+		case IRMC_SYNC_CMD_SVCLASS_ID:
+		case PBAP_PSE_SVCLASS_ID:
+			val |= 0x10;	/* Object Transfer */
+			break;
+		case HEADSET_SVCLASS_ID:
+		case HANDSFREE_SVCLASS_ID:
+			val |= 0x20;	/* Audio */
+			break;
+		case CORDLESS_TELEPHONY_SVCLASS_ID:
+		case INTERCOM_SVCLASS_ID:
+		case FAX_SVCLASS_ID:
+		case SAP_SVCLASS_ID:
+			val |= 0x40;	/* Telephony */
+			break;
+		case AUDIO_SOURCE_SVCLASS_ID:
+		case VIDEO_SOURCE_SVCLASS_ID:
+			val |= 0x08;	/* Capturing */
+			break;
+		case AUDIO_SINK_SVCLASS_ID:
+		case VIDEO_SINK_SVCLASS_ID:
+			val |= 0x04;	/* Rendering */
+			break;
+		case PANU_SVCLASS_ID:
+		case NAP_SVCLASS_ID:
+		case GN_SVCLASS_ID:
+			val |= 0x02;	/* Networking */
+			break;
+		}
+	}
+
+	debug("Service classes 0x%02x", val);
+
+	service_classes = val;
+
+	if (service_classes_callback)
+		service_classes_callback(BDADDR_ANY, val);
+}
+
+uint8_t get_service_classes(const bdaddr_t *bdaddr)
+{
+	return service_classes;
+}
+
+void set_service_classes_callback(service_classes_callback_t callback)
+{
+	service_classes_callback = callback;
+}
+
+void create_ext_inquiry_response(const char *name, uint8_t *data)
+{
+	sdp_list_t *list = sdp_get_record_list();
+	uint8_t *ptr = data;
+	uint16_t uuid[24];
+	int i, index = 0;
+
+	if (name) {
+		int len = strlen(name);
+
+		if (len > 48) {
+			len = 48;
+			ptr[1] = 0x08;
+		} else
+			ptr[1] = 0x09;
+
+		ptr[0] = len + 1;
+
+		memcpy(ptr + 2, name, len);
+
+		ptr += len + 2;
+	}
+
+	if (did_vendor != 0x0000) {
+		uint16_t source = 0x0002;
+		*ptr++ = 9;
+		*ptr++ = 11;
+		*ptr++ = (source & 0x00ff);
+		*ptr++ = (source & 0xff00) >> 8;
+		*ptr++ = (did_vendor & 0x00ff);
+		*ptr++ = (did_vendor & 0xff00) >> 8;
+		*ptr++ = (did_product & 0x00ff);
+		*ptr++ = (did_product & 0xff00) >> 8;
+		*ptr++ = (did_version & 0x00ff);
+		*ptr++ = (did_version & 0xff00) >> 8;
+	}
+
+	ptr[1] = 0x03;
+
+	for (; list; list = list->next) {
+		sdp_record_t *rec = (sdp_record_t *) list->data;
+
+		if (rec->svclass.type != SDP_UUID16)
+			continue;
+
+		if (rec->svclass.value.uuid16 < 0x1100)
+			continue;
+
+		if (index > 23) {
+			ptr[1] = 0x02;
+			break;
+		}
+
+		for (i = 0; i < index; i++)
+			if (uuid[i] == rec->svclass.value.uuid16)
+				break;
+
+		if (i == index - 1)
+			continue;
+
+		uuid[index++] = rec->svclass.value.uuid16;
+	}
+
+	if (index > 0) {
+		ptr[0] = (index * 2) + 1;
+		ptr += 2;
+
+		for (i = 0; i < index; i++) {
+			*ptr++ = (uuid[i] & 0x00ff);
+			*ptr++ = (uuid[i] & 0xff00) >> 8;
+		}
+	}
+}
+
+void register_public_browse_group(void)
+{
+	sdp_list_t *browselist;
+	uuid_t bgscid, pbgid;
+	sdp_data_t *sdpdata;
+	sdp_record_t *browse = sdp_record_alloc();
+
+	browse->handle = SDP_SERVER_RECORD_HANDLE + 1;
+
+	sdp_record_add(BDADDR_ANY, browse);
+	sdpdata = sdp_data_alloc(SDP_UINT32, &browse->handle);
+	sdp_attr_add(browse, SDP_ATTR_RECORD_HANDLE, sdpdata);
+
+	sdp_uuid16_create(&bgscid, BROWSE_GRP_DESC_SVCLASS_ID);
+	browselist = sdp_list_append(0, &bgscid);
+	sdp_set_service_classes(browse, browselist);
+	sdp_list_free(browselist, 0);
+
+	sdp_uuid16_create(&pbgid, PUBLIC_BROWSE_GROUP);
+	sdp_attr_add_new(browse, SDP_ATTR_GROUP_ID,
+				SDP_UUID16, &pbgid.value.uuid16);
+}
+
+/*
+ * The SDP server must present its own service record to
+ * the service repository. This can be accessed by service
+ * discovery clients. This method constructs a service record
+ * and stores it in the repository
+ */
+void register_server_service(void)
+{
+	sdp_list_t *classIDList;
+	uuid_t classID;
+	void **versions, **versionDTDs;
+	uint8_t dtd;
+	sdp_data_t *pData;
+	int i;
+
+	server = sdp_record_alloc();
+	server->pattern = NULL;
+
+	/* Force the record to be SDP_SERVER_RECORD_HANDLE */
+	server->handle = SDP_SERVER_RECORD_HANDLE;
+
+	sdp_record_add(BDADDR_ANY, server);
+	sdp_attr_add(server, SDP_ATTR_RECORD_HANDLE,
+				sdp_data_alloc(SDP_UINT32, &server->handle));
+
+	sdp_uuid16_create(&classID, SDP_SERVER_SVCLASS_ID);
+	classIDList = sdp_list_append(0, &classID);
+	sdp_set_service_classes(server, classIDList);
+	sdp_list_free(classIDList, 0);
+
+	/*
+	 * Set the version numbers supported, these are passed as arguments
+	 * to the server on command line. Now defaults to 1.0
+	 * Build the version number sequence first
+	 */
+	versions = (void **)malloc(sdpServerVnumEntries * sizeof(void *));
+	versionDTDs = (void **)malloc(sdpServerVnumEntries * sizeof(void *));
+	dtd = SDP_UINT16;
+	for (i = 0; i < sdpServerVnumEntries; i++) {
+		uint16_t *version = malloc(sizeof(uint16_t));
+		*version = sdpVnumArray[i].major;
+		*version = (*version << 8);
+		*version |= sdpVnumArray[i].minor;
+		versions[i] = version;
+		versionDTDs[i] = &dtd;
+	}
+	pData = sdp_seq_alloc(versionDTDs, versions, sdpServerVnumEntries);
+	for (i = 0; i < sdpServerVnumEntries; i++)
+		free(versions[i]);
+	free(versions);
+	free(versionDTDs);
+	sdp_attr_add(server, SDP_ATTR_VERSION_NUM_LIST, pData);
+
+	update_db_timestamp();
+	update_svclass_list();
+}
+
+void register_device_id(const uint16_t vendor, const uint16_t product,
+						const uint16_t version)
+{
+	const uint16_t spec = 0x0102, source = 0x0002;
+	const uint8_t primary = 1;
+	sdp_list_t *class_list, *group_list, *profile_list;
+	uuid_t class_uuid, group_uuid;
+	sdp_data_t *sdp_data, *primary_data, *source_data;
+	sdp_data_t *spec_data, *vendor_data, *product_data, *version_data;
+	sdp_profile_desc_t profile;
+	sdp_record_t *record = sdp_record_alloc();
+
+	info("Adding device id record for %04x:%04x", vendor, product);
+
+	did_vendor = vendor;
+	did_product = product;
+	did_version = version;
+
+	record->handle = sdp_next_handle();
+
+	sdp_record_add(BDADDR_ANY, record);
+	sdp_data = sdp_data_alloc(SDP_UINT32, &record->handle);
+	sdp_attr_add(record, SDP_ATTR_RECORD_HANDLE, sdp_data);
+
+	sdp_uuid16_create(&class_uuid, PNP_INFO_SVCLASS_ID);
+	class_list = sdp_list_append(0, &class_uuid);
+	sdp_set_service_classes(record, class_list);
+	sdp_list_free(class_list, NULL);
+
+	sdp_uuid16_create(&group_uuid, PUBLIC_BROWSE_GROUP);
+	group_list = sdp_list_append(NULL, &group_uuid);
+	sdp_set_browse_groups(record, group_list);
+	sdp_list_free(group_list, NULL);
+
+	sdp_uuid16_create(&profile.uuid, PNP_INFO_PROFILE_ID);
+	profile.version = spec;
+	profile_list = sdp_list_append(NULL, &profile);
+	sdp_set_profile_descs(record, profile_list);
+	sdp_list_free(profile_list, NULL);
+
+	spec_data = sdp_data_alloc(SDP_UINT16, &spec);
+	sdp_attr_add(record, 0x0200, spec_data);
+
+	vendor_data = sdp_data_alloc(SDP_UINT16, &vendor);
+	sdp_attr_add(record, 0x0201, vendor_data);
+
+	product_data = sdp_data_alloc(SDP_UINT16, &product);
+	sdp_attr_add(record, 0x0202, product_data);
+
+	version_data = sdp_data_alloc(SDP_UINT16, &version);
+	sdp_attr_add(record, 0x0203, version_data);
+
+	primary_data = sdp_data_alloc(SDP_BOOL, &primary);
+	sdp_attr_add(record, 0x0204, primary_data);
+
+	source_data = sdp_data_alloc(SDP_UINT16, &source);
+	sdp_attr_add(record, 0x0205, source_data);
+
+	update_db_timestamp();
+	update_svclass_list();
+}
+
+int add_record_to_server(bdaddr_t *src, sdp_record_t *rec)
+{
+	sdp_data_t *data;
+
+	if (rec->handle == 0xffffffff) {
+		rec->handle = sdp_next_handle();
+		if (rec->handle < 0x10000)
+			return -1;
+	} else {
+		if (sdp_record_find(rec->handle))
+			return -1; 
+	}
+
+	debug("Adding record with handle 0x%05x", rec->handle);
+
+	sdp_record_add(src, rec);
+
+	data = sdp_data_alloc(SDP_UINT32, &rec->handle);
+	sdp_attr_replace(rec, SDP_ATTR_RECORD_HANDLE, data);
+
+	if (sdp_data_get(rec, SDP_ATTR_BROWSE_GRP_LIST) == NULL) {
+		uuid_t uuid;
+		sdp_uuid16_create(&uuid, PUBLIC_BROWSE_GROUP);
+		sdp_pattern_add_uuid(rec, &uuid);
+	}
+
+	update_db_timestamp();
+	update_svclass_list();
+
+	return 0;
+}
+
+int remove_record_from_server(uint32_t handle)
+{
+	sdp_record_t *rec;
+
+	debug("Removing record with handle 0x%05x", handle);
+
+	rec = sdp_record_find(handle);
+	if (!rec)
+		return -ENOENT;
+
+	if (sdp_record_remove(handle) == 0) {
+		update_db_timestamp();
+		update_svclass_list();
+	}
+
+	sdp_record_free(rec);
+
+	return 0;
+}
+
+// FIXME: refactor for server-side
+static sdp_record_t *extract_pdu_server(bdaddr_t *device, uint8_t *p, int bufsize, uint32_t handleExpected, int *scanned)
+{
+	int extractStatus = -1, localExtractedLength = 0;
+	uint8_t dtd;
+	int seqlen = 0;
+	sdp_record_t *rec = NULL;
+	uint16_t attrId, lookAheadAttrId;
+	sdp_data_t *pAttr = NULL;
+	uint32_t handle = 0xffffffff;
+
+	*scanned = sdp_extract_seqtype_safe(p, bufsize, &dtd, &seqlen);
+	p += *scanned;
+	bufsize -= *scanned;
+
+	if (bufsize < sizeof(uint8_t) + sizeof(uint8_t)) {
+		debug("Unexpected end of packet");
+		return NULL;
+	}
+
+	lookAheadAttrId = ntohs(bt_get_unaligned((uint16_t *) (p + sizeof(uint8_t))));
+
+	debug("Look ahead attr id : %d", lookAheadAttrId);
+
+	if (lookAheadAttrId == SDP_ATTR_RECORD_HANDLE) {
+		if (bufsize < (sizeof(uint8_t) * 2) + sizeof(uint16_t) + sizeof(uint32_t)) {
+			debug("Unexpected end of packet");
+			return NULL;
+		}
+		handle = ntohl(bt_get_unaligned((uint32_t *) (p +
+				sizeof(uint8_t) + sizeof(uint16_t) +
+				sizeof(uint8_t))));
+		debug("SvcRecHandle : 0x%x", handle);
+		rec = sdp_record_find(handle);
+	} else if (handleExpected != 0xffffffff)
+		rec = sdp_record_find(handleExpected);
+
+	if (!rec) {
+		rec = sdp_record_alloc();
+		rec->attrlist = NULL;
+		if (lookAheadAttrId == SDP_ATTR_RECORD_HANDLE) {
+			rec->handle = handle;
+			sdp_record_add(device, rec);
+		} else if (handleExpected != 0xffffffff) {
+			rec->handle = handleExpected;
+			sdp_record_add(device, rec);
+		}
+	} else {
+		sdp_list_free(rec->attrlist, (sdp_free_func_t) sdp_data_free);
+		rec->attrlist = NULL;
+	}
+
+	while (localExtractedLength < seqlen) {
+		int attrSize = sizeof(uint8_t);
+		int attrValueLength = 0;
+
+		if (bufsize < attrSize + sizeof(uint16_t)) {
+			debug("Unexpected end of packet: Terminating extraction of attributes");
+			break;
+		}
+
+		debug("Extract PDU, sequenceLength: %d localExtractedLength: %d", seqlen, localExtractedLength);
+		dtd = *(uint8_t *) p;
+
+		attrId = ntohs(bt_get_unaligned((uint16_t *) (p + attrSize)));
+		attrSize += sizeof(uint16_t);
+		
+		debug("DTD of attrId : %d Attr id : 0x%x", dtd, attrId);
+
+		pAttr = sdp_extract_attr_safe(p + attrSize, bufsize - attrSize,
+							&attrValueLength, rec);
+
+		debug("Attr id : 0x%x attrValueLength : %d", attrId, attrValueLength);
+
+		attrSize += attrValueLength;
+		if (pAttr == NULL) {
+			debug("Terminating extraction of attributes");
+			break;
+		}
+		localExtractedLength += attrSize;
+		p += attrSize;
+		bufsize -= attrSize;
+		sdp_attr_replace(rec, attrId, pAttr);
+		extractStatus = 0;
+		debug("Extract PDU, seqLength: %d localExtractedLength: %d",
+					seqlen, localExtractedLength);
+	}
+
+	if (extractStatus == 0) {
+		debug("Successful extracting of Svc Rec attributes");
+#ifdef SDP_DEBUG
+		sdp_print_service_attr(rec->attrlist);
+#endif
+		*scanned += seqlen;
+	}
+	return rec;
+}
+
+/*
+ * Add the newly created service record to the service repository
+ */
+int service_register_req(sdp_req_t *req, sdp_buf_t *rsp)
+{
+	int scanned = 0;
+	sdp_data_t *handle;
+	uint8_t *p = req->buf + sizeof(sdp_pdu_hdr_t);
+	int bufsize = req->len - sizeof(sdp_pdu_hdr_t);
+	sdp_record_t *rec;
+
+	req->flags = *p++;
+	if (req->flags & SDP_DEVICE_RECORD) {
+		bacpy(&req->device, (bdaddr_t *) p);
+		p += sizeof(bdaddr_t);
+		bufsize -= sizeof(bdaddr_t);
+	}
+
+	// save image of PDU: we need it when clients request this attribute
+	rec = extract_pdu_server(&req->device, p, bufsize, 0xffffffff, &scanned);
+	if (!rec)
+		goto invalid;
+
+	if (rec->handle == 0xffffffff) {
+		rec->handle = sdp_next_handle();
+		if (rec->handle < 0x10000) {
+			sdp_record_free(rec);
+			goto invalid;
+		}
+	} else {
+		if (sdp_record_find(rec->handle)) {
+			/* extract_pdu_server will add the record handle
+			 * if it is missing. So instead of failing, skip
+			 * the record adding to avoid duplication. */
+			goto success;
+		}
+	}
+
+	sdp_record_add(&req->device, rec);
+	if (!(req->flags & SDP_RECORD_PERSIST))
+		sdp_svcdb_set_collectable(rec, req->sock);
+
+	handle = sdp_data_alloc(SDP_UINT32, &rec->handle);
+	sdp_attr_replace(rec, SDP_ATTR_RECORD_HANDLE, handle);
+
+success:
+	/* if the browse group descriptor is NULL,
+	 * ensure that the record belongs to the ROOT group */
+	if (sdp_data_get(rec, SDP_ATTR_BROWSE_GRP_LIST) == NULL) {
+		 uuid_t uuid;
+		 sdp_uuid16_create(&uuid, PUBLIC_BROWSE_GROUP);
+		 sdp_pattern_add_uuid(rec, &uuid);
+	}
+
+	update_db_timestamp();
+	update_svclass_list();
+
+	/* Build a rsp buffer */
+	bt_put_unaligned(htonl(rec->handle), (uint32_t *) rsp->data);
+	rsp->data_size = sizeof(uint32_t);
+
+	return 0;
+
+invalid:
+	bt_put_unaligned(htons(SDP_INVALID_SYNTAX), (uint16_t *) rsp->data);
+	rsp->data_size = sizeof(uint16_t);
+
+	return -1;
+}
+
+/*
+ * Update a service record
+ */
+int service_update_req(sdp_req_t *req, sdp_buf_t *rsp)
+{
+	sdp_record_t *orec;
+	int status = 0, scanned = 0;
+	uint8_t *p = req->buf + sizeof(sdp_pdu_hdr_t);
+	int bufsize = req->len - sizeof(sdp_pdu_hdr_t);
+	uint32_t handle = ntohl(bt_get_unaligned((uint32_t *) p));
+
+	debug("Svc Rec Handle: 0x%x", handle);
+
+	p += sizeof(uint32_t);
+	bufsize -= sizeof(uint32_t);
+
+	orec = sdp_record_find(handle);
+
+	debug("SvcRecOld: %p", orec);
+
+	if (orec) {
+		sdp_record_t *nrec = extract_pdu_server(BDADDR_ANY, p, bufsize, handle, &scanned);
+		if (nrec && handle == nrec->handle) {
+			update_db_timestamp();
+			update_svclass_list();
+		} else {
+			debug("SvcRecHandle : 0x%x", handle);
+			debug("SvcRecHandleNew : 0x%x", nrec->handle);
+			debug("SvcRecNew : %p", nrec);
+			debug("SvcRecOld : %p", orec);
+			debug("Failure to update, restore old value");
+
+			if (nrec)
+				sdp_record_free(nrec);
+			status = SDP_INVALID_SYNTAX;
+		}
+	} else
+		status = SDP_INVALID_RECORD_HANDLE;
+
+	p = rsp->data;
+	bt_put_unaligned(htons(status), (uint16_t *) p);
+	rsp->data_size = sizeof(uint16_t);
+	return status;
+}
+
+/*
+ * Remove a registered service record
+ */
+int service_remove_req(sdp_req_t *req, sdp_buf_t *rsp)
+{
+	uint8_t *p = req->buf + sizeof(sdp_pdu_hdr_t);
+	uint32_t handle = ntohl(bt_get_unaligned((uint32_t *) p));
+	sdp_record_t *rec;
+	int status = 0;
+
+	/* extract service record handle */
+	p += sizeof(uint32_t);
+
+	rec = sdp_record_find(handle);
+	if (rec) {
+		sdp_svcdb_collect(rec);
+		status = sdp_record_remove(handle);
+		sdp_record_free(rec);
+		if (status == 0) {
+			update_db_timestamp();
+			update_svclass_list();
+		}
+	} else {
+		status = SDP_INVALID_RECORD_HANDLE;
+		debug("Could not find record : 0x%x", handle);
+	}
+
+	p = rsp->data;
+	bt_put_unaligned(htons(status), (uint16_t *) p);
+	rsp->data_size = sizeof(uint16_t);
+
+	return status;
+}
diff --git a/hcid/sdpd.h b/hcid/sdpd.h
new file mode 100644
index 00000000..332b434d
--- /dev/null
+++ b/hcid/sdpd.h
@@ -0,0 +1,93 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2001-2002  Nokia Corporation
+ *  Copyright (C) 2002-2003  Maxim Krasnyansky <maxk@qualcomm.com>
+ *  Copyright (C) 2002-2008  Marcel Holtmann <marcel@holtmann.org>
+ *  Copyright (C) 2002-2003  Stephen Crane <steve.crane@rococosoft.com>
+ *
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/sdp.h>
+
+typedef struct request {
+	bdaddr_t device;
+	bdaddr_t bdaddr;
+	int      local;
+	int      sock;
+	int      mtu;
+	int      flags;
+	uint8_t  *buf;
+	int      len;
+} sdp_req_t;
+
+void handle_request(int sk, uint8_t *data, int len);
+
+int service_register_req(sdp_req_t *req, sdp_buf_t *rsp);
+int service_update_req(sdp_req_t *req, sdp_buf_t *rsp);
+int service_remove_req(sdp_req_t *req, sdp_buf_t *rsp);
+
+void register_public_browse_group(void);
+void register_server_service(void);
+void register_device_id(const uint16_t vendor, const uint16_t product,
+						const uint16_t version);
+
+typedef struct {
+	uint32_t timestamp;
+	union {
+		uint16_t maxBytesSent;
+		uint16_t lastIndexSent;
+	} cStateValue;
+} sdp_cont_state_t;
+
+#define SDP_CONT_STATE_SIZE (sizeof(uint8_t) + sizeof(sdp_cont_state_t))
+
+sdp_buf_t *sdp_get_cached_rsp(sdp_cont_state_t *cstate);
+void sdp_cstate_cache_init(void);
+void sdp_cstate_clean_buf(void);
+
+void sdp_svcdb_reset(void);
+void sdp_svcdb_collect_all(int sock);
+void sdp_svcdb_set_collectable(sdp_record_t *rec, int sock);
+void sdp_svcdb_collect(sdp_record_t *rec);
+sdp_record_t *sdp_record_find(uint32_t handle);
+void sdp_record_add(bdaddr_t *device, sdp_record_t *rec);
+int sdp_record_remove(uint32_t handle);
+sdp_list_t *sdp_get_record_list(void);
+sdp_list_t *sdp_get_access_list(void);
+int sdp_check_access(uint32_t handle, bdaddr_t *device);
+uint32_t sdp_next_handle(void);
+
+uint32_t sdp_get_time();
+
+#define SDP_SERVER_COMPAT (1 << 0)
+#define SDP_SERVER_MASTER (1 << 1)
+
+int start_sdp_server(uint16_t mtu, const char *did, uint32_t flags);
+void stop_sdp_server(void);
+
+int add_record_to_server(bdaddr_t *src, sdp_record_t *rec);
+int remove_record_from_server(uint32_t handle);
+
+typedef void (*service_classes_callback_t) (const bdaddr_t *bdaddr, uint8_t value);
+
+uint8_t get_service_classes(const bdaddr_t *bdaddr);
+void set_service_classes_callback(service_classes_callback_t callback);
+void create_ext_inquiry_response(const char *name, uint8_t *data);