org 0x1f80
; Re-flashing code, Modification of MicroChip sample
;----------------------------------------------
FlashEEPROM
  call DCStart
  clrf STATUS
  call FlashEEPROM_putchar
  goto FlashEEPROM_disconnect_stop
FlashEEPROM_putchar
  bsf STATUS, RP0;  ; Bank 1
  btfss TXSTA, TRMT ; Loop until ready to send
  goto $ - 1
  bcf STATUS, RP0   ; Bank 0
  movwf TXREG		    ; Push charactor over USART from w
 return
DCStart
  movlw CMDI0_ACK
  call FlashEEPROM_putchar
DCColonWait
  call GetByte
  movlw ':'               ;Wait for colon
  subwf RCREG,W
  btfss STATUS,Z
  goto DCColonWait
  call GetByte            ;Read byte count
  movwf ByteCount         ;Store in register
  movwf LineChecksum      ;Store in line checksum
  bcf STATUS,C
  rrf ByteCount,F         ;Divide byte counter by 2 to get words
  call GetByte            ;Read high byte of 16-bit address
  movwf AddrH
  addwf LineChecksum,F    ;Add high byte to line checksum
  call GetByte            ;Read low byte of 16-bit address
  movwf AddrL
  addwf LineChecksum,F    ;Add low byte to line checksum
 ; Adjust Addresses to match PIC ( divide by 2 )
  bcf STATUS, C
  rrf AddrH, f
  rrf AddrL, f
 ; Read Record Type
  call GetByte            ;Read record type
  movwf RecType
  addwf LineChecksum,F    ;Add to line checksum
DataRec                   ;Data reception
  movf RecType,F          ;Check for data record (0h)
  btfss STATUS,Z
  goto EndOfFileRec       ;Otherwise check for EOF
DRLoop
  bcf STATUS, RP0
  bcf STATUS, RP1
  movf ByteCount,F        ;Check for bytecount = 0
  btfsc STATUS,Z
  goto DRCkChecksum       ;If zero, goto checksum validation
  call GetByte            ;Read lower byte of data (2 characters)
  movwf HexDataL          ;Add received data to checksum
  addwf LineChecksum,F
  call GetByte            ;Read upper byte of data (2 characters)
  movwf HexDataH          ;Add received data to checksum
  addwf LineChecksum,F
WriteDataSequence         ;Write sequence to internal prog. mem FLASH
  banksel EEADRH
  movf AddrH,W            ;Write address to EEADRH:EEADR registers
  movwf EEADRH
  movf AddrL,W
  movwf EEADR
  movf HexDataH,W         ;Write data to EEDATH:EEDATA registers
  movwf EEDATH
  movf HexDataL,W
  movwf EEDATA
  banksel EECON1          ;Write sequence
  bsf EECON1,EEPGD        ;Set EEPGD to indicate program memory
  bsf EECON1,WREN         ;Enable writes to memory
  movlw 0x55              ;Required write sequence
  movwf EECON2
  movlw 0xaa
  movwf EECON2
  bsf EECON1,WR           ;Start internal write cycle
  nop
  nop
;  bcf EECON1,WREN                ;Disable writes
;  bsf EECON1,EEPGD               ;Set EEPGD to indicate program memory
;  bsf EECON1,RD                  ;EEPROM Read
;  nop
;  nop
;  banksel EEDATH
;  movf EEDATH,W                  ;Compare memory value to HexDataH:HexDataL
;  subwf HexDataH,W
;  btfss STATUS,Z
;  retlw CMDI0_FLASH_WRITE_ERR    ;If upper byte not equal, return FFh
;  movf EEDATA,W                  ; to indicate programming failure
;  subwf HexDataL,W
;  btfss STATUS,Z
;  retlw CMDI0_FLASH_WRITE_ERR    ;If lower byte not equal, return FFh to indicate programming failure
  incf AddrL,F                    ;Increment address for next iteration
  btfsc STATUS,Z
  incf AddrH,F
  decf ByteCount,F                ;Decrement byte count
  goto DRLoop                     ;Go back to check for ByteCount = 0
DRCkChecksum                      ;Checksum verification
  call GetByte                    ;Read in checksum
  addwf LineChecksum,W            ;Add to calculated checksum
  btfss STATUS,Z                  ;Result should be 0
  retlw CMDI0_FLASH_CHECKSUM_ERR  ;If not return 1 to indicate checksum fail
  goto DCStart                    ;Do it again
EndOfFileRec                      ;End of File record (01h)
  decf RecType,W                  ;If EOF record, decrement should = 0
  btfss STATUS,Z
  goto DCStart                    ;Not valid record type, wait for next :
  call GetByte                    ;Read in checksum
  addwf LineChecksum,W            ;Add to calculated checksum
  btfss STATUS,Z                  ;Result should be 0
  retlw CMDI0_FLASH_CHECKSUM_ERR  ; If not return 1 to indicate checksum fail
 retlw CMDI0_FLASH_SUCCESS        ;Otherwise return 0 to indicate success
GetByte
  banksel PIR1
  btfss PIR1,RCIF
  goto $ - 1
  banksel RCREG
  movf RCREG,W
 return
FlashEEPROM_disconnect_stop
  movlw FlashEEPROM_disconnect_str & 0xff
  movwf Temp
FlashEEPROM_putStringLoop
  call FlashEEPROM_fetchChar
  andlw 0xff        ; test Z
  btfsc STATUS, Z
 goto $
  call FlashEEPROM_putchar
  incf temp, f
  movf temp, w
  goto FlashEEPROM_putStringLoop
FlashEEPROM_fetchChar
 movwf PCL          ; jump to w
FlashEEPROM_disconnect_str
  dt "+++", 0x0d, 0x0d, 0x0d, "ATDH", 0x0d, ,"ATDH", 0x0d, 0x00