From fb79e3d371895c02dc7be00de2078f64a0833a42 Mon Sep 17 00:00:00 2001
From: giomba <giomba@glgprograms.it>
Date: Mon, 23 Dec 2019 21:47:25 +0100
Subject: [PATCH] added proper IPv6 flow reply for ICMP packets

---
 ipv6.h   |  3 ++-
 ipv6.ino | 23 ++++++++++++++++-------
 2 files changed, 18 insertions(+), 8 deletions(-)

diff --git a/ipv6.h b/ipv6.h
index 0a99246..1369bff 100644
--- a/ipv6.h
+++ b/ipv6.h
@@ -38,6 +38,7 @@ namespace ipv6 {
       void setSrcAddress(IPv6Addr& address);
       void setDstAddress(IPv6Addr& address);
       void setNextHeader(uint8_t next_header);
+      void setFlow(const char* flow);
       void setPayload(char* payload, int len);
       
       char* serialize();
@@ -47,7 +48,7 @@ namespace ipv6 {
   };
 
   /* Note: content of message may be changed */
-  void handleICMP(const char* src, const char* dst, char* message, int len);
+  void handleICMP(const char* src, const char* dst, const char* flow, char* message, int len);
 
   uint16_t compute_checksum(const char* src_addr, const char* dst_addr, uint8_t next_header, const char* payload, int len);
 }
diff --git a/ipv6.ino b/ipv6.ino
index 0bfc1bb..2762ed0 100644
--- a/ipv6.ino
+++ b/ipv6.ino
@@ -79,6 +79,14 @@ void IPv6Packet::setDstAddress(IPv6Addr& address) {
   memcpy(packet + 24, (void*)&address, 16);
 }
 
+void IPv6Packet::setFlow(const char* flow) {
+  /* three bytes
+    4 most significant bits are ignored
+    because only 20 bits can be used */
+  *(packet + 1) = (*(packet + 1)) | (flow[0] & 0x0f); /* pay attention to TC field */
+  memcpy(packet + 2, flow + 1, 2);
+}
+
 void IPv6Packet::setPayload(char* payload, int len) {
     memset(packet, 0, LEN);
     
@@ -96,7 +104,7 @@ void IPv6Packet::setPayload(char* payload, int len) {
     
     switch(*(packet + 6)) { /* next header */
       case NH_ICMP:
-        handleICMP(packet + 8, packet + 24, packet + 40, len);
+        handleICMP(packet + 8, packet + 24, packet + 1, packet + 40, len);
         break;
       case NH_UDP:
         // handleUDP(packet + 40, len);
@@ -108,7 +116,7 @@ void IPv6Packet::setPayload(char* payload, int len) {
   }
  }
 
-void handleICMP(const char* src_addr, const char* dst_addr, char* packet, int len) {
+void handleICMP(const char* src_addr, const char* dst_addr, const char* flow, char* packet, int len) {
   uint8_t type = *(packet);
   
   if (type == ICMP_ECHO_REQUEST_TYPE) {  /* if it is ICMPv6 Echo Request, then... */
@@ -119,7 +127,7 @@ void handleICMP(const char* src_addr, const char* dst_addr, char* packet, int le
       memset(packet + 2, 0, 2); /* zero-checksum */
       uint16_t computed_checksum = compute_checksum(src_addr, dst_addr, NH_ICMP, packet, len);
 
-      if (memcmp(&computed_checksum, received_checksum, 2) == 0) {  /* if checksum is valid, then... */
+      //if (memcmp(&computed_checksum, received_checksum, 2) == 0) {  /* if checksum is valid, then... */
         /*  Build an echo reply:
          *  the reply has the same exact format of the request,
          *  except for Type and Checksum */
@@ -131,15 +139,16 @@ void handleICMP(const char* src_addr, const char* dst_addr, char* packet, int le
   
         IPv6Packet reply_packet;
         reply_packet.setPayload(packet, len);
-
+        reply_packet.setFlow(flow);
+  
         IPv6Addr tmp_addr;
-        tmp_addr.deserialize(dst_addr);
+        memcpy(&tmp_addr, dst_addr, 16);
         reply_packet.setSrcAddress(tmp_addr);
-        tmp_addr.deserialize(src_addr);
+        memcpy(&tmp_addr, src_addr, 16);
         reply_packet.setDstAddress(tmp_addr);
         reply_packet.setNextHeader(NH_ICMP);
         slip::send((void*)&reply_packet, 40 + len);
-      }
+      //}
   }
   return;
 }