/* Compile with gcc -lnfnetlink -lnetfilter_queue  */
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <netinet/in.h>
#include <netinet/ip.h>
#include <netinet/tcp.h>
#include <linux/netfilter.h>		/* for NF_ACCEPT */
#include <arpa/inet.h>

#include <libnetfilter_queue/libnetfilter_queue.h>

#define  BUFSIZE  2048

struct in_addr foreign;
struct in_addr local;

struct queued_pckt {
	char *payload;
	int payload_len;
};

inline u_int16_t checksum_update_32(
	u_int16_t old_check,
	u_int32_t old,
	u_int32_t new)
{
	u_int32_t l;

	old_check = ~old_check;
	old = ~old;

	l = (u_int32_t)old_check + ((old >> 16) + (old & 0xffff)) + ((new >> 16) + (new & 0xffff));
	return ~((u_int16_t)((l >> 16) + (l & 0xffff)));
}

static void filter(
	unsigned char *packet, unsigned int payload_len)
{
	struct iphdr *iphdr;
	struct tcphdr *tcphdr;

	printf ("in filter function\n");

	iphdr = (struct iphdr *)packet;
	/* check need some datas */
	if (payload_len < sizeof(struct iphdr) + sizeof(struct tcphdr)) {
		return;
	}
	/* check IP version */
	if (iphdr->protocol == IPPROTO_TCP)
	{
		tcphdr = (struct tcphdr *)(((u_int32_t *)packet) + 4 * iphdr->ihl);

		if (iphdr->daddr == foreign.s_addr)
		{
			printf ("packet DEST addr = %s\n",inet_ntoa(foreign));
			printf ("local addr = %s\n",inet_ntoa(local));
			fprintf (stderr, "changing pkt's DEST addr from FOREIGN to LOCAL\n");
			iphdr->check = checksum_update_32(
				iphdr->check,
				iphdr->daddr,
				local.s_addr);
			tcphdr->check = checksum_update_32(
				tcphdr->check,
				iphdr->daddr,
				local.s_addr);
			iphdr->daddr = local.s_addr;
		}
	}

}

static int cb(struct nfq_q_handle *qh, struct nfgenmsg *nfmsg,
	      struct nfq_data *nfa, void *data)
{
	int id = 0;
	struct nfqnl_msg_packet_hdr *ph;
	struct queued_pckt q_pckt;
	u_int32_t mark,ifi; 
	int ret;
	char *payload;
	
	printf("entering callback\n");
	ph = nfq_get_msg_packet_hdr(nfa);
	if (ph){
		id = ntohl(ph->packet_id);
		printf("hw_protocol=0x%04x hook=%u id=%u ",
			ntohs(ph->hw_protocol), ph->hook, id);
	}
	
	mark = nfq_get_nfmark(nfa);
	if (mark)
		printf("mark=%u ", mark);

	ifi = nfq_get_indev(nfa);
	if (ifi)
		printf("indev=%u ", ifi);

	ifi = nfq_get_outdev(nfa);
	if (ifi)
		printf("outdev=%u ", ifi);

	q_pckt.payload_len = nfq_get_payload(nfa, &(q_pckt.payload));
	if (q_pckt.payload_len >= 0)
	{
		printf("payload_len=%d ", q_pckt.payload_len);
		fputc('\n', stdout);
		filter((unsigned char *)q_pckt.payload, q_pckt.payload_len);
	}
	
	printf("setting verdict of packet id %d\n",id);
	return nfq_set_verdict(qh, id, NF_ACCEPT, q_pckt.payload_len, q_pckt.payload);
}

int main(int argc, char **argv)
{
	struct nfq_handle *h;
	struct nfq_q_handle *qh;
	struct nfnl_handle *nh;
	int fd;
	int rv;
	unsigned char buf[BUFSIZE];

	inet_aton("10.102.35.22", &(foreign));
	inet_aton("10.102.35.24", &(local));

	printf("opening library handle\n");
	h = nfq_open();
	if (!h) {
		fprintf(stderr, "error during nfq_open()\n");
		exit(1);
	}

	printf("unbinding existing nf_queue handler for AF_INET (if any)\n");
	if (nfq_unbind_pf(h, AF_INET) < 0) {
		fprintf(stderr, "error during nfq_unbind_pf()\n");
		exit(1);
	}

	printf("binding nfnetlink_queue as nf_queue handler for AF_INET\n");
	if (nfq_bind_pf(h, AF_INET) < 0) {
		fprintf(stderr, "error during nfq_bind_pf()\n");
		exit(1);
	}

	printf("binding this socket to queue '0'\n");
	qh = nfq_create_queue(h,  0, &cb, NULL);
	if (!qh) {
		fprintf(stderr, "error during nfq_create_queue()\n");
		exit(1);
	}

	printf("setting copy_packet mode\n");
	if (nfq_set_mode(qh, NFQNL_COPY_PACKET, BUFSIZE) < 0) {
		fprintf(stderr, "can't set packet_copy mode\n");
		exit(1);
	}

	nh = nfq_nfnlh(h);
	fd = nfnl_fd(nh);

	while ((rv = recv(fd, buf, BUFSIZE, 0)) && rv >= 0) {
		printf("pkt received\n");
		nfq_handle_packet(h, buf, rv);
		printf("pkt handled\n");
	}

	printf("unbinding from queue 0\n");
	nfq_destroy_queue(qh);

	printf("closing library handle\n");
	nfq_close(h);

	exit(0);
}