void translate_6to4(struct mapping *st, char *buf, int len){
struct ip6_hdr *ip6 = (struct ip6_hdr *)buf;
struct ip ip;
struct tun_pi pi;
struct iovec iov[3];
memset(&ip, 0, sizeof(struct ip));
ip.ip_v = 4;
ip.ip_hl = 5;
ip.ip_len = htons(len - sizeof(struct ip6_hdr) + sizeof(struct ip));
ip.ip_id = ip6->ip6_flow;
ip.ip_off = htons(IP_DF);
ip.ip_ttl = ip6->ip6_hlim;
ip.ip_p = ip6->ip6_nxt;
ip.ip_src = st->mapped_ipv4_addr;
ip.ip_dst = sa46t_extract_6to4_addr(ip6->ip6_dst);
if(ip6->ip6_nxt == IPPROTO_FRAGMENT){
/* drop already fragmented packet */
return;
}
if(ip6->ip6_nxt == IPPROTO_ICMPV6){
ip.ip_p = IPPROTO_ICMP;
process_icmpv6_packet(buf + sizeof(struct ip6_hdr), len - sizeof(struct ip6_hdr));
}
if(ip6->ip6_nxt == IPPROTO_TCP){
process_tcp_packet(AF_INET, &ip, buf + sizeof(struct ip6_hdr), len - sizeof(struct ip6_hdr));
}
if(ip6->ip6_nxt == IPPROTO_UDP){
process_udp_packet(AF_INET, &ip, buf + sizeof(struct ip6_hdr), len - sizeof(struct ip6_hdr));
}
ip.ip_sum = 0;
ip.ip_sum = ip_checksum((unsigned short *)&ip, sizeof(struct ip));
tun_set_af(&pi, AF_INET);
iov[0].iov_base = π
iov[0].iov_len = sizeof(pi);
iov[1].iov_base = &ip;
iov[1].iov_len = sizeof(ip);
iov[2].iov_base = buf + sizeof(struct ip6_hdr);
iov[2].iov_len = len - sizeof(struct ip6_hdr);
send_iovec(iov, 3);
}
void translate_4to6(struct mapping *st, char *buf, int len){
struct ip *ip = (struct ip *)buf;
struct ip6_hdr ip6;
struct tun_pi pi;
struct iovec iov[4];
memset(&ip6, 0, sizeof(struct ip6_hdr));
ip6.ip6_vfc = 0x60;
ip6.ip6_plen = htons(len - sizeof(struct ip));
ip6.ip6_nxt = ip->ip_p;
ip6.ip6_hlim = ip->ip_ttl;
ip6.ip6_src = sa46t_map_4to6_addr(sa46t_addr, plane_id, ip->ip_src);
ip6.ip6_dst = st->source_ipv6_addr;
if((ip->ip_off & htons(IP_MF)) > 0){
/* drop already fragmented packet */
return;
}
if(sizeof(ip6) + len - sizeof(struct ip) > MTU){
fragment_4to6(&ip6, ip, buf + sizeof(struct ip), len - sizeof(struct ip));
return;
}
if(ip->ip_p == IPPROTO_ICMP){
ip6.ip6_nxt = IPPROTO_ICMPV6;
process_icmp_packet(&ip6, buf + sizeof(struct ip), len - sizeof(struct ip));
}
if(ip->ip_p == IPPROTO_TCP){
process_tcp_packet(AF_INET6, &ip6, buf + sizeof(struct ip), len - sizeof(struct ip));
}
if(ip->ip_p == IPPROTO_UDP){
process_udp_packet(AF_INET6, &ip6, buf + sizeof(struct ip), len - sizeof(struct ip));
}
tun_set_af(&pi, AF_INET6);
iov[0].iov_base = π
iov[0].iov_len = sizeof(pi);
iov[1].iov_base = &ip6;
iov[1].iov_len = sizeof(ip6);
iov[2].iov_base = buf + sizeof(struct ip);
iov[2].iov_len = len - sizeof(struct ip);
send_iovec(iov, 3);
}
static int cb(struct nfq_q_handle *qh, struct nfgenmsg *nfmsg,
struct nfq_data *nfa, void *data)
{
//log_debug("entering callback");
//char buf[1025];
//nfq_snprintf_xml(buf, 1024, nfa, NFQ_XML_ALL);
//log_debug("%s", buf);
struct nfqnl_msg_packet_hdr *ph;
ph = nfq_get_msg_packet_hdr(nfa);
if (!ph) {
log_error("nfq_get_msg_packet_hdr failed");
return -1;
}
u_int32_t id = ntohl(ph->packet_id);
//log_debug("packet id: %d", id);
char inout = nfq_get_outdev(nfa) ? 1 : 0; // 0 - in, 1 - out
// get data (IP header + TCP header + payload)
unsigned char *pkt_data;
int plen = nfq_get_payload(nfa, &pkt_data);
g_modified = 0;
//if (plen >= 0)
// log_debug("payload_len=%d", plen);
//hex_dump(pkt_data, plen);
struct mypacket packet;
packet.data = pkt_data;
packet.len = plen;
packet.iphdr = ip_hdr(pkt_data);
// parse ip
//char sip[16], dip[16];
//ip2str(packet.iphdr->saddr, sip);
//ip2str(packet.iphdr->daddr, dip);
//log_debug("This packet goes from %s to %s.", sip, dip);
//log_debugv("This packet goes from %s to %s.", sip, dip);
if (is_ip_in_whitelist(packet.iphdr->saddr) || is_ip_in_whitelist(packet.iphdr->daddr)) {
nfq_set_verdict(qh, id, NF_ACCEPT, 0, NULL);
return 0;
}
int ret = 0;
switch (packet.iphdr->protocol) {
case 6: // TCP
packet.tcphdr = tcp_hdr(pkt_data);
packet.payload = tcp_payload(pkt_data);
packet.payload_len = packet.len - packet.iphdr->ihl*4 - packet.tcphdr->th_off*4;
//show_packet(&packet);
ret = process_tcp_packet(&packet, inout);
break;
case 17: // UDP
packet.udphdr = udp_hdr(pkt_data);
packet.payload = udp_payload(pkt_data);
packet.payload_len = packet.len - packet.iphdr->ihl*4 - 8;
if (packet.payload_len != ntohs(packet.udphdr->uh_ulen) - 8)
log_warn("UDP payload length unmatch! %d <> %d", packet.payload_len, ntohs(packet.udphdr->uh_ulen) - 8);
//show_packet(&packet);
ret = process_udp_packet(&packet, inout);
break;
default:
log_error("Invalid protocol: %d", packet.iphdr->protocol);
}
int verdict_ret;
if (ret == 0) {
if (g_modified)
{
log_warn("Packet Modified.");
//if (packet.iphdr->protocol == 6) {
// packet.tcphdr->th_sum = tcp_checksum(packet.data, ntohs(packet.iphdr->tot_len));
//}
//packet.iphdr->check = ip_checksum(packet.data, packet.len);
verdict_ret = nfq_set_verdict(qh, id, NF_ACCEPT, packet.len, packet.data);
//log_info("VERDICT MODIFIED ACCEPT");
}
else {
verdict_ret = nfq_set_verdict(qh, id, NF_ACCEPT, 0, NULL);
//log_info("VERDICT ACCEPT");
}
}
else if (ret == 1) {
usleep(DELAY_AFTER_PACKET_INJECTION);
verdict_ret = nfq_set_verdict(qh, id, NF_ACCEPT, 0, NULL);
//log_info("VERDICT DELAYED ACCEPT");
}
else {
verdict_ret = nfq_set_verdict(qh, id, NF_DROP, 0, NULL);
//log_info("VERDICT DROP");
}
// return <0 to stop processing
return verdict_ret;
}