/*
 * packet dumper using ip_queue.ko and netfilter/libipq
 *  bushi at mizi dot com
 *
 * sudo /sbin/modprobe ip_queue
 * gcc -O2 -s -o ipq_80 ipq_80.c -lpthread -lipq -Wall
 * sudo ./ipq_80 > dropped_packets.txt
 *
 */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ctype.h>
#include <unistd.h>
#include <signal.h>
#include <time.h>
#include <setjmp.h>
#include <pthread.h>

#include <sys/types.h>
#include <sys/socket.h>
#include <sys/time.h>

#include <linux/types.h>
#include <netinet/tcp.h>
#include <netinet/ip.h>
#include <linux/netfilter.h>
#include <libipq.h>

#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>

#include <regex.h>

/* from linux kernel, GPL */
#include "list.h"
#ifndef list_first_entry
#  define list_first_entry(ptr, type, member) \
	list_entry((ptr)->next, type, member)
#endif

#ifndef countof
#  define countof(x) (sizeof(x)/sizeof(x[0]))
#endif

#define CHECK_PORT (80)
#define LOG_TO (stderr)
#define DUMP_TO (stdout)

/* enable this if you want to reschdule dumping thread for each packet */
//#define RESCHED_THREAD

#define THREAD_SCHED_INTERVAL	(1)
#define BYTES_PER_LINE		(16)

#define BUFFSZ			(4096)

static const int sig_list[] = {
		SIGHUP,   SIGINT, SIGQUIT,  SIGILL, SIGTRAP, SIGABRT,  SIGIOT,
		SIGFPE,  SIGKILL, SIGUSR1, SIGSEGV, SIGPIPE, SIGTERM,
		/*SIGCHLD, SIGCONT, SIGSTOP, SIGTSTP,*/ SIGTTIN, SIGTTOU,
#ifdef SIGSTKFLT
		SIGSTKFLT,
#endif
};

static const char *pattern_strings[] = {
	"and\\s*[0-9]=[0-9]",
	"[0-9a-zA-Z]+[']",
};

static jmp_buf setjmp_buffer;

typedef struct context_s {
	struct ipq_handle *ipq;
	unsigned char *buf;

	struct list_head msgs;
	struct list_head patterns;

	pthread_t dump_pid;
	pthread_mutex_t mutex;

	pthread_cond_t cond;
	pthread_mutex_t wait;
} context_t;

typedef struct msg_s {
	struct list_head node;
	int datalen;
	char *data;
} msg_t;

typedef struct pattern_s {
	struct list_head node;
	regex_t regex;
} pattern_t;

/* signal handler */
static void sig_handler(int sig)
{
	if (sig)
		fprintf(LOG_TO, "-- sig %d --\n", sig);
	longjmp(setjmp_buffer, 1);
}

/* FIXME please */
static void url_unescape_inplace(char *data, int len)
{
	unsigned char *s = (unsigned char *)data;
	unsigned char *d = (unsigned char *)data;
	int left = len;
	unsigned char c, c1, c2;
	while (left > 0) {
		c = *s;
		c1 = 0;
		c2 = 0;
		if (left > 1)
			c1 = *(s+1);
		if (left > 2)
			c2 = *(s+2);
		if (c == '%') {
			if(isxdigit(c1) && isxdigit(c2)) {
				c1 = tolower(c1);
				c2 = tolower(c2);
				if( c1 <= '9' )
					c1 = c1 - '0';
				else
					c1 = c1 - 'a' + 10;
				if( c2 <= '9' )
					c2 = c2 - '0';
				else
					c2 = c2 - 'a' + 10;
				*d = c1 * 16 + c2;
				s += 3;
				d += 1;
				left -= 3;
			} else if (c1 == '%') {
				*d = c;
				*(d+1) = c1;
				d += 2;
				s += 2;
				left -= 2;
			} else {
				*d = c;
				d++;
				s++;
				left--;
			}
			continue;
		}
		if( c == '+' ) {
			c = ' ';
		}
		*d = c;
		d++;
		s++;
		left--;
	}
}

