Implement CreateDevice, associated SDP discovery and other necessary changes

4 files changed, 554 insertions, 140 deletions

diff --git a/audio/headset.c b/audio/headset.c
index dbebf67c..b83bd3fe 100644
--- a/audio/headset.c
+++ b/audio/headset.c
@@ -77,12 +77,13 @@ typedef enum {
 } headset_state_t;
 
 struct pending_connect {
-	int ch;
 	DBusMessage *msg;
 	GIOChannel *io;
 };
 
 struct headset {
+	int rfcomm_ch;
+
 	GIOChannel *rfcomm;
 	GIOChannel *sco;
 
@@ -119,52 +120,6 @@ static void pending_connect_free(struct pending_connect *c)
 	g_free(c);
 }
 
-static DBusHandlerResult error_reply(DBusConnection *conn, DBusMessage *msg,
-					const char *name, const char *descr)
-{
-	DBusMessage *derr;
-
-	if (!conn || !msg)
-		return DBUS_HANDLER_RESULT_HANDLED;
-
-	derr = dbus_message_new_error(msg, name, descr);
-	if (!derr) {
-	       	error("Unable to allocate new error return");
-		return DBUS_HANDLER_RESULT_NEED_MEMORY;
-	}
-
-	return send_message_and_unref(conn, derr);
-}
-
-static DBusHandlerResult err_already_connected(DBusConnection *conn, DBusMessage *msg)
-{
-	return error_reply(conn, msg, "org.bluez.audio.Error.AlreadyConnected",
-				"Already connected to a device");
-}
-
-static DBusHandlerResult err_not_connected(DBusConnection *conn, DBusMessage *msg)
-{
-	return error_reply(conn, msg, "org.bluez.audio.Error.NotConnected",
-				"Not connected to any device");
-}
-
-static DBusHandlerResult err_not_supported(DBusConnection *conn, DBusMessage *msg)
-{
-	return error_reply(conn, msg, "org.bluez.audio.Error.NotSupported",
-			"The service is not supported by the remote device");
-}
-
-static DBusHandlerResult err_connect_failed(DBusConnection *conn, DBusMessage *msg, int err)
-{
-	return error_reply(conn, msg, "org.bluez.audio.Error.ConnectFailed",
-				strerror(err));
-}
-
-static DBusHandlerResult err_failed(DBusConnection *conn, DBusMessage *msg)
-{
-	return error_reply(conn, msg, "org.bluez.audio.Error.Failed", "Failed");
-}
-
 static gboolean headset_close_output(struct headset *hs)
 {
 	assert(hs != NULL);
@@ -661,9 +616,10 @@ static gboolean rfcomm_connect_cb(GIOChannel *chan, GIOCondition cond,
 	if (cond & G_IO_NVAL)
 		return FALSE;
 
-	assert(hs != NULL && hs->pending_connect != NULL && 
-			hs->rfcomm == NULL &&
-			hs->state == HEADSET_STATE_CONNECT_IN_PROGRESS);
+	assert(hs != NULL);
+       	assert(hs->pending_connect != NULL);
+	assert(hs->rfcomm == NULL);
+	assert(hs->state == HEADSET_STATE_CONNECT_IN_PROGRESS);
 
 	sk = g_io_channel_unix_get_fd(chan);
 
@@ -731,7 +687,7 @@ static int rfcomm_connect(audio_device_t *device, int *err)
 
 	ba2str(&device->bda, address);
 
-	debug("Connecting to %s channel %d", address, hs->pending_connect->ch);
+	debug("Connecting to %s channel %d", address, hs->rfcomm_ch);
 
 	sk = socket(PF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
 	if (sk < 0) {
@@ -761,7 +717,7 @@ static int rfcomm_connect(audio_device_t *device, int *err)
 	memset(&addr, 0, sizeof(addr));
 	addr.rc_family = AF_BLUETOOTH;
 	bacpy(&addr.rc_bdaddr, &device->bda);
-	addr.rc_channel = hs->pending_connect->ch;
+	addr.rc_channel = hs->rfcomm_ch;
 
 	hs->pending_connect->io = g_io_channel_unix_new(sk);
 	if (!hs->pending_connect->io) {
@@ -941,47 +897,12 @@ int headset_remove_ag_record(uint32_t rec_id)
 	return 0;
 }
 
-static void finish_sdp_transaction(bdaddr_t *dba) 
-{
-	char address[18], *addr_ptr = address;
-	DBusMessage *msg, *reply;
-	DBusError derr;
-
-	ba2str(dba, address);
-
-	msg = dbus_message_new_method_call("org.bluez", "/org/bluez/hci0",
-						"org.bluez.Adapter",
-						"FinishRemoteServiceTransaction");
-	if (!msg) {
-		error("Unable to allocate new method call");
-		return;
-	}
-
-	dbus_message_append_args(msg, DBUS_TYPE_STRING, &addr_ptr,
-					DBUS_TYPE_INVALID);
-
-	dbus_error_init(&derr);
-	reply = dbus_connection_send_with_reply_and_block(connection, msg,
-								-1, &derr);
-
-	dbus_message_unref(msg);
-
-	if (dbus_error_is_set(&derr) || dbus_set_error_from_message(&derr, reply)) {
-		error("FinishRemoteServiceTransaction(%s) failed: %s",
-				address, derr.message);
-		dbus_error_free(&derr);
-		return;
-	}
-
-	dbus_message_unref(reply);
-}
-
 static void get_record_reply(DBusPendingCall *call, void *data)
 {
 	DBusMessage *reply;
 	DBusError derr;
 	uint8_t *array;
-	int array_len, record_len, err = EIO;
+	int array_len, record_len, err = EIO, ch = -1;
 	sdp_record_t *record = NULL;
 	sdp_list_t *protos, *classes = NULL;
 	uuid_t uuid;
@@ -1042,17 +963,19 @@ static void get_record_reply(DBusPendingCall *call, void *data)
 	}
 
 	if (!sdp_get_access_protos(record, &protos)) {
-		c->ch = sdp_get_proto_port(protos, RFCOMM_UUID);
+		ch = sdp_get_proto_port(protos, RFCOMM_UUID);
 		sdp_list_foreach(protos, (sdp_list_func_t) sdp_list_free, NULL);
 		sdp_list_free(protos, NULL);
 		protos = NULL;
 	}
 
-	if (c->ch == -1) {
+	if (ch == -1) {
 		error("Unable to extract RFCOMM channel from service record");
 		goto failed_not_supported;
 	}
 
+	hs->rfcomm_ch = ch;
+
 	if (rfcomm_connect(device, &err) < 0) {
 		error("Unable to connect");
 		if (c->msg) 
@@ -1064,7 +987,7 @@ static void get_record_reply(DBusPendingCall *call, void *data)
 	sdp_record_free(record);
 	dbus_message_unref(reply);
 
-	finish_sdp_transaction(&device->bda);
+	finish_sdp_transaction(connection, &device->bda);
 
 	return;
 
@@ -1081,7 +1004,7 @@ failed:
 	pending_connect_free(hs->pending_connect);
 	hs->pending_connect = NULL;
 	hs->state = HEADSET_STATE_DISCONNECTED;
-	finish_sdp_transaction(&device->bda);
+	finish_sdp_transaction(connection, &device->bda);
 }
 
 static DBusHandlerResult hs_stop(DBusConnection *conn, DBusMessage *msg,
@@ -1325,6 +1248,7 @@ static DBusHandlerResult hs_connect(DBusConnection *conn, DBusMessage *msg,
 	const char *hs_svc = "hsp";
 	const char *addr_ptr;
 	char hs_address[18];
+	int err;
 
 	assert(hs != NULL);
 
@@ -1341,6 +1265,17 @@ static DBusHandlerResult hs_connect(DBusConnection *conn, DBusMessage *msg,
 
 	hs->pending_connect->msg = msg ? dbus_message_ref(msg) : NULL;
 
+	if (hs->rfcomm_ch > 0) {
+	       	if (rfcomm_connect(device, &err) < 0) {
+			error("Unable to connect");
+			pending_connect_free(hs->pending_connect);
+			hs->pending_connect = NULL;
+			hs->state = HEADSET_STATE_DISCONNECTED;
+			return err_connect_failed(conn, msg, err);
+		} else
+			return DBUS_HANDLER_RESULT_HANDLED;
+	}
+
 	msg = dbus_message_new_method_call("org.bluez", "/org/bluez/hci0",
 						"org.bluez.Adapter",
 						"GetRemoteServiceHandles");
@@ -1438,7 +1373,7 @@ static DBusHandlerResult hs_ring(DBusConnection *conn, DBusMessage *msg,
 
 	if (headset_send_ring(device) != G_IO_ERROR_NONE) {
 		dbus_message_unref(reply);
-		return err_failed(connection, msg);
+		return err_failed(connection, msg, "Failed");
 	}
 
 	hs->ring_timer = g_timeout_add(RING_INTERVAL, ring_timer_cb, device);
@@ -1593,15 +1528,42 @@ static DBusSignalVTable headset_signals[] = {
 	{ NULL, NULL }
 };
 
-headset_t *headset_init(const char *object_path)
+headset_t *headset_init(const char *object_path, sdp_record_t *record)
 {
+	int ch;
+	sdp_list_t *protos;
+	headset_t *headset;
+
 	if (!dbus_connection_register_interface(connection, object_path,
 							AUDIO_HEADSET_INTERFACE,
 							headset_methods,
 							headset_signals, NULL))
 		return NULL;
 
-	return g_new0(headset_t, 1);
+	headset = g_new0(headset_t, 1);
+
+	headset->rfcomm_ch = -1;
+
+	if (!record)
+		return headset;
+
+	if (sdp_get_access_protos(record, &protos) < 0) {
+		error("Unable to get access protos from headset record");
+		return headset;
+	}
+
+	ch = sdp_get_proto_port(protos, RFCOMM_UUID);
+
+	sdp_list_foreach(protos, (sdp_list_func_t) sdp_list_free, NULL);
+	sdp_list_free(protos, NULL);
+
+	if (ch > 0) {
+		headset->rfcomm_ch = ch;
+		debug("Discovered Headset service on RFCOMM channel %d", ch);
+	} else
+		error("Unable to get RFCOMM channel from Headset record");
+
+	return headset;
 }
 
 static gboolean headset_server_io_cb(GIOChannel *chan, GIOCondition cond, void *data)
diff --git a/audio/headset.h b/audio/headset.h
index 06ee8483..3ccbf8b2 100644
--- a/audio/headset.h
+++ b/audio/headset.h
@@ -24,6 +24,8 @@
 #define __AUDIO_HEADSET_H
 
 #include <bluetooth/bluetooth.h>
+#include <bluetooth/sdp.h>
+#include <bluetooth/sdp_lib.h>
 
 #include <dbus/dbus.h>
 
@@ -31,7 +33,7 @@
 
 typedef struct headset headset_t;
 
-headset_t *headset_init(const char *path);
+headset_t *headset_init(const char *object_path, sdp_record_t *record);
 
 gboolean headset_is_connected(headset_t *headset);
 
diff --git a/audio/manager.c b/audio/manager.c
index 00114f14..5f7f5f88 100644
--- a/audio/manager.c
+++ b/audio/manager.c
@@ -26,6 +26,7 @@
 #endif
 
 #include <stdio.h>
+#include <stdlib.h>
 #include <errno.h>
 #include <unistd.h>
 #include <stdint.h>
@@ -35,6 +36,8 @@
 
 #include <bluetooth/bluetooth.h>
 #include <bluetooth/rfcomm.h>
+#include <bluetooth/sdp.h>
+#include <bluetooth/sdp_lib.h>
 
 #include <glib.h>
 
@@ -54,6 +57,25 @@
 
 #define SOCKET_NAME "/org/bluez/audio"
 
+typedef enum {
+		GENERIC_AUDIO = 0,
+		ADVANCED_AUDIO,
+		AV_REMOTE,
+		GET_RECORDS
+} audio_sdp_state_t;
+
+struct audio_sdp_data {
+	audio_device_t *device;
+
+	DBusConnection *conn;
+	DBusMessage *msg;
+
+	GSList *handles;
+	GSList *records;
+
+	audio_sdp_state_t state;
+};
+
 static DBusConnection *connection = NULL;
 
 static audio_device_t *default_hs = NULL;
@@ -62,6 +84,10 @@ static GSList *devices = NULL;
 
 static int unix_sock = -1;
 
+static void get_next_record(struct audio_sdp_data *data);
+static DBusHandlerResult get_handles(const char *uuid,
+					struct audio_sdp_data *data);
+
 /* FIXME: Remove these once global error functions exist */
 static DBusHandlerResult error_reply(DBusConnection *conn, DBusMessage *msg,
 					const char *name, const char *descr)
@@ -80,13 +106,44 @@ static DBusHandlerResult error_reply(DBusConnection *conn, DBusMessage *msg,
 	return send_message_and_unref(conn, derr);
 }
 
-static DBusHandlerResult err_invalid_args(DBusConnection *conn, DBusMessage *msg,
+DBusHandlerResult err_invalid_args(DBusConnection *conn, DBusMessage *msg,
 						const char *descr)
 {
 	return error_reply(conn, msg, "org.bluez.audio.Error.InvalidArguments",
 			descr ? descr : "Invalid arguments in method call");
 }
 
+DBusHandlerResult err_already_connected(DBusConnection *conn, DBusMessage *msg)
+{
+	return error_reply(conn, msg, "org.bluez.audio.Error.AlreadyConnected",
+				"Already connected to a device");
+}
+
+DBusHandlerResult err_not_connected(DBusConnection *conn, DBusMessage *msg)
+{
+	return error_reply(conn, msg, "org.bluez.audio.Error.NotConnected",
+				"Not connected to any device");
+}
+
+DBusHandlerResult err_not_supported(DBusConnection *conn, DBusMessage *msg)
+{
+	return error_reply(conn, msg, "org.bluez.audio.Error.NotSupported",
+			"The service is not supported by the remote device");
+}
+
+DBusHandlerResult err_connect_failed(DBusConnection *conn,
+					DBusMessage *msg, int err)
+{
+	return error_reply(conn, msg, "org.bluez.audio.Error.ConnectFailed",
+				strerror(err));
+}
+
+DBusHandlerResult err_failed(DBusConnection *conn, DBusMessage *msg,
+				const char *dsc)
+{
+	return error_reply(conn, msg, "org.bluez.audio.Error.Failed", dsc);
+}
+
 static gboolean unix_event(GIOChannel *chan, GIOCondition cond, gpointer data)
 {
 	struct sockaddr_un addr;
@@ -185,7 +242,12 @@ static DBusMethodVTable device_methods[] = {
 	{ NULL, NULL, NULL, NULL }
 };
 
-static audio_device_t *add_device(bdaddr_t *bda)
+static void free_device(audio_device_t *device)
+{
+	g_free(device);
+}
+
+static audio_device_t *create_device(bdaddr_t *bda)
 {
 	static int device_id = 0;
 	audio_device_t *device;
@@ -197,11 +259,24 @@ static audio_device_t *add_device(bdaddr_t *bda)
 	snprintf(device->object_path, sizeof(device->object_path) - 1,
 			"%s/device%d", AUDIO_MANAGER_PATH, device_id++);
 
+	return device;
+}
+
+static void remove_device(audio_device_t *device)
+{
+	devices = g_slist_remove(devices, device);
+	dbus_connection_destroy_object_path(connection, device->object_path);
+	g_free(device->headset);
+	g_free(device);
+}
+
+
+static gboolean add_device(audio_device_t *device)
+{
 	if (!dbus_connection_create_object_path(connection, device->object_path,
 						device, NULL)) {
 		error("D-Bus failed to register %s path", device->object_path);
-		g_free(device);
-		return NULL;
+		return FALSE;
 	}
 
 	if (!dbus_connection_register_interface(connection, device->object_path,
@@ -211,21 +286,390 @@ static audio_device_t *add_device(bdaddr_t *bda)
 				AUDIO_DEVICE_INTERFACE, device->object_path);
 		dbus_connection_destroy_object_path(connection,
 							device->object_path);
-		g_free(device);
-		return NULL;
+		return FALSE;
 	}
 
 	devices = g_slist_append(devices, device);
 
-	return device;
+	return TRUE;
 }
 
-static void remove_device(audio_device_t *device)
+void finish_sdp_transaction(DBusConnection *conn, bdaddr_t *dba) 
 {
-	devices = g_slist_remove(devices, device);
-	dbus_connection_destroy_object_path(connection, device->object_path);
-	g_free(device->headset);
-	g_free(device);
+	char address[18], *addr_ptr = address;
+	DBusMessage *msg, *reply;
+	DBusError derr;
+
+	ba2str(dba, address);
+
+	msg = dbus_message_new_method_call("org.bluez", "/org/bluez/hci0",
+						"org.bluez.Adapter",
+						"FinishRemoteServiceTransaction");
+	if (!msg) {
+		error("Unable to allocate new method call");
+		return;
+	}
+
+	dbus_message_append_args(msg, DBUS_TYPE_STRING, &addr_ptr,
+					DBUS_TYPE_INVALID);
+
+	dbus_error_init(&derr);
+	reply = dbus_connection_send_with_reply_and_block(conn, msg, -1,
+								&derr);
+
+	dbus_message_unref(msg);
+
+	if (dbus_error_is_set(&derr) || dbus_set_error_from_message(&derr, reply)) {
+		error("FinishRemoteServiceTransaction(%s) failed: %s",
+				address, derr.message);
+		dbus_error_free(&derr);
+		return;
+	}
+
+	dbus_message_unref(reply);
+}
+
+static void handle_record(sdp_record_t *record, audio_device_t *device)
+{
+	sdp_list_t *classes;
+	uuid_t uuid;
+	uint16_t uuid16;
+
+	if (sdp_get_service_classes(record, &classes) < 0) {
+		error("Unable to get service classes from record");
+		return;
+	}
+
+	memcpy(&uuid, classes->data, sizeof(uuid));
+
+	if (!sdp_uuid128_to_uuid(&uuid)) {
+		error("Not a 16 bit UUID");
+		goto done;
+	}
+
+	if (uuid.type == SDP_UUID32) {
+	       	if (uuid.value.uuid32 > 0xFFFF) {
+			error("Not a 16 bit UUID");
+			goto done;
+		}
+		uuid16 = (uint16_t) uuid.value.uuid32;
+	} else
+		uuid16 = uuid.value.uuid16;
+
+	switch (uuid16) {
+	case HEADSET_SVCLASS_ID:
+		debug("Found Headset record");
+		if (device->headset)
+			debug("Multiple Headset records found");
+		else
+			device->headset = headset_init(device->object_path,
+							record);
+		break;
+	case HEADSET_AGW_SVCLASS_ID:
+		debug("Found Headset AG record");
+		break;
+	case HANDSFREE_SVCLASS_ID:
+		debug("Found Hansfree record");
+		break;
+	case HANDSFREE_AGW_SVCLASS_ID:
+		debug("Found Handsfree AG record");
+		break;
+	case AUDIO_SINK_SVCLASS_ID:
+		debug("Found Audio Sink");
+		break;
+	case AUDIO_SOURCE_SVCLASS_ID:
+		debug("Found Audio Source");
+		break;
+	case AV_REMOTE_SVCLASS_ID:
+		debug("Found AV Remote");
+		break;
+	case AV_REMOTE_TARGET_SVCLASS_ID:
+		debug("Found AV Target");
+		break;
+	default:
+		debug("Unrecognized UUID: 0x%04X", uuid16);
+		break;
+	}
+
+done:
+	sdp_list_free(classes, free);
+}
+
+static void finish_sdp(struct audio_sdp_data *data, gboolean success)
+{
+	const char *path;
+	DBusMessage *reply;
+
+	debug("Audio service discovery completed with %s",
+			success ? "success" : "failure");
+
+	finish_sdp_transaction(data->conn, &data->device->bda);
+
+	if (!success)
+		goto done;
+
+	reply = dbus_message_new_method_return(data->msg);
+	if (!reply) {
+		success = FALSE;
+		err_failed(data->conn, data->msg, "Out of memory");
+		goto done;
+	}
+
+	path = data->device->object_path;
+
+	add_device(data->device);
+	g_slist_foreach(data->records, (GFunc) handle_record, data->device);
+
+	dbus_connection_emit_signal(data->conn, AUDIO_MANAGER_PATH,
+					AUDIO_MANAGER_INTERFACE,
+					"DeviceCreated",
+					DBUS_TYPE_STRING, &path,
+					DBUS_TYPE_INVALID);
+
+	dbus_message_append_args(reply, DBUS_TYPE_STRING, &path,
+					DBUS_TYPE_INVALID);
+	send_message_and_unref(data->conn, reply);
+
+done:
+	if (!success)
+		free_device(data->device);
+	dbus_connection_unref(data->conn);
+	dbus_message_unref(data->msg);
+	g_slist_free(data->handles);
+	g_slist_foreach(data->records, (GFunc) sdp_record_free, NULL);
+	g_slist_free(data->records);
+	g_free(data);
+}
+
+static void get_record_reply(DBusPendingCall *call,
+				struct audio_sdp_data *data)
+{
+	DBusMessage *reply;
+	DBusError derr;
+	uint8_t *array;
+	int array_len, record_len;
+	sdp_record_t *record;
+
+	reply = dbus_pending_call_steal_reply(call);
+
+	dbus_error_init(&derr);
+	if (dbus_set_error_from_message(&derr, reply)) {
+		error("GetRemoteServiceRecord failed: %s", derr.message);
+		if (dbus_error_has_name(&derr,
+					"org.bluez.Error.ConnectionAttemptFailed"))
+			err_connect_failed(data->conn, data->msg, EHOSTDOWN);
+		else
+			err_failed(data->conn, data->msg, derr.message);
+		dbus_error_free(&derr);
+		goto failed;
+	}
+
+	if (!dbus_message_get_args(reply, NULL,
+				DBUS_TYPE_ARRAY, DBUS_TYPE_BYTE, &array, &array_len,
+				DBUS_TYPE_INVALID)) {
+		err_failed(data->conn, data->msg,
+				"Unable to get args from GetRecordReply");
+		goto failed;
+	}
+
+	record = sdp_extract_pdu(array, &record_len);
+	if (!record) {
+		error("Unable to extract service record from reply");
+		goto done;
+	}
+
+	if (record_len != array_len)
+		debug("warning: array len (%d) != record len (%d)",
+				array_len, record_len);
+
+	data->records = g_slist_append(data->records, record);
+
+done:
+	dbus_message_unref(reply);
+
+	if (data->handles)
+		get_next_record(data);
+	else
+		finish_sdp(data, TRUE);
+
+	return;
+
+failed:
+	if (reply)
+		dbus_message_unref(reply);
+	finish_sdp(data, FALSE);
+}
+
+static void get_next_record(struct audio_sdp_data *data)
+{
+	DBusMessage *msg;
+	DBusPendingCall *pending;
+	char address[18], *ptr = address;
+	dbus_uint32_t handle;
+
+	msg = dbus_message_new_method_call("org.bluez", "/org/bluez/hci0",
+						"org.bluez.Adapter",
+						"GetRemoteServiceRecord");
+	if (!msg) {
+		error("Unable to allocate new method call");
+		err_connect_failed(data->conn, data->msg, ENOMEM);
+		finish_sdp(data, FALSE);
+		return;
+	}
+
+	handle = (dbus_uint32_t) data->handles->data;
+
+	data->handles = g_slist_remove(data->handles, data->handles->data);
+
+	ba2str(&data->device->bda, address);
+
+	dbus_message_append_args(msg, DBUS_TYPE_STRING, &ptr,
+					DBUS_TYPE_UINT32, &handle,
+					DBUS_TYPE_INVALID);
+
+	if (!dbus_connection_send_with_reply(data->conn, msg, &pending, -1)) {
+		error("Sending GetRemoteServiceRecord failed");
+		err_connect_failed(data->conn, data->msg, EIO);
+		finish_sdp(data, FALSE);
+		return;
+	}
+
+	dbus_pending_call_set_notify(pending,
+			(DBusPendingCallNotifyFunction) get_record_reply,
+			data, NULL);
+	dbus_pending_call_unref(pending);
+	dbus_message_unref(msg);
+}
+
+static GSList *find_handle(GSList *handles, dbus_uint32_t handle)
+{
+	while (handles) {
+		if ((dbus_uint32_t) handles->data == handle)
+			return handles;
+		handles = handles->next;
+	}
+
+	return NULL;
+}
+
+static void get_handles_reply(DBusPendingCall *call,
+				struct audio_sdp_data *data)
+{
+	DBusMessage *reply;
+	DBusError derr;
+	dbus_uint32_t *array = NULL;
+	int array_len, i;
+
+	reply = dbus_pending_call_steal_reply(call);
+
+	dbus_error_init(&derr);
+	if (dbus_set_error_from_message(&derr, reply)) {
+		error("GetRemoteServiceHandles failed: %s", derr.message);
+		if (dbus_error_has_name(&derr,
+					"org.bluez.Error.ConnectionAttemptFailed"))
+			err_connect_failed(data->conn, data->msg, EHOSTDOWN);
+		else
+			err_failed(data->conn, data->msg, derr.message);
+		dbus_error_free(&derr);
+		goto failed;
+	}
+
+	if (!dbus_message_get_args(reply, NULL,
+				DBUS_TYPE_ARRAY, DBUS_TYPE_UINT32, &array, &array_len,
+				DBUS_TYPE_INVALID)) {
+	  
+		err_failed(data->conn, data->msg,
+				"Unable to get args from reply");
+		goto failed;
+	}
+
+	for (i = 0; i < array_len; i++) {
+		if (!find_handle(data->handles, array[i]))
+			data->handles = g_slist_append(data->handles,
+							(gpointer) array[i]);
+	}
+
+	data->state++;
+
+	switch (data->state) {
+	case ADVANCED_AUDIO:
+		get_handles(ADVANCED_AUDIO_UUID, data);
+		break;
+	case AV_REMOTE:
+		get_handles(AVRCP_REMOTE_UUID, data);
+		break;
+	default:
+		if (data->handles)
+			get_next_record(data);
+		else
+			finish_sdp(data, TRUE);
+	}	
+
+	dbus_message_unref(reply);
+
+	return;
+
+failed:
+	dbus_message_unref(reply);
+	finish_sdp(data, FALSE);
+}
+
+static DBusHandlerResult get_handles(const char *uuid,
+					struct audio_sdp_data *data)
+{
+	DBusPendingCall *pending;
+	char address[18];
+	const char *ptr = address;
+	DBusMessage *msg;
+
+	msg = dbus_message_new_method_call("org.bluez", "/org/bluez/hci0",
+						"org.bluez.Adapter",
+						"GetRemoteServiceHandles");
+	if (!msg) {
+		err_failed(data->conn, data->msg,
+				"Could not create a new dbus message");
+		goto failed;
+	}
+
+	ba2str(&data->device->bda, address);
+
+	dbus_message_append_args(msg, DBUS_TYPE_STRING, &ptr,
+					DBUS_TYPE_STRING, &uuid,
+					DBUS_TYPE_INVALID);
+
+	if (!dbus_connection_send_with_reply(connection, msg, &pending, -1)) {
+		err_failed(data->conn, data->msg,
+				"Sending GetRemoteServiceHandles failed");
+		goto failed;
+	}
+
+	dbus_pending_call_set_notify(pending,
+			(DBusPendingCallNotifyFunction) get_handles_reply,
+			data, NULL);
+	dbus_pending_call_unref(pending);
+	dbus_message_unref(msg);
+
+	return DBUS_HANDLER_RESULT_HANDLED;
+
+failed:
+	if (msg)
+		dbus_message_unref(msg);
+	finish_sdp(data, FALSE);
+	return DBUS_HANDLER_RESULT_HANDLED;
+}
+
+static DBusHandlerResult resolve_services(DBusConnection *conn,
+						DBusMessage *msg,
+						audio_device_t *device)
+{
+	struct audio_sdp_data *sdp_data;
+
+	sdp_data = g_new0(struct audio_sdp_data, 1);
+	sdp_data->msg = dbus_message_ref(msg);
+	sdp_data->conn = dbus_connection_ref(conn);
+	sdp_data->device = device;
+
+	return get_handles(GENERIC_AUDIO_UUID, sdp_data);
 }
 
 audio_device_t *manager_headset_connected(bdaddr_t *bda)
@@ -237,14 +681,16 @@ audio_device_t *manager_headset_connected(bdaddr_t *bda)
 	if (device && device->headset)
 		return device;
 
-	if (!device)
-		device = add_device(bda);
-
-	if (!device)
-		return NULL;
+	if (!device) {
+		device = create_device(bda);
+		if (!add_device(device)) {
+			free_device(device);
+			return NULL;
+		}
+	}
 
 	if (!device->headset)
-		device->headset = headset_init(device->object_path);
+		device->headset = headset_init(device->object_path, NULL);
 
 	if (!device->headset)
 		return NULL;
@@ -276,7 +722,6 @@ static DBusHandlerResult am_create_device(DBusConnection *conn, DBusMessage *msg
 	bdaddr_t bda;
 	audio_device_t *device;
 	DBusError derr;
-	DBusMessageIter iter, array_iter;
 
 	dbus_error_init(&derr);
 	dbus_message_get_args(msg, &derr,
@@ -292,31 +737,18 @@ static DBusHandlerResult am_create_device(DBusConnection *conn, DBusMessage *msg
 
 	device = find_device(&bda);
 	if (device) {
-		const char *iface, *path = device->object_path;
+		const char *path = device->object_path;
 		DBusMessage *reply = dbus_message_new_method_return(msg);
 		if (!reply)
 			return DBUS_HANDLER_RESULT_NEED_MEMORY;
-		dbus_message_iter_init_append(reply, &iter);
-		dbus_message_iter_append_basic(&iter, DBUS_TYPE_STRING, &path);
-		dbus_message_iter_open_container(&iter, DBUS_TYPE_ARRAY,
-							"s", &array_iter);
-		if (device->headset) {
-			iface = AUDIO_HEADSET_INTERFACE;
-			dbus_message_iter_append_basic(&array_iter,
-							DBUS_TYPE_STRING, &iface);
-		}
-
-		dbus_message_iter_close_container(&iter, &array_iter);
-
+		dbus_message_append_args(reply, DBUS_TYPE_STRING, &path,
+						DBUS_TYPE_INVALID);
 		return send_message_and_unref(conn, reply);
 	}
 
-	device = add_device(&bda);
-	/*
-	resolve_services(conn, device, msg);
-	*/
+	device = create_device(&bda);
 
-	return DBUS_HANDLER_RESULT_HANDLED;
+	return resolve_services(conn, msg, device);
 }
 
 static DBusHandlerResult am_list_devices(DBusConnection *conn,
@@ -373,19 +805,23 @@ static DBusHandlerResult am_create_headset(DBusConnection *conn, DBusMessage *ms
 	str2ba(address, &bda);
 
 	device = find_device(&bda);
-	if (!device)
-		device = add_device(&bda);
-
-	if (!device)
-		return error_reply(connection, msg,
+	if (!device) {
+		device = create_device(&bda);
+		if (!add_device(device)) {
+			free_device(device);
+			return error_reply(connection, msg,
 					"org.bluez.audio.Error.Failed",
 					"Unable to create new audio device");
+		}
+	}
 
-	device->headset = headset_init(device->object_path);
-	if (!device->headset)
+	device->headset = headset_init(device->object_path, NULL);
+	if (!device->headset) {
+		remove_device(device);
 		return error_reply(connection, msg,
 					"org.bluez.audio.Error.Failed",
 					"Unable to init Headset interface");
+	}
 
 	path = device->object_path;
 
diff --git a/audio/manager.h b/audio/manager.h
index b23a4bae..45c5fa5f 100644
--- a/audio/manager.h
+++ b/audio/manager.h
@@ -67,3 +67,17 @@ audio_device_t *manager_headset_connected(bdaddr_t *bda);
 int audio_init(DBusConnection *conn);
 
 void audio_exit(void);
+
+void finish_sdp_transaction(DBusConnection *conn, bdaddr_t *dba);
+
+
+DBusHandlerResult err_invalid_args(DBusConnection *conn, DBusMessage *msg,
+						const char *descr);
+DBusHandlerResult err_already_connected(DBusConnection *conn, DBusMessage *msg);
+DBusHandlerResult err_not_connected(DBusConnection *conn, DBusMessage *msg);
+DBusHandlerResult err_not_supported(DBusConnection *conn, DBusMessage *msg);
+DBusHandlerResult err_connect_failed(DBusConnection *conn,
+					DBusMessage *msg, int err);
+DBusHandlerResult err_failed(DBusConnection *conn, DBusMessage *msg,
+				const char *dsc);
+