150 lines
4.4 KiB
C++
150 lines
4.4 KiB
C++
#include "ipv6.h"
|
|
|
|
namespace ipv6 {
|
|
/******** costants ********/
|
|
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() {;}
|
|
|
|
IPv6Addr::IPv6Addr(const char* address) {
|
|
this->deserialize(address);
|
|
}
|
|
|
|
void IPv6Addr::deserialize(const char* address) {
|
|
memcpy(this->address, address, 16);
|
|
}
|
|
|
|
const char* IPv6Addr::serialize() {
|
|
return this->address;
|
|
}
|
|
|
|
/******** IPv6Packet ********/
|
|
char* IPv6Packet::serialize() { /* do we actually need these function? everything is already store in a tokenized/serialized form */
|
|
return packet;
|
|
}
|
|
|
|
void IPv6Packet::deserialize(const char* buffer, uint8_t len) {
|
|
memcpy(packet, buffer, len);
|
|
}
|
|
|
|
void IPv6Packet::setNextHeader(uint8_t next_header) {
|
|
*(packet + 6) = next_header;
|
|
}
|
|
|
|
void IPv6Packet::setSrcAddress(IPv6Addr& address) {
|
|
memcpy(packet + 8, (void*)&address, 16);
|
|
}
|
|
void IPv6Packet::setDstAddress(IPv6Addr& address) {
|
|
memcpy(packet + 24, (void*)&address, 16);
|
|
}
|
|
|
|
void IPv6Packet::setPayload(char* payload, uint8_t len) {
|
|
memset(packet, 0, LEN);
|
|
|
|
ipv6_packet_header* header = (ipv6_packet_header*)packet;
|
|
memcpy(header->field, "\x60\x00\x00\x00", 4);
|
|
|
|
memcpy(header->len + 1, (char*)&len, 1);
|
|
header->hop_limit = 64;
|
|
|
|
memcpy(packet + sizeof(ipv6_packet_header), payload, len);
|
|
}
|
|
|
|
void IPv6Packet::doAction() {
|
|
/* if destination is my address, or destination is "all nodes" address, then... */
|
|
if ((memcmp(&(ALL_NODES_ADDRESS), packet + 24, 16) == 0) || (memcmp(&(coppino::address), packet + 24, 16) == 0)) {
|
|
int len = ((*(packet + 4)) << 8) + (*(packet + 5));
|
|
|
|
switch(*(packet + 6)) { /* next header */
|
|
case NH_ICMP:
|
|
handleICMP(packet + 8, packet + 24, packet + 40, len);
|
|
break;
|
|
case NH_UDP:
|
|
// handleUDP(packet + 40, len);
|
|
break;
|
|
default:
|
|
/* no other protocols are known here, sorry :-) */
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
void handleICMP(const char* src_addr, const char* dst_addr, char* packet, int len) {
|
|
uint8_t type = *(packet);
|
|
|
|
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);
|
|
|
|
memset(packet + 2, 0, 2); /* zero-checksum */
|
|
uint16_t computed_checksum = compute_checksum(src_addr, dst_addr, NH_ICMP, packet, len);
|
|
|
|
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 */
|
|
|
|
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;
|
|
}
|
|
|
|
}
|