respond to PTS ICMP Ping

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,14 +395,19 @@ 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:
@@ -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: