[PATCH 1/3] android/handsfree: Add initial implementation of Codec Negotiation feature

[PATCH 1/3] android/handsfree: Add initial implementation of Codec Negotiation feature

[Marcin Kraglak]

It will handle AT+BAC command and update list of available codecs.
It will check if mandatory codec CVSD is present on list and, if
Wide Band Speech supported, MSBC codec is on next place. Other codecs
can be also used after extending codecs_defaults array.
It will also handle incorrect SLC establishment, when HF supports
Codec Negotiation, but didn't send AT+BAC.
---
 android/handsfree.c | 94 +++++++++++++++++++++++++++++++++++++++++++++++++++--
 1 file changed, 92 insertions(+), 2 deletions(-)

diff --git a/android/handsfree.c b/android/handsfree.c
index b71c28e..44d1f32 100644
--- a/android/handsfree.c
+++ b/android/handsfree.c
@@ -71,7 +71,8 @@
 
 #define HFP_AG_FEATURES ( HFP_AG_FEAT_3WAY | HFP_AG_FEAT_ECNR |\
 			HFP_AG_FEAT_VR | HFP_AG_FEAT_REJ_CALL |\
-			HFP_AG_FEAT_ECS | HFP_AG_FEAT_EXT_ERR )
+			HFP_AG_FEAT_ECS | HFP_AG_FEAT_EXT_ERR |\
+			HFP_AG_FEAT_CODEC )
 
 #define HFP_AG_CHLD "0,1,2,3"
 
@@ -87,6 +88,13 @@
 
 #define RING_TIMEOUT 2
 
