[PATCH v4 0/9] bluetooth: handsfree audio agent

[PATCH v4 0/9] bluetooth: handsfree audio agent

[=?unknown-8bit?q?Fr=C3=A9d=C3=A9ric?= Dalleau]

[-- Attachment #1: Type: text/plain, Size: 1228 bytes --]

Hello,

This is an handsfree audio agent written in C.
I could test CVSD and MSBC (patch v6) with it.

v4:
fixes an fd leak
improves mSBC playback which could be choppy
add an option for killing an agent that connects
supports 16 bits voice settings for SCO_OPTIONS (requires kernel patch v4).

v3:
fixes a bug by which only noise was sent to remote device.
handles defered socket with following code instead of using a dummy variable:
        if (recv(thread->fd, NULL, 0, 0) < 0)

Best regards,
Frédéric


Frédéric Dalleau (9):
  handsfree-audio: Initial DBUS code
  handsfree-audio: Build handsfree-audio command line tool
  handsfree-audio: Add Alsa dependancy
  handsfree-audio: Link tool with Alsa
  handsfree-audio: Implement alsa playback
  handsfree-audio: Add SBC dependency
  handsfree-audio: Link tool with SBC library
  handsfree-audio: mSBC support
  handsfree-audio: Add an option to kill incoming connections

 Makefile.am             |    6 +-
 configure.ac            |   10 +
 tools/handsfree-audio.c |  979 +++++++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 994 insertions(+), 1 deletion(-)
 create mode 100644 tools/handsfree-audio.c

-- 
1.7.9.5

[PATCH v4 1/9] handsfree-audio: Initial DBUS code

[=?unknown-8bit?q?Fr=C3=A9d=C3=A9ric?= Dalleau]

[-- Attachment #1: Type: text/plain, Size: 8857 bytes --]

This code can register an handsfree audio agent and receive NewConnection and
Release calls.
---
 tools/handsfree-audio.c |  332 +++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 332 insertions(+)
 create mode 100644 tools/handsfree-audio.c

diff --git a/tools/handsfree-audio.c b/tools/handsfree-audio.c
new file mode 100644
index 0000000..88310aa
--- /dev/null
+++ b/tools/handsfree-audio.c
@@ -0,0 +1,332 @@
+/*
+ *
+ *  oFono - Open Source Telephony
+ *
+ *  Copyright (C) 2013  Intel Corporation. All rights reserved.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2 as
+ *  published by the Free Software Foundation.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <signal.h>
+
+#include <gdbus.h>
+#include <glib.h>
+
+#define OFONO_SERVICE			"org.ofono"
+#define HFP_AUDIO_MANAGER_PATH		"/"
+#define HFP_AUDIO_MANAGER_INTERFACE	OFONO_SERVICE ".HandsfreeAudioManager"
+#define HFP_AUDIO_AGENT_PATH		"/hfpaudioagent"
+#define HFP_AUDIO_AGENT_INTERFACE	OFONO_SERVICE ".HandsfreeAudioAgent"
+
+#define HFP_AUDIO_CVSD			1
+#define HFP_AUDIO_MSBC			2
+
+#define DBG(fmt, arg...) do {\
+		g_print("%s: " fmt "\n", __FUNCTION__, ## arg);\
+	} while (0)
+
+/* DBus related */
+static GMainLoop *main_loop = NULL;
+static DBusConnection *conn;
+static GSList *hcons = NULL;
+
+static gboolean option_nocvsd = FALSE;
+static gboolean option_nomsbc = FALSE;
+
+struct hfp_audio_conn {
+	unsigned char codec;
+	int watch;
+};
+
+static void hfp_audio_conn_free(struct hfp_audio_conn *hcon)
+{
+	DBG("Freeing audio connection %p", hcon);
+
+	hcons = g_slist_remove(hcons, hcon);
+	g_source_remove(hcon->watch);
+	g_free(hcon);
+}
+
+static gboolean hfp_audio_cb(GIOChannel *io, GIOCondition cond, gpointer data)
+{
+	struct hfp_audio_conn *hcon = data;
+	gsize read;
+	gsize written;
+	char buf[60];
+
+	if (cond & (G_IO_HUP | G_IO_NVAL | G_IO_ERR))
+		goto fail;
+
+	if (g_io_channel_read_chars(io, buf, sizeof(buf), &read, NULL) !=
+			G_IO_STATUS_NORMAL)
+		goto fail;
+
+	g_io_channel_write_chars(io, buf+written, read, &written, NULL);
+
+	return TRUE;
+
+fail:
+	DBG("Disconnected");
+	hfp_audio_conn_free(hcon);
+	return FALSE;
+}
+
+static DBusMessage *agent_newconnection(DBusConnection *conn, DBusMessage *msg,
+					void *data)
+{
+	const char *card;
+	int fd;
+	unsigned char codec;
+	GIOChannel *io;
+	struct hfp_audio_conn *hcon;
+
+	DBG("New connection");
+
+	if (dbus_message_get_args(msg, NULL, DBUS_TYPE_OBJECT_PATH, &card,
+						DBUS_TYPE_UNIX_FD, &fd,
+						DBUS_TYPE_BYTE, &codec,
+						DBUS_TYPE_INVALID) == FALSE)
+		return g_dbus_create_error(msg,
+				HFP_AUDIO_AGENT_INTERFACE ".InvalidArguments",
+				"Invalid arguments");
+
+	DBG("New connection: card=%s fd=%d codec=%d", card, fd, codec);
+
+	io = g_io_channel_unix_new(fd);
+
+	hcon = g_try_malloc0(sizeof(struct hfp_audio_conn));
+	if (hcon == NULL)
+		return NULL;
+
+	hcon->codec = codec;
+	hcon->watch = g_io_add_watch(io, G_IO_IN, hfp_audio_cb, hcon);
+	hcons = g_slist_prepend(hcons, hcon);
+
+	return dbus_message_new_method_return(msg);
+}
+
+static DBusMessage *agent_release(DBusConnection *conn, DBusMessage *msg,
+					void *data)
+{
+	DBG("HFP audio agent released");
+	return dbus_message_new_method_return(msg);
+}
+
+static const GDBusMethodTable agent_methods[] = {
+	{ GDBUS_METHOD("NewConnection", NULL, NULL, agent_newconnection) },
+	{ GDBUS_METHOD("Release", NULL, NULL, agent_release) },
+	{ },
+};
+
+static void hfp_audio_agent_register_reply(DBusPendingCall *call, void *data)
+{
+	DBusMessage *reply = dbus_pending_call_steal_reply(call);
+	DBusError err;
+
+	dbus_error_init(&err);
+
+	if (dbus_set_error_from_message(&err, reply) == TRUE) {
+		DBG("Failed to register audio agent (%s: %s)", err.name,
+								err.message);
+		dbus_error_free(&err);
+	} else {
+		DBG("HFP audio agent registered");
+	}
+
+	dbus_message_unref(reply);
+}
+
+static void hfp_audio_agent_register(DBusConnection *conn)
+{
+	DBusMessage *msg;
+	DBusPendingCall *call;
+	const char *path = HFP_AUDIO_AGENT_PATH;
+	unsigned char codecs[2];
+	const unsigned char *pcodecs = codecs;
+	int ncodecs = 0;
+
+	DBG("Registering audio agent");
+
+	msg = dbus_message_new_method_call(OFONO_SERVICE,
+						HFP_AUDIO_MANAGER_PATH,
+						HFP_AUDIO_MANAGER_INTERFACE,
+						"Register");
+	if (msg == NULL) {
+		DBG("Not enough memory");
+		return;
+	}
+
+	if (option_nocvsd == FALSE)
+		codecs[ncodecs++] = HFP_AUDIO_CVSD;
+
+	if (option_nomsbc == FALSE)
+		codecs[ncodecs++] = HFP_AUDIO_MSBC;
+
+	dbus_message_append_args(msg, DBUS_TYPE_OBJECT_PATH, &path,
+					DBUS_TYPE_ARRAY, DBUS_TYPE_BYTE,
+					&pcodecs, ncodecs, DBUS_TYPE_INVALID);
+
+	if (!dbus_connection_send_with_reply(conn, msg, &call, -1)) {
+		dbus_message_unref(msg);
+		DBG("Unable to register agent");
+		return;
+	}
+
+	dbus_message_unref(msg);
+
+	if (call == NULL) {
+		DBG("Unable to register agent");
+		return;
+	}
+
+	dbus_pending_call_set_notify(call, hfp_audio_agent_register_reply,
+						NULL, NULL);
+
+	dbus_pending_call_unref(call);
+}
+
+static void hfp_audio_agent_create(DBusConnection *conn)
+{
+	DBG("Creating audio agent");
+
+	if (!g_dbus_register_interface(conn, HFP_AUDIO_AGENT_PATH,
+					HFP_AUDIO_AGENT_INTERFACE,
+					agent_methods, NULL, NULL,
+					NULL, NULL)) {
+		DBG("Unable to create local agent");
+		g_main_loop_quit(main_loop);
+	}
+}
+
+static void hfp_audio_agent_destroy(DBusConnection *conn)
+{
+	DBG("Destroying audio agent");
+
+	g_dbus_unregister_interface(conn, HFP_AUDIO_AGENT_PATH,
+						HFP_AUDIO_AGENT_INTERFACE);
+}
+
+static void ofono_connect(DBusConnection *conn, void *user_data)
+{
+	DBG("oFono appeared");
+
+	hfp_audio_agent_register(conn);
+}
+
+static void ofono_disconnect(DBusConnection *conn, void *user_data)
+{
+	DBG("oFono disappeared");
+}
+
+static void disconnect_callback(DBusConnection *conn, void *user_data)
+{
+	DBG("Disconnected from BUS");
+
+	g_main_loop_quit(main_loop);
+}
+
+static void sig_term(int sig)
+{
+	DBG("Terminating");
+
+	g_main_loop_quit(main_loop);
+}
+
+static GOptionEntry options[] = {
+	{ "nocvsd", 'c', 0, G_OPTION_ARG_NONE, &option_nocvsd,
+				"Disable CVSD support" },
+	{ "nomsbc", 'm', 0, G_OPTION_ARG_NONE, &option_nomsbc,
+				"Disable MSBC support" },
+	{ NULL },
+};
+
+int main(int argc, char **argv)
+{
+	GOptionContext *context;
+	GError *error = NULL;
+	DBusError err;
+	guint watch;
+	struct sigaction sa;
+
+	context = g_option_context_new(NULL);
+	g_option_context_add_main_entries(context, options, NULL);
+
+	if (g_option_context_parse(context, &argc, &argv, &error) == FALSE) {
+		if (error != NULL) {
+			DBG("%s", error->message);
+			g_error_free(error);
+		} else
+			DBG("An unknown error occurred");
+		exit(1);
+	}
+
+	g_option_context_free(context);
+
+	if (option_nocvsd == TRUE && option_nomsbc == TRUE) {
+		DBG("At least one codec must be supported");
+		exit(2);
+	}
+
+	main_loop = g_main_loop_new(NULL, FALSE);
+
+	dbus_error_init(&err);
+
+	conn = g_dbus_setup_bus(DBUS_BUS_SYSTEM, NULL, &err);
+	if (conn == NULL) {
+		if (dbus_error_is_set(&err) == TRUE) {
+			DBG("%s", err.message);
+			dbus_error_free(&err);
+		} else
+			DBG("Can't register with system bus");
+		exit(1);
+	}
+
+	g_dbus_set_disconnect_function(conn, disconnect_callback, NULL, NULL);
+
+	memset(&sa, 0, sizeof(sa));
+	sa.sa_handler = sig_term;
+	sigaction(SIGINT, &sa, NULL);
+	sigaction(SIGTERM, &sa, NULL);
+
+	hfp_audio_agent_create(conn);
+
+	watch = g_dbus_add_service_watch(conn, OFONO_SERVICE,
+				ofono_connect, ofono_disconnect, NULL, NULL);
+
+	g_main_loop_run(main_loop);
+
+	g_dbus_remove_watch(conn, watch);
+
+	while (hcons != NULL)
+		hfp_audio_conn_free(hcons->data);
+
+	hfp_audio_agent_destroy(conn);
+
+	dbus_connection_unref(conn);
+
+	g_main_loop_unref(main_loop);
+
+	return 0;
+}
-- 
1.7.9.5

[PATCH v4 2/9] handsfree-audio: Build handsfree-audio command line tool

[=?unknown-8bit?q?Fr=C3=A9d=C3=A9ric?= Dalleau]

[-- Attachment #1: Type: text/plain, Size: 887 bytes --]

---
 Makefile.am |    6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/Makefile.am b/Makefile.am
index 3524098..394cb91 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -721,11 +721,15 @@ TESTS = $(unit_tests)
 if TOOLS
 noinst_PROGRAMS += tools/huawei-audio tools/auto-enable \
 			tools/get-location tools/lookup-apn \
-			tools/lookup-provider-name tools/tty-redirector
+			tools/lookup-provider-name tools/tty-redirector \
+			tools/handsfree-audio
 
 tools_huawei_audio_SOURCES = $(gdbus_sources) tools/huawei-audio.c
 tools_huawei_audio_LDADD = @GLIB_LIBS@ @DBUS_LIBS@
 
+tools_handsfree_audio_SOURCES = $(gdbus_sources) tools/handsfree-audio.c
+tools_handsfree_audio_LDADD = @GLIB_LIBS@ @DBUS_LIBS@
+
 tools_auto_enable_SOURCES = $(gdbus_sources) tools/auto-enable.c
 tools_auto_enable_LDADD = @GLIB_LIBS@ @DBUS_LIBS@
 
-- 
1.7.9.5

[PATCH v4 3/9] handsfree-audio: Add Alsa dependancy

[=?unknown-8bit?q?Fr=C3=A9d=C3=A9ric?= Dalleau]

[-- Attachment #1: Type: text/plain, Size: 567 bytes --]

---
 configure.ac |    5 +++++
 1 file changed, 5 insertions(+)

diff --git a/configure.ac b/configure.ac
index a4a350f..9be3ebf 100644
--- a/configure.ac
+++ b/configure.ac
@@ -121,6 +121,11 @@ if (test "${enable_tools}" = "yes"); then
 				AC_MSG_ERROR(USB library is required))
 	AC_SUBST(USB_CFLAGS)
 	AC_SUBST(USB_LIBS)
+
+	PKG_CHECK_MODULES(ALSA, alsa >= 1.0.0, dummy=yes,
+				AC_MSG_ERROR(Alsa library is required))
+	AC_SUBST(ALSA_CFLAGS)
+	AC_SUBST(ALSA_LIBS)
 fi
 AM_CONDITIONAL(TOOLS, test "${enable_tools}" = "yes")
 
-- 
1.7.9.5

[PATCH v4 4/9] handsfree-audio: Link tool with Alsa

[=?unknown-8bit?q?Fr=C3=A9d=C3=A9ric?= Dalleau]

[-- Attachment #1: Type: text/plain, Size: 679 bytes --]

---
 Makefile.am |    2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Makefile.am b/Makefile.am
index 394cb91..2e5fa4c 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -728,7 +728,7 @@ tools_huawei_audio_SOURCES = $(gdbus_sources) tools/huawei-audio.c
 tools_huawei_audio_LDADD = @GLIB_LIBS@ @DBUS_LIBS@
 
 tools_handsfree_audio_SOURCES = $(gdbus_sources) tools/handsfree-audio.c
-tools_handsfree_audio_LDADD = @GLIB_LIBS@ @DBUS_LIBS@
+tools_handsfree_audio_LDADD = @GLIB_LIBS@ @DBUS_LIBS@ @ALSA_LIBS@ -lpthread
 
 tools_auto_enable_SOURCES = $(gdbus_sources) tools/auto-enable.c
 tools_auto_enable_LDADD = @GLIB_LIBS@ @DBUS_LIBS@
-- 
1.7.9.5

[PATCH v4 5/9] handsfree-audio: Implement alsa playback

[=?unknown-8bit?q?Fr=C3=A9d=C3=A9ric?= Dalleau]

[-- Attachment #1: Type: text/plain, Size: 15347 bytes --]

---
 tools/handsfree-audio.c |  472 +++++++++++++++++++++++++++++++++++++++++------
 1 file changed, 417 insertions(+), 55 deletions(-)

diff --git a/tools/handsfree-audio.c b/tools/handsfree-audio.c
index 88310aa..9a4f85b 100644
--- a/tools/handsfree-audio.c
+++ b/tools/handsfree-audio.c
@@ -30,10 +30,17 @@
 #include <stdlib.h>
 #include <string.h>
 #include <signal.h>
+#include <sys/time.h>
+#include <poll.h>
 
 #include <gdbus.h>
 #include <glib.h>
 
+#include <alsa/asoundlib.h>
+#include <pthread.h>
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/sco.h>
+
 #define OFONO_SERVICE			"org.ofono"
 #define HFP_AUDIO_MANAGER_PATH		"/"
 #define HFP_AUDIO_MANAGER_INTERFACE	OFONO_SERVICE ".HandsfreeAudioManager"
@@ -50,47 +57,257 @@
 /* DBus related */
 static GMainLoop *main_loop = NULL;
 static DBusConnection *conn;
-static GSList *hcons = NULL;
+static GSList *threads = NULL;
 
 static gboolean option_nocvsd = FALSE;
 static gboolean option_nomsbc = FALSE;
+static gboolean option_server = FALSE;
+static gboolean option_defer = FALSE;
+static gchar *option_client_addr = NULL;
 
-struct hfp_audio_conn {
+struct hfp_audio_thread {
 	unsigned char codec;
-	int watch;
+	int fd;
+	int running;
+	pthread_t thread;
 };
 
-static void hfp_audio_conn_free(struct hfp_audio_conn *hcon)
+static int btstr2ba(const char *str, bdaddr_t *ba)
+{
+	int i;
+
+	for (i = 5; i >= 0; i--, str += 3)
+		ba->b[i] = strtol(str, NULL, 16);
+
+	return 0;
+}
+
+static snd_pcm_t *hfp_audio_pcm_init(snd_pcm_stream_t stream)
+{
+	snd_pcm_t *pcm;
+	DBG("Initializing pcm for %s", (stream == SND_PCM_STREAM_CAPTURE) ?
+			"capture" : "playback");
+
+	if (snd_pcm_open(&pcm, "default", stream, SND_PCM_NONBLOCK) < 0) {
+		DBG("Failed to open pcm");
+		return NULL;
+	}
+
+	/* 8000 khz, 16 bits, 128000 bytes/s, 48 bytes/frame, 6000 fps */
+	if (snd_pcm_set_params(pcm, SND_PCM_FORMAT_S16_LE,
+					SND_PCM_ACCESS_RW_INTERLEAVED,
+					1, 8000, 1, 20000) < 0) {
+		DBG("Failed to set pcm params");
+		snd_pcm_close(pcm);
+		pcm = NULL;
+	}
+
+	return pcm;
+}
+
+static void hfp_audio_thread_free(struct hfp_audio_thread *hcon)
 {
 	DBG("Freeing audio connection %p", hcon);
+	if (!hcon)
+		return;
+
+	hcon->running = 0;
+	if (hcon->thread)
+		pthread_join(hcon->thread, NULL);
 
-	hcons = g_slist_remove(hcons, hcon);
-	g_source_remove(hcon->watch);
+	threads = g_slist_remove(threads, hcon);
 	g_free(hcon);
+	DBG("freed %p", hcon);
 }
 
-static gboolean hfp_audio_cb(GIOChannel *io, GIOCondition cond, gpointer data)
+/* Returns the number of data on sco socket */
+static int hfp_audio_playback(int fd, snd_pcm_t *playback)
 {
-	struct hfp_audio_conn *hcon = data;
-	gsize read;
-	gsize written;
-	char buf[60];
+	char buf[800];
+	snd_pcm_sframes_t frames;
+	int bytes;
+
+	bytes = read(fd, buf, sizeof(buf));
+	if (bytes < 0) {
+		DBG("Failed to read: bytes %d, errno %d", bytes, errno);
+		switch (errno) {
+		case ENOTCONN:
+			return -ENOTCONN;
+		case EAGAIN:
+			return 0;
+		default:
+			return -EINVAL;
+		}
+	}
 
-	if (cond & (G_IO_HUP | G_IO_NVAL | G_IO_ERR))
-		goto fail;
+	frames = snd_pcm_writei(playback, buf, bytes / 2);
+	switch (frames) {
+	case -EPIPE:
+		DBG("Playback underrun");
+		snd_pcm_prepare(playback);
+		return bytes;
+	case -EAGAIN:
+		DBG("??? %d", bytes / 2);
+		return bytes;
+	case -EBADFD:
+	case -ESTRPIPE:
+		return -EINVAL;
+	}
 
-	if (g_io_channel_read_chars(io, buf, sizeof(buf), &read, NULL) !=
-			G_IO_STATUS_NORMAL)
-		goto fail;
+	if (frames < bytes / 2)
+		DBG("played %d < requested %d", (int)frames, bytes / 2);
 
-	g_io_channel_write_chars(io, buf+written, read, &written, NULL);
+	return bytes;
+}
 
-	return TRUE;
+/* Returns the number of data on sco socket */
+static int hfp_audio_capture(int fd, snd_pcm_t *capture, GList **outq, int mtu)
+{
+	snd_pcm_sframes_t frames;
+	gchar *buf;
+
+	buf = g_try_malloc(mtu);
+	if (!buf)
+		return -ENOMEM;
+
+	frames = snd_pcm_readi(capture, buf, mtu / 2);
+	switch (frames) {
+	case -EPIPE:
+		DBG("Capture overrun");
+		snd_pcm_prepare(capture);
+		g_free(buf);
+		return 0;
+	case -EAGAIN:
+		DBG("No data to capture");
+		g_free(buf);
+		return 0;
+	case -EBADFD:
+	case -ESTRPIPE:
+		DBG("Other error");
+		g_free(buf);
+		return -EINVAL;
+	}
+
+	if (frames < mtu / 2)
+		DBG("Small frame: %d", (int) frames);
+
+	if (g_list_length(*outq) > 32)
+		DBG("Too many queued packets");
+
+	*outq = g_list_append(*outq, buf);
+
+	return frames * 2;
+}
+
+static void pop_outq(int fd, GList **outq, int mtu)
+{
+	GList *el;
+
+	el = g_list_first(*outq);
+	if (!el)
+		return;
+
+	*outq = g_list_remove_link(*outq, el);
 
+	if (write(fd, el->data, mtu) < 0)
+		DBG("Failed to write: %d", errno);
+
+	g_free(el->data);
+	g_list_free(el);
+}
+
+static void *thread_func(void *userdata)
+{
+	struct hfp_audio_thread *hcon = userdata;
+	snd_pcm_t *playback, *capture;
+	GList *outq = NULL;
+	struct sco_options  opts;
+	struct pollfd fds[8];
+
+	DBG("thread started");
+
+	capture = hfp_audio_pcm_init(SND_PCM_STREAM_CAPTURE);
+	if (!capture)
+		return NULL;
+
+	playback = hfp_audio_pcm_init(SND_PCM_STREAM_PLAYBACK);
+	if (!playback) {
+		snd_pcm_close(capture);
+		return NULL;
+	}
+
+	/* Force defered setup */
+	if (read(hcon->fd, &opts, sizeof(opts)) < 0)
+		DBG("Defered setup failed: %d (%s)", errno, strerror(errno));
+
+	/* Add SCO options
+	len = sizeof(opts);
+	if (getsockopt(hcon->fd, SOL_SCO, SCO_OPTIONS, &opts, &len) < 0) {
+		DBG("getsockopt failed %d", errno);
+		return NULL;
+	}
+	*/
+	opts.mtu = 48;
+	DBG("mtu %d", opts.mtu);
+
+	while (hcon->running) {
+		/* Queue alsa captured data (snd_pcm_poll_descriptors failed) */
+		if (hfp_audio_capture(hcon->fd, capture, &outq, opts.mtu) < 0) {
+			DBG("Failed to capture");
+			break;
+		}
+
+		memset(fds, 0, sizeof(fds));
+		fds[0].fd = hcon->fd;
+		fds[0].events = POLLIN | POLLERR | POLLHUP | POLLNVAL;
+		if (poll(fds, 1, 200) == 0)
+			continue;
+
+		if (fds[0].revents & (POLLERR | POLLHUP | POLLNVAL)) {
+			DBG("POLLERR | POLLHUP | POLLNVAL triggered");
+			break;
+		}
+
+		if (!fds[0].revents & POLLIN)
+			continue;
+
+		if (hfp_audio_playback(hcon->fd, playback) < 0) {
+			DBG("POLLIN triggered, but read error");
+			break;
+		}
+
+		/* Dequeue in sync with readings */
+		pop_outq(hcon->fd, &outq, opts.mtu);
+	}
+
+	DBG("thread terminating");
+	snd_pcm_close(playback);
+	snd_pcm_close(capture);
+	return NULL;
+}
+
+static int new_connection(int fd, int codec)
+{
+	struct hfp_audio_thread *hcon;
+	DBG("New connection: fd=%d codec=%d", fd, codec);
+	hcon = g_try_malloc0(sizeof(struct hfp_audio_thread));
+	if (hcon == NULL)
+		return -ENOMEM;
+
+	hcon->fd = fd;
+	hcon->codec = codec;
+	hcon->running = 1;
+
+	if (pthread_create(&hcon->thread, NULL, thread_func, hcon) < 0)
+		goto fail;
+
+	/* FIXME thread is not detached until we quit */
+
+	threads = g_slist_prepend(threads, hcon);
+	return 0;
 fail:
-	DBG("Disconnected");
-	hfp_audio_conn_free(hcon);
-	return FALSE;
+	hfp_audio_thread_free(hcon);
+	return -EINVAL;
 }
 
 static DBusMessage *agent_newconnection(DBusConnection *conn, DBusMessage *msg,
@@ -99,8 +316,7 @@ static DBusMessage *agent_newconnection(DBusConnection *conn, DBusMessage *msg,
 	const char *card;
 	int fd;
 	unsigned char codec;
-	GIOChannel *io;
-	struct hfp_audio_conn *hcon;
+	DBusMessage *reply;
 
 	DBG("New connection");
 
@@ -112,30 +328,34 @@ static DBusMessage *agent_newconnection(DBusConnection *conn, DBusMessage *msg,
 				HFP_AUDIO_AGENT_INTERFACE ".InvalidArguments",
 				"Invalid arguments");
 
-	DBG("New connection: card=%s fd=%d codec=%d", card, fd, codec);
+	reply = dbus_message_new_method_return(msg);
+	if (!reply)
+		goto fail;
 
-	io = g_io_channel_unix_new(fd);
+	if (new_connection(fd, codec) >= 0)
+		return reply;
 
-	hcon = g_try_malloc0(sizeof(struct hfp_audio_conn));
-	if (hcon == NULL)
-		return NULL;
+	dbus_message_unref(reply);
 
-	hcon->codec = codec;
-	hcon->watch = g_io_add_watch(io, G_IO_IN, hfp_audio_cb, hcon);
-	hcons = g_slist_prepend(hcons, hcon);
+fail:
+	close(fd);
 
-	return dbus_message_new_method_return(msg);
+	return g_dbus_create_error(msg,
+			HFP_AUDIO_AGENT_INTERFACE ".Failed", "Failed to start");
 }
 
 static DBusMessage *agent_release(DBusConnection *conn, DBusMessage *msg,
 					void *data)
 {
 	DBG("HFP audio agent released");
+	/* agent will be registered on next oFono startup */
 	return dbus_message_new_method_return(msg);
 }
 
 static const GDBusMethodTable agent_methods[] = {
-	{ GDBUS_METHOD("NewConnection", NULL, NULL, agent_newconnection) },
+	{ GDBUS_METHOD("NewConnection",
+		GDBUS_ARGS({ "path", "o" }, { "fd", "h" }, { "codec", "y" }),
+		NULL, agent_newconnection) },
 	{ GDBUS_METHOD("Release", NULL, NULL, agent_release) },
 	{ },
 };
@@ -167,7 +387,7 @@ static void hfp_audio_agent_register(DBusConnection *conn)
 	const unsigned char *pcodecs = codecs;
 	int ncodecs = 0;
 
-	DBG("Registering audio agent");
+	DBG("Registering audio agent in oFono");
 
 	msg = dbus_message_new_method_call(OFONO_SERVICE,
 						HFP_AUDIO_MANAGER_PATH,
@@ -196,20 +416,147 @@ static void hfp_audio_agent_register(DBusConnection *conn)
 
 	dbus_message_unref(msg);
 
-	if (call == NULL) {
-		DBG("Unable to register agent");
-		return;
-	}
-
 	dbus_pending_call_set_notify(call, hfp_audio_agent_register_reply,
 						NULL, NULL);
 
 	dbus_pending_call_unref(call);
 }
 
+static gboolean sco_accept_cb(GIOChannel *io, GIOCondition cond, gpointer data)
+{
+	struct sockaddr_sco addr;
+	socklen_t optlen;
+	int sk, nsk;
+
+	if (cond & (G_IO_HUP | G_IO_NVAL | G_IO_ERR))
+		goto fail;
+
+	DBG("Incoming connection");
+	sk = g_io_channel_unix_get_fd(io);
+	nsk = accept(sk, (struct sockaddr *) &addr, &optlen);
+
+	if (nsk > 0)
+		new_connection(nsk, HFP_AUDIO_CVSD);
+
+	return TRUE;
+
+fail:
+	DBG("Server disconnected");
+	return FALSE;
+}
+
+static gboolean sco_connect_cb(GIOChannel *io, GIOCondition cond, gpointer data)
+{
+	int sk;
+
+	if (cond & (G_IO_HUP | G_IO_NVAL | G_IO_ERR))
+		goto fail;
+
+	DBG("Connected");
+	sk = g_io_channel_unix_get_fd(io);
+	if (sk > 0)
+		new_connection(sk, HFP_AUDIO_CVSD);
+
+	return FALSE;
+
+fail:
+	DBG("Connection failed");
+	return FALSE;
+}
+
+static int sco_listen_watch()
+{
+	struct sockaddr_sco saddr;
+	int sk;
+	GIOChannel *io;
+
+	/* Create socket */
+	sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_SCO);
+	if (sk < 0) {
+		DBG("Can't create socket: %s (%d)", strerror(errno), errno);
+		return -1;
+	}
+
+	/* Bind to local address */
+	memset(&saddr, 0, sizeof(saddr));
+	saddr.sco_family = AF_BLUETOOTH;
+
+	if (bind(sk, (struct sockaddr *) &saddr, sizeof(saddr)) < 0) {
+		DBG("Can't bind socket: %s (%d)", strerror(errno), errno);
+		goto error;
+	}
+
+	/* Enable deferred setup */
+	if (option_defer && setsockopt(sk, SOL_BLUETOOTH, BT_DEFER_SETUP,
+				&option_defer, sizeof(option_defer)) < 0) {
+		DBG("Can't defer setup : %s (%d)", strerror(errno), errno);
+		goto error;
+	}
+
+	/* Listen for connections */
+	if (listen(sk, 10)) {
+		DBG("Can not listen socket: %s (%d)", strerror(errno), errno);
+		goto error;
+	}
+
+	DBG("Waiting for connection ...");
+	io = g_io_channel_unix_new(sk);
+	if (!io)
+		goto error;
+
+	return g_io_add_watch(io, G_IO_IN, sco_accept_cb, NULL);
+
+error:
+	close(sk);
+	return -1;
+}
+
+static int sco_connect_watch()
+{
+	struct sockaddr_sco saddr;
+	int sk;
+	GIOChannel *io;
+
+	/* Create socket */
+	sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_SCO);
+	if (sk < 0) {
+		DBG("Can't create socket: %s (%d)", strerror(errno), errno);
+		return -1;
+	}
+
+	/* Bind to local address */
+	memset(&saddr, 0, sizeof(saddr));
+	saddr.sco_family = AF_BLUETOOTH;
+
+	if (bind(sk, (struct sockaddr *) &saddr, sizeof(saddr)) < 0) {
+		DBG("Can't bind socket: %s (%d)", strerror(errno), errno);
+		goto error;
+	}
+
+	/* Connect to remote address */
+	memset(&saddr, 0, sizeof(saddr));
+	saddr.sco_family = AF_BLUETOOTH;
+	btstr2ba(option_client_addr, &saddr.sco_bdaddr);
+	if (connect(sk, (struct sockaddr *) &saddr, sizeof(saddr))) {
+		DBG("Can not connect socket: %s (%d)", strerror(errno), errno);
+		goto error;
+	}
+
+	DBG("Connecting to %s...", option_client_addr);
+	io = g_io_channel_unix_new(sk);
+	if (!io)
+		goto error;
+
+	return g_io_add_watch(io, G_IO_IN|G_IO_OUT, sco_connect_cb, NULL);
+
+error:
+	close(sk);
+	return -1;
+}
+
 static void hfp_audio_agent_create(DBusConnection *conn)
 {
-	DBG("Creating audio agent");
+	DBG("Registering audio agent on DBUS");
 
 	if (!g_dbus_register_interface(conn, HFP_AUDIO_AGENT_PATH,
 					HFP_AUDIO_AGENT_INTERFACE,
@@ -222,7 +569,7 @@ static void hfp_audio_agent_create(DBusConnection *conn)
 
 static void hfp_audio_agent_destroy(DBusConnection *conn)
 {
-	DBG("Destroying audio agent");
+	DBG("Unregistering audio agent on DBUS");
 
 	g_dbus_unregister_interface(conn, HFP_AUDIO_AGENT_PATH,
 						HFP_AUDIO_AGENT_INTERFACE);
@@ -259,6 +606,12 @@ static GOptionEntry options[] = {
 				"Disable CVSD support" },
 	{ "nomsbc", 'm', 0, G_OPTION_ARG_NONE, &option_nomsbc,
 				"Disable MSBC support" },
+	{ "defer", 'd', 0, G_OPTION_ARG_NONE, &option_defer,
+				"Defered socket support" },
+	{ "server", 'S', 0, G_OPTION_ARG_NONE, &option_server,
+				"Server" },
+	{ "client", 'C', 1, G_OPTION_ARG_STRING, &option_client_addr,
+				"Client addr" },
 	{ NULL },
 };
 
@@ -293,6 +646,11 @@ int main(int argc, char **argv)
 
 	dbus_error_init(&err);
 
+	memset(&sa, 0, sizeof(sa));
+	sa.sa_handler = sig_term;
+	sigaction(SIGINT, &sa, NULL);
+	sigaction(SIGTERM, &sa, NULL);
+
 	conn = g_dbus_setup_bus(DBUS_BUS_SYSTEM, NULL, &err);
 	if (conn == NULL) {
 		if (dbus_error_is_set(&err) == TRUE) {
@@ -305,24 +663,28 @@ int main(int argc, char **argv)
 
 	g_dbus_set_disconnect_function(conn, disconnect_callback, NULL, NULL);
 
-	memset(&sa, 0, sizeof(sa));
-	sa.sa_handler = sig_term;
-	sigaction(SIGINT, &sa, NULL);
-	sigaction(SIGTERM, &sa, NULL);
-
-	hfp_audio_agent_create(conn);
-
-	watch = g_dbus_add_service_watch(conn, OFONO_SERVICE,
+	if (option_server) {
+		watch = sco_listen_watch();
+	} else if (option_client_addr != NULL) {
+		watch = sco_connect_watch();
+	} else {
+		hfp_audio_agent_create(conn);
+		watch = g_dbus_add_service_watch(conn, OFONO_SERVICE,
 				ofono_connect, ofono_disconnect, NULL, NULL);
-
+	}
 	g_main_loop_run(main_loop);
 
-	g_dbus_remove_watch(conn, watch);
-
-	while (hcons != NULL)
-		hfp_audio_conn_free(hcons->data);
+	while (threads != NULL)
+		hfp_audio_thread_free(threads->data);
 
-	hfp_audio_agent_destroy(conn);
+	if (option_server) {
+		g_source_remove(watch);
+	} else if (option_client_addr != NULL) {
+		g_source_remove(watch);
+	} else {
+		g_dbus_remove_watch(conn, watch);
+		hfp_audio_agent_destroy(conn);
+	}
 
 	dbus_connection_unref(conn);
 
-- 
1.7.9.5

[PATCH v4 6/9] handsfree-audio: Add SBC dependency

[=?unknown-8bit?q?Fr=C3=A9d=C3=A9ric?= Dalleau]

[-- Attachment #1: Type: text/plain, Size: 570 bytes --]

---
 configure.ac |    5 +++++
 1 file changed, 5 insertions(+)

diff --git a/configure.ac b/configure.ac
index 9be3ebf..8b9d78a 100644
--- a/configure.ac
+++ b/configure.ac
@@ -126,6 +126,11 @@ if (test "${enable_tools}" = "yes"); then
 				AC_MSG_ERROR(Alsa library is required))
 	AC_SUBST(ALSA_CFLAGS)
 	AC_SUBST(ALSA_LIBS)
+
+	PKG_CHECK_MODULES(SBC, sbc >= 1.1, dummy=yes,
+				AC_MSG_ERROR(SBC library v. 1.1 is required))
+	AC_SUBST(SBC_CFLAGS)
+	AC_SUBST(SBC_LIBS)
 fi
 AM_CONDITIONAL(TOOLS, test "${enable_tools}" = "yes")
 
-- 
1.7.9.5

[PATCH v4 7/9] handsfree-audio: Link tool with SBC library

[=?unknown-8bit?q?Fr=C3=A9d=C3=A9ric?= Dalleau]

[-- Attachment #1: Type: text/plain, Size: 712 bytes --]

---
 Makefile.am |    2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Makefile.am b/Makefile.am
index 2e5fa4c..917ad38 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -728,7 +728,7 @@ tools_huawei_audio_SOURCES = $(gdbus_sources) tools/huawei-audio.c
 tools_huawei_audio_LDADD = @GLIB_LIBS@ @DBUS_LIBS@
 
 tools_handsfree_audio_SOURCES = $(gdbus_sources) tools/handsfree-audio.c
-tools_handsfree_audio_LDADD = @GLIB_LIBS@ @DBUS_LIBS@ @ALSA_LIBS@ -lpthread
+tools_handsfree_audio_LDADD = @GLIB_LIBS@ @DBUS_LIBS@ @ALSA_LIBS@ @SBC_LIBS@ -lpthread
 
 tools_auto_enable_SOURCES = $(gdbus_sources) tools/auto-enable.c
 tools_auto_enable_LDADD = @GLIB_LIBS@ @DBUS_LIBS@
-- 
1.7.9.5

[PATCH v4 8/9] handsfree-audio: mSBC support

[=?unknown-8bit?q?Fr=C3=A9d=C3=A9ric?= Dalleau]

[-- Attachment #1: Type: text/plain, Size: 21402 bytes --]

---
 tools/handsfree-audio.c |  513 ++++++++++++++++++++++++++++++++++++-----------
 1 file changed, 396 insertions(+), 117 deletions(-)

diff --git a/tools/handsfree-audio.c b/tools/handsfree-audio.c
index 9a4f85b..e8725b7 100644
--- a/tools/handsfree-audio.c
+++ b/tools/handsfree-audio.c
@@ -40,6 +40,7 @@
 #include <pthread.h>
 #include <bluetooth/bluetooth.h>
 #include <bluetooth/sco.h>
+#include <sbc/sbc.h>
 
 #define OFONO_SERVICE			"org.ofono"
 #define HFP_AUDIO_MANAGER_PATH		"/"
@@ -65,11 +66,50 @@ static gboolean option_server = FALSE;
 static gboolean option_defer = FALSE;
 static gchar *option_client_addr = NULL;
 
-struct hfp_audio_thread {
-	unsigned char codec;
+static const char sntable[4] = { 0x08, 0x38, 0xC8, 0xF8 };
+static const int audio_rates[3] = { 0, 8000, 16000 };
+
+struct msbc_parser {
+	int len;
+	uint8_t buffer[60];
+};
+
+struct msbc_codec {
+	sbc_t sbcenc; /* Coder data */
+	char *ebuffer; /* Codec transfer buffer */
+	size_t ebuffer_size; /* Size of the buffer */
+	size_t ebuffer_start; /* start of encoding data */
+	size_t ebuffer_end; /* end of encoding data */
+
+	struct msbc_parser parser; /* mSBC parser for concatenating frames */
+	sbc_t sbcdec; /* Decoder data */
+
+	size_t msbc_frame_size;
+	size_t decoded_frame_size;
+};
+
+struct hfp_thread {
 	int fd;
 	int running;
 	pthread_t thread;
+	GSList *outq;
+
+	int rate;
+	char *capture_buffer;
+	int capture_size;
+	int captured;
+	int mtu;
+	int (*init)(struct hfp_thread *);
+	int (*free)(struct hfp_thread *);
+	int (*encode)(struct hfp_thread *, char *data, int len);
+	int (*decode)(struct hfp_thread *, char *data, int len, char *out,
+								int outlen);
+	struct msbc_codec msbc;
+};
+
+struct hack_sco_options {
+	uint16_t mtu;
+	uint16_t voice_setting;
 };
 
 static int btstr2ba(const char *str, bdaddr_t *ba)
@@ -82,21 +122,232 @@ static int btstr2ba(const char *str, bdaddr_t *ba)
 	return 0;
 }
 
-static snd_pcm_t *hfp_audio_pcm_init(snd_pcm_stream_t stream)
+static void msbc_parser_reset(struct msbc_parser *p)
+{
+	p->len = 0;
+}
+
+static int msbc_state_machine(struct msbc_parser *p, uint8_t byte)
+{
+	switch (p->len) {
+	case 0:
+		if (byte == 0x01)
+			goto copy;
+		return 0;
+	case 1:
+		if (byte == 0x08 || byte == 0x38 || byte == 0xC8
+				|| byte == 0xF8)
+			goto copy;
+		break;
+	case 2:
+		if (byte == 0xAD)
+			goto copy;
+		break;
+	case 3:
+		if (byte == 0x00)
+			goto copy;
+		break;
+	case 4:
+		if (byte == 0x00)
+			goto copy;
+		break;
+	default:
+		goto copy;
+	}
+
+	p->len = 0;
+	return 0;
+
+copy:
+	p->buffer[p->len] = byte;
+	p->len++;
+
+	return p->len;
+}
+
+static size_t msbc_parse(sbc_t *sbcdec, struct msbc_parser *p, char *data,
+		int len, char *out, int outlen, int *bytes)
+{
+	size_t totalwritten = 0;
+	size_t written = 0;
+	int i;
+	*bytes = 0;
+
+	for (i = 0; i < len; i++) {
+		if (msbc_state_machine(p, data[i]) == 60) {
+			int decoded;
+			decoded = sbc_decode(sbcdec, p->buffer + 2,
+					p->len - 2 - 1, out, outlen, &written);
+			if (decoded > 0) {
+				totalwritten += written;
+				*bytes += decoded;
+			} else {
+				DBG("Error while decoding: %d", decoded);
+			}
+			msbc_parser_reset(p);
+		}
+	}
+	return totalwritten;
+}
+
+static int hfp_audio_cvsd_init(struct hfp_thread *thread)
+{
+	thread->rate = 8000;
+	thread->capture_size = 48;
+	return 0;
+}
+
+static int hfp_audio_cvsd_free(struct hfp_thread *thread)
+{
+	return 0;
+}
+
+static int hfp_audio_cvsd_encode(struct hfp_thread *thread, char *data,
+						int len)
+{
+	char *qbuf;
+
+	if (len > thread->mtu) {
+		DBG("Mtu too small: len %d, mtu %d", len, thread->mtu);
+		return -EINVAL;
+	}
+
+	qbuf = g_try_malloc(thread->mtu);
+	if (!qbuf)
+		return -ENOMEM;
+
+	memcpy(qbuf, data, len);
+
+	thread->outq = g_slist_insert(thread->outq, qbuf, -1);
+
+	return len;
+}
+
+static int hfp_audio_cvsd_decode(struct hfp_thread *thread, char *data,
+						int len, char *out, int outlen)
+{
+	int size = (len < outlen) ? len : outlen;
+	memcpy(out, data, size);
+	return size;
+}
+
+/* Run from IO thread */
+static int hfp_audio_msbc_init(struct hfp_thread *thread)
+{
+	struct msbc_codec *codec = &thread->msbc;
+	struct hack_sco_options opts;
+
+	thread->rate = 16000;
+	thread->capture_size = 240; /* decoded mSBC frame */
+
+	memset(&opts, 0, sizeof(opts));
+	opts.voice_setting = 0X0003;
+	if (setsockopt(thread->fd, SOL_SCO, SCO_OPTIONS, &opts, sizeof(opts))
+			< 0) {
+		DBG("Can't set transparent mode: %s (%d)",
+				strerror(errno), errno);
+		return -ENOTSUP;
+	}
+
+	sbc_init_msbc(&codec->sbcenc, 0);
+	sbc_init_msbc(&codec->sbcdec, 0);
+
+	codec->msbc_frame_size = 2 + sbc_get_frame_length(&codec->sbcenc) + 1;
+	codec->decoded_frame_size = sbc_get_codesize(&codec->sbcenc);
+	msbc_parser_reset(&codec->parser);
+
+	/* 5 * 48 == 10 * 24 == 4 * 60 */
+	codec->ebuffer_size = codec->msbc_frame_size * 4;
+	codec->ebuffer = g_try_malloc(codec->ebuffer_size);
+	codec->ebuffer_start = 0;
+	codec->ebuffer_end = 0;
+
+	DBG("codec->msbc_frame_size %d", (int) codec->msbc_frame_size);
+	DBG("codec->ebuffer_size %d", (int) codec->ebuffer_size);
+	DBG("codec->decoded_frame_size %d", (int) codec->decoded_frame_size);
+	return 0;
+}
+
+/* Run from IO thread */
+static int hfp_audio_msbc_free(struct hfp_thread *thread)
+{
+	struct msbc_codec *codec = &thread->msbc;
+
+	g_free(codec->ebuffer);
+	sbc_finish(&codec->sbcenc);
+	sbc_finish(&codec->sbcdec);
+
+	return 0;
+}
+
+/* Run from IO thread */
+static int hfp_audio_msbc_encode(struct hfp_thread *thread, char *data, int len)
+{
+	struct msbc_codec *codec = &thread->msbc;
+	char *h2 = codec->ebuffer + codec->ebuffer_end;
+	static int sn = 0;
+	int written = 0;
+	char *qbuf;
+
+	h2[0] = 0x01;
+	h2[1] = sntable[sn];
+	h2[59] = 0xff;
+	sn = (sn + 1) % 4;
+
+	sbc_encode(&codec->sbcenc, data, len,
+			codec->ebuffer + codec->ebuffer_end + 2,
+			codec->ebuffer_size - codec->ebuffer_end - 2,
+			(ssize_t *) &written);
+
+	written += 2 /* H2 */ + 1 /* 0xff */;
+	codec->ebuffer_end += written;
+
+	/* Split into MTU sized chunks */
+	while (codec->ebuffer_start + thread->mtu <= codec->ebuffer_end) {
+		qbuf = g_try_malloc(thread->mtu);
+		if (!qbuf)
+			return -ENOMEM;
+
+		/* DBG("Enqueuing %d bytes", thread->mtu); */
+		memcpy(qbuf, codec->ebuffer + codec->ebuffer_start, thread->mtu);
+
+		thread->outq = g_slist_insert(thread->outq, qbuf, -1);
+
+		codec->ebuffer_start += thread->mtu;
+		if (codec->ebuffer_start >= codec->ebuffer_end)
+			codec->ebuffer_start = codec->ebuffer_end = 0;
+	}
+
+	return 0;
+}
+
+/* Run from IO thread */
+static int hfp_audio_msbc_decode(struct hfp_thread *thread, char *data,
+						int len, char *out, int outlen)
+{
+	struct msbc_codec *codec = &thread->msbc;
+	int written, decoded;
+
+	written = msbc_parse(&codec->sbcdec, &codec->parser, data, len, out,
+			outlen, &decoded);
+
+	return written;
+}
+
+static snd_pcm_t *hfp_audio_pcm_init(snd_pcm_stream_t stream, int rate)
 {
 	snd_pcm_t *pcm;
 	DBG("Initializing pcm for %s", (stream == SND_PCM_STREAM_CAPTURE) ?
-			"capture" : "playback");
+							"capture" : "playback");
 
 	if (snd_pcm_open(&pcm, "default", stream, SND_PCM_NONBLOCK) < 0) {
 		DBG("Failed to open pcm");
 		return NULL;
 	}
 
-	/* 8000 khz, 16 bits, 128000 bytes/s, 48 bytes/frame, 6000 fps */
+	/* 16 bits */
 	if (snd_pcm_set_params(pcm, SND_PCM_FORMAT_S16_LE,
-					SND_PCM_ACCESS_RW_INTERLEAVED,
-					1, 8000, 1, 20000) < 0) {
+			SND_PCM_ACCESS_RW_INTERLEAVED, 1, rate, 1, 120000) < 0) {
 		DBG("Failed to set pcm params");
 		snd_pcm_close(pcm);
 		pcm = NULL;
@@ -105,29 +356,35 @@ static snd_pcm_t *hfp_audio_pcm_init(snd_pcm_stream_t stream)
 	return pcm;
 }
 
-static void hfp_audio_thread_free(struct hfp_audio_thread *hcon)
+static void hfp_audio_thread_free(struct hfp_thread *thread)
 {
-	DBG("Freeing audio connection %p", hcon);
-	if (!hcon)
+	DBG("Freeing audio connection %p", thread);
+	if (!thread)
 		return;
 
-	hcon->running = 0;
-	if (hcon->thread)
-		pthread_join(hcon->thread, NULL);
+	thread->running = 0;
+	if (thread->thread)
+		pthread_join(thread->thread, NULL);
 
-	threads = g_slist_remove(threads, hcon);
-	g_free(hcon);
-	DBG("freed %p", hcon);
+	if (shutdown(thread->fd, SHUT_RDWR) < 0)
+			DBG("Failed to shutdown socket");
+	if (close(thread->fd) < 0)
+		DBG("Failed to close socket");
+
+	threads = g_slist_remove(threads, thread);
+	g_free(thread);
+	DBG("freed %p", thread);
 }
 
 /* Returns the number of data on sco socket */
-static int hfp_audio_playback(int fd, snd_pcm_t *playback)
+static int hfp_audio_playback(struct hfp_thread *thread,
+		snd_pcm_t *playback)
 {
-	char buf[800];
+	char buf[512], out[512];
+	int bytes, outlen;
 	snd_pcm_sframes_t frames;
-	int bytes;
 
-	bytes = read(fd, buf, sizeof(buf));
+	bytes = read(thread->fd, buf, sizeof(buf));
 	if (bytes < 0) {
 		DBG("Failed to read: bytes %d, errno %d", bytes, errno);
 		switch (errno) {
@@ -140,178 +397,201 @@ static int hfp_audio_playback(int fd, snd_pcm_t *playback)
 		}
 	}
 
-	frames = snd_pcm_writei(playback, buf, bytes / 2);
+	/* DBG("received %d bytes", bytes); */
+	outlen = thread->decode(thread, buf, bytes, out, sizeof(out));
+
+	frames = snd_pcm_writei(playback, out, outlen / 2);
 	switch (frames) {
 	case -EPIPE:
-		DBG("Playback underrun");
 		snd_pcm_prepare(playback);
 		return bytes;
 	case -EAGAIN:
-		DBG("??? %d", bytes / 2);
 		return bytes;
 	case -EBADFD:
 	case -ESTRPIPE:
 		return -EINVAL;
 	}
 
-	if (frames < bytes / 2)
-		DBG("played %d < requested %d", (int)frames, bytes / 2);
+	if (frames < outlen / 2)
+		DBG("played %d < requested %d", (int)frames, outlen / 2);
 
 	return bytes;
 }
 
 /* Returns the number of data on sco socket */
-static int hfp_audio_capture(int fd, snd_pcm_t *capture, GList **outq, int mtu)
+static int hfp_audio_capture(struct hfp_thread *thread, snd_pcm_t *capture)
 {
 	snd_pcm_sframes_t frames;
-	gchar *buf;
-
-	buf = g_try_malloc(mtu);
-	if (!buf)
-		return -ENOMEM;
 
-	frames = snd_pcm_readi(capture, buf, mtu / 2);
+	frames = snd_pcm_readi(capture, thread->capture_buffer+thread->captured,
+				(thread->capture_size - thread->captured) / 2);
 	switch (frames) {
 	case -EPIPE:
-		DBG("Capture overrun");
 		snd_pcm_prepare(capture);
-		g_free(buf);
 		return 0;
 	case -EAGAIN:
-		DBG("No data to capture");
-		g_free(buf);
 		return 0;
 	case -EBADFD:
 	case -ESTRPIPE:
-		DBG("Other error");
-		g_free(buf);
+		DBG("Other error %s (%d)", strerror(frames), (int) frames);
 		return -EINVAL;
 	}
 
-	if (frames < mtu / 2)
-		DBG("Small frame: %d", (int) frames);
+	thread->captured += frames * 2;
+	if (thread->captured < thread->capture_size)
+		return frames * 2;
 
-	if (g_list_length(*outq) > 32)
-		DBG("Too many queued packets");
-
-	*outq = g_list_append(*outq, buf);
+	/* DBG("Encoding %d bytes", (int) thread->captured); */
+	thread->encode(thread, thread->capture_buffer, thread->captured);
+	thread->captured = 0;
 
 	return frames * 2;
 }
 
-static void pop_outq(int fd, GList **outq, int mtu)
+static void pop_outq(struct hfp_thread *thread)
 {
-	GList *el;
-
-	el = g_list_first(*outq);
-	if (!el)
-		return;
+	char *qbuf;
 
-	*outq = g_list_remove_link(*outq, el);
+	while (thread->outq != NULL) {
+		qbuf = thread->outq->data;
+		thread->outq = g_slist_remove(thread->outq, qbuf);
 
-	if (write(fd, el->data, mtu) < 0)
-		DBG("Failed to write: %d", errno);
+		if (write(thread->fd, qbuf, thread->mtu) < 0)
+			DBG("Failed to write: %d", errno);
 
-	g_free(el->data);
-	g_list_free(el);
+		g_free(qbuf);
+	}
 }
 
 static void *thread_func(void *userdata)
 {
-	struct hfp_audio_thread *hcon = userdata;
+	struct hfp_thread *thread = userdata;
 	snd_pcm_t *playback, *capture;
-	GList *outq = NULL;
-	struct sco_options  opts;
+	struct sco_options opts;
 	struct pollfd fds[8];
 
-	DBG("thread started");
+	DBG("thread started: rate %d", thread->rate);
+
+	if (thread->init(thread) < 0)
+		return NULL;
 
-	capture = hfp_audio_pcm_init(SND_PCM_STREAM_CAPTURE);
+	capture = hfp_audio_pcm_init(SND_PCM_STREAM_CAPTURE, thread->rate);
 	if (!capture)
 		return NULL;
 
-	playback = hfp_audio_pcm_init(SND_PCM_STREAM_PLAYBACK);
+	playback = hfp_audio_pcm_init(SND_PCM_STREAM_PLAYBACK, thread->rate);
 	if (!playback) {
 		snd_pcm_close(capture);
 		return NULL;
 	}
 
+	thread->capture_buffer = g_try_malloc(thread->capture_size);
+	if (!thread->capture_buffer) {
+		snd_pcm_close(capture);
+		snd_pcm_close(playback);
+		return NULL;
+	}
+
 	/* Force defered setup */
-	if (read(hcon->fd, &opts, sizeof(opts)) < 0)
+	if (recv(thread->fd, NULL, 0, 0) < 0)
 		DBG("Defered setup failed: %d (%s)", errno, strerror(errno));
 
 	/* Add SCO options
-	len = sizeof(opts);
-	if (getsockopt(hcon->fd, SOL_SCO, SCO_OPTIONS, &opts, &len) < 0) {
-		DBG("getsockopt failed %d", errno);
-		return NULL;
-	}
-	*/
+	 len = sizeof(opts);
+	 if (getsockopt(thread->fd, SOL_SCO, SCO_OPTIONS, &opts, &len) < 0) {
+	 DBG("getsockopt failed %d", errno);
+	 return NULL;
+	 }
+	 */
 	opts.mtu = 48;
-	DBG("mtu %d", opts.mtu);
+	thread->mtu = opts.mtu;
+	DBG("thread->mtu %d", thread->mtu);
 
-	while (hcon->running) {
+	while (thread->running) {
 		/* Queue alsa captured data (snd_pcm_poll_descriptors failed) */
-		if (hfp_audio_capture(hcon->fd, capture, &outq, opts.mtu) < 0) {
+		if (hfp_audio_capture(thread, capture) < 0) {
 			DBG("Failed to capture");
 			break;
 		}
 
 		memset(fds, 0, sizeof(fds));
-		fds[0].fd = hcon->fd;
+		fds[0].fd = thread->fd;
 		fds[0].events = POLLIN | POLLERR | POLLHUP | POLLNVAL;
 		if (poll(fds, 1, 200) == 0)
 			continue;
 
 		if (fds[0].revents & (POLLERR | POLLHUP | POLLNVAL)) {
-			DBG("POLLERR | POLLHUP | POLLNVAL triggered");
+			DBG("POLLERR | POLLHUP | POLLNVAL triggered (%d)",
+					fds[0].revents);
 			break;
 		}
 
 		if (!fds[0].revents & POLLIN)
 			continue;
 
-		if (hfp_audio_playback(hcon->fd, playback) < 0) {
+		if (hfp_audio_playback(thread, playback) < 0) {
 			DBG("POLLIN triggered, but read error");
 			break;
 		}
 
+		/* DBG("Received and send"); */
+
 		/* Dequeue in sync with readings */
-		pop_outq(hcon->fd, &outq, opts.mtu);
+		pop_outq(thread);
 	}
 
 	DBG("thread terminating");
+	g_slist_free_full(thread->outq, g_free);
+	g_free(thread->capture_buffer);
 	snd_pcm_close(playback);
 	snd_pcm_close(capture);
+
+	thread->free(thread);
+
 	return NULL;
 }
 
 static int new_connection(int fd, int codec)
 {
-	struct hfp_audio_thread *hcon;
+	struct hfp_thread *thread;
+
 	DBG("New connection: fd=%d codec=%d", fd, codec);
-	hcon = g_try_malloc0(sizeof(struct hfp_audio_thread));
-	if (hcon == NULL)
+	thread = g_try_malloc0(sizeof(struct hfp_thread));
+	if (thread == NULL)
 		return -ENOMEM;
 
-	hcon->fd = fd;
-	hcon->codec = codec;
-	hcon->running = 1;
+	thread->fd = fd;
+	thread->running = 1;
+
+	switch (codec) {
+	case HFP_AUDIO_CVSD:
+		thread->init = hfp_audio_cvsd_init;
+		thread->free = hfp_audio_cvsd_free;
+		thread->decode = hfp_audio_cvsd_decode;
+		thread->encode = hfp_audio_cvsd_encode;
+		break;
+	case HFP_AUDIO_MSBC:
+		thread->rate = 16000;
+		thread->init = hfp_audio_msbc_init;
+		thread->free = hfp_audio_msbc_free;
+		thread->decode = hfp_audio_msbc_decode;
+		thread->encode = hfp_audio_msbc_encode;
+		break;
+	}
 
-	if (pthread_create(&hcon->thread, NULL, thread_func, hcon) < 0)
-		goto fail;
+	if (pthread_create(&thread->thread, NULL, thread_func, thread) < 0) {
+		hfp_audio_thread_free(thread);
+		return -EINVAL;
+	}
 
 	/* FIXME thread is not detached until we quit */
 
-	threads = g_slist_prepend(threads, hcon);
+	threads = g_slist_prepend(threads, thread);
 	return 0;
-fail:
-	hfp_audio_thread_free(hcon);
-	return -EINVAL;
 }
 
 static DBusMessage *agent_newconnection(DBusConnection *conn, DBusMessage *msg,
-					void *data)
+								void *data)
 {
 	const char *card;
 	int fd;
@@ -321,9 +601,8 @@ static DBusMessage *agent_newconnection(DBusConnection *conn, DBusMessage *msg,
 	DBG("New connection");
 
 	if (dbus_message_get_args(msg, NULL, DBUS_TYPE_OBJECT_PATH, &card,
-						DBUS_TYPE_UNIX_FD, &fd,
-						DBUS_TYPE_BYTE, &codec,
-						DBUS_TYPE_INVALID) == FALSE)
+			DBUS_TYPE_UNIX_FD, &fd, DBUS_TYPE_BYTE, &codec,
+			DBUS_TYPE_INVALID) == FALSE)
 		return g_dbus_create_error(msg,
 				HFP_AUDIO_AGENT_INTERFACE ".InvalidArguments",
 				"Invalid arguments");
@@ -345,7 +624,7 @@ fail:
 }
 
 static DBusMessage *agent_release(DBusConnection *conn, DBusMessage *msg,
-					void *data)
+		void *data)
 {
 	DBG("HFP audio agent released");
 	/* agent will be registered on next oFono startup */
@@ -368,8 +647,8 @@ static void hfp_audio_agent_register_reply(DBusPendingCall *call, void *data)
 	dbus_error_init(&err);
 
 	if (dbus_set_error_from_message(&err, reply) == TRUE) {
-		DBG("Failed to register audio agent (%s: %s)", err.name,
-								err.message);
+		DBG("Failed to register audio agent (%s: %s)",
+				err.name, err.message);
 		dbus_error_free(&err);
 	} else {
 		DBG("HFP audio agent registered");
@@ -390,9 +669,8 @@ static void hfp_audio_agent_register(DBusConnection *conn)
 	DBG("Registering audio agent in oFono");
 
 	msg = dbus_message_new_method_call(OFONO_SERVICE,
-						HFP_AUDIO_MANAGER_PATH,
-						HFP_AUDIO_MANAGER_INTERFACE,
-						"Register");
+			HFP_AUDIO_MANAGER_PATH, HFP_AUDIO_MANAGER_INTERFACE,
+			"Register");
 	if (msg == NULL) {
 		DBG("Not enough memory");
 		return;
@@ -405,8 +683,8 @@ static void hfp_audio_agent_register(DBusConnection *conn)
 		codecs[ncodecs++] = HFP_AUDIO_MSBC;
 
 	dbus_message_append_args(msg, DBUS_TYPE_OBJECT_PATH, &path,
-					DBUS_TYPE_ARRAY, DBUS_TYPE_BYTE,
-					&pcodecs, ncodecs, DBUS_TYPE_INVALID);
+			DBUS_TYPE_ARRAY, DBUS_TYPE_BYTE, &pcodecs, ncodecs,
+			DBUS_TYPE_INVALID);
 
 	if (!dbus_connection_send_with_reply(conn, msg, &call, -1)) {
 		dbus_message_unref(msg);
@@ -416,8 +694,8 @@ static void hfp_audio_agent_register(DBusConnection *conn)
 
 	dbus_message_unref(msg);
 
-	dbus_pending_call_set_notify(call, hfp_audio_agent_register_reply,
-						NULL, NULL);
+	dbus_pending_call_set_notify(call, hfp_audio_agent_register_reply, NULL,
+			NULL);
 
 	dbus_pending_call_unref(call);
 }
@@ -483,30 +761,32 @@ static int sco_listen_watch()
 
 	if (bind(sk, (struct sockaddr *) &saddr, sizeof(saddr)) < 0) {
 		DBG("Can't bind socket: %s (%d)", strerror(errno), errno);
-		goto error;
+		goto fail;
 	}
 
 	/* Enable deferred setup */
-	if (option_defer && setsockopt(sk, SOL_BLUETOOTH, BT_DEFER_SETUP,
-				&option_defer, sizeof(option_defer)) < 0) {
+	if (option_defer
+			&& setsockopt(sk, SOL_BLUETOOTH, BT_DEFER_SETUP,
+					&option_defer, sizeof(option_defer))
+					< 0) {
 		DBG("Can't defer setup : %s (%d)", strerror(errno), errno);
-		goto error;
+		goto fail;
 	}
 
 	/* Listen for connections */
 	if (listen(sk, 10)) {
 		DBG("Can not listen socket: %s (%d)", strerror(errno), errno);
-		goto error;
+		goto fail;
 	}
 
 	DBG("Waiting for connection ...");
 	io = g_io_channel_unix_new(sk);
 	if (!io)
-		goto error;
+		goto fail;
 
 	return g_io_add_watch(io, G_IO_IN, sco_accept_cb, NULL);
 
-error:
+fail:
 	close(sk);
 	return -1;
 }
@@ -530,7 +810,7 @@ static int sco_connect_watch()
 
 	if (bind(sk, (struct sockaddr *) &saddr, sizeof(saddr)) < 0) {
 		DBG("Can't bind socket: %s (%d)", strerror(errno), errno);
-		goto error;
+		goto fail;
 	}
 
 	/* Connect to remote address */
@@ -539,17 +819,17 @@ static int sco_connect_watch()
 	btstr2ba(option_client_addr, &saddr.sco_bdaddr);
 	if (connect(sk, (struct sockaddr *) &saddr, sizeof(saddr))) {
 		DBG("Can not connect socket: %s (%d)", strerror(errno), errno);
-		goto error;
+		goto fail;
 	}
 
 	DBG("Connecting to %s...", option_client_addr);
 	io = g_io_channel_unix_new(sk);
 	if (!io)
-		goto error;
+		goto fail;
 
-	return g_io_add_watch(io, G_IO_IN|G_IO_OUT, sco_connect_cb, NULL);
+	return g_io_add_watch(io, G_IO_IN | G_IO_OUT, sco_connect_cb, NULL);
 
-error:
+fail:
 	close(sk);
 	return -1;
 }
@@ -559,9 +839,8 @@ static void hfp_audio_agent_create(DBusConnection *conn)
 	DBG("Registering audio agent on DBUS");
 
 	if (!g_dbus_register_interface(conn, HFP_AUDIO_AGENT_PATH,
-					HFP_AUDIO_AGENT_INTERFACE,
-					agent_methods, NULL, NULL,
-					NULL, NULL)) {
+			HFP_AUDIO_AGENT_INTERFACE, agent_methods, NULL, NULL,
+			NULL, NULL)) {
 		DBG("Unable to create local agent");
 		g_main_loop_quit(main_loop);
 	}
@@ -572,7 +851,7 @@ static void hfp_audio_agent_destroy(DBusConnection *conn)
 	DBG("Unregistering audio agent on DBUS");
 
 	g_dbus_unregister_interface(conn, HFP_AUDIO_AGENT_PATH,
-						HFP_AUDIO_AGENT_INTERFACE);
+			HFP_AUDIO_AGENT_INTERFACE);
 }
 
 static void ofono_connect(DBusConnection *conn, void *user_data)
-- 
1.7.9.5

[PATCH v4 9/9] handsfree-audio: Add an option to kill incoming connections

[=?unknown-8bit?q?Fr=C3=A9d=C3=A9ric?= Dalleau]

[-- Attachment #1: Type: text/plain, Size: 1147 bytes --]

---
 tools/handsfree-audio.c |    6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/tools/handsfree-audio.c b/tools/handsfree-audio.c
index e8725b7..90c274b 100644
--- a/tools/handsfree-audio.c
+++ b/tools/handsfree-audio.c
@@ -64,6 +64,7 @@ static gboolean option_nocvsd = FALSE;
 static gboolean option_nomsbc = FALSE;
 static gboolean option_server = FALSE;
 static gboolean option_defer = FALSE;
+static gboolean option_kill = FALSE;
 static gchar *option_client_addr = NULL;
 
 static const char sntable[4] = { 0x08, 0x38, 0xC8, 0xF8 };
@@ -556,6 +557,9 @@ static int new_connection(int fd, int codec)
 	struct hfp_thread *thread;
 
 	DBG("New connection: fd=%d codec=%d", fd, codec);
+	if (option_kill)
+		return -ECANCELED;
+
 	thread = g_try_malloc0(sizeof(struct hfp_thread));
 	if (thread == NULL)
 		return -ENOMEM;
@@ -891,6 +895,8 @@ static GOptionEntry options[] = {
 				"Server" },
 	{ "client", 'C', 1, G_OPTION_ARG_STRING, &option_client_addr,
 				"Client addr" },
+	{ "kill", 'k', 0, G_OPTION_ARG_NONE, &option_kill,
+				"Kill all new connections" },
 	{ NULL },
 };
 
-- 
1.7.9.5

Re: [PATCH v4 1/9] handsfree-audio: Initial DBUS code

[Denis Kenzior]

[-- Attachment #1: Type: text/plain, Size: 10238 bytes --]

Hi Fred,

On 03/25/2013 12:16 PM, Frédéric Dalleau wrote:
> This code can register an handsfree audio agent and receive NewConnection and
> Release calls.
> ---
>   tools/handsfree-audio.c |  332 +++++++++++++++++++++++++++++++++++++++++++++++
>   1 file changed, 332 insertions(+)
>   create mode 100644 tools/handsfree-audio.c
>
> diff --git a/tools/handsfree-audio.c b/tools/handsfree-audio.c
> new file mode 100644
> index 0000000..88310aa
> --- /dev/null
> +++ b/tools/handsfree-audio.c
> @@ -0,0 +1,332 @@
> +/*
> + *
> + *  oFono - Open Source Telephony
> + *
> + *  Copyright (C) 2013  Intel Corporation. All rights reserved.
> + *
> + *  This program is free software; you can redistribute it and/or modify
> + *  it under the terms of the GNU General Public License version 2 as
> + *  published by the Free Software Foundation.
> + *
> + *  This program is distributed in the hope that it will be useful,
> + *  but WITHOUT ANY WARRANTY; without even the implied warranty of
> + *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
> + *  GNU General Public License for more details.
> + *
> + *  You should have received a copy of the GNU General Public License
> + *  along with this program; if not, write to the Free Software
> + *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
> + *
> + */
> +
> +#ifdef HAVE_CONFIG_H
> +#include<config.h>
> +#endif
> +
> +#include<stdio.h>
> +#include<errno.h>
> +#include<fcntl.h>
> +#include<unistd.h>
> +#include<stdlib.h>
> +#include<string.h>
> +#include<signal.h>
> +
> +#include<gdbus.h>
> +#include<glib.h>
> +
> +#define OFONO_SERVICE			"org.ofono"
> +#define HFP_AUDIO_MANAGER_PATH		"/"
> +#define HFP_AUDIO_MANAGER_INTERFACE	OFONO_SERVICE ".HandsfreeAudioManager"
> +#define HFP_AUDIO_AGENT_PATH		"/hfpaudioagent"
> +#define HFP_AUDIO_AGENT_INTERFACE	OFONO_SERVICE ".HandsfreeAudioAgent"
> +
> +#define HFP_AUDIO_CVSD			1
> +#define HFP_AUDIO_MSBC			2
> +
> +#define DBG(fmt, arg...) do {\
> +		g_print("%s: " fmt "\n", __FUNCTION__, ## arg);\
> +	} while (0)
> +
> +/* DBus related */
> +static GMainLoop *main_loop = NULL;
> +static DBusConnection *conn;
> +static GSList *hcons = NULL;

This variable needs to be named differently.  Perhaps audio_connections, 
or active_list, or something else more descriptive.

> +
> +static gboolean option_nocvsd = FALSE;

I don't think it makes sense to have a nocvsd option.  That one is 
mandatory and we cannot register multiple agents.

> +static gboolean option_nomsbc = FALSE;
> +
> +struct hfp_audio_conn {
> +	unsigned char codec;
> +	int watch;
> +};
> +
> +static void hfp_audio_conn_free(struct hfp_audio_conn *hcon)
> +{
> +	DBG("Freeing audio connection %p", hcon);
> +
> +	hcons = g_slist_remove(hcons, hcon);
> +	g_source_remove(hcon->watch);
> +	g_free(hcon);
> +}
> +
> +static gboolean hfp_audio_cb(GIOChannel *io, GIOCondition cond, gpointer data)
> +{
> +	struct hfp_audio_conn *hcon = data;
> +	gsize read;
> +	gsize written;
> +	char buf[60];
> +
> +	if (cond&  (G_IO_HUP | G_IO_NVAL | G_IO_ERR))
> +		goto fail;
> +
> +	if (g_io_channel_read_chars(io, buf, sizeof(buf),&read, NULL) !=
> +			G_IO_STATUS_NORMAL)
> +		goto fail;
> +
> +	g_io_channel_write_chars(io, buf+written, read,&written, NULL);

Something is funky here with the written variable.  I do not see it 
being initialized.  Also, I don't really understand the logic, you're 
reading a max of 60 bytes into buf and writing the number of read into 
the io channel.  Why do you even need 'written'?

> +
> +	return TRUE;
> +
> +fail:
> +	DBG("Disconnected");
> +	hfp_audio_conn_free(hcon);
> +	return FALSE;
> +}
> +
> +static DBusMessage *agent_newconnection(DBusConnection *conn, DBusMessage *msg,
> +					void *data)
> +{
> +	const char *card;
> +	int fd;
> +	unsigned char codec;
> +	GIOChannel *io;
> +	struct hfp_audio_conn *hcon;
> +
> +	DBG("New connection");
> +
> +	if (dbus_message_get_args(msg, NULL, DBUS_TYPE_OBJECT_PATH,&card,
> +						DBUS_TYPE_UNIX_FD,&fd,
> +						DBUS_TYPE_BYTE,&codec,
> +						DBUS_TYPE_INVALID) == FALSE)
> +		return g_dbus_create_error(msg,
> +				HFP_AUDIO_AGENT_INTERFACE ".InvalidArguments",
> +				"Invalid arguments");
> +
> +	DBG("New connection: card=%s fd=%d codec=%d", card, fd, codec);
> +
> +	io = g_io_channel_unix_new(fd);
> +
> +	hcon = g_try_malloc0(sizeof(struct hfp_audio_conn));
> +	if (hcon == NULL)
> +		return NULL;

What do you do with io/fd in this case?  Please just use g_new0 here.

> +
> +	hcon->codec = codec;
> +	hcon->watch = g_io_add_watch(io, G_IO_IN, hfp_audio_cb, hcon);
> +	hcons = g_slist_prepend(hcons, hcon);
> +
> +	return dbus_message_new_method_return(msg);
> +}
> +
> +static DBusMessage *agent_release(DBusConnection *conn, DBusMessage *msg,
> +					void *data)
> +{
> +	DBG("HFP audio agent released");
> +	return dbus_message_new_method_return(msg);

You might as well clean up and exit here.

> +}
> +
> +static const GDBusMethodTable agent_methods[] = {
> +	{ GDBUS_METHOD("NewConnection", NULL, NULL, agent_newconnection) },
> +	{ GDBUS_METHOD("Release", NULL, NULL, agent_release) },
> +	{ },
> +};
> +
> +static void hfp_audio_agent_register_reply(DBusPendingCall *call, void *data)
> +{
> +	DBusMessage *reply = dbus_pending_call_steal_reply(call);
> +	DBusError err;
> +
> +	dbus_error_init(&err);
> +
> +	if (dbus_set_error_from_message(&err, reply) == TRUE) {
> +		DBG("Failed to register audio agent (%s: %s)", err.name,
> +								err.message);
> +		dbus_error_free(&err);
> +	} else {
> +		DBG("HFP audio agent registered");
> +	}
> +
> +	dbus_message_unref(reply);
> +}
> +
> +static void hfp_audio_agent_register(DBusConnection *conn)
> +{
> +	DBusMessage *msg;
> +	DBusPendingCall *call;
> +	const char *path = HFP_AUDIO_AGENT_PATH;
> +	unsigned char codecs[2];
> +	const unsigned char *pcodecs = codecs;
> +	int ncodecs = 0;
> +
> +	DBG("Registering audio agent");
> +
> +	msg = dbus_message_new_method_call(OFONO_SERVICE,
> +						HFP_AUDIO_MANAGER_PATH,
> +						HFP_AUDIO_MANAGER_INTERFACE,
> +						"Register");
> +	if (msg == NULL) {
> +		DBG("Not enough memory");
> +		return;
> +	}
> +
> +	if (option_nocvsd == FALSE)
> +		codecs[ncodecs++] = HFP_AUDIO_CVSD;
> +
> +	if (option_nomsbc == FALSE)
> +		codecs[ncodecs++] = HFP_AUDIO_MSBC;
> +
> +	dbus_message_append_args(msg, DBUS_TYPE_OBJECT_PATH,&path,
> +					DBUS_TYPE_ARRAY, DBUS_TYPE_BYTE,
> +					&pcodecs, ncodecs, DBUS_TYPE_INVALID);
> +
> +	if (!dbus_connection_send_with_reply(conn, msg,&call, -1)) {
> +		dbus_message_unref(msg);
> +		DBG("Unable to register agent");
> +		return;
> +	}
> +
> +	dbus_message_unref(msg);
> +
> +	if (call == NULL) {
> +		DBG("Unable to register agent");
> +		return;
> +	}
> +
> +	dbus_pending_call_set_notify(call, hfp_audio_agent_register_reply,
> +						NULL, NULL);
> +
> +	dbus_pending_call_unref(call);
> +}
> +
> +static void hfp_audio_agent_create(DBusConnection *conn)
> +{
> +	DBG("Creating audio agent");
> +
> +	if (!g_dbus_register_interface(conn, HFP_AUDIO_AGENT_PATH,
> +					HFP_AUDIO_AGENT_INTERFACE,
> +					agent_methods, NULL, NULL,
> +					NULL, NULL)) {
> +		DBG("Unable to create local agent");
> +		g_main_loop_quit(main_loop);
> +	}
> +}
> +
> +static void hfp_audio_agent_destroy(DBusConnection *conn)
> +{
> +	DBG("Destroying audio agent");
> +
> +	g_dbus_unregister_interface(conn, HFP_AUDIO_AGENT_PATH,
> +						HFP_AUDIO_AGENT_INTERFACE);
> +}
> +
> +static void ofono_connect(DBusConnection *conn, void *user_data)
> +{
> +	DBG("oFono appeared");
> +
> +	hfp_audio_agent_register(conn);
> +}
> +
> +static void ofono_disconnect(DBusConnection *conn, void *user_data)
> +{
> +	DBG("oFono disappeared");
> +}
> +
> +static void disconnect_callback(DBusConnection *conn, void *user_data)
> +{
> +	DBG("Disconnected from BUS");
> +
> +	g_main_loop_quit(main_loop);
> +}
> +
> +static void sig_term(int sig)
> +{
> +	DBG("Terminating");
> +
> +	g_main_loop_quit(main_loop);
> +}
> +
> +static GOptionEntry options[] = {
> +	{ "nocvsd", 'c', 0, G_OPTION_ARG_NONE,&option_nocvsd,
> +				"Disable CVSD support" },
> +	{ "nomsbc", 'm', 0, G_OPTION_ARG_NONE,&option_nomsbc,
> +				"Disable MSBC support" },
> +	{ NULL },
> +};
> +
> +int main(int argc, char **argv)
> +{
> +	GOptionContext *context;
> +	GError *error = NULL;
> +	DBusError err;
> +	guint watch;
> +	struct sigaction sa;
> +
> +	context = g_option_context_new(NULL);
> +	g_option_context_add_main_entries(context, options, NULL);
> +
> +	if (g_option_context_parse(context,&argc,&argv,&error) == FALSE) {
> +		if (error != NULL) {
> +			DBG("%s", error->message);
> +			g_error_free(error);
> +		} else
> +			DBG("An unknown error occurred");
> +		exit(1);
> +	}
> +
> +	g_option_context_free(context);
> +
> +	if (option_nocvsd == TRUE&&  option_nomsbc == TRUE) {
> +		DBG("At least one codec must be supported");
> +		exit(2);
> +	}
> +
> +	main_loop = g_main_loop_new(NULL, FALSE);
> +
> +	dbus_error_init(&err);
> +
> +	conn = g_dbus_setup_bus(DBUS_BUS_SYSTEM, NULL,&err);
> +	if (conn == NULL) {
> +		if (dbus_error_is_set(&err) == TRUE) {
> +			DBG("%s", err.message);
> +			dbus_error_free(&err);
> +		} else
> +			DBG("Can't register with system bus");
> +		exit(1);
> +	}
> +
> +	g_dbus_set_disconnect_function(conn, disconnect_callback, NULL, NULL);
> +
> +	memset(&sa, 0, sizeof(sa));
> +	sa.sa_handler = sig_term;
> +	sigaction(SIGINT,&sa, NULL);
> +	sigaction(SIGTERM,&sa, NULL);
> +
> +	hfp_audio_agent_create(conn);
> +
> +	watch = g_dbus_add_service_watch(conn, OFONO_SERVICE,
> +				ofono_connect, ofono_disconnect, NULL, NULL);
> +
> +	g_main_loop_run(main_loop);
> +
> +	g_dbus_remove_watch(conn, watch);
> +
> +	while (hcons != NULL)
> +		hfp_audio_conn_free(hcons->data);
> +
> +	hfp_audio_agent_destroy(conn);
> +
> +	dbus_connection_unref(conn);
> +
> +	g_main_loop_unref(main_loop);
> +
> +	return 0;
> +}

Regards,
-Denis

Re: [PATCH v4 5/9] handsfree-audio: Implement alsa playback

[Denis Kenzior]

[-- Attachment #1: Type: text/plain, Size: 16710 bytes --]

Hi Fred,

On 03/25/2013 12:16 PM, Frédéric Dalleau wrote:
> ---
>   tools/handsfree-audio.c |  472 +++++++++++++++++++++++++++++++++++++++++------
>   1 file changed, 417 insertions(+), 55 deletions(-)
>
> diff --git a/tools/handsfree-audio.c b/tools/handsfree-audio.c
> index 88310aa..9a4f85b 100644
> --- a/tools/handsfree-audio.c
> +++ b/tools/handsfree-audio.c
> @@ -30,10 +30,17 @@
>   #include<stdlib.h>
>   #include<string.h>
>   #include<signal.h>
> +#include<sys/time.h>
> +#include<poll.h>
>
>   #include<gdbus.h>
>   #include<glib.h>
>
> +#include<alsa/asoundlib.h>
> +#include<pthread.h>
> +#include<bluetooth/bluetooth.h>
> +#include<bluetooth/sco.h>
> +
>   #define OFONO_SERVICE			"org.ofono"
>   #define HFP_AUDIO_MANAGER_PATH		"/"
>   #define HFP_AUDIO_MANAGER_INTERFACE	OFONO_SERVICE ".HandsfreeAudioManager"
> @@ -50,47 +57,257 @@
>   /* DBus related */
>   static GMainLoop *main_loop = NULL;
>   static DBusConnection *conn;
> -static GSList *hcons = NULL;
> +static GSList *threads = NULL;
>
>   static gboolean option_nocvsd = FALSE;
>   static gboolean option_nomsbc = FALSE;
> +static gboolean option_server = FALSE;
> +static gboolean option_defer = FALSE;
> +static gchar *option_client_addr = NULL;
>
> -struct hfp_audio_conn {
> +struct hfp_audio_thread {
>   	unsigned char codec;
> -	int watch;
> +	int fd;
> +	int running;
> +	pthread_t thread;
>   };
>
> -static void hfp_audio_conn_free(struct hfp_audio_conn *hcon)
> +static int btstr2ba(const char *str, bdaddr_t *ba)
> +{
> +	int i;
> +
> +	for (i = 5; i>= 0; i--, str += 3)
> +		ba->b[i] = strtol(str, NULL, 16);
> +
> +	return 0;
> +}
> +
> +static snd_pcm_t *hfp_audio_pcm_init(snd_pcm_stream_t stream)
> +{
> +	snd_pcm_t *pcm;
> +	DBG("Initializing pcm for %s", (stream == SND_PCM_STREAM_CAPTURE) ?
> +			"capture" : "playback");
> +
> +	if (snd_pcm_open(&pcm, "default", stream, SND_PCM_NONBLOCK)<  0) {
> +		DBG("Failed to open pcm");
> +		return NULL;
> +	}
> +
> +	/* 8000 khz, 16 bits, 128000 bytes/s, 48 bytes/frame, 6000 fps */
> +	if (snd_pcm_set_params(pcm, SND_PCM_FORMAT_S16_LE,
> +					SND_PCM_ACCESS_RW_INTERLEAVED,
> +					1, 8000, 1, 20000)<  0) {
> +		DBG("Failed to set pcm params");
> +		snd_pcm_close(pcm);
> +		pcm = NULL;
> +	}
> +
> +	return pcm;
> +}
> +
> +static void hfp_audio_thread_free(struct hfp_audio_thread *hcon)
>   {
>   	DBG("Freeing audio connection %p", hcon);
> +	if (!hcon)
> +		return;
> +
> +	hcon->running = 0;
> +	if (hcon->thread)
> +		pthread_join(hcon->thread, NULL);
>
> -	hcons = g_slist_remove(hcons, hcon);
> -	g_source_remove(hcon->watch);
> +	threads = g_slist_remove(threads, hcon);
>   	g_free(hcon);
> +	DBG("freed %p", hcon);
>   }
>
> -static gboolean hfp_audio_cb(GIOChannel *io, GIOCondition cond, gpointer data)
> +/* Returns the number of data on sco socket */
> +static int hfp_audio_playback(int fd, snd_pcm_t *playback)
>   {
> -	struct hfp_audio_conn *hcon = data;
> -	gsize read;
> -	gsize written;
> -	char buf[60];
> +	char buf[800];
> +	snd_pcm_sframes_t frames;
> +	int bytes;
> +
> +	bytes = read(fd, buf, sizeof(buf));
> +	if (bytes<  0) {
> +		DBG("Failed to read: bytes %d, errno %d", bytes, errno);
> +		switch (errno) {
> +		case ENOTCONN:
> +			return -ENOTCONN;
> +		case EAGAIN:
> +			return 0;
> +		default:
> +			return -EINVAL;
> +		}
> +	}
>
> -	if (cond&  (G_IO_HUP | G_IO_NVAL | G_IO_ERR))
> -		goto fail;
> +	frames = snd_pcm_writei(playback, buf, bytes / 2);
> +	switch (frames) {
> +	case -EPIPE:
> +		DBG("Playback underrun");
> +		snd_pcm_prepare(playback);
> +		return bytes;
> +	case -EAGAIN:
> +		DBG("??? %d", bytes / 2);
> +		return bytes;
> +	case -EBADFD:
> +	case -ESTRPIPE:
> +		return -EINVAL;
> +	}
>
> -	if (g_io_channel_read_chars(io, buf, sizeof(buf),&read, NULL) !=
> -			G_IO_STATUS_NORMAL)
> -		goto fail;
> +	if (frames<  bytes / 2)
> +		DBG("played %d<  requested %d", (int)frames, bytes / 2);
>
> -	g_io_channel_write_chars(io, buf+written, read,&written, NULL);
> +	return bytes;
> +}
>
> -	return TRUE;
> +/* Returns the number of data on sco socket */
> +static int hfp_audio_capture(int fd, snd_pcm_t *capture, GList **outq, int mtu)
> +{
> +	snd_pcm_sframes_t frames;
> +	gchar *buf;
> +
> +	buf = g_try_malloc(mtu);
> +	if (!buf)
> +		return -ENOMEM;
> +
> +	frames = snd_pcm_readi(capture, buf, mtu / 2);
> +	switch (frames) {
> +	case -EPIPE:
> +		DBG("Capture overrun");
> +		snd_pcm_prepare(capture);
> +		g_free(buf);
> +		return 0;
> +	case -EAGAIN:
> +		DBG("No data to capture");
> +		g_free(buf);
> +		return 0;
> +	case -EBADFD:
> +	case -ESTRPIPE:
> +		DBG("Other error");
> +		g_free(buf);
> +		return -EINVAL;
> +	}
> +
> +	if (frames<  mtu / 2)
> +		DBG("Small frame: %d", (int) frames);
> +
> +	if (g_list_length(*outq)>  32)
> +		DBG("Too many queued packets");
> +
> +	*outq = g_list_append(*outq, buf);
> +
> +	return frames * 2;
> +}
> +
> +static void pop_outq(int fd, GList **outq, int mtu)
> +{
> +	GList *el;
> +
> +	el = g_list_first(*outq);
> +	if (!el)
> +		return;
> +
> +	*outq = g_list_remove_link(*outq, el);
>
> +	if (write(fd, el->data, mtu)<  0)
> +		DBG("Failed to write: %d", errno);
> +
> +	g_free(el->data);
> +	g_list_free(el);
> +}
> +
> +static void *thread_func(void *userdata)
> +{
> +	struct hfp_audio_thread *hcon = userdata;
> +	snd_pcm_t *playback, *capture;
> +	GList *outq = NULL;
> +	struct sco_options  opts;
> +	struct pollfd fds[8];
> +
> +	DBG("thread started");
> +
> +	capture = hfp_audio_pcm_init(SND_PCM_STREAM_CAPTURE);
> +	if (!capture)
> +		return NULL;
> +
> +	playback = hfp_audio_pcm_init(SND_PCM_STREAM_PLAYBACK);
> +	if (!playback) {
> +		snd_pcm_close(capture);
> +		return NULL;
> +	}
> +
> +	/* Force defered setup */
> +	if (read(hcon->fd,&opts, sizeof(opts))<  0)
> +		DBG("Defered setup failed: %d (%s)", errno, strerror(errno));
> +
> +	/* Add SCO options
> +	len = sizeof(opts);
> +	if (getsockopt(hcon->fd, SOL_SCO, SCO_OPTIONS,&opts,&len)<  0) {
> +		DBG("getsockopt failed %d", errno);
> +		return NULL;
> +	}
> +	*/
> +	opts.mtu = 48;
> +	DBG("mtu %d", opts.mtu);
> +
> +	while (hcon->running) {
> +		/* Queue alsa captured data (snd_pcm_poll_descriptors failed) */
> +		if (hfp_audio_capture(hcon->fd, capture,&outq, opts.mtu)<  0) {
> +			DBG("Failed to capture");
> +			break;
> +		}
> +
> +		memset(fds, 0, sizeof(fds));
> +		fds[0].fd = hcon->fd;
> +		fds[0].events = POLLIN | POLLERR | POLLHUP | POLLNVAL;
> +		if (poll(fds, 1, 200) == 0)
> +			continue;
> +
> +		if (fds[0].revents&  (POLLERR | POLLHUP | POLLNVAL)) {
> +			DBG("POLLERR | POLLHUP | POLLNVAL triggered");
> +			break;
> +		}
> +
> +		if (!fds[0].revents&  POLLIN)
> +			continue;
> +
> +		if (hfp_audio_playback(hcon->fd, playback)<  0) {
> +			DBG("POLLIN triggered, but read error");
> +			break;
> +		}
> +
> +		/* Dequeue in sync with readings */
> +		pop_outq(hcon->fd,&outq, opts.mtu);
> +	}
> +
> +	DBG("thread terminating");
> +	snd_pcm_close(playback);
> +	snd_pcm_close(capture);
> +	return NULL;
> +}
> +
> +static int new_connection(int fd, int codec)
> +{
> +	struct hfp_audio_thread *hcon;
> +	DBG("New connection: fd=%d codec=%d", fd, codec);
> +	hcon = g_try_malloc0(sizeof(struct hfp_audio_thread));
> +	if (hcon == NULL)
> +		return -ENOMEM;
> +
> +	hcon->fd = fd;
> +	hcon->codec = codec;
> +	hcon->running = 1;
> +
> +	if (pthread_create(&hcon->thread, NULL, thread_func, hcon)<  0)
> +		goto fail;
> +
> +	/* FIXME thread is not detached until we quit */
> +
> +	threads = g_slist_prepend(threads, hcon);
> +	return 0;
>   fail:
> -	DBG("Disconnected");
> -	hfp_audio_conn_free(hcon);
> -	return FALSE;
> +	hfp_audio_thread_free(hcon);
> +	return -EINVAL;
>   }
>
>   static DBusMessage *agent_newconnection(DBusConnection *conn, DBusMessage *msg,
> @@ -99,8 +316,7 @@ static DBusMessage *agent_newconnection(DBusConnection *conn, DBusMessage *msg,
>   	const char *card;
>   	int fd;
>   	unsigned char codec;
> -	GIOChannel *io;
> -	struct hfp_audio_conn *hcon;
> +	DBusMessage *reply;
>
>   	DBG("New connection");
>
> @@ -112,30 +328,34 @@ static DBusMessage *agent_newconnection(DBusConnection *conn, DBusMessage *msg,
>   				HFP_AUDIO_AGENT_INTERFACE ".InvalidArguments",
>   				"Invalid arguments");
>
> -	DBG("New connection: card=%s fd=%d codec=%d", card, fd, codec);
> +	reply = dbus_message_new_method_return(msg);
> +	if (!reply)
> +		goto fail;
>
> -	io = g_io_channel_unix_new(fd);
> +	if (new_connection(fd, codec)>= 0)
> +		return reply;
>
> -	hcon = g_try_malloc0(sizeof(struct hfp_audio_conn));
> -	if (hcon == NULL)
> -		return NULL;
> +	dbus_message_unref(reply);
>
> -	hcon->codec = codec;
> -	hcon->watch = g_io_add_watch(io, G_IO_IN, hfp_audio_cb, hcon);
> -	hcons = g_slist_prepend(hcons, hcon);
> +fail:
> +	close(fd);
>
> -	return dbus_message_new_method_return(msg);
> +	return g_dbus_create_error(msg,
> +			HFP_AUDIO_AGENT_INTERFACE ".Failed", "Failed to start");
>   }
>
>   static DBusMessage *agent_release(DBusConnection *conn, DBusMessage *msg,
>   					void *data)
>   {
>   	DBG("HFP audio agent released");
> +	/* agent will be registered on next oFono startup */
>   	return dbus_message_new_method_return(msg);
>   }
>
>   static const GDBusMethodTable agent_methods[] = {
> -	{ GDBUS_METHOD("NewConnection", NULL, NULL, agent_newconnection) },
> +	{ GDBUS_METHOD("NewConnection",
> +		GDBUS_ARGS({ "path", "o" }, { "fd", "h" }, { "codec", "y" }),
> +		NULL, agent_newconnection) },
>   	{ GDBUS_METHOD("Release", NULL, NULL, agent_release) },
>   	{ },
>   };
> @@ -167,7 +387,7 @@ static void hfp_audio_agent_register(DBusConnection *conn)
>   	const unsigned char *pcodecs = codecs;
>   	int ncodecs = 0;
>
> -	DBG("Registering audio agent");
> +	DBG("Registering audio agent in oFono");
>
>   	msg = dbus_message_new_method_call(OFONO_SERVICE,
>   						HFP_AUDIO_MANAGER_PATH,
> @@ -196,20 +416,147 @@ static void hfp_audio_agent_register(DBusConnection *conn)
>
>   	dbus_message_unref(msg);
>
> -	if (call == NULL) {
> -		DBG("Unable to register agent");
> -		return;
> -	}
> -
>   	dbus_pending_call_set_notify(call, hfp_audio_agent_register_reply,
>   						NULL, NULL);
>
>   	dbus_pending_call_unref(call);
>   }
>
> +static gboolean sco_accept_cb(GIOChannel *io, GIOCondition cond, gpointer data)
> +{
> +	struct sockaddr_sco addr;
> +	socklen_t optlen;
> +	int sk, nsk;
> +
> +	if (cond&  (G_IO_HUP | G_IO_NVAL | G_IO_ERR))
> +		goto fail;
> +
> +	DBG("Incoming connection");
> +	sk = g_io_channel_unix_get_fd(io);
> +	nsk = accept(sk, (struct sockaddr *)&addr,&optlen);
> +
> +	if (nsk>  0)
> +		new_connection(nsk, HFP_AUDIO_CVSD);
> +
> +	return TRUE;
> +
> +fail:
> +	DBG("Server disconnected");
> +	return FALSE;
> +}

I'm lost, why do we need this part?

> +
> +static gboolean sco_connect_cb(GIOChannel *io, GIOCondition cond, gpointer data)
> +{
> +	int sk;
> +
> +	if (cond&  (G_IO_HUP | G_IO_NVAL | G_IO_ERR))
> +		goto fail;
> +
> +	DBG("Connected");
> +	sk = g_io_channel_unix_get_fd(io);
> +	if (sk>  0)
> +		new_connection(sk, HFP_AUDIO_CVSD);
> +
> +	return FALSE;
> +
> +fail:
> +	DBG("Connection failed");
> +	return FALSE;
> +}

Or this part?

> +
> +static int sco_listen_watch()
> +{
> +	struct sockaddr_sco saddr;
> +	int sk;
> +	GIOChannel *io;
> +
> +	/* Create socket */
> +	sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_SCO);
> +	if (sk<  0) {
> +		DBG("Can't create socket: %s (%d)", strerror(errno), errno);
> +		return -1;
> +	}
> +
> +	/* Bind to local address */
> +	memset(&saddr, 0, sizeof(saddr));
> +	saddr.sco_family = AF_BLUETOOTH;
> +
> +	if (bind(sk, (struct sockaddr *)&saddr, sizeof(saddr))<  0) {
> +		DBG("Can't bind socket: %s (%d)", strerror(errno), errno);
> +		goto error;
> +	}
> +
> +	/* Enable deferred setup */
> +	if (option_defer&&  setsockopt(sk, SOL_BLUETOOTH, BT_DEFER_SETUP,
> +				&option_defer, sizeof(option_defer))<  0) {
> +		DBG("Can't defer setup : %s (%d)", strerror(errno), errno);
> +		goto error;
> +	}
> +
> +	/* Listen for connections */
> +	if (listen(sk, 10)) {
> +		DBG("Can not listen socket: %s (%d)", strerror(errno), errno);
> +		goto error;
> +	}
> +
> +	DBG("Waiting for connection ...");
> +	io = g_io_channel_unix_new(sk);
> +	if (!io)
> +		goto error;
> +
> +	return g_io_add_watch(io, G_IO_IN, sco_accept_cb, NULL);
> +
> +error:
> +	close(sk);
> +	return -1;
> +}

Or this part?

> +
> +static int sco_connect_watch()
> +{
> +	struct sockaddr_sco saddr;
> +	int sk;
> +	GIOChannel *io;
> +
> +	/* Create socket */
> +	sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_SCO);
> +	if (sk<  0) {
> +		DBG("Can't create socket: %s (%d)", strerror(errno), errno);
> +		return -1;
> +	}
> +
> +	/* Bind to local address */
> +	memset(&saddr, 0, sizeof(saddr));
> +	saddr.sco_family = AF_BLUETOOTH;
> +
> +	if (bind(sk, (struct sockaddr *)&saddr, sizeof(saddr))<  0) {
> +		DBG("Can't bind socket: %s (%d)", strerror(errno), errno);
> +		goto error;
> +	}
> +
> +	/* Connect to remote address */
> +	memset(&saddr, 0, sizeof(saddr));
> +	saddr.sco_family = AF_BLUETOOTH;
> +	btstr2ba(option_client_addr,&saddr.sco_bdaddr);
> +	if (connect(sk, (struct sockaddr *)&saddr, sizeof(saddr))) {
> +		DBG("Can not connect socket: %s (%d)", strerror(errno), errno);
> +		goto error;
> +	}
> +
> +	DBG("Connecting to %s...", option_client_addr);
> +	io = g_io_channel_unix_new(sk);
> +	if (!io)
> +		goto error;
> +
> +	return g_io_add_watch(io, G_IO_IN|G_IO_OUT, sco_connect_cb, NULL);
> +
> +error:
> +	close(sk);
> +	return -1;
> +}
> +

Or this?  Sounds like these belong in BlueZ, not oFono.

>   static void hfp_audio_agent_create(DBusConnection *conn)
>   {
> -	DBG("Creating audio agent");
> +	DBG("Registering audio agent on DBUS");
>
>   	if (!g_dbus_register_interface(conn, HFP_AUDIO_AGENT_PATH,
>   					HFP_AUDIO_AGENT_INTERFACE,
> @@ -222,7 +569,7 @@ static void hfp_audio_agent_create(DBusConnection *conn)
>
>   static void hfp_audio_agent_destroy(DBusConnection *conn)
>   {
> -	DBG("Destroying audio agent");
> +	DBG("Unregistering audio agent on DBUS");
>
>   	g_dbus_unregister_interface(conn, HFP_AUDIO_AGENT_PATH,
>   						HFP_AUDIO_AGENT_INTERFACE);
> @@ -259,6 +606,12 @@ static GOptionEntry options[] = {
>   				"Disable CVSD support" },
>   	{ "nomsbc", 'm', 0, G_OPTION_ARG_NONE,&option_nomsbc,
>   				"Disable MSBC support" },
> +	{ "defer", 'd', 0, G_OPTION_ARG_NONE,&option_defer,
> +				"Defered socket support" },
> +	{ "server", 'S', 0, G_OPTION_ARG_NONE,&option_server,
> +				"Server" },
> +	{ "client", 'C', 1, G_OPTION_ARG_STRING,&option_client_addr,
> +				"Client addr" },
>   	{ NULL },
>   };
>
> @@ -293,6 +646,11 @@ int main(int argc, char **argv)
>
>   	dbus_error_init(&err);
>
> +	memset(&sa, 0, sizeof(sa));
> +	sa.sa_handler = sig_term;
> +	sigaction(SIGINT,&sa, NULL);
> +	sigaction(SIGTERM,&sa, NULL);
> +
>   	conn = g_dbus_setup_bus(DBUS_BUS_SYSTEM, NULL,&err);
>   	if (conn == NULL) {
>   		if (dbus_error_is_set(&err) == TRUE) {
> @@ -305,24 +663,28 @@ int main(int argc, char **argv)
>
>   	g_dbus_set_disconnect_function(conn, disconnect_callback, NULL, NULL);
>
> -	memset(&sa, 0, sizeof(sa));
> -	sa.sa_handler = sig_term;
> -	sigaction(SIGINT,&sa, NULL);
> -	sigaction(SIGTERM,&sa, NULL);
> -
> -	hfp_audio_agent_create(conn);
> -
> -	watch = g_dbus_add_service_watch(conn, OFONO_SERVICE,
> +	if (option_server) {
> +		watch = sco_listen_watch();
> +	} else if (option_client_addr != NULL) {
> +		watch = sco_connect_watch();
> +	} else {
> +		hfp_audio_agent_create(conn);
> +		watch = g_dbus_add_service_watch(conn, OFONO_SERVICE,
>   				ofono_connect, ofono_disconnect, NULL, NULL);
> -
> +	}
>   	g_main_loop_run(main_loop);
>
> -	g_dbus_remove_watch(conn, watch);
> -
> -	while (hcons != NULL)
> -		hfp_audio_conn_free(hcons->data);
> +	while (threads != NULL)
> +		hfp_audio_thread_free(threads->data);
>
> -	hfp_audio_agent_destroy(conn);
> +	if (option_server) {
> +		g_source_remove(watch);
> +	} else if (option_client_addr != NULL) {
> +		g_source_remove(watch);
> +	} else {
> +		g_dbus_remove_watch(conn, watch);
> +		hfp_audio_agent_destroy(conn);
> +	}
>
>   	dbus_connection_unref(conn);
>

Regards,
-Denis

Re: [PATCH v4 8/9] handsfree-audio: mSBC support

[Denis Kenzior]

[-- Attachment #1: Type: text/plain, Size: 23491 bytes --]

Hi Fred,

On 03/25/2013 12:16 PM, Frédéric Dalleau wrote:
> ---
>   tools/handsfree-audio.c |  513 ++++++++++++++++++++++++++++++++++++-----------
>   1 file changed, 396 insertions(+), 117 deletions(-)

You need to break this patch up some more and structure it a bit more 
logically.  For example, I would start with the hfp_thread driver 
changes and how they affect the read / write logic.  Then I would add 
CVSD parts, mSBC parts, etc.

>
> diff --git a/tools/handsfree-audio.c b/tools/handsfree-audio.c
> index 9a4f85b..e8725b7 100644
> --- a/tools/handsfree-audio.c
> +++ b/tools/handsfree-audio.c
> @@ -40,6 +40,7 @@
>   #include<pthread.h>
>   #include<bluetooth/bluetooth.h>
>   #include<bluetooth/sco.h>
> +#include<sbc/sbc.h>
>
>   #define OFONO_SERVICE			"org.ofono"
>   #define HFP_AUDIO_MANAGER_PATH		"/"
> @@ -65,11 +66,50 @@ static gboolean option_server = FALSE;
>   static gboolean option_defer = FALSE;
>   static gchar *option_client_addr = NULL;
>
> -struct hfp_audio_thread {
> -	unsigned char codec;
> +static const char sntable[4] = { 0x08, 0x38, 0xC8, 0xF8 };
> +static const int audio_rates[3] = { 0, 8000, 16000 };
> +
> +struct msbc_parser {
> +	int len;
> +	uint8_t buffer[60];
> +};
> +
> +struct msbc_codec {
> +	sbc_t sbcenc; /* Coder data */
> +	char *ebuffer; /* Codec transfer buffer */
> +	size_t ebuffer_size; /* Size of the buffer */
> +	size_t ebuffer_start; /* start of encoding data */
> +	size_t ebuffer_end; /* end of encoding data */
> +
> +	struct msbc_parser parser; /* mSBC parser for concatenating frames */
> +	sbc_t sbcdec; /* Decoder data */
> +
> +	size_t msbc_frame_size;
> +	size_t decoded_frame_size;
> +};
> +
> +struct hfp_thread {
>   	int fd;
>   	int running;
>   	pthread_t thread;
> +	GSList *outq;
> +
> +	int rate;
> +	char *capture_buffer;
> +	int capture_size;
> +	int captured;
> +	int mtu;
> +	int (*init)(struct hfp_thread *);
> +	int (*free)(struct hfp_thread *);
> +	int (*encode)(struct hfp_thread *, char *data, int len);
> +	int (*decode)(struct hfp_thread *, char *data, int len, char *out,
> +								int outlen);
> +	struct msbc_codec msbc;
> +};
> +
> +struct hack_sco_options {
> +	uint16_t mtu;
> +	uint16_t voice_setting;
>   };
>
>   static int btstr2ba(const char *str, bdaddr_t *ba)
> @@ -82,21 +122,232 @@ static int btstr2ba(const char *str, bdaddr_t *ba)
>   	return 0;
>   }
>
> -static snd_pcm_t *hfp_audio_pcm_init(snd_pcm_stream_t stream)
> +static void msbc_parser_reset(struct msbc_parser *p)
> +{
> +	p->len = 0;
> +}
> +
> +static int msbc_state_machine(struct msbc_parser *p, uint8_t byte)
> +{
> +	switch (p->len) {
> +	case 0:
> +		if (byte == 0x01)
> +			goto copy;
> +		return 0;
> +	case 1:
> +		if (byte == 0x08 || byte == 0x38 || byte == 0xC8
> +				|| byte == 0xF8)
> +			goto copy;
> +		break;
> +	case 2:
> +		if (byte == 0xAD)
> +			goto copy;
> +		break;
> +	case 3:
> +		if (byte == 0x00)
> +			goto copy;
> +		break;
> +	case 4:
> +		if (byte == 0x00)
> +			goto copy;
> +		break;
> +	default:
> +		goto copy;
> +	}
> +
> +	p->len = 0;
> +	return 0;
> +
> +copy:
> +	p->buffer[p->len] = byte;
> +	p->len++;
> +
> +	return p->len;
> +}
> +
> +static size_t msbc_parse(sbc_t *sbcdec, struct msbc_parser *p, char *data,
> +		int len, char *out, int outlen, int *bytes)
> +{
> +	size_t totalwritten = 0;
> +	size_t written = 0;
> +	int i;
> +	*bytes = 0;
> +
> +	for (i = 0; i<  len; i++) {
> +		if (msbc_state_machine(p, data[i]) == 60) {
> +			int decoded;
> +			decoded = sbc_decode(sbcdec, p->buffer + 2,
> +					p->len - 2 - 1, out, outlen,&written);
> +			if (decoded>  0) {
> +				totalwritten += written;
> +				*bytes += decoded;
> +			} else {
> +				DBG("Error while decoding: %d", decoded);
> +			}
> +			msbc_parser_reset(p);
> +		}
> +	}
> +	return totalwritten;
> +}
> +
> +static int hfp_audio_cvsd_init(struct hfp_thread *thread)
> +{
> +	thread->rate = 8000;
> +	thread->capture_size = 48;
> +	return 0;
> +}
> +
> +static int hfp_audio_cvsd_free(struct hfp_thread *thread)
> +{
> +	return 0;
> +}
> +
> +static int hfp_audio_cvsd_encode(struct hfp_thread *thread, char *data,
> +						int len)
> +{
> +	char *qbuf;
> +
> +	if (len>  thread->mtu) {
> +		DBG("Mtu too small: len %d, mtu %d", len, thread->mtu);
> +		return -EINVAL;
> +	}
> +
> +	qbuf = g_try_malloc(thread->mtu);
> +	if (!qbuf)
> +		return -ENOMEM;
> +
> +	memcpy(qbuf, data, len);
> +
> +	thread->outq = g_slist_insert(thread->outq, qbuf, -1);
> +
> +	return len;
> +}
> +
> +static int hfp_audio_cvsd_decode(struct hfp_thread *thread, char *data,
> +						int len, char *out, int outlen)
> +{
> +	int size = (len<  outlen) ? len : outlen;
> +	memcpy(out, data, size);
> +	return size;
> +}
> +
> +/* Run from IO thread */
> +static int hfp_audio_msbc_init(struct hfp_thread *thread)
> +{
> +	struct msbc_codec *codec =&thread->msbc;
> +	struct hack_sco_options opts;
> +
> +	thread->rate = 16000;
> +	thread->capture_size = 240; /* decoded mSBC frame */
> +
> +	memset(&opts, 0, sizeof(opts));
> +	opts.voice_setting = 0X0003;
> +	if (setsockopt(thread->fd, SOL_SCO, SCO_OPTIONS,&opts, sizeof(opts))
> +			<  0) {
> +		DBG("Can't set transparent mode: %s (%d)",
> +				strerror(errno), errno);
> +		return -ENOTSUP;
> +	}
> +
> +	sbc_init_msbc(&codec->sbcenc, 0);
> +	sbc_init_msbc(&codec->sbcdec, 0);
> +
> +	codec->msbc_frame_size = 2 + sbc_get_frame_length(&codec->sbcenc) + 1;
> +	codec->decoded_frame_size = sbc_get_codesize(&codec->sbcenc);
> +	msbc_parser_reset(&codec->parser);
> +
> +	/* 5 * 48 == 10 * 24 == 4 * 60 */
> +	codec->ebuffer_size = codec->msbc_frame_size * 4;
> +	codec->ebuffer = g_try_malloc(codec->ebuffer_size);
> +	codec->ebuffer_start = 0;
> +	codec->ebuffer_end = 0;
> +
> +	DBG("codec->msbc_frame_size %d", (int) codec->msbc_frame_size);
> +	DBG("codec->ebuffer_size %d", (int) codec->ebuffer_size);
> +	DBG("codec->decoded_frame_size %d", (int) codec->decoded_frame_size);
> +	return 0;
> +}
> +
> +/* Run from IO thread */
> +static int hfp_audio_msbc_free(struct hfp_thread *thread)
> +{
> +	struct msbc_codec *codec =&thread->msbc;
> +
> +	g_free(codec->ebuffer);
> +	sbc_finish(&codec->sbcenc);
> +	sbc_finish(&codec->sbcdec);
> +
> +	return 0;
> +}
> +
> +/* Run from IO thread */
> +static int hfp_audio_msbc_encode(struct hfp_thread *thread, char *data, int len)
> +{
> +	struct msbc_codec *codec =&thread->msbc;
> +	char *h2 = codec->ebuffer + codec->ebuffer_end;
> +	static int sn = 0;
> +	int written = 0;
> +	char *qbuf;
> +
> +	h2[0] = 0x01;
> +	h2[1] = sntable[sn];
> +	h2[59] = 0xff;
> +	sn = (sn + 1) % 4;
> +
> +	sbc_encode(&codec->sbcenc, data, len,
> +			codec->ebuffer + codec->ebuffer_end + 2,
> +			codec->ebuffer_size - codec->ebuffer_end - 2,
> +			(ssize_t *)&written);
> +
> +	written += 2 /* H2 */ + 1 /* 0xff */;
> +	codec->ebuffer_end += written;
> +
> +	/* Split into MTU sized chunks */
> +	while (codec->ebuffer_start + thread->mtu<= codec->ebuffer_end) {
> +		qbuf = g_try_malloc(thread->mtu);
> +		if (!qbuf)
> +			return -ENOMEM;
> +
> +		/* DBG("Enqueuing %d bytes", thread->mtu); */
> +		memcpy(qbuf, codec->ebuffer + codec->ebuffer_start, thread->mtu);
> +
> +		thread->outq = g_slist_insert(thread->outq, qbuf, -1);
> +
> +		codec->ebuffer_start += thread->mtu;
> +		if (codec->ebuffer_start>= codec->ebuffer_end)
> +			codec->ebuffer_start = codec->ebuffer_end = 0;
> +	}
> +
> +	return 0;
> +}
> +
> +/* Run from IO thread */
> +static int hfp_audio_msbc_decode(struct hfp_thread *thread, char *data,
> +						int len, char *out, int outlen)
> +{
> +	struct msbc_codec *codec =&thread->msbc;
> +	int written, decoded;
> +
> +	written = msbc_parse(&codec->sbcdec,&codec->parser, data, len, out,
> +			outlen,&decoded);
> +
> +	return written;
> +}
> +
> +static snd_pcm_t *hfp_audio_pcm_init(snd_pcm_stream_t stream, int rate)
>   {
>   	snd_pcm_t *pcm;
>   	DBG("Initializing pcm for %s", (stream == SND_PCM_STREAM_CAPTURE) ?
> -			"capture" : "playback");
> +							"capture" : "playback");
>
>   	if (snd_pcm_open(&pcm, "default", stream, SND_PCM_NONBLOCK)<  0) {
>   		DBG("Failed to open pcm");
>   		return NULL;
>   	}
>
> -	/* 8000 khz, 16 bits, 128000 bytes/s, 48 bytes/frame, 6000 fps */
> +	/* 16 bits */
>   	if (snd_pcm_set_params(pcm, SND_PCM_FORMAT_S16_LE,
> -					SND_PCM_ACCESS_RW_INTERLEAVED,
> -					1, 8000, 1, 20000)<  0) {
> +			SND_PCM_ACCESS_RW_INTERLEAVED, 1, rate, 1, 120000)<  0) {
>   		DBG("Failed to set pcm params");
>   		snd_pcm_close(pcm);
>   		pcm = NULL;
> @@ -105,29 +356,35 @@ static snd_pcm_t *hfp_audio_pcm_init(snd_pcm_stream_t stream)
>   	return pcm;
>   }
>
> -static void hfp_audio_thread_free(struct hfp_audio_thread *hcon)
> +static void hfp_audio_thread_free(struct hfp_thread *thread)
>   {
> -	DBG("Freeing audio connection %p", hcon);
> -	if (!hcon)
> +	DBG("Freeing audio connection %p", thread);
> +	if (!thread)
>   		return;
>
> -	hcon->running = 0;
> -	if (hcon->thread)
> -		pthread_join(hcon->thread, NULL);
> +	thread->running = 0;
> +	if (thread->thread)
> +		pthread_join(thread->thread, NULL);
>
> -	threads = g_slist_remove(threads, hcon);
> -	g_free(hcon);
> -	DBG("freed %p", hcon);
> +	if (shutdown(thread->fd, SHUT_RDWR)<  0)
> +			DBG("Failed to shutdown socket");
> +	if (close(thread->fd)<  0)
> +		DBG("Failed to close socket");
> +
> +	threads = g_slist_remove(threads, thread);
> +	g_free(thread);
> +	DBG("freed %p", thread);
>   }
>
>   /* Returns the number of data on sco socket */
> -static int hfp_audio_playback(int fd, snd_pcm_t *playback)
> +static int hfp_audio_playback(struct hfp_thread *thread,
> +		snd_pcm_t *playback)
>   {
> -	char buf[800];
> +	char buf[512], out[512];
> +	int bytes, outlen;
>   	snd_pcm_sframes_t frames;
> -	int bytes;
>
> -	bytes = read(fd, buf, sizeof(buf));
> +	bytes = read(thread->fd, buf, sizeof(buf));
>   	if (bytes<  0) {
>   		DBG("Failed to read: bytes %d, errno %d", bytes, errno);
>   		switch (errno) {
> @@ -140,178 +397,201 @@ static int hfp_audio_playback(int fd, snd_pcm_t *playback)
>   		}
>   	}
>
> -	frames = snd_pcm_writei(playback, buf, bytes / 2);
> +	/* DBG("received %d bytes", bytes); */
> +	outlen = thread->decode(thread, buf, bytes, out, sizeof(out));
> +
> +	frames = snd_pcm_writei(playback, out, outlen / 2);
>   	switch (frames) {
>   	case -EPIPE:
> -		DBG("Playback underrun");
>   		snd_pcm_prepare(playback);
>   		return bytes;
>   	case -EAGAIN:
> -		DBG("??? %d", bytes / 2);
>   		return bytes;
>   	case -EBADFD:
>   	case -ESTRPIPE:
>   		return -EINVAL;
>   	}
>
> -	if (frames<  bytes / 2)
> -		DBG("played %d<  requested %d", (int)frames, bytes / 2);
> +	if (frames<  outlen / 2)
> +		DBG("played %d<  requested %d", (int)frames, outlen / 2);
>
>   	return bytes;
>   }
>
>   /* Returns the number of data on sco socket */
> -static int hfp_audio_capture(int fd, snd_pcm_t *capture, GList **outq, int mtu)
> +static int hfp_audio_capture(struct hfp_thread *thread, snd_pcm_t *capture)
>   {
>   	snd_pcm_sframes_t frames;
> -	gchar *buf;
> -
> -	buf = g_try_malloc(mtu);
> -	if (!buf)
> -		return -ENOMEM;
>
> -	frames = snd_pcm_readi(capture, buf, mtu / 2);
> +	frames = snd_pcm_readi(capture, thread->capture_buffer+thread->captured,
> +				(thread->capture_size - thread->captured) / 2);
>   	switch (frames) {
>   	case -EPIPE:
> -		DBG("Capture overrun");
>   		snd_pcm_prepare(capture);
> -		g_free(buf);
>   		return 0;
>   	case -EAGAIN:
> -		DBG("No data to capture");
> -		g_free(buf);
>   		return 0;
>   	case -EBADFD:
>   	case -ESTRPIPE:
> -		DBG("Other error");
> -		g_free(buf);
> +		DBG("Other error %s (%d)", strerror(frames), (int) frames);
>   		return -EINVAL;
>   	}
>
> -	if (frames<  mtu / 2)
> -		DBG("Small frame: %d", (int) frames);
> +	thread->captured += frames * 2;
> +	if (thread->captured<  thread->capture_size)
> +		return frames * 2;
>
> -	if (g_list_length(*outq)>  32)
> -		DBG("Too many queued packets");
> -
> -	*outq = g_list_append(*outq, buf);
> +	/* DBG("Encoding %d bytes", (int) thread->captured); */
> +	thread->encode(thread, thread->capture_buffer, thread->captured);
> +	thread->captured = 0;
>
>   	return frames * 2;
>   }
>
> -static void pop_outq(int fd, GList **outq, int mtu)
> +static void pop_outq(struct hfp_thread *thread)
>   {
> -	GList *el;
> -
> -	el = g_list_first(*outq);
> -	if (!el)
> -		return;
> +	char *qbuf;
>
> -	*outq = g_list_remove_link(*outq, el);
> +	while (thread->outq != NULL) {
> +		qbuf = thread->outq->data;
> +		thread->outq = g_slist_remove(thread->outq, qbuf);
>
> -	if (write(fd, el->data, mtu)<  0)
> -		DBG("Failed to write: %d", errno);
> +		if (write(thread->fd, qbuf, thread->mtu)<  0)
> +			DBG("Failed to write: %d", errno);
>
> -	g_free(el->data);
> -	g_list_free(el);
> +		g_free(qbuf);
> +	}
>   }
>
>   static void *thread_func(void *userdata)
>   {
> -	struct hfp_audio_thread *hcon = userdata;
> +	struct hfp_thread *thread = userdata;
>   	snd_pcm_t *playback, *capture;
> -	GList *outq = NULL;
> -	struct sco_options  opts;
> +	struct sco_options opts;
>   	struct pollfd fds[8];
>
> -	DBG("thread started");
> +	DBG("thread started: rate %d", thread->rate);
> +
> +	if (thread->init(thread)<  0)
> +		return NULL;
>
> -	capture = hfp_audio_pcm_init(SND_PCM_STREAM_CAPTURE);
> +	capture = hfp_audio_pcm_init(SND_PCM_STREAM_CAPTURE, thread->rate);
>   	if (!capture)
>   		return NULL;
>
> -	playback = hfp_audio_pcm_init(SND_PCM_STREAM_PLAYBACK);
> +	playback = hfp_audio_pcm_init(SND_PCM_STREAM_PLAYBACK, thread->rate);
>   	if (!playback) {
>   		snd_pcm_close(capture);
>   		return NULL;
>   	}
>
> +	thread->capture_buffer = g_try_malloc(thread->capture_size);
> +	if (!thread->capture_buffer) {
> +		snd_pcm_close(capture);
> +		snd_pcm_close(playback);
> +		return NULL;
> +	}
> +
>   	/* Force defered setup */
> -	if (read(hcon->fd,&opts, sizeof(opts))<  0)
> +	if (recv(thread->fd, NULL, 0, 0)<  0)
>   		DBG("Defered setup failed: %d (%s)", errno, strerror(errno));
>
>   	/* Add SCO options
> -	len = sizeof(opts);
> -	if (getsockopt(hcon->fd, SOL_SCO, SCO_OPTIONS,&opts,&len)<  0) {
> -		DBG("getsockopt failed %d", errno);
> -		return NULL;
> -	}
> -	*/
> +	 len = sizeof(opts);
> +	 if (getsockopt(thread->fd, SOL_SCO, SCO_OPTIONS,&opts,&len)<  0) {
> +	 DBG("getsockopt failed %d", errno);
> +	 return NULL;
> +	 }
> +	 */
>   	opts.mtu = 48;
> -	DBG("mtu %d", opts.mtu);
> +	thread->mtu = opts.mtu;
> +	DBG("thread->mtu %d", thread->mtu);
>
> -	while (hcon->running) {
> +	while (thread->running) {
>   		/* Queue alsa captured data (snd_pcm_poll_descriptors failed) */
> -		if (hfp_audio_capture(hcon->fd, capture,&outq, opts.mtu)<  0) {
> +		if (hfp_audio_capture(thread, capture)<  0) {
>   			DBG("Failed to capture");
>   			break;
>   		}
>
>   		memset(fds, 0, sizeof(fds));
> -		fds[0].fd = hcon->fd;
> +		fds[0].fd = thread->fd;
>   		fds[0].events = POLLIN | POLLERR | POLLHUP | POLLNVAL;
>   		if (poll(fds, 1, 200) == 0)
>   			continue;
>
>   		if (fds[0].revents&  (POLLERR | POLLHUP | POLLNVAL)) {
> -			DBG("POLLERR | POLLHUP | POLLNVAL triggered");
> +			DBG("POLLERR | POLLHUP | POLLNVAL triggered (%d)",
> +					fds[0].revents);
>   			break;
>   		}
>
>   		if (!fds[0].revents&  POLLIN)
>   			continue;
>
> -		if (hfp_audio_playback(hcon->fd, playback)<  0) {
> +		if (hfp_audio_playback(thread, playback)<  0) {
>   			DBG("POLLIN triggered, but read error");
>   			break;
>   		}
>
> +		/* DBG("Received and send"); */
> +
>   		/* Dequeue in sync with readings */
> -		pop_outq(hcon->fd,&outq, opts.mtu);
> +		pop_outq(thread);
>   	}
>
>   	DBG("thread terminating");
> +	g_slist_free_full(thread->outq, g_free);
> +	g_free(thread->capture_buffer);
>   	snd_pcm_close(playback);
>   	snd_pcm_close(capture);
> +
> +	thread->free(thread);
> +
>   	return NULL;
>   }
>
>   static int new_connection(int fd, int codec)
>   {
> -	struct hfp_audio_thread *hcon;
> +	struct hfp_thread *thread;
> +
>   	DBG("New connection: fd=%d codec=%d", fd, codec);
> -	hcon = g_try_malloc0(sizeof(struct hfp_audio_thread));
> -	if (hcon == NULL)
> +	thread = g_try_malloc0(sizeof(struct hfp_thread));
> +	if (thread == NULL)
>   		return -ENOMEM;
>
> -	hcon->fd = fd;
> -	hcon->codec = codec;
> -	hcon->running = 1;
> +	thread->fd = fd;
> +	thread->running = 1;
> +
> +	switch (codec) {
> +	case HFP_AUDIO_CVSD:
> +		thread->init = hfp_audio_cvsd_init;
> +		thread->free = hfp_audio_cvsd_free;
> +		thread->decode = hfp_audio_cvsd_decode;
> +		thread->encode = hfp_audio_cvsd_encode;
> +		break;
> +	case HFP_AUDIO_MSBC:
> +		thread->rate = 16000;
> +		thread->init = hfp_audio_msbc_init;
> +		thread->free = hfp_audio_msbc_free;
> +		thread->decode = hfp_audio_msbc_decode;
> +		thread->encode = hfp_audio_msbc_encode;
> +		break;
> +	}
>
> -	if (pthread_create(&hcon->thread, NULL, thread_func, hcon)<  0)
> -		goto fail;
> +	if (pthread_create(&thread->thread, NULL, thread_func, thread)<  0) {
> +		hfp_audio_thread_free(thread);
> +		return -EINVAL;
> +	}
>
>   	/* FIXME thread is not detached until we quit */
>
> -	threads = g_slist_prepend(threads, hcon);
> +	threads = g_slist_prepend(threads, thread);
>   	return 0;
> -fail:
> -	hfp_audio_thread_free(hcon);
> -	return -EINVAL;
>   }
>
>   static DBusMessage *agent_newconnection(DBusConnection *conn, DBusMessage *msg,
> -					void *data)
> +								void *data)

Why is this needed?

>   {
>   	const char *card;
>   	int fd;
> @@ -321,9 +601,8 @@ static DBusMessage *agent_newconnection(DBusConnection *conn, DBusMessage *msg,
>   	DBG("New connection");
>
>   	if (dbus_message_get_args(msg, NULL, DBUS_TYPE_OBJECT_PATH,&card,
> -						DBUS_TYPE_UNIX_FD,&fd,
> -						DBUS_TYPE_BYTE,&codec,
> -						DBUS_TYPE_INVALID) == FALSE)
> +			DBUS_TYPE_UNIX_FD,&fd, DBUS_TYPE_BYTE,&codec,
> +			DBUS_TYPE_INVALID) == FALSE)

Why is this needed?

>   		return g_dbus_create_error(msg,
>   				HFP_AUDIO_AGENT_INTERFACE ".InvalidArguments",
>   				"Invalid arguments");
> @@ -345,7 +624,7 @@ fail:
>   }
>
>   static DBusMessage *agent_release(DBusConnection *conn, DBusMessage *msg,
> -					void *data)
> +		void *data)
>   {
>   	DBG("HFP audio agent released");
>   	/* agent will be registered on next oFono startup */
> @@ -368,8 +647,8 @@ static void hfp_audio_agent_register_reply(DBusPendingCall *call, void *data)
>   	dbus_error_init(&err);
>
>   	if (dbus_set_error_from_message(&err, reply) == TRUE) {
> -		DBG("Failed to register audio agent (%s: %s)", err.name,
> -								err.message);
> +		DBG("Failed to register audio agent (%s: %s)",
> +				err.name, err.message);

Or this?

>   		dbus_error_free(&err);
>   	} else {
>   		DBG("HFP audio agent registered");
> @@ -390,9 +669,8 @@ static void hfp_audio_agent_register(DBusConnection *conn)
>   	DBG("Registering audio agent in oFono");
>
>   	msg = dbus_message_new_method_call(OFONO_SERVICE,
> -						HFP_AUDIO_MANAGER_PATH,
> -						HFP_AUDIO_MANAGER_INTERFACE,
> -						"Register");
> +			HFP_AUDIO_MANAGER_PATH, HFP_AUDIO_MANAGER_INTERFACE,
> +			"Register");

Or this?

>   	if (msg == NULL) {
>   		DBG("Not enough memory");
>   		return;
> @@ -405,8 +683,8 @@ static void hfp_audio_agent_register(DBusConnection *conn)
>   		codecs[ncodecs++] = HFP_AUDIO_MSBC;
>
>   	dbus_message_append_args(msg, DBUS_TYPE_OBJECT_PATH,&path,
> -					DBUS_TYPE_ARRAY, DBUS_TYPE_BYTE,
> -					&pcodecs, ncodecs, DBUS_TYPE_INVALID);
> +			DBUS_TYPE_ARRAY, DBUS_TYPE_BYTE,&pcodecs, ncodecs,
> +			DBUS_TYPE_INVALID);
>
>   	if (!dbus_connection_send_with_reply(conn, msg,&call, -1)) {
>   		dbus_message_unref(msg);
> @@ -416,8 +694,8 @@ static void hfp_audio_agent_register(DBusConnection *conn)
>
>   	dbus_message_unref(msg);
>
> -	dbus_pending_call_set_notify(call, hfp_audio_agent_register_reply,
> -						NULL, NULL);
> +	dbus_pending_call_set_notify(call, hfp_audio_agent_register_reply, NULL,
> +			NULL);
>
>   	dbus_pending_call_unref(call);
>   }
> @@ -483,30 +761,32 @@ static int sco_listen_watch()
>
>   	if (bind(sk, (struct sockaddr *)&saddr, sizeof(saddr))<  0) {
>   		DBG("Can't bind socket: %s (%d)", strerror(errno), errno);
> -		goto error;
> +		goto fail;
>   	}
>
>   	/* Enable deferred setup */
> -	if (option_defer&&  setsockopt(sk, SOL_BLUETOOTH, BT_DEFER_SETUP,
> -				&option_defer, sizeof(option_defer))<  0) {
> +	if (option_defer
> +			&&  setsockopt(sk, SOL_BLUETOOTH, BT_DEFER_SETUP,
> +					&option_defer, sizeof(option_defer))
> +					<  0) {
>   		DBG("Can't defer setup : %s (%d)", strerror(errno), errno);
> -		goto error;
> +		goto fail;
>   	}
>
>   	/* Listen for connections */
>   	if (listen(sk, 10)) {
>   		DBG("Can not listen socket: %s (%d)", strerror(errno), errno);
> -		goto error;
> +		goto fail;
>   	}
>
>   	DBG("Waiting for connection ...");
>   	io = g_io_channel_unix_new(sk);
>   	if (!io)
> -		goto error;
> +		goto fail;
>
>   	return g_io_add_watch(io, G_IO_IN, sco_accept_cb, NULL);
>
> -error:
> +fail:
>   	close(sk);
>   	return -1;

All of these should be merged into previous patches and not be part of 
this patch.

>   }
> @@ -530,7 +810,7 @@ static int sco_connect_watch()
>
>   	if (bind(sk, (struct sockaddr *)&saddr, sizeof(saddr))<  0) {
>   		DBG("Can't bind socket: %s (%d)", strerror(errno), errno);
> -		goto error;
> +		goto fail;
>   	}
>
>   	/* Connect to remote address */
> @@ -539,17 +819,17 @@ static int sco_connect_watch()
>   	btstr2ba(option_client_addr,&saddr.sco_bdaddr);
>   	if (connect(sk, (struct sockaddr *)&saddr, sizeof(saddr))) {
>   		DBG("Can not connect socket: %s (%d)", strerror(errno), errno);
> -		goto error;
> +		goto fail;
>   	}
>
>   	DBG("Connecting to %s...", option_client_addr);
>   	io = g_io_channel_unix_new(sk);
>   	if (!io)
> -		goto error;
> +		goto fail;
>
> -	return g_io_add_watch(io, G_IO_IN|G_IO_OUT, sco_connect_cb, NULL);
> +	return g_io_add_watch(io, G_IO_IN | G_IO_OUT, sco_connect_cb, NULL);
>
> -error:
> +fail:
>   	close(sk);
>   	return -1;
>   }
> @@ -559,9 +839,8 @@ static void hfp_audio_agent_create(DBusConnection *conn)
>   	DBG("Registering audio agent on DBUS");
>
>   	if (!g_dbus_register_interface(conn, HFP_AUDIO_AGENT_PATH,
> -					HFP_AUDIO_AGENT_INTERFACE,
> -					agent_methods, NULL, NULL,
> -					NULL, NULL)) {
> +			HFP_AUDIO_AGENT_INTERFACE, agent_methods, NULL, NULL,
> +			NULL, NULL)) {
>   		DBG("Unable to create local agent");
>   		g_main_loop_quit(main_loop);
>   	}
> @@ -572,7 +851,7 @@ static void hfp_audio_agent_destroy(DBusConnection *conn)
>   	DBG("Unregistering audio agent on DBUS");
>
>   	g_dbus_unregister_interface(conn, HFP_AUDIO_AGENT_PATH,
> -						HFP_AUDIO_AGENT_INTERFACE);
> +			HFP_AUDIO_AGENT_INTERFACE);
>   }
>
>   static void ofono_connect(DBusConnection *conn, void *user_data)

Same as above.

Regards,
-Denis