#include <vo_stdio.h>
#include <string.h>
#include <ctype.h>
#include "decryption.h"
#include "chacha20.h"

/*
 * Decrypt using ChaCha20, using highly optimized routines mostly written
 * in VSDSP4 assembly language. Chacha20 is a modern and strong 256-bit
 * cipher. Read the accompanying documentation for more details.
 */

#if 0
/* Activate for debug output */
#define VERBOSE
#endif


#ifdef __VSDSP__
u_int16 ToHex(register __c0 char c);
#else
u_int16 ToHex(register __c0 char c) {
#if 0
	if (isdigit(c)) return c-'0';
	if (isxdigit(c)) return toupper(c)-('A'-10);
#else
	if (c >= '0' && c <= '9') return c-'0';
	if ((c|=0x20) >= 'a' && c <= 'z') return c-('a'-10);
#endif
	return 0xffffU;
}
#endif


#if 0
#ifdef __VSDSP__
/* Calling this assembler-optimized function didn't make code faster,
   so it has been commented away. */
void XorN(register __i0 u_int16 *d, register __i2 const u_int16 *s,
	    register __c0 s_int16 cnt);
#else
void XorN(register __i0 u_int16 *d, register __i2 const u_int16 *s,
	    register __c0 s_int16 cnt) {
  int i;
  for (i=0; i<cnt; i++) {
    *d++ ^= *s++;
  }
}
#endif
#endif


ioresult CryptoStart(char *parameters, struct CryptContext *ctx) {
	s_int16 i, len;
	char *p, *e;
#ifdef VERBOSE
	 printf("CryptoStart params: %s\n", parameters);
#endif
#if 0
	 for (i=32; i<127; i++) {
	   printf("%3d%2d\t", i, ToHex(i));
	 }
#endif

	 if (parameters && ctx) {
		p = parameters;
		/* Leading WS */
		while (isspace(*p)) {
			p++;
		}
		len = strlen(p);
		e = p + len - 1;
		/* Trailing WS */
		while (isspace(*e) && p < e) {
			*e-- = 0;
			len--;
		}
		if (len == (64 + 7) && strncmp(p, "HEXKEY:", 7) == 0) {
			u_int16 k, *d = (u_int16 *)ctx->key;
			p += 7;

			for (i = 0; i < (64 / (2*2)); i++) {
				k = ToHex(*p++);
				*d = ToHex(*p++) << 8;
				*d |= ToHex(*p++) << 4;
				*d |= ToHex(*p++) << 0;
				if ((*d | k) & 0xf000) {
#ifdef VERBOSE
					printf("K: %04x D: %04x\n", k, *d);
#endif
					return S_ERROR;
				}
				*d++ |=	(k << 12);
			}
			return S_OK;
		}
		return S_ERROR;
	}
	return S_ERROR;
}


void CryptSeek(struct CryptContext *ctx, register u_int32 pos, register u_int16 bytes) {
	u_int32 counter = (pos - bytes) / (2 * CRYPTO_BUF_SIZE);
	ctx->bufIndex = (u_int16)((pos - bytes) % (2 * CRYPTO_BUF_SIZE));
#ifdef VERBOSE
	printf("CryptSeek(ctx=%04x, pos-bytes=%08lx), counter %lx, bufIdx %x\n",
	       ctx, pos-bytes, counter, ctx->bufIndex);
#endif
	Chacha20SetKey(ctx->key, counter, ctx->nonce, ctx->state);
	/* Context set, next prepare the keystream */
	memset(ctx->buf, 0, CRYPTO_BUF_SIZE);
	Chacha20Encrypt(ctx->buf, ctx->state, CRYPTO_BUF_SIZE);
#ifdef VERBOSE
	{
		int i;
		printf("#1:");
		for (i=0; i<CRYPTO_BUF_SIZE; i++) {
			printf(" %04x", ctx->buf[i]);
		}
		printf("\n");
	}
#endif
}