+#define CVSD_OFFSET 0
+#define MSBC_OFFSET 1
+#define CODECS_COUNT (MSBC_OFFSET + 1)
+
+#define CODEC_ID_CVSD 0x01
+#define CODEC_ID_MSBC 0x02
+
 struct indicator {
 	const char *name;
 	int min;
@@ -96,6 +104,12 @@ struct indicator {
 	bool active;
 };
 
+struct hfp_codec {
+	uint8_t type;
+	bool local_supported;
+	bool remote_supported;
+};
+
 static const struct indicator inds_defaults[] = {
 		{ "service",   0, 1, 0, false, true },
 		{ "call",      0, 1, 0, true, true },
@@ -106,6 +120,11 @@ static const struct indicator inds_defaults[] = {
 		{ "battchg",   0, 5, 0, false, true },
 };
 
+static const struct hfp_codec codecs_defaults[] = {
+	{ CODEC_ID_CVSD, true, false},
+	{ CODEC_ID_MSBC, false, false},
+};
+
 static struct {
 	bdaddr_t bdaddr;
 	uint8_t state;
@@ -116,6 +135,9 @@ static struct {
 	bool ccwa_enabled;
 	bool indicators_enabled;
 	struct indicator inds[IND_COUNT];
+	uint8_t negotiated_codec;
+	uint8_t proposed_codec;
+	struct hfp_codec codecs[CODECS_COUNT];
 	guint ring;
 	bool hsp;
 	struct hfp_gw *gw;
@@ -180,6 +202,8 @@ static void device_init(const bdaddr_t *bdaddr)
 
 	memcpy(device.inds, inds_defaults, sizeof(device.inds));
 
+	memcpy(device.codecs, codecs_defaults, sizeof(device.codecs));
+
 	device_set_state(HAL_EV_HANDSFREE_CONN_STATE_CONNECTING);
 }
 
@@ -924,6 +948,13 @@ static void at_cmd_cind(struct hfp_gw_result *result, enum hfp_gw_cmd_type type,
 	switch (type) {
 	case HFP_GW_CMD_TYPE_TEST:
 
+		/* If device supports Codec Negotiation, AT+BAC should be
+		 * received first
+		 */
+		if ((device.features & HFP_HF_FEAT_CODEC))
+			if (!device.codecs[CVSD_OFFSET].remote_supported)
+				break;
+
 		len = strlen("+CIND:") + 1;
 
 		for (i = 0; i < IND_COUNT; i++) {
@@ -1032,13 +1063,72 @@ static void at_cmd_chld(struct hfp_gw_result *result, enum hfp_gw_cmd_type type,
 	hfp_gw_send_result(device.gw, HFP_RESULT_ERROR);
 }
 
+static struct hfp_codec *find_codec_by_type(uint8_t type)
+{
+	int i;
+
+	for (i = 0; i < CODECS_COUNT; i++)
+		if (type == device.codecs[i].type)
+			return &device.codecs[i];
+
+	return NULL;
+}
+
 static void at_cmd_bac(struct hfp_gw_result *result, enum hfp_gw_cmd_type type,
 							void *user_data)
 {
+	unsigned int val;
+
 	DBG("");
 
-	/* TODO */
+	switch (type) {
+	case HFP_GW_CMD_TYPE_SET:
+		if (!(device.features & HFP_HF_FEAT_CODEC))
+			goto failed;
+
+		/* Clear list of codecs */
+		memcpy(device.codecs, codecs_defaults, sizeof(device.codecs));
+		device.negotiated_codec = 0;
+
+		/* At least CVSD mandatory codec must exist
+		 * HFP V1.6 4.34.1
+		 */
+		if (!hfp_gw_result_get_number(result, &val)
+				|| val != CODEC_ID_CVSD)
+			goto failed;
 
+		device.codecs[CVSD_OFFSET].remote_supported = true;
+
+		if (hfp_gw_result_get_number(result, &val)) {
+			if (val != CODEC_ID_MSBC)
+				goto failed;
+
+			device.codecs[MSBC_OFFSET].remote_supported = true;
+		}
+
+		while (hfp_gw_result_has_next(result)) {
+			struct hfp_codec *codec;
+
+			if (!hfp_gw_result_get_number(result, &val))
+				goto failed;
+
+			codec = find_codec_by_type(val);
+			if (!codec)
+				continue;
+
+			codec->remote_supported = true;
+		}
+
+		hfp_gw_send_result(device.gw, HFP_RESULT_OK);
+
+		return;
+	case HFP_GW_CMD_TYPE_TEST:
+	case HFP_GW_CMD_TYPE_READ:
+	case HFP_GW_CMD_TYPE_COMMAND:
+		break;
+	}
+
+failed:
 	hfp_gw_send_result(device.gw, HFP_RESULT_ERROR);
 }
 
-- 
1.8.3.1

[PATCH 2/3] android/handsfree: Move connect_sco() to more appropriate place

[Marcin Kraglak]

Move connect_sco as it will be used in at_cmd_bcc()
---
 android/handsfree.c | 122 ++++++++++++++++++++++++++--------------------------
 1 file changed, 61 insertions(+), 61 deletions(-)

diff --git a/android/handsfree.c b/android/handsfree.c
index 44d1f32..1aee695 100644
--- a/android/handsfree.c
+++ b/android/handsfree.c
@@ -810,6 +810,67 @@ static void at_cmd_btrh(struct hfp_gw_result *result, enum hfp_gw_cmd_type type,
 	hfp_gw_send_result(device.gw, HFP_RESULT_ERROR);
 }
 
+static gboolean sco_watch_cb(GIOChannel *chan, GIOCondition cond,
+							gpointer user_data)
+{
+	g_io_channel_shutdown(device.sco, TRUE, NULL);
+	g_io_channel_unref(device.sco);
+	device.sco = NULL;
+
+	device.sco_watch = 0;
+
+	device_set_audio_state(HAL_EV_HANDSFREE_AUDIO_STATE_DISCONNECTED);
+
+	return FALSE;
+}
+
+static void connect_sco_cb(GIOChannel *chan, GError *err, gpointer user_data)
+{
+	if (err) {
+		uint8_t status;
+
+		error("SCO: connect failed (%s)", err->message);
+		status = HAL_EV_HANDSFREE_AUDIO_STATE_DISCONNECTED;
+		device_set_audio_state(status);
+
+		return;
+	}
+
+	g_io_channel_set_close_on_unref(chan, TRUE);
+
+	device.sco = g_io_channel_ref(chan);
+	device.sco_watch = g_io_add_watch(chan, G_IO_ERR | G_IO_HUP | G_IO_NVAL,
+							sco_watch_cb, NULL);
+
+	device_set_audio_state(HAL_EV_HANDSFREE_AUDIO_STATE_CONNECTED);
+}
+
+static bool connect_sco(void)
+{
+	GIOChannel *io;
+	GError *gerr = NULL;
+
+	if (device.sco)
+		return false;
+
+	io = bt_io_connect(connect_sco_cb, NULL, NULL, &gerr,
+				BT_IO_OPT_SOURCE_BDADDR, &adapter_addr,
+				BT_IO_OPT_DEST_BDADDR, &device.bdaddr,
+				BT_IO_OPT_INVALID);
+
+	if (!io) {
+		error("SCO: unable to connect: %s", gerr->message);
+		g_error_free(gerr);
+		return false;
+	}
+
+	g_io_channel_unref(io);
+
+	device_set_audio_state(HAL_EV_HANDSFREE_AUDIO_STATE_CONNECTING);
+
+	return true;
+}
+
 static void at_cmd_bcc(struct hfp_gw_result *result, enum hfp_gw_cmd_type type,
 							void *user_data)
 {
@@ -1461,67 +1522,6 @@ failed:
 					HAL_OP_HANDSFREE_DISCONNECT, status);
 }
 
-static gboolean sco_watch_cb(GIOChannel *chan, GIOCondition cond,
-							gpointer user_data)
-{
-	g_io_channel_shutdown(device.sco, TRUE, NULL);
-	g_io_channel_unref(device.sco);
-	device.sco = NULL;
-
-	device.sco_watch = 0;
-
-	device_set_audio_state(HAL_EV_HANDSFREE_AUDIO_STATE_DISCONNECTED);
-
-	return FALSE;
-}
-
-static void connect_sco_cb(GIOChannel *chan, GError *err, gpointer user_data)
-{
-	if (err) {
-		uint8_t status;
-
-		error("SCO: connect failed (%s)", err->message);
-		status = HAL_EV_HANDSFREE_AUDIO_STATE_DISCONNECTED;
-		device_set_audio_state(status);
-
-		return;
-	}
-
-	g_io_channel_set_close_on_unref(chan, TRUE);
-
-	device.sco = g_io_channel_ref(chan);
-	device.sco_watch = g_io_add_watch(chan, G_IO_ERR | G_IO_HUP | G_IO_NVAL,
-							sco_watch_cb, NULL);
-
-	device_set_audio_state(HAL_EV_HANDSFREE_AUDIO_STATE_CONNECTED);
-}
-
-static bool connect_sco(void)
-{
-	GIOChannel *io;
-	GError *gerr = NULL;
-
-	if (device.sco)
-		return false;
-
-	io = bt_io_connect(connect_sco_cb, NULL, NULL, &gerr,
-				BT_IO_OPT_SOURCE_BDADDR, &adapter_addr,
-				BT_IO_OPT_DEST_BDADDR, &device.bdaddr,
-				BT_IO_OPT_INVALID);
-
-	if (!io) {
-		error("SCO: unable to connect: %s", gerr->message);
-		g_error_free(gerr);
-		return false;
-	}
-
-	g_io_channel_unref(io);
-
-	device_set_audio_state(HAL_EV_HANDSFREE_AUDIO_STATE_CONNECTING);
-
-	return true;
-}
-
 static bool disconnect_sco(void)
 {
 	if (!device.sco)
-- 
1.8.3.1

[PATCH 3/3] android/handsfree: Add handling of AT+BCS and AT+BCC

[Marcin Kraglak]

It will service codec nogotiation and establish SCO connection with
negotiated parameters. If SCO establishment failed, try to connect
with mandatory codec CVSD.
---
 android/handsfree.c | 116 ++++++++++++++++++++++++++++++++++++++++++++++++++--
 1 file changed, 113 insertions(+), 3 deletions(-)

diff --git a/android/handsfree.c b/android/handsfree.c
index 1aee695..7fefccd 100644
--- a/android/handsfree.c
+++ b/android/handsfree.c
@@ -824,6 +824,33 @@ static gboolean sco_watch_cb(GIOChannel *chan, GIOCondition cond,
 	return FALSE;
 }
 
+static void select_codec(uint8_t codec_type)
+{
+	uint8_t type = CODEC_ID_CVSD;
+	int i;
+
+	if (codec_type > 0) {
+		type = codec_type;
+		goto done;
+	}
+
+	for (i = CODECS_COUNT - 1; i >= CVSD_OFFSET; i--) {
+		if (!device.codecs[i].local_supported)
+			continue;
+
+		if (!device.codecs[i].remote_supported)
+			continue;
+
+		type = device.codecs[i].type;
+		break;
+	}
+
+done:
+	device.proposed_codec = type;
+
+	hfp_gw_send_info(device.gw, "+BCS: %u", type);
+}
+
 static void connect_sco_cb(GIOChannel *chan, GError *err, gpointer user_data)
 {
 	if (err) {
@@ -833,6 +860,13 @@ static void connect_sco_cb(GIOChannel *chan, GError *err, gpointer user_data)
 		status = HAL_EV_HANDSFREE_AUDIO_STATE_DISCONNECTED;
 		device_set_audio_state(status);
 
+		if (!(device.features & HFP_HF_FEAT_CODEC))
+			return;
+
+		if (device.negotiated_codec != CODEC_ID_CVSD)
+			/* If other failed, try connect CVSD */
+			select_codec(CODEC_ID_CVSD);
+
 		return;
 	}
 
@@ -849,13 +883,21 @@ static bool connect_sco(void)
 {
 	GIOChannel *io;
 	GError *gerr = NULL;
+	uint16_t voice_settings;
 
 	if (device.sco)
 		return false;
 
+	if ((device.features & HFP_HF_FEAT_CODEC) && device.negotiated_codec
+							!= CODEC_ID_CVSD)
+		voice_settings = BT_VOICE_TRANSPARENT;
+	else
+		voice_settings = BT_VOICE_CVSD_16BIT;
+
 	io = bt_io_connect(connect_sco_cb, NULL, NULL, &gerr,
 				BT_IO_OPT_SOURCE_BDADDR, &adapter_addr,
 				BT_IO_OPT_DEST_BDADDR, &device.bdaddr,
+				BT_IO_OPT_VOICE, voice_settings,
 				BT_IO_OPT_INVALID);
 
 	if (!io) {
@@ -876,7 +918,33 @@ static void at_cmd_bcc(struct hfp_gw_result *result, enum hfp_gw_cmd_type type,
 {
 	DBG("");
 
-	/* TODO */
+	switch (type) {
+	case HFP_GW_CMD_TYPE_COMMAND:
+		if (!(device.features & HFP_HF_FEAT_CODEC))
+			break;
+
+		if (hfp_gw_result_has_next(result))
+			break;
+
+		hfp_gw_send_result(device.gw, HFP_RESULT_OK);
+
+		/* we haven't negotiated codec, start selection */
+		if (!device.negotiated_codec) {
+			select_codec(0);
+			return;
+		}
+		/* we try connect to negotiated codec. If it fails, and it isn't
+		 * CVSD codec, try connect CVSD
+		 */
+		if (!connect_sco() && device.negotiated_codec != CODEC_ID_CVSD)
+			select_codec(CODEC_ID_CVSD);
+
+		return;
+	case HFP_GW_CMD_TYPE_READ:
+	case HFP_GW_CMD_TYPE_TEST:
+	case HFP_GW_CMD_TYPE_SET:
+		break;
+	}
 
 	hfp_gw_send_result(device.gw, HFP_RESULT_ERROR);
 }
@@ -884,9 +952,38 @@ static void at_cmd_bcc(struct hfp_gw_result *result, enum hfp_gw_cmd_type type,
 static void at_cmd_bcs(struct hfp_gw_result *result, enum hfp_gw_cmd_type type,
 							void *user_data)
 {
+	unsigned int val;
+
 	DBG("");
 
-	/* TODO */
+	switch (type) {
+	case HFP_GW_CMD_TYPE_SET:
+		if (!hfp_gw_result_get_number(result, &val))
+			break;
+
+		if (hfp_gw_result_has_next(result))
+			break;
+
+		/* Remote replied with other codec. Reply with error */
+		if (device.proposed_codec != val) {
+			device.proposed_codec = 0;
+			break;
+		}
+
+		device.proposed_codec = 0;
+		device.negotiated_codec = val;
+
+		hfp_gw_send_result(device.gw, HFP_RESULT_OK);
+
+		/* Connect sco with negotiated parameters */
+		connect_sco();
+
+		return;
+	case HFP_GW_CMD_TYPE_READ:
+	case HFP_GW_CMD_TYPE_TEST:
+	case HFP_GW_CMD_TYPE_COMMAND:
+		break;
+	}
 
 	hfp_gw_send_result(device.gw, HFP_RESULT_ERROR);
 }
@@ -1543,6 +1640,19 @@ static bool disconnect_sco(void)
 	return true;
 }
 
+static bool connect_audio(void)
+{
+	if ((device.features & HFP_HF_FEAT_CODEC) && !device.negotiated_codec) {
+		/* It's probably first connection, select best codec
+		 * and try connect
+		 */
+		select_codec(0);
+		return true;
+	}
+
+	return connect_sco();
+}
+
 static void handle_connect_audio(const void *buf, uint16_t len)
 {
 	const struct hal_cmd_handsfree_connect_audio *cmd = buf;
@@ -1559,7 +1669,7 @@ static void handle_connect_audio(const void *buf, uint16_t len)
 		goto done;
 	}
 
-	status = connect_sco() ? HAL_STATUS_SUCCESS : HAL_STATUS_FAILED;
+	status = connect_audio() ? HAL_STATUS_SUCCESS : HAL_STATUS_FAILED;
 
 done:
 	ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_HANDSFREE,
-- 
1.8.3.1

Re: [PATCH 1/3] android/handsfree: Add initial implementation of Codec Negotiation feature

[Szymon Janc]

hi Marcin,

On Thursday 13 March 2014 15:04:35 Marcin Kraglak wrote:
> It will handle AT+BAC command and update list of available codecs.
> It will check if mandatory codec CVSD is present on list and, if
> Wide Band Speech supported, MSBC codec is on next place. Other codecs
> can be also used after extending codecs_defaults array.
> It will also handle incorrect SLC establishment, when HF supports
> Codec Negotiation, but didn't send AT+BAC.
> ---
>  android/handsfree.c | 94
> +++++++++++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 92
> insertions(+), 2 deletions(-)
> 
> diff --git a/android/handsfree.c b/android/handsfree.c
> index b71c28e..44d1f32 100644
> --- a/android/handsfree.c
> +++ b/android/handsfree.c
> @@ -71,7 +71,8 @@
> 
>  #define HFP_AG_FEATURES ( HFP_AG_FEAT_3WAY | HFP_AG_FEAT_ECNR |\
>  			HFP_AG_FEAT_VR | HFP_AG_FEAT_REJ_CALL |\
> -			HFP_AG_FEAT_ECS | HFP_AG_FEAT_EXT_ERR )
> +			HFP_AG_FEAT_ECS | HFP_AG_FEAT_EXT_ERR |\
> +			HFP_AG_FEAT_CODEC )
> 
>  #define HFP_AG_CHLD "0,1,2,3"
> 
> @@ -87,6 +88,13 @@
> 
>  #define RING_TIMEOUT 2
> 
> +#define CVSD_OFFSET 0
> +#define MSBC_OFFSET 1
> +#define CODECS_COUNT (MSBC_OFFSET + 1)
> +
> +#define CODEC_ID_CVSD 0x01
> +#define CODEC_ID_MSBC 0x02
> +
>  struct indicator {
>  	const char *name;
>  	int min;
> @@ -96,6 +104,12 @@ struct indicator {
>  	bool active;
>  };
> 
> +struct hfp_codec {
> +	uint8_t type;
> +	bool local_supported;
> +	bool remote_supported;
> +};
> +
>  static const struct indicator inds_defaults[] = {
>  		{ "service",   0, 1, 0, false, true },
>  		{ "call",      0, 1, 0, true, true },
> @@ -106,6 +120,11 @@ static const struct indicator inds_defaults[] = {
>  		{ "battchg",   0, 5, 0, false, true },
>  };
> 
> +static const struct hfp_codec codecs_defaults[] = {
> +	{ CODEC_ID_CVSD, true, false},
> +	{ CODEC_ID_MSBC, false, false},
> +};
> +
>  static struct {
>  	bdaddr_t bdaddr;
>  	uint8_t state;
> @@ -116,6 +135,9 @@ static struct {
>  	bool ccwa_enabled;
>  	bool indicators_enabled;
>  	struct indicator inds[IND_COUNT];
> +	uint8_t negotiated_codec;
> +	uint8_t proposed_codec;
> +	struct hfp_codec codecs[CODECS_COUNT];
>  	guint ring;
>  	bool hsp;
>  	struct hfp_gw *gw;
> @@ -180,6 +202,8 @@ static void device_init(const bdaddr_t *bdaddr)
> 
>  	memcpy(device.inds, inds_defaults, sizeof(device.inds));
> 
> +	memcpy(device.codecs, codecs_defaults, sizeof(device.codecs));
> +
>  	device_set_state(HAL_EV_HANDSFREE_CONN_STATE_CONNECTING);
>  }
> 
> @@ -924,6 +948,13 @@ static void at_cmd_cind(struct hfp_gw_result *result,
> enum hfp_gw_cmd_type type, switch (type) {
>  	case HFP_GW_CMD_TYPE_TEST:
> 
> +		/* If device supports Codec Negotiation, AT+BAC should be
> +		 * received first
> +		 */
> +		if ((device.features & HFP_HF_FEAT_CODEC))
> +			if (!device.codecs[CVSD_OFFSET].remote_supported)
> +				break;
> +
>  		len = strlen("+CIND:") + 1;
> 
>  		for (i = 0; i < IND_COUNT; i++) {
> @@ -1032,13 +1063,72 @@ static void at_cmd_chld(struct hfp_gw_result
> *result, enum hfp_gw_cmd_type type, hfp_gw_send_result(device.gw,
> HFP_RESULT_ERROR);
>  }
> 
> +static struct hfp_codec *find_codec_by_type(uint8_t type)
> +{
> +	int i;
> +
> +	for (i = 0; i < CODECS_COUNT; i++)
> +		if (type == device.codecs[i].type)
> +			return &device.codecs[i];
> +
> +	return NULL;
> +}
> +
>  static void at_cmd_bac(struct hfp_gw_result *result, enum hfp_gw_cmd_type
> type, void *user_data)
>  {
> +	unsigned int val;
> +
>  	DBG("");
> 
> -	/* TODO */
> +	switch (type) {
> +	case HFP_GW_CMD_TYPE_SET:
> +		if (!(device.features & HFP_HF_FEAT_CODEC))
> +			goto failed;
> +
> +		/* Clear list of codecs */
> +		memcpy(device.codecs, codecs_defaults, sizeof(device.codecs));
> +		device.negotiated_codec = 0;
> +
> +		/* At least CVSD mandatory codec must exist
> +		 * HFP V1.6 4.34.1
> +		 */
> +		if (!hfp_gw_result_get_number(result, &val)
> +				|| val != CODEC_ID_CVSD)
> +			goto failed;
> 
> +		device.codecs[CVSD_OFFSET].remote_supported = true;
> +
> +		if (hfp_gw_result_get_number(result, &val)) {
> +			if (val != CODEC_ID_MSBC)
> +				goto failed;
> +
> +			device.codecs[MSBC_OFFSET].remote_supported = true;
> +		}
> +
> +		while (hfp_gw_result_has_next(result)) {
> +			struct hfp_codec *codec;
> +
> +			if (!hfp_gw_result_get_number(result, &val))
> +				goto failed;
> +
> +			codec = find_codec_by_type(val);
> +			if (!codec)
> +				continue;
> +
> +			codec->remote_supported = true;
> +		}
> +
> +		hfp_gw_send_result(device.gw, HFP_RESULT_OK);
> +
> +		return;
> +	case HFP_GW_CMD_TYPE_TEST:
> +	case HFP_GW_CMD_TYPE_READ:
> +	case HFP_GW_CMD_TYPE_COMMAND:
> +		break;
> +	}
> +
> +failed:
>  	hfp_gw_send_result(device.gw, HFP_RESULT_ERROR);
>  }

All patches applied, thanks.

I had to fix some coding style issues though, so please pay attention on that 
in future.

-- 
Szymon K. Janc
szymon.janc@gmail.com