static uint16_t sdp_client_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
big_endian_store_16(data, offset, transactionID);
offset += 2;
// param legnth
offset += 2;
// parameters:
// Service_search_pattern - DES (min 1 UUID, max 12)
uint16_t service_search_pattern_len = de_get_len(service_search_pattern);
memcpy(data + offset, service_search_pattern, service_search_pattern_len);
offset += service_search_pattern_len;
// MaximumAttributeByteCount - uint16_t 0x0007 - 0xffff -> mtu
big_endian_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
big_endian_store_16(data, 3, offset - 5);
return offset;
}
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);
big_endian_store_16(network_buffer, pos, HARDWARE_TYPE_ETHERNET);
pos += 2;
big_endian_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
big_endian_store_16(network_buffer, pos, ARP_OPERATION_REQUEST);
pos += 2;
bd_addr_copy(&network_buffer[pos], local_addr); // Sender Hardware Address (SHA)
pos += 6;
memset(&network_buffer[pos], 0, 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 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);
big_endian_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));
big_endian_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 a2dp_source_setup_media_header(uint8_t * media_packet, int size, int *offset, uint8_t marker, uint16_t sequence_number){
if (size < AVDTP_MEDIA_PAYLOAD_HEADER_SIZE){
log_error("small outgoing buffer");
return;
}
uint8_t rtp_version = 2;
uint8_t padding = 0;
uint8_t extension = 0;
uint8_t csrc_count = 0;
uint8_t payload_type = 0x60;
// uint16_t sequence_number = stream_endpoint->sequence_number;
uint32_t timestamp = btstack_run_loop_get_time_ms();
uint32_t ssrc = 0x11223344;
// rtp header (min size 12B)
int pos = 0;
// int mtu = l2cap_get_remote_mtu_for_local_cid(stream_endpoint->l2cap_media_cid);
media_packet[pos++] = (rtp_version << 6) | (padding << 5) | (extension << 4) | csrc_count;
media_packet[pos++] = (marker << 1) | payload_type;
big_endian_store_16(media_packet, pos, sequence_number);
pos += 2;
big_endian_store_32(media_packet, pos, timestamp);
pos += 4;
big_endian_store_32(media_packet, pos, ssrc); // only used for multicast
pos += 4;
*offset = 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);
big_endian_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);
big_endian_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));
big_endian_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);
big_endian_store_16(ipv4_header, 2, total_length);
uint16_t ipv4_checksum = calc_internet_checksum(ipv4_header, sizeof(ipv4_header));
big_endian_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
big_endian_store_16(udp_header, 0, 5355); // source port
big_endian_store_16(udp_header, 2, 5355); // destination port
big_endian_store_16(udp_header, 4, sizeof(udp_header) + sizeof(llmnr_packet) + sizeof(dns_data));
big_endian_store_16(udp_header, 6, 0); // no checksum
memcpy(&network_buffer[pos], udp_header, sizeof(udp_header));
pos += sizeof(udp_header);
// llmnr packet
memset(llmnr_packet, 0, sizeof(llmnr_packet));
big_endian_store_16(llmnr_packet, 0, 0x1234); // transaction id
big_endian_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 uint16_t sdp_client_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
big_endian_store_16(data, offset, transactionID);
offset += 2;
// param legnth
offset += 2;
// parameters:
// ServiceRecordHandle
big_endian_store_32(data, offset, serviceRecordHandle);
offset += 4;
// MaximumAttributeByteCount - uint16_t 0x0007 - 0xffff -> mtu
big_endian_store_16(data, offset, mtu);
offset += 2;
// AttibuteIDList
uint16_t attribute_id_list_len = de_get_len(attribute_id_list);
memcpy(data + offset, attribute_id_list, attribute_id_list_len);
offset += attribute_id_list_len;
// 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
big_endian_store_16(data, 3, offset - 5);
return offset;
}
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);
big_endian_store_16(ipv4_header, 2, total_length);
uint16_t ipv4_checksum = calc_internet_checksum(ipv4_header, sizeof(ipv4_header));
big_endian_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));
big_endian_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_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;
big_endian_store_16(network_buffer, pos, network_protocol_type);
pos += 2;
return pos;
}
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
memset(llmnr_packet, 0, sizeof(llmnr_packet));
big_endian_store_16(llmnr_packet, 0, 0x1235); // transaction id
big_endian_store_16(llmnr_packet, 4, 1); // one query
// ipv6 header
big_endian_store_16(ipv6_header, 4, payload_length);
// udp header
memset(udp_header, 0, sizeof(udp_header));
big_endian_store_16(udp_header, 0, 5355); // source port
big_endian_store_16(udp_header, 2, 5355); // destination port
big_endian_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)));
big_endian_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);
}