inline int regex_match(context_t *cxt, char *data, int datalen)
{
	int ret, matched = 0;
	pattern_t *pattern, *tmp;
	char errbuf[128] = {0,};

	url_unescape_inplace(data, datalen);

	list_for_each_entry_safe(pattern, tmp, &cxt->patterns, node) {
		/* FIXME : 'data' must be null terminated C string */
		ret = regexec(&pattern->regex, data, (size_t)0, NULL, 0); 
		if (ret == 0) {
			matched++;
			break;
		}
		if (ret == REG_NOMATCH)
			continue;

		regerror(ret, &pattern->regex, errbuf, sizeof(errbuf));
		fprintf(LOG_TO, "regexec(): %s\n", errbuf);
		matched++; /* to force DROP */
		break;
	}
	return matched;
}

inline void __print_hex_line(FILE *out, const int nm, const char *buf, int len)
{
	char line_buf[nm * 3 + nm + 2 + 1];
	char tmp[4];
	unsigned char *b = (unsigned char *)buf;
	int i;

	memset(line_buf, 0x20, sizeof(line_buf));
	line_buf[nm * 3] = '|';
	line_buf[sizeof(line_buf)-2] = '|';
	
	for (i = 0; i < len; i++, b++) {
		sprintf(&tmp[0], "%02X", *b);
		if (isprint(*b))
			sprintf(&tmp[2], "%c", *b);
		else
			tmp[2] = ' ';
		line_buf[i*3 + 0] = tmp[0];
		line_buf[i*3 + 1] = tmp[1];
		line_buf[nm*3 + i + 1] = tmp[2];
	}

	line_buf[sizeof(line_buf)-1] = 0;
	fprintf(out, "%s\n", line_buf);
}

inline void print_hex(FILE *out, const char *buf, int len)
{
	int left = len;

	if (len <= 0)
		return;

	while (left) {
		int this_len = left > BYTES_PER_LINE ? BYTES_PER_LINE:left;
		__print_hex_line(out, BYTES_PER_LINE, buf, this_len);
		left -= this_len;
		buf += this_len;	
	}
}

inline void print_ascii(FILE *out, const char *buf, int len)
{
	int i;
	int need_lf = 1;

	if (len <= 0)
		return;

	if (buf[len - 1] == '\n')
		need_lf = 0;
		
	for (i=0; i < len; i++, buf++) {
		int c = *buf;
		if (isprint(c) || (c == '\n') || (c == '\r'))
			putc(c, out);	
		else
			putc(' ', out);
	}
	if (need_lf)
		putc('\n', out);
}

static int pre_process(FILE *out, const ipq_packet_msg_t *pkt)
{
	const struct iphdr *iph = (struct iphdr *)&pkt->payload[0];
	unsigned int iphdr_len;
	int protocol;
	unsigned int src_ip, dest_ip, tot_len;
	char src_ip_str[16], dest_ip_str[16];

	src_ip = ntohl(iph->saddr);
	dest_ip = ntohl(iph->daddr);
	protocol = iph->protocol;
	iphdr_len = iph->ihl << 2;
	tot_len = ntohs(iph->tot_len);

	sprintf(src_ip_str, "%d.%d.%d.%d",
			(src_ip >> 24) & 0xff,
			(src_ip >> 16) & 0xff,
			(src_ip >> 8) & 0xff,
			(src_ip >> 0) & 0xff
		);
	sprintf(dest_ip_str, "%d.%d.%d.%d",
			(dest_ip >> 24) & 0xff,
			(dest_ip >> 16) & 0xff,
			(dest_ip >> 8) & 0xff,
			(dest_ip >> 0) & 0xff
		);

	switch (protocol) {
	case IPPROTO_TCP: {
		const struct tcphdr *tcp;
		unsigned int src_port, dest_port;
		unsigned int tcpdata_offset;
		int payload_length, payload_offset;

		tcp = (struct tcphdr *)(&pkt->payload[0] + iphdr_len);
		tcpdata_offset = tcp->doff << 2;
		src_port = ntohs(tcp->source);
		dest_port = ntohs(tcp->dest);

		if (dest_port != CHECK_PORT)
			break;

		payload_offset = iphdr_len + tcpdata_offset;
		payload_length = tot_len - payload_offset;

		fprintf(out, "INFO: %s:%d -> %s:%d, %d of %d bytes\n",
				src_ip_str, src_port,
				dest_ip_str, dest_port,
				payload_length, pkt->data_len);

		if (payload_length > 0)
			return payload_offset;
		}
		break;
	default:
		/* ToDo */
		break;
	}
	return 0;
}

