#include "cfiflash.h"

uint32_t cfi_get_status(flash_t *pObj)
{
	volatile uint32_t *pF;
	uint32_t status;

	pF = (uint32_t*)pObj->pBase;

	*pF = 0x00700070;

	status = *pF;

	*pF = 0x00FF00FF;

	return status;

}

void cfi_init(flash_t *pObj, uint32_t base_addr)
{
	int i;
	uint8_t *pInfo;
	uint32_t endian;
	uint8_t *pEndian;

	pObj->pBase = (void*)base_addr;

	pInfo = (uint8_t*)&pObj->info;
	for (i=0; i < sizeof(flash_info_t); i++)
	{
		pInfo[i] = 0;
	}
	pObj->eb = 0;
	endian = 0x12345678;
	pEndian = (uint8_t*)&endian;
	if (*pEndian == 0x12)
	{
		pObj->eb = 1;
	}

}

uint32_t cfi_find(flash_t *pObj)
{
	int i;
	volatile uint32_t *pF;
	volatile uint16_t *pF16;
//	uint8_t qry_ref[12] = {0x51, 0x00, 0x51, 0x00, 0x52, 0x00, 0x52, 0x00, 0x59, 0x00, 0x59, 0x00};
	uint32_t size;

	pF = (uint32_t*)pObj->pBase;
	pF16 = (uint16_t*)pObj->pBase;

	// Look for flash
	*pF = 0x00980098;
//	for (i=0; i < sizeof(qry_ref); i++)
//		if (((uint8_t*)(&pF[0x10]))[i] != qry_ref[i])
//			return (UINT32)-1;

	*pF = 0x00FF00FF;
	*pF = 0x00900090;

/*
	printf("ManID             : %8.8X\n", (UINT32)pF[0]);
	printf("Device code       : %8.8X\n", (UINT32)pF[1]);
	printf("Block info        : %8.8X\n", (UINT32)pF[2]);

	printf("VCC(min)          : %8.8X\n", (UINT32)pF[0x1B]);
	printf("VCC(max)          : %8.8X\n", (UINT32)pF[0x1C]);
	printf("VPP(min)          : %8.8X\n", (UINT32)pF[0x1D]);
	printf("VPP(max)          : %8.8X\n", (UINT32)pF[0x1E]);
	printf("Device Layout     : %8.8X\n", (UINT32)pF[0x27]);
	printf("Interface type    : %8.8X %8.8X\n", (UINT32)pF[0x28], (UINT32)pF[0x29]);
	printf("Write buffer size : %8.8X %8.8X\n", (UINT32)pF[0x2A], (UINT32)pF[0x2B]);
	printf("Num. erase blocks : %8.8X\n", (UINT32)pF[0x2C]);
	printf("Erase block info  : %8.8X %8.8X %8.8X %8.8X\n", (UINT32)pF[0x2D], (UINT32)pF[0x2E], (UINT32)pF[0x2F], (UINT32)pF[0x30]);
*/
	pObj->info.num_flash = 2;
	pObj->info.if_width = 32;
	size = (uint32_t)pF16[2*0x27];
	pObj->info.flashsize = pObj->info.num_flash;
	for (i=0; i < size; i++)
		pObj->info.flashsize *= 2;

	size = (uint32_t)(pF16[2*0x2B] << 8 | pF16[2*0x2A]);
	pObj->info.wbuf_size = pObj->info.num_flash;
	for (i=0; i < size; i++)
		pObj->info.wbuf_size *= 2;

	pObj->info.nblocks = (uint32_t)(pF16[2*0x2E] << 8 | pF16[2*0x2D]) + 1;
	pObj->info.blocksize = pObj->info.num_flash * ((uint32_t)(pF16[2*0x30] << 8 | pF16[2*0x2F]) * 256);

	*pF = 0x00FF00FF;

	return 0;
}

