;*******************************************************************************
;*                                                                             *
;*    Copyright (C) 2006 Oki Electric Industry Co., LTD.                       *
;*                                                                             *
;*    System Name    :  ML675050 CPU BOARD                                     *
;*    Module Name    :  Startup routine                                        *
;*    File   Name    :  init.s                                                 *
;*    Revision       :  01.00                                                  *
;*    Date           :  2006.02.08  initial version                            *
;*                                                                             *
;*******************************************************************************

        PRESERVE8

        INCLUDE define.s                ; common definitions
;;        INCLUDE macro_regioninit.s

;;;     IMPORT  fiq_handler				; FIQ handler	;;;(ML675050USB)

        IMPORT  IRQ_Stack_Top
        IMPORT  FIQ_Stack_Top
        IMPORT  SVC_Stack_Top
        IMPORT  USR_Stack_Top

;;        IMPORT  int_IRQ                 ; IRQ handler
        IMPORT  irq_Handler                 ; IRQ handler
        IMPORT  int_SWI                 ; SWI handler

;*******************************************************************************
;*      Exception vector                                                       *
;*              Address : 0x00000000                                           *
;*******************************************************************************
        AREA    Vector, CODE, READONLY

; --- Define entry point
        EXPORT  __main                 ; defined to ensure that C runtime system
__main                                 ; is not linked in
        EXPORT  _main                  ; defined to ensure that C runtime system
_main                                  ; is not linked in
        ENTRY

; --- setup interrupt / exception vectors
        EXPORT  ENTRY_POINT
ENTRY_POINT
                B       int_RESET       ; reset
int_UND         B       int_UND         ; undefined instruction
                B       int_SWI         ; swi
int_PABT        B       int_PABT        ; prefetch abort
int_DABT        B       int_DABT        ; data abort
                NOP                     ; reserved vector
;;                B       int_IRQ         ; irq
                B       irq_Handler         ; irq

;;;------------------------------------------------------------------------------
;; --- The FIQ handler starts here
;
;       SUB     lr_FIQ, lr_FIQ, #4      ; construct the return address
;        STMFD   sp_FIQ!, {r0-r3, lr_FIQ}; store low registers to FIQ stack
;
;        BL      fiq_handler             ; branch to FIQ handler
;
;        LDR     r0, =FIQ                ; load FIQ register address
;Debounce_FIQ                            ; do not enable FIQ till FIQ source is de-asserted
;        MOV     r3, #0
;Debounce_FIQ100
;        LDR     r1, [r0]                ; load contents of FIQ
;        MOV     r2, #0x1
;        AND     r1, r1, r2              ; zero out the high bits
;        CMP     r1, #0x0                ; is FIQ still pending?
;        BNE     Debounce_FIQ
;        ADD     r3, r3,#1
;        CMP     R3, #0x8000
;        BNE     Debounce_FIQ100
;
;        LDMFD   sp_FIQ!, {r0-r3, pc}^   ; restore registers & return from FIQ
;;;------------------------------------------------------------------------------


;*******************************************************************************
;*      Start-up routine                                                       *
;*               Load Address      : 0x00XXXXXX(Bank0)                         *
;*               Execution Address : Load Address + 0xC8000000(Bank25(ext.ROM))*
;*******************************************************************************
        AREA    Startup, CODE, READONLY

;:******************************************************************************
;; Reset Handler                                                               *
;;******************************************************************************
int_RESET

; --- initialize stack pointer registers
; enter IRQ mode and set up the IRQ stack pointer
        MOV     R0, #Mode_IRQ:OR:I_Bit:OR:F_Bit ; no interrupts
        MSR     CPSR_c, R0
        LDR     R13, =IRQ_Stack_Top     ; set IRQ mode stack.

; --- initialize stack pointer registers
; enter FIQ mode and set up the FIQ stack pointer
        MOV     R0, #Mode_FIQ:OR:I_Bit:OR:F_Bit ; no interrupts
        MSR     CPSR_c, R0
        LDR     R13, =FIQ_Stack_Top     ; set FIQ mode stack.

; set up the SVC stack pointer last and return to SVC mode
        MOV     R0, #Mode_SVC:OR:I_Bit:OR:F_Bit ; no interrupts
        MSR     CPSR_c, R0
        LDR     R13, =SVC_Stack_Top ; set SVC mode STACK.

; --- initialize memory required by C code
    IF :DEF:SCATTER
		IMPORT InitRegions
		BL InitRegions			;;;ML675050USBSample20060310
    ELSE
		BL init_regions			;;;ML675050USBSample20060310
    ENDIF

; --- now change to user mode and set up user mode stack.
;        MOV     R0, #Mode_USR:OR:F_Bit:OR:I_Bit
        MOV     R0, #Mode_USR:OR:F_Bit
        MSR     CPSR_c, R0
        LDR     sp, =USR_Stack_Top      ; set USR mode stack.

; --- now enter the C code
        IMPORT  main

        LDR     r0, =main
        MOV     lr, pc
        BX      r0                      ; branch to main function.

END_LOOP
        B       END_LOOP


;:******************************************************************************
;; Initialize regions                                                          *
;;******************************************************************************
init_regions
        ;STMFD sp!,{lr}
        ;macro_RegionInit DATA1
        ;macro_RegionInit DATA2
        ;macro_RegionInit DATA3
        ;LDMFD sp!,{pc}

        IMPORT  |Image$$RO$$Limit|
        IMPORT  |Image$$RW$$Base|
        IMPORT  |Image$$ZI$$Base|
        IMPORT  |Image$$ZI$$Limit|

        ; --- Initialise memory required by C code
        ldr     r0, =|Image$$RO$$Limit| ; Get pointer to ROM data
        ldr     r1, =|Image$$RW$$Base|  ; and RAM copy
        ldr     r3, =|Image$$ZI$$Base|  ; Zero init base => top of initialised data
        cmp     r0, r1                  ; Check that they are different
        beq     %1
0       cmp     r1, r3                  ; Copy init data
        ldrcc   r2, [r0], #4
        strcc   r2, [r1], #4
        bcc     %0
1       ldr     r1, =|Image$$ZI$$Limit| ; Top of zero init segment
        mov     r2, #0
2       cmp     r3, r1                  ; Zero init
        strcc   r2, [r3], #4
        bcc     %2

; --- copy and zi_init subroutines
; these subroutines are only used by sample programs which use scatter loading

; copy is a subroutine which copies a region, from an address given by
; r0 to an address given by r1. The address of the word beyond the end
; of this region is held in r2. r3 is used to hold the word being copied.
        EXPORT  copy
copy
        CMP     r1, r2                  ; loop whilst r1 < r2
        LDRLO   r3, [r0], #4
        STRLO   r3, [r1], #4
        BLO     copy
        MOV     pc, lr                  ; return from subroutine copy

; zi_init is a subroutine which zero-initialises a region,
; starting at the address in r0. The address of the word
; beyond the end of this region is held in r1.
        EXPORT  zi_init
zi_init
        MOV     r2, #0
        CMP     r0, r1                  ; loop whilst r0 < r1
        STRLO   r2, [r0], #4
        BLO     zi_init
        MOV     pc, lr                  ; return from subroutine zi_init

        END