[RFC BlueZ 1/5] android/gatt: Fix not detecting device found by Bluetooth HAL

[RFC BlueZ 1/5] android/gatt: Fix not detecting device found by Bluetooth HAL

[Luiz Augusto von Dentz]

From: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>

This is necessary so that devices found during regular discovery can be
handled by gatt HAL.
---
 android/bluetooth.c |  42 ++++++++++++++-------
 android/bluetooth.h |   5 ++-
 android/gatt.c      | 105 ++++++++++++++++++++++++++++++----------------------
 3 files changed, 92 insertions(+), 60 deletions(-)

diff --git a/android/bluetooth.c b/android/bluetooth.c
index d275297..834d687 100644
--- a/android/bluetooth.c
+++ b/android/bluetooth.c
@@ -179,6 +179,7 @@ static struct {
 	uint32_t current_settings;
 	uint32_t supported_settings;
 
+	bool le_scanning;
 	uint8_t cur_discovery_type;
 	uint8_t exp_discovery_type;
 	uint32_t discoverable_timeout;
@@ -1301,10 +1302,8 @@ static void mgmt_discovering_event(uint16_t index, uint16_t length,
 	}
 
 	type = adapter.exp_discovery_type;
-	adapter.exp_discovery_type = SCAN_TYPE_NONE;
-
-	if (type == SCAN_TYPE_NONE && gatt_device_found_cb)
-		type = SCAN_TYPE_LE;
+	adapter.exp_discovery_type = adapter.le_scanning ? SCAN_TYPE_LE :
+								SCAN_TYPE_NONE;
 
 	if (type != SCAN_TYPE_NONE)
 		start_discovery(type);
@@ -3392,42 +3391,57 @@ bool bt_le_set_advertising(bool advertising, bt_le_set_advertising_done cb,
 	return false;
 }
 
+bool bt_le_register(bt_le_device_found cb)
+{
+	if (gatt_device_found_cb)
+		return false;
+
+	gatt_device_found_cb = cb;
+
+	return true;
+}
+
+void bt_le_unregister(void)
+{
+	gatt_device_found_cb = NULL;
+}
+
 bool bt_le_discovery_stop(bt_le_discovery_stopped cb)
 {
+	if (!(adapter.current_settings & MGMT_SETTING_POWERED))
+		return false;
+
+	adapter.le_scanning = false;
+
 	if (adapter.cur_discovery_type != SCAN_TYPE_LE) {
 		if (cb)
 			cb();
 
-		gatt_device_found_cb = NULL;
-
 		return true;
 	}
 
 	if (!stop_discovery(SCAN_TYPE_LE))
 		return false;
 
-	gatt_device_found_cb = NULL;
 	gatt_discovery_stopped_cb = cb;
 	adapter.exp_discovery_type = SCAN_TYPE_NONE;
 
 	return true;
 }
 
-bool bt_le_discovery_start(bt_le_device_found cb)
+bool bt_le_discovery_start(void)
 {
 	if (!(adapter.current_settings & MGMT_SETTING_POWERED))
 		return false;
 
+	adapter.le_scanning = true;
+
 	/* If core is discovering, don't bother */
-	if (adapter.cur_discovery_type != SCAN_TYPE_NONE) {
-		gatt_device_found_cb = cb;
+	if (adapter.cur_discovery_type != SCAN_TYPE_NONE)
 		return true;
-	}
 
-	if (start_discovery(SCAN_TYPE_LE)) {
-		gatt_device_found_cb = cb;
+	if (start_discovery(SCAN_TYPE_LE))
 		return true;
-	}
 
 	return false;
 }
diff --git a/android/bluetooth.h b/android/bluetooth.h
index 2409d46..7c64bab 100644
--- a/android/bluetooth.h
+++ b/android/bluetooth.h
@@ -44,7 +44,10 @@ typedef void (*bt_le_device_found)(const bdaddr_t *addr, uint8_t addr_type,
 					int rssi, uint16_t eir_len,
 					const void *eir, bool discoverable,
 					bool bonded);
-bool bt_le_discovery_start(bt_le_device_found cb);
+bool bt_le_register(bt_le_device_found cb);
+void bt_le_unregister(void);
+
+bool bt_le_discovery_start(void);
 
 typedef void (*bt_le_discovery_stopped)(void);
 bool bt_le_discovery_stop(bt_le_discovery_stopped cb);
diff --git a/android/gatt.c b/android/gatt.c
index c25aed4..f797e7d 100644
--- a/android/gatt.c
+++ b/android/gatt.c
@@ -627,6 +627,47 @@ static void destroy_gatt_app(void *data)
 	free(app);
 }
 
+static void le_device_found_handler(const bdaddr_t *addr, uint8_t addr_type,
+						int rssi, uint16_t eir_len,
+						const void *eir,
+						bool discoverable, bool bonded)
+{
+	uint8_t buf[IPC_MTU];
+	struct hal_ev_gatt_client_scan_result *ev = (void *) buf;
+	struct gatt_device *dev;
+	char bda[18];
+
+	if (!scanning || (!discoverable && !bonded))
+		goto connect;
+
+	ba2str(addr, bda);
+	DBG("LE Device found: %s, rssi: %d, adv_data: %d", bda, rssi, !!eir);
+
+	bdaddr2android(addr, ev->bda);
+	ev->rssi = rssi;
+	ev->len = eir_len;
+
+	memcpy(ev->adv_data, eir, ev->len);
+
+	ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
+						HAL_EV_GATT_CLIENT_SCAN_RESULT,
+						sizeof(*ev) + ev->len, ev);
+
+connect:
+	dev = find_device_by_addr(addr);
+	if (!dev || (dev->state != DEVICE_CONNECT_INIT))
+		return;
+
+	device_set_state(dev, DEVICE_CONNECT_READY);
+	dev->bdaddr_type = addr_type;
+
+	/*
+	 * We are ok to perform connect now. Stop discovery
+	 * and once it is stopped continue with creating ACL
+	 */
+	bt_le_discovery_stop(bt_le_discovery_stop_cb);
+}
+
 static struct gatt_app *register_app(const uint8_t *uuid, gatt_type_t type)
 {
 	static int32_t application_id = 1;
@@ -637,6 +678,10 @@ static struct gatt_app *register_app(const uint8_t *uuid, gatt_type_t type)
 		return NULL;
 	}
 
