/* $USAGI: ipsec6_utils.c,v 1.18 2001/10/22 10:39:14 miyazawa Exp $ */

/* derived from iabg ipv6_ipsec-main.c -mk */
/*
 * IPSECv6 code
 * Copyright (C) 2000 Stefan Schlott
 * Copyright (C) 2001 Florian Heissenhuber
 * 
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation; either version 2 of the License, or (at your
 * option) any later version.  See <http://www.fsf.org/copyleft/gpl.txt>.
 * 
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
 * for more details.
 *
 */

#include <linux/config.h>
#include <linux/module.h>
#include <linux/init.h>
#include <linux/kernel.h>
#include <linux/ipv6.h>
#include <linux/tcp.h>
#include <linux/udp.h>
#include <linux/smp.h>
#include <linux/list.h>
#include <linux/random.h>

#include <asm/uaccess.h>

#include <net/ipv6.h>
#include <net/sadb.h>
#include <net/spd.h>

#include <linux/ipsec6.h>

#include <net/ip6_route.h>
#include <net/addrconf.h>
#include <net/snmp.h>  
#include <linux/in.h>
#include <linux/in6.h>
#include <linux/ipsec.h>
#include <net/ipsec6_utils.h>
#include <linux/ip6packettools.h>
#include <net/pfkeyv2.h>
#include "calc_hmac.h"


/* DEBUG *******************************************/
#ifdef CONFIG_IPV6_IPSEC_DEBUG 
#define ipsec6_pr(fmt, arg...) printk(KERN_DEBUG fmt,##arg)
#else
#define ipsec6_pr(fmt,arg...) \
        do { } while (0)
#endif /* CONFIG_IPV6_IPSEC_DEBUG */
/**************************************************/

/* mk added */
/* XXX must insert window size  sa_replay_window member */
int check_replay_window(struct sa_replay_window *rw, __u32 hdrseq)
{       
        __u32 diff;
	__u32 seq = ntohl(hdrseq);

	ipsec6_pr(
"check_replay_window: overflow: %u
                          size: %u
                       seq_num: %x
                      last_seq: %x
                        bitmap: %08x
                      curr seq: %x\n",
			rw->overflow, rw->size, rw->seq_num, rw->last_seq, rw->bitmap, seq);
	if (seq == 0) {
		return 0; /* first == 0 or wrapped */
	}

	if (seq > rw->last_seq) { /* new larger sequence number */
		diff = seq - rw->last_seq;
		if (diff < rw->size) {  /* In window */
			rw->bitmap <<= diff;
			rw->bitmap |= 1; /* set bit for this packet */
		} else {
			rw->bitmap = 1; /* This packet has a "way larger" */
		}

		rw->last_seq = seq;
		return 1; /* larger is good */
	}

	diff = rw->last_seq - seq;

	if (diff >= rw->size) {
	       return 0; /* too old or wrapped */
	}

	if ( rw->bitmap & ((u_long)1 << diff) ) {
	       return 0; /* already seen */
	}

	rw->bitmap |= ((u_long)1 << diff); /* mark as seen */

	return 1; /* out of order but good */
}

/* Greatest common divisor */
inline int __gcd(int a, int b) {
        int c;
#if 0 /* check by caller */
	if (a == b)
		return a;
	elsif (a < b)
		return __gcd(b,a);
        else 
#endif
	while (b != 0) {
                c = a;
                a = b;
                b = c % a;
        }
        return a;
}

/* Least common multiple */
inline int lcm(int a, int b) {
        if (a == 0 || b == 0) return 0;
	if (a == b) return a;
	return (a > b) ? __gcd(a,b) : __gcd(b,a);
}

char* build_whole_ip6_packet(struct sk_buff *skb, const void *data, 
        unsigned length, inet_getfrag_t getfrag, int *packetlen) 
{
        int totallen;
        char* result;
        
        if (!skb) return NULL;
        if (!data || !getfrag) length=0;
        totallen = ntohs(skb->nh.ipv6h->payload_len) + sizeof(struct ipv6hdr);
        if (totallen != skb->len + length) {
                if (net_ratelimit()) {
                        printk(KERN_WARNING "build_whole_ip6_packet: Something is very wrong...\n");
                        printk(KERN_DEBUG " skb->len = %u\n",skb->len);
                        printk(KERN_DEBUG " payload_len = %u\n",ntohs(skb->nh.ipv6h->payload_len));
                }
                return NULL;
        }
        result = kmalloc(totallen,GFP_ATOMIC);
        if (packetlen) *packetlen = totallen;
        if (!result) return NULL;
        memcpy(result,skb->nh.ipv6h,skb->len);
        if (length>0) {
                struct in6_addr *addr;

                addr=&(skb->nh.ipv6h->saddr);
                getfrag(data,addr,&result[skb->len],0,length);
        }
        return result;
}


