# *** Startup Code (executed after Reset) ***


# Standard definitions of Mode bits and Interrupt (I & F) flags in PSRs

        .equ    Mode_USR,       0x10
        .equ    Mode_FIQ,       0x11
        .equ    Mode_IRQ,       0x12
        .equ    Mode_SVC,       0x13
        .equ    Mode_ABT,       0x17
        .equ    Mode_UND,       0x1B
        .equ    Mode_SYS,       0x1F

        .equ    I_Bit,          0x80    /* when I bit is set, IRQ is disabled */
        .equ    F_Bit,          0x40    /* when F bit is set, FIQ is disabled */

/*
// <h> Stack Configuration
//   <o>  Top of Stack Address  <0x0-0xFFFFFFFF>
//   <h>  Stack Sizes (in Bytes)
//     <o1> Undefined Mode      <0x0-0xFFFFFFFF>
//     <o2> Supervisor Mode     <0x0-0xFFFFFFFF>
//     <o3> Abort Mode          <0x0-0xFFFFFFFF>
//     <o4> Fast Interrupt Mode <0x0-0xFFFFFFFF>
//     <o5> Interrupt Mode      <0x0-0xFFFFFFFF>
//     <o6> User/System Mode    <0x0-0xFFFFFFFF>
//   </h>
// </h>
*/
        .equ    Top_Stack,      0x40002000
        .equ    UND_Stack_Size, 0x00000004
        .equ    SVC_Stack_Size, 0x00000004
        .equ    ABT_Stack_Size, 0x00000004
        .equ    FIQ_Stack_Size, 0x00000004
        .equ    IRQ_Stack_Size, 0x00000080
        .equ    USR_Stack_Size, 0x00000400

# Starupt Code must be linked first at Address at which it expects to run.
         
        .extern TargetResetInit
        .text
        .arm
        .global _startup
        .func   _startup
_startup:


# Exception Vectors
#  Mapped to Address 0.
#  Absolute addressing mode must be used.
#  Dummy Handlers are implemented as infinite loops which can be modified.

Vectors: 
         LDR     PC, Reset_Addr         
         LDR     PC, Undef_Addr
         LDR     PC, SWI_Addr
         LDR     PC, PAbt_Addr
         LDR     PC, DAbt_Addr
         NOP                                   /* Reserved Vector   */
	 LDR     PC, [PC, #-0xff0]
         LDR     PC, FIQ_Addr

Reset_Addr:     .word   Reset_Handler
Undef_Addr:     .word   Undef_Handler
SWI_Addr:       .word   SWI_Handler
PAbt_Addr:      .word   PAbt_Handler
DAbt_Addr:      .word   DAbt_Handler
                .word   0                      /* Reserved Address */
IRQ_Addr:       .word   IRQ_Handler
FIQ_Addr:       .word   FIQ_Handler

Undef_Handler:  B       Undef_Handler
SWI_Handler: 
	MOV     R0,LR
        LDR     R0,[R0,#-4]   
        BIC     R0,#0xFF000000		      
        CMP     R0, #4
        LDRLO   PC, [PC, R0, LSL #2]
        MOVS    PC, LR

SwiFunction:
        .word     IRQDisable                  /*;0*/
        .word     IRQEnable                   /*;1*/
        .word     FIQDisable		      /*;2*/
        .word	  FIQEnable		      /*;3*/

IRQDisable:
#关IRQ中断
        MRS     R0, SPSR
        ORR     R0, R0, #I_Bit
        MSR     SPSR_c, R0
        MOVS    PC, LR

IRQEnable:
#开IRQ中断
        MRS   R0, SPSR
        BIC   R0, R0, #I_Bit
        MSR   SPSR_c, R0
        MOVS    PC, LR
        
FIQDisable:
#关FIQ中断
        MRS     R0, SPSR
        ORR     R0, R0, #F_Bit
        MSR     SPSR_c, R0
        MOVS    PC, LR

FIQEnable:
#开FIQ中断
        MRS   R0, SPSR
        BIC   R0, R0, #F_Bit
        MSR   SPSR_c, R0
        MOVS    PC, LR
# changed 2008-5-6

PAbt_Handler:   B       PAbt_Handler
DAbt_Handler:   B       DAbt_Handler
IRQ_Handler:    B       IRQ_Handler
FIQ_Handler:    B       FIQ_Handler


# Reset Handler

Reset_Handler:  
              

# Initialise Interrupt System
#  ...


# Setup Stack for each mode

                LDR     R0, =Top_Stack

#  Enter Undefined Instruction Mode and set its Stack Pointer
                MSR     CPSR_c, #Mode_UND|I_Bit|F_Bit
                MOV     SP, R0
                SUB     R0, R0, #UND_Stack_Size

#  Enter Abort Mode and set its Stack Pointer
                MSR     CPSR_c, #Mode_ABT|I_Bit|F_Bit
                MOV     SP, R0
                SUB     R0, R0, #ABT_Stack_Size

#  Enter FIQ Mode and set its Stack Pointer
                MSR     CPSR_c, #Mode_FIQ|I_Bit|F_Bit
                MOV     SP, R0
                SUB     R0, R0, #FIQ_Stack_Size

#  Enter IRQ Mode and set its Stack Pointer
                MSR     CPSR_c, #Mode_IRQ|I_Bit|F_Bit
                MOV     SP, R0
                SUB     R0, R0, #IRQ_Stack_Size

#  Enter Supervisor Mode and set its Stack Pointer
                MSR     CPSR_c, #Mode_SVC|I_Bit|F_Bit
                MOV     SP, R0
                SUB     R0, R0, #SVC_Stack_Size

#  Enter User Mode and set its Stack Pointer
                MSR     CPSR_c, #Mode_USR
                MOV     SP, R0

#  Setup a default Stack Limit (when compiled with "-mapcs-stack-check")
                SUB     SL, SP, #USR_Stack_Size


# Relocate .data section (Copy from ROM to RAM)
                LDR     R1, =_etext
                LDR     R2, =_data
                LDR     R3, =_edata
LoopRel:        CMP     R2, R3
                LDRLO   R0, [R1], #4
                STRLO   R0, [R2], #4
                BLO     LoopRel


# Clear .bss section (Zero init)
                MOV     R0, #0
                LDR     R1, =__bss_start__
                LDR     R2, =__bss_end__
LoopZI:         CMP     R1, R2
                STRLO   R0, [R1], #4
                BLO     LoopZI


# Enter the C code
				BL TargetResetInit

                ADR     LR, __main_exit
                LDR     R0, =main
                BX      R0

__main_exit:    B       __main_exit


        .size   _startup, . - _startup
        .endfunc


        .end