#/***********************************************************************/
#/*  Startup file for LPC21xx MCU applications                          */
#/*  Partially inspired by KEIL ELEKTRONIK startup code                 */
#/***********************************************************************/


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

# Standard definitions of Mode bits and Interrupt (I & F) flags in PSRs
        .set  MODE_USR, 0x10            // User Mode
        .set  MODE_FIQ, 0x11            // FIQ Mode
        .set  MODE_IRQ, 0x12            // IRQ Mode
        .set  MODE_SVC, 0x13            // Supervisor Mode
        .set  MODE_ABT, 0x17            // Abort Mode
        .set  MODE_UND, 0x1B            // Undefined Mode
        .set  MODE_SYS, 0x1F            // System Mode

        .equ  I_BIT, 0x80               // when I bit is set, IRQ is disabled
        .equ  F_BIT, 0x40               // when F bit is set, FIQ is disabled

        .set  UND_STACK_SIZE, 0x00000004
        .set  ABT_STACK_SIZE, 0x00000004
        .set  FIQ_STACK_SIZE, 0x00000004
        .set  IRQ_STACK_SIZE, 0X00000400
        .set  SVC_STACK_SIZE, 0x00000004

# Starupt Code must be linked first at Address at which it expects to run.

        .text
#       .arm

        .global _stack                  // top of stack
        .global _startup
	.global  reset_handler
        .func   _startup
_startup:

reset_handler:  

# Memory Mapping (when Interrupt Vectors are in RAM)
        .equ    MEMMAP, 0xE01FC040  /* Memory Mapping Control */

        MOV     R1, #1
        LDR     R0, =hal_vectors
	CMP     R0, #0
	BEQ     mam_sram
        MOV     R1, #2
mam_sram:
        LDR     R0, =MEMMAP
        STR     R1, [R0]

# Initialize Interrupt System
# - Set stack location for each mode
# - Leave in System Mode with Interrupts Disabled
# -----------------------------------------------
        ldr   r0,=_stack
        msr   CPSR_c,#MODE_UND|I_BIT|F_BIT // Undefined Instruction Mode
        mov   sp,r0
        sub   r0,r0,#UND_STACK_SIZE
        msr   CPSR_c,#MODE_ABT|I_BIT|F_BIT // Abort Mode
        mov   sp,r0
        sub   r0,r0,#ABT_STACK_SIZE
        msr   CPSR_c,#MODE_FIQ|I_BIT|F_BIT // FIQ Mode
        mov   sp,r0
        sub   r0,r0,#FIQ_STACK_SIZE
        msr   CPSR_c,#MODE_IRQ|I_BIT|F_BIT // IRQ Mode
        mov   sp,r0
        sub   r0,r0,#IRQ_STACK_SIZE
        msr   CPSR_c,#MODE_SVC|I_BIT|F_BIT // Supervisor Mode
        mov   sp,r0
        sub   r0,r0,#SVC_STACK_SIZE
        msr   CPSR_c,#MODE_SYS|I_BIT|F_BIT // System Mode
        mov   sp,r0

# Disable interrupt from VIC
        .equ    VICINTENABLE,    0xFFFFF010
        .equ    VICINTENCLR,     0xFFFFF014
        .equ    VICSOFTINT,      0xFFFFF018
        .equ    VICSOFTINTCLEAR, 0xFFFFF01C
        LDR     R0, =VICINTENABLE
        MOV     R1, #0
        STR     R1, [R0]
        MOV     R1, #0xFFFFFFFF
        STR     R1, [R0,#VICINTENCLR-VICINTENABLE]
        STR     R1, [R0,#VICSOFTINTCLEAR-VICINTENABLE]

# Enable interrupts and return back into supervisor mode
        msr   CPSR_c,#MODE_SVC  // Supervisor Mode

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

ZI:
# 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 _setup_board code
	ADR     LR, __main_start
        LDR     R0, =_setup_board
	CMP     R0, #0
	BEQ     __main_start
        BX      R0

__main_start:   
	ADR     LR, __main_exit
        LDR     R0, =main
        BX      R0

__main_exit:    B       __main_exit

        .size   _start, . - _start
        .endfunc

        .end