/* Set all mutable/unpredictable fields to zero. */
void prepare_hash_in_calculation(struct ip6_packet_refs *refs) 
{
        if (!refs->ipv6hdr) {
#ifdef IPSEC6_DEBUG
		ipsec6_pr("prepare_hash_in_calculation: No ipv6hdr given!\n");
#endif /* IPSEC6_DEBUG */
                return;
        }

        /* Main header */
        refs->ipv6hdr->tclass1=0;
        refs->ipv6hdr->tclass2_flow[0]=0;
        refs->ipv6hdr->tclass2_flow[1]=0;
        refs->ipv6hdr->tclass2_flow[2]=0;
        refs->ipv6hdr->hop_limit=0;
        /* Mutable/unpredictable Option headers */
        /* AH header */

        if (refs->authhdr) {
                int len = (refs->authhdr->hdrlen*4)-4;
                memset(&(refs->authhdr->auth_data),0,len);
        }
}

/* Set all mutable/predictable fields to the destination state, and all
   mutable/unpredictable fields to zero. */
void prepare_hash_out_calculation(struct ip6_packet_refs *refs) 
{
        prepare_hash_in_calculation(refs);
        /* destination address with routing header ext. */
        /* Routing header */
        /* Mutable/predictable Options */
}


void do_ipsec6_out_calc_ah(struct sock *sk, const void *data,
        unsigned length, inet_getfrag_t getfrag,
        struct sk_buff *skb, struct ipv6_auth_hdr *authhdr, void *ipsec_ptr)
{
	struct ipsec_sp *policy = (struct ipsec_sp*)ipsec_ptr;
        struct ipsec_sa *sa = NULL;
        char* packet;
        int packetlen,authhdroffset;
        struct ip6_packet_refs refs;
        __u8* authdata;
	
	ipsec6_pr("do_ipsec6_calc_ah called.\n");
	if(!policy){
		ipsec6_pr("do_ipsec6_out_calc_ah: ipsec6_ptr is NULL\n");
		return;
	}

	read_lock(&policy->lock);
	if (!policy->auth_sa_idx || !policy->auth_sa_idx->sa) {
		if (net_ratelimit())
			printk(KERN_DEBUG "do_ipsec6_out_enc(): SA missing.\n");
		read_unlock(&policy->lock);
		return;
	}
	ipsec_sa_hold(policy->auth_sa_idx->sa);
	sa = policy->auth_sa_idx->sa;
	read_unlock(&policy->lock);


        if (!data) length=0;
        if (!skb) {
		ipsec6_pr("do_ipsec6_out_cal_ah: skb is NULL!\n");
                return;
        }
        /* Handle sequence number */

        /* Build packet for calculation */
        packet = build_whole_ip6_packet(skb,data,length,getfrag,&packetlen);
        if (!packet) return;
        analyze_ip6_packet(packet,&refs);
        /* If authhdr not given: Go find it */
        if (!authhdr) {
                authhdroffset = ((char*)refs.authhdr)-packet;
                authhdr = (struct ipv6_auth_hdr*)(((char*)skb->nh.ipv6h)+authhdroffset);
        }
        /* Set authhdr values */
        /* authhdr->spi = htonl(sa->spi); */
	ipsec6_pr("do_ipsec6_out_calc_ah: spi is 0x%x\n", ntohl(sa->spi));
	authhdr->spi = sa->spi; /* -mk */
        authhdr->seq_no = htonl(sa->replay_window.seq_num);
	ipsec6_pr("do_ipsec6_out_calc_ah: authhdr->spi is 0x%x\n", ntohl(authhdr->spi));
        refs.authhdr->spi = authhdr->spi;
        refs.authhdr->seq_no = authhdr->seq_no;
        /* Finally: Calculate and copy checksum */
        prepare_hash_out_calculation(&refs);

