#include "S08_Flash.h"
#include <hidef.h>

char *srcPtr; 
char *dstPtr;
int count;
unsigned char it; 

const byte flash_cmd_r[] = {0x45, 0x18, 0x25, 0xF6, 0xAA, 0x80, 0xF7, 0x9D,
    0x9D, 0x9D, 0x9D, 0xC6, 0x18, 0x25, 0xA5, 0x30,
    0x26, 0x07, 0xC6, 0x18, 0x25, 0xA5, 0x40, 0x27,
    0xF9, 0x81};
         
void FlashInit(void)
{
  if (!(FCDIV & 0x80))
  {
    if (FSTAT&0x30)         //Check to see if FACCERR is set
    {
        FSTAT |= 0x30;  //write a 1 to FACCERR to clear
    }
    FCDIV = 0x16;  
  }
}                            

void Flash_Cmd(unsigned int Address, unsigned char bData, unsigned char cmd)
{

  srcPtr = (char *)&flash_cmd_r;
  dstPtr = (char *)0x60;
  
  for (count = 0; count < 26;  count++, dstPtr++, srcPtr++) 
  {
    *dstPtr = *srcPtr;
  }
  DisableInterrupts;
	if (FSTAT&0x30){//Check to see if FACCERR is set
		FSTAT = FSTAT | 0x30;//write a 1 to FACCERR to clear
	}
	(*((volatile unsigned char *)(Address))) = bData; //write to somewhere in flash
	FCMD = cmd;                           //set command type.	 
  asm jsr 0x60; 
  EnableInterrupts;

}

void Flash_Cmd_Burst(unsigned int Address, unsigned char size, unsigned char* DataPtr){

  for(it=0; it<size; it++){
    Flash_Cmd(Address, DataPtr[it], 0x20);
    Address++;  
  }
}