#pragma orgall 0x7CC0

#define BOOTLOADER_START_ADDR 0x7CC0
#define START_PROGRAM_ADDR 0x7FC0

static char block[64];

void Start_Program() org START_PROGRAM_ADDR{

}

unsigned short UART_Write_Loop(char send, char receive){
  unsigned int rslt = 0;

  while(1){
    Delay_5ms();
    UART2_Write(send);
    Delay_5ms();

    rslt++;
    if (rslt == 0x0200)
      return 0;
    if (UART2_Read() == receive)
      return 1;
  }
}

void Write_Begin(){
  FLASH_Erase_Write_64(START_PROGRAM_ADDR, block);
  //--- goto main
  block[0] = 0x60;  //0xF03EEF60
  block[1] = 0xEF;
  block[2] = 0x3E;
  block[3] = 0xF0;
}

void Start_Bootload(){
  char i = 0, xx, yy;
  long j = 0;

  while (1) {
    if (i == 64) {
      //--- If 32 words (64 bytes) recieved then write to flash
      if (!j)
        Write_Begin();
      if (j<BOOTLOADER_START_ADDR){
        FLASH_Erase_Write_64(j, block);
      }

      i = 0;
      j += 0x40;
    }
    //--- Ask for yy
    UART2_Write('y');
    while (!UART2_Data_Ready()) ;
    //--- Read yy
    yy = UART2_Read();
    //--- Ask for xx
    UART2_Write('x');
    while (!UART2_Data_Ready()) ;
    //--- Read xx
    xx = UART2_Read();
    //--- Save xxyy in block[i]
    block[i++] = yy;
    block[i++] = xx;
  }
}

void main() org BOOTLOADER_START_ADDR{
 // ANSELC = 0;                         // Configure PORTC pins as digital
  UART2_Init(115200);                 // Init USART at 115200
  if (UART_Write_Loop('g','r')) {     // Send 'g' for ~5 sec, if 'r'
    Start_Bootload();                 //   received start bootload
  }
  else {
    Start_Program();                  //   else start program
  }
}