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; }