	write_lock(&sa->lock);
        sa->replay_window.seq_num++;
        /* TODO: Because 2 packets increment seq_num at the same time,
                seq_num maybe increamented by atomic_inc */
	authdata=kmalloc(sa->auth_algo.digest_len, GFP_ATOMIC);
	calc_hmac(sa, packet, packetlen, authdata);          
	memcpy(&authhdr->auth_data[0],authdata,sa->auth_algo.digest_len);                                                     
	write_unlock(&sa->lock);
	ipsec_sa_put(sa);

	ipsec6_pr( "do_ipsec6_sout_calc_ah: Packet structure:\n");
	print_ip6packet(refs);
	ipsec6_pr("do_ipsec6_out_calc: End packet info (out).\n");
        kfree(authdata);
        kfree(packet); 
}


int do_ipsec6_out_req_header_size(int action, struct sock *sk, void *ipsec_ptr) 
{
        int result = 0;
	struct ipsec_sp *policy = (struct ipsec_sp*)ipsec_ptr;
        struct ipsec_sa *sa_ah = NULL;
        struct ipsec_sa *sa_esp = NULL;

	ipsec6_pr("do_ipsec6_req_header_size called.\n");

        if (!policy) return 0;

	write_lock(&policy->lock);
	if (policy->auth_sa_idx && policy->auth_sa_idx->sa) {
		ipsec_sa_hold(policy->auth_sa_idx->sa);
		sa_ah = policy->auth_sa_idx->sa;
	}

	if (policy->esp_sa_idx && policy->esp_sa_idx->sa) {
		ipsec_sa_hold(policy->esp_sa_idx->sa);
		sa_esp = policy->esp_sa_idx->sa;
	}
	write_unlock(&policy->lock);

        if (sa_ah) {
		read_lock(&sa_ah->lock);
		if ( sa_ah->auth_algo.algo != SADB_AALG_NONE) {
			result += sizeof(struct ipv6_auth_hdr)-4;
			result += sa_ah->auth_algo.digest_len;
			/* 64 bit alignment */
			if ((sa_ah->auth_algo.digest_len) %8 != 0){
				result += 8 - ((sa_ah->auth_algo.digest_len) % 8);
			}
		}
		read_unlock(&sa_ah->lock);
		ipsec_sa_put(sa_ah);
        }

        if (sa_esp) {
		read_lock(&sa_esp->lock);
		if ( sa_esp->auth_algo.algo != SADB_EALG_NONE){
			result += sizeof(struct ipv6_esp_hdr) - 8;
			result += sa_esp->esp_algo.cx.ci->ivsize;
			result += sa_esp->esp_algo.cx.ci->blocksize;
			result += lcm(sa_esp->esp_algo.cx.ci->blocksize, 8);
		}else{
			return 0;
		}
		if ( sa_esp->auth_algo.algo != SADB_AALG_NONE) {
			result += sizeof(struct ipv6_auth_hdr)-4;
			result += sa_esp->auth_algo.digest_len;
			/* 64 bit alignment */
			if ((sa_esp->auth_algo.digest_len)%8 != 0){
				result += 8 - ((sa_esp->auth_algo.digest_len) % 8);
			}
		}
		read_unlock(&sa_esp->lock);
		ipsec_sa_put(sa_esp);
        }
	ipsec6_pr("do_ipsec6_req_header_size: Calculated size is %d.\n", result);
        return result;
}


