From 3d206c0d23dc2a9e6f8ee65c6f6352c078f3bcb4 Mon Sep 17 00:00:00 2001 From: giomba Date: Fri, 24 Dec 2021 17:56:17 +0100 Subject: [PATCH] ipv6: format --- src/ipv6.cpp | 406 ++++++++++++++++++++++++++++----------------------- src/ipv6.h | 90 ++++++------ 2 files changed, 272 insertions(+), 224 deletions(-) diff --git a/src/ipv6.cpp b/src/ipv6.cpp index e4df658..f3f03dc 100644 --- a/src/ipv6.cpp +++ b/src/ipv6.cpp @@ -1,158 +1,190 @@ #include "ipv6.h" #include "udp.h" -namespace ipv6 { -/******** costants ********/ -const IPv6Addr ALL_NODES_ADDRESS("\xff\x02\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01"); /* ff02::1 */ +namespace ipv6 +{ + /******** costants ********/ + const IPv6Addr ALL_NODES_ADDRESS("\xff\x02\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01"); /* ff02::1 */ -/******** variables ********/ -uint8_t HOP_LIMIT = 64; -IPv6Addr LINK_LOCAL_ADDRESS; -IPv6Addr GLOBAL_ADDRESS; -bool slaac_enable; + /******** variables ********/ + uint8_t HOP_LIMIT = 64; + IPv6Addr LINK_LOCAL_ADDRESS; + IPv6Addr GLOBAL_ADDRESS; + bool slaac_enable; -/******** buffers ********/ -IPv6Packet packetin; -IPv6Packet packetout; + /******** buffers ********/ + IPv6Packet packetin; + IPv6Packet packetout; -/******** FUNCTIONS *******/ -void setEUI(const char* eui) { - memset(&LINK_LOCAL_ADDRESS, 0, 16); - memcpy(&LINK_LOCAL_ADDRESS, "\xfe\x80", 2); - memcpy( ((char*)&LINK_LOCAL_ADDRESS) + 8 , eui, 8); -} -void SLAAC(bool enable) { - slaac_enable = enable; -} -void setGlobalAddress(const char* address) { - memcpy(&GLOBAL_ADDRESS, address, 16); -} - -uint16_t compute_checksum(const IPv6Addr* src_addr, const IPv6Addr* dst_addr, uint8_t next_header, int upper_len, 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++; + /******** FUNCTIONS *******/ + void setEUI(const char *eui) + { + memset(&LINK_LOCAL_ADDRESS, 0, 16); + memcpy(&LINK_LOCAL_ADDRESS, "\xfe\x80", 2); + memcpy(((char *)&LINK_LOCAL_ADDRESS) + 8, eui, 8); } - /* 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] = upper_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); + void SLAAC(bool enable) + { + slaac_enable = enable; + } + void setGlobalAddress(const char *address) + { + memcpy(&GLOBAL_ADDRESS, address, 16); } - return ~((uint16_t) total); -} + uint16_t compute_checksum(const IPv6Addr *src_addr, const IPv6Addr *dst_addr, uint8_t next_header, int upper_len, const char *payload, int len) + { + char buffer[LEN]; + uint32_t total; + uint16_t *ptr; + int words; + int i = len; -/******** IPv6Addr ********/ -IPv6Addr::IPv6Addr() {;} + /* 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] = upper_len & 0xff; + i += 1; + memset(buffer + i, 0, 3); + i += 3; /* zero, as per-standard */ + buffer[i] = next_header; + i += 1; -IPv6Addr::IPv6Addr(const char* address) { - memcpy(this->address, address, 16); -} + /* do computation */ + total = 0; + ptr = (uint16_t *)buffer; + words = (i + 1) / 2; -/******** IPv6Packet ********/ -IPv6Packet::IPv6Packet() { - memset(packet, 0, LEN); - *(packet) = 0x60; /* IP version 6 */ - *(packet + 7) = HOP_LIMIT; /* hop limit / ttl */ -} + while (words--) + total += *ptr++; -IPv6Packet::IPv6Packet(const char* buffer, int len) { - memcpy(this->packet, buffer, len); -} + while (total & 0xffff0000) + { + total = (total >> 16) + (total & 0xffff); + } -void IPv6Packet::setNextHeader(uint8_t next_header) { - *(packet + 6) = next_header; -} + return ~((uint16_t)total); + } -void IPv6Packet::setLen(int len) { - *(packet + 4) = len >> 8; - *(packet + 5) = len & 0xff; -} -int IPv6Packet::getLen() { - return ((*(packet + 4)) << 8) + (*(packet + 5)); -} + /******** IPv6Addr ********/ + IPv6Addr::IPv6Addr() { ; } -void IPv6Packet::setSrcAddress(const IPv6Addr* address) { - memcpy(packet + 8, address, 16); -} -void IPv6Packet::setDstAddress(const IPv6Addr* address) { - memcpy(packet + 24, address, 16); -} + IPv6Addr::IPv6Addr(const char *address) + { + memcpy(this->address, address, 16); + } -const IPv6Addr* IPv6Packet::getSrcAddress() { - return (const IPv6Addr*) (packet + 8); -} -const IPv6Addr* IPv6Packet::getDstAddress() { - return (const IPv6Addr*) (packet + 24); -} + /******** IPv6Packet ********/ + IPv6Packet::IPv6Packet() + { + memset(packet, 0, LEN); + *(packet) = 0x60; /* IP version 6 */ + *(packet + 7) = HOP_LIMIT; /* hop limit / ttl */ + } -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); -} + IPv6Packet::IPv6Packet(const char *buffer, int len) + { + memcpy(this->packet, buffer, len); + } -void IPv6Packet::setPayload(char* payload, int len) { - *(packet + 4 + 0) = len >> 8; - *(packet + 4 + 1) = len & 0xff; - memcpy(packet + 40, payload, len); /* payload */ -} + void IPv6Packet::setNextHeader(uint8_t next_header) + { + *(packet + 6) = next_header; + } -void IPv6Packet::send() { - slip::send(packet, 40 + this->getLen()); -} + void IPv6Packet::setLen(int len) + { + *(packet + 4) = len >> 8; + *(packet + 5) = len & 0xff; + } + int IPv6Packet::getLen() + { + return ((*(packet + 4)) << 8) + (*(packet + 5)); + } -void IPv6Packet::doAction() { - /* if destination is my address (global or link-local), or destination is "all nodes" address, then... */ - if ( - (memcmp(&(ALL_NODES_ADDRESS), packet + 24, 16) == 0) || - (memcmp(&(LINK_LOCAL_ADDRESS), packet + 24, 16) == 0) || - (memcmp(&(GLOBAL_ADDRESS), packet + 24, 16) == 0)) { - int ilen = this->getLen(); /* input payload length */ - int olen; /* output payload length */ - - switch(*(packet + 6)) { /* next header */ + void IPv6Packet::setSrcAddress(const IPv6Addr *address) + { + memcpy(packet + 8, address, 16); + } + void IPv6Packet::setDstAddress(const IPv6Addr *address) + { + memcpy(packet + 24, address, 16); + } + + const IPv6Addr *IPv6Packet::getSrcAddress() + { + return (const IPv6Addr *)(packet + 8); + } + const IPv6Addr *IPv6Packet::getDstAddress() + { + return (const IPv6Addr *)(packet + 24); + } + + 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) + { + *(packet + 4 + 0) = len >> 8; + *(packet + 4 + 1) = len & 0xff; + memcpy(packet + 40, payload, len); /* payload */ + } + + void IPv6Packet::send() + { + slip::send(packet, 40 + this->getLen()); + } + + void IPv6Packet::doAction() + { + /* if destination is my address (global or link-local), or destination is "all nodes" address, then... */ + if ( + (memcmp(&(ALL_NODES_ADDRESS), packet + 24, 16) == 0) || + (memcmp(&(LINK_LOCAL_ADDRESS), packet + 24, 16) == 0) || + (memcmp(&(GLOBAL_ADDRESS), packet + 24, 16) == 0)) + { + int ilen = this->getLen(); /* input payload length */ + int olen; /* output payload length */ + + switch (*(packet + 6)) + { /* next header */ case NH_ICMP: - if ((olen = handleICMP((char*)&packetout + 40, packet + 40, ilen)) != -1) { + if ((olen = handleICMP((char *)&packetout + 40, packet + 40, ilen)) != -1) + { packetout.setLen(olen); - packetout.setFlow(((char*)(&packetin)) + 1); + packetout.setFlow(((char *)(&packetin)) + 1); packetout.setSrcAddress(packetin.getDstAddress()); packetout.setDstAddress(packetin.getSrcAddress()); packetout.setNextHeader(NH_ICMP); packetout.send(); - } + } break; case NH_UDP: - if ((olen = udp::handleUDP((char*)&packetout + 40, packet + 40, ilen)) != -1) { + if ((olen = udp::handleUDP((char *)&packetout + 40, packet + 40, ilen)) != -1) + { packetout.setLen(olen); - packetout.setFlow(((char*)(&packetin)) + 1); + packetout.setFlow(((char *)(&packetin)) + 1); packetout.setSrcAddress(packetin.getDstAddress()); packetout.setDstAddress(packetin.getSrcAddress()); packetout.setNextHeader(NH_UDP); @@ -162,72 +194,80 @@ void IPv6Packet::doAction() { default: /* no other protocols are known here, sorry :-) */ break; - } - } /* ...else discard packet */ -} - -/* handles ICMP and (possibly) generate a reply - * returns length of generated packet (written to output_buffer), or -1 if there is no reply packet -*/ -int handleICMP(char* output_buffer, const char* input_buffer, int len) { - uint8_t type = *(input_buffer); - - /* compute checksum, and assert validity of incoming message */ - char received_checksum[2]; - memcpy(received_checksum, input_buffer + 2, 2); - - memcpy(output_buffer, input_buffer, len); - memset(output_buffer + 2, 0, 2); /* zero-checksum */ - uint16_t computed_checksum = compute_checksum( - packetin.getSrcAddress(), - packetin.getDstAddress(), - NH_ICMP, - len, - output_buffer, - len); - - if (memcmp(&computed_checksum, received_checksum, 2) != 0) { /* if checksum is not valid, return */ - return -1; + } + } /* ...else discard packet */ } - if (type == ICMP_ECHO_REQUEST_TYPE) { /* if it is ICMPv6 Echo Request, then... */ - /* Build an echo reply: - * the reply has the same exact format of the request, - * except for Type and Checksum */ - *(output_buffer) = ICMP_ECHO_REPLY_TYPE; - memset(output_buffer + 2, 0, 2); /* zero-checksum */ + /* handles ICMP and (possibly) generate a reply + * returns length of generated packet (written to output_buffer), or -1 if there is no reply packet + */ + int handleICMP(char *output_buffer, const char *input_buffer, int len) + { + uint8_t type = *(input_buffer); - uint16_t checksum = compute_checksum( - packetin.getDstAddress(), - packetin.getSrcAddress(), - NH_ICMP, - len, - output_buffer, - len); - memcpy(output_buffer + 2, &checksum, 2); - return len; - - } else if (type == ICMP_RADV_TYPE && slaac_enable) { /* if it is IVMPc6 Router Advertisement, then... */ - ICMPv6_RADV* radv = (ICMPv6_RADV*)input_buffer; - HOP_LIMIT = (radv->cur_hop_limit != 0) ? radv->cur_hop_limit : 64; - - for ( - ICMPv6_RADV_Option* option = (ICMPv6_RADV_Option*) (radv + 1); - option != input_buffer + len; - option = (ICMPv6_RADV_Option*) ( ((char*)(option)) + option->len )) { - if (option->type == 3 && option->len == 4) { /* if it is a ICMPv6 Prefix Information, then... */ - ICMPv6_RADV_PrefixInformation* p = (ICMPv6_RADV_PrefixInformation*) option; - - if (p->prefix_length != 64) continue; /* only /64 prefix is supported */ - if (! (p->la_flags & 0x40)) continue; /* only SLAAC is supported */ + /* compute checksum, and assert validity of incoming message */ + char received_checksum[2]; + memcpy(received_checksum, input_buffer + 2, 2); + + memcpy(output_buffer, input_buffer, len); + memset(output_buffer + 2, 0, 2); /* zero-checksum */ + uint16_t computed_checksum = compute_checksum( + packetin.getSrcAddress(), + packetin.getDstAddress(), + NH_ICMP, + len, + output_buffer, + len); + + if (memcmp(&computed_checksum, received_checksum, 2) != 0) + { /* if checksum is not valid, return */ + return -1; + } + + if (type == ICMP_ECHO_REQUEST_TYPE) + { /* if it is ICMPv6 Echo Request, then... */ + /* Build an echo reply: + * the reply has the same exact format of the request, + * except for Type and Checksum */ + *(output_buffer) = ICMP_ECHO_REPLY_TYPE; + memset(output_buffer + 2, 0, 2); /* zero-checksum */ + + uint16_t checksum = compute_checksum( + packetin.getDstAddress(), + packetin.getSrcAddress(), + NH_ICMP, + len, + output_buffer, + len); + memcpy(output_buffer + 2, &checksum, 2); + return len; + } + else if (type == ICMP_RADV_TYPE && slaac_enable) + { /* if it is IVMPc6 Router Advertisement, then... */ + ICMPv6_RADV *radv = (ICMPv6_RADV *)input_buffer; + HOP_LIMIT = (radv->cur_hop_limit != 0) ? radv->cur_hop_limit : 64; + + for ( + ICMPv6_RADV_Option *option = (ICMPv6_RADV_Option *)(radv + 1); + option != input_buffer + len; + option = (ICMPv6_RADV_Option *)(((char *)(option)) + option->len)) + { + if (option->type == 3 && option->len == 4) + { /* if it is a ICMPv6 Prefix Information, then... */ + ICMPv6_RADV_PrefixInformation *p = (ICMPv6_RADV_PrefixInformation *)option; + + if (p->prefix_length != 64) + continue; /* only /64 prefix is supported */ + if (!(p->la_flags & 0x40)) + continue; /* only SLAAC is supported */ memcpy(&GLOBAL_ADDRESS, p->prefix, 8); /* set prefix -- leave host bits unchanged */ break; } + } + + return -1; } - return -1; } - return -1; -} } diff --git a/src/ipv6.h b/src/ipv6.h index 39b0347..f697d14 100644 --- a/src/ipv6.h +++ b/src/ipv6.h @@ -4,47 +4,52 @@ #include "Arduino.h" #include "slip.h" -namespace ipv6 { +namespace ipv6 +{ /* maximum length for an IPv6 packet */ const uint8_t LEN = 128; /* ICMP Types */ const uint8_t ICMP_ECHO_REQUEST_TYPE = 128; - const uint8_t ICMP_ECHO_REPLY_TYPE = 129; - const uint8_t ICMP_RADV_TYPE = 134; + const uint8_t ICMP_ECHO_REPLY_TYPE = 129; + const uint8_t ICMP_RADV_TYPE = 134; /* Next-Header Types */ const uint8_t NH_ICMP = 58; - const uint8_t NH_UDP = 17; + const uint8_t NH_UDP = 17; - class IPv6Addr { - private: - char address[16]; - public: - IPv6Addr(); - IPv6Addr(const char* address); + class IPv6Addr + { + private: + char address[16]; + + public: + IPv6Addr(); + IPv6Addr(const char *address); }; extern const IPv6Addr ALL_NODES_ADDRESS; - class IPv6Packet { - private: - char packet[LEN]; - public: - IPv6Packet(); - IPv6Packet(const char* buffer, int len); - void setLen(int len); - int getLen(); - void setSrcAddress(const IPv6Addr* address); - const IPv6Addr* getSrcAddress(); - void setDstAddress(const IPv6Addr* address); - const IPv6Addr* getDstAddress(); - void setNextHeader(uint8_t next_header); - void setFlow(const char* flow); - void setPayload(char* payload, int len); - void send(); + class IPv6Packet + { + private: + char packet[LEN]; - void doAction(); + public: + IPv6Packet(); + IPv6Packet(const char *buffer, int len); + void setLen(int len); + int getLen(); + void setSrcAddress(const IPv6Addr *address); + const IPv6Addr *getSrcAddress(); + void setDstAddress(const IPv6Addr *address); + const IPv6Addr *getDstAddress(); + void setNextHeader(uint8_t next_header); + void setFlow(const char *flow); + void setPayload(char *payload, int len); + void send(); + + void doAction(); }; /* incoming and outgoing packets buffer */ @@ -52,20 +57,21 @@ namespace ipv6 { extern IPv6Packet packetout; /* Note: content of message may be changed */ - int handleICMP(char* output_buffer, const char* input_buffer, int len); + int handleICMP(char *output_buffer, const char *input_buffer, int len); /* Compute internet checksum. Actually it is not so straightforward: */ uint16_t compute_checksum( - const IPv6Addr* src_addr, /* source IPv6 address */ - const IPv6Addr* dst_addr, /* destination IPv6 address */ - uint8_t next_header, /* IPv6 next header number */ - int upper_len, /* length as found in the upper layer protocol */ - const char* payload, /* IPv6 payload */ - int len /* IPv6 payload length */ - ); + const IPv6Addr *src_addr, /* source IPv6 address */ + const IPv6Addr *dst_addr, /* destination IPv6 address */ + uint8_t next_header, /* IPv6 next header number */ + int upper_len, /* length as found in the upper layer protocol */ + const char *payload, /* IPv6 payload */ + int len /* IPv6 payload length */ + ); /* RFC 4861 -- begin */ - struct ICMPv6_RADV { + struct ICMPv6_RADV + { uint8_t type; uint8_t code; char checksum[2]; @@ -76,12 +82,14 @@ namespace ipv6 { uint32_t retrans_time; }; - struct ICMPv6_RADV_Option { + struct ICMPv6_RADV_Option + { uint8_t type; uint8_t len; }; - struct ICMPv6_RADV_PrefixInformation { + struct ICMPv6_RADV_PrefixInformation + { uint8_t type; uint8_t len; uint8_t prefix_length; @@ -93,9 +101,9 @@ namespace ipv6 { }; /* RFC 4861 -- end */ - void setEUI(const char* eui); /* 64bit Extended Unique Identifier */ - void SLAAC(bool enable); /* enable IPv6 automatic prefix configuration (listens for RADV) */ - void setGlobalAddress(const char* address); /* global IPv6 address, manually configured */ + void setEUI(const char *eui); /* 64bit Extended Unique Identifier */ + void SLAAC(bool enable); /* enable IPv6 automatic prefix configuration (listens for RADV) */ + void setGlobalAddress(const char *address); /* global IPv6 address, manually configured */ } #endif