void prepare_packet(void){
int i;
counter = 0;
net_store_32( packet, 0, 0);
for (i=4;i<PACKET_SIZE;i++)
packet[i] = i-4;
}
static uint16_t setup_service_attribute_request(uint8_t * data){
uint16_t offset = 0;
transactionID++;
// uint8_t SDP_PDU_ID_t.SDP_ServiceSearchRequest;
data[offset++] = SDP_ServiceAttributeRequest;
// uint16_t transactionID
net_store_16(data, offset, transactionID);
offset += 2;
// param legnth
offset += 2;
// parameters:
// ServiceRecordHandle
net_store_32(data, offset, serviceRecordHandle);
offset += 4;
// MaximumAttributeByteCount - uint16_t 0x0007 - 0xffff -> mtu
net_store_16(data, offset, mtu);
offset += 2;
// AttibuteIDList
uint16_t attributeIDListLen = de_get_len(attributeIDList);
memcpy(data + offset, attributeIDList, attributeIDListLen);
offset += attributeIDListLen;
// ContinuationState - uint8_t number of cont. bytes N<=16
data[offset++] = continuationStateLen;
// - N-bytes previous response from server
memcpy(data + offset, continuationState, continuationStateLen);
offset += continuationStateLen;
// uint16_t paramLength
net_store_16(data, 3, offset - 5);
return offset;
}
int sdp_handle_service_search_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);
uint8_t * serviceSearchPattern = &packet[5];
uint16_t serviceSearchPatternLen = de_get_len(serviceSearchPattern);
uint16_t maximumServiceRecordCount = READ_NET_16(packet, 5 + serviceSearchPatternLen);
uint8_t * continuationState = &packet[5+serviceSearchPatternLen+2];
// calc maxumumServiceRecordCount based on remote MTU
uint16_t maxNrServiceRecordsPerResponse = (remote_mtu - (9+3))/4;
// continuation state contains index of next service record to examine
int continuation = 0;
uint16_t continuation_index = 0;
if (continuationState[0] == 2) {
continuation_index = READ_NET_16(continuationState, 1);
}
// get and limit total count
linked_item_t *it;
uint16_t total_service_count = 0;
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;
total_service_count++;
}
if (total_service_count > maximumServiceRecordCount) {
total_service_count = maximumServiceRecordCount;
}
// ServiceRecordHandleList at 9
uint16_t pos = 9;
uint16_t current_service_count = 0;
uint16_t current_service_index = 0;
uint16_t matching_service_count = 0;
for (it = (linked_item_t *) sdp_service_records; it ; it = it->next, ++current_service_index) {
service_record_item_t * item = (service_record_item_t *) it;
if (!sdp_record_matches_service_search_pattern(item->service_record, serviceSearchPattern)) continue;
matching_service_count++;
if (current_service_index < continuation_index) continue;
net_store_32(sdp_response_buffer, pos, item->service_record_handle);
pos += 4;
current_service_count++;
if (matching_service_count >= total_service_count) break;
if (current_service_count >= maxNrServiceRecordsPerResponse) {
continuation = 1;
continuation_index = current_service_index + 1;
break;
}
}
// Store continuation state
if (continuation) {
sdp_response_buffer[pos++] = 2;
net_store_16(sdp_response_buffer, pos, continuation_index);
pos += 2;
} else {
sdp_response_buffer[pos++] = 0;
}
// header
sdp_response_buffer[0] = SDP_ServiceSearchResponse;
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, total_service_count);
net_store_16(sdp_response_buffer, 7, current_service_count);
return pos;
}
void update_packet(void){
net_store_32( packet, 0, counter++);
}
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);
}
