2 files changed, 102 insertions, 6 deletions

diff --git a/extras/libmemif/examples/icmp_responder/icmp_proto.c b/extras/libmemif/examples/icmp_responder/icmp_proto.c
index a923f209126..b27abb6a231 100644
--- a/extras/libmemif/examples/icmp_responder/icmp_proto.c
+++ b/extras/libmemif/examples/icmp_responder/icmp_proto.c
@@ -137,8 +137,9 @@ resolve_eth_arp (struct ether_arp *eth_arp, void *eth_arp_resp,
 
   memcpy (resp->arp_sha,
 	  (((struct ether_header *) (eth_arp_resp -
-				     sizeof (struct ether_header)))->
-	   ether_shost), 6);
+				     sizeof (struct
+					     ether_header)))->ether_shost),
+	  6);
 
   memcpy (resp->arp_spa, ip_addr, 4);
 
@@ -172,7 +173,7 @@ resolve_ip (struct iphdr *ip, void *ip_resp, uint8_t ip_addr[4])
   resp->version = 4;
   resp->tos = 0;
   /*len updated later */
-  resp->tot_len = 0x5400;
+  resp->tot_len = 0x0000;
   resp->id = 0;
   resp->frag_off = 0;
   resp->ttl = 0x40;
@@ -183,7 +184,7 @@ resolve_ip (struct iphdr *ip, void *ip_resp, uint8_t ip_addr[4])
   ((uint8_t *) & resp->saddr)[3] = ip_addr[3];
   resp->daddr = ip->saddr;
 
-  resp->check = cksum (resp, sizeof (struct iphdr));
+  /* resp->check =  cksum (resp, sizeof (struct iphdr)); */
 
   return sizeof (struct iphdr);
 }
@@ -192,7 +193,7 @@ static ssize_t
 resolve_icmp (struct icmphdr *icmp, void *icmp_resp)
 {
   struct icmphdr *resp = (struct icmphdr *) icmp_resp;
-  resp->type = ICMP_ECHOREPLY;
+  resp->type = 0x00;
   resp->code = 0;
   resp->un.echo.id = icmp->un.echo.id;
   resp->un.echo.sequence = icmp->un.echo.sequence;
@@ -208,10 +209,13 @@ resolve_packet (void *in_pck, ssize_t in_size,
 {
   struct ether_header *eh;
   struct ether_arp *eah;
-  struct iphdr *ip;
+  struct iphdr *ip, *ip_out;
   struct icmphdr *icmp;
   *out_size = 0;
 
+  if ((in_pck == NULL) || (out_pck == NULL))
+    return -1;
+
   eh = (struct ether_header *) in_pck;
   *out_size = resolve_eth (eh, out_pck);
 
@@ -227,6 +231,7 @@ resolve_packet (void *in_pck, ssize_t in_size,
       print_packet (in_pck + *out_size);
 #endif
       ip = (struct iphdr *) (in_pck + *out_size);
+      ip_out = (struct iphdr *) (out_pck + *out_size);
       *out_size += resolve_ip (ip, out_pck + *out_size, ip_addr);
       if (ip->protocol == 1)
 	{
@@ -240,7 +245,95 @@ resolve_packet (void *in_pck, ssize_t in_size,
 	  memcpy (out_pck + *out_size, in_pck + *out_size,
 		  in_size - *out_size);
 	  *out_size = in_size;
+	  ip_out->tot_len =
+	    __bswap_16 (*out_size - sizeof (struct ether_header));
+	  ip_out->check = cksum (ip_out, sizeof (struct iphdr));
 	}
     }
   return 0;
 }
+
+static ssize_t
+generate_eth (struct ether_header *eh, uint8_t hw_daddr[6])
+{
+  uint8_t hw_addr[6];
+  int i;
+  for (i = 0; i < 6; i++)
+    {
+      hw_addr[i] = 'a';
+    }
+  memcpy (eh->ether_shost, hw_addr, 6);
+  memcpy (eh->ether_dhost, hw_daddr, 6);
+
+  eh->ether_type = 0x0008;
+
+  return sizeof (struct ether_header);
+}
+
+static ssize_t
+generate_ip (struct iphdr *ip, uint8_t saddr[4], uint8_t daddr[4])
+{
+  ip->ihl = 5;
+  ip->version = 4;
+  ip->tos = 0;
+  /*len updated later */
+  ip->tot_len = 0x5400;
+  ip->id = 0;
+  ip->frag_off = 0;
+  ip->ttl = 0x40;
+  ip->protocol = 1;
+  /* saddr */
+  ((uint8_t *) & ip->saddr)[0] = saddr[0];
+  ((uint8_t *) & ip->saddr)[1] = saddr[1];
+  ((uint8_t *) & ip->saddr)[2] = saddr[2];
+  ((uint8_t *) & ip->saddr)[3] = saddr[3];
+  /* daddr */
+  ((uint8_t *) & ip->daddr)[0] = daddr[0];
+  ((uint8_t *) & ip->daddr)[1] = daddr[1];
+  ((uint8_t *) & ip->daddr)[2] = daddr[2];
+  ((uint8_t *) & ip->daddr)[3] = daddr[3];
+
+  ip->check = cksum (ip, sizeof (struct iphdr));
+
+  return sizeof (struct iphdr);
+}
+
+static ssize_t
+generate_icmp (struct icmphdr *icmp, uint32_t seq)
+{
+  icmp->type = ICMP_ECHO;
+  icmp->code = 0;
+  icmp->un.echo.id = 0;
+  icmp->un.echo.sequence = seq;
+
+  return sizeof (struct icmphdr);
+}
+
+int
+generate_packet (void *pck, uint32_t * size, uint8_t saddr[4],
+		 uint8_t daddr[4], uint8_t hw_daddr[6], uint32_t seq)
+{
+  struct ether_header *eh;
+  struct iphdr *ip;
+  struct icmphdr *icmp;
+
+  *size = 0;
+
+  eh = (struct ether_header *) pck;
+  *size += generate_eth (eh, hw_daddr);
+
+  ip = (struct iphdr *) (pck + *size);
+  *size += generate_ip (ip, saddr, daddr);
+
+  icmp = (struct icmphdr *) (pck + *size);
+  *size += generate_icmp (icmp, seq);
+
+  ((struct icmphdr *) (pck + *size - sizeof (struct icmphdr)))->checksum =
+    cksum (pck + *size - sizeof (struct icmphdr), sizeof (struct icmphdr));
+
+  ip->tot_len = __bswap_16 (*size - sizeof (struct ether_header));
+  ip->check = 0;
+  ip->check = cksum (ip, sizeof (struct iphdr));
+
+  return 0;
+}
diff --git a/extras/libmemif/examples/icmp_responder/icmp_proto.h b/extras/libmemif/examples/icmp_responder/icmp_proto.h
index f2f22ac4629..ca72b2fe026 100644
--- a/extras/libmemif/examples/icmp_responder/icmp_proto.h
+++ b/extras/libmemif/examples/icmp_responder/icmp_proto.h
@@ -21,6 +21,9 @@
 int resolve_packet (void *in_pck, ssize_t in_size, void *out_pck,
 		    uint32_t * out_size, uint8_t ip_addr[4]);
 
+int generate_packet (void *pck, uint32_t * size, uint8_t saddr[4],
+		     uint8_t daddr[4], uint8_t hw_daddr[6], uint32_t seq);
+
 int print_packet (void *pck);
 
 #endif /* _ICMP_PROTO_H_ */