marvell/uboot/drivers/geu/asr_aes.c

#include <common.h>
#include <config.h>
#include <asm/arch/cpu.h>
#if CONFIG_PXA_DMA
#include <asm/arch/pxa_dma.h>
#endif
#include <stdbool.h>
#include <asm/io.h>
#include <malloc.h>
#include <asm/bitops.h>
#include "asr_geu.h"
#include "asr_rng.h"
#include "asr_fuse.h"

#include "aes.h"

/*
notice: 1) use hardware key if burned hardware key;
		1) use input key if not burned hardware key but input key not null;
		2) use default key if not burned rkek but input key is null;
		char default_key[64] = {"asr-aes-default-key-without-rkek"};
*/
const char default_key[64] = {"asr-aes-default-key-without-rkek"};

#define AES_ALG_MASK			(0xf)
#if CONFIG_PXA_DMA
#define	ASR_AES_DMA_THRESHOLD	(256)
#endif

static struct geu_data geu_data = {
	.geu_phybase = GEU_BASE,
	.apmu_phybase = APMU_BASE,
	.clk_en = 0,
};

uint32_t geu_read32(uint32_t reg) {
	return readl(geu_data.geu_phybase + reg);
}

void geu_write32(uint32_t reg, uint32_t val) {
	writel(val, geu_data.geu_phybase + reg);
}

void geu_reset_unit(void)
{
	uint32_t val;
	mutex_lock(&geu_data.clk_lock);
	val = readl(geu_data.apmu_phybase + AES_CLK_RES_CTRL);
	val &= ~(1 << 0);
	writel(val, geu_data.apmu_phybase + AES_CLK_RES_CTRL);
	val |= (1 << 0);
	writel(val, geu_data.apmu_phybase + AES_CLK_RES_CTRL);
	mutex_unlock(&geu_data.clk_lock);
}

void geu_reset_aes(void)
{
	uint32_t conf;
	conf = geu_read32(GEU_CONFIG);

	conf &= ~GEU_CFG_AES_MSK;

	geu_write32(GEU_CONFIG, conf);
}

void geu_enable_clk(void)
{
	uint32_t val;
	mutex_lock(&geu_data.clk_lock);
	if ( geu_data.clk_en == 0 ) {
		val = readl(geu_data.apmu_phybase + AES_CLK_RES_CTRL);
		/* it will no longer enable clk if the clk has been enabled */
		if ((val & (1 << 3 | 1 << 0)) == (1 << 3 | 1 << 0)) {
			return;
		}
		val |= (1 << 3);
		val |= (1 << 0);
		writel(val, geu_data.apmu_phybase + AES_CLK_RES_CTRL);
	}
	geu_data.clk_en ++;
	mutex_unlock(&geu_data.clk_lock);
}

void geu_disable_clk(void)
{
	uint32_t val;
	mutex_lock(&geu_data.clk_lock);

	if(geu_data.clk_en > 0) {
		geu_data.clk_en --;
		if(geu_data.clk_en == 0) {
			val = readl(geu_data.apmu_phybase + AES_CLK_RES_CTRL);
			val &= ~(1 << 3);
			writel(val, geu_data.apmu_phybase + AES_CLK_RES_CTRL);
		}
	}

	mutex_unlock(&geu_data.clk_lock);
}

void asr_geu_get(void)
{
	mutex_lock(&geu_data.eng_lock);
}

void asr_geu_put(void)
{
	mutex_unlock(&geu_data.eng_lock);
}

static int asr_aes_waiting_status(uint32_t bit_mask, uint32_t timeout,
			int set_clr/* 1: set, 0: clear */)
{
	uint32_t ret = 0;
	uint32_t mytime;
	uint32_t status;
	uint32_t start = 0;

	set_clr = !!set_clr;

	start = get_timer(0);
	if (ret != 0) {
		printf("fail to get system time");
		return -1;
	}

	do {
		status = geu_read32(GEU_STATUS);

		mytime = get_timer(start);

		if(mytime > timeout) {
			printf("aes waiting time out\n");
			return -1;
		}

	}while( (!!(status & bit_mask)) ^ set_clr );


	return 0;
}

