#include <startup.h>
#include <LPC214X.h>
#include <TFT018.h>
#include <rprintf.h>

#define LED 0x80000000

#define XTAL   12000000                      // Oscillator Frequency
#define CPUCLK  (XTAL*5)                     // CPU Clock with PLL
#define CCLK   (CPUCLK / 1000)                 // CPU Clock without PLL in kHz

struct iap_in {
  unsigned int cmd;
  unsigned int par[4];  
};

typedef void (*IAP)(struct iap_in *in, unsigned int *result);
#define iap_entry ((IAP) 0x7FFFFFF1)            // IAP entry point

/*
 * Convert 'addr' to sector number
 */
unsigned int get_secnum (void *addr)  {
  unsigned int a,n;

  a = (unsigned int)addr;

	if(  a<= 0x8000 )
  		n = a >> 12;
	else if( a < 0x78000 )
		n = a / 0x8000 + 8;
	else
		n = ( a - 0x78000 )>>12 + 22; 

  return (n);                                   // sector number
}

/*
 * Erase Sector between 'start' and 'end'
 * Return:  IAP error code (0 when OK)
 * NOTES:  start needs to be a 256 byte boundary
 *         size should be 256, 512, 1024 or 4089
 */
unsigned int erase (void* start, void* end)  {
  struct iap_in  iap;                      // IAP input parameters
  unsigned int result[16];                 // IAP results
  unsigned int save_VicInt;                // for saving of interrupt enable register

  save_VicInt = VICIntEnable;              // save interrupt enable status
  VICIntEnClr = 0xFFFFFFFF;                // disable all interrupts

  iap.cmd = 50;                            // IAP Command: Prepare Sectors for Write
  iap.par[0] = get_secnum (start);         // start sector
  iap.par[1] = get_secnum (end);           // end sector
  iap_entry (&iap, result);                // call IAP function
  if (result[0])  goto exit;               // an error occured?

  iap.cmd = 52;                            // IAP command: Erase Flash
  iap.par[0] = get_secnum (start);         // start sector
  iap.par[1] = get_secnum (end);           // end sector
  iap.par[2] = CCLK;                       // CPU clock
  iap_entry (&iap, result);                // call IAP function

exit:
  VICIntEnable = save_VicInt;              // enable interrupts
  return (result[0]);
}

/*
 * Program *data to flash_addr. number of bytes specified by size
 * Return:  IAP error code (0 when OK)
 * Note: 
 */
unsigned int program (void *flash_addr, void *data, unsigned int size)  {
  struct iap_in  iap;                      // IAP input parameters
  unsigned int result[16];                 // IAP results
  unsigned int save_VicInt;                // for saving of interrupt enable register

  save_VicInt = VICIntEnable;              // save interrupt enable status
  VICIntEnClr = 0xFFFFFFFF;                // disable all interrupts

  iap.cmd = 50;                            // IAP Command: Prepare Sectors for Write
  iap.par[0] = get_secnum (flash_addr);    // start sector
  iap.par[1] = iap.par[0];                 // end Sektor
  iap_entry (&iap, result);                // call IAP function
  if (result[0])  goto exit;               // an error occured?

  iap.cmd = 51;                            // IAP Command: Copy RAM to Flash
  iap.par[0] = (unsigned int) flash_addr;  // destination-addr
  iap.par[1] = (unsigned int) data;        // source-addr
  iap.par[2] = size;                       // number of bytes
  iap.par[3] = CCLK;                       // CPU clock
  iap_entry (&iap, result);                // call IAP function

exit:
  VICIntEnable = save_VicInt;              // enable interrupts
  return (result[0]);
}

unsigned char vals[512];
volatile unsigned char * IAPaddr;

char s[20], *sp;
int input_str(int ch){
	*sp ++ = (char)ch;
	*sp = 0;
	return ch;
}

int main (void) {
	unsigned int i;
	char s[20];

	Initialize();
	BacklightOn();
	LCD_Init();
	LCD_Clear_Screen(Blue);

	for (i = 0; i < 512; i++)  {
		vals[i] = (unsigned char) i;
		}

	sp = s;
	rprintf_devopen( input_str );
	rprintf("ERASE 0x8000");
	LCD_PutString(1,0,s,0xF800,0xFFE0);
	i=erase((void*)0x8000, (void*)0xFFFF);

	sp = s;
	rprintf_devopen( input_str );
	rprintf("WRITE 0x8000");
	LCD_PutString(1,40,s,0xF800,0xFFE0);
	i=program ((void*)0x8000, (void*)vals, 512);
	if( i )
		LCD_PutString(1,60,"ERROR!",0xF800,0xFFE0);
	else
		LCD_PutString(1,60,"OK!",0xF800,0xFFE0);

	sp = s;
	rprintf_devopen( input_str );
	rprintf("READ 0x8000");
	LCD_PutString(1,80,s,0xF800,0xFFE0);
	for (i = 0; i < 5; i++)  {
		unsigned char temp;
		IAPaddr =  (volatile unsigned char *)0x8000;
		temp = *(IAPaddr + i);
		sp = s;
		rprintf_devopen( input_str );
		rprintf("%02x",temp);
		//rprintf("%X",i);
		LCD_PutString(5+i*20,100,s,0xF800,0xFFE0);
		}


	for(;;);
	}
