/*
 * MAP66: Network Address Translation IPv6-to-IPv6 as
 * proposed in the IETF's second NAT66 draft document.
 * (c) 2010 sven-ola()gmx.de
 */

#include <linux/module.h>
#include <linux/version.h>
#include <linux/netfilter_ipv6/ip6_tables.h>
#include <net/ipv6.h>

#include <linux/tcp.h>
#include <linux/udp.h>
#include <linux/icmpv6.h>
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,14)
#  include <linux/dccp.h>
#endif

// SOT
#include <net/checksum.h>

#include "ip6t_MAP66.h"

MODULE_AUTHOR("Sven-Ola <sven-ola()gmx.de>");
MODULE_DESCRIPTION("Xtables: MAP66 - IPv6 to IPv6 SNAT");
MODULE_LICENSE("GPL");

/* One's complement add */
static inline u_int16_t add16(
	u_int16_t a,
	u_int16_t b)
{
	a += b;
	return a + (a < b);
}

/* Calc one's complement csum */
static inline u_int16_t csum16(const u_int16_t *buf, int len)
{
	u_int16_t csum = 0;
	while(len--) csum = add16(csum, *buf++);
	return csum;
}

/* Perform checksum neutral mapping */
static void map16(
	struct in6_addr* addr,
	const struct in6_addr* to,
	int len_to,
	u_int16_t csum_to)
{
	csum_to = add16(
		*((u_int16_t*)addr + len_to),
		add16(csum16((const u_int16_t *)addr, len_to), csum_to)
	);
	if (csum_to == 0xffff) csum_to = 0x0000;
	*((u_int16_t *)addr + len_to) = csum_to;
	memcpy(addr, to, sizeof(u_int16_t) * len_to);
}

/* Perform mapping with csum update */
static void map_csum(
	struct in6_addr* addr,
	const struct in6_addr* to,
	int len_to,
	u_int16_t csum_to,
	u_int16_t* csum_transport)
{
	printk("SOT len=%d, csum = %04hx\n", len_to,
		~add16(
			add16(
				~(*csum_transport),
				~csum16((u_int16_t *)&addr, len_to)
			),
			csum16((u_int16_t *)&to, len_to)
		)
	);

	*csum_transport = ~add16(
		~csum_to,
		add16(~*csum_transport, ~csum16((u_int16_t *)&addr, len_to))
	);
	memcpy(addr, to, sizeof(u_int16_t) * len_to);
}

#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
#  define rcu_dereference(p) p
#  define ipv6_addr_equal(a, b) 0 == ipv6_addr_cmp(a, b)
#  define ipv6_hdr(skb) skb->nh.ipv6h
#  ifndef bool
#    define bool int
#    define false 0
#    define true 1
#  endif
#  define xt_target ip6t_target
#  define __read_mostly
#  define xt_register_target ip6t_register_target
#  define xt_unregister_target ip6t_unregister_target
#endif

#ifndef pr_devel
#  ifdef DEBUG
#    define pr_devel(fmt, ...) printk(fmt, ##__VA_ARGS__)
#  else
#    define pr_devel(fmt, ...) ({ if (0); })
#  endif
#endif

#ifndef NIP6_FMT
#  define NIP6_FMT "%04x:%04x:%04x:%04x:%04x:%04x:%04x:%04x"
#  define NIP6(addr) \
	ntohs((addr).s6_addr16[0]), \
	ntohs((addr).s6_addr16[1]), \
	ntohs((addr).s6_addr16[2]), \
	ntohs((addr).s6_addr16[3]), \
	ntohs((addr).s6_addr16[4]), \
	ntohs((addr).s6_addr16[5]), \
	ntohs((addr).s6_addr16[6]), \
	ntohs((addr).s6_addr16[7])
#endif

#if LINUX_VERSION_CODE <= KERNEL_VERSION(2,6,24)
#  define NF_INET_PRE_ROUTING NF_IP6_PRE_ROUTING
#  define NF_INET_POST_ROUTING NF_IP6_POST_ROUTING
#endif

#if LINUX_VERSION_CODE <= KERNEL_VERSION(2,6,26)

