text: /** @file
   *  @brief Service Discovery Protocol handling.
   */
  
  /*
   * Copyright (c) 2016 Intel Corporation
   *
   * SPDX-License-Identifier: Apache-2.0
   */
  
  #include <errno.h>
  #include <sys/types.h>
  #include <sys/byteorder.h>
  #include <sys/__assert.h>
  
  #include <bluetooth/sdp.h>
  
  #define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_SDP)
  #define LOG_MODULE_NAME bt_sdp
  #include "common/log.h"
  
  #include "hci_core.h"
  #include "conn_internal.h"
  #include "l2cap_internal.h"
  #include "sdp_internal.h"
  
  #define SDP_PSM 0x0001
  
  #define SDP_CHAN(_ch) CONTAINER_OF(_ch, struct bt_sdp, chan.chan)
  
  #define IN_RANGE(val, min, max) (val >= min && val <= max)
  
  #define SDP_DATA_MTU 200
  
  #define SDP_MTU (SDP_DATA_MTU + sizeof(struct bt_sdp_hdr))
  
  #define MAX_NUM_ATT_ID_FILTER 10
  
  #define SDP_SERVICE_HANDLE_BASE 0x10000
  
  #define SDP_DATA_ELEM_NEST_LEVEL_MAX 5
  
  /* Size of Cont state length */
  #define SDP_CONT_STATE_LEN_SIZE 1
  
  /* 1 byte for the no. of services searched till this response */
  /* 2 bytes for the total no. of matching records */
  #define SDP_SS_CONT_STATE_SIZE 3
  
  /* 1 byte for the no. of attributes searched till this response */
  #define SDP_SA_CONT_STATE_SIZE 1
  
  /* 1 byte for the no. of services searched till this response */
  /* 1 byte for the no. of attributes searched till this response */
  #define SDP_SSA_CONT_STATE_SIZE 2
  
  #define SDP_INVALID 0xff
  
  struct bt_sdp {
  	struct bt_l2cap_br_chan chan;
  	struct k_fifo           partial_resp_queue;
  	/* TODO: Allow more than one pending request */
  };
  
  static struct bt_sdp_record *db;
  static u8_t num_services;
  
  static struct bt_sdp bt_sdp_pool[CONFIG_BT_MAX_CONN];
  
  /* Pool for outgoing SDP packets */
  NET_BUF_POOL_DEFINE(sdp_pool, CONFIG_BT_MAX_CONN,
  		    BT_L2CAP_BUF_SIZE(SDP_MTU), BT_BUF_USER_DATA_MIN, NULL);
  
  #define SDP_CLIENT_CHAN(_ch) CONTAINER_OF(_ch, struct bt_sdp_client, chan.chan)
  
  #define SDP_CLIENT_MTU 64
  
  struct bt_sdp_client {
  	struct bt_l2cap_br_chan              chan;
  	/* list of waiting to be resolved UUID params */
  	sys_slist_t                          reqs;
  	/* required SDP transaction ID */
  	u16_t                                tid;
  	/* UUID params holder being now resolved */
  	const struct bt_sdp_discover_params *param;
  	/* PDU continuation state object */
  	struct bt_sdp_pdu_cstate             cstate;
  	/* buffer for collecting record data */
  	struct net_buf                      *rec_buf;
  };
  
  static struct bt_sdp_client bt_sdp_client_pool[CONFIG_BT_MAX_CONN];
  
  enum {
  	BT_SDP_ITER_STOP,
  	BT_SDP_ITER_CONTINUE,
  };
  
  struct search_state {
  	u16_t att_list_size;
  	u8_t  current_svc;
  	u8_t  last_att;
  	bool     pkt_full;
  };
  
  struct select_attrs_data {
  	struct bt_sdp_record        *rec;
  	struct net_buf              *rsp_buf;
  	struct bt_sdp               *sdp;
  	struct bt_sdp_data_elem_seq *seq;
  	struct search_state         *state;
  	u32_t                       *filter;
  	u16_t                        max_att_len;
  	u16_t                        att_list_len;
  	u8_t                         cont_state_size;
  	u8_t                         num_filters;
  	bool                         new_service;
  };
  
  /* @typedef bt_sdp_attr_func_t
   *  @brief SDP attribute iterator callback.
   *
   *  @param attr Attribute found.
   *  @param att_idx Index of the found attribute in the attribute database.
   *  @param user_data Data given.
   *
   *  @return BT_SDP_ITER_CONTINUE if should continue to the next attribute
   *  or BT_SDP_ITER_STOP to stop.
   */
  typedef u8_t (*bt_sdp_attr_func_t)(struct bt_sdp_attribute *attr,
  				   u8_t att_idx, void *user_data);
  
  /* @typedef bt_sdp_svc_func_t
   * @brief SDP service record iterator callback.
   *
   * @param rec Service record found.
   * @param user_data Data given.
   *
   * @return BT_SDP_ITER_CONTINUE if should continue to the next service record
   *  or BT_SDP_ITER_STOP to stop.
   */
  typedef u8_t (*bt_sdp_svc_func_t)(struct bt_sdp_record *rec,
  				  void *user_data);
  
  /* @brief Callback for SDP connection
   *
   *  Gets called when an SDP connection is established
   *
   *  @param chan L2CAP channel
   *
   *  @return None
   */
  static void bt_sdp_connected(struct bt_l2cap_chan *chan)
  {
  	struct bt_l2cap_br_chan *ch = CONTAINER_OF(chan,
  						   struct bt_l2cap_br_chan,
  						   chan);
  
  	struct bt_sdp *sdp = CONTAINER_OF(ch, struct bt_sdp, chan);
  
  	BT_DBG("chan %p cid 0x%04x", ch, ch->tx.cid);
  
  	k_fifo_init(&sdp->partial_resp_queue);
  }
  
  /** @brief Callback for SDP disconnection
   *
   *  Gets called when an SDP connection is terminated
   *
   *  @param chan L2CAP channel
   *
   *  @return None
   */
  static void bt_sdp_disconnected(struct bt_l2cap_chan *chan)
  {
  	struct bt_l2cap_br_chan *ch = CONTAINER_OF(chan,
  						   struct bt_l2cap_br_chan,
  						   chan);
  
  	struct bt_sdp *sdp = CONTAINER_OF(ch, struct bt_sdp, chan);
  
  	BT_DBG("chan %p cid 0x%04x", ch, ch->tx.cid);
  
  	(void)memset(sdp, 0, sizeof(*sdp));
  }
  
  /* @brief Creates an SDP PDU
   *
   *  Creates an empty SDP PDU and returns the buffer
   *
   *  @param None
   *
   *  @return Pointer to the net_buf buffer
   */
  static struct net_buf *bt_sdp_create_pdu(void)
  {
  	return bt_l2cap_create_pdu(&sdp_pool, sizeof(struct bt_sdp_hdr));
  }
  
  /* @brief Sends out an SDP PDU
   *
   *  Sends out an SDP PDU after adding the relevant header
   *
   *  @param chan L2CAP channel
   *  @param buf Buffer to be sent out
   *  @param op Opcode to be used in the packet header
   *  @param tid Transaction ID to be used in the packet header
   *
   *  @return None
   */
  static void bt_sdp_send(struct bt_l2cap_chan *chan, struct net_buf *buf,
  			u8_t op, u16_t tid)
  {
  	struct bt_sdp_hdr *hdr;
  	u16_t param_len = buf->len;
  
  	hdr = net_buf_push(buf, sizeof(struct bt_sdp_hdr));
  	hdr->op_code = op;
  	hdr->tid = tid;
  	hdr->param_len = sys_cpu_to_be16(param_len);
  
  	bt_l2cap_chan_send(chan, buf);
  }
  
  /* @brief Sends an error response PDU
   *
   *  Creates and sends an error response PDU
   *
   *  @param chan L2CAP channel
   *  @param err Error code to be sent in the packet
   *  @param tid Transaction ID to be used in the packet header
   *
   *  @return None
   */
  static void send_err_rsp(struct bt_l2cap_chan *chan, u16_t err,
  			 u16_t tid)
  {
  	struct net_buf *buf;
  
  	BT_DBG("tid %u, error %u", tid, err);
  
  	buf = bt_sdp_create_pdu();
  
  	net_buf_add_be16(buf, err);
  
  	bt_sdp_send(chan, buf, BT_SDP_ERROR_RSP, tid);
  }
  
  /* @brief Parses data elements from a net_buf
   *
   * Parses the first data element from a buffer and splits it into type, size,
   * data. Used for parsing incoming requests. Net buf is advanced to the data
   * part of the element.
   *
   * @param buf Buffer to be advanced
   * @param data_elem Pointer to the parsed data element structure
   *
   * @return 0 for success, or relevant error code
   */
  static u16_t parse_data_elem(struct net_buf *buf,
  				struct bt_sdp_data_elem *data_elem)
  {
  	u8_t size_field_len = 0U; /* Space used to accommodate the size */
  
  	if (buf->len < 1) {
  		BT_WARN("Malformed packet");
  		return BT_SDP_INVALID_SYNTAX;
  	}
  
  	data_elem->type = net_buf_pull_u8(buf);
  
  	switch (data_elem->type & BT_SDP_TYPE_DESC_MASK) {
  	case BT_SDP_UINT8:
  	case BT_SDP_INT8:
  	case BT_SDP_UUID_UNSPEC:
  	case BT_SDP_BOOL:
  		data_elem->data_size = BIT(data_elem->type &
  					   BT_SDP_SIZE_DESC_MASK);
  		break;
  	case BT_SDP_TEXT_STR_UNSPEC:
  	case BT_SDP_SEQ_UNSPEC:
  	case BT_SDP_ALT_UNSPEC:
  	case BT_SDP_URL_STR_UNSPEC:
  		size_field_len = BIT((data_elem->type & BT_SDP_SIZE_DESC_MASK) -
  				     BT_SDP_SIZE_INDEX_OFFSET);
  		if (buf->len < size_field_len) {
  			BT_WARN("Malformed packet");
  			return BT_SDP_INVALID_SYNTAX;
  		}
  		switch (size_field_len) {
  		case 1:
  			data_elem->data_size = net_buf_pull_u8(buf);
  			break;
  		case 2:
  			data_elem->data_size = net_buf_pull_be16(buf);
  			break;
  		case 4:
  			data_elem->data_size = net_buf_pull_be32(buf);
  			break;
  		default:
  			BT_WARN("Invalid size in remote request");
  			return BT_SDP_INVALID_SYNTAX;
  		}
  		break;
  	default:
  		BT_WARN("Invalid type in remote request");
  		return BT_SDP_INVALID_SYNTAX;
  	}
  
  	if (buf->len < data_elem->data_size) {
  		BT_WARN("Malformed packet");
  		return BT_SDP_INVALID_SYNTAX;
  	}
  
  	data_elem->total_size = data_elem->data_size + size_field_len + 1;
  	data_elem->data = buf->data;
  
  	return 0;
  }
  
  /* @brief Searches for an UUID within an attribute
   *
   * Searches for an UUID within an attribute. If the attribute has data element
   * sequences, it recursively searches within them as well. On finding a match
   * with the UUID, it sets the found flag.
   *
   * @param elem Attribute to be used as the search space (haystack)
   * @param uuid UUID to be looked for (needle)
   * @param found Flag set to true if the UUID is found (to be returned)
   * @param nest_level Used to limit the extent of recursion into nested data
   *  elements, to avoid potential stack overflows
   *
   * @return Size of the last data element that has been searched
   *  (used in recursion)
   */
  static u32_t search_uuid(struct bt_sdp_data_elem *elem, struct bt_uuid *uuid,
  			 bool *found, u8_t nest_level)
  {
  	const u8_t *cur_elem;
  	u32_t seq_size, size;
  	union {
  		struct bt_uuid uuid;
  		struct bt_uuid_16 u16;
  		struct bt_uuid_32 u32;
  		struct bt_uuid_128 u128;
  	} u;
  
  	if (*found) {
  		return 0;
  	}
  
  	/* Limit recursion depth to avoid stack overflows */
  	if (nest_level == SDP_DATA_ELEM_NEST_LEVEL_MAX) {
  		return 0;
  	}
  
  	seq_size = elem->data_size;
  	cur_elem = elem->data;
  
  	if ((elem->type & BT_SDP_TYPE_DESC_MASK) == BT_SDP_UUID_UNSPEC) {
  		if (seq_size == 2U) {
  			u.uuid.type = BT_UUID_TYPE_16;
  			u.u16.val = *((u16_t *)cur_elem);
  			if (!bt_uuid_cmp(&u.uuid, uuid)) {
  				*found = true;
  			}
  		} else if (seq_size == 4U) {
  			u.uuid.type = BT_UUID_TYPE_32;
  			u.u32.val = *((u32_t *)cur_elem);
  			if (!bt_uuid_cmp(&u.uuid, uuid)) {
  				*found = true;
  			}
  		} else if (seq_size == 16U) {
  			u.uuid.type = BT_UUID_TYPE_128;
  			memcpy(u.u128.val, cur_elem, seq_size);
  			if (!bt_uuid_cmp(&u.uuid, uuid)) {
  				*found = true;
  			}
  		} else {
  			BT_WARN("Invalid UUID size in local database");
  			BT_ASSERT(0);
  		}
  	}
  
  	if ((elem->type & BT_SDP_TYPE_DESC_MASK) == BT_SDP_SEQ_UNSPEC ||
  	    (elem->type & BT_SDP_TYPE_DESC_MASK) == BT_SDP_ALT_UNSPEC) {
  		do {
  			/* Recursively parse data elements */
  			size = search_uuid((struct bt_sdp_data_elem *)cur_elem,
  					   uuid, found, nest_level + 1);
  			if (*found) {
  				return 0;
  			}
  			cur_elem += sizeof(struct bt_sdp_data_elem);
  			seq_size -= size;
  		} while (seq_size);
  	}
  
  	return elem->total_size;
  }
  
  /* @brief SDP service record iterator.
   *
   * Iterate over service records from a starting point.
   *
   * @param func Callback function.
   * @param user_data Data to pass to the callback.
   *
   * @return Pointer to the record where the iterator stopped, or NULL if all
   *  records are covered
   */
  static struct bt_sdp_record *bt_sdp_foreach_svc(bt_sdp_svc_func_t func,
  						void *user_data)
  {
  	struct bt_sdp_record *rec = db;
  
  	while (rec) {
  		if (func(rec, user_data) == BT_SDP_ITER_STOP) {
  			break;
  		}
  
  		rec = rec->next;
  	}
  	return rec;
  }
  
  /* @brief Inserts a service record into a record pointer list
   *
   * Inserts a service record into a record pointer list
   *
   * @param rec The current service record.
   * @param user_data Pointer to the destination record list.
   *
   * @return BT_SDP_ITER_CONTINUE to move on to the next record.
   */
  static u8_t insert_record(struct bt_sdp_record *rec, void *user_data)
  {
  	struct bt_sdp_record **rec_list = user_data;
  
  	rec_list[rec->index] = rec;
  
  	return BT_SDP_ITER_CONTINUE;
  }
  
  /* @brief Looks for matching UUIDs in a list of service records
   *
   * Parses out a sequence of UUIDs from an input buffer, and checks if a record
   * in the list contains all the UUIDs. If it doesn't, the record is removed
   * from the list, so the list contains only the records which has all the
   * input UUIDs in them.
   *
   * @param buf Incoming buffer containing all the UUIDs to be matched
   * @param matching_recs List of service records to use for storing matching
   * records
   *
   * @return 0 for success, or relevant error code
   */
  static u16_t find_services(struct net_buf *buf,
  			      struct bt_sdp_record **matching_recs)
  {
  	struct bt_sdp_data_elem data_elem;
  	struct bt_sdp_record *record;
  	u32_t uuid_list_size;
  	u16_t res;
  	u8_t att_idx, rec_idx = 0U;
  	bool found;
  	union {
  		struct bt_uuid uuid;
  		struct bt_uuid_16 u16;
  		struct bt_uuid_32 u32;
  		struct bt_uuid_128 u128;
  	} u;
  
  	res = parse_data_elem(buf, &data_elem);
  	if (res) {
  		return res;
  	}
  
  	if (((data_elem.type & BT_SDP_TYPE_DESC_MASK) != BT_SDP_SEQ_UNSPEC) &&
  	    ((data_elem.type & BT_SDP_TYPE_DESC_MASK) != BT_SDP_ALT_UNSPEC)) {
  		BT_WARN("Invalid type %x in service search pattern",
  			data_elem.type);
  		return BT_SDP_INVALID_SYNTAX;
  	}
  
  	uuid_list_size = data_elem.data_size;
  
  	bt_sdp_foreach_svc(insert_record, matching_recs);
  
  	/* Go over the sequence of UUIDs, and match one UUID at a time */
  	while (uuid_list_size) {
  		res = parse_data_elem(buf, &data_elem);
  		if (res) {
  			return res;
  		}
  
  		if ((data_elem.type & BT_SDP_TYPE_DESC_MASK) !=
  		    BT_SDP_UUID_UNSPEC) {
  			BT_WARN("Invalid type %u in service search pattern",
  				data_elem.type);
  			return BT_SDP_INVALID_SYNTAX;
  		}
  
  		if (buf->len < data_elem.data_size) {
  			BT_WARN("Malformed packet");
  			return BT_SDP_INVALID_SYNTAX;
  		}
  
  		if (data_elem.data_size == 2U) {
  			u.uuid.type = BT_UUID_TYPE_16;
  			u.u16.val = net_buf_pull_be16(buf);
  		} else if (data_elem.data_size == 4U) {
  			u.uuid.type = BT_UUID_TYPE_32;
  			u.u32.val = net_buf_pull_be32(buf);
  		} else if (data_elem.data_size == 16U) {
  			u.uuid.type = BT_UUID_TYPE_128;
  			sys_memcpy_swap(u.u128.val, buf->data,
  					data_elem.data_size);
  			net_buf_pull(buf, data_elem.data_size);
  		} else {
  			BT_WARN("Invalid UUID len %u in service search pattern",
  				data_elem.data_size);
  			net_buf_pull(buf, data_elem.data_size);
  		}
  
  		uuid_list_size -= data_elem.total_size;
  
  		/* Go over the list of services, and look for a service which
  		 * doesn't have this UUID
  		 */
  		for (rec_idx = 0U; rec_idx < num_services; rec_idx++) {
  			record = matching_recs[rec_idx];
  
  			if (!record) {
  				continue;
  			}
  
  			found = false;
  
  			/* Search for the UUID in all the attrs of the svc */
  			for (att_idx = 0U; att_idx < record->attr_count;
  			     att_idx++) {
  				search_uuid(&record->attrs[att_idx].val,
  					    &u.uuid, &found, 1);
  				if (found) {
  					break;
  				}
  			}
  
  			/* Remove the record from the list if it doesn't have
  			 * the UUID
  			 */
  			if (!found) {
  				matching_recs[rec_idx] = NULL;
  			}
  		}
  	}
  
  	return 0;
  }
  
  /* @brief Handler for Service Search Request
   *
   * Parses, processes and responds to a Service Search Request
   *
   * @param sdp Pointer to the SDP structure
   * @param buf Request net buf
   * @param tid Transaction ID
   *
   * @return 0 for success, or relevant error code
   */
  static u16_t sdp_svc_search_req(struct bt_sdp *sdp, struct net_buf *buf,
  				u16_t tid)
  {
  	struct bt_sdp_svc_rsp *rsp;
  	struct net_buf *resp_buf;
  	struct bt_sdp_record *record;
  	struct bt_sdp_record *matching_recs[BT_SDP_MAX_SERVICES];
  	u16_t max_rec_count, total_recs = 0U, current_recs = 0U, res;
  	u8_t cont_state_size, cont_state = 0U, idx = 0U, count = 0U;
  	bool pkt_full = false;
  
  	res = find_services(buf, matching_recs);
  	if (res) {
  		/* Error in parsing */
  		return res;
  	}
  
  	if (buf->len < 3) {
  		BT_WARN("Malformed packet");
  		return BT_SDP_INVALID_SYNTAX;
  	}
  
  	max_rec_count = net_buf_pull_be16(buf);
  	cont_state_size = net_buf_pull_u8(buf);
  
  	/* Zero out the matching services beyond max_rec_count */
  	for (idx = 0U; idx < num_services; idx++) {
  		if (count == max_rec_count) {
  			matching_recs[idx] = NULL;
  			continue;
  		}
  
  		if (matching_recs[idx]) {
  			count++;
  		}
  	}
  
  	/* We send out only SDP_SS_CONT_STATE_SIZE bytes continuation state in
  	 * responses, so expect only SDP_SS_CONT_STATE_SIZE bytes in requests
  	 */
  	if (cont_state_size) {
  		if (cont_state_size != SDP_SS_CONT_STATE_SIZE) {
  			BT_WARN("Invalid cont state size %u", cont_state_size);
  			return BT_SDP_INVALID_CSTATE;
  		}
  
  		if (buf->len < cont_state_size) {
  			BT_WARN("Malformed packet");
  			return BT_SDP_INVALID_SYNTAX;
  		}
  
  		cont_state = net_buf_pull_u8(buf);
  		/* We include total_recs in the continuation state. We calculate
  		 * it once and preserve it across all the partial responses
  		 */
  		total_recs = net_buf_pull_be16(buf);
  	}
  
  	BT_DBG("max_rec_count %u, cont_state %u", max_rec_count, cont_state);
  
  	resp_buf = bt_sdp_create_pdu();
  	rsp = net_buf_add(resp_buf, sizeof(*rsp));
  
  	for (; cont_state < num_services; cont_state++) {
  		record = matching_recs[cont_state];
  
  		if (!record) {
  			continue;
  		}
  
  		/* Calculate total recs only if it is first packet */
  		if (!cont_state_size) {
  			total_recs++;
  		}
  
  		if (pkt_full) {
  			continue;
  		}
  
  		/* 4 bytes per Service Record Handle */
  		/* 4 bytes for ContinuationState */
  		if ((MIN(SDP_MTU, sdp->chan.tx.mtu) - resp_buf->len) <
  		    (4 + 4 + sizeof(struct bt_sdp_hdr))) {
  			pkt_full = true;
  		}
  
  		if (pkt_full) {
  			/* Packet exhausted: Add continuation state and break */
  			BT_DBG("Packet full, num_services_covered %u",
  			       cont_state);
  			net_buf_add_u8(resp_buf, SDP_SS_CONT_STATE_SIZE);
  			net_buf_add_u8(resp_buf, cont_state);
  
  			/* If it is the first packet of a partial response,
  			 * continue dry-running to calculate total_recs.
  			 * Else break
  			 */
  			if (cont_state_size) {
  				break;
  			}
  
  			continue;
  		}
  
  		/* Add the service record handle to the packet */
  		net_buf_add_be32(resp_buf, record->handle);
  		current_recs++;
  	}
  
  	/* Add 0 continuation state if packet is exhausted */
  	if (!pkt_full) {
  		net_buf_add_u8(resp_buf, 0);
  	} else {
  		net_buf_add_be16(resp_buf, total_recs);
  	}
  
  	rsp->total_recs = sys_cpu_to_be16(total_recs);
  	rsp->current_recs = sys_cpu_to_be16(current_recs);
  
  	BT_DBG("Sending response, len %u", resp_buf->len);
  	bt_sdp_send(&sdp->chan.chan, resp_buf, BT_SDP_SVC_SEARCH_RSP, tid);
  
  	return 0;
  }
  
  /* @brief Copies an attribute into an outgoing buffer
   *
   *  Copies an attribute into a buffer. Recursively calls itself for complex
   *  attributes.
   *
   *  @param elem Attribute to be copied to the buffer
   *  @param buf Buffer where the attribute is to be copied
   *
   *  @return Size of the last data element that has been searched
   *  (used in recursion)
   */
  static u32_t copy_attribute(struct bt_sdp_data_elem *elem,
  			    struct net_buf *buf, u8_t nest_level)
  {
  	const u8_t *cur_elem;
  	u32_t size, seq_size, total_size;
  
  	/* Limit recursion depth to avoid stack overflows */
  	if (nest_level == SDP_DATA_ELEM_NEST_LEVEL_MAX) {
  		return 0;
  	}
  
  	seq_size = elem->data_size;
  	total_size = elem->total_size;
  	cur_elem = elem->data;
  
  	/* Copy the header */
  	net_buf_add_u8(buf, elem->type);
  
  	switch (total_size - (seq_size + 1U)) {
  	case 1:
  		net_buf_add_u8(buf, elem->data_size);
  		break;
  	case 2:
  		net_buf_add_be16(buf, elem->data_size);
  		break;
  	case 4:
  		net_buf_add_be32(buf, elem->data_size);
  		break;
  	}
  
  	/* Recursively parse (till the last element is not another data element)
  	 * and then fill the elements
  	 */
  	if ((elem->type & BT_SDP_TYPE_DESC_MASK) == BT_SDP_SEQ_UNSPEC ||
  	    (elem->type & BT_SDP_TYPE_DESC_MASK) == BT_SDP_ALT_UNSPEC) {
  		do {
  			size = copy_attribute((struct bt_sdp_data_elem *)
  					      cur_elem, buf, nest_level + 1);
  			cur_elem += sizeof(struct bt_sdp_data_elem);
  			seq_size -= size;
  		} while (seq_size);
  	} else if ((elem->type & BT_SDP_TYPE_DESC_MASK) == BT_SDP_UINT8 ||
  		   (elem->type & BT_SDP_TYPE_DESC_MASK) == BT_SDP_INT8 ||
  		   (elem->type & BT_SDP_TYPE_DESC_MASK) == BT_SDP_UUID_UNSPEC) {
  		if (seq_size == 1U) {
  			net_buf_add_u8(buf, *((u8_t *)elem->data));
  		} else if (seq_size == 2U) {
  			net_buf_add_be16(buf, *((u16_t *)elem->data));
  		} else if (seq_size == 4U) {
  			net_buf_add_be32(buf, *((u32_t *)elem->data));
  		} else {
  			/* TODO: Convert 32bit and 128bit values to big-endian*/
  			net_buf_add_mem(buf, elem->data, seq_size);
  		}
  	} else {
  		net_buf_add_mem(buf, elem->data, seq_size);
  	}
  
  	return total_size;
  }
  
  /* @brief SDP attribute iterator.
   *
   *  Iterate over attributes of a service record from a starting index.
   *
   *  @param record Service record whose attributes are to be iterated over.
   *  @param idx Index in the attribute list from where to start.
   *  @param func Callback function.
   *  @param user_data Data to pass to the callback.
   *
   *  @return Index of the attribute where the iterator stopped
   */
  static u8_t bt_sdp_foreach_attr(struct bt_sdp_record *record, u8_t idx,
  				bt_sdp_attr_func_t func, void *user_data)
  {
  	for (; idx < record->attr_count; idx++) {
  		if (func(&record->attrs[idx], idx, user_data) ==
  		    BT_SDP_ITER_STOP) {
  			break;
  		}
  	}
  
  	return idx;
  }
  
  /* @brief Check if an attribute matches a range, and include it in the response
   *
   *  Checks if an attribute matches a given attribute ID or range, and if so,
   *  includes it in the response packet
   *
   *  @param attr The current attribute
   *  @param att_idx Index of the current attribute in the database
   *  @param user_data Pointer to the structure containing response packet, byte
   *   count, states, etc
   *
   *  @return BT_SDP_ITER_CONTINUE if should continue to the next attribute
   *   or BT_SDP_ITER_STOP to stop.
   */
  static u8_t select_attrs(struct bt_sdp_attribute *attr, u8_t att_idx,
  			 void *user_data)
  {
  	struct select_attrs_data *sad = user_data;
  	u16_t att_id_lower, att_id_upper, att_id_cur, space;
  	u32_t attr_size, seq_size;
  	u8_t idx_filter;
  
  	for (idx_filter = 0U; idx_filter < sad->num_filters; idx_filter++) {
  
  		att_id_lower = (sad->filter[idx_filter] >> 16);
  		att_id_upper = (sad->filter[idx_filter]);
  		att_id_cur = attr->id;
  
  		/* Check for range values */
  		if (att_id_lower != 0xffff &&
  		    (!IN_RANGE(att_id_cur, att_id_lower, att_id_upper))) {
  			continue;
  		}
  
  		/* Check for match values */
  		if (att_id_lower == 0xffff && att_id_cur != att_id_upper) {
  			continue;
  		}
  
  		/* Attribute ID matches */
  
  		/* 3 bytes for Attribute ID */
  		attr_size = 3 + attr->val.total_size;
  
  		/* If this is the first attribute of the service, then we need
  		 * to account for the space required to add the per-service
  		 * data element sequence header as well.
  		 */
  		if ((sad->state->current_svc != sad->rec->index) &&
  		    sad->new_service) {
  			/* 3 bytes for Per-Service Data Elem Seq declaration */
  			seq_size = attr_size + 3;
  		} else {
  			seq_size = attr_size;
  		}
  
  		if (sad->rsp_buf) {
  			space = MIN(SDP_MTU, sad->sdp->chan.tx.mtu) -
  				sad->rsp_buf->len - sizeof(struct bt_sdp_hdr);
  
  			if ((!sad->state->pkt_full) &&
  			    ((seq_size > sad->max_att_len) ||
  			     (space < seq_size + sad->cont_state_size))) {
  				/* Packet exhausted */
  				sad->state->pkt_full = true;
  			}
  		}
  
  		/* Keep filling data only if packet is not exhausted */
  		if (!sad->state->pkt_full && sad->rsp_buf) {
  			/* Add Per-Service Data Element Seq declaration once
  			 * only when we are starting from the first attribute
  			 */
  			if (!sad->seq &&
  			    (sad->state->current_svc != sad->rec->index)) {
  				sad->seq = net_buf_add(sad->rsp_buf,
  						       sizeof(*sad->seq));
  				sad->seq->type = BT_SDP_SEQ16;
  				sad->seq->size = 0U;
  			}
  
  			/* Add attribute ID */
  			net_buf_add_u8(sad->rsp_buf, BT_SDP_UINT16);
  			net_buf_add_be16(sad->rsp_buf, att_id_cur);
  
  			/* Add attribute value */
  			copy_attribute(&attr->val, sad->rsp_buf, 1);
  
  			sad->max_att_len -= seq_size;
  			sad->att_list_len += seq_size;
  			sad->state->last_att = att_idx;
  			sad->state->current_svc = sad->rec->index;
  		}
  
  		if (sad->seq) {
  			/* Keep adding the sequence size if this packet contains
  			 * the Per-Service Data Element Seq declaration header
  			 */
  			sad->seq->size += attr_size;
  			sad->state->att_list_size += seq_size;
  		} else {
  			/* Keep adding the total attr lists size if:
  			 * It's a dry-run, calculating the total attr lists size
  			 */
  			sad->state->att_list_size += seq_size;
  		}
  
  		sad->new_service = false;
  		break;
  	}
  
  	/* End the search if:
  	 * 1. We have exhausted the packet
  	 * AND
  	 * 2. This packet doesn't contain the service element declaration header
  	 * AND
  	 * 3. This is not a dry-run (then we look for other attrs that match)
  	 */
  	if (sad->state->pkt_full && !sad->seq && sad->rsp_buf) {
  		return BT_SDP_ITER_STOP;
  	}
  
  	return BT_SDP_ITER_CONTINUE;
  }
  
  /* @brief Creates attribute list in the given buffer
   *
   *  Populates the attribute list of a service record in the buffer. To be used
   *  for responding to Service Attribute and Service Search Attribute requests
   *
   *  @param sdp Pointer to the SDP structure
   *  @param record Service record whose attributes are to be included in the
   *   response
   *  @param filter Attribute values/ranges to be used as a filter
   *  @param num_filters Number of elements in the attribute filter
   *  @param max_att_len Maximum size of attributes to be included in the response
   *  @param cont_state_size No. of additional continuation state bytes to keep
   *   space for in the packet. This will vary based on the type of the request
   *  @param next_att Starting position of the search in the service's attr list
   *  @param state State of the overall search
   *  @param rsp_buf Response buffer which is filled in
   *
   *  @return len Length of the attribute list created
   */
  static u16_t create_attr_list(struct bt_sdp *sdp, struct bt_sdp_record *record,
  			      u32_t *filter, u8_t num_filters,
  			      u16_t max_att_len, u8_t cont_state_size,
  			      u8_t next_att, struct search_state *state,
  			      struct net_buf *rsp_buf)
  {
  	struct select_attrs_data sad;
  	u8_t idx_att;
  
  	sad.num_filters = num_filters;
  	sad.rec = record;
  	sad.rsp_buf = rsp_buf;
  	sad.sdp = sdp;
  	sad.max_att_len = max_att_len;
  	sad.cont_state_size = cont_state_size;
  	sad.seq = NULL;
  	sad.filter = filter;
  	sad.state = state;
  	sad.att_list_len = 0U;
  	sad.new_service = true;
  
  	idx_att = bt_sdp_foreach_attr(sad.rec, next_att, select_attrs, &sad);
  
  	if (sad.seq) {
  		sad.seq->size = sys_cpu_to_be16(sad.seq->size);
  	}
  
  	return sad.att_list_len;
  }
  
  /* @brief Extracts the attribute search list from a buffer
   *
   *  Parses a buffer to extract the attribute search list (list of attribute IDs
   *  and ranges) which are to be used to filter attributes.
   *
   *  @param buf Buffer to be parsed for extracting the attribute search list
   *  @param filter Empty list of 4byte filters that are filled in. For attribute
   *   IDs, the lower 2 bytes contain the ID and the upper 2 bytes are set to
   *   0xFFFF. For attribute ranges, the lower 2bytes indicate the start ID and
   *   the upper 2bytes indicate the end ID
   *  @param num_filters No. of filter elements filled in (to be returned)
   *
   *  @return 0 for success, or relevant error code
   */
  static u16_t get_att_search_list(struct net_buf *buf, u32_t *filter,
  				 u8_t *num_filters)
  {
  	struct bt_sdp_data_elem data_elem;
  	u16_t res;
  	u32_t size;
  
  	*num_filters = 0U;
  	res = parse_data_elem(buf, &data_elem);
  	if (res) {
  		return res;
  	}
  
  	size = data_elem.data_size;
  
  	while (size) {
  		res = parse_data_elem(buf, &data_elem);
  		if (res) {
  			return res;
  		}
  
  		if ((data_elem.type & BT_SDP_TYPE_DESC_MASK) != BT_SDP_UINT8) {
  			BT_WARN("Invalid type %u in attribute ID list",
  				data_elem.type);
  			return BT_SDP_INVALID_SYNTAX;
  		}
  
  		if (buf->len < data_elem.data_size) {
  			BT_WARN("Malformed packet");
  			return BT_SDP_INVALID_SYNTAX;
  		}
  
  		/* This is an attribute ID */
  		if (data_elem.data_size == 2U) {
  			filter[(*num_filters)++] = 0xffff0000 |
  							net_buf_pull_be16(buf);
  		}
  
  		/* This is an attribute ID range */
  		if (data_elem.data_size == 4U) {
  			filter[(*num_filters)++] = net_buf_pull_be32(buf);
  		}
  
  		size -= data_elem.total_size;
  	}
  
  	return 0;
  }
  
  /* @brief Check if a given handle matches that of the current service
   *
   *  Checks if a given handle matches that of the current service
   *
   *  @param rec The current service record
   *  @param user_data Pointer to the service record handle to be matched
   *
   *  @return BT_SDP_ITER_CONTINUE if should continue to the next record
   *   or BT_SDP_ITER_STOP to stop.
   */
  static u8_t find_handle(struct bt_sdp_record *rec, void *user_data)
  {
  	u32_t *svc_rec_hdl = user_data;
  
  	if (rec->handle == *svc_rec_hdl) {
  		return BT_SDP_ITER_STOP;
  	}
  
  	return BT_SDP_ITER_CONTINUE;
  }
  
  /* @brief Handler for Service Attribute Request
   *
   *  Parses, processes and responds to a Service Attribute Request
   *
   *  @param sdp Pointer to the SDP structure
   *  @param buf Request buffer
   *  @param tid Transaction ID
   *
   *  @return 0 for success, or relevant error code
   */
  static u16_t sdp_svc_att_req(struct bt_sdp *sdp, struct net_buf *buf,
  			     u16_t tid)
  {
  	u32_t filter[MAX_NUM_ATT_ID_FILTER];
  	struct search_state state = {
  		.current_svc = SDP_INVALID,
  		.last_att = SDP_INVALID,
  		.pkt_full = false
  	};
  	struct bt_sdp_record *record;
  	struct bt_sdp_att_rsp *rsp;
  	struct net_buf *rsp_buf;
  	u32_t svc_rec_hdl;
  	u16_t max_att_len, res, att_list_len;
  	u8_t num_filters, cont_state_size, next_att = 0U;
  
  	if (buf->len < 6) {
  		BT_WARN("Malformed packet");
  		return BT_SDP_INVALID_SYNTAX;
  	}
  
  	svc_rec_hdl = net_buf_pull_be32(buf);
  	max_att_len = net_buf_pull_be16(buf);
  
  	/* Set up the filters */
  	res = get_att_search_list(buf, filter, &num_filters);
  	if (res) {
  		/* Error in parsing */
  		return res;
  	}
  
  	if (buf->len < 1) {
  		BT_WARN("Malformed packet");
  		return BT_SDP_INVALID_SYNTAX;
  	}
  
  	cont_state_size = net_buf_pull_u8(buf);
  
  	/* We only send out 1 byte continuation state in responses,
  	 * so expect only 1 byte in requests
  	 */
  	if (cont_state_size) {
  		if (cont_state_size != SDP_SA_CONT_STATE_SIZE) {
  			BT_WARN("Invalid cont state size %u", cont_state_size);
  			return BT_SDP_INVALID_CSTATE;
  		}
  
  		if (buf->len < cont_state_size) {
  			BT_WARN("Malformed packet");
  			return BT_SDP_INVALID_SYNTAX;
  		}
  
  		state.last_att = net_buf_pull_u8(buf) + 1;
  		next_att = state.last_att;
  	}
  
  	BT_DBG("svc_rec_hdl %u, max_att_len 0x%04x, cont_state %u", svc_rec_hdl,
  	       max_att_len, next_att);
  
  	/* Find the service */
  	record = bt_sdp_foreach_svc(find_handle, &svc_rec_hdl);
  
  	if (!record) {
  		BT_WARN("Handle %u not found", svc_rec_hdl);
  		return BT_SDP_INVALID_RECORD_HANDLE;
  	}
  
  	/* For partial responses, restore the search state */
  	if (cont_state_size) {
  		state.current_svc = record->index;
  	}
  
  	rsp_buf = bt_sdp_create_pdu();
  	rsp = net_buf_add(rsp_buf, sizeof(*rsp));
  
  	/* cont_state_size should include 1 byte header */
  	att_list_len = create_attr_list(sdp, record, filter, num_filters,
  					max_att_len, SDP_SA_CONT_STATE_SIZE + 1,
  					next_att, &state, rsp_buf);
  
  	if (!att_list_len) {
  		/* For empty responses, add an empty data element sequence */
  		net_buf_add_u8(rsp_buf, BT_SDP_SEQ8);
  		net_buf_add_u8(rsp_buf, 0);
  		att_list_len = 2U;
  	}
  
  	/* Add continuation state */
  	if (state.pkt_full) {
  		BT_DBG("Packet full, state.last_att %u", state.last_att);
  		net_buf_add_u8(rsp_buf, 1);
  		net_buf_add_u8(rsp_buf, state.last_att);
  	} else {
  		net_buf_add_u8(rsp_buf, 0);
  	}
  
  	rsp->att_list_len = sys_cpu_to_be16(att_list_len);
  
  	BT_DBG("Sending response, len %u", rsp_buf->len);
  	bt_sdp_send(&sdp->chan.chan, rsp_buf, BT_SDP_SVC_ATTR_RSP, tid);
  
  	return 0;
  }
  
  /* @brief Handler for Service Search Attribute Request
   *
   *  Parses, processes and responds to a Service Search Attribute Request
   *
   *  @param sdp Pointer to the SDP structure
   *  @param buf Request buffer
   *  @param tid Transaction ID
   *
   *  @return 0 for success, or relevant error code
   */
  static u16_t sdp_svc_search_att_req(struct bt_sdp *sdp, struct net_buf *buf,
  				    u16_t tid)
  {
  	u32_t filter[MAX_NUM_ATT_ID_FILTER];
  	struct bt_sdp_record *matching_recs[BT_SDP_MAX_SERVICES];
  	struct search_state state = {
  		.att_list_size = 0,
  		.current_svc = SDP_INVALID,
  		.last_att = SDP_INVALID,
  		.pkt_full = false
  	};
  	struct net_buf *rsp_buf, *rsp_buf_cpy;
  	struct bt_sdp_record *record;
  	struct bt_sdp_att_rsp *rsp;
  	struct bt_sdp_data_elem_seq *seq = NULL;
  	u16_t max_att_len, res, att_list_len = 0U;
  	u8_t num_filters, cont_state_size, next_svc = 0U, next_att = 0U;
  	bool dry_run = false;
  
  	res = find_services(buf, matching_recs);
  	if (res) {
  		return res;
  	}
  
  	if (buf->len < 2) {
  		BT_WARN("Malformed packet");
  		return BT_SDP_INVALID_SYNTAX;
  	}
  
  	max_att_len = net_buf_pull_be16(buf);
  
  	/* Set up the filters */
  	res = get_att_search_list(buf, filter, &num_filters);
  
  	if (res) {
  		/* Error in parsing */
  		return res;
  	}
  
  	if (buf->len < 1) {
  		BT_WARN("Malformed packet");
  		return BT_SDP_INVALID_SYNTAX;
  	}
  
  	cont_state_size = net_buf_pull_u8(buf);
  
  	/* We only send out 2 bytes continuation state in responses,
  	 * so expect only 2 bytes in requests
  	 */
  	if (cont_state_size) {
  		if (cont_state_size != SDP_SSA_CONT_STATE_SIZE) {
  			BT_WARN("Invalid cont state size %u", cont_state_size);
  			return BT_SDP_INVALID_CSTATE;
  		}
  
  		if (buf->len < cont_state_size) {
  			BT_WARN("Malformed packet");
  			return BT_SDP_INVALID_SYNTAX;
  		}
  
  		state.current_svc = net_buf_pull_u8(buf);
  		state.last_att = net_buf_pull_u8(buf) + 1;
  		next_svc = state.current_svc;
  		next_att = state.last_att;
  	}
  
  	BT_DBG("max_att_len 0x%04x, state.current_svc %u, state.last_att %u",
  	       max_att_len, state.current_svc, state.last_att);
  
  	rsp_buf = bt_sdp_create_pdu();
  
  	rsp = net_buf_add(rsp_buf, sizeof(*rsp));
  
  	/* Add headers only if this is not a partial response */
  	if (!cont_state_size) {
  		seq = net_buf_add(rsp_buf, sizeof(*seq));
  		seq->type = BT_SDP_SEQ16;
  		seq->size = 0U;
  
  		/* 3 bytes for Outer Data Element Sequence declaration */
  		att_list_len = 3U;
  	}
  
  	rsp_buf_cpy = rsp_buf;
  
  	for (; next_svc < num_services; next_svc++) {
  		record = matching_recs[next_svc];
  
  		if (!record) {
  			continue;
  		}
  
  		att_list_len += create_attr_list(sdp, record, filter,
  						 num_filters, max_att_len,
  						 SDP_SSA_CONT_STATE_SIZE + 1,
  						 next_att, &state, rsp_buf_cpy);
  
  		/* Check if packet is full and not dry run */
  		if (state.pkt_full && !dry_run) {
  			BT_DBG("Packet full, state.last_att %u",
  			       state.last_att);
  			dry_run = true;
  
  			/* Add continuation state */
  			net_buf_add_u8(rsp_buf, 2);
  			net_buf_add_u8(rsp_buf, state.current_svc);
  			net_buf_add_u8(rsp_buf, state.last_att);
  
  			/* Break if it's not a partial response, else dry-run
  			 * Dry run: Look for other services that match
  			 */
  			if (cont_state_size) {
  				break;
  			}
  
  			rsp_buf_cpy = NULL;
  		}
  
  		next_att = 0U;
  	}
  
  	if (!dry_run) {
  		if (!att_list_len) {
  			/* For empty responses, add an empty data elem seq */
  			net_buf_add_u8(rsp_buf, BT_SDP_SEQ8);
  			net_buf_add_u8(rsp_buf, 0);
  			att_list_len = 2U;
  		}
  		/* Search exhausted */
  		net_buf_add_u8(rsp_buf, 0);
  	}
  
  	rsp->att_list_len = sys_cpu_to_be16(att_list_len);
  	if (seq) {
  		seq->size = sys_cpu_to_be16(state.att_list_size);
  	}
  
  	BT_DBG("Sending response, len %u", rsp_buf->len);
  	bt_sdp_send(&sdp->chan.chan, rsp_buf, BT_SDP_SVC_SEARCH_ATTR_RSP,
  		    tid);
  
  	return 0;
  }
  
  static const struct {
  	u8_t  op_code;
  	u16_t  (*func)(struct bt_sdp *sdp, struct net_buf *buf, u16_t tid);
  } handlers[] = {
  	{ BT_SDP_SVC_SEARCH_REQ, sdp_svc_search_req },
  	{ BT_SDP_SVC_ATTR_REQ, sdp_svc_att_req },
  	{ BT_SDP_SVC_SEARCH_ATTR_REQ, sdp_svc_search_att_req },
  };
  
  /* @brief Callback for SDP data receive
   *
   *  Gets called when an SDP PDU is received. Calls the corresponding handler
   *  based on the op code of the PDU.
   *
   *  @param chan L2CAP channel
   *  @param buf Received PDU
   *
   *  @return None
   */
  static int bt_sdp_recv(struct bt_l2cap_chan *chan, struct net_buf *buf)
  {
  	struct bt_l2cap_br_chan *ch = CONTAINER_OF(chan,
  			struct bt_l2cap_br_chan, chan);
  	struct bt_sdp *sdp = CONTAINER_OF(ch, struct bt_sdp, chan);
  	struct bt_sdp_hdr *hdr;
  	u16_t err = BT_SDP_INVALID_SYNTAX;
  	size_t i;
  
  	BT_DBG("chan %p, ch %p, cid 0x%04x", chan, ch, ch->tx.cid);
  
  	BT_ASSERT(sdp);
  
  	if (buf->len < sizeof(*hdr)) {
  		BT_ERR("Too small SDP PDU received");
  		return 0;
  	}
  
  	hdr = net_buf_pull_mem(buf, sizeof(*hdr));
  	BT_DBG("Received SDP code 0x%02x len %u", hdr->op_code, buf->len);
  
  	if (sys_cpu_to_be16(hdr->param_len) != buf->len) {
  		err = BT_SDP_INVALID_PDU_SIZE;
  	} else {
  		for (i = 0; i < ARRAY_SIZE(handlers); i++) {
  			if (hdr->op_code != handlers[i].op_code) {
  				continue;
  			}
  
  			err = handlers[i].func(sdp, buf, hdr->tid);
  			break;
  		}
  	}
  
  	if (err) {
  		BT_WARN("SDP error 0x%02x", err);
  		send_err_rsp(chan, err, hdr->tid);
  	}
  
  	return 0;
  }
  
  /* @brief Callback for SDP connection accept
   *
   *  Gets called when an incoming SDP connection needs to be authorized.
   *  Registers the L2CAP callbacks and allocates an SDP context to the connection
   *
   *  @param conn BT connection object
   *  @param chan L2CAP channel structure (to be returned)
   *
   *  @return 0 for success, or relevant error code
   */
  static int bt_sdp_accept(struct bt_conn *conn, struct bt_l2cap_chan **chan)
  {
  	static struct bt_l2cap_chan_ops ops = {
  		.connected = bt_sdp_connected,
  		.disconnected = bt_sdp_disconnected,
  		.recv = bt_sdp_recv,
  	};
  	int i;
  
  	BT_DBG("conn %p", conn);
  
  	for (i = 0; i < ARRAY_SIZE(bt_sdp_pool); i++) {
  		struct bt_sdp *sdp = &bt_sdp_pool[i];
  
  		if (sdp->chan.chan.conn) {
  			continue;
  		}
  
  		sdp->chan.chan.ops = &ops;
  		sdp->chan.rx.mtu = SDP_MTU;
  
  		*chan = &sdp->chan.chan;
  
  		return 0;
  	}
  
  	BT_ERR("No available SDP context for conn %p", conn);
  
  	return -ENOMEM;
  }
  
  void bt_sdp_init(void)
  {
  	static struct bt_l2cap_server server = {
  		.psm = SDP_PSM,
  		.accept = bt_sdp_accept,
  		.sec_level = BT_SECURITY_NONE,
  	};
  	int res;
  
  	res = bt_l2cap_br_server_register(&server);
  	if (res) {
  		BT_ERR("L2CAP server registration failed with error %d", res);
  	}
  }
  
  int bt_sdp_register_service(struct bt_sdp_record *service)
  {
  	u32_t handle = SDP_SERVICE_HANDLE_BASE;
  
  	if (!service) {
  		BT_ERR("No service record specified");
  		return 0;
  	}
  
  	if (num_services == BT_SDP_MAX_SERVICES) {
  		BT_ERR("Reached max allowed registrations");
  		return -ENOMEM;
  	}
  
  	if (db) {
  		handle = db->handle + 1;
  	}
  
  	service->next = db;
  	service->index = num_services++;
  	service->handle = handle;
  	*((u32_t *)(service->attrs[0].val.data)) = handle;
  	db = service;
  
  	BT_DBG("Service registered at %u", handle);
  
  	return 0;
  }
  
  #define GET_PARAM(__node) \
  	CONTAINER_OF(__node, struct bt_sdp_discover_params, _node)
  
  /* ServiceSearchAttribute PDU, ref to BT Core 4.2, Vol 3, part B, 4.7.1 */
  static int sdp_client_ssa_search(struct bt_sdp_client *session)
  {
  	const struct bt_sdp_discover_params *param;
  	struct bt_sdp_hdr *hdr;
  	struct net_buf *buf;
  
  	/*
  	 * Select proper user params, if session->param is invalid it means
  	 * getting new UUID from top of to be resolved params list. Otherwise
  	 * the context is in a middle of partial SDP PDU responses and cached
  	 * value from context can be used.
  	 */
  	if (!session->param) {
  		param = GET_PARAM(sys_slist_peek_head(&session->reqs));
  	} else {
  		param = session->param;
  	}
  
  	if (!param) {
  		BT_WARN("No UUIDs to be resolved on remote");
  		return -EINVAL;
  	}
  
  	buf = bt_l2cap_create_pdu(&sdp_pool, 0);
  
  	hdr = net_buf_add(buf, sizeof(*hdr));
  
  	hdr->op_code = BT_SDP_SVC_SEARCH_ATTR_REQ;
  	/* BT_SDP_SEQ8 means length of sequence is on additional next byte */
  	net_buf_add_u8(buf, BT_SDP_SEQ8);
  
  	switch (param->uuid->type) {
  	case BT_UUID_TYPE_16:
  		/* Seq length */
  		net_buf_add_u8(buf, 0x03);
  		/* Seq type */
  		net_buf_add_u8(buf, BT_SDP_UUID16);
  		/* Seq value */
  		net_buf_add_be16(buf, BT_UUID_16(param->uuid)->val);
  		break;
  	case BT_UUID_TYPE_32:
  		net_buf_add_u8(buf, 0x05);
  		net_buf_add_u8(buf, BT_SDP_UUID32);
  		net_buf_add_be32(buf, BT_UUID_32(param->uuid)->val);
  		break;
  	case BT_UUID_TYPE_128:
  		net_buf_add_u8(buf, 0x11);
  		net_buf_add_u8(buf, BT_SDP_UUID128);
  		net_buf_add_mem(buf, BT_UUID_128(param->uuid)->val,
  				ARRAY_SIZE(BT_UUID_128(param->uuid)->val));
  		break;
  	default:
  		BT_ERR("Unknown UUID type %u", param->uuid->type);
  		return -EINVAL;
  	}
  
  	/* Set attribute max bytes count to be returned from server */
  	net_buf_add_be16(buf, BT_SDP_MAX_ATTR_LEN);
  	/*
  	 * Sequence definition where data is sequence of elements and where
  	 * additional next byte points the size of elements within
  	 */
  	net_buf_add_u8(buf, BT_SDP_SEQ8);
  	net_buf_add_u8(buf, 0x05);
  	/* Data element definition for two following 16bits range elements */
  	net_buf_add_u8(buf, BT_SDP_UINT32);
  	/* Get all attributes. It enables filter out wanted only attributes */
  	net_buf_add_be16(buf, 0x0000);
  	net_buf_add_be16(buf, 0xffff);
  
  	/*
  	 * Update and validate PDU ContinuationState. Initial SSA Request has
  	 * zero length continuation state since no interaction has place with
  	 * server so far, otherwise use the original state taken from remote's
  	 * last response PDU that is cached by SDP client context.
  	 */
  	if (session->cstate.length == 0U) {
  		net_buf_add_u8(buf, 0x00);
  	} else {
  		net_buf_add_u8(buf, session->cstate.length);
  		net_buf_add_mem(buf, session->cstate.data,
  				session->cstate.length);
  	}
  
  	/* set overall PDU length */
  	hdr->param_len = sys_cpu_to_be16(buf->len - sizeof(*hdr));
  
  	/* Update context param to the one being resolving now */
  	session->param = param;
  	session->tid++;
  	hdr->tid = sys_cpu_to_be16(session->tid);
  
  	return bt_l2cap_chan_send(&session->chan.chan, buf);
  }
  
  static void sdp_client_params_iterator(struct bt_sdp_client *session)
  {
  	struct bt_l2cap_chan *chan = &session->chan.chan;
  	struct bt_sdp_discover_params *param, *tmp;
  
  	SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&session->reqs, param, tmp, _node) {
  		if (param != session->param) {
  			continue;
  		}
  
  		BT_DBG("");
  
  		/* Remove already checked UUID node */
  		sys_slist_remove(&session->reqs, NULL, &param->_node);
  		/* Invalidate cached param in context */
  		session->param = NULL;
  		/* Reset continuation state in current context */
  		(void)memset(&session->cstate, 0, sizeof(session->cstate));
  
  		/* Check if there's valid next UUID */
  		if (!sys_slist_is_empty(&session->reqs)) {
  			sdp_client_ssa_search(session);
  			return;
  		}
  
  		/* No UUID items, disconnect channel */
  		bt_l2cap_chan_disconnect(chan);
  		break;
  	}
  }
  
  static u16_t sdp_client_get_total(struct bt_sdp_client *session,
  				  struct net_buf *buf, u16_t *total)
  {
  	u16_t pulled;
  	u8_t seq;
  
  	/*
  	 * Pull value of total octets of all attributes available to be
  	 * collected when response gets completed for given UUID. Such info can
  	 * be get from the very first response frame after initial SSA request
  	 * was sent. For subsequent calls related to the same SSA request input
  	 * buf and in/out function parameters stays neutral.
  	 */
  	if (session->cstate.length == 0U) {
  		seq = net_buf_pull_u8(buf);
  		pulled = 1U;
  		switch (seq) {
  		case BT_SDP_SEQ8:
  			*total = net_buf_pull_u8(buf);
  			pulled += 1U;
  			break;
  		case BT_SDP_SEQ16:
  			*total = net_buf_pull_be16(buf);
  			pulled += 2U;
  			break;
  		default:
  			BT_WARN("Sequence type 0x%02x not handled", seq);
  			*total = 0U;
  			break;
  		}
  
  		BT_DBG("Total %u octets of all attributes", *total);
  	} else {
  		pulled = 0U;
  		*total = 0U;
  	}
  
  	return pulled;
  }
  
  static u16_t get_record_len(struct net_buf *buf)
  {
  	u16_t len;
  	u8_t seq;
  
  	seq = net_buf_pull_u8(buf);
  
  	switch (seq) {
  	case BT_SDP_SEQ8:
  		len = net_buf_pull_u8(buf);
  		break;
  	case BT_SDP_SEQ16:
  		len = net_buf_pull_be16(buf);
  		break;
  	default:
  		BT_WARN("Sequence type 0x%02x not handled", seq);
  		len = 0U;
  		break;
  	}
  
  	BT_DBG("Record len %u", len);
  
  	return len;
  }
  
  enum uuid_state {
  	UUID_NOT_RESOLVED,
  	UUID_RESOLVED,
  };
  
  static void sdp_client_notify_result(struct bt_sdp_client *session,
  				     enum uuid_state state)
  {
  	struct bt_conn *conn = session->chan.chan.conn;
  	struct bt_sdp_client_result result;
  	u16_t rec_len;
  	u8_t user_ret;
  
  	result.uuid = session->param->uuid;
  
  	if (state == UUID_NOT_RESOLVED) {
  		result.resp_buf = NULL;
  		result.next_record_hint = false;
  		session->param->func(conn, &result);
  		return;
  	}
  
  	while (session->rec_buf->len) {
  		struct net_buf_simple_state buf_state;
  
  		rec_len = get_record_len(session->rec_buf);
  		/* tell the user about multi record resolution */
  		if (session->rec_buf->len > rec_len) {
  			result.next_record_hint = true;
  		} else {
  			result.next_record_hint = false;
  		}
  
  		/* save the original session buffer */
  		net_buf_simple_save(&session->rec_buf->b, &buf_state);
  		/* initialize internal result buffer instead of memcpy */
  		result.resp_buf = session->rec_buf;
  		/*
  		 * Set user internal result buffer length as same as record
  		 * length to fake user. User will see the individual record
  		 * length as rec_len insted of whole session rec_buf length.
  		 */
  		result.resp_buf->len = rec_len;
  
  		user_ret = session->param->func(conn, &result);
  
  		/* restore original session buffer */
  		net_buf_simple_restore(&session->rec_buf->b, &buf_state);
  		/*
  		 * sync session buffer data length with next record chunk not
  		 * send to user so far
  		 */
  		net_buf_pull(session->rec_buf, rec_len);
  		if (user_ret == BT_SDP_DISCOVER_UUID_STOP) {
  			break;
  		}
  	}
  }
  
  static int sdp_client_receive(struct bt_l2cap_chan *chan, struct net_buf *buf)
  {
  	struct bt_sdp_client *session = SDP_CLIENT_CHAN(chan);
  	struct bt_sdp_hdr *hdr;
  	struct bt_sdp_pdu_cstate *cstate;
  	u16_t len, tid, frame_len;
  	u16_t total;
  
  	BT_DBG("session %p buf %p", session, buf);
  
  	if (buf->len < sizeof(*hdr)) {
  		BT_ERR("Too small SDP PDU");
  		return 0;
  	}
  
  	hdr = net_buf_pull_mem(buf, sizeof(*hdr));
  	if (hdr->op_code == BT_SDP_ERROR_RSP) {
  		BT_INFO("Error SDP PDU response");
  		return 0;
  	}
  
  	len = sys_be16_to_cpu(hdr->param_len);
  	tid = sys_be16_to_cpu(hdr->tid);
  
  	BT_DBG("SDP PDU tid %u len %u", tid, len);
  
  	if (buf->len != len) {
  		BT_ERR("SDP PDU length mismatch (%u != %u)", buf->len, len);
  		return 0;
  	}
  
  	if (tid != session->tid) {
  		BT_ERR("Mismatch transaction ID value in SDP PDU");
  		return 0;
  	}
  
  	switch (hdr->op_code) {
  	case BT_SDP_SVC_SEARCH_ATTR_RSP:
  		/* Get number of attributes in this frame. */
  		frame_len = net_buf_pull_be16(buf);
  		/* Check valid buf len for attribute list and cont state */
  		if (buf->len < frame_len + SDP_CONT_STATE_LEN_SIZE) {
  			BT_ERR("Invalid frame payload length");
  			return 0;
  		}
  		/* Check valid range of attributes length */
  		if (frame_len < 2) {
  			BT_ERR("Invalid attributes data length");
  			return 0;
  		}
  
  		/* Get PDU continuation state */
  		cstate = (struct bt_sdp_pdu_cstate *)(buf->data + frame_len);
  
  		if (cstate->length > BT_SDP_MAX_PDU_CSTATE_LEN) {
  			BT_ERR("Invalid SDP PDU Continuation State length %u",
  			       cstate->length);
  			return 0;
  		}
  
  		if ((frame_len + SDP_CONT_STATE_LEN_SIZE + cstate->length) >
  		     buf->len) {
  			BT_ERR("Invalid frame payload length");
  			return 0;
  		}
  
  		/*
  		 * No record found for given UUID. The check catches case when
  		 * current response frame has Continuation State shortest and
  		 * valid and this is the first response frame as well.
  		 */
  		if (frame_len == 2U && cstate->length == 0U &&
  		    session->cstate.length == 0U) {
  			BT_DBG("record for UUID 0x%s not found",
  				bt_uuid_str(session->param->uuid));
  			/* Call user UUID handler */
  			sdp_client_notify_result(session, UUID_NOT_RESOLVED);
  			net_buf_pull(buf, frame_len + sizeof(cstate->length));
  			goto iterate;
  		}
  
  		/* Get total value of all attributes to be collected */
  		frame_len -= sdp_client_get_total(session, buf, &total);
  
  		if (total > net_buf_tailroom(session->rec_buf)) {
  			BT_WARN("Not enough room for getting records data");
  			goto iterate;
  		}
  
  		net_buf_add_mem(session->rec_buf, buf->data, frame_len);
  		net_buf_pull(buf, frame_len);
  
  		/*
  		 * check if current response says there's next portion to be
  		 * fetched
  		 */
  		if (cstate->length) {
  			/* Cache original Continuation State in context */
  			memcpy(&session->cstate, cstate,
  			       sizeof(struct bt_sdp_pdu_cstate));
  
  			net_buf_pull(buf, cstate->length +
  				     sizeof(cstate->length));
  
  			/* Request for next portion of attributes data */
  			sdp_client_ssa_search(session);
  			break;
  		}
  
  		net_buf_pull(buf, sizeof(cstate->length));
  
  		BT_DBG("UUID 0x%s resolved", bt_uuid_str(session->param->uuid));
  		sdp_client_notify_result(session, UUID_RESOLVED);
  iterate:
  		/* Get next UUID and start resolving it */
  		sdp_client_params_iterator(session);
  		break;
  	default:
  		BT_DBG("PDU 0x%0x response not handled", hdr->op_code);
  		break;
  	}
  
  	return 0;
  }
  
  static int sdp_client_chan_connect(struct bt_sdp_client *session)
  {
  	return bt_l2cap_br_chan_connect(session->chan.chan.conn,
  					&session->chan.chan, SDP_PSM);
  }
  
  static struct net_buf *sdp_client_alloc_buf(struct bt_l2cap_chan *chan)
  {
  	struct bt_sdp_client *session = SDP_CLIENT_CHAN(chan);
  	struct net_buf *buf;
  
  	BT_DBG("session %p chan %p", session, chan);
  
  	session->param = GET_PARAM(sys_slist_peek_head(&session->reqs));
  
  	buf = net_buf_alloc(session->param->pool, K_FOREVER);
  	__ASSERT_NO_MSG(buf);
  
  	return buf;
  }
  
  static void sdp_client_connected(struct bt_l2cap_chan *chan)
  {
  	struct bt_sdp_client *session = SDP_CLIENT_CHAN(chan);
  
  	BT_DBG("session %p chan %p connected", session, chan);
  
  	session->rec_buf = chan->ops->alloc_buf(chan);
  
  	sdp_client_ssa_search(session);
  }
  
  static void sdp_client_disconnected(struct bt_l2cap_chan *chan)
  {
  	struct bt_sdp_client *session = SDP_CLIENT_CHAN(chan);
  
  	BT_DBG("session %p chan %p disconnected", session, chan);
  
  	net_buf_unref(session->rec_buf);
  
  	/*
  	 * Reset session excluding L2CAP channel member. Let's the channel
  	 * resets autonomous.
  	 */
  	(void)memset(&session->reqs, 0,
  		     sizeof(*session) - sizeof(session->chan));
  }
  
  static struct bt_l2cap_chan_ops sdp_client_chan_ops = {
  		.connected = sdp_client_connected,
  		.disconnected = sdp_client_disconnected,
  		.recv = sdp_client_receive,
  		.alloc_buf = sdp_client_alloc_buf,
  };
  
  static struct bt_sdp_client *sdp_client_new_session(struct bt_conn *conn)
  {
  	int i;
  
  	for (i = 0; i < ARRAY_SIZE(bt_sdp_client_pool); i++) {
  		struct bt_sdp_client *session = &bt_sdp_client_pool[i];
  		int err;
  
  		if (session->chan.chan.conn) {
  			continue;
  		}
  
  		sys_slist_init(&session->reqs);
  
  		session->chan.chan.ops = &sdp_client_chan_ops;
  		session->chan.chan.conn = conn;
  		session->chan.rx.mtu = SDP_CLIENT_MTU;
  
  		err = sdp_client_chan_connect(session);
  		if (err) {
  			(void)memset(session, 0, sizeof(*session));
  			BT_ERR("Cannot connect %d", err);
  			return NULL;
  		}
  
  		return session;
  	}
  
  	BT_ERR("No available SDP client context");
  
  	return NULL;
  }
  
  static struct bt_sdp_client *sdp_client_get_session(struct bt_conn *conn)
  {
  	int i;
  
  	for (i = 0; i < ARRAY_SIZE(bt_sdp_client_pool); i++) {
  		if (bt_sdp_client_pool[i].chan.chan.conn == conn) {
  			return &bt_sdp_client_pool[i];
  		}
  	}
  
  	/*
  	 * Try to allocate session context since not found in pool and attempt
  	 * connect to remote SDP endpoint.
  	 */
  	return sdp_client_new_session(conn);
  }
  
  int bt_sdp_discover(struct bt_conn *conn,
  		    const struct bt_sdp_discover_params *params)
  {
  	struct bt_sdp_client *session;
  
  	if (!params || !params->uuid || !params->func || !params->pool) {
  		BT_WARN("Invalid user params");
  		return -EINVAL;
  	}
  
  	session = sdp_client_get_session(conn);
  	if (!session) {
  		return -ENOMEM;
  	}
  
  	sys_slist_append(&session->reqs, (sys_snode_t *)&params->_node);
  
  	return 0;
  }
  
  /* Helper getting length of data determined by DTD for integers */
  static inline ssize_t sdp_get_int_len(const u8_t *data, size_t len)
  {
  	BT_ASSERT(data);
  
  	switch (data[0]) {
  	case BT_SDP_DATA_NIL:
  		return 1;
  	case BT_SDP_BOOL:
  	case BT_SDP_INT8:
  	case BT_SDP_UINT8:
  		if (len < 2) {
  			break;
  		}
  
  		return 2;
  	case BT_SDP_INT16:
  	case BT_SDP_UINT16:
  		if (len < 3) {
  			break;
  		}
  
  		return 3;
  	case BT_SDP_INT32:
  	case BT_SDP_UINT32:
  		if (len < 5) {
  			break;
  		}
  
  		return 5;
  	case BT_SDP_INT64:
  	case BT_SDP_UINT64:
  		if (len < 9) {
  			break;
  		}
  
  		return 9;
  	case BT_SDP_INT128:
  	case BT_SDP_UINT128:
  	default:
  		BT_ERR("Invalid/unhandled DTD 0x%02x", data[0]);
  		return -EINVAL;
  	}
  
  	BT_ERR("Too short buffer length %zu", len);
  	return -EMSGSIZE;
  }
  
  /* Helper getting length of data determined by DTD for UUID */
  static inline ssize_t sdp_get_uuid_len(const u8_t *data, size_t len)
  {
  	BT_ASSERT(data);
  
  	switch (data[0]) {
  	case BT_SDP_UUID16:
  		if (len < 3) {
  			break;
  		}
  
  		return 3;
  	case BT_SDP_UUID32:
  		if (len < 5) {
  			break;
  		}
  
  		return 5;
  	case BT_SDP_UUID128:
  	default:
  		BT_ERR("Invalid/unhandled DTD 0x%02x", data[0]);
  		return -EINVAL;
  	}
  
  	BT_ERR("Too short buffer length %zu", len);
  	return -EMSGSIZE;
  }
  
  /* Helper getting length of data determined by DTD for strings */
  static inline ssize_t sdp_get_str_len(const u8_t *data, size_t len)
  {
  	const u8_t *pnext;
  
  	BT_ASSERT(data);
  
  	/* validate len for pnext safe use to read next 8bit value */
  	if (len < 2) {
  		goto err;
  	}
  
  	pnext = data + sizeof(u8_t);
  
  	switch (data[0]) {
  	case BT_SDP_TEXT_STR8:
  	case BT_SDP_URL_STR8:
  		if (len < (2 + pnext[0])) {
  			break;
  		}
  
  		return 2 + pnext[0];
  	case BT_SDP_TEXT_STR16:
  	case BT_SDP_URL_STR16:
  		/* validate len for pnext safe use to read 16bit value */
  		if (len < 3) {
  			break;
  		}
  
  		if (len < (3 + sys_get_be16(pnext))) {
  			break;
  		}
  
  		return 3 + sys_get_be16(pnext);
  	case BT_SDP_TEXT_STR32:
  	case BT_SDP_URL_STR32:
  	default:
  		BT_ERR("Invalid/unhandled DTD 0x%02x", data[0]);
  		return -EINVAL;
  	}
  err:
  	BT_ERR("Too short buffer length %zu", len);
  	return -EMSGSIZE;
  }
  
  /* Helper getting length of data determined by DTD for sequences */
  static inline ssize_t sdp_get_seq_len(const u8_t *data, size_t len)
  {
  	const u8_t *pnext;
  
  	BT_ASSERT(data);
  
  	/* validate len for pnext safe use to read 8bit bit value */
  	if (len < 2) {
  		goto err;
  	}
  
  	pnext = data + sizeof(u8_t);
  
  	switch (data[0]) {
  	case BT_SDP_SEQ8:
  	case BT_SDP_ALT8:
  		if (len < (2 + pnext[0])) {
  			break;
  		}
  
  		return 2 + pnext[0];
  	case BT_SDP_SEQ16:
  	case BT_SDP_ALT16:
  		/* validate len for pnext safe use to read 16bit value */
  		if (len < 3) {
  			break;
  		}
  
  		if (len < (3 + sys_get_be16(pnext))) {
  			break;
  		}
  
  		return 3 + sys_get_be16(pnext);
  	case BT_SDP_SEQ32:
  	case BT_SDP_ALT32:
  	default:
  		BT_ERR("Invalid/unhandled DTD 0x%02x", data[0]);
  		return -EINVAL;
  	}
  err:
  	BT_ERR("Too short buffer length %zu", len);
  	return -EMSGSIZE;
  }
  
  /* Helper getting length of attribute value data */
  static ssize_t sdp_get_attr_value_len(const u8_t *data, size_t len)
  {
  	BT_ASSERT(data);
  
  	BT_DBG("Attr val DTD 0x%02x", data[0]);
  
  	switch (data[0]) {
  	case BT_SDP_DATA_NIL:
  	case BT_SDP_BOOL:
  	case BT_SDP_UINT8:
  	case BT_SDP_UINT16:
  	case BT_SDP_UINT32:
  	case BT_SDP_UINT64:
  	case BT_SDP_UINT128:
  	case BT_SDP_INT8:
  	case BT_SDP_INT16:
  	case BT_SDP_INT32:
  	case BT_SDP_INT64:
  	case BT_SDP_INT128:
  		return sdp_get_int_len(data, len);
  	case BT_SDP_UUID16:
  	case BT_SDP_UUID32:
  	case BT_SDP_UUID128:
  		return sdp_get_uuid_len(data, len);
  	case BT_SDP_TEXT_STR8:
  	case BT_SDP_TEXT_STR16:
  	case BT_SDP_TEXT_STR32:
  	case BT_SDP_URL_STR8:
  	case BT_SDP_URL_STR16:
  	case BT_SDP_URL_STR32:
  		return sdp_get_str_len(data, len);
  	case BT_SDP_SEQ8:
  	case BT_SDP_SEQ16:
  	case BT_SDP_SEQ32:
  	case BT_SDP_ALT8:
  	case BT_SDP_ALT16:
  	case BT_SDP_ALT32:
  		return sdp_get_seq_len(data, len);
  	default:
  		BT_ERR("Unknown DTD 0x%02x", data[0]);
  		return -EINVAL;
  	}
  }
  
  /* Type holding UUID item and related to it specific information. */
  struct bt_sdp_uuid_desc {
  	union {
  		struct bt_uuid    uuid;
  		struct bt_uuid_16 uuid16;
  		struct bt_uuid_32 uuid32;
  	      };
  	u16_t                     attr_id;
  	u8_t                     *params;
  	u16_t                     params_len;
  };
  
  /* Generic attribute item collector. */
  struct bt_sdp_attr_item {
  	/*  Attribute identifier. */
  	u16_t                  attr_id;
  	/*  Address of beginning attribute value taken from original buffer
  	 *  holding response from server.
  	 */
  	u8_t                  *val;
  	/*  Says about the length of attribute value. */
  	u16_t                  len;
  };
  
  static int bt_sdp_get_attr(const struct net_buf *buf,
  			   struct bt_sdp_attr_item *attr, u16_t attr_id)
  {
  	u8_t *data;
  	u16_t id;
  
  	data = buf->data;
  	while (data - buf->data < buf->len) {
  		ssize_t dlen;
  
  		/* data need to point to attribute id descriptor field (DTD)*/
  		if (data[0] != BT_SDP_UINT16) {
  			BT_ERR("Invalid descriptor 0x%02x", data[0]);
  			return -EINVAL;
  		}
  
  		data += sizeof(u8_t);
  		id = sys_get_be16(data);
  		BT_DBG("Attribute ID 0x%04x", id);
  		data += sizeof(u16_t);
  
  		dlen = sdp_get_attr_value_len(data,
  					      buf->len - (data - buf->data));
  		if (dlen < 0) {
  			BT_ERR("Invalid attribute value data");
  			return -EINVAL;
  		}
  
  		if (id == attr_id) {
  			BT_DBG("Attribute ID 0x%04x Value found", id);
  			/*
  			 * Initialize attribute value buffer data using selected
  			 * data slice from original buffer.
  			 */
  			attr->val = data;
  			attr->len = dlen;
  			attr->attr_id = id;
  			return 0;
  		}
  
  		data += dlen;
  	}
  
  	return -ENOENT;
  }
  
  /* reads SEQ item length, moves input buffer data reader forward */
  static ssize_t sdp_get_seq_len_item(u8_t **data, size_t len)
  {
  	const u8_t *pnext;
  
  	BT_ASSERT(data);
  	BT_ASSERT(*data);
  
  	/* validate len for pnext safe use to read 8bit bit value */
  	if (len < 2) {
  		goto err;
  	}
  
  	pnext = *data + sizeof(u8_t);
  
  	switch (*data[0]) {
  	case BT_SDP_SEQ8:
  		if (len < (2 + pnext[0])) {
  			break;
  		}
  
  		*data += 2;
  		return pnext[0];
  	case BT_SDP_SEQ16:
  		/* validate len for pnext safe use to read 16bit value */
  		if (len < 3) {
  			break;
  		}
  
  		if (len < (3 + sys_get_be16(pnext))) {
  			break;
  		}
  
  		*data += 3;
  		return sys_get_be16(pnext);
  	case BT_SDP_SEQ32:
  		/* validate len for pnext safe use to read 32bit value */
  		if (len < 5) {
  			break;
  		}
  
  		if (len < (5 + sys_get_be32(pnext))) {
  			break;
  		}
  
  		*data += 5;
  		return sys_get_be32(pnext);
  	default:
  		BT_ERR("Invalid/unhandled DTD 0x%02x", *data[0]);
  		return -EINVAL;
  	}
  err:
  	BT_ERR("Too short buffer length %zu", len);
  	return -EMSGSIZE;
  }
  
  static int sdp_get_uuid_data(const struct bt_sdp_attr_item *attr,
  			     struct bt_sdp_uuid_desc *pd,
  			     u16_t proto_profile)
  {
  	/* get start address of attribute value */
  	u8_t *p = attr->val;
  	ssize_t slen;
  
  	BT_ASSERT(p);
  
  	/* Attribute value is a SEQ, get length of parent SEQ frame */
  	slen = sdp_get_seq_len_item(&p, attr->len);
  	if (slen < 0) {
  		return slen;
  	}
  
  	/* start reading stacked UUIDs in analyzed sequences tree */
  	while (p - attr->val < attr->len) {
  		size_t to_end, left = 0;
  
  		/* to_end tells how far to the end of input buffer */
  		to_end = attr->len - (p - attr->val);
  		/* how long is current UUID's item data associated to */
  		slen = sdp_get_seq_len_item(&p, to_end);
  		if (slen < 0) {
  			return slen;
  		}
  
  		/* left tells how far is to the end of current UUID */
  		left = slen;
  
  		/* check if at least DTD + UUID16 can be read safely */
  		if (left < 3) {
  			return -EMSGSIZE;
  		}
  
  		/* check DTD and get stacked UUID value */
  		switch (p[0]) {
  		case BT_SDP_UUID16:
  			memcpy(&pd->uuid16,
  			       BT_UUID_DECLARE_16(sys_get_be16(++p)),
  			       sizeof(struct bt_uuid_16));
  			p += sizeof(u16_t);
  			left -= sizeof(u16_t);
  			break;
  		case BT_SDP_UUID32:
  			/* check if valid UUID32 can be read safely */
  			if (left < 5) {
  				return -EMSGSIZE;
  			}
  
  			memcpy(&pd->uuid32,
  			       BT_UUID_DECLARE_32(sys_get_be32(++p)),
  			       sizeof(struct bt_uuid_32));
  			p += sizeof(u32_t);
  			left -= sizeof(u32_t);
  			break;
  		default:
  			BT_ERR("Invalid/unhandled DTD 0x%02x\n", p[0]);
  			return -EINVAL;
  		}
  
  		/* include last DTD in p[0] size itself updating left */
  		left -= sizeof(p[0]);
  
  		/*
  		 * Check if current UUID value matches input one given by user.
  		 * If found save it's location and length and return.
  		 */
  		if ((proto_profile == BT_UUID_16(&pd->uuid)->val) ||
  		    (proto_profile == BT_UUID_32(&pd->uuid)->val)) {
  			pd->params = p;
  			pd->params_len = left;
  
  			BT_DBG("UUID 0x%s found", bt_uuid_str(&pd->uuid));
  			return 0;
  		}
  
  		/* skip left octets to point beginning of next UUID in tree */
  		p += left;
  	}
  
  	BT_DBG("Value 0x%04x not found", proto_profile);
  	return -ENOENT;
  }
  
  /*
   * Helper extracting specific parameters associated with UUID node given in
   * protocol descriptor list or profile descriptor list.
   */
  static int sdp_get_param_item(struct bt_sdp_uuid_desc *pd_item, u16_t *param)
  {
  	const u8_t *p = pd_item->params;
  	bool len_err = false;
  
  	BT_ASSERT(p);
  
  	BT_DBG("Getting UUID's 0x%s params", bt_uuid_str(&pd_item->uuid));
  
  	switch (p[0]) {
  	case BT_SDP_UINT8:
  		/* check if 8bits value can be read safely */
  		if (pd_item->params_len < 2) {
  			len_err = true;
  			break;
  		}
  		*param = (++p)[0];
  		p += sizeof(u8_t);
  		break;
  	case BT_SDP_UINT16:
  		/* check if 16bits value can be read safely */
  		if (pd_item->params_len < 3) {
  			len_err = true;
  			break;
  		}
  		*param = sys_get_be16(++p);
  		p += sizeof(u16_t);
  		break;
  	case BT_SDP_UINT32:
  		/* check if 32bits value can be read safely */
  		if (pd_item->params_len < 5) {
  			len_err = true;
  			break;
  		}
  		*param = sys_get_be32(++p);
  		p += sizeof(u32_t);
  		break;
  	default:
  		BT_ERR("Invalid/unhandled DTD 0x%02x\n", p[0]);
  		return -EINVAL;
  	}
  	/*
  	 * Check if no more data than already read is associated with UUID. In
  	 * valid case after getting parameter we should reach data buf end.
  	 */
  	if (p - pd_item->params != pd_item->params_len || len_err) {
  		BT_DBG("Invalid param buffer length");
  		return -EMSGSIZE;
  	}
  
  	return 0;
  }
  
  int bt_sdp_get_proto_param(const struct net_buf *buf, enum bt_sdp_proto proto,
  			   u16_t *param)
  {
  	struct bt_sdp_attr_item attr;
  	struct bt_sdp_uuid_desc pd;
  	int res;
  
  	if (proto != BT_SDP_PROTO_RFCOMM && proto != BT_SDP_PROTO_L2CAP) {
  		BT_ERR("Invalid protocol specifier");
  		return -EINVAL;
  	}
  
  	res = bt_sdp_get_attr(buf, &attr, BT_SDP_ATTR_PROTO_DESC_LIST);
  	if (res < 0) {
  		BT_WARN("Attribute 0x%04x not found, err %d",
  			BT_SDP_ATTR_PROTO_DESC_LIST, res);
  		return res;
  	}
  
  	res = sdp_get_uuid_data(&attr, &pd, proto);
  	if (res < 0) {
  		BT_WARN("Protocol specifier 0x%04x not found, err %d", proto,
  			res);
  		return res;
  	}
  
  	return sdp_get_param_item(&pd, param);
  }
  
  int bt_sdp_get_profile_version(const struct net_buf *buf, u16_t profile,
  			       u16_t *version)
  {
  	struct bt_sdp_attr_item attr;
  	struct bt_sdp_uuid_desc pd;
  	int res;
  
  	res = bt_sdp_get_attr(buf, &attr, BT_SDP_ATTR_PROFILE_DESC_LIST);
  	if (res < 0) {
  		BT_WARN("Attribute 0x%04x not found, err %d",
  			BT_SDP_ATTR_PROFILE_DESC_LIST, res);
  		return res;
  	}
  
  	res = sdp_get_uuid_data(&attr, &pd, profile);
  	if (res < 0) {
  		BT_WARN("Profile 0x%04x not found, err %d", profile, res);
  		return res;
  	}
  
  	return sdp_get_param_item(&pd, version);
  }
  
  int bt_sdp_get_features(const struct net_buf *buf, u16_t *features)
  {
  	struct bt_sdp_attr_item attr;
  	const u8_t *p;
  	int res;
  
  	res = bt_sdp_get_attr(buf, &attr, BT_SDP_ATTR_SUPPORTED_FEATURES);
  	if (res < 0) {
  		BT_WARN("Attribute 0x%04x not found, err %d",
  			BT_SDP_ATTR_SUPPORTED_FEATURES, res);
  		return res;
  	}
  
  	p = attr.val;
  	BT_ASSERT(p);
  
  	if (p[0] != BT_SDP_UINT16) {
  		BT_ERR("Invalid DTD 0x%02x", p[0]);
  		return -EINVAL;
  	}
  
  	/* assert 16bit can be read safely */
  	if (attr.len < 3) {
  		BT_ERR("Data length too short %u", attr.len);
  		return -EMSGSIZE;
  	}
  
  	*features = sys_get_be16(++p);
  	p += sizeof(u16_t);
  
  	if (p - attr.val != attr.len) {
  		BT_ERR("Invalid data length %u", attr.len);
  		return -EMSGSIZE;
  	}
  
  	return 0;
  }
  