269 lines
7.4 KiB
C++
269 lines
7.4 KiB
C++
#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;
|
|
}
|
|
|
|
}
|