SECTORS_DONE:
        SPLK    #0,flashAlgoVars.ALGO_STATUS    ;No errors encountered.
        B       STACK_RESTORE

NO_SECTORS:
        SPLK    #11,flashAlgoVars.ALGO_STATUS   ;No sectors specified for erase.
        B       STACK_RESTORE

ERROR1: POPD    flashAlgoVars.PAD               ;Adjust stack depth.
        SPLK    #2,flashAlgoVars.ALGO_STATUS    ;Error encountered in ERASE.
        B       STACK_RESTORE

;--------------------------------------------------------------------------------------------------
; Restore the hardware stack and exit to caller.
;--------------------------------------------------------------------------------------------------
STACK_RESTORE:
        LAR     AR2,#flashAlgoVars.STACK7       ;Point AR2 to the last of the stored stack contents
        MAR     *,AR2                           ;Make AR2 the current AR
        
        RPT     #7                              ;Restore ALL stack locations.
        PSHD    *-
        MAR     *,AR1                           ;Make AR1 the current AR
        RET                                     ;Return to caller


;--------------------------------------------------------------------------------------------------    
; ERASE_SECTOR:
;--------------------------------------------------------------------------------------------------    
; This function is the implementation of the erase algorithm for the pre-program
; conditioning of the flash array. This function is desginated as a LEVEL_1 
; subroutine.
;--------------------------------------------------------------------------------------------------
ERASE_SECTOR:
        SPLK    #MX_ECNT,flashAlgoVars.PLS_CNT              ;Initialize the program pulse count. 
        SPLK    #0ffffh,flashAlgoVars.DATA                  ;Load 0ffffh for compare.
    
        BLDD    #flashAlgoVars.FL_SECST,flashAlgoVars.ADDR  ;Point address to the start of sector.
        SPLK    #0000,flashAlgoVars.LASTVER                 ;Initialize LASTVERIFY Flag 
;--------------------------------------------------------------------------------------------------
;   ENABLE
;--------------------------------------------------------------------------------------------------
; Enable the flash for erase:
; To accept commands the following must happen:
;   1. Place the flash in the register mode.
;   2. Copy the sector key to the SECT register.
;   3. Enable the core by setting Bit 0 of the ENAB Register
;--------------------------------------------------------------------------------------------------
ENABLE: ACCESS_REGS                                         ;Put the flash in register mode.

        LACC    #SECT                                       ;Enable sector.
        BLDD    #flashAlgoVars.SECTOR_KEY,flashAlgoVars.PAD ;Copy the sector key
        TBLW    flashAlgoVars.PAD                           ;
                                                            ;
        LACC    #ENAB                                       ;Enable core.
        SPLK    #0001h,flashAlgoVars.PAD                    ;
        TBLW    flashAlgoVars.PAD                           ;

        B    ERVER                                          ;Enter erase loop from the erase verify.

;--------------------------------------------------------------------------------------------------
;   PERASE:
;--------------------------------------------------------------------------------------------------
;   This loop erases the sector, based on the address passed by the caller in
;   flashAlgoVars.FL_SECST.
;--------------------------------------------------------------------------------------------------
PERASE:
        LACC    flashAlgoVars.PLS_CNT                       ;Get the pulse counter value.
        BCND    ERROR,EQ                                    ;If remaining pulses == 0 then
                                                            ;commence the error handling.
;--------------------------------------------------------------------------------------------------
        LACC    #WADDR                                      ;The sector is selected by writing
                                                            ;WADDR with the first address of 
                                                            ;the sector
        CALL    SETWADDR                                    ;Execute the write to WADDR.

        LACC    #WDATA                                      ;Load the WDATA register with 0xffff.
        TBLW    flashAlgoVars.DATA
;--------------------------------------------------------------------------------------------------
        LACC    #CTRL                                       ;Setup the erase mode.
        SPLK    #0001,flashAlgoVars.PAD
        TBLW    flashAlgoVars.PAD
;--------------------------------------------------------------------------------------------------
        SDELAY    #T_esu_er                                 ;T_esu(ER) Delay.
;--------------------------------------------------------------------------------------------------
        LACC    #PMPC                                       ;Activate the erase mode.
        SPLK    #0005h,flashAlgoVars.PAD    
        TBLW    flashAlgoVars.PAD    
;--------------------------------------------------------------------------------------------------
EPW:    LAR     AR2,#ERASE_PULSE                            ;Call the delay routine to define the
        CALL    DELAY,AR2                                   ;erase pulse width to T_erase(E)
;--------------------------------------------------------------------------------------------------
        SPLK    #0000h,flashAlgoVars.PAD1                   ;De-activate the erase mode.
        CALL    CLRCMD
;--------------------------------------------------------------------------------------------------
        SDELAY  #T_eh_er    ;T_eh(ER)                       ;Algo delay.
;--------------------------------------------------------------------------------------------------
        LACC    #CTRL                                       ;Exit the Erase mode.
        SPLK    #0000,flashAlgoVars.PAD
        TBLW    flashAlgoVars.PAD
;--------------------------------------------------------------------------------------------------
        SDELAY  #Teh_C                                     ;Wait in normal mode.
;--------------------------------------------------------------------------------------------------
        LACC    flashAlgoVars.PLS_CNT                       ;Advance the erase pulse counter.
        SUB     #1h
        SACL    flashAlgoVars.PLS_CNT
;--------------------------------------------------------------------------------------------------
; ERVER: ERASE Verify
;--------------------------------------------------------------------------------------------------
; This subroutine checks the flash to see if the flash is erased.
; To do this it does the followign steps:
;   1.  Reads the flash array in erase verify mode.
;   2.  Branches to the Erase routine if a word is non-FFFFh
;   3.  If all the words in the sector are ffffh then it transfers control
;       to PASSERVER.
;--------------------------------------------------------------------------------------------------
ERVER:  BLDD    #flashAlgoVars.FL_SECST,flashAlgoVars.ADDR  ;Init ADDR to first location in sector.
    
B0      LACC    #CTRL                                       ;Init the Erase verify mode.
        SPLK    #0002,flashAlgoVars.PAD
        TBLW    flashAlgoVars.PAD
        SDELAY  #T_eva_e                                    ;T_eva(E) ; Algo Delay.

B1

        CALL    READWORD                                    ;Read out the word in erase ver mode.

COMPARE:
        LACL    flashAlgoVars.READ                          ;Check to see if the word read out is
        XOR     #0FFFFh                                     ;0ffffh.
        BCND    PROCEED_TO_ERASE,NEQ                                  ;If not, then proceed to apply an erase 
                                                            ;pulse to the sector.
    
        LACC    flashAlgoVars.ADDR                          ;See if the current address is 
        SUB     flashAlgoVars.FL_SECEND                     ;end-of-sector. If so, transfer control
        BCND    PASSERVER,EQ                                ;to PASSERVER.

        ADD     flashAlgoVars.FL_SECEND                     ;Increment the ADDR counter variable.
        ADD     #1                                          ;to next address
        SACL    flashAlgoVars.ADDR                          ;Store back incremented variable.

        B       B1                                          ;Proceed to check the next word.

PROCEED_TO_ERASE:

        LACC    #CTRL                                       ;Exit the Erase verify mode.
        SPLK    #0000,flashAlgoVars.PAD
        TBLW    flashAlgoVars.PAD

        SDELAY  #Tevh_C                                     ;Wait in normal mode.

        B       PERASE

;--------------------------------------------------------------------------------------------------
;PASSERVER
;--------------------------------------------------------------------------------------------------
; This section of code is executed upon successful erase verify.
; This then
;       1. Checks to see if compaction verify has passed.
;       2. If yes, the erase operation is complete for this sector, and the
;          control is transferred to 'DONE' which finishes up.
;       3. If not, control is passed to the compaction verify routine.
;--------------------------------------------------------------------------------------------------
PASSERVER:

        LACC    #CTRL                                       ;Exit the Erase verify mode.
        SPLK    #0000,flashAlgoVars.PAD
        TBLW    flashAlgoVars.PAD

        SDELAY  #Tevh_C                                     ;Wait in normal mode.


        LACC    flashAlgoVars.LASTVER                       ;Check to see if compaction ver passed.
        BCND    DONE,NEQ                                    ;If yes, the erase is complete.
        B       CMPCTVER                                    ;else start compaction verify.

;--------------------------------------------------------------------------------------------------
;ERROR Error handling
;--------------------------------------------------------------------------------------------------
; This section of code is executed upon encountering errors.
;       1. Clears control registers.
;       2. Transfers control to an error exit point.
;--------------------------------------------------------------------------------------------------

ERROR:  SPLK    #6,flashAlgoVars.PAD1                       ; Set up for clearing 6 control regs
        CALL    CLRCMD                                      ; Call routine to do so.
        B       ERROR1

;--------------------------------------------------------------------------------------------------
; READWORD
; This routine performs the following operations
;    1.    Reads the word at the location pointed to by ADDR.
;    2.  Stores the word in the variable READ.
;    3.  Returns to the caller in Register Mode.
;--------------------------------------------------------------------------------------------------
READWORD
        ACCESS_ARRAY
        LACC    flashAlgoVars.ADDR                          ;Read word flash 
        TBLR    flashAlgoVars.READ                          ;store word in READ (data space)
        ACCESS_REGS

        RET

;--------------------------------------------------------------------------------------------------
; CMPCTVER: 
;--------------------------------------------------------------------------------------------------
; This section performs the following functions:
;   1.  Checks to see if compaction operation is needed.
;   2.  Calls the compaction routine if needed.