From a6b131fb47ad84c95e69d73bae60d3c9253a9b0a Mon Sep 17 00:00:00 2001
From: Szymon Janc <szymon.janc@tieto.com>
Date: Mon, 3 Mar 2014 00:50:48 +0100
Subject: [PATCH] android/handsfree: Allow to connect to HSP or HFP handsfree
 unit

This allows to connect to HSP (it HFP is not enabled) or fallback to
HSP if HFP is not supported by remote device.
---
 android/handsfree.c | 113 +++++++++++++++++++++++++++++++++++++++++---
 1 file changed, 106 insertions(+), 7 deletions(-)

diff --git a/android/handsfree.c b/android/handsfree.c
index 0b97a5d2a..6149c62de 100644
--- a/android/handsfree.c
+++ b/android/handsfree.c
@@ -184,7 +184,7 @@ drop:
 	g_io_channel_shutdown(chan, TRUE, NULL);
 }
 
-static void sdp_search_cb(sdp_list_t *recs, int err, gpointer data)
+static void sdp_hsp_search_cb(sdp_list_t *recs, int err, gpointer data)
 {
 	sdp_list_t *protos, *classes;
 	GError *gerr = NULL;
@@ -195,12 +195,13 @@ static void sdp_search_cb(sdp_list_t *recs, int err, gpointer data)
 	DBG("");
 
 	if (err < 0) {
-		error("handsfree: unable to get SDP record: %s", strerror(-err));
+		error("handsfree: unable to get SDP record: %s",
+								strerror(-err));
 		goto fail;
 	}
 
 	if (!recs || !recs->data) {
-		error("handsfree: no SDP records found");
+		info("handsfree: no HSP SDP records found");
 		goto fail;
 	}
 
@@ -215,6 +216,94 @@ static void sdp_search_cb(sdp_list_t *recs, int err, gpointer data)
 		goto fail;
 	}
 
+	/* TODO read remote version? */
+	/* TODO read volume control support */
+
+	memcpy(&uuid, classes->data, sizeof(uuid));
+	sdp_list_free(classes, free);
+
+	if (!sdp_uuid128_to_uuid(&uuid) || uuid.type != SDP_UUID16 ||
+			uuid.value.uuid16 != HEADSET_SVCLASS_ID) {
+		sdp_list_free(protos, NULL);
+		error("handsfree: invalid service record or not HSP");
+		goto fail;
+	}
+
+	channel = sdp_get_proto_port(protos, RFCOMM_UUID);
+	sdp_list_foreach(protos, (sdp_list_func_t) sdp_list_free, NULL);
+	sdp_list_free(protos, NULL);
+	if (channel <= 0) {
+		error("handsfree: unable to get RFCOMM channel from record");
+		goto fail;
+	}
+
+	io = bt_io_connect(connect_cb, NULL, NULL, &gerr,
+				BT_IO_OPT_SOURCE_BDADDR, &adapter_addr,
+				BT_IO_OPT_DEST_BDADDR, &device.bdaddr,
+				BT_IO_OPT_SEC_LEVEL, BT_IO_SEC_MEDIUM,
+				BT_IO_OPT_CHANNEL, channel,
+				BT_IO_OPT_INVALID);
+	if (!io) {
+		error("handsfree: unable to connect: %s", gerr->message);
+		g_error_free(gerr);
+		goto fail;
+	}
+
+	g_io_channel_unref(io);
+	return;
+
+fail:
+	device_cleanup();
+}
+
+static int sdp_search_hsp(void)
+{
+	uuid_t uuid;
+
+	sdp_uuid16_create(&uuid, HEADSET_SVCLASS_ID);
+
+	return bt_search_service(&adapter_addr, &device.bdaddr, &uuid,
+					sdp_hsp_search_cb, NULL, NULL, 0);
+}
+
+static void sdp_hfp_search_cb(sdp_list_t *recs, int err, gpointer data)
+{
+	sdp_list_t *protos, *classes;
+	GError *gerr = NULL;
+	GIOChannel *io;
+	uuid_t uuid;
+	int channel;
+
+	DBG("");
+
+	if (err < 0) {
+		error("handsfree: unable to get SDP record: %s",
+								strerror(-err));
+		goto fail;
+	}
+
+	if (!recs || !recs->data) {
+		info("handsfree: no HFP SDP records found, trying HSP");
+
+		if (sdp_search_hsp() < 0) {
+			error("handsfree: HSP SDP search failed");
+			goto fail;
+		}
+
+		return;
+	}
+
+	if (sdp_get_service_classes(recs->data, &classes) < 0) {
+		error("handsfree: unable to get service classes from record");
+		goto fail;
+	}
+
+	if (sdp_get_access_protos(recs->data, &protos) < 0) {
+		error("handsfree: unable to get access protocols from record");
+		sdp_list_free(classes, free);
+		goto fail;
+	}
+
 	/* TODO read remote version? */
 
 	memcpy(&uuid, classes->data, sizeof(uuid));
@@ -254,13 +343,23 @@ fail:
 	device_cleanup();
 }
 
+static int sdp_search_hfp(void)
+{
+	uuid_t uuid;
+
+	sdp_uuid16_create(&uuid, HANDSFREE_SVCLASS_ID);
+
+	return bt_search_service(&adapter_addr, &device.bdaddr, &uuid,
+					sdp_hfp_search_cb, NULL, NULL, 0);
+}
+
 static void handle_connect(const void *buf, uint16_t len)
 {
 	const struct hal_cmd_handsfree_connect *cmd = buf;
 	char addr[18];
 	uint8_t status;
-	uuid_t uuid;
 	bdaddr_t bdaddr;
+	int ret;
 
 	DBG("");
 
@@ -276,9 +375,9 @@ static void handle_connect(const void *buf, uint16_t len)
 
 	device_init(&bdaddr);
 
-	sdp_uuid16_create(&uuid, HANDSFREE_SVCLASS_ID);
-	if (bt_search_service(&adapter_addr, &device.bdaddr, &uuid,
-					sdp_search_cb, NULL, NULL, 0) < 0) {
+	/* prefer HFP over HSP */
+	ret = hfp_server ? sdp_search_hfp() : sdp_search_hsp();
+	if (ret < 0) {
 		error("handsfree: SDP search failed");
 		device_cleanup();
 		status = HAL_STATUS_FAILED;
-- 
2.47.3