#include <stdio.h>
#include <stdlib.h>
#include "libsys.h"
#include "cfiflash.h"

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

	pF = (UINT32*)pObj->pBase;

	*pF = 0x00700070;

	status = *pF;

	*pF = 0x00FF00FF;

	return status;

}

void cfi_init(flash_t *pObj, UINT32 base_addr)
{
	int i;
	UINT8 *pInfo;

	pObj->pBase = (void*)base_addr;

	pInfo = (UINT8*)&pObj->info;
	for (i=0; i < sizeof(flash_info_t); i++)
	{
		pInfo[i] = 0;
	}
}

UINT32 cfi_find(flash_t *pObj)
{
	int i;
	volatile UINT32 *pF;
	volatile UINT16 *pF16;
	UINT8 qry_ref[12] = {0x51, 0x00, 0x51, 0x00, 0x52, 0x00, 0x52, 0x00, 0x59, 0x00, 0x59, 0x00};
	UINT32 size;

	pF = (UINT32*)pObj->pBase;
	pF16 = (UINT16*)pObj->pBase;

	// Look for flash
	*pF = 0x00980098;
	for (i=0; i < sizeof(qry_ref); i++)
		if (((UINT8*)(&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)pF16[2*0x27];
	pObj->info.flashsize = pObj->info.num_flash;
	for (i=0; i < size; i++)
		pObj->info.flashsize *= 2;

	size = (UINT32)(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 = pObj->info.num_flash * ((UINT32)(pF16[2*0x2E] << 8 | pF16[2*0x2D]) + 1);
	pObj->info.blocksize = pObj->info.num_flash * ((UINT32)(pF16[2*0x30] << 8 | pF16[2*0x2F]) * 256);

	*pF = 0x00FF00FF;

	return 0;
}

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

	pF = (UINT32*)pObj->pBase;

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

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

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

	do
	{
		status = pF[block_index];

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

	pF[block_index] = 0x00FF00FF;

	return 0;
}

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

	pF = (UINT32*)pObj->pBase;

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

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

	do
	{
		status = pF[word_index];

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

	pF[word_index] = 0x00FF00FF;

	return 0;

}

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

	pF = (UINT32*)pObj->pBase;

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

	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;

//		printf("bcurr        : %8.8X\n", bcurr);
//		printf("bnext        : %8.8X\n", bnext);
//		printf("windex       : %8.8X\n", word_index);
//		printf("num_words    : %d\n", num_words);
//		printf("nblock_write : %d\n", nblock_write);

		while(nblock_write)
		{
			pF[bcurr] = 0x00E800E8;
			do
			{
				status = pF[bcurr];
//				printf("status       : %8.8X\n", status);

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

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

//			printf("nbuf_write   : %d\n", nbuf_write);

			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;

			pF[bcurr] = 0x00D000D0;
			do
			{
				status = pF[bcurr];
//				printf("status       : %8.8X\n", status);

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

			pF[bcurr] = 0x00FF00FF;
		}
		bcurr = bnext;
	}

	return 0;

}

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

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

	offset_end = offset + size;

	for (i=offset; i < offset_end; i += pObj->info.blocksize)
		if (cfi_block_erase(pObj, i/4) < 0)
			return -1;

	return 0;

}

UINT32 flash_program(flash_t *pObj, UINT32 offset, UINT8 *pData, UINT32 size)
{
	return cfi_program_multi(pObj, offset/4, (UINT32*)pData, size/4);
}

UINT32 flash_verify(flash_t *pObj, UINT32 offset, UINT8 *pData, UINT32 size)
{
	int i;
	UINT8 *pF;

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

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

	return 0;
}