uint32_t cfi_block_is_locked(flash_t *pObj, uint32_t word_index)
{
	volatile uint32_t *pF;
	uint32_t status, block_index, block_mask;
	uint32_t error;

	pF = (uint32_t*)pObj->pBase;

	if (word_index >= (pObj->info.flashsize/4))
		return CFI_ERR_INVPARAM;

	block_mask = ((pObj->info.nblocks-1) << 16);
	block_index = word_index & block_mask;

	*pF = 0x00900090;

	error = ((uint32_t)pF[block_index+2] & 0x00010001) != 0;
	*pF = 0x00FF00FF;

	return error;
}

uint32_t cfi_block_erase(flash_t *pObj, uint32_t word_index)
{
	volatile uint32_t *pF;
	uint32_t status, block_index, block_mask;
	uint32_t error;

	pF = (uint32_t*)pObj->pBase;

	if (word_index >= (pObj->info.flashsize/4))
		return CFI_ERR_INVPARAM;

	block_mask = ((pObj->info.nblocks-1) << 16);
	block_index = word_index & block_mask;

	pF[block_index] = 0x00200020;
	pF[block_index] = 0x00D000D0;

	error = 0;
	do
	{
		status = pF[block_index];

	} while((status & SR_BIT_ISMS) != SR_BIT_ISMS);

	if (status & (SR_BIT_ECLBS | SR_BIT_VPENS | SR_BIT_DPS))
	{
		error = CFI_ERR_DEV_FAIL | status;
	}

	pF[block_index] = 0x00FF00FF;

	return error;
}

uint32_t cfi_block_lock(flash_t *pObj, uint32_t word_index)
{
	volatile uint32_t *pF;
	uint32_t status, block_index, block_mask;
	uint32_t error;

	pF = (uint32_t*)pObj->pBase;

	if (word_index >= (pObj->info.flashsize/4))
		return CFI_ERR_INVPARAM;

	block_mask = ((pObj->info.nblocks-1) << 16);
	block_index = word_index & block_mask;

	pF[block_index] = 0x00600060;
	pF[block_index] = 0x00010001;

	error = 0;
	do
	{
		status = pF[block_index];

	} while((status & SR_BIT_ISMS) != SR_BIT_ISMS);

	if (status & (SR_BIT_PSLBS | SR_BIT_VPENS | SR_BIT_ECLBS))
	{
		error = CFI_ERR_DEV_FAIL | status;
	}

	pF[block_index] = 0x00FF00FF;

	return error;
}

uint32_t cfi_block_unlock(flash_t *pObj, uint32_t word_index)
{
	volatile uint32_t *pF;
	uint32_t status, block_index, block_mask;
	uint32_t error;

	pF = (uint32_t*)pObj->pBase;

	if (word_index >= (pObj->info.flashsize/4))
		return CFI_ERR_INVPARAM;

	block_mask = ((pObj->info.nblocks-1) << 16);
	block_index = word_index & block_mask;

	pF[block_index] = 0x00600060;
	pF[block_index] = 0x00D000D0;

	error = 0;
	do
	{
		status = pF[block_index];

	} while((status & SR_BIT_ISMS) != SR_BIT_ISMS);

	if (status & (SR_BIT_PSLBS | SR_BIT_VPENS | SR_BIT_ECLBS))
	{
		error = CFI_ERR_DEV_FAIL | status;
	}

	pF[block_index] = 0x00FF00FF;

	return error;
}

uint32_t cfi_program_single(flash_t *pObj, uint32_t word_index, uint32_t word)
{
	volatile uint32_t *pF;
	uint32_t status, error;

	pF = (uint32_t*)pObj->pBase;

	if (word_index >= (pObj->info.flashsize/4))
		return CFI_ERR_INVPARAM;

	pF[word_index] = 0x00400040;
	pF[word_index] = word;

	error = 0;
	do
	{
		status = pF[word_index];

	} while((status & SR_BIT_ISMS) != SR_BIT_ISMS);

	if (status & (SR_BIT_PSLBS | SR_BIT_VPENS | SR_BIT_DPS))
	{
		error = CFI_ERR_DEV_FAIL | status;
	}

	pF[word_index] = 0x00FF00FF;

	return error;

}

