/*
 *
 *			   IPSEC for Linux
 *		         Preliminary Release
 * 
 *	 Copyright (C) 1996, 1997, John Ioannidis <ji@hol.gr>
 * 
 * Changes by Angelos D. Keromytis and Niels Provos
 * ported from OpenBSD 2.2 by Petr Novak, <pn@i.cz>
 *
 * 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.
 *
 * 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.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
 * USA
 *
 */

/*
 * Based on draft-ietf-ipsec-esp-v2-00.txt and
 * draft-ietf-ipsec-ciph-{des,3des}-{derived,expiv}-00.txt
 */

#include <linux/config.h>
#include <asm/segment.h>
#include <asm/system.h>
#include <linux/types.h>
#include <linux/kernel.h>
#include <linux/sched.h>
#include <linux/string.h>
#include <linux/errno.h>
#include <linux/config.h>

#include <linux/socket.h>
#include <linux/sockios.h>
#include <linux/in.h>
#include <linux/inet.h>
#include <linux/netdevice.h>
#include <linux/etherdevice.h>
#include <linux/icmp.h>
#include <linux/udp.h>
#include <net/ip.h>
#include <net/protocol.h>
#include <net/route.h>
#include <net/tcp.h>
#include <net/udp.h>
#include <net/sock.h>
#include <net/icmp.h>

#include <net/checksum.h>

#include <linux/fs.h>
#include <linux/mm.h>
#include <linux/miscdevice.h>

#include <linux/skbuff.h>
#include <linux/proc_fs.h>
#include <linux/stat.h>

#include <net/netlink.h>
#include <unistd.h>
#include "radij.h"
#include "ipsec_encap.h"
#include "ipsec_radij.h"
#include "ipsec_netlink.h"
#include "ipsec_xform.h"
#ifdef CONFIG_IPSEC_AH
#include "ipsec_ah.h"
#endif
#ifdef CONFIG_IPSEC_ESP
#include "ipsec_esp.h"
#endif

extern void des_ecb3_encrypt(caddr_t, caddr_t, caddr_t, caddr_t, caddr_t, int);
extern void des_ecb_encrypt(caddr_t, caddr_t, caddr_t, int);
extern void des_set_key(caddr_t, caddr_t);

static void des1_encrypt(void *, u_int8_t *);
static void des3_encrypt(void *, u_int8_t *);
static void blf_encrypt(void *, u_int8_t *);
static void cast5_encrypt(void *, u_int8_t *);
static void des1_decrypt(void *, u_int8_t *);
static void des3_decrypt(void *, u_int8_t *);
static void blf_decrypt(void *, u_int8_t *);
static void cast5_decrypt(void *, u_int8_t *);

struct esp_hash esp_new_hash[] = {
     { ALG_AUTH_MD5, "HMAC-MD5-96",
       AH_MD5_ALEN,
       sizeof(MD5_CTX),
       (void (*) (void *)) MD5Init,
       (void (*) (void *, u_int8_t *, u_int16_t)) MD5Update,
       (void (*) (u_int8_t *, void *)) MD5Final
     },
     { ALG_AUTH_SHA1, "HMAC-SHA1-96",
       AH_SHA1_ALEN,
       sizeof(SHA1_CTX),
       (void (*) (void *)) SHA1Init,
       (void (*) (void *, u_int8_t *, u_int16_t)) SHA1Update,
       (void (*) (u_int8_t *, void *)) SHA1Final
     },
     { ALG_AUTH_RMD160, "HMAC-RIPEMD-160-96",
       AH_RMD160_ALEN,
       sizeof(RMD160_CTX),
       (void (*)(void *)) RMD160Init,
       (void (*)(void *, u_int8_t *, u_int16_t)) RMD160Update,
       (void (*)(u_int8_t *, void *)) RMD160Final
     }
};

