b           InitReset               ; 0 - 0x00 SoftReset
                ldr         pc, [pc, #0x18]         ; 1 - 0x04 UndefHandler
                ldr         pc, [pc, #0x18]         ; 2 - 0x08 SWIHandler
                ldr         pc, [pc, #0x18]         ; 3 - 0x0C PrefetchAbortHandler
                ldr         pc, [pc, #0x18]         ; 4 - 0x10 DataAbortHandler
                nop                                 ; 5 - 0x14 Reserved
                ldr         pc, [pc, #0x18]         ; 6 - 0x18 IRQ_Handler_Entry
                ldr         pc, [pc, #0x18]         ; 7 - 0x1C FIQ_Handler_Entry
;- There are only 5 offsets as the vectoring is used.
                DCD         InitReset               ; 0x20
                DCD         AT91F_Undef             ; 0x24
                DCD         SWIHandler              ; 0x28
                DCD         AT91F_FetchAbort        ; 0x2c
                DCD         AT91F_DataAbort         ; 0x30
                DCD         ReservedHandler         ; 0x34
                DCD         IRQHandler              ; 0x38
                DCD         FIQHandler              ; 0x3c

        MODULE  ?cstartup

       ;; Forward declaration of sections.
        SECTION PROGRAM_DATA:CODE
        SECTION IRQ_STACK:DATA:NOROOT(3)
        SECTION FIQ_STACK:DATA:NOROOT(3)
        SECTION CSTACK:DATA:NOROOT(3)

        SECTION .intvec:CODE:NOROOT(2)

        PUBLIC  __vector
        PUBLIC  __vector_0x14
        PUBLIC  __iar_program_start
        EXTERN  Undefined_Handler
        EXTERN  SWI_Handler
        EXTERN  Prefetch_Handler
        EXTERN  Abort_Handler
        EXTERN  IRQ_Handler
        EXTERN  FIQ_Handler

        ARM

__vector:
       ; All default exception handlers (except reset) are
       ; defined as weak symbol definitions.
       ; If a handler is defined by the application it will take precedence.

;      ldr     PC,Reset_Addr          ; To allow remap, a relative branch is required !
    B       __iar_program_start
        ldr     PC,Undefined_Addr      ; Undefined instructions
        ldr     PC,SWI_Addr            ; Software interrupt (SWI/SVC)
        ldr     PC,Prefetch_Addr       ; Prefetch abort
        ldr     PC,Abort_Addr          ; Data abort
__vector_0x14:
        DCD     SFE(PROGRAM_DATA)      ; End address of all const and text sections.

;       ldr     PC,IRQ_Addr            ; IRQ
        ldr     PC, [PC,# -0xF20]       ;AIC direct
        ldr     PC,FIQ_Addr            ; FIQ

        DATA

Reset_Addr:     DCD   __iar_program_start
Undefined_Addr: DCD   Undefined_Handler
SWI_Addr:       DCD   SWI_Handler
Prefetch_Addr:  DCD   Prefetch_Handler
Abort_Addr:     DCD   Abort_Handler
IRQ_Addr:       DCD   IRQ_Handler
FIQ_Addr:       DCD   FIQ_Handler

__vector_end:

; --------------------------------------------------
; ?cstartup -- low-level system initialization code.
;
; After a reser execution starts here, the mode is ARM, supervisor
; with interrupts disabled.
;


        SECTION .text:CODE:NOROOT(2)

        EXTERN  ?main
        REQUIRE __vector

        ARM

__iar_program_start:

/**********************************************************************
* ?CSTARTUP
*
* Execution starts here.
* After a reset, the mode is ARM, Supervisor, interrupts disabled.
*/
?cstartup:

        mvn     r0, #0xFF        ;remap here
        bic     r0,r0, #0x1100
        mov     r1, #0x03
            str     r1, [r0, #+0]
        
        mrc     p15, 0, r0, c1, c0,0               ; read cp15 control register (cp15 r1) r0
        ldr     r3, =0xC0001085                    ; Disable Cache, Disable MMU
        ldr     r4, =0x40005004                    ; Round Robin replacement 0x40004000
        bic     r0, r0, r3
        orr     r0, r0, r4
        mcr     p15, 0, r0, c1, c0,0               ; write cp15 control register (cp15 r1)
;--------------------------------------------------------------------------------
    

;--------------------------------------------------------------------------------
; Initialize the stack pointers.
       ;; Set up the interrupt stack pointer.

        msr     cpsr_c,#0xd2           ; Change mode to IRQ mode, FIQ and IRQ disabled
        ldr     sp, =SFE(IRQ_STACK)    ; End of IRQ_STACK
        bic     sp,sp,#0x7             ; Make sure SP is aligned on 8 byte boundary
    
       ;; Set up the fast interrupt stack pointer.

        msr     cpsr_c,#0xd1           ; Change mode to FIQ mode, FIQ and IRQ disabled
        ldr     sp, =SFE(FIQ_STACK)    ; End of FIQ_STACK
        bic     sp,sp,#0x7             ; Make sure SP is aligned on 8 byte boundary

       ;; Set up the normal stack pointer.

        msr     cpsr_c,#0xdf           ; Change mode to SYS mode, FIQ and IRQ disabled
   ;     msr     cpsr_c, #0xd3
         ldr     sp, =SFE(CSTACK)       ; End of CSTACK
         bic     sp,sp,#0x7             ; Make sure SP is aligned on 8 byte boundary

        ;msr     cpsr_c,#0xd2

; Jump to startup code in library
        ldr     r0,=?main
        bx      r0

        LTORG
        END

; ***** EOF **********************************************************