void sdp_set_seq_len(uint8_t *ptr, uint32_t length)
{
uint8_t dtd = *(uint8_t *) ptr++;
switch (dtd) {
case SDP_SEQ8:
case SDP_ALT8:
case SDP_TEXT_STR8:
case SDP_URL_STR8:
*(uint8_t *)ptr = (uint8_t) length;
break;
case SDP_SEQ16:
case SDP_ALT16:
case SDP_TEXT_STR16:
case SDP_URL_STR16:
bt_put_unaligned(htons(length), (uint16_t *) ptr);
break;
case SDP_SEQ32:
case SDP_ALT32:
case SDP_TEXT_STR32:
case SDP_URL_STR32:
bt_put_unaligned(htonl(length), (uint32_t *) ptr);
break;
}
}
/*
* Remove a registered service record
*/
int service_remove_req(sdp_req_t *req, sdp_buf_t *rsp)
{
uint8_t *p = req->buf + sizeof(sdp_pdu_hdr_t);
uint32_t handle = ntohl(bt_get_unaligned((uint32_t *) p));
sdp_record_t *rec;
int status = 0;
/* extract service record handle */
rec = sdp_record_find(handle);
if (rec) {
sdp_svcdb_collect(rec);
status = sdp_record_remove(handle);
sdp_record_free(rec);
if (status == 0)
update_db_timestamp();
} else {
status = SDP_INVALID_RECORD_HANDLE;
SDPDBG("Could not find record : 0x%x", handle);
}
p = rsp->data;
bt_put_unaligned(htons(status), (uint16_t *) p);
rsp->data_size = sizeof(uint16_t);
return status;
}
/*
* Update a service record
*/
int service_update_req(sdp_req_t *req, sdp_buf_t *rsp)
{
sdp_record_t *orec, *nrec;
int status = 0, scanned = 0;
uint8_t *p = req->buf + sizeof(sdp_pdu_hdr_t);
int bufsize = req->len - sizeof(sdp_pdu_hdr_t);
uint32_t handle = ntohl(bt_get_unaligned((uint32_t *) p));
SDPDBG("Svc Rec Handle: 0x%x", handle);
p += sizeof(uint32_t);
bufsize -= sizeof(uint32_t);
orec = sdp_record_find(handle);
SDPDBG("SvcRecOld: %p", orec);
if (!orec) {
status = SDP_INVALID_RECORD_HANDLE;
goto done;
}
nrec = extract_pdu_server(BDADDR_ANY, p, bufsize, handle, &scanned);
if (!nrec) {
status = SDP_INVALID_SYNTAX;
goto done;
}
assert(nrec == orec);
update_db_timestamp();
done:
p = rsp->data;
bt_put_unaligned(htons(status), (uint16_t *) p);
rsp->data_size = sizeof(uint16_t);
return status;
}
/*
* Service search request PDU. This method extracts the search pattern
* (a sequence of UUIDs) and calls the matching function
* to find matching services
*/
static int service_search_req(sdp_req_t *req, sdp_buf_t *buf)
{
int status = 0, i, plen, mlen, mtu, scanned;
sdp_list_t *pattern = NULL;
uint16_t expected, actual, rsp_count = 0;
uint8_t dtd;
sdp_cont_state_t *cstate = NULL;
uint8_t *pCacheBuffer = NULL;
int handleSize = 0;
uint32_t cStateId = 0;
short *pTotalRecordCount, *pCurrentRecordCount;
uint8_t *pdata = req->buf + sizeof(sdp_pdu_hdr_t);
size_t data_left = req->len - sizeof(sdp_pdu_hdr_t);
scanned = extract_des(pdata, data_left, &pattern, &dtd, SDP_TYPE_UUID);
if (scanned == -1) {
status = SDP_INVALID_SYNTAX;
goto done;
}
pdata += scanned;
data_left -= scanned;
plen = ntohs(((sdp_pdu_hdr_t *)(req->buf))->plen);
mlen = scanned + sizeof(uint16_t) + 1;
// ensure we don't read past buffer
if (plen < mlen || plen != mlen + *(uint8_t *)(pdata+sizeof(uint16_t))) {
status = SDP_INVALID_SYNTAX;
goto done;
}
if (data_left < sizeof(uint16_t)) {
status = SDP_INVALID_SYNTAX;
goto done;
}
expected = ntohs(bt_get_unaligned((uint16_t *)pdata));
SDPDBG("Expected count: %d", expected);
SDPDBG("Bytes scanned : %d", scanned);
pdata += sizeof(uint16_t);
data_left -= sizeof(uint16_t);
/*
* Check if continuation state exists, if yes attempt
* to get rsp remainder from cache, else send error
*/
if (sdp_cstate_get(pdata, data_left, &cstate) < 0) {
status = SDP_INVALID_SYNTAX;
goto done;
}
mtu = req->mtu - sizeof(sdp_pdu_hdr_t) - sizeof(uint16_t) - sizeof(uint16_t) - SDP_CONT_STATE_SIZE;
actual = MIN(expected, mtu >> 2);
/* make space in the rsp buffer for total and current record counts */
pdata = buf->data;
/* total service record count = 0 */
pTotalRecordCount = (short *)pdata;
bt_put_unaligned(0, (uint16_t *)pdata);
pdata += sizeof(uint16_t);
buf->data_size += sizeof(uint16_t);
/* current service record count = 0 */
pCurrentRecordCount = (short *)pdata;
bt_put_unaligned(0, (uint16_t *)pdata);
pdata += sizeof(uint16_t);
buf->data_size += sizeof(uint16_t);
if (cstate == NULL) {
/* for every record in the DB, do a pattern search */
sdp_list_t *list = sdp_get_record_list();
handleSize = 0;
for (; list && rsp_count < expected; list = list->next) {
sdp_record_t *rec = (sdp_record_t *) list->data;
SDPDBG("Checking svcRec : 0x%x", rec->handle);
if (sdp_match_uuid(pattern, rec->pattern) > 0 &&
sdp_check_access(rec->handle, &req->device)) {
rsp_count++;
bt_put_unaligned(htonl(rec->handle), (uint32_t *)pdata);
pdata += sizeof(uint32_t);
handleSize += sizeof(uint32_t);
}
}
SDPDBG("Match count: %d", rsp_count);
buf->data_size += handleSize;
bt_put_unaligned(htons(rsp_count), (uint16_t *)pTotalRecordCount);
bt_put_unaligned(htons(rsp_count), (uint16_t *)pCurrentRecordCount);
if (rsp_count > actual) {
/* cache the rsp and generate a continuation state */
cStateId = sdp_cstate_alloc_buf(buf);
/*
* subtract handleSize since we now send only
* a subset of handles
*/
buf->data_size -= handleSize;
} else {
/* NULL continuation state */
sdp_set_cstate_pdu(buf, NULL);
}
}
/* under both the conditions below, the rsp buffer is not built yet */
if (cstate || cStateId > 0) {
short lastIndex = 0;
if (cstate) {
/*
* Get the previous sdp_cont_state_t and obtain
* the cached rsp
*/
sdp_buf_t *pCache = sdp_get_cached_rsp(cstate);
if (pCache) {
pCacheBuffer = pCache->data;
/* get the rsp_count from the cached buffer */
rsp_count = ntohs(bt_get_unaligned((uint16_t *)pCacheBuffer));
/* get index of the last sdp_record_t sent */
lastIndex = cstate->cStateValue.lastIndexSent;
} else {
status = SDP_INVALID_CSTATE;
goto done;
}
} else {
pCacheBuffer = buf->data;
lastIndex = 0;
}
/*
* Set the local buffer pointer to after the
* current record count and increment the cached
* buffer pointer to beyond the counters
*/
pdata = (uint8_t *) pCurrentRecordCount + sizeof(uint16_t);
/* increment beyond the totalCount and the currentCount */
pCacheBuffer += 2 * sizeof(uint16_t);
if (cstate) {
handleSize = 0;
for (i = lastIndex; (i - lastIndex) < actual && i < rsp_count; i++) {
bt_put_unaligned(bt_get_unaligned((uint32_t *)(pCacheBuffer + i * sizeof(uint32_t))), (uint32_t *)pdata);
pdata += sizeof(uint32_t);
handleSize += sizeof(uint32_t);
}
} else {
handleSize = actual << 2;
i = actual;
}
buf->data_size += handleSize;
bt_put_unaligned(htons(rsp_count), (uint16_t *)pTotalRecordCount);
bt_put_unaligned(htons(i - lastIndex), (uint16_t *)pCurrentRecordCount);
if (i == rsp_count) {
/* set "null" continuationState */
sdp_set_cstate_pdu(buf, NULL);
} else {
/*
* there's more: set lastIndexSent to
* the new value and move on
*/
sdp_cont_state_t newState;
SDPDBG("Setting non-NULL sdp_cstate_t");
if (cstate)
memcpy(&newState, cstate, sizeof(sdp_cont_state_t));
else {
memset(&newState, 0, sizeof(sdp_cont_state_t));
newState.timestamp = cStateId;
}
newState.cStateValue.lastIndexSent = i;
sdp_set_cstate_pdu(buf, &newState);
}
}
done:
free(cstate);
if (pattern)
sdp_list_free(pattern, free);
return status;
}
/*
* Generic data element sequence extractor. Builds
* a list whose elements are those found in the
* sequence. The data type of elements found in the
* sequence is returned in the reference pDataType
*/
static int extract_des(uint8_t *buf, int len, sdp_list_t **svcReqSeq, uint8_t *pDataType, uint8_t expectedType)
{
uint8_t seqType;
int scanned, data_size = 0;
short numberOfElements = 0;
int seqlen = 0;
sdp_list_t *pSeq = NULL;
uint8_t dataType;
int status = 0;
const uint8_t *p;
size_t bufsize;
scanned = sdp_extract_seqtype(buf, len, &seqType, &data_size);
SDPDBG("Seq type : %d", seqType);
if (!scanned || (seqType != SDP_SEQ8 && seqType != SDP_SEQ16)) {
error("Unknown seq type");
return -1;
}
p = buf + scanned;
bufsize = len - scanned;
SDPDBG("Data size : %d", data_size);
for (;;) {
char *pElem = NULL;
int localSeqLength = 0;
if (bufsize < sizeof(uint8_t)) {
SDPDBG("->Unexpected end of buffer");
goto failed;
}
dataType = *p;
SDPDBG("Data type: 0x%02x", dataType);
if (expectedType == SDP_TYPE_UUID) {
if (dataType != SDP_UUID16 && dataType != SDP_UUID32 && dataType != SDP_UUID128) {
SDPDBG("->Unexpected Data type (expected UUID_ANY)");
goto failed;
}
} else if (expectedType == SDP_TYPE_ATTRID &&
(dataType != SDP_UINT16 && dataType != SDP_UINT32)) {
SDPDBG("->Unexpected Data type (expected 0x%02x or 0x%02x)",
SDP_UINT16, SDP_UINT32);
goto failed;
} else if (expectedType != SDP_TYPE_ATTRID && dataType != expectedType) {
SDPDBG("->Unexpected Data type (expected 0x%02x)", expectedType);
goto failed;
}
switch (dataType) {
case SDP_UINT16:
p += sizeof(uint8_t);
seqlen += sizeof(uint8_t);
bufsize -= sizeof(uint8_t);
if (bufsize < sizeof(uint16_t)) {
SDPDBG("->Unexpected end of buffer");
goto failed;
}
if (expectedType == SDP_TYPE_ATTRID) {
struct attrid *aid;
aid = malloc(sizeof(struct attrid));
aid->dtd = dataType;
bt_put_unaligned(ntohs(bt_get_unaligned((uint16_t *)p)), (uint16_t *)&aid->uint16);
pElem = (char *) aid;
} else {
pElem = malloc(sizeof(uint16_t));
bt_put_unaligned(ntohs(bt_get_unaligned((uint16_t *)p)), (uint16_t *)pElem);
}
p += sizeof(uint16_t);
seqlen += sizeof(uint16_t);
bufsize -= sizeof(uint16_t);
break;
case SDP_UINT32:
p += sizeof(uint8_t);
seqlen += sizeof(uint8_t);
bufsize -= sizeof(uint8_t);
if (bufsize < (int)sizeof(uint32_t)) {
SDPDBG("->Unexpected end of buffer");
goto failed;
}
if (expectedType == SDP_TYPE_ATTRID) {
struct attrid *aid;
aid = malloc(sizeof(struct attrid));
aid->dtd = dataType;
bt_put_unaligned(ntohl(bt_get_unaligned((uint32_t *)p)), (uint32_t *)&aid->uint32);
pElem = (char *) aid;
} else {
pElem = malloc(sizeof(uint32_t));
bt_put_unaligned(ntohl(bt_get_unaligned((uint32_t *)p)), (uint32_t *)pElem);
}
p += sizeof(uint32_t);
seqlen += sizeof(uint32_t);
bufsize -= sizeof(uint32_t);
break;
case SDP_UUID16:
case SDP_UUID32:
case SDP_UUID128:
pElem = malloc(sizeof(uuid_t));
status = sdp_uuid_extract(p, bufsize, (uuid_t *) pElem, &localSeqLength);
if (status < 0) {
free(pElem);
goto failed;
}
seqlen += localSeqLength;
p += localSeqLength;
bufsize -= localSeqLength;
break;
default:
return -1;
}
if (status == 0) {
pSeq = sdp_list_append(pSeq, pElem);
numberOfElements++;
SDPDBG("No of elements : %d", numberOfElements);
if (seqlen == data_size)
break;
else if (seqlen > data_size || seqlen > len)
goto failed;
} else
free(pElem);
}
*svcReqSeq = pSeq;
scanned += seqlen;
*pDataType = dataType;
return scanned;
failed:
sdp_list_free(pSeq, free);
return -1;
}
/*
* Add the newly created service record to the service repository
*/
int service_register_req(sdp_req_t *req, sdp_buf_t *rsp)
{
int scanned = 0;
sdp_data_t *handle;
uint8_t *p = req->buf + sizeof(sdp_pdu_hdr_t);
int bufsize = req->len - sizeof(sdp_pdu_hdr_t);
sdp_record_t *rec;
req->flags = *p++;
if (req->flags & SDP_DEVICE_RECORD) {
bacpy(&req->device, (bdaddr_t *) p);
p += sizeof(bdaddr_t);
bufsize -= sizeof(bdaddr_t);
}
/* save image of PDU: we need it when clients request this attribute */
rec = extract_pdu_server(&req->device, p, bufsize, 0xffffffff, &scanned);
if (!rec)
goto invalid;
if (rec->handle == 0xffffffff) {
rec->handle = sdp_next_handle();
if (rec->handle < 0x10000) {
sdp_record_free(rec);
goto invalid;
}
} else {
if (sdp_record_find(rec->handle)) {
/* extract_pdu_server will add the record handle
* if it is missing. So instead of failing, skip
* the record adding to avoid duplication. */
goto success;
}
}
sdp_record_add(&req->device, rec);
if (!(req->flags & SDP_RECORD_PERSIST))
sdp_svcdb_set_collectable(rec, req->sock);
handle = sdp_data_alloc(SDP_UINT32, &rec->handle);
sdp_attr_replace(rec, SDP_ATTR_RECORD_HANDLE, handle);
success:
/* if the browse group descriptor is NULL,
* ensure that the record belongs to the ROOT group */
if (sdp_data_get(rec, SDP_ATTR_BROWSE_GRP_LIST) == NULL) {
uuid_t uuid;
sdp_uuid16_create(&uuid, PUBLIC_BROWSE_GROUP);
sdp_pattern_add_uuid(rec, &uuid);
}
update_db_timestamp();
/* Build a rsp buffer */
bt_put_unaligned(htonl(rec->handle), (uint32_t *) rsp->data);
rsp->data_size = sizeof(uint32_t);
return 0;
invalid:
bt_put_unaligned(htons(SDP_INVALID_SYNTAX), (uint16_t *) rsp->data);
rsp->data_size = sizeof(uint16_t);
return -1;
}
/*
* Generic data element sequence extractor. Builds
* a list whose elements are those found in the
* sequence. The data type of elements found in the
* sequence is returned in the reference pDataType
*/
static int extract_des(uint8_t *buf, int len, sdp_list_t **svcReqSeq, uint8_t *pDataType, uint8_t expectedType)
{
uint8_t seqType;
int scanned, data_size = 0;
short numberOfElements = 0;
int seqlen = 0;
sdp_list_t *pSeq = NULL;
uint8_t dataType;
int status = 0;
const uint8_t *p;
scanned = sdp_extract_seqtype(buf, &seqType, &data_size);
debug("Seq type : %d", seqType);
if (!scanned || (seqType != SDP_SEQ8 && seqType != SDP_SEQ16)) {
error("Unknown seq type");
return -1;
}
p = buf + scanned;
debug("Data size : %d", data_size);
for (;;) {
char *pElem = NULL;
int localSeqLength = 0;
dataType = *(uint8_t *)p;
debug("Data type: 0x%02x", dataType);
if (expectedType == SDP_TYPE_UUID) {
if (dataType != SDP_UUID16 && dataType != SDP_UUID32 && dataType != SDP_UUID128) {
debug("->Unexpected Data type (expected UUID_ANY)");
return -1;
}
} else if (expectedType != SDP_TYPE_ANY && dataType != expectedType) {
debug("->Unexpected Data type (expected 0x%02x)", expectedType);
return -1;
}
switch (dataType) {
case SDP_UINT16:
p += sizeof(uint8_t);
seqlen += sizeof(uint8_t);
pElem = malloc(sizeof(uint16_t));
bt_put_unaligned(ntohs(bt_get_unaligned((uint16_t *)p)), (uint16_t *)pElem);
p += sizeof(uint16_t);
seqlen += sizeof(uint16_t);
break;
case SDP_UINT32:
p += sizeof(uint8_t);
seqlen += sizeof(uint8_t);
pElem = malloc(sizeof(uint32_t));
bt_put_unaligned(ntohl(bt_get_unaligned((uint32_t *)p)), (uint32_t *)pElem);
p += sizeof(uint32_t);
seqlen += sizeof(uint32_t);
break;
case SDP_UUID16:
case SDP_UUID32:
case SDP_UUID128:
pElem = malloc(sizeof(uuid_t));
status = sdp_uuid_extract(p, (uuid_t *)pElem, &localSeqLength);
if (status == 0) {
seqlen += localSeqLength;
p += localSeqLength;
}
break;
default:
return -1;
}
if (status == 0) {
pSeq = sdp_list_append(pSeq, pElem);
numberOfElements++;
debug("No of elements : %d", numberOfElements);
if (seqlen == data_size)
break;
else if (seqlen > data_size || seqlen > len)
return -1;
} else
free(pElem);
}
*svcReqSeq = pSeq;
scanned += seqlen;
*pDataType = dataType;
return scanned;
}
