Bring the audio code up-to-date with the current API spec

5 files changed, 506 insertions, 105 deletions

diff --git a/audio/headset.c b/audio/headset.c
index 4c2b8423..d9e26459 100644
--- a/audio/headset.c
+++ b/audio/headset.c
@@ -56,6 +56,7 @@
 #include "headset.h"
 
 #define DEFAULT_HS_AG_CHANNEL 12
+#define DEFAULT_HF_AG_CHANNEL 13
 
 #define RING_INTERVAL 3000
 
@@ -76,12 +77,20 @@ typedef enum {
 	HEADSET_STATE_PLAYING,
 } headset_state_t;
 
+typedef enum {
+	SVC_HEADSET,
+	SVC_HANDSFREE
+} hs_type;
+
 struct pending_connect {
 	DBusMessage *msg;
 	GIOChannel *io;
 };
 
 struct headset {
+	uint32_t hsp_handle;
+	uint32_t hfp_handle;
+
 	int rfcomm_ch;
 
 	GIOChannel *rfcomm;
@@ -93,6 +102,8 @@ struct headset {
 	int data_start;
 	int data_length;
 
+	hs_type type;
+
 	headset_state_t state;
 	struct pending_connect *pending_connect;
 };
@@ -100,9 +111,13 @@ struct headset {
 static DBusHandlerResult hs_disconnect(DBusConnection *conn, DBusMessage *msg,
 					void *data);
 
+static gboolean disable_hfp = FALSE;
+
 static GIOChannel *hs_server = NULL;
+static GIOChannel *hf_server = NULL;
 
 static uint32_t hs_record_id = 0;
+static uint32_t hf_record_id = 0;
 
 static DBusConnection *connection = NULL;
 
@@ -184,7 +199,7 @@ static void close_sco(audio_device_t *device)
 static gboolean rfcomm_io_cb(GIOChannel *chan, GIOCondition cond,
 				audio_device_t *device)
 {
-	struct headset *hs = device->headset;
+	struct headset *hs;
 	unsigned char buf[BUF_SIZE];
 	char *cr, rsp[BUF_SIZE];
 	gsize bytes_read = 0;
@@ -195,6 +210,8 @@ static gboolean rfcomm_io_cb(GIOChannel *chan, GIOCondition cond,
 	if (cond & G_IO_NVAL)
 		return FALSE;
 
+	hs = device->headset;
+
 	if (cond & (G_IO_ERR | G_IO_HUP))
 		goto failed;
 
@@ -343,11 +360,13 @@ static void auth_callback(DBusPendingCall *call, void *data)
 
 static gboolean sco_cb(GIOChannel *chan, GIOCondition cond, audio_device_t *device)
 {
-	struct headset *hs = device->headset;
+	struct headset *hs;
 
 	if (cond & G_IO_NVAL)
 		return FALSE;
 
+	hs = device->headset;
+
 	error("Audio connection got disconnected");
 
 	if (hs->sco)
@@ -501,6 +520,8 @@ static int rfcomm_connect(audio_device_t *device, int *err)
 	assert(hs != NULL && hs->pending_connect != NULL && 
 			hs->state == HEADSET_STATE_CONNECT_IN_PROGRESS);
 
+	hs->type = hs->hfp_handle ? SVC_HANDSFREE : SVC_HEADSET;
+
 	ba2str(&device->bda, address);
 
 	debug("Connecting to %s channel %d", address, hs->rfcomm_ch);
@@ -567,7 +588,7 @@ failed:
 	return -1;
 }
 
-static int create_ag_record(sdp_buf_t *buf, uint8_t ch)
+static int create_hsp_ag_record(sdp_buf_t *buf, uint8_t ch)
 {
 	sdp_list_t *svclass_id, *pfseq, *apseq, *root;
 	uuid_t root_uuid, svclass_uuid, ga_svclass_uuid, l2cap_uuid, rfcomm_uuid;
@@ -607,7 +628,76 @@ static int create_ag_record(sdp_buf_t *buf, uint8_t ch)
 	aproto = sdp_list_append(0, apseq);
 	sdp_set_access_protos(&record, aproto);
 
-	sdp_set_info_attr(&record, "Headset", 0, 0);
+	sdp_set_info_attr(&record, "Headset Audio Gateway", 0, 0);
+
+	if (sdp_gen_record_pdu(&record, buf) < 0)
+		ret = -1;
+	else
+		ret = 0;
+
+	sdp_data_free(channel);
+	sdp_list_free(proto[0], 0);
+	sdp_list_free(proto[1], 0);
+	sdp_list_free(apseq, 0);
+	sdp_list_free(pfseq, 0);
+	sdp_list_free(aproto, 0);
+	sdp_list_free(root, 0);
+	sdp_list_free(svclass_id, 0);
+	sdp_list_free(record.attrlist, (sdp_free_func_t) sdp_data_free);
+	sdp_list_free(record.pattern, free);
+
+	return ret;
+}
+
+static int create_hfp_ag_record(sdp_buf_t *buf, uint8_t ch)
+{
+	sdp_list_t *svclass_id, *pfseq, *apseq, *root;
+	uuid_t root_uuid, svclass_uuid, ga_svclass_uuid, l2cap_uuid, rfcomm_uuid;
+	sdp_profile_desc_t profile;
+	sdp_list_t *aproto, *proto[2];
+	sdp_record_t record;
+	uint16_t u16 = 0x0009;
+	sdp_data_t *channel, *features;
+	uint8_t netid =  0x01;
+	sdp_data_t *network = sdp_data_alloc(SDP_UINT8, &netid);
+	int ret;
+
+	memset(&record, 0, sizeof(sdp_record_t));
+
+	sdp_uuid16_create(&root_uuid, PUBLIC_BROWSE_GROUP);
+	root = sdp_list_append(0, &root_uuid);
+	sdp_set_browse_groups(&record, root);
+
+	sdp_uuid16_create(&svclass_uuid, HANDSFREE_AGW_SVCLASS_ID);
+	svclass_id = sdp_list_append(0, &svclass_uuid);
+	sdp_uuid16_create(&ga_svclass_uuid, GENERIC_AUDIO_SVCLASS_ID);
+	svclass_id = sdp_list_append(svclass_id, &ga_svclass_uuid);
+	sdp_set_service_classes(&record, svclass_id);
+
+	sdp_uuid16_create(&profile.uuid, HANDSFREE_PROFILE_ID);
+	profile.version = 0x0105;
+	pfseq = sdp_list_append(0, &profile);
+	sdp_set_profile_descs(&record, pfseq);
+
+	sdp_uuid16_create(&l2cap_uuid, L2CAP_UUID);
+	proto[0] = sdp_list_append(0, &l2cap_uuid);
+	apseq = sdp_list_append(0, proto[0]);
+
+	sdp_uuid16_create(&rfcomm_uuid, RFCOMM_UUID);
+	proto[1] = sdp_list_append(0, &rfcomm_uuid);
+	channel = sdp_data_alloc(SDP_UINT8, &ch);
+	proto[1] = sdp_list_append(proto[1], channel);
+	apseq = sdp_list_append(apseq, proto[1]);
+
+	features = sdp_data_alloc(SDP_UINT16, &u16);
+	sdp_attr_add(&record, SDP_ATTR_SUPPORTED_FEATURES, features);
+
+	aproto = sdp_list_append(0, apseq);
+	sdp_set_access_protos(&record, aproto);
+
+	sdp_set_info_attr(&record, "Hands-Free Audio Gateway", 0, 0);
+
+	sdp_attr_add(&record, SDP_ATTR_EXTERNAL_NETWORK, network);
 
 	if (sdp_gen_record_pdu(&record, buf) < 0)
 		ret = -1;
@@ -628,12 +718,11 @@ static int create_ag_record(sdp_buf_t *buf, uint8_t ch)
 	return ret;
 }
 
-static uint32_t headset_add_ag_record(uint8_t channel)
+static uint32_t headset_add_ag_record(uint8_t channel, sdp_buf_t *buf)
 {
 	DBusMessage *msg, *reply;
 	DBusError derr;
 	dbus_uint32_t rec_id;
-	sdp_buf_t buf;
 
 	msg = dbus_message_new_method_call("org.bluez", "/org/bluez",
 				"org.bluez.Database", "AddServiceRecord");
@@ -642,20 +731,13 @@ static uint32_t headset_add_ag_record(uint8_t channel)
 		return 0;
 	}
 
-	if (create_ag_record(&buf, channel) < 0) {
-		error("Unable to allocate new service record");
-		dbus_message_unref(msg);
-		return 0;
-	}
-
 	dbus_message_append_args(msg, DBUS_TYPE_ARRAY, DBUS_TYPE_BYTE,
-				&buf.data, buf.data_size, DBUS_TYPE_INVALID);
+				&buf->data, buf->data_size, DBUS_TYPE_INVALID);
 
 	dbus_error_init(&derr);
 	reply = dbus_connection_send_with_reply_and_block(connection, msg,
 								-1, &derr);
 
-	free(buf.data);
 	dbus_message_unref(msg);
 
 	if (dbus_error_is_set(&derr) || dbus_set_error_from_message(&derr, reply)) {
@@ -901,7 +983,7 @@ static DBusHandlerResult hs_disconnect(DBusConnection *conn, DBusMessage *msg,
 	}
 
 	if (hs->state > HEADSET_STATE_CONNECTED)
-		hs_stop(NULL, NULL, hs);
+		hs_stop(NULL, NULL, device);
 
 	if (hs->rfcomm) {
 		g_io_channel_close(hs->rfcomm);
@@ -1344,28 +1426,14 @@ static DBusSignalVTable headset_signals[] = {
 	{ NULL, NULL }
 };
 
-headset_t *headset_init(const char *object_path, sdp_record_t *record)
+static void headset_set_channel(headset_t *headset, 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;
-
-	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;
+		return;
 	}
 
 	ch = sdp_get_proto_port(protos, RFCOMM_UUID);
@@ -1378,10 +1446,113 @@ headset_t *headset_init(const char *object_path, sdp_record_t *record)
 		debug("Discovered Headset service on RFCOMM channel %d", ch);
 	} else
 		error("Unable to get RFCOMM channel from Headset record");
+}
+
+void headset_update(headset_t *headset, sdp_record_t *record, uint16_t svc)
+{
+	switch (svc) {
+	case HANDSFREE_SVCLASS_ID:
+		if (disable_hfp) {
+			debug("Ignoring Handsfree record since HFP support"
+					" has been disabled");
+			return;
+		}
+
+		if (headset->hfp_handle &&
+				(headset->hfp_handle != record->handle)) {
+			error("More than one HFP record found on device");
+			return;
+		}
+
+		headset->hfp_handle = record->handle;
+		break;
+
+	case HEADSET_SVCLASS_ID:
+		if (headset->hsp_handle &&
+				(headset->hsp_handle != record->handle)) {
+			error("More than one HSP record found on device");
+			return;
+		}
+
+		headset->hsp_handle = record->handle;
+
+		/* Ignore this record if we already have access to HFP */
+		if (headset->hfp_handle)
+			return;
+
+		break;
+
+	default:
+		debug("Invalid record passed to headset_update");
+		return;
+	}
+
+	headset_set_channel(headset, record);
+}
+
+headset_t *headset_init(const char *object_path, sdp_record_t *record,
+			uint16_t svc)
+{
+	headset_t *headset;
+
+	headset = g_new0(headset_t, 1);
+	headset->rfcomm_ch = -1;
+
+	if (!record)
+		goto register_iface;
+
+	switch (svc) {
+	case HANDSFREE_SVCLASS_ID:
+		if (disable_hfp) {
+			debug("Ignoring Handsfree record since HFP support"
+				       " has been disabled");
+			g_free(headset);
+			return NULL;
+		}
+
+		headset->hfp_handle = record->handle;
+		break;
+
+	case HEADSET_SVCLASS_ID:
+		headset->hsp_handle = record->handle;
+		break;
+
+	default:
+		debug("Invalid record passed to headset_init");
+		g_free(headset);
+		return NULL;
+	}
+
+register_iface:
+	if (!dbus_connection_register_interface(connection, object_path,
+							AUDIO_HEADSET_INTERFACE,
+							headset_methods,
+							headset_signals, NULL)) {
+		g_free(headset);
+		return NULL;
+	}
+
+	if (record)
+		headset_set_channel(headset, record);
 
 	return headset;
 }
 
+void headset_free(const char *object_path)
+{ 
+	audio_device_t *device;
+
+	if (!dbus_connection_get_object_user_data(connection, object_path,
+						(void *) &device))
+		return;
+
+	if (device->headset->state != HEADSET_STATE_DISCONNECTED)
+		hs_disconnect(NULL, NULL, device);
+
+	g_free(device->headset);
+	device->headset = NULL;
+}
+
 static gboolean headset_server_io_cb(GIOChannel *chan, GIOCondition cond, void *data)
 {
 	int srv_sk, cli_sk;
@@ -1434,6 +1605,11 @@ static gboolean headset_server_io_cb(GIOChannel *chan, GIOCondition cond, void *
 		return TRUE;
 	}
 
+	if (chan == hs_server)
+		hs->type = SVC_HEADSET;
+	else
+		hs->type = SVC_HANDSFREE;
+
 	auth = dbus_message_new_method_call("org.bluez", "/org/bluez", "org.bluez.Database",
 						"RequestAuthorization");
 	if (!auth) {
@@ -1490,7 +1666,7 @@ static GIOChannel *server_socket(uint8_t *channel)
 	memset(&addr, 0, sizeof(addr));
 	addr.rc_family = AF_BLUETOOTH;
 	bacpy(&addr.rc_bdaddr, BDADDR_ANY);
-	addr.rc_channel = 0;
+	addr.rc_channel = channel ? *channel : 0;
 
 	if (bind(sock, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
 		error("server bind: %s", strerror(errno), errno);
@@ -1526,9 +1702,10 @@ gboolean headset_is_connected(headset_t *headset)
 		return FALSE;
 }
 
-int headset_server_init(DBusConnection *conn)
+int headset_server_init(DBusConnection *conn, gboolean no_hfp)
 {
 	uint8_t chan = DEFAULT_HS_AG_CHANNEL;
+	sdp_buf_t buf;
 
 	connection = dbus_connection_ref(conn);
 
@@ -1536,11 +1713,15 @@ int headset_server_init(DBusConnection *conn)
 	if (!hs_server)
 		return -1;
 
-	if (!hs_record_id)
-		hs_record_id = headset_add_ag_record(chan);
+	if (create_hsp_ag_record(&buf, chan) < 0) {
+		error("Unable to allocate new service record");
+		return -1;
+	}
 
+	hs_record_id = headset_add_ag_record(chan, &buf);
+	free(buf.data);
 	if (!hs_record_id) {
-		error("Unable to register service record");
+		error("Unable to register HS AG service record");
 		g_io_channel_unref(hs_server);
 		hs_server = NULL;
 		return -1;
@@ -1549,6 +1730,34 @@ int headset_server_init(DBusConnection *conn)
 	g_io_add_watch(hs_server, G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
 				(GIOFunc) headset_server_io_cb, NULL);
 
+	disable_hfp = no_hfp;
+
+	if (disable_hfp)
+		return 0;
+
+	chan = DEFAULT_HF_AG_CHANNEL;
+
+	hf_server = server_socket(&chan);
+	if (!hf_server)
+		return -1;
+
+	if (create_hfp_ag_record(&buf, chan) < 0) {
+		error("Unable to allocate new service record");
+		return -1;
+	}
+
+	hf_record_id = headset_add_ag_record(chan, &buf);
+	free(buf.data);
+	if (!hf_record_id) {
+		error("Unable to register HS AG service record");
+		g_io_channel_unref(hf_server);
+		hs_server = NULL;
+		return -1;
+	}
+
+	g_io_add_watch(hf_server, G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
+				(GIOFunc) headset_server_io_cb, NULL);
+
 	return 0;
 }
 
@@ -1564,6 +1773,16 @@ void headset_exit(void)
 		hs_server = NULL;
 	}
 
+	if (hf_record_id) {
+		headset_remove_ag_record(hf_record_id);
+		hf_record_id = 0;
+	}
+
+	if (hf_server) {
+		g_io_channel_unref(hf_server);
+		hf_server = NULL;
+	}
+
 	dbus_connection_unref(connection);
 	connection = NULL;
 }
diff --git a/audio/headset.h b/audio/headset.h
index 3ccbf8b2..c02f36a3 100644
--- a/audio/headset.h
+++ b/audio/headset.h
@@ -33,11 +33,16 @@
 
 typedef struct headset headset_t;
 
-headset_t *headset_init(const char *object_path, sdp_record_t *record);
+headset_t *headset_init(const char *object_path, sdp_record_t *record,
+			uint16_t svc);
+
+void headset_free(const char *object_path);
+
+void headset_update(headset_t *headset, sdp_record_t *record, uint16_t svc);
 
 gboolean headset_is_connected(headset_t *headset);
 
-int headset_server_init(DBusConnection *conn);
+int headset_server_init(DBusConnection *conn, gboolean disable_hfp);
 
 void headset_exit(void);
 
diff --git a/audio/main.c b/audio/main.c
index 98dc1d4f..80e3fabf 100644
--- a/audio/main.c
+++ b/audio/main.c
@@ -84,7 +84,7 @@ int main(int argc, char *argv[])
 		exit(1);
 	}
 
-	if (headset_server_init(conn) < 0) {
+	if (headset_server_init(conn, FALSE) < 0) {
 		error("Headset initialization failed!");
 		exit(1);
 	}
diff --git a/audio/manager.c b/audio/manager.c
index bafc7d9d..85d52e86 100644
--- a/audio/manager.c
+++ b/audio/manager.c
@@ -27,6 +27,7 @@
 
 #include <stdio.h>
 #include <stdlib.h>
+#include <stdarg.h>
 #include <errno.h>
 #include <unistd.h>
 #include <stdint.h>
@@ -49,6 +50,16 @@
 #include "manager.h"
 
 typedef enum {
+	HEADSET	= 1 << 0,
+	GATEWAY	= 1 << 1,
+	SINK	= 1 << 2,
+	SOURCE	= 1 << 3,
+	CONTROL	= 1 << 4,
+	TARGET	= 1 << 5,
+	INVALID	= 1 << 6
+} audio_service_type;
+
+typedef enum {
 		GENERIC_AUDIO = 0,
 		ADVANCED_AUDIO,
 		AV_REMOTE,
@@ -58,10 +69,10 @@ typedef enum {
 struct audio_sdp_data {
 	audio_device_t *device;
 
-	DBusMessage *msg;
+	DBusMessage *msg;	/* Method call or NULL */
 
-	GSList *handles;
-	GSList *records;
+	GSList *handles;	/* uint32_t * */
+	GSList *records;	/* sdp_record_t * */
 
 	audio_sdp_state_t state;
 };
@@ -224,8 +235,8 @@ static audio_device_t *create_device(bdaddr_t *bda)
 static void remove_device(audio_device_t *device)
 {
 	devices = g_slist_remove(devices, device);
+	headset_free(device->object_path);
 	dbus_connection_destroy_object_path(connection, device->object_path);
-	g_free(device->headset);
 	g_free(device);
 }
 
@@ -253,6 +264,40 @@ static gboolean add_device(audio_device_t *device)
 	return TRUE;
 }
 
+static uint16_t get_service_uuid(const sdp_record_t *record)
+{
+	sdp_list_t *classes;
+	uuid_t uuid;
+	uint16_t uuid16 = 0;
+
+	if (sdp_get_service_classes(record, &classes) < 0) {
+		error("Unable to get service classes from record");
+		return 0;
+	}
+
+	memcpy(&uuid, classes->data, sizeof(uuid));
+	
+	if (!sdp_uuid128_to_uuid(&uuid)) {
+		error("Not a 16 bit UUID");
+		sdp_list_free(classes, free);
+		return 0;
+	}
+
+	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;
+
+done:
+	sdp_list_free(classes, free);
+
+	return uuid16;
+}
+
 void finish_sdp_transaction(DBusConnection *conn, bdaddr_t *dba) 
 {
 	char address[18], *addr_ptr = address;
@@ -290,39 +335,18 @@ void finish_sdp_transaction(DBusConnection *conn, bdaddr_t *dba)
 
 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;
+	uuid16 = get_service_uuid(record);
 
 	switch (uuid16) {
 	case HEADSET_SVCLASS_ID:
 		debug("Found Headset record");
 		if (device->headset)
-			debug("Multiple Headset records found");
+			headset_update(device->headset, record, uuid16);
 		else
 			device->headset = headset_init(device->object_path,
-							record);
+							record, uuid16);
 		break;
 	case HEADSET_AGW_SVCLASS_ID:
 		debug("Found Headset AG record");
@@ -349,15 +373,45 @@ static void handle_record(sdp_record_t *record, audio_device_t *device)
 		debug("Unrecognized UUID: 0x%04X", uuid16);
 		break;
 	}
+}
 
-done:
-	sdp_list_free(classes, free);
+static gint record_iface_cmp(gconstpointer a, gconstpointer b)
+{
+	const sdp_record_t *record = a;
+	const char *interface = b;
+
+	switch (get_service_uuid(record)) {
+	case HEADSET_SVCLASS_ID:
+	case HANDSFREE_SVCLASS_ID:
+		return strcmp(interface, AUDIO_HEADSET_INTERFACE);
+
+	case HEADSET_AGW_SVCLASS_ID:
+	case HANDSFREE_AGW_SVCLASS_ID:
+		return strcmp(interface, AUDIO_GATEWAY_INTERFACE);
+
+	case AUDIO_SINK_SVCLASS_ID:
+		return strcmp(interface, AUDIO_SINK_INTERFACE);
+
+	case AUDIO_SOURCE_SVCLASS_ID:
+		return strcmp(interface, AUDIO_SOURCE_INTERFACE);
+
+	case AV_REMOTE_SVCLASS_ID:
+		return strcmp(interface, AUDIO_CONTROL_INTERFACE);
+
+	case AV_REMOTE_TARGET_SVCLASS_ID:
+		return strcmp(interface, AUDIO_TARGET_INTERFACE);
+
+	default:
+		return -1;
+	}
 }
 
 static void finish_sdp(struct audio_sdp_data *data, gboolean success)
 {
-	const char *path;
-	DBusMessage *reply;
+	const char *path, *addr;
+	char **required;
+	int required_len, i;
+	DBusMessage *reply = NULL;
 
 	debug("Audio service discovery completed with %s",
 			success ? "success" : "failure");
@@ -367,6 +421,36 @@ static void finish_sdp(struct audio_sdp_data *data, gboolean success)
 	if (!success)
 		goto done;
 
+	if (!data->msg)
+		goto update;
+
+	if (!dbus_message_get_args(data->msg, NULL,
+					DBUS_TYPE_STRING, &addr,
+					DBUS_TYPE_ARRAY, DBUS_TYPE_STRING,
+					&required, &required_len,
+					DBUS_TYPE_INVALID)) {
+		error("Unable to get message args");
+		success = FALSE;
+		goto done;
+	}
+
+	for (i = 0; i < required_len; i++) {
+		const char *iface = required[i];
+
+		if (g_slist_find_custom(data->records, iface, record_iface_cmp))
+			continue;
+
+		debug("Required interface %s not supported", iface);
+		success = FALSE;
+		err_not_supported(connection, data->msg);
+		dbus_free_string_array(required);
+		goto done;
+	}
+
+	dbus_free_string_array(required);
+
+	path = data->device->object_path;
+
 	reply = dbus_message_new_method_return(data->msg);
 	if (!reply) {
 		success = FALSE;
@@ -374,25 +458,28 @@ static void finish_sdp(struct audio_sdp_data *data, gboolean success)
 		goto done;
 	}
 
-	path = data->device->object_path;
-
 	add_device(data->device);
+
+update:
 	g_slist_foreach(data->records, (GFunc) handle_record, data->device);
 
-	dbus_connection_emit_signal(connection, AUDIO_MANAGER_PATH,
-					AUDIO_MANAGER_INTERFACE,
-					"DeviceCreated",
-					DBUS_TYPE_STRING, &path,
-					DBUS_TYPE_INVALID);
+	if (reply) {
+		dbus_connection_emit_signal(connection, 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(connection, reply);
+		dbus_message_append_args(reply, DBUS_TYPE_STRING, &path,
+				DBUS_TYPE_INVALID);
+		send_message_and_unref(connection, reply);
+	}
 
 done:
 	if (!success)
 		free_device(data->device);
-	dbus_message_unref(data->msg);
+	if (data->msg)
+		dbus_message_unref(data->msg);
 	g_slist_foreach(data->handles, (GFunc) g_free, NULL);
 	g_slist_free(data->handles);
 	g_slist_foreach(data->records, (GFunc) sdp_record_free, NULL);
@@ -654,19 +741,21 @@ audio_device_t *manager_headset_connected(bdaddr_t *bda)
 	}
 
 	if (!device->headset)
-		device->headset = headset_init(device->object_path, NULL);
+		device->headset = headset_init(device->object_path, NULL, 0);
 
 	if (!device->headset)
 		return NULL;
 
 	path = device->object_path;
 
-	if (created)
+	if (created) {
 		dbus_connection_emit_signal(connection, AUDIO_MANAGER_PATH,
 						AUDIO_MANAGER_INTERFACE,
 						"DeviceCreated",
 						DBUS_TYPE_STRING, &path,
 						DBUS_TYPE_INVALID);
+		resolve_services(NULL, device);
+	}
 
 	dbus_connection_emit_signal(connection, AUDIO_MANAGER_PATH,
 					AUDIO_MANAGER_INTERFACE,
@@ -686,17 +775,62 @@ audio_device_t *manager_headset_connected(bdaddr_t *bda)
 	return device;
 }
 
+static gboolean device_supports_interface(audio_device_t *device,
+						const char *iface)
+{
+		if (strcmp(iface, AUDIO_HEADSET_INTERFACE) == 0)
+		       	return device->headset ? TRUE : FALSE;
+
+		if (strcmp(iface, AUDIO_GATEWAY_INTERFACE) == 0)
+			return device->gateway ? TRUE : FALSE;
+
+		if (strcmp(iface, AUDIO_SOURCE_INTERFACE) == 0)
+			return device->gateway ? TRUE : FALSE;
+
+		if (strcmp(iface, AUDIO_SINK_INTERFACE) == 0)
+			return device->sink ? TRUE : FALSE;
+
+		if (strcmp(iface, AUDIO_CONTROL_INTERFACE) == 0)
+			return device->control ? TRUE : FALSE;
+
+		if (strcmp(iface, AUDIO_TARGET_INTERFACE) == 0)
+			return device->target ? TRUE : FALSE;
+
+		debug("Unknown interface %s", iface);
+
+		return FALSE;
+}
+
+static gboolean device_matches(audio_device_t *device, char **interfaces)
+{
+	int i;
+
+	for (i = 0; interfaces[i]; i++) {
+		if (device_supports_interface(device, interfaces[i]))
+			continue;
+		debug("Device does not support interface %s", interfaces[i]);
+		return FALSE;
+	}
+
+	return TRUE;
+}
+
 static DBusHandlerResult am_create_device(DBusConnection *conn, DBusMessage *msg,
 						void *data)
 {
-	const char *address;
+	const char *address, *path;
+	char **required;
+	int required_len;
 	bdaddr_t bda;
 	audio_device_t *device;
+	DBusMessage *reply;
 	DBusError derr;
 
 	dbus_error_init(&derr);
 	dbus_message_get_args(msg, &derr,
 				DBUS_TYPE_STRING, &address,
+				DBUS_TYPE_ARRAY, DBUS_TYPE_STRING,
+				&required, &required_len,
 				DBUS_TYPE_INVALID);
 	if (dbus_error_is_set(&derr)) {
 		err_invalid_args(connection, msg, derr.message);
@@ -707,19 +841,29 @@ static DBusHandlerResult am_create_device(DBusConnection *conn, DBusMessage *msg
 	str2ba(address, &bda);
 
 	device = find_device(&bda);
-	if (device) {
-		const char *path = device->object_path;
-		DBusMessage *reply = dbus_message_new_method_return(msg);
-		if (!reply)
-			return DBUS_HANDLER_RESULT_NEED_MEMORY;
-		dbus_message_append_args(reply, DBUS_TYPE_STRING, &path,
-						DBUS_TYPE_INVALID);
-		return send_message_and_unref(conn, reply);
+	if (!device) {
+		device = create_device(&bda);
+		dbus_free_string_array(required);
+		return resolve_services(msg, device);
 	}
 
-	device = create_device(&bda);
+	if (!device_matches(device, required)) {
+		dbus_free_string_array(required);
+		return err_not_supported(conn, msg);
+	}
+
+	dbus_free_string_array(required);
+
+	path = device->object_path;
+
+	reply = dbus_message_new_method_return(msg);
+	if (!reply)
+		return DBUS_HANDLER_RESULT_NEED_MEMORY;
 
-	return resolve_services(msg, device);
+	dbus_message_append_args(reply, DBUS_TYPE_STRING, &path,
+					DBUS_TYPE_INVALID);
+
+	return send_message_and_unref(conn, reply);
 }
 
 static DBusHandlerResult am_list_devices(DBusConnection *conn,
@@ -728,7 +872,21 @@ static DBusHandlerResult am_list_devices(DBusConnection *conn,
 {
 	DBusMessageIter iter, array_iter;
 	DBusMessage *reply;
+	DBusError derr;
 	GSList *l;
+	char **required;
+	int required_len;
+
+	dbus_error_init(&derr);
+	dbus_message_get_args(msg, &derr,
+				DBUS_TYPE_ARRAY, DBUS_TYPE_STRING,
+				&required, &required_len,
+				DBUS_TYPE_INVALID);
+	if (dbus_error_is_set(&derr)) {
+		err_invalid_args(connection, msg, derr.message);
+		dbus_error_free(&derr);
+		return DBUS_HANDLER_RESULT_HANDLED;
+	}
 
 	reply = dbus_message_new_method_return(msg);
 	if (!reply)
@@ -743,6 +901,9 @@ static DBusHandlerResult am_list_devices(DBusConnection *conn,
 		audio_device_t *device = l->data;
 		const char *path;
 
+		if (!device_matches(device, required))
+			continue;
+
 		path = device->object_path;
 
 		dbus_message_iter_append_basic(&array_iter,
@@ -751,6 +912,8 @@ static DBusHandlerResult am_list_devices(DBusConnection *conn,
 
 	dbus_message_iter_close_container(&iter, &array_iter);
 
+	dbus_free_string_array(required);
+
 	return send_message_and_unref(connection, reply);
 }
 
@@ -786,12 +949,14 @@ static DBusHandlerResult am_create_headset(DBusConnection *conn, DBusMessage *ms
 		}
 	}
 
-	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");
+		device->headset = headset_init(device->object_path, NULL, 0);
+		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;
@@ -1005,11 +1170,11 @@ static DBusHandlerResult am_change_default_headset(DBusConnection *conn, DBusMes
 
 static DBusMethodVTable manager_methods[] = {
 	{ "CreateDevice",		am_create_device,
-		"s",	"s"		},
+		"sas",	"s"		},
 	{ "RemoveDevice",		am_remove_device,
 		"s",	""		},
 	{ "ListDevices",		am_list_devices,
-		"",	"as"	},
+		"as",	"as"	},
 	{ "CreateHeadset",		am_create_headset,
 		"s",	"s"		},
 	{ "RemoveHeadset",		am_remove_headset,
diff --git a/audio/manager.h b/audio/manager.h
index 45c5fa5f..78ec63ab 100644
--- a/audio/manager.h
+++ b/audio/manager.h
@@ -48,18 +48,30 @@
 #define AVRCP_REMOTE_UUID	"0000110E-0000-1000-8000-00805F9B34FB"
 #define AVRCP_TARGET_UUID	"0000110C-0000-1000-8000-00805F9B34FB"
 
+/* Move these to respective .h files once they exist */
+#define AUDIO_GATEWAY_INTERFACE	"org.bluez.audio.Gateway"
+#define AUDIO_SINK_INTERFACE	"org.bluez.audio.Sink"
+#define AUDIO_SOURCE_INTERFACE	"org.bluez.audio.Source"
+#define AUDIO_CONTROL_INTERFACE	"org.bluez.audio.Control"
+#define AUDIO_TARGET_INTERFACE	"org.bluez.audio.Target"
+typedef struct gateway gateway_t;
+typedef struct sink sink_t;
+typedef struct source source_t;
+typedef struct control control_t;
+typedef struct target target_t;
+
 typedef struct audio_device {
 	char object_path[128];
 	bdaddr_t bda;
 
 	headset_t *headset;
-/*
-	audio_gw_t *audio_gw;
+
+	gateway_t *gateway;
 	sink_t *sink;
 	source_t *source;
 	control_t *control;
 	target_t *target;
-*/
+
 } audio_device_t;
 
 audio_device_t *manager_headset_connected(bdaddr_t *bda);