/** @file * @brief Service Discovery Protocol handling. */ /* * Copyright (c) 2016 Intel Corporation * * SPDX-License-Identifier: Apache-2.0 */ #include #include #include #include #include #include #include "common/bt_str.h" #include "common/assert.h" #include "hci_core.h" #include "conn_internal.h" #include "l2cap_internal.h" #include "sdp_internal.h" #define LOG_LEVEL CONFIG_BT_SDP_LOG_LEVEL #include 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 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 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), 8, 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 */ 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; }; 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; 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; uint8_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); /* @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); LOG_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); LOG_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 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 = 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 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 uint16_t parse_data_elem(struct net_buf *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_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_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: 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 = 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 uint8_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 uint16_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; uint32_t uuid_list_size; uint16_t res; uint8_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)) { LOG_WRN("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) { LOG_WRN("Invalid type %u 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; } 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 { LOG_WRN("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 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; struct bt_sdp_record *matching_recs[BT_SDP_MAX_SERVICES]; uint16_t max_rec_count, total_recs = 0U, current_recs = 0U, res; uint8_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) { 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); /* 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) { 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_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); } LOG_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 */ LOG_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); 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 * * 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 uint32_t copy_attribute(struct bt_sdp_data_elem *elem, struct net_buf *buf, uint8_t nest_level) { const uint8_t *cur_elem; uint32_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, *((uint8_t *)elem->data)); } else if (seq_size == 2U) { net_buf_add_be16(buf, *((uint16_t *)elem->data)); } else if (seq_size == 4U) { net_buf_add_be32(buf, *((uint32_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 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, seq_size; uint8_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 uint16_t create_attr_list(struct bt_sdp *sdp, struct bt_sdp_record *record, uint32_t *filter, uint8_t num_filters, uint16_t max_att_len, uint8_t cont_state_size, uint8_t next_att, struct search_state *state, struct net_buf *rsp_buf) { struct select_attrs_data sad; 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, 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 uint16_t get_att_search_list(struct net_buf *buf, uint32_t *filter, uint8_t *num_filters) { struct bt_sdp_data_elem data_elem; uint16_t res; uint32_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) { 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 = SDP_INVALID, .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; uint8_t num_filters, cont_state_size, next_att = 0U; 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); /* Set up the filters */ res = get_att_search_list(buf, 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) + 1; next_att = state.last_att; } LOG_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) { 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, 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) { LOG_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); 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 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; uint16_t max_att_len, res, att_list_len = 0U; uint8_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) { LOG_WRN("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) { LOG_WRN("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) { 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.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; } LOG_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) { LOG_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); } 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, hdr->tid); break; } } if (err) { LOG_WRN("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_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); } } int bt_sdp_register_service(struct bt_sdp_record *service) { uint32_t handle = SDP_SERVICE_HANDLE_BASE; if (!service) { LOG_ERR("No service record specified"); return 0; } if (num_services == BT_SDP_MAX_SERVICES) { LOG_ERR("Reached max allowed registrations"); return -ENOMEM; } if (db) { handle = db->handle + 1; } service->next = db; service->index = num_services++; service->handle = handle; *((uint32_t *)(service->attrs[0].val.data)) = handle; db = service; LOG_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 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) { LOG_WRN("No UUIDs to be resolved on remote"); return -EINVAL; } 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, 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: LOG_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); } /* Update context param to the one being resolving now */ session->param = param; session->tid++; return bt_sdp_send(&session->chan.chan, buf, BT_SDP_SVC_SEARCH_ATTR_REQ, session->tid); } 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, ¶m->_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 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; 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_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; default: LOG_WRN("Sequence type 0x%02x not handled", seq); len = 0U; break; } LOG_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; uint16_t rec_len; uint8_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 instead 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; uint16_t len, tid, frame_len; uint16_t total; 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)); if (hdr->op_code == BT_SDP_ERROR_RSP) { LOG_INF("Error SDP PDU response"); return 0; } 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; } 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) { LOG_ERR("Invalid frame payload length"); return 0; } /* Check valid range of attributes length */ if (frame_len < 2) { LOG_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) { LOG_ERR("Invalid SDP PDU Continuation State length %u", cstate->length); return 0; } if ((frame_len + SDP_CONT_STATE_LEN_SIZE + cstate->length) > buf->len) { LOG_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) { LOG_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)) { LOG_WRN("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)); LOG_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: LOG_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; 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); 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); LOG_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 const 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)); LOG_ERR("Cannot connect %d", err); return NULL; } return session; } LOG_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) { LOG_WRN("Invalid user params"); return -EINVAL; } session = sdp_client_get_session(conn); if (!session) { return -ENOMEM; } sys_slist_append(&session->reqs, (sys_snode_t *)¶ms->_node); return 0; } /* 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 < 3) { break; } return 3; case BT_SDP_UUID32: if (len < 5) { break; } return 5; case BT_SDP_UUID128: 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: 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; } 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) { 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) { 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; const uint8_t *p; int res; res = bt_sdp_get_attr(buf, &attr, BT_SDP_ATTR_SUPPORTED_FEATURES); if (res < 0) { LOG_WRN("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) { LOG_ERR("Invalid DTD 0x%02x", p[0]); return -EINVAL; } /* assert 16bit can be read safely */ if (attr.len < 3) { LOG_ERR("Data length too short %u", attr.len); return -EMSGSIZE; } *features = sys_get_be16(++p); p += sizeof(uint16_t); if (p - attr.val != attr.len) { LOG_ERR("Invalid data length %u", attr.len); return -EMSGSIZE; } return 0; }