Google Git
Sign in
pigweed / third_party / github / zephyrproject-rtos / zephyr / bb0b45dd53c41505bcbfabc2821c00c0c20b97ba / . / subsys / bluetooth / host / classic / sdp.c
blob: 1ed6e1598e54f98d1d95613b5bf228d3d9b94b29 [file] [log] [blame]
/** @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 <zephyr/sys/byteorder.h>
#include <zephyr/sys/__assert.h>
#include <zephyr/bluetooth/buf.h>
#include <zephyr/bluetooth/classic/sdp.h>
#include "common/bt_str.h"
#include "common/assert.h"
#include "host/hci_core.h"
#include "host/conn_internal.h"
#include "l2cap_br_internal.h"
#include "sdp_internal.h"
#define LOG_LEVEL CONFIG_BT_SDP_LOG_LEVEL
#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(bt_sdp);
#define SDP_PSM 0x0001
#define SDP_CHAN(_ch) CONTAINER_OF(_ch, struct bt_sdp, chan.chan)
#define SDP_DATA_MTU 200
#define SDP_MTU (SDP_DATA_MTU + sizeof(struct bt_sdp_hdr))
#define MAX_NUM_ATT_ID_FILTER 10
#define MAX_NUM_SSP_UUID 12
#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 */
/* 4 bytes for the index of current attribute searched till this response */
#define SDP_SA_CONT_STATE_SIZE 5
/* 1 byte for the no. of services searched till this response */
/* 1 byte for the no. of attributes searched till this response */
/* 4 bytes for the index of current attribute searched till this response */
#define SDP_SSA_CONT_STATE_SIZE 6
#define SDP_INVALID 0xff
/* SDP record handle size */
#define SDP_RECORD_HANDLE_SIZE 4
struct bt_sdp {
struct bt_l2cap_br_chan chan;
/* TODO: Allow more than one pending request */
};
static sys_slist_t sdp_db = SYS_SLIST_STATIC_INIT(&sdp_db);
static uint8_t num_services;
static struct bt_sdp bt_sdp_pool[CONFIG_BT_MAX_CONN];
/* Pool for outgoing SDP packets */
NET_BUF_POOL_FIXED_DEFINE(sdp_pool, CONFIG_BT_MAX_CONN, BT_L2CAP_BUF_SIZE(SDP_MTU),
CONFIG_BT_CONN_TX_USER_DATA_SIZE, NULL);
#define SDP_CLIENT_CHAN(_ch) CONTAINER_OF(_ch, struct bt_sdp_client, chan.chan)
#define SDP_CLIENT_MTU 64
#define SDP_SA_MAX_ATTR_BYTE_COUNT 0xffff
#define SDP_SA_MIN_ATTR_BYTE_COUNT 0x0007
#define SDP_SA_ATTR_BYTE_IN_RANGE(count) \
((count) >= SDP_SA_MIN_ATTR_BYTE_COUNT && (count) <= SDP_SA_MAX_ATTR_BYTE_COUNT)
#define SDP_SSA_MAX_ATTR_BYTE_COUNT 0xffff
#define SDP_SSA_MIN_ATTR_BYTE_COUNT 0x0007
#define SDP_SSA_ATTR_BYTE_IN_RANGE(count) \
((count) >= SDP_SSA_MIN_ATTR_BYTE_COUNT && (count) <= SDP_SSA_MAX_ATTR_BYTE_COUNT)
enum sdp_client_state {
SDP_CLIENT_RELEASED,
SDP_CLIENT_CONNECTING,
SDP_CLIENT_CONNECTED,
SDP_CLIENT_DISCONNECTING,
};
struct bt_sdp_client {
/* semaphore for lock/unlock */
struct k_sem sem_lock;
struct bt_l2cap_br_chan chan;
/* list of waiting to create sdp connection again */
sys_slist_t reqs_next;
/* list of waiting to be resolved UUID params */
sys_slist_t reqs;
/* required SDP transaction ID */
uint16_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;
/* The total length of response */
uint32_t total_len;
/* Received data length */
uint32_t recv_len;
/* client state */
enum sdp_client_state state;
};
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 {
uint16_t att_list_size;
uint8_t current_svc;
uint8_t last_att;
uint32_t last_att_index;
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;
uint32_t *filter;
uint16_t max_att_len;
uint16_t att_list_len;
uint8_t cont_state_size;
size_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 uint8_t (*bt_sdp_attr_func_t)(struct bt_sdp_attribute *attr,
uint8_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 uint8_t (*bt_sdp_svc_func_t)(struct bt_sdp_record *rec,
void *user_data);
static int sdp_client_new_session(struct bt_conn *conn, struct bt_sdp_client *session);
/* @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 __unused = CONTAINER_OF(ch, struct bt_sdp, chan);
LOG_DBG("chan %p cid 0x%04x", ch, ch->tx.cid);
}
/** @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 __unused = CONTAINER_OF(ch, struct bt_sdp, chan);
LOG_DBG("chan %p cid 0x%04x", ch, ch->tx.cid);
}
/* @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 int bt_sdp_send(struct bt_l2cap_chan *chan, struct net_buf *buf,
uint8_t op, uint16_t tid)
{
struct bt_sdp_hdr *hdr;
uint16_t param_len = buf->len;
int err;
hdr = net_buf_push(buf, sizeof(struct bt_sdp_hdr));
hdr->op_code = op;
hdr->tid = sys_cpu_to_be16(tid);
hdr->param_len = sys_cpu_to_be16(param_len);
err = bt_l2cap_chan_send(chan, buf);
if (err < 0) {
net_buf_unref(buf);
}
return err;
}
/* @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, uint16_t err,
uint16_t tid)
{
struct net_buf *buf;
LOG_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
*
* 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 uint16_t parse_data_elem(struct net_buf_simple *buf,
struct bt_sdp_data_elem *data_elem)
{
uint8_t size_field_len = 0U; /* Space used to accommodate the size */
if (buf->len < 1) {
LOG_WRN("Malformed packet");
return BT_SDP_INVALID_SYNTAX;
}
data_elem->type = net_buf_simple_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) {
LOG_WRN("Malformed packet");
return BT_SDP_INVALID_SYNTAX;
}
switch (size_field_len) {
case 1:
data_elem->data_size = net_buf_simple_pull_u8(buf);
break;
case 2:
data_elem->data_size = net_buf_simple_pull_be16(buf);
break;
case 4:
data_elem->data_size = net_buf_simple_pull_be32(buf);
break;
default:
LOG_WRN("Invalid size in remote request");
return BT_SDP_INVALID_SYNTAX;
}
break;
default:
LOG_WRN("Invalid type in remote request");
return BT_SDP_INVALID_SYNTAX;
}
if (buf->len < data_elem->data_size) {
LOG_WRN("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 uint32_t search_uuid(struct bt_sdp_data_elem *elem, struct bt_uuid *uuid,
bool *found, uint8_t nest_level)
{
const uint8_t *cur_elem;
uint32_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 = *((uint16_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 = *((uint32_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 {
LOG_WRN("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, *next;
SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&sdp_db, rec, next, node) {
if (func(rec, user_data) == BT_SDP_ITER_STOP) {
break;
}
}
return rec;
}
/* @brief Parse service search pattern
*
* Parse service search pattern
*
* @param buf Request net buf
* @param ssp Service search pattern buffer
*
* @return 0 for success, or relevant error code
*/
static uint16_t parse_service_search_pattern(struct net_buf *buf, struct net_buf_simple *ssp)
{
struct net_buf_simple_state state;
struct bt_sdp_data_elem data_elem;
uint16_t res;
uint8_t uuid_count = 0U;
res = parse_data_elem(&buf->b, &data_elem);
if (res != 0) {
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)) {
LOG_WRN("Invalid type %x in service search pattern", data_elem.type);
return BT_SDP_INVALID_SYNTAX;
}
if (buf->len < data_elem.data_size) {
LOG_WRN("Malformed packet");
return BT_SDP_INVALID_SYNTAX;
}
net_buf_simple_init_with_data(ssp, net_buf_pull_mem(buf, data_elem.data_size),
data_elem.data_size);
net_buf_simple_save(ssp, &state);
/* Check the service search pattern is valid or not */
while (ssp->len > 0) {
res = parse_data_elem(ssp, &data_elem);
if (res != 0) {
return res;
}
if ((data_elem.type & BT_SDP_TYPE_DESC_MASK) != BT_SDP_UUID_UNSPEC) {
LOG_WRN("Invalid type %u in service search pattern", data_elem.type);
return BT_SDP_INVALID_SYNTAX;
}
if (ssp->len < data_elem.data_size) {
LOG_WRN("Malformed packet");
return BT_SDP_INVALID_SYNTAX;
}
if (!((data_elem.data_size == BT_UUID_SIZE_16) ||
(data_elem.data_size == BT_UUID_SIZE_32) ||
(data_elem.data_size == BT_UUID_SIZE_128))) {
LOG_WRN("INvalid UUID size");
return BT_SDP_INVALID_SYNTAX;
}
net_buf_simple_pull(ssp, data_elem.data_size);
uuid_count++;
if (uuid_count > MAX_NUM_SSP_UUID) {
LOG_WRN("Too many UUIDs in ssp %u> %u", uuid_count, MAX_NUM_SSP_UUID);
return BT_SDP_INVALID_SYNTAX;
}
}
net_buf_simple_restore(ssp, &state);
return 0;
}
/* @brief Match service search pattern in the specific SDP record
*
* Match service search pattern in the specific SDP record
*
* @param buf Service search pattern
* @param record SDP record to match against
*
* @return true if matched, false for no match
*/
static bool service_search_pattern_matched(struct net_buf_simple *buf, struct bt_sdp_record *record)
{
struct net_buf_simple_state state;
struct bt_sdp_data_elem data_elem;
uint16_t res;
union {
struct bt_uuid uuid;
struct bt_uuid_16 u16;
struct bt_uuid_32 u32;
struct bt_uuid_128 u128;
} u;
bool found = false;
net_buf_simple_save(buf, &state);
while (buf->len > 0) {
res = parse_data_elem(buf, &data_elem);
if (res != 0) {
break;
}
if ((data_elem.type & BT_SDP_TYPE_DESC_MASK) != BT_SDP_UUID_UNSPEC) {
LOG_WRN("Invalid type %u in service search pattern", data_elem.type);
break;
}
if (buf->len < data_elem.data_size) {
LOG_WRN("Malformed packet");
break;
}
if (data_elem.data_size == BT_UUID_SIZE_16) {
u.uuid.type = BT_UUID_TYPE_16;
u.u16.val = net_buf_simple_pull_be16(buf);
} else if (data_elem.data_size == BT_UUID_SIZE_32) {
u.uuid.type = BT_UUID_TYPE_32;
u.u32.val = net_buf_simple_pull_be32(buf);
} else if (data_elem.data_size == BT_UUID_SIZE_128) {
u.uuid.type = BT_UUID_TYPE_128;
/* Change big endian to little endian */
sys_memcpy_swap(u.u128.val, buf->data, data_elem.data_size);
net_buf_simple_pull(buf, data_elem.data_size);
} else {
LOG_WRN("Invalid UUID len %u in service search pattern",
data_elem.data_size);
net_buf_simple_pull(buf, data_elem.data_size);
continue;
}
for (size_t index = 0; index < record->attr_count; index++) {
struct bt_sdp_attribute *attr;
attr = &record->attrs[index];
(void)search_uuid(&attr->val, &u.uuid, &found, 1);
if (found) {
goto matched;
}
}
}
matched:
net_buf_simple_restore(buf, &state);
return found;
}
/* @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 uint16_t sdp_svc_search_req(struct bt_sdp *sdp, struct net_buf *buf, uint16_t tid)
{
struct bt_sdp_svc_rsp *rsp;
struct net_buf *resp_buf;
struct bt_sdp_record *record, *next;
uint16_t max_rec_count, total_recs = 0U, res;
uint8_t cont_state_size;
uint8_t cont_recs = 0U;
uint8_t matched_recs = 0U;
uint8_t current_recs = 0U;
bool pkt_full = false;
struct net_buf_simple ssp;
res = parse_service_search_pattern(buf, &ssp);
if (res) {
/* Error in parsing */
return res;
}
if (buf->len < (sizeof(max_rec_count) + sizeof(cont_state_size))) {
LOG_WRN("Malformed packet");
return BT_SDP_INVALID_SYNTAX;
}
max_rec_count = net_buf_pull_be16(buf);
cont_state_size = net_buf_pull_u8(buf);
/* 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) {
LOG_WRN("Invalid cont state size %u", cont_state_size);
return BT_SDP_INVALID_CSTATE;
}
if (buf->len < cont_state_size) {
LOG_WRN("Malformed packet");
return BT_SDP_INVALID_SYNTAX;
}
cont_recs = 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);
}
LOG_DBG("max_rec_count %u, cont_recs %u", max_rec_count, cont_recs);
resp_buf = bt_sdp_create_pdu();
rsp = net_buf_add(resp_buf, sizeof(*rsp));
SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&sdp_db, record, next, node) {
uint16_t require_len;
if (!service_search_pattern_matched(&ssp, record)) {
continue;
}
matched_recs++;
/* Calculate total recs only if it is first packet */
if (!cont_state_size) {
total_recs++;
} else if (matched_recs > total_recs) {
LOG_WRN("Reached total records %u > %u", matched_recs, total_recs);
break;
}
if (matched_recs > max_rec_count) {
LOG_WRN("Reached MAX record count %u > %u", matched_recs, max_rec_count);
break;
}
if (matched_recs < cont_recs) {
continue;
}
if (pkt_full) {
continue;
}
require_len = sizeof(record->handle) + sizeof(cont_state_size) +
SDP_SS_CONT_STATE_SIZE + sizeof(struct bt_sdp_hdr);
if ((MIN(SDP_MTU, sdp->chan.tx.mtu) - resp_buf->len) < require_len) {
pkt_full = true;
}
if (pkt_full) {
/* Packet exhausted: Add continuation state and break */
LOG_DBG("Packet full, num_services_covered %u", current_recs);
net_buf_add_u8(resp_buf, SDP_SS_CONT_STATE_SIZE);
net_buf_add_u8(resp_buf, matched_recs);
/* 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 Service Record Handle */
net_buf_add_be32(resp_buf, record->handle);
current_recs++;
}
/* If packet is not exhausted, add 0 continuation state.
* Else, add total records count for continuation state.
*/
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);
LOG_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
*
* This function handles partial copying of attributes into a response buffer
* with careful management of offsets, indices, and remaining space.
*
* @param buf Destination network buffer for attribute data
* @param data Source attribute data
* @param length Total length of attribute data
* @param offset Current offset in the attribute data
* @param index Current index tracking progress
* @param space Remaining available space in the buffer
*
* @return Number of bytes copied or 0 if no bytes copied
*/
static uint16_t copy_attribute_from_buf(struct net_buf *buf, const uint8_t *data, uint32_t length,
uint32_t *offset, uint32_t *index, uint16_t *space)
{
uint32_t end;
uint32_t start = *offset;
uint16_t len;
end = start + length;
*offset = end;
/* Copied data range is start ~ end */
if (*index < start) {
len = MIN(length, *space);
net_buf_add_mem(buf, data, len);
*space -= len;
*index += len;
return len;
}
/* Copied data range is *index ~ end */
if (*index < end) {
len = end - *index;
len = MIN(len, *space);
net_buf_add_mem(buf, &data[*index - start], len);
*space -= len;
*index += len;
return len;
}
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
* @param offset Pointer to the current offset in the attribute
* @param index Pointer to the current index for partial copying
* @param space Pointer to the remaining buffer space
* @param nest_level Current nesting level of the attribute
*
* @return Size of the last data element that has been searched
* (used in recursion)
*/
static uint32_t copy_attribute(const struct bt_sdp_data_elem *elem, struct net_buf *buf,
uint32_t *offset, uint32_t *index, uint16_t *space,
uint8_t nest_level)
{
const struct bt_sdp_data_elem *sub_elem;
uint32_t size, seq_size, total_size;
uint16_t copy_len = 0;
uint32_t total_len = 0;
NET_BUF_SIMPLE_DEFINE(attr_buf, 21); /* 1 + 4 + 16 bytes */
/* Limit recursion depth to avoid stack overflows */
if (nest_level == SDP_DATA_ELEM_NEST_LEVEL_MAX) {
return 0;
}
if (*space == 0) {
return 0;
}
seq_size = elem->data_size;
total_size = elem->total_size;
sub_elem = (const struct bt_sdp_data_elem *)elem->data;
__ASSERT(!((nest_level == 1) && (*index >= total_size)), "Invalid attr index %u >= %u",
*index, total_size);
/* Copy the header */
net_buf_simple_reset(&attr_buf);
net_buf_simple_add_u8(&attr_buf, elem->type);
switch (total_size - (seq_size + 1U)) {
case 1:
net_buf_simple_add_u8(&attr_buf, elem->data_size);
break;
case 2:
net_buf_simple_add_be16(&attr_buf, elem->data_size);
break;
case 4:
net_buf_simple_add_be32(&attr_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) {
copy_len = copy_attribute_from_buf(buf, attr_buf.data, attr_buf.len, offset,
index, space);
total_len += copy_len;
if (*space == 0) {
goto exit;
}
do {
size = copy_attribute(sub_elem, buf, offset, index, space, nest_level + 1);
total_len += size;
if (*space == 0) {
goto exit;
}
__ASSERT(seq_size >= sub_elem->total_size, "Invalid sequence size %u < %u",
seq_size, sub_elem->total_size);
seq_size -= sub_elem->total_size;
sub_elem++;
} 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_simple_add_u8(&attr_buf, *((const uint8_t *)elem->data));
} else if (seq_size == 2U) {
net_buf_simple_add_be16(&attr_buf, *((const uint16_t *)elem->data));
} else if (seq_size == 4U) {
net_buf_simple_add_be32(&attr_buf, *((const uint32_t *)elem->data));
} else if (seq_size == 8U) {
net_buf_simple_add_be64(&attr_buf, *((const uint64_t *)elem->data));
} else {
__ASSERT(seq_size == 0x10, "Invalid sequence size");
uint8_t val[seq_size];
sys_memcpy_swap(val, elem->data, sizeof(val));
net_buf_simple_add_mem(&attr_buf, val, seq_size);
}
copy_len = copy_attribute_from_buf(buf, attr_buf.data, attr_buf.len, offset,
index, space);
total_len += copy_len;
if (*space == 0) {
goto exit;
}
} else {
copy_len = copy_attribute_from_buf(buf, attr_buf.data, attr_buf.len, offset,
index, space);
total_len += copy_len;
if (*space == 0) {
goto exit;
}
copy_len = copy_attribute_from_buf(buf, (const uint8_t *)elem->data,
elem->data_size, offset, index, space);
total_len += copy_len;
if (*space == 0) {
goto exit;
}
}
exit:
if ((nest_level == 1) && (*index >= total_size)) {
/* Reset index to zero */
*index = 0;
}
return total_len;
}
/* @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 uint8_t bt_sdp_foreach_attr(struct bt_sdp_record *record, uint8_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 uint8_t select_attrs(struct bt_sdp_attribute *attr, uint8_t att_idx,
void *user_data)
{
struct select_attrs_data *sad = user_data;
uint16_t att_id_lower, att_id_upper, att_id_cur, space;
uint32_t attr_size = 0, seq_size;
uint32_t attr_data_index;
size_t idx_filter;
uint32_t offset = 0;
uint32_t copy_len;
uint32_t required_len;
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->last_att_index == 0) && (sad->state->last_att == 0) &&
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);
space = MIN(space, sad->max_att_len);
if ((!sad->state->pkt_full) && (space <= sad->cont_state_size)) {
/* Packet exhausted */
sad->state->pkt_full = true;
} else {
space -= sad->cont_state_size;
}
}
/* Keep filling data only if packet is not exhausted */
if (!sad->state->pkt_full && sad->rsp_buf) {
attr_data_index = sad->state->last_att_index;
required_len = 0;
/* Add Per-Service Data Element Seq declaration once
* only when we are starting from the first attribute
*/
if (!sad->seq &&
(sad->state->last_att_index == 0) && (sad->state->last_att == 0)) {
required_len += sizeof(*sad->seq);
}
if (attr_data_index == 0) {
required_len += sizeof(uint8_t) + sizeof(uint16_t);
}
if (space <= required_len) {
/* Packet exhausted */
sad->state->pkt_full = true;
goto out;
}
/* Add Per-Service Data Element Seq declaration once
* only when we are starting from the first attribute
*/
if (!sad->seq &&
(sad->state->last_att_index == 0) && (sad->state->last_att == 0)) {
sad->seq = net_buf_add(sad->rsp_buf,
sizeof(*sad->seq));
sad->seq->type = BT_SDP_SEQ16;
sad->seq->size = 0U;
space -= sizeof(*sad->seq);
sad->max_att_len -= sizeof(*sad->seq);
sad->att_list_len += sizeof(*sad->seq);
}
if (attr_data_index == 0) {
/* Add attribute ID */
net_buf_add_u8(sad->rsp_buf, BT_SDP_UINT16);
net_buf_add_be16(sad->rsp_buf, att_id_cur);
space -= sizeof(uint8_t) + sizeof(uint16_t);
sad->max_att_len -= sizeof(uint8_t) + sizeof(uint16_t);
sad->att_list_len += sizeof(uint8_t) + sizeof(uint16_t);
}
/* Add attribute value */
copy_len = copy_attribute(&attr->val, sad->rsp_buf, &offset,
&attr_data_index, &space, 1);
sad->max_att_len -= copy_len;
sad->att_list_len += copy_len;
sad->state->last_att = att_idx;
sad->state->last_att_index = attr_data_index;
sad->state->current_svc = sad->rec->index;
if (attr_data_index == 0) {
/* It means the all data of attribute is copied.
* The att index needs to be incremented.
*/
sad->state->last_att++;
}
if (space == 0) {
/* Packet exhausted */
sad->state->pkt_full = true;
goto out;
}
}
out:
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;
}
/* Calculate the total sequence size */
sad->state->att_list_size += seq_size;
sad->new_service = false;
break;
}
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 state State of the overall search
* @param rsp_buf Response buffer which is filled in
*
* @return len Length of the attribute list created
*/
static uint16_t create_attr_list(struct bt_sdp *sdp, struct bt_sdp_record *record,
uint32_t *filter, size_t num_filters, uint16_t max_att_len,
uint8_t cont_state_size, struct search_state *state,
struct net_buf *rsp_buf)
{
struct select_attrs_data sad;
__maybe_unused uint8_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, state->last_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 max_filters Max element slots of filter to be filled in
* @param num_filters No. of filter elements filled in (to be returned)
*
* @return 0 for success, or relevant error code
*/
static uint16_t get_att_search_list(struct net_buf *buf, uint32_t *filter, size_t max_filters,
size_t *num_filters)
{
struct bt_sdp_data_elem data_elem;
uint16_t res;
uint32_t size;
*num_filters = 0U;
res = parse_data_elem(&buf->b, &data_elem);
if (res) {
return res;
}
size = data_elem.data_size;
while (size) {
if (*num_filters >= max_filters) {
LOG_WRN("Exceeded maximum array length %zu of %p", max_filters, filter);
return 0;
}
res = parse_data_elem(&buf->b, &data_elem);
if (res) {
return res;
}
if ((data_elem.type & BT_SDP_TYPE_DESC_MASK) != BT_SDP_UINT8) {
LOG_WRN("Invalid type %u in attribute ID list", data_elem.type);
return BT_SDP_INVALID_SYNTAX;
}
if (buf->len < data_elem.data_size) {
LOG_WRN("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 uint8_t find_handle(struct bt_sdp_record *rec, void *user_data)
{
uint32_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 uint16_t sdp_svc_att_req(struct bt_sdp *sdp, struct net_buf *buf, uint16_t tid)
{
uint32_t filter[MAX_NUM_ATT_ID_FILTER];
struct search_state state = {
.current_svc = SDP_INVALID,
.last_att = 0,
.last_att_index = 0,
.pkt_full = false
};
struct bt_sdp_record *record;
struct bt_sdp_att_rsp *rsp;
struct net_buf *rsp_buf;
uint32_t svc_rec_hdl;
uint16_t max_att_len, res, att_list_len;
size_t num_filters;
uint8_t cont_state_size;
if (buf->len < 6) {
LOG_WRN("Malformed packet");
return BT_SDP_INVALID_SYNTAX;
}
svc_rec_hdl = net_buf_pull_be32(buf);
max_att_len = net_buf_pull_be16(buf);
if (!SDP_SA_ATTR_BYTE_IN_RANGE(max_att_len)) {
LOG_WRN("Invalid max attribute length %u", max_att_len);
return BT_SDP_INVALID_SYNTAX;
}
/* Set up the filters */
res = get_att_search_list(buf, filter, ARRAY_SIZE(filter), &num_filters);
if (res) {
/* Error in parsing */
return res;
}
if (buf->len < 1) {
LOG_WRN("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) {
LOG_WRN("Invalid cont state size %u", cont_state_size);
return BT_SDP_INVALID_CSTATE;
}
if (buf->len < cont_state_size) {
LOG_WRN("Malformed packet");
return BT_SDP_INVALID_SYNTAX;
}
state.last_att = net_buf_pull_u8(buf);
state.last_att_index = net_buf_pull_be32(buf);
}
LOG_DBG("svc_rec_hdl %u, max_att_len 0x%04x, cont_state %u %u", svc_rec_hdl, max_att_len,
state.last_att, state.last_att_index);
/* Find the service */
record = bt_sdp_foreach_svc(find_handle, &svc_rec_hdl);
if (!record) {
LOG_WRN("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, &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;
}
if (state.current_svc != record->index) {
/* It is a corner case that the remaining free space of the responding is emtpy,
* and all attributes are sent, clear state.pkt_full to avoid further processing.
*/
state.pkt_full = false;
}
/* Add continuation state */
if (state.pkt_full) {
LOG_DBG("Packet full, state.last_att %u", state.last_att);
net_buf_add_u8(rsp_buf, SDP_SA_CONT_STATE_SIZE);
net_buf_add_u8(rsp_buf, state.last_att);
net_buf_add_be32(rsp_buf, state.last_att_index);
} else {
net_buf_add_u8(rsp_buf, 0);
}
rsp->att_list_len = sys_cpu_to_be16(att_list_len);
LOG_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 uint16_t sdp_svc_search_att_req(struct bt_sdp *sdp, struct net_buf *buf, uint16_t tid)
{
uint32_t filter[MAX_NUM_ATT_ID_FILTER];
struct search_state state = {
.att_list_size = 0,
.current_svc = SDP_INVALID,
.last_att = 0,
.last_att_index = 0,
.pkt_full = false
};
struct net_buf *rsp_buf, *rsp_buf_cpy;
struct bt_sdp_record *record, *next;
struct bt_sdp_att_rsp *rsp;
struct bt_sdp_data_elem_seq *seq = NULL;
uint16_t max_att_len, res, att_list_len = 0U;
uint16_t sending_len;
size_t num_filters;
uint8_t cont_state_size, next_svc = 0U;
bool dry_run = false;
struct net_buf_simple ssp;
res = parse_service_search_pattern(buf, &ssp);
if (res) {
/* Error in parsing */
return res;
}
if (buf->len < sizeof(max_att_len)) {
LOG_WRN("Malformed packet - Maximum Attribute Byte Count");
return BT_SDP_INVALID_SYNTAX;
}
max_att_len = net_buf_pull_be16(buf);
if (!SDP_SSA_ATTR_BYTE_IN_RANGE(max_att_len)) {
LOG_WRN("Invalid max attribute length %u", max_att_len);
return BT_SDP_INVALID_SYNTAX;
}
if (max_att_len < sizeof(*seq)) {
LOG_WRN("Invalid maximum attribute byte count %u < %u", max_att_len, sizeof(*seq));
return BT_SDP_INVALID_SYNTAX;
}
/* Set up the filters */
res = get_att_search_list(buf, filter, ARRAY_SIZE(filter), &num_filters);
if (res) {
/* Error in parsing */
return res;
}
if (buf->len < sizeof(cont_state_size)) {
LOG_WRN("Malformed packet - Continuation State");
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) {
LOG_WRN("Invalid cont state size %u", cont_state_size);
return BT_SDP_INVALID_CSTATE;
}
if (buf->len < cont_state_size) {
LOG_WRN("Malformed packet - Continuation State Size");
return BT_SDP_INVALID_SYNTAX;
}
state.current_svc = net_buf_pull_u8(buf);
state.last_att = net_buf_pull_u8(buf);
state.last_att_index = net_buf_pull_be32(buf);
next_svc = state.current_svc;
}
LOG_DBG("max_att_len 0x%04x, cont_state %u %u %u", max_att_len, next_svc,
state.last_att, state.last_att_index);
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 = sizeof(*seq);
max_att_len -= sizeof(*seq);
}
rsp_buf_cpy = rsp_buf;
SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&sdp_db, record, next, node) {
if (!service_search_pattern_matched(&ssp, record)) {
continue;
}
if (record->index < next_svc) {
continue;
}
/* reset the `state.last_att` and `state.last_att_index`
* if the index of current record is not same with `state.current_svc`.
*/
if (state.current_svc != record->index) {
state.current_svc = record->index;
state.last_att = 0;
state.last_att_index = 0;
}
sending_len = create_attr_list(sdp, record, filter, num_filters, max_att_len,
SDP_SSA_CONT_STATE_SIZE + 1, &state, rsp_buf_cpy);
att_list_len += sending_len;
if (max_att_len < sending_len) {
LOG_ERR("Att len exceeds %u < %u", max_att_len, sending_len);
net_buf_unref(rsp_buf);
return BT_SDP_INVALID_SYNTAX;
}
max_att_len -= sending_len;
/* Check if packet is full and not dry run */
if (state.pkt_full && !dry_run) {
LOG_DBG("Packet full, state.last_att %u", state.last_att);
if (state.current_svc < num_services) {
dry_run = true;
/* Add continuation state */
net_buf_add_u8(rsp_buf, SDP_SSA_CONT_STATE_SIZE);
net_buf_add_u8(rsp_buf, state.current_svc);
net_buf_add_u8(rsp_buf, state.last_att);
net_buf_add_be32(rsp_buf, state.last_att_index);
}
/* 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;
}
if (dry_run) {
/* Reset the state.last_att and state.last_att_index to calculate all
* sequence size
*/
state.last_att = 0;
state.last_att_index = 0;
}
}
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);
}
LOG_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 {
uint8_t op_code;
uint16_t (*func)(struct bt_sdp *sdp, struct net_buf *buf, uint16_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;
uint16_t err = BT_SDP_INVALID_SYNTAX;
size_t i;
LOG_DBG("chan %p, ch %p, cid 0x%04x", chan, ch, ch->tx.cid);
BT_ASSERT(sdp);
if (buf->len < sizeof(*hdr)) {
LOG_ERR("Too small SDP PDU received");
return 0;
}
hdr = net_buf_pull_mem(buf, sizeof(*hdr));
LOG_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, sys_be16_to_cpu(hdr->tid));
break;
}
}
if (err) {
LOG_WRN("SDP error 0x%02x", err);
send_err_rsp(chan, err, sys_be16_to_cpu(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_server *server,
struct bt_l2cap_chan **chan)
{
static const struct bt_l2cap_chan_ops ops = {
.connected = bt_sdp_connected,
.disconnected = bt_sdp_disconnected,
.recv = bt_sdp_recv,
};
int i;
LOG_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;
}
LOG_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_L0,
};
int res;
res = bt_l2cap_br_server_register(&server);
if (res) {
LOG_ERR("L2CAP server registration failed with error %d", res);
}
ARRAY_FOR_EACH(bt_sdp_client_pool, i) {
/* Locking semaphore initialized to 1 (unlocked) */
k_sem_init(&bt_sdp_client_pool[i].sem_lock, 1, 1);
}
}
int bt_sdp_register_service(struct bt_sdp_record *service)
{
uint8_t index = 0;
if (!service) {
LOG_ERR("No service record specified");
return 0;
}
if (sys_slist_find(&sdp_db, &service->node, NULL)) {
LOG_ERR("Service already registered");
return -EEXIST;
}
if (!sys_slist_is_empty(&sdp_db)) {
struct bt_sdp_record *last;
last = CONTAINER_OF(sys_slist_peek_tail(&sdp_db), struct bt_sdp_record, node);
index = last->index + 1;
if (last->index > index) {
LOG_ERR("Registered record is full");
return -EOVERFLOW;
}
}
service->index = index;
service->handle = SDP_SERVICE_HANDLE_BASE + index;
*((uint32_t *)(service->attrs[0].val.data)) = service->handle;
sys_slist_append(&sdp_db, &service->node);
num_services++;
LOG_DBG("Service registered at %u", service->handle);
return 0;
}
static int sdp_client_discover(struct bt_sdp_client *session);
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;
}
LOG_DBG("");
/* Remove already checked UUID node */
sys_slist_remove(&session->reqs, NULL, &param->_node);
/* Invalidate cached param in context */
session->param = NULL;
if (session->rec_buf != NULL) {
net_buf_unref(session->rec_buf);
session->rec_buf = NULL;
}
/* Reset continuation state in current context */
(void)memset(&session->cstate, 0, sizeof(session->cstate));
/* Clear total length */
session->total_len = 0;
/* Clear received length */
session->recv_len = 0;
k_sem_take(&session->sem_lock, K_FOREVER);
/* Check if there's valid next UUID */
if (!sys_slist_is_empty(&session->reqs)) {
k_sem_give(&session->sem_lock);
sdp_client_discover(session);
return;
}
/* No UUID items, disconnect channel */
session->state = SDP_CLIENT_DISCONNECTING;
k_sem_give(&session->sem_lock);
bt_l2cap_chan_disconnect(chan);
break;
}
}
static uint16_t sdp_client_get_total(struct bt_sdp_client *session,
struct net_buf *buf, uint16_t *total)
{
uint16_t pulled;
uint8_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;
case BT_SDP_SEQ32:
*total = net_buf_pull_be32(buf);
pulled += 4U;
break;
default:
LOG_WRN("Sequence type 0x%02x not handled", seq);
*total = 0U;
break;
}
LOG_DBG("Total %u octets of all attributes", *total);
} else {
pulled = 0U;
*total = 0U;
}
return pulled;
}
static uint16_t get_ss_record_len(struct net_buf *buf)
{
if (buf->len >= SDP_RECORD_HANDLE_SIZE) {
return SDP_RECORD_HANDLE_SIZE;
}
LOG_WRN("Invalid service record handle length");
return 0;
}
static uint16_t get_ssa_record_len(struct net_buf *buf)
{
uint16_t len;
uint8_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;
case BT_SDP_SEQ32:
len = net_buf_pull_be32(buf);
break;
default:
LOG_WRN("Sequence type 0x%02x not handled", seq);
len = 0U;
break;
}
return len;
}
static uint16_t get_record_len(struct bt_sdp_client *session)
{
struct net_buf *buf;
uint16_t len;
buf = session->rec_buf;
if (!session->param) {
return buf->len;
}
switch (session->param->type) {
case BT_SDP_DISCOVER_SERVICE_SEARCH:
len = get_ss_record_len(buf);
break;
case BT_SDP_DISCOVER_SERVICE_SEARCH_ATTR:
len = get_ssa_record_len(buf);
break;
case BT_SDP_DISCOVER_SERVICE_ATTR:
default:
len = buf->len;
break;
}
LOG_DBG("Record len %u", len);
return len;
}
enum uuid_state {
UUID_NOT_RESOLVED,
UUID_RESOLVED,
UUID_PARTIAL_RESOLVED,
};
static int 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;
uint16_t rec_len;
uint8_t user_ret;
if ((state == UUID_NOT_RESOLVED) || (session->rec_buf->len == 0U)) {
result.resp_buf = NULL;
result.next_record_hint = false;
session->param->func(conn, &result, session->param);
return 0;
}
while (session->rec_buf->len) {
struct net_buf_simple_state buf_state;
net_buf_simple_save(&session->rec_buf->b, &buf_state);
rec_len = get_record_len(session);
/* tell the user about multi record resolution */
if (session->rec_buf->len > rec_len) {
result.next_record_hint = true;
} else {
if (state == UUID_PARTIAL_RESOLVED) {
struct net_buf *buf;
uint8_t *src, *dst;
uint16_t len;
net_buf_simple_restore(&session->rec_buf->b, &buf_state);
/* Partial resolution, continue processing */
src = session->rec_buf->data;
len = session->rec_buf->len;
/* The allocated buffer is full. Try to allocate a new buffer from
* the same pool. Use allocated buffer to continue the SDP
* discovery if the new buffer allocated. Otherwise, use the
* current allocated to continue the SDP discovery.
*/
buf = net_buf_alloc(session->param->pool, K_NO_WAIT);
if (buf != NULL) {
if (net_buf_tailroom(buf) < len) {
goto no_more_space;
}
net_buf_add_mem(buf, src, len);
if (net_buf_tailroom(buf) <=
net_buf_tailroom(session->rec_buf)) {
goto no_more_space;
}
net_buf_unref(session->rec_buf);
session->rec_buf = buf;
LOG_DBG("Continue discovery with new buf %p", buf);
return 0;
no_more_space:
LOG_ERR("Allocated buffer has not more space for the next "
"SDP discover. Need to increase date size of the "
"receiving pool.");
net_buf_unref(buf);
return -ENOMEM;
}
net_buf_reset(session->rec_buf);
dst = net_buf_add(session->rec_buf, len);
if (dst == src) {
LOG_ERR("No more buffer space for SDP discover. Need to "
"increase buffer size of the receiving pool.");
return -ENOMEM;
}
memmove(dst, src, len);
LOG_DBG("Continue discovery with current buf");
return 0;
}
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 instead of whole session rec_buf length.
*/
result.resp_buf->len = rec_len;
user_ret = session->param->func(conn, &result, session->param);
/* 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) {
return -ECANCELED;
}
}
return 0;
}
static int sdp_client_ssa_sa_notify(struct bt_sdp_client *session)
{
return sdp_client_notify_result(session, UUID_PARTIAL_RESOLVED);
}
#define GET_PARAM(__node) \
CONTAINER_OF(__node, struct bt_sdp_discover_params, _node)
/* ServiceSearch PDU, ref to BT Core 5.4, Vol 3, part B, 4.5.1 */
static int sdp_client_ss_search(struct bt_sdp_client *session,
const struct bt_sdp_discover_params *param)
{
struct net_buf *buf;
uint8_t uuid128[BT_UUID_SIZE_128];
/* Update context param directly. */
session->param = param;
buf = bt_sdp_create_pdu();
/* 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, sizeof(uint8_t) + BT_UUID_SIZE_16);
/* 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, sizeof(uint8_t) + BT_UUID_SIZE_32);
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, sizeof(uint8_t) + BT_UUID_SIZE_128);
net_buf_add_u8(buf, BT_SDP_UUID128);
sys_memcpy_swap(uuid128, BT_UUID_128(param->uuid)->val, sizeof(uuid128));
net_buf_add_mem(buf, uuid128, sizeof(uuid128));
break;
default:
LOG_ERR("Unknown UUID type %u", param->uuid->type);
net_buf_unref(buf);
return -EINVAL;
}
/* Set maximum number of service record handles */
net_buf_add_be16(buf, net_buf_tailroom(session->rec_buf) / SDP_RECORD_HANDLE_SIZE);
/*
* 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);
}
session->tid++;
return bt_sdp_send(&session->chan.chan, buf, BT_SDP_SVC_SEARCH_REQ, session->tid);
}
static uint16_t sdp_client_get_attribute_id_list_len(struct bt_sdp_attribute_id_list *ids)
{
uint16_t len = 0;
if (ids == NULL || ids->count == 0) {
return sizeof(uint8_t) + sizeof(uint32_t);
}
for (size_t i = 0; i < ids->count; i++) {
if (ids->ranges[i].beginning == ids->ranges[i].ending) {
len += sizeof(uint8_t) + sizeof(uint16_t);
} else {
len += sizeof(uint8_t) + sizeof(uint32_t);
}
}
return len;
}
static void sdp_client_add_attribute_id(struct net_buf *buf, struct bt_sdp_attribute_id_list *ids)
{
uint16_t len;
len = sdp_client_get_attribute_id_list_len(ids);
/*
* Sequence definition where data is sequence of elements and where
* additional next byte points the size of elements within
*/
if (len > UINT8_MAX) {
net_buf_add_u8(buf, BT_SDP_SEQ16);
net_buf_add_be16(buf, len);
} else {
net_buf_add_u8(buf, BT_SDP_SEQ8);
net_buf_add_u8(buf, len);
}
if (ids == NULL || ids->count == 0) {
/* 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);
return;
}
for (size_t i = 0; i < ids->count; i++) {
if (ids->ranges[i].beginning == ids->ranges[i].ending) {
/* Data element definition for one following 16bits range elements */
net_buf_add_u8(buf, BT_SDP_UINT16);
/* Get all attributes. It enables filter out wanted only attributes */
net_buf_add_be16(buf, ids->ranges[i].beginning);
} else {
/* 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, ids->ranges[i].beginning);
net_buf_add_be16(buf, ids->ranges[i].ending);
}
}
}
static uint16_t sdp_client_get_total_len(struct bt_sdp_client *session,
const struct bt_sdp_discover_params *param)
{
uint16_t len;
len = sdp_client_get_attribute_id_list_len(param->ids);
if (len > UINT8_MAX) {
len += sizeof(uint8_t) + sizeof(uint16_t);
} else {
len += sizeof(uint8_t) + sizeof(uint8_t);
}
len += sizeof(session->cstate.length) + session->cstate.length;
return len;
}
/* ServiceAttribute PDU, ref to BT Core 5.4, Vol 3, part B, 4.6.1 */
static int sdp_client_sa_search(struct bt_sdp_client *session,
const struct bt_sdp_discover_params *param)
{
struct net_buf *buf;
uint16_t len;
/* Update context param directly. */
session->param = param;
len = net_buf_tailroom(session->rec_buf);
if (!SDP_SA_ATTR_BYTE_IN_RANGE(len)) {
LOG_WRN("No more space to start next SDP discovery");
return -ENOMEM;
}
buf = bt_sdp_create_pdu();
/* Add service record handle */
net_buf_add_be32(buf, param->handle);
/* Set attribute max bytes count to be returned from server */
net_buf_add_be16(buf, len);
/* Check the tailroom of the buffer */
len = sdp_client_get_total_len(session, param);
if (len > net_buf_tailroom(buf)) {
LOG_ERR("No space to add attribute ID");
net_buf_unref(buf);
return -ENOMEM;
}
/* Add attribute ID List */
sdp_client_add_attribute_id(buf, param->ids);
/*
* 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);
}
session->tid++;
return bt_sdp_send(&session->chan.chan, buf, BT_SDP_SVC_ATTR_REQ, session->tid);
}
/* 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 net_buf *buf;
uint8_t uuid128[BT_UUID_SIZE_128];
uint16_t len;
/* Update context param directly. */
session->param = param;
len = net_buf_tailroom(session->rec_buf);
if (!SDP_SSA_ATTR_BYTE_IN_RANGE(len)) {
int err;
LOG_WRN("No more space to start next SDP discovery");
if (session->rec_buf->len == 0) {
return -ENOMEM;
}
/* Notify current received data */
err = sdp_client_ssa_sa_notify(session);
if (err != 0) {
LOG_ERR("Failed to notify received data: %d", err);
return err;
}
len = net_buf_tailroom(session->rec_buf);
if (!SDP_SSA_ATTR_BYTE_IN_RANGE(len)) {
LOG_WRN("No more space of buffer for SDP discovery");
return -ENOMEM;
}
}
buf = bt_sdp_create_pdu();
/* 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, sizeof(uint8_t) + BT_UUID_SIZE_16);
/* 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, sizeof(uint8_t) + BT_UUID_SIZE_32);
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, sizeof(uint8_t) + BT_UUID_SIZE_128);
net_buf_add_u8(buf, BT_SDP_UUID128);
sys_memcpy_swap(uuid128, BT_UUID_128(param->uuid)->val, sizeof(uuid128));
net_buf_add_mem(buf, uuid128, sizeof(uuid128));
break;
default:
LOG_ERR("Unknown UUID type %u", param->uuid->type);
net_buf_unref(buf);
return -EINVAL;
}
/* Set attribute max bytes count to be returned from server */
net_buf_add_be16(buf, len);
/* Check the tailroom of the buffer */
len = sdp_client_get_total_len(session, param);
if (len > net_buf_tailroom(buf)) {
LOG_ERR("No space to add attribute ID");
net_buf_unref(buf);
return -ENOMEM;
}
/* Add attribute ID List */
sdp_client_add_attribute_id(buf, param->ids);
/*
* 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);
}
session->tid++;
return bt_sdp_send(&session->chan.chan, buf, BT_SDP_SVC_SEARCH_ATTR_REQ,
session->tid);
}
static int sdp_client_discover(struct bt_sdp_client *session)
{
const struct bt_sdp_discover_params *param;
int err;
/*
* 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.
*/
k_sem_take(&session->sem_lock, K_FOREVER);
if (!session->param) {
param = GET_PARAM(sys_slist_peek_head(&session->reqs));
} else {
param = session->param;
}
if (param != NULL && session->rec_buf == NULL) {
session->rec_buf = net_buf_alloc(param->pool, K_FOREVER);
}
if (param == NULL || session->rec_buf == NULL) {
struct bt_l2cap_chan *chan = &session->chan.chan;
session->state = SDP_CLIENT_DISCONNECTING;
k_sem_give(&session->sem_lock);
LOG_WRN("No more request, disconnect channel");
/* No UUID items, disconnect channel */
return bt_l2cap_chan_disconnect(chan);
}
k_sem_give(&session->sem_lock);
switch (param->type) {
case BT_SDP_DISCOVER_SERVICE_SEARCH:
err = sdp_client_ss_search(session, param);
break;
case BT_SDP_DISCOVER_SERVICE_ATTR:
err = sdp_client_sa_search(session, param);
break;
case BT_SDP_DISCOVER_SERVICE_SEARCH_ATTR:
err = sdp_client_ssa_search(session, param);
break;
default:
err = -EINVAL;
break;
}
if (err) {
/* Notify the result */
sdp_client_notify_result(session, UUID_NOT_RESOLVED);
/* Get next UUID and start resolving it */
sdp_client_params_iterator(session);
}
return 0;
}
static int sdp_client_receive_ss(struct bt_sdp_client *session, struct net_buf *buf)
{
struct bt_sdp_pdu_cstate *cstate;
uint16_t current_count;
uint16_t total_count;
uint32_t record_len;
uint32_t received_count;
/* Check the buffer len for the total_count field */
if (buf->len < sizeof(total_count)) {
LOG_ERR("Invalid frame payload length");
return -EINVAL;
}
/* Get total service record count. */
total_count = net_buf_pull_be16(buf);
/* Check the buffer len for the current_count field */
if (buf->len < sizeof(current_count)) {
LOG_ERR("Invalid frame payload length");
return -EINVAL;
}
/* Get current service record count. */
current_count = net_buf_pull_be16(buf);
/* Check valid of current service record count */
if (current_count > total_count) {
LOG_ERR("Invalid current service record count");
return -EINVAL;
}
received_count = session->rec_buf->len / SDP_RECORD_HANDLE_SIZE;
if ((received_count + current_count) > total_count) {
LOG_ERR("Excess data received");
return -EINVAL;
}
record_len = current_count * SDP_RECORD_HANDLE_SIZE;
if (record_len >= buf->len) {
LOG_ERR("Invalid packet");
return -EINVAL;
}
/* Get PDU continuation state */
cstate = (struct bt_sdp_pdu_cstate *)(buf->data + record_len);
if (cstate->length > BT_SDP_MAX_PDU_CSTATE_LEN) {
LOG_ERR("Invalid SDP PDU Continuation State length %u", cstate->length);
return -EINVAL;
}
if ((record_len + SDP_CONT_STATE_LEN_SIZE + cstate->length) > buf->len) {
LOG_ERR("Invalid payload length");
return -EINVAL;
}
/*
* 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 (!current_count && cstate->length == 0U && session->cstate.length == 0U) {
LOG_WRN("Service record handle 0x%x not found", session->param->handle);
return -EINVAL;
}
if (record_len > net_buf_tailroom(session->rec_buf)) {
LOG_WRN("Not enough room for getting records data");
return -EINVAL;
}
net_buf_add_mem(session->rec_buf, buf->data, record_len);
net_buf_pull(buf, record_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.
* All failure case are handled internally in the function.
* Ignore the return value.
*/
(void)sdp_client_discover(session);
return 0;
}
net_buf_pull(buf, sizeof(cstate->length));
LOG_DBG("UUID 0x%s resolved", bt_uuid_str(session->param->uuid));
sdp_client_notify_result(session, UUID_RESOLVED);
/* Get next UUID and start resolving it */
sdp_client_params_iterator(session);
return 0;
}
static int sdp_client_receive_ssa_sa(struct bt_sdp_client *session, struct net_buf *buf)
{
struct bt_sdp_pdu_cstate *cstate;
uint16_t frame_len;
uint16_t total;
/* Check the buffer len for the frame_len field */
if (buf->len < sizeof(frame_len)) {
LOG_ERR("Invalid frame payload length");
return -EINVAL;
}
/* 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) {
LOG_ERR("Invalid frame payload length");
return -EINVAL;
}
/* Check valid range of attributes length */
if ((session->cstate.length == 0) && (frame_len < 2)) {
LOG_ERR("Invalid attributes data length");
return -EINVAL;
}
/* Get PDU continuation state */
cstate = (struct bt_sdp_pdu_cstate *)(buf->data + frame_len);
if (cstate->length > BT_SDP_MAX_PDU_CSTATE_LEN) {
LOG_ERR("Invalid SDP PDU Continuation State length %u", cstate->length);
return -EINVAL;
}
if ((frame_len + SDP_CONT_STATE_LEN_SIZE + cstate->length) > buf->len) {
LOG_ERR("Invalid frame payload length");
return -EINVAL;
}
/* No more data found for given UUID and Continuation State length is not zero.
* It means the remaining tailroom of the RX buffer is not enough to store the data.
* Try to notify the received data, and request the next portion of data by sending a
* continuation request.
*/
if (frame_len == 0 && cstate->length != 0) {
/* Notify current received data */
int err;
err = sdp_client_ssa_sa_notify(session);
if (err != 0) {
LOG_ERR("Failed to notify received data: %d", err);
return err;
}
}
/*
* 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) {
LOG_WRN("Record for UUID 0x%s not found", bt_uuid_str(session->param->uuid));
return -EINVAL;
}
/* Get total value of all attributes to be collected */
frame_len -= sdp_client_get_total(session, buf, &total);
/*
* If total is not 0, there are two valid cases,
* Case 1, the continuation state length is 0, the frame_len should equal total,
* Case 2, the continuation state length is not 0, it means there are more data will be
* received. So the frame_len is less than total.
*/
if (total && (frame_len > total)) {
LOG_ERR("Invalid attribute lists");
return -EINVAL;
}
if (session->cstate.length == 0U) {
session->total_len = total;
}
session->recv_len += frame_len;
if (frame_len > net_buf_tailroom(session->rec_buf)) {
LOG_WRN("Not enough room for getting records data");
return -EINVAL;
}
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.
* All failure case are handled internally in the function.
* Ignore the return value.
*/
(void)sdp_client_discover(session);
return 0;
}
if (session->total_len && (session->recv_len != session->total_len)) {
LOG_WRN("The received len %d is mismatched with total len %d", session->recv_len,
session->total_len);
return -EINVAL;
}
net_buf_pull(buf, sizeof(cstate->length));
LOG_DBG("UUID 0x%s resolved", bt_uuid_str(session->param->uuid));
sdp_client_notify_result(session, UUID_RESOLVED);
/* Get next UUID and start resolving it */
sdp_client_params_iterator(session);
return 0;
}
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;
uint16_t len, tid;
int err = -EINVAL;
LOG_DBG("session %p buf %p", session, buf);
if (buf->len < sizeof(*hdr)) {
LOG_ERR("Too small SDP PDU");
return 0;
}
hdr = net_buf_pull_mem(buf, sizeof(*hdr));
len = sys_be16_to_cpu(hdr->param_len);
tid = sys_be16_to_cpu(hdr->tid);
LOG_DBG("SDP PDU tid %u len %u", tid, len);
if (buf->len != len) {
LOG_ERR("SDP PDU length mismatch (%u != %u)", buf->len, len);
return 0;
}
if (tid != session->tid) {
LOG_ERR("Mismatch transaction ID value in SDP PDU");
return 0;
}
if (session->param == NULL) {
LOG_WRN("No request in progress");
return 0;
}
switch (hdr->op_code) {
case BT_SDP_SVC_SEARCH_RSP:
err = sdp_client_receive_ss(session, buf);
break;
case BT_SDP_SVC_ATTR_RSP:
case BT_SDP_SVC_SEARCH_ATTR_RSP:
err = sdp_client_receive_ssa_sa(session, buf);
break;
case BT_SDP_ERROR_RSP:
LOG_INF("Invalid SDP request");
break;
default:
LOG_DBG("PDU 0x%0x response not handled", hdr->op_code);
break;
}
if (err < 0) {
sdp_client_notify_result(session, UUID_NOT_RESOLVED);
sdp_client_params_iterator(session);
}
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;
LOG_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);
LOG_DBG("session %p chan %p connected", session, chan);
k_sem_take(&session->sem_lock, K_FOREVER);
session->rec_buf = chan->ops->alloc_buf(chan);
if (!session->rec_buf) {
session->state = SDP_CLIENT_DISCONNECTING;
k_sem_give(&session->sem_lock);
bt_l2cap_chan_disconnect(chan);
return;
}
k_sem_give(&session->sem_lock);
sdp_client_discover(session);
}
static void sdp_client_clean_after_disconnect(struct bt_sdp_client *session)
{
/*
* keep the follow fields:
* sem_lock - it is always valid to protect the session, never clean it after bt_sdp_init.
* state - the session's state.
* chan - it is still used before released callback.
* reqs_next - the pending reqs in the disconnecting phase.
*/
sys_slist_init(&session->reqs);
session->tid = 0U;
session->param = NULL;
memset(&session->cstate, 0, sizeof(session->cstate));
if (session->rec_buf) {
net_buf_unref(session->rec_buf);
session->rec_buf = NULL;
}
session->total_len = 0U;
session->recv_len = 0U;
}
static void sdp_client_clean_after_release(struct bt_sdp_client *session)
{
/*
* keep the follow fields:
* sem_lock - it is always valid to protect the session, never clean it after bt_sdp_init.
* chan - it is maintained by l2cap layer.
*/
session->state = SDP_CLIENT_RELEASED;
sys_slist_init(&session->reqs_next);
sdp_client_clean_after_disconnect(session);
}
static void sdp_client_disconnected(struct bt_l2cap_chan *chan)
{
struct bt_sdp_discover_params *param, *tmp;
struct bt_sdp_client *session = SDP_CLIENT_CHAN(chan);
LOG_DBG("session %p chan %p disconnected", session, chan);
/* The disconnecting may be triggered by acl disconnection or failed sdp connecting */
k_sem_take(&session->sem_lock, K_FOREVER);
session->state = SDP_CLIENT_DISCONNECTING;
k_sem_give(&session->sem_lock);
/* callback all the sdp reqs */
SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&session->reqs, param, tmp, _node) {
session->param = param;
sdp_client_notify_result(session, UUID_NOT_RESOLVED);
/* Remove already callbacked UUID node */
sys_slist_find_and_remove(&session->reqs, &param->_node);
}
if (session->rec_buf) {
net_buf_unref(session->rec_buf);
session->rec_buf = NULL;
}
sdp_client_clean_after_disconnect(session);
}
void sdp_client_released(struct bt_l2cap_chan *chan)
{
struct bt_sdp_client *session = SDP_CLIENT_CHAN(chan);
struct bt_sdp_discover_params *param, *tmp;
struct bt_conn *conn;
sys_slist_t cb_reqs;
int err;
k_sem_take(&session->sem_lock, K_FOREVER);
if (!sys_slist_is_empty(&session->reqs_next)) {
/* put the reqs_next to reqs */
SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&session->reqs_next, param, tmp, _node) {
sys_slist_append(&session->reqs, &param->_node);
/* Remove already proccessed node */
sys_slist_remove(&session->reqs_next, NULL, &param->_node);
}
conn = bt_conn_lookup_index(ARRAY_INDEX(bt_sdp_client_pool, session));
err = sdp_client_new_session(conn, session);
if (err) {
sys_slist_init(&cb_reqs);
SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&session->reqs, param, tmp, _node) {
sys_slist_append(&cb_reqs, &param->_node);
}
sdp_client_clean_after_release(session);
}
k_sem_give(&session->sem_lock);
if (err) {
struct bt_sdp_client_result result;
result.resp_buf = NULL;
result.next_record_hint = false;
SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&cb_reqs, param, tmp, _node) {
param->func(conn, &result, param);
}
}
bt_conn_unref(conn);
} else {
sdp_client_clean_after_release(session);
k_sem_give(&session->sem_lock);
}
}
static const struct bt_l2cap_chan_ops sdp_client_chan_ops = {
.connected = sdp_client_connected,
.disconnected = sdp_client_disconnected,
.released = sdp_client_released,
.recv = sdp_client_receive,
.alloc_buf = sdp_client_alloc_buf,
};
static int sdp_client_new_session(struct bt_conn *conn, struct bt_sdp_client *session)
{
int err;
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) {
LOG_ERR("Cannot connect %d", err);
return err;
}
session->state = SDP_CLIENT_CONNECTING;
return err;
}
static int sdp_client_discovery_start(struct bt_conn *conn,
struct bt_sdp_discover_params *params)
{
int err;
struct bt_sdp_client *session;
session = &bt_sdp_client_pool[bt_conn_index(conn)];
k_sem_take(&session->sem_lock, K_FOREVER);
if (session->state == SDP_CLIENT_CONNECTING ||
session->state == SDP_CLIENT_CONNECTED) {
sys_slist_append(&session->reqs, &params->_node);
k_sem_give(&session->sem_lock);
return 0;
}
/* put in `reqs_next` for next round after disconnected */
if (session->state == SDP_CLIENT_DISCONNECTING) {
sys_slist_append(&session->reqs_next, &params->_node);
k_sem_give(&session->sem_lock);
return 0;
}
/*
* Try to allocate session context since not found in pool and attempt
* connect to remote SDP endpoint.
*/
sys_slist_init(&session->reqs);
sys_slist_init(&session->reqs_next);
sys_slist_append(&session->reqs, &params->_node);
err = sdp_client_new_session(conn, session);
if (err) {
sdp_client_clean_after_release(session);
}
k_sem_give(&session->sem_lock);
return err;
}
int bt_sdp_discover(struct bt_conn *conn,
struct bt_sdp_discover_params *params)
{
if (params == NULL || params->uuid == NULL || params->func == NULL ||
params->pool == NULL ||
(params->ids != NULL && params->ids->count != 0 && params->ids->ranges == NULL)) {
LOG_WRN("Invalid user params");
return -EINVAL;
}
if (params->ids != NULL) {
for (size_t i = 0; i < params->ids->count; i++) {
struct bt_sdp_attribute_id_range *range;
range = &params->ids->ranges[i];
if (range->beginning <= range->ending) {
continue;
}
LOG_WRN("Invalid range %u > %u", range->beginning, range->ending);
return -EINVAL;
}
}
return sdp_client_discovery_start(conn, params);
}
/* Helper getting length of data determined by DTD for integers */
static inline ssize_t sdp_get_int_len(const uint8_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:
LOG_ERR("Invalid/unhandled DTD 0x%02x", data[0]);
return -EINVAL;
}
LOG_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 uint8_t *data, size_t len)
{
BT_ASSERT(data);
switch (data[0]) {
case BT_SDP_UUID16:
if (len < (sizeof(uint8_t) + BT_UUID_SIZE_16)) {
break;
}
return sizeof(uint8_t) + BT_UUID_SIZE_16;
case BT_SDP_UUID32:
if (len < (sizeof(uint8_t) + BT_UUID_SIZE_32)) {
break;
}
return sizeof(uint8_t) + BT_UUID_SIZE_32;
case BT_SDP_UUID128:
if (len < (sizeof(uint8_t) + BT_UUID_SIZE_128)) {
break;
}
return sizeof(uint8_t) + BT_UUID_SIZE_128;
default:
LOG_ERR("Invalid/unhandled DTD 0x%02x", data[0]);
return -EINVAL;
}
LOG_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 uint8_t *data, size_t len)
{
const uint8_t *pnext;
BT_ASSERT(data);
/* validate len for pnext safe use to read next 8bit value */
if (len < 2) {
goto err;
}
pnext = data + sizeof(uint8_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:
LOG_ERR("Invalid/unhandled DTD 0x%02x", data[0]);
return -EINVAL;
}
err:
LOG_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 uint8_t *data, size_t len)
{
const uint8_t *pnext;
BT_ASSERT(data);
/* validate len for pnext safe use to read 8bit bit value */
if (len < 2) {
goto err;
}
pnext = data + sizeof(uint8_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:
/* validate len for pnext safe use to read 32bit value */
if (len < 5) {
break;
}
if (len < (5 + sys_get_be32(pnext))) {
break;
}
return 5 + sys_get_be32(pnext);
default:
LOG_ERR("Invalid/unhandled DTD 0x%02x", data[0]);
return -EINVAL;
}
err:
LOG_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 uint8_t *data, size_t len)
{
BT_ASSERT(data);
LOG_DBG("Attr val DTD 0x%02x", data[0]);
if (len < 1) {
goto err;
}
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:
LOG_ERR("Unknown DTD 0x%02x", data[0]);
return -EINVAL;
}
err:
LOG_ERR("Too short buffer length %zu", len);
return -EMSGSIZE;
}
/* 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;
};
uint16_t attr_id;
uint8_t *params;
uint16_t params_len;
};
/* Generic attribute item collector. */
struct bt_sdp_attr_item {
/* Attribute identifier. */
uint16_t attr_id;
/* Address of beginning attribute value taken from original buffer
* holding response from server.
*/
uint8_t *val;
/* Says about the length of attribute value. */
uint16_t len;
};
static int bt_sdp_get_attr(const struct net_buf *buf,
struct bt_sdp_attr_item *attr, uint16_t attr_id)
{
uint8_t *data;
uint16_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) {
LOG_ERR("Invalid descriptor 0x%02x", data[0]);
return -EINVAL;
}
data += sizeof(uint8_t);
if ((data + sizeof(id) - buf->data) > buf->len) {
return -EINVAL;
}
id = sys_get_be16(data);
LOG_DBG("Attribute ID 0x%04x", id);
data += sizeof(uint16_t);
dlen = sdp_get_attr_value_len(data,
buf->len - (data - buf->data));
if (dlen < 0) {
LOG_ERR("Invalid attribute value data");
return -EINVAL;
}
if (id == attr_id) {
LOG_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(uint8_t **data, size_t len)
{
const uint8_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(uint8_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:
LOG_ERR("Invalid/unhandled DTD 0x%02x", *data[0]);
return -EINVAL;
}
err:
LOG_ERR("Too short buffer length %zu", len);
return -EMSGSIZE;
}
static int sdp_loop_seqs(uint8_t **data, size_t len)
{
ssize_t slen;
ssize_t pre_slen;
uint8_t *end;
if (len <= 0) {
return -EMSGSIZE;
}
pre_slen = -EINVAL;
slen = -EINVAL;
end = *data + len;
/* loop all the SEQ */
while (*data < end) {
/* how long is current UUID's item data associated to */
slen = sdp_get_seq_len_item(data, end - *data);
if (slen < 0) {
break;
}
pre_slen = slen;
}
/* return the last seq len */
if (pre_slen < 0) {
return slen;
}
return pre_slen;
}
static int sdp_get_uuid_data(const struct bt_sdp_attr_item *attr,
struct bt_sdp_uuid_desc *pd,
uint16_t proto_profile,
uint8_t proto_profile_index)
{
/* get start address of attribute value */
uint8_t *p = attr->val;
ssize_t slen;
BT_ASSERT(p);
/* start reading stacked UUIDs in analyzed sequences tree */
while (p - attr->val < attr->len) {
size_t to_end, left = 0;
uint8_t dtd;
/* to_end tells how far to the end of input buffer */
to_end = attr->len - (p - attr->val);
/* loop all the SEQ, get the last SEQ len */
slen = sdp_loop_seqs(&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 < (sizeof(dtd) + BT_UUID_SIZE_16)) {
return -EMSGSIZE;
}
/* check DTD and get stacked UUID value */
dtd = p[0];
p++;
/* include last DTD in p[0] size itself updating left */
left -= sizeof(dtd);
switch (dtd) {
case BT_SDP_UUID16:
memcpy(&pd->uuid16,
BT_UUID_DECLARE_16(sys_get_be16(p)),
sizeof(struct bt_uuid_16));
p += sizeof(uint16_t);
left -= sizeof(uint16_t);
break;
case BT_SDP_UUID32:
/* check if valid UUID32 can be read safely */
if (left < BT_UUID_SIZE_32) {
return -EMSGSIZE;
}
memcpy(&pd->uuid32,
BT_UUID_DECLARE_32(sys_get_be32(p)),
sizeof(struct bt_uuid_32));
p += sizeof(BT_UUID_SIZE_32);
left -= sizeof(BT_UUID_SIZE_32);
break;
default:
LOG_ERR("Invalid/unhandled DTD 0x%02x\n", dtd);
return -EINVAL;
}
/*
* 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;
LOG_DBG("UUID 0x%s found", bt_uuid_str(&pd->uuid));
if (proto_profile_index > 0U) {
proto_profile_index--;
p += left;
continue;
} else {
return 0;
}
}
/* skip left octets to point beginning of next UUID in tree */
p += left;
}
LOG_DBG("Value 0x%04x index %d not found", proto_profile, proto_profile_index);
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, uint16_t *param)
{
const uint8_t *p = pd_item->params;
bool len_err = false;
BT_ASSERT(p);
LOG_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(uint8_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(uint16_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(uint32_t);
break;
default:
LOG_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) {
LOG_DBG("Invalid param buffer length");
return -EMSGSIZE;
}
return 0;
}
static int sdp_get_u16_data(const struct bt_sdp_attr_item *attr, uint16_t *u16)
{
const uint8_t *p;
if (!u16) {
LOG_ERR("Invalid pointer.");
return -EINVAL;
}
/* assert 16bit can be read safely */
if (attr->len != (sizeof(uint8_t) + sizeof(*u16))) {
LOG_ERR("Invalid data length %u", attr->len);
return -EMSGSIZE;
}
p = attr->val;
__ASSERT(p != NULL, "attr->val cannot be NULL");
if (p[0] != BT_SDP_UINT16) {
LOG_ERR("Invalid DTD 0x%02x", p[0]);
return -EINVAL;
}
*u16 = sys_get_be16(++p);
return 0;
}
int bt_sdp_get_proto_param(const struct net_buf *buf, enum bt_sdp_proto proto,
uint16_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 &&
proto != BT_SDP_PROTO_AVDTP) {
LOG_ERR("Invalid protocol specifier");
return -EINVAL;
}
res = bt_sdp_get_attr(buf, &attr, BT_SDP_ATTR_PROTO_DESC_LIST);
if (res < 0) {
LOG_WRN("Attribute 0x%04x not found, err %d", BT_SDP_ATTR_PROTO_DESC_LIST, res);
return res;
}
res = sdp_get_uuid_data(&attr, &pd, proto, 0U);
if (res < 0) {
LOG_WRN("Protocol specifier 0x%04x not found, err %d", proto, res);
return res;
}
return sdp_get_param_item(&pd, param);
}
int bt_sdp_get_addl_proto_param(const struct net_buf *buf, enum bt_sdp_proto proto,
uint8_t param_index, uint16_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 &&
proto != BT_SDP_PROTO_AVDTP) {
LOG_ERR("Invalid protocol specifier");
return -EINVAL;
}
res = bt_sdp_get_attr(buf, &attr, BT_SDP_ATTR_ADD_PROTO_DESC_LIST);
if (res < 0) {
LOG_WRN("Attribute 0x%04x not found, err %d", BT_SDP_ATTR_PROTO_DESC_LIST, res);
return res;
}
res = sdp_get_uuid_data(&attr, &pd, proto, param_index);
if (res < 0) {
LOG_WRN("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, uint16_t profile,
uint16_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) {
LOG_WRN("Attribute 0x%04x not found, err %d", BT_SDP_ATTR_PROFILE_DESC_LIST, res);
return res;
}
res = sdp_get_uuid_data(&attr, &pd, profile, 0U);
if (res < 0) {
LOG_WRN("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, uint16_t *features)
{
struct bt_sdp_attr_item attr;
int err;
err = bt_sdp_get_attr(buf, &attr, BT_SDP_ATTR_SUPPORTED_FEATURES);
if (err < 0) {
LOG_WRN("Attribute 0x%04x not found, err %d", BT_SDP_ATTR_SUPPORTED_FEATURES, err);
return err;
}
return sdp_get_u16_data(&attr, features);
}
int bt_sdp_get_vendor_id(const struct net_buf *buf, uint16_t *vendor_id)
{
struct bt_sdp_attr_item attr;
int err;
err = bt_sdp_get_attr(buf, &attr, BT_SDP_ATTR_VENDOR_ID);
if (err < 0) {
LOG_WRN("Attribute 0x%04x not found, err %d", BT_SDP_ATTR_VENDOR_ID, err);
return err;
}
return sdp_get_u16_data(&attr, vendor_id);
}
int bt_sdp_get_product_id(const struct net_buf *buf, uint16_t *product_id)
{
struct bt_sdp_attr_item attr;
int err;
err = bt_sdp_get_attr(buf, &attr, BT_SDP_ATTR_PRODUCT_ID);
if (err < 0) {
LOG_WRN("Attribute 0x%04x not found, err %d", BT_SDP_ATTR_PRODUCT_ID, err);
return err;
}
return sdp_get_u16_data(&attr, product_id);
}