+	/* Register LE once the first app register */
+	if (queue_isempty(gatt_apps))
+		bt_le_register(le_device_found_handler);
+
 	app = new0(struct gatt_app, 1);
 	if (!app) {
 		error("gatt: Cannot allocate memory for registering app");
@@ -956,47 +1001,6 @@ static struct service *create_service(uint8_t id, bool primary, char *uuid,
 	return s;
 }
 
-static void le_device_found_handler(const bdaddr_t *addr, uint8_t addr_type,
-						int rssi, uint16_t eir_len,
-						const void *eir,
-						bool discoverable, bool bonded)
-{
-	uint8_t buf[IPC_MTU];
-	struct hal_ev_gatt_client_scan_result *ev = (void *) buf;
-	struct gatt_device *dev;
-	char bda[18];
-
-	if (!scanning || (!discoverable && !bonded))
-		goto connect;
-
-	ba2str(addr, bda);
-	DBG("LE Device found: %s, rssi: %d, adv_data: %d", bda, rssi, !!eir);
-
-	bdaddr2android(addr, ev->bda);
-	ev->rssi = rssi;
-	ev->len = eir_len;
-
-	memcpy(ev->adv_data, eir, ev->len);
-
-	ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
-						HAL_EV_GATT_CLIENT_SCAN_RESULT,
-						sizeof(*ev) + ev->len, ev);
-
-connect:
-	dev = find_device_by_addr(addr);
-	if (!dev || (dev->state != DEVICE_CONNECT_INIT))
-		return;
-
-	device_set_state(dev, DEVICE_CONNECT_READY);
-	dev->bdaddr_type = addr_type;
-
-	/*
-	 * We are ok to perform connect now. Stop discovery
-	 * and once it is stopped continue with creating ACL
-	 */
-	bt_le_discovery_stop(bt_le_discovery_stop_cb);
-}
-
 static gboolean disconnected_cb(GIOChannel *io, GIOCondition cond,
 							gpointer user_data)
 {
@@ -1207,7 +1211,7 @@ reply:
 
 	/* Check if we should restart scan */
 	if (scanning)
-		bt_le_discovery_start(le_device_found_handler);
+		bt_le_discovery_start();
 
 	/* FIXME: What to do if discovery won't start here. */
 }
@@ -1293,7 +1297,7 @@ static void handle_client_scan(const void *buf, uint16_t len)
 	}
 
 	/* Turn on scan */
-	if (!bt_le_discovery_start(le_device_found_handler)) {
+	if (!bt_le_discovery_start()) {
 		error("gatt: LE scan switch failed");
 		status = HAL_STATUS_FAILED;
 		goto reply;
@@ -1325,7 +1329,7 @@ static void bt_le_discovery_stop_cb(void)
 
 	/* Check now if there is any device ready to connect */
 	if (connect_next_dev() < 0)
-		bt_le_discovery_start(le_device_found_handler);
+		bt_le_discovery_start();
 }
 
 static struct gatt_device *create_device(const bdaddr_t *addr)
@@ -1434,7 +1438,7 @@ static bool trigger_connection(struct app_connection *connection)
 
 	/* after state change trigger discovering */
 	if (!scanning && (connection->device->state == DEVICE_CONNECT_INIT))
-		if (!bt_le_discovery_start(le_device_found_handler)) {
+		if (!bt_le_discovery_start()) {
 			error("gatt: Could not start scan");
 
 			return false;
@@ -5917,6 +5921,15 @@ bool bt_gatt_register(struct ipc *ipc, const bdaddr_t *addr)
 	if (!start_listening_io())
 		return false;
 
+	if (!bt_le_register(le_device_found_handler)) {
+		error("gatt: bt_le_register failed");
+
+		g_io_channel_unref(listening_io);
+		listening_io = NULL;
+
+		return false;
+	}
+
 	crypto = bt_crypto_new();
 	if (!crypto) {
 		error("gatt: Failed to setup crypto");
@@ -6001,6 +6014,8 @@ void bt_gatt_unregister(void)
 
 	bt_crypto_unref(crypto);
 	crypto = NULL;
+
+	bt_le_unregister();
 }
 
 
-- 
1.9.3

[RFC BlueZ 2/5] android/gatt: Do not stop discovery if scanning is not set

[Luiz Augusto von Dentz]

From: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>

If scanning is not set the discovery might have been initiated by another
HAL so do not stop it.
---
 android/gatt.c | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/android/gatt.c b/android/gatt.c
index f797e7d..8a8419c 100644
--- a/android/gatt.c
+++ b/android/gatt.c
@@ -661,6 +661,11 @@ connect:
 	device_set_state(dev, DEVICE_CONNECT_READY);
 	dev->bdaddr_type = addr_type;
 
+	if (!scanning) {
+		connect_next_dev();
+		return;
+	}
+
 	/*
 	 * We are ok to perform connect now. Stop discovery
 	 * and once it is stopped continue with creating ACL
-- 
1.9.3

[RFC BlueZ 3/5] android/gatt: Create device and connect bonded devices

[Luiz Augusto von Dentz]

From: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>

Devices bonded should be connected so later their services can be
discovered.
---
 android/gatt.c | 618 +++++++++++++++++++++++++++++----------------------------
 1 file changed, 313 insertions(+), 305 deletions(-)

diff --git a/android/gatt.c b/android/gatt.c
index 8a8419c..013337c 100644
--- a/android/gatt.c
+++ b/android/gatt.c
@@ -627,151 +627,99 @@ static void destroy_gatt_app(void *data)
 	free(app);
 }
 
-static void le_device_found_handler(const bdaddr_t *addr, uint8_t addr_type,
-						int rssi, uint16_t eir_len,
-						const void *eir,
-						bool discoverable, bool bonded)
-{
-	uint8_t buf[IPC_MTU];
-	struct hal_ev_gatt_client_scan_result *ev = (void *) buf;
-	struct gatt_device *dev;
-	char bda[18];
-
-	if (!scanning || (!discoverable && !bonded))
-		goto connect;
-
-	ba2str(addr, bda);
-	DBG("LE Device found: %s, rssi: %d, adv_data: %d", bda, rssi, !!eir);
-
-	bdaddr2android(addr, ev->bda);
-	ev->rssi = rssi;
-	ev->len = eir_len;
-
-	memcpy(ev->adv_data, eir, ev->len);
+enum pend_req_state {
+	REQUEST_INIT,
+	REQUEST_PENDING,
+	REQUEST_DONE,
+};
 
-	ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
-						HAL_EV_GATT_CLIENT_SCAN_RESULT,
-						sizeof(*ev) + ev->len, ev);
+struct pending_request {
+	uint16_t handle;
+	int length;
+	uint8_t *value;
+	uint16_t offset;
 
-connect:
-	dev = find_device_by_addr(addr);
-	if (!dev || (dev->state != DEVICE_CONNECT_INIT))
-		return;
+	uint8_t *filter_value;
+	uint16_t filter_vlen;
 
-	device_set_state(dev, DEVICE_CONNECT_READY);
-	dev->bdaddr_type = addr_type;
+	enum pend_req_state state;
+	uint8_t error;
+};
 
-	if (!scanning) {
-		connect_next_dev();
-		return;
-	}
+static void destroy_pending_request(void *data)
+{
+	struct pending_request *entry = data;
 
-	/*
-	 * We are ok to perform connect now. Stop discovery
-	 * and once it is stopped continue with creating ACL
-	 */
-	bt_le_discovery_stop(bt_le_discovery_stop_cb);
+	free(entry->value);
+	free(entry->filter_value);
+	free(entry);
 }
 
-static struct gatt_app *register_app(const uint8_t *uuid, gatt_type_t type)
+static void destroy_device(void *data)
 {
-	static int32_t application_id = 1;
-	struct gatt_app *app;
-
-	if (queue_find(gatt_apps, match_app_by_uuid, uuid)) {
-		error("gatt: app uuid is already on list");
-		return NULL;
-	}
-
-	/* Register LE once the first app register */
-	if (queue_isempty(gatt_apps))
-		bt_le_register(le_device_found_handler);
-
-	app = new0(struct gatt_app, 1);
-	if (!app) {
-		error("gatt: Cannot allocate memory for registering app");
-		return 0;
-	}
-
-	app->type = type;
+	struct gatt_device *dev = data;
 
-	if (app->type == GATT_CLIENT) {
-		app->notifications = queue_new();
-		if (!app->notifications) {
-			error("gatt: couldn't allocate notifications queue");
-			destroy_gatt_app(app);
-			return NULL;
-		}
-	}
+	if (!dev)
+		return;
 
-	memcpy(app->uuid, uuid, sizeof(app->uuid));
+	queue_destroy(dev->services, destroy_service);
+	queue_destroy(dev->pending_requests, destroy_pending_request);
 
-	app->id = application_id++;
+	free(dev);
+}
 
-	if (!queue_push_head(gatt_apps, app)) {
-		error("gatt: Cannot push app on the list");
-		destroy_gatt_app(app);
+static struct gatt_device *device_ref(struct gatt_device *device)
+{
+	if (!device)
 		return NULL;
-	}
 
-	if ((app->type == GATT_SERVER) &&
-			!queue_push_tail(listen_apps, INT_TO_PTR(app->id))) {
-		error("gatt: Cannot push server on the list");
-		destroy_gatt_app(app);
-		return NULL;
-	}
+	device->ref++;
 
-	return app;
+	return device;
 }
 
-static void handle_client_register(const void *buf, uint16_t len)
+static void device_unref(struct gatt_device *device)
 {
-	const struct hal_cmd_gatt_client_register *cmd = buf;
-	struct hal_ev_gatt_client_register_client ev;
-	struct gatt_app *app;
-
-	DBG("");
-
-	memset(&ev, 0, sizeof(ev));
-
-	app = register_app(cmd->uuid, GATT_CLIENT);
-
-	if (app) {
-		ev.client_if = app->id;
-		ev.status = GATT_SUCCESS;
-	} else
-		ev.status = GATT_FAILURE;
-
-	/* We should send notification with given in cmd UUID */
-	memcpy(ev.app_uuid, cmd->uuid, sizeof(ev.app_uuid));
+	if (!device)
+		return;
 
-	ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
-			HAL_EV_GATT_CLIENT_REGISTER_CLIENT, sizeof(ev), &ev);
+	if (--device->ref)
+		return;
 
-	ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT, HAL_OP_GATT_CLIENT_REGISTER,
-							HAL_STATUS_SUCCESS);
+	destroy_device(device);
 }
 
-static void send_client_disconnection_notify(struct app_connection *connection,
-								int32_t status)
+static struct gatt_device *create_device(const bdaddr_t *addr)
 {
-	struct hal_ev_gatt_client_disconnect ev;
+	struct gatt_device *dev;
 
-	if (connection->app->func) {
-		connection->app->func(&connection->device->bdaddr, -ENOTCONN,
-						connection->device->attrib);
-		return;
-	}
+	dev = new0(struct gatt_device, 1);
+	if (!dev)
+		return NULL;
 
-	ev.client_if = connection->app->id;
-	ev.conn_id = connection->id;
-	ev.status = status;
+	bacpy(&dev->bdaddr, addr);
 
-	bdaddr2android(&connection->device->bdaddr, &ev.bda);
+	dev->services = queue_new();
+	if (!dev->services) {
+		error("gatt: Failed to allocate memory for client");
+		destroy_device(dev);
+		return NULL;
+	}
 
-	ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
-				HAL_EV_GATT_CLIENT_DISCONNECT, sizeof(ev), &ev);
+	dev->pending_requests = queue_new();
+	if (!dev->pending_requests) {
+		error("gatt: Failed to allocate memory for client");
+		destroy_device(dev);
+		return NULL;
+	}
 
+	if (!queue_push_head(gatt_devices, dev)) {
+		error("gatt: Cannot push device to queue");
+		destroy_device(dev);
+		return NULL;
+	}
+
+	return device_ref(dev);
 }
 
 static void send_client_connection_notify(struct app_connection *connection,
@@ -818,6 +766,28 @@ static void send_server_connection_notify(struct app_connection *connection,
 				HAL_EV_GATT_SERVER_CONNECTION, sizeof(ev), &ev);
 }
 
+static void send_client_disconnection_notify(struct app_connection *connection,
+								int32_t status)
+{
+	struct hal_ev_gatt_client_disconnect ev;
+
+	if (connection->app->func) {
+		connection->app->func(&connection->device->bdaddr, -ENOTCONN,
+						connection->device->attrib);
+		return;
+	}
+
+	ev.client_if = connection->app->id;
+	ev.conn_id = connection->id;
+	ev.status = status;
+
+	bdaddr2android(&connection->device->bdaddr, &ev.bda);
+
+	ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
+				HAL_EV_GATT_CLIENT_DISCONNECT, sizeof(ev), &ev);
+
+}
+
 static void send_app_disconnect_notify(struct app_connection *connection,
 								int32_t status)
 {
@@ -851,68 +821,6 @@ static void disconnect_notify_by_device(void *data, void *user_data)
 		send_app_connect_notify(conn, GATT_FAILURE);
 }
 
-enum pend_req_state {
-	REQUEST_INIT,
-	REQUEST_PENDING,
-	REQUEST_DONE,
-};
-
-struct pending_request {
-	uint16_t handle;
-	int length;
-	uint8_t *value;
-	uint16_t offset;
-
-	uint8_t *filter_value;
-	uint16_t filter_vlen;
-
-	enum pend_req_state state;
-	uint8_t error;
-};
-
-static void destroy_pending_request(void *data)
-{
-	struct pending_request *entry = data;
-
-	free(entry->value);
-	free(entry->filter_value);
-	free(entry);
-}
-
-static void destroy_device(void *data)
-{
-	struct gatt_device *dev = data;
-
-	if (!dev)
-		return;
-
-	queue_destroy(dev->services, destroy_service);
-	queue_destroy(dev->pending_requests, destroy_pending_request);
-
-	free(dev);
-}
-
-static struct gatt_device *device_ref(struct gatt_device *device)
-{
-	if (!device)
-		return NULL;
-
-	device->ref++;
-
-	return device;
-}
-
-static void device_unref(struct gatt_device *device)
-{
-	if (!device)
-		return;
-
-	if (--device->ref)
-		return;
-
-	destroy_device(device);
-}
-
 static void destroy_connection(void *data)
 {
 	struct app_connection *conn = data;
@@ -940,72 +848,6 @@ static void device_disconnect_clients(struct gatt_device *dev)
 							destroy_connection);
 }
 
-static void send_client_primary_notify(void *data, void *user_data)
-{
-	struct hal_ev_gatt_client_search_result ev;
-	struct service *p = data;
-	int32_t conn_id = PTR_TO_INT(user_data);
-
-	/* In service queue we will have also included services */
-	if (!p->primary)
-		return;
-
-	ev.conn_id  = conn_id;
-	element_id_to_hal_srvc_id(&p->id, 1, &ev.srvc_id);
-
-	uuid2android(&p->id.uuid, ev.srvc_id.uuid);
-
-	ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
-			HAL_EV_GATT_CLIENT_SEARCH_RESULT, sizeof(ev), &ev);
-}
-
-static struct service *create_service(uint8_t id, bool primary, char *uuid,
-								void *data)
-{
-	struct service *s;
-
-	s = new0(struct service, 1);
-	if (!s) {
-		error("gatt: Cannot allocate memory for gatt_primary");
-		return NULL;
-	}
-
-	s->chars = queue_new();
-	if (!s->chars) {
-		error("gatt: Cannot allocate memory for char cache");
-		free(s);
-		return NULL;
-	}
-
-	if (bt_string_to_uuid(&s->id.uuid, uuid) < 0) {
-		error("gatt: Cannot convert string to uuid");
-		queue_destroy(s->chars, NULL);
-		free(s);
-		return NULL;
-	}
-
-	s->id.instance = id;
-
-	/* Put primary service to our local list */
-	s->primary = primary;
-	if (s->primary) {
-		memcpy(&s->prim, data, sizeof(s->prim));
-	} else {
-		memcpy(&s->incl, data, sizeof(s->incl));
-		return s;
-	}
-
-	/* For primary service allocate queue for included services */
-	s->included = queue_new();
-	if (!s->included) {
-		queue_destroy(s->chars, NULL);
-		free(s);
-		return NULL;
-	}
-
-	return s;
-}
-
 static gboolean disconnected_cb(GIOChannel *io, GIOCondition cond,
 							gpointer user_data)
 {
@@ -1023,20 +865,6 @@ static gboolean disconnected_cb(GIOChannel *io, GIOCondition cond,
 	return FALSE;
 }
 
-struct connect_data {
-	struct gatt_device *dev;
-	int32_t status;
-};
-
-static void send_app_connect_notifications(void *data, void *user_data)
-{
-	struct app_connection *conn = data;
-	struct connect_data *con_data = user_data;
-
-	if (conn->device == con_data->dev)
-		send_app_connect_notify(conn, con_data->status);
-}
-
 static void att_handler(const uint8_t *ipdu, uint16_t len, gpointer user_data);
 
 static void exchange_mtu_cb(guint8 status, const guint8 *pdu, guint16 plen,
@@ -1140,6 +968,20 @@ static void notify_att_range_change(struct gatt_device *dev,
 		g_attrib_send(dev->attrib, 0, pdu, length, NULL, NULL, NULL);
 }
 
+struct connect_data {
+	struct gatt_device *dev;
+	int32_t status;
+};
+
+static void send_app_connect_notifications(void *data, void *user_data)
+{
+	struct app_connection *conn = data;
+	struct connect_data *con_data = user_data;
+
+	if (conn->device == con_data->dev)
+		send_app_connect_notify(conn, con_data->status);
+}
+
 static void connect_cb(GIOChannel *io, GError *gerr, gpointer user_data)
 {
 	struct gatt_device *dev = user_data;
@@ -1267,6 +1109,218 @@ static int connect_le(struct gatt_device *dev)
 	return 0;
 }
 
+static int connect_next_dev(void)
+{
+	struct gatt_device *dev;
+
+	DBG("");
+
+	dev = find_device_by_state(DEVICE_CONNECT_READY);
+	if (!dev)
+		return -ENODEV;
+
+	return connect_le(dev);
+}
+
+static void le_device_found_handler(const bdaddr_t *addr, uint8_t addr_type,
+						int rssi, uint16_t eir_len,
+						const void *eir,
+						bool discoverable, bool bonded)
+{
+	uint8_t buf[IPC_MTU];
+	struct hal_ev_gatt_client_scan_result *ev = (void *) buf;
+	struct gatt_device *dev;
+	char bda[18];
+
+	if (!scanning || (!discoverable && !bonded))
+		goto connect;
+
+	ba2str(addr, bda);
+	DBG("LE Device found: %s, rssi: %d, adv_data: %d", bda, rssi, !!eir);
+
+	bdaddr2android(addr, ev->bda);
+	ev->rssi = rssi;
+	ev->len = eir_len;
+
+	memcpy(ev->adv_data, eir, ev->len);
+
+	ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
+						HAL_EV_GATT_CLIENT_SCAN_RESULT,
+						sizeof(*ev) + ev->len, ev);
+
+connect:
+	dev = find_device_by_addr(addr);
+	if (!dev) {
+		if (!bonded)
+			return;
+
+		dev = create_device(addr);
+		if (!dev)
+			return;
+		connect_le(dev);
+		return;
+	}
+
+	device_set_state(dev, DEVICE_CONNECT_READY);
+	dev->bdaddr_type = addr_type;
+
+	if (!scanning) {
+		connect_next_dev();
+		return;
+	}
+
+	/*
+	 * We are ok to perform connect now. Stop discovery
+	 * and once it is stopped continue with creating ACL
+	 */
+	bt_le_discovery_stop(bt_le_discovery_stop_cb);
+}
+
+static struct gatt_app *register_app(const uint8_t *uuid, gatt_type_t type)
+{
+	static int32_t application_id = 1;
+	struct gatt_app *app;
+
+	if (queue_find(gatt_apps, match_app_by_uuid, uuid)) {
+		error("gatt: app uuid is already on list");
+		return NULL;
+	}
+
+	/* Register LE once the first app register */
+	if (queue_isempty(gatt_apps))
+		bt_le_register(le_device_found_handler);
+
+	app = new0(struct gatt_app, 1);
+	if (!app) {
+		error("gatt: Cannot allocate memory for registering app");
+		return 0;
+	}
+
+	app->type = type;
+
+	if (app->type == GATT_CLIENT) {
+		app->notifications = queue_new();
+		if (!app->notifications) {
+			error("gatt: couldn't allocate notifications queue");
+			destroy_gatt_app(app);
+			return NULL;
+		}
+	}
+
+	memcpy(app->uuid, uuid, sizeof(app->uuid));
+
+	app->id = application_id++;
+
+	if (!queue_push_head(gatt_apps, app)) {
+		error("gatt: Cannot push app on the list");
+		destroy_gatt_app(app);
+		return NULL;
+	}
+
+	if ((app->type == GATT_SERVER) &&
+			!queue_push_tail(listen_apps, INT_TO_PTR(app->id))) {
+		error("gatt: Cannot push server on the list");
+		destroy_gatt_app(app);
+		return NULL;
+	}
+
+	return app;
+}
+
+static void handle_client_register(const void *buf, uint16_t len)
+{
+	const struct hal_cmd_gatt_client_register *cmd = buf;
+	struct hal_ev_gatt_client_register_client ev;
+	struct gatt_app *app;
+
+	DBG("");
+
+	memset(&ev, 0, sizeof(ev));
+
+	app = register_app(cmd->uuid, GATT_CLIENT);
+
+	if (app) {
+		ev.client_if = app->id;
+		ev.status = GATT_SUCCESS;
+	} else
+		ev.status = GATT_FAILURE;
+
+	/* We should send notification with given in cmd UUID */
+	memcpy(ev.app_uuid, cmd->uuid, sizeof(ev.app_uuid));
+
+	ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
+			HAL_EV_GATT_CLIENT_REGISTER_CLIENT, sizeof(ev), &ev);
+
+	ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT, HAL_OP_GATT_CLIENT_REGISTER,
+							HAL_STATUS_SUCCESS);
+}
+
+static void send_client_primary_notify(void *data, void *user_data)
+{
+	struct hal_ev_gatt_client_search_result ev;
+	struct service *p = data;
+	int32_t conn_id = PTR_TO_INT(user_data);
+
+	/* In service queue we will have also included services */
+	if (!p->primary)
+		return;
+
+	ev.conn_id  = conn_id;
+	element_id_to_hal_srvc_id(&p->id, 1, &ev.srvc_id);
+
+	uuid2android(&p->id.uuid, ev.srvc_id.uuid);
+
+	ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
+			HAL_EV_GATT_CLIENT_SEARCH_RESULT, sizeof(ev), &ev);
+}
+
+static struct service *create_service(uint8_t id, bool primary, char *uuid,
+								void *data)
+{
+	struct service *s;
+
+	s = new0(struct service, 1);
+	if (!s) {
+		error("gatt: Cannot allocate memory for gatt_primary");
+		return NULL;
+	}
+
+	s->chars = queue_new();
+	if (!s->chars) {
+		error("gatt: Cannot allocate memory for char cache");
+		free(s);
+		return NULL;
+	}
+
+	if (bt_string_to_uuid(&s->id.uuid, uuid) < 0) {
+		error("gatt: Cannot convert string to uuid");
+		queue_destroy(s->chars, NULL);
+		free(s);
+		return NULL;
+	}
+
+	s->id.instance = id;
+
+	/* Put primary service to our local list */
+	s->primary = primary;
+	if (s->primary) {
+		memcpy(&s->prim, data, sizeof(s->prim));
+	} else {
+		memcpy(&s->incl, data, sizeof(s->incl));
+		return s;
+	}
+
+	/* For primary service allocate queue for included services */
+	s->included = queue_new();
+	if (!s->included) {
+		queue_destroy(s->chars, NULL);
+		free(s);
+		return NULL;
+	}
+
+	return s;
+}
+
 static void handle_client_scan(const void *buf, uint16_t len)
 {
 	const struct hal_cmd_gatt_client_scan *cmd = buf;
@@ -1315,19 +1369,6 @@ reply:
 									status);
 }
 
-static int connect_next_dev(void)
-{
-	struct gatt_device *dev;
-
-	DBG("");
-
-	dev = find_device_by_state(DEVICE_CONNECT_READY);
-	if (!dev)
-		return -ENODEV;
-
-	return connect_le(dev);
-}
-
 static void bt_le_discovery_stop_cb(void)
 {
 	DBG("");
@@ -1337,39 +1378,6 @@ static void bt_le_discovery_stop_cb(void)
 		bt_le_discovery_start();
 }
 
-static struct gatt_device *create_device(const bdaddr_t *addr)
-{
-	struct gatt_device *dev;
-
-	dev = new0(struct gatt_device, 1);
-	if (!dev)
-		return NULL;
-
-	bacpy(&dev->bdaddr, addr);
-
-	dev->services = queue_new();
-	if (!dev->services) {
-		error("gatt: Failed to allocate memory for client");
-		destroy_device(dev);
-		return NULL;
-	}
-
-	dev->pending_requests = queue_new();
-	if (!dev->pending_requests) {
-		error("gatt: Failed to allocate memory for client");
-		destroy_device(dev);
-		return NULL;
-	}
-
-	if (!queue_push_head(gatt_devices, dev)) {
-		error("gatt: Cannot push device to queue");
-		destroy_device(dev);
-		return NULL;
-	}
-
-	return device_ref(dev);
-}
-
 static struct app_connection *create_connection(struct gatt_device *device,
 						struct gatt_app *app)
 {
-- 
1.9.3

[RFC BlueZ 4/5] android/bluetooth: Add bt_device_set_uuids

[Luiz Augusto von Dentz]

From: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>

---
 android/bluetooth.c | 13 +++++++++++++
 android/bluetooth.h |  1 +
 2 files changed, 14 insertions(+)

diff --git a/android/bluetooth.c b/android/bluetooth.c
index 834d687..f8e9181 100644
--- a/android/bluetooth.c
+++ b/android/bluetooth.c
@@ -1409,6 +1409,19 @@ bool bt_device_is_bonded(const bdaddr_t *bdaddr)
 	return false;
 }
 
+bool bt_device_set_uuids(const bdaddr_t *addr, GSList *uuids)
+{
+	struct device *dev;
+
+	dev = find_device(addr);
+	if (!dev)
+		return false;
+
+	set_device_uuids(dev, uuids);
+
+	return true;
+}
+
 static bool rssi_above_threshold(int old, int new)
 {
 	/* only 8 dBm or more */
diff --git a/android/bluetooth.h b/android/bluetooth.h
index 7c64bab..eb50fe1 100644
--- a/android/bluetooth.h
+++ b/android/bluetooth.h
@@ -61,6 +61,7 @@ bool bt_is_device_le(const bdaddr_t *addr);
 
 const char *bt_get_adapter_name(void);
 bool bt_device_is_bonded(const bdaddr_t *bdaddr);
+bool bt_device_set_uuids(const bdaddr_t *bdaddr, GSList *uuids);
 
 typedef void (*bt_read_device_rssi_done)(uint8_t status, const bdaddr_t *addr,
 						int8_t rssi, void *user_data);
-- 
1.9.3

[RFC BlueZ 5/5] android/gatt: Automatically discover primary services for bonded devices

[Luiz Augusto von Dentz]

From: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>

---
 android/gatt.c | 637 ++++++++++++++++++++++++++++++++-------------------------
 1 file changed, 353 insertions(+), 284 deletions(-)

diff --git a/android/gatt.c b/android/gatt.c
index 013337c..4e8ff61 100644
--- a/android/gatt.c
+++ b/android/gatt.c
@@ -791,6 +791,9 @@ static void send_client_disconnection_notify(struct app_connection *connection,
 static void send_app_disconnect_notify(struct app_connection *connection,
 								int32_t status)
 {
+	if (!connection->app)
+		return;
+
 	if (connection->app->type == GATT_CLIENT)
 		send_client_disconnection_notify(connection, status);
 	else
@@ -800,6 +803,9 @@ static void send_app_disconnect_notify(struct app_connection *connection,
 static void send_app_connect_notify(struct app_connection *connection,
 								int32_t status)
 {
+	if (!connection->app)
+		return;
+
 	if (connection->app->type == GATT_CLIENT)
 		send_client_connection_notify(connection, status);
 	else if (connection->app->type == GATT_SERVER)
@@ -811,7 +817,7 @@ static void disconnect_notify_by_device(void *data, void *user_data)
 	struct app_connection *conn = data;
 	struct gatt_device *dev = user_data;
 
-	if (dev != conn->device)
+	if (dev != conn->device || !conn->app)
 		return;
 
 	if (dev->state == DEVICE_CONNECTED)
@@ -968,6 +974,317 @@ static void notify_att_range_change(struct gatt_device *dev,
 		g_attrib_send(dev->attrib, 0, pdu, length, NULL, NULL, NULL);
 }
 
+static struct app_connection *create_connection(struct gatt_device *device,
+						struct gatt_app *app)
+{
+	struct app_connection *new_conn;
+	static int32_t last_conn_id = 1;
+
+	/* Check if already connected */
+	new_conn = new0(struct app_connection, 1);
+	if (!new_conn)
+		return NULL;
+
+	/* Make connection id unique to connection record (app, device) pair */
+	new_conn->app = app;
+	new_conn->id = last_conn_id++;
+
+	new_conn->transactions = queue_new();
+	if (!new_conn->transactions) {
+		free(new_conn);
+		return NULL;
+	}
+
+	if (!queue_push_head(app_connections, new_conn)) {
+		error("gatt: Cannot push client on the client queue!?");
+		queue_destroy(new_conn->transactions, free);
+		free(new_conn);
+		return NULL;
+	}
+
+	new_conn->device = device_ref(device);
+	new_conn->device->conn_cnt++;
+
+	return new_conn;
+}
+
+static struct service *create_service(uint8_t id, bool primary, char *uuid,
+								void *data)
+{
+	struct service *s;
+
+	s = new0(struct service, 1);
+	if (!s) {
+		error("gatt: Cannot allocate memory for gatt_primary");
+		return NULL;
+	}
+
+	s->chars = queue_new();
+	if (!s->chars) {
+		error("gatt: Cannot allocate memory for char cache");
+		free(s);
+		return NULL;
+	}
+
+	if (bt_string_to_uuid(&s->id.uuid, uuid) < 0) {
+		error("gatt: Cannot convert string to uuid");
+		queue_destroy(s->chars, NULL);
+		free(s);
+		return NULL;
+	}
+
+	s->id.instance = id;
+
+	/* Put primary service to our local list */
+	s->primary = primary;
+	if (s->primary) {
+		memcpy(&s->prim, data, sizeof(s->prim));
+	} else {
+		memcpy(&s->incl, data, sizeof(s->incl));
+		return s;
+	}
+
+	/* For primary service allocate queue for included services */
+	s->included = queue_new();
+	if (!s->included) {
+		queue_destroy(s->chars, NULL);
+		free(s);
+		return NULL;
+	}
+
+	return s;
+}
+
+static void send_client_primary_notify(void *data, void *user_data)
+{
+	struct hal_ev_gatt_client_search_result ev;
+	struct service *p = data;
+	int32_t conn_id = PTR_TO_INT(user_data);
+
+	/* In service queue we will have also included services */
+	if (!p->primary)
+		return;
+
+	ev.conn_id  = conn_id;
+	element_id_to_hal_srvc_id(&p->id, 1, &ev.srvc_id);
+
+	uuid2android(&p->id.uuid, ev.srvc_id.uuid);
+
+	ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
+			HAL_EV_GATT_CLIENT_SEARCH_RESULT, sizeof(ev), &ev);
+}
+
+static void send_client_search_complete_notify(int32_t status, int32_t conn_id)
+{
+	struct hal_ev_gatt_client_search_complete ev;
+
+	ev.status = status;
+	ev.conn_id = conn_id;
+	ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
+			HAL_EV_GATT_CLIENT_SEARCH_COMPLETE, sizeof(ev), &ev);
+}
+
+struct discover_srvc_data {
+	bt_uuid_t uuid;
+	struct app_connection *conn;
+};
+
+static void discover_srvc_by_uuid_cb(uint8_t status, GSList *ranges,
+								void *user_data)
+{
+	struct discover_srvc_data *cb_data = user_data;
+	struct gatt_primary prim;
+	struct service *s;
+	int32_t gatt_status;
+	struct gatt_device *dev = cb_data->conn->device;
+	uint8_t instance_id = queue_length(dev->services);
+
+	DBG("Status %d", status);
+
+	if (status) {
+		error("gatt: Discover pri srvc filtered by uuid failed: %s",
+							att_ecode2str(status));
+		gatt_status = GATT_FAILURE;
+		goto reply;
+	}
+
+	if (!ranges) {
+		info("gatt: No primary services searched by uuid found");
+		gatt_status = GATT_SUCCESS;
+		goto reply;
+	}
+
+	bt_uuid_to_string(&cb_data->uuid, prim.uuid, sizeof(prim.uuid));
+	/*
+	 * If multiple instances of the same service (as identified by UUID)
+	 * exist, the first instance of the service is returned.
+	 */
+	memcpy(&prim.range, ranges->data, sizeof(prim.range));
+
+	s = create_service(instance_id++, true, prim.uuid, &prim);
+	if (!s) {
+		gatt_status = GATT_FAILURE;
+		goto reply;
+	}
+
+	if (!queue_push_tail(dev->services, s)) {
+		error("gatt: Cannot push primary service to the list");
+		gatt_status = GATT_FAILURE;
+		goto reply;
+	}
+
+	send_client_primary_notify(s, INT_TO_PTR(cb_data->conn->id));
+
+	DBG("attr handle = 0x%04x, end grp handle = 0x%04x uuid: %s",
+		prim.range.start, prim.range.end, prim.uuid);
+
+	/* Partial search service scanning was performed */
+	dev->partial_srvc_search = true;
+	gatt_status = GATT_SUCCESS;
+
+reply:
+	send_client_search_complete_notify(gatt_status, cb_data->conn->id);
+	free(cb_data);
+}
+
+static void discover_srvc_all_cb(uint8_t status, GSList *services,
+								void *user_data)
+{
+	struct discover_srvc_data *cb_data = user_data;
+	struct gatt_device *dev = cb_data->conn->device;
+	int32_t gatt_status;
+	GSList *l;
+	/*
+	 * There might be multiply services with same uuid. Therefore make sure
+	 * each primary service one has unique instance_id
+	 */
+	uint8_t instance_id = queue_length(dev->services);
+
+	DBG("Status %d", status);
+
+	if (status) {
+		error("gatt: Discover all primary services failed: %s",
+							att_ecode2str(status));
+		gatt_status = GATT_FAILURE;
+		goto reply;
+	}
+
+	if (!services) {
+		info("gatt: No primary services found");
+		gatt_status = GATT_SUCCESS;
+		goto reply;
+	}
+
+	for (l = services; l; l = l->next) {
+		struct gatt_primary *prim = l->data;
+		struct service *p;
+
+		if (queue_find(dev->services, match_srvc_by_range,
+								&prim->range))
+			continue;
+
+		p = create_service(instance_id++, true, prim->uuid, prim);
+		if (!p)
+			continue;
+
+		if (!queue_push_tail(dev->services, p)) {
+			error("gatt: Cannot push primary service to the list");
+			free(p);
+			continue;
+		}
+
+		DBG("attr handle = 0x%04x, end grp handle = 0x%04x uuid: %s",
+			prim->range.start, prim->range.end, prim->uuid);
+	}
+
+	/*
+	 * Send all found services notifications - first cache,
+	 * then send notifies
+	 */
+	queue_foreach(dev->services, send_client_primary_notify,
+						INT_TO_PTR(cb_data->conn->id));
+
+	/* Full search service scanning was performed */
+	dev->partial_srvc_search = false;
+	gatt_status = GATT_SUCCESS;
+
+reply:
+	send_client_search_complete_notify(gatt_status, cb_data->conn->id);
+	free(cb_data);
+}
+
+static void discover_primary_cb(uint8_t status, GSList *services,
+								void *user_data)
+{
+	struct discover_srvc_data *cb_data = user_data;
+	struct app_connection *conn = cb_data->conn;
+	struct gatt_device *dev = conn->device;
+	GSList *l, *uuids = NULL;
+
+	DBG("Status %d", status);
+
+	if (status) {
+		error("gatt: Discover all primary services failed: %s",
+							att_ecode2str(status));
+		return;
+	}
+
+	if (!services) {
+		info("gatt: No primary services found");
+		return;
+	}
+
+	for (l = services; l; l = l->next) {
+		struct gatt_primary *prim = l->data;
+		uint8_t *new_uuid;
+		bt_uuid_t uuid;
+
+		DBG("uuid: %s", prim->uuid);
+
+		if (bt_string_to_uuid(&uuid, prim->uuid) < 0) {
+			error("gatt: Cannot convert string to uuid");
+			continue;
+		}
+
+		new_uuid = g_memdup(&uuid.value.u128, sizeof(uuid.value.u128));
+
+		uuids = g_slist_prepend(uuids, new_uuid);
+	}
+
+	bt_device_set_uuids(&dev->bdaddr, uuids);
+
+	free(cb_data);
+	queue_remove(app_connections, conn);
+	destroy_connection(conn);
+}
+
+static guint search_dev_for_srvc(struct app_connection *conn, bt_uuid_t *uuid)
+{
+	struct discover_srvc_data *cb_data =
+					new0(struct discover_srvc_data, 1);
+
+	if (!cb_data) {
+		error("gatt: Cannot allocate cb data");
+		return 0;
+	}
+
+	cb_data->conn = conn;
+
+	if (uuid) {
+		memcpy(&cb_data->uuid, uuid, sizeof(cb_data->uuid));
+		return gatt_discover_primary(conn->device->attrib, uuid,
+					discover_srvc_by_uuid_cb, cb_data);
+	}
+
+
+	if (conn->app)
+		return gatt_discover_primary(conn->device->attrib, NULL,
+						discover_srvc_all_cb, cb_data);
+
+	return gatt_discover_primary(conn->device->attrib, NULL,
+						discover_primary_cb, cb_data);
+}
+
 struct connect_data {
 	struct gatt_device *dev;
 	int32_t status;
@@ -1051,6 +1368,19 @@ static void connect_cb(GIOChannel *io, GError *gerr, gpointer user_data)
 	status = GATT_SUCCESS;
 
 reply:
+	if (!dev->conn_cnt) {
+		struct app_connection *conn;
+
+		if (!dev->attrib)
+			return;
+
+		conn = create_connection(dev, NULL);
+		if (!conn)
+			return;
+
+		search_dev_for_srvc(conn, NULL);
+	}
+
 	data.dev = dev;
 	data.status = status;
 	queue_foreach(app_connections, send_app_connect_notifications, &data);
@@ -1220,105 +1550,39 @@ static struct gatt_app *register_app(const uint8_t *uuid, gatt_type_t type)
 	if ((app->type == GATT_SERVER) &&
 			!queue_push_tail(listen_apps, INT_TO_PTR(app->id))) {
 		error("gatt: Cannot push server on the list");
-		destroy_gatt_app(app);
-		return NULL;
-	}
-
-	return app;
-}
-
-static void handle_client_register(const void *buf, uint16_t len)
-{
-	const struct hal_cmd_gatt_client_register *cmd = buf;
-	struct hal_ev_gatt_client_register_client ev;
-	struct gatt_app *app;
-
-	DBG("");
-
-	memset(&ev, 0, sizeof(ev));
-
-	app = register_app(cmd->uuid, GATT_CLIENT);
-
-	if (app) {
-		ev.client_if = app->id;
-		ev.status = GATT_SUCCESS;
-	} else
-		ev.status = GATT_FAILURE;
-
-	/* We should send notification with given in cmd UUID */
-	memcpy(ev.app_uuid, cmd->uuid, sizeof(ev.app_uuid));
-
-	ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
-			HAL_EV_GATT_CLIENT_REGISTER_CLIENT, sizeof(ev), &ev);
-
-	ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT, HAL_OP_GATT_CLIENT_REGISTER,
-							HAL_STATUS_SUCCESS);
-}
-
-static void send_client_primary_notify(void *data, void *user_data)
-{
-	struct hal_ev_gatt_client_search_result ev;
-	struct service *p = data;
-	int32_t conn_id = PTR_TO_INT(user_data);
-
-	/* In service queue we will have also included services */
-	if (!p->primary)
-		return;
-
-	ev.conn_id  = conn_id;
-	element_id_to_hal_srvc_id(&p->id, 1, &ev.srvc_id);
-
-	uuid2android(&p->id.uuid, ev.srvc_id.uuid);
+		destroy_gatt_app(app);
+		return NULL;
+	}
 
-	ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
-			HAL_EV_GATT_CLIENT_SEARCH_RESULT, sizeof(ev), &ev);
+	return app;
 }
 
-static struct service *create_service(uint8_t id, bool primary, char *uuid,
-								void *data)
+static void handle_client_register(const void *buf, uint16_t len)
 {
-	struct service *s;
+	const struct hal_cmd_gatt_client_register *cmd = buf;
+	struct hal_ev_gatt_client_register_client ev;
+	struct gatt_app *app;
 
-	s = new0(struct service, 1);
-	if (!s) {
-		error("gatt: Cannot allocate memory for gatt_primary");
-		return NULL;
-	}
+	DBG("");
 
-	s->chars = queue_new();
-	if (!s->chars) {
-		error("gatt: Cannot allocate memory for char cache");
-		free(s);
-		return NULL;
-	}
+	memset(&ev, 0, sizeof(ev));
 
-	if (bt_string_to_uuid(&s->id.uuid, uuid) < 0) {
-		error("gatt: Cannot convert string to uuid");
-		queue_destroy(s->chars, NULL);
-		free(s);
-		return NULL;
-	}
+	app = register_app(cmd->uuid, GATT_CLIENT);
 
-	s->id.instance = id;
+	if (app) {
+		ev.client_if = app->id;
+		ev.status = GATT_SUCCESS;
+	} else
+		ev.status = GATT_FAILURE;
 
-	/* Put primary service to our local list */
-	s->primary = primary;
-	if (s->primary) {
-		memcpy(&s->prim, data, sizeof(s->prim));
-	} else {
-		memcpy(&s->incl, data, sizeof(s->incl));
-		return s;
-	}
+	/* We should send notification with given in cmd UUID */
+	memcpy(ev.app_uuid, cmd->uuid, sizeof(ev.app_uuid));
 
-	/* For primary service allocate queue for included services */
-	s->included = queue_new();
-	if (!s->included) {
-		queue_destroy(s->chars, NULL);
-		free(s);
-		return NULL;
-	}
+	ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
+			HAL_EV_GATT_CLIENT_REGISTER_CLIENT, sizeof(ev), &ev);
 
-	return s;
+	ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT, HAL_OP_GATT_CLIENT_REGISTER,
+							HAL_STATUS_SUCCESS);
 }
 
 static void handle_client_scan(const void *buf, uint16_t len)
@@ -1378,40 +1642,6 @@ static void bt_le_discovery_stop_cb(void)
 		bt_le_discovery_start();
 }
 
-static struct app_connection *create_connection(struct gatt_device *device,
-						struct gatt_app *app)
-{
-	struct app_connection *new_conn;
-	static int32_t last_conn_id = 1;
-
-	/* Check if already connected */
-	new_conn = new0(struct app_connection, 1);
-	if (!new_conn)
-		return NULL;
-
-	/* Make connection id unique to connection record (app, device) pair */
-	new_conn->app = app;
-	new_conn->id = last_conn_id++;
-
-	new_conn->transactions = queue_new();
-	if (!new_conn->transactions) {
-		free(new_conn);
-		return NULL;
-	}
-
-	if (!queue_push_head(app_connections, new_conn)) {
-		error("gatt: Cannot push client on the client queue!?");
-		queue_destroy(new_conn->transactions, free);
-		free(new_conn);
-		return NULL;
-	}
-
-	new_conn->device = device_ref(device);
-	new_conn->device->conn_cnt++;
-
-	return new_conn;
-}
-
 static void trigger_disconnection(struct app_connection *connection)
 {
 	/* Notify client */
@@ -1762,167 +1992,6 @@ done:
 									status);
 }
 
-struct discover_srvc_data {
-	bt_uuid_t uuid;
-	struct app_connection *conn;
-};
-
-static void send_client_search_complete_notify(int32_t status, int32_t conn_id)
-{
-	struct hal_ev_gatt_client_search_complete ev;
-
-	ev.status = status;
-	ev.conn_id = conn_id;
-	ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
-			HAL_EV_GATT_CLIENT_SEARCH_COMPLETE, sizeof(ev), &ev);
-}
-
-static void discover_srvc_all_cb(uint8_t status, GSList *services,
-								void *user_data)
-{
-	struct discover_srvc_data *cb_data = user_data;
-	struct gatt_device *dev = cb_data->conn->device;
-	int32_t gatt_status;
-	GSList *l;
-	/*
-	 * There might be multiply services with same uuid. Therefore make sure
-	 * each primary service one has unique instance_id
-	 */
-	uint8_t instance_id = queue_length(dev->services);
-
-	DBG("Status %d", status);
-
-	if (status) {
-		error("gatt: Discover all primary services failed: %s",
-							att_ecode2str(status));
-		gatt_status = GATT_FAILURE;
-		goto reply;
-	}
-
-	if (!services) {
-		info("gatt: No primary services found");
-		gatt_status = GATT_SUCCESS;
-		goto reply;
-	}
-
-	for (l = services; l; l = l->next) {
-		struct gatt_primary *prim = l->data;
-		struct service *p;
-
-		if (queue_find(dev->services, match_srvc_by_range,
-								&prim->range))
-			continue;
-
-		p = create_service(instance_id++, true, prim->uuid, prim);
-		if (!p)
-			continue;
-
-		if (!queue_push_tail(dev->services, p)) {
-			error("gatt: Cannot push primary service to the list");
-			free(p);
-			continue;
-		}
-
-		DBG("attr handle = 0x%04x, end grp handle = 0x%04x uuid: %s",
-			prim->range.start, prim->range.end, prim->uuid);
-	}
-
-	/*
-	 * Send all found services notifications - first cache,
-	 * then send notifies
-	 */
-	queue_foreach(dev->services, send_client_primary_notify,
-						INT_TO_PTR(cb_data->conn->id));
-
-	/* Full search service scanning was performed */
-	dev->partial_srvc_search = false;
-	gatt_status = GATT_SUCCESS;
-
-reply:
-	send_client_search_complete_notify(gatt_status, cb_data->conn->id);
-	free(cb_data);
-}
-
-static void discover_srvc_by_uuid_cb(uint8_t status, GSList *ranges,
-								void *user_data)
-{
-	struct discover_srvc_data *cb_data = user_data;
-	struct gatt_primary prim;
-	struct service *s;
-	int32_t gatt_status;
-	struct gatt_device *dev = cb_data->conn->device;
-	uint8_t instance_id = queue_length(dev->services);
-
-	DBG("Status %d", status);
-
-	if (status) {
-		error("gatt: Discover pri srvc filtered by uuid failed: %s",
-							att_ecode2str(status));
-		gatt_status = GATT_FAILURE;
-		goto reply;
-	}
-
-	if (!ranges) {
-		info("gatt: No primary services searched by uuid found");
-		gatt_status = GATT_SUCCESS;
-		goto reply;
-	}
-
-	bt_uuid_to_string(&cb_data->uuid, prim.uuid, sizeof(prim.uuid));
-	/*
-	 * If multiple instances of the same service (as identified by UUID)
-	 * exist, the first instance of the service is returned.
-	 */
-	memcpy(&prim.range, ranges->data, sizeof(prim.range));
-
-	s = create_service(instance_id++, true, prim.uuid, &prim);
-	if (!s) {
-		gatt_status = GATT_FAILURE;
-		goto reply;
-	}
-
-	if (!queue_push_tail(dev->services, s)) {
-		error("gatt: Cannot push primary service to the list");
-		gatt_status = GATT_FAILURE;
-		goto reply;
-	}
-
-	send_client_primary_notify(s, INT_TO_PTR(cb_data->conn->id));
-
-	DBG("attr handle = 0x%04x, end grp handle = 0x%04x uuid: %s",
-		prim.range.start, prim.range.end, prim.uuid);
-
-	/* Partial search service scanning was performed */
-	dev->partial_srvc_search = true;
-	gatt_status = GATT_SUCCESS;
-
-reply:
-	send_client_search_complete_notify(gatt_status, cb_data->conn->id);
-	free(cb_data);
-}
-
-static guint search_dev_for_srvc(struct app_connection *conn, bt_uuid_t *uuid)
-{
-	struct discover_srvc_data *cb_data =
-					new0(struct discover_srvc_data, 1);
-
-	if (!cb_data) {
-		error("gatt: Cannot allocate cb data");
-		return 0;
-	}
-
-	cb_data->conn = conn;
-
-	if (uuid) {
-		memcpy(&cb_data->uuid, uuid, sizeof(cb_data->uuid));
-		return gatt_discover_primary(conn->device->attrib, uuid,
-					discover_srvc_by_uuid_cb, cb_data);
-	}
-
-	return gatt_discover_primary(conn->device->attrib, NULL,
-						discover_srvc_all_cb, cb_data);
-}
-
 static void handle_client_search_service(const void *buf, uint16_t len)
 {
 	const struct hal_cmd_gatt_client_search_service *cmd = buf;
-- 
1.9.3