[PATCH 1/5] bluez5: Add SCO server register/unregister

[PATCH 1/5] bluez5: Add SCO server register/unregister

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

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

SCO server should be shared by both HFP HF and HFP AG plugins.
---
 plugins/bluez5.c        |  117 +++++++++++++++++++++++++++++++++++++++++++++++
 plugins/bluez5.h        |    5 ++
 plugins/hfp_hf_bluez5.c |   72 ++---------------------------
 3 files changed, 127 insertions(+), 67 deletions(-)

diff --git a/plugins/bluez5.c b/plugins/bluez5.c
index d5c566e..5d10f44 100644
--- a/plugins/bluez5.c
+++ b/plugins/bluez5.c
@@ -26,6 +26,8 @@
 #include <errno.h>
 #include <stdint.h>
 #include <stdio.h>
+#include <unistd.h>
+#include <fcntl.h>
 #include <sys/socket.h>
 #include <string.h>
 
@@ -41,6 +43,9 @@
 
 #define BLUEZ_PROFILE_MGMT_INTERFACE   BLUEZ_SERVICE ".ProfileManager1"
 
+static guint sco_watch;
+static GSList *sco_cbs;
+
 void bt_bacpy(bdaddr_t *dst, const bdaddr_t *src)
 {
 	memcpy(dst, src, sizeof(bdaddr_t));
@@ -57,6 +62,118 @@ int bt_bacmp(const bdaddr_t *ba1, const bdaddr_t *ba2)
 	return memcmp(ba1, ba2, sizeof(bdaddr_t));
 }
 