void InitCrypt(struct CryptContext *ctx, const char *name) {
#ifdef VERBOSE
	if (name) {
		printf("Init crypt for: %s\n", name);
	} else {
		printf("Init crypt\n");
	}
#endif
	/* Read nonce */
	if (ctx->fi->fileSize >= 12) {
	  fseek(ctx->fi, -3 * 4, SEEK_END);
	  fread(ctx->nonce, 2, 3, ctx->fi);
	  fseek(ctx->fi, 0, SEEK_SET);
	  ctx->fi->fileSize -= 12; /* Nonce size */
	  ctx->pos = 0;
	}
	CryptSeek(ctx, 0, 0);
#ifdef VERBOSE
	{
		u_int16 i;
		u_int32 *d = ctx->key;
		for (i = 0; i < 11; i++) {
			printf("%08lx ", *d++);
		}
		printf("\n");
	}
#endif
}


/* Decrypting function */
#define CHUNK_SIZE CRYPTO_BUF_SIZE
#if (CRYPTO_BUF_SIZE != 32)
Someone broke optimization
#endif
#define CRYPTO_BLOCK_BMASK ((CRYPTO_BUF_SIZE * 2) - 1)
void Decrypt(void *buf, u_int16 destinationIndex, u_int16 bytes, struct CryptContext *ctx, u_int32 pos) {
	static u_int16 tmpBuf[CHUNK_SIZE];
	static u_int16 keyBuf[CHUNK_SIZE];
	u_int16 chunkBytes, i, *data = buf;
	if (pos != (ctx->pos + bytes)) {
		CryptSeek(ctx, pos, bytes);
	}
#if 0
	printf("Decrypt(buf=%x, dIdx=%x, byt=%x, ctx=%x, ctx->pos=%lx, pos=%lx\n",
	       buf, destinationIndex, bytes, ctx, ctx->pos, pos);
#endif
	ctx->pos = pos;
	while (bytes) {
		chunkBytes = CHUNK_SIZE * 2;
		if (chunkBytes > bytes) {
			chunkBytes = bytes;
		}
		if (chunkBytes > (CRYPTO_BUF_SIZE * 2 - ctx->bufIndex)) {
			chunkBytes = CRYPTO_BUF_SIZE * 2 - ctx->bufIndex;
		}
		/* Read encrypted data */
		MemCopyPackedBigEndian(tmpBuf, 0, data, destinationIndex, chunkBytes);
		/* Read keystream data */
		MemCopyPackedBigEndian(keyBuf, 0, ctx->buf, ctx->bufIndex, chunkBytes);
		/* Decipher */
		{
		  u_int16 *p1 = tmpBuf, *p2 = keyBuf;
		  for (i=0; i<CHUNK_SIZE; i++) *p1++ ^= *p2++;
		}
		/* Write back */
		MemCopyPackedBigEndian(data, destinationIndex, tmpBuf, 0, chunkBytes);
		bytes -= chunkBytes;
		destinationIndex += chunkBytes;
		data += (destinationIndex >> 1);
		destinationIndex &= 1;
		if ((ctx->bufIndex += chunkBytes) >= (CRYPTO_BUF_SIZE * 2)) {
			/* This optimization if() makes crypto typically over 30% faster. */
			if ((destinationIndex == 0)) {
				/* context buffer consumed,
				 * data is real destination and
				 * encryption function takes only
				 * multiple CRYPTO_BUF_SIZE words */
				Chacha20Encrypt(data, ctx->state, (bytes >> 1));
				data += (bytes & ~CRYPTO_BLOCK_BMASK) >> 1;
				bytes &= CRYPTO_BLOCK_BMASK;
			}

			/* Reload context buffer */
			memset(ctx->buf, 0, CRYPTO_BUF_SIZE);
			ctx->bufIndex = 0;
			Chacha20Encrypt(ctx->buf, ctx->state, CRYPTO_BUF_SIZE);

#ifdef VERBOSE
			{
				int i;
				printf("#2:");
				for (i=0; i<CRYPTO_BUF_SIZE; i++) {
					printf(" %04x", ctx->buf[i]);
				}
				printf("\n");
			}

#endif
		}
	}
}
