static void send_arp_probe_ipv4(void){
// "random address"
static uint8_t requested_address[4] = {169, 254, 1, 0};
requested_address[3]++;
int pos = setup_ethernet_header(1, 0, 1, NETWORK_TYPE_IPv4);
net_store_16(network_buffer, pos, HARDWARE_TYPE_ETHERNET);
pos += 2;
net_store_16(network_buffer, pos, NETWORK_TYPE_IPv4);
pos += 2;
network_buffer[pos++] = 6; // Hardware length (HLEN) - 6 MAC Address
network_buffer[pos++] = 4; // Protocol length (PLEN) - 4 IPv4 Address
net_store_16(network_buffer, pos, ARP_OPERATION_REQUEST);
pos += 2;
BD_ADDR_COPY(&network_buffer[pos], local_addr); // Sender Hardware Address (SHA)
pos += 6;
bzero(&network_buffer[pos], 4); // Sender Protocol Adress (SPA)
pos += 4;
BD_ADDR_COPY(&network_buffer[pos], other_addr); // Target Hardware Address (THA) (ignored for requests)
pos += 6;
memcpy(&network_buffer[pos], requested_address, 4);
pos += 4;
// magically, add some extra bytes for Ethernet padding
pos += 18;
send_buffer(pos);
}
static int sdp_create_error_response(uint16_t transaction_id, uint16_t error_code) {
sdp_response_buffer[0] = SDP_ErrorResponse;
net_store_16(sdp_response_buffer, 1, transaction_id);
net_store_16(sdp_response_buffer, 3, 2);
net_store_16(sdp_response_buffer, 5, error_code); // invalid syntax
return 7;
}
static void sdpc_trysend()
{
uint8_t buf[128];
if (state != SENDING) return;
if (!l2cap_cid) return;
if (!l2cap_can_send_packet_now(l2cap_cid)) return;
buf[0] = SDP_ServiceSearchAttributeRequest;
net_store_16(buf, 1, transitionid++);
uint8_t *param = &buf[5];
de_create_sequence(param);
de_add_number(param, DE_UUID, DE_SIZE_16, serviceids[current_server]);
uint16_t size = de_get_len(param);
net_store_16(param, size, 30); // max length is 30 bytes
size+=2;
de_create_sequence(param + size);
//de_add_number(param + size, DE_UINT, DE_SIZE_16, SDP_ServiceClassIDList);
de_add_number(param + size, DE_UINT, DE_SIZE_16, SDP_ProtocolDescriptorList);
size += de_get_len(param + size);
param[size++] = 0;
net_store_16(buf, 3, size);
hexdump(buf, size + 5);
int err = l2cap_send_internal(l2cap_cid, buf, size + 5);
if (!err)
{
state = RECV;
log_info("sdp request sent.\n");
}
else
{
log_info("sdpc_trysend l2cap_send_internal error: %d\n", err);
}
}
static uint16_t setup_service_search_request(uint8_t * data){
uint16_t offset = 0;
transactionID++;
// uint8_t SDP_PDU_ID_t.SDP_ServiceSearchRequest;
data[offset++] = SDP_ServiceSearchRequest;
// uint16_t transactionID
net_store_16(data, offset, transactionID);
offset += 2;
// param legnth
offset += 2;
// parameters:
// ServiceSearchPattern - DES (min 1 UUID, max 12)
uint16_t serviceSearchPatternLen = de_get_len(serviceSearchPattern);
memcpy(data + offset, serviceSearchPattern, serviceSearchPatternLen);
offset += serviceSearchPatternLen;
// MaximumAttributeByteCount - uint16_t 0x0007 - 0xffff -> mtu
net_store_16(data, offset, mtu);
offset += 2;
// 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;
}
static void send_ndp_probe_ipv6(void){
uint8_t ipv6_header[] = {
// ip
0x60, 0x00, 0x00, 0x00, // version (6) + traffic class (8) + flow label (24)
0x00, 0x00, 58, 0x01, // payload length(16), next header = IPv6-ICMP, hop limit
0x00, 0x00, 0x00, 0x00, // source IP address
0x00, 0x00, 0x00, 0x00, // source IP address
0x00, 0x00, 0x00, 0x00, // source IP address
0x00, 0x00, 0x00, 0x00, // source IP address
0xfe, 0x80, 0x00, 0x00, // destination IP address
0x00, 0x00, 0x00, 0x00, // destination IP address
0x00, 0x00, 0x00, 0x00, // destination IP address
0x00, 0x00, 0x00, 0x00, // destination IP address
};
uint8_t icmp_packet[] = {
// icmp
0x87, 0x00, 0x00, 0x00, // type: 0x80 PING Request, code = 0, checksum(16)
0x00, 0x00, 0x00, 0x00 // message
};
// ethernet header
int pos = setup_ethernet_header(1, 0, 0, NETWORK_TYPE_IPv6);
// ipv6
int payload_length = sizeof(icmp_packet);
net_store_16(ipv6_header, 4, payload_length);
// source address ::
// dest addresss - Modified EUI-64
// ipv6_header[24..31] = FE80::
ipv6_header[32] = local_addr[0] ^ 0x2;
ipv6_header[33] = local_addr[1];
ipv6_header[34] = local_addr[2];
ipv6_header[35] = 0xff;
ipv6_header[36] = 0xfe;
ipv6_header[37] = local_addr[3];
ipv6_header[38] = local_addr[4];
ipv6_header[39] = local_addr[5];
int checksum = calc_internet_checksum(&ipv6_header[8], 32);
checksum = sum_ones_complement(checksum, payload_length);
checksum = sum_ones_complement(checksum, ipv6_header[6] << 8);
memcpy(&network_buffer[pos], ipv6_header, sizeof(ipv6_header));
pos += sizeof(ipv6_header);
// icmp
uint16_t icmp_checksum = calc_internet_checksum(icmp_packet, sizeof(icmp_packet));
net_store_16(icmp_packet, 2, icmp_checksum);
memcpy(&network_buffer[pos], icmp_packet, sizeof(icmp_packet));
pos += sizeof(icmp_packet);
// send
send_buffer(pos);
}
/* Untested */
static void send_ping_request_ipv6(void){
uint8_t ipv6_header[] = {
// ip
0x60, 0x00, 0x00, 0x00, // version (4) + traffic class (8) + flow label (24)
0x00, 0x00, 58, 0x01, // payload length(16), next header = IPv6-ICMP, hop limit
0x00, 0x00, 0x00, 0x00, // source IP address
0x00, 0x00, 0x00, 0x00, // source IP address
0x00, 0x00, 0x00, 0x00, // source IP address
0x00, 0x00, 0x00, 0x00, // source IP address
0x00, 0x00, 0x00, 0x00, // destination IP address
0x00, 0x00, 0x00, 0x00, // destination IP address
0x00, 0x00, 0x00, 0x00, // destination IP address
0x00, 0x00, 0x00, 0x00, // destination IP address
};
uint8_t icmp_packet[] = {
// icmp
0x80, 0x00, 0x00, 0x00, // type: 0x80 PING Request, codde = 0, checksum(16)
0x00, 0x00, 0x00, 0x00 // message
};
// ethernet header
int pos = setup_ethernet_header(1, 0, 0, NETWORK_TYPE_IPv4); // IPv4
// ipv6
int payload_length = sizeof(icmp_packet);
net_store_16(ipv6_header, 4, payload_length);
// TODO: also set src/dest ip address
int checksum = calc_internet_checksum(&ipv6_header[8], 32);
checksum = sum_ones_complement(checksum, payload_length);
checksum = sum_ones_complement(checksum, 58 << 8);
net_store_16(icmp_packet, 2, checksum);
memcpy(&network_buffer[pos], ipv6_header, sizeof(ipv6_header));
pos += sizeof(ipv6_header);
// icmp
uint16_t icmp_checksum = calc_internet_checksum(icmp_packet, sizeof(icmp_packet));
net_store_16(icmp_packet, 2, icmp_checksum);
memcpy(&network_buffer[pos], icmp_packet, sizeof(icmp_packet));
pos += sizeof(icmp_packet);
// send
send_buffer(pos);
}
static void send_llmnr_request_ipv4(void){
uint8_t ipv4_header[] = {
0x45, 0x00, 0x00, 0x00, // version + ihl, dscp } ecn, total len
0x00, 0x00, 0x00, 0x00, // identification (16), flags + fragment offset
0x01, 0x11, 0x00, 0x00, // time to live, procotol: UDP, checksum (16),
192, 168, 167, 152, // source IP address
224, 0, 0, 252, // destination IP address
};
uint8_t udp_header[8];
uint8_t llmnr_packet[12];
uint8_t dns_data[] = { 0x08, 0x61, 0x61, 0x70, 0x70, 0x6c, 0x65, 0x74, 0x76,
0x05, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x00, 0x00,
0x01, 0x00, 0x01 };
// ethernet header
int pos = setup_ethernet_header(1, 0, 0, NETWORK_TYPE_IPv4); // IPv4
// ipv4
int total_length = sizeof(ipv4_header) + sizeof(udp_header) + sizeof (llmnr_packet) + sizeof(dns_data);
net_store_16(ipv4_header, 2, total_length);
uint16_t ipv4_checksum = calc_internet_checksum(ipv4_header, sizeof(ipv4_header));
net_store_16(ipv4_header, 10, ~ipv4_checksum);
// TODO: also set src/dest ip address
memcpy(&network_buffer[pos], ipv4_header, sizeof(ipv4_header));
pos += sizeof(ipv4_header);
// udp packet
net_store_16(udp_header, 0, 5355); // source port
net_store_16(udp_header, 2, 5355); // destination port
net_store_16(udp_header, 4, sizeof(udp_header) + sizeof(llmnr_packet) + sizeof(dns_data));
net_store_16(udp_header, 6, 0); // no checksum
memcpy(&network_buffer[pos], udp_header, sizeof(udp_header));
pos += sizeof(udp_header);
// llmnr packet
bzero(llmnr_packet, sizeof(llmnr_packet));
net_store_16(llmnr_packet, 0, 0x1234); // transaction id
net_store_16(llmnr_packet, 4, 1); // one query
memcpy(&network_buffer[pos], llmnr_packet, sizeof(llmnr_packet));
pos += sizeof(llmnr_packet);
memcpy(&network_buffer[pos], dns_data, sizeof(dns_data));
pos += sizeof(dns_data);
// send
send_buffer(pos);
}
static void send_ping_response_ipv4(void){
uint8_t ipv4_header[] = {
// ip
0x45, 0x00, 0x00, 0x00, // version + ihl, dscp } ecn, total len
0x00, 0x00, 0x00, 0x00, // identification (16), flags + fragment offset
0x01, 0x01, 0x00, 0x00, // time to live, procotol: icmp, checksum (16),
0x00, 0x00, 0x00, 0x00, // source IP address
0x00, 0x00, 0x00, 0x00, // destination IP address
};
uint8_t icmp_packet[] = {
// icmp
0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00
};
// ethernet header
int pos = setup_ethernet_header(1, 0, 0, NETWORK_TYPE_IPv4); // IPv4
// ipv4
int total_length = sizeof(ipv4_header) + sizeof(icmp_packet);
net_store_16(ipv4_header, 2, total_length);
uint16_t ipv4_checksum = calc_internet_checksum(ipv4_header, sizeof(ipv4_header));
net_store_16(ipv4_header, 10, ipv4_checksum);
// TODO: also set src/dest ip address
memcpy(&network_buffer[pos], ipv4_header, sizeof(ipv4_header));
pos += sizeof(ipv4_header);
// icmp
uint16_t icmp_checksum = calc_internet_checksum(icmp_packet, sizeof(icmp_packet));
net_store_16(icmp_packet, 2, icmp_checksum);
memcpy(&network_buffer[pos], icmp_packet, sizeof(icmp_packet));
pos += sizeof(icmp_packet);
// send
send_buffer(pos);
}
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;
}
static uint16_t setup_ethernet_header(int src_compressed, int dst_compressed, int broadcast, uint16_t network_protocol_type){
// setup packet
int pos = 0;
// destination
if (broadcast){
BD_ADDR_COPY(&network_buffer[pos], broadcast_addr);
} else {
BD_ADDR_COPY(&network_buffer[pos], dst_compressed ? pts_addr : other_addr);
}
pos += 6;
// source
BD_ADDR_COPY(&network_buffer[pos], src_compressed ? local_addr : other_addr);
pos += 6;
net_store_16(network_buffer, pos, network_protocol_type);
pos += 2;
return pos;
}
void sdp_query_rfcomm_channel_and_name_for_uuid(bd_addr_t remote, uint16_t uuid){
net_store_16(des_serviceSearchPattern, 3, uuid);
sdp_query_rfcomm_channel_and_name_for_search_pattern(remote, (uint8_t*)des_serviceSearchPattern);
}
static void send_llmnr_request_ipv6(void){
// https://msdn.microsoft.com/en-us/library/dd240361.aspx
uint8_t ipv6_header[] = {
0x60, 0x00, 0x00, 0x00, // version (6) + traffic class (8) + flow label (24)
0x00, 0x00, 17, 0x01, // payload length(16), next header = UDP, hop limit
0xfe, 0x80, 0x00, 0x00, // source IP address
0x00, 0x00, 0x00, 0x00, // source IP address
0xd9, 0xf6, 0xce, 0x2e, // source IP address
0x48, 0x75, 0xab, 0x03, // source IP address
0xff, 0x02, 0x00, 0x00, // destination IP address
0x00, 0x00, 0x00, 0x00, // destination IP address
0x00, 0x00, 0x00, 0x00, // destination IP address
0x00, 0x01, 0x00, 0x03, // destination IP address
};
uint8_t udp_header[8];
uint8_t llmnr_packet[12];
uint8_t dns_data[] = { 0x08, 0x61, 0x61, 0x70, 0x70, 0x6c, 0x65, 0x74, 0x76,
0x05, 0x6c, 0x6f, 0x63, 0x61, 0x6c, 0x00, 0x00,
0x01, 0x00, 0x01 };
int payload_length = sizeof(udp_header) + sizeof(llmnr_packet) + sizeof(dns_data);
// llmnr header
bzero(llmnr_packet, sizeof(llmnr_packet));
net_store_16(llmnr_packet, 0, 0x1235); // transaction id
net_store_16(llmnr_packet, 4, 1); // one query
// ipv6 header
net_store_16(ipv6_header, 4, payload_length);
// udp header
bzero(udp_header, sizeof(udp_header));
net_store_16(udp_header, 0, 5355); // source port
net_store_16(udp_header, 2, 5355); // destination port
net_store_16(udp_header, 4, payload_length);
int checksum = calc_internet_checksum(&ipv6_header[8], 32);
checksum = sum_ones_complement(checksum, payload_length); // payload len
checksum = sum_ones_complement(checksum, ipv6_header[6] << 8); // next header
checksum = sum_ones_complement(checksum, calc_internet_checksum(udp_header, sizeof(udp_header)));
checksum = sum_ones_complement(checksum, calc_internet_checksum(llmnr_packet, sizeof(llmnr_packet)));
checksum = sum_ones_complement(checksum, calc_internet_checksum(dns_data, sizeof(dns_data)));
net_store_16(udp_header, 6, ~checksum);
// ethernet header
int pos = setup_ethernet_header(1, 0, 1, NETWORK_TYPE_IPv6); // IPv6
memcpy(&network_buffer[pos], ipv6_header, sizeof(ipv6_header));
pos += sizeof(ipv6_header);
memcpy(&network_buffer[pos], udp_header, sizeof(udp_header));
pos += sizeof(udp_header);
memcpy(&network_buffer[pos], llmnr_packet, sizeof(llmnr_packet));
pos += sizeof(llmnr_packet);
memcpy(&network_buffer[pos], dns_data, sizeof(dns_data));
pos += sizeof(dns_data);
// send
send_buffer(pos);
}
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;
}
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 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);
}