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; }