struct esp_xform esp_new_xform[] = {
     { ALG_ENC_DES, "Data Encryption Standard (DES)",
       ESP_DES_BLKS, ESP_DES_IVS,
       8, 8, 8 | 1,
       des1_encrypt,
       des1_decrypt
     },
     { ALG_ENC_3DES, "Tripple DES (3DES)",
       ESP_3DES_BLKS, ESP_3DES_IVS,
       24, 24, 8 | 1,
       des3_encrypt,
       des3_decrypt
     },
     { ALG_ENC_BLF, "Blowfish",
       ESP_BLF_BLKS, ESP_BLF_IVS,
       5, BLF_MAXKEYLEN, 8,
       blf_encrypt,
       blf_decrypt
     },
     { ALG_ENC_CAST, "CAST",
       ESP_CAST_BLKS, ESP_CAST_IVS,
       5, 16, 8,
       cast5_encrypt,
       cast5_decrypt
     }
};

static void
des1_encrypt(void *pxd, u_int8_t *blk)
{
     struct esp_new_xdata *xd = pxd;
     des_ecb_encrypt(blk, blk, (caddr_t) (xd->edx_eks[0]), 1);
}

static void
des1_decrypt(void *pxd, u_int8_t *blk)
{
     struct esp_new_xdata *xd = pxd;
     des_ecb_encrypt(blk, blk, (caddr_t) (xd->edx_eks[0]), 0);
}

static void
des3_encrypt(void *pxd, u_int8_t *blk)
{
     struct esp_new_xdata *xd = pxd;
     des_ecb3_encrypt(blk, blk, (caddr_t) (xd->edx_eks[0]),
		      (caddr_t) (xd->edx_eks[1]),
		      (caddr_t) (xd->edx_eks[2]), 1);
}

static void
des3_decrypt(void *pxd, u_int8_t *blk)
{
     struct esp_new_xdata *xd = pxd;
     des_ecb3_encrypt(blk, blk, (caddr_t) (xd->edx_eks[2]),
		      (caddr_t) (xd->edx_eks[1]),
		      (caddr_t) (xd->edx_eks[0]), 0);
}

static void
blf_encrypt(void *pxd, u_int8_t *blk)
{
     struct esp_new_xdata *xd = pxd;
     Blowfish_encipher(&xd->edx_bks, (u_int32_t *)blk,
		       (u_int32_t *) (blk + 4));
}

static void
blf_decrypt(void *pxd, u_int8_t *blk)
{
     struct esp_new_xdata *xd = pxd;
     Blowfish_decipher(&xd->edx_bks, (u_int32_t *)blk,
		       (u_int32_t *) (blk + 4));
}

static void
cast5_encrypt(void *pxd, u_int8_t *blk)
{
     struct esp_new_xdata *xd = pxd;
     cast_encrypt(&xd->edx_cks, blk, blk);
}

static void
cast5_decrypt(void *pxd, u_int8_t *blk)
{
     struct esp_new_xdata *xd = pxd;
     cast_decrypt(&xd->edx_cks, blk, blk);
}

#ifdef DEBUG_IPSEC_ESP
static void
print_ip_pkt(struct iphdr *ip)
{
	int i, len;
	unsigned char *d = (unsigned char *)ip;
	len = ntohs(ip->tot_len);
	
	printk("packet length: %d", len);
	for (i = 0; i < len; i++)
		printk(" %02x", d[i]);
	printk("\n");
}
#endif

#ifdef DEBUG_IPSEC_ESP
static void dmp(char *s, caddr_t bb, int len)
{
        int i;
        unsigned char *b = bb;

        if (debug_ah & DB_AH_DMP)
        {
                printk("at %s, len=%d:", s, len);
                for (i=0; i < len; i++)
                        printk(" %02x", *b++);
                printk("\n");
        }
}
#else
#define dmp(_x, _y, _z)
#endif


	
/*
 * Check-replay-window routine, adapted from the original 
 * by J. Hughes, from draft-ietf-ipsec-esp-des-md5-03.txt
 *
 *  This is a routine that implements a 32 packet window. This is intend-
 *  ed on being an implementation sample.
 *
 * return 0 on success
 * return 1 for counter == 0
 * return 2 for very old packet
 * return 3 for packet within current window but already received
 */

