#include <linux/config.h>
#include <linux/module.h>
#include <linux/init.h>
#include <asm/byteorder.h>
#include <linux/kernel.h>
#include <linux/errno.h>
#include <linux/sysctl.h>
#include <linux/inet.h>
#include <linux/ipv6.h>
#include <linux/smp.h>
#include <linux/list.h>
#include <linux/skbuff.h>
#include <asm/uaccess.h>
#include <net/ipv6.h>
#include <net/sadb.h>
#include <net/ipcomp.h>
#include <linux/ipsec.h>
#include <linux/ipsec6.h>
#include <net/pfkeyv2.h> /* sa proto type */

void ipsec6_compress(/* const */ void *data, u32 len, u8 proto,
		                void **newdata, u32 *newlen, struct ipsec_sp *policy)
{
	struct ipsec_sa *sa = NULL;
	unsigned int totallen;
	int complen;
	__u32 calgo; 
	struct ipcomp_algo_func *algo;
	__u8 *compdata = NULL;
	__u8 *totaldata = NULL;
	
	IPSEC6_DEBUG("called.\n");

	if (!data) {
		printk(KERN_ERR "ipsec6_compress: data is null.\n");
		return;
	}

	if (!policy) {
		if (net_ratelimit())
			printk(KERN_WARNING "%s: ipcomp policy is NULL\n", __FUNCTION__);
		return;
	}

	read_lock_bh(&policy->lock);
	if (!policy->comp_sa_idx || !policy->comp_sa_idx->sa) {
		if (net_ratelimit()) 
			printk(KERN_WARNING "%s: ipcomp SA missing.\n", __FUNCTION__);
		read_unlock_bh(&policy->lock);
		return;
	}
	ipsec_sa_hold(policy->comp_sa_idx->sa);
	sa = policy->comp_sa_idx->sa;
	read_unlock_bh(&policy->lock);

	write_lock_bh(&sa->lock);
	calgo = ntohl(sa->spi);
	switch (calgo) {
	case IPCOMP_RESERVED: /* not apply ipcomp */
		IPSEC6_DEBUG("comp algo: none, skip\n");
		goto unlock_finish; 
	case IPCOMP_DEFLATE:
		IPSEC6_DEBUG("comp algo: Deflate\n");
		algo = ipcomp_algo_lookup(calgo);
		if (!algo) {
			printk(KERN_ERR "%s: can't find compression algorithm.\n", __FUNCTION__);
			goto unlock_finish;
		}
		break;
	/* not supported to avoid patent issue */
	case IPCOMP_OUI:   case IPCOMP_LZS:   case IPCOMP_LZJH: 
	default:
		printk(KERN_ERR "%s: unsupported algorithm.\n", __FUNCTION__);
		goto unlock_finish; 
	}

	printk(KERN_ALERT "%s: threshold: %d\n", __FUNCTION__, algo->threshold);
	if (len < algo->threshold) { /* threshold for deflate (rfc2394) */
		printk(KERN_INFO "ipsec6_compress: data is small. skip!\n");
		goto unlock_finish;
	}

	//totallen = len - (sizeof(struct ipcomphdr) - 4) - (len <= 512 ? 32 : len >> 4); 
	//totallen = len - sizeof(struct ipcomphdr);
	totallen = len + (len>>6) + 16;
	printk(KERN_ALERT "%s: totallen: %d\n", __FUNCTION__, totallen);

	complen = algo->compress(data, len, &compdata, totallen);
	printk(KERN_ALERT "%s: back to ipsec6_compress\n", __FUNCTION__);
	printk(KERN_ALERT "%s: complen:%d\n", __FUNCTION__, complen);
	printk(KERN_ALERT "%s: compdata:%p\n", __FUNCTION__, compdata);
	if (complen < 0 || !compdata) {
		printk(KERN_ERR "ipsec6_compress: compression failed\n");
		goto unlock_finish;
	}
	totaldata = kmalloc(complen+sizeof(struct ipcomphdr), GFP_ATOMIC);
	if (!totaldata) {
		printk(KERN_ERR "ipsec6_compress: can't allocate memory.\n");
		kfree(compdata);
		goto unlock_finish;
	}

	((struct ipcomphdr *)(totaldata))->ipcomp_nh = proto;
	((struct ipcomphdr *)(totaldata))->ipcomp_flags = 0;
	((struct ipcomphdr *)(totaldata))->ipcomp_cpi = htons(calgo); /* IPCOMP_DEFLATE */
	memcpy((totaldata+sizeof(struct ipcomphdr)), compdata, complen);

	if (!sa->fuse_time) {
		sa->fuse_time = jiffies;
		sa->lifetime_c.usetime = (sa->fuse_time)/HZ;
		ipsec_sa_mod_timer(sa);
		IPSEC6_DEBUG("set fuse_time = %lu\n", sa->fuse_time);
	}
	sa->lifetime_c.bytes += totallen;
	IPSEC6_DEBUG("sa->lifetime_c.bytes=%-9u %-9u\n",        /* XXX: %-18Lu */
	(__u32)((sa->lifetime_c.bytes) >> 32), (__u32)(sa->lifetime_c.bytes));
	if (sa->lifetime_c.bytes >= sa->lifetime_s.bytes && sa->lifetime_s.bytes) {
		sa->state = SADB_SASTATE_DYING;
		IPSEC6_DEBUG("change sa state DYING\n");
	} 
	if (sa->lifetime_c.bytes >= sa->lifetime_h.bytes && sa->lifetime_h.bytes) {
		sa->state = SADB_SASTATE_DEAD;
		IPSEC6_DEBUG("change sa state DEAD\n");
	}

	write_unlock_bh(&sa->lock);
	ipsec_sa_put(sa);

	/* Set return values */
	*newdata = totaldata;
	*newlen = complen+sizeof(struct ipcomphdr);
	kfree(compdata);
	printk(KERN_ALERT "%s: normal finished.\n", __FUNCTION__);
	return;

unlock_finish:
	write_unlock_bh(&sa->lock);
	ipsec_sa_put(sa);
	printk(KERN_ALERT "%s: unlock finished.\n", __FUNCTION__);
	return;
}