struct xt_target_param {
	const struct net_device *in, *out;
	const void *targinfo;
	unsigned int hooknum;
};

struct xt_tgchk_param {
	void *targinfo;
};

#endif

static bool is_my_ipv6_addr(
	const struct net_device *dev,
	const struct in6_addr *addr)
{
	pr_devel("MAP66: is_my_ipv6_addr(%s, " NIP6_FMT ")\n", NULL != dev ? dev->name : "", NIP6(*addr));
	if (NULL != dev) {
		const struct inet6_ifaddr *ifa;
		const struct inet6_dev *idev = rcu_dereference(dev->ip6_ptr);
		for (ifa = idev->addr_list; NULL != ifa; ifa = ifa->if_next) {
			pr_devel("MAP66: cmp " NIP6_FMT "\n", NIP6(ifa->addr));
			if (ipv6_addr_equal(&ifa->addr, addr)) return true;
		}
	}
	return false;
}

static unsigned int MAP66_tg6(
	struct sk_buff *skb,
	const struct xt_target_param *par)
{
	u_int16_t* csum_transport = NULL;
	struct ipv6hdr *hdr = ipv6_hdr(skb);
	const struct ip6t_MAP66_info *info = par->targinfo;

	pr_devel("MAP66: enter in=%s, out=%s, saddr=" NIP6_FMT ", daddr=" NIP6_FMT "\n",
		NULL != par->in ? par->in->name : "",
		NULL != par->out ? par->out->name : "",
		NIP6(hdr->saddr), NIP6(hdr->daddr));

#if 0
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
	if (skb_cloned(skb) && !skb->sk) {
		struct sk_buff *nskb = skb_copy(skb, GFP_ATOMIC);
		if (!nskb) {
			pr_devel("MAP66: cannot copy, dropped\n");
			return NF_DROP;
		}
		kfree_skb(skb);
		skb = nskb;
	}
#else
	if (!skb_make_writable(skb, sizeof(struct ipv6hdr))) {
		pr_devel("MAP66: unwriteable, dropped\n");
		return NF_DROP;
	}
#endif
	hdr = ipv6_hdr(skb);
#endif

	if (0 != (IP6T_MAP66_OPT_CSUM & info->mapflags)) {
		u8 nexthdr = hdr->nexthdr;
		unsigned char* transport = (unsigned char* )hdr + sizeof(struct ipv6hdr);
		pr_devel("SOT: hdr=%p, nexthdr=%d, transport=%p\n", hdr, nexthdr, transport);
		if (ipv6_ext_hdr(nexthdr)) {
			int hoff = ipv6_skip_exthdr(skb, sizeof(struct ipv6hdr), &nexthdr
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
				, skb->len - sizeof(struct ipv6hdr)
#endif
			);
			if (hoff < 0) {
				pr_devel("MAP66: Unsupported packet dropped\n");
				return NF_DROP;
			}
			transport += hoff;
			pr_devel("SOT: nexthdr=%d, transport%p\n", nexthdr, transport);
		}
		switch(nexthdr) {
			case IPPROTO_TCP:
				csum_transport = &((struct tcphdr*)transport)->check;
			break;
			case IPPROTO_UDP:
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20)
			case IPPROTO_UDPLITE:
#endif
				csum_transport = &((struct udphdr*)transport)->check;
			break;
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,14)
			case IPPROTO_DCCP:
				csum_transport = &((struct dccp_hdr*)transport)->dccph_checksum;
			break;
#endif
			case IPPROTO_ICMPV6:
				csum_transport = &((struct icmp6hdr*)transport)->icmp6_cksum;
				pr_devel("SOT icmpv6:, transport=%p, cksum=%p(%04hx)\n", transport, csum_transport, *csum_transport);
				{
					u_int16_t csum = 0;
					u_int16_t old_csum = *csum_transport;
					*csum_transport = 0;
					csum = add16(csum, csum16((u_int16_t*)&hdr->saddr, sizeof(hdr->saddr) / 2));
					csum = add16(csum, csum16((u_int16_t*)&hdr->daddr, sizeof(hdr->daddr) / 2));
					csum = add16(csum, hdr->payload_len);
					csum = add16(csum, ntohs(hdr->nexthdr));
					csum = add16(csum, csum16((u_int16_t*)transport, ntohs(hdr->payload_len) / 2));
					pr_devel("SOT: csum should=%04hx\n", ~csum);
					*csum_transport = old_csum;
				}
			break;
			default:
				pr_devel("MAP66: Unsupported protocol %d\n", nexthdr);
				return NF_DROP;
		}
	}

	if (0 != (IP6T_MAP66_OPT_DST_TO & info->mapflags)) {
		pr_devel("MAP66: DST_TO, ip_summed=%d\n", skb->ip_summed);
		if (0 != (IP6T_MAP66_OPT_NOCHECK & info->mapflags) || !is_my_ipv6_addr(
			NF_INET_PRE_ROUTING == par->hooknum ? par->in : par->out, &hdr->daddr))
		{
			if (0 != (IP6T_MAP66_OPT_CSUM & info->mapflags)) {
				map_csum(&hdr->daddr, &info->pfix_dst_to, info->pfix_dst_len, info->pfix_dst_csum, csum_transport);
				pr_devel("SOT dst: cksum=%p(%04hx)\n", csum_transport, *csum_transport);
			}
			else {
				map16(&hdr->daddr, &info->pfix_dst_to, info->pfix_dst_len, info->pfix_dst_csum);
			}
		}
	}
	
	if (0 != (IP6T_MAP66_OPT_SRC_TO & info->mapflags)) {
		pr_devel("MAP66: SRC_TO, ip_summed=%d\n", skb->ip_summed);
		if (0 != (IP6T_MAP66_OPT_CSUM & info->mapflags)) {
			map_csum(&hdr->saddr, &info->pfix_src_to, info->pfix_src_len, info->pfix_src_csum, csum_transport);
			pr_devel("SOT src: cksum=%p(%04hx)\n", csum_transport, *csum_transport);
			{
				u_int16_t csum = 0;
				u_int16_t old_csum = *csum_transport;
				*csum_transport = 0;
				csum = add16(csum, csum16((u_int16_t*)&hdr->saddr, sizeof(hdr->saddr) / 2));
				csum = add16(csum, csum16((u_int16_t*)&hdr->daddr, sizeof(hdr->daddr) / 2));
				csum = add16(csum, hdr->payload_len);
				csum = add16(csum, ntohs(hdr->nexthdr));
				csum = add16(csum, csum16(((u_int16_t*)hdr) + 20, ntohs(hdr->payload_len) / 2));
				pr_devel("SOT: csum should=%04hx\n", ~csum);
				*csum_transport = old_csum;
			}
		}
		else {
			map16(&hdr->saddr, &info->pfix_src_to, info->pfix_src_len, info->pfix_src_csum);
		}
		if (0 == (IP6T_MAP66_OPT_NOCHECK & info->mapflags) && is_my_ipv6_addr(
			NF_INET_PRE_ROUTING == par->hooknum ? par->in : par->out, &hdr->saddr))
		{
			return NF_DROP;
		}
	}

	pr_devel("MAP66: exit in=%s, out=%s, saddr=" NIP6_FMT ", daddr=" NIP6_FMT "\n",
		NULL != par->in ? par->in->name : "",
		NULL != par->out ? par->out->name : "",
		NIP6(hdr->saddr), NIP6(hdr->daddr));

	return IP6T_CONTINUE;
}

