static uint16_t sdp_get_size_for_service_search_attribute_response(uint8_t * serviceSearchPattern, uint8_t * attributeIDList) {
uint16_t total_response_size = 0;
linked_item_t *it;
for (it = (linked_item_t *) sdp_service_records; it ; it = it->next) {
service_record_item_t * item = (service_record_item_t *) it;
if (!sdp_record_matches_service_search_pattern(item->service_record, serviceSearchPattern)) continue;
// for all service records that match
total_response_size += 3 + spd_get_filtered_size(item->service_record, attributeIDList);
}
return total_response_size;
}
int sdp_handle_service_search_attribute_request(uint8_t * packet, uint16_t remote_mtu) {
// SDP header before attribute sevice list: 7
// Continuation, worst case: 5
// get request details
uint16_t transaction_id = READ_NET_16(packet, 1);
// not used yet - uint16_t param_len = READ_NET_16(packet, 3);
uint8_t * serviceSearchPattern = &packet[5];
uint16_t serviceSearchPatternLen = de_get_len(serviceSearchPattern);
uint16_t maximumAttributeByteCount = READ_NET_16(packet, 5 + serviceSearchPatternLen);
uint8_t * attributeIDList = &packet[5+serviceSearchPatternLen+2];
uint16_t attributeIDListLen = de_get_len(attributeIDList);
uint8_t * continuationState = &packet[5+serviceSearchPatternLen+2+attributeIDListLen];
// calc maximumAttributeByteCount based on remote MTU, SDP header and reserved Continuation block
uint16_t maximumAttributeByteCount2 = remote_mtu - 12;
if (maximumAttributeByteCount2 < maximumAttributeByteCount) {
maximumAttributeByteCount = maximumAttributeByteCount2;
}
// continuation state contains: index of next service record to examine
// continuation state contains: byte offset into this service record
uint16_t continuation_service_index = 0;
uint16_t continuation_offset = 0;
if (continuationState[0] == 4) {
continuation_service_index = READ_NET_16(continuationState, 1);
continuation_offset = READ_NET_16(continuationState, 3);
}
// log_info("--> sdp_handle_service_search_attribute_request, cont %u/%u, max %u", continuation_service_index, continuation_offset, maximumAttributeByteCount);
// AttributeLists - starts at offset 7
uint16_t pos = 7;
// add DES with total size for first request
if (continuation_service_index == 0 && continuation_offset == 0) {
uint16_t total_response_size = sdp_get_size_for_service_search_attribute_response(serviceSearchPattern, attributeIDList);
de_store_descriptor_with_len(&sdp_response_buffer[pos], DE_DES, DE_SIZE_VAR_16, total_response_size);
// log_info("total response size %u", total_response_size);
pos += 3;
maximumAttributeByteCount -= 3;
}
// create attribute list
int first_answer = 1;
int continuation = 0;
uint16_t current_service_index = 0;
linked_item_t *it = (linked_item_t *) sdp_service_records;
for ( ; it ; it = it->next, ++current_service_index) {
service_record_item_t * item = (service_record_item_t *) it;
if (current_service_index < continuation_service_index ) continue;
if (!sdp_record_matches_service_search_pattern(item->service_record, serviceSearchPattern)) continue;
if (continuation_offset == 0) {
// get size of this record
uint16_t filtered_attributes_size = spd_get_filtered_size(item->service_record, attributeIDList);
// stop if complete record doesn't fits into response but we already have a partial response
if ((filtered_attributes_size + 3 > maximumAttributeByteCount) && !first_answer) {
continuation = 1;
break;
}
// store DES
de_store_descriptor_with_len(&sdp_response_buffer[pos], DE_DES, DE_SIZE_VAR_16, filtered_attributes_size);
pos += 3;
maximumAttributeByteCount -= 3;
}
first_answer = 0;
// copy maximumAttributeByteCount from record
uint16_t bytes_used;
int complete = sdp_filter_attributes_in_attributeIDList(item->service_record, attributeIDList, continuation_offset, maximumAttributeByteCount, &bytes_used, &sdp_response_buffer[pos]);
pos += bytes_used;
maximumAttributeByteCount -= bytes_used;
if (complete) {
continuation_offset = 0;
continue;
}
continuation = 1;
continuation_offset += bytes_used;
break;
}
uint16_t attributeListsByteCount = pos - 7;
// Continuation State
if (continuation) {
sdp_response_buffer[pos++] = 4;
net_store_16(sdp_response_buffer, pos, (uint16_t) current_service_index);
pos += 2;
net_store_16(sdp_response_buffer, pos, continuation_offset);
pos += 2;
} else {
// complete
sdp_response_buffer[pos++] = 0;
}
// create SDP header
sdp_response_buffer[0] = SDP_ServiceSearchAttributeResponse;
net_store_16(sdp_response_buffer, 1, transaction_id);
net_store_16(sdp_response_buffer, 3, pos - 5); // size of variable payload
net_store_16(sdp_response_buffer, 5, attributeListsByteCount);
return pos;
}
int sdp_handle_service_attribute_request(uint8_t * packet, uint16_t remote_mtu) {
// get request details
uint16_t transaction_id = READ_NET_16(packet, 1);
// not used yet - uint16_t param_len = READ_NET_16(packet, 3);
uint32_t serviceRecordHandle = READ_NET_32(packet, 5);
uint16_t maximumAttributeByteCount = READ_NET_16(packet, 9);
uint8_t * attributeIDList = &packet[11];
uint16_t attributeIDListLen = de_get_len(attributeIDList);
uint8_t * continuationState = &packet[11+attributeIDListLen];
// calc maximumAttributeByteCount based on remote MTU
uint16_t maximumAttributeByteCount2 = remote_mtu - (7+3);
if (maximumAttributeByteCount2 < maximumAttributeByteCount) {
maximumAttributeByteCount = maximumAttributeByteCount2;
}
// continuation state contains the offset into the complete response
uint16_t continuation_offset = 0;
if (continuationState[0] == 2) {
continuation_offset = READ_NET_16(continuationState, 1);
}
// get service record
service_record_item_t * item = sdp_get_record_for_handle(serviceRecordHandle);
if (!item) {
// service record handle doesn't exist
return sdp_create_error_response(transaction_id, 0x0002); /// invalid Service Record Handle
}
// AttributeList - starts at offset 7
uint16_t pos = 7;
if (continuation_offset == 0) {
// get size of this record
uint16_t filtered_attributes_size = spd_get_filtered_size(item->service_record, attributeIDList);
// store DES
de_store_descriptor_with_len(&sdp_response_buffer[pos], DE_DES, DE_SIZE_VAR_16, filtered_attributes_size);
maximumAttributeByteCount -= 3;
pos += 3;
}
// copy maximumAttributeByteCount from record
uint16_t bytes_used;
int complete = sdp_filter_attributes_in_attributeIDList(item->service_record, attributeIDList, continuation_offset, maximumAttributeByteCount, &bytes_used, &sdp_response_buffer[pos]);
pos += bytes_used;
uint16_t attributeListByteCount = pos - 7;
if (complete) {
sdp_response_buffer[pos++] = 0;
} else {
continuation_offset += bytes_used;
sdp_response_buffer[pos++] = 2;
net_store_16(sdp_response_buffer, pos, continuation_offset);
pos += 2;
}
// header
sdp_response_buffer[0] = SDP_ServiceAttributeResponse;
net_store_16(sdp_response_buffer, 1, transaction_id);
net_store_16(sdp_response_buffer, 3, pos - 5); // size of variable payload
net_store_16(sdp_response_buffer, 5, attributeListByteCount);
return pos;
}
void sdp_test(){
const uint16_t remote_mtu = 48;
uint8_t allAttributeIDs[20]; //
// create an attribute list
de_create_sequence(allAttributeIDs);
de_add_number(allAttributeIDs, DE_UINT, DE_SIZE_32, 0x0000ffff);
// create two records with 2 attributes each
de_create_sequence(record);
de_add_number(record, DE_UINT, DE_SIZE_16, SDP_ServiceRecordHandle);
de_add_number(record, DE_UINT, DE_SIZE_32, 0x10001);
de_add_number(record, DE_UINT, DE_SIZE_16, SDP_ServiceClassIDList);
de_add_number(record, DE_UUID, DE_SIZE_16, 0x0001);
de_add_number(record, DE_UINT, DE_SIZE_16, SDP_BrowseGroupList);
de_add_number(record, DE_UUID, DE_SIZE_16, 0x0001);
uint32_t handle_1 = sdp_register_service_internal(NULL, record);
de_dump_data_element(record);
de_create_sequence(record);
de_add_number(record, DE_UINT, DE_SIZE_16, SDP_ServiceRecordHandle);
de_add_number(record, DE_UINT, DE_SIZE_32, 0x10002);
de_add_number(record, DE_UINT, DE_SIZE_16, SDP_ServiceClassIDList);
de_add_number(record, DE_UUID, DE_SIZE_16, 0x0002);
de_add_number(record, DE_UINT, DE_SIZE_16, SDP_BrowseGroupList);
de_add_number(record, DE_UUID, DE_SIZE_16, 0x0001);
sdp_register_service_internal(NULL, record);
de_dump_data_element(record);
uint16_t size = spd_get_filtered_size(record, allAttributeIDs);
printf("Attribute size %u\n", size);
uint16_t transactionID = 1;
uint8_t * attributeIDList;
uint16_t attributeIDListLen;
uint16_t response_pos;
uint8_t * serviceSearchPattern;
uint16_t serviceSearchPatternLen;
#if 1
// sdp_handle_service_search_request
uint16_t nr_services = 1;
request[0] = SDP_ServiceSearchRequest;
net_store_16(request, 1, transactionID++); // transaction ID
serviceSearchPattern = &request[5];
de_create_sequence(serviceSearchPattern);
{
de_add_number(serviceSearchPattern, DE_UUID, DE_SIZE_16, 0x0001);
}
serviceSearchPatternLen = de_get_len(serviceSearchPattern);
net_store_16(request, 5 + serviceSearchPatternLen, 2); // max
request[5 + serviceSearchPatternLen + 2] = 0; // cont
sdp_handle_service_search_request(request, 16);
dump_service_search_response();
memcpy(request + 5 + serviceSearchPatternLen + 2, sdp_response_buffer + 9 + nr_services*4, 3);
sdp_handle_service_search_request(request, remote_mtu);
dump_service_search_response();
#endif
#if 1
// sdp_handle_service_attribute_request
request[0] = SDP_ServiceAttributeRequest;
net_store_16(request, 1, transactionID++); // transaction ID
net_store_32(request, 5, handle_1); // record handle
net_store_16(request, 9, 11); // max bytes
attributeIDList = request + 11;
de_create_sequence(attributeIDList);
de_add_number(attributeIDList, DE_UINT, DE_SIZE_32, 0x0000ffff);
attributeIDListLen = de_get_len(attributeIDList);
request[11+attributeIDListLen] = 0;
response_pos = 0;
while(1) {
sdp_handle_service_attribute_request(request, remote_mtu);
uint16_t attributeListByteCount = READ_NET_16(sdp_response_buffer, 5);
memcpy( &response[response_pos], &sdp_response_buffer[7], attributeListByteCount);
response_pos += attributeListByteCount;
printf("attributeListByteCount %u\n", attributeListByteCount);
printf("Continuation %u\n", sdp_response_buffer[7+attributeListByteCount]);
if (sdp_response_buffer[7+attributeListByteCount] == 0) break;
printf("Continuation {%u}\n", READ_NET_16(sdp_response_buffer, 7+attributeListByteCount+1));
memcpy(request+11+attributeIDListLen, sdp_response_buffer+7+attributeListByteCount, 3);
}
de_dump_data_element(response);
#endif
#if 1
// sdp_handle_service_search_attribute_request
request[0] = SDP_ServiceSearchAttributeRequest;
net_store_16(request, 1, transactionID++); // transaction ID
serviceSearchPattern = &request[5];
de_create_sequence(serviceSearchPattern);
{
de_add_number(serviceSearchPattern, DE_UUID, DE_SIZE_16, 0x0001);
}
serviceSearchPatternLen = de_get_len(serviceSearchPattern);
net_store_16(request, 5 + serviceSearchPatternLen, 11); // MaximumAttributeByteCount:
attributeIDList = request + 5 + serviceSearchPatternLen + 2;
de_create_sequence(attributeIDList);
de_add_number(attributeIDList, DE_UINT, DE_SIZE_32, 0x0000ffff);
attributeIDListLen = de_get_len(attributeIDList);
request[5 + serviceSearchPatternLen + 2 + attributeIDListLen] = 0;
response_pos = 0;
while (1) {
sdp_handle_service_search_attribute_request(request, remote_mtu);
uint16_t attributeListByteCount = READ_NET_16(sdp_response_buffer, 5);
memcpy( &response[response_pos], &sdp_response_buffer[7], attributeListByteCount);
response_pos += attributeListByteCount;
printf("attributeListByteCount %u\n", attributeListByteCount);
printf("Continuation %u\n", sdp_response_buffer[7+attributeListByteCount]);
if (sdp_response_buffer[7+attributeListByteCount] == 0) break;
printf("Continuation {%u,%u}\n", READ_NET_16(sdp_response_buffer, 7+attributeListByteCount+1),
READ_NET_16(sdp_response_buffer, 7+attributeListByteCount+3));
memcpy(request+5 + serviceSearchPatternLen + 2 + attributeIDListLen, sdp_response_buffer+7+attributeListByteCount, 5);
}
de_dump_data_element(response);
#endif
exit(0);
}
