/* .include "vector_defns.h" */



.section .privileged_code, "ax"
.arm


.weak __mbed_fiq
.weak __mbed_undef
.weak __mbed_prefetch_abort
.weak __mbed_data_abort
.weak __mbed_irq
.weak __mbed_swi
.weak __mbed_dcc_irq
.weak __mbed_reset
.global __mbed_init_realmonitor
.weak SVC_Handler
.weak IRQ_Handler
/*  .global __mbed_init */




__mbed_fiq:
        B __mbed_fiq
__mbed_undef:
        LDR PC, =0x7fffffa0
__mbed_prefetch_abort:
        LDR PC, =0x7fffffb0
__mbed_data_abort:
        LDR PC, =0x7fffffc0
__mbed_irq:
/*
    If RTOS is enabled then goto RTOS IRQ handler
*/
        PUSH   {R0}
        LDR    R0, =IRQ_Handler
        CMP    R0, #0
        POP   {R0}
        BNE   IRQ_Handler
/*
    else use CMSIS IRQ handler
*/
        MSR CPSR_c, #0x1F|0x80|0x40
        
        STMDB sp!, {r0-r3,r12,lr}
        
        MOV r0, #0xFFFFFF00
        LDR r0, [r0]
        
        MOV lr, pc
        BX r0
        
        MOV r0, #0xFFFFFF00
        STR r0, [r0] 
        
        LDMFD sp!,{r0-r3,r12,lr}
        
        MSR CPSR_c, #0x12|0x80|0x40
        
        SUBS pc, lr, #4

__mbed_swi:
/*
    If RTOS is enabled then goto RTOS SVC handler
*/
        PUSH   {R0}
        LDR    R0, =SVC_Handler
        CMP    R0, #0
        POP   {R0}
        BNE   SVC_Handler
/*
    else use CMSIS SVC handler
*/
        STMFD sp!, {a4, r4, ip, lr}
        
        LDR r4, =0x40000040
        
        LDR a4, =0x00940000
        LDR PC, =0x7ffff820

__mbed_dcc_irq:
        LDMFD sp!,{r0-r3,r12,lr}
        
        MSR CPSR_c, #0x12|0x80|0x40
        
        SUB lr, lr, #4 
        STMFD sp!, {ip,lr} 
        
        LDR LR, =0xfffff000
        STR LR, [LR, #0xf00]
        
        LDR PC, =0x7fffffe0
/*
 __mbed_reset is called after reset
 we setup the stacks and realmonitor, then call Reset_Handler like on M3
*/

.section .text, "ax"
.arm
.global Reset_handler
Reset_Handler:   
        .extern __libc_init_array
        .extern SystemInit
        .weak   software_init_hook
        LDR     R0, =SystemInit
        MOV     LR, PC
        BX      R0

/* if (software_init_hook) // give control to the RTOS
        software_init_hook(); // this will also call __libc_init_array
*/
        LDR    R0, =software_init_hook
        CMP    R0, #0
        BEQ    nortos
        ORR    R0,R0,#1      /* set thumb address */
        BX     R0
/* else */
nortos:
        LDR     R0, =__libc_init_array
        MOV     LR, PC
        BX      R0

        MSR CPSR_c, #0x1F /* enable irq */

        LDR     R0, =main
        BX      R0

__mbed_reset:
        LDR R0, =( __SRAM_segment_end__ )
        
        MSR CPSR_c, #0x1B|0x80|0x40
        MOV SP, R0
        SUB R0, R0, #0x00000040
        
        MSR CPSR_c, #0x17|0x80|0x40
        MOV SP, R0
        SUB R0, R0, #0x00000040
        
        MSR CPSR_c, #0x11|0x80|0x40
        MOV SP, R0
        SUB R0, R0, #0x00000040
        
        MSR CPSR_c, #0x12|0x80|0x40
        MOV SP, R0
        SUB R0, R0, #0x00000040
        
        MSR CPSR_c, #0x13|0x80|0x40
        MOV SP, R0
        SUB R0, R0, #0x00000040
        
/*
        MSR CPSR_c, #0x10|0x80|0x40
        MOV SP, R0
*/
        MSR CPSR_c, #0x1F|0x80|0x40
        MOV SP, R0

        MSR CPSR_c, #0x13|0x80|0x40 /* execute in Supervisor mode */
        
/*  Relocate .data section (Copy from ROM to RAM) */
        LDR     R1, =__text_end__        /* _etext */ 
        LDR     R2, =__data_start__      /* _data  */
        LDR     R3, =__data_end__        /* _edata */ 
        CMP     R2, R3
        BEQ     DataIsEmpty
LoopRel:        CMP     R2, R3 
        LDRLO   R0, [R1], #4 
        STRLO   R0, [R2], #4 
        BLO     LoopRel 
DataIsEmpty:

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


/* Init realmonitor */
/*
        LDR R0, =__mbed_init_realmonitor
        MOV LR, PC
        BX R0
*/
        
/* Go to Reset_Handler */ 
        LDR     R0, =Reset_Handler
        BX R0