From 8ac177673ba3731bf5945babdc3425fff4cfbce8 Mon Sep 17 00:00:00 2001
From: Chan-yeol Park <chanyeol.park@samsung.com>
Date: Tue, 9 Oct 2012 20:35:44 +0900
Subject: [PATCH] audio: Add check for vendor specific A2DP codec

This patch adds checks(vendor ID, vendor specific codec ID) to make sure of
vendor specific A2DP codec selection.
---
 audio/a2dp-codecs.h |  6 +++++
 audio/a2dp.c        | 57 +++++++++++++++++++++++++++++++++++++++++++--
 2 files changed, 61 insertions(+), 2 deletions(-)

diff --git a/audio/a2dp-codecs.h b/audio/a2dp-codecs.h
index 51c796a5b..2afafa518 100644
--- a/audio/a2dp-codecs.h
+++ b/audio/a2dp-codecs.h
@@ -26,6 +26,7 @@
 #define A2DP_CODEC_MPEG12		0x01
 #define A2DP_CODEC_MPEG24		0x02
 #define A2DP_CODEC_ATRAC		0x03
+#define A2DP_CODEC_VENDOR		0xFF
 
 #define SBC_SAMPLING_FREQ_16000		(1 << 3)
 #define SBC_SAMPLING_FREQ_32000		(1 << 2)
@@ -114,3 +115,8 @@ typedef struct {
 #else
 #error "Unknown byte order"
 #endif
+
+typedef struct {
+	uint8_t vendor_id[4];
+	uint8_t codec_id[2];
+} __attribute__ ((packed)) a2dp_vendor_codec_t;
diff --git a/audio/a2dp.c b/audio/a2dp.c
index fd1c49452..77994201c 100644
--- a/audio/a2dp.c
+++ b/audio/a2dp.c
@@ -44,6 +44,7 @@
 #include "sink.h"
 #include "source.h"
 #include "a2dp.h"
+#include "a2dp-codecs.h"
 #include "sdpd.h"
 
 /* The duration that streams without users are allowed to stay in
@@ -1427,11 +1428,54 @@ done:
 	finalize_select(setup);
 }
 
+static gboolean check_vendor_codec(struct a2dp_sep *sep, uint8_t *cap,
+								size_t len)
+{
+	uint8_t *capabilities;
+	size_t length;
+	a2dp_vendor_codec_t *local_codec;
+	a2dp_vendor_codec_t *remote_codec;
+
+	if (len < sizeof(a2dp_vendor_codec_t))
+		return FALSE;
+
+	remote_codec = (a2dp_vendor_codec_t *) cap;
+
+	if (sep->endpoint == NULL)
+		return FALSE;
+
+	length = sep->endpoint->get_capabilities(sep,
+				&capabilities, sep->user_data);
+
+	if (length < sizeof(a2dp_vendor_codec_t))
+		return FALSE;
+
+	local_codec = (a2dp_vendor_codec_t *) capabilities;
+
+	if (memcmp(remote_codec->vendor_id, local_codec->vendor_id,
+					sizeof(local_codec->vendor_id)))
+		return FALSE;
+
+	if (memcmp(remote_codec->codec_id, local_codec->codec_id,
+					sizeof(local_codec->codec_id)))
+		return FALSE;
+
+	DBG("vendor 0x%02x%02x%02x%02x codec 0x%02x%02x",
+			remote_codec->vendor_id[0], remote_codec->vendor_id[1],
+			remote_codec->vendor_id[2], remote_codec->vendor_id[3],
+			remote_codec->codec_id[0], remote_codec->codec_id[1]);
+
+	return TRUE;
+}
+
 static struct a2dp_sep *a2dp_find_sep(struct avdtp *session, GSList *list,
 					const char *sender)
 {
 	for (; list; list = list->next) {
 		struct a2dp_sep *sep = list->data;
+		struct avdtp_remote_sep *rsep;
+		struct avdtp_media_codec_capability *cap;
+		struct avdtp_service_capability *service;
 
 		/* Use sender's endpoint if available */
 		if (sender) {
@@ -1445,10 +1489,19 @@ static struct a2dp_sep *a2dp_find_sep(struct avdtp *session, GSList *list,
 				continue;
 		}
 
-		if (avdtp_find_remote_sep(session, sep->lsep) == NULL)
+		rsep = avdtp_find_remote_sep(session, sep->lsep);
+		if (rsep == NULL)
 			continue;
 
-		return sep;
+		service = avdtp_get_codec(rsep);
+		cap = (struct avdtp_media_codec_capability *) service->data;
+
+		if (cap->media_codec_type != A2DP_CODEC_VENDOR)
+			return sep;
+
+		if (check_vendor_codec(sep, cap->data,
+					service->length - sizeof(*cap)))
+			return sep;
 	}
 
 	return NULL;
-- 
2.47.3