From 11e56ac38c97102bedb32761edd8752aae515093 Mon Sep 17 00:00:00 2001 From: giomba Date: Mon, 23 Dec 2019 18:57:21 +0100 Subject: [PATCH] code polishing --- coppino.h | 2 -- coppino.ino | 54 ----------------------------- ipv6.h | 9 ++++- ipv6.ino | 98 +++++++++++++++++++++++++++++++++++++---------------- 4 files changed, 77 insertions(+), 86 deletions(-) diff --git a/coppino.h b/coppino.h index f399890..3467a27 100644 --- a/coppino.h +++ b/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(); } diff --git a/coppino.ino b/coppino.ino index 1e6dcb9..11c2743 100644 --- a/coppino.ino +++ b/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)); - } } diff --git a/ipv6.h b/ipv6.h index 53e09ed..87b0de7 100644 --- a/ipv6.h +++ b/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 diff --git a/ipv6.ino b/ipv6.ino index a76a8ef..c9fc33f 100644 --- a/ipv6.ino +++ b/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) { - case 128: - /* TODO compute checksum, and assert validity of incoming message */ - /* then... */ + if (type == ICMP_ECHO_REQUEST_TYPE) { /* if it is ICMPv6 Echo Request, then... */ + /* compute checksum, and assert validity of incoming message */ + char received_checksum[2]; + memcpy(received_checksum, packet + 2, 2); - /* Build an echo reply: - * the reply has the same exact format of the request, - * except for Type and Checksum */ - *(packet) = 129; /* echo reply type */ - *(packet + 2) = *(packet + 3) = 0; /* zero-checksum */ + memset(packet + 2, 0, 2); /* zero-checksum */ + uint16_t computed_checksum = compute_checksum(src_addr, dst_addr, NH_ICMP, packet, len); - checksum = coppino::compute_checksum(src_addr, dst_addr, 58, packet, len); - memcpy(packet + 2, &checksum, 2); + 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 */ + *(packet) = ICMP_ECHO_REPLY_TYPE; + memset(packet + 2, 0, 2); /* zero-checksum */ - IPv6Packet reply_packet; - reply_packet.setPayload(packet, len); - reply_packet.setSrcAddress(coppino::address); - dst.deserialize(src_addr); - reply_packet.setDstAddress(dst); - reply_packet.setNextHeader(58); // ICMPv6 - slip::send((void*)&reply_packet, 40 + len); - break; - default: - /* no other types have been implemented */ - break; + uint16_t checksum = compute_checksum(src_addr, dst_addr, NH_ICMP, packet, len); + memcpy(packet + 2, &checksum, 2); + + IPv6Packet reply_packet; + reply_packet.setPayload(packet, len); + + IPv6Addr tmp_addr; + tmp_addr.deserialize(dst_addr); + reply_packet.setSrcAddress(tmp_addr); + tmp_addr.deserialize(src_addr); + reply_packet.setDstAddress(tmp_addr); + reply_packet.setNextHeader(NH_ICMP); + slip::send((void*)&reply_packet, 40 + len); + } } return; }