int
checkreplaywindow32(__u32 seq, __u32 initial, __u32 *lastseq,
		__u32 window, __u32 *bitmap)
{
	__u32 diff;

	seq -= initial;

	if (seq == 0) 
		return 1;

	if (seq > *lastseq - initial)	/* new larger sequence number */
	{
		diff = seq - (*lastseq - initial);
		if (diff < window) 	/* In win, set bit for this pkt */
			*bitmap = ((*bitmap) << diff) | 1;
		else
			*bitmap = 1;		/* This packet has way larger */

		*lastseq = seq + initial;
		return 0;			/* larger is good */
	}

	diff = *lastseq - initial - seq;
	if (diff >= window)			/* too old or wrapped */
		return 2;
	
	if (*bitmap & (((__u32)1) << diff))	/* this packet already seen */
		return 3;
	*bitmap |= (((__u32)1) << diff);	/* mark as seen */
	return 0;				/* out of order but good */
}

int
esp_new_attach(void)
{
	printk("esp_new_attach: called.\n");
	return 0;
}

/*
 * esp_new_init() is called when an SPI is being set up. It interprets the
 * encap_msghdr present in em, and sets up the transformation data, in
 * this case, the encryption and decryption key schedules
 */

int
esp_new_init(struct tdb *tdbp, struct xformsw *xsp, struct encap_msghdr *em)
{
	struct esp_new_xdata *xd;
	struct esp_new_xencap txd;
	struct esp_xform *txform;
	struct esp_hash *thash = NULL;
	caddr_t buffer = NULL;
	__u32 rk[14];
	int i;
	int blocklen = HMAC_BLOCK_LEN;


	if (em->em_msglen - EMT_SETSPI_FLEN <= ESP_NEW_XENCAP_LEN)
	{
		printk("esp_new_init(): initialization failed\n");
		return EINVAL;
	}

	/* Just copy the standard fields */
	memcpy(&txd, em->em_dat, ESP_NEW_XENCAP_LEN);

	/* Check wheather the encryption algorithm is supported */
	for (i = sizeof(esp_new_xform) / sizeof(struct esp_xform) - 1; i >= 0; i--)
		if (txd.edx_enc_algorithm == esp_new_xform[i].type)
			break;

	if (i < 0)
	{
		printk("esp_new_init(): unsupported encryption algorithm %d specified\n", txd.edx_enc_algorithm);
		return EINVAL;
	}

	txform = &esp_new_xform[i];

#ifdef	DEBUG_IPSEC_ESP
		printk("esp_new_init(): initialized TDB with enc algorithm %d: %s\n",
			txd.edx_enc_algorithm, esp_new_xform[i].name);
#endif

	/* Check wheather the authentication algorithm is supported */
	if (txd.edx_flags & ESP_NEW_FLAG_AUTH)
	{
		for (i = sizeof(esp_new_hash) / sizeof(struct esp_hash) - 1; i>=0; i--)
			if (txd.edx_hash_algorithm == esp_new_hash[i].type)
				break;

		if (i < 0)
		{
			printk("esp_new_init(): unsupported authentication algorithm %d specified\n", txd.edx_hash_algorithm);
			return EINVAL;
		}

#ifdef	DEBUG_IPSEC_ESP
			printk("esp_new_init(): initialized TDB with hash algorithm %d: %s\n",
				txd.edx_hash_algorithm, esp_new_hash[i].name);
#endif

		blocklen = HMAC_BLOCK_LEN;
		thash = &esp_new_hash[i];
	}

	if (txd.edx_ivlen + txd.edx_confkeylen + txd.edx_authkeylen +
		EMT_SETSPI_FLEN + ESP_NEW_XENCAP_LEN != em->em_msglen)
	{
		printk("esp_new_init(): message length %d doesn't match\n",
			em->em_msglen);
		return EINVAL;
	}

	/* Check the IV length */
	if (((txd.edx_ivlen == 0) && !(txform->ivmask&1)) ||
 	    ((txd.edx_ivlen != 0) && (
		!(txd.edx_ivlen & txform->ivmask) ||
		(txd.edx_ivlen & (txd.edx_ivlen - 1)))))
	{
		printk("esp_new_init(): unsupported IV length %d\n", txd.edx_ivlen);
		return EINVAL;
	}

	/* Check the key length */
	if (txd.edx_confkeylen < txform->minkey ||
	    txd.edx_confkeylen > txform->maxkey)
	{
		printk("esp_new_init(): bad key length %d\n", txd.edx_confkeylen);
		return EINVAL;
	}

	tdbp->tdb_xdata = (caddr_t)kmalloc(sizeof (struct esp_new_xdata), GFP_ATOMIC);

	if (tdbp->tdb_xdata == NULL)
	{
		printk("esp_new_init(): malloc failed for xdata\n");
	  	return ENOBUFS;
	}

	memset(tdbp->tdb_xdata, 0, sizeof (struct esp_new_xdata));

	xd = (struct esp_new_xdata *)tdbp->tdb_xdata;

	/* Pointer to the transform */
	tdbp->tdb_xform = xsp;

	xd->edx_ivlen = txd.edx_ivlen;
	xd->edx_enc_algorithm = txd.edx_enc_algorithm;
	xd->edx_wnd = txd.edx_wnd;
	xd->edx_flags = txd.edx_flags;
	xd->edx_hash_algorithm = txd.edx_hash_algorithm;
	xd->edx_bitmap = 0;
	xd->edx_xform = txform;

	/* Pass name of enc algorithm for procfs */
	tdbp->tdb_confname = xd->edx_xform->name;

	/* Replay counters are mandatory, even without auth */
	xd->edx_rpl = AH_HMAC_INITIAL_RPL;

	/* Copy the IV */
	memcpy(xd->edx_iv, em->em_dat + ESP_NEW_XENCAP_LEN, xd->edx_ivlen);

	/* Copy the key material */
	memcpy(rk, em->em_dat + ESP_NEW_XENCAP_LEN + xd->edx_ivlen, txd.edx_confkeylen);

	switch (xd->edx_enc_algorithm)
	{
		case ALG_ENC_DES:
			des_set_key((caddr_t)rk, (caddr_t)(xd->edx_eks[0]));
			break;

		case ALG_ENC_3DES:
			des_set_key((caddr_t)rk, (caddr_t)(xd->edx_eks[0]));
			des_set_key((caddr_t)(rk+2), (caddr_t)(xd->edx_eks[1]));
			des_set_key((caddr_t)(rk+4), (caddr_t)(xd->edx_eks[2]));
			break;
		case ALG_ENC_BLF:
			blf_key(&xd->edx_bks, (caddr_t)rk, txd.edx_confkeylen);
			break;
		case ALG_ENC_CAST:
			cast_setkey(&xd->edx_cks, (caddr_t)rk, txd.edx_confkeylen);
			break;
	}

	if (txd.edx_flags & ESP_NEW_FLAG_AUTH)
	{
		xd->edx_hash = thash;

		/* Pass name if auth algorithm for procfs */
		tdbp->tdb_authname =  xd->edx_hash->name;

#ifdef DEBUG_IPSEC_ESP
		printk("esp_new_init(): using %d bytes of authentication key\n",
			txd.edx_authkeylen);
#endif

		buffer = (caddr_t)kmalloc(txd.edx_authkeylen < blocklen ? blocklen : txd.edx_authkeylen, GFP_ATOMIC);

		if (buffer == NULL)
		{
			printk("esp_new_init(): malloc failed for buffer\n");
			kfree(tdbp->tdb_xdata);
			return ENOBUFS;
		}

		memset(buffer, 0, txd.edx_authkeylen < blocklen ?
			blocklen : txd.edx_authkeylen);

		/* Copy the key to the buffer */
		memcpy(buffer, em->em_dat + ESP_NEW_XENCAP_LEN + xd->edx_ivlen + txd.edx_confkeylen, txd.edx_authkeylen);

		/* Shorten the key if necessary */
		if (txd.edx_authkeylen > blocklen)
		{
			xd->edx_hash->Init(&xd->edx_ictx);
			xd->edx_hash->Update(&xd->edx_ictx, buffer, txd.edx_authkeylen);
			memset(buffer, 0, txd.edx_authkeylen < blocklen ?
				blocklen : txd.edx_authkeylen);
			xd->edx_hash->Final(buffer, &xd->edx_ictx);
		}

		/* Precompute the I and O pads of the HMAC */
		for (i = 0; i < blocklen; i++)
			buffer[i] ^= HMAC_IPAD_VAL;

		xd->edx_hash->Init(&xd->edx_ictx);
		xd->edx_hash->Update(&xd->edx_ictx, buffer, blocklen);

		for (i = 0; i < blocklen; i++)
			buffer[i] ^= (HMAC_IPAD_VAL ^ HMAC_OPAD_VAL);

		xd->edx_hash->Init(&xd->edx_octx);
		xd->edx_hash->Update(&xd->edx_octx, buffer, blocklen);

		memset(buffer, 0, blocklen);
		kfree(buffer);
	}

	memset(rk, 0, 14*sizeof(__u32));		/* paranoid */
	memset(ipseczeroes, 0, IPSEC_ZEROES_SIZE);	/* paranoid */

	return 0;
}

