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

[=?unknown-8bit?q?Fr=C3=A9d=C3=A9ric?= Dalleau <frederic.dalleau@linux.intel.com>]

[-- 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