struct ipv6_txoptions * do_ipsec6_out_insert_ah(struct sock *sk,
	const void *data, unsigned length, struct ipv6_txoptions *opt,
	void *ipsec_ptr) {
	struct ipv6_txoptions *dummyopt = NULL;
	struct ipv6_auth_hdr *dummyauthhdr = NULL;
	struct ipsec_sp *policy = (struct ipsec_sp*)ipsec_ptr;
	struct ipsec_sa *sa = NULL;
	int ahlen;
	
	ipsec6_pr("do_ipsec6_out_insert_ah called.\n");
	if (!policy) {
		if (net_ratelimit())
			printk(KERN_INFO"do_ipsec6_out_insert_ah: ipsec6_ptr/policy is NULL.\n");
	}

	read_lock(&policy->lock);
	if (!policy->auth_sa_idx || !policy->auth_sa_idx->sa) {
		if (net_ratelimit())
			printk(KERN_DEBUG "do_ipsec6_insert_ah(): SA missing.\n");
		read_unlock(&policy->lock);
		return NULL;
	}
	ipsec_sa_hold(policy->auth_sa_idx->sa);
	sa = policy->auth_sa_idx->sa;
	read_unlock(&policy->lock);

	write_lock(&sa->lock);
	ipsec6_pr("do_ipsec6_out_insert_ah: use kerneli version\n");
	if ( sa->auth_algo.algo == SADB_AALG_NONE ) {
		if (net_ratelimit())
			printk(KERN_INFO "do_ipsec6_out_insert_ah: Hash algorithm %d not present.\n",
				sa->auth_algo.algo);
		write_unlock(&sa->lock);
		ipsec_sa_put(sa);
	}

	ahlen = sizeof(struct ipv6_auth_hdr) - 4 + sa->auth_algo.digest_len;
	if ((sa->auth_algo.digest_len - 4) % 8 != 0)
		ahlen += 8 - ((sa->auth_algo.digest_len - 4) % 8);
	ipsec6_pr("do_ipsec6_out_insert_ah: used kerneli version\n");
	ipsec6_pr("do_ipsec6_out_insert_ah: ahlen=%d hash_size=%d\n", ahlen, sa->auth_algo.digest_len);
	write_unlock(&sa->lock);
	ipsec_sa_put(sa);

	if (opt) {
		dummyopt = kmalloc(opt->tot_len + ahlen, GFP_ATOMIC);
		if (!dummyopt) {
			if (net_ratelimit())
				printk(KERN_WARNING "Couldn't allocate AH header - out of memory.\n");
			return NULL;
		}
		memcpy(dummyopt, opt, opt->tot_len);
		/* Oh Jesus... please adjust pointers of new dummyopt! (Todo) */
		dummyauthhdr = (struct ipv6_auth_hdr*) (((__u8*)dummyopt) + opt->tot_len);
	} else {
		dummyopt = kmalloc(sizeof(struct ipv6_txoptions) + ahlen, GFP_ATOMIC);
		if (!dummyopt) {
			if (net_ratelimit())
				printk(KERN_WARNING "Couldn't allocate AH header - out of memory.\n");
			return NULL;
		}
		memset(dummyopt, 0, sizeof(struct ipv6_txoptions));
		dummyauthhdr = (struct ipv6_auth_hdr*) (((__u8*)dummyopt) + sizeof(struct ipv6_txoptions));
	}
	memset(dummyauthhdr, 0, ahlen);
	dummyopt->auth = (struct ipv6_opt_hdr*) dummyauthhdr;
	dummyopt->tot_len += ahlen;
	dummyopt->opt_flen += ahlen;
	dummyauthhdr->hdrlen = (ahlen/4) - 2;
	ipsec6_pr("do_ipsec6_insert_ah: Dummy auth header inserted in opt\n"); 
	return dummyopt;
}

/*** ESP ***/