+static gboolean sco_accept(GIOChannel *io, GIOCondition cond,
+							gpointer user_data)
+{
+	struct sockaddr_sco saddr;
+	socklen_t alen;
+	int sk, nsk;
+	GSList *l;
+
+	if (cond & (G_IO_ERR | G_IO_HUP | G_IO_NVAL))
+		return FALSE;
+
+	sk = g_io_channel_unix_get_fd(io);
+
+	memset(&saddr, 0, sizeof(saddr));
+	alen = sizeof(saddr);
+
+	nsk = accept(sk, (struct sockaddr *) &saddr, &alen);
+	if (nsk < 0)
+		return TRUE;
+
+	for (l = sco_cbs; l; l = l->next) {
+		bt_sco_accept_cb cb = l->data;
+
+		if (cb(nsk, &saddr))
+			return TRUE;
+	}
+
+	ofono_warn("No SCO callback for incoming connection");
+	close(nsk);
+
+	return TRUE;
+}
+
+static int sco_init(void)
+{
+	GIOChannel *sco_io;
+	struct sockaddr_sco saddr;
+	int sk, defer_setup = 1;
+
+	sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET | O_NONBLOCK | SOCK_CLOEXEC,
+								BTPROTO_SCO);
+	if (sk < 0)
+		return -errno;
+
+	/* Bind to local address */
+	memset(&saddr, 0, sizeof(saddr));
+	saddr.sco_family = AF_BLUETOOTH;
+	bt_bacpy(&saddr.sco_bdaddr, BDADDR_ANY);
+
+	if (bind(sk, (struct sockaddr *) &saddr, sizeof(saddr)) < 0) {
+		close(sk);
+		return -errno;
+	}
+
+	if (setsockopt(sk, SOL_BLUETOOTH, BT_DEFER_SETUP,
+				&defer_setup, sizeof(defer_setup)) < 0)
+		ofono_warn("Can't enable deferred setup: %s (%d)",
+						strerror(errno), errno);
+
+	if (listen(sk, 5) < 0) {
+		close(sk);
+		return -errno;
+	}
+
+	sco_io = g_io_channel_unix_new(sk);
+	g_io_channel_set_close_on_unref(sco_io, TRUE);
+
+	sco_watch = g_io_add_watch(sco_io,
+				G_IO_IN | G_IO_ERR | G_IO_HUP | G_IO_NVAL,
+				sco_accept, NULL);
+
+	g_io_channel_unref(sco_io);
+
+	return 0;
+}
+
+int bt_register_sco_server(bt_sco_accept_cb cb)
+{
+	int err;
+
+	if (!cb) {
+		ofono_error("SCO: invalid callback");
+		return -1;
+	}
+
+	if (!sco_cbs) {
+		err = sco_init();
+		if (err < 0) {
+			ofono_error("SCO: %s(%d)", strerror(-err), -err);
+			return err;
+		}
+	}
+
+	sco_cbs = g_slist_append(sco_cbs, cb);
+
+	return 0;
+}
+
+void bt_unregister_sco_server(bt_sco_accept_cb cb)
+{
+	if (!cb) {
+		ofono_error("SCO: invalid callback");
+		return;
+	}
+
+	sco_cbs = g_slist_remove(sco_cbs, cb);
+	if (sco_cbs)
+		return;
+
+	g_source_remove(sco_watch);
+}
+
 static void profile_register_cb(DBusPendingCall *call, gpointer user_data)
 {
 	DBusMessage *reply;
diff --git a/plugins/bluez5.h b/plugins/bluez5.h
index cdbfe72..29eba12 100644
--- a/plugins/bluez5.h
+++ b/plugins/bluez5.h
@@ -69,6 +69,11 @@ int bt_ba2str(const bdaddr_t *ba, char *str);
 
 int bt_bacmp(const bdaddr_t *ba1, const bdaddr_t *ba2);
 
+typedef gboolean (*bt_sco_accept_cb)(int fd, struct sockaddr_sco *saddr);
+
+int bt_register_sco_server(bt_sco_accept_cb cb);
+void bt_unregister_sco_server(bt_sco_accept_cb cb);
+
 int bluetooth_register_profile(DBusConnection *conn, const char *uuid,
 					const char *name, const char *object);
 
diff --git a/plugins/hfp_hf_bluez5.c b/plugins/hfp_hf_bluez5.c
index 2f4a89e..a8738bd 100644
--- a/plugins/hfp_hf_bluez5.c
+++ b/plugins/hfp_hf_bluez5.c
@@ -65,7 +65,6 @@ struct hfp {
 
 static GHashTable *modem_hash = NULL;
 static GDBusClient *bluez = NULL;
-static guint sco_watch = 0;
 
 static void hfp_debug(const char *str, void *user_data)
 {
@@ -360,69 +359,11 @@ static const GDBusMethodTable profile_methods[] = {
 	{ }
 };
 
-static gboolean sco_accept(GIOChannel *io, GIOCondition cond,
-							gpointer user_data)
+static gboolean hfp_hf_sco_accept(int fd, struct sockaddr_sco *saddr)
 {
-	struct sockaddr_sco saddr;
-	socklen_t alen;
-	int sk, nsk;
-
-	if (cond & (G_IO_ERR | G_IO_HUP | G_IO_NVAL))
-		return FALSE;
-
-	sk = g_io_channel_unix_get_fd(io);
-
-	memset(&saddr, 0, sizeof(saddr));
-	alen = sizeof(saddr);
-
-	nsk = accept(sk, (struct sockaddr *) &saddr, &alen);
-	if (nsk < 0)
-		return TRUE;
-
 	/* TODO: Verify if the device has a modem */
 
-	return TRUE;
-}
-
-static int sco_init(void)
-{
-	GIOChannel *sco_io;
-	struct sockaddr_sco saddr;
-	int sk, defer_setup = 1;
-
-	sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET | O_NONBLOCK | SOCK_CLOEXEC,
-								BTPROTO_SCO);
-	if (sk < 0)
-		return -errno;
-
-	/* Bind to local address */
-	memset(&saddr, 0, sizeof(saddr));
-	saddr.sco_family = AF_BLUETOOTH;
-	bt_bacpy(&saddr.sco_bdaddr, BDADDR_ANY);
-
-	if (bind(sk, (struct sockaddr *) &saddr, sizeof(saddr)) < 0) {
-		close(sk);
-		return -errno;
-	}
-
-	if (setsockopt(sk, SOL_BLUETOOTH, BT_DEFER_SETUP,
-				&defer_setup, sizeof(defer_setup)) < 0)
-		ofono_warn("Can't enable deferred setup: %s (%d)",
-						strerror(errno), errno);
-
-	if (listen(sk, 5) < 0) {
-		close(sk);
-		return -errno;
-	}
-
-	sco_io = g_io_channel_unix_new(sk);
-	sco_watch = g_io_add_watch(sco_io,
-				G_IO_IN | G_IO_ERR | G_IO_HUP | G_IO_NVAL,
-				sco_accept, NULL);
-
-	g_io_channel_unref(sco_io);
-
-	return 0;
+	return FALSE;
 }
 
 static void connect_handler(DBusConnection *conn, void *user_data)
@@ -561,11 +502,9 @@ static int hfp_init(void)
 	if (DBUS_TYPE_UNIX_FD < 0)
 		return -EBADF;
 
-	err = sco_init();
-	if (err < 0) {
-		ofono_error("SCO: %s(%d)", strerror(-err), -err);
+	err = bt_register_sco_server(hfp_hf_sco_accept);
+	if (err < 0)
 		return err;
-	}
 
 	/* Registers External Profile handler */
 	if (!g_dbus_register_interface(conn, HFP_EXT_PROFILE_PATH,
@@ -614,8 +553,7 @@ static void hfp_exit(void)
 
 	g_hash_table_destroy(modem_hash);
 
-	if (sco_watch > 0)
-		g_source_remove(sco_watch);
+	bt_unregister_sco_server(hfp_hf_sco_accept);
 }
 
 OFONO_PLUGIN_DEFINE(hfp_bluez5, "External Hands-Free Profile Plugin", VERSION,
-- 
1.7.9.5

[PATCH 2/5] include: Add set/get data APIs to emulator

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

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

---
 include/emulator.h |    5 +++++
 1 file changed, 5 insertions(+)

diff --git a/include/emulator.h b/include/emulator.h
index 5cd894b..6dc1938 100644
--- a/include/emulator.h
+++ b/include/emulator.h
@@ -74,6 +74,11 @@ void ofono_emulator_register(struct ofono_emulator *em, int fd);
 
 void ofono_emulator_remove(struct ofono_emulator *em);
 
+void ofono_emulator_set_data(struct ofono_emulator *em, void *data,
+						ofono_destroy_func destroy);
+
+void *ofono_emulator_get_data(struct ofono_emulator *em);
+
 void ofono_emulator_send_final(struct ofono_emulator *em,
 				const struct ofono_error *final);
 void ofono_emulator_send_unsolicited(struct ofono_emulator *em,
-- 
1.7.9.5

[PATCH 3/5] emulator: Add set/get data APIs

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

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

---
 src/emulator.c |   17 +++++++++++++++++
 1 file changed, 17 insertions(+)

diff --git a/src/emulator.c b/src/emulator.c
index c3165eb..14a39f3 100644
--- a/src/emulator.c
+++ b/src/emulator.c
@@ -52,6 +52,8 @@ struct ofono_emulator {
 	gboolean clip;
 	gboolean ccwa;
 	int pns_id;
+	void *data;
+	ofono_destroy_func destroy;
 };
 
 struct indicator {
@@ -951,6 +953,9 @@ static void emulator_remove(struct ofono_atom *atom)
 
 	DBG("atom: %p", atom);
 
+	if (em->destroy)
+		em->destroy(em->data);
+
 	g_free(em);
 }
 
@@ -994,6 +999,18 @@ void ofono_emulator_remove(struct ofono_emulator *em)
 	__ofono_atom_free(em->atom);
 }
 
+void ofono_emulator_set_data(struct ofono_emulator *em, void *data,
+						ofono_destroy_func destroy)
+{
+	em->data = data;
+	em->destroy = destroy;
+}
+
+void *ofono_emulator_get_data(struct ofono_emulator *em)
+{
+	return em->data;
+}
+
 void ofono_emulator_send_final(struct ofono_emulator *em,
 				const struct ofono_error *final)
 {
-- 
1.7.9.5

[PATCH 4/5] hfp_ag_bluez5: Add list of HFP AG emulators

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

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

This will be used to retrieve hfp_ag related to the incoming SCO
---
 plugins/hfp_ag_bluez5.c |   56 +++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 56 insertions(+)

diff --git a/plugins/hfp_ag_bluez5.c b/plugins/hfp_ag_bluez5.c
index 80dd601..c6b858e 100644
--- a/plugins/hfp_ag_bluez5.c
+++ b/plugins/hfp_ag_bluez5.c
@@ -44,9 +44,29 @@
 
 #define HFP_AG_EXT_PROFILE_PATH   "/bluetooth/profile/hfp_ag"
 
+struct hfp_ag {
+	struct ofono_emulator *em;
+	bdaddr_t local;
+	bdaddr_t peer;
+};
+
 static guint modemwatch_id;
 static GList *modems;
 static GHashTable *sim_hash = NULL;
+static GSList *hfp_ags;
+
+static void free_hfp_ag(void *data)
+{
+	struct hfp_ag *hfp_ag = data;
+
+	DBG("");
+
+	if (hfp_ag == NULL)
+		return;
+
+	hfp_ags = g_slist_remove(hfp_ags, hfp_ag);
+	g_free(hfp_ag);
+}
 
 static DBusMessage *profile_new_connection(DBusConnection *conn,
 						DBusMessage *msg, void *data)
@@ -54,8 +74,13 @@ static DBusMessage *profile_new_connection(DBusConnection *conn,
 	DBusMessageIter entry;
 	const char *device;
 	int fd;
+	struct sockaddr_rc saddr;
+	bdaddr_t local;
+	bdaddr_t peer;
+	socklen_t optlen;
 	struct ofono_emulator *em;
 	struct ofono_modem *modem;
+	struct hfp_ag *hfp_ag;
 
 	DBG("Profile handler NewConnection");
 
@@ -79,6 +104,26 @@ static DBusMessage *profile_new_connection(DBusConnection *conn,
 
 	DBG("%s", device);
 
+	memset(&saddr, 0, sizeof(saddr));
+	optlen = sizeof(saddr);
+	if (getsockname(fd, (struct sockaddr *) &saddr, &optlen) < 0) {
+		ofono_error("RFCOMM getsockname(): %s (%d)", strerror(errno),
+									errno);
+		goto error;
+	}
+
+	local = saddr.rc_bdaddr;
+
+	memset(&saddr, 0, sizeof(saddr));
+	optlen = sizeof(saddr);
+	if (getpeername(fd, (struct sockaddr *) &saddr, &optlen) < 0) {
+		ofono_error("RFCOMM getpeername(): %s (%d)", strerror(errno),
+									errno);
+		goto error;
+	}
+
+	peer = saddr.rc_bdaddr;
+
 	/* Pick the first voicecall capable modem */
 	modem = modems->data;
 	if (modem == NULL) {
@@ -100,8 +145,19 @@ static DBusMessage *profile_new_connection(DBusConnection *conn,
 
 	ofono_emulator_register(em, fd);
 
+	hfp_ag = g_new0(struct hfp_ag, 1);
+	hfp_ag->em = em;
+	hfp_ag->local = local;
+	hfp_ag->peer = peer;
+	ofono_emulator_set_data(em, hfp_ag, free_hfp_ag);
+
+	hfp_ags = g_slist_append(hfp_ags, hfp_ag);
+
 	return dbus_message_new_method_return(msg);
 
+error:
+	close(fd);
+
 invalid:
 	return g_dbus_create_error(msg, BLUEZ_ERROR_INTERFACE ".Rejected",
 					"Invalid arguments in method call");
-- 
1.7.9.5

[PATCH 5/5] hfp_ag_bluez5: Add SCO listen socket

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

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

This patch adds the initial SCO server socket handling.
---
 plugins/hfp_ag_bluez5.c |   83 +++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 83 insertions(+)

diff --git a/plugins/hfp_ag_bluez5.c b/plugins/hfp_ag_bluez5.c
index c6b858e..ee8c8bf 100644
--- a/plugins/hfp_ag_bluez5.c
+++ b/plugins/hfp_ag_bluez5.c
@@ -48,6 +48,7 @@ struct hfp_ag {
 	struct ofono_emulator *em;
 	bdaddr_t local;
 	bdaddr_t peer;
+	guint sco_watch;
 };
 
 static guint modemwatch_id;
@@ -64,6 +65,9 @@ static void free_hfp_ag(void *data)
 	if (hfp_ag == NULL)
 		return;
 
+	if (hfp_ag->sco_watch)
+		g_source_remove(hfp_ag->sco_watch);
+
 	hfp_ags = g_slist_remove(hfp_ags, hfp_ag);
 	g_free(hfp_ag);
 }
@@ -206,6 +210,82 @@ static const GDBusMethodTable profile_methods[] = {
 	{ }
 };
 
+static gboolean sco_cb(GIOChannel *io, GIOCondition cond,
+							gpointer user_data)
+
+{
+	struct hfp_ag *hfp_ag = user_data;
+	char adapter_address[18];
+	char device_address[18];
+
+	if (cond & (G_IO_ERR | G_IO_HUP | G_IO_NVAL)) {
+		hfp_ag->sco_watch = 0;
+
+		bt_ba2str(&hfp_ag->local, adapter_address);
+		bt_ba2str(&hfp_ag->peer, device_address);
+		DBG("SCO: %s - %s (closed)", adapter_address, device_address);
+
+		return FALSE;
+	}
+
+	return TRUE;
+}
+
+static gboolean hfp_ag_sco_accept(int fd, struct sockaddr_sco *raddr)
+{
+	struct sockaddr_sco laddr;
+	socklen_t optlen;
+	GSList *l;
+	char adapter_address[18];
+	char device_address[18];
+	struct hfp_ag *hfp_ag = NULL;
+	GIOChannel *sco_io;
+	int err;
+
+	memset(&laddr, 0, sizeof(laddr));
+	optlen = sizeof(laddr);
+	if (getsockname(fd, (struct sockaddr *) &laddr, &optlen) < 0) {
+		err = errno;
+		ofono_error("SCO getsockname(): %s (%d)", strerror(err), err);
+		return FALSE;
+	}
+
+	for (l = hfp_ags; l; l = l->next) {
+		struct hfp_ag *tmp = l->data;
+
+		if (bt_bacmp(&laddr.sco_bdaddr, &tmp->local) != 0)
+			continue;
+
+		if (bt_bacmp(&raddr->sco_bdaddr, &tmp->peer) != 0)
+			continue;
+
+		hfp_ag = tmp;
+		break;
+	}
+
+	if (!hfp_ag) {
+		ofono_error("Rejecting SCO: SLC connection missing!");
+		return FALSE;
+	}
+
+	bt_ba2str(&laddr.sco_bdaddr, adapter_address);
+	bt_ba2str(&raddr->sco_bdaddr, device_address);
+	DBG("SCO: %s < %s (incoming)", adapter_address, device_address);
+
+	sco_io = g_io_channel_unix_new(fd);
+
+	g_io_channel_set_close_on_unref(sco_io, TRUE);
+	g_io_channel_set_flags(sco_io, G_IO_FLAG_NONBLOCK, NULL);
+
+	hfp_ag->sco_watch = g_io_add_watch(sco_io,
+						G_IO_ERR | G_IO_HUP | G_IO_NVAL,
+						sco_cb, hfp_ag);
+
+	g_io_channel_unref(sco_io);
+
+	return TRUE;
+}
+
 static void sim_state_watch(enum ofono_sim_state new_state, void *data)
 {
 	struct ofono_modem *modem = data;
@@ -219,6 +299,7 @@ static void sim_state_watch(enum ofono_sim_state new_state, void *data)
 		if (modems != NULL)
 			return;
 
+		bt_unregister_sco_server(hfp_ag_sco_accept);
 		bluetooth_unregister_profile(conn, HFP_AG_EXT_PROFILE_PATH);
 
 		return;
@@ -234,6 +315,8 @@ static void sim_state_watch(enum ofono_sim_state new_state, void *data)
 
 	bluetooth_register_profile(conn, HFP_AG_UUID, "hfp_ag",
 						HFP_AG_EXT_PROFILE_PATH);
+
+	bt_register_sco_server(hfp_ag_sco_accept);
 }
 
 static gboolean sim_watch_remove(gpointer key, gpointer value,
-- 
1.7.9.5