fish

35 files changed, 11606 insertions, 68 deletions

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..35c695c
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,7 @@
+main
+test.zip
+*.o
+core.*
+*~
+nrfdfu
+.*.swp
diff --git a/Makefile b/Makefile
index 08587b8..f4a387b 100644
--- a/Makefile
+++ b/Makefile
@@ -1,8 +1,10 @@
 CSRCS=nrfdfu.c util.c zip.c ble.c manifest.c dfu.c
+BLUEZ_SRCS=bluez/att.c bluez/queue.c bluez/crypto.c bluez/util.c bluez/io-mainloop.c bluez/timeout-mainloop.c bluez/mainloop.c bluez/gatt-db.c bluez/uuid.c bluez/gatt-client.c bluez/gatt-helpers.c
 
 PROG=nrfdfu
 
 OPT=-g
+WARN=-Wall
 
 
 CPROTO=cproto
@@ -17,8 +19,8 @@ BLUEZ_LIBS=$(shell pkg-config --libs bluez)
 INCLUDES=${ZIP_INCLUDES} ${JSON_C_INCLUDES} ${BLUEZ_INCLUDES}
 LIBS=${ZIP_LIBS} ${JSON_C_LIBS} ${BLUEZ_LIBS}
 CPPFLAGS=${DEFINES} ${INCLUDES}
-CFLAGS=${OPT}
-OBJS=${CSRCS:%.c=%.o}
+CFLAGS=${OPT} ${WARN}
+OBJS=${CSRCS:%.c=%.o} ${BLUEZ_SRCS:%.c=%.o}
 
 
 all: ${PROG}
@@ -32,4 +34,8 @@ protos:
 	mv prototypes.new prototypes.h
 
 clean:
-	/bin/rm -f ${OBJS}
+	/bin/rm -f ${OBJS} ${PROG} *~ core.* core
+
+test: ${PROG}
+	./${PROG}  -b fd:f9:62:4a:8a:c8 -p test.zip
+
diff --git a/ble.c b/ble.c
index 9f0b683..6e16adf 100644
--- a/ble.c
+++ b/ble.c
@@ -1,9 +1,60 @@
 #include "project.h"
 
+#include "bluez/gatt-client.h"
+#include "bluez/mainloop.h"
+
 static int verbose = 1;
 
 #define ATT_CID 4
 
+static bt_uuid_t fota_uuid,cp_uuid,data_uuid,cccd_uuid;
+
+
+
+
+static void
+att_debug_cb (const char *str, void *user_data)
+{
+  const char *prefix = user_data;
+
+  fprintf (stderr, "%s:%s\n", prefix, str);
+}
+
+static void
+gatt_debug_cb (const char *str, void *user_data)
+{
+  const char *prefix = user_data;
+
+  fprintf (stderr, "%s:%s\n", prefix, str);
+}
+
+static void
+log_service_event (struct gatt_db_attribute *attr, const char *str)
+{
+  char uuid_str[MAX_LEN_UUID_STR];
+  bt_uuid_t uuid;
+  uint16_t start, end;
+
+  gatt_db_attribute_get_service_uuid (attr, &uuid);
+  bt_uuid_to_string (&uuid, uuid_str, sizeof (uuid_str));
+
+  gatt_db_attribute_get_service_handles (attr, &start, &end);
+
+  fprintf (stderr, "%s - UUID: %s start: 0x%04x end: 0x%04x\n", str, uuid_str,
+           start, end);
+}
+
+static void
+service_added_cb (struct gatt_db_attribute *attr, void *user_data)
+{
+  log_service_event (attr, "Service Added");
+}
+
+static void
+service_removed_cb (struct gatt_db_attribute *attr, void *user_data)
+{
+  log_service_event (attr, "Service Removed");
+}
 
 
 static int
@@ -81,23 +132,167 @@ l2cap_le_att_connect (bdaddr_t * src, bdaddr_t * dst, uint8_t dst_type,
 
 
 
-void ble_finish(struct ble *ble)
+void
+ble_close (BLE *ble)
+{
+  if (!ble)
+    return;
+
+
+  if (ble->notify_id)
+	bt_gatt_client_unregister_notify(ble->gatt, ble->notify_id);
+
+  if (ble->gatt)
+        bt_gatt_client_unref(ble->gatt);
+
+  if (ble->db) 
+	gatt_db_unref(ble->db);
+
+  if (ble->att)
+    bt_att_unref (ble->att);
+  if (ble->fd > 0)
+    close (ble->fd);
+
+
+  free (ble);
+}
+
+
+static void
+att_disconnect_cb (int err, void *user_data)
+{
+  printf ("Device disconnected: %s\n", strerror (err));
+
+  mainloop_exit_failure ();
+}
+
+static void print_uuid(const bt_uuid_t *uuid)
+{
+        char uuid_str[MAX_LEN_UUID_STR];
+        bt_uuid_t uuid128;
+
+        bt_uuid_to_uuid128(uuid, &uuid128);
+        bt_uuid_to_string(&uuid128, uuid_str, sizeof(uuid_str));
+
+        printf("%s\n", uuid_str);
+}
+
+
+
+static void scan_desc(struct gatt_db_attribute *attr, void *user_data)
+{
+BLE *ble=user_data;
+uint16_t handle=gatt_db_attribute_get_handle(attr);
+const bt_uuid_t *uuid=gatt_db_attribute_get_type(attr);
+
+        printf("\t\tdesc - handle: 0x%04x, uuid: ",handle);
+        print_uuid(uuid);
+
+	if (!bt_uuid_cmp(uuid,&cccd_uuid))
+		ble->cccd_handle=handle;
+
+}
+
+
+
+static void scan_chrc(struct gatt_db_attribute *attr, void *user_data)
 {
-if (!ble) return;
+	BLE *ble=user_data;
+        uint16_t handle, value_handle;
+        uint8_t properties;
+        bt_uuid_t uuid;
+
+        if (!gatt_db_attribute_get_char_data(attr, &handle,
+                                                                &value_handle,
+                                                                &properties,
+                                                                &uuid))
+                return;
+
+        printf("\tchar - start: 0x%04x, value: 0x%04x, props: 0x%02x, uuid: ",
+                                        handle, value_handle, properties);
 
-if (ble->att) bt_att_unref(ble->att);
-if (ble->fd>0) close(ble->fd);
 
-free(ble);
+        print_uuid(&uuid);
+
+	if (!bt_uuid_cmp(&uuid,&data_uuid)) 
+		ble->data_handle=value_handle;
+
+	if (!bt_uuid_cmp(&uuid,&cp_uuid))  {
+		ble->cp_handle=value_handle;
+		gatt_db_service_foreach_desc(attr, scan_desc, ble);
+        }
+
 }
 
 
-struct ble *ble_start (const char *bdaddr)
+
+
+static void scan_service(struct gatt_db_attribute *attr, void *user_data)
 {
-  struct ble *ble;
+        BLE *ble = user_data;
+        uint16_t start, end;
+        bool primary;
+        bt_uuid_t uuid;
 
-  ble=xmalloc(sizeof(*ble));
-  memset(ble,0,sizeof(*ble));
+        if (!gatt_db_attribute_get_service_data(attr, &start, &end, &primary,
+                                                                        &uuid))
+                return;
+
+        gatt_db_service_foreach_char(attr, scan_chrc, ble);
+
+        printf("\n");
+}
+
+
+static void
+ready_cb (bool success, uint8_t att_ecode, void *user_data)
+{
+  BLE *ble = user_data;
+
+  if (!success)
+    {
+      fprintf (stderr,
+               "GATT discovery procedures failed - error code: 0x%02x\n",
+               att_ecode);
+      mainloop_exit_failure ();
+      return;
+    }
+
+  printf ("GATT discovery procedures complete\n");
+
+
+  gatt_db_foreach_service(ble->db, &fota_uuid, scan_service, ble);
+
+
+  printf("Handles:\n\tdata: 0x%04x\n\tcp  : 0x%04x\n\tcccd: 0x%04x\n",
+	ble->data_handle,ble->cp_handle,ble->cccd_handle);	
+
+  if (ble->cccd_handle && ble->cp_handle && ble->data_handle) {
+  	mainloop_exit_success ();
+	return;
+  }
+ 
+  mainloop_exit_failure();
+}
+
+
+void ble_init(void)
+{
+  bt_string_to_uuid(&fota_uuid, "00001530-1212-efde-1523-785feabcd123");
+  bt_string_to_uuid(&cp_uuid, "00001531-1212-efde-1523-785feabcd123");
+  bt_string_to_uuid(&data_uuid, "00001532-1212-efde-1523-785feabcd123");
+  bt_string_to_uuid(&cccd_uuid, "00002902-0000-1000-8000-00805f9b34fb");
+
+  mainloop_init ();
+}
+
+BLE *
+ble_open (const char *bdaddr)
+{
+  BLE *ble;
+
+  ble = xmalloc (sizeof (*ble));
+  memset (ble, 0, sizeof (*ble));
 
   ble->sec = BT_SECURITY_LOW;
   ble->dst_type = BDADDR_LE_RANDOM;
@@ -106,42 +301,122 @@ struct ble *ble_start (const char *bdaddr)
   if (str2ba (bdaddr, &ble->dst_addr) < 0)
     {
       fprintf (stderr, "Invalid remote address: %s\n", bdaddr);
-      ble_finish(ble);
+      ble_close (ble);
       return NULL;
     }
 
 
-  ble->fd = l2cap_le_att_connect (&ble->src_addr, &ble->dst_addr, ble->dst_type, ble->sec);
+  ble->fd =
+    l2cap_le_att_connect (&ble->src_addr, &ble->dst_addr, ble->dst_type,
+                          ble->sec);
   if (ble->fd < 0)
     {
       perror ("l2cap_le_att_connect");
-      ble_finish(ble);
+      ble_close (ble);
       return NULL;
     }
 
 
-    ble->att = bt_att_new(ble->fd);
+  ble->att = bt_att_new (ble->fd);
+
+  if (!ble->att)
+    {
+      fprintf (stderr, "Failed to initialze ATT transport layer\n");
+      ble_close (ble);
+      return NULL;
+    }
+
+  if (!bt_att_set_close_on_unref (ble->att, true))
+    {
+      fprintf (stderr, "Failed to set up ATT transport layer\n");
+      ble_close (ble);
+      return NULL;
+    }
 
-        if (!ble->att) {
-                fprintf(stderr, "Failed to initialze ATT transport layer\n");
-		ble_finish(ble);
-                return NULL;
+  if (!bt_att_register_disconnect (ble->att, att_disconnect_cb, NULL, NULL))
+    {
+      fprintf (stderr, "Failed to set ATT disconnect handler\n");
+      ble_close (ble);
+      return NULL;
+    }
+
+
+  ble->db = gatt_db_new ();
+  ble->gatt = bt_gatt_client_new (ble->db, ble->att, ble->mtu);
+
+
+  gatt_db_register (ble->db, service_added_cb, service_removed_cb,
+                    NULL, NULL);
+
+  if (verbose)
+    {
+      bt_att_set_debug (ble->att, att_debug_cb, "att: ", NULL);
+      bt_gatt_client_set_debug (ble->gatt, gatt_debug_cb, "gatt: ", NULL);
+    }
+
+  bt_gatt_client_set_ready_handler (ble->gatt, ready_cb, ble, NULL);
+
+
+  if (mainloop_run () == EXIT_SUCCESS)
+    return ble;
+
+  ble_close (ble);
+  return NULL;
+}
+
+static void notify_cb(uint16_t value_handle, const uint8_t *value,
+                                        uint16_t length, void *user_data)
+{
+        int i;
+
+        printf("Handle Value Not/Ind: 0x%04x - ", value_handle);
+
+        if (length == 0) {
+                printf("(0 bytes)\n");
+                return;
         }
 
-        if (!bt_att_set_close_on_unref(ble->att, true)) {
-                fprintf(stderr, "Failed to set up ATT transport layer\n");
-		ble_finish(ble);
-                return NULL;
+        printf("(%u bytes): ", length);
+
+        for (i = 0; i < length; i++)
+                printf("%02x ", value[i]);
+
+        printf("\n");
+}
+
+static void register_notify_cb(uint16_t att_ecode, void *user_data)
+{
+        if (att_ecode) {
+                printf("Failed to register notify handler "
+                                        "- error code: 0x%02x\n", att_ecode);
+		mainloop_exit_failure();
+                return;
         }
 
-        if (!bt_att_register_disconnect(ble->att, att_disconnect_cb, NULL,
-                                                                NULL)) {
-                fprintf(stderr, "Failed to set ATT disconnect handler\n");
-		ble_finish(ble);
-                return NULL;
+        printf("Registered notify handler!\n");
+	mainloop_exit_success();
+}
+
+
+int ble_register_notify(BLE *ble) {
+
+
+
+        if (!bt_gatt_client_is_ready(ble->gatt)) {
+                printf("GATT client not initialized\n");
+                return EXIT_FAILURE;
         }
 
+        ble->notify_id = bt_gatt_client_register_notify(ble->gatt, ble->cp_handle,
+                                                        register_notify_cb,
+                                                        notify_cb, NULL, NULL);
+        if (!ble->notify_id) {
+                printf("Failed to register notify handler\n");
+                return EXIT_FAILURE;
+        }
 
+        printf("requesting notify\n");
 
-  printf ("Connected!\n");
+	return mainloop_run();
 }
+
diff --git a/ble.h b/ble.h
index 634191e..5073cd6 100644
--- a/ble.h
+++ b/ble.h
@@ -1,13 +1,22 @@
-struct ble {
-    bdaddr_t src_addr, dst_addr;
-    int sec;
-    uint8_t dst_type;
+struct ble
+{
+  bdaddr_t src_addr, dst_addr;
+  int sec;
+  uint8_t dst_type;
 
-    int fd;
+  uint16_t mtu;
 
-    struct bt_att *att;
-    struct gatt_db *db;
-    struct bt_gatt_client *gatt;
-};
+  int fd;
+
+  struct bt_att *att;
+  struct gatt_db *db;
+  struct bt_gatt_client *gatt;
 
+  unsigned notify_id;
+
+  uint16_t cp_handle;
+  uint16_t cccd_handle;
+  uint16_t data_handle;
+};
 
+typedef struct ble BLE;
diff --git a/bluez/att-types.h b/bluez/att-types.h
new file mode 100644
index 0000000..aa7f0da
--- /dev/null
+++ b/bluez/att-types.h
@@ -0,0 +1,118 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2014  Google Inc.
+ *
+ *
+ *  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
+ *
+ */
+
+#include <stdint.h>
+
+#ifndef __packed
+#define __packed __attribute__((packed))
+#endif
+
+#define BT_ATT_DEFAULT_LE_MTU	23
+#define BT_ATT_MAX_LE_MTU	517
+#define BT_ATT_MAX_VALUE_LEN	512
+
+/* ATT protocol opcodes */
+#define BT_ATT_OP_ERROR_RSP			0x01
+#define BT_ATT_OP_MTU_REQ			0x02
+#define BT_ATT_OP_MTU_RSP			0x03
+#define BT_ATT_OP_FIND_INFO_REQ			0x04
+#define BT_ATT_OP_FIND_INFO_RSP			0x05
+#define BT_ATT_OP_FIND_BY_TYPE_VAL_REQ		0x06
+#define BT_ATT_OP_FIND_BY_TYPE_VAL_RSP		0x07
+#define BT_ATT_OP_READ_BY_TYPE_REQ		0x08
+#define BT_ATT_OP_READ_BY_TYPE_RSP		0x09
+#define BT_ATT_OP_READ_REQ			0x0a
+#define BT_ATT_OP_READ_RSP			0x0b
+#define BT_ATT_OP_READ_BLOB_REQ			0x0c
+#define BT_ATT_OP_READ_BLOB_RSP			0x0d
+#define BT_ATT_OP_READ_MULT_REQ			0x0e
+#define BT_ATT_OP_READ_MULT_RSP			0x0f
+#define BT_ATT_OP_READ_BY_GRP_TYPE_REQ		0x10
+#define BT_ATT_OP_READ_BY_GRP_TYPE_RSP		0x11
+#define BT_ATT_OP_WRITE_REQ			0x12
+#define BT_ATT_OP_WRITE_RSP			0x13
+#define BT_ATT_OP_WRITE_CMD			0x52
+#define BT_ATT_OP_SIGNED_WRITE_CMD		0xD2
+#define BT_ATT_OP_PREP_WRITE_REQ		0x16
+#define BT_ATT_OP_PREP_WRITE_RSP		0x17
+#define BT_ATT_OP_EXEC_WRITE_REQ		0x18
+#define BT_ATT_OP_EXEC_WRITE_RSP		0x19
+#define BT_ATT_OP_HANDLE_VAL_NOT		0x1B
+#define BT_ATT_OP_HANDLE_VAL_IND		0x1D
+#define BT_ATT_OP_HANDLE_VAL_CONF		0x1E
+
+/* Packed struct definitions for ATT protocol PDUs */
+/* TODO: Complete these definitions for all opcodes */
+struct bt_att_pdu_error_rsp {
+	uint8_t opcode;
+	uint16_t handle;
+	uint8_t ecode;
+} __packed;
+
+/* Special opcode to receive all requests (legacy servers) */
+#define BT_ATT_ALL_REQUESTS 0x00
+
+/* Error codes for Error response PDU */
+#define BT_ATT_ERROR_INVALID_HANDLE			0x01
+#define BT_ATT_ERROR_READ_NOT_PERMITTED			0x02
+#define BT_ATT_ERROR_WRITE_NOT_PERMITTED		0x03
+#define BT_ATT_ERROR_INVALID_PDU			0x04
+#define BT_ATT_ERROR_AUTHENTICATION			0x05
+#define BT_ATT_ERROR_REQUEST_NOT_SUPPORTED		0x06
+#define BT_ATT_ERROR_INVALID_OFFSET			0x07
+#define BT_ATT_ERROR_AUTHORIZATION			0x08
+#define BT_ATT_ERROR_PREPARE_QUEUE_FULL			0x09
+#define BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND		0x0A
+#define BT_ATT_ERROR_ATTRIBUTE_NOT_LONG			0x0B
+#define BT_ATT_ERROR_INSUFFICIENT_ENCRYPTION_KEY_SIZE	0x0C
+#define BT_ATT_ERROR_INVALID_ATTRIBUTE_VALUE_LEN	0x0D
+#define BT_ATT_ERROR_UNLIKELY				0x0E
+#define BT_ATT_ERROR_INSUFFICIENT_ENCRYPTION		0x0F
+#define BT_ATT_ERROR_UNSUPPORTED_GROUP_TYPE		0x10
+#define BT_ATT_ERROR_INSUFFICIENT_RESOURCES		0x11
+
+/*
+ * ATT attribute permission bitfield values. Permissions are grouped as
+ * "Access", "Encryption", "Authentication", and "Authorization". A bitmask of
+ * permissions is a byte that encodes a combination of these.
+ */
+#define BT_ATT_PERM_READ	0x01
+#define BT_ATT_PERM_WRITE	0x02
+#define BT_ATT_PERM_ENCRYPT	0x04
+#define BT_ATT_PERM_AUTHEN	0x08
+#define BT_ATT_PERM_AUTHOR	0x10
+#define BT_ATT_PERM_NONE	0x20
+
+/* GATT Characteristic Properties Bitfield values */
+#define BT_GATT_CHRC_PROP_BROADCAST			0x01
+#define BT_GATT_CHRC_PROP_READ				0x02
+#define BT_GATT_CHRC_PROP_WRITE_WITHOUT_RESP		0x04
+#define BT_GATT_CHRC_PROP_WRITE				0x08
+#define BT_GATT_CHRC_PROP_NOTIFY			0x10
+#define BT_GATT_CHRC_PROP_INDICATE			0x20
+#define BT_GATT_CHRC_PROP_AUTH				0x40
+#define BT_GATT_CHRC_PROP_EXT_PROP			0x80
+
+/* GATT Characteristic Extended Properties Bitfield values */
+#define BT_GATT_CHRC_EXT_PROP_RELIABLE_WRITE	0x01
+#define BT_GATT_CHRC_EXT_PROP_WRITABLE_AUX	0x02
diff --git a/bluez/att.c b/bluez/att.c
new file mode 100644
index 0000000..fd6ef05
--- /dev/null
+++ b/bluez/att.c
@@ -0,0 +1,1434 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2014  Google Inc.
+ *
+ *
+ *  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
+ *
+ */
+
+#include <stdint.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <errno.h>
+
+#include <bluetooth/bluetooth.h>
+
+#include "io.h"
+#include "queue.h"
+#include "util.h"
+#include "timeout.h"
+#include "uuid.h"
+#include "att.h"
+#include "crypto.h"
+
+#define ATT_MIN_PDU_LEN			1  /* At least 1 byte for the opcode. */
+#define ATT_OP_CMD_MASK			0x40
+#define ATT_OP_SIGNED_MASK		0x80
+#define ATT_TIMEOUT_INTERVAL		30000  /* 30000 ms */
+
+/*
+ * Common Profile and Service Error Code descriptions (see Supplement to the
+ * Bluetooth Core Specification, sections 1.2 and 2). The error codes within
+ * 0xE0-0xFC are reserved for future use. The remaining 3 are defined as the
+ * following:
+ */
+#define BT_ERROR_CCC_IMPROPERLY_CONFIGURED	0xfd
+#define BT_ERROR_ALREADY_IN_PROGRESS		0xfe
+#define BT_ERROR_OUT_OF_RANGE			0xff
+
+/* Length of signature in write signed packet */
+#define BT_ATT_SIGNATURE_LEN		12
+
+struct att_send_op;
+
+struct bt_att {
+	int ref_count;
+	int fd;
+	struct io *io;
+	bool io_on_l2cap;
+	int io_sec_level;		/* Only used for non-L2CAP */
+
+	struct queue *req_queue;	/* Queued ATT protocol requests */
+	struct att_send_op *pending_req;
+	struct queue *ind_queue;	/* Queued ATT protocol indications */
+	struct att_send_op *pending_ind;
+	struct queue *write_queue;	/* Queue of PDUs ready to send */
+	bool writer_active;
+
+	struct queue *notify_list;	/* List of registered callbacks */
+	struct queue *disconn_list;	/* List of disconnect handlers */
+
+	bool in_req;			/* There's a pending incoming request */
+
+	uint8_t *buf;
+	uint16_t mtu;
+
+	unsigned int next_send_id;	/* IDs for "send" ops */
+	unsigned int next_reg_id;	/* IDs for registered callbacks */
+
+	bt_att_timeout_func_t timeout_callback;
+	bt_att_destroy_func_t timeout_destroy;
+	void *timeout_data;
+
+	bt_att_debug_func_t debug_callback;
+	bt_att_destroy_func_t debug_destroy;
+	void *debug_data;
+
+	struct bt_crypto *crypto;
+
+	struct sign_info *local_sign;
+	struct sign_info *remote_sign;
+};
+
+struct sign_info {
+	uint8_t key[16];
+	bt_att_counter_func_t counter;
+	void *user_data;
+};
+
+enum att_op_type {
+	ATT_OP_TYPE_REQ,
+	ATT_OP_TYPE_RSP,
+	ATT_OP_TYPE_CMD,
+	ATT_OP_TYPE_IND,
+	ATT_OP_TYPE_NOT,
+	ATT_OP_TYPE_CONF,
+	ATT_OP_TYPE_UNKNOWN,
+};
+
+static const struct {
+	uint8_t opcode;
+	enum att_op_type type;
+} att_opcode_type_table[] = {
+	{ BT_ATT_OP_ERROR_RSP,			ATT_OP_TYPE_RSP },
+	{ BT_ATT_OP_MTU_REQ,			ATT_OP_TYPE_REQ },
+	{ BT_ATT_OP_MTU_RSP,			ATT_OP_TYPE_RSP },
+	{ BT_ATT_OP_FIND_INFO_REQ,		ATT_OP_TYPE_REQ },
+	{ BT_ATT_OP_FIND_INFO_RSP,		ATT_OP_TYPE_RSP },
+	{ BT_ATT_OP_FIND_BY_TYPE_VAL_REQ,	ATT_OP_TYPE_REQ },
+	{ BT_ATT_OP_FIND_BY_TYPE_VAL_RSP,	ATT_OP_TYPE_RSP },
+	{ BT_ATT_OP_READ_BY_TYPE_REQ,		ATT_OP_TYPE_REQ },
+	{ BT_ATT_OP_READ_BY_TYPE_RSP,		ATT_OP_TYPE_RSP },
+	{ BT_ATT_OP_READ_REQ,			ATT_OP_TYPE_REQ },
+	{ BT_ATT_OP_READ_RSP,			ATT_OP_TYPE_RSP },
+	{ BT_ATT_OP_READ_BLOB_REQ,		ATT_OP_TYPE_REQ },
+	{ BT_ATT_OP_READ_BLOB_RSP,		ATT_OP_TYPE_RSP },
+	{ BT_ATT_OP_READ_MULT_REQ,		ATT_OP_TYPE_REQ },
+	{ BT_ATT_OP_READ_MULT_RSP,		ATT_OP_TYPE_RSP },
+	{ BT_ATT_OP_READ_BY_GRP_TYPE_REQ,	ATT_OP_TYPE_REQ },
+	{ BT_ATT_OP_READ_BY_GRP_TYPE_RSP,	ATT_OP_TYPE_RSP },
+	{ BT_ATT_OP_WRITE_REQ,			ATT_OP_TYPE_REQ },
+	{ BT_ATT_OP_WRITE_RSP,			ATT_OP_TYPE_RSP },
+	{ BT_ATT_OP_WRITE_CMD,			ATT_OP_TYPE_CMD },
+	{ BT_ATT_OP_SIGNED_WRITE_CMD,		ATT_OP_TYPE_CMD },
+	{ BT_ATT_OP_PREP_WRITE_REQ,		ATT_OP_TYPE_REQ },
+	{ BT_ATT_OP_PREP_WRITE_RSP,		ATT_OP_TYPE_RSP },
+	{ BT_ATT_OP_EXEC_WRITE_REQ,		ATT_OP_TYPE_REQ },
+	{ BT_ATT_OP_EXEC_WRITE_RSP,		ATT_OP_TYPE_RSP },
+	{ BT_ATT_OP_HANDLE_VAL_NOT,		ATT_OP_TYPE_NOT },
+	{ BT_ATT_OP_HANDLE_VAL_IND,		ATT_OP_TYPE_IND },
+	{ BT_ATT_OP_HANDLE_VAL_CONF,		ATT_OP_TYPE_CONF },
+	{ }
+};
+
+static enum att_op_type get_op_type(uint8_t opcode)
+{
+	int i;
+
+	for (i = 0; att_opcode_type_table[i].opcode; i++) {
+		if (att_opcode_type_table[i].opcode == opcode)
+			return att_opcode_type_table[i].type;
+	}
+
+	return ATT_OP_TYPE_UNKNOWN;
+}
+
+static const struct {
+	uint8_t req_opcode;
+	uint8_t rsp_opcode;
+} att_req_rsp_mapping_table[] = {
+	{ BT_ATT_OP_MTU_REQ,			BT_ATT_OP_MTU_RSP },
+	{ BT_ATT_OP_FIND_INFO_REQ,		BT_ATT_OP_FIND_INFO_RSP},
+	{ BT_ATT_OP_FIND_BY_TYPE_VAL_REQ,	BT_ATT_OP_FIND_BY_TYPE_VAL_RSP },
+	{ BT_ATT_OP_READ_BY_TYPE_REQ,		BT_ATT_OP_READ_BY_TYPE_RSP },
+	{ BT_ATT_OP_READ_REQ,			BT_ATT_OP_READ_RSP },
+	{ BT_ATT_OP_READ_BLOB_REQ,		BT_ATT_OP_READ_BLOB_RSP },
+	{ BT_ATT_OP_READ_MULT_REQ,		BT_ATT_OP_READ_MULT_RSP },
+	{ BT_ATT_OP_READ_BY_GRP_TYPE_REQ,	BT_ATT_OP_READ_BY_GRP_TYPE_RSP },
+	{ BT_ATT_OP_WRITE_REQ,			BT_ATT_OP_WRITE_RSP },
+	{ BT_ATT_OP_PREP_WRITE_REQ,		BT_ATT_OP_PREP_WRITE_RSP },
+	{ BT_ATT_OP_EXEC_WRITE_REQ,		BT_ATT_OP_EXEC_WRITE_RSP },
+	{ }
+};
+
+static uint8_t get_req_opcode(uint8_t rsp_opcode)
+{
+	int i;
+
+	for (i = 0; att_req_rsp_mapping_table[i].rsp_opcode; i++) {
+		if (att_req_rsp_mapping_table[i].rsp_opcode == rsp_opcode)
+			return att_req_rsp_mapping_table[i].req_opcode;
+	}
+
+	return 0;
+}
+
+struct att_send_op {
+	unsigned int id;
+	unsigned int timeout_id;
+	enum att_op_type type;
+	uint16_t opcode;
+	void *pdu;
+	uint16_t len;
+	bt_att_response_func_t callback;
+	bt_att_destroy_func_t destroy;
+	void *user_data;
+};
+
+static void destroy_att_send_op(void *data)
+{
+	struct att_send_op *op = data;
+
+	if (op->timeout_id)
+		timeout_remove(op->timeout_id);
+
+	if (op->destroy)
+		op->destroy(op->user_data);
+
+	free(op->pdu);
+	free(op);
+}
+
+static void cancel_att_send_op(struct att_send_op *op)
+{
+	if (op->destroy)
+		op->destroy(op->user_data);
+
+	op->user_data = NULL;
+	op->callback = NULL;
+	op->destroy = NULL;
+}
+
+struct att_notify {
+	unsigned int id;
+	uint16_t opcode;
+	bt_att_notify_func_t callback;
+	bt_att_destroy_func_t destroy;
+	void *user_data;
+};
+
+static void destroy_att_notify(void *data)
+{
+	struct att_notify *notify = data;
+
+	if (notify->destroy)
+		notify->destroy(notify->user_data);
+
+	free(notify);
+}
+
+static bool match_notify_id(const void *a, const void *b)
+{
+	const struct att_notify *notify = a;
+	unsigned int id = PTR_TO_UINT(b);
+
+	return notify->id == id;
+}
+
+struct att_disconn {
+	unsigned int id;
+	bool removed;
+	bt_att_disconnect_func_t callback;
+	bt_att_destroy_func_t destroy;
+	void *user_data;
+};
+
+static void destroy_att_disconn(void *data)
+{
+	struct att_disconn *disconn = data;
+
+	if (disconn->destroy)
+		disconn->destroy(disconn->user_data);
+
+	free(disconn);
+}
+
+static bool match_disconn_id(const void *a, const void *b)
+{
+	const struct att_disconn *disconn = a;
+	unsigned int id = PTR_TO_UINT(b);
+
+	return disconn->id == id;
+}
+
+static bool encode_pdu(struct bt_att *att, struct att_send_op *op,
+					const void *pdu, uint16_t length)
+{
+	uint16_t pdu_len = 1;
+	struct sign_info *sign = att->local_sign;
+	uint32_t sign_cnt;
+
+	if (sign && (op->opcode & ATT_OP_SIGNED_MASK))
+		pdu_len += BT_ATT_SIGNATURE_LEN;
+
+	if (length && pdu)
+		pdu_len += length;
+
+	if (pdu_len > att->mtu)
+		return false;
+
+	op->len = pdu_len;
+	op->pdu = malloc(op->len);
+	if (!op->pdu)
+		return false;
+
+	((uint8_t *) op->pdu)[0] = op->opcode;
+	if (pdu_len > 1)
+		memcpy(op->pdu + 1, pdu, length);
+
+	if (!sign || !(op->opcode & ATT_OP_SIGNED_MASK))
+		return true;
+
+	if (!sign->counter(&sign_cnt, sign->user_data))
+		goto fail;
+
+	if ((bt_crypto_sign_att(att->crypto, sign->key, op->pdu, 1 + length,
+				sign_cnt, &((uint8_t *) op->pdu)[1 + length])))
+		return true;
+
+	util_debug(att->debug_callback, att->debug_data,
+					"ATT unable to generate signature");
+
+fail:
+	free(op->pdu);
+	return false;
+}
+
+static struct att_send_op *create_att_send_op(struct bt_att *att,
+						uint8_t opcode,
+						const void *pdu,
+						uint16_t length,
+						bt_att_response_func_t callback,
+						void *user_data,
+						bt_att_destroy_func_t destroy)
+{
+	struct att_send_op *op;
+	enum att_op_type op_type;
+
+	if (length && !pdu)
+		return NULL;
+
+	op_type = get_op_type(opcode);
+	if (op_type == ATT_OP_TYPE_UNKNOWN)
+		return NULL;
+
+	/* If the opcode corresponds to an operation type that does not elicit a
+	 * response from the remote end, then no callback should have been
+	 * provided, since it will never be called.
+	 */
+	if (callback && op_type != ATT_OP_TYPE_REQ && op_type != ATT_OP_TYPE_IND)
+		return NULL;
+
+	/* Similarly, if the operation does elicit a response then a callback
+	 * must be provided.
+	 */
+	if (!callback && (op_type == ATT_OP_TYPE_REQ || op_type == ATT_OP_TYPE_IND))
+		return NULL;
+
+	op = new0(struct att_send_op, 1);
+	if (!op)
+		return NULL;
+
+	op->type = op_type;
+	op->opcode = opcode;
+	op->callback = callback;
+	op->destroy = destroy;
+	op->user_data = user_data;
+
+	if (!encode_pdu(att, op, pdu, length)) {
+		free(op);
+		return NULL;
+	}
+
+	return op;
+}
+
+static struct att_send_op *pick_next_send_op(struct bt_att *att)
+{
+	struct att_send_op *op;
+
+	/* See if any operations are already in the write queue */
+	op = queue_pop_head(att->write_queue);
+	if (op)
+		return op;
+
+	/* If there is no pending request, pick an operation from the
+	 * request queue.
+	 */
+	if (!att->pending_req) {
+		op = queue_pop_head(att->req_queue);
+		if (op)
+			return op;
+	}
+
+	/* There is either a request pending or no requests queued. If there is
+	 * no pending indication, pick an operation from the indication queue.
+	 */
+	if (!att->pending_ind) {
+		op = queue_pop_head(att->ind_queue);
+		if (op)
+			return op;
+	}
+
+	return NULL;
+}
+
+struct timeout_data {
+	struct bt_att *att;
+	unsigned int id;
+};
+
+static bool timeout_cb(void *user_data)
+{
+	struct timeout_data *timeout = user_data;
+	struct bt_att *att = timeout->att;
+	struct att_send_op *op = NULL;
+
+	if (att->pending_req && att->pending_req->id == timeout->id) {
+		op = att->pending_req;
+		att->pending_req = NULL;
+	} else if (att->pending_ind && att->pending_ind->id == timeout->id) {
+		op = att->pending_ind;
+		att->pending_ind = NULL;
+	}
+
+	if (!op)
+		return false;
+
+	util_debug(att->debug_callback, att->debug_data,
+				"Operation timed out: 0x%02x", op->opcode);
+
+	if (att->timeout_callback)
+		att->timeout_callback(op->id, op->opcode, att->timeout_data);
+
+	op->timeout_id = 0;
+	destroy_att_send_op(op);
+
+	/*
+	 * Directly terminate the connection as required by the ATT protocol.
+	 * This should trigger an io disconnect event which will clean up the
+	 * io and notify the upper layer.
+	 */
+	io_shutdown(att->io);
+
+	return false;
+}
+
+static void write_watch_destroy(void *user_data)
+{
+	struct bt_att *att = user_data;
+
+	att->writer_active = false;
+}
+
+static bool can_write_data(struct io *io, void *user_data)
+{
+	struct bt_att *att = user_data;
+	struct att_send_op *op;
+	struct timeout_data *timeout;
+	ssize_t ret;
+	struct iovec iov;
+
+	op = pick_next_send_op(att);
+	if (!op)
+		return false;
+
+	iov.iov_base = op->pdu;
+	iov.iov_len = op->len;
+
+	ret = io_send(io, &iov, 1);
+	if (ret < 0) {
+		util_debug(att->debug_callback, att->debug_data,
+					"write failed: %s", strerror(-ret));
+		if (op->callback)
+			op->callback(BT_ATT_OP_ERROR_RSP, NULL, 0,
+							op->user_data);
+
+		destroy_att_send_op(op);
+		return true;
+	}
+
+	util_debug(att->debug_callback, att->debug_data,
+					"ATT op 0x%02x", op->opcode);
+
+	util_hexdump('<', op->pdu, ret, att->debug_callback, att->debug_data);
+
+	/* Based on the operation type, set either the pending request or the
+	 * pending indication. If it came from the write queue, then there is
+	 * no need to keep it around.
+	 */
+	switch (op->type) {
+	case ATT_OP_TYPE_REQ:
+		att->pending_req = op;
+		break;
+	case ATT_OP_TYPE_IND:
+		att->pending_ind = op;
+		break;
+	case ATT_OP_TYPE_RSP:
+		/* Set in_req to false to indicate that no request is pending */
+		att->in_req = false;
+
+		/* Fall through to the next case */
+	case ATT_OP_TYPE_CMD:
+	case ATT_OP_TYPE_NOT:
+	case ATT_OP_TYPE_CONF:
+	case ATT_OP_TYPE_UNKNOWN:
+	default:
+		destroy_att_send_op(op);
+		return true;
+	}
+
+	timeout = new0(struct timeout_data, 1);
+	if (!timeout)
+		return true;
+
+	timeout->att = att;
+	timeout->id = op->id;
+	op->timeout_id = timeout_add(ATT_TIMEOUT_INTERVAL, timeout_cb,
+								timeout, free);
+
+	/* Return true as there may be more operations ready to write. */
+	return true;
+}
+
+static void wakeup_writer(struct bt_att *att)
+{
+	if (att->writer_active)
+		return;
+
+	/* Set the write handler only if there is anything that can be sent
+	 * at all.
+	 */
+	if (queue_isempty(att->write_queue)) {
+		if ((att->pending_req || queue_isempty(att->req_queue)) &&
+			(att->pending_ind || queue_isempty(att->ind_queue)))
+			return;
+	}
+
+	if (!io_set_write_handler(att->io, can_write_data, att,
+							write_watch_destroy))
+		return;
+
+	att->writer_active = true;
+}
+
+static void disconn_handler(void *data, void *user_data)
+{
+	struct att_disconn *disconn = data;
+	int err = PTR_TO_INT(user_data);
+
+	if (disconn->removed)
+		return;
+
+	if (disconn->callback)
+		disconn->callback(err, disconn->user_data);
+}
+
+static bool disconnect_cb(struct io *io, void *user_data)
+{
+	struct bt_att *att = user_data;
+	int err;
+	socklen_t len;
+
+	len = sizeof(err);
+
+	if (getsockopt(att->fd, SOL_SOCKET, SO_ERROR, &err, &len) < 0) {
+		util_debug(att->debug_callback, att->debug_data,
+					"Failed to obtain disconnect error: %s",
+					strerror(errno));
+		err = 0;
+	}
+
+	util_debug(att->debug_callback, att->debug_data,
+					"Physical link disconnected: %s",
+					strerror(err));
+
+	io_destroy(att->io);
+	att->io = NULL;
+
+	bt_att_cancel_all(att);
+
+	bt_att_ref(att);
+
+	queue_foreach(att->disconn_list, disconn_handler, INT_TO_PTR(err));
+
+	bt_att_unregister_all(att);
+	bt_att_unref(att);
+
+	return false;
+}
+
+static void handle_rsp(struct bt_att *att, uint8_t opcode, uint8_t *pdu,
+								ssize_t pdu_len)
+{
+	struct att_send_op *op = att->pending_req;
+	uint8_t req_opcode;
+	uint8_t rsp_opcode;
+	uint8_t *rsp_pdu = NULL;
+	uint16_t rsp_pdu_len = 0;
+
+	/*
+	 * If no request is pending, then the response is unexpected. Disconnect
+	 * the bearer.
+	 */
+	if (!op) {
+		util_debug(att->debug_callback, att->debug_data,
+					"Received unexpected ATT response");
+		io_shutdown(att->io);
+		return;
+	}
+
+	/*
+	 * If the received response doesn't match the pending request, or if
+	 * the request is malformed, end the current request with failure.
+	 */
+	if (opcode == BT_ATT_OP_ERROR_RSP) {
+		if (pdu_len != 4)
+			goto fail;
+
+		req_opcode = pdu[0];
+	} else if (!(req_opcode = get_req_opcode(opcode)))
+		goto fail;
+
+	if (req_opcode != op->opcode)
+		goto fail;
+
+	rsp_opcode = opcode;
+
+	if (pdu_len > 0) {
+		rsp_pdu = pdu;
+		rsp_pdu_len = pdu_len;
+	}
+
+	goto done;
+
+fail:
+	util_debug(att->debug_callback, att->debug_data,
+			"Failed to handle response PDU; opcode: 0x%02x", opcode);
+
+	rsp_opcode = BT_ATT_OP_ERROR_RSP;
+
+done:
+	if (op->callback)
+		op->callback(rsp_opcode, rsp_pdu, rsp_pdu_len, op->user_data);
+
+	destroy_att_send_op(op);
+	att->pending_req = NULL;
+
+	wakeup_writer(att);
+}
+
+static void handle_conf(struct bt_att *att, uint8_t *pdu, ssize_t pdu_len)
+{
+	struct att_send_op *op = att->pending_ind;
+
+	/*
+	 * Disconnect the bearer if the confirmation is unexpected or the PDU is
+	 * invalid.
+	 */
+	if (!op || pdu_len) {
+		util_debug(att->debug_callback, att->debug_data,
+				"Received unexpected/invalid ATT confirmation");
+		io_shutdown(att->io);
+		return;
+	}
+
+	if (op->callback)
+		op->callback(BT_ATT_OP_HANDLE_VAL_CONF, NULL, 0, op->user_data);
+
+	destroy_att_send_op(op);
+	att->pending_ind = NULL;
+
+	wakeup_writer(att);
+}
+
+struct notify_data {
+	uint8_t opcode;
+	uint8_t *pdu;
+	ssize_t pdu_len;
+	bool handler_found;
+};
+
+static bool opcode_match(uint8_t opcode, uint8_t test_opcode)
+{
+	enum att_op_type op_type = get_op_type(test_opcode);
+
+	if (opcode == BT_ATT_ALL_REQUESTS && (op_type == ATT_OP_TYPE_REQ ||
+						op_type == ATT_OP_TYPE_CMD))
+		return true;
+
+	return opcode == test_opcode;
+}
+
+static void respond_not_supported(struct bt_att *att, uint8_t opcode)
+{
+	uint8_t pdu[4];
+
+	pdu[0] = opcode;
+	pdu[1] = 0;
+	pdu[2] = 0;
+	pdu[3] = BT_ATT_ERROR_REQUEST_NOT_SUPPORTED;
+
+	bt_att_send(att, BT_ATT_OP_ERROR_RSP, pdu, sizeof(pdu), NULL, NULL,
+									NULL);
+}
+
+static bool handle_signed(struct bt_att *att, uint8_t opcode, uint8_t *pdu,
+								ssize_t pdu_len)
+{
+	uint8_t *signature;
+	uint32_t sign_cnt;
+	struct sign_info *sign;
+
+	/* Check if there is enough data for a signature */
+	if (pdu_len < 2 + BT_ATT_SIGNATURE_LEN)
+		goto fail;
+
+	sign = att->remote_sign;
+	if (!sign)
+		goto fail;
+
+	signature = pdu + (pdu_len - BT_ATT_SIGNATURE_LEN);
+	sign_cnt = get_le32(signature);
+
+	/* Validate counter */
+	if (!sign->counter(&sign_cnt, sign->user_data))
+		goto fail;
+
+	/* Generate signature and verify it */
+	if (!bt_crypto_sign_att(att->crypto, sign->key, pdu,
+				pdu_len - BT_ATT_SIGNATURE_LEN, sign_cnt,
+				signature))
+		goto fail;
+
+	return true;
+
+fail:
+	util_debug(att->debug_callback, att->debug_data,
+			"ATT failed to verify signature: 0x%02x", opcode);
+
+	return false;
+}
+
+static void handle_notify(struct bt_att *att, uint8_t opcode, uint8_t *pdu,
+								ssize_t pdu_len)
+{
+	const struct queue_entry *entry;
+	bool found;
+
+	if (opcode & ATT_OP_SIGNED_MASK) {
+		if (!handle_signed(att, opcode, pdu, pdu_len))
+			return;
+		pdu_len -= BT_ATT_SIGNATURE_LEN;
+	}
+
+	bt_att_ref(att);
+
+	found = false;
+	entry = queue_get_entries(att->notify_list);
+
+	while (entry) {
+		struct att_notify *notify = entry->data;
+
+		entry = entry->next;
+
+		if (!opcode_match(notify->opcode, opcode))
+			continue;
+
+		found = true;
+
+		if (notify->callback)
+			notify->callback(opcode, pdu, pdu_len,
+							notify->user_data);
+
+		/* callback could remove all entries from notify list */
+		if (queue_isempty(att->notify_list))
+			break;
+	}
+
+	/*
+	 * If this was a request and no handler was registered for it, respond
+	 * with "Not Supported"
+	 */
+	if (!found && get_op_type(opcode) == ATT_OP_TYPE_REQ)
+		respond_not_supported(att, opcode);
+
+	bt_att_unref(att);
+}
+
+static bool can_read_data(struct io *io, void *user_data)
+{
+	struct bt_att *att = user_data;
+	uint8_t opcode;
+	uint8_t *pdu;
+	ssize_t bytes_read;
+
+	bytes_read = read(att->fd, att->buf, att->mtu);
+	if (bytes_read < 0)
+		return false;
+
+	util_hexdump('>', att->buf, bytes_read,
+					att->debug_callback, att->debug_data);
+
+	if (bytes_read < ATT_MIN_PDU_LEN)
+		return true;
+
+	pdu = att->buf;
+	opcode = pdu[0];
+
+	bt_att_ref(att);
+
+	/* Act on the received PDU based on the opcode type */
+	switch (get_op_type(opcode)) {
+	case ATT_OP_TYPE_RSP:
+		util_debug(att->debug_callback, att->debug_data,
+				"ATT response received: 0x%02x", opcode);
+		handle_rsp(att, opcode, pdu + 1, bytes_read - 1);
+		break;
+	case ATT_OP_TYPE_CONF:
+		util_debug(att->debug_callback, att->debug_data,
+				"ATT confirmation received: 0x%02x", opcode);
+		handle_conf(att, pdu + 1, bytes_read - 1);
+		break;
+	case ATT_OP_TYPE_REQ:
+		/*
+		 * If a request is currently pending, then the sequential
+		 * protocol was violated. Disconnect the bearer, which will
+		 * promptly notify the upper layer via disconnect handlers.
+		 */
+		if (att->in_req) {
+			util_debug(att->debug_callback, att->debug_data,
+					"Received request while another is "
+					"pending: 0x%02x", opcode);
+			io_shutdown(att->io);
+			bt_att_unref(att);
+
+			return false;
+		}
+
+		att->in_req = true;
+
+		/* Fall through to the next case */
+	case ATT_OP_TYPE_CMD:
+	case ATT_OP_TYPE_NOT:
+	case ATT_OP_TYPE_UNKNOWN:
+	case ATT_OP_TYPE_IND:
+	default:
+		/* For all other opcodes notify the upper layer of the PDU and
+		 * let them act on it.
+		 */
+		util_debug(att->debug_callback, att->debug_data,
+					"ATT PDU received: 0x%02x", opcode);
+		handle_notify(att, opcode, pdu + 1, bytes_read - 1);
+		break;
+	}
+
+	bt_att_unref(att);
+
+	return true;
+}
+
+static bool is_io_l2cap_based(int fd)
+{
+	int domain;
+	int proto;
+	int err;
+	socklen_t len;
+
+	domain = 0;
+	len = sizeof(domain);
+	err = getsockopt(fd, SOL_SOCKET, SO_DOMAIN, &domain, &len);
+	if (err < 0)
+		return false;
+
+	if (domain != AF_BLUETOOTH)
+		return false;
+
+	proto = 0;
+	len = sizeof(proto);
+	err = getsockopt(fd, SOL_SOCKET, SO_PROTOCOL, &proto, &len);
+	if (err < 0)
+		return false;
+
+	return proto == BTPROTO_L2CAP;
+}
+
+static void bt_att_free(struct bt_att *att)
+{
+	if (att->pending_req)
+		destroy_att_send_op(att->pending_req);
+
+	if (att->pending_ind)
+		destroy_att_send_op(att->pending_ind);
+
+	io_destroy(att->io);
+	bt_crypto_unref(att->crypto);
+
+	queue_destroy(att->req_queue, NULL);
+	queue_destroy(att->ind_queue, NULL);
+	queue_destroy(att->write_queue, NULL);
+	queue_destroy(att->notify_list, NULL);
+	queue_destroy(att->disconn_list, NULL);
+
+	if (att->timeout_destroy)
+		att->timeout_destroy(att->timeout_data);
+
+	if (att->debug_destroy)
+		att->debug_destroy(att->debug_data);
+
+	free(att->local_sign);
+	free(att->remote_sign);
+
+	free(att->buf);
+
+	free(att);
+}
+
+struct bt_att *bt_att_new(int fd)
+{
+	struct bt_att *att;
+
+	if (fd < 0)
+		return NULL;
+
+	att = new0(struct bt_att, 1);
+	if (!att)
+		return NULL;
+
+	att->fd = fd;
+
+	att->mtu = BT_ATT_DEFAULT_LE_MTU;
+	att->buf = malloc(att->mtu);
+	if (!att->buf)
+		goto fail;
+
+	att->io = io_new(fd);
+	if (!att->io)
+		goto fail;
+
+	/* crypto is optional, if not available leave it NULL */
+	att->crypto = bt_crypto_new();
+
+	att->req_queue = queue_new();
+	if (!att->req_queue)
+		goto fail;
+
+	att->ind_queue = queue_new();
+	if (!att->ind_queue)
+		goto fail;
+
+	att->write_queue = queue_new();
+	if (!att->write_queue)
+		goto fail;
+
+	att->notify_list = queue_new();
+	if (!att->notify_list)
+		goto fail;
+
+	att->disconn_list = queue_new();
+	if (!att->disconn_list)
+		goto fail;
+
+	if (!io_set_read_handler(att->io, can_read_data, att, NULL))
+		goto fail;
+
+	if (!io_set_disconnect_handler(att->io, disconnect_cb, att, NULL))
+		goto fail;
+
+	att->io_on_l2cap = is_io_l2cap_based(att->fd);
+	if (!att->io_on_l2cap)
+		att->io_sec_level = BT_SECURITY_LOW;
+
+	return bt_att_ref(att);
+
+fail:
+	bt_att_free(att);
+
+	return NULL;
+}
+
+struct bt_att *bt_att_ref(struct bt_att *att)
+{
+	if (!att)
+		return NULL;
+
+	__sync_fetch_and_add(&att->ref_count, 1);
+
+	return att;
+}
+
+void bt_att_unref(struct bt_att *att)
+{
+	if (!att)
+		return;
+
+	if (__sync_sub_and_fetch(&att->ref_count, 1))
+		return;
+
+	bt_att_unregister_all(att);
+	bt_att_cancel_all(att);
+
+	bt_att_free(att);
+}
+
+bool bt_att_set_close_on_unref(struct bt_att *att, bool do_close)
+{
+	if (!att || !att->io)
+		return false;
+
+	return io_set_close_on_destroy(att->io, do_close);
+}
+
+int bt_att_get_fd(struct bt_att *att)
+{
+	if (!att)
+		return -1;
+
+	return att->fd;
+}
+
+bool bt_att_set_debug(struct bt_att *att, bt_att_debug_func_t callback,
+				void *user_data, bt_att_destroy_func_t destroy)
+{
+	if (!att)
+		return false;
+
+	if (att->debug_destroy)
+		att->debug_destroy(att->debug_data);
+
+	att->debug_callback = callback;
+	att->debug_destroy = destroy;
+	att->debug_data = user_data;
+
+	return true;
+}
+
+uint16_t bt_att_get_mtu(struct bt_att *att)
+{
+	if (!att)
+		return 0;
+
+	return att->mtu;
+}
+
+bool bt_att_set_mtu(struct bt_att *att, uint16_t mtu)
+{
+	void *buf;
+
+	if (!att)
+		return false;
+
+	if (mtu < BT_ATT_DEFAULT_LE_MTU)
+		return false;
+
+	buf = malloc(mtu);
+	if (!buf)
+		return false;
+
+	free(att->buf);
+
+	att->mtu = mtu;
+	att->buf = buf;
+
+	return true;
+}
+
+bool bt_att_set_timeout_cb(struct bt_att *att, bt_att_timeout_func_t callback,
+						void *user_data,
+						bt_att_destroy_func_t destroy)
+{
+	if (!att)
+		return false;
+
+	if (att->timeout_destroy)
+		att->timeout_destroy(att->timeout_data);
+
+	att->timeout_callback = callback;
+	att->timeout_destroy = destroy;
+	att->timeout_data = user_data;
+
+	return true;
+}
+
+unsigned int bt_att_register_disconnect(struct bt_att *att,
+					bt_att_disconnect_func_t callback,
+					void *user_data,
+					bt_att_destroy_func_t destroy)
+{
+	struct att_disconn *disconn;
+
+	if (!att || !att->io)
+		return 0;
+
+	disconn = new0(struct att_disconn, 1);
+	if (!disconn)
+		return 0;
+
+	disconn->callback = callback;
+	disconn->destroy = destroy;
+	disconn->user_data = user_data;
+
+	if (att->next_reg_id < 1)
+		att->next_reg_id = 1;
+
+	disconn->id = att->next_reg_id++;
+
+	if (!queue_push_tail(att->disconn_list, disconn)) {
+		free(disconn);
+		return 0;
+	}
+
+	return disconn->id;
+}
+
+bool bt_att_unregister_disconnect(struct bt_att *att, unsigned int id)
+{
+	struct att_disconn *disconn;
+
+	if (!att || !id)
+		return false;
+
+	disconn = queue_remove_if(att->disconn_list, match_disconn_id,
+							UINT_TO_PTR(id));
+	if (!disconn)
+		return false;
+
+	destroy_att_disconn(disconn);
+	return true;
+}
+
+unsigned int bt_att_send(struct bt_att *att, uint8_t opcode,
+				const void *pdu, uint16_t length,
+				bt_att_response_func_t callback, void *user_data,
+				bt_att_destroy_func_t destroy)
+{
+	struct att_send_op *op;
+	bool result;
+
+	if (!att || !att->io)
+		return 0;
+
+	op = create_att_send_op(att, opcode, pdu, length, callback, user_data,
+								destroy);
+	if (!op)
+		return 0;
+
+	if (att->next_send_id < 1)
+		att->next_send_id = 1;
+
+	op->id = att->next_send_id++;
+
+	/* Add the op to the correct queue based on its type */
+	switch (op->type) {
+	case ATT_OP_TYPE_REQ:
+		result = queue_push_tail(att->req_queue, op);
+		break;
+	case ATT_OP_TYPE_IND:
+		result = queue_push_tail(att->ind_queue, op);
+		break;
+	case ATT_OP_TYPE_CMD:
+	case ATT_OP_TYPE_NOT:
+	case ATT_OP_TYPE_UNKNOWN:
+	case ATT_OP_TYPE_RSP:
+	case ATT_OP_TYPE_CONF:
+	default:
+		result = queue_push_tail(att->write_queue, op);
+		break;
+	}
+
+	if (!result) {
+		free(op->pdu);
+		free(op);
+		return 0;
+	}
+
+	wakeup_writer(att);
+
+	return op->id;
+}
+
+static bool match_op_id(const void *a, const void *b)
+{
+	const struct att_send_op *op = a;
+	unsigned int id = PTR_TO_UINT(b);
+
+	return op->id == id;
+}
+
+bool bt_att_cancel(struct bt_att *att, unsigned int id)
+{
+	struct att_send_op *op;
+
+	if (!att || !id)
+		return false;
+
+	if (att->pending_req && att->pending_req->id == id) {
+		/* Don't cancel the pending request; remove it's handlers */
+		cancel_att_send_op(att->pending_req);
+		return true;
+	}
+
+	if (att->pending_ind && att->pending_ind->id == id) {
+		/* Don't cancel the pending indication; remove it's handlers */
+		cancel_att_send_op(att->pending_ind);
+		return true;
+	}
+
+	op = queue_remove_if(att->req_queue, match_op_id, UINT_TO_PTR(id));
+	if (op)
+		goto done;
+
+	op = queue_remove_if(att->ind_queue, match_op_id, UINT_TO_PTR(id));
+	if (op)
+		goto done;
+
+	op = queue_remove_if(att->write_queue, match_op_id, UINT_TO_PTR(id));
+	if (op)
+		goto done;
+
+	if (!op)
+		return false;
+
+done:
+	destroy_att_send_op(op);
+
+	wakeup_writer(att);
+
+	return true;
+}
+
+bool bt_att_cancel_all(struct bt_att *att)
+{
+	if (!att)
+		return false;
+
+	queue_remove_all(att->req_queue, NULL, NULL, destroy_att_send_op);
+	queue_remove_all(att->ind_queue, NULL, NULL, destroy_att_send_op);
+	queue_remove_all(att->write_queue, NULL, NULL, destroy_att_send_op);
+
+	if (att->pending_req)
+		/* Don't cancel the pending request; remove it's handlers */
+		cancel_att_send_op(att->pending_req);
+
+	if (att->pending_ind)
+		/* Don't cancel the pending request; remove it's handlers */
+		cancel_att_send_op(att->pending_ind);
+
+	return true;
+}
+
+static uint8_t att_ecode_from_error(int err)
+{
+	/*
+	 * If the error fits in a single byte, treat it as an ATT protocol
+	 * error as is. Since "0" is not a valid ATT protocol error code, we map
+	 * that to UNLIKELY below.
+	 */
+	if (err > 0 && err < UINT8_MAX)
+		return err;
+
+	/*
+	 * Since we allow UNIX errnos, map them to appropriate ATT protocol
+	 * and "Common Profile and Service" error codes.
+	 */
+	switch (err) {
+	case -ENOENT:
+		return BT_ATT_ERROR_INVALID_HANDLE;
+	case -ENOMEM:
+		return BT_ATT_ERROR_INSUFFICIENT_RESOURCES;
+	case -EALREADY:
+		return BT_ERROR_ALREADY_IN_PROGRESS;
+	case -EOVERFLOW:
+		return BT_ERROR_OUT_OF_RANGE;
+	}
+
+	return BT_ATT_ERROR_UNLIKELY;
+}
+
+unsigned int bt_att_send_error_rsp(struct bt_att *att, uint8_t opcode,
+						uint16_t handle, int error)
+{
+	struct bt_att_pdu_error_rsp pdu;
+	uint8_t ecode;
+
+	if (!att || !opcode)
+		return 0;
+
+	ecode = att_ecode_from_error(error);
+
+	memset(&pdu, 0, sizeof(pdu));
+
+	pdu.opcode = opcode;
+	put_le16(handle, &pdu.handle);
+	pdu.ecode = ecode;
+
+	return bt_att_send(att, BT_ATT_OP_ERROR_RSP, &pdu, sizeof(pdu),
+							NULL, NULL, NULL);
+}
+
+unsigned int bt_att_register(struct bt_att *att, uint8_t opcode,
+						bt_att_notify_func_t callback,
+						void *user_data,
+						bt_att_destroy_func_t destroy)
+{
+	struct att_notify *notify;
+
+	if (!att || !callback || !att->io)
+		return 0;
+
+	notify = new0(struct att_notify, 1);
+	if (!notify)
+		return 0;
+
+	notify->opcode = opcode;
+	notify->callback = callback;
+	notify->destroy = destroy;
+	notify->user_data = user_data;
+
+	if (att->next_reg_id < 1)
+		att->next_reg_id = 1;
+
+	notify->id = att->next_reg_id++;
+
+	if (!queue_push_tail(att->notify_list, notify)) {
+		free(notify);
+		return 0;
+	}
+
+	return notify->id;
+}
+
+bool bt_att_unregister(struct bt_att *att, unsigned int id)
+{
+	struct att_notify *notify;
+
+	if (!att || !id)
+		return false;
+
+	notify = queue_remove_if(att->notify_list, match_notify_id,
+							UINT_TO_PTR(id));
+	if (!notify)
+		return false;
+
+	destroy_att_notify(notify);
+	return true;
+}
+
+bool bt_att_unregister_all(struct bt_att *att)
+{
+	if (!att)
+		return false;
+
+	queue_remove_all(att->notify_list, NULL, NULL, destroy_att_notify);
+	queue_remove_all(att->disconn_list, NULL, NULL, destroy_att_disconn);
+
+	return true;
+}
+
+int bt_att_get_sec_level(struct bt_att *att)
+{
+	struct bt_security sec;
+	socklen_t len;
+
+	if (!att)
+		return -EINVAL;
+
+	if (!att->io_on_l2cap)
+		return att->io_sec_level;
+
+	memset(&sec, 0, sizeof(sec));
+	len = sizeof(sec);
+	if (getsockopt(att->fd, SOL_BLUETOOTH, BT_SECURITY, &sec, &len) < 0)
+		return -EIO;
+
+	return sec.level;
+}
+
+bool bt_att_set_sec_level(struct bt_att *att, int level)
+{
+	struct bt_security sec;
+
+	if (!att || level < BT_SECURITY_LOW || level > BT_SECURITY_HIGH)
+		return false;
+
+	if (!att->io_on_l2cap) {
+		att->io_sec_level = level;
+		return true;
+	}
+
+	memset(&sec, 0, sizeof(sec));
+	sec.level = level;
+
+	if (setsockopt(att->fd, SOL_BLUETOOTH, BT_SECURITY, &sec,
+							sizeof(sec)) < 0)
+		return false;
+
+	return true;
+}
+
+static bool sign_set_key(struct sign_info **sign, uint8_t key[16],
+				bt_att_counter_func_t func, void *user_data)
+{
+	if (!(*sign)) {
+		*sign = new0(struct sign_info, 1);
+		if (!(*sign))
+			return false;
+	}
+
+	(*sign)->counter = func;
+	(*sign)->user_data = user_data;
+	memcpy((*sign)->key, key, 16);
+
+	return true;
+}
+
+bool bt_att_set_local_key(struct bt_att *att, uint8_t sign_key[16],
+				bt_att_counter_func_t func, void *user_data)
+{
+	if (!att)
+		return false;
+
+	return sign_set_key(&att->local_sign, sign_key, func, user_data);
+}
+
+bool bt_att_set_remote_key(struct bt_att *att, uint8_t sign_key[16],
+				bt_att_counter_func_t func, void *user_data)
+{
+	if (!att)
+		return false;
+
+	return sign_set_key(&att->remote_sign, sign_key, func, user_data);
+}
+
+bool bt_att_has_crypto(struct bt_att *att)
+{
+	if (!att)
+		return false;
+
+	return att->crypto ? true : false;
+}
diff --git a/bluez/att.h b/bluez/att.h
new file mode 100644
index 0000000..aa75c25
--- /dev/null
+++ b/bluez/att.h
@@ -0,0 +1,93 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2014  Google Inc.
+ *
+ *
+ *  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
+ *
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "att-types.h"
+
+struct bt_att;
+
+struct bt_att *bt_att_new(int fd);
+
+struct bt_att *bt_att_ref(struct bt_att *att);
+void bt_att_unref(struct bt_att *att);
+
+bool bt_att_set_close_on_unref(struct bt_att *att, bool do_close);
+
+int bt_att_get_fd(struct bt_att *att);
+
+typedef void (*bt_att_response_func_t)(uint8_t opcode, const void *pdu,
+					uint16_t length, void *user_data);
+typedef void (*bt_att_notify_func_t)(uint8_t opcode, const void *pdu,
+					uint16_t length, void *user_data);
+typedef void (*bt_att_destroy_func_t)(void *user_data);
+typedef void (*bt_att_debug_func_t)(const char *str, void *user_data);
+typedef void (*bt_att_timeout_func_t)(unsigned int id, uint8_t opcode,
+							void *user_data);
+typedef void (*bt_att_disconnect_func_t)(int err, void *user_data);
+typedef bool (*bt_att_counter_func_t)(uint32_t *sign_cnt, void *user_data);
+
+bool bt_att_set_debug(struct bt_att *att, bt_att_debug_func_t callback,
+				void *user_data, bt_att_destroy_func_t destroy);
+
+uint16_t bt_att_get_mtu(struct bt_att *att);
+bool bt_att_set_mtu(struct bt_att *att, uint16_t mtu);
+
+bool bt_att_set_timeout_cb(struct bt_att *att, bt_att_timeout_func_t callback,
+						void *user_data,
+						bt_att_destroy_func_t destroy);
+
+unsigned int bt_att_send(struct bt_att *att, uint8_t opcode,
+					const void *pdu, uint16_t length,
+					bt_att_response_func_t callback,
+					void *user_data,
+					bt_att_destroy_func_t destroy);
+bool bt_att_cancel(struct bt_att *att, unsigned int id);
+bool bt_att_cancel_all(struct bt_att *att);
+
+unsigned int bt_att_send_error_rsp(struct bt_att *att, uint8_t opcode,
+						uint16_t handle, int error);
+
+unsigned int bt_att_register(struct bt_att *att, uint8_t opcode,
+						bt_att_notify_func_t callback,
+						void *user_data,
+						bt_att_destroy_func_t destroy);
+bool bt_att_unregister(struct bt_att *att, unsigned int id);
+
+unsigned int bt_att_register_disconnect(struct bt_att *att,
+					bt_att_disconnect_func_t callback,
+					void *user_data,
+					bt_att_destroy_func_t destroy);
+bool bt_att_unregister_disconnect(struct bt_att *att, unsigned int id);
+
+bool bt_att_unregister_all(struct bt_att *att);
+
+int bt_att_get_sec_level(struct bt_att *att);
+bool bt_att_set_sec_level(struct bt_att *att, int level);
+
+bool bt_att_set_local_key(struct bt_att *att, uint8_t sign_key[16],
+			bt_att_counter_func_t func, void *user_data);
+bool bt_att_set_remote_key(struct bt_att *att, uint8_t sign_key[16],
+			bt_att_counter_func_t func, void *user_data);
+bool bt_att_has_crypto(struct bt_att *att);
diff --git a/bluez/crypto.c b/bluez/crypto.c
new file mode 100644
index 0000000..1f5a963
--- /dev/null
+++ b/bluez/crypto.c
@@ -0,0 +1,681 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2012-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
+ *
+ */
+
+#include <bluetooth/bluetooth.h>
+
+#include <fcntl.h>
+#include <unistd.h>
+#include <string.h>
+#include <sys/socket.h>
+
+#include "util.h"
+#include "crypto.h"
+
+#ifndef HAVE_LINUX_IF_ALG_H
+#ifndef HAVE_LINUX_TYPES_H
+typedef uint8_t __u8;
+typedef uint16_t __u16;
+typedef uint32_t __u32;
+#else
+#include <linux/types.h>
+#endif
+
+struct sockaddr_alg {
+	__u16   salg_family;
+	__u8    salg_type[14];
+	__u32   salg_feat;
+	__u32   salg_mask;
+	__u8    salg_name[64];
+};
+
+struct af_alg_iv {
+	__u32   ivlen;
+	__u8    iv[0];
+};
+
+#define ALG_SET_KEY                     1
+#define ALG_SET_IV                      2
+#define ALG_SET_OP                      3
+
+#define ALG_OP_DECRYPT                  0
+#define ALG_OP_ENCRYPT                  1
+
+#define PF_ALG		38	/* Algorithm sockets.  */
+#define AF_ALG		PF_ALG
+#else
+#include <linux/if_alg.h>
+#endif
+
+#ifndef SOL_ALG
+#define SOL_ALG		279
+#endif
+
+/* Maximum message length that can be passed to aes_cmac */
+#define CMAC_MSG_MAX	80
+
+struct bt_crypto {
+	int ref_count;
+	int ecb_aes;
+	int urandom;
+	int cmac_aes;
+};
+
+static int urandom_setup(void)
+{
+	int fd;
+
+	fd = open("/dev/urandom", O_RDONLY);
+	if (fd < 0)
+		return -1;
+
+	return fd;
+}
+
+static int ecb_aes_setup(void)
+{
+	struct sockaddr_alg salg;
+	int fd;
+
+	fd = socket(PF_ALG, SOCK_SEQPACKET | SOCK_CLOEXEC, 0);
+	if (fd < 0)
+		return -1;
+
+	memset(&salg, 0, sizeof(salg));
+	salg.salg_family = AF_ALG;
+	strcpy((char *) salg.salg_type, "skcipher");
+	strcpy((char *) salg.salg_name, "ecb(aes)");
+
+	if (bind(fd, (struct sockaddr *) &salg, sizeof(salg)) < 0) {
+		close(fd);
+		return -1;
+	}
+
+	return fd;
+}
+
+static int cmac_aes_setup(void)
+{
+	struct sockaddr_alg salg;
+	int fd;
+
+	fd = socket(PF_ALG, SOCK_SEQPACKET | SOCK_CLOEXEC, 0);
+	if (fd < 0)
+		return -1;
+
+	memset(&salg, 0, sizeof(salg));
+	salg.salg_family = AF_ALG;
+	strcpy((char *) salg.salg_type, "hash");
+	strcpy((char *) salg.salg_name, "cmac(aes)");
+
+	if (bind(fd, (struct sockaddr *) &salg, sizeof(salg)) < 0) {
+		close(fd);
+		return -1;
+	}
+
+	return fd;
+}
+
+struct bt_crypto *bt_crypto_new(void)
+{
+	struct bt_crypto *crypto;
+
+	crypto = new0(struct bt_crypto, 1);
+	if (!crypto)
+		return NULL;
+
+	crypto->ecb_aes = ecb_aes_setup();
+	if (crypto->ecb_aes < 0) {
+		free(crypto);
+		return NULL;
+	}
+
+	crypto->urandom = urandom_setup();
+	if (crypto->urandom < 0) {
+		close(crypto->ecb_aes);
+		free(crypto);
+		return NULL;
+	}
+
+	crypto->cmac_aes = cmac_aes_setup();
+	if (crypto->cmac_aes < 0) {
+		close(crypto->urandom);
+		close(crypto->ecb_aes);
+		free(crypto);
+		return NULL;
+	}
+
+	return bt_crypto_ref(crypto);
+}
+
+struct bt_crypto *bt_crypto_ref(struct bt_crypto *crypto)
+{
+	if (!crypto)
+		return NULL;
+
+	__sync_fetch_and_add(&crypto->ref_count, 1);
+
+	return crypto;
+}
+
+void bt_crypto_unref(struct bt_crypto *crypto)
+{
+	if (!crypto)
+		return;
+
+	if (__sync_sub_and_fetch(&crypto->ref_count, 1))
+		return;
+
+	close(crypto->urandom);
+	close(crypto->ecb_aes);
+	close(crypto->cmac_aes);
+
+	free(crypto);
+}
+
+bool bt_crypto_random_bytes(struct bt_crypto *crypto,
+					uint8_t *buf, uint8_t num_bytes)
+{
+	ssize_t len;
+
+	if (!crypto)
+		return false;
+
+	len = read(crypto->urandom, buf, num_bytes);
+	if (len < num_bytes)
+		return false;
+
+	return true;
+}
+
+static int alg_new(int fd, const void *keyval, socklen_t keylen)
+{
+	if (setsockopt(fd, SOL_ALG, ALG_SET_KEY, keyval, keylen) < 0)
+		return -1;
+
+	/* FIXME: This should use accept4() with SOCK_CLOEXEC */
+	return accept(fd, NULL, 0);
+}
+
+static bool alg_encrypt(int fd, const void *inbuf, size_t inlen,
+						void *outbuf, size_t outlen)
+{
+	__u32 alg_op = ALG_OP_ENCRYPT;
+	char cbuf[CMSG_SPACE(sizeof(alg_op))];
+	struct cmsghdr *cmsg;
+	struct msghdr msg;
+	struct iovec iov;
+	ssize_t len;
+
+	memset(cbuf, 0, sizeof(cbuf));
+	memset(&msg, 0, sizeof(msg));
+
+	msg.msg_control = cbuf;
+	msg.msg_controllen = sizeof(cbuf);
+
+	cmsg = CMSG_FIRSTHDR(&msg);
+	cmsg->cmsg_level = SOL_ALG;
+	cmsg->cmsg_type = ALG_SET_OP;
+	cmsg->cmsg_len = CMSG_LEN(sizeof(alg_op));
+	memcpy(CMSG_DATA(cmsg), &alg_op, sizeof(alg_op));
+
+	iov.iov_base = (void *) inbuf;
+	iov.iov_len = inlen;
+
+	msg.msg_iov = &iov;
+	msg.msg_iovlen = 1;
+
+	len = sendmsg(fd, &msg, 0);
+	if (len < 0)
+		return false;
+
+	len = read(fd, outbuf, outlen);
+	if (len < 0)
+		return false;
+
+	return true;
+}
+
+static inline void swap_buf(const uint8_t *src, uint8_t *dst, uint16_t len)
+{
+	int i;
+
+	for (i = 0; i < len; i++)
+		dst[len - 1 - i] = src[i];
+}
+
+bool bt_crypto_sign_att(struct bt_crypto *crypto, const uint8_t key[16],
+				const uint8_t *m, uint16_t m_len,
+				uint32_t sign_cnt, uint8_t signature[12])
+{
+	int fd;
+	int len;
+	uint8_t tmp[16], out[16];
+	uint16_t msg_len = m_len + sizeof(uint32_t);
+	uint8_t msg[msg_len];
+	uint8_t msg_s[msg_len];
+
+	if (!crypto)
+		return false;
+
+	memset(msg, 0, msg_len);
+	memcpy(msg, m, m_len);
+
+	/* Add sign_counter to the message */
+	put_le32(sign_cnt, msg + m_len);
+
+	/* The most significant octet of key corresponds to key[0] */
+	swap_buf(key, tmp, 16);
+
+	fd = alg_new(crypto->cmac_aes, tmp, 16);
+	if (fd < 0)
+		return false;
+
+	/* Swap msg before signing */
+	swap_buf(msg, msg_s, msg_len);
+
+	len = send(fd, msg_s, msg_len, 0);
+	if (len < 0) {
+		close(fd);
+		return false;
+	}
+
+	len = read(fd, out, 16);
+	if (len < 0) {
+		close(fd);
+		return false;
+	}
+
+	close(fd);
+
+	/*
+	 * As to BT spec. 4.1 Vol[3], Part C, chapter 10.4.1 sign counter should
+	 * be placed in the signature
+	 */
+	put_be32(sign_cnt, out + 8);
+
+	/*
+	 * The most significant octet of hash corresponds to out[0]  - swap it.
+	 * Then truncate in most significant bit first order to a length of
+	 * 12 octets
+	 */
+	swap_buf(out, tmp, 16);
+	memcpy(signature, tmp + 4, 12);
+
+	return true;
+}
+/*
+ * Security function e
+ *
+ * Security function e generates 128-bit encryptedData from a 128-bit key
+ * and 128-bit plaintextData using the AES-128-bit block cypher:
+ *
+ *   encryptedData = e(key, plaintextData)
+ *
+ * The most significant octet of key corresponds to key[0], the most
+ * significant octet of plaintextData corresponds to in[0] and the
+ * most significant octet of encryptedData corresponds to out[0].
+ *
+ */
+bool bt_crypto_e(struct bt_crypto *crypto, const uint8_t key[16],
+			const uint8_t plaintext[16], uint8_t encrypted[16])
+{
+	uint8_t tmp[16], in[16], out[16];
+	int fd;
+
+	if (!crypto)
+		return false;
+
+	/* The most significant octet of key corresponds to key[0] */
+	swap_buf(key, tmp, 16);
+
+	fd = alg_new(crypto->ecb_aes, tmp, 16);
+	if (fd < 0)
+		return false;
+
+
+	/* Most significant octet of plaintextData corresponds to in[0] */
+	swap_buf(plaintext, in, 16);
+
+	if (!alg_encrypt(fd, in, 16, out, 16)) {
+		close(fd);
+		return false;
+	}
+
+	/* Most significant octet of encryptedData corresponds to out[0] */
+	swap_buf(out, encrypted, 16);
+
+	close(fd);
+
+	return true;
+}
+
+/*
+ * Random Address Hash function ah
+ *
+ * The random address hash function ah is used to generate a hash value
+ * that is used in resolvable private addresses.
+ *
+ * The following are inputs to the random address hash function ah:
+ *
+ *   k is 128 bits
+ *   r is 24 bits
+ *   padding is 104 bits
+ *
+ * r is concatenated with padding to generate r' which is used as the
+ * 128-bit input parameter plaintextData to security function e:
+ *
+ *   r' = padding || r
+ *
+ * The least significant octet of r becomes the least significant octet
+ * of r’ and the most significant octet of padding becomes the most
+ * significant octet of r'.
+ *
+ * For example, if the 24-bit value r is 0x423456 then r' is
+ * 0x00000000000000000000000000423456.
+ *
+ * The output of the random address function ah is:
+ *
+ *   ah(k, r) = e(k, r') mod 2^24
+ *
+ * The output of the security function e is then truncated to 24 bits by
+ * taking the least significant 24 bits of the output of e as the result
+ * of ah.
+ */
+bool bt_crypto_ah(struct bt_crypto *crypto, const uint8_t k[16],
+					const uint8_t r[3], uint8_t hash[3])
+{
+	uint8_t rp[16];
+	uint8_t encrypted[16];
+
+	if (!crypto)
+		return false;
+
+	/* r' = padding || r */
+	memcpy(rp, r, 3);
+	memset(rp + 3, 0, 13);
+
+	/* e(k, r') */
+	if (!bt_crypto_e(crypto, k, rp, encrypted))
+		return false;
+
+	/* ah(k, r) = e(k, r') mod 2^24 */
+	memcpy(hash, encrypted, 3);
+
+	return true;
+}
+
+typedef struct {
+	uint64_t a, b;
+} u128;
+
+static inline void u128_xor(const uint8_t p[16], const uint8_t q[16],
+								uint8_t r[16])
+{
+	u128 pp, qq, rr;
+
+	memcpy(&pp, p, 16);
+	memcpy(&qq, q, 16);
+
+	rr.a = pp.a ^ qq.a;
+	rr.b = pp.b ^ qq.b;
+
+	memcpy(r, &rr, 16);
+}
+
+/*
+ * Confirm value generation function c1
+ *
+ * During the pairing process confirm values are exchanged. This confirm
+ * value generation function c1 is used to generate the confirm values.
+ *
+ * The following are inputs to the confirm value generation function c1:
+ *
+ *   k is 128 bits
+ *   r is 128 bits
+ *   pres is 56 bits
+ *   preq is 56 bits
+ *   iat is 1 bit
+ *   ia is 48 bits
+ *   rat is 1 bit
+ *   ra is 48 bits
+ *   padding is 32 bits of 0
+ *
+ * iat is concatenated with 7-bits of 0 to create iat' which is 8 bits
+ * in length. iat is the least significant bit of iat'
+ *
+ * rat is concatenated with 7-bits of 0 to create rat' which is 8 bits
+ * in length. rat is the least significant bit of rat'
+ *
+ * pres, preq, rat' and iat' are concatenated to generate p1 which is
+ * XORed with r and used as 128-bit input parameter plaintextData to
+ * security function e:
+ *
+ *   p1 = pres || preq || rat' || iat'
+ *
+ * The octet of iat' becomes the least significant octet of p1 and the
+ * most significant octet of pres becomes the most significant octet of
+ * p1.
+ *
+ * ra is concatenated with ia and padding to generate p2 which is XORed
+ * with the result of the security function e using p1 as the input
+ * paremter plaintextData and is then used as the 128-bit input
+ * parameter plaintextData to security function e:
+ *
+ *   p2 = padding || ia || ra
+ *
+ * The least significant octet of ra becomes the least significant octet
+ * of p2 and the most significant octet of padding becomes the most
+ * significant octet of p2.
+ *
+ * The output of the confirm value generation function c1 is:
+ *
+ *   c1(k, r, preq, pres, iat, rat, ia, ra) = e(k, e(k, r XOR p1) XOR p2)
+ *
+ * The 128-bit output of the security function e is used as the result
+ * of confirm value generation function c1.
+ */
+bool bt_crypto_c1(struct bt_crypto *crypto, const uint8_t k[16],
+			const uint8_t r[16], const uint8_t pres[7],
+			const uint8_t preq[7], uint8_t iat,
+			const uint8_t ia[6], uint8_t rat,
+			const uint8_t ra[6], uint8_t res[16])
+{
+	uint8_t p1[16], p2[16];
+
+	/* p1 = pres || preq || _rat || _iat */
+	p1[0] = iat;
+	p1[1] = rat;
+	memcpy(p1 + 2, preq, 7);
+	memcpy(p1 + 9, pres, 7);
+
+	/* p2 = padding || ia || ra */
+	memcpy(p2, ra, 6);
+	memcpy(p2 + 6, ia, 6);
+	memset(p2 + 12, 0, 4);
+
+	/* res = r XOR p1 */
+	u128_xor(r, p1, res);
+
+	/* res = e(k, res) */
+	if (!bt_crypto_e(crypto, k, res, res))
+		return false;
+
+	/* res = res XOR p2 */
+	u128_xor(res, p2, res);
+
+	/* res = e(k, res) */
+	return bt_crypto_e(crypto, k, res, res);
+}
+
+/*
+ * Key generation function s1
+ *
+ * The key generation function s1 is used to generate the STK during the
+ * pairing process.
+ *
+ * The following are inputs to the key generation function s1:
+ *
+ *   k is 128 bits
+ *   r1 is 128 bits
+ *   r2 is 128 bits
+ *
+ * The most significant 64-bits of r1 are discarded to generate r1' and
+ * the most significant 64-bits of r2 are discarded to generate r2'.
+ *
+ * r1' is concatenated with r2' to generate r' which is used as the
+ * 128-bit input parameter plaintextData to security function e:
+ *
+ *   r' = r1' || r2'
+ *
+ * The least significant octet of r2' becomes the least significant
+ * octet of r' and the most significant octet of r1' becomes the most
+ * significant octet of r'.
+ *
+ * The output of the key generation function s1 is:
+ *
+ *   s1(k, r1, r2) = e(k, r')
+ *
+ * The 128-bit output of the security function e is used as the result
+ * of key generation function s1.
+ */
+bool bt_crypto_s1(struct bt_crypto *crypto, const uint8_t k[16],
+			const uint8_t r1[16], const uint8_t r2[16],
+			uint8_t res[16])
+{
+	memcpy(res, r2, 8);
+	memcpy(res + 8, r1, 8);
+
+	return bt_crypto_e(crypto, k, res, res);
+}
+
+static bool aes_cmac(struct bt_crypto *crypto, uint8_t key[16], uint8_t *msg,
+					size_t msg_len, uint8_t res[16])
+{
+	uint8_t key_msb[16], out[16], msg_msb[CMAC_MSG_MAX];
+	ssize_t len;
+	int fd;
+
+	if (msg_len > CMAC_MSG_MAX)
+		return false;
+
+	swap_buf(key, key_msb, 16);
+	fd = alg_new(crypto->cmac_aes, key_msb, 16);
+	if (fd < 0)
+		return false;
+
+	swap_buf(msg, msg_msb, msg_len);
+	len = send(fd, msg_msb, msg_len, 0);
+	if (len < 0) {
+		close(fd);
+		return false;
+	}
+
+	len = read(fd, out, 16);
+	if (len < 0) {
+		close(fd);
+		return false;
+	}
+
+	swap_buf(out, res, 16);
+
+	close(fd);
+
+	return true;
+}
+
+bool bt_crypto_f4(struct bt_crypto *crypto, uint8_t u[32], uint8_t v[32],
+				uint8_t x[16], uint8_t z, uint8_t res[16])
+{
+	uint8_t m[65];
+
+	if (!crypto)
+		return false;
+
+	m[0] = z;
+	memcpy(&m[1], v, 32);
+	memcpy(&m[33], u, 32);
+
+	return aes_cmac(crypto, x, m, sizeof(m), res);
+}
+
+bool bt_crypto_f5(struct bt_crypto *crypto, uint8_t w[32], uint8_t n1[16],
+				uint8_t n2[16], uint8_t a1[7], uint8_t a2[7],
+				uint8_t mackey[16], uint8_t ltk[16])
+{
+	uint8_t btle[4] = { 0x65, 0x6c, 0x74, 0x62 };
+	uint8_t salt[16] = { 0xbe, 0x83, 0x60, 0x5a, 0xdb, 0x0b, 0x37, 0x60,
+			     0x38, 0xa5, 0xf5, 0xaa, 0x91, 0x83, 0x88, 0x6c };
+	uint8_t length[2] = { 0x00, 0x01 };
+	uint8_t m[53], t[16];
+
+	if (!aes_cmac(crypto, salt, w, 32, t))
+		return false;
+
+	memcpy(&m[0], length, 2);
+	memcpy(&m[2], a2, 7);
+	memcpy(&m[9], a1, 7);
+	memcpy(&m[16], n2, 16);
+	memcpy(&m[32], n1, 16);
+	memcpy(&m[48], btle, 4);
+
+	m[52] = 0; /* Counter */
+	if (!aes_cmac(crypto, t, m, sizeof(m), mackey))
+		return false;
+
+	m[52] = 1; /* Counter */
+	return aes_cmac(crypto, t, m, sizeof(m), ltk);
+}
+
+bool bt_crypto_f6(struct bt_crypto *crypto, uint8_t w[16], uint8_t n1[16],
+			uint8_t n2[16], uint8_t r[16], uint8_t io_cap[3],
+			uint8_t a1[7], uint8_t a2[7], uint8_t res[16])
+{
+	uint8_t m[65];
+
+	memcpy(&m[0], a2, 7);
+	memcpy(&m[7], a1, 7);
+	memcpy(&m[14], io_cap, 3);
+	memcpy(&m[17], r, 16);
+	memcpy(&m[33], n2, 16);
+	memcpy(&m[49], n1, 16);
+
+	return aes_cmac(crypto, w, m, sizeof(m), res);
+}
+
+bool bt_crypto_g2(struct bt_crypto *crypto, uint8_t u[32], uint8_t v[32],
+				uint8_t x[16], uint8_t y[16], uint32_t *val)
+{
+	uint8_t m[80], tmp[16];
+
+	memcpy(&m[0], y, 16);
+	memcpy(&m[16], v, 32);
+	memcpy(&m[48], u, 32);
+
+	if (!aes_cmac(crypto, x, m, sizeof(m), tmp))
+		return false;
+
+	*val = get_le32(tmp);
+	*val %= 1000000;
+
+	return true;
+}
diff --git a/bluez/crypto.h b/bluez/crypto.h
new file mode 100644
index 0000000..9ba5803
--- /dev/null
+++ b/bluez/crypto.h
@@ -0,0 +1,61 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2012-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
+ *
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+struct bt_crypto;
+
+struct bt_crypto *bt_crypto_new(void);
+
+struct bt_crypto *bt_crypto_ref(struct bt_crypto *crypto);
+void bt_crypto_unref(struct bt_crypto *crypto);
+
+bool bt_crypto_random_bytes(struct bt_crypto *crypto,
+					uint8_t *buf, uint8_t num_bytes);
+
+bool bt_crypto_e(struct bt_crypto *crypto, const uint8_t key[16],
+			const uint8_t plaintext[16], uint8_t encrypted[16]);
+bool bt_crypto_ah(struct bt_crypto *crypto, const uint8_t k[16],
+					const uint8_t r[3], uint8_t hash[3]);
+bool bt_crypto_c1(struct bt_crypto *crypto, const uint8_t k[16],
+			const uint8_t r[16], const uint8_t pres[7],
+			const uint8_t preq[7], uint8_t iat,
+			const uint8_t ia[6], uint8_t rat,
+			const uint8_t ra[6], uint8_t res[16]);
+bool bt_crypto_s1(struct bt_crypto *crypto, const uint8_t k[16],
+			const uint8_t r1[16], const uint8_t r2[16],
+			uint8_t res[16]);
+bool bt_crypto_f4(struct bt_crypto *crypto, uint8_t u[32], uint8_t v[32],
+				uint8_t x[16], uint8_t z, uint8_t res[16]);
+bool bt_crypto_f5(struct bt_crypto *crypto, uint8_t w[32], uint8_t n1[16],
+				uint8_t n2[16], uint8_t a1[7], uint8_t a2[7],
+				uint8_t mackey[16], uint8_t ltk[16]);
+bool bt_crypto_f6(struct bt_crypto *crypto, uint8_t w[16], uint8_t n1[16],
+			uint8_t n2[16], uint8_t r[16], uint8_t io_cap[3],
+			uint8_t a1[7], uint8_t a2[7], uint8_t res[16]);
+bool bt_crypto_g2(struct bt_crypto *crypto, uint8_t u[32], uint8_t v[32],
+				uint8_t x[16], uint8_t y[16], uint32_t *val);
+bool bt_crypto_sign_att(struct bt_crypto *crypto, const uint8_t key[16],
+				const uint8_t *m, uint16_t m_len,
+				uint32_t sign_cnt, uint8_t signature[12]);
diff --git a/bluez/gatt-client.c b/bluez/gatt-client.c
new file mode 100644
index 0000000..a569992
--- /dev/null
+++ b/bluez/gatt-client.c
@@ -0,0 +1,3013 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2014  Google Inc.
+ *
+ *
+ *  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
+ *
+ */
+
+#include "att.h"
+#include <bluetooth/bluetooth.h>
+#include "uuid.h"
+#include "gatt-helpers.h"
+#include "util.h"
+#include "queue.h"
+#include "gatt-db.h"
+#include "gatt-client.h"
+
+#include <assert.h>
+#include <limits.h>
+#include <sys/uio.h>
+
+#ifndef MAX
+#define MAX(a, b) ((a) > (b) ? (a) : (b))
+#endif
+
+#ifndef MIN
+#define MIN(a, b) ((a) < (b) ? (a) : (b))
+#endif
+
+#define UUID_BYTES (BT_GATT_UUID_SIZE * sizeof(uint8_t))
+
+#define GATT_SVC_UUID	0x1801
+#define SVC_CHNGD_UUID	0x2a05
+
+struct bt_gatt_client {
+	struct bt_att *att;
+	int ref_count;
+
+	bt_gatt_client_callback_t ready_callback;
+	bt_gatt_client_destroy_func_t ready_destroy;
+	void *ready_data;
+
+	bt_gatt_client_service_changed_callback_t svc_chngd_callback;
+	bt_gatt_client_destroy_func_t svc_chngd_destroy;
+	void *svc_chngd_data;
+
+	bt_gatt_client_debug_func_t debug_callback;
+	bt_gatt_client_destroy_func_t debug_destroy;
+	void *debug_data;
+
+	struct gatt_db *db;
+	bool in_init;
+	bool ready;
+
+	/*
+	 * Queue of long write requests. An error during "prepare write"
+	 * requests can result in a cancel through "execute write". To prevent
+	 * cancelation of prepared writes to the wrong attribute and multiple
+	 * requests to the same attribute that may result in a corrupted final
+	 * value, we avoid interleaving prepared writes.
+	 */
+	struct queue *long_write_queue;
+	bool in_long_write;
+
+	unsigned int reliable_write_session_id;
+
+	/* List of registered disconnect/notification/indication callbacks */
+	struct queue *notify_list;
+	struct queue *notify_chrcs;
+	int next_reg_id;
+	unsigned int disc_id, notify_id, ind_id;
+
+	/*
+	 * Handles of the GATT Service and the Service Changed characteristic
+	 * value handle. These will have the value 0 if they are not present on
+	 * the remote peripheral.
+	 */
+	unsigned int svc_chngd_ind_id;
+	bool svc_chngd_registered;
+	struct queue *svc_chngd_queue;  /* Queued service changed events */
+	bool in_svc_chngd;
+
+	/*
+	 * List of pending read/write operations. For operations that span
+	 * across multiple PDUs, this list provides a mapping from an operation
+	 * id to an ATT request id.
+	 */
+	struct queue *pending_requests;
+	unsigned int next_request_id;
+
+	struct bt_gatt_request *discovery_req;
+	unsigned int mtu_req_id;
+};
+
+struct request {
+	struct bt_gatt_client *client;
+	bool long_write;
+	bool prep_write;
+	bool removed;
+	int ref_count;
+	unsigned int id;
+	unsigned int att_id;
+	void *data;
+	void (*destroy)(void *);
+};
+
+static struct request *request_ref(struct request *req)
+{
+	__sync_fetch_and_add(&req->ref_count, 1);
+
+	return req;
+}
+
+static struct request *request_create(struct bt_gatt_client *client)
+{
+	struct request *req;
+
+	req = new0(struct request, 1);
+	if (!req)
+		return NULL;
+
+	if (client->next_request_id < 1)
+		client->next_request_id = 1;
+
+	queue_push_tail(client->pending_requests, req);
+	req->client = client;
+	req->id = client->next_request_id++;
+
+	return request_ref(req);
+}
+
+static void request_unref(void *data)
+{
+	struct request *req = data;
+
+	if (__sync_sub_and_fetch(&req->ref_count, 1))
+		return;
+
+	if (req->destroy)
+		req->destroy(req->data);
+
+	if (!req->removed)
+		queue_remove(req->client->pending_requests, req);
+
+	free(req);
+}
+
+struct notify_chrc {
+	uint16_t value_handle;
+	uint16_t ccc_handle;
+	uint16_t properties;
+	int notify_count;  /* Reference count of registered notify callbacks */
+
+	/* Pending calls to register_notify are queued here so that they can be
+	 * processed after a write that modifies the CCC descriptor.
+	 */
+	struct queue *reg_notify_queue;
+	unsigned int ccc_write_id;
+};
+
+struct notify_data {
+	struct bt_gatt_client *client;
+	unsigned int id;
+	unsigned int att_id;
+	int ref_count;
+	struct notify_chrc *chrc;
+	bt_gatt_client_register_callback_t callback;
+	bt_gatt_client_notify_callback_t notify;
+	void *user_data;
+	bt_gatt_client_destroy_func_t destroy;
+};
+
+static struct notify_data *notify_data_ref(struct notify_data *notify_data)
+{
+	__sync_fetch_and_add(&notify_data->ref_count, 1);
+
+	return notify_data;
+}
+
+static void notify_data_unref(void *data)
+{
+	struct notify_data *notify_data = data;
+
+	if (__sync_sub_and_fetch(&notify_data->ref_count, 1))
+		return;
+
+	if (notify_data->destroy)
+		notify_data->destroy(notify_data->user_data);
+
+	free(notify_data);
+}
+
+static void find_ccc(struct gatt_db_attribute *attr, void *user_data)
+{
+	struct gatt_db_attribute **ccc_ptr = user_data;
+	bt_uuid_t uuid;
+
+	if (*ccc_ptr)
+		return;
+
+	bt_uuid16_create(&uuid, GATT_CLIENT_CHARAC_CFG_UUID);
+
+	if (bt_uuid_cmp(&uuid, gatt_db_attribute_get_type(attr)))
+		return;
+
+	*ccc_ptr = attr;
+}
+
+static struct notify_chrc *notify_chrc_create(struct bt_gatt_client *client,
+							uint16_t value_handle)
+{
+	struct gatt_db_attribute *attr, *ccc;
+	struct notify_chrc *chrc;
+	bt_uuid_t uuid;
+	uint8_t properties;
+
+	/* Check that chrc_value_handle belongs to a known characteristic */
+	attr = gatt_db_get_attribute(client->db, value_handle - 1);
+	if (!attr)
+		return NULL;
+
+	bt_uuid16_create(&uuid, GATT_CHARAC_UUID);
+	if (bt_uuid_cmp(&uuid, gatt_db_attribute_get_type(attr)))
+		return NULL;
+
+	if (!gatt_db_attribute_get_char_data(attr, NULL, NULL,
+							&properties, NULL))
+			return NULL;
+
+	chrc = new0(struct notify_chrc, 1);
+	if (!chrc)
+		return NULL;
+
+	chrc->reg_notify_queue = queue_new();
+	if (!chrc->reg_notify_queue) {
+		free(chrc);
+		return NULL;
+	}
+
+	/*
+	 * Find the CCC characteristic. Some characteristics that allow
+	 * notifications may not have a CCC descriptor. We treat these as
+	 * automatically successful.
+	 */
+	ccc = NULL;
+	gatt_db_service_foreach_desc(attr, find_ccc, &ccc);
+	if (ccc)
+		chrc->ccc_handle = gatt_db_attribute_get_handle(ccc);
+
+	chrc->value_handle = value_handle;
+	chrc->properties = properties;
+
+	queue_push_tail(client->notify_chrcs, chrc);
+
+	return chrc;
+}
+
+static void notify_chrc_free(void *data)
+{
+	struct notify_chrc *chrc = data;
+
+	queue_destroy(chrc->reg_notify_queue, notify_data_unref);
+	free(chrc);
+}
+
+static bool match_notify_data_id(const void *a, const void *b)
+{
+	const struct notify_data *notify_data = a;
+	unsigned int id = PTR_TO_UINT(b);
+
+	return notify_data->id == id;
+}
+
+struct handle_range {
+	uint16_t start;
+	uint16_t end;
+};
+
+static bool match_notify_data_handle_range(const void *a, const void *b)
+{
+	const struct notify_data *notify_data = a;
+	struct notify_chrc *chrc = notify_data->chrc;
+	const struct handle_range *range = b;
+
+	return chrc->value_handle >= range->start &&
+					chrc->value_handle <= range->end;
+}
+
+static bool match_notify_chrc_handle_range(const void *a, const void *b)
+{
+	const struct notify_chrc *chrc = a;
+	const struct handle_range *range = b;
+
+	return chrc->value_handle >= range->start &&
+					chrc->value_handle <= range->end;
+}
+
+static void gatt_client_remove_all_notify_in_range(
+				struct bt_gatt_client *client,
+				uint16_t start_handle, uint16_t end_handle)
+{
+	struct handle_range range;
+
+	range.start = start_handle;
+	range.end = end_handle;
+
+	queue_remove_all(client->notify_list, match_notify_data_handle_range,
+						&range, notify_data_unref);
+}
+
+static void gatt_client_remove_notify_chrcs_in_range(
+				struct bt_gatt_client *client,
+				uint16_t start_handle, uint16_t end_handle)
+{
+	struct handle_range range;
+
+	range.start = start_handle;
+	range.end = end_handle;
+
+	queue_remove_all(client->notify_chrcs, match_notify_chrc_handle_range,
+						&range, notify_chrc_free);
+}
+
+struct discovery_op;
+
+typedef void (*discovery_op_complete_func_t)(struct discovery_op *op,
+							bool success,
+							uint8_t att_ecode);
+typedef void (*discovery_op_fail_func_t)(struct discovery_op *op);
+
+struct discovery_op {
+	struct bt_gatt_client *client;
+	struct queue *pending_svcs;
+	struct queue *pending_chrcs;
+	struct queue *tmp_queue;
+	struct gatt_db_attribute *cur_svc;
+	bool success;
+	uint16_t start;
+	uint16_t end;
+	int ref_count;
+	discovery_op_complete_func_t complete_func;
+	discovery_op_fail_func_t failure_func;
+};
+
+static void discovery_op_free(struct discovery_op *op)
+{
+	queue_destroy(op->pending_svcs, NULL);
+	queue_destroy(op->pending_chrcs, free);
+	queue_destroy(op->tmp_queue, NULL);
+	free(op);
+}
+
+static struct discovery_op *discovery_op_create(struct bt_gatt_client *client,
+				uint16_t start, uint16_t end,
+				discovery_op_complete_func_t complete_func,
+				discovery_op_fail_func_t failure_func)
+{
+	struct discovery_op *op;
+
+	op = new0(struct discovery_op, 1);
+	if (!op)
+		return NULL;
+
+	op->pending_svcs = queue_new();
+	if (!op->pending_svcs)
+		goto fail;
+
+	op->pending_chrcs = queue_new();
+	if (!op->pending_chrcs)
+		goto fail;
+
+	op->tmp_queue = queue_new();
+	if (!op->tmp_queue)
+		goto fail;
+
+	op->client = client;
+	op->complete_func = complete_func;
+	op->failure_func = failure_func;
+	op->start = start;
+	op->end = end;
+
+	return op;
+
+fail:
+	discovery_op_free(op);
+	return NULL;
+}
+
+static struct discovery_op *discovery_op_ref(struct discovery_op *op)
+{
+	__sync_fetch_and_add(&op->ref_count, 1);
+
+	return op;
+}
+
+static void discovery_op_unref(void *data)
+{
+	struct discovery_op *op = data;
+
+	if (__sync_sub_and_fetch(&op->ref_count, 1))
+		return;
+
+	if (!op->success)
+		op->failure_func(op);
+
+	discovery_op_free(op);
+}
+
+static void discovery_req_clear(struct bt_gatt_client *client)
+{
+	if (!client->discovery_req)
+		return;
+
+	bt_gatt_request_unref(client->discovery_req);
+	client->discovery_req = NULL;
+}
+
+static void discover_chrcs_cb(bool success, uint8_t att_ecode,
+						struct bt_gatt_result *result,
+						void *user_data);
+
+static void discover_incl_cb(bool success, uint8_t att_ecode,
+				struct bt_gatt_result *result, void *user_data)
+{
+	struct discovery_op *op = user_data;
+	struct bt_gatt_client *client = op->client;
+	struct bt_gatt_iter iter;
+	struct gatt_db_attribute *attr, *tmp;
+	uint16_t handle, start, end;
+	uint128_t u128;
+	bt_uuid_t uuid;
+	char uuid_str[MAX_LEN_UUID_STR];
+	unsigned int includes_count, i;
+
+	discovery_req_clear(client);
+
+	if (!success) {
+		if (att_ecode == BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND)
+			goto next;
+
+		goto failed;
+	}
+
+	/* Get the currently processed service */
+	attr = op->cur_svc;
+	if (!attr)
+		goto failed;
+
+	if (!result || !bt_gatt_iter_init(&iter, result))
+		goto failed;
+
+	includes_count = bt_gatt_result_included_count(result);
+	if (includes_count == 0)
+		goto failed;
+
+	util_debug(client->debug_callback, client->debug_data,
+						"Included services found: %u",
+						includes_count);
+
+	for (i = 0; i < includes_count; i++) {
+		if (!bt_gatt_iter_next_included_service(&iter, &handle, &start,
+							&end, u128.data))
+			break;
+
+		bt_uuid128_create(&uuid, u128);
+
+		/* Log debug message */
+		bt_uuid_to_string(&uuid, uuid_str, sizeof(uuid_str));
+		util_debug(client->debug_callback, client->debug_data,
+				"handle: 0x%04x, start: 0x%04x, end: 0x%04x,"
+				"uuid: %s", handle, start, end, uuid_str);
+
+		tmp = gatt_db_get_attribute(client->db, start);
+		if (!tmp)
+			goto failed;
+
+		tmp = gatt_db_service_add_included(attr, tmp);
+		if (!tmp)
+			goto failed;
+
+		/*
+		 * GATT requires that all include definitions precede
+		 * characteristic declarations. Based on the order we're adding
+		 * these entries, the correct handle must be assigned to the new
+		 * attribute.
+		 */
+		if (gatt_db_attribute_get_handle(tmp) != handle)
+			goto failed;
+	}
+
+next:
+	/* Move on to the next service */
+	attr = queue_pop_head(op->pending_svcs);
+	if (!attr) {
+		struct queue *tmp_queue;
+
+		tmp_queue = op->pending_svcs;
+		op->pending_svcs = op->tmp_queue;
+		op->tmp_queue = tmp_queue;
+
+		/*
+		 * We have processed all include definitions. Move on to
+		 * characteristics.
+		 */
+		attr = queue_pop_head(op->pending_svcs);
+		if (!attr)
+			goto failed;
+
+		if (!gatt_db_attribute_get_service_handles(attr, &start, &end))
+			goto failed;
+
+		op->cur_svc = attr;
+
+		client->discovery_req = bt_gatt_discover_characteristics(
+							client->att,
+							start, end,
+							discover_chrcs_cb,
+							discovery_op_ref(op),
+							discovery_op_unref);
+		if (client->discovery_req)
+			return;
+
+		util_debug(client->debug_callback, client->debug_data,
+				"Failed to start characteristic discovery");
+		discovery_op_unref(op);
+		goto failed;
+	}
+
+	queue_push_tail(op->tmp_queue, attr);
+	op->cur_svc = attr;
+	if (!gatt_db_attribute_get_service_handles(attr, &start, &end))
+		goto failed;
+
+	if (start == end)
+		goto next;
+
+	client->discovery_req = bt_gatt_discover_included_services(client->att,
+							start, end,
+							discover_incl_cb,
+							discovery_op_ref(op),
+							discovery_op_unref);
+	if (client->discovery_req)
+		return;
+
+	util_debug(client->debug_callback, client->debug_data,
+					"Failed to start included discovery");
+	discovery_op_unref(op);
+
+failed:
+	op->success = false;
+	op->complete_func(op, false, att_ecode);
+}
+
+struct chrc {
+	uint16_t start_handle;
+	uint16_t end_handle;
+	uint16_t value_handle;
+	uint8_t properties;
+	bt_uuid_t uuid;
+};
+
+static void discover_descs_cb(bool success, uint8_t att_ecode,
+						struct bt_gatt_result *result,
+						void *user_data);
+
+static bool discover_descs(struct discovery_op *op, bool *discovering)
+{
+	struct bt_gatt_client *client = op->client;
+	struct gatt_db_attribute *attr;
+	struct chrc *chrc_data;
+	uint16_t desc_start;
+
+	*discovering = false;
+
+	while ((chrc_data = queue_pop_head(op->pending_chrcs))) {
+		attr = gatt_db_service_add_characteristic(op->cur_svc,
+							&chrc_data->uuid, 0,
+							chrc_data->properties,
+							NULL, NULL, NULL);
+
+		if (!attr)
+			goto failed;
+
+		if (gatt_db_attribute_get_handle(attr) !=
+							chrc_data->value_handle)
+			goto failed;
+
+		desc_start = chrc_data->value_handle + 1;
+
+		if (desc_start > chrc_data->end_handle) {
+			free(chrc_data);
+			continue;
+		}
+
+		client->discovery_req = bt_gatt_discover_descriptors(
+							client->att, desc_start,
+							chrc_data->end_handle,
+							discover_descs_cb,
+							discovery_op_ref(op),
+							discovery_op_unref);
+		if (client->discovery_req) {
+			*discovering = true;
+			goto done;
+		}
+
+		util_debug(client->debug_callback, client->debug_data,
+					"Failed to start descriptor discovery");
+		discovery_op_unref(op);
+
+		goto failed;
+	}
+
+done:
+	free(chrc_data);
+	return true;
+
+failed:
+	free(chrc_data);
+	return false;
+}
+
+static void discover_descs_cb(bool success, uint8_t att_ecode,
+						struct bt_gatt_result *result,
+						void *user_data)
+{
+	struct discovery_op *op = user_data;
+	struct bt_gatt_client *client = op->client;
+	struct bt_gatt_iter iter;
+	struct gatt_db_attribute *attr;
+	uint16_t handle, start, end;
+	uint128_t u128;
+	bt_uuid_t uuid;
+	char uuid_str[MAX_LEN_UUID_STR];
+	unsigned int desc_count;
+	bool discovering;
+
+	discovery_req_clear(client);
+
+	if (!success) {
+		if (att_ecode == BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND) {
+			success = true;
+			goto next;
+		}
+
+		goto done;
+	}
+
+	if (!result || !bt_gatt_iter_init(&iter, result))
+		goto failed;
+
+	desc_count = bt_gatt_result_descriptor_count(result);
+	if (desc_count == 0)
+		goto failed;
+
+	util_debug(client->debug_callback, client->debug_data,
+					"Descriptors found: %u", desc_count);
+
+	while (bt_gatt_iter_next_descriptor(&iter, &handle, u128.data)) {
+		bt_uuid128_create(&uuid, u128);
+
+		/* Log debug message */
+		bt_uuid_to_string(&uuid, uuid_str, sizeof(uuid_str));
+		util_debug(client->debug_callback, client->debug_data,
+						"handle: 0x%04x, uuid: %s",
+						handle, uuid_str);
+
+		attr = gatt_db_service_add_descriptor(op->cur_svc, &uuid, 0,
+							NULL, NULL, NULL);
+		if (!attr)
+			goto failed;
+
+		if (gatt_db_attribute_get_handle(attr) != handle)
+			goto failed;
+	}
+
+	if (!discover_descs(op, &discovering))
+		goto failed;
+
+	if (discovering)
+		return;
+
+next:
+	/* Done with the current service */
+	gatt_db_service_set_active(op->cur_svc, true);
+
+	attr = queue_pop_head(op->pending_svcs);
+	if (!attr)
+		goto done;
+
+	if (!gatt_db_attribute_get_service_handles(attr, &start, &end))
+		goto failed;
+
+	if (start == end)
+		goto next;
+
+	/* Move on to the next service */
+	op->cur_svc = attr;
+
+	client->discovery_req = bt_gatt_discover_characteristics(client->att,
+							start, end,
+							discover_chrcs_cb,
+							discovery_op_ref(op),
+							discovery_op_unref);
+	if (client->discovery_req)
+		return;
+
+	util_debug(client->debug_callback, client->debug_data,
+				"Failed to start characteristic discovery");
+	discovery_op_unref(op);
+
+failed:
+	success = false;
+
+done:
+	op->success = success;
+	op->complete_func(op, success, att_ecode);
+}
+
+static void discover_chrcs_cb(bool success, uint8_t att_ecode,
+						struct bt_gatt_result *result,
+						void *user_data)
+{
+	struct discovery_op *op = user_data;
+	struct bt_gatt_client *client = op->client;
+	struct bt_gatt_iter iter;
+	struct gatt_db_attribute *attr;
+	struct chrc *chrc_data;
+	uint16_t start, end, value;
+	uint8_t properties;
+	uint128_t u128;
+	bt_uuid_t uuid;
+	char uuid_str[MAX_LEN_UUID_STR];
+	unsigned int chrc_count;
+	bool discovering;
+
+	discovery_req_clear(client);
+
+	if (!success) {
+		if (att_ecode == BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND) {
+			success = true;
+			goto next;
+		}
+
+		goto done;
+	}
+
+	if (!op->cur_svc || !result || !bt_gatt_iter_init(&iter, result))
+		goto failed;
+
+	chrc_count = bt_gatt_result_characteristic_count(result);
+	util_debug(client->debug_callback, client->debug_data,
+				"Characteristics found: %u", chrc_count);
+
+	if (chrc_count == 0)
+		goto failed;
+
+	while (bt_gatt_iter_next_characteristic(&iter, &start, &end, &value,
+						&properties, u128.data)) {
+		bt_uuid128_create(&uuid, u128);
+
+		/* Log debug message */
+		bt_uuid_to_string(&uuid, uuid_str, sizeof(uuid_str));
+		util_debug(client->debug_callback, client->debug_data,
+				"start: 0x%04x, end: 0x%04x, value: 0x%04x, "
+				"props: 0x%02x, uuid: %s",
+				start, end, value, properties, uuid_str);
+
+		chrc_data = new0(struct chrc, 1);
+		if (!chrc_data)
+			goto failed;
+
+		chrc_data->start_handle = start;
+		chrc_data->end_handle = end;
+		chrc_data->value_handle = value;
+		chrc_data->properties = properties;
+		chrc_data->uuid = uuid;
+
+		queue_push_tail(op->pending_chrcs, chrc_data);
+	}
+
+	/*
+	 * Sequentially discover descriptors for each characteristic and insert
+	 * the characteristics into the database as we proceed.
+	 */
+	if (!discover_descs(op, &discovering))
+		goto failed;
+
+	if (discovering)
+		return;
+
+next:
+	/* Done with the current service */
+	gatt_db_service_set_active(op->cur_svc, true);
+
+	attr = queue_pop_head(op->pending_svcs);
+	if (!attr)
+		goto done;
+
+	if (!gatt_db_attribute_get_service_handles(attr, &start, &end))
+		goto failed;
+
+	if (start == end)
+		goto next;
+
+	/* Move on to the next service */
+	op->cur_svc = attr;
+
+	client->discovery_req = bt_gatt_discover_characteristics(client->att,
+							start, end,
+							discover_chrcs_cb,
+							discovery_op_ref(op),
+							discovery_op_unref);
+	if (client->discovery_req)
+		return;
+
+	util_debug(client->debug_callback, client->debug_data,
+				"Failed to start characteristic discovery");
+	discovery_op_unref(op);
+
+failed:
+	success = false;
+
+done:
+	op->success = success;
+	op->complete_func(op, success, att_ecode);
+}
+
+static void discover_secondary_cb(bool success, uint8_t att_ecode,
+						struct bt_gatt_result *result,
+						void *user_data)
+{
+	struct discovery_op *op = user_data;
+	struct bt_gatt_client *client = op->client;
+	struct bt_gatt_iter iter;
+	struct gatt_db_attribute *attr;
+	uint16_t start, end;
+	uint128_t u128;
+	bt_uuid_t uuid;
+	char uuid_str[MAX_LEN_UUID_STR];
+
+	discovery_req_clear(client);
+
+	if (!success) {
+		util_debug(client->debug_callback, client->debug_data,
+					"Secondary service discovery failed."
+					" ATT ECODE: 0x%02x", att_ecode);
+		switch (att_ecode) {
+		case BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND:
+		case BT_ATT_ERROR_UNSUPPORTED_GROUP_TYPE:
+			goto next;
+		default:
+			goto done;
+		}
+	}
+
+	if (!result || !bt_gatt_iter_init(&iter, result)) {
+		success = false;
+		goto done;
+	}
+
+	util_debug(client->debug_callback, client->debug_data,
+					"Secondary services found: %u",
+					bt_gatt_result_service_count(result));
+
+	while (bt_gatt_iter_next_service(&iter, &start, &end, u128.data)) {
+		bt_uuid128_create(&uuid, u128);
+
+		/* Log debug message */
+		bt_uuid_to_string(&uuid, uuid_str, sizeof(uuid_str));
+		util_debug(client->debug_callback, client->debug_data,
+				"start: 0x%04x, end: 0x%04x, uuid: %s",
+				start, end, uuid_str);
+
+		/* Store the service */
+		attr = gatt_db_insert_service(client->db, start, &uuid, false,
+							end - start + 1);
+		if (!attr) {
+			util_debug(client->debug_callback, client->debug_data,
+						"Failed to create service");
+			success = false;
+			goto done;
+		}
+
+		queue_push_tail(op->pending_svcs, attr);
+	}
+
+next:
+	/* Sequentially discover included services */
+	attr = queue_pop_head(op->pending_svcs);
+
+	/* Complete with success if queue is empty */
+	if (!attr)
+		goto done;
+
+	/*
+	 * Store the service in the tmp queue to be reused during
+	 * characteristics discovery later.
+	 */
+	queue_push_tail(op->tmp_queue, attr);
+	op->cur_svc = attr;
+
+	if (!gatt_db_attribute_get_service_handles(attr, &start, &end)) {
+		success = false;
+		goto done;
+	}
+
+	client->discovery_req = bt_gatt_discover_included_services(client->att,
+							start, end,
+							discover_incl_cb,
+							discovery_op_ref(op),
+							discovery_op_unref);
+	if (client->discovery_req)
+		return;
+
+	util_debug(client->debug_callback, client->debug_data,
+				"Failed to start included services discovery");
+	discovery_op_unref(op);
+
+done:
+	op->success = success;
+	op->complete_func(op, success, att_ecode);
+}
+
+static void discover_primary_cb(bool success, uint8_t att_ecode,
+						struct bt_gatt_result *result,
+						void *user_data)
+{
+	struct discovery_op *op = user_data;
+	struct bt_gatt_client *client = op->client;
+	struct bt_gatt_iter iter;
+	struct gatt_db_attribute *attr;
+	uint16_t start, end;
+	uint128_t u128;
+	bt_uuid_t uuid;
+	char uuid_str[MAX_LEN_UUID_STR];
+
+	discovery_req_clear(client);
+
+	if (!success) {
+		util_debug(client->debug_callback, client->debug_data,
+					"Primary service discovery failed."
+					" ATT ECODE: 0x%02x", att_ecode);
+		goto secondary;
+	}
+
+	if (!result || !bt_gatt_iter_init(&iter, result)) {
+		success = false;
+		goto done;
+	}
+
+	util_debug(client->debug_callback, client->debug_data,
+					"Primary services found: %u",
+					bt_gatt_result_service_count(result));
+
+	while (bt_gatt_iter_next_service(&iter, &start, &end, u128.data)) {
+		bt_uuid128_create(&uuid, u128);
+
+		/* Log debug message. */
+		bt_uuid_to_string(&uuid, uuid_str, sizeof(uuid_str));
+		util_debug(client->debug_callback, client->debug_data,
+				"start: 0x%04x, end: 0x%04x, uuid: %s",
+				start, end, uuid_str);
+
+		attr = gatt_db_insert_service(client->db, start, &uuid, true,
+							end - start + 1);
+		if (!attr) {
+			util_debug(client->debug_callback, client->debug_data,
+						"Failed to store service");
+			success = false;
+			goto done;
+		}
+
+		queue_push_tail(op->pending_svcs, attr);
+	}
+
+secondary:
+	/* Discover secondary services */
+	client->discovery_req = bt_gatt_discover_secondary_services(client->att,
+						NULL, op->start, op->end,
+						discover_secondary_cb,
+						discovery_op_ref(op),
+						discovery_op_unref);
+	if (client->discovery_req)
+		return;
+
+	util_debug(client->debug_callback, client->debug_data,
+				"Failed to start secondary service discovery");
+	discovery_op_unref(op);
+	success = false;
+
+done:
+	op->success = success;
+	op->complete_func(op, success, att_ecode);
+}
+
+static void notify_client_ready(struct bt_gatt_client *client, bool success,
+							uint8_t att_ecode)
+{
+	if (!client->ready_callback)
+		return;
+
+	bt_gatt_client_ref(client);
+	client->ready_callback(success, att_ecode, client->ready_data);
+	bt_gatt_client_unref(client);
+}
+
+static void exchange_mtu_cb(bool success, uint8_t att_ecode, void *user_data)
+{
+	struct discovery_op *op = user_data;
+	struct bt_gatt_client *client = op->client;
+
+	op->success = success;
+	client->mtu_req_id = 0;
+
+	if (!success) {
+		util_debug(client->debug_callback, client->debug_data,
+				"MTU Exchange failed. ATT ECODE: 0x%02x",
+				att_ecode);
+
+		client->in_init = false;
+		notify_client_ready(client, success, att_ecode);
+
+		return;
+	}
+
+	util_debug(client->debug_callback, client->debug_data,
+					"MTU exchange complete, with MTU: %u",
+					bt_att_get_mtu(client->att));
+
+	/* Don't do discovery if the database was pre-populated */
+	if (!gatt_db_isempty(client->db)) {
+		op->complete_func(op, true, 0);
+		return;
+	}
+
+	client->discovery_req = bt_gatt_discover_all_primary_services(
+							client->att, NULL,
+							discover_primary_cb,
+							discovery_op_ref(op),
+							discovery_op_unref);
+	if (client->discovery_req)
+		return;
+
+	util_debug(client->debug_callback, client->debug_data,
+			"Failed to initiate primary service discovery");
+
+	client->in_init = false;
+	notify_client_ready(client, false, att_ecode);
+
+	discovery_op_unref(op);
+}
+
+struct service_changed_op {
+	struct bt_gatt_client *client;
+	uint16_t start_handle;
+	uint16_t end_handle;
+};
+
+static void service_changed_reregister_cb(uint16_t att_ecode, void *user_data)
+{
+	struct bt_gatt_client *client = user_data;
+
+	if (!att_ecode) {
+		util_debug(client->debug_callback, client->debug_data,
+			"Re-registered handler for \"Service Changed\" after "
+			"change in GATT service");
+		client->svc_chngd_registered = true;
+		return;
+	}
+
+	util_debug(client->debug_callback, client->debug_data,
+		"Failed to register handler for \"Service Changed\"");
+	client->svc_chngd_ind_id = 0;
+}
+
+static void process_service_changed(struct bt_gatt_client *client,
+							uint16_t start_handle,
+							uint16_t end_handle);
+static void service_changed_cb(uint16_t value_handle, const uint8_t *value,
+					uint16_t length, void *user_data);
+
+static void get_first_attribute(struct gatt_db_attribute *attrib,
+								void *user_data)
+{
+	struct gatt_db_attribute **stored = user_data;
+
+	if (*stored)
+		return;
+
+	*stored = attrib;
+}
+
+static void service_changed_complete(struct discovery_op *op, bool success,
+							uint8_t att_ecode)
+{
+	struct bt_gatt_client *client = op->client;
+	struct service_changed_op *next_sc_op;
+	uint16_t start_handle = op->start;
+	uint16_t end_handle = op->end;
+	struct gatt_db_attribute *attr = NULL;
+	bt_uuid_t uuid;
+
+	client->in_svc_chngd = false;
+
+	if (!success && att_ecode != BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND) {
+		util_debug(client->debug_callback, client->debug_data,
+			"Failed to discover services within changed range - "
+			"error: 0x%02x", att_ecode);
+
+		gatt_db_clear_range(client->db, start_handle, end_handle);
+	}
+
+	/* Notify the upper layer of changed services */
+	if (client->svc_chngd_callback)
+		client->svc_chngd_callback(start_handle, end_handle,
+							client->svc_chngd_data);
+
+	/* Process any queued events */
+	next_sc_op = queue_pop_head(client->svc_chngd_queue);
+	if (next_sc_op) {
+		process_service_changed(client, next_sc_op->start_handle,
+							next_sc_op->end_handle);
+		free(next_sc_op);
+		return;
+	}
+
+	bt_uuid16_create(&uuid, SVC_CHNGD_UUID);
+
+	gatt_db_find_by_type(client->db, start_handle, end_handle, &uuid,
+						get_first_attribute, &attr);
+	if (!attr)
+		return;
+
+	/* The GATT service was modified. Re-register the handler for
+	 * indications from the "Service Changed" characteristic.
+	 */
+	client->svc_chngd_registered = false;
+	client->svc_chngd_ind_id = bt_gatt_client_register_notify(client,
+					gatt_db_attribute_get_handle(attr),
+					service_changed_reregister_cb,
+					service_changed_cb,
+					client, NULL);
+	if (client->svc_chngd_ind_id)
+		return;
+
+	util_debug(client->debug_callback, client->debug_data,
+		"Failed to re-register handler for \"Service Changed\"");
+}
+
+static void service_changed_failure(struct discovery_op *op)
+{
+	struct bt_gatt_client *client = op->client;
+
+	gatt_db_clear_range(client->db, op->start, op->end);
+}
+
+static void process_service_changed(struct bt_gatt_client *client,
+							uint16_t start_handle,
+							uint16_t end_handle)
+{
+	struct discovery_op *op;
+
+	/* Invalidate and remove all effected notify callbacks */
+	gatt_client_remove_all_notify_in_range(client, start_handle,
+								end_handle);
+	gatt_client_remove_notify_chrcs_in_range(client, start_handle,
+								end_handle);
+
+	/* Remove all services that overlap the modified range since we'll
+	 * rediscover them
+	 */
+	gatt_db_clear_range(client->db, start_handle, end_handle);
+
+	op = discovery_op_create(client, start_handle, end_handle,
+						service_changed_complete,
+						service_changed_failure);
+	if (!op)
+		goto fail;
+
+	client->discovery_req = bt_gatt_discover_primary_services(client->att,
+						NULL, start_handle, end_handle,
+						discover_primary_cb,
+						discovery_op_ref(op),
+						discovery_op_unref);
+	if (client->discovery_req) {
+		client->in_svc_chngd = true;
+		return;
+	}
+
+	discovery_op_free(op);
+
+fail:
+	util_debug(client->debug_callback, client->debug_data,
+					"Failed to initiate service discovery"
+					" after Service Changed");
+}
+
+static void service_changed_cb(uint16_t value_handle, const uint8_t *value,
+					uint16_t length, void *user_data)
+{
+	struct bt_gatt_client *client = user_data;
+	struct service_changed_op *op;
+	uint16_t start, end;
+
+	if (length != 4)
+		return;
+
+	start = get_le16(value);
+	end = get_le16(value + 2);
+
+	if (start > end) {
+		util_debug(client->debug_callback, client->debug_data,
+			"Service Changed received with invalid handles");
+		return;
+	}
+
+	util_debug(client->debug_callback, client->debug_data,
+			"Service Changed received - start: 0x%04x end: 0x%04x",
+			start, end);
+
+	if (!client->in_svc_chngd) {
+		process_service_changed(client, start, end);
+		return;
+	}
+
+	op = new0(struct service_changed_op, 1);
+	if (!op)
+		return;
+
+	op->start_handle = start;
+	op->end_handle = end;
+
+	queue_push_tail(client->svc_chngd_queue, op);
+}
+
+static void service_changed_register_cb(uint16_t att_ecode, void *user_data)
+{
+	bool success;
+	struct bt_gatt_client *client = user_data;
+
+	if (att_ecode) {
+		util_debug(client->debug_callback, client->debug_data,
+			"Failed to register handler for \"Service Changed\"");
+		success = false;
+		client->svc_chngd_ind_id = 0;
+		goto done;
+	}
+
+	client->svc_chngd_registered = true;
+	client->ready = true;
+	success = true;
+	util_debug(client->debug_callback, client->debug_data,
+			"Registered handler for \"Service Changed\": %u",
+			client->svc_chngd_ind_id);
+
+done:
+	notify_client_ready(client, success, att_ecode);
+}
+
+static void init_complete(struct discovery_op *op, bool success,
+							uint8_t att_ecode)
+{
+	struct bt_gatt_client *client = op->client;
+	struct gatt_db_attribute *attr = NULL;
+	bt_uuid_t uuid;
+
+	client->in_init = false;
+
+	if (!success)
+		goto fail;
+
+	bt_uuid16_create(&uuid, SVC_CHNGD_UUID);
+
+	gatt_db_find_by_type(client->db, 0x0001, 0xffff, &uuid,
+						get_first_attribute, &attr);
+	if (!attr) {
+		client->ready = true;
+		goto done;
+	}
+
+	/* Register an indication handler for the "Service Changed"
+	 * characteristic and report ready only if the handler is registered
+	 * successfully. Temporarily set "ready" to true so that we can register
+	 * the handler using the existing framework.
+	 */
+	client->ready = true;
+	client->svc_chngd_ind_id = bt_gatt_client_register_notify(client,
+					gatt_db_attribute_get_handle(attr),
+					service_changed_register_cb,
+					service_changed_cb,
+					client, NULL);
+
+	if (!client->svc_chngd_registered)
+		client->ready = false;
+
+	if (client->svc_chngd_ind_id)
+		return;
+
+	util_debug(client->debug_callback, client->debug_data,
+			"Failed to register handler for \"Service Changed\"");
+	success = false;
+
+fail:
+	util_debug(client->debug_callback, client->debug_data,
+			"Failed to initialize gatt-client");
+
+	op->success = false;
+
+done:
+	notify_client_ready(client, success, att_ecode);
+}
+
+static void init_fail(struct discovery_op *op)
+{
+	struct bt_gatt_client *client = op->client;
+
+	gatt_db_clear(client->db);
+}
+
+static bool gatt_client_init(struct bt_gatt_client *client, uint16_t mtu)
+{
+	struct discovery_op *op;
+
+	if (client->in_init || client->ready)
+		return false;
+
+	op = discovery_op_create(client, 0x0001, 0xffff, init_complete,
+								init_fail);
+	if (!op)
+		return false;
+
+	/* Configure the MTU */
+	client->mtu_req_id = bt_gatt_exchange_mtu(client->att,
+						MAX(BT_ATT_DEFAULT_LE_MTU, mtu),
+						exchange_mtu_cb,
+						discovery_op_ref(op),
+						discovery_op_unref);
+	if (!client->mtu_req_id) {
+		discovery_op_free(op);
+		return false;
+	}
+
+	client->in_init = true;
+
+	return true;
+}
+
+struct pdu_data {
+	const void *pdu;
+	uint16_t length;
+};
+
+static void complete_notify_request(void *data)
+{
+	struct notify_data *notify_data = data;
+
+	/* Increment the per-characteristic ref count of notify handlers */
+	__sync_fetch_and_add(&notify_data->chrc->notify_count, 1);
+
+	notify_data->att_id = 0;
+	notify_data->callback(0, notify_data->user_data);
+}
+
+static bool notify_data_write_ccc(struct notify_data *notify_data, bool enable,
+						bt_att_response_func_t callback)
+{
+	uint8_t pdu[4];
+	unsigned int att_id;
+
+	assert(notify_data->chrc->ccc_handle);
+	memset(pdu, 0, sizeof(pdu));
+	put_le16(notify_data->chrc->ccc_handle, pdu);
+
+	if (enable) {
+		/* Try to enable notifications and/or indications based on
+		 * whatever the characteristic supports.
+		 */
+		if (notify_data->chrc->properties & BT_GATT_CHRC_PROP_NOTIFY)
+			pdu[2] = 0x01;
+
+		if (notify_data->chrc->properties & BT_GATT_CHRC_PROP_INDICATE)
+			pdu[2] |= 0x02;
+
+		if (!pdu[2])
+			return false;
+	}
+
+	att_id = bt_att_send(notify_data->client->att, BT_ATT_OP_WRITE_REQ,
+						pdu, sizeof(pdu), callback,
+						notify_data_ref(notify_data),
+						notify_data_unref);
+	notify_data->chrc->ccc_write_id = notify_data->att_id = att_id;
+
+	return !!att_id;
+}
+
+static uint8_t process_error(const void *pdu, uint16_t length)
+{
+	const struct bt_att_pdu_error_rsp *error_pdu;
+
+	if (!pdu || length != sizeof(struct bt_att_pdu_error_rsp))
+		return 0;
+
+	error_pdu = pdu;
+
+	return error_pdu->ecode;
+}
+
+static void enable_ccc_callback(uint8_t opcode, const void *pdu,
+					uint16_t length, void *user_data)
+{
+	struct notify_data *notify_data = user_data;
+	uint16_t att_ecode;
+
+	assert(!notify_data->chrc->notify_count);
+	assert(notify_data->chrc->ccc_write_id);
+
+	notify_data->chrc->ccc_write_id = 0;
+
+	if (opcode == BT_ATT_OP_ERROR_RSP) {
+		att_ecode = process_error(pdu, length);
+
+		/* Failed to enable. Complete the current request and move on to
+		 * the next one in the queue. If there was an error sending the
+		 * write request, then just move on to the next queued entry.
+		 */
+		queue_remove(notify_data->client->notify_list, notify_data);
+		notify_data->callback(att_ecode, notify_data->user_data);
+
+		while ((notify_data = queue_pop_head(
+					notify_data->chrc->reg_notify_queue))) {
+
+			if (notify_data_write_ccc(notify_data, true,
+							enable_ccc_callback))
+				return;
+		}
+
+		return;
+	}
+
+	/* Success! Report success for all remaining requests. */
+	bt_gatt_client_ref(notify_data->client);
+
+	complete_notify_request(notify_data);
+	queue_remove_all(notify_data->chrc->reg_notify_queue, NULL, NULL,
+						complete_notify_request);
+
+	bt_gatt_client_unref(notify_data->client);
+}
+
+static void disable_ccc_callback(uint8_t opcode, const void *pdu,
+					uint16_t length, void *user_data)
+{
+	struct notify_data *notify_data = user_data;
+	struct notify_data *next_data;
+
+	assert(!notify_data->chrc->notify_count);
+	assert(notify_data->chrc->ccc_write_id);
+
+	notify_data->chrc->ccc_write_id = 0;
+
+	/* This is a best effort procedure, so ignore errors and process any
+	 * queued requests.
+	 */
+	while (1) {
+		next_data = queue_pop_head(notify_data->chrc->reg_notify_queue);
+		if (!next_data || notify_data_write_ccc(notify_data, true,
+							enable_ccc_callback))
+			return;
+	}
+}
+
+static void complete_unregister_notify(void *data)
+{
+	struct notify_data *notify_data = data;
+
+	/*
+	 * If a procedure to enable the CCC is still pending, then cancel it and
+	 * return.
+	 */
+	if (notify_data->att_id) {
+		bt_att_cancel(notify_data->client->att, notify_data->att_id);
+		goto done;
+	}
+
+	if (__sync_sub_and_fetch(&notify_data->chrc->notify_count, 1) ||
+						!notify_data->chrc->ccc_handle)
+		goto done;
+
+	if (notify_data_write_ccc(notify_data, false, disable_ccc_callback))
+		return;
+
+done:
+	notify_data_unref(notify_data);
+}
+
+static void notify_handler(void *data, void *user_data)
+{
+	struct notify_data *notify_data = data;
+	struct pdu_data *pdu_data = user_data;
+	uint16_t value_handle;
+	const uint8_t *value = NULL;
+
+	value_handle = get_le16(pdu_data->pdu);
+
+	if (notify_data->chrc->value_handle != value_handle)
+		return;
+
+	if (pdu_data->length > 2)
+		value = pdu_data->pdu + 2;
+
+	/*
+	 * Even if the notify data has a pending ATT request to write to the
+	 * CCC, there is really no reason not to notify the handlers.
+	 */
+	if (notify_data->notify)
+		notify_data->notify(value_handle, value, pdu_data->length - 2,
+							notify_data->user_data);
+}
+
+static void notify_cb(uint8_t opcode, const void *pdu, uint16_t length,
+								void *user_data)
+{
+	struct bt_gatt_client *client = user_data;
+	struct pdu_data pdu_data;
+
+	bt_gatt_client_ref(client);
+
+	memset(&pdu_data, 0, sizeof(pdu_data));
+	pdu_data.pdu = pdu;
+	pdu_data.length = length;
+
+	queue_foreach(client->notify_list, notify_handler, &pdu_data);
+
+	if (opcode == BT_ATT_OP_HANDLE_VAL_IND)
+		bt_att_send(client->att, BT_ATT_OP_HANDLE_VAL_CONF, NULL, 0,
+							NULL, NULL, NULL);
+
+	bt_gatt_client_unref(client);
+}
+
+static void notify_data_cleanup(void *data)
+{
+	struct notify_data *notify_data = data;
+
+	if (notify_data->att_id)
+		bt_att_cancel(notify_data->client->att, notify_data->att_id);
+
+	notify_data_unref(notify_data);
+}
+
+static void bt_gatt_client_free(struct bt_gatt_client *client)
+{
+	bt_gatt_client_cancel_all(client);
+
+	queue_destroy(client->notify_list, notify_data_cleanup);
+
+	if (client->ready_destroy)
+		client->ready_destroy(client->ready_data);
+
+	if (client->debug_destroy)
+		client->debug_destroy(client->debug_data);
+
+	if (client->att) {
+		bt_att_unregister_disconnect(client->att, client->disc_id);
+		bt_att_unregister(client->att, client->notify_id);
+		bt_att_unregister(client->att, client->ind_id);
+		bt_att_unref(client->att);
+	}
+
+	gatt_db_unref(client->db);
+
+	queue_destroy(client->svc_chngd_queue, free);
+	queue_destroy(client->long_write_queue, request_unref);
+	queue_destroy(client->notify_chrcs, notify_chrc_free);
+	queue_destroy(client->pending_requests, request_unref);
+
+	free(client);
+}
+
+static void att_disconnect_cb(int err, void *user_data)
+{
+	struct bt_gatt_client *client = user_data;
+	bool in_init = client->in_init;
+
+	client->disc_id = 0;
+
+	bt_att_unref(client->att);
+	client->att = NULL;
+
+	client->in_init = false;
+	client->ready = false;
+
+	if (in_init)
+		notify_client_ready(client, false, 0);
+}
+
+struct bt_gatt_client *bt_gatt_client_new(struct gatt_db *db,
+							struct bt_att *att,
+							uint16_t mtu)
+{
+	struct bt_gatt_client *client;
+
+	if (!att || !db)
+		return NULL;
+
+	client = new0(struct bt_gatt_client, 1);
+	if (!client)
+		return NULL;
+
+	client->disc_id = bt_att_register_disconnect(att, att_disconnect_cb,
+								client, NULL);
+	if (!client->disc_id)
+		goto fail;
+
+	client->long_write_queue = queue_new();
+	if (!client->long_write_queue)
+		goto fail;
+
+	client->svc_chngd_queue = queue_new();
+	if (!client->svc_chngd_queue)
+		goto fail;
+
+	client->notify_list = queue_new();
+	if (!client->notify_list)
+		goto fail;
+
+	client->notify_chrcs = queue_new();
+	if (!client->notify_chrcs)
+		goto fail;
+
+	client->pending_requests = queue_new();
+	if (!client->pending_requests)
+		goto fail;
+
+	client->notify_id = bt_att_register(att, BT_ATT_OP_HANDLE_VAL_NOT,
+						notify_cb, client, NULL);
+	if (!client->notify_id)
+		goto fail;
+
+	client->ind_id = bt_att_register(att, BT_ATT_OP_HANDLE_VAL_IND,
+						notify_cb, client, NULL);
+	if (!client->ind_id)
+		goto fail;
+
+	client->att = bt_att_ref(att);
+	client->db = gatt_db_ref(db);
+
+	if (!gatt_client_init(client, mtu))
+		goto fail;
+
+	return bt_gatt_client_ref(client);
+
+fail:
+	bt_gatt_client_free(client);
+	return NULL;
+}
+
+struct bt_gatt_client *bt_gatt_client_ref(struct bt_gatt_client *client)
+{
+	if (!client)
+		return NULL;
+
+	__sync_fetch_and_add(&client->ref_count, 1);
+
+	return client;
+}
+
+void bt_gatt_client_unref(struct bt_gatt_client *client)
+{
+	if (!client)
+		return;
+
+	if (__sync_sub_and_fetch(&client->ref_count, 1))
+		return;
+
+	bt_gatt_client_free(client);
+}
+
+bool bt_gatt_client_is_ready(struct bt_gatt_client *client)
+{
+	return (client && client->ready);
+}
+
+bool bt_gatt_client_set_ready_handler(struct bt_gatt_client *client,
+					bt_gatt_client_callback_t callback,
+					void *user_data,
+					bt_gatt_client_destroy_func_t destroy)
+{
+	if (!client)
+		return false;
+
+	if (client->ready_destroy)
+		client->ready_destroy(client->ready_data);
+
+	client->ready_callback = callback;
+	client->ready_destroy = destroy;
+	client->ready_data = user_data;
+
+	return true;
+}
+
+bool bt_gatt_client_set_service_changed(struct bt_gatt_client *client,
+			bt_gatt_client_service_changed_callback_t callback,
+			void *user_data,
+			bt_gatt_client_destroy_func_t destroy)
+{
+	if (!client)
+		return false;
+
+	if (client->svc_chngd_destroy)
+		client->svc_chngd_destroy(client->svc_chngd_data);
+
+	client->svc_chngd_callback = callback;
+	client->svc_chngd_destroy = destroy;
+	client->svc_chngd_data = user_data;
+
+	return true;
+}
+
+bool bt_gatt_client_set_debug(struct bt_gatt_client *client,
+					bt_gatt_client_debug_func_t callback,
+					void *user_data,
+					bt_gatt_client_destroy_func_t destroy) {
+	if (!client)
+		return false;
+
+	if (client->debug_destroy)
+		client->debug_destroy(client->debug_data);
+
+	client->debug_callback = callback;
+	client->debug_destroy = destroy;
+	client->debug_data = user_data;
+
+	return true;
+}
+
+uint16_t bt_gatt_client_get_mtu(struct bt_gatt_client *client)
+{
+	if (!client || !client->att)
+		return 0;
+
+	return bt_att_get_mtu(client->att);
+}
+
+static bool match_req_id(const void *a, const void *b)
+{
+	const struct request *req = a;
+	unsigned int id = PTR_TO_UINT(b);
+
+	return req->id == id;
+}
+
+static void cancel_long_write_cb(uint8_t opcode, const void *pdu, uint16_t len,
+								void *user_data)
+{
+	struct bt_gatt_client *client = user_data;
+
+	if (queue_isempty(client->long_write_queue))
+		client->in_long_write = false;
+}
+
+static bool cancel_long_write_req(struct bt_gatt_client *client,
+							struct request *req)
+{
+	uint8_t pdu = 0x00;
+
+	/*
+	 * att_id == 0 means that request has been queued and no prepare write
+	 * has been sent so far.Let's just remove if from the queue.
+	 * Otherwise execute write needs to be send.
+	 */
+	if (!req->att_id)
+		return queue_remove(client->long_write_queue, req);
+
+	return !!bt_att_send(client->att, BT_ATT_OP_EXEC_WRITE_REQ, &pdu,
+							sizeof(pdu),
+							cancel_long_write_cb,
+							client, NULL);
+
+}
+
+static void cancel_prep_write_cb(uint8_t opcode, const void *pdu, uint16_t len,
+								void *user_data)
+{
+	struct request *req = user_data;
+	struct bt_gatt_client *client = req->client;
+
+	client->reliable_write_session_id = 0;
+}
+
+static bool cancel_prep_write_session(struct bt_gatt_client *client,
+							struct request *req)
+{
+	uint8_t pdu = 0x00;
+
+	return !!bt_att_send(client->att, BT_ATT_OP_EXEC_WRITE_REQ, &pdu,
+							sizeof(pdu),
+							cancel_prep_write_cb,
+							req, request_unref);
+}
+
+bool bt_gatt_client_cancel(struct bt_gatt_client *client, unsigned int id)
+{
+	struct request *req;
+
+	if (!client || !id || !client->att)
+		return false;
+
+	req = queue_remove_if(client->pending_requests, match_req_id,
+							UINT_TO_PTR(id));
+	if (!req)
+		return false;
+
+	req->removed = true;
+
+	if (!bt_att_cancel(client->att, req->att_id) && !req->long_write &&
+							!req->prep_write)
+		return false;
+
+	/* If this was a long-write, we need to abort all prepared writes */
+	if (req->long_write)
+		return cancel_long_write_req(client, req);
+
+	if (req->prep_write)
+		return cancel_prep_write_session(client, req);
+
+	return true;
+}
+
+static void cancel_request(void *data)
+{
+	struct request *req = data;
+
+	req->removed = true;
+
+	bt_att_cancel(req->client->att, req->att_id);
+
+	if (req->long_write)
+		cancel_long_write_req(req->client, req);
+
+	if (req->prep_write)
+		cancel_prep_write_session(req->client, req);
+}
+
+bool bt_gatt_client_cancel_all(struct bt_gatt_client *client)
+{
+	if (!client || !client->att)
+		return false;
+
+	queue_remove_all(client->pending_requests, NULL, NULL, cancel_request);
+
+	if (client->discovery_req) {
+		bt_gatt_request_cancel(client->discovery_req);
+		bt_gatt_request_unref(client->discovery_req);
+		client->discovery_req = NULL;
+	}
+
+	if (client->mtu_req_id)
+		bt_att_cancel(client->att, client->mtu_req_id);
+
+	return true;
+}
+
+struct read_op {
+	bt_gatt_client_read_callback_t callback;
+	void *user_data;
+	bt_gatt_client_destroy_func_t destroy;
+};
+
+static void destroy_read_op(void *data)
+{
+	struct read_op *op = data;
+
+	if (op->destroy)
+		op->destroy(op->user_data);
+
+	free(op);
+}
+
+static void read_cb(uint8_t opcode, const void *pdu, uint16_t length,
+								void *user_data)
+{
+	struct request *req = user_data;
+	struct read_op *op = req->data;
+	bool success;
+	uint8_t att_ecode = 0;
+	const uint8_t *value = NULL;
+	uint16_t value_len = 0;
+
+	if (opcode == BT_ATT_OP_ERROR_RSP) {
+		success = false;
+		att_ecode = process_error(pdu, length);
+		goto done;
+	}
+
+	if (opcode != BT_ATT_OP_READ_RSP || (!pdu && length)) {
+		success = false;
+		goto done;
+	}
+
+	success = true;
+	value_len = length;
+	if (value_len)
+		value = pdu;
+
+done:
+	if (op->callback)
+		op->callback(success, att_ecode, value, length, op->user_data);
+}
+
+unsigned int bt_gatt_client_read_value(struct bt_gatt_client *client,
+					uint16_t value_handle,
+					bt_gatt_client_read_callback_t callback,
+					void *user_data,
+					bt_gatt_client_destroy_func_t destroy)
+{
+	struct request *req;
+	struct read_op *op;
+	uint8_t pdu[2];
+
+	if (!client)
+		return 0;
+
+	op = new0(struct read_op, 1);
+	if (!op)
+		return 0;
+
+	req = request_create(client);
+	if (!req) {
+		free(op);
+		return 0;
+	}
+
+	op->callback = callback;
+	op->user_data = user_data;
+	op->destroy = destroy;
+
+	req->data = op;
+	req->destroy = destroy_read_op;
+
+	put_le16(value_handle, pdu);
+
+	req->att_id = bt_att_send(client->att, BT_ATT_OP_READ_REQ,
+							pdu, sizeof(pdu),
+							read_cb, req,
+							request_unref);
+	if (!req->att_id) {
+		op->destroy = NULL;
+		request_unref(req);
+		return 0;
+	}
+
+	return req->id;
+}
+
+static void read_multiple_cb(uint8_t opcode, const void *pdu, uint16_t length,
+								void *user_data)
+{
+	struct request *req = user_data;
+	struct read_op *op = req->data;
+	uint8_t att_ecode;
+	bool success;
+
+	if (opcode != BT_ATT_OP_READ_MULT_RSP || (!pdu && length)) {
+		success = false;
+
+		if (opcode == BT_ATT_OP_ERROR_RSP)
+			att_ecode = process_error(pdu, length);
+		else
+			att_ecode = 0;
+
+		pdu = NULL;
+		length = 0;
+	} else {
+		success = true;
+		att_ecode = 0;
+	}
+
+	if (op->callback)
+		op->callback(success, att_ecode, pdu, length, op->user_data);
+}
+
+unsigned int bt_gatt_client_read_multiple(struct bt_gatt_client *client,
+					uint16_t *handles, uint8_t num_handles,
+					bt_gatt_client_read_callback_t callback,
+					void *user_data,
+					bt_gatt_client_destroy_func_t destroy)
+{
+	uint8_t pdu[num_handles * 2];
+	struct request *req;
+	struct read_op *op;
+	int i;
+
+	if (!client)
+		return 0;
+
+	if (num_handles < 2)
+		return 0;
+
+	if (num_handles * 2 > bt_att_get_mtu(client->att) - 1)
+		return 0;
+
+	op = new0(struct read_op, 1);
+	if (!op)
+		return 0;
+
+	req = request_create(client);
+	if (!req) {
+		free(op);
+		return 0;
+	}
+
+	op->callback = callback;
+	op->user_data = user_data;
+	op->destroy = destroy;
+
+	req->data = op;
+	req->destroy = destroy_read_op;
+
+	for (i = 0; i < num_handles; i++)
+		put_le16(handles[i], pdu + (2 * i));
+
+	req->att_id = bt_att_send(client->att, BT_ATT_OP_READ_MULT_REQ,
+							pdu, sizeof(pdu),
+							read_multiple_cb, req,
+							request_unref);
+	if (!req->att_id) {
+		op->destroy = NULL;
+		request_unref(req);
+		return 0;
+	}
+
+	return req->id;
+}
+
+struct read_long_op {
+	struct bt_gatt_client *client;
+	int ref_count;
+	uint16_t value_handle;
+	uint16_t offset;
+	struct iovec iov;
+	bt_gatt_client_read_callback_t callback;
+	void *user_data;
+	bt_gatt_client_destroy_func_t destroy;
+};
+
+static void destroy_read_long_op(void *data)
+{
+	struct read_long_op *op = data;
+
+	if (op->destroy)
+		op->destroy(op->user_data);
+
+	free(op->iov.iov_base);
+	free(op);
+}
+
+static bool append_chunk(struct read_long_op *op, const uint8_t *data,
+								uint16_t len)
+{
+	void *buf;
+
+	/* Truncate if the data would exceed maximum length */
+	if (op->offset + len > BT_ATT_MAX_VALUE_LEN)
+		len = BT_ATT_MAX_VALUE_LEN - op->offset;
+
+	buf = realloc(op->iov.iov_base, op->iov.iov_len + len);
+	if (!buf)
+		return false;
+
+	op->iov.iov_base = buf;
+
+	memcpy(op->iov.iov_base + op->iov.iov_len, data, len);
+
+	op->iov.iov_len += len;
+	op->offset += len;
+
+	return true;
+}
+
+static void read_long_cb(uint8_t opcode, const void *pdu,
+					uint16_t length, void *user_data)
+{
+	struct request *req = user_data;
+	struct read_long_op *op = req->data;
+	bool success;
+	uint8_t att_ecode = 0;
+
+	if (opcode == BT_ATT_OP_ERROR_RSP) {
+		success = false;
+		att_ecode = process_error(pdu, length);
+		goto done;
+	}
+
+	if (opcode != BT_ATT_OP_READ_BLOB_RSP || (!pdu && length)) {
+		success = false;
+		goto done;
+	}
+
+	if (!length)
+		goto success;
+
+	if (!append_chunk(op, pdu, length)) {
+		success = false;
+		goto done;
+	}
+
+	if (op->offset >= BT_ATT_MAX_VALUE_LEN)
+		goto success;
+
+	if (length >= bt_att_get_mtu(op->client->att) - 1) {
+		uint8_t pdu[4];
+
+		put_le16(op->value_handle, pdu);
+		put_le16(op->offset, pdu + 2);
+
+		req->att_id = bt_att_send(op->client->att,
+							BT_ATT_OP_READ_BLOB_REQ,
+							pdu, sizeof(pdu),
+							read_long_cb,
+							request_ref(req),
+							request_unref);
+		if (req->att_id)
+			return;
+
+		request_unref(req);
+		success = false;
+		goto done;
+	}
+
+success:
+	success = true;
+
+done:
+	if (op->callback)
+		op->callback(success, att_ecode, op->iov.iov_base,
+						op->iov.iov_len, op->user_data);
+}
+
+unsigned int bt_gatt_client_read_long_value(struct bt_gatt_client *client,
+					uint16_t value_handle, uint16_t offset,
+					bt_gatt_client_read_callback_t callback,
+					void *user_data,
+					bt_gatt_client_destroy_func_t destroy)
+{
+	struct request *req;
+	struct read_long_op *op;
+	uint8_t pdu[4];
+
+	if (!client)
+		return 0;
+
+	op = new0(struct read_long_op, 1);
+	if (!op)
+		return 0;
+
+	req = request_create(client);
+	if (!req) {
+		free(op);
+		return 0;
+	}
+
+	op->client = client;
+	op->value_handle = value_handle;
+	op->offset = offset;
+	op->callback = callback;
+	op->user_data = user_data;
+	op->destroy = destroy;
+
+	req->data = op;
+	req->destroy = destroy_read_long_op;
+
+	put_le16(value_handle, pdu);
+	put_le16(offset, pdu + 2);
+
+	req->att_id = bt_att_send(client->att, BT_ATT_OP_READ_BLOB_REQ,
+							pdu, sizeof(pdu),
+							read_long_cb, req,
+							request_unref);
+	if (!req->att_id) {
+		op->destroy = NULL;
+		request_unref(req);
+		return 0;
+	}
+
+	return req->id;
+}
+
+unsigned int bt_gatt_client_write_without_response(
+					struct bt_gatt_client *client,
+					uint16_t value_handle,
+					bool signed_write,
+					const uint8_t *value, uint16_t length) {
+	uint8_t pdu[2 + length];
+	struct request *req;
+	int security;
+	uint8_t op;
+
+	if (!client)
+		return 0;
+
+	req = request_create(client);
+	if (!req)
+		return 0;
+
+	/* Only use signed write if unencrypted */
+	if (signed_write) {
+		security = bt_att_get_sec_level(client->att);
+		op = security > BT_SECURITY_LOW ?  BT_ATT_OP_WRITE_CMD :
+						BT_ATT_OP_SIGNED_WRITE_CMD;
+	} else
+		op = BT_ATT_OP_WRITE_CMD;
+
+	put_le16(value_handle, pdu);
+	memcpy(pdu + 2, value, length);
+
+	req->att_id = bt_att_send(client->att, op, pdu, sizeof(pdu), NULL, req,
+								request_unref);
+	if (!req->att_id) {
+		request_unref(req);
+		return 0;
+	}
+
+	return req->id;
+}
+
+struct write_op {
+	struct bt_gatt_client *client;
+	bt_gatt_client_callback_t callback;
+	void *user_data;
+	bt_gatt_destroy_func_t destroy;
+};
+
+static void destroy_write_op(void *data)
+{
+	struct write_op *op = data;
+
+	if (op->destroy)
+		op->destroy(op->user_data);
+
+	free(op);
+}
+
+static void write_cb(uint8_t opcode, const void *pdu, uint16_t length,
+								void *user_data)
+{
+	struct request *req = user_data;
+	struct write_op *op = req->data;
+	bool success = true;
+	uint8_t att_ecode = 0;
+
+	if (opcode == BT_ATT_OP_ERROR_RSP) {
+		success = false;
+		att_ecode = process_error(pdu, length);
+		goto done;
+	}
+
+	if (opcode != BT_ATT_OP_WRITE_RSP || pdu || length)
+		success = false;
+
+done:
+	if (op->callback)
+		op->callback(success, att_ecode, op->user_data);
+}
+
+unsigned int bt_gatt_client_write_value(struct bt_gatt_client *client,
+					uint16_t value_handle,
+					const uint8_t *value, uint16_t length,
+					bt_gatt_client_callback_t callback,
+					void *user_data,
+					bt_gatt_client_destroy_func_t destroy)
+{
+	struct request *req;
+	struct write_op *op;
+	uint8_t pdu[2 + length];
+
+	if (!client)
+		return 0;
+
+	op = new0(struct write_op, 1);
+	if (!op)
+		return 0;
+
+	req = request_create(client);
+	if (!req) {
+		free(op);
+		return 0;
+	}
+
+	op->callback = callback;
+	op->user_data = user_data;
+	op->destroy = destroy;
+
+	req->data = op;
+	req->destroy = destroy_write_op;
+
+	put_le16(value_handle, pdu);
+	memcpy(pdu + 2, value, length);
+
+	req->att_id = bt_att_send(client->att, BT_ATT_OP_WRITE_REQ,
+							pdu, sizeof(pdu),
+							write_cb, req,
+							request_unref);
+	if (!req->att_id) {
+		op->destroy = NULL;
+		request_unref(req);
+		return 0;
+	}
+
+	return req->id;
+}
+
+struct long_write_op {
+	struct bt_gatt_client *client;
+	bool reliable;
+	bool success;
+	uint8_t att_ecode;
+	bool reliable_error;
+	uint16_t value_handle;
+	uint8_t *value;
+	uint16_t length;
+	uint16_t offset;
+	uint16_t index;
+	uint16_t cur_length;
+	bt_gatt_client_write_long_callback_t callback;
+	void *user_data;
+	bt_gatt_client_destroy_func_t destroy;
+};
+
+static void long_write_op_free(void *data)
+{
+	struct long_write_op *op = data;
+
+	if (op->destroy)
+		op->destroy(op->user_data);
+
+	free(op->value);
+	free(op);
+}
+
+static void prepare_write_cb(uint8_t opcode, const void *pdu, uint16_t length,
+							void *user_data);
+static void complete_write_long_op(struct request *req, bool success,
+					uint8_t att_ecode, bool reliable_error);
+
+static void handle_next_prep_write(struct request *req)
+{
+	struct long_write_op *op = req->data;
+	bool success = true;
+	uint8_t *pdu;
+
+	pdu = malloc(op->cur_length + 4);
+	if (!pdu) {
+		success = false;
+		goto done;
+	}
+
+	put_le16(op->value_handle, pdu);
+	put_le16(op->offset + op->index, pdu + 2);
+	memcpy(pdu + 4, op->value + op->index, op->cur_length);
+
+	req->att_id = bt_att_send(op->client->att, BT_ATT_OP_PREP_WRITE_REQ,
+							pdu, op->cur_length + 4,
+							prepare_write_cb,
+							request_ref(req),
+							request_unref);
+	if (!req->att_id) {
+		request_unref(req);
+		success = false;
+	}
+
+	free(pdu);
+
+	/* If so far successful, then the operation should continue.
+	 * Otherwise, there was an error and the procedure should be
+	 * completed.
+	 */
+	if (success)
+		return;
+
+done:
+	complete_write_long_op(req, success, 0, false);
+}
+
+static void start_next_long_write(struct bt_gatt_client *client)
+{
+	struct request *req;
+
+	if (queue_isempty(client->long_write_queue)) {
+		client->in_long_write = false;
+		return;
+	}
+
+	req  = queue_pop_head(client->long_write_queue);
+	if (!req)
+		return;
+
+	handle_next_prep_write(req);
+
+	/*
+	 * send_next_prep_write adds an extra ref. Unref here to clean up if
+	 * necessary, since we also added a ref before pushing to the queue.
+	 */
+	request_unref(req);
+}
+
+static void execute_write_cb(uint8_t opcode, const void *pdu, uint16_t length,
+								void *user_data)
+{
+	struct request *req = user_data;
+	struct long_write_op *op = req->data;
+	bool success = op->success;
+	uint8_t att_ecode = op->att_ecode;
+
+	if (opcode == BT_ATT_OP_ERROR_RSP) {
+		success = false;
+		att_ecode = process_error(pdu, length);
+	} else if (opcode != BT_ATT_OP_EXEC_WRITE_RSP || pdu || length)
+		success = false;
+
+	if (op->callback)
+		op->callback(success, op->reliable_error, att_ecode,
+								op->user_data);
+
+	start_next_long_write(op->client);
+}
+
+static void complete_write_long_op(struct request *req, bool success,
+					uint8_t att_ecode, bool reliable_error)
+{
+	struct long_write_op *op = req->data;
+	uint8_t pdu;
+
+	op->success = success;
+	op->att_ecode = att_ecode;
+	op->reliable_error = reliable_error;
+
+	if (success)
+		pdu = 0x01;  /* Write */
+	else
+		pdu = 0x00;  /* Cancel */
+
+	req->att_id = bt_att_send(op->client->att, BT_ATT_OP_EXEC_WRITE_REQ,
+							&pdu, sizeof(pdu),
+							execute_write_cb,
+							request_ref(req),
+							request_unref);
+	if (req->att_id)
+		return;
+
+
+	request_unref(req);
+	success = false;
+
+	if (op->callback)
+		op->callback(success, reliable_error, att_ecode, op->user_data);
+
+	start_next_long_write(op->client);
+}
+
+static void prepare_write_cb(uint8_t opcode, const void *pdu, uint16_t length,
+								void *user_data)
+{
+	struct request *req = user_data;
+	struct long_write_op *op = req->data;
+	bool success = true;
+	bool reliable_error = false;
+	uint8_t att_ecode = 0;
+	uint16_t next_index;
+
+	if (opcode == BT_ATT_OP_ERROR_RSP) {
+		success = false;
+		att_ecode = process_error(pdu, length);
+		goto done;
+	}
+
+	if (opcode != BT_ATT_OP_PREP_WRITE_RSP) {
+		success = false;
+		goto done;
+	}
+
+	if (op->reliable) {
+		if (!pdu || length != (op->cur_length + 4)) {
+			success = false;
+			reliable_error = true;
+			goto done;
+		}
+
+		if (get_le16(pdu) != op->value_handle ||
+				get_le16(pdu + 2) != (op->offset + op->index)) {
+			success = false;
+			reliable_error = true;
+			goto done;
+		}
+
+		if (memcmp(pdu + 4, op->value + op->index, op->cur_length)) {
+			success = false;
+			reliable_error = true;
+			goto done;
+		}
+	}
+
+	next_index = op->index + op->cur_length;
+	if (next_index == op->length) {
+		/* All bytes written */
+		goto done;
+	}
+
+	/* If the last written length was greater than or equal to what can fit
+	 * inside a PDU, then there is more data to send.
+	 */
+	if (op->cur_length >= bt_att_get_mtu(op->client->att) - 5) {
+		op->index = next_index;
+		op->cur_length = MIN(op->length - op->index,
+					bt_att_get_mtu(op->client->att) - 5);
+		handle_next_prep_write(req);
+		return;
+	}
+
+done:
+	complete_write_long_op(req, success, att_ecode, reliable_error);
+}
+
+unsigned int bt_gatt_client_write_long_value(struct bt_gatt_client *client,
+				bool reliable,
+				uint16_t value_handle, uint16_t offset,
+				const uint8_t *value, uint16_t length,
+				bt_gatt_client_write_long_callback_t callback,
+				void *user_data,
+				bt_gatt_client_destroy_func_t destroy)
+{
+	struct request *req;
+	struct long_write_op *op;
+	uint8_t *pdu;
+
+	if (!client)
+		return 0;
+
+	if ((size_t)(length + offset) > UINT16_MAX)
+		return 0;
+
+	/* Don't allow writing a 0-length value using this procedure. The
+	 * upper-layer should use bt_gatt_write_value for that instead.
+	 */
+	if (!length || !value)
+		return 0;
+
+	op = new0(struct long_write_op, 1);
+	if (!op)
+		return 0;
+
+	op->value = malloc(length);
+	if (!op->value) {
+		free(op);
+		return 0;
+	}
+
+	req = request_create(client);
+	if (!req) {
+		free(op->value);
+		free(op);
+		return 0;
+	}
+
+	memcpy(op->value, value, length);
+
+	op->client = client;
+	op->reliable = reliable;
+	op->value_handle = value_handle;
+	op->length = length;
+	op->offset = offset;
+	op->cur_length = MIN(length, bt_att_get_mtu(client->att) - 5);
+	op->callback = callback;
+	op->user_data = user_data;
+	op->destroy = destroy;
+
+	req->data = op;
+	req->destroy = long_write_op_free;
+	req->long_write = true;
+
+	if (client->in_long_write || client->reliable_write_session_id > 0) {
+		queue_push_tail(client->long_write_queue, req);
+		return req->id;
+	}
+
+	pdu = malloc(op->cur_length + 4);
+	if (!pdu) {
+		free(op->value);
+		free(op);
+		return 0;
+	}
+
+	put_le16(value_handle, pdu);
+	put_le16(offset, pdu + 2);
+	memcpy(pdu + 4, op->value, op->cur_length);
+
+	req->att_id = bt_att_send(client->att, BT_ATT_OP_PREP_WRITE_REQ,
+							pdu, op->cur_length + 4,
+							prepare_write_cb, req,
+							request_unref);
+	free(pdu);
+
+	if (!req->att_id) {
+		op->destroy = NULL;
+		request_unref(req);
+		return 0;
+	}
+
+	client->in_long_write = true;
+
+	return req->id;
+}
+
+struct prep_write_op {
+	bt_gatt_client_write_long_callback_t callback;
+	void *user_data;
+	bt_gatt_destroy_func_t destroy;
+	uint8_t *pdu;
+	uint16_t pdu_len;
+};
+
+static void destroy_prep_write_op(void *data)
+{
+	struct prep_write_op *op = data;
+
+	if (op->destroy)
+		op->destroy(op->user_data);
+
+	free(op->pdu);
+	free(op);
+}
+
+static void prep_write_cb(uint8_t opcode, const void *pdu, uint16_t length,
+								void *user_data)
+{
+	struct request *req = user_data;
+	struct prep_write_op *op = req->data;
+	bool success;
+	uint8_t att_ecode;
+	bool reliable_error;
+
+	if (opcode == BT_ATT_OP_ERROR_RSP) {
+		success = false;
+		reliable_error = false;
+		att_ecode = process_error(pdu, length);
+		goto done;
+	}
+
+	if (opcode != BT_ATT_OP_PREP_WRITE_RSP) {
+		success = false;
+		reliable_error = false;
+		att_ecode = 0;
+		goto done;
+	}
+
+	if (!pdu || length != op->pdu_len ||
+					memcmp(pdu, op->pdu, op->pdu_len)) {
+		success = false;
+		reliable_error = true;
+		att_ecode = 0;
+		goto done;
+	}
+
+	success = true;
+	reliable_error = false;
+	att_ecode = 0;
+
+done:
+	if (op->callback)
+		op->callback(success, reliable_error, att_ecode, op->user_data);
+}
+
+static struct request *get_reliable_request(struct bt_gatt_client *client,
+							unsigned int id)
+{
+	struct request *req;
+	struct prep_write_op *op;
+
+	op = new0(struct prep_write_op, 1);
+	if (!op)
+		return NULL;
+
+	/* Following prepare writes */
+	if (id != 0)
+		req = queue_find(client->pending_requests, match_req_id,
+							UINT_TO_PTR(id));
+	else
+		req = request_create(client);
+
+	if (!req) {
+		free(op);
+		return NULL;
+	}
+
+	req->data = op;
+
+	return req;
+}
+
+unsigned int bt_gatt_client_prepare_write(struct bt_gatt_client *client,
+				unsigned int id, uint16_t value_handle,
+				uint16_t offset, uint8_t *value,
+				uint16_t length,
+				bt_gatt_client_write_long_callback_t callback,
+				void *user_data,
+				bt_gatt_client_destroy_func_t destroy)
+{
+	struct request *req;
+	struct prep_write_op *op;
+	uint8_t pdu[4 + length];
+
+	if (!client)
+		return 0;
+
+	if (client->in_long_write)
+		return 0;
+
+	/*
+	 * Make sure that client who owns reliable session continues with
+	 * prepare writes or this is brand new reliable session (id == 0)
+	 */
+	if (id != client->reliable_write_session_id) {
+		util_debug(client->debug_callback, client->debug_data,
+			"There is other reliable write session ongoing %u",
+			client->reliable_write_session_id);
+
+		return 0;
+	}
+
+	req = get_reliable_request(client, id);
+	if (!req)
+		return 0;
+
+	op = (struct prep_write_op *)req->data;
+
+	op->callback = callback;
+	op->user_data = user_data;
+	op->destroy = destroy;
+
+	req->destroy = destroy_prep_write_op;
+	req->prep_write = true;
+
+	put_le16(value_handle, pdu);
+	put_le16(offset, pdu + 2);
+	memcpy(pdu + 4, value, length);
+
+	/*
+	 * Before sending command we need to remember pdu as we need to validate
+	 * it in the response. Store handle, offset and value. Therefore
+	 * increase length by 4 (handle + offset) as we need it in couple places
+	 * below
+	 */
+	length += 4;
+
+	op->pdu = malloc(length);
+	if (!op->pdu) {
+		op->destroy = NULL;
+		request_unref(req);
+		return 0;
+	}
+
+	memcpy(op->pdu, pdu, length);
+	op->pdu_len = length;
+
+	/*
+	 * Now we are ready to send command
+	 * Note that request_unref will be done on write execute
+	 */
+	req->att_id = bt_att_send(client->att, BT_ATT_OP_PREP_WRITE_REQ, pdu,
+					sizeof(pdu), prep_write_cb, req,
+					NULL);
+	if (!req->att_id) {
+		op->destroy = NULL;
+		request_unref(req);
+		return 0;
+	}
+
+	/*
+	 * Store first request id for prepare write and treat it as a session id
+	 * valid until write execute is done
+	 */
+	if (client->reliable_write_session_id == 0)
+		client->reliable_write_session_id = req->id;
+
+	return client->reliable_write_session_id;
+}
+
+static void exec_write_cb(uint8_t opcode, const void *pdu, uint16_t length,
+								void *user_data)
+{
+	struct request *req = user_data;
+	struct write_op *op = req->data;
+	bool success;
+	uint8_t att_ecode;
+
+	if (opcode == BT_ATT_OP_ERROR_RSP) {
+		success = false;
+		att_ecode = process_error(pdu, length);
+		goto done;
+	}
+
+	if (opcode != BT_ATT_OP_EXEC_WRITE_RSP || pdu || length) {
+		success = false;
+		att_ecode = 0;
+		goto done;
+	}
+
+	success = true;
+	att_ecode = 0;
+
+done:
+	if (op->callback)
+		op->callback(success, att_ecode, op->user_data);
+
+	op->client->reliable_write_session_id = 0;
+
+	start_next_long_write(op->client);
+}
+
+unsigned int bt_gatt_client_write_execute(struct bt_gatt_client *client,
+					unsigned int id,
+					bt_gatt_client_callback_t callback,
+					void *user_data,
+					bt_gatt_client_destroy_func_t destroy)
+{
+	struct request *req;
+	struct write_op *op;
+	uint8_t pdu;
+
+	if (!client)
+		return 0;
+
+	if (client->in_long_write)
+		return 0;
+
+	if (client->reliable_write_session_id != id)
+		return 0;
+
+	op = new0(struct write_op, 1);
+	if (!op)
+		return 0;
+
+	req = queue_find(client->pending_requests, match_req_id,
+							UINT_TO_PTR(id));
+	if (!req) {
+		free(op);
+		return 0;
+	}
+
+	op->client = client;
+	op->callback = callback;
+	op->user_data = user_data;
+	op->destroy = destroy;
+
+	pdu = 0x01;
+
+	req->data = op;
+	req->destroy = destroy_write_op;
+
+	req->att_id = bt_att_send(client->att, BT_ATT_OP_EXEC_WRITE_REQ, &pdu,
+						sizeof(pdu), exec_write_cb,
+						req, request_unref);
+	if (!req->att_id) {
+		op->destroy = NULL;
+		request_unref(req);
+		return 0;
+	}
+
+	return id;
+}
+
+static bool match_notify_chrc_value_handle(const void *a, const void *b)
+{
+	const struct notify_chrc *chrc = a;
+	uint16_t value_handle = PTR_TO_UINT(b);
+
+	return chrc->value_handle == value_handle;
+}
+
+unsigned int bt_gatt_client_register_notify(struct bt_gatt_client *client,
+				uint16_t chrc_value_handle,
+				bt_gatt_client_register_callback_t callback,
+				bt_gatt_client_notify_callback_t notify,
+				void *user_data,
+				bt_gatt_client_destroy_func_t destroy)
+{
+	struct notify_data *notify_data;
+	struct notify_chrc *chrc = NULL;
+
+	if (!client || !client->db || !chrc_value_handle || !callback)
+		return 0;
+
+	if (!bt_gatt_client_is_ready(client) || client->in_svc_chngd)
+		return 0;
+
+	/* Check if a characteristic ref count has been started already */
+	chrc = queue_find(client->notify_chrcs, match_notify_chrc_value_handle,
+						UINT_TO_PTR(chrc_value_handle));
+
+	if (!chrc) {
+		/*
+		 * Create an entry if the characteristic is known and has a CCC
+		 * descriptor.
+		 */
+		chrc = notify_chrc_create(client, chrc_value_handle);
+		if (!chrc)
+			return 0;
+	}
+
+	/* Fail if we've hit the maximum allowed notify sessions */
+	if (chrc->notify_count == INT_MAX)
+		return 0;
+
+	notify_data = new0(struct notify_data, 1);
+	if (!notify_data)
+		return 0;
+
+	notify_data->client = client;
+	notify_data->ref_count = 1;
+	notify_data->chrc = chrc;
+	notify_data->callback = callback;
+	notify_data->notify = notify;
+	notify_data->user_data = user_data;
+	notify_data->destroy = destroy;
+
+	/* Add the handler to the bt_gatt_client's general list */
+	queue_push_tail(client->notify_list, notify_data);
+
+	/* Assign an ID to the handler. */
+	if (client->next_reg_id < 1)
+		client->next_reg_id = 1;
+
+	notify_data->id = client->next_reg_id++;
+
+	/*
+	 * If a write to the CCC descriptor is in progress, then queue this
+	 * request.
+	 */
+	if (chrc->ccc_write_id) {
+		queue_push_tail(chrc->reg_notify_queue, notify_data);
+		return notify_data->id;
+	}
+
+	/*
+	 * If the ref count is not zero, then notifications are already enabled.
+	 */
+	if (chrc->notify_count > 0 || !chrc->ccc_handle) {
+		complete_notify_request(notify_data);
+		return notify_data->id;
+	}
+
+	/* Write to the CCC descriptor */
+	if (!notify_data_write_ccc(notify_data, true, enable_ccc_callback)) {
+		queue_remove(client->notify_list, notify_data);
+		free(notify_data);
+		return 0;
+	}
+
+	return notify_data->id;
+}
+
+bool bt_gatt_client_unregister_notify(struct bt_gatt_client *client,
+							unsigned int id)
+{
+	struct notify_data *notify_data;
+
+	if (!client || !id)
+		return false;
+
+	notify_data = queue_remove_if(client->notify_list, match_notify_data_id,
+							UINT_TO_PTR(id));
+	if (!notify_data)
+		return false;
+
+	assert(notify_data->chrc->notify_count > 0);
+	assert(!notify_data->chrc->ccc_write_id);
+
+	complete_unregister_notify(notify_data);
+	return true;
+}
+
+bool bt_gatt_client_set_sec_level(struct bt_gatt_client *client,
+								int level)
+{
+	if (!client)
+		return false;
+
+	return bt_att_set_sec_level(client->att, level);
+}
+
+int bt_gatt_client_get_sec_level(struct bt_gatt_client *client)
+{
+	if (!client)
+		return -1;
+
+	return bt_att_get_sec_level(client->att);
+}
diff --git a/bluez/gatt-client.h b/bluez/gatt-client.h
new file mode 100644
index 0000000..8e5e0f5
--- /dev/null
+++ b/bluez/gatt-client.h
@@ -0,0 +1,134 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2014  Google Inc.
+ *
+ *
+ *  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
+ *
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <stddef.h>
+
+#define BT_GATT_UUID_SIZE 16
+
+struct bt_gatt_client;
+
+struct bt_gatt_client *bt_gatt_client_new(struct gatt_db *db,
+							struct bt_att *att,
+							uint16_t mtu);
+
+struct bt_gatt_client *bt_gatt_client_ref(struct bt_gatt_client *client);
+void bt_gatt_client_unref(struct bt_gatt_client *client);
+
+typedef void (*bt_gatt_client_destroy_func_t)(void *user_data);
+typedef void (*bt_gatt_client_callback_t)(bool success, uint8_t att_ecode,
+							void *user_data);
+typedef void (*bt_gatt_client_debug_func_t)(const char *str, void *user_data);
+typedef void (*bt_gatt_client_read_callback_t)(bool success, uint8_t att_ecode,
+					const uint8_t *value, uint16_t length,
+					void *user_data);
+typedef void (*bt_gatt_client_write_long_callback_t)(bool success,
+					bool reliable_error, uint8_t att_ecode,
+					void *user_data);
+typedef void (*bt_gatt_client_notify_callback_t)(uint16_t value_handle,
+					const uint8_t *value, uint16_t length,
+					void *user_data);
+typedef void (*bt_gatt_client_register_callback_t)(uint16_t att_ecode,
+							void *user_data);
+typedef void (*bt_gatt_client_service_changed_callback_t)(uint16_t start_handle,
+							uint16_t end_handle,
+							void *user_data);
+
+bool bt_gatt_client_is_ready(struct bt_gatt_client *client);
+bool bt_gatt_client_set_ready_handler(struct bt_gatt_client *client,
+					bt_gatt_client_callback_t callback,
+					void *user_data,
+					bt_gatt_client_destroy_func_t destroy);
+bool bt_gatt_client_set_service_changed(struct bt_gatt_client *client,
+			bt_gatt_client_service_changed_callback_t callback,
+			void *user_data,
+			bt_gatt_client_destroy_func_t destroy);
+bool bt_gatt_client_set_debug(struct bt_gatt_client *client,
+					bt_gatt_client_debug_func_t callback,
+					void *user_data,
+					bt_gatt_client_destroy_func_t destroy);
+
+uint16_t bt_gatt_client_get_mtu(struct bt_gatt_client *client);
+
+bool bt_gatt_client_cancel(struct bt_gatt_client *client, unsigned int id);
+bool bt_gatt_client_cancel_all(struct bt_gatt_client *client);
+
+unsigned int bt_gatt_client_read_value(struct bt_gatt_client *client,
+					uint16_t value_handle,
+					bt_gatt_client_read_callback_t callback,
+					void *user_data,
+					bt_gatt_client_destroy_func_t destroy);
+unsigned int bt_gatt_client_read_long_value(struct bt_gatt_client *client,
+					uint16_t value_handle, uint16_t offset,
+					bt_gatt_client_read_callback_t callback,
+					void *user_data,
+					bt_gatt_client_destroy_func_t destroy);
+unsigned int bt_gatt_client_read_multiple(struct bt_gatt_client *client,
+					uint16_t *handles, uint8_t num_handles,
+					bt_gatt_client_read_callback_t callback,
+					void *user_data,
+					bt_gatt_client_destroy_func_t destroy);
+
+unsigned int bt_gatt_client_write_without_response(
+					struct bt_gatt_client *client,
+					uint16_t value_handle,
+					bool signed_write,
+					const uint8_t *value, uint16_t length);
+unsigned int bt_gatt_client_write_value(struct bt_gatt_client *client,
+					uint16_t value_handle,
+					const uint8_t *value, uint16_t length,
+					bt_gatt_client_callback_t callback,
+					void *user_data,
+					bt_gatt_client_destroy_func_t destroy);
+unsigned int bt_gatt_client_write_long_value(struct bt_gatt_client *client,
+				bool reliable,
+				uint16_t value_handle, uint16_t offset,
+				const uint8_t *value, uint16_t length,
+				bt_gatt_client_write_long_callback_t callback,
+				void *user_data,
+				bt_gatt_client_destroy_func_t destroy);
+unsigned int bt_gatt_client_prepare_write(struct bt_gatt_client *client,
+				unsigned int id,
+				uint16_t value_handle, uint16_t offset,
+				uint8_t *value, uint16_t length,
+				bt_gatt_client_write_long_callback_t callback,
+				void *user_data,
+				bt_gatt_client_destroy_func_t destroy);
+unsigned int bt_gatt_client_write_execute(struct bt_gatt_client *client,
+					unsigned int id,
+					bt_gatt_client_callback_t callback,
+					void *user_data,
+					bt_gatt_client_destroy_func_t destroy);
+
+unsigned int bt_gatt_client_register_notify(struct bt_gatt_client *client,
+				uint16_t chrc_value_handle,
+				bt_gatt_client_register_callback_t callback,
+				bt_gatt_client_notify_callback_t notify,
+				void *user_data,
+				bt_gatt_client_destroy_func_t destroy);
+bool bt_gatt_client_unregister_notify(struct bt_gatt_client *client,
+							unsigned int id);
+
+bool bt_gatt_client_set_sec_level(struct bt_gatt_client *client, int level);
+int bt_gatt_client_get_sec_level(struct bt_gatt_client *client);
diff --git a/bluez/gatt-db.c b/bluez/gatt-db.c
new file mode 100644
index 0000000..29dda55
--- /dev/null
+++ b/bluez/gatt-db.c
@@ -0,0 +1,1636 @@
+/*
+ *
+ *  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
+ *
+ */
+
+#include <stdbool.h>
+#include <errno.h>
+
+#include <bluetooth/bluetooth.h>
+#include "uuid.h"
+#include "util.h"
+#include "queue.h"
+#include "timeout.h"
+#include "att.h"
+#include "gatt-db.h"
+
+#ifndef MAX
+#define MAX(a, b) ((a) > (b) ? (a) : (b))
+#endif
+
+#define MAX_CHAR_DECL_VALUE_LEN 19
+#define MAX_INCLUDED_VALUE_LEN 6
+#define ATTRIBUTE_TIMEOUT 5000
+
+static const bt_uuid_t primary_service_uuid = { .type = BT_UUID16,
+					.value.u16 = GATT_PRIM_SVC_UUID };
+static const bt_uuid_t secondary_service_uuid = { .type = BT_UUID16,
+					.value.u16 = GATT_SND_SVC_UUID };
+static const bt_uuid_t characteristic_uuid = { .type = BT_UUID16,
+					.value.u16 = GATT_CHARAC_UUID };
+static const bt_uuid_t included_service_uuid = { .type = BT_UUID16,
+					.value.u16 = GATT_INCLUDE_UUID };
+
+struct gatt_db {
+	int ref_count;
+	uint16_t next_handle;
+	struct queue *services;
+
+	struct queue *notify_list;
+	unsigned int next_notify_id;
+};
+
+struct notify {
+	unsigned int id;
+	gatt_db_attribute_cb_t service_added;
+	gatt_db_attribute_cb_t service_removed;
+	gatt_db_destroy_func_t destroy;
+	void *user_data;
+};
+
+struct pending_read {
+	struct gatt_db_attribute *attrib;
+	unsigned int id;
+	unsigned int timeout_id;
+	gatt_db_attribute_read_t func;
+	void *user_data;
+};
+
+struct pending_write {
+	struct gatt_db_attribute *attrib;
+	unsigned int id;
+	unsigned int timeout_id;
+	gatt_db_attribute_write_t func;
+	void *user_data;
+};
+
+struct gatt_db_attribute {
+	struct gatt_db_service *service;
+	uint16_t handle;
+	bt_uuid_t uuid;
+	uint32_t permissions;
+	uint16_t value_len;
+	uint8_t *value;
+
+	gatt_db_read_t read_func;
+	gatt_db_write_t write_func;
+	void *user_data;
+
+	unsigned int read_id;
+	struct queue *pending_reads;
+
+	unsigned int write_id;
+	struct queue *pending_writes;
+};
+
+struct gatt_db_service {
+	struct gatt_db *db;
+	bool active;
+	bool claimed;
+	uint16_t num_handles;
+	struct gatt_db_attribute **attributes;
+};
+
+static void pending_read_result(struct pending_read *p, int err,
+					const uint8_t *data, size_t length)
+{
+	if (p->timeout_id > 0)
+		timeout_remove(p->timeout_id);
+
+	p->func(p->attrib, err, data, length, p->user_data);
+
+	free(p);
+}
+
+static void pending_read_free(void *data)
+{
+	struct pending_read *p = data;
+
+	pending_read_result(p, -ECANCELED, NULL, 0);
+}
+
+static void pending_write_result(struct pending_write *p, int err)
+{
+	if (p->timeout_id > 0)
+		timeout_remove(p->timeout_id);
+
+	p->func(p->attrib, err, p->user_data);
+
+	free(p);
+}
+
+static void pending_write_free(void *data)
+{
+	struct pending_write *p = data;
+
+	pending_write_result(p, -ECANCELED);
+}
+
+static void attribute_destroy(struct gatt_db_attribute *attribute)
+{
+	/* Attribute was not initialized by user */
+	if (!attribute)
+		return;
+
+	queue_destroy(attribute->pending_reads, pending_read_free);
+	queue_destroy(attribute->pending_writes, pending_write_free);
+
+	free(attribute->value);
+	free(attribute);
+}
+
+static struct gatt_db_attribute *new_attribute(struct gatt_db_service *service,
+							const bt_uuid_t *type,
+							const uint8_t *val,
+							uint16_t len)
+{
+	struct gatt_db_attribute *attribute;
+
+	attribute = new0(struct gatt_db_attribute, 1);
+	if (!attribute)
+		return NULL;
+
+	attribute->service = service;
+	attribute->uuid = *type;
+	attribute->value_len = len;
+	if (len) {
+		attribute->value = malloc0(len);
+		if (!attribute->value)
+			goto failed;
+
+		memcpy(attribute->value, val, len);
+	}
+
+	attribute->pending_reads = queue_new();
+	if (!attribute->pending_reads)
+		goto failed;
+
+	attribute->pending_writes = queue_new();
+	if (!attribute->pending_reads)
+		goto failed;
+
+	return attribute;
+
+failed:
+	attribute_destroy(attribute);
+	return NULL;
+}
+
+struct gatt_db *gatt_db_ref(struct gatt_db *db)
+{
+	if (!db)
+		return NULL;
+
+	__sync_fetch_and_add(&db->ref_count, 1);
+
+	return db;
+}
+
+struct gatt_db *gatt_db_new(void)
+{
+	struct gatt_db *db;
+
+	db = new0(struct gatt_db, 1);
+	if (!db)
+		return NULL;
+
+	db->services = queue_new();
+	if (!db->services) {
+		free(db);
+		return NULL;
+	}
+
+	db->notify_list = queue_new();
+	if (!db->notify_list) {
+		queue_destroy(db->services, NULL);
+		free(db);
+		return NULL;
+	}
+
+	db->next_handle = 0x0001;
+
+	return gatt_db_ref(db);
+}
+
+static void notify_destroy(void *data)
+{
+	struct notify *notify = data;
+
+	if (notify->destroy)
+		notify->destroy(notify->user_data);
+
+	free(notify);
+}
+
+static bool match_notify_id(const void *a, const void *b)
+{
+	const struct notify *notify = a;
+	unsigned int id = PTR_TO_UINT(b);
+
+	return notify->id == id;
+}
+
+struct notify_data {
+	struct gatt_db_attribute *attr;
+	bool added;
+};
+
+static void handle_notify(void *data, void *user_data)
+{
+	struct notify *notify = data;
+	struct notify_data *notify_data = user_data;
+
+	if (notify_data->added)
+		notify->service_added(notify_data->attr, notify->user_data);
+	else
+		notify->service_removed(notify_data->attr, notify->user_data);
+}
+
+static void notify_service_changed(struct gatt_db *db,
+						struct gatt_db_service *service,
+						bool added)
+{
+	struct notify_data data;
+
+	if (queue_isempty(db->notify_list))
+		return;
+
+	data.attr = service->attributes[0];
+	data.added = added;
+
+	gatt_db_ref(db);
+
+	queue_foreach(db->notify_list, handle_notify, &data);
+
+	gatt_db_unref(db);
+}
+
+static void gatt_db_service_destroy(void *data)
+{
+	struct gatt_db_service *service = data;
+	int i;
+
+	if (service->active)
+		notify_service_changed(service->db, service, false);
+
+	for (i = 0; i < service->num_handles; i++)
+		attribute_destroy(service->attributes[i]);
+
+	free(service->attributes);
+	free(service);
+}
+
+static void gatt_db_destroy(struct gatt_db *db)
+{
+	if (!db)
+		return;
+
+	/*
+	 * Clear the notify list before clearing the services to prevent the
+	 * latter from sending service_removed events.
+	 */
+	queue_destroy(db->notify_list, notify_destroy);
+	db->notify_list = NULL;
+
+	queue_destroy(db->services, gatt_db_service_destroy);
+	free(db);
+}
+
+void gatt_db_unref(struct gatt_db *db)
+{
+	if (!db)
+		return;
+
+	if (__sync_sub_and_fetch(&db->ref_count, 1))
+		return;
+
+	gatt_db_destroy(db);
+}
+
+bool gatt_db_isempty(struct gatt_db *db)
+{
+	if (!db)
+		return true;
+
+	return queue_isempty(db->services);
+}
+
+static int uuid_to_le(const bt_uuid_t *uuid, uint8_t *dst)
+{
+	bt_uuid_t uuid128;
+
+	if (uuid->type == BT_UUID16) {
+		put_le16(uuid->value.u16, dst);
+		return bt_uuid_len(uuid);
+	}
+
+	bt_uuid_to_uuid128(uuid, &uuid128);
+	bswap_128(&uuid128.value.u128, dst);
+	return bt_uuid_len(&uuid128);
+}
+
+static bool le_to_uuid(const uint8_t *src, size_t len, bt_uuid_t *uuid)
+{
+	uint128_t u128;
+
+	if (len == 2) {
+		bt_uuid16_create(uuid, get_le16(src));
+		return true;
+	}
+
+	if (len == 4) {
+		bt_uuid32_create(uuid, get_le32(src));
+		return true;
+	}
+
+	if (len != 16)
+		return false;
+
+	bswap_128(src, &u128);
+	bt_uuid128_create(uuid, u128);
+
+	return true;
+}
+
+static struct gatt_db_service *gatt_db_service_create(const bt_uuid_t *uuid,
+							bool primary,
+							uint16_t num_handles)
+{
+	struct gatt_db_service *service;
+	const bt_uuid_t *type;
+	uint8_t value[16];
+	uint16_t len;
+
+	if (num_handles < 1)
+		return NULL;
+
+	service = new0(struct gatt_db_service, 1);
+	if (!service)
+		return NULL;
+
+	service->attributes = new0(struct gatt_db_attribute *, num_handles);
+	if (!service->attributes) {
+		free(service);
+		return NULL;
+	}
+
+	if (primary)
+		type = &primary_service_uuid;
+	else
+		type = &secondary_service_uuid;
+
+	len = uuid_to_le(uuid, value);
+
+	service->attributes[0] = new_attribute(service, type, value, len);
+	if (!service->attributes[0]) {
+		gatt_db_service_destroy(service);
+		return NULL;
+	}
+
+	return service;
+}
+
+
+bool gatt_db_remove_service(struct gatt_db *db,
+					struct gatt_db_attribute *attrib)
+{
+	struct gatt_db_service *service;
+
+	if (!db || !attrib)
+		return false;
+
+	service = attrib->service;
+
+	queue_remove(db->services, service);
+
+	gatt_db_service_destroy(service);
+
+	return true;
+}
+
+bool gatt_db_clear(struct gatt_db *db)
+{
+	if (!db)
+		return false;
+
+	queue_remove_all(db->services, NULL, NULL, gatt_db_service_destroy);
+
+	db->next_handle = 0;
+
+	return true;
+}
+
+static void gatt_db_service_get_handles(const struct gatt_db_service *service,
+							uint16_t *start_handle,
+							uint16_t *end_handle)
+{
+	if (start_handle)
+		*start_handle = service->attributes[0]->handle;
+
+	if (end_handle)
+		*end_handle = service->attributes[0]->handle +
+						service->num_handles - 1;
+}
+
+struct clear_range {
+	uint16_t start, end;
+};
+
+static bool match_range(const void *a, const void *b)
+{
+	const struct gatt_db_service *service = a;
+	const struct clear_range *range = b;
+	uint16_t svc_start, svc_end;
+
+	gatt_db_service_get_handles(service, &svc_start, &svc_end);
+
+	return svc_start <= range->end && svc_end >= range->start;
+}
+
+bool gatt_db_clear_range(struct gatt_db *db, uint16_t start_handle,
+							uint16_t end_handle)
+{
+	struct clear_range range;
+
+	if (!db || start_handle > end_handle)
+		return false;
+
+	range.start = start_handle;
+	range.end = end_handle;
+
+	queue_remove_all(db->services, match_range, &range,
+						gatt_db_service_destroy);
+
+	return true;
+}
+
+static bool find_insert_loc(struct gatt_db *db, uint16_t start, uint16_t end,
+						struct gatt_db_service **after)
+{
+	const struct queue_entry *services_entry;
+	struct gatt_db_service *service;
+	uint16_t cur_start, cur_end;
+
+	*after = NULL;
+
+	services_entry = queue_get_entries(db->services);
+
+	while (services_entry) {
+		service = services_entry->data;
+
+		gatt_db_service_get_handles(service, &cur_start, &cur_end);
+
+		if (start >= cur_start && start <= cur_end)
+			return false;
+
+		if (end >= cur_start && end <= cur_end)
+			return false;
+
+		if (end < cur_start)
+			return true;
+
+		*after = service;
+		services_entry = services_entry->next;
+	}
+
+	return true;
+}
+
+struct gatt_db_attribute *gatt_db_insert_service(struct gatt_db *db,
+							uint16_t handle,
+							const bt_uuid_t *uuid,
+							bool primary,
+							uint16_t num_handles)
+{
+	struct gatt_db_service *service, *after;
+
+	after = NULL;
+
+	if (!db || handle < 1)
+		return NULL;
+
+	if (num_handles < 1 || (handle + num_handles - 1) > UINT16_MAX)
+		return NULL;
+
+	if (!find_insert_loc(db, handle, handle + num_handles - 1, &after))
+		return NULL;
+
+	service = gatt_db_service_create(uuid, primary, num_handles);
+
+	if (!service)
+		return NULL;
+
+	if (after) {
+		if (!queue_push_after(db->services, after, service))
+			goto fail;
+	} else if (!queue_push_head(db->services, service)) {
+		goto fail;
+	}
+
+	service->db = db;
+	service->attributes[0]->handle = handle;
+	service->num_handles = num_handles;
+
+	/* Fast-forward next_handle if the new service was added to the end */
+	db->next_handle = MAX(handle + num_handles, db->next_handle);
+
+	return service->attributes[0];
+
+fail:
+	gatt_db_service_destroy(service);
+	return NULL;
+}
+
+struct gatt_db_attribute *gatt_db_add_service(struct gatt_db *db,
+						const bt_uuid_t *uuid,
+						bool primary,
+						uint16_t num_handles)
+{
+	return gatt_db_insert_service(db, db->next_handle, uuid, primary,
+								num_handles);
+}
+
+unsigned int gatt_db_register(struct gatt_db *db,
+					gatt_db_attribute_cb_t service_added,
+					gatt_db_attribute_cb_t service_removed,
+					void *user_data,
+					gatt_db_destroy_func_t destroy)
+{
+	struct notify *notify;
+
+	if (!db || !(service_added || service_removed))
+		return 0;
+
+	notify = new0(struct notify, 1);
+	if (!notify)
+		return 0;
+
+	notify->service_added = service_added;
+	notify->service_removed = service_removed;
+	notify->destroy = destroy;
+	notify->user_data = user_data;
+
+	if (db->next_notify_id < 1)
+		db->next_notify_id = 1;
+
+	notify->id = db->next_notify_id++;
+
+	if (!queue_push_tail(db->notify_list, notify)) {
+		free(notify);
+		return 0;
+	}
+
+	return notify->id;
+}
+
+bool gatt_db_unregister(struct gatt_db *db, unsigned int id)
+{
+	struct notify *notify;
+
+	if (!db || !id)
+		return false;
+
+	notify = queue_find(db->notify_list, match_notify_id, UINT_TO_PTR(id));
+	if (!notify)
+		return false;
+
+	queue_remove(db->notify_list, notify);
+	notify_destroy(notify);
+
+	return true;
+}
+
+static uint16_t get_attribute_index(struct gatt_db_service *service,
+							int end_offset)
+{
+	int i = 0;
+
+	/* Here we look for first free attribute index with given offset */
+	while (i < (service->num_handles - end_offset) &&
+						service->attributes[i])
+		i++;
+
+	return i == (service->num_handles - end_offset) ? 0 : i;
+}
+
+static uint16_t get_handle_at_index(struct gatt_db_service *service,
+								int index)
+{
+	return service->attributes[index]->handle;
+}
+
+static struct gatt_db_attribute *
+attribute_update(struct gatt_db_service *service, int index)
+{
+	uint16_t previous_handle;
+
+	/* We call this function with index > 0, because index 0 is reserved
+	 * for service declaration, and is set in add_service()
+	 */
+	previous_handle = service->attributes[index - 1]->handle;
+	service->attributes[index]->handle = previous_handle + 1;
+
+	return service->attributes[index];
+}
+
+static void set_attribute_data(struct gatt_db_attribute *attribute,
+						gatt_db_read_t read_func,
+						gatt_db_write_t write_func,
+						uint32_t permissions,
+						void *user_data)
+{
+	attribute->permissions = permissions;
+	attribute->read_func = read_func;
+	attribute->write_func = write_func;
+	attribute->user_data = user_data;
+}
+
+struct gatt_db_attribute *
+gatt_db_service_add_characteristic(struct gatt_db_attribute *attrib,
+					const bt_uuid_t *uuid,
+					uint32_t permissions,
+					uint8_t properties,
+					gatt_db_read_t read_func,
+					gatt_db_write_t write_func,
+					void *user_data)
+{
+	struct gatt_db_service *service;
+	uint8_t value[MAX_CHAR_DECL_VALUE_LEN];
+	uint16_t len = 0;
+	int i;
+
+	if (!attrib)
+		return NULL;
+
+	service = attrib->service;
+
+	i = get_attribute_index(service, 1);
+	if (!i)
+		return NULL;
+
+	value[0] = properties;
+	len += sizeof(properties);
+	/* We set handle of characteristic value, which will be added next */
+	put_le16(get_handle_at_index(service, i - 1) + 2, &value[1]);
+	len += sizeof(uint16_t);
+	len += uuid_to_le(uuid, &value[3]);
+
+	service->attributes[i] = new_attribute(service, &characteristic_uuid,
+								value, len);
+	if (!service->attributes[i])
+		return NULL;
+
+	attribute_update(service, i++);
+
+	service->attributes[i] = new_attribute(service, uuid, NULL, 0);
+	if (!service->attributes[i]) {
+		free(service->attributes[i - 1]);
+		return NULL;
+	}
+
+	set_attribute_data(service->attributes[i], read_func, write_func,
+							permissions, user_data);
+
+	return attribute_update(service, i);
+}
+
+struct gatt_db_attribute *
+gatt_db_service_add_descriptor(struct gatt_db_attribute *attrib,
+					const bt_uuid_t *uuid,
+					uint32_t permissions,
+					gatt_db_read_t read_func,
+					gatt_db_write_t write_func,
+					void *user_data)
+{
+	struct gatt_db_service *service;
+	int i;
+
+	if (!attrib)
+		return NULL;
+
+	service = attrib->service;
+
+	i = get_attribute_index(service, 0);
+	if (!i)
+		return NULL;
+
+	service->attributes[i] = new_attribute(service, uuid, NULL, 0);
+	if (!service->attributes[i])
+		return NULL;
+
+	set_attribute_data(service->attributes[i], read_func, write_func,
+							permissions, user_data);
+
+	return attribute_update(service, i);
+}
+
+struct gatt_db_attribute *
+gatt_db_service_add_included(struct gatt_db_attribute *attrib,
+					struct gatt_db_attribute *include)
+{
+	struct gatt_db_service *service, *included;
+	uint8_t value[MAX_INCLUDED_VALUE_LEN];
+	uint16_t included_handle, len = 0;
+	int index;
+
+	if (!attrib || !include)
+		return NULL;
+
+	service = attrib->service;
+	included = include->service;
+
+	/* Adjust include to point to the first attribute */
+	if (include != included->attributes[0])
+		include = included->attributes[0];
+
+	included_handle = include->handle;
+
+	put_le16(included_handle, &value[len]);
+	len += sizeof(uint16_t);
+
+	put_le16(included_handle + included->num_handles - 1, &value[len]);
+	len += sizeof(uint16_t);
+
+	/* The Service UUID shall only be present when the UUID is a 16-bit
+	 * Bluetooth UUID. Vol 2. Part G. 3.2
+	 */
+	if (include->value_len == sizeof(uint16_t)) {
+		memcpy(&value[len], include->value, include->value_len);
+		len += include->value_len;
+	}
+
+	index = get_attribute_index(service, 0);
+	if (!index)
+		return NULL;
+
+	service->attributes[index] = new_attribute(service,
+							&included_service_uuid,
+							value, len);
+	if (!service->attributes[index])
+		return NULL;
+
+	/* The Attribute Permissions shall be read only and not require
+	 * authentication or authorization. Vol 2. Part G. 3.2
+	 *
+	 * TODO handle permissions
+	 */
+	set_attribute_data(service->attributes[index], NULL, NULL, 0, NULL);
+
+	return attribute_update(service, index);
+}
+
+bool gatt_db_service_set_active(struct gatt_db_attribute *attrib, bool active)
+{
+	struct gatt_db_service *service;
+
+	if (!attrib)
+		return false;
+
+	service = attrib->service;
+
+	if (service->active == active)
+		return true;
+
+	service->active = active;
+
+	notify_service_changed(service->db, service, active);
+
+	return true;
+}
+
+bool gatt_db_service_get_active(struct gatt_db_attribute *attrib)
+{
+	if (!attrib)
+		return false;
+
+	return attrib->service->active;
+}
+
+bool gatt_db_service_set_claimed(struct gatt_db_attribute *attrib,
+								bool claimed)
+{
+	if (!attrib)
+		return false;
+
+	attrib->service->claimed = claimed;
+
+	return true;
+}
+
+bool gatt_db_service_get_claimed(struct gatt_db_attribute *attrib)
+{
+	if (!attrib)
+		return false;
+
+	return attrib->service->claimed;
+}
+
+void gatt_db_read_by_group_type(struct gatt_db *db, uint16_t start_handle,
+							uint16_t end_handle,
+							const bt_uuid_t type,
+							struct queue *queue)
+{
+	const struct queue_entry *services_entry;
+	struct gatt_db_service *service;
+	uint16_t grp_start, grp_end, uuid_size;
+
+	uuid_size = 0;
+
+	services_entry = queue_get_entries(db->services);
+
+	while (services_entry) {
+		service = services_entry->data;
+
+		if (!service->active)
+			goto next_service;
+
+		if (bt_uuid_cmp(&type, &service->attributes[0]->uuid))
+			goto next_service;
+
+		grp_start = service->attributes[0]->handle;
+		grp_end = grp_start + service->num_handles - 1;
+
+		if (grp_end < start_handle || grp_start > end_handle)
+			goto next_service;
+
+		if (grp_start < start_handle || grp_start > end_handle)
+			goto next_service;
+
+		if (!uuid_size)
+			uuid_size = service->attributes[0]->value_len;
+		else if (uuid_size != service->attributes[0]->value_len)
+			return;
+
+		queue_push_tail(queue, service->attributes[0]);
+
+next_service:
+		services_entry = services_entry->next;
+	}
+}
+
+struct find_by_type_value_data {
+	bt_uuid_t uuid;
+	uint16_t start_handle;
+	uint16_t end_handle;
+	gatt_db_attribute_cb_t func;
+	void *user_data;
+	const void *value;
+	size_t value_len;
+	unsigned int num_of_res;
+};
+
+static void find_by_type(void *data, void *user_data)
+{
+	struct find_by_type_value_data *search_data = user_data;
+	struct gatt_db_service *service = data;
+	struct gatt_db_attribute *attribute;
+	int i;
+
+	if (!service->active)
+		return;
+
+	for (i = 0; i < service->num_handles; i++) {
+		attribute = service->attributes[i];
+
+		if (!attribute)
+			continue;
+
+		if ((attribute->handle < search_data->start_handle) ||
+				(attribute->handle > search_data->end_handle))
+			continue;
+
+		if (bt_uuid_cmp(&search_data->uuid, &attribute->uuid))
+			continue;
+
+		/* TODO: fix for read-callback based attributes */
+		if (search_data->value && memcmp(attribute->value,
+							search_data->value,
+							search_data->value_len))
+			continue;
+
+		search_data->num_of_res++;
+		search_data->func(attribute, search_data->user_data);
+	}
+}
+
+unsigned int gatt_db_find_by_type(struct gatt_db *db, uint16_t start_handle,
+						uint16_t end_handle,
+						const bt_uuid_t *type,
+						gatt_db_attribute_cb_t func,
+						void *user_data)
+{
+	struct find_by_type_value_data data;
+
+	memset(&data, 0, sizeof(data));
+
+	data.uuid = *type;
+	data.start_handle = start_handle;
+	data.end_handle = end_handle;
+	data.func = func;
+	data.user_data = user_data;
+
+	queue_foreach(db->services, find_by_type, &data);
+
+	return data.num_of_res;
+}
+
+unsigned int gatt_db_find_by_type_value(struct gatt_db *db,
+						uint16_t start_handle,
+						uint16_t end_handle,
+						const bt_uuid_t *type,
+						const void *value,
+						size_t value_len,
+						gatt_db_attribute_cb_t func,
+						void *user_data)
+{
+	struct find_by_type_value_data data;
+
+	data.uuid = *type;
+	data.start_handle = start_handle;
+	data.end_handle = end_handle;
+	data.func = func;
+	data.user_data = user_data;
+	data.value = value;
+	data.value_len = value_len;
+
+	queue_foreach(db->services, find_by_type, &data);
+
+	return data.num_of_res;
+}
+
+struct read_by_type_data {
+	struct queue *queue;
+	bt_uuid_t uuid;
+	uint16_t start_handle;
+	uint16_t end_handle;
+};
+
+static void read_by_type(void *data, void *user_data)
+{
+	struct read_by_type_data *search_data = user_data;
+	struct gatt_db_service *service = data;
+	struct gatt_db_attribute *attribute;
+	int i;
+
+	if (!service->active)
+		return;
+
+	for (i = 0; i < service->num_handles; i++) {
+		attribute = service->attributes[i];
+		if (!attribute)
+			continue;
+
+		if (attribute->handle < search_data->start_handle)
+			continue;
+
+		if (attribute->handle > search_data->end_handle)
+			return;
+
+		if (bt_uuid_cmp(&search_data->uuid, &attribute->uuid))
+			continue;
+
+		queue_push_tail(search_data->queue, attribute);
+	}
+}
+
+void gatt_db_read_by_type(struct gatt_db *db, uint16_t start_handle,
+						uint16_t end_handle,
+						const bt_uuid_t type,
+						struct queue *queue)
+{
+	struct read_by_type_data data;
+	data.uuid = type;
+	data.start_handle = start_handle;
+	data.end_handle = end_handle;
+	data.queue = queue;
+
+	queue_foreach(db->services, read_by_type, &data);
+}
+
+
+struct find_information_data {
+	struct queue *queue;
+	uint16_t start_handle;
+	uint16_t end_handle;
+};
+
+static void find_information(void *data, void *user_data)
+{
+	struct find_information_data *search_data = user_data;
+	struct gatt_db_service *service = data;
+	struct gatt_db_attribute *attribute;
+	int i;
+
+	if (!service->active)
+		return;
+
+	/* Check if service is in range */
+	if ((service->attributes[0]->handle + service->num_handles - 1) <
+						search_data->start_handle)
+		return;
+
+	for (i = 0; i < service->num_handles; i++) {
+		attribute = service->attributes[i];
+		if (!attribute)
+			continue;
+
+		if (attribute->handle < search_data->start_handle)
+			continue;
+
+		if (attribute->handle > search_data->end_handle)
+			return;
+
+		queue_push_tail(search_data->queue, attribute);
+	}
+}
+
+void gatt_db_find_information(struct gatt_db *db, uint16_t start_handle,
+							uint16_t end_handle,
+							struct queue *queue)
+{
+	struct find_information_data data;
+
+	data.start_handle = start_handle;
+	data.end_handle = end_handle;
+	data.queue = queue;
+
+	queue_foreach(db->services, find_information, &data);
+}
+
+void gatt_db_foreach_service(struct gatt_db *db, const bt_uuid_t *uuid,
+						gatt_db_attribute_cb_t func,
+						void *user_data)
+{
+	gatt_db_foreach_service_in_range(db, uuid, func, user_data, 0x0001,
+									0xffff);
+}
+
+struct foreach_data {
+	gatt_db_attribute_cb_t func;
+	const bt_uuid_t *uuid;
+	void *user_data;
+	uint16_t start, end;
+};
+
+static void foreach_service_in_range(void *data, void *user_data)
+{
+	struct gatt_db_service *service = data;
+	struct foreach_data *foreach_data = user_data;
+	uint16_t svc_start;
+	bt_uuid_t uuid;
+
+	svc_start = get_handle_at_index(service, 0);
+
+	if (svc_start > foreach_data->end || svc_start < foreach_data->start)
+		return;
+
+	if (foreach_data->uuid) {
+		gatt_db_attribute_get_service_uuid(service->attributes[0],
+									&uuid);
+		if (bt_uuid_cmp(&uuid, foreach_data->uuid))
+			return;
+	}
+
+	foreach_data->func(service->attributes[0], foreach_data->user_data);
+}
+
+void gatt_db_foreach_service_in_range(struct gatt_db *db,
+						const bt_uuid_t *uuid,
+						gatt_db_attribute_cb_t func,
+						void *user_data,
+						uint16_t start_handle,
+						uint16_t end_handle)
+{
+	struct foreach_data data;
+
+	if (!db || !func || start_handle > end_handle)
+		return;
+
+	data.func = func;
+	data.uuid = uuid;
+	data.user_data = user_data;
+	data.start = start_handle;
+	data.end = end_handle;
+
+	queue_foreach(db->services, foreach_service_in_range, &data);
+}
+
+void gatt_db_service_foreach(struct gatt_db_attribute *attrib,
+						const bt_uuid_t *uuid,
+						gatt_db_attribute_cb_t func,
+						void *user_data)
+{
+	struct gatt_db_service *service;
+	struct gatt_db_attribute *attr;
+	uint16_t i;
+
+	if (!attrib || !func)
+		return;
+
+	service = attrib->service;
+
+	for (i = 0; i < service->num_handles; i++) {
+		attr = service->attributes[i];
+		if (!attr)
+			continue;
+
+		if (uuid && bt_uuid_cmp(uuid, &attr->uuid))
+			continue;
+
+		func(attr, user_data);
+	}
+}
+
+void gatt_db_service_foreach_char(struct gatt_db_attribute *attrib,
+						gatt_db_attribute_cb_t func,
+						void *user_data)
+{
+	gatt_db_service_foreach(attrib, &characteristic_uuid, func, user_data);
+}
+
+void gatt_db_service_foreach_desc(struct gatt_db_attribute *attrib,
+						gatt_db_attribute_cb_t func,
+						void *user_data)
+{
+	struct gatt_db_service *service;
+	struct gatt_db_attribute *attr;
+	uint16_t i;
+
+	if (!attrib || !func)
+		return;
+
+	/* Return if this attribute is not a characteristic declaration */
+	if (bt_uuid_cmp(&characteristic_uuid, &attrib->uuid))
+		return;
+
+	service = attrib->service;
+
+	/* Start from the attribute following the value handle */
+	i = attrib->handle - service->attributes[0]->handle + 2;
+	for (; i < service->num_handles; i++) {
+		attr = service->attributes[i];
+		if (!attr)
+			continue;
+
+		/* Return if we reached the end of this characteristic */
+		if (!bt_uuid_cmp(&characteristic_uuid, &attr->uuid) ||
+			!bt_uuid_cmp(&included_service_uuid, &attr->uuid))
+			return;
+
+		func(attr, user_data);
+	}
+}
+
+void gatt_db_service_foreach_incl(struct gatt_db_attribute *attrib,
+						gatt_db_attribute_cb_t func,
+						void *user_data)
+{
+	gatt_db_service_foreach(attrib, &included_service_uuid, func,
+								user_data);
+}
+
+static bool find_service_for_handle(const void *data, const void *user_data)
+{
+	const struct gatt_db_service *service = data;
+	uint16_t handle = PTR_TO_UINT(user_data);
+	uint16_t start, end;
+
+	gatt_db_service_get_handles(service, &start, &end);
+
+	return (start <= handle) && (handle <= end);
+}
+
+struct gatt_db_attribute *gatt_db_get_attribute(struct gatt_db *db,
+							uint16_t handle)
+{
+	struct gatt_db_service *service;
+	uint16_t service_handle;
+
+	if (!db || !handle)
+		return NULL;
+
+	service = queue_find(db->services, find_service_for_handle,
+							UINT_TO_PTR(handle));
+	if (!service)
+		return NULL;
+
+	service_handle = service->attributes[0]->handle;
+
+	/*
+	 * We can safely get attribute from attributes array with offset,
+	 * because find_service_for_handle() check if given handle is
+	 * in service range.
+	 */
+	return service->attributes[handle - service_handle];
+}
+
+static bool find_service_with_uuid(const void *data, const void *user_data)
+{
+	const struct gatt_db_service *service = data;
+	const bt_uuid_t *uuid = user_data;
+	bt_uuid_t svc_uuid;
+
+	gatt_db_attribute_get_service_uuid(service->attributes[0], &svc_uuid);
+
+	return bt_uuid_cmp(uuid, &svc_uuid) == 0;
+}
+
+struct gatt_db_attribute *gatt_db_get_service_with_uuid(struct gatt_db *db,
+							const bt_uuid_t *uuid)
+{
+	struct gatt_db_service *service;
+
+	if (!db || !uuid)
+		return NULL;
+
+	service = queue_find(db->services, find_service_with_uuid, uuid);
+	if (!service)
+		return NULL;
+
+	return service->attributes[0];
+}
+
+const bt_uuid_t *gatt_db_attribute_get_type(
+					const struct gatt_db_attribute *attrib)
+{
+	if (!attrib)
+		return NULL;
+
+	return &attrib->uuid;
+}
+
+uint16_t gatt_db_attribute_get_handle(const struct gatt_db_attribute *attrib)
+{
+	if (!attrib)
+		return 0;
+
+	return attrib->handle;
+}
+
+bool gatt_db_attribute_get_service_uuid(const struct gatt_db_attribute *attrib,
+							bt_uuid_t *uuid)
+{
+	struct gatt_db_service *service;
+
+	if (!attrib || !uuid)
+		return false;
+
+	service = attrib->service;
+
+	if (service->attributes[0]->value_len == sizeof(uint16_t)) {
+		uint16_t value;
+
+		value = get_le16(service->attributes[0]->value);
+		bt_uuid16_create(uuid, value);
+
+		return true;
+	}
+
+	if (service->attributes[0]->value_len == sizeof(uint128_t)) {
+		uint128_t value;
+
+		bswap_128(service->attributes[0]->value, &value);
+		bt_uuid128_create(uuid, value);
+
+		return true;
+	}
+
+	return false;
+}
+
+bool gatt_db_attribute_get_service_handles(
+					const struct gatt_db_attribute *attrib,
+					uint16_t *start_handle,
+					uint16_t *end_handle)
+{
+	struct gatt_db_service *service;
+
+	if (!attrib)
+		return false;
+
+	service = attrib->service;
+
+	gatt_db_service_get_handles(service, start_handle, end_handle);
+
+	return true;
+}
+
+bool gatt_db_attribute_get_service_data(const struct gatt_db_attribute *attrib,
+							uint16_t *start_handle,
+							uint16_t *end_handle,
+							bool *primary,
+							bt_uuid_t *uuid)
+{
+	struct gatt_db_service *service;
+	struct gatt_db_attribute *decl;
+
+	if (!attrib)
+		return false;
+
+	service = attrib->service;
+	decl = service->attributes[0];
+
+	gatt_db_service_get_handles(service, start_handle, end_handle);
+
+	if (primary)
+		*primary = bt_uuid_cmp(&decl->uuid, &secondary_service_uuid);
+
+	if (!uuid)
+		return true;
+
+	/*
+	 * The service declaration attribute value is the 16 or 128 bit service
+	 * UUID.
+	 */
+	return le_to_uuid(decl->value, decl->value_len, uuid);
+}
+
+bool gatt_db_attribute_get_char_data(const struct gatt_db_attribute *attrib,
+							uint16_t *handle,
+							uint16_t *value_handle,
+							uint8_t *properties,
+							bt_uuid_t *uuid)
+{
+	if (!attrib)
+		return false;
+
+	if (bt_uuid_cmp(&characteristic_uuid, &attrib->uuid))
+		return false;
+
+	/*
+	 * Characteristic declaration value:
+	 * 1 octet: Characteristic properties
+	 * 2 octets: Characteristic value handle
+	 * 2 or 16 octets: characteristic UUID
+	 */
+	if (!attrib->value || (attrib->value_len != 5 &&
+						attrib->value_len != 19))
+		return false;
+
+	if (handle)
+		*handle = attrib->handle;
+
+	if (properties)
+		*properties = attrib->value[0];
+
+	if (value_handle)
+		*value_handle = get_le16(attrib->value + 1);
+
+	if (!uuid)
+		return true;
+
+	return le_to_uuid(attrib->value + 3, attrib->value_len - 3, uuid);
+}
+
+bool gatt_db_attribute_get_incl_data(const struct gatt_db_attribute *attrib,
+							uint16_t *handle,
+							uint16_t *start_handle,
+							uint16_t *end_handle)
+{
+	if (!attrib)
+		return false;
+
+	if (bt_uuid_cmp(&included_service_uuid, &attrib->uuid))
+		return false;
+
+	/*
+	 * Include definition value:
+	 * 2 octets: start handle of included service
+	 * 2 octets: end handle of included service
+	 * optional 2 octets: 16-bit Bluetooth UUID
+	 */
+	if (!attrib->value || attrib->value_len < 4 || attrib->value_len > 6)
+		return false;
+
+	/*
+	 * We only return the handles since the UUID can be easily obtained
+	 * from the corresponding attribute.
+	 */
+	if (handle)
+		*handle = attrib->handle;
+
+	if (start_handle)
+		*start_handle = get_le16(attrib->value);
+
+	if (end_handle)
+		*end_handle = get_le16(attrib->value + 2);
+
+	return true;
+}
+
+uint32_t
+gatt_db_attribute_get_permissions(const struct gatt_db_attribute *attrib)
+{
+	if (!attrib)
+		return 0;
+
+	return attrib->permissions;
+}
+
+static bool read_timeout(void *user_data)
+{
+	struct pending_read *p = user_data;
+
+	p->timeout_id = 0;
+
+	queue_remove(p->attrib->pending_reads, p);
+
+	pending_read_result(p, -ETIMEDOUT, NULL, 0);
+
+	return false;
+}
+
+bool gatt_db_attribute_read(struct gatt_db_attribute *attrib, uint16_t offset,
+				uint8_t opcode, struct bt_att *att,
+				gatt_db_attribute_read_t func, void *user_data)
+{
+	uint8_t *value;
+
+	if (!attrib || !func)
+		return false;
+
+	if (attrib->read_func) {
+		struct pending_read *p;
+
+		p = new0(struct pending_read, 1);
+		if (!p)
+			return false;
+
+		p->attrib = attrib;
+		p->id = ++attrib->read_id;
+		p->timeout_id = timeout_add(ATTRIBUTE_TIMEOUT, read_timeout,
+								p, NULL);
+		p->func = func;
+		p->user_data = user_data;
+
+		queue_push_tail(attrib->pending_reads, p);
+
+		attrib->read_func(attrib, p->id, offset, opcode, att,
+							attrib->user_data);
+		return true;
+	}
+
+	/* Check boundary if value is stored in the db */
+	if (offset > attrib->value_len) {
+		func(attrib, BT_ATT_ERROR_INVALID_OFFSET, NULL, 0, user_data);
+		return true;
+	}
+
+	/* Guard against invalid access if offset equals to value length */
+	value = offset == attrib->value_len ? NULL : &attrib->value[offset];
+
+	func(attrib, 0, value, attrib->value_len - offset, user_data);
+
+	return true;
+}
+
+static bool find_pending(const void *a, const void *b)
+{
+	const struct pending_read *p = a;
+	unsigned int id = PTR_TO_UINT(b);
+
+	return p->id == id;
+}
+
+bool gatt_db_attribute_read_result(struct gatt_db_attribute *attrib,
+					unsigned int id, int err,
+					const uint8_t *value, size_t length)
+{
+	struct pending_read *p;
+
+	if (!attrib || !id)
+		return false;
+
+	p = queue_remove_if(attrib->pending_reads, find_pending,
+							UINT_TO_PTR(id));
+	if (!p)
+		return false;
+
+	pending_read_result(p, err, value, length);
+
+	return true;
+}
+
+static bool write_timeout(void *user_data)
+{
+	struct pending_write *p = user_data;
+
+	p->timeout_id = 0;
+
+	queue_remove(p->attrib->pending_writes, p);
+
+	pending_write_result(p, -ETIMEDOUT);
+
+	return false;
+}
+
+bool gatt_db_attribute_write(struct gatt_db_attribute *attrib, uint16_t offset,
+					const uint8_t *value, size_t len,
+					uint8_t opcode, struct bt_att *att,
+					gatt_db_attribute_write_t func,
+					void *user_data)
+{
+	if (!attrib || !func)
+		return false;
+
+	if (attrib->write_func) {
+		struct pending_write *p;
+
+		p = new0(struct pending_write, 1);
+		if (!p)
+			return false;
+
+		p->attrib = attrib;
+		p->id = ++attrib->write_id;
+		p->timeout_id = timeout_add(ATTRIBUTE_TIMEOUT, write_timeout,
+								p, NULL);
+		p->func = func;
+		p->user_data = user_data;
+
+		queue_push_tail(attrib->pending_writes, p);
+
+		attrib->write_func(attrib, p->id, offset, value, len, opcode,
+							att, attrib->user_data);
+		return true;
+	}
+
+	/* Nothing to write just skip */
+	if (len == 0)
+		goto done;
+
+	/* For values stored in db allocate on demand */
+	if (!attrib->value || offset >= attrib->value_len ||
+				len > (unsigned) (attrib->value_len - offset)) {
+		void *buf;
+
+		buf = realloc(attrib->value, len + offset);
+		if (!buf)
+			return false;
+
+		attrib->value = buf;
+
+		/* Init data in the first allocation */
+		if (!attrib->value_len)
+			memset(attrib->value, 0, offset);
+
+		attrib->value_len = len + offset;
+	}
+
+	memcpy(&attrib->value[offset], value, len);
+
+done:
+	func(attrib, 0, user_data);
+
+	return true;
+}
+
+bool gatt_db_attribute_write_result(struct gatt_db_attribute *attrib,
+						unsigned int id, int err)
+{
+	struct pending_write *p;
+
+	if (!attrib || !id)
+		return false;
+
+	p = queue_remove_if(attrib->pending_writes, find_pending,
+							UINT_TO_PTR(id));
+	if (!p)
+		return false;
+
+	pending_write_result(p, err);
+
+	return true;
+}
+
+bool gatt_db_attribute_reset(struct gatt_db_attribute *attrib)
+{
+	if (!attrib)
+		return false;
+
+	if (!attrib->value || !attrib->value_len)
+		return true;
+
+	free(attrib->value);
+	attrib->value = NULL;
+	attrib->value_len = 0;
+
+	return true;
+}
diff --git a/bluez/gatt-db.h b/bluez/gatt-db.h
new file mode 100644
index 0000000..74b37bc
--- /dev/null
+++ b/bluez/gatt-db.h
@@ -0,0 +1,219 @@
+/*
+ *
+ *  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
+ *
+ */
+
+struct gatt_db;
+struct gatt_db_attribute;
+
+struct gatt_db *gatt_db_new(void);
+
+struct gatt_db *gatt_db_ref(struct gatt_db *db);
+void gatt_db_unref(struct gatt_db *db);
+
+bool gatt_db_isempty(struct gatt_db *db);
+
+struct gatt_db_attribute *gatt_db_add_service(struct gatt_db *db,
+						const bt_uuid_t *uuid,
+						bool primary,
+						uint16_t num_handles);
+
+bool gatt_db_remove_service(struct gatt_db *db,
+					struct gatt_db_attribute *attrib);
+bool gatt_db_clear(struct gatt_db *db);
+bool gatt_db_clear_range(struct gatt_db *db, uint16_t start_handle,
+							uint16_t end_handle);
+
+struct gatt_db_attribute *gatt_db_insert_service(struct gatt_db *db,
+							uint16_t handle,
+							const bt_uuid_t *uuid,
+							bool primary,
+							uint16_t num_handles);
+
+typedef void (*gatt_db_read_t) (struct gatt_db_attribute *attrib,
+					unsigned int id, uint16_t offset,
+					uint8_t opcode, struct bt_att *att,
+					void *user_data);
+
+typedef void (*gatt_db_write_t) (struct gatt_db_attribute *attrib,
+					unsigned int id, uint16_t offset,
+					const uint8_t *value, size_t len,
+					uint8_t opcode, struct bt_att *att,
+					void *user_data);
+
+struct gatt_db_attribute *
+gatt_db_service_add_characteristic(struct gatt_db_attribute *attrib,
+					const bt_uuid_t *uuid,
+					uint32_t permissions,
+					uint8_t properties,
+					gatt_db_read_t read_func,
+					gatt_db_write_t write_func,
+					void *user_data);
+
+struct gatt_db_attribute *
+gatt_db_service_add_descriptor(struct gatt_db_attribute *attrib,
+					const bt_uuid_t *uuid,
+					uint32_t permissions,
+					gatt_db_read_t read_func,
+					gatt_db_write_t write_func,
+					void *user_data);
+
+struct gatt_db_attribute *
+gatt_db_service_add_included(struct gatt_db_attribute *attrib,
+					struct gatt_db_attribute *include);
+
+bool gatt_db_service_set_active(struct gatt_db_attribute *attrib, bool active);
+bool gatt_db_service_get_active(struct gatt_db_attribute *attrib);
+
+bool gatt_db_service_set_claimed(struct gatt_db_attribute *attrib,
+								bool claimed);
+bool gatt_db_service_get_claimed(struct gatt_db_attribute *attrib);
+
+typedef void (*gatt_db_attribute_cb_t)(struct gatt_db_attribute *attrib,
+							void *user_data);
+
+void gatt_db_read_by_group_type(struct gatt_db *db, uint16_t start_handle,
+							uint16_t end_handle,
+							const bt_uuid_t type,
+							struct queue *queue);
+
+unsigned int gatt_db_find_by_type(struct gatt_db *db, uint16_t start_handle,
+						uint16_t end_handle,
+						const bt_uuid_t *type,
+						gatt_db_attribute_cb_t func,
+						void *user_data);
+
+unsigned int gatt_db_find_by_type_value(struct gatt_db *db,
+						uint16_t start_handle,
+						uint16_t end_handle,
+						const bt_uuid_t *type,
+						const void *value,
+						size_t value_len,
+						gatt_db_attribute_cb_t func,
+						void *user_data);
+
+void gatt_db_read_by_type(struct gatt_db *db, uint16_t start_handle,
+							uint16_t end_handle,
+							const bt_uuid_t type,
+							struct queue *queue);
+
+void gatt_db_find_information(struct gatt_db *db, uint16_t start_handle,
+							uint16_t end_handle,
+							struct queue *queue);
+
+
+void gatt_db_foreach_service(struct gatt_db *db, const bt_uuid_t *uuid,
+						gatt_db_attribute_cb_t func,
+						void *user_data);
+void gatt_db_foreach_service_in_range(struct gatt_db *db,
+						const bt_uuid_t *uuid,
+						gatt_db_attribute_cb_t func,
+						void *user_data,
+						uint16_t start_handle,
+						uint16_t end_handle);
+
+void gatt_db_service_foreach(struct gatt_db_attribute *attrib,
+						const bt_uuid_t *uuid,
+						gatt_db_attribute_cb_t func,
+						void *user_data);
+void gatt_db_service_foreach_char(struct gatt_db_attribute *attrib,
+						gatt_db_attribute_cb_t func,
+						void *user_data);
+void gatt_db_service_foreach_desc(struct gatt_db_attribute *attrib,
+						gatt_db_attribute_cb_t func,
+						void *user_data);
+void gatt_db_service_foreach_incl(struct gatt_db_attribute *attrib,
+						gatt_db_attribute_cb_t func,
+						void *user_data);
+
+typedef void (*gatt_db_destroy_func_t)(void *user_data);
+
+unsigned int gatt_db_register(struct gatt_db *db,
+					gatt_db_attribute_cb_t service_added,
+					gatt_db_attribute_cb_t service_removed,
+					void *user_data,
+					gatt_db_destroy_func_t destroy);
+bool gatt_db_unregister(struct gatt_db *db, unsigned int id);
+
+struct gatt_db_attribute *gatt_db_get_attribute(struct gatt_db *db,
+							uint16_t handle);
+
+struct gatt_db_attribute *gatt_db_get_service_with_uuid(struct gatt_db *db,
+							const bt_uuid_t *uuid);
+
+const bt_uuid_t *gatt_db_attribute_get_type(
+					const struct gatt_db_attribute *attrib);
+
+uint16_t gatt_db_attribute_get_handle(const struct gatt_db_attribute *attrib);
+
+bool gatt_db_attribute_get_service_uuid(const struct gatt_db_attribute *attrib,
+							bt_uuid_t *uuid);
+
+bool gatt_db_attribute_get_service_handles(
+					const struct gatt_db_attribute *attrib,
+					uint16_t *start_handle,
+					uint16_t *end_handle);
+
+bool gatt_db_attribute_get_service_data(const struct gatt_db_attribute *attrib,
+							uint16_t *start_handle,
+							uint16_t *end_handle,
+							bool *primary,
+							bt_uuid_t *uuid);
+
+bool gatt_db_attribute_get_char_data(const struct gatt_db_attribute *attrib,
+							uint16_t *handle,
+							uint16_t *value_handle,
+							uint8_t *properties,
+							bt_uuid_t *uuid);
+
+bool gatt_db_attribute_get_incl_data(const struct gatt_db_attribute *attrib,
+							uint16_t *handle,
+							uint16_t *start_handle,
+							uint16_t *end_handle);
+
+uint32_t
+gatt_db_attribute_get_permissions(const struct gatt_db_attribute *attrib);
+
+typedef void (*gatt_db_attribute_read_t) (struct gatt_db_attribute *attrib,
+						int err, const uint8_t *value,
+						size_t length, void *user_data);
+
+bool gatt_db_attribute_read(struct gatt_db_attribute *attrib, uint16_t offset,
+				uint8_t opcode, struct bt_att *att,
+				gatt_db_attribute_read_t func, void *user_data);
+
+bool gatt_db_attribute_read_result(struct gatt_db_attribute *attrib,
+					unsigned int id, int err,
+					const uint8_t *value, size_t length);
+
+typedef void (*gatt_db_attribute_write_t) (struct gatt_db_attribute *attrib,
+						int err, void *user_data);
+
+bool gatt_db_attribute_write(struct gatt_db_attribute *attrib, uint16_t offset,
+					const uint8_t *value, size_t len,
+					uint8_t opcode, struct bt_att *att,
+					gatt_db_attribute_write_t func,
+					void *user_data);
+
+bool gatt_db_attribute_write_result(struct gatt_db_attribute *attrib,
+						unsigned int id, int err);
+
+bool gatt_db_attribute_reset(struct gatt_db_attribute *attrib);
diff --git a/bluez/gatt-helpers.c b/bluez/gatt-helpers.c
new file mode 100644
index 0000000..b337697
--- /dev/null
+++ b/bluez/gatt-helpers.c
@@ -0,0 +1,1490 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2014  Google Inc.
+ *
+ *
+ *  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
+ *
+ */
+
+
+#include <bluetooth/bluetooth.h>
+
+#include "queue.h"
+#include "att.h"
+#include "uuid.h"
+#include "gatt-helpers.h"
+#include "util.h"
+
+#ifndef MIN
+#define MIN(a, b) ((a) < (b) ? (a) : (b))
+#endif
+
+struct bt_gatt_result {
+	uint8_t opcode;
+	void *pdu;
+	uint16_t pdu_len;
+	uint16_t data_len;
+
+	void *op;  /* Discovery operation data */
+
+	struct bt_gatt_result *next;
+};
+
+static struct bt_gatt_result *result_create(uint8_t opcode, const void *pdu,
+							uint16_t pdu_len,
+							uint16_t data_len,
+							void *op)
+{
+	struct bt_gatt_result *result;
+
+	result = new0(struct bt_gatt_result, 1);
+	if (!result)
+		return NULL;
+
+	result->pdu = malloc(pdu_len);
+	if (!result->pdu) {
+		free(result);
+		return NULL;
+	}
+
+	result->opcode = opcode;
+	result->pdu_len = pdu_len;
+	result->data_len = data_len;
+	result->op = op;
+
+	memcpy(result->pdu, pdu, pdu_len);
+
+	return result;
+}
+
+static void result_destroy(struct bt_gatt_result *result)
+{
+	struct bt_gatt_result *next;
+
+	while (result) {
+		next = result->next;
+
+		free(result->pdu);
+		free(result);
+
+		result = next;
+	}
+}
+
+static unsigned int result_element_count(struct bt_gatt_result *result)
+{
+	unsigned int count = 0;
+	struct bt_gatt_result *cur;
+
+	cur = result;
+
+	while (cur) {
+		count += cur->pdu_len / cur->data_len;
+		cur = cur->next;
+	}
+
+	return count;
+}
+
+unsigned int bt_gatt_result_service_count(struct bt_gatt_result *result)
+{
+	if (!result)
+		return 0;
+
+	if (result->opcode != BT_ATT_OP_READ_BY_GRP_TYPE_RSP &&
+			result->opcode != BT_ATT_OP_FIND_BY_TYPE_VAL_RSP)
+		return 0;
+
+	return result_element_count(result);
+}
+
+unsigned int bt_gatt_result_characteristic_count(struct bt_gatt_result *result)
+{
+	if (!result)
+		return 0;
+
+	if (result->opcode != BT_ATT_OP_READ_BY_TYPE_RSP)
+		return 0;
+
+	/*
+	 * Data length contains 7 or 21 octets:
+	 * 2 octets: Attribute handle
+	 * 1 octet: Characteristic properties
+	 * 2 octets: Characteristic value handle
+	 * 2 or 16 octets: characteristic UUID
+	 */
+	if (result->data_len != 21 && result->data_len != 7)
+		return 0;
+
+	return result_element_count(result);
+}
+
+unsigned int bt_gatt_result_descriptor_count(struct bt_gatt_result *result)
+{
+	if (!result)
+		return 0;
+
+	if (result->opcode != BT_ATT_OP_FIND_INFO_RSP)
+		return 0;
+
+	return result_element_count(result);
+}
+
+unsigned int bt_gatt_result_included_count(struct bt_gatt_result *result)
+{
+	struct bt_gatt_result *cur;
+	unsigned int count = 0;
+
+	if (!result)
+		return 0;
+
+	if (result->opcode != BT_ATT_OP_READ_BY_TYPE_RSP)
+		return 0;
+
+	/*
+	 * Data length can be of length 6 or 8 octets:
+	 * 2 octets - include service handle
+	 * 2 octets - start handle of included service
+	 * 2 octets - end handle of included service
+	 * 2 octets (optionally) - 16 bit Bluetooth UUID
+	 */
+	if (result->data_len != 6 && result->data_len != 8)
+		return 0;
+
+	for (cur = result; cur; cur = cur->next)
+		if (cur->opcode == BT_ATT_OP_READ_BY_TYPE_RSP)
+			count += cur->pdu_len / cur->data_len;
+
+	return count;
+}
+
+bool bt_gatt_iter_init(struct bt_gatt_iter *iter, struct bt_gatt_result *result)
+{
+	if (!iter || !result)
+		return false;
+
+	iter->result = result;
+	iter->pos = 0;
+
+	return true;
+}
+
+static const uint8_t bt_base_uuid[16] = {
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00,
+	0x80, 0x00, 0x00, 0x80, 0x5F, 0x9B, 0x34, 0xFB
+};
+
+static bool convert_uuid_le(const uint8_t *src, size_t len, uint8_t dst[16])
+{
+	if (len == 16) {
+		bswap_128(src, dst);
+		return true;
+	}
+
+	if (len != 2)
+		return false;
+
+	memcpy(dst, bt_base_uuid, sizeof(bt_base_uuid));
+	dst[2] = src[1];
+	dst[3] = src[0];
+
+	return true;
+}
+
+struct bt_gatt_request {
+	struct bt_att *att;
+	unsigned int id;
+	uint16_t end_handle;
+	int ref_count;
+	bt_uuid_t uuid;
+	uint16_t service_type;
+	struct bt_gatt_result *result_head;
+	struct bt_gatt_result *result_tail;
+	bt_gatt_request_callback_t callback;
+	void *user_data;
+	bt_gatt_destroy_func_t destroy;
+};
+
+static struct bt_gatt_result *result_append(uint8_t opcode, const void *pdu,
+						uint16_t pdu_len,
+						uint16_t data_len,
+						struct bt_gatt_request *op)
+{
+	struct bt_gatt_result *result;
+
+	result = result_create(opcode, pdu, pdu_len, data_len, op);
+	if (!result)
+		return NULL;
+
+	if (!op->result_head)
+		op->result_head = op->result_tail = result;
+	else {
+		op->result_tail->next = result;
+		op->result_tail = result;
+	}
+
+	return result;
+}
+
+bool bt_gatt_iter_next_included_service(struct bt_gatt_iter *iter,
+				uint16_t *handle, uint16_t *start_handle,
+				uint16_t *end_handle, uint8_t uuid[16])
+{
+	struct bt_gatt_result *read_result;
+	struct bt_gatt_request *op;
+	const void *pdu_ptr;
+	int i = 0;
+
+	if (!iter || !iter->result || !handle || !start_handle || !end_handle
+								|| !uuid)
+		return false;
+
+
+	if (iter->result->opcode != BT_ATT_OP_READ_BY_TYPE_RSP)
+		return false;
+
+	/* UUID in discovery_op is set in read_by_type and service_discovery */
+	op = iter->result->op;
+	if (op->uuid.type != BT_UUID_UNSPEC)
+		return false;
+	/*
+	 * iter->result points to READ_BY_TYPE_RSP with data length containing:
+	 * 2 octets - include service handle
+	 * 2 octets - start handle of included service
+	 * 2 octets - end handle of included service
+	 * optional 2  octets - Bluetooth UUID
+	 */
+	if (iter->result->data_len != 8 && iter->result->data_len != 6)
+		return false;
+
+	pdu_ptr = iter->result->pdu + iter->pos;
+
+	/* This result contains 16 bit UUID */
+	if (iter->result->data_len == 8) {
+		*handle = get_le16(pdu_ptr);
+		*start_handle = get_le16(pdu_ptr + 2);
+		*end_handle = get_le16(pdu_ptr + 4);
+		convert_uuid_le(pdu_ptr + 6, 2, uuid);
+
+		iter->pos += iter->result->data_len;
+
+		if (iter->pos == iter->result->pdu_len) {
+			iter->result = iter->result->next;
+			iter->pos = 0;
+		}
+
+		return true;
+	}
+
+	*handle = get_le16(pdu_ptr);
+	*start_handle = get_le16(pdu_ptr + 2);
+	*end_handle = get_le16(pdu_ptr + 4);
+	read_result = iter->result;
+
+	/*
+	 * Find READ_RSP with include service UUID.
+	 * If number of current data set in READ_BY_TYPE_RSP is n, then we must
+	 * go to n'th PDU next to current item->result
+	 */
+	for (read_result = read_result->next; read_result; i++) {
+		if (i >= (iter->pos / iter->result->data_len))
+			break;
+
+		read_result = read_result->next;
+	}
+
+	if (!read_result)
+		return false;
+
+	convert_uuid_le(read_result->pdu, read_result->data_len, uuid);
+	iter->pos += iter->result->data_len;
+	if (iter->pos == iter->result->pdu_len) {
+		iter->result = read_result->next;
+		iter->pos = 0;
+	}
+
+	return true;
+}
+
+bool bt_gatt_iter_next_service(struct bt_gatt_iter *iter,
+				uint16_t *start_handle, uint16_t *end_handle,
+				uint8_t uuid[16])
+{
+	struct bt_gatt_request *op;
+	const void *pdu_ptr;
+	bt_uuid_t tmp;
+
+	if (!iter || !iter->result || !start_handle || !end_handle || !uuid)
+		return false;
+
+	op = iter->result->op;
+	pdu_ptr = iter->result->pdu + iter->pos;
+
+	switch (iter->result->opcode) {
+	case BT_ATT_OP_READ_BY_GRP_TYPE_RSP:
+		*start_handle = get_le16(pdu_ptr);
+		*end_handle = get_le16(pdu_ptr + 2);
+		convert_uuid_le(pdu_ptr + 4, iter->result->data_len - 4, uuid);
+		break;
+	case BT_ATT_OP_FIND_BY_TYPE_VAL_RSP:
+		*start_handle = get_le16(pdu_ptr);
+		*end_handle = get_le16(pdu_ptr + 2);
+
+		bt_uuid_to_uuid128(&op->uuid, &tmp);
+		memcpy(uuid, tmp.value.u128.data, 16);
+		break;
+	default:
+		return false;
+	}
+
+
+	iter->pos += iter->result->data_len;
+	if (iter->pos == iter->result->pdu_len) {
+		iter->result = iter->result->next;
+		iter->pos = 0;
+	}
+
+	return true;
+}
+
+bool bt_gatt_iter_next_characteristic(struct bt_gatt_iter *iter,
+				uint16_t *start_handle, uint16_t *end_handle,
+				uint16_t *value_handle, uint8_t *properties,
+				uint8_t uuid[16])
+{
+	struct bt_gatt_request *op;
+	const void *pdu_ptr;
+
+	if (!iter || !iter->result || !start_handle || !end_handle ||
+					!value_handle || !properties || !uuid)
+		return false;
+
+	if (iter->result->opcode != BT_ATT_OP_READ_BY_TYPE_RSP)
+		return false;
+
+	/* UUID in discovery_op is set in read_by_type and service_discovery */
+	op = iter->result->op;
+	if (op->uuid.type != BT_UUID_UNSPEC)
+		return false;
+	/*
+	 * Data length contains 7 or 21 octets:
+	 * 2 octets: Attribute handle
+	 * 1 octet: Characteristic properties
+	 * 2 octets: Characteristic value handle
+	 * 2 or 16 octets: characteristic UUID
+	 */
+	if (iter->result->data_len != 21 && iter->result->data_len != 7)
+		return false;
+
+	pdu_ptr = iter->result->pdu + iter->pos;
+
+	*start_handle = get_le16(pdu_ptr);
+	*properties = ((uint8_t *) pdu_ptr)[2];
+	*value_handle = get_le16(pdu_ptr + 3);
+	convert_uuid_le(pdu_ptr + 5, iter->result->data_len - 5, uuid);
+
+	iter->pos += iter->result->data_len;
+	if (iter->pos == iter->result->pdu_len) {
+		iter->result = iter->result->next;
+		iter->pos = 0;
+	}
+
+	if (!iter->result) {
+		*end_handle = op->end_handle;
+		return true;
+	}
+
+	*end_handle = get_le16(iter->result->pdu + iter->pos) - 1;
+
+	return true;
+}
+
+bool bt_gatt_iter_next_descriptor(struct bt_gatt_iter *iter, uint16_t *handle,
+							uint8_t uuid[16])
+{
+	const void *pdu_ptr;
+
+	if (!iter || !iter->result || !handle || !uuid)
+		return false;
+
+	if (iter->result->opcode != BT_ATT_OP_FIND_INFO_RSP)
+		return false;
+
+	pdu_ptr = iter->result->pdu + iter->pos;
+
+	*handle = get_le16(pdu_ptr);
+	convert_uuid_le(pdu_ptr + 2, iter->result->data_len - 2, uuid);
+
+	iter->pos += iter->result->data_len;
+	if (iter->pos == iter->result->pdu_len) {
+		iter->result = iter->result->next;
+		iter->pos = 0;
+	}
+
+	return true;
+}
+
+bool bt_gatt_iter_next_read_by_type(struct bt_gatt_iter *iter,
+				uint16_t *handle, uint16_t *length,
+				const uint8_t **value)
+{
+	struct bt_gatt_request *op;
+	const void *pdu_ptr;
+
+	if (!iter || !iter->result || !handle || !length || !value)
+		return false;
+
+	if (iter->result->opcode != BT_ATT_OP_READ_BY_TYPE_RSP)
+		return false;
+
+	/*
+	 * Check if UUID is set, otherwise results can contain characteristic
+	 * discovery service or included service discovery results
+	 */
+	op = iter->result->op;
+	if (op->uuid.type == BT_UUID_UNSPEC)
+		return false;
+
+	pdu_ptr = iter->result->pdu + iter->pos;
+
+	*handle = get_le16(pdu_ptr);
+	*length = iter->result->data_len - 2;
+	*value = pdu_ptr + 2;
+
+	iter->pos += iter->result->data_len;
+	if (iter->pos == iter->result->pdu_len) {
+		iter->result = iter->result->next;
+		iter->pos = 0;
+	}
+
+	return true;
+}
+
+struct mtu_op {
+	struct bt_att *att;
+	uint16_t client_rx_mtu;
+	bt_gatt_result_callback_t callback;
+	void *user_data;
+	bt_gatt_destroy_func_t destroy;
+};
+
+static void destroy_mtu_op(void *user_data)
+{
+	struct mtu_op *op = user_data;
+
+	if (op->destroy)
+		op->destroy(op->user_data);
+
+	free(op);
+}
+
+static uint8_t process_error(const void *pdu, uint16_t length)
+{
+	const struct bt_att_pdu_error_rsp *error_pdu;
+
+	if (!pdu || length != sizeof(struct bt_att_pdu_error_rsp))
+		return 0;
+
+	error_pdu = pdu;
+
+	return error_pdu->ecode;
+}
+
+static void mtu_cb(uint8_t opcode, const void *pdu, uint16_t length,
+								void *user_data)
+{
+	struct mtu_op *op = user_data;
+	bool success = true;
+	uint8_t att_ecode = 0;
+	uint16_t server_rx_mtu;
+
+	if (opcode == BT_ATT_OP_ERROR_RSP) {
+		success = false;
+		att_ecode = process_error(pdu, length);
+		goto done;
+	}
+
+	if (opcode != BT_ATT_OP_MTU_RSP || !pdu || length != 2) {
+		success = false;
+		goto done;
+	}
+
+	server_rx_mtu = get_le16(pdu);
+	bt_att_set_mtu(op->att, MIN(op->client_rx_mtu, server_rx_mtu));
+
+done:
+	if (op->callback)
+		op->callback(success, att_ecode, op->user_data);
+}
+
+unsigned int bt_gatt_exchange_mtu(struct bt_att *att, uint16_t client_rx_mtu,
+					bt_gatt_result_callback_t callback,
+					void *user_data,
+					bt_gatt_destroy_func_t destroy)
+{
+	struct mtu_op *op;
+	uint8_t pdu[2];
+	unsigned int id;
+
+	if (!att || !client_rx_mtu)
+		return false;
+
+	op = new0(struct mtu_op, 1);
+	if (!op)
+		return false;
+
+	op->att = att;
+	op->client_rx_mtu = client_rx_mtu;
+	op->callback = callback;
+	op->user_data = user_data;
+	op->destroy = destroy;
+
+	put_le16(client_rx_mtu, pdu);
+
+	id = bt_att_send(att, BT_ATT_OP_MTU_REQ, pdu, sizeof(pdu), mtu_cb, op,
+								destroy_mtu_op);
+	if (!id)
+		free(op);
+
+	return id;
+}
+
+static inline int get_uuid_len(const bt_uuid_t *uuid)
+{
+	if (!uuid)
+		return 0;
+
+	return (uuid->type == BT_UUID16) ? 2 : 16;
+}
+
+struct bt_gatt_request *bt_gatt_request_ref(struct bt_gatt_request *req)
+{
+	if (!req)
+		return NULL;
+
+	__sync_fetch_and_add(&req->ref_count, 1);
+
+	return req;
+}
+
+void bt_gatt_request_unref(struct bt_gatt_request *req)
+{
+	if (!req)
+		return;
+
+	if (__sync_sub_and_fetch(&req->ref_count, 1))
+		return;
+
+	bt_gatt_request_cancel(req);
+
+	if (req->destroy)
+		req->destroy(req->user_data);
+
+	result_destroy(req->result_head);
+
+	free(req);
+}
+
+void bt_gatt_request_cancel(struct bt_gatt_request *req)
+{
+	if (!req)
+		return;
+
+	if (!req->id)
+		return;
+
+	bt_att_cancel(req->att, req->id);
+	req->id = 0;
+}
+
+static void async_req_unref(void *data)
+{
+	struct bt_gatt_request *req = data;
+
+	bt_gatt_request_unref(req);
+}
+
+static void discovery_op_complete(struct bt_gatt_request *op, bool success,
+								uint8_t ecode)
+{
+	if (op->callback)
+		op->callback(success, ecode, success ? op->result_head : NULL,
+								op->user_data);
+
+	if (!op->id)
+		async_req_unref(op);
+	else
+		op->id = 0;
+
+}
+
+static void read_by_grp_type_cb(uint8_t opcode, const void *pdu,
+					uint16_t length, void *user_data)
+{
+	struct bt_gatt_request *op = user_data;
+	bool success;
+	uint8_t att_ecode = 0;
+	struct bt_gatt_result *cur_result;
+	size_t data_length;
+	size_t list_length;
+	uint16_t last_end;
+
+	if (opcode == BT_ATT_OP_ERROR_RSP) {
+		success = false;
+		att_ecode = process_error(pdu, length);
+
+		if (att_ecode == BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND &&
+								op->result_head)
+			goto success;
+
+		goto done;
+	}
+
+	/* PDU must contain at least the following (sans opcode):
+	 * - Attr Data Length (1 octet)
+	 * - Attr Data List (at least 6 octets):
+	 *   -- 2 octets: Attribute handle
+	 *   -- 2 octets: End group handle
+	 *   -- 2 or 16 octets: service UUID
+	 */
+	if (opcode != BT_ATT_OP_READ_BY_GRP_TYPE_RSP || !pdu || length < 7) {
+		success = false;
+		goto done;
+	}
+
+	data_length = ((uint8_t *) pdu)[0];
+	list_length = length - 1;
+
+	if ((data_length != 6 && data_length != 20) ||
+					(list_length % data_length)) {
+		success = false;
+		goto done;
+	}
+
+	/* PDU is correctly formatted. Get the last end handle to process the
+	 * next request and store the PDU.
+	 */
+	cur_result = result_append(opcode, pdu + 1, list_length, data_length,
+									op);
+	if (!cur_result) {
+		success = false;
+		goto done;
+	}
+
+	last_end = get_le16(pdu + length - data_length + 2);
+	if (last_end < op->end_handle) {
+		uint8_t pdu[6];
+
+		put_le16(last_end + 1, pdu);
+		put_le16(op->end_handle, pdu + 2);
+		put_le16(op->service_type, pdu + 4);
+
+		op->id = bt_att_send(op->att, BT_ATT_OP_READ_BY_GRP_TYPE_REQ,
+						pdu, sizeof(pdu),
+						read_by_grp_type_cb,
+						bt_gatt_request_ref(op),
+						async_req_unref);
+		if (op->id)
+			return;
+
+		success = false;
+		goto done;
+	}
+
+	/* Some devices incorrectly return 0xffff as the end group handle when
+	 * the read-by-group-type request is performed within a smaller range.
+	 * Manually set the end group handle that we report in the result to the
+	 * end handle in the original request.
+	 */
+	if (last_end == 0xffff && last_end != op->end_handle)
+		put_le16(op->end_handle,
+				cur_result->pdu + length - data_length + 1);
+
+success:
+	success = true;
+
+done:
+	discovery_op_complete(op, success, att_ecode);
+}
+
+static void find_by_type_val_cb(uint8_t opcode, const void *pdu,
+					uint16_t length, void *user_data)
+{
+	struct bt_gatt_request *op = user_data;
+	bool success;
+	uint8_t att_ecode = 0;
+	uint16_t last_end;
+
+	if (opcode == BT_ATT_OP_ERROR_RSP) {
+		success = false;
+		att_ecode = process_error(pdu, length);
+
+		if (att_ecode == BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND &&
+								op->result_head)
+			goto success;
+
+		goto done;
+	}
+
+	/* PDU must contain 4 bytes and it must be a multiple of 4, where each
+	 * 4 bytes contain the 16-bit attribute and group end handles.
+	 */
+	if (opcode != BT_ATT_OP_FIND_BY_TYPE_VAL_RSP || !pdu || !length ||
+								length % 4) {
+		success = false;
+		goto done;
+	}
+
+	if (!result_append(opcode, pdu, length, 4, op)) {
+		success = false;
+		goto done;
+	}
+
+	/*
+	 * Each data set contains:
+	 * 2 octets with start handle
+	 * 2 octets with end handle
+	 * last_end is end handle of last data set
+	 */
+	last_end = get_le16(pdu + length - 2);
+	if (last_end < op->end_handle) {
+		uint8_t pdu[6 + get_uuid_len(&op->uuid)];
+
+		put_le16(last_end + 1, pdu);
+		put_le16(op->end_handle, pdu + 2);
+		put_le16(op->service_type, pdu + 4);
+		bt_uuid_to_le(&op->uuid, pdu + 6);
+
+		op->id = bt_att_send(op->att, BT_ATT_OP_FIND_BY_TYPE_VAL_REQ,
+						pdu, sizeof(pdu),
+						find_by_type_val_cb,
+						bt_gatt_request_ref(op),
+						async_req_unref);
+		if (op->id)
+			return;
+
+		success = false;
+		goto done;
+	}
+
+success:
+	success = true;
+
+done:
+	discovery_op_complete(op, success, att_ecode);
+}
+
+static struct bt_gatt_request *discover_services(struct bt_att *att,
+					bt_uuid_t *uuid,
+					uint16_t start, uint16_t end,
+					bt_gatt_request_callback_t callback,
+					void *user_data,
+					bt_gatt_destroy_func_t destroy,
+					bool primary)
+{
+	struct bt_gatt_request *op;
+
+	if (!att)
+		return NULL;
+
+	op = new0(struct bt_gatt_request, 1);
+	if (!op)
+		return NULL;
+
+	op->att = att;
+	op->end_handle = end;
+	op->callback = callback;
+	op->user_data = user_data;
+	op->destroy = destroy;
+	/* set service uuid to primary or secondary */
+	op->service_type = primary ? GATT_PRIM_SVC_UUID : GATT_SND_SVC_UUID;
+
+	/* If UUID is NULL, then discover all primary services */
+	if (!uuid) {
+		uint8_t pdu[6];
+
+		put_le16(start, pdu);
+		put_le16(end, pdu + 2);
+		put_le16(op->service_type, pdu + 4);
+
+		op->id = bt_att_send(att, BT_ATT_OP_READ_BY_GRP_TYPE_REQ,
+						pdu, sizeof(pdu),
+						read_by_grp_type_cb,
+						bt_gatt_request_ref(op),
+						async_req_unref);
+	} else {
+		uint8_t pdu[6 + get_uuid_len(uuid)];
+
+		if (uuid->type == BT_UUID_UNSPEC) {
+			free(op);
+			return NULL;
+		}
+
+		/* Discover by UUID */
+		op->uuid = *uuid;
+
+		put_le16(start, pdu);
+		put_le16(end, pdu + 2);
+		put_le16(op->service_type, pdu + 4);
+		bt_uuid_to_le(&op->uuid, pdu + 6);
+
+		op->id = bt_att_send(att, BT_ATT_OP_FIND_BY_TYPE_VAL_REQ,
+						pdu, sizeof(pdu),
+						find_by_type_val_cb,
+						bt_gatt_request_ref(op),
+						async_req_unref);
+	}
+
+	if (!op->id) {
+		free(op);
+		return NULL;
+	}
+
+	return bt_gatt_request_ref(op);
+}
+
+struct bt_gatt_request *bt_gatt_discover_all_primary_services(
+					struct bt_att *att, bt_uuid_t *uuid,
+					bt_gatt_request_callback_t callback,
+					void *user_data,
+					bt_gatt_destroy_func_t destroy)
+{
+	return bt_gatt_discover_primary_services(att, uuid, 0x0001, 0xffff,
+							callback, user_data,
+							destroy);
+}
+
+struct bt_gatt_request *bt_gatt_discover_primary_services(
+					struct bt_att *att, bt_uuid_t *uuid,
+					uint16_t start, uint16_t end,
+					bt_gatt_request_callback_t callback,
+					void *user_data,
+					bt_gatt_destroy_func_t destroy)
+{
+	return discover_services(att, uuid, start, end, callback, user_data,
+								destroy, true);
+}
+
+struct bt_gatt_request *bt_gatt_discover_secondary_services(
+					struct bt_att *att, bt_uuid_t *uuid,
+					uint16_t start, uint16_t end,
+					bt_gatt_request_callback_t callback,
+					void *user_data,
+					bt_gatt_destroy_func_t destroy)
+{
+	return discover_services(att, uuid, start, end, callback, user_data,
+								destroy, false);
+}
+
+struct read_incl_data {
+	struct bt_gatt_request *op;
+	struct bt_gatt_result *result;
+	int pos;
+	int ref_count;
+};
+
+static struct read_incl_data *new_read_included(struct bt_gatt_result *res)
+{
+	struct read_incl_data *data;
+
+	data = new0(struct read_incl_data, 1);
+	if (!data)
+		return NULL;
+
+	data->op = bt_gatt_request_ref(res->op);
+	data->result = res;
+
+	return data;
+};
+
+static struct read_incl_data *read_included_ref(struct read_incl_data *data)
+{
+	__sync_fetch_and_add(&data->ref_count, 1);
+
+	return data;
+}
+
+static void read_included_unref(void *data)
+{
+	struct read_incl_data *read_data = data;
+
+	if (__sync_sub_and_fetch(&read_data->ref_count, 1))
+		return;
+
+	async_req_unref(read_data->op);
+
+	free(read_data);
+}
+
+static void discover_included_cb(uint8_t opcode, const void *pdu,
+					uint16_t length, void *user_data);
+
+static void read_included_cb(uint8_t opcode, const void *pdu,
+					uint16_t length, void *user_data)
+{
+	struct read_incl_data *data = user_data;
+	struct bt_gatt_request *op = data->op;
+	uint8_t att_ecode = 0;
+	uint8_t read_pdu[2];
+	bool success;
+
+	if (opcode == BT_ATT_OP_ERROR_RSP) {
+		success = false;
+		att_ecode = process_error(pdu, length);
+		goto done;
+	}
+
+	if (opcode != BT_ATT_OP_READ_RSP || (!pdu && length)) {
+		success = false;
+		goto done;
+	}
+
+	/*
+	 * UUID should be in 128 bit format, as it couldn't be read in
+	 * READ_BY_TYPE request
+	 */
+	if (length != 16) {
+		success = false;
+		goto done;
+	}
+
+	if (!result_append(opcode, pdu, length, length, op)) {
+		success = false;
+		goto done;
+	}
+
+	if (data->pos == data->result->pdu_len) {
+		uint16_t last_handle;
+		uint8_t pdu[6];
+
+		last_handle = get_le16(data->result->pdu + data->pos -
+							data->result->data_len);
+		if (last_handle == op->end_handle) {
+			success = true;
+			goto done;
+		}
+
+		put_le16(last_handle + 1, pdu);
+		put_le16(op->end_handle, pdu + 2);
+		put_le16(GATT_INCLUDE_UUID, pdu + 4);
+
+		op->id = bt_att_send(op->att, BT_ATT_OP_READ_BY_TYPE_REQ,
+						pdu, sizeof(pdu),
+						discover_included_cb,
+						bt_gatt_request_ref(op),
+						async_req_unref);
+		if (op->id)
+			return;
+
+		success = false;
+		goto done;
+	}
+
+	memcpy(read_pdu, data->result->pdu + data->pos + 2, sizeof(uint16_t));
+
+	data->pos += data->result->data_len;
+
+	if (bt_att_send(op->att, BT_ATT_OP_READ_REQ, read_pdu, sizeof(read_pdu),
+				read_included_cb, read_included_ref(data),
+				read_included_unref))
+		return;
+
+	read_included_unref(data);
+	success = false;
+
+done:
+	discovery_op_complete(op, success, att_ecode);
+}
+
+static void read_included(struct read_incl_data *data)
+{
+	struct bt_gatt_request *op = data->op;
+	uint8_t pdu[2];
+
+	memcpy(pdu, data->result->pdu + 2, sizeof(uint16_t));
+
+	data->pos += data->result->data_len;
+
+	if (bt_att_send(op->att, BT_ATT_OP_READ_REQ, pdu, sizeof(pdu),
+							read_included_cb,
+							read_included_ref(data),
+							read_included_unref))
+		return;
+
+	if (op->callback)
+		op->callback(false, 0, NULL, data->op->user_data);
+
+	read_included_unref(data);
+}
+
+static void discover_included_cb(uint8_t opcode, const void *pdu,
+					uint16_t length, void *user_data)
+{
+	struct bt_gatt_request *op = user_data;
+	struct bt_gatt_result *cur_result;
+	uint8_t att_ecode = 0;
+	uint16_t last_handle;
+	size_t data_length;
+	bool success;
+
+	if (opcode == BT_ATT_OP_ERROR_RSP) {
+		att_ecode = process_error(pdu, length);
+
+		if (att_ecode == BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND &&
+							op->result_head)
+			goto done;
+
+		success = false;
+		goto failed;
+	}
+
+	if (opcode != BT_ATT_OP_READ_BY_TYPE_RSP || !pdu || length < 6) {
+		success = false;
+		goto failed;
+	}
+
+	data_length = ((const uint8_t *) pdu)[0];
+
+	/*
+	 * Check if PDU contains data sets with length declared in the beginning
+	 * of frame and if this length is correct.
+	 * Data set length may be 6 or 8 octets:
+	 * 2 octets - include service handle
+	 * 2 octets - start handle of included service
+	 * 2 octets - end handle of included service
+	 * optional 2 octets - Bluetooth UUID of included service
+	 */
+	if ((data_length != 8 && data_length != 6) ||
+						(length - 1) % data_length) {
+		success = false;
+		goto failed;
+	}
+
+	cur_result = result_append(opcode, pdu + 1, length - 1, data_length,
+									op);
+	if (!cur_result) {
+		success = false;
+		goto failed;
+	}
+
+	if (data_length == 6) {
+		struct read_incl_data *data;
+
+		data = new_read_included(cur_result);
+		if (!data) {
+			success = false;
+			goto failed;
+		}
+
+		read_included(data);
+		return;
+	}
+
+	last_handle = get_le16(pdu + length - data_length);
+	if (last_handle != op->end_handle) {
+		uint8_t pdu[6];
+
+		put_le16(last_handle + 1, pdu);
+		put_le16(op->end_handle, pdu + 2);
+		put_le16(GATT_INCLUDE_UUID, pdu + 4);
+
+		op->id = bt_att_send(op->att, BT_ATT_OP_READ_BY_TYPE_REQ,
+						pdu, sizeof(pdu),
+						discover_included_cb,
+						bt_gatt_request_ref(op),
+						async_req_unref);
+		if (op->id)
+			return;
+
+		success = false;
+		goto failed;
+	}
+
+done:
+	success = true;
+
+failed:
+	discovery_op_complete(op, success, att_ecode);
+}
+
+struct bt_gatt_request *bt_gatt_discover_included_services(struct bt_att *att,
+					uint16_t start, uint16_t end,
+					bt_gatt_request_callback_t callback,
+					void *user_data,
+					bt_gatt_destroy_func_t destroy)
+{
+	struct bt_gatt_request *op;
+	uint8_t pdu[6];
+
+	if (!att)
+		return false;
+
+	op = new0(struct bt_gatt_request, 1);
+	if (!op)
+		return false;
+
+	op->att = att;
+	op->callback = callback;
+	op->user_data = user_data;
+	op->destroy = destroy;
+	op->end_handle = end;
+
+	put_le16(start, pdu);
+	put_le16(end, pdu + 2);
+	put_le16(GATT_INCLUDE_UUID, pdu + 4);
+
+	op->id = bt_att_send(att, BT_ATT_OP_READ_BY_TYPE_REQ, pdu, sizeof(pdu),
+				discover_included_cb, bt_gatt_request_ref(op),
+				async_req_unref);
+	if (!op->id) {
+		free(op);
+		return NULL;
+	}
+
+	return bt_gatt_request_ref(op);
+}
+
+static void discover_chrcs_cb(uint8_t opcode, const void *pdu,
+					uint16_t length, void *user_data)
+{
+	struct bt_gatt_request *op = user_data;
+	bool success;
+	uint8_t att_ecode = 0;
+	size_t data_length;
+	uint16_t last_handle;
+
+	if (opcode == BT_ATT_OP_ERROR_RSP) {
+		success = false;
+		att_ecode = process_error(pdu, length);
+
+		if (att_ecode == BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND &&
+							op->result_head)
+			goto success;
+
+		goto done;
+	}
+
+	/* PDU must contain at least the following (sans opcode):
+	 * - Attr Data Length (1 octet)
+	 * - Attr Data List (at least 7 octets):
+	 *   -- 2 octets: Attribute handle
+	 *   -- 1 octet: Characteristic properties
+	 *   -- 2 octets: Characteristic value handle
+	 *   -- 2 or 16 octets: characteristic UUID
+	 */
+	if (opcode != BT_ATT_OP_READ_BY_TYPE_RSP || !pdu || length < 8) {
+		success = false;
+		goto done;
+	}
+
+	data_length = ((uint8_t *) pdu)[0];
+
+	if ((data_length != 7 && data_length != 21) ||
+					((length - 1) % data_length)) {
+		success = false;
+		goto done;
+	}
+
+	if (!result_append(opcode, pdu + 1, length - 1,
+							data_length, op)) {
+		success = false;
+		goto done;
+	}
+	last_handle = get_le16(pdu + length - data_length);
+	if (last_handle != op->end_handle) {
+		uint8_t pdu[6];
+
+		put_le16(last_handle + 1, pdu);
+		put_le16(op->end_handle, pdu + 2);
+		put_le16(GATT_CHARAC_UUID, pdu + 4);
+
+		op->id = bt_att_send(op->att, BT_ATT_OP_READ_BY_TYPE_REQ,
+						pdu, sizeof(pdu),
+						discover_chrcs_cb,
+						bt_gatt_request_ref(op),
+						async_req_unref);
+		if (op->id)
+			return;
+
+		success = false;
+		goto done;
+	}
+
+success:
+	success = true;
+
+done:
+	discovery_op_complete(op, success, att_ecode);
+}
+
+struct bt_gatt_request *bt_gatt_discover_characteristics(struct bt_att *att,
+					uint16_t start, uint16_t end,
+					bt_gatt_request_callback_t callback,
+					void *user_data,
+					bt_gatt_destroy_func_t destroy)
+{
+	struct bt_gatt_request *op;
+	uint8_t pdu[6];
+
+	if (!att)
+		return false;
+
+	op = new0(struct bt_gatt_request, 1);
+	if (!op)
+		return false;
+
+	op->att = att;
+	op->callback = callback;
+	op->user_data = user_data;
+	op->destroy = destroy;
+	op->end_handle = end;
+
+	put_le16(start, pdu);
+	put_le16(end, pdu + 2);
+	put_le16(GATT_CHARAC_UUID, pdu + 4);
+
+	op->id = bt_att_send(att, BT_ATT_OP_READ_BY_TYPE_REQ, pdu, sizeof(pdu),
+				discover_chrcs_cb, bt_gatt_request_ref(op),
+				async_req_unref);
+	if (!op->id) {
+		free(op);
+		return NULL;
+	}
+
+	return bt_gatt_request_ref(op);
+}
+
+static void read_by_type_cb(uint8_t opcode, const void *pdu,
+					uint16_t length, void *user_data)
+{
+	struct bt_gatt_request *op = user_data;
+	bool success;
+	uint8_t att_ecode = 0;
+	size_t data_length;
+	uint16_t last_handle;
+
+	if (opcode == BT_ATT_OP_ERROR_RSP) {
+		att_ecode = process_error(pdu, length);
+
+		if (att_ecode == BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND &&
+							op->result_head)
+			success = true;
+		else
+			success = false;
+
+		goto done;
+	}
+
+	if (opcode != BT_ATT_OP_READ_BY_TYPE_RSP || !pdu) {
+		success = false;
+		att_ecode = 0;
+		goto done;
+	}
+
+	data_length = ((uint8_t *) pdu)[0];
+	if (((length - 1) % data_length)) {
+		success = false;
+		att_ecode = 0;
+		goto done;
+	}
+
+	if (!result_append(opcode, pdu + 1, length - 1, data_length, op)) {
+		success = false;
+		att_ecode = 0;
+		goto done;
+	}
+
+	last_handle = get_le16(pdu + length - data_length);
+	if (last_handle != op->end_handle) {
+		uint8_t pdu[4 + get_uuid_len(&op->uuid)];
+
+		put_le16(last_handle + 1, pdu);
+		put_le16(op->end_handle, pdu + 2);
+		bt_uuid_to_le(&op->uuid, pdu + 4);
+
+		op->id = bt_att_send(op->att, BT_ATT_OP_READ_BY_TYPE_REQ,
+						pdu, sizeof(pdu),
+						read_by_type_cb,
+						bt_gatt_request_ref(op),
+						async_req_unref);
+		if (op->id)
+			return;
+
+		success = false;
+		goto done;
+	}
+
+	success = true;
+
+done:
+	discovery_op_complete(op, success, att_ecode);
+}
+
+bool bt_gatt_read_by_type(struct bt_att *att, uint16_t start, uint16_t end,
+					const bt_uuid_t *uuid,
+					bt_gatt_request_callback_t callback,
+					void *user_data,
+					bt_gatt_destroy_func_t destroy)
+{
+	struct bt_gatt_request *op;
+	uint8_t pdu[4 + get_uuid_len(uuid)];
+
+	if (!att || !uuid || uuid->type == BT_UUID_UNSPEC)
+		return false;
+
+	op = new0(struct bt_gatt_request, 1);
+	if (!op)
+		return false;
+
+	op->att = att;
+	op->callback = callback;
+	op->user_data = user_data;
+	op->destroy = destroy;
+	op->end_handle = end;
+	op->uuid = *uuid;
+
+	put_le16(start, pdu);
+	put_le16(end, pdu + 2);
+	bt_uuid_to_le(uuid, pdu + 4);
+
+	op->id = bt_att_send(att, BT_ATT_OP_READ_BY_TYPE_REQ, pdu, sizeof(pdu),
+						read_by_type_cb,
+						bt_gatt_request_ref(op),
+						async_req_unref);
+	if (op->id)
+		return true;
+
+	free(op);
+	return false;
+}
+
+static void discover_descs_cb(uint8_t opcode, const void *pdu,
+					uint16_t length, void *user_data)
+{
+	struct bt_gatt_request *op = user_data;
+	bool success;
+	uint8_t att_ecode = 0;
+	uint8_t format;
+	uint16_t last_handle;
+	size_t data_length;
+
+	if (opcode == BT_ATT_OP_ERROR_RSP) {
+		success = false;
+		att_ecode = process_error(pdu, length);
+
+		if (att_ecode == BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND &&
+								op->result_head)
+			goto success;
+
+		goto done;
+	}
+
+	/* The PDU should contain the following data (sans opcode):
+	 * - Format (1 octet)
+	 * - Attr Data List (at least 4 octets):
+	 *   -- 2 octets: Attribute handle
+	 *   -- 2 or 16 octets: UUID.
+	 */
+	if (opcode != BT_ATT_OP_FIND_INFO_RSP || !pdu || length < 5) {
+		success = false;
+		goto done;
+	}
+
+	format = ((uint8_t *) pdu)[0];
+
+	if (format == 0x01)
+		data_length = 4;
+	else if (format == 0x02)
+		data_length = 18;
+	else {
+		success = false;
+		goto done;
+	}
+
+	if ((length - 1) % data_length) {
+		success = false;
+		goto done;
+	}
+
+	if (!result_append(opcode, pdu + 1, length - 1, data_length, op)) {
+		success = false;
+		goto done;
+	}
+
+	last_handle = get_le16(pdu + length - data_length);
+	if (last_handle != op->end_handle) {
+		uint8_t pdu[4];
+
+		put_le16(last_handle + 1, pdu);
+		put_le16(op->end_handle, pdu + 2);
+
+		op->id = bt_att_send(op->att, BT_ATT_OP_FIND_INFO_REQ,
+						pdu, sizeof(pdu),
+						discover_descs_cb,
+						bt_gatt_request_ref(op),
+						async_req_unref);
+		if (op->id)
+			return;
+
+		success = false;
+		goto done;
+	}
+
+success:
+	success = true;
+
+done:
+	discovery_op_complete(op, success, att_ecode);
+}
+
+struct bt_gatt_request *bt_gatt_discover_descriptors(struct bt_att *att,
+					uint16_t start, uint16_t end,
+					bt_gatt_request_callback_t callback,
+					void *user_data,
+					bt_gatt_destroy_func_t destroy)
+{
+	struct bt_gatt_request *op;
+	uint8_t pdu[4];
+
+	if (!att)
+		return false;
+
+	op = new0(struct bt_gatt_request, 1);
+	if (!op)
+		return false;
+
+	op->att = att;
+	op->callback = callback;
+	op->user_data = user_data;
+	op->destroy = destroy;
+	op->end_handle = end;
+
+	put_le16(start, pdu);
+	put_le16(end, pdu + 2);
+
+	op->id = bt_att_send(att, BT_ATT_OP_FIND_INFO_REQ, pdu, sizeof(pdu),
+						discover_descs_cb,
+						bt_gatt_request_ref(op),
+						async_req_unref);
+	if (!op->id) {
+		free(op);
+		return NULL;
+	}
+
+	return bt_gatt_request_ref(op);
+}
diff --git a/bluez/gatt-helpers.h b/bluez/gatt-helpers.h
new file mode 100644
index 0000000..dd9dd1c
--- /dev/null
+++ b/bluez/gatt-helpers.h
@@ -0,0 +1,116 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2014  Google Inc.
+ *
+ *
+ *  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
+ *
+ */
+
+/* This file defines helpers for performing client-side procedures defined by
+ * the Generic Attribute Profile.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+struct bt_gatt_result;
+
+struct bt_gatt_iter {
+	struct bt_gatt_result *result;
+	uint16_t pos;
+};
+
+unsigned int bt_gatt_result_service_count(struct bt_gatt_result *result);
+unsigned int bt_gatt_result_characteristic_count(struct bt_gatt_result *result);
+unsigned int bt_gatt_result_descriptor_count(struct bt_gatt_result *result);
+unsigned int bt_gatt_result_included_count(struct bt_gatt_result *result);
+
+bool bt_gatt_iter_init(struct bt_gatt_iter *iter, struct bt_gatt_result *result);
+bool bt_gatt_iter_next_service(struct bt_gatt_iter *iter,
+				uint16_t *start_handle, uint16_t *end_handle,
+				uint8_t uuid[16]);
+bool bt_gatt_iter_next_characteristic(struct bt_gatt_iter *iter,
+				uint16_t *start_handle, uint16_t *end_handle,
+				uint16_t *value_handle, uint8_t *properties,
+				uint8_t uuid[16]);
+bool bt_gatt_iter_next_descriptor(struct bt_gatt_iter *iter, uint16_t *handle,
+							uint8_t uuid[16]);
+bool bt_gatt_iter_next_included_service(struct bt_gatt_iter *iter,
+				uint16_t *handle, uint16_t *start_handle,
+				uint16_t *end_handle, uint8_t uuid[16]);
+bool bt_gatt_iter_next_read_by_type(struct bt_gatt_iter *iter,
+				uint16_t *handle, uint16_t *length,
+				const uint8_t **value);
+
+typedef void (*bt_gatt_destroy_func_t)(void *user_data);
+
+typedef void (*bt_gatt_result_callback_t)(bool success, uint8_t att_ecode,
+							void *user_data);
+typedef void (*bt_gatt_request_callback_t)(bool success, uint8_t att_ecode,
+						struct bt_gatt_result *result,
+						void *user_data);
+
+struct bt_gatt_request;
+
+struct bt_gatt_request *bt_gatt_request_ref(struct bt_gatt_request *req);
+void bt_gatt_request_unref(struct bt_gatt_request *req);
+void bt_gatt_request_cancel(struct bt_gatt_request *req);
+
+unsigned int bt_gatt_exchange_mtu(struct bt_att *att, uint16_t client_rx_mtu,
+					bt_gatt_result_callback_t callback,
+					void *user_data,
+					bt_gatt_destroy_func_t destroy);
+
+struct bt_gatt_request *bt_gatt_discover_all_primary_services(
+					struct bt_att *att, bt_uuid_t *uuid,
+					bt_gatt_request_callback_t callback,
+					void *user_data,
+					bt_gatt_destroy_func_t destroy);
+struct bt_gatt_request *bt_gatt_discover_primary_services(
+					struct bt_att *att, bt_uuid_t *uuid,
+					uint16_t start, uint16_t end,
+					bt_gatt_request_callback_t callback,
+					void *user_data,
+					bt_gatt_destroy_func_t destroy);
+struct bt_gatt_request *bt_gatt_discover_secondary_services(
+					struct bt_att *att, bt_uuid_t *uuid,
+					uint16_t start, uint16_t end,
+					bt_gatt_request_callback_t callback,
+					void *user_data,
+					bt_gatt_destroy_func_t destroy);
+struct bt_gatt_request *bt_gatt_discover_included_services(struct bt_att *att,
+					uint16_t start, uint16_t end,
+					bt_gatt_request_callback_t callback,
+					void *user_data,
+					bt_gatt_destroy_func_t destroy);
+struct bt_gatt_request *bt_gatt_discover_characteristics(struct bt_att *att,
+					uint16_t start, uint16_t end,
+					bt_gatt_request_callback_t callback,
+					void *user_data,
+					bt_gatt_destroy_func_t destroy);
+struct bt_gatt_request *bt_gatt_discover_descriptors(struct bt_att *att,
+					uint16_t start, uint16_t end,
+					bt_gatt_request_callback_t callback,
+					void *user_data,
+					bt_gatt_destroy_func_t destroy);
+
+bool bt_gatt_read_by_type(struct bt_att *att, uint16_t start, uint16_t end,
+					const bt_uuid_t *uuid,
+					bt_gatt_request_callback_t callback,
+					void *user_data,
+					bt_gatt_destroy_func_t destroy);
diff --git a/bluez/io-mainloop.c b/bluez/io-mainloop.c
new file mode 100644
index 0000000..752b05d
--- /dev/null
+++ b/bluez/io-mainloop.c
@@ -0,0 +1,325 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2012-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
+ *
+ */
+
+#include <bluetooth/bluetooth.h>
+
+#include <unistd.h>
+#include <errno.h>
+#include <sys/socket.h>
+
+#include "mainloop.h"
+#include "util.h"
+#include "io.h"
+
+struct io {
+	int ref_count;
+	int fd;
+	uint32_t events;
+	bool close_on_destroy;
+	io_callback_func_t read_callback;
+	io_destroy_func_t read_destroy;
+	void *read_data;
+	io_callback_func_t write_callback;
+	io_destroy_func_t write_destroy;
+	void *write_data;
+	io_callback_func_t disconnect_callback;
+	io_destroy_func_t disconnect_destroy;
+	void *disconnect_data;
+};
+
+static struct io *io_ref(struct io *io)
+{
+	if (!io)
+		return NULL;
+
+	__sync_fetch_and_add(&io->ref_count, 1);
+
+	return io;
+}
+
+static void io_unref(struct io *io)
+{
+	if (!io)
+		return;
+
+	if (__sync_sub_and_fetch(&io->ref_count, 1))
+		return;
+
+	free(io);
+}
+
+static void io_cleanup(void *user_data)
+{
+	struct io *io = user_data;
+
+	if (io->write_destroy)
+		io->write_destroy(io->write_data);
+
+	if (io->read_destroy)
+		io->read_destroy(io->read_data);
+
+	if (io->disconnect_destroy)
+		io->disconnect_destroy(io->disconnect_data);
+
+	if (io->close_on_destroy)
+		close(io->fd);
+
+	io->fd = -1;
+}
+
+static void io_callback(int fd, uint32_t events, void *user_data)
+{
+	struct io *io = user_data;
+
+	io_ref(io);
+
+	if ((events & (EPOLLRDHUP | EPOLLHUP | EPOLLERR))) {
+		io->read_callback = NULL;
+		io->write_callback = NULL;
+
+		if (!io->disconnect_callback) {
+			mainloop_remove_fd(io->fd);
+			io_unref(io);
+			return;
+		}
+
+		if (!io->disconnect_callback(io, io->disconnect_data)) {
+			if (io->disconnect_destroy)
+				io->disconnect_destroy(io->disconnect_data);
+
+			io->disconnect_callback = NULL;
+			io->disconnect_destroy = NULL;
+			io->disconnect_data = NULL;
+
+			io->events &= ~EPOLLRDHUP;
+
+			mainloop_modify_fd(io->fd, io->events);
+		}
+	}
+
+	if ((events & EPOLLIN) && io->read_callback) {
+		if (!io->read_callback(io, io->read_data)) {
+			if (io->read_destroy)
+				io->read_destroy(io->read_data);
+
+			io->read_callback = NULL;
+			io->read_destroy = NULL;
+			io->read_data = NULL;
+
+			io->events &= ~EPOLLIN;
+
+			mainloop_modify_fd(io->fd, io->events);
+		}
+	}
+
+	if ((events & EPOLLOUT) && io->write_callback) {
+		if (!io->write_callback(io, io->write_data)) {
+			if (io->write_destroy)
+				io->write_destroy(io->write_data);
+
+			io->write_callback = NULL;
+			io->write_destroy = NULL;
+			io->write_data = NULL;
+
+			io->events &= ~EPOLLOUT;
+
+			mainloop_modify_fd(io->fd, io->events);
+		}
+	}
+
+	io_unref(io);
+}
+
+struct io *io_new(int fd)
+{
+	struct io *io;
+
+	if (fd < 0)
+		return NULL;
+
+	io = new0(struct io, 1);
+	if (!io)
+		return NULL;
+
+	io->fd = fd;
+	io->events = 0;
+	io->close_on_destroy = false;
+
+	if (mainloop_add_fd(io->fd, io->events, io_callback,
+						io, io_cleanup) < 0) {
+		free(io);
+		return NULL;
+	}
+
+	return io_ref(io);
+}
+
+void io_destroy(struct io *io)
+{
+	if (!io)
+		return;
+
+	io->read_callback = NULL;
+	io->write_callback = NULL;
+	io->disconnect_callback = NULL;
+
+	mainloop_remove_fd(io->fd);
+
+	io_unref(io);
+}
+
+int io_get_fd(struct io *io)
+{
+	if (!io)
+		return -ENOTCONN;
+
+	return io->fd;
+}
+
+bool io_set_close_on_destroy(struct io *io, bool do_close)
+{
+	if (!io)
+		return false;
+
+	io->close_on_destroy = do_close;
+
+	return true;
+}
+
+bool io_set_read_handler(struct io *io, io_callback_func_t callback,
+				void *user_data, io_destroy_func_t destroy)
+{
+	uint32_t events;
+
+	if (!io || io->fd < 0)
+		return false;
+
+	if (io->read_destroy)
+		io->read_destroy(io->read_data);
+
+	if (callback)
+		events = io->events | EPOLLIN;
+	else
+		events = io->events & ~EPOLLIN;
+
+	io->read_callback = callback;
+	io->read_destroy = destroy;
+	io->read_data = user_data;
+
+	if (events == io->events)
+		return true;
+
+	if (mainloop_modify_fd(io->fd, events) < 0)
+		return false;
+
+	io->events = events;
+
+	return true;
+}
+
+bool io_set_write_handler(struct io *io, io_callback_func_t callback,
+				void *user_data, io_destroy_func_t destroy)
+{
+	uint32_t events;
+
+	if (!io || io->fd < 0)
+		return false;
+
+	if (io->write_destroy)
+		io->write_destroy(io->write_data);
+
+	if (callback)
+		events = io->events | EPOLLOUT;
+	else
+		events = io->events & ~EPOLLOUT;
+
+	io->write_callback = callback;
+	io->write_destroy = destroy;
+	io->write_data = user_data;
+
+	if (events == io->events)
+		return true;
+
+	if (mainloop_modify_fd(io->fd, events) < 0)
+		return false;
+
+	io->events = events;
+
+	return true;
+}
+
+bool io_set_disconnect_handler(struct io *io, io_callback_func_t callback,
+				void *user_data, io_destroy_func_t destroy)
+{
+	uint32_t events;
+
+	if (!io || io->fd < 0)
+		return false;
+
+	if (io->disconnect_destroy)
+		io->disconnect_destroy(io->disconnect_data);
+
+	if (callback)
+		events = io->events | EPOLLRDHUP;
+	else
+		events = io->events & ~EPOLLRDHUP;
+
+	io->disconnect_callback = callback;
+	io->disconnect_destroy = destroy;
+	io->disconnect_data = user_data;
+
+	if (events == io->events)
+		return true;
+
+	if (mainloop_modify_fd(io->fd, events) < 0)
+		return false;
+
+	io->events = events;
+
+	return true;
+}
+
+ssize_t io_send(struct io *io, const struct iovec *iov, int iovcnt)
+{
+	ssize_t ret;
+
+	if (!io || io->fd < 0)
+		return -ENOTCONN;
+
+	do {
+		ret = writev(io->fd, iov, iovcnt);
+	} while (ret < 0 && errno == EINTR);
+
+	if (ret < 0)
+		return -errno;
+
+	return ret;
+}
+
+bool io_shutdown(struct io *io)
+{
+	if (!io || io->fd < 0)
+		return false;
+
+	return shutdown(io->fd, SHUT_RDWR) == 0;
+}
diff --git a/bluez/io.h b/bluez/io.h
new file mode 100644
index 0000000..8bc1111
--- /dev/null
+++ b/bluez/io.h
@@ -0,0 +1,47 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2012-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
+ *
+ */
+
+#include <stdbool.h>
+#include <sys/uio.h>
+
+typedef void (*io_destroy_func_t)(void *data);
+
+struct io;
+
+struct io *io_new(int fd);
+void io_destroy(struct io *io);
+
+int io_get_fd(struct io *io);
+bool io_set_close_on_destroy(struct io *io, bool do_close);
+
+ssize_t io_send(struct io *io, const struct iovec *iov, int iovcnt);
+bool io_shutdown(struct io *io);
+
+typedef bool (*io_callback_func_t)(struct io *io, void *user_data);
+
+bool io_set_read_handler(struct io *io, io_callback_func_t callback,
+				void *user_data, io_destroy_func_t destroy);
+bool io_set_write_handler(struct io *io, io_callback_func_t callback,
+				void *user_data, io_destroy_func_t destroy);
+bool io_set_disconnect_handler(struct io *io, io_callback_func_t callback,
+				void *user_data, io_destroy_func_t destroy);
diff --git a/bluez/mainloop.c b/bluez/mainloop.c
new file mode 100644
index 0000000..b769c8f
--- /dev/null
+++ b/bluez/mainloop.c
@@ -0,0 +1,405 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2011-2014  Intel Corporation
+ *  Copyright (C) 2002-2010  Marcel Holtmann <marcel@holtmann.org>
+ *
+ *
+ *  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
+ *
+ */
+
+#include <bluetooth/bluetooth.h>
+
+#include <stdio.h>
+#include <errno.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <signal.h>
+#include <sys/signalfd.h>
+#include <sys/timerfd.h>
+#include <sys/epoll.h>
+
+#include "mainloop.h"
+
+#define MAX_EPOLL_EVENTS 10
+
+static int epoll_fd;
+static int epoll_terminate;
+static int exit_status;
+
+struct mainloop_data {
+	int fd;
+	uint32_t events;
+	mainloop_event_func callback;
+	mainloop_destroy_func destroy;
+	void *user_data;
+};
+
+#define MAX_MAINLOOP_ENTRIES 128
+
+static struct mainloop_data *mainloop_list[MAX_MAINLOOP_ENTRIES];
+
+struct timeout_data {
+	int fd;
+	mainloop_timeout_func callback;
+	mainloop_destroy_func destroy;
+	void *user_data;
+};
+
+struct signal_data {
+	int fd;
+	sigset_t mask;
+	mainloop_signal_func callback;
+	mainloop_destroy_func destroy;
+	void *user_data;
+};
+
+static struct signal_data *signal_data;
+
+void mainloop_init(void)
+{
+	unsigned int i;
+
+	epoll_fd = epoll_create1(EPOLL_CLOEXEC);
+
+	for (i = 0; i < MAX_MAINLOOP_ENTRIES; i++)
+		mainloop_list[i] = NULL;
+
+	epoll_terminate = 0;
+}
+
+void mainloop_quit(void)
+{
+	epoll_terminate = 1;
+}
+
+void mainloop_exit_success(void)
+{
+	exit_status = EXIT_SUCCESS;
+	epoll_terminate = 1;
+}
+
+void mainloop_exit_failure(void)
+{
+	exit_status = EXIT_FAILURE;
+	epoll_terminate = 1;
+}
+
+static void signal_callback(int fd, uint32_t events, void *user_data)
+{
+	struct signal_data *data = user_data;
+	struct signalfd_siginfo si;
+	ssize_t result;
+
+	if (events & (EPOLLERR | EPOLLHUP)) {
+		mainloop_quit();
+		return;
+	}
+
+	result = read(fd, &si, sizeof(si));
+	if (result != sizeof(si))
+		return;
+
+	if (data->callback)
+		data->callback(si.ssi_signo, data->user_data);
+}
+
+int mainloop_run(void)
+{
+	if (signal_data) {
+		if (sigprocmask(SIG_BLOCK, &signal_data->mask, NULL) < 0)
+			return EXIT_FAILURE;
+
+		signal_data->fd = signalfd(-1, &signal_data->mask,
+						SFD_NONBLOCK | SFD_CLOEXEC);
+		if (signal_data->fd < 0)
+			return EXIT_FAILURE;
+
+		if (mainloop_add_fd(signal_data->fd, EPOLLIN,
+				signal_callback, signal_data, NULL) < 0) {
+			close(signal_data->fd);
+			return EXIT_FAILURE;
+		}
+	}
+
+	exit_status = EXIT_SUCCESS;
+
+	while (!epoll_terminate) {
+		struct epoll_event events[MAX_EPOLL_EVENTS];
+		int n, nfds;
+
+		nfds = epoll_wait(epoll_fd, events, MAX_EPOLL_EVENTS, -1);
+		if (nfds < 0)
+			continue;
+
+		for (n = 0; n < nfds; n++) {
+			struct mainloop_data *data = events[n].data.ptr;
+
+			data->callback(data->fd, events[n].events,
+							data->user_data);
+		}
+	}
+
+	if (signal_data) {
+		mainloop_remove_fd(signal_data->fd);
+		close(signal_data->fd);
+
+		if (signal_data->destroy)
+			signal_data->destroy(signal_data->user_data);
+	}
+	epoll_terminate = 0;
+
+
+	return exit_status;
+}
+
+void mainloop_finish(void)
+{
+	unsigned int i;
+
+	for (i = 0; i < MAX_MAINLOOP_ENTRIES; i++) {
+		struct mainloop_data *data = mainloop_list[i];
+
+		mainloop_list[i] = NULL;
+
+		if (data) {
+			epoll_ctl(epoll_fd, EPOLL_CTL_DEL, data->fd, NULL);
+
+			if (data->destroy)
+				data->destroy(data->user_data);
+
+			free(data);
+		}
+	}
+
+	close(epoll_fd);
+	epoll_fd = 0;
+}
+
+int mainloop_add_fd(int fd, uint32_t events, mainloop_event_func callback,
+				void *user_data, mainloop_destroy_func destroy)
+{
+	struct mainloop_data *data;
+	struct epoll_event ev;
+	int err;
+
+	if (fd < 0 || fd > MAX_MAINLOOP_ENTRIES - 1 || !callback)
+		return -EINVAL;
+
+	data = malloc(sizeof(*data));
+	if (!data)
+		return -ENOMEM;
+
+	memset(data, 0, sizeof(*data));
+	data->fd = fd;
+	data->events = events;
+	data->callback = callback;
+	data->destroy = destroy;
+	data->user_data = user_data;
+
+	memset(&ev, 0, sizeof(ev));
+	ev.events = events;
+	ev.data.ptr = data;
+
+	err = epoll_ctl(epoll_fd, EPOLL_CTL_ADD, data->fd, &ev);
+	if (err < 0) {
+		free(data);
+		return err;
+	}
+
+	mainloop_list[fd] = data;
+
+	return 0;
+}
+
+int mainloop_modify_fd(int fd, uint32_t events)
+{
+	struct mainloop_data *data;
+	struct epoll_event ev;
+	int err;
+
+	if (fd < 0 || fd > MAX_MAINLOOP_ENTRIES - 1)
+		return -EINVAL;
+
+	data = mainloop_list[fd];
+	if (!data)
+		return -ENXIO;
+
+	memset(&ev, 0, sizeof(ev));
+	ev.events = events;
+	ev.data.ptr = data;
+
+	err = epoll_ctl(epoll_fd, EPOLL_CTL_MOD, data->fd, &ev);
+	if (err < 0)
+		return err;
+
+	data->events = events;
+
+	return 0;
+}
+
+int mainloop_remove_fd(int fd)
+{
+	struct mainloop_data *data;
+	int err;
+
+	if (fd < 0 || fd > MAX_MAINLOOP_ENTRIES - 1)
+		return -EINVAL;
+
+	data = mainloop_list[fd];
+	if (!data)
+		return -ENXIO;
+
+	mainloop_list[fd] = NULL;
+
+	err = epoll_ctl(epoll_fd, EPOLL_CTL_DEL, data->fd, NULL);
+
+	if (data->destroy)
+		data->destroy(data->user_data);
+
+	free(data);
+
+	return err;
+}
+
+static void timeout_destroy(void *user_data)
+{
+	struct timeout_data *data = user_data;
+
+	close(data->fd);
+	data->fd = -1;
+
+	if (data->destroy)
+		data->destroy(data->user_data);
+}
+
+static void timeout_callback(int fd, uint32_t events, void *user_data)
+{
+	struct timeout_data *data = user_data;
+	uint64_t expired;
+	ssize_t result;
+
+	if (events & (EPOLLERR | EPOLLHUP))
+		return;
+
+	result = read(data->fd, &expired, sizeof(expired));
+	if (result != sizeof(expired))
+		return;
+
+	if (data->callback)
+		data->callback(data->fd, data->user_data);
+}
+
+static inline int timeout_set(int fd, unsigned int msec)
+{
+	struct itimerspec itimer;
+	unsigned int sec = msec / 1000;
+
+	memset(&itimer, 0, sizeof(itimer));
+	itimer.it_interval.tv_sec = 0;
+	itimer.it_interval.tv_nsec = 0;
+	itimer.it_value.tv_sec = sec;
+	itimer.it_value.tv_nsec = (msec - (sec * 1000)) * 1000;
+
+	return timerfd_settime(fd, 0, &itimer, NULL);
+}
+
+int mainloop_add_timeout(unsigned int msec, mainloop_timeout_func callback,
+				void *user_data, mainloop_destroy_func destroy)
+{
+	struct timeout_data *data;
+
+	if (!callback)
+		return -EINVAL;
+
+	data = malloc(sizeof(*data));
+	if (!data)
+		return -ENOMEM;
+
+	memset(data, 0, sizeof(*data));
+	data->callback = callback;
+	data->destroy = destroy;
+	data->user_data = user_data;
+
+	data->fd = timerfd_create(CLOCK_MONOTONIC, TFD_NONBLOCK | TFD_CLOEXEC);
+	if (data->fd < 0) {
+		free(data);
+		return -EIO;
+	}
+
+	if (msec > 0) {
+		if (timeout_set(data->fd, msec) < 0) {
+			close(data->fd);
+			free(data);
+			return -EIO;
+		}
+	}
+
+	if (mainloop_add_fd(data->fd, EPOLLIN | EPOLLONESHOT,
+				timeout_callback, data, timeout_destroy) < 0) {
+		close(data->fd);
+		free(data);
+		return -EIO;
+	}
+
+	return data->fd;
+}
+
+int mainloop_modify_timeout(int id, unsigned int msec)
+{
+	if (msec > 0) {
+		if (timeout_set(id, msec) < 0)
+			return -EIO;
+	}
+
+	if (mainloop_modify_fd(id, EPOLLIN | EPOLLONESHOT) < 0)
+		return -EIO;
+
+	return 0;
+}
+
+int mainloop_remove_timeout(int id)
+{
+	return mainloop_remove_fd(id);
+}
+
+int mainloop_set_signal(sigset_t *mask, mainloop_signal_func callback,
+				void *user_data, mainloop_destroy_func destroy)
+{
+	struct signal_data *data;
+
+	if (!mask || !callback)
+		return -EINVAL;
+
+	data = malloc(sizeof(*data));
+	if (!data)
+		return -ENOMEM;
+
+	memset(data, 0, sizeof(*data));
+	data->callback = callback;
+	data->destroy = destroy;
+	data->user_data = user_data;
+
+	data->fd = -1;
+	memcpy(&data->mask, mask, sizeof(sigset_t));
+
+	free(signal_data);
+	signal_data = data;
+
+	return 0;
+}
diff --git a/bluez/mainloop.h b/bluez/mainloop.h
new file mode 100644
index 0000000..bababcb
--- /dev/null
+++ b/bluez/mainloop.h
@@ -0,0 +1,52 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2011-2014  Intel Corporation
+ *  Copyright (C) 2002-2010  Marcel Holtmann <marcel@holtmann.org>
+ *
+ *
+ *  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
+ *
+ */
+
+#include <signal.h>
+#include <sys/epoll.h>
+
+typedef void (*mainloop_destroy_func) (void *user_data);
+
+typedef void (*mainloop_event_func) (int fd, uint32_t events, void *user_data);
+typedef void (*mainloop_timeout_func) (int id, void *user_data);
+typedef void (*mainloop_signal_func) (int signum, void *user_data);
+
+void mainloop_init(void);
+void mainloop_quit(void);
+void mainloop_exit_success(void);
+void mainloop_exit_failure(void);
+int mainloop_run(void);
+void mainloop_finish(void);
+
+int mainloop_add_fd(int fd, uint32_t events, mainloop_event_func callback,
+				void *user_data, mainloop_destroy_func destroy);
+int mainloop_modify_fd(int fd, uint32_t events);
+int mainloop_remove_fd(int fd);
+
+int mainloop_add_timeout(unsigned int msec, mainloop_timeout_func callback,
+				void *user_data, mainloop_destroy_func destroy);
+int mainloop_modify_timeout(int fd, unsigned int msec);
+int mainloop_remove_timeout(int id);
+
+int mainloop_set_signal(sigset_t *mask, mainloop_signal_func callback,
+				void *user_data, mainloop_destroy_func destroy);
diff --git a/bluez/queue.c b/bluez/queue.c
new file mode 100644
index 0000000..f1795dc
--- /dev/null
+++ b/bluez/queue.c
@@ -0,0 +1,417 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2012-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
+ *
+ */
+
+#include <bluetooth/bluetooth.h>
+
+#include "util.h"
+#include "queue.h"
+
+struct queue {
+	int ref_count;
+	struct queue_entry *head;
+	struct queue_entry *tail;
+	unsigned int entries;
+};
+
+static struct queue *queue_ref(struct queue *queue)
+{
+	if (!queue)
+		return NULL;
+
+	__sync_fetch_and_add(&queue->ref_count, 1);
+
+	return queue;
+}
+
+static void queue_unref(struct queue *queue)
+{
+	if (__sync_sub_and_fetch(&queue->ref_count, 1))
+		return;
+
+	free(queue);
+}
+
+struct queue *queue_new(void)
+{
+	struct queue *queue;
+
+	queue = new0(struct queue, 1);
+	if (!queue)
+		return NULL;
+
+	queue->head = NULL;
+	queue->tail = NULL;
+	queue->entries = 0;
+
+	return queue_ref(queue);
+}
+
+void queue_destroy(struct queue *queue, queue_destroy_func_t destroy)
+{
+	if (!queue)
+		return;
+
+	queue_remove_all(queue, NULL, NULL, destroy);
+
+	queue_unref(queue);
+}
+
+static struct queue_entry *queue_entry_ref(struct queue_entry *entry)
+{
+	if (!entry)
+		return NULL;
+
+	__sync_fetch_and_add(&entry->ref_count, 1);
+
+	return entry;
+}
+
+static void queue_entry_unref(struct queue_entry *entry)
+{
+	if (__sync_sub_and_fetch(&entry->ref_count, 1))
+		return;
+
+	free(entry);
+}
+
+static struct queue_entry *queue_entry_new(void *data)
+{
+	struct queue_entry *entry;
+
+	entry = new0(struct queue_entry, 1);
+	if (!entry)
+		return NULL;
+
+	entry->data = data;
+
+	return queue_entry_ref(entry);
+}
+
+bool queue_push_tail(struct queue *queue, void *data)
+{
+	struct queue_entry *entry;
+
+	if (!queue)
+		return false;
+
+	entry = queue_entry_new(data);
+	if (!entry)
+		return false;
+
+	if (queue->tail)
+		queue->tail->next = entry;
+
+	queue->tail = entry;
+
+	if (!queue->head)
+		queue->head = entry;
+
+	queue->entries++;
+
+	return true;
+}
+
+bool queue_push_head(struct queue *queue, void *data)
+{
+	struct queue_entry *entry;
+
+	if (!queue)
+		return false;
+
+	entry = queue_entry_new(data);
+	if (!entry)
+		return false;
+
+	entry->next = queue->head;
+
+	queue->head = entry;
+
+	if (!queue->tail)
+		queue->tail = entry;
+
+	queue->entries++;
+
+	return true;
+}
+
+bool queue_push_after(struct queue *queue, void *entry, void *data)
+{
+	struct queue_entry *qentry, *tmp, *new_entry;
+
+	qentry = NULL;
+
+	if (!queue)
+		return false;
+
+	for (tmp = queue->head; tmp; tmp = tmp->next) {
+		if (tmp->data == entry) {
+			qentry = tmp;
+			break;
+		}
+	}
+
+	if (!qentry)
+		return false;
+
+	new_entry = queue_entry_new(data);
+	if (!new_entry)
+		return false;
+
+	new_entry->next = qentry->next;
+
+	if (!qentry->next)
+		queue->tail = new_entry;
+
+	qentry->next = new_entry;
+	queue->entries++;
+
+	return true;
+}
+
+void *queue_pop_head(struct queue *queue)
+{
+	struct queue_entry *entry;
+	void *data;
+
+	if (!queue || !queue->head)
+		return NULL;
+
+	entry = queue->head;
+
+	if (!queue->head->next) {
+		queue->head = NULL;
+		queue->tail = NULL;
+	} else
+		queue->head = queue->head->next;
+
+	data = entry->data;
+
+	queue_entry_unref(entry);
+	queue->entries--;
+
+	return data;
+}
+
+void *queue_peek_head(struct queue *queue)
+{
+	if (!queue || !queue->head)
+		return NULL;
+
+	return queue->head->data;
+}
+
+void *queue_peek_tail(struct queue *queue)
+{
+	if (!queue || !queue->tail)
+		return NULL;
+
+	return queue->tail->data;
+}
+
+void queue_foreach(struct queue *queue, queue_foreach_func_t function,
+							void *user_data)
+{
+	struct queue_entry *entry;
+
+	if (!queue || !function)
+		return;
+
+	entry = queue->head;
+	if (!entry)
+		return;
+
+	queue_ref(queue);
+	while (entry && queue->head && queue->ref_count > 1) {
+		struct queue_entry *next;
+
+		queue_entry_ref(entry);
+
+		function(entry->data, user_data);
+
+		next = entry->next;
+
+		queue_entry_unref(entry);
+
+		entry = next;
+	}
+	queue_unref(queue);
+}
+
+static bool direct_match(const void *a, const void *b)
+{
+	return a == b;
+}
+
+void *queue_find(struct queue *queue, queue_match_func_t function,
+							const void *match_data)
+{
+	struct queue_entry *entry;
+
+	if (!queue)
+		return NULL;
+
+	if (!function)
+		function = direct_match;
+
+	for (entry = queue->head; entry; entry = entry->next)
+		if (function(entry->data, match_data))
+			return entry->data;
+
+	return NULL;
+}
+
+bool queue_remove(struct queue *queue, void *data)
+{
+	struct queue_entry *entry, *prev;
+
+	if (!queue)
+		return false;
+
+	for (entry = queue->head, prev = NULL; entry;
+					prev = entry, entry = entry->next) {
+		if (entry->data != data)
+			continue;
+
+		if (prev)
+			prev->next = entry->next;
+		else
+			queue->head = entry->next;
+
+		if (!entry->next)
+			queue->tail = prev;
+
+		queue_entry_unref(entry);
+		queue->entries--;
+
+		return true;
+	}
+
+	return false;
+}
+
+void *queue_remove_if(struct queue *queue, queue_match_func_t function,
+							void *user_data)
+{
+	struct queue_entry *entry, *prev = NULL;
+
+	if (!queue || !function)
+		return NULL;
+
+	entry = queue->head;
+
+	while (entry) {
+		if (function(entry->data, user_data)) {
+			void *data;
+
+			if (prev)
+				prev->next = entry->next;
+			else
+				queue->head = entry->next;
+
+			if (!entry->next)
+				queue->tail = prev;
+
+			data = entry->data;
+
+			queue_entry_unref(entry);
+			queue->entries--;
+
+			return data;
+		} else {
+			prev = entry;
+			entry = entry->next;
+		}
+	}
+
+	return NULL;
+}
+
+unsigned int queue_remove_all(struct queue *queue, queue_match_func_t function,
+				void *user_data, queue_destroy_func_t destroy)
+{
+	struct queue_entry *entry;
+	unsigned int count = 0;
+
+	if (!queue)
+		return 0;
+
+	entry = queue->head;
+
+	if (function) {
+		while (entry) {
+			void *data;
+			unsigned int entries = queue->entries;
+
+			data = queue_remove_if(queue, function, user_data);
+			if (entries == queue->entries)
+				break;
+
+			if (destroy)
+				destroy(data);
+
+			count++;
+		}
+	} else {
+		queue->head = NULL;
+		queue->tail = NULL;
+		queue->entries = 0;
+
+		while (entry) {
+			struct queue_entry *tmp = entry;
+
+			entry = entry->next;
+
+			if (destroy)
+				destroy(tmp->data);
+
+			queue_entry_unref(tmp);
+			count++;
+		}
+	}
+
+	return count;
+}
+
+const struct queue_entry *queue_get_entries(struct queue *queue)
+{
+	if (!queue)
+		return NULL;
+
+	return queue->head;
+}
+
+unsigned int queue_length(struct queue *queue)
+{
+	if (!queue)
+		return 0;
+
+	return queue->entries;
+}
+
+bool queue_isempty(struct queue *queue)
+{
+	if (!queue)
+		return true;
+
+	return queue->entries == 0;
+}
diff --git a/bluez/queue.h b/bluez/queue.h
new file mode 100644
index 0000000..3bc8d2e
--- /dev/null
+++ b/bluez/queue.h
@@ -0,0 +1,65 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2012-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
+ *
+ */
+
+#include <stdbool.h>
+
+typedef void (*queue_destroy_func_t)(void *data);
+
+struct queue;
+
+struct queue_entry {
+	int ref_count;
+	void *data;
+	struct queue_entry *next;
+};
+
+struct queue *queue_new(void);
+void queue_destroy(struct queue *queue, queue_destroy_func_t destroy);
+
+bool queue_push_tail(struct queue *queue, void *data);
+bool queue_push_head(struct queue *queue, void *data);
+bool queue_push_after(struct queue *queue, void *entry, void *data);
+void *queue_pop_head(struct queue *queue);
+void *queue_peek_head(struct queue *queue);
+void *queue_peek_tail(struct queue *queue);
+
+typedef void (*queue_foreach_func_t)(void *data, void *user_data);
+
+void queue_foreach(struct queue *queue, queue_foreach_func_t function,
+							void *user_data);
+
+typedef bool (*queue_match_func_t)(const void *data, const void *match_data);
+
+void *queue_find(struct queue *queue, queue_match_func_t function,
+							const void *match_data);
+
+bool queue_remove(struct queue *queue, void *data);
+void *queue_remove_if(struct queue *queue, queue_match_func_t function,
+							void *user_data);
+unsigned int queue_remove_all(struct queue *queue, queue_match_func_t function,
+				void *user_data, queue_destroy_func_t destroy);
+
+const struct queue_entry *queue_get_entries(struct queue *queue);
+
+unsigned int queue_length(struct queue *queue);
+bool queue_isempty(struct queue *queue);
diff --git a/bluez/timeout-mainloop.c b/bluez/timeout-mainloop.c
new file mode 100644
index 0000000..f099312
--- /dev/null
+++ b/bluez/timeout-mainloop.c
@@ -0,0 +1,85 @@
+/*
+ *
+ *  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.
+ *
+ */
+
+#include <stdlib.h>
+
+#include "mainloop.h"
+#include "util.h"
+#include "timeout.h"
+
+struct timeout_data {
+	int id;
+	timeout_func_t func;
+	timeout_destroy_func_t destroy;
+	unsigned int timeout;
+	void *user_data;
+};
+
+static void timeout_callback(int id, void *user_data)
+{
+	struct timeout_data *data = user_data;
+
+	if (data->func(data->user_data) &&
+			!mainloop_modify_timeout(data->id, data->timeout))
+		return;
+
+	mainloop_remove_timeout(data->id);
+}
+
+static void timeout_destroy(void *user_data)
+{
+	struct timeout_data *data = user_data;
+
+	if (data->destroy)
+		data->destroy(data->user_data);
+
+	free(data);
+}
+
+unsigned int timeout_add(unsigned int timeout, timeout_func_t func,
+			void *user_data, timeout_destroy_func_t destroy)
+{
+	struct timeout_data *data;
+
+	data = new0(struct timeout_data, 1);
+	if (!data)
+		return 0;
+
+	data->func = func;
+	data->user_data = user_data;
+	data->timeout = timeout;
+	data->destroy = destroy;
+
+	data->id = mainloop_add_timeout(timeout, timeout_callback, data,
+							timeout_destroy);
+	if (data->id < 0) {
+		free(data);
+		return 0;
+	}
+
+	return (unsigned int) data->id;
+}
+
+void timeout_remove(unsigned int id)
+{
+	if (!id)
+		return;
+
+	mainloop_remove_timeout((int) id);
+}
diff --git a/bluez/timeout.h b/bluez/timeout.h
new file mode 100644
index 0000000..4930ce1
--- /dev/null
+++ b/bluez/timeout.h
@@ -0,0 +1,27 @@
+/*
+ *
+ *  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.
+ *
+ */
+
+#include <stdbool.h>
+
+typedef bool (*timeout_func_t)(void *user_data);
+typedef void (*timeout_destroy_func_t)(void *user_data);
+
+unsigned int timeout_add(unsigned int timeout, timeout_func_t func,
+			void *user_data, timeout_destroy_func_t destroy);
+void timeout_remove(unsigned int id);
diff --git a/bluez/util.c b/bluez/util.c
new file mode 100644
index 0000000..d7faea7
--- /dev/null
+++ b/bluez/util.c
@@ -0,0 +1,135 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2012-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
+ *
+ */
+
+#include <bluetooth/bluetooth.h>
+
+#include <stdio.h>
+#include <ctype.h>
+#include <stdarg.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <unistd.h>
+#include <dirent.h>
+#include <limits.h>
+#include <string.h>
+
+#include "util.h"
+
+void util_debug(util_debug_func_t function, void *user_data,
+						const char *format, ...)
+{
+	char str[78];
+	va_list ap;
+
+	if (!function || !format)
+		return;
+
+	va_start(ap, format);
+	vsnprintf(str, sizeof(str), format, ap);
+	va_end(ap);
+
+	function(str, user_data);
+}
+
+void util_hexdump(const char dir, const unsigned char *buf, size_t len,
+				util_debug_func_t function, void *user_data)
+{
+	static const char hexdigits[] = "0123456789abcdef";
+	char str[68];
+	size_t i;
+
+	if (!function || !len)
+		return;
+
+	str[0] = dir;
+
+	for (i = 0; i < len; i++) {
+		str[((i % 16) * 3) + 1] = ' ';
+		str[((i % 16) * 3) + 2] = hexdigits[buf[i] >> 4];
+		str[((i % 16) * 3) + 3] = hexdigits[buf[i] & 0xf];
+		str[(i % 16) + 51] = isprint(buf[i]) ? buf[i] : '.';
+
+		if ((i + 1) % 16 == 0) {
+			str[49] = ' ';
+			str[50] = ' ';
+			str[67] = '\0';
+			function(str, user_data);
+			str[0] = ' ';
+		}
+	}
+
+	if (i % 16 > 0) {
+		size_t j;
+		for (j = (i % 16); j < 16; j++) {
+			str[(j * 3) + 1] = ' ';
+			str[(j * 3) + 2] = ' ';
+			str[(j * 3) + 3] = ' ';
+			str[j + 51] = ' ';
+		}
+		str[49] = ' ';
+		str[50] = ' ';
+		str[67] = '\0';
+		function(str, user_data);
+	}
+}
+
+/* Helper for getting the dirent type in case readdir returns DT_UNKNOWN */
+unsigned char util_get_dt(const char *parent, const char *name)
+{
+	char filename[PATH_MAX];
+	struct stat st;
+
+	snprintf(filename, sizeof(filename), "%s/%s", parent, name);
+	if (lstat(filename, &st) == 0 && S_ISDIR(st.st_mode))
+		return DT_DIR;
+
+	return DT_UNKNOWN;
+}
+
+/* Helpers for bitfield operations */
+
+/* Find unique id in range from 1 to max but no bigger then
+ * sizeof(int) * 8. ffs() is used since it is POSIX standard
+ */
+uint8_t util_get_uid(unsigned int *bitmap, uint8_t max)
+{
+	uint8_t id;
+
+	id = ffs(~*bitmap);
+
+	if (!id || id > max)
+		return 0;
+
+	*bitmap |= 1 << (id - 1);
+
+	return id;
+}
+
+/* Clear id bit in bitmap */
+void util_clear_uid(unsigned int *bitmap, uint8_t id)
+{
+	if (!id)
+		return;
+
+	*bitmap &= ~(1 << (id - 1));
+}
diff --git a/bluez/util.h b/bluez/util.h
new file mode 100644
index 0000000..30b7d92
--- /dev/null
+++ b/bluez/util.h
@@ -0,0 +1,157 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2012-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
+ *
+ */
+
+#include <stdint.h>
+#include <stdlib.h>
+#include <alloca.h>
+#include <byteswap.h>
+
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+#define le16_to_cpu(val) (val)
+#define le32_to_cpu(val) (val)
+#define le64_to_cpu(val) (val)
+#define cpu_to_le16(val) (val)
+#define cpu_to_le32(val) (val)
+#define cpu_to_le64(val) (val)
+#define be16_to_cpu(val) bswap_16(val)
+#define be32_to_cpu(val) bswap_32(val)
+#define be64_to_cpu(val) bswap_64(val)
+#define cpu_to_be16(val) bswap_16(val)
+#define cpu_to_be32(val) bswap_32(val)
+#define cpu_to_be64(val) bswap_64(val)
+#elif __BYTE_ORDER == __BIG_ENDIAN
+#define le16_to_cpu(val) bswap_16(val)
+#define le32_to_cpu(val) bswap_32(val)
+#define le64_to_cpu(val) bswap_64(val)
+#define cpu_to_le16(val) bswap_16(val)
+#define cpu_to_le32(val) bswap_32(val)
+#define cpu_to_le64(val) bswap_64(val)
+#define be16_to_cpu(val) (val)
+#define be32_to_cpu(val) (val)
+#define be64_to_cpu(val) (val)
+#define cpu_to_be16(val) (val)
+#define cpu_to_be32(val) (val)
+#define cpu_to_be64(val) (val)
+#else
+#error "Unknown byte order"
+#endif
+
+#define get_unaligned(ptr)			\
+__extension__ ({				\
+	struct __attribute__((packed)) {	\
+		__typeof__(*(ptr)) __v;		\
+	} *__p = (__typeof__(__p)) (ptr);	\
+	__p->__v;				\
+})
+
+#define put_unaligned(val, ptr)			\
+do {						\
+	struct __attribute__((packed)) {	\
+		__typeof__(*(ptr)) __v;		\
+	} *__p = (__typeof__(__p)) (ptr);	\
+	__p->__v = (val);			\
+} while (0)
+
+#define PTR_TO_UINT(p) ((unsigned int) ((uintptr_t) (p)))
+#define UINT_TO_PTR(u) ((void *) ((uintptr_t) (u)))
+
+#define PTR_TO_INT(p) ((int) ((intptr_t) (p)))
+#define INT_TO_PTR(u) ((void *) ((intptr_t) (u)))
+
+#define new0(t, n) ((t*) calloc((n), sizeof(t)))
+#define newa(t, n) ((t*) alloca(sizeof(t)*(n)))
+#define malloc0(n) (calloc((n), 1))
+
+typedef void (*util_debug_func_t)(const char *str, void *user_data);
+
+void util_debug(util_debug_func_t function, void *user_data,
+						const char *format, ...)
+					__attribute__((format(printf, 3, 4)));
+
+void util_hexdump(const char dir, const unsigned char *buf, size_t len,
+				util_debug_func_t function, void *user_data);
+
+unsigned char util_get_dt(const char *parent, const char *name);
+
+uint8_t util_get_uid(unsigned int *bitmap, uint8_t max);
+void util_clear_uid(unsigned int *bitmap, uint8_t id);
+
+static inline uint16_t get_le16(const void *ptr)
+{
+	return le16_to_cpu(get_unaligned((const uint16_t *) ptr));
+}
+
+static inline uint16_t get_be16(const void *ptr)
+{
+	return be16_to_cpu(get_unaligned((const uint16_t *) ptr));
+}
+
+static inline uint32_t get_le32(const void *ptr)
+{
+	return le32_to_cpu(get_unaligned((const uint32_t *) ptr));
+}
+
+static inline uint32_t get_be32(const void *ptr)
+{
+	return be32_to_cpu(get_unaligned((const uint32_t *) ptr));
+}
+
+static inline uint64_t get_le64(const void *ptr)
+{
+	return le64_to_cpu(get_unaligned((const uint64_t *) ptr));
+}
+
+static inline uint64_t get_be64(const void *ptr)
+{
+	return be64_to_cpu(get_unaligned((const uint64_t *) ptr));
+}
+
+static inline void put_le16(uint16_t val, void *dst)
+{
+	put_unaligned(cpu_to_le16(val), (uint16_t *) dst);
+}
+
+static inline void put_be16(uint16_t val, const void *ptr)
+{
+	put_unaligned(cpu_to_be16(val), (uint16_t *) ptr);
+}
+
+static inline void put_le32(uint32_t val, void *dst)
+{
+	put_unaligned(cpu_to_le32(val), (uint32_t *) dst);
+}
+
+static inline void put_be32(uint32_t val, void *dst)
+{
+	put_unaligned(cpu_to_be32(val), (uint32_t *) dst);
+}
+
+static inline void put_le64(uint64_t val, void *dst)
+{
+	put_unaligned(cpu_to_le64(val), (uint64_t *) dst);
+}
+
+static inline void put_be64(uint64_t val, void *dst)
+{
+	put_unaligned(cpu_to_be64(val), (uint64_t *) dst);
+}
diff --git a/bluez/uuid.c b/bluez/uuid.c
new file mode 100644
index 0000000..6a10fbb
--- /dev/null
+++ b/bluez/uuid.c
@@ -0,0 +1,311 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2011  Nokia Corporation
+ *  Copyright (C) 2011  Marcel Holtmann <marcel@holtmann.org>
+ *
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program 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 General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#include <bluetooth/bluetooth.h>
+
+#include <string.h>
+#include <stdlib.h>
+#include <errno.h>
+
+#include "uuid.h"
+
+static uint128_t bluetooth_base_uuid = {
+	.data = {	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00,
+			0x80, 0x00, 0x00, 0x80, 0x5F, 0x9B, 0x34, 0xFB }
+};
+
+#define BASE_UUID16_OFFSET	2
+#define BASE_UUID32_OFFSET	0
+
+static void bt_uuid16_to_uuid128(const bt_uuid_t *src, bt_uuid_t *dst)
+{
+	uint16_t be16;
+
+	dst->value.u128 = bluetooth_base_uuid;
+	dst->type = BT_UUID128;
+
+	/*
+	 * No matter the system: 128-bit UUIDs should be stored
+	 * as big-endian. 16-bit UUIDs are stored on host order.
+	 */
+
+	be16 = htons(src->value.u16);
+	memcpy(&dst->value.u128.data[BASE_UUID16_OFFSET], &be16, sizeof(be16));
+}
+
+static void bt_uuid32_to_uuid128(const bt_uuid_t *src, bt_uuid_t *dst)
+{
+	uint32_t be32;
+
+	dst->value.u128 = bluetooth_base_uuid;
+	dst->type = BT_UUID128;
+
+	/*
+	 * No matter the system: 128-bit UUIDs should be stored
+	 * as big-endian. 32-bit UUIDs are stored on host order.
+	 */
+
+	be32 = htonl(src->value.u32);
+	memcpy(&dst->value.u128.data[BASE_UUID32_OFFSET], &be32, sizeof(be32));
+}
+
+void bt_uuid_to_uuid128(const bt_uuid_t *src, bt_uuid_t *dst)
+{
+	switch (src->type) {
+	case BT_UUID128:
+		*dst = *src;
+		break;
+	case BT_UUID32:
+		bt_uuid32_to_uuid128(src, dst);
+		break;
+	case BT_UUID16:
+		bt_uuid16_to_uuid128(src, dst);
+		break;
+	case BT_UUID_UNSPEC:
+	default:
+		break;
+	}
+}
+
+static int bt_uuid128_cmp(const bt_uuid_t *u1, const bt_uuid_t *u2)
+{
+	return memcmp(&u1->value.u128, &u2->value.u128, sizeof(uint128_t));
+}
+
+int bt_uuid16_create(bt_uuid_t *btuuid, uint16_t value)
+{
+	memset(btuuid, 0, sizeof(bt_uuid_t));
+	btuuid->type = BT_UUID16;
+	btuuid->value.u16 = value;
+
+	return 0;
+}
+
+int bt_uuid32_create(bt_uuid_t *btuuid, uint32_t value)
+{
+	memset(btuuid, 0, sizeof(bt_uuid_t));
+	btuuid->type = BT_UUID32;
+	btuuid->value.u32 = value;
+
+	return 0;
+}
+
+int bt_uuid128_create(bt_uuid_t *btuuid, uint128_t value)
+{
+	memset(btuuid, 0, sizeof(bt_uuid_t));
+	btuuid->type = BT_UUID128;
+	btuuid->value.u128 = value;
+
+	return 0;
+}
+
+int bt_uuid_cmp(const bt_uuid_t *uuid1, const bt_uuid_t *uuid2)
+{
+	bt_uuid_t u1, u2;
+
+	bt_uuid_to_uuid128(uuid1, &u1);
+	bt_uuid_to_uuid128(uuid2, &u2);
+
+	return bt_uuid128_cmp(&u1, &u2);
+}
+
+/*
+ * convert the UUID to string, copying a maximum of n characters.
+ */
+int bt_uuid_to_string(const bt_uuid_t *uuid, char *str, size_t n)
+{
+	if (!uuid) {
+		snprintf(str, n, "NULL");
+		return -EINVAL;
+	}
+
+	switch (uuid->type) {
+	case BT_UUID16:
+		snprintf(str, n, "%.4x", uuid->value.u16);
+		break;
+	case BT_UUID32:
+		snprintf(str, n, "%.8x", uuid->value.u32);
+		break;
+	case BT_UUID128: {
+		unsigned int   data0;
+		unsigned short data1;
+		unsigned short data2;
+		unsigned short data3;
+		unsigned int   data4;
+		unsigned short data5;
+
+		const uint8_t *data = (uint8_t *) &uuid->value.u128;
+
+		memcpy(&data0, &data[0], 4);
+		memcpy(&data1, &data[4], 2);
+		memcpy(&data2, &data[6], 2);
+		memcpy(&data3, &data[8], 2);
+		memcpy(&data4, &data[10], 4);
+		memcpy(&data5, &data[14], 2);
+
+		snprintf(str, n, "%.8x-%.4x-%.4x-%.4x-%.8x%.4x",
+				ntohl(data0), ntohs(data1),
+				ntohs(data2), ntohs(data3),
+				ntohl(data4), ntohs(data5));
+		}
+		break;
+	case BT_UUID_UNSPEC:
+	default:
+		snprintf(str, n, "Type of UUID (%x) unknown.", uuid->type);
+		return -EINVAL;	/* Enum type of UUID not set */
+	}
+
+	return 0;
+}
+
+static inline int is_uuid128(const char *string)
+{
+	return (strlen(string) == 36 &&
+			string[8] == '-' &&
+			string[13] == '-' &&
+			string[18] == '-' &&
+			string[23] == '-');
+}
+
+static inline int is_base_uuid128(const char *string)
+{
+	uint16_t uuid;
+	char dummy;
+
+	if (!is_uuid128(string))
+		return 0;
+
+	return sscanf(string,
+		"0000%04hx-0000-1000-8000-00805%1[fF]9%1[bB]34%1[fF]%1[bB]",
+		&uuid, &dummy, &dummy, &dummy, &dummy) == 5;
+}
+
+static inline int is_uuid32(const char *string)
+{
+	return (strlen(string) == 8 || strlen(string) == 10);
+}
+
+static inline int is_uuid16(const char *string)
+{
+	return (strlen(string) == 4 || strlen(string) == 6);
+}
+
+static int bt_string_to_uuid16(bt_uuid_t *uuid, const char *string)
+{
+	uint16_t u16;
+	char *endptr = NULL;
+
+	u16 = strtol(string, &endptr, 16);
+	if (endptr && (*endptr == '\0' || *endptr == '-')) {
+		bt_uuid16_create(uuid, u16);
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int bt_string_to_uuid32(bt_uuid_t *uuid, const char *string)
+{
+	uint32_t u32;
+	char *endptr = NULL;
+
+	u32 = strtol(string, &endptr, 16);
+	if (endptr && *endptr == '\0') {
+		bt_uuid32_create(uuid, u32);
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int bt_string_to_uuid128(bt_uuid_t *uuid, const char *string)
+{
+	uint32_t data0, data4;
+	uint16_t data1, data2, data3, data5;
+	uint128_t u128;
+	uint8_t *val = (uint8_t *) &u128;
+
+	if (sscanf(string, "%08x-%04hx-%04hx-%04hx-%08x%04hx",
+				&data0, &data1, &data2,
+				&data3, &data4, &data5) != 6)
+		return -EINVAL;
+
+	data0 = htonl(data0);
+	data1 = htons(data1);
+	data2 = htons(data2);
+	data3 = htons(data3);
+	data4 = htonl(data4);
+	data5 = htons(data5);
+
+	memcpy(&val[0], &data0, 4);
+	memcpy(&val[4], &data1, 2);
+	memcpy(&val[6], &data2, 2);
+	memcpy(&val[8], &data3, 2);
+	memcpy(&val[10], &data4, 4);
+	memcpy(&val[14], &data5, 2);
+
+	bt_uuid128_create(uuid, u128);
+
+	return 0;
+}
+
+int bt_string_to_uuid(bt_uuid_t *uuid, const char *string)
+{
+	if (is_base_uuid128(string))
+		return bt_string_to_uuid16(uuid, string + 4);
+	else if (is_uuid128(string))
+		return bt_string_to_uuid128(uuid, string);
+	else if (is_uuid32(string))
+		return bt_string_to_uuid32(uuid, string);
+	else if (is_uuid16(string))
+		return bt_string_to_uuid16(uuid, string);
+
+	return -EINVAL;
+}
+
+int bt_uuid_strcmp(const void *a, const void *b)
+{
+	return strcasecmp(a, b);
+}
+
+int bt_uuid_to_le(const bt_uuid_t *src, void *dst)
+{
+	bt_uuid_t uuid;
+
+	switch (src->type) {
+	case BT_UUID16:
+		bt_put_le16(src->value.u16, dst);
+		return 0;
+	case BT_UUID32:
+		bt_uuid_to_uuid128(src, &uuid);
+		/* Fallthrough */
+	case BT_UUID128:
+		/* Convert from 128-bit BE to LE */
+		bswap_128(&src->value.u128, dst);
+		return 0;
+	case BT_UUID_UNSPEC:
+	default:
+		return -EINVAL;
+	}
+}
diff --git a/bluez/uuid.h b/bluez/uuid.h
new file mode 100644
index 0000000..2dcfe9e
--- /dev/null
+++ b/bluez/uuid.h
@@ -0,0 +1,181 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2011  Nokia Corporation
+ *  Copyright (C) 2011  Marcel Holtmann <marcel@holtmann.org>
+ *
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program 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 General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifndef __BLUETOOTH_UUID_H
+#define __BLUETOOTH_UUID_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+
+#define GENERIC_AUDIO_UUID	"00001203-0000-1000-8000-00805f9b34fb"
+
+#define HSP_HS_UUID		"00001108-0000-1000-8000-00805f9b34fb"
+#define HSP_AG_UUID		"00001112-0000-1000-8000-00805f9b34fb"
+
+#define HFP_HS_UUID		"0000111e-0000-1000-8000-00805f9b34fb"
+#define HFP_AG_UUID		"0000111f-0000-1000-8000-00805f9b34fb"
+
+#define ADVANCED_AUDIO_UUID	"0000110d-0000-1000-8000-00805f9b34fb"
+
+#define A2DP_SOURCE_UUID	"0000110a-0000-1000-8000-00805f9b34fb"
+#define A2DP_SINK_UUID		"0000110b-0000-1000-8000-00805f9b34fb"
+
+#define AVRCP_REMOTE_UUID	"0000110e-0000-1000-8000-00805f9b34fb"
+#define AVRCP_TARGET_UUID	"0000110c-0000-1000-8000-00805f9b34fb"
+
+#define PANU_UUID		"00001115-0000-1000-8000-00805f9b34fb"
+#define NAP_UUID		"00001116-0000-1000-8000-00805f9b34fb"
+#define GN_UUID			"00001117-0000-1000-8000-00805f9b34fb"
+#define BNEP_SVC_UUID		"0000000f-0000-1000-8000-00805f9b34fb"
+
+#define PNPID_UUID		"00002a50-0000-1000-8000-00805f9b34fb"
+#define DEVICE_INFORMATION_UUID	"0000180a-0000-1000-8000-00805f9b34fb"
+
+#define GATT_UUID		"00001801-0000-1000-8000-00805f9b34fb"
+#define IMMEDIATE_ALERT_UUID	"00001802-0000-1000-8000-00805f9b34fb"
+#define LINK_LOSS_UUID		"00001803-0000-1000-8000-00805f9b34fb"
+#define TX_POWER_UUID		"00001804-0000-1000-8000-00805f9b34fb"
+#define BATTERY_UUID		"0000180f-0000-1000-8000-00805f9b34fb"
+#define SCAN_PARAMETERS_UUID	"00001813-0000-1000-8000-00805f9b34fb"
+
+#define SAP_UUID		"0000112D-0000-1000-8000-00805f9b34fb"
+
+#define HEART_RATE_UUID			"0000180d-0000-1000-8000-00805f9b34fb"
+#define HEART_RATE_MEASUREMENT_UUID	"00002a37-0000-1000-8000-00805f9b34fb"
+#define BODY_SENSOR_LOCATION_UUID	"00002a38-0000-1000-8000-00805f9b34fb"
+#define HEART_RATE_CONTROL_POINT_UUID	"00002a39-0000-1000-8000-00805f9b34fb"
+
+#define HEALTH_THERMOMETER_UUID		"00001809-0000-1000-8000-00805f9b34fb"
+#define TEMPERATURE_MEASUREMENT_UUID	"00002a1c-0000-1000-8000-00805f9b34fb"
+#define TEMPERATURE_TYPE_UUID		"00002a1d-0000-1000-8000-00805f9b34fb"
+#define INTERMEDIATE_TEMPERATURE_UUID	"00002a1e-0000-1000-8000-00805f9b34fb"
+#define MEASUREMENT_INTERVAL_UUID	"00002a21-0000-1000-8000-00805f9b34fb"
+
+#define CYCLING_SC_UUID		"00001816-0000-1000-8000-00805f9b34fb"
+#define CSC_MEASUREMENT_UUID	"00002a5b-0000-1000-8000-00805f9b34fb"
+#define CSC_FEATURE_UUID	"00002a5c-0000-1000-8000-00805f9b34fb"
+#define SENSOR_LOCATION_UUID	"00002a5d-0000-1000-8000-00805f9b34fb"
+#define SC_CONTROL_POINT_UUID	"00002a55-0000-1000-8000-00805f9b34fb"
+
+#define RFCOMM_UUID_STR		"00000003-0000-1000-8000-00805f9b34fb"
+
+#define HDP_UUID		"00001400-0000-1000-8000-00805f9b34fb"
+#define HDP_SOURCE_UUID		"00001401-0000-1000-8000-00805f9b34fb"
+#define HDP_SINK_UUID		"00001402-0000-1000-8000-00805f9b34fb"
+
+#define HID_UUID		"00001124-0000-1000-8000-00805f9b34fb"
+
+#define DUN_GW_UUID		"00001103-0000-1000-8000-00805f9b34fb"
+
+#define GAP_UUID		"00001800-0000-1000-8000-00805f9b34fb"
+#define PNP_UUID		"00001200-0000-1000-8000-00805f9b34fb"
+
+#define SPP_UUID		"00001101-0000-1000-8000-00805f9b34fb"
+
+#define OBEX_SYNC_UUID		"00001104-0000-1000-8000-00805f9b34fb"
+#define OBEX_OPP_UUID		"00001105-0000-1000-8000-00805f9b34fb"
+#define OBEX_FTP_UUID		"00001106-0000-1000-8000-00805f9b34fb"
+#define OBEX_PCE_UUID		"0000112e-0000-1000-8000-00805f9b34fb"
+#define OBEX_PSE_UUID		"0000112f-0000-1000-8000-00805f9b34fb"
+#define OBEX_PBAP_UUID		"00001130-0000-1000-8000-00805f9b34fb"
+#define OBEX_MAS_UUID		"00001132-0000-1000-8000-00805f9b34fb"
+#define OBEX_MNS_UUID		"00001133-0000-1000-8000-00805f9b34fb"
+#define OBEX_MAP_UUID		"00001134-0000-1000-8000-00805f9b34fb"
+
+/* GATT UUIDs section */
+#define GATT_PRIM_SVC_UUID				0x2800
+#define GATT_SND_SVC_UUID				0x2801
+#define GATT_INCLUDE_UUID				0x2802
+#define GATT_CHARAC_UUID				0x2803
+
+/* GATT Characteristic Types */
+#define GATT_CHARAC_DEVICE_NAME				0x2A00
+#define GATT_CHARAC_APPEARANCE				0x2A01
+#define GATT_CHARAC_PERIPHERAL_PRIV_FLAG		0x2A02
+#define GATT_CHARAC_RECONNECTION_ADDRESS		0x2A03
+#define GATT_CHARAC_PERIPHERAL_PREF_CONN		0x2A04
+#define GATT_CHARAC_SERVICE_CHANGED			0x2A05
+#define GATT_CHARAC_SYSTEM_ID				0x2A23
+#define GATT_CHARAC_MODEL_NUMBER_STRING			0x2A24
+#define GATT_CHARAC_SERIAL_NUMBER_STRING		0x2A25
+#define GATT_CHARAC_FIRMWARE_REVISION_STRING		0x2A26
+#define GATT_CHARAC_HARDWARE_REVISION_STRING		0x2A27
+#define GATT_CHARAC_SOFTWARE_REVISION_STRING		0x2A28
+#define GATT_CHARAC_MANUFACTURER_NAME_STRING		0x2A29
+#define GATT_CHARAC_PNP_ID				0x2A50
+
+/* GATT Characteristic Descriptors */
+#define GATT_CHARAC_EXT_PROPER_UUID			0x2900
+#define GATT_CHARAC_USER_DESC_UUID			0x2901
+#define GATT_CLIENT_CHARAC_CFG_UUID			0x2902
+#define GATT_SERVER_CHARAC_CFG_UUID			0x2903
+#define GATT_CHARAC_FMT_UUID				0x2904
+#define GATT_CHARAC_AGREG_FMT_UUID			0x2905
+#define GATT_CHARAC_VALID_RANGE_UUID			0x2906
+#define GATT_EXTERNAL_REPORT_REFERENCE			0x2907
+#define GATT_REPORT_REFERENCE				0x2908
+
+typedef struct {
+	enum {
+		BT_UUID_UNSPEC = 0,
+		BT_UUID16 = 16,
+		BT_UUID32 = 32,
+		BT_UUID128 = 128,
+	} type;
+	union {
+		uint16_t  u16;
+		uint32_t  u32;
+		uint128_t u128;
+	} value;
+} bt_uuid_t;
+
+int bt_uuid_strcmp(const void *a, const void *b);
+
+int bt_uuid16_create(bt_uuid_t *btuuid, uint16_t value);
+int bt_uuid32_create(bt_uuid_t *btuuid, uint32_t value);
+int bt_uuid128_create(bt_uuid_t *btuuid, uint128_t value);
+
+int bt_uuid_cmp(const bt_uuid_t *uuid1, const bt_uuid_t *uuid2);
+void bt_uuid_to_uuid128(const bt_uuid_t *src, bt_uuid_t *dst);
+
+#define MAX_LEN_UUID_STR 37
+
+int bt_uuid_to_string(const bt_uuid_t *uuid, char *str, size_t n);
+int bt_string_to_uuid(bt_uuid_t *uuid, const char *string);
+
+int bt_uuid_to_le(const bt_uuid_t *uuid, void *dst);
+
+static inline int bt_uuid_len(const bt_uuid_t *uuid)
+{
+	return uuid->type / 8;
+}
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __BLUETOOTH_UUID_H */
diff --git a/dfu.c b/dfu.c
index d59acac..be1f952 100644
--- a/dfu.c
+++ b/dfu.c
@@ -4,10 +4,36 @@ void
 dfu (const char *bdaddr, const char *type, const char *version, uint8_t * dat,
      size_t dat_sz, uint8_t * bin, size_t bin_sz)
 {
+ BLE *b;
+ uint8_t buf[32];
 
 
-  bt_thing (bdaddr);
+  ble_init();
 
 
 
+
+
+ do {
+
+  b=ble_open (bdaddr);
+  
+  if (!b) 
+	break;
+
+  if (ble_register_notify(b)) break;
+
+  ble_send_cp(
+
+  ble_close (b);
+  return;
+
+
+} while (0);
+
+
+
+  ble_close (b);
+	exit(EXIT_FAILURE);
+
 }
diff --git a/dfu.h b/dfu.h
index 54a6ac2..f85bc62 100644
--- a/dfu.h
+++ b/dfu.h
@@ -8,26 +8,32 @@
 
 typedef enum
 {
-    BLE_DFU_RESP_VAL_SUCCESS = 1,                                       /**< Success.*/
-    BLE_DFU_RESP_VAL_INVALID_STATE,                                     /**< Invalid state.*/
-    BLE_DFU_RESP_VAL_NOT_SUPPORTED,                                     /**< Operation not supported.*/
-    BLE_DFU_RESP_VAL_DATA_SIZE,                                         /**< Data size exceeds limit.*/
-    BLE_DFU_RESP_VAL_CRC_ERROR,                                         /**< CRC Error.*/
-    BLE_DFU_RESP_VAL_OPER_FAILED                                        /**< Operation failed.*/
+  BLE_DFU_RESP_VAL_SUCCESS = 1,                                         /**< Success.*/
+  BLE_DFU_RESP_VAL_INVALID_STATE,                                       /**< Invalid state.*/
+  BLE_DFU_RESP_VAL_NOT_SUPPORTED,                                       /**< Operation not supported.*/
+  BLE_DFU_RESP_VAL_DATA_SIZE,                                           /**< Data size exceeds limit.*/
+  BLE_DFU_RESP_VAL_CRC_ERROR,                                           /**< CRC Error.*/
+  BLE_DFU_RESP_VAL_OPER_FAILED                                          /**< Operation failed.*/
 } ble_dfu_resp_val_t;
 
 enum
 {
-    OP_CODE_START_DFU          = 1,                                             /**< Value of the Op code field for 'Start DFU' command.*/
-    OP_CODE_RECEIVE_INIT       = 2,                                             /**< Value of the Op code field for 'Initialize DFU parameters' command.*/
-    OP_CODE_RECEIVE_FW         = 3,                                             /**< Value of the Op code field for 'Receive firmware image' command.*/
-    OP_CODE_VALIDATE           = 4,                                             /**< Value of the Op code field for 'Validate firmware' command.*/
-    OP_CODE_ACTIVATE_N_RESET   = 5,                                             /**< Value of the Op code field for 'Activate & Reset' command.*/
-    OP_CODE_SYS_RESET          = 6,                                             /**< Value of the Op code field for 'Reset System' command.*/
-    OP_CODE_IMAGE_SIZE_REQ     = 7,                                             /**< Value of the Op code field for 'Report received image size' command.*/
-    OP_CODE_PKT_RCPT_NOTIF_REQ = 8,                                             /**< Value of the Op code field for 'Request packet receipt notification.*/
-    OP_CODE_RESPONSE           = 16,                                            /**< Value of the Op code field for 'Response.*/
-    OP_CODE_PKT_RCPT_NOTIF     = 17                                             /**< Value of the Op code field for 'Packets Receipt Notification'.*/
+  OP_CODE_START_DFU = 1,                                                        /**< Value of the Op code field for 'Start DFU' command.*/
+  OP_CODE_RECEIVE_INIT = 2,                                                     /**< Value of the Op code field for 'Initialize DFU parameters' command.*/
+  OP_CODE_RECEIVE_FW = 3,                                                       /**< Value of the Op code field for 'Receive firmware image' command.*/
+  OP_CODE_VALIDATE = 4,                                                         /**< Value of the Op code field for 'Validate firmware' command.*/
+  OP_CODE_ACTIVATE_N_RESET = 5,                                                 /**< Value of the Op code field for 'Activate & Reset' command.*/
+  OP_CODE_SYS_RESET = 6,                                                        /**< Value of the Op code field for 'Reset System' command.*/
+  OP_CODE_IMAGE_SIZE_REQ = 7,                                                   /**< Value of the Op code field for 'Report received image size' command.*/
+  OP_CODE_PKT_RCPT_NOTIF_REQ = 8,                                               /**< Value of the Op code field for 'Request packet receipt notification.*/
+  OP_CODE_RESPONSE = 16,                                                        /**< Value of the Op code field for 'Response.*/
+  OP_CODE_PKT_RCPT_NOTIF = 17                                                   /**< Value of the Op code field for 'Packets Receipt Notification'.*/
 };
 
+enum {
+ DFU_MODE_SOFTDEVICE  = 1, 
+ DFU_MODE_BOOTLOADER  = 2,
+ DFU_MODE_SD_BL       = 3,
+ DFU_MODE_APPLICATION = 4
+};
 
diff --git a/manifest.c b/manifest.c
index 795e833..dbda6ef 100644
--- a/manifest.c
+++ b/manifest.c
@@ -25,7 +25,6 @@ struct manifest *
 parse_manifest (const char *str)
 {
   json_object *json, *manifest;
-  enum json_type type;
   enum json_tokener_error jerr = json_tokener_success;
   struct manifest *m;
 
diff --git a/manifest.h b/manifest.h
index 5588535..89bdeb0 100644
--- a/manifest.h
+++ b/manifest.h
@@ -1,7 +1,7 @@
-struct manifest  {
-	const char *type;
-	const char *dat_file;
-	const char *bin_file;
-        const char *dfu_version;
+struct manifest
+{
+  const char *type;
+  const char *dat_file;
+  const char *bin_file;
+  const char *dfu_version;
 };
-
diff --git a/nrfdfu.c b/nrfdfu.c
index 96f99d9..ab58464 100644
--- a/nrfdfu.c
+++ b/nrfdfu.c
@@ -65,10 +65,11 @@ main (int argc, char *argv[])
   bin_size = read_file_from_zip (zip, m->bin_file, &bin);
 
 
-  printf ("%d bytes init_data, %d bytes firmware\n", dat_size, bin_size);
+  printf ("%u bytes init_data, %u bytes firmware\n",(unsigned) dat_size, (unsigned) bin_size);
 
 
   dfu (bdaddr, m->type, m->dfu_version, dat, dat_size, bin, bin_size);
 
 
+return EXIT_SUCCESS;
 }
diff --git a/project.h b/project.h
index 4d8853e..be211ee 100644
--- a/project.h
+++ b/project.h
@@ -16,10 +16,12 @@
 //#include <bluetooth/uuid.h>
 
 
+#include "bluez/uuid.h"
+#include "bluez/att.h"
+#include "bluez/queue.h"
+#include "bluez/gatt-db.h"
+
 #include "manifest.h"
 #include "ble.h"
 
 #include "prototypes.h"
-
-
-
diff --git a/prototypes.h b/prototypes.h
index 015fb69..ab6773c 100644
--- a/prototypes.h
+++ b/prototypes.h
@@ -8,7 +8,10 @@ extern void fatal_zip(struct zip *zip);
 extern struct zip *open_zip(const char *fn);
 extern size_t read_file_from_zip(struct zip *zip, const char *fn, void *_buf);
 /* ble.c */
-extern int bt_thing(const char *bdaddr);
+extern void ble_close(BLE *ble);
+extern void ble_init(void);
+extern BLE *ble_open(const char *bdaddr);
+extern int ble_register_notify(BLE *ble);
 /* manifest.c */
 extern json_object *_json_object_object_get(json_object *obj, const char *name);
 extern struct manifest *parse_manifest(const char *str);
diff --git a/util.c b/util.c
index 554b4f9..9be342b 100644
--- a/util.c
+++ b/util.c
@@ -7,6 +7,7 @@ xmalloc (size_t s)
   char *ret = malloc (s);
   if (!ret)
     abort ();
+  return ret;
 }
 
 void *
@@ -15,4 +16,5 @@ xrealloc (void *p, size_t s)
   char *ret = realloc (p, s);
   if (!ret)
     abort ();
+  return ret;
 }