static bool MAP66_tg6_check(
	const struct xt_tgchk_param *par)
{
	const struct ip6t_MAP66_info *info = par->targinfo;

	if (0 == ((IP6T_MAP66_OPT_DST_TO | IP6T_MAP66_OPT_SRC_TO) & info->mapflags)) {
		printk("MAP66: No --" IP6T_MAP66_DST_TO " nor --" IP6T_MAP66_SRC_TO "\n");
		return false;
	}

	if (0 != (IP6T_MAP66_OPT_DST_TO & info->mapflags) && (0 >= info->pfix_dst_len ||
	   (0 != (IP6T_MAP66_OPT_CSUM & info->mapflags) ? 8 : 7) < info->pfix_dst_len))
	{
		if (8 == info->pfix_dst_len) {
			printk("MAP66: --" IP6T_MAP66_DST_TO " prefix length /%d only possible with --csum\n", 16 * info->pfix_dst_len);
		}
		else {
			printk("MAP66: Unsupported --" IP6T_MAP66_DST_TO " prefix length /%d\n", 16 * info->pfix_dst_len);
		}
		return false;
	}

	if (0 != (IP6T_MAP66_OPT_SRC_TO & info->mapflags) && (0 >= info->pfix_src_len ||
	   (0 != (IP6T_MAP66_OPT_CSUM & info->mapflags) ? 8 : 7) < info->pfix_src_len))
	{
		if (8 == info->pfix_src_len) {
			printk("MAP66: --" IP6T_MAP66_SRC_TO " prefix length /%d only possible with --csum\n", 16 * info->pfix_src_len);
		}
		else {
			printk("MAP66: Unsupported --" IP6T_MAP66_SRC_TO " prefix length /%d\n", 16 * info->pfix_src_len);
		}
		return false;
	}
	return true;
}

