unsigned char WriteFlash(unsigned char *UserBuffer, unsigned char AddrH){

   unsigned char i,j;
   unsigned int n;

   Store[0]=TBLPTRU; Store[1]=TBLPTRH;   Store[2]=TBLPTRL; Store[3]=TABLAT;
   TBLPTRU = 0; TBLPTRH = AddrH;

   for (i=0;i<4;i++){
      TBLPTRL = i*64;
      EECON1 = 0b10010100;
      EECON2 = 0x55;
      EECON2 = 0xAA;
      EECON1bits.WR = 1;
   }
   TBLPTRL=0;TBLPTRH = AddrH;
   for(i=0;i<32;i++) {
      for(j=0;j<8;j++) {
         TABLAT=UserBuffer[i*8+j];
         _asm TBLWTPOSTINC _endasm
      }
      _asm TBLRDPOSTDEC _endasm
      EECON1 = 0b10000100;
      EECON2 = 0x55;
      EECON2 = 0xAA;
      EECON1bits.WR = 1;
      Nop();
      _asm TBLRDPOSTINC _endasm         
   }
   EECON1=0;

   TBLPTRL=0;TBLPTRH = AddrH;
   for(n=0;n<256;n++) {
      _asm TBLRDPOSTINC _endasm
      if(TABLAT != UserBuffer[n]) {
         TBLPTRU = Store[0]; TBLPTRH = Store[1];   TBLPTRL = Store[2]; TABLAT = Store[3];
         return FLASH_WRITE_ERROR;
      }
   }   
   TBLPTRU=Store[0]; TBLPTRH=Store[1];   TBLPTRL=Store[2]; TABLAT=Store[3];
   return 0;
}

#define VERSION 0x3130     //
#define VERSION_WORD 0x7B8F
#ROM VERSION_WORD = {VERSION}
//#########################################
#byte rINTCON  = 0x0FF2
#byte rINTCON2 = 0x0FF1
#byte rINTCON3 = 0x0FF0
#byte rPIE_1    = 0x0F9D
#byte rPIE_2    = 0x0FA0
#bit  rWRERR    = 0x0FA6.3
//#byte PIR1     = 0x0F9E
//#byte RCSTA    = 0x0FAB
#byte rTXREG    = 0x0FAD
#byte rTXSTA    = 0x0FAC
#bit  rTXEN     = 0xFAC.5
#bit  rTXIF     = 0x0F9E.4
#bit  rTRMT     = 0x0FAC.1     // Transmit Buffer Emty (1 = Emty)
//#bit  RCIF     = 0x0F9E.5
#bit  rCOM_EN   = 0xF8C.0      // RS485 Enable/Diable
#bit  rUART_EN  = 0x0FAB.7     // SPEN (Serial Port Enable)
#bit  rSYNC    = 0xFAC.4


#ORG 0x7B90,0x7FFF AUTO=0 DEFAULT
void Bootloader()
{#use rs232(baud=691200,parity=N,xmit=PIN_C6,rcv=PIN_C7,bits=8,enable=PIN_D5)
int8 i,cnt1,cnt2,cnt3,bltmp1,bltmp2,version_h,version_l;
int16 wrdata;
int32 fladr;
rINTCON = 0x00;
rINTCON2 = 0x80;
rINTCON3 = 0;
rPIE_1 = 0;
rPIE_2 = 0;
cnt1  = 0;
cnt2  = 0;
rx_stat = 0;
If (CON_BTLDR == True)
{version_h = (int8)(read_program_eeprom(VERSION_WORD)>>8);
  version_l = read_program_eeprom(VERSION_WORD);
printf("K");putc(0x09);printf("My Device Bootloader %C.%C",version_h,version_l);}
else break;
CON_BTLDR = False;
boot_check:
if (!kbhit())  goto poll_boot_end;
  if (!ch1)
   {
   fladr = (int32)getc()<<24;
   ch1 = True;       // Most significant 8 bits <31:24>
   }else
   if (!ch2)
   {fladr+= (int32)getc()<<16;
    ch2 = True;      // Next 8 bits  <23:16>
   }else
   if (!ch3)
   {fladr+= (int16)getc()<<8;
    ch3 = True;      // <15:8>
   }else
   if (!ch4)
   {fladr+= getc();
    ch4 = True;      // <7:0>
   }else
   if (!ch5)
   {rx_buffer.DAT_LNG = getc();        // Retrieve Data Length in rx_buffer
   if (rx_buffer.DAT_LNG > 0)
          {ch5 = True;
          bltmp1 = 0;
          bltmp2 = 0;}
          else {rx_stat = 0;goto boot_end;}   // If Data Length is 0 (ERROR) then reset
   }else
   if (!ch6)
       {rx_buffer.buffer[bltmp1] = getc();
      bltmp2 += rx_buffer.buffer[bltmp1];           // Update checksum
      bltmp1 ++;
      if (bltmp1 > rx_buffer.DAT_LNG ) {
         bltmp2 = bltmp2 - rx_buffer.buffer[bltmp1-1];      // Substract last byte from checksum
         ch6 = True;
         bltmp2+= ((int8)(fladr>>24)) + ((int8)(fladr>>16)) + ((int8)(fladr>>8)) + fladr;
         if (bltmp2 == rx_buffer.buffer[bltmp1-1])           // Verify last byte with checksum
         {rx_stat = 0;
          cnt1 = 0;
          cnt2 = 0;
          cnt3 = 0;
BootSector:
         i = 0;
         if (fladr<0xF00000)
         {  rINTCON = 0x00;
            rINTCON2 = 0x80;
            rINTCON3 = 0;
            rPIE_1 = 0;
            rPIE_2 = 0;
            write_program_memory(fladr,rx_buffer.buffer,rx_buffer.DAT_LNG);
//            delay_ms(2);
#ifdef shit
             #ASM
             fwrite_wait:
               btfsc   EECON1,1
             goto   fwrite_wait
             #ENDASM
#endif

         }
          else
           do
            {write_eeprom(fladr,rx_buffer.buffer[i]);
             fladr++;
             i=i+2;
             #ASM
             write_wait:
               btfsc   EECON1,1
             goto   write_wait
             #ENDASM
            }
          while (i<rx_buffer.DAT_LNG);
         printf("OK");
         }else printf("RE");};
      };

poll_boot_end:
#ASM
decfsz cnt1,f
goto   boot_check
decfsz cnt2,f
goto   boot_check
decfsz cnt3,f
goto   boot_check
#ENDASM
boot_end:
reset_cpu();//Restart
}
#org default