Add SCO server socket (needed for HFP)

3 files changed, 90 insertions, 10 deletions

diff --git a/audio/headset.c b/audio/headset.c
index 87d0ee7b..4683c356 100644
--- a/audio/headset.c
+++ b/audio/headset.c
@@ -1877,6 +1877,18 @@ int headset_connect_rfcomm(struct audio_device *dev, GIOChannel *io)
 	return hs->tmp_rfcomm ? 0 : -EINVAL;
 }
 
+int headset_connect_sco(struct audio_device *dev, GIOChannel *io)
+{
+	struct headset *hs = dev->headset;
+
+	if (hs->sco)
+		return -EISCONN;
+
+	hs->sco = io;
+
+	return 0;
+}
+
 int headset_close_rfcomm(struct audio_device *dev)
 {
 	struct headset *hs = dev->headset;
diff --git a/audio/headset.h b/audio/headset.h
index 65c3f411..a211d7dd 100644
--- a/audio/headset.h
+++ b/audio/headset.h
@@ -61,6 +61,7 @@ void set_hfp_active(struct audio_device *dev, gboolean active);
 void headset_set_authorized(struct audio_device *dev);
 int headset_connect_rfcomm(struct audio_device *dev, GIOChannel *chan);
 int headset_close_rfcomm(struct audio_device *dev);
+int headset_connect_sco(struct audio_device *dev, GIOChannel *io);
 
 headset_state_t headset_get_state(struct audio_device *dev);
 void headset_set_state(struct audio_device *dev, headset_state_t state);
diff --git a/audio/manager.c b/audio/manager.c
index f405014c..18f53273 100644
--- a/audio/manager.c
+++ b/audio/manager.c
@@ -31,6 +31,7 @@
 #include <stdarg.h>
 #include <errno.h>
 #include <unistd.h>
+#include <fcntl.h>
 #include <stdint.h>
 #include <sys/stat.h>
 #include <dirent.h>
@@ -93,6 +94,7 @@ struct audio_adapter {
 	GIOChannel *hsp_ag_server;
 	GIOChannel *hfp_ag_server;
 	GIOChannel *hsp_hs_server;
+	GIOChannel *sco_server;
 };
 
 static DBusConnection *connection = NULL;
@@ -468,6 +470,50 @@ static void auth_cb(DBusError *derr, void *user_data)
 	}
 }
 
+static void sco_server_cb(GIOChannel *chan, int err, const bdaddr_t *src,
+			const bdaddr_t *dst, gpointer data)
+{
+	int sk;
+	struct audio_device *device;
+	char addr[18];
+
+	if (err < 0) {
+		error("accept: %s (%d)", strerror(-err), -err);
+		return;
+	}
+
+	device = manager_find_device(dst, NULL, FALSE);
+	if (!device)
+		goto drop;
+
+	if (headset_get_state(device) < HEADSET_STATE_CONNECTED) {
+		debug("Refusing SCO from non-connected headset");
+		goto drop;
+	}
+
+	ba2str(dst, addr);
+
+	if (!get_hfp_active(device)) {
+		error("Refusing non-HFP SCO connect attempt from %s", addr);
+		goto drop;
+	}
+
+	sk = g_io_channel_unix_get_fd(chan);
+	fcntl(sk, F_SETFL, 0);
+
+	if (headset_connect_sco(device, chan) == 0) {
+		debug("Accepted SCO connection from %s", addr);
+		headset_set_state(device, HEADSET_STATE_PLAYING);
+	}
+
+	return;
+
+drop:
+	g_io_channel_close(chan);
+	g_io_channel_unref(chan);
+	return;
+}
+
 static void ag_io_cb(GIOChannel *chan, int err, const bdaddr_t *src,
 			const bdaddr_t *dst, gpointer data)
 {
@@ -560,6 +606,13 @@ static int headset_server_init(struct audio_adapter *adapter)
 			hfp = tmp;
 	}
 
+	adapter->sco_server = bt_sco_listen(&adapter->src, 0, sco_server_cb,
+						adapter);
+	if (!adapter->sco_server) {
+		error("Unable to start SCO server socket");
+		return -1;
+	}
+
 	flags = RFCOMM_LM_AUTH | RFCOMM_LM_ENCRYPT;
 
 	if (master)
@@ -568,20 +621,18 @@ static int headset_server_init(struct audio_adapter *adapter)
 	adapter->hsp_ag_server = bt_rfcomm_listen(&adapter->src, chan, flags,
 							ag_io_cb, adapter);
 	if (!adapter->hsp_ag_server)
-		return -1;
+		goto failed;
 
 	record = hsp_ag_record(chan);
 	if (!record) {
 		error("Unable to allocate new service record");
-		return -1;
+		goto failed;
 	}
 
 	if (add_record_to_server(&adapter->src, record) < 0) {
 		error("Unable to register HS AG service record");
 		sdp_record_free(record);
-		g_io_channel_unref(adapter->hsp_ag_server);
-		adapter->hsp_ag_server = NULL;
-		return -1;
+		goto failed;
 	}
 	adapter->hsp_ag_record_id = record->handle;
 
@@ -595,24 +646,40 @@ static int headset_server_init(struct audio_adapter *adapter)
 	adapter->hfp_ag_server = bt_rfcomm_listen(&adapter->src, chan, flags,
 							ag_io_cb, adapter);
 	if (!adapter->hfp_ag_server)
-		return -1;
+		goto failed;
 
 	record = hfp_ag_record(chan, features);
 	if (!record) {
 		error("Unable to allocate new service record");
-		return -1;
+		goto failed;
 	}
 
 	if (add_record_to_server(&adapter->src, record) < 0) {
 		error("Unable to register HF AG service record");
 		sdp_record_free(record);
-		g_io_channel_unref(adapter->hfp_ag_server);
-		adapter->hfp_ag_server = NULL;
-		return -1;
+		goto failed;
 	}
 	adapter->hfp_ag_record_id = record->handle;
 
 	return 0;
+
+failed:
+	if (adapter->sco_server) {
+		g_io_channel_unref(adapter->sco_server);
+		adapter->sco_server = NULL;
+	}
+
+	if (adapter->hsp_ag_server) {
+		g_io_channel_unref(adapter->hsp_ag_server);
+		adapter->hsp_ag_server = NULL;
+	}
+
+	if (adapter->hfp_ag_server) {
+		g_io_channel_unref(adapter->hfp_ag_server);
+		adapter->hfp_ag_server = NULL;
+	}
+
+	return -1;
 }
 
 static int gateway_server_init(struct audio_adapter *adapter)