#include <platform.h>
#include "aesni-intel_glue.h"

namespace _innovaphone_aes {

#define CONFIG_X86_32
#define ALIGN(x, a)		__ALIGN_KERNEL((x), (a))
#define ALIGN_DOWN(x, a)	__ALIGN_KERNEL((x) - ((a) - 1), (a))
#define __ALIGN_MASK(x, mask)	__ALIGN_KERNEL_MASK((x), (mask))
#define PTR_ALIGN(p, a)		((typeof(p))ALIGN((unsigned long)(p), (a)))
#define IS_ALIGNED(x, a)		(((x) & ((typeof(x))(a) - 1)) == 0)
#define __ALIGN_KERNEL(x, a)		__ALIGN_KERNEL_MASK(x, (typeof(x))(a) - 1)
#define __ALIGN_KERNEL_MASK(x, mask)	(((x) + (mask)) & ~(mask))
#ifndef EINVAL
#	define EINVAL (1)
#endif

#define DVL_TRACE 0
#if		DVL_TRACE
#	define DBG_DVL(_args_) debug->printf _args_
#else
#	define DBG_DVL(_args_)
#endif

#ifdef __cplusplus
#	define CPP_ASMLINKAGE extern "C"
#else
#	define CPP_ASMLINKAGE
#endif

#ifdef CONFIG_X86_32
#	define asmlinkage CPP_ASMLINKAGE __attribute__((regparm(0)))	// cdecl 
#endif /* CONFIG_X86_32 */

	asmlinkage int aesni_set_key(crypto_aesni_ctx* ctx, const byte* in_key, unsigned int key_len);
	asmlinkage void aesni_enc(crypto_aesni_ctx* ctx, byte* dst, const byte* src);
	asmlinkage void aesni_dec(crypto_aesni_ctx* ctx, byte* dst, const byte* src);
	asmlinkage void aesni_ecb_enc(crypto_aesni_ctx* ctx, byte* dst, const byte* src, size_t len);
	asmlinkage void aesni_ecb_dec(crypto_aesni_ctx* ctx, byte* dst, const byte* src, size_t len);
	asmlinkage void aesni_cbc_enc(crypto_aesni_ctx* ctx, byte* dst, byte* src, size_t len, byte* iv);
	asmlinkage void aesni_cbc_dec(crypto_aesni_ctx* ctx, byte* dst, byte* src, size_t len, byte* iv);

	typedef struct _fxregs_state {
		word			cwd; /* Control Word			*/
		word			swd; /* Status Word			*/
		word			twd; /* Tag Word			*/
		word			fop; /* Last Instruction Opcode		*/
		union {
			struct {
				ulong64	rip; /* Instruction Pointer		*/
				ulong64	rdp; /* Data Pointer			*/
			};
			struct {
				dword	fip; /* FPU IP Offset			*/
				dword	fcs; /* FPU IP Selector			*/
				dword	foo; /* FPU Operand Offset		*/
				dword	fos; /* FPU Operand Selector		*/
			};
		};
		dword			mxcsr;		/* MXCSR Register State */
		dword			mxcsr_mask;	/* MXCSR Mask		*/

		/* 8*16 bytes for each FP-reg = 128 bytes:			*/
		dword			st_space[32];

		/* 16*16 bytes for each XMM-reg = 256 bytes:			*/
		dword			xmm_space[64];

		dword			padding[12];

		union {
			dword		padding1[12];
			dword		sw_reserved[12];
		};

	} __attribute__((aligned(16))) fxregs_state;

#define ALIGN_PTR(p, a) (((uintptr_t)p & ~((uintptr_t)a-1)) ? (((uintptr_t)p + (uintptr_t)a) & ~((uintptr_t)a-1)) : ((uintptr_t)p))
#define ALLOCA_ALIGN(sz,a) ALIGN_PTR(alloca(sz + a), a)

#if FPU_CONTEXT_SAVE_RESTORE_GLUE_CODE
#	define FPU_CONTEXT_SAVE fxregs_state* fxs = (fxregs_state*)ALLOCA_ALIGN(sizeof(*fxs), 16);\
							fxsave(fxs)
#	define FPU_CONTEXT_RESTORE fxrstor(fxs)
#else	// SAVE_RESTORE_FPU_CONTEXT
#	define FPU_CONTEXT_SAVE_GLUE_CODE
#	define FPU_CONTEXT_RESTORE_GLUE_CODE
#endif	// SAVE_RESTORE_FPU_CONTEXT

	static inline void fxsave(fxregs_state* fx)
	{
		__asm volatile("fxsave %[fx]" : [fx] "=m" (*fx));
	}

	static inline void fxrstor(fxregs_state* fx)
	{
		__asm volatile("fxrstor %[fx]" : : [fx] "m" (*fx));
	}

#define rd_cpuid(func, ax, bx, cx, dx) __asm__ __volatile__ ("cpuid": "=a"(ax), "=b"(bx), "=c"(cx), "=d"(dx) : "a"(func));
	static bool Check_CPU_support_AES()
	{
		unsigned int a, b, c, d;
		rd_cpuid(1, a, b, c, d);
		return !!(c & 0x2000000);
	}

	inline crypto_aesni_ctx* aesni_from_raw_contect(void* raw_context)
	{
		byte* ctx = (byte*)raw_context;
		ctx = (byte*)ALIGN_PTR(ctx, AESNI_ALIGN);
		return (crypto_aesni_ctx*)ctx;
	}