inline void queue_msg(context_t *cxt, msg_t *msg)
{
	INIT_LIST_HEAD(&msg->node);
	pthread_mutex_lock(&cxt->mutex);
	list_add_tail(&msg->node, &cxt->msgs);
	pthread_mutex_unlock(&cxt->mutex);
}

inline msg_t *dequeue_msg(context_t *cxt)
{
	msg_t *msg = NULL;
	pthread_mutex_lock(&cxt->mutex);
	if (!list_empty(&cxt->msgs)) {
		msg = list_first_entry(&cxt->msgs, msg_t, node);
		list_del(&msg->node);
	}
	pthread_mutex_unlock(&cxt->mutex);
	return msg;
}

static void dump_msgs(context_t *cxt)
{
	msg_t *msg;
	while ((msg = dequeue_msg(cxt))) {
		print_ascii(DUMP_TO, msg->data, msg->datalen);
		print_hex(DUMP_TO, msg->data, msg->datalen);
		free(msg);
	}
}

inline void resched_thread(context_t *cxt)
{
	pthread_mutex_lock(&cxt->wait);
	pthread_cond_signal(&cxt->cond);
	pthread_mutex_unlock(&cxt->wait);
}

static void dump_thread_cleanup(void *arg)
{
	/* nothing to do */
	fprintf(LOG_TO, "\nthread killed\n");
}

static void *dump_thread(void *arg)
{
	context_t *cxt = arg;
	struct timespec ts;
	struct timeval tp;

	pthread_cleanup_push(dump_thread_cleanup, cxt)

	while (1) {
		int ret;
		pthread_testcancel();

		pthread_mutex_lock(&cxt->wait);
		gettimeofday(&tp, NULL);
		ts.tv_sec  = tp.tv_sec;
		ts.tv_nsec = tp.tv_usec * 1000;
		ts.tv_sec += THREAD_SCHED_INTERVAL;
		ret = pthread_cond_timedwait(&cxt->cond, &cxt->wait, &ts);
		pthread_mutex_unlock(&cxt->wait);

		if (ret && (ret != ETIMEDOUT)) {
			/* FIXME: use strerror_r() instead of strerror() */
			fprintf(LOG_TO, "thread::wait: %s\n", strerror(ret));
			break; /* or continue */
		}

		dump_msgs(cxt);
	}

	pthread_cleanup_pop(1);

	return (void**)NULL;
}

inline int pass_this(context_t *cxt, ipq_packet_msg_t *pkt)
{
	fprintf(LOG_TO, "... PASSed!\n");
	return ipq_set_verdict(cxt->ipq, pkt->packet_id, NF_ACCEPT, 0, NULL);
}

inline int drop_this(context_t *cxt, ipq_packet_msg_t *pkt)
{
	fprintf(LOG_TO, "... DROPed!\n");
	return ipq_set_verdict(cxt->ipq, pkt->packet_id, NF_DROP, 0, NULL);
}