int
ipsec6_input_check_comp(struct sk_buff **skb, u8 *nexthdr, struct ipcomphdr *comphdr, struct sa_index *sa_idx)
{
	int len = 0;
	int rtn = IPSEC_ACTION_DROP;
	int totallen = 0;
	int complen = 0;
	int hint_decomplen = 0;
	int decomplen = 0;
	u8 *compdata = NULL;
	u8 *decompdata = NULL;
	struct ipcomp_algo_func *algo;

	if (!(*skb && sa_idx)) {
		printk(KERN_ERR "ipsec6_input_check_comp: parameters are invalid.\n");
		goto finish;
	}

	ipv6_addr_copy(&((struct sockaddr_in6 *)&sa_idx->dst)->sin6_addr,
		&(*skb)->nh.ipv6h->daddr);
	((struct sockaddr_in6 *)&sa_idx->dst)->sin6_family = AF_INET6;
	sa_idx->prefixlen_d = 128;
	sa_idx->ipsec_proto = SADB_X_SATYPE_COMP;
	sa_idx->spi = htonl(ntohs(comphdr->ipcomp_cpi)); /* XXX */
	printk(KERN_ALERT "spi: %d\n", ntohl(sa_idx->spi));

	sa_idx->sa = sadb_find_by_sa_index(sa_idx);

	if (!sa_idx->sa) {
		printk(KERN_ERR "ipsec6_input_check_comp: not found SA for ipcmop.\n");
		goto finish;
	}

	algo = ipcomp_algo_lookup(ntohl(sa_idx->spi));
	if (!algo) {
		printk(KERN_ERR "%s: can't find compression algorithm.\n", __FUNCTION__);
		goto finish;
	}

	write_lock_bh(&sa_idx->sa->lock);

	len = ntohs((*skb)->nh.ipv6h->payload_len) + sizeof(struct ipv6hdr);
	totallen = len - (((u8 *)comphdr) - ((u8 *)(*skb)->nh.ipv6h));
	complen = totallen - sizeof(struct ipcomphdr);
	compdata = (u8 *)comphdr + sizeof(struct ipcomphdr);
	hint_decomplen = (*skb)->dev ? ( (*skb)->dev->mtu < 16260 ? 16260 : (*skb)->dev->mtu ) : (65520 - sizeof(struct ipv6hdr)) ;

	printk(KERN_ALERT "%s: totallen: %d\n", __FUNCTION__, totallen);
	printk(KERN_ALERT "%s: complen: %d\n", __FUNCTION__, complen);
	printk(KERN_ALERT "%s: compdata: %p\n", __FUNCTION__, compdata);
	printk(KERN_ALERT "%s: decomplen: %d\n", __FUNCTION__, hint_decomplen);

	decomplen = algo->decompress(compdata, complen, &decompdata, hint_decomplen);
	printk(KERN_ALERT "%s: decomplen: %d\n", __FUNCTION__, decomplen);
	printk(KERN_ALERT "%s: decompdata: %p\n", __FUNCTION__, decompdata);
	if (decomplen < 0 || !decompdata) {
		printk(KERN_ERR "ipsec6_input_check_comp: decompression failed\n");
		goto unlock_finish;
	}
	*nexthdr = comphdr->ipcomp_nh;
	memcpy(comphdr, decompdata, decomplen);

	/* XXX */
	skb_trim(*skb, (*skb)->len + decomplen - totallen);
	(*skb)->nh.ipv6h->payload_len = htons(((char *)comphdr - (char *)((*skb)->nh.ipv6h)) - sizeof(struct ipv6hdr) + decomplen);

	rtn = IPSEC_ACTION_COMP;

	/* read comment in skbuff.h */
       (*skb)->ip_summed = CHECKSUM_UNNECESSARY; 

       printk(KERN_ALERT "%s: passed\n", __FUNCTION__, decompdata);

       if (!sa_idx->sa->fuse_time) {
	       sa_idx->sa->fuse_time = jiffies;
	       sa_idx->sa->lifetime_c.usetime = (sa_idx->sa->fuse_time) / HZ;
	       ipsec_sa_mod_timer(sa_idx->sa);
	       IPSEC6_DEBUG("set fuse_time = %lu\n", (sa_idx->sa->fuse_time));
       }
       sa_idx->sa->lifetime_c.bytes += totallen;
       IPSEC6_DEBUG("sa->bytes=%-9u %-9u\n",                   /* XXX: %-18Lu */
       (__u32)((sa_idx->sa->lifetime_c.bytes) >> 32), (__u32)(sa_idx->sa->lifetime_c.bytes));
       if (sa_idx->sa->lifetime_c.bytes >= sa_idx->sa->lifetime_s.bytes && sa_idx->sa->lifetime_s.bytes) {
	       sa_idx->sa->state = SADB_SASTATE_DYING;
	       IPSEC6_DEBUG("change sa state DYING\n");
       }
       if (sa_idx->sa->lifetime_c.bytes >= sa_idx->sa->lifetime_h.bytes && sa_idx->sa->lifetime_h.bytes) {
	       sa_idx->sa->state = SADB_SASTATE_DEAD;
	       IPSEC6_DEBUG("change sa state DEAD\n");
       }

       printk(KERN_ALERT "%s: try to kfree(decompdata)...", __FUNCTION__, decompdata);
       kfree(decompdata); 
       decompdata = NULL;
       printk(KERN_ALERT "done!\n");

unlock_finish:
	write_unlock_bh(&sa_idx->sa->lock);
	ipsec_sa_put(sa_idx->sa);

finish: 
	printk(KERN_ALERT "%s: return rtn:", __FUNCTION__, rtn);
	return rtn;
}
