/*
* Add a service record to the repository
*/
void sdp_record_add(const bdaddr_t *device, sdp_record_t *rec)
{
struct btd_adapter *adapter;
sdp_access_t *dev;
SDPDBG("Adding rec : 0x%lx", (long) rec);
SDPDBG("with handle : 0x%x", rec->handle);
service_db = sdp_list_insert_sorted(service_db, rec, record_sort);
dev = malloc(sizeof(*dev));
if (!dev)
return;
bacpy(&dev->device, device);
dev->handle = rec->handle;
access_db = sdp_list_insert_sorted(access_db, dev, access_sort);
if (bacmp(device, BDADDR_ANY) == 0) {
adapter_foreach(adapter_service_insert, rec);
return;
}
adapter = adapter_find(device);
if (adapter)
adapter_service_insert(adapter, rec);
}
static int sdp_cstate_get(uint8_t *buffer, size_t len,
sdp_cont_state_t **cstate)
{
uint8_t cStateSize = *buffer;
SDPDBG("Continuation State size : %d", cStateSize);
if (cStateSize == 0) {
*cstate = NULL;
return 0;
}
buffer++;
len--;
if (len < sizeof(sdp_cont_state_t))
return -EINVAL;
/*
* Check if continuation state exists, if yes attempt
* to get response remainder from cache, else send error
*/
*cstate = malloc(sizeof(sdp_cont_state_t));
if (!(*cstate))
return -ENOMEM;
memcpy(*cstate, buffer, sizeof(sdp_cont_state_t));
SDPDBG("Cstate TS : 0x%x", (*cstate)->timestamp);
SDPDBG("Bytes sent : %d", (*cstate)->cStateValue.maxBytesSent);
return 0;
}
/*
* The matching process is defined as "each and every UUID
* specified in the "search pattern" must be present in the
* "target pattern". Here "search pattern" is the set of UUIDs
* specified by the service discovery client and "target pattern"
* is the set of UUIDs present in a service record.
*
* Return 1 if each and every UUID in the search
* pattern exists in the target pattern, 0 if the
* match succeeds and -1 on error.
*/
static int sdp_match_uuid(sdp_list_t *search, sdp_list_t *pattern)
{
/*
* The target is a sorted list, so we need not look
* at all elements to confirm existence of an element
* from the search pattern
*/
int patlen = sdp_list_len(pattern);
SDPDBG("");
if (patlen < sdp_list_len(search))
return -1;
for (; search; search = search->next) {
uuid_t *uuid128;
void *data = search->data;
sdp_list_t *list;
if (data == NULL)
return -1;
// create 128-bit form of the search UUID
uuid128 = sdp_uuid_to_uuid128((uuid_t *)data);
list = sdp_list_find(pattern, uuid128, sdp_uuid128_cmp);
free(uuid128);
if (!list)
return 0;
}
return 1;
}
static void update_adapter_svclass_list(struct btd_adapter *adapter)
{
sdp_list_t *list = adapter_get_services(adapter);
uint8_t val = 0;
for (; list; list = list->next) {
sdp_record_t *rec = (sdp_record_t *) list->data;
if (rec->svclass.type != SDP_UUID16)
continue;
switch (rec->svclass.value.uuid16) {
case DIALUP_NET_SVCLASS_ID:
case CIP_SVCLASS_ID:
val |= 0x42; /* Telephony & Networking */
break;
case IRMC_SYNC_SVCLASS_ID:
case OBEX_OBJPUSH_SVCLASS_ID:
case OBEX_FILETRANS_SVCLASS_ID:
case IRMC_SYNC_CMD_SVCLASS_ID:
case PBAP_PSE_SVCLASS_ID:
val |= 0x10; /* Object Transfer */
break;
case HEADSET_SVCLASS_ID:
case HANDSFREE_SVCLASS_ID:
val |= 0x20; /* Audio */
break;
case CORDLESS_TELEPHONY_SVCLASS_ID:
case INTERCOM_SVCLASS_ID:
case FAX_SVCLASS_ID:
case SAP_SVCLASS_ID:
/*
* Setting the telephony bit for the handsfree audio gateway
* role is not required by the HFP specification, but the
* Nokia 616 carkit is just plain broken! It will refuse
* pairing without this bit set.
*/
case HANDSFREE_AGW_SVCLASS_ID:
val |= 0x40; /* Telephony */
break;
case AUDIO_SOURCE_SVCLASS_ID:
case VIDEO_SOURCE_SVCLASS_ID:
val |= 0x08; /* Capturing */
break;
case AUDIO_SINK_SVCLASS_ID:
case VIDEO_SINK_SVCLASS_ID:
val |= 0x04; /* Rendering */
break;
case PANU_SVCLASS_ID:
case NAP_SVCLASS_ID:
case GN_SVCLASS_ID:
val |= 0x02; /* Networking */
break;
}
}
SDPDBG("Service classes 0x%02x", val);
manager_update_svc(adapter, val);
}
/*
* 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 = bt_get_be32(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_be16(status, p);
rsp->data_size = sizeof(uint16_t);
return status;
}
/*
* Add a service record to the repository
*/
void sdp_record_add(const bdaddr_t *device, sdp_record_t *rec)
{
sdp_access_t *dev;
SDPDBG("Adding rec : 0x%lx", (long) rec);
SDPDBG("with handle : 0x%x", rec->handle);
service_db = sdp_list_insert_sorted(service_db, rec, record_sort);
dev = malloc(sizeof(*dev));
if (!dev)
return;
bacpy(&dev->device, device);
dev->handle = rec->handle;
access_db = sdp_list_insert_sorted(access_db, dev, access_sort);
}
/*
* Extract attribute identifiers from the request PDU.
* Clients could request a subset of attributes (by id)
* from a service record, instead of the whole set. The
* requested identifiers are present in the PDU form of
* the request
*/
static int extract_attrs(sdp_record_t *rec, sdp_list_t *seq, sdp_buf_t *buf)
{
sdp_buf_t pdu;
if (!rec)
return SDP_INVALID_RECORD_HANDLE;
if (seq) {
SDPDBG("Entries in attr seq : %d", sdp_list_len(seq));
} else {
SDPDBG("NULL attribute descriptor");
}
if (seq == NULL) {
SDPDBG("Attribute sequence is NULL");
return 0;
}
sdp_gen_record_pdu(rec, &pdu);
for (; seq; seq = seq->next) {
struct attrid *aid = seq->data;
SDPDBG("AttrDataType : %d", aid->dtd);
if (aid->dtd == SDP_UINT16) {
uint16_t attr = bt_get_unaligned((uint16_t *)&aid->uint16);
sdp_data_t *a = (sdp_data_t *)sdp_data_get(rec, attr);
if (a)
sdp_append_to_pdu(buf, a);
} else if (aid->dtd == SDP_UINT32) {
uint32_t range = bt_get_unaligned((uint32_t *)&aid->uint32);
uint16_t attr;
uint16_t low = (0xffff0000 & range) >> 16;
uint16_t high = 0x0000ffff & range;
sdp_data_t *data;
SDPDBG("attr range : 0x%x", range);
SDPDBG("Low id : 0x%x", low);
SDPDBG("High id : 0x%x", high);
if (low == 0x0000 && high == 0xffff && pdu.data_size <= buf->buf_size) {
/* copy it */
memcpy(buf->data, pdu.data, pdu.data_size);
buf->data_size = pdu.data_size;
break;
}
/* (else) sub-range of attributes */
for (attr = low; attr < high; attr++) {
data = sdp_data_get(rec, attr);
if (data)
sdp_append_to_pdu(buf, data);
}
data = sdp_data_get(rec, high);
if (data)
sdp_append_to_pdu(buf, data);
} else {
/*
* Given a service record handle, find the record associated with it.
*/
sdp_record_t *sdp_record_find(uint32_t handle)
{
sdp_list_t *p = record_locate(handle);
if (!p) {
SDPDBG("Couldn't find record for : 0x%x", handle);
return 0;
}
return p->data;
}
static sdp_cont_state_t *sdp_cstate_get(char *buffer)
{
char *pdata = buffer;
uint8_t cStateSize = *(uint8_t *)pdata;
/*
* Check if continuation state exists, if yes attempt
* to get response remainder from cache, else send error
*/
SDPDBG("Continuation State size : %d\n", cStateSize);
pdata += sizeof(uint8_t);
if (cStateSize != 0) {
sdp_cont_state_t *cstate = (sdp_cont_state_t *)pdata;
SDPDBG("Cstate TS : 0x%lx\n", cstate->timestamp);
SDPDBG("Bytes sent : %d\n", cstate->cStateValue.maxBytesSent);
return cstate;
}
return NULL;
}
void sdp_svcdb_set_collectable(sdp_record_t *record, int sock)
{
sdp_indexed_t *item = malloc(sizeof(sdp_indexed_t));
if (!item) {
SDPDBG("No memory");
return;
}
item->sock = sock;
item->record = record;
socket_index = sdp_list_insert_sorted(socket_index, item, compare_indices);
}
/*
* 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 = bt_get_be32(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_be16(status, p);
rsp->data_size = sizeof(uint16_t);
return status;
}
static sdp_list_t *access_locate(uint32_t handle)
{
if (access_db) {
sdp_list_t *p;
sdp_access_t a;
a.handle = handle;
p = sdp_list_find(access_db, &a, access_sort);
return p;
}
SDPDBG("Could not find access data for : 0x%x", handle);
return NULL;
}
static sdp_list_t *record_locate(uint32_t handle)
{
if (service_db) {
sdp_list_t *p;
sdp_record_t r;
r.handle = handle;
p = sdp_list_find(service_db, &r, record_sort);
return p;
}
SDPDBG("Could not find svcRec for : 0x%x", handle);
return NULL;
}
/*
* Extract attribute identifiers from the request PDU.
* Clients could request a subset of attributes (by id)
* from a service record, instead of the whole set. The
* requested identifiers are present in the PDU form of
* the request
*/
static int extract_attrs(sdp_record_t *rec, sdp_list_t *seq, uint8_t dtd, sdp_buf_t *buf)
{
if (!rec)
return SDP_INVALID_RECORD_HANDLE;
#ifdef SDP_DEBUG
if (seq)
SDPDBG("Entries in attr seq : %d\n", sdp_list_len(seq));
else
SDPDBG("NULL attribute descriptor\n");
SDPDBG("AttrDataType : %d\n", dtd);
#endif
if (seq == NULL) {
SDPDBG("Attribute sequence is NULL\n");
return 0;
}
if (dtd == SDP_UINT16)
for (; seq; seq = seq->next) {
uint16_t attr = sdp_get_unaligned((uint16_t *)seq->data);
sdp_data_t *a = (sdp_data_t *)sdp_data_get(rec, attr);
if (a)
sdp_append_to_pdu(buf, a);
}
else if (dtd == SDP_UINT32) {
sdp_buf_t pdu;
sdp_gen_record_pdu(rec, &pdu);
for (; seq; seq = seq->next) {
uint32_t range = sdp_get_unaligned((uint32_t *)seq->data);
uint16_t attr;
uint16_t low = (0xffff0000 & range) >> 16;
uint16_t high = 0x0000ffff & range;
SDPDBG("attr range : 0x%x\n", range);
SDPDBG("Low id : 0x%x\n", low);
SDPDBG("High id : 0x%x\n", high);
if (low == 0x0000 && high == 0xffff && pdu.data_size <= buf->buf_size) {
/* copy it */
memcpy(buf->data, pdu.data, pdu.data_size);
buf->data_size = pdu.data_size;
break;
}
/* (else) sub-range of attributes */
for (attr = low; attr <= high; attr++) {
sdp_data_t *a = (sdp_data_t *)sdp_data_get(rec, attr);
if (a)
sdp_append_to_pdu(buf, a);
}
}
free(pdu.data);
} else {
static int sdp_set_cstate_pdu(sdp_buf_t *buf, sdp_cont_state_t *cstate)
{
uint8_t *pdata = buf->data + buf->data_size;
int length = 0;
if (cstate) {
SDPDBG("Non null sdp_cstate_t id : 0x%x", cstate->timestamp);
*pdata = sizeof(sdp_cont_state_t);
pdata += sizeof(uint8_t);
length += sizeof(uint8_t);
memcpy(pdata, cstate, sizeof(sdp_cont_state_t));
length += sizeof(sdp_cont_state_t);
} else {
/* set "null" continuation state */
*pdata = 0;
length += sizeof(uint8_t);
}
buf->data_size += length;
return length;
}
void sdp_init_services_list(bdaddr_t *device)
{
sdp_list_t *p;
DBG("");
for (p = access_db; p != NULL; p = p->next) {
sdp_access_t *access = p->data;
sdp_record_t *rec;
if (bacmp(BDADDR_ANY, &access->device))
continue;
rec = sdp_record_find(access->handle);
if (rec == NULL)
continue;
SDPDBG("adding record with handle %x", access->handle);
adapter_foreach(adapter_service_insert, rec);
}
}
/* FIXME: refactor for server-side */
static sdp_record_t *extract_pdu_server(bdaddr_t *device, uint8_t *p,
unsigned int bufsize,
uint32_t handleExpected, int *scanned)
{
int extractStatus = -1, localExtractedLength = 0;
uint8_t dtd;
int seqlen = 0;
sdp_record_t *rec = NULL;
uint16_t attrId, lookAheadAttrId;
sdp_data_t *pAttr = NULL;
uint32_t handle = 0xffffffff;
*scanned = sdp_extract_seqtype(p, bufsize, &dtd, &seqlen);
p += *scanned;
bufsize -= *scanned;
if (bufsize < sizeof(uint8_t) + sizeof(uint8_t)) {
SDPDBG("Unexpected end of packet");
return NULL;
}
lookAheadAttrId = bt_get_be16(p + sizeof(uint8_t));
SDPDBG("Look ahead attr id : %d", lookAheadAttrId);
if (lookAheadAttrId == SDP_ATTR_RECORD_HANDLE) {
if (bufsize < (sizeof(uint8_t) * 2) +
sizeof(uint16_t) + sizeof(uint32_t)) {
SDPDBG("Unexpected end of packet");
return NULL;
}
handle = bt_get_be32(p + sizeof(uint8_t) + sizeof(uint16_t) +
sizeof(uint8_t));
SDPDBG("SvcRecHandle : 0x%x", handle);
rec = sdp_record_find(handle);
} else if (handleExpected != 0xffffffff)
rec = sdp_record_find(handleExpected);
if (!rec) {
rec = sdp_record_alloc();
rec->attrlist = NULL;
if (lookAheadAttrId == SDP_ATTR_RECORD_HANDLE) {
rec->handle = handle;
sdp_record_add(device, rec);
} else if (handleExpected != 0xffffffff) {
rec->handle = handleExpected;
sdp_record_add(device, rec);
}
} else {
sdp_list_free(rec->attrlist, (sdp_free_func_t) sdp_data_free);
rec->attrlist = NULL;
}
while (localExtractedLength < seqlen) {
int attrSize = sizeof(uint8_t);
int attrValueLength = 0;
if (bufsize < attrSize + sizeof(uint16_t)) {
SDPDBG("Unexpected end of packet: Terminating extraction of attributes");
break;
}
SDPDBG("Extract PDU, sequenceLength: %d localExtractedLength: %d",
seqlen, localExtractedLength);
dtd = *(uint8_t *) p;
attrId = bt_get_be16(p + attrSize);
attrSize += sizeof(uint16_t);
SDPDBG("DTD of attrId : %d Attr id : 0x%x", dtd, attrId);
pAttr = sdp_extract_attr(p + attrSize, bufsize - attrSize,
&attrValueLength, rec);
SDPDBG("Attr id : 0x%x attrValueLength : %d", attrId, attrValueLength);
attrSize += attrValueLength;
if (pAttr == NULL) {
SDPDBG("Terminating extraction of attributes");
break;
}
localExtractedLength += attrSize;
p += attrSize;
bufsize -= attrSize;
sdp_attr_replace(rec, attrId, pAttr);
extractStatus = 0;
SDPDBG("Extract PDU, seqLength: %d localExtractedLength: %d",
seqlen, localExtractedLength);
}
if (extractStatus == 0) {
SDPDBG("Successful extracting of Svc Rec attributes");
#ifdef SDP_DEBUG
sdp_print_service_attr(rec->attrlist);
#endif
*scanned += seqlen;
}
return rec;
}
/*
* 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(char *buf, sdp_list_t **svcReqSeq, uint8_t *pDataType, uint8_t expectedType)
{
uint8_t seqType;
int data_size = 0;
int scanned = sdp_extract_seqtype(buf, &seqType, &data_size);
short numberOfElements = 0;
int seqlen = 0;
sdp_list_t *pSeq = NULL;
uint8_t dataType;
int status = 0;
const char *p;
SDPDBG("Seq type : %d\n", seqType);
if (!scanned || (seqType != SDP_SEQ8 && seqType != SDP_SEQ16)) {
SDPERR("Unknown seq type \n");
return -1;
}
p = buf + scanned;
SDPDBG("Data size : %d\n", data_size);
for (;;) {
char *pElem = NULL;
int localSeqLength = 0;
dataType = *(uint8_t *)p;
SDPDBG("Data type: 0x%02x\n", dataType);
if (expectedType == SDP_TYPE_UUID) {
if (dataType != SDP_UUID16 && dataType != SDP_UUID32 && dataType != SDP_UUID128) {
SDPDBG("->Unexpected Data type (expected UUID_ANY)\n");
return -1;
}
} else if (expectedType != SDP_TYPE_ANY && dataType != expectedType) {
SDPDBG("->Unexpected Data type (expected 0x%02x)\n", expectedType);
return -1;
}
switch (dataType) {
case SDP_UINT16:
p += sizeof(uint8_t);
seqlen += sizeof(uint8_t);
pElem = (char *)malloc(sizeof(uint16_t));
sdp_put_unaligned(ntohs(sdp_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 = (char *)malloc(sizeof(uint32_t));
sdp_put_unaligned(ntohl(sdp_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 = (char *)malloc(sizeof(uuid_t));
status = sdp_uuid_extract(p, (uuid_t *)pElem, &localSeqLength);
if (status == 0) {
seqlen += localSeqLength;
p += localSeqLength;
}
break;
}
if (status == 0) {
pSeq = sdp_list_append(pSeq, pElem);
numberOfElements++;
SDPDBG("No of elements : %d\n", numberOfElements);
if (seqlen == data_size)
break;
else if (seqlen > data_size)
return -1;
} else
free(pElem);
}
*svcReqSeq = pSeq;
scanned += seqlen;
*pDataType = dataType;
return scanned;
}
/*
* 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;
}
