bgt     write_loop                      ; yes, continue

        mov     r6,#0x90                                ; Reset Unlock Block
        EXPAND  r6
        str     r6,[r1]
        mov     r6,#0x0
        str     r6,[r1]

        cmp     R9,#0                           ; want verify?
        beq     done_write                      ; no, just quit

        mov     R2,R9                           ; reset count
        sub     R4,R4,R2                        ; back up to start of block to verify
        sub     R5,R5,R2                        ; back up to start of buffer to verify
        b       FLASH_validate                  ; go on to validate code

done_write
        b       flash_exit                      ; Stop routine on breakpoint

;* -----------------------------------------------------------------------
;  FLASH_validate - validate write to a Flash block
;
;  Notes:
;     - this compares a buffer and a flash block. The caller uses this
;       after a write to verify the write worked.
;     - the host knows about block sizes and all, and passes in the needed
;       info.
;
;  Input/Output:
;     - In: R1=Base of Flash block to verify
;           R0=Page of Flash (no meaning for ARM)
;           R2=Count of bytes to verify
;           R4=Offset in block to verify from
;           R5=Address of buffer to verify against
;           R8=Page of buffer (no meaning for ARM)
;     - Out:R0=status code (0=OK, else error)
;     - Scratch: R6,R7
;  ----------------------------------------------------------------------- */

FLASH_validate
        mov     R6,#0
        mov     R7,#0
validate_loop
        LOAD    R7,[R5],#WIDTH  ; load value from buffer
        LOAD    R6,[R4],#WIDTH  ; load value from Flash
        cmp     R6,R7           ; same?
        bne     bad_validate    ; no, failed

        subs    R2,R2,#WIDTH    ; decrement count, more?
        bgt     validate_loop   ; yes, continue

        b       flash_exit      ; Stop routine on breakpoint
        
bad_validate
        mov     R0,#1           ; compare
        b       flash_exit      ; Stop routine on breakpoint

;* -----------------------------------------------------------------------
;  FLASH_break - end of function entry for all Flash routines
;
;  Notes:
;     - this routine is the Stop routine for the flash code. It runs over
;       what should be a break (set by host). If the host messes up, it
;       will loop forever allowing the host to halt it.
;
;  Input/Output:
;     - In: R0=status code to return (0=OK, else error)
;           R1=base of Flash block operated on
;   ----------------------------------------------------------------------- */

Local_init                      ; we have no local init
flash_exit_reset
                bl              reset_flash
flash_exit

FLASH_break
        nop                     ; place to put breakpoint from host
forever
        b       forever         ; in case host screws up

;************************************************************************/
;* STATIC FUNCTIONS                                                     */
;************************************************************************/

;* -----------------------------------------------------------------------
;  erase_block - erase a sector of the flash and check if OK
;
;  Notes:
;     - this handles erase of a Flash erase and returns status in R0
;       as well as the Z flag
;
;  Input/Output:
;     - In: R1=Base of Flash
;     - Out:R0=Status (0=OK, else failed)
;     - Scratch: R0,R6
;  ----------------------------------------------------------------------- */

erase_block
       
        mov     r0,r1,LSR#13
        mov     r0,r0,LSL#13                    ; R0 now has sector base with
                                                                        ; bottom 13 bits sets to 0
        ldr     R7,=0x555                       ; address of erase msg 1
        add     R7,R0,R7,LSL#2                          
        mov     R6,#0xAA                        ; data of erase setup msg 1
        EXPAND  R6
        STORE   R6,[R7]                         ; send Flash erase
        
        ldr     R7,=0x2AA                       ; address of erase msg 2
        add     R7,R0,R7,LSL#2                          
        mov     R6,#0x55                        ; data of erase setup msg 2
        EXPAND  R6
        STORE   R6,[R7]                         ; send Flash erase

        ldr     R7,=0x555                       ; address of erase msg 3
        add     R7,R0,R7,LSL#2                          
        mov     R6,#0x80                        ; data of erase setup msg 3
        EXPAND  R6
        STORE   R6,[R7]                         ; send Flash erase

        ldr     R7,=0x555                       ; address of erase msg 4
        add     R7,R0,R7,LSL#2                          
        mov     R6,#0xAA                        ; data of erase setup msg 4
        EXPAND  R6
        STORE   R6,[R7]                         ; send Flash erase
        
        ldr     R7,=0x2AA                       ; address of erase msg 5
        add     R7,R0,R7,LSL#2                          
        mov     R6,#0x55                        ; data of erase setup msg 5
        EXPAND  R6
        STORE   R6,[R7]                         ; send Flash erase
        
        mov     R6,#0x30                        ; data of erase setup msg 6
        EXPAND  R6
        STORE   R6,[R1]                         ; Finally erase sector

        mov     R6,R14                          ; save link for calls below
                
        bl      wait_for_ready_erase    ; Check flash has erased
        
        mov     R15,R6                          ; Return with code in R0 and Z bit
                
