code polishing

coppino.h

@@ -16,8 +16,6 @@ namespace coppino {
 
   void hexdump(const char* pointer, int len);
 
-  uint16_t compute_checksum(const char* src_addr, const char* dst_addr, uint8_t next_header, const char* payload, int len);
-
   void blink();
 }
 

coppino.ino

@@ -22,58 +22,4 @@ namespace coppino {
       Serial.print('.');
     }
   }
-
-  uint16_t compute_checksum(const char* src_addr, const char* dst_addr, uint8_t next_header, const char* payload, int len) {
-    char buffer[LEN];
-    uint32_t total;
-    uint16_t* ptr;
-    int words;
-    int i = len;
-
-    memcpy(buffer, payload, len);
-    if (len % 2) {
-      buffer[len] = 0;
-      i++;
-    }
-    memcpy(buffer + i, src_addr, 16); i += 16;
-    memcpy(buffer + i, dst_addr, 16); i += 16;
-    memcpy(buffer + i, "\0\0", 2); i += 2;
-    buffer[i] = len >> 8; i += 1;
-    buffer[i] = len & 0xff; i += 1;
-    memcpy(buffer + i, "\0\0\0", 3); i += 3;
-    buffer[i] = next_header; i += 1;
-
-    total = 0;
-    ptr = (uint16_t*)buffer;
-    words = (i + 1) / 2;
-
-    while (words--) total += *ptr++;
-
-    while (total & 0xffff0000)
-      total = (total >> 16) + (total & 0xffff);
-
-    //total -= 64*256; // TODO -- this is an ugly fix, something is wrong somewhere else
-
-    return ~((uint16_t) total);
-    /*
-    uint16_t checksum = 0;
-      for (i = 0; i < len; i = i + 2) {
-        part = ((*(packet + i + 0)) << 8) + *(packet + i + 1);
-        checksum += part;
-      }
-      for (i = 0; i < 16; i = i + 2 ) {
-        part = ((*(src_addr + i + 0)) << 8) + *(src_addr + i + 1);
-        checksum += part;
-        part = ((*(dst_addr + i + 0)) << 8) + *(dst_addr + i + 1);
-        checksum += part;
-      }
-      checksum += (len + 58);  // upper-layer packet length + next header
-      while (checksum >> 16)
-        checksum = (checksum & 0xffff) + (checksum >> 16);
-    */  
-  }
-
-  void blink() {
-    digitalWrite(13, !digitalRead(13));
-  }
 }

ipv6.h

@@ -4,6 +4,11 @@
 namespace ipv6 {
 
   const uint8_t LEN = 128;
+  const uint8_t ICMP_ECHO_REQUEST_TYPE = 128;
+  const uint8_t ICMP_ECHO_REPLY_TYPE   = 129;
+
+  const uint8_t NH_ICMP = 58;
+  const uint8_t NH_UDP  = 17;
 
   class IPv6Addr {
     private:
@@ -15,7 +20,7 @@ namespace ipv6 {
       const char* serialize();
   };
 
-  extern IPv6Addr ALL_NODES_ADDRESS;
+  extern const IPv6Addr ALL_NODES_ADDRESS;
 
   struct ipv6_packet_header {
     char field[4];
@@ -43,6 +48,8 @@ namespace ipv6 {
 
   /* Note: content of message may be changed */
   void handleICMP(const char* src, const char* dst, 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);
 }
 
 #endif

ipv6.ino

@@ -2,7 +2,47 @@
 
 namespace ipv6 {
 /******** costants ********/
-IPv6Addr ALL_NODES_ADDRESS("\xff\x02\x00\x00\x00\x00\x00\x00\x01"); /* ff02::1 */
+const IPv6Addr ALL_NODES_ADDRESS("\xff\x02\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01"); /* ff02::1 */
+
+/******** FUNCTIONS *******/
+uint16_t compute_checksum(const char* src_addr, const char* dst_addr, uint8_t next_header, const char* payload, int len) {
+  char buffer[LEN];
+  uint32_t total;
+  uint16_t* ptr;
+  int words;
+  int i = len;
+
+  /* prepare buffer with all data to checksum:
+      - the actual payload
+      - the pseudo ipv6 header
+  */
+  /* actual payload */
+  memcpy(buffer, payload, len);
+  if (len % 2) {  /* pad with 0 if length is odd */
+    buffer[len] = 0;
+    i++;
+  }
+  /* pseudo ipv6 header -- see RFC 2460, Section 8.1 */
+  memcpy(buffer + i, src_addr, 16); i += 16;
+  memcpy(buffer + i, dst_addr, 16); i += 16;
+  memset(buffer + i, 0, 3); i+= 3;  /* first 3 bytes always 0, because len always < 256 */
+  buffer[i] = len & 0xff; i += 1;
+  memset(buffer + i, 0, 3); i += 3; /* zero, as per-standard */
+  buffer[i] = next_header; i += 1;
+
+  /* do computation */
+  total = 0;
+  ptr = (uint16_t*)buffer;
+  words = (i + 1) / 2;
+
+  while (words--) total += *ptr++;
+
+  while (total & 0xffff0000) {
+    total = (total >> 16) + (total & 0xffff);
+  }
+
+  return ~((uint16_t) total);
+}
 
 /******** IPv6Addr ********/
 IPv6Addr::IPv6Addr() {;}
@@ -57,10 +97,10 @@ void IPv6Packet::setPayload(char* payload, uint8_t len) {
     int len = ((*(packet + 4)) << 8) + (*(packet + 5));
     
     switch(*(packet + 6)) { /* next header */
-      case 58:
+      case NH_ICMP:
         handleICMP(packet + 8, packet + 24, packet + 40, len);
         break;
-      case 17:
+      case NH_UDP:
         // handleUDP(packet + 40, len);
         break;
       default:
@@ -72,36 +112,36 @@ void IPv6Packet::setPayload(char* payload, uint8_t len) {
 
 void handleICMP(const char* src_addr, const char* dst_addr, char* packet, int len) {
   uint8_t type = *(packet);
-  uint32_t checksum = ((*(packet + 2)) << 8) + (*(packet + 3));
-  uint16_t part;
-  IPv6Addr dst;
-  int i;
   
-  switch (type) {
+  if (type == ICMP_ECHO_REQUEST_TYPE) {  /* if it is ICMPv6 Echo Request, then... */
-    case 128:
+      /* compute checksum, and assert validity of incoming message */
-      /* TODO compute checksum, and assert validity of incoming message */
+      char received_checksum[2];
-      /* then... */
+      memcpy(received_checksum, packet + 2, 2);
 
-      /*  Build an echo reply:
+      memset(packet + 2, 0, 2); /* zero-checksum */
-       *  the reply has the same exact format of the request,
+      uint16_t computed_checksum = compute_checksum(src_addr, dst_addr, NH_ICMP, packet, len);
-       *  except for Type and Checksum */
-      *(packet) = 129;  /* echo reply type */
-      *(packet + 2) = *(packet + 3) = 0;  /* zero-checksum */
 
-      checksum = coppino::compute_checksum(src_addr, dst_addr, 58, packet, len);
+      if (memcmp(&computed_checksum, received_checksum, 2) == 0) {  /* if checksum is valid, then... */
-      memcpy(packet + 2, &checksum, 2);
+        /*  Build an echo reply:
+         *  the reply has the same exact format of the request,
+         *  except for Type and Checksum */
+        *(packet) = ICMP_ECHO_REPLY_TYPE;
+        memset(packet + 2, 0, 2); /* zero-checksum */
 
-      IPv6Packet reply_packet;
+        uint16_t checksum = compute_checksum(src_addr, dst_addr, NH_ICMP, packet, len);
-      reply_packet.setPayload(packet, len);
+        memcpy(packet + 2, &checksum, 2);
-      reply_packet.setSrcAddress(coppino::address);
+  
-      dst.deserialize(src_addr);
+        IPv6Packet reply_packet;
-      reply_packet.setDstAddress(dst);
+        reply_packet.setPayload(packet, len);
-      reply_packet.setNextHeader(58); // ICMPv6
+
-      slip::send((void*)&reply_packet, 40 + len);
+        IPv6Addr tmp_addr;
-      break;
+        tmp_addr.deserialize(dst_addr);
-    default:
+        reply_packet.setSrcAddress(tmp_addr);
-      /* no other types have been implemented */
+        tmp_addr.deserialize(src_addr);
-      break;
+        reply_packet.setDstAddress(tmp_addr);
+        reply_packet.setNextHeader(NH_ICMP);
+        slip::send((void*)&reply_packet, 40 + len);
+      }
   }
   return;
 }