#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\x01"); /* ff02::1 */

/******** buffer ********/
IPv6Packet packetin;
IPv6Packet packetout;

/******** FUNCTIONS *******/
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) = 64;             /* 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, or destination is "all nodes" address, then... */
  if ((memcmp(&(ALL_NODES_ADDRESS), packet + 24, 16) == 0) || (memcmp(&(coppino::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;
    }
  }
}

int handleICMP(char* output_buffer, const char* input_buffer, int len) {
  uint8_t type = *(input_buffer);
  
  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, 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 valid, 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;
      }
  }

  return -1;
}

}