inline int accept_this(context_t *cxt, ipq_packet_msg_t *pkt)
{
	fprintf(LOG_TO, "... ACCEPTed!\n");
	return ipq_set_verdict(cxt->ipq, pkt->packet_id, NF_ACCEPT, 0, NULL);
}

inline int process_packet(context_t *cxt)
{
	ipq_packet_msg_t *pkt;
	int ret = -1;
	msg_t *new_msg;
	const char *data;
	int datalen, dataoffset;

	pkt = ipq_get_packet(cxt->buf);
	if (!pkt) {
		/* is ipq error */
		return -1;
	}

	dataoffset = pre_process(LOG_TO, pkt);
	if (dataoffset <= 0) {
		return pass_this(cxt, pkt);
	}

	data = (char *)&pkt->payload[dataoffset];
	datalen = pkt->data_len - ret;

	new_msg = malloc(sizeof(msg_t) + datalen);
	if (!new_msg) {
		/* FIXME: use strerror_r() instead of strerror() */
		fprintf(LOG_TO, "malloc(): %s\n", strerror(errno));
		/* is not ipq error */
		pass_this(cxt, pkt);
		return 0;
	}
	new_msg->data = (char*)(new_msg + 1);
	memcpy(new_msg->data, data, datalen);
	new_msg->datalen = datalen;

	if (!regex_match(cxt, new_msg->data, new_msg->datalen)) {
		free(new_msg);
		return accept_this(cxt, pkt);
	}

	ret = drop_this(cxt, pkt);
	if (ret < 0) {
		free(new_msg);
		return ret;
	}

	queue_msg(cxt, new_msg);

#ifdef RESCHED_THREAD
	/* wakeup immediately */
	resched_thread(cxt);
#endif
	return 0;
}

static void process_ipq(context_t *cxt)
{
	int ret, type;

	fprintf(LOG_TO, "\nprocessing...\n");

	do {
		ret = ipq_read(cxt->ipq, cxt->buf, BUFFSZ, 0);
		if (ret < 0) {
			/* error */
			fprintf(LOG_TO, "ipq_read(): %s\n", ipq_errstr());
			return;
		}
		if (ret == 0) {
			/* timed-out */
			continue;
		}

		type = ipq_message_type(cxt->buf);
		switch (type) {
		case IPQM_PACKET:
			ret = process_packet(cxt);
			if (ret < 0) {
				fprintf(LOG_TO, "ipq: %s\n", ipq_errstr());
			}
			break;
		case NLMSG_ERROR:
			ret = ipq_get_msgerr(cxt->buf);
			/* FIXME: use strerror_r() instead of strerror() */
			fprintf(LOG_TO, "NETLINK: %s\n", strerror(ret));
			return;
		default:
			fprintf(LOG_TO, "unknown packet type.\n");
		}
	} while (1);
}

static void deinit_patterns(context_t *cxt)
{
	pattern_t *pattern, *tmp;
	list_for_each_entry_safe(pattern, tmp, &cxt->patterns, node) {
		list_del(&pattern->node);
		regfree(&pattern->regex);
		free(pattern);
	}
}

static int init_patterns(context_t *cxt, const char **pats, int n)
{
	int pattern_index, ret;
	pattern_t *pattern;
	char errbuf[128] = {0,};

	for (pattern_index = 0; pattern_index < n; pattern_index++) {
		const char *pat;

		pat = pats[pattern_index];

		pattern = malloc(sizeof(pattern_t));
		if (!pattern) {
			fprintf(LOG_TO, "malloc(): %s\n", strerror(errno));
			return -1;
		}

		ret = regcomp(&pattern->regex, pat, 0);
		if (ret) {
			regerror(ret, &pattern->regex, errbuf, sizeof(errbuf));
			fprintf(LOG_TO, "regcomp(): %s\n", errbuf);
			free(pattern);
			return -1;
		}
	
		INIT_LIST_HEAD(&pattern->node);
		list_add_tail(&pattern->node, &cxt->patterns);
	}
	return 0;
}

