Make codec parameters support available for application.

7 files changed, 545 insertions, 212 deletions

diff --git a/audio/a2dp.c b/audio/a2dp.c
index 5e08b25b..5ecd8159 100644
--- a/audio/a2dp.c
+++ b/audio/a2dp.c
@@ -76,6 +76,7 @@ struct a2dp_stream_setup {
 	struct avdtp *session;
 	struct device *dev;
 	struct avdtp_stream *stream;
+	struct avdtp_service_capability *media_codec;
 	gboolean start;
 	gboolean canceled;
 	GSList *cb;
@@ -138,6 +139,188 @@ static void stream_state_changed(struct avdtp_stream *stream,
 	sep->stream = NULL;
 }
 
+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 = MAX(2, 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;
+}
+
+static 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;
+
+	if (!setup)
+		return FALSE;
+
+	if (setup->media_codec)
+		memcpy(&sbc_cap, setup->media_codec->data, sizeof(sbc_cap));
+	else {
+		media_codec = avdtp_get_codec(rsep);
+		if (!media_codec)
+			return FALSE;
+
+		select_sbc_params(&sbc_cap,
+				(struct sbc_codec_cap *) media_codec->data);
+	}
+
+	media_transport = avdtp_service_cap_new(AVDTP_MEDIA_TRANSPORT,
+						NULL, 0);
+
+	*caps = g_slist_append(*caps, media_transport);
+
+	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 discovery_complete(struct avdtp *session, GSList *seps, int err,
+				void *user_data)
+{
+	struct avdtp_local_sep *lsep;
+	struct avdtp_remote_sep *rsep;
+	GSList *caps = NULL;
+
+	if (err < 0) {
+		setup->stream = NULL;
+		finalize_stream_setup(setup);
+		return;
+	}
+
+	debug("Discovery complete");
+
+	if (avdtp_get_seps(session, AVDTP_SEP_TYPE_SINK, AVDTP_MEDIA_TYPE_AUDIO,
+				A2DP_CODEC_SBC, &lsep, &rsep) < 0) {
+		error("No matching ACP and INT SEPs found");
+		finalize_stream_setup(setup);
+		return;
+	}
+
+	if (!a2dp_select_capabilities(rsep, &caps)) {
+		error("Unable to select remote SEP capabilities");
+		finalize_stream_setup(setup);
+		return;
+	}
+
+	err = avdtp_set_configuration(session, rsep, lsep, caps,
+					&setup->stream);
+	if (err < 0) {
+		error("avdtp_set_configuration: %s", strerror(-err));
+		finalize_stream_setup(setup);
+		return;
+	}
+
+	/* Notify sink.c of the new stream */
+	sink_new_stream(setup->dev, session, setup->stream);
+}
+
 static gboolean setconf_ind(struct avdtp *session,
 				struct avdtp_local_sep *sep,
 				struct avdtp_stream *stream,
@@ -328,7 +511,7 @@ static void open_cfm(struct avdtp *session, struct avdtp_local_sep *sep,
 
 		error("avdtp_start failed");
 		setup->stream = NULL;
-	} 
+	}
 
 finalize:
 	finalize_stream_setup(setup);
@@ -456,6 +639,30 @@ static void close_cfm(struct avdtp *session, struct avdtp_local_sep *sep,
 		debug("SBC Sink: Close_Cfm");
 	else
 		debug("SBC Source: Close_Cfm");
+
+	if (!setup)
+		return;
+
+	if (setup->canceled) {
+		stream_setup_free(setup);
+		return;
+	}
+
+	if (err) {
+		setup->stream = NULL;
+		goto finalize;
+	}
+
+	if (setup->start) {
+		if (avdtp_discover(session, discovery_complete, setup) == 0)
+			return;
+
+		error("avdtp_discover failed");
+		setup->stream = NULL;
+	}
+
+finalize:
+	finalize_stream_setup(setup);
 }
 
 static gboolean abort_ind(struct avdtp *session, struct avdtp_local_sep *sep,
@@ -506,6 +713,32 @@ static void reconf_cfm(struct avdtp *session, struct avdtp_local_sep *sep,
 		debug("SBC Sink: ReConfigure_Cfm");
 	else
 		debug("SBC Source: ReConfigure_Cfm");
+
+	if (!setup)
+		return;
+
+	if (setup->canceled) {
+		if (!err)
+			avdtp_close(session, stream);
+		stream_setup_free(setup);
+		return;
+	}
+
+	if (err) {
+		setup->stream = NULL;
+		goto finalize;
+	}
+
+	if (setup->start) {
+		if (avdtp_start(session, stream) == 0)
+			return;
+
+		error("avdtp_start failed");
+		setup->stream = NULL;
+	}
+
+finalize:
+	finalize_stream_setup(setup);
 }
 
 static struct avdtp_sep_cfm cfm = {
@@ -705,181 +938,6 @@ void a2dp_exit()
 	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 = MAX(2, 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;
-}
-
-static 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;
-}
-
-static void discovery_complete(struct avdtp *session, GSList *seps, int err,
-				void *user_data)
-{
-	struct avdtp_local_sep *lsep;
-	struct avdtp_remote_sep *rsep;
-	GSList *caps = NULL;
-
-	if (err < 0) {
-		error("Discovery failed: %s (%d)", strerror(-err), -err);
-		setup->stream = NULL;
-		finalize_stream_setup(setup);
-		return;
-	}
-
-	debug("Discovery complete");
-
-	if (avdtp_get_seps(session, AVDTP_SEP_TYPE_SINK, AVDTP_MEDIA_TYPE_AUDIO,
-				A2DP_CODEC_SBC, &lsep, &rsep) < 0) {
-		error("No matching ACP and INT SEPs found");
-		finalize_stream_setup(setup);
-		return;
-	}
-
-	if (!a2dp_select_capabilities(rsep, &caps)) {
-		error("Unable to select remote SEP capabilities");
-		finalize_stream_setup(setup);
-		return;
-	}                       
-
-	err = avdtp_set_configuration(session, rsep, lsep, caps,
-					&setup->stream);
-	if (err < 0) {
-		error("avdtp_set_configuration: %s", strerror(-err));
-		finalize_stream_setup(setup);
-		return;
-	}
-
-	/* Notify sink.c of the new stream */
-	sink_new_stream(setup->dev, session, setup->stream);
-}
-
 gboolean a2dp_source_cancel_stream(struct device *dev, unsigned int id)
 {
 	struct a2dp_stream_cb *cb_data;
@@ -914,7 +972,8 @@ unsigned int a2dp_source_request_stream(struct avdtp *session,
 						gboolean start,
 						a2dp_stream_cb_t cb,
 						void *user_data,
-						struct a2dp_sep **ret)
+						struct a2dp_sep **ret,
+						struct avdtp_service_capability *media_codec)
 {
 	struct a2dp_stream_cb *cb_data;
 	static unsigned int cb_id = 0;
@@ -967,6 +1026,7 @@ unsigned int a2dp_source_request_stream(struct avdtp *session,
 	setup->cb = g_slist_append(setup->cb, cb_data);
 	setup->start = start;
 	setup->stream = sep->stream;
+	setup->media_codec = media_codec;
 
 	switch (avdtp_sep_get_state(sep->sep)) {
 	case AVDTP_STATE_IDLE:
@@ -982,7 +1042,21 @@ unsigned int a2dp_source_request_stream(struct avdtp *session,
 		}
 		if (sep->starting)
 			break;
-		if (avdtp_start(session, sep->stream) < 0) {
+		if (setup->media_codec) {
+			if (avdtp_stream_has_capability(setup->stream,
+				setup->media_codec)) {
+				if (avdtp_start(session, sep->stream) < 0) {
+					error("avdtp_start failed");
+					goto failed;
+				}
+			} else {
+				if (avdtp_close(session, sep->stream) < 0) {
+					error("avdtp_close failed");
+					goto failed;
+				}
+			}
+		}
+		else if (avdtp_start(session, sep->stream) < 0) {
 			error("avdtp_start failed");
 			goto failed;
 		}
@@ -1006,7 +1080,7 @@ unsigned int a2dp_source_request_stream(struct avdtp *session,
 		error("SEP in bad state for requesting a new stream");
 		goto failed;
 	}
-	
+
 	if (ret)
 		*ret = sep;
 
diff --git a/audio/a2dp.h b/audio/a2dp.h
index 7358473e..ede0c70b 100644
--- a/audio/a2dp.h
+++ b/audio/a2dp.h
@@ -71,11 +71,11 @@ unsigned int a2dp_source_request_stream(struct avdtp *session,
 					struct device *dev,
 					gboolean start, a2dp_stream_cb_t cb,
 					void *user_data,
-					struct a2dp_sep **sep);
+					struct a2dp_sep **sep,
+					struct avdtp_service_capability *media_codec);
 gboolean a2dp_source_cancel_stream(struct device *dev, unsigned int id);
 
 gboolean a2dp_source_lock(struct device *dev, struct avdtp *session);
 gboolean a2dp_source_unlock(struct device *dev, struct avdtp *session);
 gboolean a2dp_source_suspend(struct device *dev, struct avdtp *session);
 gboolean a2dp_source_start_stream(struct device *dev, struct avdtp *session);
-
diff --git a/audio/avdtp.c b/audio/avdtp.c
index a967be76..d03afbae 100644
--- a/audio/avdtp.c
+++ b/audio/avdtp.c
@@ -552,9 +552,6 @@ static void stream_free(struct avdtp_stream *stream)
 	if (stream->timer)
 		g_source_remove(stream->timer);
 
-	if (stream->io)
-		g_source_remove(stream->io);
-
 	g_slist_foreach(stream->callbacks, (GFunc) g_free, NULL);
 	g_slist_free(stream->callbacks);
 
@@ -718,7 +715,7 @@ void avdtp_unref(struct avdtp *session)
 			session->sock = -1;
 		}
 
-	       	if (session->sock >= 0)
+		if (session->sock >= 0)
 			set_disconnect_timer(session);
 		else if (!session->free_lock) /* Drop the local ref if we
 						 aren't connected */
@@ -954,7 +951,7 @@ static gboolean avdtp_setconf_cmd(struct avdtp *session,
 		err = AVDTP_SEP_IN_USE;
 		goto failed;
 	}
-	
+
 	stream = g_new0(struct avdtp_stream, 1);
 	stream->session = session;
 	stream->lsep = sep;
@@ -1074,7 +1071,7 @@ static gboolean avdtp_start_cmd(struct avdtp *session, struct start_req *req,
 		error("Too short start request");
 		return FALSE;
 	}
-	
+
 	seid_count = 1 + size - sizeof(struct start_req);
 
 	seid = &req->first_seid;
@@ -1936,8 +1933,8 @@ static gboolean avdtp_abort_resp(struct avdtp *session,
 {
 	struct avdtp_local_sep *sep = stream->lsep;
 
-	if (sep->cfm && sep->cfm->suspend)
-		sep->cfm->suspend(session, sep, stream, NULL, sep->user_data);
+	if (sep->cfm && sep->cfm->abort)
+		sep->cfm->abort(session, sep, stream, NULL, sep->user_data);
 
 	avdtp_sep_set_state(session, sep, AVDTP_STATE_IDLE);
 
@@ -2196,7 +2193,7 @@ struct avdtp *avdtp_get(bdaddr_t *src, bdaddr_t *dst)
 gboolean avdtp_is_connected(bdaddr_t *src, bdaddr_t *dst)
 {
 	struct avdtp *session;
-	
+
 	session = find_session(src, dst);
 
 	if (!session)
@@ -2208,6 +2205,24 @@ gboolean avdtp_is_connected(bdaddr_t *src, bdaddr_t *dst)
 	return FALSE;
 }
 
+gboolean avdtp_stream_has_capability(struct avdtp_stream *stream,
+				struct avdtp_service_capability *cap)
+{
+	GSList *l;
+	struct avdtp_service_capability *stream_cap;
+
+	for (l = stream->caps; l; l = g_slist_next(l)) {
+		stream_cap = l->data;
+		if (stream_cap->category == cap->category &&
+			stream_cap->length == cap->length) {
+			if (!memcmp(stream_cap->data, cap->data, cap->length))
+				return TRUE;
+		}
+	}
+
+	return FALSE;
+}
+
 gboolean avdtp_stream_get_transport(struct avdtp_stream *stream, int *sock,
 					uint16_t *mtu, GSList **caps)
 {
@@ -2219,7 +2234,7 @@ gboolean avdtp_stream_get_transport(struct avdtp_stream *stream, int *sock,
 
 	if (mtu)
 		*mtu = stream->mtu;
-	
+
 	if (caps)
 		*caps = stream->caps;
 
@@ -2452,9 +2467,14 @@ int avdtp_set_configuration(struct avdtp *session,
 	return ret;
 }
 
-int avdtp_reconfigure(struct avdtp *session, struct avdtp_stream *stream)
+int avdtp_reconfigure(struct avdtp *session, GSList *caps,
+			struct avdtp_stream *stream)
 {
-	struct seid_req req;
+	struct reconf_req *req;
+	unsigned char *ptr;
+	int caps_len;
+	GSList *l;
+	struct avdtp_service_capability *cap;
 
 	if (!g_slist_find(session->streams, stream))
 		return -EINVAL;
@@ -2462,11 +2482,26 @@ int avdtp_reconfigure(struct avdtp *session, struct avdtp_stream *stream)
 	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->rseid;
+	/* 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 reconf_req) + caps_len);
+
+	init_request(&req->header, AVDTP_RECONFIGURE);
+	req->acp_seid = stream->rseid;
+
+	/* 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;
+	}
 
-	return send_request(session, FALSE, NULL, &req, sizeof(req));
+	return send_request(session, FALSE, stream, req, sizeof(*req)
+				+ caps_len);
 }
 
 int avdtp_open(struct avdtp *session, struct avdtp_stream *stream)
diff --git a/audio/avdtp.h b/audio/avdtp.h
index d759be48..d429b435 100644
--- a/audio/avdtp.h
+++ b/audio/avdtp.h
@@ -189,6 +189,9 @@ gboolean avdtp_stream_remove_cb(struct avdtp *session,
 gboolean avdtp_stream_get_transport(struct avdtp_stream *stream, int *sock,
 					uint16_t *mtu, GSList **caps);
 
+gboolean avdtp_stream_has_capability(struct avdtp_stream *stream,
+				struct avdtp_service_capability *cap);
+
 int avdtp_set_configuration(struct avdtp *session,
 				struct avdtp_remote_sep *rsep,
 				struct avdtp_local_sep *lsep,
@@ -199,7 +202,8 @@ 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_reconfigure(struct avdtp *session, GSList *caps,
+			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);
diff --git a/audio/pcm_bluetooth.c b/audio/pcm_bluetooth.c
index d031b318..c6dc0931 100644
--- a/audio/pcm_bluetooth.c
+++ b/audio/pcm_bluetooth.c
@@ -893,19 +893,13 @@ static int bluetooth_a2dp_init(struct bluetooth_data *data,
 	return 0;
 }
 
-static int bluetooth_cfg(struct bluetooth_data *data, snd_config_t *conf)
+static int bluetooth_cfg_init(struct ipc_packet *pkt, 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;
 	snd_config_iterator_t i, next;
-	const char *addr, *pref;
-
-	DBG("Sending PKT_TYPE_CFG_REQ...");
-
-	memset(buf, 0, sizeof(buf));
+	const char *addr, *pref, *mode, *allocation, *rate, *channels,
+		*subbands, *blocks, *bitpool;
 
 	snd_config_for_each(i, next, conf) {
 		snd_config_t *n = snd_config_iterator_entry(i);
@@ -944,14 +938,114 @@ static int bluetooth_cfg(struct bluetooth_data *data, snd_config_t *conf)
 			continue;
 		}
 
+		if (strcmp(id, "rate") == 0) {
+			if (snd_config_get_string(n, &rate) < 0) {
+				SNDERR("Invalid type for %s", id);
+				return -EINVAL;
+			}
+
+			cfg->rate = strtod(rate, NULL);
+			continue;
+		}
+
+		if (strcmp(id, "channels") == 0) {
+			if (snd_config_get_string(n, &channels) < 0) {
+				SNDERR("Invalid type for %s", id);
+				return -EINVAL;
+			}
+
+			cfg->channels = strtod(channels, NULL);
+			continue;
+		}
+
+		if (strcmp(id, "channel_mode") == 0) {
+			if (snd_config_get_string(n, &mode) < 0) {
+				SNDERR("Invalid type for %s", id);
+				return -EINVAL;
+			}
+
+			if (strcmp(pref, "mono") == 0)
+				cfg->channel_mode = CFG_CHANNEL_MODE_MONO;
+			else if (strcmp(pref, "dual") == 0)
+				cfg->channel_mode = CFG_CHANNEL_MODE_DUAL_CHANNEL;
+			else if (strcmp(pref, "stereo") == 0)
+				cfg->channel_mode = CFG_CHANNEL_MODE_STEREO;
+			else if (strcmp(pref, "joint") == 0)
+				cfg->channel_mode = CFG_CHANNEL_MODE_JOINT_STEREO;
+			continue;
+		}
+
+		if (strcmp(id, "allocation") == 0) {
+			if (snd_config_get_string(n, &allocation) < 0) {
+				SNDERR("Invalid type for %s", id);
+				return -EINVAL;
+			}
+
+			if (strcmp(pref, "snr") == 0)
+				sbc->allocation = CODEC_SBC_ALLOCATION_SNR;
+			else if (strcmp(pref, "loudness") == 0)
+				sbc->allocation = CODEC_SBC_ALLOCATION_LOUDNESS;
+			continue;
+		}
+
+		if (strcmp(id, "subbands") == 0) {
+			if (snd_config_get_string(n, &subbands) < 0) {
+				SNDERR("Invalid type for %s", id);
+				return -EINVAL;
+			}
+
+			sbc->subbands = strtod(subbands, NULL);
+			continue;
+		}
+
+		if (strcmp(id, "blocks") == 0) {
+			if (snd_config_get_string(n, &blocks) < 0) {
+				SNDERR("Invalid type for %s", id);
+				return -EINVAL;
+			}
+
+			sbc->blocks = strtod(blocks, NULL);
+			continue;
+		}
+
+		if (strcmp(id, "bitpool") == 0) {
+			if (snd_config_get_string(n, &bitpool) < 0) {
+				SNDERR("Invalid type for %s", id);
+				return -EINVAL;
+			}
+
+			sbc->bitpool = strtod(bitpool, NULL);
+			continue;
+		}
+
 		SNDERR("Unknown field %s", id);
 		return -EINVAL;
 	}
 
+	pkt->length = sizeof(*cfg) + sizeof(*sbc);
 	pkt->type = PKT_TYPE_CFG_REQ;
 	pkt->error = PKT_ERROR_NONE;
 
-	ret = send(data->sock, pkt, sizeof(struct ipc_packet), 0);
+	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));
+
+	ret = bluetooth_cfg_init(pkt, conf);
+	if (ret < 0)
+		return -ret;
+
+	ret = send(data->sock, pkt, sizeof(*pkt) + pkt->length, 0);
 	if (ret < 0)
 		return -errno;
 	else if (ret == 0)
@@ -1026,7 +1120,7 @@ done:
 	while (recv(data->stream_fd, data->buffer, data->cfg.pkt_len,
 				MSG_DONTWAIT) > 0);
 
-	memset(data->buffer, 0, data->cfg.pkt_len);
+	memset(data->buffer, 0, sizeof(data->buffer));
 
 	return 0;
 }
diff --git a/audio/sink.c b/audio/sink.c
index 5b699fb9..599fdaaf 100644
--- a/audio/sink.c
+++ b/audio/sink.c
@@ -188,7 +188,8 @@ static DBusHandlerResult sink_connect(DBusConnection *conn,
 	sink->connect = pending;
 
 	id = a2dp_source_request_stream(sink->session, dev, FALSE,
-					stream_setup_complete, pending, NULL);
+					stream_setup_complete, pending, NULL,
+					NULL);
 	if (id == 0) {
 		pending_request_free(pending);
 		sink->connect = NULL;
diff --git a/audio/unix.c b/audio/unix.c
index 6c420a24..ef8d6673 100644
--- a/audio/unix.c
+++ b/audio/unix.c
@@ -67,6 +67,7 @@ struct a2dp_data {
 struct unix_client {
 	struct device *dev;
 	struct avdtp_local_sep *sep;
+	struct avdtp_service_capability *media_codec;
 	service_type_t type;
 	char *interface;
 	union {
@@ -106,6 +107,8 @@ static void client_free(struct unix_client *client)
 
 	if (client->sock >= 0)
 		close(client->sock);
+	if (client->media_codec)
+		g_free(client->media_codec);
 	g_free(client->interface);
 	g_free(client);
 }
@@ -236,7 +239,6 @@ static int unix_send_cfg(int sock, struct ipc_data_cfg *cfg, int fd)
 	return 0;
 }
 
-
 static void headset_setup_complete(struct device *dev, void *user_data)
 {
 	struct unix_client *client = user_data;
@@ -405,7 +407,8 @@ static void create_stream(struct device *dev, struct unix_client *client)
 
 		id = a2dp_source_request_stream(a2dp->session, dev,
 						TRUE, a2dp_setup_complete,
-						client, &a2dp->sep);
+						client, &a2dp->sep,
+						client->media_codec);
 		client->cancel_stream = a2dp_source_cancel_stream;
 		break;
 	case TYPE_HEADSET:
@@ -442,11 +445,126 @@ static void create_cb(struct device *dev, void *user_data)
 		create_stream(dev, client);
 }
 
+static int cfg_to_caps(struct ipc_data_cfg *cfg, struct sbc_codec_cap *sbc_cap)
+{
+	struct ipc_codec_sbc *sbc = (void *) cfg->data;
+
+	sbc_cap->cap.media_type = AVDTP_MEDIA_TYPE_AUDIO;
+	sbc_cap->cap.media_codec_type = A2DP_CODEC_SBC;
+
+	if (cfg->rate > 0) {
+		switch (cfg->rate) {
+		case 48000:
+			sbc_cap->frequency = A2DP_SAMPLING_FREQ_48000;
+			break;
+		case 44100:
+			sbc_cap->frequency = A2DP_SAMPLING_FREQ_44100;
+			break;
+		case 32000:
+			sbc_cap->frequency = A2DP_SAMPLING_FREQ_32000;
+			break;
+		case 16000:
+			sbc_cap->frequency = A2DP_SAMPLING_FREQ_16000;
+			break;
+		default:
+			return -EINVAL;
+		}
+	} else {
+		sbc_cap->frequency = ( A2DP_SAMPLING_FREQ_48000 |
+					A2DP_SAMPLING_FREQ_44100 |
+					A2DP_SAMPLING_FREQ_32000 |
+					A2DP_SAMPLING_FREQ_16000 );
+	}
+
+	if (cfg->channel_mode > 0) {
+		switch (cfg->channel_mode) {
+		case A2DP_CHANNEL_MODE_JOINT_STEREO:
+		case A2DP_CHANNEL_MODE_STEREO:
+		case A2DP_CHANNEL_MODE_DUAL_CHANNEL:
+		case A2DP_CHANNEL_MODE_MONO:
+			sbc_cap->channel_mode = cfg->channel_mode;
+			break;
+		default:
+			return -EINVAL;
+		}
+	} else {
+		sbc_cap->channel_mode = ( A2DP_CHANNEL_MODE_JOINT_STEREO |
+					A2DP_CHANNEL_MODE_STEREO |
+					A2DP_CHANNEL_MODE_DUAL_CHANNEL |
+					A2DP_CHANNEL_MODE_MONO );
+	}
+
+	if (sbc->allocation > 0) {
+		switch (sbc->allocation) {
+		case A2DP_ALLOCATION_LOUDNESS:
+		case A2DP_ALLOCATION_SNR:
+			sbc_cap->allocation_method = sbc->allocation;
+			break;
+		default:
+			return -EINVAL;
+		}
+	} else
+		sbc_cap->allocation_method = ( A2DP_ALLOCATION_LOUDNESS |
+						A2DP_ALLOCATION_SNR );
+
+	if (sbc->subbands > 0) {
+		switch (sbc->subbands) {
+		case 8:
+			sbc_cap->subbands = A2DP_SUBBANDS_8;
+			break;
+		case 4:
+			sbc_cap->subbands = A2DP_SUBBANDS_4;
+			break;
+		default:
+			return -EINVAL;
+		}
+	} else
+		sbc_cap->subbands = ( A2DP_SUBBANDS_8 | A2DP_SUBBANDS_4 );
+
+	if (sbc->blocks > 0) {
+		switch (sbc->blocks) {
+		case 16:
+			sbc_cap->block_length = A2DP_BLOCK_LENGTH_16;
+			break;
+		case 12:
+			sbc_cap->block_length = A2DP_BLOCK_LENGTH_12;
+			break;
+		case 8:
+			sbc_cap->block_length = A2DP_BLOCK_LENGTH_8;
+			break;
+		case 4:
+			sbc_cap->block_length = A2DP_BLOCK_LENGTH_4;
+			break;
+		default:
+			return -EINVAL;
+		}
+	} else {
+		sbc_cap->block_length = ( A2DP_BLOCK_LENGTH_16 |
+					A2DP_BLOCK_LENGTH_12 |
+					A2DP_BLOCK_LENGTH_8 |
+					A2DP_BLOCK_LENGTH_4 );
+	}
+
+	if (sbc->bitpool > 250)
+		return -EINVAL;
+	else if (sbc->bitpool > 0)
+		sbc_cap->min_bitpool = sbc_cap->max_bitpool = sbc->bitpool;
+	else {
+		sbc_cap->min_bitpool = 2;
+		sbc_cap->max_bitpool = 250;
+	}
+
+	return 0;
+}
+
+
 static void cfg_event(struct unix_client *client, struct ipc_packet *pkt,
 			int len)
 {
 	struct device *dev;
 	bdaddr_t bdaddr;
+	struct ipc_data_cfg *cfg = (void *) pkt->data;
+	struct sbc_codec_cap sbc_cap;
 
 	str2ba(pkt->device, &bdaddr);
 
@@ -460,6 +578,12 @@ static void cfg_event(struct unix_client *client, struct ipc_packet *pkt,
 	else if (pkt->role == PKT_ROLE_HIFI)
 		client->interface = g_strdup(AUDIO_SINK_INTERFACE);
 
+	if (cfg_to_caps(cfg, &sbc_cap) < 0)
+		goto failed;
+
+	client->media_codec = avdtp_service_cap_new(AVDTP_MEDIA_CODEC, &sbc_cap,
+							sizeof(sbc_cap));
+
 	if (!manager_find_device(&bdaddr, NULL, FALSE)) {
 		if (!bacmp(&bdaddr, BDADDR_ANY))
 			goto failed;
@@ -475,6 +599,7 @@ static void cfg_event(struct unix_client *client, struct ipc_packet *pkt,
 		goto failed;
 
 	create_stream(dev, client);
+
 	return;
 
 failed: