android/gatt.c

/*
 *
 *  BlueZ - Bluetooth protocol stack for Linux
 *
 *  Copyright (C) 2014  Intel Corporation. All rights reserved.
 *
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License as published by the Free Software Foundation; either
 *  version 2.1 of the License, or (at your option) any later version.
 *
 *  This library is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 */

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <stdbool.h>
#include <stdlib.h>
#include <stdint.h>
#include <glib.h>
#include <errno.h>
#include <sys/socket.h>

#include "ipc.h"
#include "ipc-common.h"
#include "lib/sdp.h"
#include "lib/sdp_lib.h"
#include "lib/uuid.h"
#include "bluetooth.h"
#include "gatt.h"
#include "src/log.h"
#include "hal-msg.h"
#include "utils.h"
#include "src/shared/util.h"
#include "src/shared/queue.h"
#include "src/shared/att.h"
#include "src/shared/gatt-db.h"
#include "attrib/gattrib.h"
#include "attrib/att.h"
#include "attrib/gatt.h"
#include "btio/btio.h"

/* set according to Android bt_gatt_client.h */
#define GATT_MAX_ATTR_LEN 600

#define GATT_SUCCESS	0x00000000
#define GATT_FAILURE	0x00000101

#define BASE_UUID16_OFFSET     12

#define GATT_PERM_READ			0x00000001
#define GATT_PERM_READ_ENCRYPTED	0x00000002
#define GATT_PERM_READ_MITM		0x00000004
#define GATT_PERM_READ_AUTHORIZATION	0x00000008
#define GATT_PERM_WRITE			0x00000100
#define GATT_PERM_WRITE_ENCRYPTED	0x00000200
#define GATT_PERM_WRITE_MITM		0x00000400
#define GATT_PERM_WRITE_AUTHORIZATION	0x00000800
#define GATT_PERM_WRITE_SIGNED		0x00010000
#define GATT_PERM_WRITE_SIGNED_MITM	0x00020000
#define GATT_PERM_NONE			0x10000000

#define GATT_PAIR_CONN_TIMEOUT 30
#define GATT_CONN_TIMEOUT 2

static const uint8_t BLUETOOTH_UUID[] = {
	0xfb, 0x34, 0x9b, 0x5f, 0x80, 0x00, 0x00, 0x80,
	0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};

typedef enum {
	DEVICE_DISCONNECTED = 0,
	DEVICE_CONNECT_INIT,		/* connection procedure initiated */
	DEVICE_CONNECT_READY,		/* dev found during LE scan */
	DEVICE_CONNECTED,		/* connection has been established */
} gatt_device_state_t;

static const char *device_state_str[] = {
	"DISCONNECTED",
	"CONNECT INIT",
	"CONNECT READY",
	"CONNECTED",
};

struct pending_trans_data {
	unsigned int id;
	uint8_t opcode;
	struct gatt_db_attribute *attrib;
	unsigned int serial_id;
};

struct gatt_app {
	int32_t id;
	uint8_t uuid[16];

	gatt_type_t type;

	/* Valid for client applications */
	struct queue *notifications;

	gatt_conn_cb_t func;
};

struct element_id {
	bt_uuid_t uuid;
	uint8_t instance;
};

struct descriptor {
	struct element_id id;
	uint16_t handle;
};

struct characteristic {
	struct element_id id;
	struct gatt_char ch;
	uint16_t end_handle;

	struct queue *descriptors;
};

struct service {
	struct element_id id;
	struct gatt_primary prim;
	struct gatt_included incl;

	bool primary;

	struct queue *chars;
	struct queue *included;	/* Valid only for primary services */
	bool incl_search_done;
};

struct notification_data {
	struct hal_gatt_srvc_id service;
	struct hal_gatt_gatt_id ch;
	struct app_connection *conn;
	guint notif_id;
	guint ind_id;
	int ref;
};

struct gatt_device {
	bdaddr_t bdaddr;

	gatt_device_state_t state;

	GAttrib *attrib;
	GIOChannel *att_io;
	struct queue *services;
	bool partial_srvc_search;

	guint watch_id;
	guint server_id;
	guint ind_id;

	int ref;

	struct queue *autoconnect_apps;

	struct queue *pending_requests;
};

struct app_connection {
	struct gatt_device *device;
	struct gatt_app *app;
	struct queue *transactions;
	int32_t id;

	guint timeout_id;

	bool wait_execute_write;
};

struct service_sdp {
	int32_t service_handle;
	uint32_t sdp_handle;
};

static struct ipc *hal_ipc = NULL;
static bdaddr_t adapter_addr;
static bool scanning = false;
static unsigned int advertising_cnt = 0;

static struct queue *gatt_apps = NULL;
static struct queue *gatt_devices = NULL;
static struct queue *app_connections = NULL;

static struct queue *services_sdp = NULL;

static struct queue *listen_apps = NULL;
static struct gatt_db *gatt_db = NULL;

static struct gatt_db_attribute *service_changed_attrib = NULL;

static GIOChannel *le_io = NULL;
static GIOChannel *bredr_io = NULL;

static uint32_t gatt_sdp_handle = 0;
static uint32_t gap_sdp_handle = 0;
static uint32_t dis_sdp_handle = 0;

static struct bt_crypto *crypto = NULL;

static int test_client_if = 0;
static const uint8_t TEST_UUID[] = {
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x01, 0x02, 0x03, 0x04
};

static bool is_bluetooth_uuid(const uint8_t *uuid)
{
	int i;

	for (i = 0; i < 16; i++) {
		/* ignore minimal uuid (16) value */
		if (i == 12 || i == 13)
			continue;

		if (uuid[i] != BLUETOOTH_UUID[i])
			return false;
	}

	return true;
}

static void android2uuid(const uint8_t *uuid, bt_uuid_t *dst)
{
	if (is_bluetooth_uuid(uuid)) {
		/* copy 16 bit uuid value from full android 128bit uuid */
		dst->type = BT_UUID16;
		dst->value.u16 = (uuid[13] << 8) + uuid[12];
	} else {
		int i;

		dst->type = BT_UUID128;
		for (i = 0; i < 16; i++)
			dst->value.u128.data[i] = uuid[15 - i];
	}
}

static void uuid2android(const bt_uuid_t *src, uint8_t *uuid)
{
	bt_uuid_t uu128;
	uint8_t i;

	if (src->type != BT_UUID128) {
		bt_uuid_to_uuid128(src, &uu128);
		src = &uu128;
	}

	for (i = 0; i < 16; i++)
		uuid[15 - i] = src->value.u128.data[i];
}

static void hal_srvc_id_to_element_id(const struct hal_gatt_srvc_id *from,
							struct element_id *to)
{
	to->instance = from->inst_id;
	android2uuid(from->uuid, &to->uuid);
}

static void element_id_to_hal_srvc_id(const struct element_id *from,
						uint8_t primary,
						struct hal_gatt_srvc_id *to)
{
	to->is_primary = primary;
	to->inst_id = from->instance;
	uuid2android(&from->uuid, to->uuid);
}

static void hal_gatt_id_to_element_id(const struct hal_gatt_gatt_id *from,
							struct element_id *to)
{
	to->instance = from->inst_id;
	android2uuid(from->uuid, &to->uuid);
}

static void element_id_to_hal_gatt_id(const struct element_id *from,
						struct hal_gatt_gatt_id *to)
{
	to->inst_id = from->instance;
	uuid2android(&from->uuid, to->uuid);
}

static void destroy_characteristic(void *data)
{
	struct characteristic *chars = data;

	if (!chars)
		return;

	queue_destroy(chars->descriptors, free);
	free(chars);
}

static void destroy_service(void *data)
{
	struct service *srvc = data;

	if (!srvc)
		return;

	queue_destroy(srvc->chars, destroy_characteristic);

	/*
	 * Included services we keep on two queues.
	 * 1. On the same queue with primary services.
	 * 2. On the queue inside primary service.
	 * So we need to free service memory only once but we need to destroy
	 * two queues
	 */
	queue_destroy(srvc->included, NULL);

	free(srvc);
}

static bool match_app_by_uuid(const void *data, const void *user_data)
{
	const uint8_t *exp_uuid = user_data;
	const struct gatt_app *client = data;

	return !memcmp(exp_uuid, client->uuid, sizeof(client->uuid));
}

static bool match_app_by_id(const void *data, const void *user_data)
{
	int32_t exp_id = PTR_TO_INT(user_data);
	const struct gatt_app *client = data;

	return client->id == exp_id;
}

static struct gatt_app *find_app_by_id(int32_t id)
{
	return queue_find(gatt_apps, match_app_by_id, INT_TO_PTR(id));
}

static bool match_device_by_bdaddr(const void *data, const void *user_data)
{
	const struct gatt_device *dev = data;
	const bdaddr_t *addr = user_data;

	return !bacmp(&dev->bdaddr, addr);
}

static bool match_device_by_state(const void *data, const void *user_data)
{
	const struct gatt_device *dev = data;

	if (dev->state != PTR_TO_UINT(user_data))
		return false;

	return true;
}

static bool match_pending_device(const void *data, const void *user_data)
{
	const struct gatt_device *dev = data;

	if ((dev->state == DEVICE_CONNECT_INIT) ||
					(dev->state == DEVICE_CONNECT_READY))
		return true;

	return false;
}

static bool match_connection_by_id(const void *data, const void *user_data)
{
	const struct app_connection *conn = data;
	const int32_t id = PTR_TO_INT(user_data);

	return conn->id == id;
}

static bool match_connection_by_device_and_app(const void *data,
							const void *user_data)
{
	const struct app_connection *conn = data;
	const struct app_connection *match = user_data;

	return conn->device == match->device && conn->app == match->app;
}

static struct app_connection *find_connection_by_id(int32_t conn_id)
{
	struct app_connection *conn;

	conn = queue_find(app_connections, match_connection_by_id,
							INT_TO_PTR(conn_id));
	if (conn && conn->device->state == DEVICE_CONNECTED)
		return conn;

	return NULL;
}

static bool match_connection_by_device(const void *data, const void *user_data)
{
	const struct app_connection *conn = data;
	const struct gatt_device *dev = user_data;

	return conn->device == dev;
}

static bool match_connection_by_app(const void *data, const void *user_data)
{
	const struct app_connection *conn = data;
	const struct gatt_app *app = user_data;

	return conn->app == app;
}

static struct gatt_device *find_device_by_addr(const bdaddr_t *addr)
{
	return queue_find(gatt_devices, match_device_by_bdaddr, addr);
}

static struct gatt_device *find_pending_device(void)
{
	return queue_find(gatt_devices, match_pending_device, NULL);
}

static struct gatt_device *find_device_by_state(uint32_t state)
{
	return queue_find(gatt_devices, match_device_by_state,
							UINT_TO_PTR(state));
}

static bool match_srvc_by_element_id(const void *data, const void *user_data)
{
	const struct element_id *exp_id = user_data;
	const struct service *service = data;

	if (service->id.instance == exp_id->instance)
		return !bt_uuid_cmp(&service->id.uuid, &exp_id->uuid);

	return false;
}

static bool match_srvc_by_higher_inst_id(const void *data,
							const void *user_data)
{
	const struct service *s = data;
	uint8_t inst_id = PTR_TO_INT(user_data);

	/* For now we match inst_id as it is unique */
	return inst_id < s->id.instance;
}

static bool match_srvc_by_bt_uuid(const void *data, const void *user_data)
{
	const bt_uuid_t *exp_uuid = user_data;
	const struct service *service = data;

	return !bt_uuid_cmp(exp_uuid, &service->id.uuid);
}

static bool match_srvc_by_range(const void *data, const void *user_data)
{
	const struct service *srvc = data;
	const struct att_range *range = user_data;

	return !memcmp(&srvc->prim.range, range, sizeof(srvc->prim.range));
}

static bool match_char_by_higher_inst_id(const void *data,
							const void *user_data)
{
	const struct characteristic *ch = data;
	uint8_t inst_id = PTR_TO_INT(user_data);

	/* For now we match inst_id as it is unique, we'll match uuids later */
	return inst_id < ch->id.instance;
}

static bool match_descr_by_element_id(const void *data, const void *user_data)
{
	const struct element_id *exp_id = user_data;
	const struct descriptor *descr = data;

	if (exp_id->instance == descr->id.instance)
		return !bt_uuid_cmp(&descr->id.uuid, &exp_id->uuid);

	return false;
}

static bool match_descr_by_higher_inst_id(const void *data,
							const void *user_data)
{
	const struct descriptor *descr = data;
	uint8_t instance = PTR_TO_INT(user_data);

	/* For now we match instance as it is unique */
	return instance < descr->id.instance;
}

static bool match_notification(const void *a, const void *b)
{
	const struct notification_data *a1 = a;
	const struct notification_data *b1 = b;

	if (a1->conn != b1->conn)
		return false;

	if (memcmp(&a1->ch, &b1->ch, sizeof(a1->ch)))
		return false;

	if (memcmp(&a1->service, &b1->service, sizeof(a1->service)))
		return false;

	return true;
}

static bool match_char_by_element_id(const void *data, const void *user_data)
{
	const struct element_id *exp_id = user_data;
	const struct characteristic *chars = data;

	if (exp_id->instance == chars->id.instance)
		return !bt_uuid_cmp(&chars->id.uuid, &exp_id->uuid);

	return false;
}

static void destroy_notification(void *data)
{
	struct notification_data *notification = data;
	struct gatt_app *app;

	if (!notification)
		return;

	if (--notification->ref)
		return;

	app = notification->conn->app;
	queue_remove_if(app->notifications, match_notification, notification);
	free(notification);
}

static void unregister_notification(void *data)
{
	struct notification_data *notification = data;
	struct gatt_device *dev = notification->conn->device;

	/*
	 * No device means it was already disconnected and client cleanup was
	 * triggered afterwards, but once client unregisters, device stays if
	 * used by others. Then just unregister single handle.
	 */
	if (!queue_find(gatt_devices, NULL, dev))
		return;

	if (notification->notif_id && dev)
		g_attrib_unregister(dev->attrib, notification->notif_id);

	if (notification->ind_id && dev)
		g_attrib_unregister(dev->attrib, notification->ind_id);
}

static void device_set_state(struct gatt_device *dev, uint32_t state)
{
	char bda[18];

	if (dev->state == state)
		return;

	ba2str(&dev->bdaddr, bda);
	DBG("gatt: Device %s state changed %s -> %s", bda,
			device_state_str[dev->state], device_state_str[state]);

	dev->state = state;
}

static bool auto_connect_le(struct gatt_device *dev)
{
	/*  For LE devices use auto connect feature if possible */
	if (bt_kernel_conn_control()) {
		if (!bt_auto_connect_add(bt_get_id_addr(&dev->bdaddr, NULL)))
			return false;
	} else {
		/* Trigger discovery if not already started */
		if (!scanning && !bt_le_discovery_start()) {
			error("gatt: Could not start scan");
			return false;
		}
	}

	device_set_state(dev, DEVICE_CONNECT_INIT);
	return true;
}

static void connection_cleanup(struct gatt_device *device)
{
	if (device->watch_id) {
		g_source_remove(device->watch_id);
		device->watch_id = 0;
	}

	if (device->att_io) {
		g_io_channel_shutdown(device->att_io, FALSE, NULL);
		g_io_channel_unref(device->att_io);
		device->att_io = NULL;
	}

	if (device->attrib) {
		GAttrib *attrib = device->attrib;

		if (device->server_id > 0)
			g_attrib_unregister(device->attrib, device->server_id);

		if (device->ind_id > 0)
			g_attrib_unregister(device->attrib, device->ind_id);

		device->attrib = NULL;
		g_attrib_cancel_all(attrib);
		g_attrib_unref(attrib);
	}

	/*
	 * If device was in connection_pending or connectable state we
	 * search device list if we should stop the scan.
	 */
	if (!scanning && (device->state == DEVICE_CONNECT_INIT ||
				device->state == DEVICE_CONNECT_READY)) {
		if (!find_pending_device())
			bt_le_discovery_stop(NULL);
	}

	/* If device is not bonded service cache should be refreshed */
	if (!bt_device_is_bonded(&device->bdaddr))
		queue_remove_all(device->services, NULL, NULL, destroy_service);

	device_set_state(device, DEVICE_DISCONNECTED);

	if (!queue_isempty(device->autoconnect_apps))
		auto_connect_le(device);
	else
		bt_auto_connect_remove(&device->bdaddr);
}

static void destroy_gatt_app(void *data)
{
	struct gatt_app *app = data;

	if (!app)
		return;

	/*
	 * First we want to get all notifications and unregister them.
	 * We don't pass unregister_notification to queue_destroy,
	 * because destroy notification performs operations on queue
	 * too. So remove all elements and then destroy queue.
	 */

	if (app->type == GATT_CLIENT)
		while (queue_peek_head(app->notifications)) {
			struct notification_data *notification;

			notification = queue_pop_head(app->notifications);
			unregister_notification(notification);
		}

	queue_destroy(app->notifications, free);

	free(app);
}

struct pending_request {
	struct gatt_db_attribute *attrib;
	int length;
	uint8_t *value;
	uint16_t offset;

	uint8_t *filter_value;
	uint16_t filter_vlen;

	bool completed;
	uint8_t error;
};

static void destroy_pending_request(void *data)
{
	struct pending_request *entry = data;

	if (!entry)
		return;

	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);
	queue_destroy(dev->autoconnect_apps, NULL);

	bt_auto_connect_remove(&dev->bdaddr);

	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 struct gatt_device *create_device(const bdaddr_t *addr)
{
	struct gatt_device *dev;

	dev = new0(struct gatt_device, 1);

	bacpy(&dev->bdaddr, addr);

	dev->services = queue_new();
	dev->autoconnect_apps = queue_new();
	dev->pending_requests = queue_new();

	queue_push_head(gatt_devices, dev);

	return device_ref(dev);
}

static void send_client_connect_status_notify(struct app_connection *conn,
								int32_t status)
{
	struct hal_ev_gatt_client_connect ev;

	if (conn->app->func) {
		conn->app->func(&conn->device->bdaddr,
					status == GATT_SUCCESS ? 0 : -ENOTCONN,
					conn->device->attrib);
		return;
	}

	ev.client_if = conn->app->id;
	ev.conn_id = conn->id;
	ev.status = status;

	bdaddr2android(&conn->device->bdaddr, &ev.bda);

	ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT, HAL_EV_GATT_CLIENT_CONNECT,
							sizeof(ev), &ev);
}

static void send_server_connection_state_notify(struct app_connection *conn,
								bool connected)
{
	struct hal_ev_gatt_server_connection ev;

	if (conn->app->func) {
		conn->app->func(&conn->device->bdaddr,
					connected ? 0 : -ENOTCONN,
					conn->device->attrib);
		return;
	}

	ev.server_if = conn->app->id;
	ev.conn_id = conn->id;
	ev.connected = connected;

	bdaddr2android(&conn->device->bdaddr, &ev.bdaddr);

	ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
				HAL_EV_GATT_SERVER_CONNECTION, sizeof(ev), &ev);
}

static void send_client_disconnect_status_notify(struct app_connection *conn,
								int32_t status)
{
	struct hal_ev_gatt_client_disconnect ev;

	if (conn->app->func) {
		conn->app->func(&conn->device->bdaddr, -ENOTCONN,
						conn->device->attrib);
		return;
	}

	ev.client_if = conn->app->id;
	ev.conn_id = conn->id;
	ev.status = status;

	bdaddr2android(&conn->device->bdaddr, &ev.bda);

	ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
				HAL_EV_GATT_CLIENT_DISCONNECT, sizeof(ev), &ev);

}

static void notify_app_disconnect_status(struct app_connection *conn,
								int32_t status)
{
	if (!conn->app)
		return;

	if (conn->app->type == GATT_CLIENT)
		send_client_disconnect_status_notify(conn, status);
	else
		send_server_connection_state_notify(conn, !!status);
}

static void notify_app_connect_status(struct app_connection *conn,
								int32_t status)
{
	if (!conn->app)
		return;

	if (conn->app->type == GATT_CLIENT)
		send_client_connect_status_notify(conn, status);
	else
		send_server_connection_state_notify(conn, !status);
}

static void destroy_connection(void *data)
{
	struct app_connection *conn = data;

	if (!conn)
		return;

	if (conn->timeout_id > 0)
		g_source_remove(conn->timeout_id);

	switch (conn->device->state) {
	case DEVICE_CONNECTED:
		notify_app_disconnect_status(conn, GATT_SUCCESS);
		break;
	case DEVICE_CONNECT_INIT:
	case DEVICE_CONNECT_READY:
		notify_app_connect_status(conn, GATT_FAILURE);
		break;
	case DEVICE_DISCONNECTED:
		break;
	}

	if (!queue_find(app_connections, match_connection_by_device,
							conn->device))
		connection_cleanup(conn->device);

	queue_destroy(conn->transactions, free);
	device_unref(conn->device);
	free(conn);
}

static gboolean disconnected_cb(GIOChannel *io, GIOCondition cond,
							gpointer user_data)
{
	struct gatt_device *dev = user_data;
	int sock, err = 0;
	socklen_t len;

	sock = g_io_channel_unix_get_fd(io);
	len = sizeof(err);
	if (!getsockopt(sock, SOL_SOCKET, SO_ERROR, &err, &len))
		DBG("%s (%d)", strerror(err), err);

	queue_remove_all(app_connections, match_connection_by_device, dev,
							destroy_connection);

	return FALSE;
}

static bool get_local_mtu(struct gatt_device *dev, uint16_t *mtu)
{
	GIOChannel *io;
	uint16_t imtu, omtu;

	io = g_attrib_get_channel(dev->attrib);

	if (!bt_io_get(io, NULL, BT_IO_OPT_IMTU, &imtu, BT_IO_OPT_OMTU, &omtu,
							BT_IO_OPT_INVALID)) {
		error("gatt: Failed to get local MTU");
		return false;
	}

	/*
	 * Limit MTU to  MIN(IMTU, OMTU). This is to avoid situation where
	 * local OMTU < MIN(remote MTU, IMTU)
	 */
	if (mtu)
		*mtu = MIN(imtu, omtu);

	return true;
}

static void notify_client_mtu_change(struct app_connection *conn, bool success)
{
	struct hal_ev_gatt_client_configure_mtu ev;
	size_t mtu;

	g_attrib_get_buffer(conn->device->attrib, &mtu);

	ev.conn_id = conn->id;
	ev.status = success ? GATT_SUCCESS : GATT_FAILURE;
	ev.mtu = mtu;

	ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
			HAL_EV_GATT_CLIENT_CONFIGURE_MTU, sizeof(ev), &ev);
}

static void notify_server_mtu(struct app_connection *conn)
{
	struct hal_ev_gatt_server_mtu_changed ev;
	size_t mtu;

	g_attrib_get_buffer(conn->device->attrib, &mtu);

	ev.conn_id = conn->id;
	ev.mtu = mtu;

	ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
			HAL_EV_GATT_SERVER_MTU_CHANGED, sizeof(ev), &ev);
}

static void notify_mtu_change(void *data, void *user_data)
{
	struct gatt_device *device = user_data;
	struct app_connection *conn = data;

	if (conn->device != device)
		return;

	if (!conn->app) {
		error("gatt: can't notify mtu - no app registered for conn");
		return;
	}

	switch (conn->app->type) {
	case GATT_CLIENT:
		notify_client_mtu_change(conn, true);
		break;
	case GATT_SERVER:
		notify_server_mtu(conn);
		break;
	default:
		break;
	}
}

static bool update_mtu(struct gatt_device *device, uint16_t rmtu)
{
	uint16_t mtu, lmtu;

	if (!get_local_mtu(device, &lmtu))
		return false;

	DBG("remote_mtu:%d local_mtu:%d", rmtu, lmtu);

	if (rmtu < ATT_DEFAULT_LE_MTU) {
		error("gatt: remote MTU invalid (%u bytes)", rmtu);
		return false;
	}

	mtu = MIN(lmtu, rmtu);

	if (mtu == ATT_DEFAULT_LE_MTU)
		return true;

	if (!g_attrib_set_mtu(device->attrib, mtu)) {
		error("gatt: Failed to set MTU");
		return false;
	}

	queue_foreach(app_connections, notify_mtu_change, device);

	return true;
}

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,
							gpointer user_data)
{
	struct gatt_device *device = user_data;
	uint16_t rmtu;

	DBG("");

	if (status) {
		error("gatt: MTU exchange: %s", att_ecode2str(status));
		goto failed;
	}

	if (!dec_mtu_resp(pdu, plen, &rmtu)) {
		error("gatt: MTU exchange: protocol error");
		goto failed;
	}

	update_mtu(device, rmtu);

failed:
	device_unref(device);
}

static void send_exchange_mtu_request(struct gatt_device *device)
{
	uint16_t mtu;

	if (!get_local_mtu(device, &mtu))
		return;

	DBG("mtu %u", mtu);

	if (!gatt_exchange_mtu(device->attrib, mtu, exchange_mtu_cb,
							device_ref(device)))
		device_unref(device);
}

static void ignore_confirmation_cb(guint8 status, const guint8 *pdu,
					guint16 len, gpointer user_data)
{
	/* Ignored. */
}

static void notify_att_range_change(struct gatt_device *dev,
							struct att_range *range)
{
	uint16_t handle;
	uint16_t length = 0;
	uint16_t ccc;
	uint8_t *pdu;
	size_t mtu;
	GAttribResultFunc confirmation_cb = NULL;

	handle = gatt_db_attribute_get_handle(service_changed_attrib);
	if (!handle)
		return;

	ccc = bt_get_gatt_ccc(&dev->bdaddr);
	if (!ccc)
		return;

	pdu = g_attrib_get_buffer(dev->attrib, &mtu);

	switch (ccc) {
	case 0x0001:
		length = enc_notification(handle, (uint8_t *) range,
						sizeof(*range), pdu, mtu);
		break;
	case 0x0002:
		length = enc_indication(handle, (uint8_t *) range,
						sizeof(*range), pdu, mtu);
		confirmation_cb = ignore_confirmation_cb;
		break;
	default:
		/* 0xfff4 reserved for future use */
		break;
	}

	g_attrib_send(dev->attrib, 0, pdu, length, confirmation_cb, 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);

	/* 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();

	queue_push_head(app_connections, new_conn);

	new_conn->device = device_ref(device);

	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 (bt_string_to_uuid(&s->id.uuid, uuid) < 0) {
		error("gatt: Cannot convert string to uuid");
		free(s);
		return NULL;
	}

	s->chars = queue_new();
	s->included = queue_new();
	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;
}

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));

	for (; ranges; ranges = ranges->next) {
		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;
		}

		queue_push_tail(dev->services, s);

		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;

		queue_push_tail(dev->services, p);

		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 gboolean connection_timeout(void *user_data)
{
	struct app_connection *conn = user_data;

	conn->timeout_id = 0;

	queue_remove(app_connections, conn);
	destroy_connection(conn);

	return FALSE;
}

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));
		free(cb_data);

		return;
	}

	if (!services) {
		info("gatt: No primary services found");
		free(cb_data);

		return;
	}

	for (l = services; l; l = l->next) {
		struct gatt_primary *prim = l->data;
		uint8_t *new_uuid;
		bt_uuid_t uuid, u128;

		DBG("uuid: %s", prim->uuid);

		if (bt_string_to_uuid(&uuid, prim->uuid) < 0) {
			error("gatt: Cannot convert string to uuid");
			continue;
		}

		bt_uuid_to_uuid128(&uuid, &u128);
		new_uuid = g_memdup(&u128.value.u128, sizeof(u128.value.u128));

		uuids = g_slist_prepend(uuids, new_uuid);
	}

	bt_device_set_uuids(&dev->bdaddr, uuids);

	free(cb_data);

	conn->timeout_id = g_timeout_add_seconds(GATT_CONN_TIMEOUT,
						connection_timeout, conn);
}

static guint search_dev_for_srvc(struct app_connection *conn, bt_uuid_t *uuid)
{
	struct discover_srvc_data *cb_data;

	cb_data = new0(struct discover_srvc_data, 1);
	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;
};

static void notify_app_connect_status_by_device(void *data, void *user_data)
{
	struct app_connection *conn = data;
	struct connect_data *con_data = user_data;

	if (conn->device == con_data->dev)
		notify_app_connect_status(conn, con_data->status);
}

static struct app_connection *find_conn_without_app(struct gatt_device *dev)
{
	struct app_connection conn_match;

	conn_match.device = dev;
	conn_match.app = NULL;

	return queue_find(app_connections, match_connection_by_device_and_app,
								&conn_match);
}

static struct app_connection *find_conn(const bdaddr_t *addr, int32_t app_id)
{
	struct app_connection conn_match;
	struct gatt_device *dev;
	struct gatt_app *app;

	/* Check if app is registered */
	app = find_app_by_id(app_id);
	if (!app) {
		error("gatt: Client id %d not found", app_id);
		return NULL;
	}

	/* Check if device is known */
	dev = find_device_by_addr(addr);
	if (!dev) {
		error("gatt: Client id %d not found", app_id);
		return NULL;
	}

	conn_match.device = dev;
	conn_match.app = app;

	return queue_find(app_connections, match_connection_by_device_and_app,
								&conn_match);
}

static void create_app_connection(void *data, void *user_data)
{
	struct gatt_device *dev = user_data;
	struct gatt_app *app;

	app = find_app_by_id(PTR_TO_INT(data));
	if (!app)
		return;

	DBG("Autoconnect application id=%d", app->id);

	if (!find_conn(&dev->bdaddr, PTR_TO_INT(data)))
		create_connection(dev, app);
}

static void ind_handler(const uint8_t *cmd, uint16_t cmd_len,
							gpointer user_data)
{
	struct gatt_device *dev = user_data;
	uint16_t resp_length = 0;
	size_t length;
	uint8_t *opdu = g_attrib_get_buffer(dev->attrib, &length);

	/*
	 * We have to send confirmation here. If some client is
	 * registered for this indication, event will be send in
	 * handle_notification
	 */

	resp_length = enc_confirmation(opdu, length);
	g_attrib_send(dev->attrib, 0, opdu, resp_length, NULL, NULL, NULL);
}

static void connect_cb(GIOChannel *io, GError *gerr, gpointer user_data)
{
	struct gatt_device *dev = user_data;
	struct connect_data data;
	struct att_range range;
	uint32_t status;
	GError *err = NULL;
	GAttrib *attrib;
	uint16_t mtu, cid;

	if (dev->state != DEVICE_CONNECT_READY) {
		error("gatt: Device not in a connecting state!?");
		g_io_channel_shutdown(io, TRUE, NULL);
		return;
	}

	if (dev->att_io) {
		g_io_channel_unref(dev->att_io);
		dev->att_io = NULL;
	}

	if (gerr) {
		error("gatt: connection failed %s", gerr->message);
		device_set_state(dev, DEVICE_DISCONNECTED);
		status = GATT_FAILURE;
		goto reply;
	}

	if (!bt_io_get(io, &err, BT_IO_OPT_IMTU, &mtu, BT_IO_OPT_CID, &cid,
							BT_IO_OPT_INVALID)) {
		error("gatt: Could not get imtu or cid: %s", err->message);
		device_set_state(dev, DEVICE_DISCONNECTED);
		status = GATT_FAILURE;
		g_error_free(err);
		goto reply;
	}

	/* on BR/EDR MTU must not be less then minimal allowed MTU */
	if (cid != ATT_CID && mtu < ATT_DEFAULT_L2CAP_MTU) {
		error("gatt: MTU too small (%u bytes)", mtu);
		device_set_state(dev, DEVICE_DISCONNECTED);
		status = GATT_FAILURE;
		goto reply;
	}

	DBG("mtu %u cid %u", mtu, cid);

	/* on LE we always start with default MTU */
	if (cid == ATT_CID)
		mtu = ATT_DEFAULT_LE_MTU;

	attrib = g_attrib_new(io, mtu, true);
	if (!attrib) {
		error("gatt: unable to create new GAttrib instance");
		device_set_state(dev, DEVICE_DISCONNECTED);
		status = GATT_FAILURE;
		goto reply;
	}

	dev->attrib = attrib;
	dev->watch_id = g_io_add_watch(io, G_IO_HUP | G_IO_ERR | G_IO_NVAL,
							disconnected_cb, dev);

	dev->server_id = g_attrib_register(attrib, GATTRIB_ALL_REQS,
						GATTRIB_ALL_HANDLES,
						att_handler, dev, NULL);
	dev->ind_id = g_attrib_register(attrib, ATT_OP_HANDLE_IND,
						GATTRIB_ALL_HANDLES,
						ind_handler, dev, NULL);
	if ((dev->server_id && dev->ind_id) == 0)
		error("gatt: Could not attach to server");

	device_set_state(dev, DEVICE_CONNECTED);

	/* Send exchange mtu request as we assume being client and server */
	/* TODO: Dont exchange mtu if no client apps */

	/* MTU exchange shall not be used on BR/EDR - Vol 3. Part G. 4.3.1 */
	if (cid == ATT_CID)
		send_exchange_mtu_request(dev);

	/*
	 * Service Changed Characteristic and CCC Descriptor handles
	 * should not change if there are bonded devices. We have them
	 * constant all the time, thus they should be excluded from
	 * range indicating changes.
	 */
	range.start = gatt_db_attribute_get_handle(service_changed_attrib) + 2;
	range.end = 0xffff;

	/*
	 * If there is ccc stored for that device we were acting as server for
	 * it, and as we dont have last connect and last services (de)activation
	 * timestamps we should always assume something has changed.
	 */
	notify_att_range_change(dev, &range);

	status = GATT_SUCCESS;

reply:
	/*
	 * Make sure there are app_connections for all apps interested in auto
	 * connect to that device
	 */
	queue_foreach(dev->autoconnect_apps, create_app_connection, dev);

	if (!queue_find(app_connections, match_connection_by_device, dev)) {
		struct app_connection *conn;

		if (!dev->attrib)
			return;

		conn = create_connection(dev, NULL);
		if (!conn)
			return;

		if (bt_is_pairing(&dev->bdaddr))
			/*
			 * If there is bonding ongoing lets wait for paired
			 * callback. Once we get that we can start search
			 * services
			 */
			conn->timeout_id = g_timeout_add_seconds(
						GATT_PAIR_CONN_TIMEOUT,
						connection_timeout, conn);
		else
			/*
			 * There is no ongoing bonding, lets search for primary
			 * services
			 */
			search_dev_for_srvc(conn, NULL);
	}

	data.dev = dev;
	data.status = status;
	queue_foreach(app_connections, notify_app_connect_status_by_device,
									&data);

	/* For BR/EDR notify about MTU since it is not negotiable*/
	if (cid != ATT_CID && status == GATT_SUCCESS)
		queue_foreach(app_connections, notify_mtu_change, dev);

	device_unref(dev);

	/* Check if we should restart scan */
	if (scanning)
		bt_le_discovery_start();

	/* FIXME: What to do if discovery won't start here. */
}

static int connect_le(struct gatt_device *dev)
{
	GIOChannel *io;
	GError *gerr = NULL;
	char addr[18];
	const bdaddr_t *bdaddr;
	uint8_t bdaddr_type;

	ba2str(&dev->bdaddr, addr);

	/* There is one connection attempt going on */
	if (dev->att_io) {
		info("gatt: connection to dev %s is ongoing", addr);
		return -EALREADY;
	}

	DBG("Connection attempt to: %s", addr);

	bdaddr = bt_get_id_addr(&dev->bdaddr, &bdaddr_type);

	/*
	 * This connection will help us catch any PDUs that comes before
	 * pairing finishes
	 */
	io = bt_io_connect(connect_cb, device_ref(dev), NULL, &gerr,
					BT_IO_OPT_SOURCE_BDADDR, &adapter_addr,
					BT_IO_OPT_SOURCE_TYPE, BDADDR_LE_PUBLIC,
					BT_IO_OPT_DEST_BDADDR, bdaddr,
					BT_IO_OPT_DEST_TYPE, bdaddr_type,
					BT_IO_OPT_CID, ATT_CID,
					BT_IO_OPT_SEC_LEVEL, BT_IO_SEC_LOW,
					BT_IO_OPT_INVALID);
	if (!io) {
		error("gatt: Failed bt_io_connect(%s): %s", addr,
							gerr->message);
		g_error_free(gerr);
		return -EIO;
	}

	/* Keep this, so we can cancel the connection */
	dev->att_io = io;

	device_set_state(dev, DEVICE_CONNECT_READY);

	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 bt_le_discovery_stop_cb(void)
{
	DBG("");

	/* Check now if there is any device ready to connect */
	if (connect_next_dev() < 0)
		bt_le_discovery_start();
}

static void le_device_found_handler(const bdaddr_t *addr, int rssi,
					uint16_t eir_len, const void *eir,
					bool connectable, 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)
		goto done;

	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);

done:
	if (!connectable)
		return;

	/* We use auto connect feature from kernel if possible */
	if (bt_kernel_conn_control())
		return;

	dev = find_device_by_addr(addr);
	if (!dev) {
		if (!bonded)
			return;

		dev = create_device(addr);
	}

	if (dev->state != DEVICE_CONNECT_INIT)
		return;

	device_set_state(dev, DEVICE_CONNECT_READY);

	/*
	 * 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;
	}

	app = new0(struct gatt_app, 1);

	app->type = type;

	if (app->type == GATT_CLIENT)
		app->notifications = queue_new();

	memcpy(app->uuid, uuid, sizeof(app->uuid));

	app->id = application_id++;

	queue_push_head(gatt_apps, app);

	if (app->type == GATT_SERVER)
		queue_push_tail(listen_apps, INT_TO_PTR(app->id));

	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 handle_client_scan(const void *buf, uint16_t len)
{
	const struct hal_cmd_gatt_client_scan *cmd = buf;
	uint8_t status;

	DBG("new state %d", cmd->start);

	if (cmd->client_if != 0) {
		void *registered = find_app_by_id(cmd->client_if);

		if (!registered) {
			error("gatt: Client not registered");
			status = HAL_STATUS_FAILED;
			goto reply;
		}
	}

	/* Turn off scan */
	if (!cmd->start) {
		DBG("Stopping LE SCAN");

		if (scanning) {
			bt_le_discovery_stop(NULL);
			scanning = false;
		}

		status = HAL_STATUS_SUCCESS;
		goto reply;
	}

	/* Reply success if we already do scan */
	if (scanning) {
		status = HAL_STATUS_SUCCESS;
		goto reply;
	}

	/* Turn on scan */
	if (!bt_le_discovery_start()) {
		error("gatt: LE scan switch failed");
		status = HAL_STATUS_FAILED;
		goto reply;
	}

	scanning = true;
	status = HAL_STATUS_SUCCESS;

reply:
	ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT, HAL_OP_GATT_CLIENT_SCAN,
									status);
}

static int connect_bredr(struct gatt_device *dev)
{
	BtIOSecLevel sec_level;
	GIOChannel *io;
	GError *gerr = NULL;
	char addr[18];

	ba2str(&dev->bdaddr, addr);

	/* There is one connection attempt going on */
	if (dev->att_io) {
		info("gatt: connection to dev %s is ongoing", addr);
		return -EALREADY;
	}

	DBG("Connection attempt to: %s", addr);

	sec_level = bt_device_is_bonded(&dev->bdaddr) ? BT_IO_SEC_MEDIUM :
								BT_IO_SEC_LOW;

	io = bt_io_connect(connect_cb, device_ref(dev), NULL, &gerr,
					BT_IO_OPT_SOURCE_BDADDR, &adapter_addr,
					BT_IO_OPT_SOURCE_TYPE, BDADDR_BREDR,
					BT_IO_OPT_DEST_BDADDR, &dev->bdaddr,
					BT_IO_OPT_DEST_TYPE, BDADDR_BREDR,
					BT_IO_OPT_PSM, ATT_PSM,
					BT_IO_OPT_SEC_LEVEL, sec_level,
					BT_IO_OPT_INVALID);
	if (!io) {
		error("gatt: Failed bt_io_connect(%s): %s", addr,
							gerr->message);
		g_error_free(gerr);
		return -EIO;
	}

	device_set_state(dev, DEVICE_CONNECT_READY);

	/* Keep this, so we can cancel the connection */
	dev->att_io = io;

	return 0;
}

static bool trigger_connection(struct app_connection *conn, bool direct)
{
	switch (conn->device->state) {
	case DEVICE_DISCONNECTED:
		/*
		 *  If device was last seen over BR/EDR connect over it.
		 *  Note: Connection state is handled in connect_bredr() func
		 */
		if (bt_device_last_seen_bearer(&conn->device->bdaddr) ==
								BDADDR_BREDR)
			return connect_bredr(conn->device) == 0;

		if (direct)
			return connect_le(conn->device) == 0;

		bt_gatt_add_autoconnect(conn->app->id, &conn->device->bdaddr);
		return auto_connect_le(conn->device);
	case DEVICE_CONNECTED:
		notify_app_connect_status(conn, GATT_SUCCESS);
		return true;
	case DEVICE_CONNECT_READY:
	case DEVICE_CONNECT_INIT:
	default:
		/* In those cases connection is already triggered. */
		return true;
	}
}

static void remove_autoconnect_device(struct gatt_device *dev)
{
	bt_auto_connect_remove(&dev->bdaddr);

	if (dev->state == DEVICE_CONNECT_INIT)
		device_set_state(dev, DEVICE_DISCONNECTED);

	device_unref(dev);
}

static void clear_autoconnect_devices(void *data, void *user_data)
{
	struct gatt_device *dev = data;

	if (queue_remove(dev->autoconnect_apps, user_data))
		if (queue_isempty(dev->autoconnect_apps))
			remove_autoconnect_device(dev);
}

static uint8_t unregister_app(int client_if)
{
	struct gatt_app *cl;

	/*
	 * Make sure that there is no devices in auto connect list for this
	 * application
	 */
	queue_foreach(gatt_devices, clear_autoconnect_devices,
							INT_TO_PTR(client_if));

	cl = queue_remove_if(gatt_apps, match_app_by_id, INT_TO_PTR(client_if));
	if (!cl) {
		error("gatt: client_if=%d not found", client_if);

		return HAL_STATUS_FAILED;
	}

	/* Destroy app connections with proper notifications for this app. */
	queue_remove_all(app_connections, match_connection_by_app, cl,
							destroy_connection);
	destroy_gatt_app(cl);

	return HAL_STATUS_SUCCESS;
}

static void send_client_listen_notify(int32_t id, int32_t status)
{
	struct hal_ev_gatt_client_listen ev;

	/* Server if because of typo in android headers */
	ev.server_if = id;
	ev.status = status;

	ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT, HAL_EV_GATT_CLIENT_LISTEN,
							sizeof(ev), &ev);
}

struct listen_data {
	int32_t client_id;
	bool start;
};

static struct listen_data *create_listen_data(int32_t client_id, bool start)
{
	struct listen_data *d;

	d = new0(struct listen_data, 1);
	d->client_id = client_id;
	d->start = start;

	return d;
}

static void set_advertising_cb(uint8_t status, void *user_data)
{
	struct listen_data *l = user_data;

	send_client_listen_notify(l->client_id, status);

	/* In case of success update advertising state*/
	if (!status)
		advertising_cnt = l->start ? 1 : 0;

	/*
	 * Let's remove client from the list in two cases
	 * 1. Start failed
	 * 2. Stop succeed
	 */
	if ((l->start && status) || (!l->start && !status))
		queue_remove(listen_apps, INT_TO_PTR(l->client_id));

	free(l);
}

static void handle_client_unregister(const void *buf, uint16_t len)
{
	const struct hal_cmd_gatt_client_unregister *cmd = buf;
	uint8_t status;
	void *listening_client;
	struct listen_data *data;

	DBG("");

	listening_client = queue_find(listen_apps, NULL,
						INT_TO_PTR(cmd->client_if));

	if (listening_client) {
		advertising_cnt--;
		queue_remove(listen_apps, INT_TO_PTR(cmd->client_if));
	} else {
		status = unregister_app(cmd->client_if);
		goto reply;
	}

	if (!advertising_cnt) {
		data = create_listen_data(cmd->client_if, false);

		if (!bt_le_set_advertising(data->start, set_advertising_cb,
								data)) {
			error("gatt: Could not set advertising");
			status = HAL_STATUS_FAILED;
			free(data);
			goto reply;
		}
	}

	status = unregister_app(cmd->client_if);

reply:
	ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
					HAL_OP_GATT_CLIENT_UNREGISTER, status);
}

static uint8_t handle_connect(int32_t app_id, const bdaddr_t *addr, bool direct)
{
	struct app_connection conn_match;
	struct app_connection *conn;
	struct gatt_device *device;
	struct gatt_app *app;

	DBG("");

	app = find_app_by_id(app_id);
	if (!app)
		return HAL_STATUS_FAILED;

	device = find_device_by_addr(addr);
	if (!device)
		device = create_device(addr);

	conn_match.device = device;
	conn_match.app = app;

	conn = queue_find(app_connections, match_connection_by_device_and_app,
								&conn_match);
	if (!conn) {
		conn = create_connection(device, app);
		if (!conn)
			return HAL_STATUS_NOMEM;
	}

	if (!trigger_connection(conn, direct))
		return HAL_STATUS_FAILED;

	return HAL_STATUS_SUCCESS;
}

static void handle_client_connect(const void *buf, uint16_t len)
{
	const struct hal_cmd_gatt_client_connect *cmd = buf;
	uint8_t status;
	bdaddr_t addr;

	DBG("is_direct:%u transport:%u", cmd->is_direct, cmd->transport);

	android2bdaddr(&cmd->bdaddr, &addr);

	/* TODO handle transport flag */

	status = handle_connect(cmd->client_if, &addr, cmd->is_direct);

	ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT, HAL_OP_GATT_CLIENT_CONNECT,
								status);
}

static void handle_client_disconnect(const void *buf, uint16_t len)
{
	const struct hal_cmd_gatt_client_disconnect *cmd = buf;
	struct app_connection *conn;
	uint8_t status;

	DBG("");

	/* TODO: should we care to match also bdaddr when conn_id is unique? */
	conn = queue_remove_if(app_connections, match_connection_by_id,
						INT_TO_PTR(cmd->conn_id));
	destroy_connection(conn);

	status = HAL_STATUS_SUCCESS;

	ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
					HAL_OP_GATT_CLIENT_DISCONNECT, status);
}

static void handle_client_listen(const void *buf, uint16_t len)
{
	const struct hal_cmd_gatt_client_listen *cmd = buf;
	uint8_t status;
	struct listen_data *data;
	bool req_sent = false;
	void *listening_client;

	DBG("");

	if (!find_app_by_id(cmd->client_if)) {
		error("gatt: Client not registered");
		status = HAL_STATUS_FAILED;
		goto reply;
	}

	listening_client = queue_find(listen_apps, NULL,
						INT_TO_PTR(cmd->client_if));
	/* Start listening */
	if (cmd->start) {
		if (listening_client) {
			status = HAL_STATUS_SUCCESS;
			goto reply;
		}

		queue_push_tail(listen_apps, INT_TO_PTR(cmd->client_if));

		/* If listen is already on just return success*/
		if (advertising_cnt > 0) {
			advertising_cnt++;
			status = HAL_STATUS_SUCCESS;
			goto reply;
		}
	} else {
		/* Stop listening. Check if client was listening */
		if (!listening_client) {
			error("gatt: This client %d does not listen",
							cmd->client_if);
			status = HAL_STATUS_FAILED;
			goto reply;
		}

		/*
		 * In case there is more listening clients don't stop
		 * advertising
		 */
		if (advertising_cnt > 1) {
			advertising_cnt--;
			queue_remove(listen_apps, INT_TO_PTR(cmd->client_if));
			status = HAL_STATUS_SUCCESS;
			goto reply;
		}
	}

	data = create_listen_data(cmd->client_if, cmd->start);

	if (!bt_le_set_advertising(cmd->start, set_advertising_cb, data)) {
		error("gatt: Could not set advertising");
		status = HAL_STATUS_FAILED;
		free(data);
		goto reply;
	}

	/*
	 * Use this flag to keep in mind that we are waiting for callback with
	 * result
	 */
	req_sent = true;

	status = HAL_STATUS_SUCCESS;

reply:
	ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT, HAL_OP_GATT_CLIENT_LISTEN,
							status);

	/* In case of early success or error, just send notification up */
	if (!req_sent) {
		int32_t gatt_status = status == HAL_STATUS_SUCCESS ?
						GATT_SUCCESS : GATT_FAILURE;
		send_client_listen_notify(cmd->client_if, gatt_status);
	}
}

static void handle_client_refresh(const void *buf, uint16_t len)
{
	const struct hal_cmd_gatt_client_refresh *cmd = buf;
	struct gatt_device *dev;
	uint8_t status;
	bdaddr_t bda;

	/*
	 * This is Android's framework hidden API call. It seams that no
	 * notification is expected and Bluedroid silently updates device's
	 * cache under the hood. As we use lazy caching ,we can just clear the
	 * cache and we're done.
	 */

	DBG("");

	android2bdaddr(&cmd->bdaddr, &bda);
	dev = find_device_by_addr(&bda);
	if (!dev) {
		status = HAL_STATUS_FAILED;
		goto done;
	}

	queue_remove_all(dev->services, NULL, NULL, destroy_service);

	status = HAL_STATUS_SUCCESS;

done:
	ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT, HAL_OP_GATT_CLIENT_REFRESH,
									status);
}

static void handle_client_search_service(const void *buf, uint16_t len)
{
	const struct hal_cmd_gatt_client_search_service *cmd = buf;
	struct app_connection *conn;
	uint8_t status;
	struct service *s;
	bt_uuid_t uuid;
	guint srvc_search_success;

	DBG("");

	if (len != sizeof(*cmd) + (cmd->filtered ? 16 : 0)) {
		error("Invalid search service size (%u bytes), terminating",
									len);
		raise(SIGTERM);
		return;
	}

	conn = find_connection_by_id(cmd->conn_id);
	if (!conn) {
		error("gatt: dev with conn_id=%d not found", cmd->conn_id);

		status = HAL_STATUS_FAILED;
		goto reply;
	}

	if (conn->device->state != DEVICE_CONNECTED) {
		char bda[18];

		ba2str(&conn->device->bdaddr, bda);
		error("gatt: device %s not connected", bda);

		status = HAL_STATUS_FAILED;
		goto reply;
	}

	if (cmd->filtered)
		android2uuid(cmd->filter_uuid, &uuid);

	/* Services not cached yet */
	if (queue_isempty(conn->device->services)) {
		if (cmd->filtered)
			srvc_search_success = search_dev_for_srvc(conn, &uuid);
		else
			srvc_search_success = search_dev_for_srvc(conn, NULL);

		if (!srvc_search_success) {
			status = HAL_STATUS_FAILED;
			goto reply;
		}

		status = HAL_STATUS_SUCCESS;
		goto reply;
	}

	/* Search in cached services for given service */
	if (cmd->filtered) {
		/* Search in cache for service by uuid */
		s = queue_find(conn->device->services, match_srvc_by_bt_uuid,
									&uuid);

		if (s) {
			send_client_primary_notify(s, INT_TO_PTR(conn->id));
		} else {
			if (!search_dev_for_srvc(conn, &uuid)) {
				status = HAL_STATUS_FAILED;
				goto reply;
			}

			status = HAL_STATUS_SUCCESS;
			goto reply;
		}
	} else {
		/* Refresh service cache if only partial search was performed */
		if (conn->device->partial_srvc_search) {
			srvc_search_success = search_dev_for_srvc(conn, NULL);
			if (!srvc_search_success) {
				status = HAL_STATUS_FAILED;
				goto reply;
			}
		} else
			queue_foreach(conn->device->services,
						send_client_primary_notify,
						INT_TO_PTR(cmd->conn_id));
	}

	send_client_search_complete_notify(GATT_SUCCESS, conn->id);

	status = HAL_STATUS_SUCCESS;

reply:
	ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
			HAL_OP_GATT_CLIENT_SEARCH_SERVICE, status);
}

static void send_client_incl_service_notify(const struct element_id *srvc_id,
						const struct service *incl,
						int32_t conn_id)
{
	struct hal_ev_gatt_client_get_inc_service ev;

	memset(&ev, 0, sizeof(ev));

	ev.conn_id = conn_id;

	element_id_to_hal_srvc_id(srvc_id, 1, &ev.srvc_id);

	if (incl) {
		element_id_to_hal_srvc_id(&incl->id, 0, &ev.incl_srvc_id);
		ev.status = GATT_SUCCESS;
	} else {
		ev.status = GATT_FAILURE;
	}

	ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT ,
					HAL_EV_GATT_CLIENT_GET_INC_SERVICE,
					sizeof(ev), &ev);
}

struct get_included_data {
	struct service *prim;
	struct app_connection *conn;
};

static int get_inst_id_of_prim_services(const struct gatt_device *dev)
{
	struct service *s = queue_peek_tail(dev->services);

	if (s)
		return s->id.instance;

	return -1;
}

static void get_included_cb(uint8_t status, GSList *included, void *user_data)
{
	struct get_included_data *data = user_data;
	struct app_connection *conn = data->conn;
	struct service *service = data->prim;
	struct service *incl = NULL;
	int instance_id;

	DBG("");

	free(data);

	if (status) {
		error("gatt: no included services found");
		goto failed;
	}

	/* Remember that we already search included services.*/
	service->incl_search_done = true;

	/*
	 * There might be multiply services with same uuid. Therefore make sure
	 * each service has unique instance id. Let's take the latest instance
	 * id of primary service and start iterate included services from this
	 * point.
	 */
	instance_id = get_inst_id_of_prim_services(conn->device);
	if (instance_id < 0)
		goto failed;

	for (; included; included = included->next) {
		struct gatt_included *included_service = included->data;

		incl = create_service(++instance_id, false,
							included_service->uuid,
							included_service);
		if (!incl)
			continue;

		/*
		 * Lets keep included service on two queues.
		 * 1. on services queue together with primary service
		 * 2. on special queue inside primary service
		 */
		queue_push_tail(service->included, incl);
		queue_push_tail(conn->device->services, incl);
	}

	/*
	 * Notify upper layer about first included service.
	 * Android framework will iterate for next one.
	 */
	incl = queue_peek_head(service->included);

failed:
	send_client_incl_service_notify(&service->id, incl, conn->id);
}

static void search_included_services(struct app_connection *conn,
							struct service *service)
{
	struct get_included_data *data;
	uint16_t start, end;

	data = new0(struct get_included_data, 1);
	data->prim = service;
	data->conn = conn;

	if (service->primary) {
		start = service->prim.range.start;
		end = service->prim.range.end;
	} else {
		start = service->incl.range.start;
		end = service->incl.range.end;
	}

	gatt_find_included(conn->device->attrib, start, end, get_included_cb,
									data);
}

static bool find_service(int32_t conn_id, struct element_id *service_id,
					struct app_connection **connection,
					struct service **service)
{
	struct service *srvc;
	struct app_connection *conn;

	conn = find_connection_by_id(conn_id);
	if (!conn) {
		error("gatt: conn_id=%d not found", conn_id);
		return false;
	}

	srvc = queue_find(conn->device->services, match_srvc_by_element_id,
								service_id);
	if (!srvc) {
		error("gatt: Service with inst_id: %d not found",
							service_id->instance);
		return false;
	}

	*connection = conn;
	*service = srvc;

	return true;
}

static void handle_client_get_included_service(const void *buf, uint16_t len)
{
	const struct hal_cmd_gatt_client_get_included_service *cmd = buf;
	struct app_connection *conn;
	struct service *prim_service;
	struct service *incl_service = NULL;
	struct element_id match_id;
	struct element_id srvc_id;
	uint8_t status;

	DBG("");

	hal_srvc_id_to_element_id(&cmd->srvc_id, &srvc_id);

	if (len != sizeof(*cmd) +
			(cmd->continuation ? sizeof(cmd->incl_srvc_id[0]) : 0)) {
		error("Invalid get incl services size (%u bytes), terminating",
									len);
		raise(SIGTERM);
		return;
	}

	hal_srvc_id_to_element_id(&cmd->srvc_id, &match_id);
	if (!find_service(cmd->conn_id, &match_id, &conn, &prim_service)) {
		status = HAL_STATUS_FAILED;
		goto notify;
	}

	if (!prim_service->incl_search_done) {
		search_included_services(conn, prim_service);
		status = HAL_STATUS_SUCCESS;
		goto reply;
	}

	/* Try to use cache here */
	if (!cmd->continuation) {
		incl_service = queue_peek_head(prim_service->included);
	} else {
		uint8_t inst_id = cmd->incl_srvc_id[0].inst_id;

		incl_service = queue_find(prim_service->included,
						match_srvc_by_higher_inst_id,
						INT_TO_PTR(inst_id));
	}

	status = HAL_STATUS_SUCCESS;

notify:
	/*
	 * In case of error in handling request we need to send event with
	 * service id of cmd and gatt failure status.
	 */
	send_client_incl_service_notify(&srvc_id, incl_service, cmd->conn_id);

reply:
	ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
			HAL_OP_GATT_CLIENT_GET_INCLUDED_SERVICE, status);
}

static void send_client_char_notify(const struct hal_gatt_srvc_id *service,
					const struct hal_gatt_gatt_id *charac,
					int32_t char_prop, int32_t conn_id)
{
	struct hal_ev_gatt_client_get_characteristic ev;

	ev.conn_id = conn_id;

	if (charac) {
		memcpy(&ev.char_id, charac, sizeof(struct hal_gatt_gatt_id));
		ev.char_prop = char_prop;
		ev.status = GATT_SUCCESS;
	} else {
		memset(&ev.char_id, 0, sizeof(struct hal_gatt_gatt_id));
		ev.char_prop = 0;
		ev.status = GATT_FAILURE;
	}

	memcpy(&ev.srvc_id, service, sizeof(struct hal_gatt_srvc_id));

	ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
					HAL_EV_GATT_CLIENT_GET_CHARACTERISTIC,
					sizeof(ev), &ev);
}

static void convert_send_client_char_notify(const struct characteristic *ch,
						int32_t conn_id,
						const struct service *service)
{
	struct hal_gatt_srvc_id srvc;
	struct hal_gatt_gatt_id charac;

	element_id_to_hal_srvc_id(&service->id, service->primary, &srvc);

	if (ch) {
		element_id_to_hal_gatt_id(&ch->id, &charac);
		send_client_char_notify(&srvc, &charac, ch->ch.properties,
								conn_id);
	} else {
		send_client_char_notify(&srvc, NULL, 0, conn_id);
	}
}

static void cache_all_srvc_chars(struct service *srvc, GSList *characteristics)
{
	uint16_t inst_id = 0;
	bt_uuid_t uuid;

	for (; characteristics; characteristics = characteristics->next) {
		struct characteristic *ch;

		ch = new0(struct characteristic, 1);
		ch->descriptors = queue_new();

		memcpy(&ch->ch, characteristics->data, sizeof(ch->ch));

		bt_string_to_uuid(&uuid, ch->ch.uuid);
		bt_uuid_to_uuid128(&uuid, &ch->id.uuid);

		/*
		 * For now we increment inst_id and use it as characteristic
		 * handle
		 */
		ch->id.instance = ++inst_id;

		/* Store end handle to use later for descriptors discovery */
		if (characteristics->next) {
			struct gatt_char *next = characteristics->next->data;

			ch->end_handle = next->handle - 1;
		} else {
			ch->end_handle = srvc->primary ? srvc->prim.range.end :
							srvc->incl.range.end;
		}

		DBG("attr handle = 0x%04x, end handle = 0x%04x uuid: %s",
				ch->ch.handle, ch->end_handle, ch->ch.uuid);

		queue_push_tail(srvc->chars, ch);
	}
}

struct discover_char_data {
	int32_t conn_id;
	struct service *service;
};

static void discover_char_cb(uint8_t status, GSList *characteristics,
								void *user_data)
{
	struct discover_char_data *data = user_data;
	struct service *srvc = data->service;

	if (status) {
		error("gatt: Failed to get characteristics: %s",
							att_ecode2str(status));
		convert_send_client_char_notify(NULL, data->conn_id, srvc);
		goto done;
	}

	if (queue_isempty(srvc->chars))
		cache_all_srvc_chars(srvc, characteristics);

	convert_send_client_char_notify(queue_peek_head(srvc->chars),
							data->conn_id, srvc);

done:
	free(data);
}

static void handle_client_get_characteristic(const void *buf, uint16_t len)
{
	const struct hal_cmd_gatt_client_get_characteristic *cmd = buf;
	struct characteristic *ch;
	struct element_id match_id;
	struct app_connection *conn;
	struct service *srvc;
	uint8_t status;

	DBG("");

	if (len != sizeof(*cmd) + (cmd->continuation ? sizeof(cmd->char_id[0]) : 0)) {
		error("Invalid get characteristic size (%u bytes), terminating",
									len);
		raise(SIGTERM);
		return;
	}

	hal_srvc_id_to_element_id(&cmd->srvc_id, &match_id);
	if (!find_service(cmd->conn_id, &match_id, &conn, &srvc)) {
		status = HAL_STATUS_FAILED;
		goto done;
	}

	/* Discover all characteristics for services if not cached yet */
	if (queue_isempty(srvc->chars)) {
		struct discover_char_data *cb_data;
		struct att_range range;

		cb_data = new0(struct discover_char_data, 1);
		cb_data->service = srvc;
		cb_data->conn_id = conn->id;

		range = srvc->primary ? srvc->prim.range : srvc->incl.range;

		if (!gatt_discover_char(conn->device->attrib, range.start,
						range.end, NULL,
						discover_char_cb, cb_data)) {
			free(cb_data);

			status = HAL_STATUS_FAILED;
			goto done;
		}

		status = HAL_STATUS_SUCCESS;
		goto done;
	}

	if (cmd->continuation)
		ch = queue_find(srvc->chars, match_char_by_higher_inst_id,
					INT_TO_PTR(cmd->char_id[0].inst_id));
	else
		ch = queue_peek_head(srvc->chars);

	convert_send_client_char_notify(ch, conn->id, srvc);

	status = HAL_STATUS_SUCCESS;

done:
	if (status != HAL_STATUS_SUCCESS)
		send_client_char_notify(&cmd->srvc_id, NULL, 0, cmd->conn_id);

	ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
				HAL_OP_GATT_CLIENT_GET_CHARACTERISTIC, status);
}

static void send_client_descr_notify(int32_t status, int32_t conn_id,
					bool primary,
					const struct element_id *srvc,
					const struct element_id *ch,
					const struct element_id *opt_descr)
{
	struct hal_ev_gatt_client_get_descriptor ev;

	memset(&ev, 0, sizeof(ev));

	ev.status = status;
	ev.conn_id = conn_id;

	element_id_to_hal_srvc_id(srvc, primary, &ev.srvc_id);
	element_id_to_hal_gatt_id(ch, &ev.char_id);

	if (opt_descr)
		element_id_to_hal_gatt_id(opt_descr, &ev.descr_id);

	ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
			HAL_EV_GATT_CLIENT_GET_DESCRIPTOR, sizeof(ev), &ev);
}

struct discover_desc_data {
	struct app_connection *conn;
	struct service *srvc;
	struct characteristic *ch;
};

static void gatt_discover_desc_cb(guint8 status, GSList *descs,
							gpointer user_data)
{
	struct discover_desc_data *data = user_data;
	struct app_connection *conn = data->conn;
	struct service *srvc = data->srvc;
	struct characteristic *ch = data->ch;
	struct descriptor *descr;
	int i = 0;

	if (status != 0) {
		error("Discover all characteristic descriptors failed [%s]: %s",
					ch->ch.uuid, att_ecode2str(status));
		goto reply;
	}

	for ( ; descs; descs = descs->next) {
		struct gatt_desc *desc = descs->data;
		bt_uuid_t uuid;

		descr = new0(struct descriptor, 1);

		bt_string_to_uuid(&uuid, desc->uuid);
		bt_uuid_to_uuid128(&uuid, &descr->id.uuid);

		descr->id.instance = ++i;
		descr->handle = desc->handle;

		DBG("attr handle = 0x%04x, uuid: %s", desc->handle, desc->uuid);

		queue_push_tail(ch->descriptors, descr);
	}

reply:
	descr = queue_peek_head(ch->descriptors);

	send_client_descr_notify(status ? GATT_FAILURE : GATT_SUCCESS, conn->id,
					srvc->primary, &srvc->id, &ch->id,
					descr ? &descr->id : NULL);

	free(data);
}

static bool build_descr_cache(struct app_connection *conn, struct service *srvc,
						struct characteristic *ch)
{
	struct discover_desc_data *cb_data;
	uint16_t start, end;

	/* Clip range to given characteristic */
	start = ch->ch.value_handle + 1;
	end = ch->end_handle;

	/* If there are no descriptors, notify with fail status. */
	if (start > end)
		return false;

	cb_data = new0(struct discover_desc_data, 1);
	cb_data->conn = conn;
	cb_data->srvc = srvc;
	cb_data->ch = ch;

	if (!gatt_discover_desc(conn->device->attrib, start, end, NULL,
					gatt_discover_desc_cb, cb_data)) {
		free(cb_data);
		return false;
	}

	return true;
}

static void handle_client_get_descriptor(const void *buf, uint16_t len)
{
	const struct hal_cmd_gatt_client_get_descriptor *cmd = buf;
	struct descriptor *descr = NULL;
	struct characteristic *ch;
	struct service *srvc;
	struct element_id srvc_id;
	struct element_id char_id;
	struct app_connection *conn;
	int32_t conn_id;
	uint8_t primary;
	uint8_t status;

	DBG("");

	if (len != sizeof(*cmd) +
			(cmd->continuation ? sizeof(cmd->descr_id[0]) : 0)) {
		error("gatt: Invalid get descr command (%u bytes), terminating",
									len);

		raise(SIGTERM);
		return;
	}

	conn_id = cmd->conn_id;
	primary = cmd->srvc_id.is_primary;

	hal_srvc_id_to_element_id(&cmd->srvc_id, &srvc_id);
	hal_gatt_id_to_element_id(&cmd->char_id, &char_id);

	if (!find_service(conn_id, &srvc_id, &conn, &srvc)) {
		error("gatt: Get descr. could not find service");

		status = HAL_STATUS_FAILED;
		goto failed;
	}

	ch = queue_find(srvc->chars, match_char_by_element_id, &char_id);
	if (!ch) {
		error("gatt: Get descr. could not find characteristic");

		status = HAL_STATUS_FAILED;
		goto failed;
	}

	if (queue_isempty(ch->descriptors)) {
		if (build_descr_cache(conn, srvc, ch)) {
			ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
					HAL_OP_GATT_CLIENT_GET_DESCRIPTOR,
					HAL_STATUS_SUCCESS);
			return;
		}
	}

	status = HAL_STATUS_SUCCESS;

	/* Send from cache */
	if (cmd->continuation)
		descr = queue_find(ch->descriptors,
					match_descr_by_higher_inst_id,
					INT_TO_PTR(cmd->descr_id[0].inst_id));
	else
		descr = queue_peek_head(ch->descriptors);

failed:
	send_client_descr_notify(descr ? GATT_SUCCESS : GATT_FAILURE, conn_id,
						primary, &srvc_id, &char_id,
						&descr->id);

	ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
				HAL_OP_GATT_CLIENT_GET_DESCRIPTOR, status);
}

struct char_op_data {
	int32_t conn_id;
	const struct element_id *srvc_id;
	const struct element_id *char_id;
	uint8_t primary;
};

static struct char_op_data *create_char_op_data(int32_t conn_id,
						const struct element_id *s_id,
						const struct element_id *ch_id,
						bool primary)
{
	struct char_op_data *d;

	d = new0(struct char_op_data, 1);
	d->conn_id = conn_id;
	d->srvc_id = s_id;
	d->char_id = ch_id;
	d->primary = primary;

	return d;
}

static void send_client_read_char_notify(int32_t status, const uint8_t *pdu,
						uint16_t len, int32_t conn_id,
						const struct element_id *s_id,
						const struct element_id *ch_id,
						uint8_t primary)
{
	uint8_t buf[IPC_MTU];
	struct hal_ev_gatt_client_read_characteristic *ev = (void *) buf;
	ssize_t vlen;

	memset(buf, 0, sizeof(buf));

	ev->conn_id = conn_id;
	ev->status = status;
	ev->data.status = status;

	element_id_to_hal_srvc_id(s_id, primary, &ev->data.srvc_id);
	element_id_to_hal_gatt_id(ch_id, &ev->data.char_id);

	if (status == 0 && pdu) {
		vlen = dec_read_resp(pdu, len, ev->data.value, sizeof(buf));
		if (vlen < 0) {
			error("gatt: Protocol error");
			ev->status = GATT_FAILURE;
		} else {
			ev->data.len = vlen;
		}
	}

	ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
					HAL_EV_GATT_CLIENT_READ_CHARACTERISTIC,
					sizeof(*ev) + ev->data.len, ev);
}

static void read_char_cb(guint8 status, const guint8 *pdu, guint16 len,
							gpointer user_data)
{
	struct char_op_data *data = user_data;

	send_client_read_char_notify(status, pdu, len, data->conn_id,
						data->srvc_id, data->char_id,
						data->primary);

	free(data);
}

static int get_cid(struct gatt_device *dev)
{
	GIOChannel *io;
	uint16_t cid;

	io = g_attrib_get_channel(dev->attrib);

	if (!bt_io_get(io, NULL, BT_IO_OPT_CID, &cid, BT_IO_OPT_INVALID)) {
		error("gatt: Failed to get CID");
		return -1;
	}

	return cid;
}

static int get_sec_level(struct gatt_device *dev)
{
	GIOChannel *io;
	int sec_level;

	io = g_attrib_get_channel(dev->attrib);

	if (!bt_io_get(io, NULL, BT_IO_OPT_SEC_LEVEL, &sec_level,
							BT_IO_OPT_INVALID)) {
		error("gatt: Failed to get sec_level");
		return -1;
	}

	return sec_level;
}

static bool set_security(struct gatt_device *device, int req_sec_level)
{
	int sec_level;
	GError *gerr = NULL;
	GIOChannel *io;

	sec_level = get_sec_level(device);
	if (sec_level < 0)
		return false;

	if (req_sec_level <= sec_level)
		return true;

	io = g_attrib_get_channel(device->attrib);
	if (!io)
		return false;

	bt_io_set(io, &gerr, BT_IO_OPT_SEC_LEVEL, req_sec_level,
							BT_IO_OPT_INVALID);
	if (gerr) {
		error("gatt: Failed to set security level: %s", gerr->message);
		g_error_free(gerr);
		return false;
	}

	return true;
}

bool bt_gatt_set_security(const bdaddr_t *bdaddr, int sec_level)
{
	struct gatt_device *device;

	device = find_device_by_addr(bdaddr);
	if (!device)
		return false;

	return set_security(device, sec_level);
}

static bool set_auth_type(struct gatt_device *device, int auth_type)
{
	int sec_level;

	switch (auth_type) {
	case HAL_GATT_AUTHENTICATION_MITM:
		sec_level = BT_SECURITY_HIGH;
		break;
	case HAL_GATT_AUTHENTICATION_NO_MITM:
		sec_level = BT_SECURITY_MEDIUM;
		break;
	case HAL_GATT_AUTHENTICATION_NONE:
		sec_level = BT_SECURITY_LOW;
		break;
	default:
		error("gatt: Invalid auth_type value: %d", auth_type);
		return false;
	}

	return set_security(device, sec_level);
}

static void handle_client_read_characteristic(const void *buf, uint16_t len)
{
	const struct hal_cmd_gatt_client_read_characteristic *cmd = buf;
	struct char_op_data *cb_data;
	struct characteristic *ch;
	struct app_connection *conn;
	struct service *srvc;
	struct element_id srvc_id;
	struct element_id char_id;
	uint8_t status;

	DBG("");

	/* TODO authorization needs to be handled */

	hal_srvc_id_to_element_id(&cmd->srvc_id, &srvc_id);
	hal_gatt_id_to_element_id(&cmd->char_id, &char_id);

	if (!find_service(cmd->conn_id, &srvc_id, &conn, &srvc)) {
		status = HAL_STATUS_FAILED;
		goto failed;
	}

	/* search characteristics by element id */
	ch = queue_find(srvc->chars, match_char_by_element_id, &char_id);
	if (!ch) {
		error("gatt: Characteristic with inst_id: %d not found",
							cmd->char_id.inst_id);
		status = HAL_STATUS_FAILED;
		goto failed;
	}

	cb_data = create_char_op_data(cmd->conn_id, &srvc->id, &ch->id,
						cmd->srvc_id.is_primary);

	if (!set_auth_type(conn->device, cmd->auth_req)) {
		error("gatt: Failed to set security %d", cmd->auth_req);
		status = HAL_STATUS_FAILED;
		free(cb_data);
		goto failed;
	}

	if (!gatt_read_char(conn->device->attrib, ch->ch.value_handle,
						read_char_cb, cb_data)) {
		error("gatt: Cannot read characteristic with inst_id: %d",
							cmd->char_id.inst_id);
		status = HAL_STATUS_FAILED;
		free(cb_data);
		goto failed;
	}

	status = HAL_STATUS_SUCCESS;

failed:
	ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
				HAL_OP_GATT_CLIENT_READ_CHARACTERISTIC, status);

	/*
	 * We should send notification with service, characteristic id in case
	 * of errors.
	 */
	if (status != HAL_STATUS_SUCCESS)
		send_client_read_char_notify(GATT_FAILURE, NULL, 0,
						cmd->conn_id, &srvc_id,
						&char_id,
						cmd->srvc_id.is_primary);
}

static void send_client_write_char_notify(int32_t status, int32_t conn_id,
					const struct element_id *srvc_id,
					const struct element_id *char_id,
					uint8_t primary)
{
	struct hal_ev_gatt_client_write_characteristic ev;

	memset(&ev, 0, sizeof(ev));

	ev.conn_id = conn_id;
	ev.status = status;
	ev.data.status = status;

	element_id_to_hal_srvc_id(srvc_id, primary, &ev.data.srvc_id);
	element_id_to_hal_gatt_id(char_id, &ev.data.char_id);

	ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
					HAL_EV_GATT_CLIENT_WRITE_CHARACTERISTIC,
					sizeof(ev), &ev);
}

static void write_char_cb(guint8 status, const guint8 *pdu, guint16 len,
							gpointer user_data)
{
	struct char_op_data *data = user_data;

	send_client_write_char_notify(status, data->conn_id, data->srvc_id,
						data->char_id, data->primary);

	free(data);
}

static guint signed_write_cmd(struct gatt_device *dev, uint16_t handle,
					const uint8_t *value, uint16_t vlen)
{
	uint8_t csrk[16];
	uint32_t sign_cnt;
	guint res;

	memset(csrk, 0, 16);

	if (!bt_get_csrk(&dev->bdaddr, true, csrk, &sign_cnt, NULL)) {
		error("gatt: Could not get csrk key");
		return 0;
	}

	res = gatt_signed_write_cmd(dev->attrib, handle, value, vlen, crypto,
						csrk, sign_cnt, NULL, NULL);
	if (!res) {
		error("gatt: Signed write command failed");
		return 0;
	}

	bt_update_sign_counter(&dev->bdaddr, true, ++sign_cnt);

	return res;
}

static void handle_client_write_characteristic(const void *buf, uint16_t len)
{
	const struct hal_cmd_gatt_client_write_characteristic *cmd = buf;
	struct char_op_data *cb_data = NULL;
	struct characteristic *ch;
	struct app_connection *conn;
	struct service *srvc;
	struct element_id srvc_id;
	struct element_id char_id;
	uint8_t status;
	guint res;

	DBG("");

	if (len != sizeof(*cmd) + cmd->len) {
		error("Invalid write char size (%u bytes), terminating", len);
		raise(SIGTERM);
		return;
	}

	hal_srvc_id_to_element_id(&cmd->srvc_id, &srvc_id);
	hal_gatt_id_to_element_id(&cmd->char_id, &char_id);

	if (!find_service(cmd->conn_id, &srvc_id, &con