static int asr_aes_setmode(int encrypt, int mode)
{
	uint32_t conf;
	conf = geu_read32(GEU_CONFIG);

	switch(mode) {
	case AES_ECB:
		conf &= ~GEU_CFG_CBC_ECB;
		conf &= ~GEU_CFG_OCB_BYP;
		break;
	case AES_CBC:
		conf |= GEU_CFG_CBC_ECB;
		conf |= GEU_CFG_OCB_BYP;
		break;
	default :
		printf("Unsupported AES mode\n");
		return -1;
	}

	if(encrypt)
		conf &= ~GEU_CFG_ENC_DEC;
	else
		conf |= GEU_CFG_ENC_DEC;

	geu_write32(GEU_CONFIG, conf);

	return 0;
}

static int asr_aes_rkek_burned(void)
{
	uint32_t val;

	/* If RKEK is burned, SW access to it must be disabled as well */
	if (cpu_is_asr1901() || cpu_is_asr1906()) {
		/* check if LCS_DM is burned */
		val = geu_read32(GEU_KSTR_BANK6_LCS);
		val >>= GEU_KSTR_LCS_DM_BASE;
		val &= GEU_KSTR_LCS_MASK;
		if (hweight32(val) > 1)
			return 1;
	} else { /* 1803, 1806, 1828, 1903 */
		/* check if secure key access disable is burned */
		val = geu_read32(GEU_FUSE_APCFG2);
		if (val & GEU_SECURE_KEY_ACCESS_DISABLED)
			return 1;
	}
	
	return 0;
}

static int asr_aes_setkey(uint32_t *key, uint32_t key_size, bool use_rkek)
{
	int ret = 0;
	uint32_t index;
	uint32_t conf, status;
	int rkek_burned = 0;

	key_size &= ~3;
	conf = geu_read32(GEU_CONFIG);
	status = geu_read32(GEU_STATUS);
	conf |= GEU_CFG_PWR_BYP;
	conf &= ~GEU_CFG_KEY_SIZE(3); /* clear key size conf */
	rkek_burned = asr_aes_rkek_burned();
	if (use_rkek && rkek_burned) {
		/* using rkek from fuse block */
		conf |= GEU_CFG_ENA_RKEK;
		conf |= GEU_CFG_KEY_SIZE((key_size >> 3) - 1);
	} else {
		conf &= ~GEU_CFG_ENA_RKEK;
		conf |= GEU_CFG_KEY_SIZE((key_size >> 3) - 1);
		for (index = 0; index < 8; index ++) 
			geu_write32(GEU_INIT_KEY(index), 0); /* clear key reg */
		for (index = 0; index < (key_size >> 2); index++)
			geu_write32(GEU_INIT_KEY(index), key[index]);
	}

	geu_write32(GEU_CONFIG, conf);

	status |= GEU_STATUS_ROUND_KEY_START;
	geu_write32(GEU_STATUS, status);

	ret = asr_aes_waiting_status(GEU_STATUS_ROUND_KEY_READY, 100, 1);

	return ret;
}

static int asr_aes_setiv(uint32_t *iv)
{
	uint32_t conf;
	int index;

	for (index = 0; index < 4; index ++)
		geu_write32(GEU_INIT_IV(index), iv[index]);

	conf = geu_read32(GEU_CONFIG);
	conf |= GEU_CFG_WRITE_IV;
	geu_write32(GEU_CONFIG, conf);

	return 0;
}

static int asr_aes_getiv(uint32_t *iv)
{
	iv[0] = geu_read32(GEU_OUT_DATA(0));
	iv[1] = geu_read32(GEU_OUT_DATA(1));
	iv[2] = geu_read32(GEU_OUT_DATA(2));
	iv[3] = geu_read32(GEU_OUT_DATA(3));
	return 0;
}

#if CONFIG_PXA_DMA
static int asr_aes_enable_dma(void)
{
	uint32_t conf;

	conf = geu_read32(GEU_CONFIG);
	conf |= GEU_CFG_DMA_MODE_EN;
	geu_write32(GEU_CONFIG, conf);

	return 0;
}

static int asr_aes_disable_dma(void)
{
	uint32_t conf;

	conf = geu_read32(GEU_CONFIG);
	conf &= ~GEU_CFG_DMA_MODE_EN;
	geu_write32(GEU_CONFIG, conf);

	return 0;
}
#endif

static int asr_aes_process_pio(uint32_t *in, uint32_t len, uint32_t *out)
{
	int ret;
	int len_bytes = len;
	uint32_t status;
	uint32_t *in_buf = in;
	uint32_t *out_buf = out;

	assert(!(len_bytes % 16));

	while(len_bytes > 0) {
		geu_write32(GEU_IN_DATA(0), in_buf[0]);
		geu_write32(GEU_IN_DATA(1), in_buf[1]);
		geu_write32(GEU_IN_DATA(2), in_buf[2]);
		geu_write32(GEU_IN_DATA(3), in_buf[3]);

		status = geu_read32(GEU_STATUS);
		status |= GEU_STATUS_DATA_ENCDEC_ENA;
		geu_write32(GEU_STATUS, status);

		ret = asr_aes_waiting_status(GEU_STATUS_DATA_ENCDEC_READY, 100, 1);
		if( ret ) return -1;

		out_buf[0] = geu_read32(GEU_OUT_DATA(0));
		out_buf[1] = geu_read32(GEU_OUT_DATA(1));
		out_buf[2] = geu_read32(GEU_OUT_DATA(2));
		out_buf[3] = geu_read32(GEU_OUT_DATA(3));

		len_bytes -= 16;
		in_buf += 4;
		out_buf += 4;
	}

	return 0;
}

#if CONFIG_PXA_DMA
static int geu_dma_read_prepare(uint32_t dst, uint32_t src,
								uint32_t size, uint32_t chl, struct dma_descp *desc)
{
#define MAX_DATA_SIZE_OF_EACH_DESC	(0x1000)
	uint32_t cmd = 0;
	cmd =  DCMD_INCSRCADDR | DCMD_FLOWTRG;
	cmd |= DCMD_BURST16 | DCMD_WIDTH4;

	if (desc) {
		uint32_t desc_src, desc_cmd;
		uint32_t remain_len = size;
		struct dma_descp *desc_curr = NULL;
		struct dma_descp *desc_next = NULL;

		for (uint32_t i = 0; i < size / MAX_DATA_SIZE_OF_EACH_DESC; i++) {
			remain_len -= MAX_DATA_SIZE_OF_EACH_DESC;
			desc_cmd = (cmd & ~(DCMD_LENGTH)) | MAX_DATA_SIZE_OF_EACH_DESC;
			desc_src = (uint32_t)(src + MAX_DATA_SIZE_OF_EACH_DESC * i);
			desc_curr = desc + i;
			if (0 == remain_len) {
				desc_next = 0;
				config_descriptor((unsigned int *)desc_curr,
						(unsigned int *)desc_next,
						desc_src, dst, desc_cmd, 1);
			} else {
				desc_next = desc + (i + 1);
				config_descriptor((unsigned int *)desc_curr,
						(unsigned int *)desc_next,
						desc_src, dst, desc_cmd, 0);
			}
		}
		if ((remain_len > 0) && (remain_len < MAX_DATA_SIZE_OF_EACH_DESC )) {
			desc_cmd = (cmd & ~(DCMD_LENGTH)) | remain_len;
			desc_src = (uint32_t)(src + (size - remain_len));
			desc_curr = desc + (size / MAX_DATA_SIZE_OF_EACH_DESC);
			desc_next = 0;
			config_descriptor((unsigned int *)desc_curr,
						(unsigned int *)desc_next,
						desc_src, dst, desc_cmd, 1);
		}
		
		load_descriptor((struct dma_descp *)desc, chl);
	} else {
		DCSR(chl) |= DCSR_NODESC;
		DTADR(chl) = dst;
		DSADR(chl) = src;
		DCMD(chl) = cmd | (size & DCMD_LENGTH);
	}
}

static int geu_dma_write_prepare(uint32_t dst, uint32_t src,
								uint32_t size, uint32_t chl, struct dma_descp *desc)
{
	uint32_t cmd = 0;
	cmd =  DCMD_INCTRGADDR | DCMD_FLOWSRC;
	cmd |= DCMD_BURST16 | DCMD_WIDTH4;

	if (desc) {
		uint32_t desc_dst, desc_cmd;
		uint32_t remain_len = size;
		struct dma_descp *desc_curr = NULL;
		struct dma_descp *desc_next = NULL;

		for (uint32_t i = 0; i < size/MAX_DATA_SIZE_OF_EACH_DESC; i++) {
			remain_len -= MAX_DATA_SIZE_OF_EACH_DESC;
			desc_cmd = (cmd & ~(DCMD_LENGTH)) | MAX_DATA_SIZE_OF_EACH_DESC;
			desc_dst = (uint32_t)(dst + MAX_DATA_SIZE_OF_EACH_DESC*i);
			desc_curr = desc + i;

			if (0 == remain_len) {
				desc_next = 0;
				config_descriptor((unsigned int *)desc_curr,
						(unsigned int *)desc_next,
						src, desc_dst, desc_cmd, 1);
			} else {
				desc_next = desc + (i + 1);
				config_descriptor((unsigned int *)desc_curr,
						(unsigned int *)desc_next,
						src, desc_dst, desc_cmd, 0);				
			}
		}
		if ((remain_len > 0) && (remain_len < MAX_DATA_SIZE_OF_EACH_DESC )) {
			desc_cmd = (cmd & ~(DCMD_LENGTH)) | remain_len;
			desc_dst = (uint32_t)(dst + (size - remain_len));
			desc_curr = desc + (size/MAX_DATA_SIZE_OF_EACH_DESC);
			desc_next = 0;
			config_descriptor((unsigned int *)desc_curr,
						(unsigned int *)desc_next,
						src, desc_dst, desc_cmd, 1);
		}
		
		load_descriptor((struct dma_descp *)desc, chl);
	} else {
		DCSR(chl) |= DCSR_NODESC;
		DTADR(chl) = dst;
		DSADR(chl) = src;
		DCMD(chl) = cmd;
		DCMD(chl) = cmd | (size & DCMD_LENGTH);
	}
}

static int asr_aes_process_dma(uint32_t *in, uint32_t len, uint32_t *out)
{
	int init = 0;
	int ret = 0;
	int remain_len = 0;
	uint32_t start = 0, src, dst, step_szie;
	struct dma_descp rc_desc[64] __attribute__ ((aligned (32)));
	struct dma_descp tx_desc[64] __attribute__ ((aligned (32)));
	
	if (!init) {
		dmac_map_device_to_channel(DMA_DEVICE_GEU_RXREQ,
			GEU_RX_CHANNEL);
		dmac_map_device_to_channel(DMA_DEVICE_GEU_TXREQ,
			GEU_TX_CHANNEL);

		dmac_user_aligment(GEU_RX_CHANNEL);
		dmac_user_aligment(GEU_TX_CHANNEL);
		init = 1;
	}

	src = (uint32_t)virt_to_phys((void *)in);
	dst = (uint32_t)virt_to_phys((void *)out);
	remain_len = len;
	step_szie = EACH_DMA_PROCESS_SIZE;

	while (remain_len > 0) {
		step_szie = (remain_len > step_szie) ? step_szie:remain_len;

		geu_dma_read_prepare(GEU_BASE + GEU_IN_DATA(0), (uint32_t)src,\
							step_szie, GEU_RX_CHANNEL, rc_desc);
		geu_dma_write_prepare((uint32_t)dst, GEU_BASE + GEU_OUT_DATA(0), \
							step_szie, GEU_TX_CHANNEL, tx_desc);

		flush_dcache_all();

		dmac_start_transfer(GEU_RX_CHANNEL);
		dmac_start_transfer(GEU_TX_CHANNEL);

		asr_aes_enable_dma();
		
		start = get_timer(0);
		do {
			for (uint32_t i = 0; i < 500; i++)
				nop();
			if (dmac_read_dcsr(GEU_TX_CHANNEL) & DCSR_STOPSTATE)
				break;
			if (2000 < get_timer(start)) {
				printf("aes dma tx timeout !\n");
				ret = -1;
				break;
				goto exit;
			}
		} while (1);

		start = get_timer(0);
		do {
			if (dmac_read_dcsr(GEU_RX_CHANNEL) & DCSR_STOPSTATE)
				break;

			if (2000 < get_timer(start)) {
				printf("aes dma rx timeout !\n");
				ret = -1;
				break;
				goto exit;
			}
		} while (1);

		if (!(dmac_read_dcsr(GEU_TX_CHANNEL) & DCSR_STOPSTATE)) {
			printf("Error:DMA write operate timeout.\n");
			ret = -1;
			goto exit;
		}
		if (!(dmac_read_dcsr(GEU_RX_CHANNEL) & DCSR_STOPSTATE)) {
			printf("Error:DMA read operate timeout.\n");
			ret = -1;
			goto exit;
		}

		asr_aes_disable_dma();
		remain_len -= step_szie;
		src += step_szie;
		dst += step_szie;
	}

exit:
	asr_aes_disable_dma();
	return ret;
}
#endif

int aes_ecb_encrypt_geu(const uint8_t *key, uint32_t keylen, bool use_rkek,\
						const void* plaintext, void* ciphertext, size_t size)
{
	int ret = 0;
#if CONFIG_PXA_DMA
	bool use_dma = 0;
#endif
	uint32_t crylen = 0;
	void *ptext = NULL, *ctext = NULL;
	uint8_t use_key[32];

	if ((keylen > 32) && (keylen < 16)) {
		printf("aes ecb encrypt key len %d error\n", keylen);
		return -1;
	}

	if ((key == NULL) && (use_rkek == false)) {
		printf("%s error: key can't NULL when not use rkek\n", __func__);
		return -1;
	}

	if (key == NULL) {
		memcpy(use_key, default_key, keylen);
	} else {
		memcpy(use_key, key, keylen);
	}

	crylen = (size + AES_ALG_MASK) & (~AES_ALG_MASK);
	/* if not align, malloc tmp alignmask */
	ptext = buf_align(plaintext, AES_ALG_MASK);
	if (ptext != plaintext) {
		ptext = memalign(16, crylen);
		if (!ptext) {
			printf("%s: %d, malloc failed\n", __func__, __LINE__);
			ret = -1;
			goto exit;
		}
		memcpy(ptext, plaintext, size);
	}
	ctext = buf_align(ciphertext, AES_ALG_MASK);
	if (ctext != ciphertext) {
		ctext = memalign(16, crylen);
		if (!ctext) {
			printf("%s: %d, malloc failed\n", __func__, __LINE__);
			ret = -1;
			goto exit;
		}
		memset(ctext, 0, crylen);
	}

	asr_geu_get();
	geu_reset_unit();
	geu_enable_clk();
	geu_reset_aes();

	ret = asr_aes_setmode(AES_ENCRYPT, AES_ECB);
	if(ret) {
		printf("ecb encrypt set mode error\n");
		goto exit;
	}

	ret = asr_aes_setkey((uint32_t *)key, keylen, use_rkek);
	if(ret) {
		printf("ecb encrypt set key error\n");
		goto exit;
	}	

#if CONFIG_PXA_DMA
	use_dma = (size >= ASR_AES_DMA_THRESHOLD);
	if (use_dma)
		ret = asr_aes_process_dma((uint32_t *)ptext, size, (uint32_t *)ctext);
	else
#endif
		ret = asr_aes_process_pio((uint32_t *)ptext, size, (uint32_t *)ctext);

	if (ctext != ciphertext)
		memcpy(ciphertext, ctext, size);

exit:
	geu_disable_clk();
	asr_geu_put();
	if (ptext && (ptext != plaintext))
		free(ptext);
	if (ctext && (ctext != ciphertext))
		free(ctext);
	return ret;	
}

int aes_ecb_decrypt_geu(const uint8_t *key, uint32_t keylen, bool use_rkek,\
						const void* ciphertext, void* plaintext, size_t size)
{
	int ret = 0;
#if CONFIG_PXA_DMA
	bool use_dma = 0;
#endif
	uint32_t crylen = 0;
	void *ptext = NULL, *ctext = NULL;
	uint8_t use_key[32];

	if ((keylen > 32) && (keylen < 16)) {
		printf("aes ecb decrypt key len %d error\n", keylen);
		return -1;
	}

	if ((key == NULL) && (use_rkek == false)) {
		printf("%s error: key can't NULL when not use rkek\n", __func__);
		return -1;
	}

	if (key == NULL) {
		memcpy(use_key, default_key, keylen);
	} else {
		memcpy(use_key, key, keylen);
	}

	crylen = (size + AES_ALG_MASK) & (~AES_ALG_MASK);
	/* malloc tmp alignmask alignment mem to encrypt */
	ptext = buf_align(plaintext, AES_ALG_MASK);
	if (ptext != plaintext) {
		ptext = memalign(16, crylen);
		if (!ptext) {
			printf("%s: %d, malloc failed\n", __func__, __LINE__);
			ret = -1;
			goto exit;
		}
		memset(ptext, 0, crylen);
	}
	ctext = buf_align(ciphertext, AES_ALG_MASK);
	if (ctext != ciphertext) {
		ctext = memalign(16, crylen);
		if (!ctext) {
			printf("%s: %d, malloc failed\n", __func__, __LINE__);
			ret = -1;
			goto exit;
		}
		memcpy(ctext, ciphertext, size);
	}

	asr_geu_get();
	geu_reset_unit();
	geu_enable_clk();
	geu_reset_aes();

	ret = asr_aes_setmode(AES_DECRYPT, AES_ECB);
	if(ret) {
		printf("ecb decrypt set mode error\n");
		goto exit;
	}
	ret = asr_aes_setkey((uint32_t *)key, keylen, use_rkek);
	if(ret) {
		printf("ecb decrypt set key error\n");
		goto exit;
	}
#if CONFIG_PXA_DMA
	use_dma = (size >= ASR_AES_DMA_THRESHOLD);
	if (use_dma)
		ret = asr_aes_process_dma((uint32_t *)ctext, size, (uint32_t *)ptext);
	else
#endif
		ret = asr_aes_process_pio((uint32_t *)ctext, size, (uint32_t *)ptext);

	if (ptext != plaintext)
		memcpy(plaintext, ptext, size);

exit:
	geu_disable_clk();
	asr_geu_put();
	if (ptext && (ptext != plaintext))
		free(ptext);
	if (ctext && (ctext != ciphertext))
		free(ctext);
	return ret;	
}

int aes_cbc_encrypt_geu(const uint8_t *iv, const uint8_t *key, uint32_t keylen, \
						bool use_rkek, const void* plaintext, void* ciphertext, size_t size)
{
	int ret = 0;
#if CONFIG_PXA_DMA
	bool use_dma = 0;
#endif
	uint32_t crylen = 0;
	void *ptext = NULL, *ctext = NULL;
	uint8_t use_key[32];

	if ((keylen > 32) && (keylen < 16)) {
		printf("aes cbc encrypt key len %d error\n", keylen);
		return -1;
	}

	if ((key == NULL) && (use_rkek == false)) {
		printf("%s error: key can't NULL when not use rkek\n", __func__);
		return -1;
	}

	if (key == NULL) {
		memcpy(use_key, default_key, keylen);
	} else {
		memcpy(use_key, key, keylen);
	}

	crylen = (size + AES_ALG_MASK) & (~AES_ALG_MASK);
	/* if not align, malloc tmp alignmask */
	ptext = buf_align(plaintext, AES_ALG_MASK);
	if (ptext != plaintext) {
		ptext = memalign(16, crylen);
		if (!ptext) {
			printf("%s: %d, malloc failed\n", __func__, __LINE__);
			ret = -1;
			goto exit;
		}
		memcpy(ptext, plaintext, size);
	}
	ctext = buf_align(ciphertext, AES_ALG_MASK);
	if (ctext != ciphertext) {
		ctext = memalign(16, crylen);
		if (!ctext) {
			printf("%s: %d, malloc failed\n", __func__, __LINE__);
			ret = -1;
			goto exit;
		}
		memset(ctext, 0, crylen);
	}

	asr_geu_get();
	geu_reset_unit();
	geu_enable_clk();
	geu_reset_aes();

	ret = asr_aes_setmode(AES_ENCRYPT, AES_CBC);
	if(ret) {
		printf("cbc encrypt: set mode error\n");
		goto exit;
	}

	ret = asr_aes_setkey((uint32_t *)key, keylen, use_rkek);
	if(ret) {
		printf("cbc encrypt: set key error\n");
		goto exit;
	}	

	ret = asr_aes_setiv(iv);
	if(ret) {
		printf("cbc encrypt: set iv error\n");
		goto exit;
	}	

#if CONFIG_PXA_DMA
	use_dma = (size >= ASR_AES_DMA_THRESHOLD);
	if (use_dma)
		ret = asr_aes_process_dma((uint32_t *)ptext, size, (uint32_t *)ctext);
	else
#endif
		ret = asr_aes_process_pio((uint32_t *)ptext, size, (uint32_t *)ctext);

	if (ctext != ciphertext)
		memcpy(ciphertext, ctext, size);

exit:
	geu_disable_clk();
	asr_geu_put();
	if (ptext && (ptext != plaintext))
		free(ptext);
	if (ctext && (ctext != ciphertext))
		free(ctext);
	return ret;	
}

int aes_cbc_decrypt_geu(const uint8_t *iv, const uint8_t *key, uint32_t keylen, \
						bool use_rkek, const void* ciphertext, void* plaintext, size_t size)
{
	int ret = 0;
#if CONFIG_PXA_DMA
	bool use_dma = 0;
#endif
	uint32_t crylen = 0;
	void *ptext = NULL, *ctext = NULL;
	uint8_t use_key[32];

	if ((keylen > 32) && (keylen < 16)) {
		printf("aes cbc decrypt key len %d error\n", keylen);
		return -1;
	}

	if ((key == NULL) && (use_rkek == false)) {
		printf("%s error: key can't NULL when not use rkek\n", __func__);
		return -1;
	}

	if (key == NULL) {
		memcpy(use_key, default_key, keylen);
	} else {
		memcpy(use_key, key, keylen);
	}

	crylen = (size + AES_ALG_MASK) & (~AES_ALG_MASK);
	/* if not align, malloc tmp alignmask */
	ptext = buf_align(plaintext, AES_ALG_MASK);
	if (ptext != plaintext) {
		ptext = memalign(16, crylen);
		if (!ptext) {
			printf("%s: %d, malloc failed\n", __func__, __LINE__);
			ret = -1;
			goto exit;
		}
		memset(ptext, 0, crylen);
	}
	ctext = buf_align(ciphertext, AES_ALG_MASK);
	if (ctext != ciphertext) {
		ctext = memalign(16, crylen);
		if (!ctext) {
			printf("%s: %d, malloc failed\n", __func__, __LINE__);
			ret = -1;
			goto exit;
		}
		memcpy(ctext, ciphertext, size);
	}

	asr_geu_get();
	geu_reset_unit();
	geu_enable_clk();
	geu_reset_aes();

	ret = asr_aes_setmode(AES_DECRYPT, AES_CBC);
	if(ret) {
		printf("cbc decrypt: set mode error\n");
		goto exit;
	}

	ret = asr_aes_setkey((uint32_t *)key, keylen, use_rkek);
	if(ret) {
		printf("cbc decrypt: set key error\n");
		goto exit;
	}	

	ret = asr_aes_setiv(iv);
	if(ret) {
		printf("cbc decrypt: set iv error\n");
		goto exit;
	}	

#if CONFIG_PXA_DMA
	use_dma = (size >= ASR_AES_DMA_THRESHOLD);
	if (use_dma)
		ret = asr_aes_process_dma((uint32_t *)ctext, size, (uint32_t *)ptext);
	else
#endif
		ret = asr_aes_process_pio((uint32_t *)ctext, size, (uint32_t *)ptext);

	if (ptext != plaintext)
		memcpy(plaintext, ptext, size);

exit:
	geu_disable_clk();
	asr_geu_put();
	if (ptext && (ptext != plaintext))
		free(ptext);
	if (ctext && (ctext != ciphertext))
		free(ctext);
	return ret;	
}

#ifdef GEU_SELFTEST
static int asr_aes_ecb_sfotware_test(void)
{
#define DATA_SIZE 		256*1024
#define BLOCK_SIZE 		16
#define KEY_SIZE		32

	uint8_t plaintext_raw[DATA_SIZE + 63], *plaintext;
	uint8_t ciphertext_raw[DATA_SIZE + 63], *ciphertext;
    plaintext = buf_align(plaintext_raw, 0xf);
    ciphertext = buf_align(ciphertext_raw, 0xf);
	memset(plaintext, 0xAB, DATA_SIZE);
	memset(ciphertext, 0, DATA_SIZE);
	uint8_t zero_key[KEY_SIZE] = {0x7a, 0xca, 0x6a, 0x07,\
                             0x78, 0x57, 0xe3, 0xf9,\
                             0xab, 0x30, 0xbb, 0xf2,\
                             0x07, 0xdd, 0x9f, 0x40,\
                             0xf7, 0xaf, 0x1f, 0x34,\
                             0xe0, 0xb8, 0x45, 0xda,\
                             0x12, 0x11, 0xee, 0x66,\
                             0x02, 0x51, 0x4c, 0x5f};
    uint32_t key_schedule[AES_EXPAND_KEY_LENGTH/4] = {0};

	aes_expand_key(zero_key, (u8 *)key_schedule);

    for(uint32_t i = 0; i < DATA_SIZE; i += (AES_STATECOLS * 4))
    {
       aes_encrypt(plaintext + i, (u8 *)key_schedule, ciphertext + i);
    }

    for(uint32_t i = 0; i < DATA_SIZE; i += (AES_STATECOLS * 4))
    {
       aes_decrypt(ciphertext + i, (u8 *)key_schedule, ciphertext + i);
    }

	if (memcmp(plaintext, ciphertext, DATA_SIZE)) {
        printf("sfot_plaintext:\n");
        for (uint32_t i = 0; i < DATA_SIZE; i++) {
            if ((i % 30) == 0)
                printf("\n");
            printf("%02x ", plaintext[i]);
        }

        printf("soft_ciphertext:\n");
        for (uint32_t i = 0; i < DATA_SIZE; i++) {
            if ((i % 30) == 0)
                printf("\n");
            printf("%02x ", ciphertext[i]);
        }
		printf("soft aes ecb test failed");
		goto err;
	}
    printf("soft aes ecb test result: pass\n");
	return 0;
err:
	return -1;
}

static int asr_aes_test(void)
{

#define DATA_SIZE 		0x850
#define BLOCK_SIZE 		16
#define KEY_SIZE		32

	uint8_t plaintext_raw[DATA_SIZE + 63], *plaintext;
	uint8_t ciphertext_raw[DATA_SIZE + 63], *ciphertext;

    plaintext = buf_align(plaintext_raw, 0xf);
    ciphertext = buf_align(ciphertext_raw, 0xf);

	memset(plaintext, 0xAB, DATA_SIZE);
	memset(ciphertext, 0, DATA_SIZE);
	uint8_t iv[BLOCK_SIZE];
	uint8_t key[KEY_SIZE] = {0x7a, 0xca, 0x6a, 0x07,\
                             0x78, 0x57, 0xe3, 0xf9,\
                             0xab, 0x30, 0xbb, 0xf2,\
                             0x07, 0xdd, 0x9f, 0x40,\
                             0xf7, 0xaf, 0x1f, 0x34,\
                             0xe0, 0xb8, 0x45, 0xda,\
                             0x12, 0x11, 0xee, 0x66,\
                             0x02, 0x51, 0x4c, 0x5f};
	/*step1: aes ecb test*/
	if (0 > aes_ecb_encrypt(key, KEY_SIZE, true, plaintext, ciphertext, DATA_SIZE)) {
		printf("aes_ecb_encrypt error\n");
		goto err;
	}

	if (0 > aes_ecb_decrypt(key, KEY_SIZE, true, ciphertext, ciphertext, DATA_SIZE)) {
		printf("aes_ecb_decrypt error\n");
		goto err;
	}

	if (memcmp(plaintext, ciphertext, DATA_SIZE)) {
        printf("plaintext:\n");
        for (uint32_t i = 0; i < DATA_SIZE; i++) {
            if ((i % 30) == 0)
                printf("\n");
            printf("%02x ", plaintext[i]);
        }

        printf("ciphertext:\n");
        for (uint32_t i = 0; i < DATA_SIZE; i++) {
            if ((i % 30) == 0)
                printf("\n");
            printf("%02x ", ciphertext[i]);
        }
		printf("aes ecb test failed");
		goto err;
	}
    printf("aes ecb test result: pass\n");

#if 1
	/*step2: aes cbc test*/
	memset(iv, 0xB, BLOCK_SIZE);
	memset(plaintext, 0xCD, DATA_SIZE);
	memset(ciphertext, 0, DATA_SIZE);
	if (0 > aes_cbc_encrypt(iv, key, KEY_SIZE, true, plaintext, ciphertext, DATA_SIZE)) {
		printf("aes_cbc_encrypt error\n");
		goto err;
	}
	
	if (0 > aes_cbc_decrypt(iv, key, KEY_SIZE, true, ciphertext, ciphertext, DATA_SIZE)) {
		printf("aes_cbc_decrypt error\n");
		goto err;
	}

	if (memcmp(plaintext, ciphertext, DATA_SIZE)) {
        printf("plaintext:\n");
        for (uint32_t i = 0; i < DATA_SIZE; i++) {
            if ((i % 30) == 0)
                printf("\n");
            printf("%02x ", plaintext[i]);
        }

        printf("ciphertext:\n");
        for (uint32_t i = 0; i < DATA_SIZE; i++) {
            if ((i % 30) == 0)
                printf("\n");
            printf("%02x ", ciphertext[i]);
        }
		printf("aes cbc test failed");
		goto err;
	}
    printf("aes cbc test result: pass\n");
#endif
	return 0;
err:
	return -1;
}

void asr_geu_test_init(void)
{
	uint32_t start = 0, time = 0;

    printf("soft aes ecb start test:\n");
	start = get_timer(0);
	asr_aes_ecb_sfotware_test();
	time = get_timer(start);
	printf("soft aes ecb time = %d\n", time);

    printf("aes ecb start test:\n");
	start = get_timer(0);
	asr_aes_test();
	time = get_timer(start);
	printf("hard aes ecb time = %d\n", time);

    printf("aes fuse start test:\n");
	geu_fuse_test();

    printf("aes rng start test:\n");
	geu_random_test();
}
#endif