uint32_t cfi_program_multi(flash_t *pObj, uint32_t word_index, uint32_t *pWords, uint32_t num_words)
{
	int i, j, k;
	volatile uint32_t *pF;
	uint32_t status, bcurr, bnext, block_mask, nblock_write, nbuf_write;
	uint32_t error;

	pF = (uint32_t*)pObj->pBase;

	if (word_index >= (pObj->info.flashsize/4))
		return CFI_ERR_INVPARAM;

	block_mask = ((pObj->info.nblocks-1) << 16);

	k = 0;
	while(num_words)
	{

		bcurr = word_index & block_mask;
		bnext = bcurr + (1 << 16);
		nblock_write = bnext - word_index;
		if (nblock_write > num_words)
			nblock_write = num_words;

		while(nblock_write)
		{
			error = 0;
			pF[bcurr] = 0x00E800E8;
			do
			{
				status = pF[bcurr];

			} while((status & SR_BIT_ISMS) != SR_BIT_ISMS);
			if (status & (SR_BIT_PSLBS | SR_BIT_VPENS | SR_BIT_DPS))
			{
				error = CFI_ERR_DEV_FAIL | status;
				break;
			}

			nbuf_write = nblock_write;
			if (nblock_write > pObj->info.wbuf_size/4)
				nbuf_write = pObj->info.wbuf_size/4;

			pF[bcurr] = (nbuf_write-1) << 16 | (nbuf_write-1);

			for (j=0; j < nbuf_write; j++)
				pF[word_index+j] = pWords[k++];

			word_index += j;
			nblock_write -= j;
			num_words -= j;

			error = 0;
			pF[bcurr] = 0x00D000D0;
			do
			{
				status = pF[bcurr];

			} while((status & SR_BIT_ISMS) != SR_BIT_ISMS);
			if (status & (SR_BIT_PSLBS | SR_BIT_VPENS | SR_BIT_DPS))
			{
				error = CFI_ERR_DEV_FAIL | status;
				break;
			}

			pF[bcurr] = 0x00FF00FF;
		}

		if (CFI_IS_ERROR(error))
		{
			pF[bcurr] = 0x00FF00FF;
			break;
		}
		bcurr = bnext;
	}

	return error;

}

uint32_t flash_find(flash_t *pObj, uint32_t base_addr)
{
	cfi_init(pObj, base_addr);
	return cfi_find(pObj);
}

uint32_t flash_erase(flash_t *pObj, uint32_t offset, uint32_t size)
{
	int i;
	uint32_t offset_end, error;

	if (size % 4)
		size = 4*(size/4 + 1);

	offset_end = offset + size;

	for (i=offset; i < offset_end; i += pObj->info.blocksize)
	{
		if (cfi_block_is_locked(pObj, i/4))
		{
			error = cfi_block_unlock(pObj, i/4);
			if (CFI_IS_ERROR(error))
				break;
		}
		error = cfi_block_erase(pObj, i/4);
		if (CFI_IS_ERROR(error))
			break;
	}

	return error;

}

uint32_t flash_program(flash_t *pObj, uint32_t offset, uint8_t *pData, uint32_t size)
{
	if (size % 4)
		size = 4*(size/4 + 1);

	return cfi_program_multi(pObj, offset/4, (uint32_t*)pData, size/4);
}

uint32_t flash_verify(flash_t *pObj, uint32_t offset, uint8_t *pData, uint32_t size)
{
	int i;
	uint8_t *pF;

	pF = (uint8_t*)(pObj->pBase + offset);

	for (i=0; i < size; i++)
		if (pF[i] != pData[i])
			return CFI_ERR_VFY_FAIL;

	return 0;
}

uint32_t flash_get_blocknum_by_offset(flash_t *pObj, uint32_t offset)
{
	uint32_t blocknum;

	blocknum = offset/pObj->info.blocksize;
	if (offset%pObj->info.blocksize)
		blocknum++;

	return blocknum;
}

uint32_t flash_get_offset_by_blocknum(flash_t *pObj, uint32_t blocknum)
{
	if (blocknum >= pObj->info.nblocks)
		blocknum = pObj->info.nblocks - 1;

	return blocknum*pObj->info.blocksize;
}

uint32_t flash_get_offset_blockaligned(flash_t *pObj, uint32_t offset)
{
	return flash_get_blocknum_by_offset(pObj, offset) * pObj->info.blocksize;
}