	int aesni_set_key_glue(void* raw_context, const byte* in_key, unsigned int key_len)
	{
		int						err = 0;
		crypto_aesni_ctx		* ctx = NULL;
		DBG_DVL(("%s:ctx=0x%x key(l=%u)='%.*H'", __FUNCTION__, raw_context, key_len, key_len, in_key));

		if (key_len != AES_KEYSIZE_128 && key_len != AES_KEYSIZE_192 && key_len != AES_KEYSIZE_256) {
			err = -EINVAL; goto ret;
		}

		ctx = aesni_from_raw_contect(raw_context);
		FPU_CONTEXT_SAVE_GLUE_CODE;
		err = aesni_set_key(ctx, in_key, key_len);
		FPU_CONTEXT_RESTORE_GLUE_CODE;

	ret:
		if (err != 0) {
			ERR(("%s:err=%u ctx=0x%x key_len=%u", __FUNCTION__, (-err), ctx, key_len));
		}
		return err;
	}

	void aesni_enc_glue(void* raw_context, byte* out, const byte* in)
	{
		DBG_DVL(("%s:ctx=0x%x in(l=%u)='%.*H'", __FUNCTION__, raw_context, AES_BLOCK_SIZE, AES_BLOCK_SIZE, in));
		crypto_aesni_ctx* ctx = aesni_from_raw_contect(raw_context);
		FPU_CONTEXT_SAVE_GLUE_CODE;
		aesni_enc(ctx, out, in);
		FPU_CONTEXT_RESTORE_GLUE_CODE;
		DBG_DVL(("%s:ctx=0x%x out(l=%u)='%.*H'", __FUNCTION__, ctx, AES_BLOCK_SIZE, AES_BLOCK_SIZE, out));
	}

	void aesni_dec_glue(void* raw_context, byte* out, const byte* in)
	{
		DBG_DVL(("%s:ctx=0x%x in(l=%u)='%.*H'", __FUNCTION__, raw_context, AES_BLOCK_SIZE, AES_BLOCK_SIZE, in));
		crypto_aesni_ctx* ctx = aesni_from_raw_contect(raw_context);
		FPU_CONTEXT_SAVE_GLUE_CODE;
		aesni_dec(ctx, out, in);
		FPU_CONTEXT_RESTORE_GLUE_CODE;
		DBG_DVL(("%s:ctx=0x%x out(l=%u)='%.*H'", __FUNCTION__, ctx, AES_BLOCK_SIZE, AES_BLOCK_SIZE, out));
	}

	void aesni_ecb_enc_glue(void* raw_context, byte* out, const byte* in, size_t len)
	{
		DBG_DVL(("%s:ctx=0x%x in(l=%u)='%.*H'", __FUNCTION__, raw_context, len, len, in));
		crypto_aesni_ctx* ctx = aesni_from_raw_contect(raw_context);
		FPU_CONTEXT_SAVE_GLUE_CODE;
		aesni_ecb_enc(ctx, out, in, len);
		FPU_CONTEXT_RESTORE_GLUE_CODE;
		DBG_DVL(("%s:ctx=0x%x out(l=%u)='%.*H'", __FUNCTION__, ctx, len, len, out));
	}

	void aesni_ecb_dec_glue(void* raw_context, byte* out, const byte* in, size_t len)
	{
		DBG_DVL(("%s:ctx=0x%x in(l=%u)='%.*H'", __FUNCTION__, raw_context, len, len, in));
		crypto_aesni_ctx* ctx = aesni_from_raw_contect(raw_context);
		FPU_CONTEXT_SAVE_GLUE_CODE;
		aesni_ecb_dec(ctx, out, in, len);
		FPU_CONTEXT_RESTORE_GLUE_CODE;
		DBG_DVL(("%s:ctx=0x%x out(l=%u)='%.*H'", __FUNCTION__, ctx, len, len, out));
	}

	void aesni_cbc_enc_glue(void* raw_context, byte* out, byte* in, size_t len, byte* iv)
	{
		DBG_DVL(("%s:ctx=0x%x iv='%.*H' in(l=%u)='%.*H'", __FUNCTION__, raw_context, AES_BLOCK_SIZE, iv, len, len, in));
		crypto_aesni_ctx* ctx = aesni_from_raw_contect(raw_context);
		FPU_CONTEXT_SAVE_GLUE_CODE;
		aesni_cbc_enc(ctx, out, in, len, iv);
		FPU_CONTEXT_RESTORE_GLUE_CODE;
		DBG_DVL(("%s:ctx=0x%x out(l=%u)='%.*H'", __FUNCTION__, ctx, len, len, out));
	}

	void aesni_cbc_dec_glue(void* raw_context, byte* out, byte* in, size_t len, byte* iv)
	{
		DBG_DVL(("%s:ctx=0x%x iv='%.*H' in(l=%u)='%.*H'", __FUNCTION__, raw_context, AES_BLOCK_SIZE, iv, len, len, in));
		crypto_aesni_ctx* ctx = aesni_from_raw_contect(raw_context);
		FPU_CONTEXT_SAVE_GLUE_CODE;
		aesni_cbc_dec(ctx, out, in, len, iv);
		FPU_CONTEXT_RESTORE_GLUE_CODE;
		DBG_DVL(("%s:ctx=0x%x out(l=%u)='%.*H'", __FUNCTION__, ctx, len, len, out));
	}

	bool has_cpu_aesni = Check_CPU_support_AES();

}