diff --git a/test/pts/bnep_test.c b/test/pts/bnep_test.c index 5159539a5..1f2323d3e 100644 --- a/test/pts/bnep_test.c +++ b/test/pts/bnep_test.c @@ -66,6 +66,11 @@ #include "pan.h" #include "stdin_support.h" +#define NETWORK_TYPE_IPv4 0x800 +#define NETWORK_TYPE_IPv6 0x86DD +#define ICMP_TYPE_PING_REQUEST 0x08 +#define ICMP_TYPE_PING_RESPONSE 0x00 + // prototypes static void show_usage(); @@ -134,7 +139,7 @@ static void send_buffer(uint16_t pos){ } static void send_ethernet_packet(int src_compressed, int dst_compressed){ - int pos = setup_ethernet_header(src_compressed, dst_compressed, 0, 0x0800); // IPv4 + int pos = setup_ethernet_header(src_compressed, dst_compressed, 0, NETWORK_TYPE_IPv4); // IPv4 // dummy data Ethernet packet int i; for (i = 60; i >= 0 ; i--){ @@ -178,7 +183,7 @@ static void send_arp_probe_ipv4(void){ net_store_16(network_buffer, pos, 0x0001); // Hardware type (HTYPE), 1 = Ethernet pos += 2; - net_store_16(network_buffer, pos, 0x0800); // Protocol type (PTYPE), 0x800 = IPv4 + net_store_16(network_buffer, pos, NETWORK_TYPE_IPv4); // Protocol type (PTYPE), 0x800 = IPv4 pos += 2; network_buffer[pos++] = 6; // Hardware length (HLEN) - 6 MAC Address network_buffer[pos++] = 4; // Protocol length (PLEN) - 4 IPv4 Address @@ -220,7 +225,7 @@ static void send_some_ipv6_packet(void){ bd_addr_t an_addr = { 0x33, 0x33, 0x00, 0x00, 0x00, 0x16}; memcpy(other_addr, an_addr, 6); - int pos = setup_ethernet_header(1, 0, 0, 0x86DD); // IPv6 + int pos = setup_ethernet_header(1, 0, 0, NETWORK_TYPE_IPv6); // IPv6 uint8_t ipv6_packet[] = { 0x60, 0x00, 0x00, 0x00, 0x00, 0x24, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0x02, @@ -239,7 +244,7 @@ static void send_some_ipv6_packet_2(void){ bd_addr_t an_addr = { 0x33, 0x33, 0xFF, 0x60, 0x7B, 0x87}; memcpy(other_addr, an_addr, 6); - int pos = setup_ethernet_header(1, 0, 0, 0x86DD); // IPv6 + int pos = setup_ethernet_header(1, 0, 0, NETWORK_TYPE_IPv6); // IPv6 uint8_t ipv6_packet[] = { 0x60, 0x00, 0x00, 0x00, 0x00, 0x18, 0x3a, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0x02, @@ -252,6 +257,60 @@ static void send_some_ipv6_packet_2(void){ send_buffer(pos); } +static uint16_t calc_internet_checksum(uint8_t * data, int size){ + uint32_t checksum = 0; + while (size){ + // add 16-bit value + checksum += *(uint16_t*)data; + data += 2; + size -= 2; + // handle end-aroud-carry + while (checksum > 0xffff){ + checksum = (checksum & 0xffff) + 1; + } + } + return checksum; +} + +static void send_ping_response(bd_addr_t src_addr, bd_addr_t dst_addr){ + + uint8_t ipv4_packet[] = { + // 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_IPv6); // IPv6 + + // ipv4 + int total_length = sizeof(ipv4_packet) + sizeof(icmp_packet); + net_store_16(ipv4_packet, 2, total_length); + uint16_t ipv4_checksum = calc_internet_checksum(ipv4_packet, sizeof(ipv4_packet)); + net_store_16(ipv4_packet, 10, ipv4_checksum); + // TODO: also set src/dest ip address + memcpy(&network_buffer[pos], ipv4_packet, sizeof(ipv4_packet)); + pos += sizeof(ipv4_packet); + + // 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 show_usage(void){ printf("\n--- Bluetooth BNEP Test Console ---\n"); @@ -336,15 +395,20 @@ static int stdin_process(struct data_source *ds){ } /*************** PANU client routines *********************/ - static void packet_handler (void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) { uint8_t event; bd_addr_t event_addr; + bd_addr_t src_addr; + bd_addr_t dst_addr; uint16_t uuid_source; uint16_t uuid_dest; uint16_t mtu; - + uint16_t network_type; + uint8_t icmp_type; + int ihl; + int payload_offset; + switch (packet_type) { case HCI_EVENT_PACKET: event = packet[0]; @@ -408,12 +472,34 @@ static void packet_handler (void * connection, uint8_t packet_type, uint16_t cha break; case BNEP_DATA_PACKET: // show received packet on console + // bt_flip_addr(dst_addr, &packet[0]); + // bt_flip_addr(src_addr, &packet[6]); + memcpy(dst_addr, &packet[0], 6); + memcpy(src_addr, &packet[6], 6); + network_type = READ_NET_16(packet, 12); printf("BNEP packet received\n"); printf("Dst Addr: %s\n", bd_addr_to_str(&packet[0])); printf("Src Addr: %s\n", bd_addr_to_str(&packet[6])); - printf("Net Type: %04x\n", READ_NET_16(packet, 12)); + printf("Net Type: %04x\n", network_type); // ignore the next 60 bytes hexdumpf(&packet[74], size - 74); + switch (network_type){ + case NETWORK_TYPE_IPv4: + ihl = packet[14] & 0x0f; + payload_offset = 14 + (ihl << 2); + // + icmp_type = packet[payload_offset]; + hexdumpf(&packet[payload_offset], size - payload_offset); + printf("ICMP packet of type %x\n", icmp_type); + switch (icmp_type){ + case ICMP_TYPE_PING_REQUEST: + printf("IPv4 Ping Request received, sending pong\n"); + send_ping_response(src_addr, dst_addr); + break; + break; + } + } + break; default: