#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 */ /******** variables ********/ uint8_t HOP_LIMIT = 64; IPv6Addr LINK_LOCAL_ADDRESS; IPv6Addr GLOBAL_ADDRESS; /******** 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 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++; } /* 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); } return ~((uint16_t)total); } /******** IPv6Addr ********/ IPv6Addr::IPv6Addr() { ; } IPv6Addr::IPv6Addr(const char *address) { memcpy(this->address, address, 16); } /******** IPv6Packet ********/ IPv6Packet::IPv6Packet() { memset(packet, 0, LEN); *(packet) = 0x60; /* IP version 6 */ *(packet + 7) = HOP_LIMIT; /* hop limit / ttl */ } IPv6Packet::IPv6Packet(const char *buffer, int len) { memcpy(this->packet, buffer, len); } void IPv6Packet::setNextHeader(uint8_t next_header) { *(packet + 6) = next_header; } void IPv6Packet::setLen(int len) { *(packet + 4) = len >> 8; *(packet + 5) = len & 0xff; } int IPv6Packet::getLen() { return ((*(packet + 4)) << 8) + (*(packet + 5)); } 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) { packetout.setLen(olen); 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) { packetout.setLen(olen); packetout.setFlow(((char *)(&packetin)) + 1); packetout.setSrcAddress(packetin.getDstAddress()); packetout.setDstAddress(packetin.getSrcAddress()); packetout.setNextHeader(NH_UDP); packetout.send(); } break; 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; } 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 && ENABLE_SLAAC) { /* 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; } }