text: /*
   *
   *  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
   *
   */
  
  #ifdef HAVE_CONFIG_H
  #include <config.h>
  #endif
  
  #include <sys/uio.h>
  #include <errno.h>
  
  #include "src/shared/att.h"
  #include "lib/bluetooth.h"
  #include "lib/uuid.h"
  #include "src/shared/queue.h"
  #include "src/shared/gatt-db.h"
  #include "src/shared/gatt-server.h"
  #include "src/shared/gatt-helpers.h"
  #include "src/shared/util.h"
  
  #ifndef MAX
  #define MAX(a, b) ((a) > (b) ? (a) : (b))
  #endif
  
  #ifndef MIN
  #define MIN(a, b) ((a) < (b) ? (a) : (b))
  #endif
  
  /*
   * TODO: This is an arbitrary limit. Come up with something reasonable or
   * perhaps an API to set this value if there is a use case for it.
   */
  #define DEFAULT_MAX_PREP_QUEUE_LEN 30
  
  struct async_read_op {
  	struct bt_gatt_server *server;
  	uint8_t opcode;
  	bool done;
  	uint8_t *pdu;
  	size_t pdu_len;
  	size_t value_len;
  	struct queue *db_data;
  };
  
  struct async_write_op {
  	struct bt_gatt_server *server;
  	uint8_t opcode;
  };
  
  struct prep_write_data {
  	struct bt_gatt_server *server;
  	uint8_t *value;
  	uint16_t handle;
  	uint16_t offset;
  	uint16_t length;
  
  	bool reliable_supported;
  };
  
  static void prep_write_data_destroy(void *user_data)
  {
  	struct prep_write_data *data = user_data;
  
  	free(data->value);
  	free(data);
  }
  
  struct bt_gatt_server {
  	struct gatt_db *db;
  	struct bt_att *att;
  	int ref_count;
  	uint16_t mtu;
  
  	unsigned int mtu_id;
  	unsigned int read_by_grp_type_id;
  	unsigned int read_by_type_id;
  	unsigned int find_info_id;
  	unsigned int find_by_type_value_id;
  	unsigned int write_id;
  	unsigned int write_cmd_id;
  	unsigned int read_id;
  	unsigned int read_blob_id;
  	unsigned int read_multiple_id;
  	unsigned int prep_write_id;
  	unsigned int exec_write_id;
  
  	uint8_t min_enc_size;
  
  	struct queue *prep_queue;
  	unsigned int max_prep_queue_len;
  
  	struct async_read_op *pending_read_op;
  	struct async_write_op *pending_write_op;
  
  	bt_gatt_server_debug_func_t debug_callback;
  	bt_gatt_server_destroy_func_t debug_destroy;
  	void *debug_data;
  };
  
  static void bt_gatt_server_free(struct bt_gatt_server *server)
  {
  	if (server->debug_destroy)
  		server->debug_destroy(server->debug_data);
  
  	bt_att_unregister(server->att, server->mtu_id);
  	bt_att_unregister(server->att, server->read_by_grp_type_id);
  	bt_att_unregister(server->att, server->read_by_type_id);
  	bt_att_unregister(server->att, server->find_info_id);
  	bt_att_unregister(server->att, server->find_by_type_value_id);
  	bt_att_unregister(server->att, server->write_id);
  	bt_att_unregister(server->att, server->write_cmd_id);
  	bt_att_unregister(server->att, server->read_id);
  	bt_att_unregister(server->att, server->read_blob_id);
  	bt_att_unregister(server->att, server->read_multiple_id);
  	bt_att_unregister(server->att, server->prep_write_id);
  	bt_att_unregister(server->att, server->exec_write_id);
  
  	if (server->pending_read_op)
  		server->pending_read_op->server = NULL;
  
  	if (server->pending_write_op)
  		server->pending_write_op->server = NULL;
  
  	queue_destroy(server->prep_queue, prep_write_data_destroy);
  
  	gatt_db_unref(server->db);
  	bt_att_unref(server->att);
  	free(server);
  }
  
  static bool get_uuid_le(const uint8_t *uuid, size_t len, bt_uuid_t *out_uuid)
  {
  	uint128_t u128;
  
  	switch (len) {
  	case 2:
  		bt_uuid16_create(out_uuid, get_le16(uuid));
  		return true;
  	case 16:
  		bswap_128(uuid, &u128.data);
  		bt_uuid128_create(out_uuid, u128);
  		return true;
  	default:
  		return false;
  	}
  
  	return false;
  }
  
  static void attribute_read_cb(struct gatt_db_attribute *attrib, int err,
  					const uint8_t *value, size_t length,
  					void *user_data)
  {
  	struct iovec *iov = user_data;
  
  	iov->iov_base = (void *) value;
  	iov->iov_len = length;
  }
  
  static bool encode_read_by_grp_type_rsp(struct gatt_db *db, struct queue *q,
  						struct bt_att *att,
  						uint16_t mtu, uint8_t *pdu,
  						uint16_t *len)
  {
  	int iter = 0;
  	uint16_t start_handle, end_handle;
  	struct iovec value;
  	uint8_t data_val_len;
  
  	*len = 0;
  
  	while (queue_peek_head(q)) {
  		struct gatt_db_attribute *attrib = queue_pop_head(q);
  
  		value.iov_base = NULL;
  		value.iov_len = 0;
  
  		/*
  		 * This should never be deferred to the read callback for
  		 * primary/secondary service declarations.
  		 */
  		if (!gatt_db_attribute_read(attrib, 0,
  						BT_ATT_OP_READ_BY_GRP_TYPE_REQ,
  						att, attribute_read_cb,
  						&value) || !value.iov_len)
  			return false;
  
  		/*
  		 * Use the first attribute to determine the length of each
  		 * attribute data unit. Stop the list when a different attribute
  		 * value is seen.
  		 */
  		if (iter == 0) {
  			data_val_len = MIN(MIN((unsigned)mtu - 6, 251),
  								value.iov_len);
  			pdu[0] = data_val_len + 4;
  			iter++;
  		} else if (value.iov_len != data_val_len)
  			break;
  
  		/* Stop if this unit would surpass the MTU */
  		if (iter + data_val_len + 4 > mtu - 1)
  			break;
  
  		gatt_db_attribute_get_service_handles(attrib, &start_handle,
  								&end_handle);
  
  		put_le16(start_handle, pdu + iter);
  		put_le16(end_handle, pdu + iter + 2);
  		memcpy(pdu + iter + 4, value.iov_base, data_val_len);
  
  		iter += data_val_len + 4;
  	}
  
  	*len = iter;
  
  	return true;
  }
  
  static void read_by_grp_type_cb(uint8_t opcode, const void *pdu,
  					uint16_t length, void *user_data)
  {
  	struct bt_gatt_server *server = user_data;
  	uint16_t start, end;
  	bt_uuid_t type;
  	bt_uuid_t prim, snd;
  	uint16_t mtu = bt_att_get_mtu(server->att);
  	uint8_t rsp_pdu[mtu];
  	uint16_t rsp_len;
  	uint8_t ecode = 0;
  	uint16_t ehandle = 0;
  	struct queue *q = NULL;
  
  	if (length != 6 && length != 20) {
  		ecode = BT_ATT_ERROR_INVALID_PDU;
  		goto error;
  	}
  
  	q = queue_new();
  
  	start = get_le16(pdu);
  	end = get_le16(pdu + 2);
  	get_uuid_le(pdu + 4, length - 4, &type);
  
  	util_debug(server->debug_callback, server->debug_data,
  				"Read By Grp Type - start: 0x%04x end: 0x%04x",
  				start, end);
  
  	if (!start || !end) {
  		ecode = BT_ATT_ERROR_INVALID_HANDLE;
  		goto error;
  	}
  
  	ehandle = start;
  
  	if (start > end) {
  		ecode = BT_ATT_ERROR_INVALID_HANDLE;
  		goto error;
  	}
  
  	/*
  	 * GATT defines that only the <<Primary Service>> and
  	 * <<Secondary Service>> group types can be used for the
  	 * "Read By Group Type" request (Core v4.1, Vol 3, sec 2.5.3). Return an
  	 * error if any other group type is given.
  	 */
  	bt_uuid16_create(&prim, GATT_PRIM_SVC_UUID);
  	bt_uuid16_create(&snd, GATT_SND_SVC_UUID);
  	if (bt_uuid_cmp(&type, &prim) && bt_uuid_cmp(&type, &snd)) {
  		ecode = BT_ATT_ERROR_UNSUPPORTED_GROUP_TYPE;
  		goto error;
  	}
  
  	gatt_db_read_by_group_type(server->db, start, end, type, q);
  
  	if (queue_isempty(q)) {
  		ecode = BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND;
  		goto error;
  	}
  
  	if (!encode_read_by_grp_type_rsp(server->db, q, server->att, mtu,
  							rsp_pdu, &rsp_len)) {
  		ecode = BT_ATT_ERROR_UNLIKELY;
  		goto error;
  	}
  
  	queue_destroy(q, NULL);
  
  	bt_att_send(server->att, BT_ATT_OP_READ_BY_GRP_TYPE_RSP,
  							rsp_pdu, rsp_len,
  							NULL, NULL, NULL);
  
  	return;
  
  error:
  	queue_destroy(q, NULL);
  	bt_att_send_error_rsp(server->att, opcode, ehandle, ecode);
  }
  
  static void async_read_op_destroy(struct async_read_op *op)
  {
  	if (op->server)
  		op->server->pending_read_op = NULL;
  
  	queue_destroy(op->db_data, NULL);
  	free(op->pdu);
  	free(op);
  }
  
  static void process_read_by_type(struct async_read_op *op);
  
  static void read_by_type_read_complete_cb(struct gatt_db_attribute *attr,
  						int err, const uint8_t *value,
  						size_t len, void *user_data)
  {
  	struct async_read_op *op = user_data;
  	struct bt_gatt_server *server = op->server;
  	uint16_t mtu;
  	uint16_t handle;
  
  	if (!server) {
  		async_read_op_destroy(op);
  		return;
  	}
  
  	mtu = bt_att_get_mtu(server->att);
  	handle = gatt_db_attribute_get_handle(attr);
  
  	/* Terminate the operation if there was an error */
  	if (err) {
  		bt_att_send_error_rsp(server->att, BT_ATT_OP_READ_BY_TYPE_REQ,
  								handle, err);
  		async_read_op_destroy(op);
  		return;
  	}
  
  	if (op->pdu_len == 0) {
  		op->value_len = MIN(MIN((unsigned) mtu - 4, 253), len);
  		op->pdu[0] = op->value_len + 2;
  		op->pdu_len++;
  	} else if (len != op->value_len) {
  		op->done = true;
  		goto done;
  	}
  
  	/* Stop if this would surpass the MTU */
  	if (op->pdu_len + op->value_len + 2 > (unsigned) mtu - 1) {
  		op->done = true;
  		goto done;
  	}
  
  	/* Encode the current value */
  	put_le16(handle, op->pdu + op->pdu_len);
  	memcpy(op->pdu + op->pdu_len + 2, value, op->value_len);
  
  	op->pdu_len += op->value_len + 2;
  
  	if (op->pdu_len == (unsigned) mtu - 1)
  		op->done = true;
  
  done:
  	process_read_by_type(op);
  }
  
  static bool check_min_key_size(uint8_t min_size, uint8_t size)
  {
  	if (!min_size || !size)
  		return true;
  
  	return min_size <= size;
  }
  
  static uint8_t check_permissions(struct bt_gatt_server *server,
  				struct gatt_db_attribute *attr, uint32_t mask)
  {
  	uint8_t enc_size;
  	uint32_t perm;
  	int security;
  
  	perm = gatt_db_attribute_get_permissions(attr);
  
  	if (perm && mask & BT_ATT_PERM_READ && !(perm & BT_ATT_PERM_READ))
  		return BT_ATT_ERROR_READ_NOT_PERMITTED;
  
  	if (perm && mask & BT_ATT_PERM_WRITE && !(perm & BT_ATT_PERM_WRITE))
  		return BT_ATT_ERROR_WRITE_NOT_PERMITTED;
  
  	perm &= mask;
  	if (!perm)
  		return 0;
  
  	security = bt_att_get_security(server->att, &enc_size);
  	if (security < 0)
  		return BT_ATT_ERROR_UNLIKELY;
  
  	if (perm & BT_ATT_PERM_SECURE) {
  		if (security < BT_ATT_SECURITY_FIPS)
  			return BT_ATT_ERROR_AUTHENTICATION;
  
  		if (!check_min_key_size(server->min_enc_size, enc_size))
  			return BT_ATT_ERROR_INSUFFICIENT_ENCRYPTION_KEY_SIZE;
  	}
  
  	if (perm & BT_ATT_PERM_AUTHEN) {
  		if (security < BT_ATT_SECURITY_HIGH)
  			return BT_ATT_ERROR_AUTHENTICATION;
  
  		if (!check_min_key_size(server->min_enc_size, enc_size))
  			return BT_ATT_ERROR_INSUFFICIENT_ENCRYPTION_KEY_SIZE;
  	}
  
  	if (perm & BT_ATT_PERM_ENCRYPT) {
  		if (security < BT_ATT_SECURITY_MEDIUM)
  			return BT_ATT_ERROR_INSUFFICIENT_ENCRYPTION;
  
  		if (!check_min_key_size(server->min_enc_size, enc_size))
  			return BT_ATT_ERROR_INSUFFICIENT_ENCRYPTION_KEY_SIZE;
  	}
  
  	return 0;
  }
  
  static void process_read_by_type(struct async_read_op *op)
  {
  	struct bt_gatt_server *server = op->server;
  	uint8_t ecode;
  	struct gatt_db_attribute *attr;
  
  	attr = queue_pop_head(op->db_data);
  
  	if (op->done || !attr) {
  		bt_att_send(server->att, BT_ATT_OP_READ_BY_TYPE_RSP, op->pdu,
  								op->pdu_len,
  								NULL, NULL,
  								NULL);
  		async_read_op_destroy(op);
  		return;
  	}
  
  	ecode = check_permissions(server, attr, BT_ATT_PERM_READ |
  						BT_ATT_PERM_READ_AUTHEN |
  						BT_ATT_PERM_READ_ENCRYPT);
  	if (ecode)
  		goto error;
  
  	if (gatt_db_attribute_read(attr, 0, op->opcode, server->att,
  					read_by_type_read_complete_cb, op))
  		return;
  
  	ecode = BT_ATT_ERROR_UNLIKELY;
  
  error:
  	bt_att_send_error_rsp(server->att, BT_ATT_OP_READ_BY_TYPE_REQ,
  				gatt_db_attribute_get_handle(attr), ecode);
  	async_read_op_destroy(op);
  }
  
  static void read_by_type_cb(uint8_t opcode, const void *pdu,
  					uint16_t length, void *user_data)
  {
  	struct bt_gatt_server *server = user_data;
  	uint16_t start, end;
  	bt_uuid_t type;
  	uint16_t ehandle = 0;
  	uint8_t ecode;
  	struct queue *q = NULL;
  	struct async_read_op *op;
  
  	if (length != 6 && length != 20) {
  		ecode = BT_ATT_ERROR_INVALID_PDU;
  		goto error;
  	}
  
  	q = queue_new();
  
  	start = get_le16(pdu);
  	end = get_le16(pdu + 2);
  	get_uuid_le(pdu + 4, length - 4, &type);
  
  	util_debug(server->debug_callback, server->debug_data,
  				"Read By Type - start: 0x%04x end: 0x%04x",
  				start, end);
  
  	if (!start || !end) {
  		ecode = BT_ATT_ERROR_INVALID_HANDLE;
  		goto error;
  	}
  
  	ehandle = start;
  
  	if (start > end) {
  		ecode = BT_ATT_ERROR_INVALID_HANDLE;
  		goto error;
  	}
  
  	gatt_db_read_by_type(server->db, start, end, type, q);
  
  	if (queue_isempty(q)) {
  		ecode = BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND;
  		goto error;
  	}
  
  	if (server->pending_read_op) {
  		ecode = BT_ATT_ERROR_UNLIKELY;
  		goto error;
  	}
  
  	op = new0(struct async_read_op, 1);
  	op->pdu = malloc(bt_att_get_mtu(server->att));
  	if (!op->pdu) {
  		free(op);
  		ecode = BT_ATT_ERROR_INSUFFICIENT_RESOURCES;
  		goto error;
  	}
  
  	op->opcode = opcode;
  	op->server = server;
  	op->db_data = q;
  	server->pending_read_op = op;
  
  	process_read_by_type(op);
  
  	return;
  
  error:
  	bt_att_send_error_rsp(server->att, opcode, ehandle, ecode);
  	queue_destroy(q, NULL);
  }
  
  static bool encode_find_info_rsp(struct gatt_db *db, struct queue *q,
  						uint16_t mtu,
  						uint8_t *pdu, uint16_t *len)
  {
  	uint16_t handle;
  	struct gatt_db_attribute *attr;
  	const bt_uuid_t *type;
  	int uuid_len, cur_uuid_len;
  	int iter = 0;
  
  	*len = 0;
  
  	while (queue_peek_head(q)) {
  		attr = queue_pop_head(q);
  		handle = gatt_db_attribute_get_handle(attr);
  		type = gatt_db_attribute_get_type(attr);
  		if (!handle || !type)
  			return false;
  
  		cur_uuid_len = bt_uuid_len(type);
  
  		if (iter == 0) {
  			switch (cur_uuid_len) {
  			case 2:
  				uuid_len = 2;
  				pdu[0] = 0x01;
  				break;
  			case 4:
  			case 16:
  				uuid_len = 16;
  				pdu[0] = 0x02;
  				break;
  			default:
  				return false;
  			}
  
  			iter++;
  		} else if (cur_uuid_len != uuid_len)
  			break;
  
  		if (iter + uuid_len + 2 > mtu - 1)
  			break;
  
  		put_le16(handle, pdu + iter);
  		bt_uuid_to_le(type, pdu + iter + 2);
  
  		iter += uuid_len + 2;
  	}
  
  	*len = iter;
  
  	return true;
  }
  
  static void find_info_cb(uint8_t opcode, const void *pdu,
  					uint16_t length, void *user_data)
  {
  	struct bt_gatt_server *server = user_data;
  	uint16_t start, end;
  	uint16_t mtu = bt_att_get_mtu(server->att);
  	uint8_t rsp_pdu[mtu];
  	uint16_t rsp_len;
  	uint8_t ecode = 0;
  	uint16_t ehandle = 0;
  	struct queue *q = NULL;
  
  	if (length != 4) {
  		ecode = BT_ATT_ERROR_INVALID_PDU;
  		goto error;
  	}
  
  	q = queue_new();
  
  	start = get_le16(pdu);
  	end = get_le16(pdu + 2);
  
  	util_debug(server->debug_callback, server->debug_data,
  					"Find Info - start: 0x%04x end: 0x%04x",
  					start, end);
  
  	if (!start || !end) {
  		ecode = BT_ATT_ERROR_INVALID_HANDLE;
  		goto error;
  	}
  
  	ehandle = start;
  
  	if (start > end) {
  		ecode = BT_ATT_ERROR_INVALID_HANDLE;
  		goto error;
  	}
  
  	gatt_db_find_information(server->db, start, end, q);
  
  	if (queue_isempty(q)) {
  		ecode = BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND;
  		goto error;
  	}
  
  	if (!encode_find_info_rsp(server->db, q, mtu, rsp_pdu, &rsp_len)) {
  		ecode = BT_ATT_ERROR_UNLIKELY;
  		goto error;
  	}
  
  	bt_att_send(server->att, BT_ATT_OP_FIND_INFO_RSP, rsp_pdu, rsp_len,
  							NULL, NULL, NULL);
  	queue_destroy(q, NULL);
  
  	return;
  
  error:
  	bt_att_send_error_rsp(server->att, opcode, ehandle, ecode);
  	queue_destroy(q, NULL);
  
  }
  
  struct find_by_type_val_data {
  	uint8_t *pdu;
  	uint16_t len;
  	uint16_t mtu;
  	uint8_t ecode;
  };
  
  static void find_by_type_val_att_cb(struct gatt_db_attribute *attrib,
  								void *user_data)
  {
  	uint16_t handle, end_handle;
  	struct find_by_type_val_data *data = user_data;
  
  	if (data->ecode)
  		return;
  
  	if (data->len + 4 > data->mtu - 1)
  		return;
  
  	/*
  	 * This OP is only valid for Primary Service per the spec
  	 * page 562, so this should work.
  	 */
  	gatt_db_attribute_get_service_data(attrib, &handle, &end_handle, NULL,
  									NULL);
  
  	if (!handle || !end_handle) {
  		data->ecode = BT_ATT_ERROR_UNLIKELY;
  		return;
  	}
  
  	put_le16(handle, data->pdu + data->len);
  	put_le16(end_handle, data->pdu + data->len + 2);
  
  	data->len += 4;
  }
  
  static void find_by_type_val_cb(uint8_t opcode, const void *pdu,
  					uint16_t length, void *user_data)
  {
  	struct bt_gatt_server *server = user_data;
  	uint16_t start, end, uuid16;
  	struct find_by_type_val_data data;
  	uint16_t mtu = bt_att_get_mtu(server->att);
  	uint8_t rsp_pdu[mtu];
  	uint16_t ehandle = 0;
  	bt_uuid_t uuid;
  
  	if (length < 6) {
  		data.ecode = BT_ATT_ERROR_INVALID_PDU;
  		goto error;
  	}
  
  	data.pdu = rsp_pdu;
  	data.len = 0;
  	data.mtu = mtu;
  	data.ecode = 0;
  
  	start = get_le16(pdu);
  	end = get_le16(pdu + 2);
  	uuid16 = get_le16(pdu + 4);
  
  	util_debug(server->debug_callback, server->debug_data,
  			"Find By Type Value - start: 0x%04x end: 0x%04x uuid: 0x%04x",
  			start, end, uuid16);
  	ehandle = start;
  	if (start > end) {
  		data.ecode = BT_ATT_ERROR_INVALID_HANDLE;
  		goto error;
  	}
  
  	bt_uuid16_create(&uuid, uuid16);
  	gatt_db_find_by_type_value(server->db, start, end, &uuid, pdu + 6,
  							length - 6,
  							find_by_type_val_att_cb,
  							&data);
  
  	if (!data.len)
  		data.ecode = BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND;
  
  	if (data.ecode)
  		goto error;
  
  	bt_att_send(server->att, BT_ATT_OP_FIND_BY_TYPE_RSP, data.pdu,
  						data.len, NULL, NULL, NULL);
  
  	return;
  
  error:
  	bt_att_send_error_rsp(server->att, opcode, ehandle, data.ecode);
  }
  
  static void async_write_op_destroy(struct async_write_op *op)
  {
  	if (op->server)
  		op->server->pending_write_op = NULL;
  
  	free(op);
  }
  
  static void write_complete_cb(struct gatt_db_attribute *attr, int err,
  								void *user_data)
  {
  	struct async_write_op *op = user_data;
  	struct bt_gatt_server *server = op->server;
  	uint16_t handle;
  
  	if (!server || op->opcode == BT_ATT_OP_WRITE_CMD) {
  		async_write_op_destroy(op);
  		return;
  	}
  
  	handle = gatt_db_attribute_get_handle(attr);
  
  	if (err)
  		bt_att_send_error_rsp(server->att, op->opcode, handle, err);
  	else
  		bt_att_send(server->att, BT_ATT_OP_WRITE_RSP, NULL, 0,
  							NULL, NULL, NULL);
  
  	async_write_op_destroy(op);
  }
  
  static void write_cb(uint8_t opcode, const void *pdu,
  					uint16_t length, void *user_data)
  {
  	struct bt_gatt_server *server = user_data;
  	struct gatt_db_attribute *attr;
  	uint16_t handle = 0;
  	struct async_write_op *op = NULL;
  	uint8_t ecode;
  
  	if (length < 2) {
  		ecode = BT_ATT_ERROR_INVALID_PDU;
  		goto error;
  	}
  
  	handle = get_le16(pdu);
  	attr = gatt_db_get_attribute(server->db, handle);
  	if (!attr) {
  		ecode = BT_ATT_ERROR_INVALID_HANDLE;
  		goto error;
  	}
  
  	util_debug(server->debug_callback, server->debug_data,
  				"Write %s - handle: 0x%04x",
  				(opcode == BT_ATT_OP_WRITE_REQ) ? "Req" : "Cmd",
  				handle);
  
  	ecode = check_permissions(server, attr, BT_ATT_PERM_WRITE |
  						BT_ATT_PERM_WRITE_AUTHEN |
  						BT_ATT_PERM_WRITE_ENCRYPT);
  	if (ecode)
  		goto error;
  
  	if (server->pending_write_op) {
  		ecode = BT_ATT_ERROR_UNLIKELY;
  		goto error;
  	}
  
  	op = new0(struct async_write_op, 1);
  	op->server = server;
  	op->opcode = opcode;
  	server->pending_write_op = op;
  
  	if (gatt_db_attribute_write(attr, 0, pdu + 2, length - 2, opcode,
  							server->att,
  							write_complete_cb, op))
  		return;
  
  	async_write_op_destroy(op);
  
  	ecode = BT_ATT_ERROR_UNLIKELY;
  
  error:
  	if (opcode == BT_ATT_OP_WRITE_CMD)
  		return;
  
  	bt_att_send_error_rsp(server->att, opcode, handle, ecode);
  }
  
  static uint8_t get_read_rsp_opcode(uint8_t opcode)
  {
  
  	switch (opcode) {
  	case BT_ATT_OP_READ_REQ:
  		return BT_ATT_OP_READ_RSP;
  	case BT_ATT_OP_READ_BLOB_REQ:
  		return BT_ATT_OP_READ_BLOB_RSP;
  	default:
  		/*
  		 * Should never happen
  		 *
  		 * TODO: It would be nice to have a debug-mode assert macro
  		 * for development builds. This way bugs could be easily catched
  		 * during development and there would be self documenting code
  		 * that wouldn't be crash release builds.
  		 */
  		return 0;
  	}
  
  	return 0;
  }
  
  static void read_complete_cb(struct gatt_db_attribute *attr, int err,
  					const uint8_t *value, size_t len,
  					void *user_data)
  {
  	struct async_read_op *op = user_data;
  	struct bt_gatt_server *server = op->server;
  	uint8_t rsp_opcode;
  	uint16_t mtu;
  	uint16_t handle;
  
  	if (!server) {
  		async_read_op_destroy(op);
  		return;
  	}
  
  	mtu = bt_att_get_mtu(server->att);
  	handle = gatt_db_attribute_get_handle(attr);
  
  	if (err) {
  		bt_att_send_error_rsp(server->att, op->opcode, handle, err);
  		async_read_op_destroy(op);
  		return;
  	}
  
  	rsp_opcode = get_read_rsp_opcode(op->opcode);
  
  	bt_att_send(server->att, rsp_opcode, len ? value : NULL,
  						MIN((unsigned) mtu - 1, len),
  						NULL, NULL, NULL);
  	async_read_op_destroy(op);
  }
  
  static void handle_read_req(struct bt_gatt_server *server, uint8_t opcode,
  								uint16_t handle,
  								uint16_t offset)
  {
  	struct gatt_db_attribute *attr;
  	uint8_t ecode;
  	struct async_read_op *op = NULL;
  
  	attr = gatt_db_get_attribute(server->db, handle);
  	if (!attr) {
  		ecode = BT_ATT_ERROR_INVALID_HANDLE;
  		goto error;
  	}
  
  	util_debug(server->debug_callback, server->debug_data,
  			"Read %sReq - handle: 0x%04x",
  			opcode == BT_ATT_OP_READ_BLOB_REQ ? "Blob " : "",
  			handle);
  
  	ecode = check_permissions(server, attr, BT_ATT_PERM_READ |
  						BT_ATT_PERM_READ_AUTHEN |
  						BT_ATT_PERM_READ_ENCRYPT);
  	if (ecode)
  		goto error;
  
  	if (server->pending_read_op) {
  		ecode = BT_ATT_ERROR_UNLIKELY;
  		goto error;
  	}
  
  	op = new0(struct async_read_op, 1);
  	op->opcode = opcode;
  	op->server = server;
  	server->pending_read_op = op;
  
  	if (gatt_db_attribute_read(attr, offset, opcode, server->att,
  							read_complete_cb, op))
  		return;
  
  	ecode = BT_ATT_ERROR_UNLIKELY;
  
  error:
  	if (op)
  		async_read_op_destroy(op);
  
  	bt_att_send_error_rsp(server->att, opcode, handle, ecode);
  }
  
  static void read_cb(uint8_t opcode, const void *pdu,
  					uint16_t length, void *user_data)
  {
  	struct bt_gatt_server *server = user_data;
  	uint16_t handle;
  
  	if (length != 2) {
  		bt_att_send_error_rsp(server->att, opcode, 0,
  						BT_ATT_ERROR_INVALID_PDU);
  		return;
  	}
  
  	handle = get_le16(pdu);
  
  	handle_read_req(server, opcode, handle, 0);
  }
  
  static void read_blob_cb(uint8_t opcode, const void *pdu,
  					uint16_t length, void *user_data)
  {
  	struct bt_gatt_server *server = user_data;
  	uint16_t handle, offset;
  
  	if (length != 4) {
  		bt_att_send_error_rsp(server->att, opcode, 0,
  						BT_ATT_ERROR_INVALID_PDU);
  		return;
  	}
  
  	handle = get_le16(pdu);
  	offset = get_le16(pdu + 2);
  
  	handle_read_req(server, opcode, handle, offset);
  }
  
  struct read_multiple_resp_data {
  	struct bt_gatt_server *server;
  	uint16_t *handles;
  	size_t cur_handle;
  	size_t num_handles;
  	uint8_t *rsp_data;
  	size_t length;
  	size_t mtu;
  };
  
  static void read_multiple_resp_data_free(struct read_multiple_resp_data *data)
  {
  	free(data->handles);
  	free(data->rsp_data);
  	free(data);
  }
  
  static void read_multiple_complete_cb(struct gatt_db_attribute *attr, int err,
  					const uint8_t *value, size_t len,
  					void *user_data)
  {
  	struct read_multiple_resp_data *data = user_data;
  	struct gatt_db_attribute *next_attr;
  	uint16_t handle = gatt_db_attribute_get_handle(attr);
  	uint8_t ecode;
  
  	if (err != 0) {
  		bt_att_send_error_rsp(data->server->att,
  					BT_ATT_OP_READ_MULT_REQ, handle, err);
  		read_multiple_resp_data_free(data);
  		return;
  	}
  
  	ecode = check_permissions(data->server, attr, BT_ATT_PERM_READ |
  						BT_ATT_PERM_READ_AUTHEN |
  						BT_ATT_PERM_READ_ENCRYPT);
  	if (ecode) {
  		bt_att_send_error_rsp(data->server->att,
  					BT_ATT_OP_READ_MULT_REQ, handle, ecode);
  		read_multiple_resp_data_free(data);
  		return;
  	}
  
  	len = MIN(len, data->mtu - data->length - 1);
  
  	memcpy(data->rsp_data + data->length, value, len);
  	data->length += len;
  
  	data->cur_handle++;
  
  	if ((data->length >= data->mtu - 1) ||
  				(data->cur_handle == data->num_handles)) {
  		bt_att_send(data->server->att, BT_ATT_OP_READ_MULT_RSP,
  				data->rsp_data, data->length, NULL, NULL, NULL);
  		read_multiple_resp_data_free(data);
  		return;
  	}
  
  	util_debug(data->server->debug_callback, data->server->debug_data,
  				"Read Multiple Req - #%zu of %zu: 0x%04x",
  				data->cur_handle + 1, data->num_handles,
  				data->handles[data->cur_handle]);
  
  	next_attr = gatt_db_get_attribute(data->server->db,
  					data->handles[data->cur_handle]);
  
  	if (!next_attr) {
  		bt_att_send_error_rsp(data->server->att,
  					BT_ATT_OP_READ_MULT_REQ,
  					data->handles[data->cur_handle],
  					BT_ATT_ERROR_INVALID_HANDLE);
  		read_multiple_resp_data_free(data);
  		return;
  	}
  
  	if (!gatt_db_attribute_read(next_attr, 0, BT_ATT_OP_READ_MULT_REQ,
  					data->server->att,
  					read_multiple_complete_cb, data)) {
  		bt_att_send_error_rsp(data->server->att,
  						BT_ATT_OP_READ_MULT_REQ,
  						data->handles[data->cur_handle],
  						BT_ATT_ERROR_UNLIKELY);
  		read_multiple_resp_data_free(data);
  	}
  }
  
  static void read_multiple_cb(uint8_t opcode, const void *pdu,
  					uint16_t length, void *user_data)
  {
  	struct bt_gatt_server *server = user_data;
  	struct gatt_db_attribute *attr;
  	struct read_multiple_resp_data *data = NULL;
  	uint8_t ecode = BT_ATT_ERROR_UNLIKELY;
  	size_t i = 0;
  
  	if (length < 4) {
  		ecode = BT_ATT_ERROR_INVALID_PDU;
  		goto error;
  	}
  
  	data = new0(struct read_multiple_resp_data, 1);
  	data->handles = NULL;
  	data->rsp_data = NULL;
  	data->server = server;
  	data->num_handles = length / 2;
  	data->cur_handle = 0;
  	data->mtu = bt_att_get_mtu(server->att);
  	data->length = 0;
  	data->rsp_data = malloc(data->mtu - 1);
  
  	if (!data->rsp_data)
  		goto error;
  
  	data->handles = new0(uint16_t, data->num_handles);
  
  	for (i = 0; i < data->num_handles; i++)
  		data->handles[i] = get_le16(pdu + i * 2);
  
  	util_debug(server->debug_callback, server->debug_data,
  			"Read Multiple Req - %zu handles, 1st: 0x%04x",
  			data->num_handles, data->handles[0]);
  
  	attr = gatt_db_get_attribute(server->db, data->handles[0]);
  
  	if (!attr) {
  		ecode = BT_ATT_ERROR_INVALID_HANDLE;
  		goto error;
  	}
  
  	if (gatt_db_attribute_read(attr, 0, opcode, server->att,
  					read_multiple_complete_cb, data))
  		return;
  
  error:
  	if (data)
  		read_multiple_resp_data_free(data);
  
  	bt_att_send_error_rsp(server->att, opcode, 0, ecode);
  }
  
  static bool append_prep_data(struct prep_write_data *prep_data, uint16_t handle,
  					uint16_t length, uint8_t *value)
  {
  	uint8_t *val;
  	uint16_t len;
  
  	if (!length)
  		return true;
  
  	len = prep_data->length + length;
  
  	val = realloc(prep_data->value, len);
  	if (!val)
  		return false;
  
  	memcpy(val + prep_data->length, value, length);
  
  	prep_data->value = val;
  	prep_data->length = len;
  
  	return true;
  }
  
  static bool is_reliable_write_supported(const struct bt_gatt_server  *server,
  							uint16_t handle)
  {
  	struct gatt_db_attribute *attr;
  	uint16_t ext_prop;
  
  	attr = gatt_db_get_attribute(server->db, handle);
  	if (!attr)
  		return false;
  
  	if (!gatt_db_attribute_get_char_data(attr, NULL, NULL, NULL, &ext_prop,
  									NULL))
  		return false;
  
  	return (ext_prop & BT_GATT_CHRC_EXT_PROP_RELIABLE_WRITE);
  }
  
  static bool prep_data_new(struct bt_gatt_server *server,
  					uint16_t handle, uint16_t offset,
  					uint16_t length, uint8_t *value)
  {
  	struct prep_write_data *prep_data;
  
  	prep_data = new0(struct prep_write_data, 1);
  
  	if (!append_prep_data(prep_data, handle, length, value)) {
  		prep_write_data_destroy(prep_data);
  		return false;
  	}
  
  	prep_data->server = server;
  	prep_data->handle = handle;
  	prep_data->offset = offset;
  
  	/*
  	 * Handle is the value handle. We need characteristic declaration
  	 * handle which in BlueZ is handle_value -1
  	 */
  	prep_data->reliable_supported = is_reliable_write_supported(server,
  								handle - 1);
  
  	queue_push_tail(server->prep_queue, prep_data);
  
  	return true;
  }
  
  static bool store_prep_data(struct bt_gatt_server *server,
  					uint16_t handle, uint16_t offset,
  					uint16_t length, uint8_t *value)
  {
  	struct prep_write_data *prep_data = NULL;
  
  	/*
  	 * Now lets check if prep write is a continuation of long write
  	 * If so do aggregation of data
  	 */
  	prep_data = queue_peek_tail(server->prep_queue);
  	if (prep_data && (prep_data->handle == handle) &&
  			(offset == (prep_data->length + prep_data->offset)))
  		return append_prep_data(prep_data, handle, length, value);
  
  	return prep_data_new(server, handle, offset, length, value);
  }
  
  struct prep_write_complete_data {
  	void *pdu;
  	uint16_t length;
  	struct bt_gatt_server *server;
  };
  
  static void prep_write_complete_cb(struct gatt_db_attribute *attr, int err,
  								void *user_data)
  {
  	struct prep_write_complete_data *pwcd = user_data;
  	uint16_t handle = 0;
  	uint16_t offset;
  
  	handle = get_le16(pwcd->pdu);
  
  	if (err) {
  		bt_att_send_error_rsp(pwcd->server->att,
  					BT_ATT_OP_PREP_WRITE_REQ, handle, err);
  		free(pwcd->pdu);
  		free(pwcd);
  
  		return;
  	}
  
  	offset = get_le16(pwcd->pdu + 2);
  
  	if (!store_prep_data(pwcd->server, handle, offset, pwcd->length - 4,
  						&((uint8_t *) pwcd->pdu)[4]))
  		bt_att_send_error_rsp(pwcd->server->att,
  					BT_ATT_OP_PREP_WRITE_RSP, handle,
  					BT_ATT_ERROR_INSUFFICIENT_RESOURCES);
  
  	bt_att_send(pwcd->server->att, BT_ATT_OP_PREP_WRITE_RSP, pwcd->pdu,
  						pwcd->length, NULL, NULL, NULL);
  
  	free(pwcd->pdu);
  	free(pwcd);
  }
  
  static void prep_write_cb(uint8_t opcode, const void *pdu,
  					uint16_t length, void *user_data)
  {
  	struct bt_gatt_server *server = user_data;
  	uint16_t handle = 0;
  	uint16_t offset;
  	struct gatt_db_attribute *attr;
  	struct prep_write_complete_data *pwcd;
  	uint8_t ecode, status;
  
  	if (length < 4) {
  		ecode = BT_ATT_ERROR_INVALID_PDU;
  		goto error;
  	}
  
  	if (queue_length(server->prep_queue) >= server->max_prep_queue_len) {
  		ecode = BT_ATT_ERROR_PREPARE_QUEUE_FULL;
  		goto error;
  	}
  
  	handle = get_le16(pdu);
  	offset = get_le16(pdu + 2);
  
  	attr = gatt_db_get_attribute(server->db, handle);
  	if (!attr) {
  		ecode = BT_ATT_ERROR_INVALID_HANDLE;
  		goto error;
  	}
  
  	util_debug(server->debug_callback, server->debug_data,
  				"Prep Write Req - handle: 0x%04x", handle);
  
  	ecode = check_permissions(server, attr, BT_ATT_PERM_WRITE |
  						BT_ATT_PERM_WRITE_AUTHEN |
  						BT_ATT_PERM_WRITE_ENCRYPT);
  	if (ecode)
  		goto error;
  
  	pwcd = new0(struct prep_write_complete_data, 1);
  	pwcd->pdu = malloc(length);
  	memcpy(pwcd->pdu, pdu, length);
  	pwcd->length = length;
  	pwcd->server = server;
  
  	status = gatt_db_attribute_write(attr, offset, NULL, 0,
  						BT_ATT_OP_PREP_WRITE_REQ,
  						server->att,
  						prep_write_complete_cb, pwcd);
  
  	if (status)
  		return;
  
  	ecode = BT_ATT_ERROR_UNLIKELY;
  
  error:
  	bt_att_send_error_rsp(server->att, opcode, handle, ecode);
  }
  
  static void exec_next_prep_write(struct bt_gatt_server *server,
  						uint16_t ehandle, int err);
  
  static void exec_write_complete_cb(struct gatt_db_attribute *attr, int err,
  								void *user_data)
  {
  	struct bt_gatt_server *server = user_data;
  	uint16_t handle = gatt_db_attribute_get_handle(attr);
  
  	exec_next_prep_write(server, handle, err);
  }
  
  static void exec_next_prep_write(struct bt_gatt_server *server,
  						uint16_t ehandle, int err)
  {
  	struct prep_write_data *next = NULL;
  	struct gatt_db_attribute *attr;
  	bool status;
  
  	if (err)
  		goto error;
  
  	next = queue_pop_head(server->prep_queue);
  	if (!next) {
  		bt_att_send(server->att, BT_ATT_OP_EXEC_WRITE_RSP, NULL, 0,
  							NULL, NULL, NULL);
  		return;
  	}
  
  	attr = gatt_db_get_attribute(server->db, next->handle);
  	if (!attr) {
  		err = BT_ATT_ERROR_UNLIKELY;
  		goto error;
  	}
  
  	status = gatt_db_attribute_write(attr, next->offset,
  						next->value, next->length,
  						BT_ATT_OP_EXEC_WRITE_REQ,
  						server->att,
  						exec_write_complete_cb, server);
  
  	prep_write_data_destroy(next);
  
  	if (status)
  		return;
  
  	err = BT_ATT_ERROR_UNLIKELY;
  
  error:
  	queue_remove_all(server->prep_queue, NULL, NULL,
  						prep_write_data_destroy);
  
  	bt_att_send_error_rsp(server->att, BT_ATT_OP_EXEC_WRITE_REQ,
  								ehandle, err);
  }
  
  static bool find_no_reliable_characteristic(const void *data,
  						const void *match_data)
  {
  	const struct prep_write_data *prep_data = data;
  
  	return !prep_data->reliable_supported;
  }
  
  static void exec_write_cb(uint8_t opcode, const void *pdu,
  					uint16_t length, void *user_data)
  {
  	struct bt_gatt_server *server = user_data;
  	uint8_t flags;
  	uint8_t ecode;
  	bool write;
  	uint16_t ehandle = 0;
  
  	if (length != 1) {
  		ecode = BT_ATT_ERROR_INVALID_PDU;
  		goto error;
  	}
  
  	flags = ((uint8_t *) pdu)[0];
  
  	util_debug(server->debug_callback, server->debug_data,
  				"Exec Write Req - flags: 0x%02x", flags);
  
  	if (flags == 0x00)
  		write = false;
  	else if (flags == 0x01)
  		write = true;
  	else {
  		ecode = BT_ATT_ERROR_INVALID_PDU;
  		goto error;
  	}
  
  	if (!write) {
  		queue_remove_all(server->prep_queue, NULL, NULL,
  						prep_write_data_destroy);
  		bt_att_send(server->att, BT_ATT_OP_EXEC_WRITE_RSP, NULL, 0,
  							NULL, NULL, NULL);
  		return;
  	}
  
  	/* If there is more than one prep request, we are in reliable session */
  	if (queue_length(server->prep_queue) > 1) {
  		struct prep_write_data *prep_data;
  
  		prep_data = queue_find(server->prep_queue,
  					find_no_reliable_characteristic, NULL);
  		if (prep_data) {
  			ecode = BT_ATT_ERROR_REQUEST_NOT_SUPPORTED;
  			ehandle = prep_data->handle;
  			goto error;
  		}
  	}
  
  	exec_next_prep_write(server, 0, 0);
  
  	return;
  
  error:
  	queue_remove_all(server->prep_queue, NULL, NULL,
  						prep_write_data_destroy);
  	bt_att_send_error_rsp(server->att, opcode, ehandle, ecode);
  }
  
  static void exchange_mtu_cb(uint8_t opcode, const void *pdu,
  					uint16_t length, void *user_data)
  {
  	struct bt_gatt_server *server = user_data;
  	uint16_t client_rx_mtu;
  	uint16_t final_mtu;
  	uint8_t rsp_pdu[2];
  
  	if (length != 2) {
  		bt_att_send_error_rsp(server->att, opcode, 0,
  						BT_ATT_ERROR_INVALID_PDU);
  		return;
  	}
  
  	client_rx_mtu = get_le16(pdu);
  	final_mtu = MAX(MIN(client_rx_mtu, server->mtu), BT_ATT_DEFAULT_LE_MTU);
  
  	/* Respond with the server MTU */
  	put_le16(server->mtu, rsp_pdu);
  	bt_att_send(server->att, BT_ATT_OP_MTU_RSP, rsp_pdu, 2, NULL, NULL,
  									NULL);
  
  	/* Set MTU to be the minimum */
  	server->mtu = final_mtu;
  	bt_att_set_mtu(server->att, final_mtu);
  
  	util_debug(server->debug_callback, server->debug_data,
  			"MTU exchange complete, with MTU: %u", final_mtu);
  }
  
  static bool gatt_server_register_att_handlers(struct bt_gatt_server *server)
  {
  	/* Exchange MTU */
  	server->mtu_id = bt_att_register(server->att, BT_ATT_OP_MTU_REQ,
  								exchange_mtu_cb,
  								server, NULL);
  	if (!server->mtu_id)
  		return false;
  
  	/* Read By Group Type */
  	server->read_by_grp_type_id = bt_att_register(server->att,
  						BT_ATT_OP_READ_BY_GRP_TYPE_REQ,
  						read_by_grp_type_cb,
  						server, NULL);
  	if (!server->read_by_grp_type_id)
  		return false;
  
  	/* Read By Type */
  	server->read_by_type_id = bt_att_register(server->att,
  						BT_ATT_OP_READ_BY_TYPE_REQ,
  						read_by_type_cb,
  						server, NULL);
  	if (!server->read_by_type_id)
  		return false;
  
  	/* Find Information */
  	server->find_info_id = bt_att_register(server->att,
  							BT_ATT_OP_FIND_INFO_REQ,
  							find_info_cb,
  							server, NULL);
  	if (!server->find_info_id)
  		return false;
  
  	/* Find By Type Value */
  	server->find_by_type_value_id = bt_att_register(server->att,
  						BT_ATT_OP_FIND_BY_TYPE_REQ,
  						find_by_type_val_cb,
  						server, NULL);
  
  	if (!server->find_by_type_value_id)
  		return false;
  
  	/* Write Request */
  	server->write_id = bt_att_register(server->att, BT_ATT_OP_WRITE_REQ,
  								write_cb,
  								server, NULL);
  	if (!server->write_id)
  		return false;
  
  	/* Write Command */
  	server->write_cmd_id = bt_att_register(server->att, BT_ATT_OP_WRITE_CMD,
  								write_cb,
  								server, NULL);
  	if (!server->write_cmd_id)
  		return false;
  
  	/* Read Request */
  	server->read_id = bt_att_register(server->att, BT_ATT_OP_READ_REQ,
  								read_cb,
  								server, NULL);
  	if (!server->read_id)
  		return false;
  
  	/* Read Blob Request */
  	server->read_blob_id = bt_att_register(server->att,
  							BT_ATT_OP_READ_BLOB_REQ,
  							read_blob_cb,
  							server, NULL);
  	if (!server->read_blob_id)
  		return false;
  
  	/* Read Multiple Request */
  	server->read_multiple_id = bt_att_register(server->att,
  							BT_ATT_OP_READ_MULT_REQ,
  							read_multiple_cb,
  							server, NULL);
  
  	if (!server->read_multiple_id)
  		return false;
  
  	/* Prepare Write Request */
  	server->prep_write_id = bt_att_register(server->att,
  						BT_ATT_OP_PREP_WRITE_REQ,
  						prep_write_cb, server, NULL);
  	if (!server->prep_write_id)
  		return false;
  
  	/* Execute Write Request */
  	server->exec_write_id = bt_att_register(server->att,
  						BT_ATT_OP_EXEC_WRITE_REQ,
  						exec_write_cb, server, NULL);
  	if (!server->exec_write_id)
  		return NULL;
  
  	return true;
  }
  
  struct bt_gatt_server *bt_gatt_server_new(struct gatt_db *db,
  					struct bt_att *att, uint16_t mtu,
  					uint8_t min_enc_size)
  {
  	struct bt_gatt_server *server;
  
  	if (!att || !db)
  		return NULL;
  
  	server = new0(struct bt_gatt_server, 1);
  	server->db = gatt_db_ref(db);
  	server->att = bt_att_ref(att);
  	server->mtu = MAX(mtu, BT_ATT_DEFAULT_LE_MTU);
  	server->max_prep_queue_len = DEFAULT_MAX_PREP_QUEUE_LEN;
  	server->prep_queue = queue_new();
  	server->min_enc_size = min_enc_size;
  
  	if (!gatt_server_register_att_handlers(server)) {
  		bt_gatt_server_free(server);
  		return NULL;
  	}
  
  	return bt_gatt_server_ref(server);
  }
  
  uint16_t bt_gatt_server_get_mtu(struct bt_gatt_server *server)
  {
  	if (!server || !server->att)
  		return 0;
  
  	return bt_att_get_mtu(server->att);
  }
  
  struct bt_gatt_server *bt_gatt_server_ref(struct bt_gatt_server *server)
  {
  	if (!server)
  		return NULL;
  
  	__sync_fetch_and_add(&server->ref_count, 1);
  
  	return server;
  }
  
  void bt_gatt_server_unref(struct bt_gatt_server *server)
  {
  	if (!server)
  		return;
  
  	if (__sync_sub_and_fetch(&server->ref_count, 1))
  		return;
  
  	bt_gatt_server_free(server);
  }
  
  bool bt_gatt_server_set_debug(struct bt_gatt_server *server,
  					bt_gatt_server_debug_func_t callback,
  					void *user_data,
  					bt_gatt_server_destroy_func_t destroy)
  {
  	if (!server)
  		return false;
  
  	if (server->debug_destroy)
  		server->debug_destroy(server->debug_data);
  
  	server->debug_callback = callback;
  	server->debug_destroy = destroy;
  	server->debug_data = user_data;
  
  	return true;
  }
  
  bool bt_gatt_server_send_notification(struct bt_gatt_server *server,
  					uint16_t handle, const uint8_t *value,
  					uint16_t length)
  {
  	uint16_t pdu_len;
  	uint8_t *pdu;
  	bool result;
  
  	if (!server || (length && !value))
  		return false;
  
  	pdu_len = MIN(bt_att_get_mtu(server->att) - 1, length + 2);
  	pdu = malloc(pdu_len);
  	if (!pdu)
  		return false;
  
  	put_le16(handle, pdu);
  	memcpy(pdu + 2, value, pdu_len - 2);
  
  	result = !!bt_att_send(server->att, BT_ATT_OP_HANDLE_VAL_NOT, pdu,
  						pdu_len, NULL, NULL, NULL);
  	free(pdu);
  
  	return result;
  }
  
  struct ind_data {
  	bt_gatt_server_conf_func_t callback;
  	bt_gatt_server_destroy_func_t destroy;
  	void *user_data;
  };
  
  static void destroy_ind_data(void *user_data)
  {
  	struct ind_data *data = user_data;
  
  	if (data->destroy)
  		data->destroy(data->user_data);
  
  	free(data);
  }
  
  static void conf_cb(uint8_t opcode, const void *pdu,
  					uint16_t length, void *user_data)
  {
  	struct ind_data *data = user_data;
  
  	if (data->callback)
  		data->callback(data->user_data);
  }
  
  bool bt_gatt_server_send_indication(struct bt_gatt_server *server,
  					uint16_t handle, const uint8_t *value,
  					uint16_t length,
  					bt_gatt_server_conf_func_t callback,
  					void *user_data,
  					bt_gatt_server_destroy_func_t destroy)
  {
  	uint16_t pdu_len;
  	uint8_t *pdu;
  	struct ind_data *data;
  	bool result;
  
  	if (!server || (length && !value))
  		return false;
  
  	pdu_len = MIN(bt_att_get_mtu(server->att) - 1, length + 2);
  	pdu = malloc(pdu_len);
  	if (!pdu)
  		return false;
  
  	data = new0(struct ind_data, 1);
  
  	data->callback = callback;
  	data->destroy = destroy;
  	data->user_data = user_data;
  
  	put_le16(handle, pdu);
  	memcpy(pdu + 2, value, pdu_len - 2);
  
  	result = !!bt_att_send(server->att, BT_ATT_OP_HANDLE_VAL_IND, pdu,
  							pdu_len, conf_cb,
  							data, destroy_ind_data);
  	if (!result)
  		destroy_ind_data(data);
  
  	free(pdu);
  
  	return result;
  }
  