Integrate A2DP work from Johan's and Luiz's GIT trees

17 files changed, 4451 insertions, 343 deletions

diff --git a/audio/a2dp.c b/audio/a2dp.c
index db0ed7cb..3afc133d 100644
--- a/audio/a2dp.c
+++ b/audio/a2dp.c
@@ -25,4 +25,615 @@
 #include <config.h>
 #endif
 
+#include <stdlib.h>
+
+#include <dbus/dbus.h>
+#include <glib.h>
+
+#include <bluetooth/sdp.h>
+#include <bluetooth/sdp_lib.h>
+
+#include "logging.h"
+#include "manager.h"
+#include "avdtp.h"
 #include "a2dp.h"
+
+static DBusConnection *connection = NULL;
+
+static uint32_t sink_record_id = 0;
+static uint32_t source_record_id = 0;
+
+static struct avdtp_local_sep *sink_sep = NULL;
+static struct avdtp_local_sep *source_sep = NULL;
+
+static gboolean setconf_ind(struct avdtp_local_sep *sep,
+				struct avdtp_stream *stream,
+				uint8_t int_seid, GSList *caps,
+				uint8_t *err)
+{
+	if (sep == sink_sep)
+		debug("SBC Sink: Set_Configuration_Ind");
+	else
+		debug("SBC Source: Set_Configuration_Ind");
+
+	return TRUE;
+}
+
+static gboolean getcap_ind(struct avdtp_local_sep *sep,
+				GSList **caps, uint8_t *err)
+{
+	struct avdtp_service_capability *media_transport, *media_codec;
+	struct sbc_codec_cap sbc_cap;
+
+	if (sep == sink_sep)
+		debug("SBC Sink: Get_Capability_Ind");
+	else
+		debug("SBC Source: Get_Capability_Ind");
+
+	*caps = NULL;
+
+	media_transport = avdtp_service_cap_new(AVDTP_MEDIA_TRANSPORT,
+						NULL, 0);
+
+	*caps = g_slist_append(*caps, media_transport);
+
+	memset(&sbc_cap, 0, sizeof(struct sbc_codec_cap));
+
+	sbc_cap.cap.media_type = AVDTP_MEDIA_TYPE_AUDIO;
+	sbc_cap.cap.media_codec_type = A2DP_CODEC_SBC;
+
+	sbc_cap.frequency = ( A2DP_SAMPLING_FREQ_48000 |
+				A2DP_SAMPLING_FREQ_44100 |
+				A2DP_SAMPLING_FREQ_32000 |
+				A2DP_SAMPLING_FREQ_16000 );
+
+	sbc_cap.channel_mode = ( A2DP_CHANNEL_MODE_JOINT_STEREO |
+					A2DP_CHANNEL_MODE_STEREO |
+					A2DP_CHANNEL_MODE_DUAL_CHANNEL |
+					A2DP_CHANNEL_MODE_MONO );
+
+	sbc_cap.block_length = ( A2DP_BLOCK_LENGTH_16 |
+					A2DP_BLOCK_LENGTH_12 |
+					A2DP_BLOCK_LENGTH_8 |
+					A2DP_BLOCK_LENGTH_4 );
+
+	sbc_cap.subbands = ( A2DP_SUBBANDS_8 | A2DP_SUBBANDS_4 );
+
+	sbc_cap.allocation_method = ( A2DP_ALLOCATION_LOUDNESS |
+					A2DP_ALLOCATION_SNR );
+
+	sbc_cap.min_bitpool = 2;
+	sbc_cap.max_bitpool = 250;
+
+	media_codec = avdtp_service_cap_new(AVDTP_MEDIA_CODEC, &sbc_cap,
+						sizeof(sbc_cap));
+
+	*caps = g_slist_append(*caps, media_codec);
+
+	return TRUE;
+}
+
+static void setconf_cfm(struct avdtp_local_sep *sep,
+				struct avdtp_stream *stream)
+{
+	if (sep == sink_sep)
+		debug("SBC Sink: Set_Configuration_Cfm");
+	else
+		debug("SBC Source: Set_Configuration_Cfm");
+}
+
+static gboolean getconf_ind(struct avdtp_local_sep *sep, uint8_t *err)
+{
+	if (sep == sink_sep)
+		debug("SBC Sink: Get_Configuration_Ind");
+	else
+		debug("SBC Source: Get_Configuration_Ind");
+	return TRUE;
+}
+
+static void getconf_cfm(struct avdtp_local_sep *sep,
+			struct avdtp_stream *stream)
+{
+	if (sep == sink_sep)
+		debug("SBC Sink: Set_Configuration_Cfm");
+	else
+		debug("SBC Source: Set_Configuration_Cfm");
+}
+
+static gboolean open_ind(struct avdtp_local_sep *sep,
+				struct avdtp_stream *stream, uint8_t *err)
+{
+	if (sep == sink_sep)
+		debug("SBC Sink: Open_Ind");
+	else
+		debug("SBC Source: Open_Ind");
+	return TRUE;
+}
+
+static void open_cfm(struct avdtp_local_sep *sep, struct avdtp_stream *stream)
+{
+	if (sep == sink_sep)
+		debug("SBC Sink: Open_Cfm");
+	else
+		debug("SBC Source: Open_Cfm");
+}
+
+static gboolean start_ind(struct avdtp_local_sep *sep,
+				struct avdtp_stream *stream, uint8_t *err)
+{
+	if (sep == sink_sep)
+		debug("SBC Sink: Start_Ind");
+	else
+		debug("SBC Source: Start_Ind");
+	return TRUE;
+}
+
+static void start_cfm(struct avdtp_local_sep *sep, struct avdtp_stream *stream)
+{
+	if (sep == sink_sep)
+		debug("SBC Sink: Start_Cfm");
+	else
+		debug("SBC Source: Start_Cfm");
+}
+
+static gboolean suspend_ind(struct avdtp_local_sep *sep,
+				struct avdtp_stream *stream, uint8_t *err)
+{
+	if (sep == sink_sep)
+		debug("SBC Sink: Suspend_Ind");
+	else
+		debug("SBC Source: Suspend_Ind");
+	return TRUE;
+}
+
+static void suspend_cfm(struct avdtp_local_sep *sep,
+				struct avdtp_stream *stream)
+{
+	if (sep == sink_sep)
+		debug("SBC Sink: Suspend_Cfm");
+	else
+		debug("SBC Source: Suspend_Cfm");
+}
+
+static gboolean close_ind(struct avdtp_local_sep *sep,
+				struct avdtp_stream *stream, uint8_t *err)
+{
+	if (sep == sink_sep)
+		debug("SBC Sink: Close_Ind");
+	else
+		debug("SBC Source: Close_Ind");
+	return TRUE;
+}
+
+static void close_cfm(struct avdtp_local_sep *sep, struct avdtp_stream *stream)
+{
+	if (sep == sink_sep)
+		debug("SBC Sink: Close_Cfm");
+	else
+		debug("SBC Source: Close_Cfm");
+}
+
+static gboolean abort_ind(struct avdtp_local_sep *sep,
+				struct avdtp_stream *stream, uint8_t *err)
+{
+	if (sep == sink_sep)
+		debug("SBC Sink: Abort_Ind");
+	else
+		debug("SBC Source: Abort_Ind");
+	return TRUE;
+}
+
+static void abort_cfm(struct avdtp_local_sep *sep, struct avdtp_stream *stream)
+{
+	if (sep == sink_sep)
+		debug("SBC Sink: Abort_Cfm");
+	else
+		debug("SBC Source: Abort_Cfm");
+}
+
+static gboolean reconf_ind(struct avdtp_local_sep *sep, uint8_t *err)
+{
+	if (sep == sink_sep)
+		debug("SBC Sink: ReConfigure_Ind");
+	else
+		debug("SBC Source: ReConfigure_Ind");
+	return TRUE;
+}
+
+static void reconf_cfm(struct avdtp_local_sep *sep)
+{
+	if (sep == sink_sep)
+		debug("SBC Sink: ReConfigure_Cfm");
+	else
+		debug("SBC Source: ReConfigure_Cfm");
+}
+
+static struct avdtp_sep_cfm cfm = {
+	.set_configuration	= setconf_cfm,
+	.get_configuration	= getconf_cfm,
+	.open			= open_cfm,
+	.start			= start_cfm,
+	.suspend		= suspend_cfm,
+	.close			= close_cfm,
+	.abort			= abort_cfm,
+	.reconfigure		= reconf_cfm
+};
+
+static struct avdtp_sep_ind ind = {
+	.get_capability		= getcap_ind,
+	.set_configuration	= setconf_ind,
+	.get_configuration	= getconf_ind,
+	.open			= open_ind,
+	.start			= start_ind,
+	.suspend		= suspend_ind,
+	.close			= close_ind,
+	.abort			= abort_ind,
+	.reconfigure		= reconf_ind
+};
+
+static int a2dp_source_record(sdp_buf_t *buf)
+{
+	sdp_list_t *svclass_id, *pfseq, *apseq, *root;
+	uuid_t root_uuid, l2cap, avdtp, a2src;
+	sdp_profile_desc_t profile[1];
+	sdp_list_t *aproto, *proto[2];
+	sdp_record_t record;
+	sdp_data_t *psm, *version, *features;
+	uint16_t lp = AVDTP_UUID, ver = 0x0100, feat = 0x000F;
+	int ret = 0;
+
+	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(&a2src, AUDIO_SOURCE_SVCLASS_ID);
+	svclass_id = sdp_list_append(0, &a2src);
+	sdp_set_service_classes(&record, svclass_id);
+
+	sdp_uuid16_create(&profile[0].uuid, ADVANCED_AUDIO_PROFILE_ID);
+	profile[0].version = 0x0100;
+	pfseq = sdp_list_append(0, &profile[0]);
+	sdp_set_profile_descs(&record, pfseq);
+
+	sdp_uuid16_create(&l2cap, L2CAP_UUID);
+	proto[0] = sdp_list_append(0, &l2cap);
+	psm = sdp_data_alloc(SDP_UINT16, &lp);
+	proto[0] = sdp_list_append(proto[0], psm);
+	apseq = sdp_list_append(0, proto[0]);
+
+	sdp_uuid16_create(&avdtp, AVDTP_UUID);
+	proto[1] = sdp_list_append(0, &avdtp);
+	version = sdp_data_alloc(SDP_UINT16, &ver);
+	proto[1] = sdp_list_append(proto[1], version);
+	apseq = sdp_list_append(apseq, proto[1]);
+
+	aproto = sdp_list_append(0, apseq);
+	sdp_set_access_protos(&record, aproto);
+
+	features = sdp_data_alloc(SDP_UINT16, &feat);
+	sdp_attr_add(&record, SDP_ATTR_SUPPORTED_FEATURES, features);
+
+	sdp_set_info_attr(&record, "Audio Source", 0, 0);
+
+	if (sdp_gen_record_pdu(&record, buf) < 0)
+		ret = -1;
+	else
+		ret = 0;
+
+	free(psm);
+	free(version);
+	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 a2dp_sink_record(sdp_buf_t *buf)
+{
+	return 0;
+}
+
+int a2dp_init(DBusConnection *conn, gboolean enable_sink, gboolean enable_source)
+{
+	sdp_buf_t buf;
+
+	if (!enable_sink && !enable_source)
+		return 0;
+
+	connection = dbus_connection_ref(conn);
+
+	avdtp_init();
+
+	if (enable_sink) {
+		source_sep = avdtp_register_sep(AVDTP_SEP_TYPE_SOURCE,
+						AVDTP_MEDIA_TYPE_AUDIO,
+						&ind, &cfm);
+		if (source_sep == NULL)
+			return -1;
+
+		if (a2dp_source_record(&buf) < 0) {
+			error("Unable to allocate new service record");
+			return -1;
+		}
+
+		source_record_id = add_service_record(conn, &buf);
+		free(buf.data);
+		if (!source_record_id) {
+			error("Unable to register A2DP Source service record");
+			return -1;
+		}
+	}
+
+	if (enable_source) {
+		sink_sep = avdtp_register_sep(AVDTP_SEP_TYPE_SINK,
+						AVDTP_MEDIA_TYPE_AUDIO,
+						&ind, &cfm);
+		if (sink_sep == NULL)
+			return -1;
+
+		if (a2dp_sink_record(&buf) < 0) {
+			error("Unable to allocate new service record");
+			return -1;
+		}
+
+		sink_record_id = add_service_record(conn, &buf);
+		free(buf.data);
+		if (!sink_record_id) {
+			error("Unable to register A2DP Sink service record");
+			return -1;
+		}
+	}
+
+	return 0;
+}
+
+void a2dp_exit()
+{
+	if (sink_sep)
+		avdtp_unregister_sep(sink_sep);
+
+	if (source_sep)
+		avdtp_unregister_sep(source_sep);
+
+	if (source_record_id) {
+		remove_service_record(connection, source_record_id);
+		source_record_id = 0;
+	}
+
+	if (sink_record_id) {
+		remove_service_record(connection, sink_record_id);
+		sink_record_id = 0;
+	}
+
+	dbus_connection_unref(connection);
+}
+
+static uint8_t default_bitpool(uint8_t freq, uint8_t mode) {
+	switch (freq) {
+	case A2DP_SAMPLING_FREQ_16000:
+	case A2DP_SAMPLING_FREQ_32000:
+		return 53;
+	case A2DP_SAMPLING_FREQ_44100:
+		switch (mode) {
+		case A2DP_CHANNEL_MODE_MONO:
+		case A2DP_CHANNEL_MODE_DUAL_CHANNEL:
+			return 31;
+		case A2DP_CHANNEL_MODE_STEREO:
+		case A2DP_CHANNEL_MODE_JOINT_STEREO:
+			return 53;
+		default:
+			error("Invalid channel mode %u", mode);
+			return 53;
+		}
+	case A2DP_SAMPLING_FREQ_48000:
+		switch (mode) {
+		case A2DP_CHANNEL_MODE_MONO:
+		case A2DP_CHANNEL_MODE_DUAL_CHANNEL:
+			return 29;
+		case A2DP_CHANNEL_MODE_STEREO:
+		case A2DP_CHANNEL_MODE_JOINT_STEREO:
+			return 51;
+		default:
+			error("Invalid channel mode %u", mode);
+			return 51;
+		}
+	default:
+		error("Invalid sampling freq %u", freq);
+		return 53;
+	}
+}
+
+static gboolean select_sbc_params(struct sbc_codec_cap *cap,
+					struct sbc_codec_cap *supported)
+{
+	uint max_bitpool, min_bitpool;
+
+	memset(cap, 0, sizeof(struct sbc_codec_cap));
+
+	cap->cap.media_type = AVDTP_MEDIA_TYPE_AUDIO;
+	cap->cap.media_codec_type = A2DP_CODEC_SBC;
+
+	if (supported->frequency & A2DP_SAMPLING_FREQ_48000)
+		cap->frequency = A2DP_SAMPLING_FREQ_48000;
+	else if (supported->frequency & A2DP_SAMPLING_FREQ_44100)
+		cap->frequency = A2DP_SAMPLING_FREQ_44100;
+	else if (supported->frequency & A2DP_SAMPLING_FREQ_32000)
+		cap->frequency = A2DP_SAMPLING_FREQ_32000;
+	else if (supported->frequency & A2DP_SAMPLING_FREQ_16000)
+		cap->frequency = A2DP_SAMPLING_FREQ_16000;
+	else {
+		error("No supported frequencies");
+		return FALSE;
+	}
+
+	if (supported->channel_mode & A2DP_CHANNEL_MODE_JOINT_STEREO)
+		cap->channel_mode = A2DP_CHANNEL_MODE_JOINT_STEREO;
+	else if (supported->channel_mode & A2DP_CHANNEL_MODE_STEREO)
+		cap->channel_mode = A2DP_CHANNEL_MODE_STEREO;
+	else if (supported->channel_mode & A2DP_CHANNEL_MODE_DUAL_CHANNEL)
+		cap->channel_mode = A2DP_CHANNEL_MODE_DUAL_CHANNEL;
+	else if (supported->channel_mode & A2DP_CHANNEL_MODE_MONO)
+		cap->channel_mode = A2DP_CHANNEL_MODE_MONO;
+	else {
+		error("No supported channel modes");
+		return FALSE;
+	}
+
+	if (supported->block_length & A2DP_BLOCK_LENGTH_16)
+		cap->block_length = A2DP_BLOCK_LENGTH_16;
+	else if (supported->block_length & A2DP_BLOCK_LENGTH_12)
+		cap->block_length = A2DP_BLOCK_LENGTH_12;
+	else if (supported->block_length & A2DP_BLOCK_LENGTH_8)
+		cap->block_length = A2DP_BLOCK_LENGTH_8;
+	else if (supported->block_length & A2DP_BLOCK_LENGTH_4)
+		cap->block_length = A2DP_BLOCK_LENGTH_4;
+	else {
+		error("No supported block lengths");
+		return FALSE;
+	}
+
+	if (supported->subbands & A2DP_SUBBANDS_8)
+		cap->subbands = A2DP_SUBBANDS_8;
+	else if (supported->subbands & A2DP_SUBBANDS_4)
+		cap->subbands = A2DP_SUBBANDS_4;
+	else {
+		error("No supported subbands");
+		return FALSE;
+	}
+
+	if (supported->allocation_method & A2DP_ALLOCATION_LOUDNESS)
+		cap->allocation_method = A2DP_ALLOCATION_LOUDNESS;
+	else if (supported->allocation_method & A2DP_ALLOCATION_SNR)
+		cap->allocation_method = A2DP_ALLOCATION_SNR;
+
+	min_bitpool = MIN(default_bitpool(cap->frequency, cap->channel_mode),
+				supported->min_bitpool);
+	max_bitpool = MIN(default_bitpool(cap->frequency, cap->channel_mode),
+				supported->max_bitpool);
+
+	cap->min_bitpool = min_bitpool;
+	cap->max_bitpool = max_bitpool;
+
+	return TRUE;
+}
+
+gboolean a2dp_select_capabilities(struct avdtp_remote_sep *rsep, GSList **caps)
+{
+	struct avdtp_service_capability *media_transport, *media_codec;
+	struct sbc_codec_cap sbc_cap, *acp_sbc;
+
+	media_codec = avdtp_get_codec(rsep);
+	if (!media_codec)
+		return FALSE;
+
+	acp_sbc = (void *) media_codec->data;
+
+	media_transport = avdtp_service_cap_new(AVDTP_MEDIA_TRANSPORT,
+						NULL, 0);
+
+	*caps = g_slist_append(*caps, media_transport);
+
+	select_sbc_params(&sbc_cap, acp_sbc);
+
+	media_codec = avdtp_service_cap_new(AVDTP_MEDIA_CODEC, &sbc_cap,
+						sizeof(sbc_cap));
+
+	*caps = g_slist_append(*caps, media_codec);
+
+	return TRUE;
+}
+
+gboolean a2dp_get_config(struct avdtp_stream *stream, struct ipc_data_cfg **cfg)
+{
+	struct avdtp_service_capability *cap;
+	struct avdtp_media_codec_capability *codec_cap = NULL;
+	struct sbc_codec_cap *sbc_cap;
+	struct ipc_data_cfg *rsp;
+	struct ipc_codec_sbc *sbc;
+	GSList *caps;
+
+	rsp = g_malloc0(sizeof(struct ipc_data_cfg) +
+				sizeof(struct ipc_codec_sbc));
+	rsp->fd = -1;
+	sbc = (void *) rsp->data;
+
+	if (!avdtp_stream_get_transport(stream, &rsp->fd, &rsp->pkt_len,
+					&caps)) {
+		g_free(rsp);
+		return FALSE;
+	}
+
+	for (; caps; caps = g_slist_next(caps)) {
+		cap = caps->data;
+		if (cap->category == AVDTP_MEDIA_CODEC) {
+			codec_cap = (void *) cap->data;
+			break;
+		}
+	}
+
+	if (codec_cap == NULL) {
+		g_free(rsp);
+		return FALSE;
+	}
+
+	rsp->fd_opt = CFG_FD_OPT_WRITE;
+
+	*cfg = rsp;
+
+	if (codec_cap->media_codec_type != A2DP_CODEC_SBC)
+		return TRUE;
+
+	sbc_cap = (struct sbc_codec_cap *) codec_cap;
+	rsp->channels = sbc_cap->channel_mode ==
+				A2DP_CHANNEL_MODE_MONO ? 1 : 2;
+	rsp->channel_mode = sbc_cap->channel_mode;
+	rsp->sample_size = 2;
+
+	switch (sbc_cap->frequency) {
+		case A2DP_SAMPLING_FREQ_16000:
+			rsp->rate = 16000;
+			break;
+		case A2DP_SAMPLING_FREQ_32000:
+			rsp->rate = 32000;
+			break;
+		case A2DP_SAMPLING_FREQ_44100:
+			rsp->rate = 44100;
+			break;
+		case A2DP_SAMPLING_FREQ_48000:
+			rsp->rate = 48000;
+			break;
+	}
+
+	rsp->codec = CFG_CODEC_SBC;
+	sbc->allocation = sbc_cap->allocation_method == A2DP_ALLOCATION_SNR ?
+				0x01 : 0x00;
+	sbc->subbands = sbc_cap->subbands == A2DP_SUBBANDS_4 ? 4 : 8;
+
+	switch (sbc_cap->block_length) {
+		case A2DP_BLOCK_LENGTH_4:
+			sbc->blocks = 4;
+			break;
+		case A2DP_BLOCK_LENGTH_8:
+			sbc->blocks = 8;
+			break;
+		case A2DP_BLOCK_LENGTH_12:
+			sbc->blocks = 12;
+			break;
+		case A2DP_BLOCK_LENGTH_16:
+			sbc->blocks = 16;
+			break;
+	}
+
+	sbc->bitpool = sbc_cap->max_bitpool;
+
+	return TRUE;
+}
diff --git a/audio/a2dp.h b/audio/a2dp.h
index 153a1731..7e53f7a9 100644
--- a/audio/a2dp.h
+++ b/audio/a2dp.h
@@ -21,3 +21,51 @@
  *
  */
 
+#include <dbus.h>
+#include <glib.h>
+#include "avdtp.h"
+
+#define A2DP_CODEC_SBC			0x00
+#define A2DP_CODEC_MPEG12		0x01
+#define A2DP_CODEC_MPEG24		0x02
+#define A2DP_CODEC_ATRAC		0x03
+
+#define A2DP_SAMPLING_FREQ_16000	(1 << 3)
+#define A2DP_SAMPLING_FREQ_32000	(1 << 2)
+#define A2DP_SAMPLING_FREQ_44100	(1 << 1)
+#define A2DP_SAMPLING_FREQ_48000	1
+
+#define A2DP_CHANNEL_MODE_MONO		(1 << 3)
+#define A2DP_CHANNEL_MODE_DUAL_CHANNEL	(1 << 2)
+#define A2DP_CHANNEL_MODE_STEREO	(1 << 1)
+#define A2DP_CHANNEL_MODE_JOINT_STEREO	1
+
+#define A2DP_BLOCK_LENGTH_4		(1 << 3)
+#define A2DP_BLOCK_LENGTH_8		(1 << 2)
+#define A2DP_BLOCK_LENGTH_12		(1 << 1)
+#define A2DP_BLOCK_LENGTH_16		1
+
+#define A2DP_SUBBANDS_4			(1 << 1)
+#define A2DP_SUBBANDS_8			1
+
+#define A2DP_ALLOCATION_SNR		(1 << 1)
+#define A2DP_ALLOCATION_LOUDNESS	1
+
+struct sbc_codec_cap {
+	struct avdtp_media_codec_capability cap;
+	uint8_t channel_mode:4;
+	uint8_t frequency:4;
+	uint8_t allocation_method:2;
+	uint8_t subbands:2;
+	uint8_t block_length:4;
+	uint8_t min_bitpool;
+	uint8_t max_bitpool;
+} __attribute__ ((packed));
+
+int a2dp_init(DBusConnection *conn, gboolean enable_sink,
+			gboolean enable_source);
+void a2dp_exit(void);
+
+gboolean a2dp_select_capabilities(struct avdtp_remote_sep *rsep, GSList **caps);
+
+gboolean a2dp_get_config(struct avdtp_stream *stream, struct ipc_data_cfg **cfg);
diff --git a/audio/avdtp.c b/audio/avdtp.c
index 2f4ab59e..8615a18a 100644
--- a/audio/avdtp.c
+++ b/audio/avdtp.c
@@ -24,3 +24,2177 @@
 #ifdef HAVE_CONFIG_H
 #include <config.h>
 #endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <unistd.h>
+#include <assert.h>
+#include <signal.h>
+#include <netinet/in.h>
+
+#include <glib.h>
+
+#include "avdtp.h"
+#include "logging.h"
+#include "dbus.h"
+
+#include <bluetooth/l2cap.h>
+
+#define AVDTP_PSM 25
+
+#define MAX_SEID 0x3E
+
+#define AVDTP_DISCOVER				0x01
+#define AVDTP_GET_CAPABILITIES			0x02
+#define AVDTP_SET_CONFIGURATION			0x03
+#define AVDTP_GET_CONFIGURATION			0x04
+#define AVDTP_RECONFIGURE			0x05
+#define AVDTP_OPEN				0x06
+#define AVDTP_START				0x07
+#define AVDTP_CLOSE				0x08
+#define AVDTP_SUSPEND				0x09
+#define AVDTP_ABORT				0x0A
+#define AVDTP_SECURITY_CONTROL			0x0B
+
+#define AVDTP_PKT_TYPE_SINGLE			0x00
+#define AVDTP_PKT_TYPE_START			0x01
+#define AVDTP_PKT_TYPE_CONTINUE			0x02
+#define AVDTP_PKT_TYPE_END			0x03
+
+#define AVDTP_MSG_TYPE_COMMAND			0x00
+#define AVDTP_MSG_TYPE_ACCEPT			0x02
+#define AVDTP_MSG_TYPE_REJECT			0x03
+
+#define REQ_TIMEOUT 2000
+#define DISCONNECT_TIMEOUT 5000
+
+typedef enum {
+	AVDTP_ERROR_ERRNO,
+	AVDTP_ERROR_ERROR_CODE
+} avdtp_error_type_t;
+
+typedef enum {
+	AVDTP_SESSION_STATE_DISCONNECTED,
+	AVDTP_SESSION_STATE_CONNECTING,
+	AVDTP_SESSION_STATE_CONNECTED
+} avdtp_session_state_t;
+
+struct avdtp_header {
+	uint8_t message_type:2;
+	uint8_t packet_type:2;
+	uint8_t transaction:4;
+	uint8_t signal_id:6;
+	uint8_t rfa0:2;
+} __attribute__ ((packed));
+
+typedef struct seid_info {
+	uint8_t rfa0:1;
+	uint8_t inuse:1;
+	uint8_t seid:6;
+	uint8_t rfa2:3;
+	uint8_t type:1;
+	uint8_t media_type:4;
+} __attribute__ ((packed)) seid_info_t;
+
+/* packets */
+
+struct discover_req {
+	struct avdtp_header header;
+} __attribute__ ((packed));
+
+struct discover_resp {
+	struct avdtp_header header;
+	struct seid_info seps[0];
+} __attribute__ ((packed));
+
+struct getcap_resp {
+	struct avdtp_header header;
+	uint8_t caps[0];
+} __attribute__ ((packed));
+
+struct seid_req {
+	struct avdtp_header header;
+	uint8_t rfa0:2;
+	uint8_t acp_seid:6;
+} __attribute__ ((packed));
+
+struct seid_rej {
+	struct avdtp_header header;
+	uint8_t error;
+} __attribute__ ((packed));
+
+struct setconf_req {
+	struct avdtp_header header;
+
+	uint8_t rfa0:2;
+	uint8_t acp_seid:6;
+	uint8_t rfa1:2;
+	uint8_t int_seid:6;
+
+	uint8_t caps[0];
+} __attribute__ ((packed));
+
+struct setconf_resp {
+	struct avdtp_header header;
+} __attribute__ ((packed));
+
+struct conf_rej {
+	struct avdtp_header header;
+	uint8_t category;
+	uint8_t error;
+} __attribute__ ((packed));
+
+struct stream_rej {
+	struct avdtp_header header;
+	uint8_t rfa0;
+	uint8_t acp_seid:6;
+	uint8_t error_code;
+} __attribute__ ((packed));
+
+struct reconf_req {
+	struct avdtp_header header;
+
+	uint8_t rfa0:2;
+	uint8_t acp_seid:6;
+
+	uint8_t serv_cap;
+	uint8_t serv_cap_len;
+
+	uint8_t caps[0];
+} __attribute__ ((packed));
+
+struct reconf_resp {
+	struct avdtp_header header;
+} __attribute__ ((packed));
+
+struct abort_resp {
+	struct avdtp_header header;
+} __attribute__ ((packed));
+
+struct stream_pause_resp {
+	struct avdtp_header header;
+	uint8_t rfa0:2;
+	uint8_t acp_seid:6;
+	uint8_t error;
+} __attribute__ ((packed));
+
+struct avdtp_general_rej {
+	uint8_t message_type:2;
+	uint8_t packet_type:2;
+	uint8_t transaction:4;
+	uint8_t rfa0;
+} __attribute__ ((packed));
+
+struct pending_req {
+	struct avdtp_header *msg;
+	int msg_size;
+	struct avdtp_stream *stream; /* Set if the request targeted a stream */
+	guint timeout;
+};
+
+struct avdtp_remote_sep {
+	uint8_t seid;
+	uint8_t type;
+	uint8_t media_type;
+	struct avdtp_service_capability *codec;
+	GSList *caps; /* of type struct avdtp_service_capability */
+	struct avdtp_stream *stream;
+};
+
+struct avdtp_local_sep {
+	avdtp_state_t state;
+	struct avdtp_stream *stream;
+	struct seid_info info;
+	uint8_t codec;
+	GSList *caps;
+	struct avdtp_sep_ind *ind;
+	struct avdtp_sep_cfm *cfm;
+	void *data;
+};
+
+struct avdtp_stream {
+	int sock;
+	uint16_t mtu;
+	struct avdtp *session;
+	struct avdtp_local_sep *lsep;
+	struct avdtp_remote_sep *rsep;
+	GSList *caps;
+	avdtp_stream_state_cb cb;
+	void *user_data;
+	guint io;		/* Transport GSource ID */
+	guint close_timer;	/* Waiting for other side to close transport */
+	gboolean open_acp;	/* If we are in ACT role for Open */
+	gboolean close_int;	/* If we are in INT role for Close */
+};
+
+/* Structure describing an AVDTP connection between two devices */
+struct avdtp {
+	int ref;
+
+	bdaddr_t src;
+	bdaddr_t dst;
+
+	avdtp_session_state_t last_state;
+	avdtp_session_state_t state;
+
+	guint io;
+	int sock;
+
+	GSList *seps; /* Elements of type struct avdtp_remote_sep * */
+
+	GSList *streams; /* Elements of type struct avdtp_stream * */
+
+	GSList *req_queue; /* Elements of type struct pending_req * */
+	GSList *prio_queue; /* Same as req_queue but is processed before it */
+
+	struct avdtp_stream *pending_open;
+
+	uint16_t mtu;
+	char *buf;
+
+	avdtp_discover_cb_t discov_cb;
+	void *user_data;
+
+	struct pending_req *req;
+
+	guint dc_timer;
+};
+
+struct avdtp_error {
+	avdtp_error_type_t type;
+	union {
+		uint8_t error_code;
+		int posix_errno;
+	} err;
+};
+
+static uint8_t free_seid = 1;
+static GSList *local_seps = NULL;
+
+static GIOChannel *avdtp_server = NULL;
+
+static GSList *sessions = NULL;
+
+static int send_request(struct avdtp *session, gboolean priority,
+			struct avdtp_stream *stream, void *buffer, int size);
+static gboolean avdtp_parse_resp(struct avdtp *session,
+					struct avdtp_stream *stream,
+					struct avdtp_header *header, int size);
+static gboolean avdtp_parse_rej(struct avdtp *session,
+				struct avdtp_stream *stream,
+				struct avdtp_header *header, int size);
+static int process_queue(struct avdtp *session);
+
+static const char *avdtp_statestr(avdtp_state_t state)
+{
+	switch (state) {
+	case AVDTP_STATE_IDLE:
+		return "IDLE";
+	case AVDTP_STATE_CONFIGURED:
+		return "CONFIGURED";
+	case AVDTP_STATE_OPEN:
+		return "OPEN";
+	case AVDTP_STATE_STREAMING:
+		return "STREAMING";
+	case AVDTP_STATE_CLOSING:
+		return "CLOSING";
+	case AVDTP_STATE_ABORTING:
+		return "ABORTING";
+	default:
+		return "<unknown state>";
+	}
+}
+
+static gboolean avdtp_send(struct avdtp *session, void *data, int len)
+{
+	int ret;
+
+	ret = send(session->sock, data, len, 0);
+
+	if (ret < 0)
+		ret = -errno;
+	else if (ret != len)
+		ret = -EIO;
+
+	if (ret < 0) {
+		error("avdtp_send: %s (%d)", strerror(-ret), -ret);
+		return FALSE;
+	}
+
+	return TRUE;
+}
+
+static void pending_req_free(struct pending_req *req)
+{
+	if (req->timeout)
+		g_source_remove(req->timeout);
+	g_free(req->msg);
+	g_free(req);
+}
+
+#if 0
+static gboolean stream_close_timeout(gpointer user_data)
+{
+	struct avdtp_stream *stream = user_data;
+
+	close(stream->sock);
+
+	return FALSE;
+}
+#endif
+
+static gboolean disconnect_timeout(gpointer user_data)
+{
+	struct avdtp *session = user_data;
+
+	assert(session->ref == 1);
+
+	sessions = g_slist_remove(sessions, session);
+
+	session->dc_timer = 0;
+
+	avdtp_unref(session);
+
+	return FALSE;
+}
+
+static void remove_disconnect_timer(struct avdtp *session)
+{
+	g_source_remove(session->dc_timer);
+	session->dc_timer = 0;
+}
+
+static void set_disconnect_timer(struct avdtp *session)
+{
+	if (session->dc_timer)
+		remove_disconnect_timer(session);
+
+	session->dc_timer = g_timeout_add(DISCONNECT_TIMEOUT,
+						disconnect_timeout, session);
+}
+
+static void avdtp_error_init(struct avdtp_error *err, uint8_t type, int id)
+{
+	err->type = type;
+	switch (type) {
+	case AVDTP_ERROR_ERRNO:
+		err->err.posix_errno = id;
+		break;
+	case AVDTP_ERROR_ERROR_CODE:
+		err->err.error_code = id;
+		break;
+	}
+}
+
+static void stream_free(struct avdtp_stream *stream)
+{
+	stream->lsep->info.inuse = 0;
+	stream->lsep->stream = NULL;
+	stream->rsep->stream = NULL;
+	if (stream->caps) {
+		g_slist_foreach(stream->caps, (GFunc) g_free, NULL);
+		g_slist_free(stream->caps);
+	}
+	g_free(stream);
+}
+
+static void avdtp_sep_set_state(struct avdtp *session,
+				struct avdtp_local_sep *sep,
+				avdtp_state_t state)
+{
+	struct avdtp_stream *stream = sep->stream;
+	avdtp_state_t old_state;
+
+	if (sep->state == state)
+		return;
+
+	debug("stream state changed: %s -> %s", avdtp_statestr(sep->state),
+			avdtp_statestr(state));
+
+	old_state = sep->state;
+	sep->state = state;
+
+	if (stream && stream->cb)
+		stream->cb(stream, old_state, state, NULL,
+				stream->user_data);
+
+	if (state == AVDTP_STATE_IDLE) {
+		session->streams = g_slist_remove(session->streams, stream);
+		stream_free(stream);
+	}
+}
+
+static void finalize_discovery(struct avdtp *session, int err)
+{
+	if (!session->discov_cb)
+		return;
+
+	session->discov_cb(session, session->seps, err,
+				session->user_data);
+
+	session->discov_cb = NULL;
+	session->user_data = NULL;
+}
+
+static void release_stream(struct avdtp_stream *stream, struct avdtp *session)
+{
+	if (stream->sock >= 0)
+		close(stream->sock);
+	if (stream->io)
+		g_source_remove(stream->io);
+	avdtp_sep_set_state(session, stream->lsep, AVDTP_STATE_IDLE);
+}
+
+static void connection_lost(struct avdtp *session)
+{
+	if (session->state == AVDTP_SESSION_STATE_CONNECTED) {
+		char address[18];
+
+		ba2str(&session->dst, address);
+		debug("Disconnected from %s", address);
+	}
+
+	if (session->discov_cb)
+		finalize_discovery(session, -ECONNABORTED);
+
+	if (session->sock >= 0) {
+		close(session->sock);
+		session->sock = -1;
+	}
+
+	session->state = AVDTP_SESSION_STATE_DISCONNECTED;
+
+	if (session->io) {
+		g_source_remove(session->io);
+		session->io = 0;
+	}
+
+	g_slist_foreach(session->streams, (GFunc) release_stream, session);
+	session->streams = NULL;
+}
+
+void avdtp_unref(struct avdtp *session)
+{
+	if (!session)
+		return;
+
+	if (!g_slist_find(sessions, session)) {
+		error("avdtp_unref: trying to unref a unknown session");
+		return;
+	}
+
+	session->ref--;
+
+	debug("avdtp_unref: ref=%d", session->ref);
+
+	if (session->ref == 1) {
+		if (session->dc_timer)
+			remove_disconnect_timer(session);
+		if (session->sock >= 0)
+			set_disconnect_timer(session);
+		return;
+	}
+
+	if (session->ref > 0)
+		return;
+
+	if (session->dc_timer)
+		remove_disconnect_timer(session);
+
+	connection_lost(session);
+
+	sessions = g_slist_remove(sessions, session);
+
+	if (session->req)
+		pending_req_free(session->req);
+
+	g_slist_foreach(session->seps, (GFunc) g_free, NULL);
+	g_slist_free(session->seps);
+
+	g_free(session->buf);
+
+	g_free(session);
+}
+
+struct avdtp *avdtp_ref(struct avdtp *session)
+{
+	session->ref++;
+	debug("avdtp_ref: ref=%d", session->ref);
+	if (session->dc_timer)
+		remove_disconnect_timer(session);
+	return session;
+}
+
+static struct avdtp_local_sep *find_local_sep_by_seid(uint8_t seid)
+{
+	GSList *l;
+
+	for (l = local_seps; l != NULL; l = g_slist_next(l)) {
+		struct avdtp_local_sep *sep = l->data;
+
+		if (sep->info.seid == seid)
+			return sep;
+	}
+
+	return NULL;
+}
+
+static struct avdtp_local_sep *find_local_sep(uint8_t type, uint8_t media_type,
+						uint8_t codec)
+{
+	GSList *l;
+
+	for (l = local_seps; l != NULL; l = g_slist_next(l)) {
+		struct avdtp_local_sep *sep = l->data;
+
+		if (sep->info.inuse)
+			continue;
+
+		if (sep->info.type == type &&
+				sep->info.media_type == media_type &&
+				sep->codec == codec)
+			return sep;
+	}
+
+	return NULL;
+}
+
+static void init_response(struct avdtp_header *rsp, struct avdtp_header *req,
+				gboolean accept)
+{
+	rsp->packet_type = AVDTP_PKT_TYPE_SINGLE;
+	rsp->message_type = accept ? AVDTP_MSG_TYPE_ACCEPT :
+					AVDTP_MSG_TYPE_REJECT;
+	rsp->transaction = req->transaction;
+	rsp->signal_id = req->signal_id;
+	rsp->rfa0 = 0;
+}
+
+static gboolean avdtp_unknown_cmd(struct avdtp *session,
+					struct avdtp_header *req, int size)
+{
+	struct avdtp_general_rej rej;
+
+	memset(&rej, 0, sizeof(rej));
+
+	rej.packet_type = AVDTP_PKT_TYPE_SINGLE;
+	rej.message_type = AVDTP_MSG_TYPE_REJECT;
+	rej.transaction = req->transaction;
+
+	return avdtp_send(session, &rej, sizeof(rej));
+}
+
+static gboolean avdtp_discover_cmd(struct avdtp *session,
+					struct discover_req *req, int size)
+{
+	GSList *l;
+	struct discover_resp *rsp = (struct discover_resp *) session->buf;
+	struct seid_info *info;
+	int rsp_size;
+
+	init_response(&rsp->header, &req->header, TRUE);
+	rsp_size = sizeof(struct discover_resp);
+	info = rsp->seps;
+
+	for (l = local_seps; l != NULL; l = l->next) {
+		struct avdtp_local_sep *sep = l->data;
+
+		if (rsp_size + sizeof(struct seid_info) > session->mtu)
+			break;
+
+		memcpy(&info, &sep->info, sizeof(struct seid_info));
+		rsp_size += sizeof(struct seid_info);
+		info++;
+	}
+
+	return avdtp_send(session, session->buf, rsp_size);
+}
+
+static gboolean avdtp_getcap_cmd(struct avdtp *session,
+					struct seid_req *req, int size)
+{
+	GSList *l, *caps;
+	struct avdtp_local_sep *sep = NULL;
+	struct seid_rej rej;
+	struct getcap_resp *rsp = (struct getcap_resp *) session->buf;
+	int rsp_size;
+	unsigned char *ptr;
+	uint8_t err;
+
+	if (size < sizeof(struct seid_req)) {
+		error("Too short getcap request");
+		return FALSE;
+	}
+
+	sep = find_local_sep_by_seid(req->acp_seid);
+	if (!sep) {
+		err = AVDTP_BAD_ACP_SEID;
+		goto failed;
+	}
+
+	if (!sep->ind->get_capability(sep, &caps, &err))
+		goto failed;
+
+	init_response(&rsp->header, &req->header, TRUE);
+	rsp_size = sizeof(struct getcap_resp);
+	ptr = rsp->caps;
+
+	for (l = caps; l != NULL; l = g_slist_next(l)) {
+		struct avdtp_service_capability *cap = l->data;
+
+		if (rsp_size + cap->length + 2 > session->mtu)
+			break;
+
+		memcpy(ptr, cap, cap->length + 2);
+		rsp_size += cap->length + 2;
+		ptr += cap->length + 2;
+
+		g_free(cap);
+	}
+
+	g_slist_free(caps);
+
+	return avdtp_send(session, session->buf, rsp_size);
+
+failed:
+	init_response(&rej.header, &req->header, FALSE);
+	rej.error = AVDTP_BAD_ACP_SEID;
+	return avdtp_send(session, &rej, sizeof(rej));
+}
+
+static gboolean avdtp_setconf_cmd(struct avdtp *session,
+					struct setconf_req *req, int size)
+{
+	struct conf_rej rej;
+	struct setconf_resp *rsp = (struct setconf_resp *) session->buf;
+	struct avdtp_local_sep *lsep;
+	gboolean ret;
+
+	if (size < sizeof(struct setconf_req)) {
+		error("Too short getcap request");
+		return FALSE;
+	}
+
+	lsep = find_local_sep_by_seid(req->acp_seid);
+	if (!lsep || !lsep->stream) {
+		init_response(&rej.header, &req->header, FALSE);
+		rej.error = AVDTP_BAD_ACP_SEID;
+		return avdtp_send(session, &rej, sizeof(rej));
+	}
+
+	init_response(&rsp->header, &req->header, TRUE);
+
+	ret = avdtp_send(session, rsp, sizeof(struct setconf_req));
+
+	if (ret)
+		avdtp_sep_set_state(session, lsep, AVDTP_STATE_CONFIGURED);
+
+	return ret;
+}
+
+static gboolean avdtp_getconf_cmd(struct avdtp *session, struct seid_req *req,
+					int size)
+{
+	return avdtp_unknown_cmd(session, (void *) req, size);
+}
+
+static gboolean avdtp_reconf_cmd(struct avdtp *session, struct seid_req *req,
+					int size)
+{
+	return avdtp_unknown_cmd(session, (void *) req, size);
+}
+
+static gboolean avdtp_open_cmd(struct avdtp *session, struct seid_req *req,
+				int size)
+{
+	return avdtp_unknown_cmd(session, (void *) req, size);
+}
+
+static gboolean avdtp_start_cmd(struct avdtp *session, struct seid_req *req,
+				int size)
+{
+	return avdtp_unknown_cmd(session, (void *) req, size);
+}
+
+static gboolean avdtp_close_cmd(struct avdtp *session, struct seid_req *req,
+				int size)
+{
+	return avdtp_unknown_cmd(session, (void *) req, size);
+}
+
+static gboolean avdtp_suspend_cmd(struct avdtp *session, struct seid_req *req,
+				int size)
+{
+	return avdtp_unknown_cmd(session, (void *) req, size);
+}
+
+static gboolean avdtp_abort_cmd(struct avdtp *session, struct seid_req *req,
+				int size)
+{
+	struct avdtp_local_sep *sep;
+	struct abort_resp *rsp = (struct abort_resp *) session->buf;
+	struct seid_rej rej;
+	uint8_t err;
+	gboolean ret;
+
+	if (size < sizeof(struct seid_req)) {
+		error("Too short abort request");
+		return FALSE;
+	}
+
+	sep = find_local_sep_by_seid(req->acp_seid);
+	if (!sep || !sep->stream) {
+		err = AVDTP_BAD_ACP_SEID;
+		goto failed;
+	}
+
+	if (sep->ind && sep->ind->abort) {
+		if (!sep->ind->abort(sep, sep->stream, &err))
+			goto failed;
+	}
+
+	init_response(&rsp->header, &req->header, TRUE);
+	ret = avdtp_send(session, rsp, sizeof(struct abort_resp));
+
+	if (ret)
+		avdtp_sep_set_state(session, sep, AVDTP_STATE_ABORTING);
+
+	return ret;
+
+failed:
+	init_response(&rej.header, &req->header, FALSE);
+	rej.error = err;
+	return avdtp_send(session, &rej, sizeof(rej));
+}
+
+static gboolean avdtp_secctl_cmd(struct avdtp *session, struct seid_req *req,
+					int size)
+{
+	return avdtp_unknown_cmd(session, (void *) req, size);
+}
+
+static gboolean avdtp_parse_cmd(struct avdtp *session,
+				struct avdtp_header *header, int size)
+{
+	switch (header->signal_id) {
+	case AVDTP_DISCOVER:
+		debug("Received DISCOVER_CMD");
+		return avdtp_discover_cmd(session, (void *) header, size);
+	case AVDTP_GET_CAPABILITIES:
+		debug("Received  GET_CAPABILITIES_CMD");
+		return avdtp_getcap_cmd(session, (void *) header, size);
+	case AVDTP_SET_CONFIGURATION:
+		debug("Received SET_CONFIGURATION_CMD");
+		return avdtp_setconf_cmd(session, (void *) header, size);
+	case AVDTP_GET_CONFIGURATION:
+		debug("Received GET_CONFIGURATION_CMD");
+		return avdtp_getconf_cmd(session, (void *) header, size);
+	case AVDTP_RECONFIGURE:
+		debug("Received RECONFIGURE_CMD");
+		return avdtp_reconf_cmd(session, (void *) header, size);
+	case AVDTP_OPEN:
+		debug("Received OPEN_CMD");
+		return avdtp_open_cmd(session, (void *) header, size);
+	case AVDTP_START:
+		debug("Received START_CMD");
+		return avdtp_start_cmd(session, (void *) header, size);
+	case AVDTP_CLOSE:
+		debug("Received CLOSE_CMD");
+		return avdtp_close_cmd(session, (void *) header, size);
+	case AVDTP_SUSPEND:
+		debug("Received SUSPEND_CMD");
+		return avdtp_suspend_cmd(session, (void *) header, size);
+	case AVDTP_ABORT:
+		debug("Received ABORT_CMD");
+		return avdtp_abort_cmd(session, (void *) header, size);
+	case AVDTP_SECURITY_CONTROL:
+		debug("Received SECURITY_CONTROL_CMD");
+		return avdtp_secctl_cmd(session, (void *) header, size);
+	default:
+		debug("Received unknown request id %u", header->signal_id);
+		return avdtp_unknown_cmd(session, (void *) header, size);
+	}
+}
+
+static gboolean transport_cb(GIOChannel *chan, GIOCondition cond,
+				gpointer data)
+{
+	struct avdtp_stream *stream = data;
+	struct avdtp_local_sep *sep = stream->lsep;
+
+	if (stream->close_int && sep->cfm && sep->cfm->close)
+		sep->cfm->close(sep, stream);
+
+	avdtp_sep_set_state(stream->session, sep, AVDTP_STATE_IDLE);
+
+	return FALSE;
+}
+
+static void handle_transport_connect(struct avdtp *session, int sock,
+					uint16_t mtu)
+{
+	struct avdtp_stream *stream = session->pending_open;
+	struct avdtp_local_sep *sep = stream->lsep;
+	GIOChannel *channel;
+
+	session->pending_open = NULL;
+
+	stream->sock = sock;
+	stream->mtu = mtu;
+
+	if (sep->cfm && sep->cfm->open)
+		sep->cfm->open(sep, stream);
+
+	channel = g_io_channel_unix_new(stream->sock);
+
+	stream->io = g_io_add_watch(channel, G_IO_ERR | G_IO_HUP | G_IO_NVAL,
+					(GIOFunc) transport_cb, stream);
+	g_io_channel_unref(channel);
+
+	avdtp_sep_set_state(session, sep, AVDTP_STATE_OPEN);
+}
+
+static void init_request(struct avdtp_header *header, int request_id)
+{
+	static int transaction = 0;
+
+	header->packet_type = AVDTP_PKT_TYPE_SINGLE;
+	header->message_type = AVDTP_MSG_TYPE_COMMAND;
+	header->transaction = transaction;
+	header->signal_id = request_id;
+
+	/* clear rfa bits */
+	header->rfa0 = 0;
+
+	transaction = (transaction + 1) % 16;
+}
+
+static gboolean session_cb(GIOChannel *chan, GIOCondition cond,
+				gpointer data)
+{
+	struct avdtp *session = data;
+	struct avdtp_header *header;
+	gsize size;
+
+	debug("session_cb");
+
+	if (cond & G_IO_NVAL)
+		return FALSE;
+
+	if (cond & (G_IO_HUP | G_IO_ERR))
+		goto failed;
+
+	if (g_io_channel_read(chan, session->buf, session->mtu,
+				&size) != G_IO_ERROR_NONE) {
+		error("IO Channel read error");
+		goto failed;
+	}
+
+	if (size < sizeof(struct avdtp_header)) {
+		error("Received too small packet (%d bytes)", size);
+		goto failed;
+	}
+
+	header = (struct avdtp_header *) session->buf;
+
+	if (header->message_type == AVDTP_MSG_TYPE_COMMAND) {
+		if (!avdtp_parse_cmd(session, header, size)) {
+			error("Unable to handle command. Disconnecting");
+			goto failed;
+		}
+
+		if (session->ref == 1 && !session->streams)
+			set_disconnect_timer(session);
+
+		return TRUE;
+	}
+
+	if (session->req == NULL) {
+		error("No pending request, rejecting message");
+		return TRUE;
+	}
+
+	if (header->transaction != session->req->msg->transaction) {
+		error("Transaction label doesn't match");
+		return TRUE;
+	}
+
+	if (header->signal_id != session->req->msg->signal_id) {
+		error("Reponse signal doesn't match");
+		return TRUE;
+	}
+
+	g_source_remove(session->req->timeout);
+	session->req->timeout = 0;
+
+	switch(header->message_type) {
+	case AVDTP_MSG_TYPE_ACCEPT:
+		if (!avdtp_parse_resp(session, session->req->stream, header,
+					size)) {
+			error("Unable to parse accept response");
+			goto failed;
+		}
+		break;
+	case AVDTP_MSG_TYPE_REJECT:
+		if (!avdtp_parse_rej(session, session->req->stream, header,
+					size)) {
+			error("Unable to parse reject response");
+			goto failed;
+		}
+		break;
+	default:
+		error("Unknown message type");
+		break;
+	}
+
+	pending_req_free(session->req);
+	session->req = NULL;
+
+	process_queue(session);
+
+	return TRUE;
+
+failed:
+	connection_lost(session);
+	avdtp_unref(session);
+	return FALSE;
+}
+
+static gboolean l2cap_connect_cb(GIOChannel *chan, GIOCondition cond,
+					gpointer data)
+{
+	struct avdtp *session = data;
+	struct l2cap_options l2o;
+	socklen_t len;
+	int ret, err, sk;
+	char address[18];
+
+	if (cond & G_IO_NVAL)
+		return FALSE;
+
+	if (!g_slist_find(sessions, session)) {
+		debug("l2cap_connect_cb: session got removed");
+		return FALSE;
+	}
+
+	if (cond & (G_IO_ERR | G_IO_HUP)) {
+		err = EIO;
+		goto failed;
+	}
+
+	sk = g_io_channel_unix_get_fd(chan);
+	len = sizeof(ret);
+	if (getsockopt(sk, SOL_SOCKET, SO_ERROR, &ret, &len) < 0) {
+		err = errno;
+		error("getsockopt(SO_ERROR): %s (%d)", strerror(err), err);
+		goto failed;
+	}
+
+	if (ret != 0) {
+		err = ret;
+		error("connect(): %s (%d)", strerror(err), err);
+		goto failed;
+	}
+
+	ba2str(&session->dst, address);
+	debug("AVDTP: connected %s channel to %s",
+			session->pending_open ? "transport" : "signaling",
+			address);
+
+	memset(&l2o, 0, sizeof(l2o));
+	len = sizeof(l2o);
+	if (getsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &l2o,
+				&len) < 0) {
+		err = errno;
+		error("getsockopt(L2CAP_OPTIONS): %s (%d)", strerror(err),
+				err);
+		goto failed;
+	}
+
+	if (session->state == AVDTP_SESSION_STATE_CONNECTING) {
+		session->sock = sk;
+		session->mtu = l2o.imtu;
+		session->buf = g_malloc0(session->mtu);
+		session->state = AVDTP_SESSION_STATE_CONNECTED;
+		session->io = g_io_add_watch(chan,
+						G_IO_IN | G_IO_ERR | G_IO_HUP
+						| G_IO_NVAL,
+						(GIOFunc) session_cb, session);
+	}
+	else if (session->pending_open)
+		handle_transport_connect(session, sk, l2o.imtu);
+
+	process_queue(session);
+
+	return FALSE;
+
+failed:
+	if (session->pending_open) {
+		avdtp_sep_set_state(session, session->pending_open->lsep,
+					AVDTP_STATE_IDLE);
+		session->pending_open = NULL;
+	} else {
+		finalize_discovery(session, -err);
+		connection_lost(session);
+		avdtp_unref(session);
+	}
+
+	return FALSE;
+}
+
+static int l2cap_connect(struct avdtp *session)
+{
+	struct sockaddr_l2 l2a;
+	GIOChannel *io;
+	int sk;
+
+	memset(&l2a, 0, sizeof(l2a));
+	l2a.l2_family = AF_BLUETOOTH;
+	bacpy(&l2a.l2_bdaddr, &session->src);
+
+	sk = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
+	if (sk < 0) {
+		error("Cannot create L2CAP socket. %s(%d)", strerror(errno),
+				errno);
+		return -errno;
+	}
+
+	if (bind(sk, (struct sockaddr *) &l2a, sizeof(l2a)) < 0) {
+		error("Bind failed. %s (%d)", strerror(errno), errno);
+		return -errno;
+	}
+
+	memset(&l2a, 0, sizeof(l2a));
+	l2a.l2_family = AF_BLUETOOTH;
+	bacpy(&l2a.l2_bdaddr, &session->dst);
+	l2a.l2_psm = htobs(AVDTP_PSM);
+
+	if (set_nonblocking(sk) < 0) {
+		error("Set non blocking: %s (%d)", strerror(errno), errno);
+		return -errno;
+	}
+
+	io = g_io_channel_unix_new(sk);
+	g_io_channel_set_close_on_unref(io, FALSE);
+
+	if (connect(sk, (struct sockaddr *) &l2a, sizeof(l2a)) < 0) {
+		if (!(errno == EAGAIN || errno == EINPROGRESS)) {
+			error("Connect failed. %s(%d)", strerror(errno),
+					errno);
+			finalize_discovery(session, errno);
+			g_io_channel_close(io);
+			g_io_channel_unref(io);
+			return -errno;
+		}
+		g_io_add_watch(io, G_IO_OUT | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
+				(GIOFunc) l2cap_connect_cb, session);
+
+		if (session->state == AVDTP_SESSION_STATE_DISCONNECTED)
+			session->state = AVDTP_SESSION_STATE_CONNECTING;
+	} else
+		l2cap_connect_cb(io, G_IO_OUT, session);
+
+	g_io_channel_unref(io);
+
+	return 0;
+}
+
+static void queue_request(struct avdtp *session, struct pending_req *req,
+			gboolean priority)
+{
+	if (priority)
+		session->prio_queue = g_slist_append(session->prio_queue, req);
+	else
+		session->req_queue = g_slist_append(session->req_queue, req);
+}
+
+static struct avdtp_remote_sep *find_remote_sep(GSList *seps, uint8_t seid)
+{
+	GSList *l;
+
+	for (l = seps; l != NULL; l = g_slist_next(l)) {
+		struct avdtp_remote_sep *sep = l->data;
+
+		if (sep->seid == seid)
+			return sep;
+	}
+
+	return NULL;
+}
+
+static gboolean request_timeout(gpointer user_data)
+{
+	struct avdtp *session = user_data;
+	struct pending_req *req;
+	struct seid_req sreq;
+	struct avdtp_remote_sep *sep;
+	struct avdtp_stream *stream;
+	uint8_t seid;
+
+	error("Request timed out");
+
+	req = session->req;
+	session->req = NULL;
+
+	switch (req->msg->signal_id) {
+	case AVDTP_DISCOVER:
+	case AVDTP_GET_CAPABILITIES:
+	case AVDTP_SET_CONFIGURATION:
+	case AVDTP_ABORT:
+		goto failed;
+	}
+
+	seid = ((struct seid_req *) (req->msg))->acp_seid;
+
+	sep = find_remote_sep(session->seps, seid);
+	if (!sep) {
+		error("Unable to find matching remote SEID %u", seid);
+		goto failed;
+	}
+
+	stream = sep->stream;
+
+	memset(&sreq, 0, sizeof(sreq));
+	init_request(&sreq.header, AVDTP_ABORT);
+	sreq.acp_seid = seid;
+
+	if (!send_request(session, TRUE, stream, &sreq, sizeof(sreq))) {
+		error("Unable to send abort request");
+		goto failed;
+	}
+
+	goto done;
+
+failed:
+	connection_lost(session);
+	avdtp_unref(session);
+done:
+	pending_req_free(req);
+	return FALSE;
+}
+
+static int send_req(struct avdtp *session, gboolean priority,
+			struct pending_req *req)
+{
+	int err;
+
+	if (session->state == AVDTP_SESSION_STATE_DISCONNECTED) {
+		err = l2cap_connect(session);
+		if (err < 0)
+			goto failed;
+	}
+
+	if (session->state < AVDTP_SESSION_STATE_CONNECTED ||
+			session->req != NULL) {
+		queue_request(session, req, priority);
+		return 0;
+	}
+
+	/* FIXME: Should we retry to send if the buffer
+	was not totally sent or in case of EINTR? */
+	err = avdtp_send(session, req->msg, req->msg_size);
+	if (err < 0)
+		goto failed;
+
+	session->req = req;
+
+	req->timeout = g_timeout_add(REQ_TIMEOUT, request_timeout,
+					session);
+	return 0;
+
+failed:
+	g_free(req->msg);
+	g_free(req);
+	return err;
+}
+
+static int send_request(struct avdtp *session, gboolean priority,
+			struct avdtp_stream *stream, void *buffer, int size)
+{
+	struct pending_req *req;
+
+	req = g_new0(struct pending_req, 1);
+	req->msg = g_malloc(size);
+	memcpy(req->msg, buffer, size);
+	req->msg_size = size;
+	req->stream = stream;
+
+	return send_req(session, priority, req);
+}
+
+static gboolean avdtp_discover_resp(struct avdtp *session,
+					struct discover_resp *resp, int size)
+{
+	int sep_count, i, isize = sizeof(struct seid_info);
+
+	sep_count = (size - sizeof(struct avdtp_header)) / isize;
+
+	for (i = 0; i < sep_count; i++) {
+		struct avdtp_remote_sep *sep;
+		struct seid_req req;
+		int ret;
+
+		/* Skip SEP's which are in use */
+		if (resp->seps[i].inuse)
+			continue;
+
+		sep = find_remote_sep(session->seps, resp->seps[i].seid);
+		if (!sep) {
+			sep = g_new0(struct avdtp_remote_sep, 1);
+			session->seps = g_slist_append(session->seps, sep);
+		}
+		else if (sep && sep->stream)
+			continue;
+
+		sep->seid = resp->seps[i].seid;
+		sep->type = resp->seps[i].type;
+		sep->media_type = resp->seps[i].media_type;
+
+		memset(&req, 0, sizeof(req));
+		init_request(&req.header, AVDTP_GET_CAPABILITIES);
+		req.acp_seid = sep->seid;
+
+		ret = send_request(session, TRUE, NULL, &req, sizeof(req));
+		if (ret < 0) {
+			finalize_discovery(session, ret);
+			break;
+		}
+	}
+
+	return TRUE;
+}
+
+static gboolean avdtp_get_capabilities_resp(struct avdtp *session,
+						struct getcap_resp *resp,
+						int size)
+{
+	int processed;
+	struct avdtp_remote_sep *sep;
+	unsigned char *ptr;
+	uint8_t seid;
+
+	/* Check for minimum required packet size includes:
+	 *   1. getcap resp header
+	 *   2. media transport capability (2 bytes)
+	 *   3. media codec capability type + length (2 bytes)
+	 *   4. the actual media codec elements
+	 * */
+	if (size < (sizeof(struct getcap_resp) + 4 +
+				sizeof(struct avdtp_media_codec_capability))) {
+		error("Too short getcap resp packet");
+		return FALSE;
+	}
+
+	seid = ((struct seid_req *) session->req->msg)->acp_seid;
+
+	sep = find_remote_sep(session->seps, seid);
+
+	if (sep->caps) {
+		g_slist_foreach(sep->caps, (GFunc) g_free, NULL);
+		g_slist_free(sep->caps);
+		sep->caps = NULL;
+		sep->codec = NULL;
+	}
+
+	ptr = resp->caps;
+	processed = sizeof(struct getcap_resp);
+
+	while (processed + 2 < size) {
+		struct avdtp_service_capability *cap;
+		uint8_t length, category;
+
+		category = ptr[0];
+		length = ptr[1];
+
+		if (processed + 2 + length > size) {
+			error("Invalid capability data in getcap resp");
+			return FALSE;
+		}
+
+		cap = g_malloc(sizeof(struct avdtp_service_capability) +
+					length);
+		memcpy(cap, ptr, 2 + length);
+
+		processed += 2 + length;
+		ptr += 2 + length;
+
+		sep->caps = g_slist_append(sep->caps, cap);
+
+		if (category == AVDTP_MEDIA_CODEC &&
+				length >=
+				sizeof(struct avdtp_media_codec_capability))
+			sep->codec = cap;
+
+	}
+
+	return TRUE;
+}
+
+static gboolean avdtp_set_configuration_resp(struct avdtp *session,
+						struct avdtp_stream *stream,
+						struct avdtp_header *resp,
+						int size)
+{
+	struct avdtp_local_sep *sep = stream->lsep;
+
+	if (sep->cfm && sep->cfm->set_configuration)
+		sep->cfm->set_configuration(sep, stream);
+
+	avdtp_sep_set_state(session, sep, AVDTP_STATE_CONFIGURED);
+
+	return TRUE;
+}
+
+static gboolean avdtp_reconfigure_resp(struct avdtp *session,
+					struct avdtp_stream *stream,
+					struct avdtp_header *resp, int size)
+{
+	return TRUE;
+}
+
+static gboolean avdtp_open_resp(struct avdtp *session, struct avdtp_stream *stream,
+				struct seid_rej *resp, int size)
+{
+	struct avdtp_local_sep *sep = stream->lsep;
+
+	if (l2cap_connect(session) < 0)
+		avdtp_sep_set_state(session, sep, AVDTP_STATE_IDLE);
+
+	session->pending_open = stream;
+
+	return TRUE;
+}
+
+static gboolean avdtp_start_resp(struct avdtp *session,
+					struct avdtp_stream *stream,
+					struct seid_rej *resp, int size)
+{
+	struct avdtp_local_sep *sep = stream->lsep;
+
+	if (sep->cfm && sep->cfm->start)
+		sep->cfm->start(sep, stream);
+
+	avdtp_sep_set_state(session, sep, AVDTP_STATE_STREAMING);
+
+	return TRUE;
+}
+
+static gboolean avdtp_close_resp(struct avdtp *session,
+					struct avdtp_stream *stream,
+					struct seid_rej *resp, int size)
+{
+	struct avdtp_local_sep *sep = stream->lsep;
+
+	avdtp_sep_set_state(session, sep, AVDTP_STATE_CLOSING);
+
+	close(stream->sock);
+	stream->sock = -1;
+
+	return TRUE;
+}
+
+static gboolean avdtp_suspend_resp(struct avdtp *session,
+					struct avdtp_stream *stream,
+					struct stream_pause_resp *resp,
+					int size)
+{
+	struct avdtp_local_sep *sep = stream->lsep;
+
+	if (sep->cfm && sep->cfm->suspend)
+		sep->cfm->suspend(sep, stream);
+
+	avdtp_sep_set_state(session, sep, AVDTP_STATE_OPEN);
+
+	return TRUE;
+}
+
+static gboolean avdtp_abort_resp(struct avdtp *session,
+					struct avdtp_stream *stream,
+					struct seid_rej *resp, int size)
+{
+	struct avdtp_local_sep *sep = stream->lsep;
+
+	if (sep->cfm && sep->cfm->suspend)
+		sep->cfm->suspend(sep, stream);
+
+	avdtp_sep_set_state(session, sep, AVDTP_STATE_IDLE);
+
+	return TRUE;
+}
+
+static gboolean avdtp_parse_resp(struct avdtp *session,
+					struct avdtp_stream *stream,
+					struct avdtp_header *header, int size)
+{
+	struct avdtp_header *next;
+
+	if (session->prio_queue)
+		next = ((struct pending_req *)
+				(session->prio_queue->data))->msg;
+	else if (session->req_queue)
+		next = ((struct pending_req *)
+				(session->req_queue->data))->msg;
+	else
+		next = NULL;
+
+	switch (header->signal_id) {
+	case AVDTP_DISCOVER:
+		debug("DISCOVER request succeeded");
+		return avdtp_discover_resp(session, (void *) header, size);
+	case AVDTP_GET_CAPABILITIES:
+		debug("GET_CAPABILITIES request succeeded");
+		if (!avdtp_get_capabilities_resp(session, (void *) header,
+							size))
+			return FALSE;
+		if (!(next && next->signal_id == AVDTP_GET_CAPABILITIES))
+			finalize_discovery(session, 0);
+		return TRUE;
+	case AVDTP_SET_CONFIGURATION:
+		debug("SET_CONFIGURATION request succeeded");
+		return avdtp_set_configuration_resp(session, stream,
+							(void *) header, size);
+	case AVDTP_RECONFIGURE:
+		debug("RECONFIGURE request succeeded");
+		return avdtp_reconfigure_resp(session, stream, (void *) header,
+						size);
+	case AVDTP_OPEN:
+		debug("OPEN request succeeded");
+		return avdtp_open_resp(session, stream, (void *) header, size);
+	case AVDTP_SUSPEND:
+		debug("SUSPEND request succeeded");
+		return avdtp_suspend_resp(session, stream, (void *) header,
+						size);
+	case AVDTP_START:
+		debug("START request succeeded");
+		return avdtp_start_resp(session, stream, (void *) header,
+					size);
+	case AVDTP_CLOSE:
+		debug("CLOSE request succeeded");
+		return avdtp_close_resp(session, stream, (void *) header,
+					size);
+	case AVDTP_ABORT:
+		debug("ABORT request succeeded");
+		return avdtp_abort_resp(session, stream, (void *) header,
+					size);
+	}
+
+	error("Unknown signal id in accept response: %u", header->signal_id);
+
+	return TRUE;
+}
+
+static gboolean seid_rej_to_err(struct seid_rej *rej, int size,
+					struct avdtp_error *err)
+{
+	if (size < sizeof(struct seid_rej)) {
+		error("Too small packet for seid_rej");
+		return FALSE;
+	}
+
+	avdtp_error_init(err, AVDTP_ERROR_ERROR_CODE, rej->error);
+
+	return TRUE;
+}
+
+static gboolean conf_rej_to_err(struct conf_rej *rej, int size,
+				struct avdtp_error *err, uint8_t *category)
+{
+	if (size < sizeof(struct conf_rej)) {
+		error("Too small packet for conf_rej");
+		return FALSE;
+	}
+
+	avdtp_error_init(err, AVDTP_ERROR_ERROR_CODE, rej->error);
+
+	if (category)
+		*category = rej->category;
+
+	return TRUE;
+}
+
+static gboolean stream_rej_to_err(struct stream_rej *rej, int size,
+					struct avdtp_error *err, uint8_t *acp_seid)
+{
+	if (size < sizeof(struct conf_rej)) {
+		error("Too small packet for stream_rej");
+		return FALSE;
+	}
+
+	avdtp_error_init(err, AVDTP_ERROR_ERROR_CODE, rej->error_code);
+
+	if (acp_seid)
+		*acp_seid = rej->acp_seid;
+
+	return TRUE;
+}
+
+static gboolean avdtp_parse_rej(struct avdtp *session, struct avdtp_stream *stream,
+				struct avdtp_header *header, int size)
+{
+	struct avdtp_error err;
+	uint8_t acp_seid, category;
+
+	switch (header->signal_id) {
+	case AVDTP_DISCOVER:
+		if (!seid_rej_to_err((void *) header, size, &err))
+			return FALSE;
+		error("DISCOVER request rejected: %s (%d)",
+				avdtp_strerror(&err), err.err.error_code);
+		return TRUE;
+	case AVDTP_GET_CAPABILITIES:
+		if (!seid_rej_to_err((void *) header, size, &err))
+			return FALSE;
+		error("GET_CAPABILITIES request rejected: %s (%d)",
+				avdtp_strerror(&err), err.err.error_code);
+		return TRUE;
+	case AVDTP_OPEN:
+		if (!seid_rej_to_err((void *) header, size, &err))
+			return FALSE;
+		error("OPEN request rejected: %s (%d)",
+				avdtp_strerror(&err), err.err.error_code);
+		return TRUE;
+	case AVDTP_SET_CONFIGURATION:
+		if (!conf_rej_to_err((void *) header, size, &err, &category))
+			return FALSE;
+		error("SET_CONFIGURATION request rejected: %s (%d)",
+				avdtp_strerror(&err), err.err.error_code);
+		return TRUE;
+	case AVDTP_RECONFIGURE:
+		if (!conf_rej_to_err((void *) header, size, &err, &category))
+			return FALSE;
+		error("RECONFIGURE request rejected: %s (%d)",
+				avdtp_strerror(&err), err.err.error_code);
+		return TRUE;
+	case AVDTP_START:
+		if (!stream_rej_to_err((void *) header, size, &err, &acp_seid))
+			return FALSE;
+		error("START request rejected: %s (%d)",
+				avdtp_strerror(&err), err.err.error_code);
+		return TRUE;
+	case AVDTP_SUSPEND:
+		if (!stream_rej_to_err((void *) header, size, &err, &acp_seid))
+			return FALSE;
+		error("SUSPEND request rejected: %s (%d)",
+				avdtp_strerror(&err), err.err.error_code);
+		return TRUE;
+	case AVDTP_CLOSE:
+		if (!stream_rej_to_err((void *) header, size, &err, &acp_seid))
+			return FALSE;
+		error("CLOSE request rejected: %s (%d)",
+				avdtp_strerror(&err), err.err.error_code);
+		return TRUE;
+	case AVDTP_ABORT:
+		if (!stream_rej_to_err((void *) header, size, &err, &acp_seid))
+			return FALSE;
+		error("ABORT request rejected: %s (%d)",
+				avdtp_strerror(&err), err.err.error_code);
+		return TRUE;
+	default:
+		error("Unknown reject response signal id: %u",
+				header->signal_id);
+		return TRUE;
+	}
+}
+
+static struct avdtp *find_session(bdaddr_t *src, bdaddr_t *dst)
+{
+	GSList *l;
+
+	for (l = sessions; l != NULL; l = g_slist_next(l)) {
+		struct avdtp *s = l->data;
+
+		if (bacmp(src, &s->src) || bacmp(dst, &s->dst))
+			continue;
+
+		return s;
+	}
+
+	return NULL;
+}
+
+struct avdtp *avdtp_get(bdaddr_t *src, bdaddr_t *dst)
+{
+	struct avdtp *session;
+
+	assert(src != NULL);
+	assert(dst != NULL);
+
+	session = find_session(src, dst);
+	if (session)
+		return avdtp_ref(session);
+
+	session = g_new0(struct avdtp, 1);
+
+	session->sock = -1;
+	bacpy(&session->src, src);
+	bacpy(&session->dst, dst);
+	session->ref = 1;
+	session->state = AVDTP_SESSION_STATE_DISCONNECTED;
+
+	sessions = g_slist_append(sessions, session);
+
+	return avdtp_ref(session);
+}
+
+gboolean avdtp_stream_get_transport(struct avdtp_stream *stream, int *sock,
+					uint16_t *mtu, GSList **caps)
+{
+	if (stream->sock < 0)
+		return FALSE;
+
+	if (sock)
+		*sock = stream->sock;
+
+	if (mtu)
+		*mtu = stream->mtu;
+	
+	if (caps)
+		*caps = stream->caps;
+
+	return TRUE;
+}
+
+static int process_queue(struct avdtp *session)
+{
+	GSList **queue, *l;
+	struct pending_req *req;
+
+	if (session->req)
+		return 0;
+
+	if (session->prio_queue)
+		queue = &session->prio_queue;
+	else
+		queue = &session->req_queue;
+
+	if (!*queue)
+		return 0;
+
+	l = *queue;
+	req = l->data;
+
+	*queue = g_slist_remove(*queue, req);
+
+	return send_req(session, FALSE, req);
+}
+
+struct avdtp_service_capability *avdtp_get_codec(struct avdtp_remote_sep *sep)
+{
+	return sep->codec;
+}
+
+struct avdtp_service_capability *avdtp_service_cap_new(uint8_t category,
+							void *data, int length)
+{
+	struct avdtp_service_capability *cap;
+
+	cap = g_malloc(sizeof(struct avdtp_service_capability) + length);
+	cap->category = category;
+	cap->length = length;
+	memcpy(cap->data, data, length);
+
+	return cap;
+}
+
+int avdtp_discover(struct avdtp *session, avdtp_discover_cb_t cb, void *user_data)
+{
+	struct discover_req req;
+	int ret;
+
+	if (session->discov_cb)
+		return -EBUSY;
+
+	memset(&req, 0, sizeof(req));
+	init_request(&req.header, AVDTP_DISCOVER);
+
+	ret = send_request(session, FALSE, NULL, &req, sizeof(req));
+	if (ret == 0) {
+		session->discov_cb = cb;
+		session->user_data = user_data;
+	}
+
+	return ret;
+}
+
+int avdtp_get_seps(struct avdtp *session, uint8_t acp_type, uint8_t media_type,
+			uint8_t codec, struct avdtp_local_sep **lsep,
+			struct avdtp_remote_sep **rsep)
+{
+	GSList *l;
+	uint8_t int_type;
+
+	int_type = acp_type == AVDTP_SEP_TYPE_SINK ?
+				AVDTP_SEP_TYPE_SOURCE : AVDTP_SEP_TYPE_SINK;
+
+	*lsep = find_local_sep(int_type, media_type, codec);
+	if (!*lsep)
+		return -EINVAL;
+
+	for (l = session->seps; l != NULL; l = g_slist_next(l)) {
+		struct avdtp_remote_sep *sep = l->data;
+		struct avdtp_service_capability *cap;
+		struct avdtp_media_codec_capability *codec_data;
+
+		if (sep->type != acp_type)
+			continue;
+
+		if (sep->media_type != media_type)
+			continue;
+
+		if (!sep->codec)
+			continue;
+
+		cap = sep->codec;
+		codec_data = (void *) cap->data;
+
+		if (codec_data->media_codec_type != codec)
+			continue;
+
+		if (!sep->stream) {
+			*rsep = sep;
+			return 0;
+		}
+	}
+
+	return -EINVAL;
+}
+
+void avdtp_stream_set_cb(struct avdtp *session, struct avdtp_stream *stream,
+				avdtp_stream_state_cb cb, void *data)
+{
+	stream->cb = cb;
+	stream->user_data = data;
+}
+
+int avdtp_get_configuration(struct avdtp *session, struct avdtp_stream *stream)
+{
+	struct seid_req req;
+
+	if (session->state < AVDTP_SESSION_STATE_CONNECTED)
+		return -EINVAL;
+
+	memset(&req, 0, sizeof(req));
+	init_request(&req.header, AVDTP_GET_CONFIGURATION);
+	req.acp_seid = stream->rsep->seid;
+
+	return send_request(session, FALSE, stream, &req, sizeof(req));
+}
+
+int avdtp_set_configuration(struct avdtp *session,
+				struct avdtp_remote_sep *rsep,
+				struct avdtp_local_sep *lsep,
+				GSList *caps,
+				struct avdtp_stream **stream)
+{
+	struct setconf_req *req;
+	struct avdtp_stream *new_stream;
+	unsigned char *ptr;
+	int ret, caps_len;
+	struct avdtp_service_capability *cap;
+	GSList *l;
+
+	if (session->state != AVDTP_SESSION_STATE_CONNECTED)
+		return -ENOTCONN;
+
+	if (!(lsep && rsep))
+		return -EINVAL;
+
+	new_stream = g_new0(struct avdtp_stream, 1);
+
+	new_stream->session = session;
+	new_stream->lsep = lsep;
+	new_stream->rsep = rsep;
+	new_stream->caps = caps;
+
+	/* Calculate total size of request */
+	for (l = caps, caps_len = 0; l != NULL; l = g_slist_next(l)) {
+		cap = l->data;
+		caps_len += cap->length + 2;
+	}
+
+	req = g_malloc0(sizeof(struct setconf_req) + caps_len);
+
+	init_request(&req->header, AVDTP_SET_CONFIGURATION);
+	req->acp_seid = lsep->info.seid;
+	req->int_seid = rsep->seid;
+
+	/* Copy the capabilities into the request */
+	for (l = caps, ptr = req->caps; l != NULL; l = g_slist_next(l)) {
+		cap = l->data;
+		memcpy(ptr, cap, cap->length + 2);
+		ptr += cap->length + 2;
+	}
+
+	ret = send_request(session, FALSE, new_stream, req,
+				sizeof(struct setconf_req) + caps_len);
+	if (ret < 0)
+		stream_free(new_stream);
+	else {
+		lsep->info.inuse = 1;
+		lsep->stream = new_stream;
+		rsep->stream = new_stream;
+		session->streams = g_slist_append(session->streams, new_stream);
+		if (stream)
+			*stream = new_stream;
+	}
+
+	g_free(req);
+
+	return ret;
+}
+
+int avdtp_reconfigure(struct avdtp *session, struct avdtp_stream *stream)
+{
+	struct seid_req req;
+
+	if (!g_slist_find(session->streams, stream))
+		return -EINVAL;
+
+	if (stream->lsep->state != AVDTP_STATE_OPEN)
+		return -EINVAL;
+
+	memset(&req, 0, sizeof(req));
+	init_request(&req.header, AVDTP_GET_CONFIGURATION);
+	req.acp_seid = stream->rsep->seid;
+
+	return send_request(session, FALSE, NULL, &req, sizeof(req));
+}
+
+int avdtp_open(struct avdtp *session, struct avdtp_stream *stream)
+{
+	struct seid_req req;
+
+	if (!g_slist_find(session->streams, stream))
+		return -EINVAL;
+
+	if (stream->lsep->state > AVDTP_STATE_CONFIGURED)
+		return -EINVAL;
+
+	memset(&req, 0, sizeof(req));
+	init_request(&req.header, AVDTP_OPEN);
+	req.acp_seid = stream->rsep->seid;
+
+	return send_request(session, FALSE, stream, &req, sizeof(req));
+}
+
+int avdtp_start(struct avdtp *session, struct avdtp_stream *stream)
+{
+	struct seid_req req;
+
+	if (!g_slist_find(session->streams, stream))
+		return -EINVAL;
+
+	if (stream->lsep->state != AVDTP_STATE_OPEN)
+		return -EINVAL;
+
+	memset(&req, 0, sizeof(req));
+	init_request(&req.header, AVDTP_START);
+	req.acp_seid = stream->rsep->seid;
+
+	return send_request(session, FALSE, stream, &req, sizeof(req));
+}
+
+int avdtp_close(struct avdtp *session, struct avdtp_stream *stream)
+{
+	struct seid_req req;
+	int ret;
+
+	if (!g_slist_find(session->streams, stream))
+		return -EINVAL;
+
+	if (stream->lsep->state < AVDTP_STATE_OPEN)
+		return -EINVAL;
+
+	memset(&req, 0, sizeof(req));
+	init_request(&req.header, AVDTP_CLOSE);
+	req.acp_seid = stream->rsep->seid;
+
+	ret = send_request(session, FALSE, stream, &req, sizeof(req));
+	if (ret == 0)
+		stream->close_int = TRUE;
+
+	return ret;
+}
+
+int avdtp_suspend(struct avdtp *session, struct avdtp_stream *stream)
+{
+	struct seid_req req;
+	int ret;
+
+	if (!g_slist_find(session->streams, stream))
+		return -EINVAL;
+
+	if (stream->lsep->state <= AVDTP_STATE_OPEN)
+		return -EINVAL;
+
+	memset(&req, 0, sizeof(req));
+	init_request(&req.header, AVDTP_SUSPEND);
+	req.acp_seid = stream->rsep->seid;
+
+	ret = send_request(session, FALSE, stream, &req, sizeof(req));
+	if (ret == 0)
+		avdtp_sep_set_state(session, stream->lsep,
+					AVDTP_STATE_OPEN);
+
+	return ret;
+}
+
+int avdtp_abort(struct avdtp *session, struct avdtp_stream *stream)
+{
+	struct seid_req req;
+	int ret;
+
+	if (!g_slist_find(session->streams, stream))
+		return -EINVAL;
+
+	if (stream->lsep->state <= AVDTP_STATE_OPEN)
+		return -EINVAL;
+
+	memset(&req, 0, sizeof(req));
+	init_request(&req.header, AVDTP_ABORT);
+	req.acp_seid = stream->rsep->seid;
+
+	ret = send_request(session, FALSE, stream, &req, sizeof(req));
+	if (ret == 0)
+		avdtp_sep_set_state(session, stream->lsep,
+					AVDTP_STATE_ABORTING);
+
+	return 0;
+}
+
+struct avdtp_local_sep *avdtp_register_sep(uint8_t type, uint8_t media_type,
+						struct avdtp_sep_ind *ind,
+						struct avdtp_sep_cfm *cfm)
+{
+	struct avdtp_local_sep *sep;
+
+	if (free_seid > MAX_SEID)
+		return NULL;
+
+	sep = g_new0(struct avdtp_local_sep, 1);
+
+	sep->state = AVDTP_STATE_IDLE;
+	sep->info.seid = free_seid++;
+	sep->info.type = type;
+	sep->info.media_type = media_type;
+	sep->ind = ind;
+	sep->cfm = cfm;
+
+	local_seps = g_slist_append(local_seps, sep);
+
+	return sep;
+}
+
+int avdtp_unregister_sep(struct avdtp_local_sep *sep)
+{
+	if (!sep)
+		return -EINVAL;
+
+	if (sep->info.inuse)
+		return -EBUSY;
+
+	local_seps = g_slist_remove(local_seps, sep);
+
+	g_free(sep);
+
+	return 0;
+}
+
+static gboolean avdtp_server_cb(GIOChannel *chan, GIOCondition cond, void *data)
+{
+	int srv_sk, cli_sk;
+	socklen_t size;
+	struct sockaddr_l2 addr;
+	struct l2cap_options l2o;
+	bdaddr_t src, dst;
+	struct avdtp *session;
+	GIOChannel *cli_io;
+	char address[18];
+
+	if (cond & G_IO_NVAL)
+		return FALSE;
+
+	if (cond & (G_IO_HUP | G_IO_ERR)) {
+		error("Hangup or error on AVDTP server socket");
+		g_io_channel_close(chan);
+		raise(SIGTERM);
+		return FALSE;
+	}
+
+	srv_sk = g_io_channel_unix_get_fd(chan);
+
+	size = sizeof(struct sockaddr_l2);
+	cli_sk = accept(srv_sk, (struct sockaddr *) &addr, &size);
+	if (cli_sk < 0) {
+		error("AVDTP accept: %s (%d)", strerror(errno), errno);
+		return TRUE;
+	}
+
+	bacpy(&dst, &addr.l2_bdaddr);
+
+	ba2str(&dst, address);
+	debug("AVDTP: incoming connect from %s", address);
+
+	size = sizeof(struct sockaddr_l2);
+	if (getsockname(srv_sk, (struct sockaddr *) &addr, &size) < 0) {
+		error("getsockname: %s (%d)", strerror(errno), errno);
+		close(cli_sk);
+		return TRUE;
+	}
+
+	bacpy(&src, &addr.l2_bdaddr);
+
+	memset(&l2o, 0, sizeof(l2o));
+	size = sizeof(l2o);
+	if (getsockopt(cli_sk, SOL_L2CAP, L2CAP_OPTIONS, &l2o, &size) < 0) {
+		error("getsockopt(L2CAP_OPTIONS): %s (%d)", strerror(errno),
+			errno);
+		close(cli_sk);
+		return TRUE;
+	}
+
+	session = avdtp_get(&src, &dst);
+
+	if (session->pending_open && session->pending_open->open_acp) {
+		handle_transport_connect(session, cli_sk, l2o.imtu);
+		return TRUE;
+	}
+
+	if (session->sock >= 0) {
+		error("Refusing unexpected connect from %s", address);
+		close(cli_sk);
+		return TRUE;
+	}
+
+	if (session->ref == 1)
+		set_disconnect_timer(session);
+
+	session->mtu = l2o.imtu;
+	session->buf = g_malloc0(session->mtu);
+	session->sock = cli_sk;
+	session->state = AVDTP_SESSION_STATE_CONNECTED;
+
+	cli_io = g_io_channel_unix_new(session->sock);
+	session->io = g_io_add_watch(cli_io,
+				G_IO_IN | G_IO_ERR | G_IO_HUP | G_IO_NVAL,
+				(GIOFunc) session_cb, session);
+	g_io_channel_unref(cli_io);
+
+	return TRUE;
+}
+
+static GIOChannel *avdtp_server_socket(void)
+{
+	int sock, lm;
+	struct sockaddr_l2 addr;
+	GIOChannel *io;
+
+	sock = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
+	if (sock < 0) {
+		error("AVDTP server socket: %s (%d)", strerror(errno), errno);
+		return NULL;
+	}
+
+	lm = L2CAP_LM_SECURE;
+	if (setsockopt(sock, SOL_L2CAP, L2CAP_LM, &lm, sizeof(lm)) < 0) {
+		error("AVDTP server setsockopt: %s (%d)", strerror(errno), errno);
+		close(sock);
+		return NULL;
+	}
+
+	memset(&addr, 0, sizeof(addr));
+	addr.l2_family = AF_BLUETOOTH;
+	bacpy(&addr.l2_bdaddr, BDADDR_ANY);
+	addr.l2_psm = htobs(AVDTP_PSM);
+
+	if (bind(sock, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+		error("AVDTP server bind: %s", strerror(errno), errno);
+		close(sock);
+		return NULL;
+	}
+
+	if (listen(sock, 4) < 0) {
+		error("AVDTP server listen: %s", strerror(errno), errno);
+		close(sock);
+		return NULL;
+	}
+
+	io = g_io_channel_unix_new(sock);
+	if (!io) {
+		error("Unable to allocate new io channel");
+		close(sock);
+		return NULL;
+	}
+
+	return io;
+}
+
+const char *avdtp_strerror(struct avdtp_error *err)
+{
+	if (err->type == AVDTP_ERROR_ERRNO)
+		return strerror(err->err.posix_errno);
+
+	switch(err->err.error_code) {
+	case AVDTP_BAD_HEADER_FORMAT:
+		return "Bad Header Format";
+	case AVDTP_BAD_LENGTH:
+		return "Bad Packet Lenght";
+	case AVDTP_BAD_ACP_SEID:
+		return "Bad Acceptor SEID";
+	case AVDTP_SEP_IN_USE:
+		return "Stream End Point in Use";
+	case AVDTP_SEP_NOT_IN_USE:
+		return "Stream End Point Not in Use";
+	case AVDTP_BAD_SERV_CATEGORY:
+		return "Bad Service Category";
+	case AVDTP_BAD_PAYLOAD_FORMAT:
+		return "Bad Payload format";
+	case AVDTP_NOT_SUPPORTED_COMMAND:
+		return "Command Not Supported";
+	case AVDTP_INVALID_CAPABILITIES:
+		return "Invalid Capabilities";
+	case AVDTP_BAD_RECOVERY_TYPE:
+		return "Bad Recovery Type";
+	case AVDTP_BAD_MEDIA_TRANSPORT_FORMAT:
+		return "Bad Media Transport Format";
+	case AVDTP_BAD_RECOVERY_FORMAT:
+		return "Bad Recovery Format";
+	case AVDTP_BAD_ROHC_FORMAT:
+		return "Bad Header Compression Format";
+	case AVDTP_BAD_CP_FORMAT:
+		return "Bad Content Protetion Format";
+	case AVDTP_BAD_MULTIPLEXING_FORMAT:
+		return "Bad Multiplexing Format";
+	case AVDTP_UNSUPPORTED_CONFIGURATION:
+		return "Configuration not supported";
+	case AVDTP_BAD_STATE:
+		return "Bad State";
+	default:
+		return "Unknow error";
+	}
+}
+
+int avdtp_init(void)
+{
+	if (avdtp_server)
+		return 0;
+
+	avdtp_server = avdtp_server_socket();
+	if (!avdtp_server)
+		return -1;
+
+	g_io_add_watch(avdtp_server, G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
+			(GIOFunc) avdtp_server_cb, NULL);
+
+	return 0;
+}
+
+void avdtp_exit(void)
+{
+	if (!avdtp_server)
+		return;
+
+	g_io_channel_close(avdtp_server);
+	g_io_channel_unref(avdtp_server);
+	avdtp_server = NULL;
+}
diff --git a/audio/avdtp.h b/audio/avdtp.h
index 153a1731..b4af9eb3 100644
--- a/audio/avdtp.h
+++ b/audio/avdtp.h
@@ -20,4 +20,181 @@
  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  *
  */
+#ifndef __AVDTP_H__
+#define __AVDTP_H__
 
+#include <stdint.h>
+#include <bluetooth/bluetooth.h>
+
+struct avdtp;
+struct avdtp_stream;
+struct avdtp_local_sep;
+struct avdtp_remote_sep;
+struct avdtp_error;
+
+/* SEP capability categories */
+#define AVDTP_MEDIA_TRANSPORT			0x01
+#define AVDTP_REPORTING				0x02
+#define AVDTP_RECOVERY				0x03
+#define AVDTP_CONTENT_PROTECTION		0x04
+#define AVDTP_HEADER_COMPRESSION		0x05
+#define AVDTP_MULTIPLEXING			0x06
+#define AVDTP_MEDIA_CODEC			0x07
+
+/* AVDTP error definitions */
+#define AVDTP_BAD_HEADER_FORMAT			0x01
+#define AVDTP_BAD_LENGTH			0x11
+#define AVDTP_BAD_ACP_SEID			0x12
+#define AVDTP_SEP_IN_USE			0x13
+#define AVDTP_SEP_NOT_IN_USE			0x14
+#define AVDTP_BAD_SERV_CATEGORY			0x17
+#define AVDTP_BAD_PAYLOAD_FORMAT		0x18
+#define AVDTP_NOT_SUPPORTED_COMMAND		0x19
+#define AVDTP_INVALID_CAPABILITIES		0x1A
+#define AVDTP_BAD_RECOVERY_TYPE			0x22
+#define AVDTP_BAD_MEDIA_TRANSPORT_FORMAT	0x23
+#define AVDTP_BAD_RECOVERY_FORMAT		0x25
+#define AVDTP_BAD_ROHC_FORMAT			0x26
+#define AVDTP_BAD_CP_FORMAT			0x27
+#define AVDTP_BAD_MULTIPLEXING_FORMAT		0x28
+#define AVDTP_UNSUPPORTED_CONFIGURATION		0x29
+#define AVDTP_BAD_STATE				0x31
+
+/* SEP types definitions */
+#define AVDTP_SEP_TYPE_SOURCE			0x00
+#define AVDTP_SEP_TYPE_SINK			0x01
+
+/* Media types definitions */
+#define AVDTP_MEDIA_TYPE_AUDIO			0x00
+#define AVDTP_MEDIA_TYPE_VIDEO			0x01
+#define AVDTP_MEDIA_TYPE_MULTIMEDIA		0x02
+
+typedef enum {
+	AVDTP_STATE_IDLE,
+	AVDTP_STATE_CONFIGURED,
+	AVDTP_STATE_OPEN,
+	AVDTP_STATE_STREAMING,
+	AVDTP_STATE_CLOSING,
+	AVDTP_STATE_ABORTING,
+} avdtp_state_t;
+
+struct avdtp_service_capability {
+	uint8_t category;
+	uint8_t length;
+	uint8_t data[0];
+} __attribute__ ((packed));
+
+struct avdtp_media_codec_capability {
+	uint8_t rfa0:4;
+	uint8_t media_type:4;
+	uint8_t media_codec_type;
+	uint8_t data[0];
+} __attribute__ ((packed));
+
+typedef void (*avdtp_stream_state_cb) (struct avdtp_stream *stream,
+					avdtp_state_t old_state,
+					avdtp_state_t new_state,
+					struct avdtp_error *err,
+					void *user_data);
+
+/* Callbacks for when a reply is received to a command that we sent */
+struct avdtp_sep_cfm {
+	void (*set_configuration) (struct avdtp_local_sep *lsep,
+					struct avdtp_stream *stream);
+	void (*get_configuration) (struct avdtp_local_sep *lsep,
+					struct avdtp_stream *stream);
+	void (*open) (struct avdtp_local_sep *lsep,
+				struct avdtp_stream *stream);
+	void (*start) (struct avdtp_local_sep *lsep,
+			struct avdtp_stream *stream);
+	void (*suspend) (struct avdtp_local_sep *lsep,
+				struct avdtp_stream *stream);
+	void (*close) (struct avdtp_local_sep *lsep,
+				struct avdtp_stream *stream);
+	void (*abort) (struct avdtp_local_sep *lsep,
+				struct avdtp_stream *stream);
+	void (*reconfigure) (struct avdtp_local_sep *lsep);
+};
+
+/* Callbacks for indicating when we received a new command. The return value
+ * indicates whether the command should be rejected or accepted */
+struct avdtp_sep_ind {
+	gboolean (*get_capability) (struct avdtp_local_sep *sep,
+					GSList **caps, uint8_t *err);
+	gboolean (*set_configuration) (struct avdtp_local_sep *lsep,
+					struct avdtp_stream *stream,
+					uint8_t int_seid, GSList *caps,
+					uint8_t *err);
+	gboolean (*get_configuration) (struct avdtp_local_sep *lsep,
+					uint8_t *err);
+	gboolean (*open) (struct avdtp_local_sep *lsep,
+				struct avdtp_stream *stream, uint8_t *err);
+	gboolean (*start) (struct avdtp_local_sep *lsep,
+				struct avdtp_stream *stream,
+				uint8_t *err);
+	gboolean (*suspend) (struct avdtp_local_sep *sep,
+				struct avdtp_stream *stream,
+				uint8_t *err);
+	gboolean (*close) (struct avdtp_local_sep *sep,
+				struct avdtp_stream *stream,
+				uint8_t *err);
+	gboolean (*abort) (struct avdtp_local_sep *sep,
+				struct avdtp_stream *stream, uint8_t *err);
+	gboolean (*reconfigure) (struct avdtp_local_sep *lsep, uint8_t *err);
+};
+
+typedef void (*avdtp_discover_cb_t) (struct avdtp *session, GSList *seps,
+					int err, void *user_data);
+
+struct avdtp *avdtp_get(bdaddr_t *src, bdaddr_t *dst);
+
+void avdtp_unref(struct avdtp *session);
+struct avdtp *avdtp_ref(struct avdtp *session);
+
+struct avdtp_service_capability *avdtp_service_cap_new(uint8_t category,
+							void *data, int size);
+
+struct avdtp_service_capability *avdtp_get_codec(struct avdtp_remote_sep *sep);
+
+int avdtp_discover(struct avdtp *session, avdtp_discover_cb_t cb,
+			void *user_data);
+
+void avdtp_stream_set_cb(struct avdtp *session, struct avdtp_stream *stream,
+				avdtp_stream_state_cb cb, void *data);
+
+gboolean avdtp_stream_get_transport(struct avdtp_stream *stream, int *sock,
+					uint16_t *mtu, GSList **caps);
+
+int avdtp_set_configuration(struct avdtp *session,
+				struct avdtp_remote_sep *rsep,
+				struct avdtp_local_sep *lsep,
+				GSList *caps,
+				struct avdtp_stream **stream);
+
+int avdtp_get_configuration(struct avdtp *session,
+				struct avdtp_stream *stream);
+
+int avdtp_open(struct avdtp *session, struct avdtp_stream *stream);
+int avdtp_reconfigure(struct avdtp *session, struct avdtp_stream *stream);
+int avdtp_start(struct avdtp *session, struct avdtp_stream *stream);
+int avdtp_suspend(struct avdtp *session, struct avdtp_stream *stream);
+int avdtp_close(struct avdtp *session, struct avdtp_stream *stream);
+int avdtp_abort(struct avdtp *session, struct avdtp_stream *stream);
+
+struct avdtp_local_sep *avdtp_register_sep(uint8_t type, uint8_t media_type,
+						struct avdtp_sep_ind *ind,
+						struct avdtp_sep_cfm *cfm);
+
+/* Find a matching pair of local and remote SEP ID's */
+int avdtp_get_seps(struct avdtp *session, uint8_t type, uint8_t media,
+			uint8_t codec, struct avdtp_local_sep **lsep,
+			struct avdtp_remote_sep **rsep);
+
+int avdtp_unregister_sep(struct avdtp_local_sep *sep);
+
+const char *avdtp_strerror(struct avdtp_error *err);
+
+int avdtp_init(void);
+void avdtp_exit(void);
+
+#endif
diff --git a/audio/device.c b/audio/device.c
index 785d0115..af7cca80 100644
--- a/audio/device.c
+++ b/audio/device.c
@@ -100,21 +100,24 @@ static DBusMethodVTable device_methods[] = {
 	{ NULL, NULL, NULL, NULL }
 };
 
-static void device_free(struct device *device)
+static void device_free(struct device *dev)
 {
-	if (device->headset)
-		headset_free(device);
+	if (dev->headset)
+		headset_free(dev);
 
-	if (device->conn)
-		dbus_connection_unref(device->conn);
+	if (dev->sink)
+		sink_free(dev);
 
-	if (device->adapter_path)
-		g_free(device->adapter_path);
+	if (dev->conn)
+		dbus_connection_unref(dev->conn);
 
-	if (device->path)
-		g_free(device->path);
+	if (dev->adapter_path)
+		g_free(dev->adapter_path);
 
-	g_free(device);
+	if (dev->path)
+		g_free(dev->path);
+
+	g_free(dev);
 }
 
 static void device_unregister(DBusConnection *conn, void *data)
@@ -129,7 +132,7 @@ static void device_unregister(DBusConnection *conn, void *data)
 struct device *device_register(DBusConnection *conn,
 					const char *path, bdaddr_t *bda)
 {
-	struct device *device;
+	struct device *dev;
 	bdaddr_t src;
 	int dev_id;
 
@@ -141,12 +144,12 @@ struct device *device_register(DBusConnection *conn,
 	if ((dev_id < 0) || (hci_devba(dev_id, &src) < 0))
 		return NULL;
 
-	device = g_new0(struct device, 1);
+	dev = g_new0(struct device, 1);
 
-	if (!dbus_connection_create_object_path(conn, path, device,
+	if (!dbus_connection_create_object_path(conn, path, dev,
 							device_unregister)) {
 		error("D-Bus failed to register %s path", path);
-		device_free(device);
+		device_free(dev);
 		return NULL;
 	}
 
@@ -158,58 +161,69 @@ struct device *device_register(DBusConnection *conn,
 		return NULL;
 	}
 
-	device->path = g_strdup(path);
-	bacpy(&device->dst, bda);
-	bacpy(&device->src, &src);
-	device->conn = dbus_connection_ref(conn);
-	device->adapter_path = g_malloc0(16);
-	snprintf(device->adapter_path, 16, "/org/bluez/hci%d", dev_id);
+	dev->path = g_strdup(path);
+	bacpy(&dev->dst, bda);
+	bacpy(&dev->src, &src);
+	dev->conn = dbus_connection_ref(conn);
+	dev->adapter_path = g_malloc0(16);
+	snprintf(dev->adapter_path, 16, "/org/bluez/hci%d", dev_id);
 
-	return device;
+	return dev;
 }
 
-int device_store(struct device *device, gboolean is_default)
+int device_store(struct device *dev, gboolean is_default)
 {
 	char value[64];
 	char filename[PATH_MAX + 1];
 	char src_addr[18], dst_addr[18];
+	int offset = 0;
 
-	if (!device->path)
+	if (!dev->path)
 		return -EINVAL;
 
-	ba2str(&device->dst, dst_addr);
-	ba2str(&device->src, src_addr);
+	ba2str(&dev->dst, dst_addr);
+	ba2str(&dev->src, src_addr);
 
 	create_name(filename, PATH_MAX, STORAGEDIR, src_addr, "audio");
 	create_file(filename, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
 
 	if (is_default)
 		textfile_put(filename, "default", dst_addr);
-	if (device->headset)
-		snprintf(value, 64, "headset");
-	else if (device->gateway)
-		snprintf(value, 64, "gateway");
-	else if (device->sink)
-		snprintf(value, 64, "sink");
-	else if (device->source)
-		snprintf(value, 64, "source");
-	else if (device->control)
-		snprintf(value, 64, "control");
-	else
-		snprintf(value, 64, "target");
+	if (dev->headset) {
+		snprintf(value, 64, "headset ");
+		offset += strlen("headset ");
+	}
+	if (dev->gateway) {
+		snprintf(value + offset, 64 - offset, "gateway ");
+		offset += strlen("gateway ");
+	}
+	if (dev->sink) {
+		snprintf(value + offset, 64 - offset, "sink ");
+		offset += strlen("sink ");
+	}
+	if (dev->source) {
+		snprintf(value + offset, 64 - offset, "source ");
+		offset += strlen("source ");
+	}
+	if (dev->control) {
+		snprintf(value + offset, 64 - offset, "control ");
+		offset += strlen("control ");
+	}
+	if (dev->target)
+		snprintf(value + offset, 64 - offset, "target");
 
 	return textfile_put(filename, dst_addr, value);
 }
 
-void device_finish_sdp_transaction(struct device *device)
+void device_finish_sdp_transaction(struct device *dev)
 {
 	char address[18], *addr_ptr = address;
 	DBusMessage *msg, *reply;
 	DBusError derr;
 
-	ba2str(&device->dst, address);
+	ba2str(&dev->dst, address);
 
-	msg = dbus_message_new_method_call("org.bluez", device->adapter_path,
+	msg = dbus_message_new_method_call("org.bluez", dev->adapter_path,
 						"org.bluez.Adapter",
 						"FinishRemoteServiceTransaction");
 	if (!msg) {
@@ -221,7 +235,7 @@ void device_finish_sdp_transaction(struct device *device)
 				 DBUS_TYPE_INVALID);
 
 	dbus_error_init(&derr);
-	reply = dbus_connection_send_with_reply_and_block(device->conn,
+	reply = dbus_connection_send_with_reply_and_block(dev->conn,
 							msg, -1, &derr);
 
 	dbus_message_unref(msg);
@@ -236,3 +250,114 @@ void device_finish_sdp_transaction(struct device *device)
 
 	dbus_message_unref(reply);
 }
+
+int device_get_config(struct device *dev, int sock, struct ipc_packet *req,
+			int pkt_len, struct ipc_data_cfg **rsp)
+{
+	if (dev->sink && sink_is_active(dev))
+		return sink_get_config(dev, sock, req, pkt_len, rsp);
+	else if (dev->headset && headset_is_active(dev))
+		return headset_get_config(dev, sock, req, pkt_len, rsp);
+	else if (dev->sink)
+		return sink_get_config(dev, sock, req, pkt_len, rsp);
+	else if (dev->headset)
+		return headset_get_config(dev, sock, req, pkt_len, rsp);
+
+	return -EINVAL;
+}
+
+static avdtp_state_t ipc_to_avdtp_state(uint8_t ipc_state)
+{
+	switch (ipc_state) {
+	case STATE_DISCONNECTED:
+		return AVDTP_STATE_IDLE;
+	case STATE_CONNECTING:
+		return AVDTP_STATE_CONFIGURED;
+	case STATE_CONNECTED:
+		return AVDTP_STATE_OPEN;
+	case STATE_STREAM_STARTING:
+	case STATE_STREAMING:
+		return AVDTP_STATE_STREAMING;
+	default:
+		error("Unknown ipc state");
+		return AVDTP_STATE_IDLE;
+	}
+}
+
+static headset_state_t ipc_to_hs_state(uint8_t ipc_state)
+{
+	switch (ipc_state) {
+	case STATE_DISCONNECTED:
+		return HEADSET_STATE_DISCONNECTED;
+	case STATE_CONNECTING:
+		return HEADSET_STATE_CONNECT_IN_PROGRESS;
+	case STATE_CONNECTED:
+		return HEADSET_STATE_CONNECTED;
+	case STATE_STREAM_STARTING:
+		return HEADSET_STATE_PLAY_IN_PROGRESS;
+	case STATE_STREAMING:
+		return HEADSET_STATE_PLAYING;
+	default:
+		error("Unknown ipc state");
+		return HEADSET_STATE_DISCONNECTED;
+	}
+}
+
+void device_set_state(struct device *dev, uint8_t state)
+{
+	if (dev->sink && sink_is_active(dev))
+		sink_set_state(dev, ipc_to_avdtp_state(state));
+	else if (dev->headset && headset_is_active(dev))
+		headset_set_state(dev, ipc_to_hs_state(state));
+}
+
+static uint8_t avdtp_to_ipc_state(avdtp_state_t state)
+{
+	switch (state) {
+	case AVDTP_STATE_IDLE:
+		return STATE_DISCONNECTED;
+	case AVDTP_STATE_CONFIGURED:
+		return STATE_CONNECTING;
+	case AVDTP_STATE_OPEN:
+		return STATE_CONNECTED;
+	case AVDTP_STATE_STREAMING:
+		return STATE_STREAMING;
+	default:
+		error("Unknown avdt state");
+		return AVDTP_STATE_IDLE;
+	}
+}
+
+static uint8_t hs_to_ipc_state(headset_state_t state)
+{
+	switch (state) {
+	case HEADSET_STATE_DISCONNECTED:
+		return STATE_DISCONNECTED;
+	case HEADSET_STATE_CONNECT_IN_PROGRESS:
+		return STATE_CONNECTING;
+	case HEADSET_STATE_CONNECTED:
+		return STATE_CONNECTED;
+	case HEADSET_STATE_PLAY_IN_PROGRESS:
+		return STATE_STREAMING;
+	default:
+		error("Unknown headset state");
+		return AVDTP_STATE_IDLE;
+	}
+}
+
+uint8_t device_get_state(struct device *dev)
+{
+	avdtp_state_t sink_state;
+	headset_state_t hs_state;
+
+	if (dev->sink && sink_is_active(dev)) {
+		sink_state = sink_get_state(dev);
+		return avdtp_to_ipc_state(sink_state);
+	}
+	else if (dev->headset && headset_is_active(dev)) {
+		hs_state = headset_get_state(dev);
+		return hs_to_ipc_state(hs_state);
+	}
+
+	return STATE_DISCONNECTED;
+}
diff --git a/audio/device.h b/audio/device.h
index 71e1053f..62c13e9e 100644
--- a/audio/device.h
+++ b/audio/device.h
@@ -75,3 +75,10 @@ struct device *device_register(DBusConnection *conn,
 int device_store(struct device *device, gboolean is_default);
 
 void device_finish_sdp_transaction(struct device *device);
+
+int device_get_config(struct device *dev, int sock, struct ipc_packet *req,
+			int pkt_len, struct ipc_data_cfg **rsp);
+
+void device_set_state(struct device *dev, uint8_t state);
+
+uint8_t device_get_state(struct device *dev);
diff --git a/audio/headset.c b/audio/headset.c
index 9dccc5ad..2bc8583b 100644
--- a/audio/headset.c
+++ b/audio/headset.c
@@ -53,6 +53,7 @@
 #include "logging.h"
 #include "manager.h"
 #include "error.h"
+#include "unix.h"
 
 #define RING_INTERVAL 3000
 
@@ -68,6 +69,7 @@ struct pending_connect {
 	DBusMessage *msg;
 	GIOChannel *io;
 	struct ipc_packet *pkt;
+	int pkt_len;
 	guint io_id;
 	int sock;
 	int err;
@@ -102,7 +104,7 @@ static int rfcomm_connect(struct device *device, struct pending_connect *c);
 static void pending_connect_free(struct pending_connect *c)
 {
 	if (c->pkt)
-		unix_send_cfg(c->sock, c->pkt);
+		g_free(c->pkt);
 	if (c->io) {
 		g_io_channel_close(c->io);
 		g_io_channel_unref(c->io);
@@ -339,8 +341,19 @@ static void pending_connect_ok(struct pending_connect *c, struct device *dev)
 		if (reply)
 			send_message_and_unref(dev->conn, reply);
 	}
-	else if (c->pkt)
-		headset_get_config(dev, c->sock, c->pkt);
+	else if (c->pkt) {
+		struct ipc_data_cfg *rsp;
+		int ret;
+
+		ret = headset_get_config(dev, c->sock, c->pkt,
+						c->pkt_len, &rsp);
+		if (ret == 0) {
+			unix_send_cfg(c->sock, rsp);
+			g_free(rsp);
+		}
+		else
+			unix_send_cfg(c->sock, NULL);
+	}
 
 	pending_connect_free(c);
 }
@@ -349,6 +362,8 @@ static void pending_connect_failed(struct pending_connect *c, struct device *dev
 {
 	if (c->msg)
 		err_connect_failed(dev->conn, c->msg, strerror(c->err));
+	if (c->pkt)
+		unix_send_cfg(c->sock, NULL);
 	pending_connect_free(c);
 }
 
@@ -1358,7 +1373,8 @@ register_iface:
 
 void headset_free(void *device)
 {
-	struct headset *hs = ((struct device *) device)->headset;
+	struct device *dev = device;
+	struct headset *hs = dev->headset;
 
 	if (hs->sco) {
 		g_io_channel_close(hs->sco);
@@ -1371,54 +1387,54 @@ void headset_free(void *device)
 	}
 
 	g_free(hs);
-	hs = NULL;
+	dev->headset = NULL;
 }
 
-int headset_get_config(void *device, int sock, struct ipc_packet *pkt)
+int headset_get_config(void *device, int sock, struct ipc_packet *pkt,
+			int pkt_len, struct ipc_data_cfg **cfg)
 {
 	struct headset *hs = ((struct device *) device)->headset;
-	struct ipc_data_cfg *cfg = (struct ipc_data_cfg *) pkt->data;
 	int err = EINVAL;
 	struct pending_connect *c;
+	struct ipc_data_cfg *rsp;
+
+	if (hs->rfcomm && hs->sco)
+		goto proceed;
 
-	if (hs->rfcomm == NULL) {
-		c = g_try_new0(struct pending_connect, 1);
-		if (c == NULL)
-			goto error;
-		c->sock = sock;
-		c->pkt = pkt;
+	c = g_new0(struct pending_connect, 1);
+	c->sock = sock;
+	c->pkt = g_malloc(pkt_len);
+	memcpy(c->pkt, pkt, pkt_len);
+
+	if (hs->rfcomm == NULL)
 		err = rfcomm_connect(device, c);
-		if (err < 0)
-			goto error;
-		return 0;
-	}
-	else if (hs->sco == NULL) {
-		c = g_try_new0(struct pending_connect, 1);
-		if (c == NULL)
-			goto error;
-		c->sock = sock;
-		c->pkt = pkt;
+	else if (hs->sco == NULL)
 		err = sco_connect(device, c);
-		if (err < 0)
-			goto error;
-		return 0;
-	}
+	else
+		goto error;
+
+	if (err < 0)
+		goto error;
 
-	cfg->fd = g_io_channel_unix_get_fd(hs->sco);
-	cfg->fd_opt = CFG_FD_OPT_READWRITE;
-	cfg->encoding = 0;
-	cfg->bitpool = 0;
-	cfg->channels = 1;
-	cfg->pkt_len = 48;
-	cfg->sample_size = 2;
-	cfg->rate = 8000;
+	return 1;
+
+proceed:
+	*cfg = g_new0(struct ipc_data_cfg, 1);
+	rsp = *cfg;
+	rsp->fd = g_io_channel_unix_get_fd(hs->sco);
+	rsp->fd_opt = CFG_FD_OPT_READWRITE;
+	rsp->codec = CFG_CODEC_NONE;
+	rsp->channels = 1;
+	rsp->channel_mode = CFG_CHANNEL_MODE_MONO;
+	rsp->pkt_len = 48;
+	rsp->sample_size = 2;
+	rsp->rate = 8000;
 
 	return 0;
 
 error:
 	if (c)
 		pending_connect_free(c);
-	cfg->fd = -1;
 	return -err;
 }
 
@@ -1547,3 +1563,14 @@ int headset_get_channel(void *device)
 
 	return hs->rfcomm_ch;
 }
+
+gboolean headset_is_active(void *device)
+{
+	struct device *dev = device;
+	struct headset *hs = dev->headset;
+
+	if (hs->state != HEADSET_STATE_DISCONNECTED)
+		return TRUE;
+
+	return FALSE;
+}
diff --git a/audio/headset.h b/audio/headset.h
index d3fd86d9..530bdea8 100644
--- a/audio/headset.h
+++ b/audio/headset.h
@@ -20,12 +20,15 @@
  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  *
  */
+#ifndef __AUDIO_HEADSET_H__
+#define __AUDIO_HEADSET_H__
+
 #include <bluetooth/sdp.h>
 #include <bluetooth/sdp_lib.h>
 
 #include <dbus/dbus.h>
 
-#include "unix.h"
+#include "ipc.h"
 
 #define AUDIO_HEADSET_INTERFACE "org.bluez.audio.Headset"
 
@@ -40,11 +43,11 @@ typedef enum {
 } headset_event_t;
 
 typedef enum {
-	HEADSET_STATE_DISCONNECTED = STATE_DISCONNECTED,
-	HEADSET_STATE_CONNECT_IN_PROGRESS = STATE_CONNECTING,
-	HEADSET_STATE_CONNECTED = STATE_CONNECTED,
-	HEADSET_STATE_PLAY_IN_PROGRESS = STATE_STREAM_STARTING,
-	HEADSET_STATE_PLAYING = STATE_STREAMING,
+	HEADSET_STATE_DISCONNECTED,
+	HEADSET_STATE_CONNECT_IN_PROGRESS,
+	HEADSET_STATE_CONNECTED,
+	HEADSET_STATE_PLAY_IN_PROGRESS,
+	HEADSET_STATE_PLAYING
 } headset_state_t;
 
 typedef enum {
@@ -55,13 +58,14 @@ typedef enum {
 struct headset;
 
 struct headset *headset_init(void *device, sdp_record_t *record,
-			uint16_t svc);
+				uint16_t svc);
 
 void headset_free(void *device);
 
 void headset_update(void *device, sdp_record_t *record, uint16_t svc);
 
-int headset_get_config(void *device, int sock, struct ipc_packet *pkt);
+int headset_get_config(void *device, int sock, struct ipc_packet *pkt,
+			int pkt_len, struct ipc_data_cfg **rsp);
 
 headset_type_t headset_get_type(void *device);
 void headset_set_type(void *device, headset_type_t type);
@@ -73,3 +77,7 @@ headset_state_t headset_get_state(void *device);
 void headset_set_state(void *device, headset_state_t state);
 
 int headset_get_channel(void *device);
+
+gboolean headset_is_active(void *device);
+
+#endif
diff --git a/audio/ipc.h b/audio/ipc.h
index e56dca24..bd31abbc 100644
--- a/audio/ipc.h
+++ b/audio/ipc.h
@@ -20,11 +20,15 @@
  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  *
  */
+#ifndef __AUDIO_IPC_H__
+#define __AUDIO_IPC_H__
 
 #include <stdint.h>
 
 #define IPC_TYPE_CONNECT  0x0001
 
+#define IPC_MTU 32
+
 #define IPC_SOCKET_NAME "\0/org/bluez/audio"
 
 #ifndef UNIX_PATH_MAX
@@ -32,22 +36,22 @@
 #endif
 
 /* Supported roles */
-#define PKT_ROLE_NONE		0
-#define PKT_ROLE_AUTO		1
-#define PKT_ROLE_VOICE		2
-#define PKT_ROLE_HIFI		3
+#define PKT_ROLE_NONE			0
+#define PKT_ROLE_AUTO			1
+#define PKT_ROLE_VOICE			2
+#define PKT_ROLE_HIFI			3
 
 /* Packet types */
-#define PKT_TYPE_CFG_REQ	0
-#define PKT_TYPE_CFG_RSP	1
-#define PKT_TYPE_STATE_REQ	2
-#define PKT_TYPE_STATE_RSP	3
-#define PKT_TYPE_CTL_REQ	4
-#define PKT_TYPE_CTL_RSP	5
-#define PKT_TYPE_CTL_NTFY	6
+#define PKT_TYPE_CFG_REQ		0
+#define PKT_TYPE_CFG_RSP		1
+#define PKT_TYPE_STATE_REQ		2
+#define PKT_TYPE_STATE_RSP		3
+#define PKT_TYPE_CTL_REQ		4
+#define PKT_TYPE_CTL_RSP		5
+#define PKT_TYPE_CTL_NTFY		6
 
 /* Errors codes */
-#define PKT_ERROR_NONE		0
+#define PKT_ERROR_NONE			0
 
 struct ipc_packet {
 	uint8_t id;		/* Device id */
@@ -59,52 +63,76 @@ struct ipc_packet {
 } __attribute__ ((packed));
 
 /* File descriptor options */
-#define CFG_FD_OPT_READ		0
-#define CFG_FD_OPT_WRITE	1
-#define CFG_FD_OPT_READWRITE	2
+#define CFG_FD_OPT_READ			0
+#define CFG_FD_OPT_WRITE		1
+#define CFG_FD_OPT_READWRITE		2
+
+/* Audio channel mode */
+#define CFG_CHANNEL_MODE_MONO		(1 << 3)
+#define CFG_CHANNEL_MODE_DUAL_CHANNEL	(1 << 2)
+#define CFG_CHANNEL_MODE_STEREO		(1 << 1)
+#define CFG_CHANNEL_MODE_JOINT_STEREO	1
+
+/* Codec options */
+#define CFG_CODEC_NONE			0
+#define CFG_CODEC_SBC			1
 
 struct ipc_data_cfg {
 	int fd;			/* Stream file descriptor */
 	uint8_t fd_opt;		/* Stream file descriptor options: read, write or readwrite*/
-	uint8_t encoding;	/* Stream encoding */
-	uint8_t bitpool;	/* Encoding bitpool */
 	uint8_t channels;	/* Number of audio channel */
-	uint8_t pkt_len;	/* Stream packet length */
+	uint8_t channel_mode;	/* Audio channel mode*/
+	uint16_t pkt_len;	/* Stream packet length */
 	uint8_t sample_size;	/* Sample size in bytes */
 	uint16_t rate;		/* Stream sample rate */
+	uint8_t codec;		/* Stream codec */
+	uint8_t data[0];	/* Codec payload */
+} __attribute__ ((packed));
+
+/* SBC codec options */
+#define CODEC_SBC_ALLOCATION_SNR	(1 << 1)
+#define CODEC_SBC_ALLOCATION_LOUDNESS	1
+
+struct ipc_codec_sbc {
+	uint8_t allocation;
+	uint8_t subbands;
+	uint8_t blocks;
+	uint8_t bitpool;
 } __attribute__ ((packed));
 
 /* Device status */
-#define STATE_DISCONNECTED	0
-#define STATE_CONNECTING	1
-#define STATE_CONNECTED		2
-#define STATE_STREAM_STARTING	3
-#define STATE_STREAMING		4
+#define STATE_DISCONNECTED		0
+#define STATE_CONNECTING		1
+#define STATE_CONNECTED			2
+#define STATE_STREAM_STARTING		3
+#define STATE_STREAMING			4
 
 struct ipc_data_state {
 	uint8_t state;		/* Stream state */
 } __attribute__ ((packed));
 
-#define CTL_MODE_PLAYBACK	0
-#define CTL_MODE_CAPTURE	1
-#define CTL_MODE_GENERAL	2
+#define CTL_MODE_PLAYBACK		0
+#define CTL_MODE_CAPTURE		1
+#define CTL_MODE_GENERAL		2
 
 /* Supported control operations */
-#define CTL_KEY_POWER		0x40
-#define CTL_KEY_VOL_UP		0x41
-#define CTL_KEY_VOL_DOWN	0x42
-#define CTL_KEY_MUTE		0x43
-#define CTL_KEY_PLAY		0x44
-#define CTL_KEY_STOP		0x45
-#define CTL_KEY_PAUSE		0x46
-#define CTL_KEY_RECORD		0x47
-#define CTL_KEY_REWIND		0x48
-#define CTL_KEY_FAST_FORWARD	0x49
-#define CTL_KEY_EJECT		0x4A
-#define CTL_KEY_FORWARD		0x4B
-#define CTL_KEY_BACKWARD	0x4C
+#define CTL_KEY_POWER			0x40
+#define CTL_KEY_VOL_UP			0x41
+#define CTL_KEY_VOL_DOWN		0x42
+#define CTL_KEY_MUTE			0x43
+#define CTL_KEY_PLAY			0x44
+#define CTL_KEY_STOP			0x45
+#define CTL_KEY_PAUSE			0x46
+#define CTL_KEY_RECORD			0x47
+#define CTL_KEY_REWIND			0x48
+#define CTL_KEY_FAST_FORWARD		0x49
+#define CTL_KEY_EJECT			0x4A
+#define CTL_KEY_FORWARD			0x4B
+#define CTL_KEY_BACKWARD		0x4C
 
 struct ipc_data_ctl {
 	uint8_t mode;		/* Control Mode */
 	uint8_t key;		/* Control Key */
 }  __attribute__ ((packed));
+
+#endif
diff --git a/audio/main.c b/audio/main.c
index 4da85a29..72e54133 100644
--- a/audio/main.c
+++ b/audio/main.c
@@ -35,7 +35,7 @@
 
 #include "dbus.h"
 #include "logging.h"
-
+#include "unix.h"
 #include "manager.h"
 
 static gboolean disable_hfp = TRUE;
@@ -43,6 +43,15 @@ static gboolean sco_hci = FALSE;
 
 static GMainLoop *main_loop = NULL;
 
+static struct enabled_interfaces enabled = {
+	.headset	= TRUE,
+	.gateway	= FALSE,
+	.sink		= TRUE,
+	.source		= FALSE,
+	.control	= FALSE,
+	.target		= FALSE,
+};
+
 static void sig_term(int sig)
 {
 	g_main_loop_quit(main_loop);
@@ -53,7 +62,7 @@ static void read_config(const char *file)
 	GKeyFile *keyfile;
 	GError *err = NULL;
 	gboolean no_hfp;
-	char *sco_routing;
+	char *str;
 
 	keyfile = g_key_file_new();
 
@@ -64,21 +73,42 @@ static void read_config(const char *file)
 		return;
 	}
 
-	sco_routing = g_key_file_get_string(keyfile, "General",
+	str = g_key_file_get_string(keyfile, "General",
 						"SCORouting", &err);
 	if (err) {
 		debug("%s: %s", file, err->message);
 		g_error_free(err);
 		err = NULL;
 	} else {
-		if (strcmp(sco_routing, "PCM") == 0)
+		if (strcmp(str, "PCM") == 0)
 			sco_hci = FALSE;
-		else if (strcmp(sco_routing, "HCI") == 0)
+		else if (strcmp(str, "HCI") == 0)
 			sco_hci = TRUE;
 		else
-			error("Invalid Headset Routing value: %s",
-					sco_routing);
-		g_free(sco_routing);
+			error("Invalid Headset Routing value: %s", str);
+		g_free(str);
+	}
+
+	str = g_key_file_get_string(keyfile, "General",
+						"Disabled", &err);
+	if (err) {
+		debug("%s: %s", file, err->message);
+		g_error_free(err);
+		err = NULL;
+	} else {
+		if (strstr(str, "Headset"))
+			enabled.headset = FALSE;
+		if (strstr(str, "Gateway"))
+			enabled.gateway = FALSE;
+		if (strstr(str, "Sink"))
+			enabled.sink = FALSE;
+		if (strstr(str, "Source"))
+			enabled.source = FALSE;
+		if (strstr(str, "Control"))
+			enabled.control = FALSE;
+		if (strstr(str, "Target"))
+			enabled.target = FALSE;
+		g_free(str);
 	}
 
 	no_hfp = g_key_file_get_boolean(keyfile, "Headset",
@@ -131,7 +161,7 @@ int main(int argc, char *argv[])
 		exit(1);
 	}
 
-	if (audio_init(conn, disable_hfp, sco_hci) < 0) {
+	if (audio_init(conn, &enabled, disable_hfp, sco_hci) < 0) {
 		error("Audio init failed!");
 		exit(1);
 	}
diff --git a/audio/manager.c b/audio/manager.c
index 7e250785..58f19ed0 100644
--- a/audio/manager.c
+++ b/audio/manager.c
@@ -52,6 +52,8 @@
 #include "textfile.h"
 #include "manager.h"
 #include "error.h"
+#include "a2dp.h"
+#include "avdtp.h"
 
 typedef enum {
 	HEADSET	= 1 << 0,
@@ -93,6 +95,8 @@ static uint32_t hf_record_id = 0;
 static GIOChannel *hs_server = NULL;
 static GIOChannel *hf_server = NULL;
 
+static const struct enabled_interfaces *enabled;
+
 static void get_next_record(struct audio_sdp_data *data);
 static DBusHandlerResult get_handles(const char *uuid,
 					struct audio_sdp_data *data);
@@ -190,6 +194,8 @@ static gboolean server_is_enabled(uint16_t svc)
 	case HANDSFREE_SVCLASS_ID:
 		ret = (hf_server != NULL);
 		break;
+	case AUDIO_SINK_SVCLASS_ID:
+		return enabled->sink;
 	default:
 		ret = FALSE;
 		break;
@@ -233,6 +239,8 @@ static void handle_record(sdp_record_t *record, struct device *device)
 		break;
 	case AUDIO_SINK_SVCLASS_ID:
 		debug("Found Audio Sink");
+		if (device->sink == NULL)
+			device->sink = sink_init(device);
 		break;
 	case AUDIO_SOURCE_SVCLASS_ID:
 		debug("Found Audio Source");
@@ -713,7 +721,7 @@ static gboolean device_supports_interface(struct device *device,
 		return device->source ? TRUE : FALSE;
 
 	if (strcmp(iface, AUDIO_SINK_INTERFACE) == 0)
-			return device->sink ? TRUE : FALSE;
+		return device->sink ? TRUE : FALSE;
 
 	if (strcmp(iface, AUDIO_CONTROL_INTERFACE) == 0)
 		return device->control ? TRUE : FALSE;
@@ -1135,9 +1143,10 @@ static void parse_stored_devices(char *key, char *value, void *data)
 	if (!device)
 		return;
 
-	if (strncmp(value, "headset", strlen("headset")) == 0)
+	if (strstr(value, "headset"))
 		device->headset = headset_init(device, NULL, 0);
-
+	if (strstr(value, "sink"))
+		device->sink = sink_init(device);
 	add_device(device);
 }
 
@@ -1353,7 +1362,7 @@ static int hfp_ag_record(sdp_buf_t *buf, uint8_t ch)
 	return ret;
 }
 
-static uint32_t add_record(uint8_t channel, sdp_buf_t *buf)
+uint32_t add_service_record(DBusConnection *conn, sdp_buf_t *buf)
 {
 	DBusMessage *msg, *reply;
 	DBusError derr;
@@ -1397,12 +1406,12 @@ static uint32_t add_record(uint8_t channel, sdp_buf_t *buf)
 
 	dbus_message_unref(reply);
 
-	debug("add_record: got record id 0x%x", rec_id);
+	debug("add_service_record: got record id 0x%x", rec_id);
 
 	return rec_id;
 }
 
-static int remove_record(uint32_t rec_id)
+int remove_service_record(DBusConnection *conn, uint32_t rec_id)
 {
 	DBusMessage *msg, *reply;
 	DBusError derr;
@@ -1633,11 +1642,14 @@ static GIOChannel *server_socket(uint8_t *channel)
 	return io;
 }
 
-static int server_init(DBusConnection *conn, gboolean no_hfp)
+static int headset_server_init(DBusConnection *conn, gboolean no_hfp)
 {
 	uint8_t chan = DEFAULT_HS_AG_CHANNEL;
 	sdp_buf_t buf;
 
+	if (!(enabled->headset || enabled->gateway))
+		return 0;
+
 	hs_server = server_socket(&chan);
 	if (!hs_server)
 		return -1;
@@ -1647,7 +1659,7 @@ static int server_init(DBusConnection *conn, gboolean no_hfp)
 		return -1;
 	}
 
-	hs_record_id = add_record(chan, &buf);
+	hs_record_id = add_service_record(conn, &buf);
 	free(buf.data);
 	if (!hs_record_id) {
 		error("Unable to register HS AG service record");
@@ -1673,7 +1685,7 @@ static int server_init(DBusConnection *conn, gboolean no_hfp)
 		return -1;
 	}
 
-	hf_record_id = add_record(chan, &buf);
+	hf_record_id = add_service_record(conn, &buf);
 	free(buf.data);
 	if (!hf_record_id) {
 		error("Unable to register HS AG service record");
@@ -1691,7 +1703,7 @@ static int server_init(DBusConnection *conn, gboolean no_hfp)
 static void server_exit(void)
 {
 	if (hs_record_id) {
-		remove_record(hs_record_id);
+		remove_service_record(connection, hs_record_id);
 		hs_record_id = 0;
 	}
 
@@ -1701,7 +1713,7 @@ static void server_exit(void)
 	}
 
 	if (hf_record_id) {
-		remove_record(hf_record_id);
+		remove_service_record(connection, hf_record_id);
 		hf_record_id = 0;
 	}
 
@@ -1711,11 +1723,17 @@ static void server_exit(void)
 	}
 }
 
-int audio_init(DBusConnection *conn, gboolean no_hfp, gboolean sco_hci)
+int audio_init(DBusConnection *conn, struct enabled_interfaces *enable,
+		gboolean no_hfp, gboolean sco_hci)
 {
 	connection = dbus_connection_ref(conn);
 
-	if (server_init(conn, no_hfp) < 0)
+	enabled = enable;
+
+	if (headset_server_init(conn, no_hfp) < 0)
+		goto failed;
+
+	if (a2dp_init(conn, enable->sink, enable->source) < 0)
 		goto failed;
 
 	if (!dbus_connection_create_object_path(conn, AUDIO_MANAGER_PATH,
@@ -1754,7 +1772,24 @@ void audio_exit(void)
 	connection = NULL;
 }
 
-struct device *manager_default_device()
+struct device *manager_default_device(void)
 {
 	return default_dev;
 }
+
+struct device *manager_get_connected_device(void)
+{
+	GSList *l;
+
+	for (l = devices; l != NULL; l = g_slist_next(l)) {
+		struct device *device = l->data;
+
+		if (device->sink && sink_is_active(device))
+			return device;
+
+		if (device->headset && headset_is_active(device))
+			return device;
+	}
+
+	return NULL;
+}
diff --git a/audio/manager.h b/audio/manager.h
index 79fe9090..9fbc4940 100644
--- a/audio/manager.h
+++ b/audio/manager.h
@@ -29,10 +29,25 @@
 #define AUDIO_MANAGER_PATH "/org/bluez/audio"
 #define AUDIO_MANAGER_INTERFACE "org.bluez.audio.Manager"
 
-int audio_init(DBusConnection *conn, gboolean no_hfp, gboolean sco_hci);
+struct enabled_interfaces {
+	gboolean headset;
+	gboolean gateway;
+	gboolean sink;
+	gboolean source;
+	gboolean control;
+	gboolean target;
+};
+
+int audio_init(DBusConnection *conn, struct enabled_interfaces *enabled,
+		gboolean no_hfp, gboolean sco_hci);
 
 void audio_exit(void);
 
+uint32_t add_service_record(DBusConnection *conn, sdp_buf_t *buf);
+int remove_service_record(DBusConnection *conn, uint32_t rec_id);
+
 struct device *manager_device_connected(bdaddr_t *bda);
 
 struct device *manager_default_device();
+
+struct device *manager_get_connected_device(void);
diff --git a/audio/pcm_bluetooth.c b/audio/pcm_bluetooth.c
index 92d0383c..3f428ecd 100644
--- a/audio/pcm_bluetooth.c
+++ b/audio/pcm_bluetooth.c
@@ -27,6 +27,9 @@
 
 #include <sys/socket.h>
 #include <sys/un.h>
+#include <sys/time.h>
+
+#include <netinet/in.h>
 
 #include <alsa/asoundlib.h>
 #include <alsa/pcm_external.h>
@@ -35,6 +38,11 @@
 #include <bluetooth/sco.h>
 
 #include "ipc.h"
+#include "sbc.h"
+
+/*#define ENABLE_DEBUG */
+
+#define BUFFER_SIZE 1024
 
 #ifdef ENABLE_DEBUG
 #define DBG(fmt, arg...)  printf("DEBUG: %s: " fmt "\n" , __FUNCTION__ , ## arg)
@@ -50,15 +58,65 @@
 #define SCO_RXBUFS 0x04
 #endif
 
+struct rtp_header {
+	uint8_t cc:4;
+	uint8_t x:1;
+	uint8_t p:1;
+	uint8_t v:2;
+
+	uint8_t pt:7;
+	uint8_t m:1;
+
+	uint16_t sequence_number;
+	uint32_t timestamp;
+	uint32_t ssrc;
+	uint32_t csrc[0];
+} __attribute__ ((packed));
+
+struct rtp_payload {
+	uint8_t frame_count:4;
+	uint8_t rfa0:1;
+	uint8_t is_last_fragment:1;
+	uint8_t is_first_fragment:1;
+	uint8_t is_fragmented:1;
+} __attribute__ ((packed));
+
+struct bluetooth_a2dp {
+	sbc_t sbc;			/* Codec data */
+	int samples;			/* Number of encoded samples */
+	time_t timestamp;		/* Codec samples timestamp */
+	uint8_t buffer[BUFFER_SIZE];	/* Codec transfer buffer */
+	int count;			/* Codec transfer buffer counter */
+
+	int nsamples;			/* Cumulative number of codec samples */
+	struct timeval ntimestamp;	/* Cumulative timeval */
+	uint16_t seq_num;		/* */
+	int frame_count;		/* */
+
+	int bandwithcount;
+	struct timeval bandwithtimestamp;
+};
+
 struct bluetooth_data {
 	snd_pcm_ioplug_t io;
 	snd_pcm_sframes_t hw_ptr;
 	struct ipc_data_cfg cfg;	/* Bluetooth device config */
 	int sock;			/* Daemon unix socket */
-	uint8_t *buffer;		/* Transfer buffer */
-	uint8_t count;			/* Transfer buffer counter */
+	uint8_t buffer[BUFFER_SIZE];	/* Encoded transfer buffer */
+	int count;			/* Transfer buffer counter */
+	struct bluetooth_a2dp a2dp;	/* a2dp data */
 };
 
+void memcpy_changeendian(void *dst, const void *src, int size)
+{
+	int i;
+	const uint16_t *ptrsrc = src;
+	uint16_t *ptrdst = dst;
+	for (i = 0; i < size / 2; i++) {
+		*ptrdst++ = htons(*ptrsrc++);
+	}
+}
+
 static int bluetooth_start(snd_pcm_ioplug_t *io)
 {
 	DBG("bluetooth_start %p", io);
@@ -77,49 +135,21 @@ static snd_pcm_sframes_t bluetooth_pointer(snd_pcm_ioplug_t *io)
 {
 	struct bluetooth_data *data = io->private_data;
 
-	DBG("bluetooth_pointer %p", io);
-
-	DBG("hw_ptr=%lu", data->hw_ptr);
+#if 0
+	DBG("bluetooth_pointer %p, hw_ptr=%lu", io, data->hw_ptr);
+#endif
 
 	return data->hw_ptr;
 }
 
 static void bluetooth_exit(struct bluetooth_data *data)
 {
-	int ret, len = sizeof(struct ipc_packet) + sizeof(struct ipc_data_state);
-	struct ipc_packet *pkt;
-	struct ipc_data_state *state;
-
-	DBG("Sending PKT_TYPE_STATUS_REQ...");
-
-	if ((pkt = malloc(len)) == NULL)
-		goto done;
-
-	memset(pkt, 0, len);
-	pkt->type = PKT_TYPE_STATE_REQ;
-	pkt->role = PKT_ROLE_NONE;
-	pkt->error = PKT_ERROR_NONE;
-
-	state = (struct ipc_data_state *) pkt->data;
-	state->state = STATE_DISCONNECTED;
-
-	if ((ret = send(data->sock, pkt, len, 0)) < 0)
-		DBG("Error %s (%d)", strerror(errno), errno);
-
-	free(pkt);
-done:
-	if (data == NULL)
-		return;
-
 	if (data->sock >= 0)
 		close(data->sock);
 
 	if (data->cfg.fd >= 0)
 		close(data->cfg.fd);
 
-	if (data->buffer)
-		free(data->buffer);
-
 	free(data);
 }
 
@@ -127,7 +157,7 @@ static int bluetooth_close(snd_pcm_ioplug_t *io)
 {
 	struct bluetooth_data *data = io->private_data;
 
-	DBG("bluetooth_close %p", io);
+	DBG("%p", io);
 
 	bluetooth_exit(data);
 
@@ -170,7 +200,7 @@ static int bluetooth_hw_params(snd_pcm_ioplug_t *io, snd_pcm_hw_params_t *params
 		return 0;
 
 	opt_name = (io->stream == SND_PCM_STREAM_PLAYBACK) ?
-		SO_SNDBUF : SO_RCVBUF;
+			SO_SNDBUF : SO_RCVBUF;
 
 	if (setsockopt(cfg.fd, SOL_SCO, opt_name, &period_count,
 			sizeof(period_count)) == 0)
@@ -178,14 +208,15 @@ static int bluetooth_hw_params(snd_pcm_ioplug_t *io, snd_pcm_hw_params_t *params
 
 	err = errno;
 	SNDERR("%s (%d)", strerror(err), err);
+	bluetooth_close(io);
 
 	return -err;
 }
 
-static snd_pcm_sframes_t bluetooth_read(snd_pcm_ioplug_t *io,
-					const snd_pcm_channel_area_t *areas,
-					snd_pcm_uframes_t offset,
-					snd_pcm_uframes_t size)
+static snd_pcm_sframes_t bluetooth_hsp_read(snd_pcm_ioplug_t *io,
+						const snd_pcm_channel_area_t *areas,
+						snd_pcm_uframes_t offset,
+						snd_pcm_uframes_t size)
 {
 	struct bluetooth_data *data = io->private_data;
 	struct ipc_data_cfg cfg = data->cfg;
@@ -193,8 +224,9 @@ static snd_pcm_sframes_t bluetooth_read(snd_pcm_ioplug_t *io,
 	unsigned char *buff;
 	int nrecv, frame_size = 0;
 
-	DBG("areas->step=%u, areas->first=%u, offset=%lu, size=%lu, io->nonblock=%u",
-		areas->step, areas->first, offset, size, io->nonblock);
+	DBG("areas->step=%u, areas->first=%u, offset=%lu, size=%lu,"
+		"io->nonblock=%u", areas->step, areas->first, offset, size,
+		io->nonblock);
 
 	if (data->count > 0)
 		goto proceed;
@@ -216,10 +248,12 @@ static snd_pcm_sframes_t bluetooth_read(snd_pcm_ioplug_t *io,
 	}
 
 	/* Increment hardware transmition pointer */
-	data->hw_ptr = (data->hw_ptr + cfg.pkt_len / cfg.sample_size) % io->buffer_size;
+	data->hw_ptr = (data->hw_ptr + cfg.pkt_len / cfg.sample_size) %
+			io->buffer_size;
 
 proceed:
-	buff = (unsigned char *) areas->addr + (areas->first + areas->step * offset) / 8;
+	buff = (unsigned char *) areas->addr +
+			(areas->first + areas->step * offset) / 8;
 
 	if ((data->count + size * frame_size) <= cfg.pkt_len)
 		frames_to_write = size;
@@ -238,10 +272,10 @@ done:
 	return ret;
 }
 
-static snd_pcm_sframes_t bluetooth_write(snd_pcm_ioplug_t *io,
-					const snd_pcm_channel_area_t *areas,
-					snd_pcm_uframes_t offset,
-					snd_pcm_uframes_t size)
+static snd_pcm_sframes_t bluetooth_hsp_write(snd_pcm_ioplug_t *io,
+						const snd_pcm_channel_area_t *areas,
+						snd_pcm_uframes_t offset,
+						snd_pcm_uframes_t size)
 {
 	struct bluetooth_data *data = io->private_data;
 	struct ipc_data_cfg cfg = data->cfg;
@@ -263,10 +297,11 @@ static snd_pcm_sframes_t bluetooth_write(snd_pcm_ioplug_t *io,
 	DBG("count = %d, frames_to_read = %lu", data->count, frames_to_read);
 
 	/* Ready for more data */
-	buff = (uint8_t *) areas->addr + (areas->first + areas->step * offset) / 8;
+	buff = (uint8_t *) areas->addr +
+			(areas->first + areas->step * offset) / 8;
 	memcpy(data->buffer + data->count, buff, frame_size * frames_to_read);
 
-	/* Remember we have some frame in the pipe now */
+	/* Remember we have some frames in the pipe now */
 	data->count += frames_to_read * frame_size;
 	if (data->count != cfg.pkt_len) {
 		ret = frames_to_read;
@@ -290,28 +325,245 @@ static snd_pcm_sframes_t bluetooth_write(snd_pcm_ioplug_t *io,
 		ret = -EIO;
 
 done:
-	DBG("returning %d", (int)ret);
+	DBG("returning %lu", ret);
+	return ret;
+}
+
+static snd_pcm_sframes_t bluetooth_a2dp_read(snd_pcm_ioplug_t *io,
+						const snd_pcm_channel_area_t *areas,
+						snd_pcm_uframes_t offset,
+						snd_pcm_uframes_t size)
+{
+	snd_pcm_uframes_t ret = 0;
+	return ret;
+}
+
+static int avdtp_write(struct bluetooth_a2dp *a2dp, struct ipc_data_cfg *cfg,
+			unsigned int nonblock)
+{
+	int count = 0;
+	int written;
+	struct rtp_header *header;
+	struct rtp_payload *payload;
+#ifdef ENABLE_DEBUG
+	static struct timeval send_date = { 0, 0 };
+	static struct timeval prev_date = { 0, 0 };
+	struct timeval send_delay = { 0, 0 };
+	struct timeval sendz_delay = { 0, 0 };
+#endif
+
+	header = (void *) a2dp->buffer;
+	payload = (void *) (a2dp->buffer + sizeof(*header));
+
+	memset(a2dp->buffer, 0, sizeof(*header) + sizeof(*payload));
+
+	payload->frame_count = a2dp->frame_count;
+	header->v = 2;
+	header->pt = 1;
+	header->sequence_number = htons(a2dp->seq_num);
+	header->timestamp = htonl(a2dp->nsamples);
+	header->ssrc = htonl(1);
+
+	while (count++ < 50) {
+#ifdef ENABLE_DEBUG
+		gettimeofday(&send_date, NULL);
+#endif
+		written = send(cfg->fd, a2dp->buffer, a2dp->count,
+				nonblock ? MSG_DONTWAIT : 0);
+
+#ifdef ENABLE_DEBUG
+		if ((written >= 0 || errno == EAGAIN) && prev_date.tv_sec != 0) {
+			long delay, real, theo, delta;
+
+			delay = (long) (send_delay.tv_sec * 1000 +
+						send_delay.tv_usec / 1000),
+			real = (long) (sendz_delay.tv_sec * 1000 +
+						sendz_delay.tv_usec / 1000);
+			theo = (long) (((float) a2dp->nsamples) /
+						cfg->rate * 1000.0);
+			delta = (long) (sendz_delay.tv_sec * 1000 +
+						sendz_delay.tv_usec / 1000) -
+					(long) (((float) a2dp->nsamples) /
+							cfg->rate * 1000.0);
+
+			timersub(&send_date, &prev_date, &send_delay);
+			timersub(&send_date, &a2dp->ntimestamp, &sendz_delay);
+
+			DBG("send %d (cumul=%d) samples (delay=%ld ms,"
+					" real=%ld ms, theo=%ld ms,"
+					" delta=%ld ms).", a2dp->samples,
+					a2dp->nsamples, delay, real, theo,
+					delta);
+		}
+#endif
+		if (written >= 0)
+			break;
+
+		if (errno != EAGAIN)
+			break;
+
+		DBG("send (retry).");
+		usleep(150000);
+	}
+
+#ifdef ENABLE_DEBUG
+	prev_date = send_date;
+#endif
+
+	/* Send our data */
+	if (written != a2dp->count)
+		DBG("Wrote %d not %d bytes", written, a2dp->count);
+#if 0
+	else {
+		/* Measure bandwith usage */
+		struct timeval now = { 0, 0 };
+		struct timeval interval = { 0, 0 };
+
+		if(a2dp->bandwithtimestamp.tv_sec == 0)
+			gettimeofday(&a2dp->bandwithtimestamp, NULL);
+
+		/* See if we must wait again */
+		gettimeofday(&now, NULL);
+		timersub(&now, &a2dp->bandwithtimestamp, &interval);
+		if(interval.tv_sec > 0)
+			DBG("Bandwith: %d (%d kbps)", a2dp->bandwithcount,
+				a2dp->bandwithcount/128);
+		a2dp->bandwithtimestamp = now;
+		a2dp->bandwithcount = 0;
+	}
+
+	a2dp->bandwithcount += written;
+
+#endif
+	/* Reset buffer of data to send */
+	a2dp->count = sizeof(struct rtp_header) + sizeof(struct rtp_payload);
+	a2dp->frame_count = 0;
+	a2dp->samples = 0;
+	a2dp->seq_num++;
+
+	return written;
+}
+
+static snd_pcm_sframes_t bluetooth_a2dp_write(snd_pcm_ioplug_t *io,
+						const snd_pcm_channel_area_t *areas,
+						snd_pcm_uframes_t offset,
+						snd_pcm_uframes_t size)
+{
+	struct bluetooth_data *data = io->private_data;
+	struct bluetooth_a2dp *a2dp = &data->a2dp;
+	snd_pcm_sframes_t ret = 0;
+	snd_pcm_uframes_t frames_to_read;
+	int frame_size, encoded;
+	uint8_t *buff;
+	static int codesize = 0;
+
+	DBG("areas->step=%u, areas->first=%u, offset=%lu, size=%lu,"
+			"io->nonblock=%u", areas->step, areas->first,
+			offset, size, io->nonblock);
+
+	if (codesize == 0) {
+		/* How much data can be encoded by sbc at a time? */
+		codesize = a2dp->sbc.subbands * a2dp->sbc.blocks *
+				a2dp->sbc.channels * 2;
+		/* Reserv header space in outgoing buffer */
+		a2dp->count = sizeof(struct rtp_header) +
+				sizeof(struct rtp_payload);
+		gettimeofday(&a2dp->ntimestamp, NULL);
+	}
+
+	frame_size = areas->step / 8;
+	if ((data->count + size * frame_size) <= codesize)
+		frames_to_read = size;
+	else
+		frames_to_read = (codesize - data->count) / frame_size;
+
+	DBG("count = %d, frames_to_read = %lu", data->count, frames_to_read);
+	DBG("a2dp.count = %d cfg.pkt_len = %d", a2dp->count,
+			data->cfg.pkt_len);
+
+	/* FIXME: If state is not streaming then return */
+
+	/* Ready for more data */
+	buff = (uint8_t *) areas->addr +
+		(areas->first + areas->step * offset) / 8;
+	memcpy_changeendian(data->buffer + data->count, buff,
+				frame_size * frames_to_read);
+
+	/* Remember we have some frames in the pipe now */
+	data->count += frames_to_read * frame_size;
+	if (data->count != codesize) {
+		ret = frames_to_read;
+		goto done;
+	}
+
+	/* Enough data to encode (sbc wants 1k blocks) */
+	encoded = sbc_encode(&(a2dp->sbc), data->buffer, codesize);
+	if (encoded <= 0) {
+		DBG("Encoding error %d", encoded);
+		goto done;
+	}
+
+	data->count -= encoded;
+
+	DBG("encoded = %d  a2dp.sbc.len= %d", encoded, a2dp->sbc.len);
+
+	if (a2dp->count + a2dp->sbc.len >= data->cfg.pkt_len)
+		avdtp_write(a2dp, &data->cfg, io->nonblock);
+
+	memcpy(a2dp->buffer + a2dp->count, a2dp->sbc.data, a2dp->sbc.len);
+	a2dp->count += a2dp->sbc.len;
+	a2dp->frame_count++;
+	a2dp->samples += encoded / frame_size;
+	a2dp->nsamples += encoded / frame_size;
+	/* Increment hardware transmition pointer */
+	data->hw_ptr = (data->hw_ptr + codesize / frame_size)
+			% io->buffer_size;
+
+	ret = frames_to_read;
+
+done:
+	DBG("returning %lu", ret);
 	return ret;
 }
 
-static snd_pcm_ioplug_callback_t bluetooth_playback_callback = {
+static snd_pcm_ioplug_callback_t bluetooth_hsp_playback = {
+	.start		= bluetooth_start,
+	.stop		= bluetooth_stop,
+	.pointer	= bluetooth_pointer,
+	.close		= bluetooth_close,
+	.hw_params	= bluetooth_hw_params,
+	.prepare	= bluetooth_prepare,
+	.transfer	= bluetooth_hsp_write,
+};
+
+static snd_pcm_ioplug_callback_t bluetooth_hsp_capture = {
 	.start		= bluetooth_start,
 	.stop		= bluetooth_stop,
 	.pointer	= bluetooth_pointer,
 	.close		= bluetooth_close,
 	.hw_params	= bluetooth_hw_params,
 	.prepare	= bluetooth_prepare,
-	.transfer	= bluetooth_write,
+	.transfer	= bluetooth_hsp_read,
 };
 
-static snd_pcm_ioplug_callback_t bluetooth_capture_callback = {
+static snd_pcm_ioplug_callback_t bluetooth_a2dp_playback = {
 	.start		= bluetooth_start,
 	.stop		= bluetooth_stop,
 	.pointer	= bluetooth_pointer,
 	.close		= bluetooth_close,
 	.hw_params	= bluetooth_hw_params,
 	.prepare	= bluetooth_prepare,
-	.transfer	= bluetooth_read,
+	.transfer	= bluetooth_a2dp_write,
+};
+
+static snd_pcm_ioplug_callback_t bluetooth_a2dp_capture = {
+	.start		= bluetooth_start,
+	.stop		= bluetooth_stop,
+	.pointer	= bluetooth_pointer,
+	.close		= bluetooth_close,
+	.hw_params	= bluetooth_hw_params,
+	.prepare	= bluetooth_prepare,
+	.transfer	= bluetooth_a2dp_read,
 };
 
 #define ARRAY_NELEMS(a) (sizeof((a)) / sizeof((a)[0]))
@@ -390,7 +642,6 @@ static int bluetooth_recvmsg_fd(struct bluetooth_data *data)
 	};
 
 	ret = recvmsg(data->sock, &msgh, 0);
-
 	if (ret < 0) {
 		err = errno;
 		SNDERR("Unable to receive fd: %s (%d)", strerror(err), err);
@@ -415,102 +666,143 @@ static int bluetooth_recvmsg_fd(struct bluetooth_data *data)
 	return -EINVAL;
 }
 
-static int bluetooth_cfg(struct bluetooth_data *data)
+static int bluetooth_a2dp_init(struct bluetooth_data *data,
+				struct ipc_codec_sbc *sbc)
 {
-	int ret, len = sizeof(struct ipc_packet) + sizeof(struct ipc_data_cfg);
-	struct ipc_packet *pkt;
+	struct bluetooth_a2dp *a2dp = &data->a2dp;
+	struct ipc_data_cfg *cfg = &data->cfg;
 
-	DBG("Sending PKT_TYPE_CFG_REQ...");
+	if (cfg == NULL) {
+		SNDERR("Error getting codec parameters");
+		return -1;
+	}
 
-	if ((pkt = malloc(len)) == 0)
-		return -ENOMEM;
+	if (cfg->codec != CFG_CODEC_SBC)
+		return -1;
+
+	/* FIXME: init using flags? */
+	sbc_init(&a2dp->sbc, 0);
+	a2dp->sbc.rate = cfg->rate;
+	a2dp->sbc.channels = cfg->channels;
+	if (cfg->channel_mode == CFG_CHANNEL_MODE_MONO ||
+			cfg->channel_mode == CFG_CHANNEL_MODE_JOINT_STEREO)
+		a2dp->sbc.joint = 1;
+	a2dp->sbc.allocation = sbc->allocation;
+	a2dp->sbc.subbands = sbc->subbands;
+	a2dp->sbc.blocks = sbc->blocks;
+	a2dp->sbc.bitpool = sbc->bitpool;
 
-	memset(pkt, 0, len);
+	return 0;
+}
+
+static int bluetooth_cfg(struct bluetooth_data *data, snd_config_t *conf)
+{
+	int ret, total;
+	char buf[IPC_MTU];
+	struct ipc_packet *pkt = (void *) buf;
+	struct ipc_data_cfg *cfg = (void *) pkt->data;
+	struct ipc_codec_sbc *sbc = (void *) cfg->data;
+
+	DBG("Sending PKT_TYPE_CFG_REQ...");
+
+	memset(buf, 0, sizeof(buf));
 	pkt->type = PKT_TYPE_CFG_REQ;
 	pkt->role = PKT_ROLE_NONE;
 	pkt->error = PKT_ERROR_NONE;
 
-	if ((ret = send(data->sock, pkt, len, 0)) < 0) {
-		ret = -errno;
-		goto done;
-	} else if (ret == 0) {
-		ret = -EIO;
-		goto done;
-	}
+	ret = send(data->sock, pkt, sizeof(struct ipc_packet), 0);
+	if (ret < 0)
+		return -errno;
+	else if (ret == 0)
+		return -EIO;
 
-	DBG("OK - %d bytes sent", ret);
+	DBG("OK - %d bytes sent. Waiting for response...", ret);
 
-	DBG("Waiting for response...");
+	memset(buf, 0, sizeof(buf));
 
-	memset(pkt, 0, len);
-	if ((ret = recv(data->sock, pkt, len, 0)) < 0) {
-		ret = -errno;
-		goto done;
-	} else if (ret == 0) {
-		ret = -EIO;
-		goto done;
-	}
+	ret = recv(data->sock, buf, sizeof(*pkt) + sizeof(*cfg), 0);
+	if (ret < 0)
+		return -errno;
+	else if (ret == 0)
+		return -EIO;
 
-	DBG("OK - %d bytes received", ret);
+	total = ret;
 
 	if (pkt->type != PKT_TYPE_CFG_RSP) {
 		SNDERR("Unexpected packet type received: type = %d",
 				pkt->type);
-		ret = -EINVAL;
-		goto done;
+		return -EINVAL;
 	}
 
 	if (pkt->error != PKT_ERROR_NONE) {
 		SNDERR("Error while configuring device: error = %d",
 				pkt->error);
-		ret = pkt->error;
-		goto done;
+		return pkt->error;
 	}
 
-	if (pkt->length != sizeof(struct ipc_data_cfg)) {
+	if (cfg->codec != CFG_CODEC_SBC)
+		goto done;
+
+	ret = recv(data->sock, sbc, sizeof(*sbc), 0);
+	if (ret < 0)
+		return -errno;
+	else if (ret == 0)
+		return -EIO;
+
+	total += ret;
+
+done:
+	DBG("OK - %d bytes received", total);
+
+	if (pkt->length != (total - sizeof(struct ipc_packet))) {
 		SNDERR("Error while configuring device: packet size doesn't "
 				"match");
-		ret = -EINVAL;
-		goto done;
+		return -EINVAL;
 	}
 
-	memcpy(&data->cfg, pkt->data, sizeof(struct ipc_data_cfg));
+	memcpy(&data->cfg, cfg, sizeof(*cfg));
 
 	DBG("Device configuration:");
 
-	DBG("fd=%d, fd_opt=%u, channels=%u, pkt_len=%u, sample_size=%u,"
-			"rate=%u", data->cfg.fd, data->cfg.fd_opt,
-			data->cfg.channels, data->cfg.pkt_len,
-			data->cfg.sample_size, data->cfg.rate);
+	DBG("\n\tfd=%d\n\tfd_opt=%u\n\tchannels=%u\n\tpkt_len=%u\n"
+		"\tsample_size=%u\n\trate=%u", data->cfg.fd,
+		data->cfg.fd_opt, data->cfg.channels, data->cfg.pkt_len,
+		data->cfg.sample_size, data->cfg.rate);
+
+	if (data->cfg.codec == CFG_CODEC_SBC) {
+		struct bluetooth_a2dp *a2dp = &data->a2dp;
+		ret = bluetooth_a2dp_init(data, sbc);
+		if (ret < 0)
+			return ret;
+		printf("\tallocation=%u\n\tsubbands=%u\n\tblocks=%u\n\t"
+				"bitpool=%u\n", a2dp->sbc.allocation,
+				a2dp->sbc.subbands, a2dp->sbc.blocks,
+				a2dp->sbc.bitpool);
+	}
 
 	if (data->cfg.fd == -1) {
 		SNDERR("Error while configuring device: could not acquire "
 				"audio socket");
-		ret = -EINVAL;
-		goto done;
+		return -EINVAL;
 	}
 
-	if ((ret = bluetooth_recvmsg_fd(data)) < 0)
-		goto done;
-
-	if ((data->buffer = malloc(data->cfg.pkt_len)) == 0)
-		return -ENOMEM;
+	ret = bluetooth_recvmsg_fd(data);
+	if (ret < 0)
+		return ret;
 
 	/* It is possible there is some outstanding
 	data in the pipe - we have to empty it */
-	while(recv(data->cfg.fd, data->buffer, data->cfg.pkt_len,
-		MSG_DONTWAIT) > 0);
+	while (recv(data->cfg.fd, data->buffer, data->cfg.pkt_len,
+				MSG_DONTWAIT) > 0);
 
 	memset(data->buffer, 0, data->cfg.pkt_len);
 
-done:
-	free(pkt);
-	return ret;
+	return 0;
 }
 
-static int bluetooth_init(struct bluetooth_data *data)
+static int bluetooth_init(struct bluetooth_data *data, snd_config_t *conf)
 {
-	int sk, err, id;
+	int sk, err;
 	struct sockaddr_un addr = {
 		AF_UNIX, IPC_SOCKET_NAME
 	};
@@ -522,28 +814,24 @@ static int bluetooth_init(struct bluetooth_data *data)
 
 	data->sock = -1;
 
-	id = abs(getpid() * rand());
-
-	if ((sk = socket(PF_LOCAL, SOCK_STREAM, 0)) < 0) {
-		err = -errno;
-		SNDERR("Can't open socket");
-		return -errno;
+	sk = socket(PF_LOCAL, SOCK_STREAM, 0);
+	if (sk < 0) {
+		err = errno;
+		SNDERR("Cannot open socket: %s (%d)", strerror(err), err);
+		return -err;
 	}
 
 	DBG("Connecting to address: %s", addr.sun_path + 1);
 	if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
-		err = -errno;
-		SNDERR("Can't connect socket");
+		err = errno;
+		SNDERR("Connection fail", strerror(err), err);
 		close(sk);
-		return err;
+		return -err;
 	}
 
 	data->sock = sk;
 
-	if ((err = bluetooth_cfg(data)) < 0)
-		return err;
-
-	return 0;
+	return bluetooth_cfg(data, conf);
 }
 
 SND_PCM_PLUGIN_DEFINE_FUNC(bluetooth)
@@ -555,27 +843,32 @@ SND_PCM_PLUGIN_DEFINE_FUNC(bluetooth)
 		stream == SND_PCM_STREAM_PLAYBACK ? "Playback" : "Capture");
 
 	data = malloc(sizeof(struct bluetooth_data));
-	memset(data, 0, sizeof(struct bluetooth_data));
 	if (!data) {
 		err = -ENOMEM;
 		goto error;
 	}
 
-	err = bluetooth_init(data);
+	err = bluetooth_init(data, conf);
 	if (err < 0)
 		goto error;
 
 	data->io.version = SND_PCM_IOPLUG_VERSION;
 	data->io.name = "Bluetooth Audio Device";
 	data->io.mmap_rw = 0; /* No direct mmap communication */
-
-	data->io.callback = stream == SND_PCM_STREAM_PLAYBACK ?
-		&bluetooth_playback_callback : &bluetooth_capture_callback;
 	data->io.poll_fd = data->cfg.fd;
 	data->io.poll_events = stream == SND_PCM_STREAM_PLAYBACK ?
 					POLLOUT : POLLIN;
 	data->io.private_data = data;
 
+	if (data->cfg.codec == CFG_CODEC_SBC)
+		data->io.callback = stream == SND_PCM_STREAM_PLAYBACK ?
+			&bluetooth_a2dp_playback :
+			&bluetooth_a2dp_capture;
+	else
+		data->io.callback = stream == SND_PCM_STREAM_PLAYBACK ?
+			&bluetooth_hsp_playback :
+			&bluetooth_hsp_capture;
+
 	err = snd_pcm_ioplug_create(&data->io, name, stream, mode);
 	if (err < 0)
 		goto error;
diff --git a/audio/sink.c b/audio/sink.c
index 436dfbce..f7e32647 100644
--- a/audio/sink.c
+++ b/audio/sink.c
@@ -25,4 +25,433 @@
 #include <config.h>
 #endif
 
+#include <errno.h>
+
+#include <glib.h>
+#include <dbus/dbus.h>
+
+#include "dbus.h"
+#include "dbus-helper.h"
+#include "logging.h"
+
 #include "avdtp.h"
+#include "device.h"
+#include "a2dp.h"
+#include "error.h"
+#include "unix.h"
+
+struct pending_connect {
+	DBusMessage *msg;
+	struct ipc_packet *pkt;
+	int pkt_len;
+	int sock;
+};
+
+struct sink {
+	struct avdtp *session;
+	struct avdtp_stream *stream;
+	uint8_t state;
+	struct pending_connect *c;
+	DBusConnection *conn;
+};
+
+static void pending_connect_free(struct pending_connect *c)
+{
+	if (c->pkt)
+		g_free(c->pkt);
+	if (c->msg)
+		dbus_message_unref(c->msg);
+	g_free(c);
+}
+
+void stream_state_changed(struct avdtp_stream *stream, avdtp_state_t old_state,
+				avdtp_state_t new_state,
+				struct avdtp_error *err, void *user_data)
+{
+	struct device *dev = user_data;
+	struct sink *sink = dev->sink;
+	struct pending_connect *c = NULL;
+	DBusMessage *reply;
+	int cmd_err;
+
+	if (err)
+		goto failed;
+
+	switch (new_state) {
+	case AVDTP_STATE_IDLE:
+		dbus_connection_emit_signal(dev->conn, dev->path,
+						AUDIO_SINK_INTERFACE,
+						"Disconnected",
+						DBUS_TYPE_INVALID);
+		if (sink->session) {
+			avdtp_unref(sink->session);
+			sink->session = NULL;
+		}
+		c = sink->c;
+		break;
+	case AVDTP_STATE_CONFIGURED:
+		cmd_err = avdtp_open(sink->session, stream);
+		if (cmd_err < 0) {
+			error("Error on avdtp_open %s (%d)", strerror(-cmd_err),
+				cmd_err);
+			goto failed;
+		}
+		break;
+	case AVDTP_STATE_OPEN:
+		if (old_state == AVDTP_STATE_CONFIGURED)
+			dbus_connection_emit_signal(dev->conn, dev->path,
+							AUDIO_SINK_INTERFACE,
+							"Connected",
+							DBUS_TYPE_INVALID);
+		if (sink->c && sink->c->pkt) {
+			cmd_err = avdtp_start(sink->session, stream);
+			if (cmd_err < 0) {
+				error("Error on avdtp_start %s (%d)",
+					strerror(-cmd_err), cmd_err);
+				goto failed;
+			}
+		}
+		else
+			c = sink->c;
+		break;
+	case AVDTP_STATE_STREAMING:
+		c = sink->c;
+		break;
+	case AVDTP_STATE_CLOSING:
+		break;
+	case AVDTP_STATE_ABORTING:
+		break;
+	}
+
+	sink->state = new_state;
+
+	if (c) {
+		if (c->msg) {
+			reply = dbus_message_new_method_return(c->msg);
+			send_message_and_unref(dev->conn, reply);
+		}
+		if (c->pkt) {
+			struct ipc_data_cfg *rsp;
+			int ret;
+
+			ret = sink_get_config(dev, c->sock, c->pkt,
+						c->pkt_len, &rsp);
+			if (ret == 0) {
+				unix_send_cfg(c->sock, rsp);
+				g_free(rsp);
+			}
+			else
+				unix_send_cfg(c->sock, NULL);
+		}
+
+		pending_connect_free(c);
+		sink->c = NULL;
+	}
+
+	return;
+
+failed:
+	if (sink->c) {
+		if (sink->c->msg && err)
+			err_failed(dev->conn, sink->c->msg,
+					avdtp_strerror(err));
+
+		pending_connect_free(sink->c);
+		sink->c = NULL;
+	}
+
+	if (new_state == AVDTP_STATE_IDLE) {
+		avdtp_unref(sink->session);
+		sink->session = NULL;
+	}
+}
+
+
+
+static void discovery_complete(struct avdtp *session, GSList *seps, int err,
+				void *user_data)
+{
+	struct device *dev = user_data;
+	struct sink *sink = dev->sink;
+	struct avdtp_local_sep *lsep;
+	struct avdtp_remote_sep *rsep;
+	GSList *caps = NULL;
+	const char *err_str = NULL;
+
+	if (err < 0) {
+		error("Discovery failed");
+		err_str = strerror(-err);
+		goto failed;
+	}
+
+	debug("Discovery complete");
+
+	if (avdtp_get_seps(session, AVDTP_SEP_TYPE_SINK, AVDTP_MEDIA_TYPE_AUDIO,
+				A2DP_CODEC_SBC, &lsep, &rsep) < 0) {
+		err_str = "No matching ACP and INT SEPs found";
+		goto failed;
+	}
+
+	if (!a2dp_select_capabilities(rsep, &caps)) {
+		err_str = "Unable to select remote SEP capabilities";
+		goto failed;
+	}
+
+	err = avdtp_set_configuration(session, rsep, lsep, caps,
+					&sink->stream);
+	if (err < 0) {
+		error("avdtp_set_configuration: %s", strerror(-err));
+		err_str = "Unable to set configuration";
+		goto failed;
+	}
+
+	avdtp_stream_set_cb(session, sink->stream, stream_state_changed, dev);
+
+	return;
+
+failed:
+	error("%s", err_str);
+	if (sink->c) {
+		if (sink->c->msg)
+			err_failed(dev->conn, sink->c->msg, err_str);
+		pending_connect_free(sink->c);
+		sink->c = NULL;
+	}
+	if (sink->session) {
+		avdtp_unref(sink->session);
+		sink->session = NULL;
+	}
+}
+
+static DBusHandlerResult sink_connect(DBusConnection *conn,
+					DBusMessage *msg, void *data)
+{
+	struct device *dev = data;
+	struct sink *sink = dev->sink;
+	struct pending_connect *c;
+	int err;
+
+	if (!sink->session)
+		sink->session = avdtp_get(&dev->src, &dev->dst);
+
+	if (sink->c)
+		return err_connect_failed(conn, msg, "Connect in progress");
+
+	if (sink->state >= AVDTP_STATE_OPEN)
+		return err_already_connected(conn, msg);
+
+	c = g_new0(struct pending_connect, 1);
+	c->msg = dbus_message_ref(msg);
+	sink->c = c;
+
+	err = avdtp_discover(sink->session, discovery_complete, data);
+	if (err < 0) {
+		dbus_message_unref(c->msg);
+		pending_connect_free(c);
+		sink->c = NULL;
+		return err_connect_failed(conn, msg, strerror(err));
+	}
+
+	return DBUS_HANDLER_RESULT_HANDLED;
+}
+
+static DBusHandlerResult sink_disconnect(DBusConnection *conn,
+						DBusMessage *msg, void *data)
+{
+	struct device *device = data;
+	struct sink *sink = device->sink;
+	struct pending_connect *c;
+	int err;
+
+	if (!sink->session)
+		return err_not_connected(conn, msg);
+
+	if (sink->c)
+		return err_failed(conn, msg, strerror(EBUSY));
+
+	if (sink->state < AVDTP_STATE_OPEN) {
+		avdtp_unref(sink->session);
+		sink->session = NULL;
+	} else {
+		err = avdtp_close(sink->session, sink->stream);
+		if (err < 0)
+			return err_failed(conn, msg, strerror(-err));
+	}
+
+	c = g_new0(struct pending_connect, 1);
+	c->msg = dbus_message_ref(msg);
+	sink->c = c;
+
+	return DBUS_HANDLER_RESULT_HANDLED;
+}
+
+static DBusHandlerResult sink_is_connected(DBusConnection *conn,
+						DBusMessage *msg,
+						void *data)
+{
+	struct device *device = data;
+	struct sink *sink = device->sink;
+	DBusMessage *reply;
+	dbus_bool_t connected;
+
+	reply = dbus_message_new_method_return(msg);
+	if (!reply)
+		return DBUS_HANDLER_RESULT_NEED_MEMORY;
+
+	connected = (sink->state != AVDTP_STATE_IDLE);
+
+	dbus_message_append_args(reply, DBUS_TYPE_BOOLEAN, &connected,
+					DBUS_TYPE_INVALID);
+
+	send_message_and_unref(conn, reply);
+
+	return DBUS_HANDLER_RESULT_HANDLED;
+}
+
+static DBusMethodVTable sink_methods[] = {
+	{ "Connect",		sink_connect,		"",	""	},
+	{ "Disconnect",		sink_disconnect,	"",	""	},
+	{ "IsConnected",	sink_is_connected,	"",	"b"	},
+	{ NULL, NULL, NULL, NULL }
+};
+
+static DBusSignalVTable sink_signals[] = {
+	{ "Connected",			""	},
+	{ "Disconnected",		""	},
+	{ NULL, NULL }
+};
+
+struct sink *sink_init(void *device)
+{
+	struct device *dev = device;
+
+	if (!dbus_connection_register_interface(dev->conn, dev->path,
+						AUDIO_SINK_INTERFACE,
+						sink_methods,
+						sink_signals, NULL))
+		return NULL;
+
+	return g_new0(struct sink, 1);
+}
+
+void sink_free(void *device)
+{
+	struct device *dev = device;
+	struct sink *sink = dev->sink;
+
+	if (sink->session)
+		avdtp_unref(sink->session);
+
+	if (sink->c)
+		pending_connect_free(sink->c);
+
+	g_free(sink);
+	dev->sink = NULL;
+}
+
+int sink_get_config(void *device, int sock, struct ipc_packet *req,
+			int pkt_len, struct ipc_data_cfg **rsp)
+{
+	struct device *dev = device;
+	struct sink *sink = dev->sink;
+	int err = EINVAL;
+	struct pending_connect *c = NULL;
+
+	if (sink->state == AVDTP_STATE_STREAMING)
+		goto proceed;
+
+	if (!sink->session)
+		sink->session = avdtp_get(&dev->src, &dev->dst);
+
+	c = g_new0(struct pending_connect, 1);
+	c->sock = sock;
+	c->pkt = g_malloc(pkt_len);
+	memcpy(c->pkt, req, pkt_len);
+	sink->c = c;
+
+	if (sink->state == AVDTP_STATE_IDLE)
+		err = avdtp_discover(sink->session, discovery_complete, device);
+	else if (sink->state < AVDTP_STATE_STREAMING)
+		err = avdtp_start(sink->session, sink->stream);
+	else
+		goto error;
+
+	if (err < 0)
+		goto error;
+
+	return 1;
+
+proceed:
+	if (!a2dp_get_config(sink->stream, rsp))
+		goto error;
+
+	return 0;
+
+error:
+	if (c)
+		pending_connect_free(c);
+	return -err;
+}
+
+gboolean sink_is_active(void *device)
+{
+	struct device *dev = device;
+	struct sink *sink = dev->sink;
+
+	if (sink->session)
+		return TRUE;
+
+	return FALSE;
+}
+
+void sink_set_state(void *device, avdtp_state_t state)
+{
+	struct device *dev = device;
+	struct sink *sink = dev->sink;
+	int err = 0;
+
+	if (sink->state == state)
+		return;
+
+	if (!sink->session || !sink->stream)
+		goto failed;
+
+	switch (sink->state) {
+	case AVDTP_STATE_OPEN:
+		if (state == AVDTP_STATE_STREAMING) {
+			err = avdtp_start(sink->session, sink->stream);
+			if (err == 0)
+				return;
+		}
+		else if (state == AVDTP_STATE_IDLE) {
+			err = avdtp_close(sink->session, sink->stream);
+			if (err == 0)
+				return;
+		}
+		break;
+	case AVDTP_STATE_STREAMING:
+		if (state == AVDTP_STATE_OPEN) {
+			err = avdtp_suspend(sink->session, sink->stream);
+			if (err == 0)
+				return;
+		}
+		else if (state == AVDTP_STATE_IDLE) {
+			err = avdtp_close(sink->session, sink->stream);
+			if (err == 0)
+				return;
+		}
+		break;
+	default:
+		goto failed;
+	}
+failed:
+	error("%s: Error changing states", dev->path);
+}
+
+avdtp_state_t sink_get_state(void *device)
+{
+	struct device *dev = device;
+	struct sink *sink = dev->sink;
+
+	return sink->state;
+}
diff --git a/audio/sink.h b/audio/sink.h
index 4cc3a0e4..9d65e278 100644
--- a/audio/sink.h
+++ b/audio/sink.h
@@ -21,7 +21,18 @@
  *
  */
 
+#include "ipc.h"
+#include "avdtp.h"
+
 #define AUDIO_SINK_INTERFACE "org.bluez.audio.Sink"
 
 struct sink;
 
+struct sink *sink_init(void *device);
+void sink_new_stream(void *device, void *lsep);
+void sink_free(void *device);
+int sink_get_config(void *device, int sock, struct ipc_packet *req,
+			int pkt_len, struct ipc_data_cfg **rsp);
+gboolean sink_is_active(void *device);
+void sink_set_state(void *device, avdtp_state_t state);
+avdtp_state_t sink_get_state(void *device);
diff --git a/audio/unix.c b/audio/unix.c
index fbda7ed9..0880f4ff 100644
--- a/audio/unix.c
+++ b/audio/unix.c
@@ -37,11 +37,28 @@
 
 #include "logging.h"
 #include "dbus.h"
-
 #include "manager.h"
+#include "ipc.h"
+#include "unix.h"
+
+struct unix_client {
+	struct device *dev;
+	int sock;
+};
+
+static GSList *clients = NULL;
 
 static int unix_sock = -1;
 
+static int unix_send_state(int sock, struct ipc_packet *pkt);
+
+static void client_free(struct unix_client *client)
+{
+	if (client->sock >= 0)
+		close(client->sock);
+	g_free(client);
+}
+
 /* Pass file descriptor through local domain sockets (AF_LOCAL, formerly AF_UNIX)
 and the sendmsg() system call with the cmsg_type field of a "struct cmsghdr" set
 to SCM_RIGHTS and the data being an integer value equal to the handle of the 
@@ -75,52 +92,120 @@ static int unix_sendmsg_fd(int sock, int fd, struct ipc_packet *pkt)
 	return sendmsg(sock, &msgh, MSG_NOSIGNAL);
 }
 
-static void cfg_event(int clisk, struct ipc_packet *pkt)
+static void cfg_event(struct unix_client *client, struct ipc_packet *pkt,
+			int len)
 {
-	struct ipc_data_cfg *cfg = (struct ipc_data_cfg *) pkt->data;
-	struct device *device;
+	struct ipc_data_cfg *rsp;
+	struct device *dev;
+	int ret;
 
-	memset(cfg, 0, sizeof(struct ipc_data_cfg));
+	dev = manager_get_connected_device();
+	if (dev)
+		goto proceed;
 
-	if ((device = manager_default_device())) {
-		if (device->headset)
-			headset_get_config(device, clisk, pkt);
-	}
-	else
-		cfg->fd = -1;
+	dev = manager_default_device();
+	if (!dev)
+		goto failed;
 
-	if (cfg->fd != 0)
-		unix_send_cfg(clisk, pkt);
+proceed:
+	ret = device_get_config(dev, client->sock, pkt, len, &rsp);
+	if (ret < 0)
+		goto failed;
+
+	client->dev = dev;
+
+	/* Connecting in progress */
+	if (ret == 1)
+		return;
+
+	unix_send_cfg(client->sock, rsp);
+	g_free(rsp);
+
+	return;
+
+failed:
+	unix_send_cfg(client->sock, NULL);
 }
 
-static void ctl_event(int clisk, struct ipc_packet *pkt)
+static void ctl_event(struct unix_client *client, struct ipc_packet *pkt,
+			int len)
 {
 }
 
-static void state_event(int clisk, struct ipc_packet *pkt)
+static void state_event(struct unix_client *client, struct ipc_packet *pkt,
+				int len)
 {
 	struct ipc_data_state *state = (struct ipc_data_state *) pkt->data;
-	struct device *device;
+	struct device *dev = client->dev;
 
-	if (!(device = manager_default_device()))
-		return;
+	if (len > sizeof(struct ipc_packet))
+		device_set_state(dev, state->state);
+	else
+		state->state = device_get_state(dev);
 
-	if (device->headset)
-		headset_set_state(device, state->state);
+	unix_send_state(client->sock, pkt);
+}
+
+static gboolean client_cb(GIOChannel *chan, GIOCondition cond, gpointer data)
+{
+	char buf[IPC_MTU];
+	struct ipc_packet *pkt = (void *) buf;
+	struct unix_client *client = data;
+	int len, len_check;
+
+	if (cond & G_IO_NVAL)
+		return FALSE;
+
+	if (cond & (G_IO_HUP | G_IO_ERR)) {
+		debug("Unix client disconnected");
+		device_set_state(client->dev, STATE_CONNECTED);
+		goto failed;
+	}
+
+	memset(buf, 0, sizeof(buf));
+
+	len = recv(client->sock, buf, sizeof(buf), 0);
+	if (len < 0) {
+		error("recv: %s (%d)", strerror(errno), errno);
+		goto failed;
+	}
+
+	len_check = pkt->length + sizeof(struct ipc_packet);
+	if (len != len_check) {
+		error("Packet lenght doesn't match");
+		goto failed;
+	}
+
+	switch (pkt->type) {
+	case PKT_TYPE_CFG_REQ:
+		info("Package PKT_TYPE_CFG_REQ:%u", pkt->role);
+		cfg_event(client, pkt, len);
+		break;
+	case PKT_TYPE_STATE_REQ:
+		info("Package PKT_TYPE_STATE_REQ");
+		state_event(client, pkt, len);
+		break;
+	case PKT_TYPE_CTL_REQ:
+		info("Package PKT_TYPE_CTL_REQ");
+		ctl_event(client, pkt, len);
+		break;
+	}
 
-	unix_send_status(clisk, pkt);
+	return TRUE;
 
-	g_free(pkt);
+failed:
+	clients = g_slist_remove(clients, client);
+	client_free(client);
+	return FALSE;
 }
 
-static gboolean unix_event(GIOChannel *chan, GIOCondition cond, gpointer data)
+static gboolean server_cb(GIOChannel *chan, GIOCondition cond, gpointer data)
 {
 	struct sockaddr_un addr;
 	socklen_t addrlen;
-	struct ipc_packet *pkt;
-	int sk, clisk, len;
-
-	debug("chan %p cond %td data %p", chan, cond, data);
+	int sk, cli_sk;
+	struct unix_client *client;
+	GIOChannel *io;
 
 	if (cond & G_IO_NVAL)
 		return FALSE;
@@ -135,32 +220,22 @@ static gboolean unix_event(GIOChannel *chan, GIOCondition cond, gpointer data)
 	memset(&addr, 0, sizeof(addr));
 	addrlen = sizeof(addr);
 
-	clisk = accept(sk, (struct sockaddr *) &addr, &addrlen);
-	if (clisk < 0) {
+	cli_sk = accept(sk, (struct sockaddr *) &addr, &addrlen);
+	if (cli_sk < 0) {
 		error("accept: %s (%d)", strerror(errno), errno);
 		return TRUE;
 	}
 
-	len = sizeof(struct ipc_packet) + sizeof(struct ipc_data_cfg);
-	pkt = g_malloc0(len);
-	len = recv(clisk, pkt, len, 0);
+	debug("Accepted new client connection on unix socket");
 
-	debug("path %s len %d", addr.sun_path + 1, len);
+	client = g_new(struct unix_client, 1);
+	client->sock = cli_sk;
+	clients = g_slist_append(clients, client);
 
-	switch (pkt->type) {
-	case PKT_TYPE_CFG_REQ:
-		info("Package PKT_TYPE_CFG_REQ:%u", pkt->role);
-		cfg_event(clisk, pkt);
-		break;
-	case PKT_TYPE_STATE_REQ:
-		info("Package PKT_TYPE_STATE_REQ");
-		state_event(clisk, pkt);
-		break;
-	case PKT_TYPE_CTL_REQ:
-		info("Package PKT_TYPE_CTL_REQ");
-		ctl_event(clisk, pkt);
-		break;
-	}
+	io = g_io_channel_unix_new(cli_sk);
+	g_io_add_watch(io, G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
+			client_cb, client);
+	g_io_channel_unref(io);
 
 	return TRUE;
 }
@@ -194,10 +269,8 @@ int unix_init(void)
 	listen(sk, 1);
 
 	io = g_io_channel_unix_new(sk);
-
 	g_io_add_watch(io, G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
-							unix_event, NULL);
-
+			server_cb, NULL);
 	g_io_channel_unref(io);
 
 	info("Unix socket created: %d", sk);
@@ -207,59 +280,77 @@ int unix_init(void)
 
 void unix_exit(void)
 {
+	g_slist_foreach(clients, (GFunc) client_free, NULL);
+	g_slist_free(clients);
 	close(unix_sock);
 	unix_sock = -1;
 }
 
-int unix_send_cfg(int sock, struct ipc_packet *pkt)
+int unix_send_cfg(int sock, struct ipc_data_cfg *cfg)
 {
-	struct ipc_data_cfg *cfg = (struct ipc_data_cfg *) pkt->data;
-	int len;
+	char buf[IPC_MTU];
+	struct ipc_packet *pkt = (void *) buf;
+	int len, codec_len;
 
-	info("fd=%d, fd_opt=%u, channels=%u, pkt_len=%u, sample_size=%u,"
-		"rate=%u", cfg->fd, cfg->fd_opt, cfg->channels,
-		cfg->pkt_len, cfg->sample_size, cfg->rate);
+	memset(buf, 0, sizeof(buf));
 
 	pkt->type = PKT_TYPE_CFG_RSP;
-	pkt->length = sizeof(struct ipc_data_cfg);
+
+	if (!cfg) {
+		pkt->error = EINVAL;
+		len = send(sock, pkt, sizeof(struct ipc_packet), 0);
+		if (len < 0)
+			error("send: %s (%d)", strerror(errno), errno);
+		return len;
+	}
+
+	debug("fd=%d, fd_opt=%u, channels=%u, pkt_len=%u,"
+		"sample_size=%u, rate=%u", cfg->fd, cfg->fd_opt,
+		cfg->channels, cfg->pkt_len, cfg->sample_size, cfg->rate);
+
+	if (cfg->codec == CFG_CODEC_SBC)
+		codec_len = sizeof(struct ipc_codec_sbc);
+	else
+		codec_len = 0;
+
 	pkt->error = PKT_ERROR_NONE;
+	pkt->length = sizeof(struct ipc_data_cfg) + codec_len;
+	memcpy(pkt->data, cfg, pkt->length);
 
-	len = sizeof(struct ipc_packet) + sizeof(struct ipc_data_cfg);
+	len = sizeof(struct ipc_packet) + pkt->length;
 	len = send(sock, pkt, len, 0);
 	if (len < 0)
-		info("Error %s(%d)", strerror(errno), errno);
+		error("Error %s(%d)", strerror(errno), errno);
 
-	info("%d bytes sent", len);
+	debug("%d bytes sent", len);
 
 	if (cfg->fd != -1) {
 		len = unix_sendmsg_fd(sock, cfg->fd, pkt);
 		if (len < 0)
-			info("Error %s(%d)", strerror(errno), errno);
-		info("%d bytes sent", len);
+			error("Error %s(%d)", strerror(errno), errno);
+		debug("%d bytes sent", len);
 	}
 
-	g_free(pkt);
 	return 0;
 }
 
-int unix_send_status(int sock, struct ipc_packet *pkt)
+static int unix_send_state(int sock, struct ipc_packet *pkt)
 {
 	struct ipc_data_state *state = (struct ipc_data_state *) pkt->data;
 	int len;
 
 	info("status=%u", state->state);
 
-	pkt->type = PKT_TYPE_CFG_RSP;
+	pkt->type = PKT_TYPE_STATE_RSP;
 	pkt->length = sizeof(struct ipc_data_state);
 	pkt->error = PKT_ERROR_NONE;
 
 	len = sizeof(struct ipc_packet) + sizeof(struct ipc_data_state);
 	len = send(sock, pkt, len, 0);
 	if (len < 0)
-		info("Error %s(%d)", strerror(errno), errno);
+		error("Error %s(%d)", strerror(errno), errno);
 
-	info("%d bytes sent", len);
+	debug("%d bytes sent", len);
 
-	g_free(pkt);
 	return 0;
 }
diff --git a/audio/unix.h b/audio/unix.h
index 32cf4af9..c771b965 100644
--- a/audio/unix.h
+++ b/audio/unix.h
@@ -24,7 +24,6 @@
 #include "ipc.h"
 
 int unix_init(void);
-
 void unix_exit(void);
-int unix_send_cfg(int sock, struct ipc_packet *pkt);
-int unix_send_status(int sock, struct ipc_packet *pkt);
+
+int unix_send_cfg(int sock, struct ipc_data_cfg *cfg);