void do_ipsec6_out_enc(struct sock *sk, const void *data, unsigned length,
			u8 proto, void **newdata, unsigned *newlength, void *ipsec_ptr) 
{
	struct ipsec_sp *policy = (struct ipsec_sp*)ipsec_ptr;
        struct ipsec_sa *sa = NULL;
	struct ipv6_esp_hdr *esphdr = NULL;
	u8* srcdata = NULL;
	u8* authdata = NULL;
	int encblocksize = 0;
	int encsize = 0, hashsize = 0, totalsize = 0;
	int i;

	ipsec6_pr(KERN_DEBUG "do_ipsec6_out_enc() called. "
			"Data ptr is %p, data length is %d.\n",data,length);

	if (!policy) {
		if (net_ratelimit())
			printk(KERN_DEBUG "do_ipsec6_out_enc(); ipsec_ptr/policy is NULL\n");
		return;
	}

	read_lock(&policy->lock);
	if (!policy->esp_sa_idx || !policy->esp_sa_idx->sa) {
		if (net_ratelimit())
			printk(KERN_DEBUG "do_ipsec6_out_enc(): SA missing.\n");
		read_unlock(&policy->lock);
		return;
	}
	ipsec_sa_hold(policy->esp_sa_idx->sa);
	sa = policy->esp_sa_idx->sa;
	read_unlock(&policy->lock);

	write_lock(&sa->lock);
	/* Get algorithms */
	if (sa->esp_algo.algo == SADB_EALG_NONE) {
		printk(KERN_DEBUG "do_ipsec6_out_enc(): Encryption algorithm not present.\n");
		goto unlock_finish;
		return;
	}
	if (!(sa->esp_algo.cx.ci)){
		printk(KERN_DEBUG "do_ipsec6_out_enc(): Encryption algorithm not present.\n");
		goto unlock_finish;
		return;
	}

	/* Calculate size */
	encblocksize = lcm(sa->esp_algo.cx.ci->blocksize,8);
	encsize = length + 2 - ((length+2)%encblocksize);
	if ((length+2)%encblocksize != 0)
		encsize += encblocksize;
		
	/* The tail of payload does not have to be aligned with a multiple number of 64 bit.	*/
	/* 64 bit alignment is adapted to the position of top of header. 			*/

	if (sa->auth_algo.algo != SADB_AALG_NONE)
		hashsize = sa->auth_algo.digest_len;


	totalsize = sizeof(struct ipv6_esp_hdr) - 8 + sa->esp_algo.cx.ci->ivsize + encsize + hashsize;
	ipsec6_pr("IV size=%d, enc size=%d hash size=%d, total size=%d\n",
			 sa->esp_algo.cx.ci->ivsize, encsize, hashsize, totalsize);
	
	/* Get memory */
	esphdr = kmalloc(totalsize, GFP_ATOMIC);
	srcdata = kmalloc(encsize, GFP_ATOMIC);
	if (!esphdr || !srcdata) {
		if (net_ratelimit())
			printk(KERN_WARNING "do_ipsec6_out_enc: Out of memory.\n");
		if (esphdr) kfree(esphdr);
		goto unlock_finish;
		return;
	}

	memset(esphdr, 0, totalsize);
	memset(srcdata, 0, encsize);
	/* Handle sequence number and fill in header fields */
	sa->replay_window.seq_num++;
	esphdr->spi = sa->spi;
	esphdr->seq_no = htonl(sa->replay_window.seq_num++);
	/* Get source data, fill in padding and trailing fields */
	memcpy(srcdata, data, length);
	for (i = length; i < encsize-2; i++) 
		srcdata[i] = (u8)(i-length+1);
	srcdata[encsize-2] = (encsize-2)-length;
	ipsec6_pr("do_ipsec6_out_enc(): length=%d, encsize=%d\n", length, encsize);
	ipsec6_pr("do_ipsec6_out_enc(): encsize-2=%d\n", srcdata[encsize-2]);
	srcdata[encsize-1] = proto;
	/* Do encryption */

	memset(sa->esp_algo.cx.iv, 0, MAX_IV_SIZE);
	get_random_bytes(sa->esp_algo.cx.iv, sa->esp_algo.cx.ci->ivsize);
#ifdef CONFIG_IPV6_IPSEC_DEBUG
	{
		int i;
		ipsec6_pr("do_ipsec6_out_enc(): IV is 0x");
		for (i=0; i < sa->esp_algo.cx.ci->ivsize ; i++) {
			ipsec6_pr("%x", sa->esp_algo.cx.iv[i]);
		}
		ipsec6_pr("\n");
	}
#endif /* CONFIG_IPV6_IPSEC_DEBUG */
	sa->esp_algo.cx.ci->encrypt_atomic(&(sa->esp_algo.cx), srcdata,
					(char*)&(esphdr->enc_data) + sa->esp_algo.cx.ci->ivsize, encsize);
	memcpy(esphdr->enc_data, sa->esp_algo.cx.iv, sa->esp_algo.cx.ci->ivsize);
	kfree(srcdata);
	srcdata=NULL;

	if(sa->auth_algo.algo){
		authdata = kmalloc(sa->auth_algo.digest_len, GFP_ATOMIC);
		if (!authdata) {
			if (net_ratelimit())
				printk(KERN_WARNING "do_ipsec6_out_enc: Out of memory.\n");
			kfree(esphdr);
			goto unlock_finish;
			return;
		}
		memset(authdata, 0, sa->auth_algo.digest_len);
		calc_hmac(sa, (char*)esphdr, totalsize-hashsize, authdata);
		memcpy(&(((char*)esphdr)[8 + sa->esp_algo.cx.ci->ivsize + encsize]),
				authdata, sa->auth_algo.digest_len);
		kfree(authdata);
	}	
	write_unlock(&sa->lock);
	ipsec_sa_put(sa);

	authdata = NULL;
	/* Set return values */
	*newdata = esphdr;
	*newlength = totalsize;
	return;

unlock_finish:
	write_unlock(&sa->lock);
	ipsec_sa_put(sa);
	return;
}

void ipsec6_out_finish(struct ipv6_txoptions *opt, void *ipsec_ptr) {

	if (opt) {
		kfree(opt);
	}

	if (ipsec_ptr) {
		ipsec_sp_put((struct ipsec_sp*) ipsec_ptr);
	}
}