int
esp_new_zeroize(struct tdb *tdbp)
{
#ifdef	DEBUG_IPSEC_ESP
		printk("esp_new_zeroize(): freeing memory\n");
#endif

	if (tdbp->tdb_xdata)
	{
		kfree(tdbp->tdb_xdata);
		tdbp->tdb_xdata = NULL;
	}

	return 0;
}

int
esp_new_print(void *xdat, char *buf)
{
	int size;
	int i, ds;
	unsigned long flags;

	struct esp_new_xdata *xd = (struct esp_new_xdata *)xdat;
	flags = xd->edx_flags;

	size = sprintf(buf, "enc alg=%d (%s) ivlen=%d wnd=%d flags=%08lx", xd->edx_enc_algorithm, xd->edx_xform->name, xd->edx_ivlen, xd->edx_wnd, flags);
	buf += size;

	if (flags & ESP_NEW_FLAG_AUTH)
	{
		ds = sprintf(buf, " auth alg=%d (%s)", xd->edx_hash_algorithm, xd->edx_hash->name);
		buf += ds;
		size += ds;
	}

	if (flags & ESP_NEW_FLAG_NPADDING) 
	{
		ds = sprintf(buf, " newpadding");
		buf += ds;
		size += ds;
	}
		

	if (xd->edx_ivlen)
	{
		ds = sprintf(buf, " iv=");
		buf += ds;
		size += ds;
	
		for (i = 0; i < xd->edx_ivlen; i++)
		{
			ds = sprintf(buf, " %02x", xd->edx_iv[i]);
			buf += ds;
			size += ds;
		}
	}

	if (xd->edx_wnd >= 0)
	{
		ds = sprintf(buf, " seq = 0x%08lx, bit = %08lx", (unsigned long)xd->edx_rpl, (unsigned long)xd->edx_bitmap);
		buf += ds;
		size += ds;
	}

#ifdef ESPPRINTKEYS
	ds = sprintf(buf, ", ks = ");
	buf += ds;
	size += ds;
	
	for (i = 8; i < 384 + 8; i++)
	{
		ds = sprintf(buf, " %02x", xd->edx_iv[i]);
		buf += ds;
		size += ds;
	}
#endif
	return size;
}

int
esp_new_room(struct tdb *tp, int iphlen, int tlen, int *hr, int *tr)
{
	struct esp_new_xdata *xd = (struct esp_new_xdata *)(tp->tdb_xdata) ;
	int blks;
	
	blks = xd->edx_xform->blocksize;

	*hr += 2*sizeof(__u32) + xd->edx_ivlen; /* size of header */
		
	*tr += ((blks - ((tlen + 2) % blks)) % blks) + 2 /* padding */ +
		((xd->edx_flags & ESP_NEW_FLAG_AUTH) ? AH_HMAC_HASHLEN : 0);
	
	return 0;
}

struct sk_buff *
esp_new_input(struct sk_buff *skb, struct tdb *tdbp)
{
	struct iphdr *ipp;
	struct esp_new_xdata *xd;
	struct esp_new *espp;
	__u32 seq;
	int iphlen, esphlen, proto, len, pad, alen, ilen, blks, l, i, errc;

	HASH_CTX ctx;

	unsigned char *dat, *idat, *odat;
	unsigned char buf[AH_ALEN_MAX],	
			iv[ESP_MAX_IVS], niv[ESP_MAX_IVS], blk[ESP_MAX_BLKS];
	
	len = skb->len; dat = skb->data;

	ipp = (struct iphdr *)dat;
	iphlen = ipp->ihl << 2;
	espp = (struct esp_new *)(dat + iphlen);
	
	xd = (struct esp_new_xdata *)tdbp->tdb_xdata;
	blks = xd->edx_xform->blocksize;

	alen = (xd->edx_flags & ESP_NEW_FLAG_AUTH) ? AH_HMAC_HASHLEN : 0;

	esphlen = 2*sizeof(__u32) + xd->edx_ivlen; /* SPI, RPL, IV */

	idat = dat + iphlen;
	ilen = len - iphlen - esphlen - alen;

	if (ilen & (blks-1))
	{
		printk("ipsec_espdes: got packet with ilen = %d \n", ilen);
		return NULL;
	}

	/* Replay window checking */
	if (xd->edx_wnd >= 0)
	{
		seq = ntohl(espp->esp_rpl);

		if ((errc = checkreplaywindow32(seq, 0, &(xd->edx_rpl), xd->edx_wnd, &(xd->edx_bitmap))) != 0)
		{
			switch (errc)
			{
				case 1:
					printk("esp_new_input(): replay counter wrapped for packets from %lx to %lx, spi %08lx\n",
						(unsigned long)ipp->saddr,
						(unsigned long)ipp->daddr,
						ntohl(espp->esp_spi));
					break;

				case 2:
				case 3:
					printk("esp_new_input(): duplicate packet received, %lx->%lx spi %08lx\n",
						(unsigned long)ipp->saddr,
						(unsigned long)ipp->daddr,
						ntohl(espp->esp_spi));
				break;
			}
			return NULL;
		}
	}

	/*
	 * verify authenticator
	 */

	if (xd->edx_flags & ESP_NEW_FLAG_AUTH)
	{
		memcpy(&ctx, &(xd->edx_ictx), xd->edx_hash->ctxsize);

		/* Auth covers SPI + SN + IV and the payload */
		xd->edx_hash->Update(&ctx, idat, ilen + esphlen);
		xd->edx_hash->Final(buf, &ctx);
		memcpy(&ctx, &(xd->edx_octx), xd->edx_hash->ctxsize);
		xd->edx_hash->Update(&ctx, buf, xd->edx_hash->hashsize);
		xd->edx_hash->Final(buf, &ctx);

		if (memcmp(buf, dat+len-AH_HMAC_HASHLEN, AH_HMAC_HASHLEN))
		{
			printk("esp_new_input(): authentication failed for packet from %lx to %lx, spi %08lx\n",
				(unsigned long)ipp->saddr,
				(unsigned long)ipp->daddr,
				ntohl(espp->esp_spi));
			return NULL;
		}
	}

	idat += esphlen;

	if (xd->edx_ivlen == 0)		/* Derived IV in use */
	{
		memcpy(iv, (caddr_t)&(espp->esp_rpl), sizeof(espp->esp_rpl));
		iv[4] = ~iv[0];
		iv[5] = ~iv[1];
		iv[6] = ~iv[2];
		iv[7] = ~iv[3];
	}
	else				/* Explicit IV in use */
	{
		memcpy(iv, idat - xd->edx_ivlen, xd->edx_ivlen);
	}

	/* Use the ECB mode block cipher in CBC mode */

	odat = idat; i = 0;
	for (l = 0; l < ilen; l++)
	{
		blk[i] = niv[i] = idat[l];
		i++;

		if (i == blks)
		{
			xd->edx_xform->decrypt(xd, blk);

			for (i=0; i<blks; i++)
			{
				blk[i] ^= iv[i];
				*odat++ = blk[i];
				iv[i] = niv[i];
			}

			i = 0;
		}
	}

	proto = blk[blks-1];

	if ((xd->edx_flags & ESP_NEW_FLAG_NPADDING) == 0)
	{
		if ((blk[blks-2] != blk[blks-3]) && (blk[blks-2] != 0))
		{
			printk("esp_new_input(): decryption failed for packet from %lx to %lx, SA %lx/%08lx (new padding), proto=%d\n",
				(unsigned long)ipp->saddr,
				(unsigned long)ipp->daddr,
				(unsigned long)tdbp->tdb_dst.s_addr,
				ntohl(tdbp->tdb_spi),
				proto);
			return NULL;
		}
		pad = blk[blks-2] + 2 + alen; /* Old type padding */
	}
	else
	{
		if (blk[blks-2] == 0)
		{
			printk("esp_new_input(): decryption failed for packet from %lx to %lx, SA %lx/%08lx (old padding), proto=%d\n",
				(unsigned long)ipp->saddr,
				(unsigned long)ipp->daddr,
				(unsigned long)tdbp->tdb_dst.s_addr,
				ntohl(tdbp->tdb_spi),
				proto);
			return NULL;
		}
		else
			if (blk[blks-2] != blk[blks-3] + 1)
			{
				printk("esp_new_input(): decryption failed for packet from %lx to %lx, SA %lx/%08lx (old padding), proto=%d\n",
					(unsigned long)ipp->saddr,
					(unsigned long)ipp->daddr,
					(unsigned long)tdbp->tdb_dst.s_addr,
					ntohl(tdbp->tdb_spi),
					proto);
				return NULL;
			}
		pad = blk[blks-2] + 1 + alen;	/* New style padding */
	}

#ifdef DEBUG_IPSEC_ESP
	if (debug_esp & DB_ES_IPAD)
	  printk("esp_new_input: proto = %d, padding = %d\n", proto, pad);
#endif

	/*
	 *	Discard the ESP header
	 */
	 
	ipp->tot_len = htons(ntohs(ipp->tot_len) - esphlen - pad);
	ipp->protocol = proto;

	/* Move the IP header up over the the ESP header */
	memmove((void *)(skb->data + esphlen), (void *)skb->data, iphlen);

	/* Chop off the start of the skb */
	skb_pull(skb, esphlen);
	/* Chop off the end of the packet */
	skb_trim(skb, len - esphlen - pad);

	/*
	 *	Adjust pointers
	 */
	
	len = skb->len; dat = skb->data;
	

	skb->h.iph=(struct iphdr *)skb->data;
	skb->ip_hdr=(struct iphdr *)skb->data;

	memset(skb->proto_priv, 0, sizeof(struct options));
	
	ipp = (struct iphdr *)dat;
	ipp->check = 0;
	ipp->check = ip_fast_csum((unsigned char *)dat, iphlen >> 2);

	return skb;
}

int
esp_new_output(struct sk_buff *skb, struct tdb *tdbp)
{
	struct iphdr *ipp;
	struct esp_new_xdata *xd;
	struct esp_new *espp;
	int iphlen, esphlen, blks, padding, len, ilen, alen, rlen, l, i;
	unsigned char *dat, *idat, *odat;
	HASH_CTX ctx;
	unsigned char thash[AH_ALEN_MAX];
	unsigned char iv[ESP_MAX_IVS], blk[ESP_MAX_BLKS];
	
	xd = (struct esp_new_xdata *)(tdbp->tdb_xdata);
	blks = xd->edx_xform->blocksize;

	alen = (xd->edx_flags & ESP_NEW_FLAG_AUTH) ? AH_HMAC_HASHLEN : 0;

	esphlen = 2*sizeof(__u32) + xd->edx_ivlen; /* size of header */

	ipp = (struct iphdr *)(skb->data);
#ifdef DEBUG_IPSEC_ESP
	if (debug_esp & DB_ES_OINFO)
	{
		printk("esp_new_output: old header (headroom = %d, tailroom = %d\n", skb_headroom(skb), skb_tailroom(skb));
		print_ip_pkt(ipp);
	}

#endif	
	iphlen = ipp->ihl << 2;

	if (ntohs(ipp->frag_off) & IP_OFFSET)
	{
		printk("esp_new_output: attempted to encrypt fragment!\n");
		return -1;
	}

	if (xd->edx_rpl == 0)
	{
		printk("esp_new_output(): SA %lx/%08lx should have expired, RPL wrapped\n",
			(unsigned long)tdbp->tdb_dst.s_addr,
			ntohl(tdbp->tdb_spi));
		return -1;
	}

	rlen = skb->len - iphlen;
	padding = ((blks - ((rlen + 2) % blks)) % blks) + 2;

	dat = skb_push(skb, esphlen);
	ilen = skb->len;
	len = ilen + padding + alen;
	
#ifdef DEBUG_IPSEC_ESP
	if (debug_esp & DB_ES_OINFO2)
		printk("before skb_put: %d,%d (need: %d,%d)", skb_headroom(skb), skb_tailroom(skb), esphlen, padding + alen);
#endif

	skb_put(skb, padding + alen);

#ifdef DEBUG_IPSEC_ESP
	if (debug_esp & DB_ES_OINFO2)
		printk(" after skb_put: %d,%d\n", skb_headroom(skb), skb_tailroom(skb));
#endif	

	/* Self describing padding */
	for (i = 0; i < padding - 2; i++)
		dat[ilen+i] = i + 1;

	dat[ilen + padding - 2] = padding -
		((xd->edx_flags & ESP_NEW_FLAG_NPADDING) ? 1 : 2);

	dat[ilen + padding - 1] = ipp->protocol;

	/* Move the IP header down to new place */
	memmove((void *)dat, (void *)(dat + esphlen), iphlen);
	ipp = (struct iphdr *)dat;

	espp = (struct esp_new *)(dat + iphlen);
	espp->esp_spi = tdbp->tdb_spi;
	espp->esp_rpl = htonl(xd->edx_rpl++);

	if (xd->edx_ivlen == 0)		/* We derive the IV from the RPL */
	{
		memcpy(iv, &(espp->esp_rpl), sizeof(__u32));
		iv[4] = ~iv[0];
		iv[5] = ~iv[1];
		iv[6] = ~iv[2];
		iv[7] = ~iv[3];
	}
	else				/* We use an explicit IV */
	{
		memcpy(iv, &(xd->edx_iv), xd->edx_ivlen);
		memcpy(&(espp->esp_iv), iv, xd->edx_ivlen);
	}

	idat = odat = dat + iphlen + esphlen;
	ilen = rlen + padding;

#ifdef DEBUG_IPSEC_ESP
	if (debug_esp & DB_ES_OINFO)
		printk("encrypting %d bytes\n", ilen);
#endif	
	/* Use the ECB block cipher in CBC mode */
	i = 0;
	for (l = 0; l < ilen; l++)
	{
		blk[i] = (*idat++) ^ iv[i];
		i++;

		if (i==blks)
		{
			xd->edx_xform->encrypt(xd, blk);

			for (i = 0; i < blks; i++)
			{
				*odat++ = iv[i] = blk[i];
			}

			i = 0;
		}
	}

	/* Save the last encrypted block, to be used as the next IV */
	if (xd->edx_ivlen)
		memcpy(xd->edx_iv, blk, xd->edx_ivlen);

	if (alen)
	{
		memcpy(&ctx, &(xd->edx_ictx), xd->edx_hash->ctxsize);
		xd->edx_hash->Update(&ctx, (caddr_t)espp, len - iphlen - alen);
		xd->edx_hash->Final(thash, &ctx);
		memcpy(&ctx, &(xd->edx_octx), xd->edx_hash->ctxsize);
		xd->edx_hash->Update(&ctx, thash, xd->edx_hash->hashsize);
		xd->edx_hash->Final(thash, &ctx);

		/* Copy in the final authenticator */
		memcpy(dat + len - alen, thash, alen);
	}

	ipp->protocol		=	IPPROTO_ESP;
	ipp->tot_len		=	htons(len);

	skb->ip_hdr = skb->h.iph = (struct iphdr *) skb->data;
	ipp->check = 0;
	ipp->check = ip_fast_csum((unsigned char *)ipp, ipp->ihl);

#ifdef DEBUG_IPSEC_ESP
	if (debug_esp & DB_ES_OH)
	{
		printk("esp_new_output: new header (headroom = %d, tailroom = %d\n", skb_headroom(skb), skb_tailroom(skb));
		print_ip_pkt(ipp);
	}
#endif

	return 0;
}
