#include "h/flashmem.h"
#include "h/p33FJ64GP802.h"

unsigned char __attribute__((space(auto_psv), aligned(1024))) flashBuffer[512*3];
unsigned int iec0;
unsigned int iec1;
unsigned int iec2;
unsigned int iec3;
unsigned int iec4;

void disableInterrupts(void)
{
	iec0=IEC0;
	iec1=IEC1;
	iec2=IEC2;
	iec3=IEC3;
	iec4=IEC4;
	IEC0=0;
	IEC1=0;
	IEC2=0;
	IEC3=0;
	IEC4=0;
} 

void enableInterrupts(void)
{
	IEC0=iec0;
	IEC1=iec1;
	IEC2=iec2;
	IEC3=iec3;
	IEC4=iec4;
}

void eraseFlashRow(void)
{
	unsigned int i;

	NVMCON=0x4042;
	TBLPAG=__builtin_tblpage(&flashBuffer[0]);
	i=__builtin_tbloffset(&flashBuffer[0]);
	__builtin_tblwtl(i, 0);
	asm("disi #5");
	__builtin_write_NVM();
}

void readFlashRow(void)
{
	// read the flash memory to RAM
	unsigned int i, j, l, p, x;
	TBLPAG=__builtin_tblpage(&flashBuffer[0]);
	i=__builtin_tbloffset(&flashBuffer[0]);
	l=0;
	p=0;
	for(j=0; j<512; j++)
	{
	x=__builtin_tblrdl(i+l);
	myBuffer[p]=x;
	myBuffer[p+1]=(x>>8);
	x=__builtin_tblrdh(i+l+1);
	myBuffer[p+2]=x;
	l+=2;
	p+=3;
	}
}

void writeFlashRow(void)
{
	// write the contents of RAM to Flash Memory
	unsigned int i, j, u, p, l;

	disableInterrupts();
	eraseFlashRow();
	TBLPAG=__builtin_tblpage(&flashBuffer[0]);
	i=__builtin_tbloffset(&flashBuffer[0]);
	l=0;
	p=0;
	for(u=0; u<8; u++)
	{
	for(j=0; j<64; j++)
	{
	__builtin_tblwtl(i+l, (unsigned int)(myBuffer[p]+(myBuffer[p+1]<<8)));
	__builtin_tblwth(i+l+1, (unsigned int)myBuffer[p+2]);
	l+=2;
	p+=3;
	}
	NVMCON=0x4001;
	asm("disi #5");
	__builtin_write_NVM();
	}
	enableInterrupts();
}

unsigned long readFlashMemory(unsigned int offset)
{
	unsigned long x;
	unsigned int i;
	TBLPAG=__builtin_tblpage(&flashBuffer[0]);
	i=__builtin_tbloffset(&flashBuffer[0]);
	i+=(offset+offset);
	x=(unsigned long)__builtin_tblrdh(i+1);
	x=x<<16;
	x+=(unsigned long)__builtin_tblrdl(i);
	return x;
}

unsigned long writeFlashMemory(unsigned int offset, unsigned long data)
{
	unsigned int i;
	readFlashRow();
	i=offset+offset+offset;
	myBuffer[i+2]=(data>>16);
	myBuffer[i+1]=(data>>8);	
	myBuffer[i]=data;
	writeFlashRow();
	return data;
}
