void rewriteIpv6( char* argv, int len, char* ipv6 )
{
PETH_HDR ether;
PIP6_HDR ip6;
PTCP_HDR tcp;
PUDP_HDR udp;
PICMP6_HDR icmp6;
int ulen;
ether = (PETH_HDR)argv;
// IPv6
if ( ntohs( ether->ether_type ) == ETHER_TYPE_IP6 )
{
#ifdef DEBUG
printf("rewrite.c - IPv6 rewrite\n");
#endif
if ( checkPacketNotLen( len, ( ETHER_HDRLEN + IP6_HDRLEN ) ) )
return;
ip6 = (PIP6_HDR)(argv + ETHER_HDRLEN);
inet_pton( AF_INET6, ipv6, &(ip6->ip6_src) );
// TCP checksum
if ( ip6->ip6_nxt == IPPROTO_TCP )
{
ulen = len - ETHER_HDRLEN - IP6_HDRLEN;
if ( checkPacketNotLen( len, ( ETHER_HDRLEN + IP6_HDRLEN + TCP_HDRLEN ) ) )
return;
tcp = (PTCP_HDR)(argv + ETHER_HDRLEN + IP6_HDRLEN );
tcp->tcp_checksum = 0;
tcp->tcp_checksum = tcp6_checksum( tcp, ulen, ip6->ip6_src, ip6->ip6_dst );
#ifdef DEBUG
printf("rewrite.c - TCPv6 checksum recalculated = %x\n", ntohs( tcp->tcp_checksum ) );
#endif
}
// UDP checksum
if ( ip6->ip6_nxt == IPPROTO_UDP )
{
ulen = len - ETHER_HDRLEN - IP6_HDRLEN;
if ( checkPacketNotLen( len, ( ETHER_HDRLEN + IP6_HDRLEN + UDP_HDRLEN ) ) )
return;
udp = (PUDP_HDR)(argv + ETHER_HDRLEN + IP6_HDRLEN );
udp->udp_checksum = 0;
udp->udp_checksum = udp6_checksum( udp, ulen, ip6->ip6_src, ip6->ip6_dst );
#ifdef DEBUG
printf("rewrite.c - UDPv6 checksum recalculated = %x\n", ntohs( udp->udp_checksum ) );
#endif
}
// ICMPv6 checksum
if ( ip6->ip6_nxt == IPPROTO_ICMPV6 )
{
ulen = len - ETHER_HDRLEN - IP6_HDRLEN;
if ( checkPacketNotLen( len, ( ETHER_HDRLEN + IP6_HDRLEN + ICMP6_HDRLEN ) ) )
return;
icmp6 = (PICMP6_HDR)(argv + ETHER_HDRLEN + IP6_HDRLEN );
icmp6->icmp6_checksum = 0;
icmp6->icmp6_checksum = icmp6_checksum( icmp6, ulen, ip6->ip6_src, ip6->ip6_dst );
#ifdef DEBUG
printf("rewrite.c - ICMPv6 checksum recalculated = %x\n", ntohs( icmp6->icmp6_checksum ) );
#endif
}
}
}
static void send_fragment(int fd_raw, struct sockaddr *addr, socklen_t alen,
int offset, bool ipv6)
{
int frag_len;
int res;
int payload_offset = offset > 0 ? offset - UDP_HLEN : 0;
uint8_t *frag_start = ipv6 ? ip_frame + IP6_HLEN + FRAG_HLEN :
ip_frame + IP4_HLEN;
if (offset == 0) {
struct udphdr udphdr;
udphdr.source = htons(cfg_port + 1);
udphdr.dest = htons(cfg_port);
udphdr.len = htons(UDP_HLEN + payload_len);
udphdr.check = 0;
if (ipv6)
udphdr.check = udp6_checksum((struct ip6_hdr *)ip_frame, &udphdr);
else
udphdr.check = udp_checksum((struct ip *)ip_frame, &udphdr);
memcpy(frag_start, &udphdr, UDP_HLEN);
}
if (ipv6) {
struct ip6_hdr *ip6hdr = (struct ip6_hdr *)ip_frame;
struct ip6_frag *fraghdr = (struct ip6_frag *)(ip_frame + IP6_HLEN);
if (payload_len - payload_offset <= max_frag_len && offset > 0) {
/* This is the last fragment. */
frag_len = FRAG_HLEN + payload_len - payload_offset;
fraghdr->ip6f_offlg = htons(offset);
} else {
frag_len = FRAG_HLEN + max_frag_len;
fraghdr->ip6f_offlg = htons(offset | IP6_MF);
}
ip6hdr->ip6_plen = htons(frag_len);
if (offset == 0)
memcpy(frag_start + UDP_HLEN, udp_payload,
frag_len - FRAG_HLEN - UDP_HLEN);
else
memcpy(frag_start, udp_payload + payload_offset,
frag_len - FRAG_HLEN);
frag_len += IP6_HLEN;
} else {
struct ip *iphdr = (struct ip *)ip_frame;
if (payload_len - payload_offset <= max_frag_len && offset > 0) {
/* This is the last fragment. */
frag_len = IP4_HLEN + payload_len - payload_offset;
iphdr->ip_off = htons(offset / 8);
} else {
frag_len = IP4_HLEN + max_frag_len;
iphdr->ip_off = htons(offset / 8 | IP4_MF);
}
iphdr->ip_len = htons(frag_len);
if (offset == 0)
memcpy(frag_start + UDP_HLEN, udp_payload,
frag_len - IP4_HLEN - UDP_HLEN);
else
memcpy(frag_start, udp_payload + payload_offset,
frag_len - IP4_HLEN);
}
res = sendto(fd_raw, ip_frame, frag_len, 0, addr, alen);
if (res < 0)
error(1, errno, "send_fragment");
if (res != frag_len)
error(1, 0, "send_fragment: %d vs %d", res, frag_len);
frag_counter++;
}
