#include <common.h>
#include <asm/io.h>
#include "rx63xflash.h"

static int wait_frdy(void) __attribute__((section(".iram.text")));
static int wait_frdy(void)
{
	int i;
	volatile int j;
	for (i = 0; i < 10; i++) {
		if (__raw_readb(0x7fffb0) & 0x80)
			return 0;
		for(j = 0; j < 10000; j++);
	}
	return -1;
}
	
static void normal_mode(unsigned long addr) __attribute__((section(".iram.text")));
static void normal_mode(unsigned long addr)
{
	__raw_writeb(0xff, addr);
	__raw_writew(FENTRYR_KEY | 0x00, FENTRYR);
	__raw_writeb(0x02, FWEPROR);
}

static int pe_mode(unsigned long addr, unsigned char fentryd)
	__attribute__((section(".iram.text")));
static int pe_mode(unsigned long addr, unsigned char fentryd)
{
	/* Enter P/E mode */
	__raw_writew(FENTRYR_KEY | 0x00, FENTRYR);
	__raw_writew(FENTRYR_KEY | fentryd, FENTRYR);
	__raw_writeb(0x01, FWEPROR);
	/* clear error status */
	if (__raw_readb(FSTATR0) & FSTATR0_ILGLERR) {
		if (__raw_readb(FASTAT) != FASTAT_CMDLK)
			__raw_writeb(FASTAT_CMDLK, FASTAT);
	}
	/* clock setting */
	__raw_writeb(FCU_STATCLR, addr);
	__raw_writew(CONFIG_SYS_CLK_FREQ / 1000000, PCKAR);
	__raw_writeb(FCU_CLK1, addr);
	__raw_writeb(FCU_CLK2, addr);
	__raw_writew(FCU_CLK345, addr);
	__raw_writew(FCU_CLK345, addr);
	__raw_writew(FCU_CLK345, addr);
	__raw_writeb(FCU_CLK6, addr);
	if(wait_frdy() < 0)
		return -1;
	return 0;
}

static int protect_c(unsigned long addr, int prot, unsigned char fentry)
	__attribute__((section(".iram.text")));
static int protect_c(unsigned long addr, int prot, unsigned char fentry)
{
	pe_mode(addr, fentry);
	if (prot) {
		__raw_writeb(0x77, addr);
		__raw_writeb(0xd0, addr);
		if (wait_frdy() < 0) {
			normal_mode(addr);
			return ERR_TIMOUT;
		}
	} else {
		__raw_writeb(0x71, addr);
		if (wait_frdy() < 0) {
			normal_mode(addr);
			return ERR_TIMOUT;
		}
		if (__raw_readb(addr) == 0) {
			__raw_writew(0x5501, FPROTR);
			__raw_writeb(0xff, addr);
			__raw_writeb(0x20, addr);
			__raw_writeb(0xd0, addr);
			if (wait_frdy() < 0) {
				normal_mode(addr);
				return ERR_TIMOUT;
			}
		}
	}
	normal_mode(addr);
	return 0;
}

static int write_c(uchar *src, ulong addr, ulong cnt, unsigned char fentry)
	__attribute__((section(".iram.text")));
static int write_c(uchar *src, ulong addr, ulong cnt, unsigned char fentry)
{
	ulong dst;
	int i;
	pe_mode(addr, fentry);
	dst = addr & ~0x000007f;
	while(cnt > 0) {
		__raw_writeb(FCU_PROG, dst);
		__raw_writeb(128 / 2, dst);
		for(i = 0; i < 128; dst += 2, i += 2) {
			if (dst < addr || cnt == 0) 
				__raw_writew(0xffff, dst);
			else {
				__raw_writew(*(unsigned short *)src, dst);
				cnt -= 2;
				src += 2;
			}
		}
		__raw_writeb(FCU_PROG_DONE, dst);
		if (wait_frdy() < 0) {
			normal_mode(addr);
			return ERR_TIMOUT;
		}
	}
	normal_mode(addr);
	return 0;
}

static int erase_page_c(unsigned int addr, unsigned char fentry)
	__attribute__((section(".iram.text")));
static int erase_page_c(unsigned int addr, unsigned char fentry)
{
	pe_mode(addr, fentry);
	__raw_writeb(0x20, addr);
	__raw_writeb(0xd0, addr);
	if (wait_frdy() < 0) {
		normal_mode(addr);
		return ERR_TIMOUT;
	}
	normal_mode(addr);
	return 0;
}

int (*rx_flash_wait_frdy)(void) = wait_frdy;
int (*rx_flash_pe_mode)(unsigned long, unsigned char) = pe_mode;
void (*rx_flash_normal_mode)(unsigned long) = normal_mode;
int (*rx_flash_write_c)(uchar *, ulong, ulong, unsigned char) = write_c;
int (*rx_flash_protect_c)(unsigned long, int, unsigned char) = protect_c;
int (*rx_flash_erase_page_c)(unsigned int, unsigned char) = erase_page_c;