#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)

static unsigned int MAP66_tg6_24(
	struct sk_buff **pskb,
	unsigned int hooknum,
	const struct net_device *in,
	const struct net_device *out,
	const void *targinfo,
	void *userinfo)
{
	const struct xt_target_param par = {
		.in = in,
		.out = out,
		.hooknum = hooknum,
		.targinfo = targinfo,
	};
	return MAP66_tg6(*pskb, &par);
}

static int MAP66_tg6_check_24(
	const char *table,
	const struct ip6t_entry *entry,
	void *targinfo,
	unsigned int targinfosize,
	unsigned int hook_mask)
{
	const struct xt_tgchk_param par = {
		.targinfo = targinfo,
	};
	if (0 != (hook_mask & ~((1 << NF_IP6_PRE_ROUTING) | (1 << NF_IP6_POST_ROUTING)))) {
		printk("MAP66: Only valid for PRE_ROUTING or POST_ROUTING.\n");
		return 0;
	}
	return MAP66_tg6_check(&par);
}

#elif LINUX_VERSION_CODE < KERNEL_VERSION(2,6,28)

static unsigned int MAP66_tg6_26(
	struct sk_buff *skb,
	const struct net_device *in,
	const struct net_device *out,
	unsigned int hooknum,
	const struct xt_target *target,
	const void *targinfo)
{
	const struct xt_target_param par = {
		.in = in,
		.out = out,
		.hooknum = hooknum,
		.targinfo = targinfo,
	};
	return MAP66_tg6(skb, &par);
}

static bool MAP66_tg6_check_26(
	const char *table,
	const void *entryinfo,
	const struct xt_target *target,
	void *targinfo,
	unsigned int hook_mask)
{
	const struct xt_tgchk_param par = {
		.targinfo = targinfo,
	};
	return MAP66_tg6_check(&par);
}

#endif

static struct xt_target MAP66_tg6_reg __read_mostly = {
	.name 		= "MAP66",
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
	.target 	= MAP66_tg6_24,
	.checkentry	= MAP66_tg6_check_24,
#else
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,28)
	.family		= AF_INET6,
	.target 	= MAP66_tg6_26,
	.checkentry	= MAP66_tg6_check_26,
	.destroy	= NULL,
#else
	.family		= NFPROTO_IPV6,
	.target 	= MAP66_tg6,
	.checkentry	= MAP66_tg6_check,
#endif
	.targetsize	= sizeof(struct ip6t_MAP66_info),
	.table		= "mangle",
	.hooks		= (1 << NF_INET_POST_ROUTING) | (1 << NF_INET_PRE_ROUTING),
#endif
	.me 		= THIS_MODULE,
};

static int __init MAP66_tg6_init(void)
{
	pr_devel("MAP66: register\n");
	return xt_register_target(&MAP66_tg6_reg);
}

static void __exit MAP66_tg6_exit(void)
{
	pr_devel("MAP66: unregister\n");
	xt_unregister_target(&MAP66_tg6_reg);
}

module_init(MAP66_tg6_init);
module_exit(MAP66_tg6_exit);