;* -----------------------------------------------------------------------
;  wait_for_ready_erase - wait for Flash to complete an erase operation 
;                                         (DQ7= 1) and check the erase was successful (DQ5=0).
;
;  Notes:
;     - this reads the error status of the Flash and returns in R0.
;     - DQ7 = Ready (when high in erase or nDQ7 in program mode)
;     - DQ6 = Toggles during embedded erase or program
;     - DQ5 = Erase/Program Successful (low when successful)
;     - DQ3 = Sector Erase Timer (Timeout when high)
;     - DQ2 = Toggles when addresses are made in sector being erased
;     - DQ1 = Write to Buffer abort (low when no abort)
;
;  Input/Output:
;     - In: R1=Base of Flash
;     - Scratch: R0,R7
;  ----------------------------------------------------------------------- */


wait_for_ready_erase
        
        LOAD    R0,[R1]                         ; read status of flash

        mov     R7,#0xA0                                ; Check DQ5 has not gone high while DQ7 low
        EXPAND  R7
        ands    R0,R0,R7
        mov     r7,#0x20
        cmp     r0,r7
        beq     flash_exit_reset                ; Need to reset flash if DQ5 high
        
        LOAD    R0,[R1]                         ; read status of flash

        mov     R7,#0x80                                ; Check DQ7 has gone high
        EXPAND  R7
        ands    R0,R0,R7
        beq     wait_for_ready_erase
        
check_erase_data
        LOAD    R0,[R1]                                 ; Read Flash
        ldr     R7,=0xffffffff                  ; Check data has returned to 0xffffffff
        cmp     R0,R7
        bne     check_erase_data
        
        mov     R0,#0x0                 ; No Error to report
        mov     R15,R14                         ; Return

;* -----------------------------------------------------------------------
;  wait_for_ready_program - wait for Flash to complete an program operation.
;
;  Notes:
;     - this reads the error status of the Flash and returns in R0.
;     - DQ7 = Ready (when high in erase or nDQ7 in program mode)
;     - DQ6 = Toggles during embedded erase or program
;     - DQ5 = Erase/Program Successful (low when successful)
;     - DQ3 = Sector Erase Timer (Timeout when high)
;     - DQ2 = Toggles when addresses are made in sector being erased
;     - DQ1 = Write to Buffer abort (low when no abort)
;
;  Input/Output:
;     - In: R4=Address in flash writing to
;     - In: R7= Value being written
;     - Scratch: R0,R6,R7
;  ----------------------------------------------------------------------- */

wait_for_ready_program

        mov     R6,#0x80
        EXPAND  R6
        and     R7,R7,R6                        ; find out whether DQ7 and DQ23 should be
                                                                        ; be high or low while programming
                ; DEBUG CODE
                ; Copy register values into SRAM
                ;ldr     r0,=0x38100000
                ;str     r1,[r0]
                ;str     r2,[r0,#0x4]
                ;str     r3,[r0,#0x8]
                ;str     r4,[r0,#0xC]
                ;str     r5,[r0,#0x10]
                ;str     r6,[r0,#0x14]
                ;str     r7,[r0,#0x18]
                ;str     r8,[r0,#0x1C]
                ;str     r9,[r0,#0x20]
                ;END DEBUG CODE



wait_for_write          
        LOAD    R0,[R4,#-0x4]       ; read status of flash at program address
        and     R0,R0,R6
        cmp     R0,R7                           ; Wait until DQ7 and DQ23 are not inverted      
        
        bne             wait_for_write
        
        mov     R0,#0x0             ; No Error to report
        mov     R15,R14                 ; Return

;* -----------------------------------------------------------------------
;  reset flash - Reset Flash
;
;  Input/Output:
;     - In: R1=Base of Flash
;     - Scratch: R7
;  ----------------------------------------------------------------------- */

reset_flash
        
        mov     r7,#0xf0
        EXPAND  r7
        str     r7,[r1]
        mov     R15,R14                 ; Return

        END