void FLASH_byte_prog(unsigned int addr, unsigned char data)
{
 unsigned char _pgm[sizeof(ROM_PGM) + 2];
 
 _pgm[0] = (unsigned char)((addr & 0xff00) >> 8);
 _pgm[1] = (unsigned char)(addr & 0x00ff);
 (void)memcpy(_pgm + 2, ROM_PGM, sizeof(ROM_PGM));
 _pgm[7] = FLASH_CMD_BYTEPROG; // change FCMD value
 if (FSTAT&0x10) // Check to see if FACCERR is set
 {
  FSTAT = FSTAT | 0x10; // write a 1 to FACCERR to clear
 }
 __asm
 {
  LDA data // pass 'data' thru A
  TSX // transfer SP to HX
  JSR 2,X // first 2 bytes of _pgm[] are 'addr'
 }
}

void FLASHdatas_prog(unsigned int addr, unsigned char * data, unsigned int _length)
{
 unsigned char _pgm[sizeof(ROM_FAST_PGM) + 6];
 
 if (_length == 0) return;
 _pgm[0] = (unsigned char)(((addr) & 0xff00) >> 8);
 _pgm[1] = (unsigned char) ((addr) & 0x00ff);
 _pgm[2] = (unsigned char)((((unsigned int)data) & 0xff00) >> 8);
 _pgm[3] = (unsigned char) (((unsigned int)data) & 0x00ff);
 _pgm[4] = (unsigned char)(((_length) & 0xff00) >> 8);
 _pgm[5] = (unsigned char) ((_length) & 0x00ff);
 
 (void)memcpy(_pgm + 6, ROM_FAST_PGM, sizeof(ROM_FAST_PGM));
 if (FSTAT&0x10) // Check to see if FACCERR is set
 {
  FSTAT = FSTAT | 0x10; // write a 1 to FACCERR to clear
 }
 __asm
 {
  TSX // transfer SP to HX
  JSR 6,X // first 2 bytes of _pgm[] are 'addr'
 }
}

void FLASH_page_erase(unsigned int addr)
{
 unsigned char _pgm[sizeof(ROM_PGM) + 2];
 
 _pgm[0] = (unsigned char)((addr & 0xff00) >> 8);
 _pgm[1] = (unsigned char)(addr & 0x00ff);
 (void)memcpy(_pgm + 2, ROM_PGM, sizeof(ROM_PGM));
 _pgm[7] = FLASH_CMD_PAGEERASE;// change FCMD value
 if (FSTAT&0x10)  // Check to see if FACCERR is set
 {
  FSTAT = FSTAT | 0x10; // write a 1 to FACCERR to clear
 }
 __asm
 {
  TSX // transfer SP to HX
  JSR 2,X // first 2 bytes of _pgm[] are 'addr'
 }
}