static int init_context(context_t *cxt)
{
	int sig_index;
	int ret = -1;
	struct ipq_handle *h;

	fprintf(LOG_TO, "\ninitialising...\n");

	/* initialize , must be fist of all */
	INIT_LIST_HEAD(&cxt->msgs);
	INIT_LIST_HEAD(&cxt->patterns);
	pthread_mutex_init(&cxt->mutex, NULL);
	pthread_mutex_init(&cxt->wait, NULL);
	pthread_cond_init(&cxt->cond, NULL);

	/* make pattern expression */
	ret = init_patterns(cxt, &pattern_strings[0], countof(pattern_strings));
	if (ret < 0)
		goto out_init_context;

	/* create ipq */
	h = ipq_create_handle(0, PF_INET);
	if (!h) {
		fprintf(LOG_TO, "ipq_create_handle(): %s\n", ipq_errstr());
		goto destroy_pattern;
	}
	cxt->ipq = h;

	/* set mode of ipq */
	if (ipq_set_mode(h, IPQ_COPY_PACKET, BUFFSZ) < 0) {
		fprintf(LOG_TO, "ipq_set_mode(): %s\n", ipq_errstr());
		goto destroy_ipq;
	}

	/* create buffer */
	cxt->buf = malloc(BUFFSZ);
	if (!cxt->buf) {
		fprintf(LOG_TO, "malloc(): %s\n", strerror(errno));
		goto destroy_ipq;
	}

	/* create thread */
	ret = pthread_create(&cxt->dump_pid, NULL, dump_thread, cxt);
	if (ret) {
		fprintf(LOG_TO, "pthread_create(): %s\n", strerror(ret));
		goto destroy_buf;
	}

	/* register signal handler */
	for (sig_index = 0; sig_index < countof(sig_list); sig_index++) {
		if (signal(sig_list[sig_index], sig_handler) == SIG_ERR) {
			/* continue */
		}
	}

	return 0;

 destroy_buf:
	free(cxt->buf);
	cxt->buf = NULL;
 destroy_ipq:
	ipq_destroy_handle(h);
	cxt->ipq = NULL;
 destroy_pattern:
	deinit_patterns(cxt);
 out_init_context:
	pthread_mutex_destroy(&cxt->mutex);
	pthread_mutex_destroy(&cxt->wait);
	pthread_cond_destroy(&cxt->cond);
	return -1;
}

static void deinit_context(context_t *cxt)
{
	int i;

	fprintf(LOG_TO, "\ndeinitialising...\n");

	/* unregister signal handler */
	for (i=0;i<countof(sig_list);i++) {
		signal(sig_list[i], SIG_DFL);
	}

	/* kill thread */
	if (cxt->dump_pid) {
		pthread_cancel(cxt->dump_pid);
		pthread_join(cxt->dump_pid, (void**)NULL);
		cxt->dump_pid = 0;
	}

	/* destroy ipq */
	ipq_destroy_handle(cxt->ipq);
	cxt->ipq = NULL;

	/* destroy buffer */
	if (cxt->buf)
		free(cxt->buf);
	cxt->buf = NULL;

	/* destroy pattern expression */
        deinit_patterns(cxt);

	/* flush and destroy all queued messages */
	dump_msgs(cxt);

	/* destroy pthread stuff */
	pthread_mutex_destroy(&cxt->mutex);
	pthread_mutex_destroy(&cxt->wait);
	pthread_cond_destroy(&cxt->cond);
}

int main(int argc, char **argv)
{
	context_t context = { .ipq = NULL, .buf = NULL};

	if (init_context(&context)) {
		return EXIT_FAILURE;
	}

	if (!setjmp(setjmp_buffer)) {
		process_ipq(&context);
	}

	deinit_context(&context);	

	return EXIT_SUCCESS;
}