/* ***************** reset.s ****************************************/
/*          SBC-DBG crt0 for ARMCortex-A9.
/*
/*
/* (c) OJSC ELVEES, multicore.ru
/*
/* *******************************************************************/

/*==================================================================
/* BootROM Program (c) Elvees 2013
/*================================================================== */
    .text
    .code 32
    .global _start
    .global _end

/************Defines***************************************************/
    .equ PMCTR_BASE,        0x38095000
    .equ WARM_RST_EN,       0x02c
    .equ WARM_RST_STATUS,   0x044
    .equ PDM_RST_STATUS,    0x048
    .equ ALWAYS_MISC0,      0x070
    .equ DDR_PIN_RET,       0x024
    .equ DDR_INIT_END,      0x028
    .equ CORE_PWR_UP,       0x080
    .equ GATE_CORE_CTR,     0x38094048
    .equ GATE_SYS_CTR,      0x3809404c
    .equ SEL_APLL,          0x38094100
    .equ POINTER_DDR0,      0x20000020
    .equ DDR_REMAP,         0x3809600c
/**********************************************************************/

.func _start
/*===================================================================
/* Entry point
/*=================================================================== */
_start:
    B       _reset                  /* Reset_Handler                   */
    LDR     pc, =0x20000004         /* Undefined_Handler               */
    LDR     pc, =0x20000008         /* SWI_Handler                     */
    LDR     pc, =0x2000000c         /* Prefetch_Handler                */
    LDR     pc, =0x20000010         /* Data_Handler                    */
    LDR     pc, =0x20000014         /* Reserved vector                 */
    LDR     pc, =0x20000018         /* IRQ_Handler                     */
    LDR     pc, =0x2000001c         /* FIQ Handler is after this table */
.align 4 /* re-align to the word boundary */


_reset:
    @LDR     r3, =PMCTR_BASE
    @LDR     r0, =0x1
    @STR     r0, [r3, #CORE_PWR_UP]
    @LDR     r4, =GATE_CORE_CTR
    @LDR     r0, [r4]
    @ORR     r0, r0, #0x1 //Enable L0_CLK
    @STR     r0, [r4]
    @LDR     r4, =GATE_SYS_CTR
    @LDR     r0, [r4]
    @ORR     r0, r0, #0x1 //Enable SYS_CLK
    @STR     r0, [r4]

/*==================================================================
/* Disable caches, MMU and branch prediction
/*==================================================================*/

    /* Read CP15 System Control register */
    @MRC     p15, 0, r0, c1, c0, 0
    /* Clear I bit 12 to disable I Cache */
    /* Clear C bit  2 to disable D Cache */
    /* Clear M bit  0 to disable MMU     */
    /* Clear Z bit 11 to disable branch prediction */
    @LDR     r1, =0x1805
    @BIC     r0, r1
    @MCR     p15, 0, r0, c1, c0, 0       /* Write value back to CP15 System Control register*/

/* Init stacks */
    BL      set_cpu_stack

/* Enable NEON */
        MRC p15,0,r0,c1,c0,2    // Read CP Access register
        ORR r0,r0,#0x00f00000   // Enable full access to NEON/VFP (Coprocessors 10 and 11)
        MCR p15,0,r0,c1,c0,2    // Write CP Access register
        ISB
        MOV r0,#0x40000000      // Switch on the VFP and NEON hardware
        VMSR FPEXC,r0           // Set EN bit in FPEXC
/* Clear .bss */
    .extern _fbss
    .extern __end__

    LDR     r0, =0x0
    LDR     r1, =_fbss
    LDR     r2, =__bss_end__
    B       clean_bss_1
clean_bss:
    STR     r0, [r1]
    ADD     r1, r1, #0x4
clean_bss_1:
    CMP     r1, r2
    BNE     clean_bss
go_to_main:
    LDR     r12, =main
    BLX                r12
.endfunc

.align 4
_irq_undefined:                                /* Undefined handler */
        dsb
        LDR     sp, =__irq_stack0_top__   /* IRQ Stack*/
        stmdb        sp!,{r0-r3,r12,lr}        /* state save from compiled code */
        mov     r0, r14                 /*Instruction abort address */
#        blx        undefinedInstructionInterrupt        /* undefinedInstructionInterrupt: call C function here */
        ldmia        sp!,{r0-r3,r12,lr}        /* state restore from compiled code */
        subs        pc, lr, #4

.align 4
_prefetchAbortHandler:        /* Prefetch Abort handler */
        dsb
        LDR     sp, =__irq_stack0_top__   /* IRQ Stack*/
        stmdb        sp!,{r0-r3,r12,lr}        /* state save from compiled code */
        mov     r0, r14                 /*Instruction abort address */
#        blx        prefetchAbortInterrupt        /* PrefetchAbortInterrupt: call C function here */
        ldmia        sp!,{r0-r3,r12,lr}        /* state restore from compiled code */
        subs        pc, lr, #4                        /* adjust return */

.align 4
_dataAbortHandler:                /* Data Abort handler */
        dsb
        LDR     sp, =__irq_stack0_top__   /* IRQ Stack*/
        stmdb        sp!,{r0-r3,r12,lr}        /* state save from compiled code */
        mov     r0, r14                 /*Instruction abort address */
#        blx        dataAbortInterrupt                /*DataAbortInterrupt :call C function here */
        ldmia        sp!,{r0-r3,r12,lr}        /* state restore from compiled code */
        subs        pc, lr, #4                        /* adjust return */

.align 4
_FIQHandler:                        /* FIQ vector handler */
        stmdb        sp!,{r0-r3,r12,lr}        /* state save from compiled code */
FIQLoop:
#        blx        FIQInterrupt                        /* FIQ vector */
        ldmia        sp!,{r0-r3,r12,lr}        /* state restore from compiled code */
        subs        pc, lr, #4                        /* adjust return */

.func set_cpu_stack
/*===================================================================
/* Init cpu0, cpu1 stacks
/*=================================================================== */
set_cpu_stack:
    MOV     r12, r14
    MRC     p15, 0, r0, c0, c0, 5      /* Read CPU ID register*/
    ANDS    r0, r0, #0x03              /* Mask off, leaving the CPU ID field*/
    BEQ     set_cpu0_stack
set_cpu1_stack:
    MSR     CPSR_c, 0x12   /* IRQ Mode */
    LDR     sp, =__irq_stack1_top__   /* IRQ Stack*/

    MSR     CPSR_c, 0xD3 /* Supervisor Mode*/
    LDR     sp, =__svc_stack1_top__  /* SVC Stack*/
    MOV     pc, r12
set_cpu0_stack:
    MSR     CPSR_c, 0x12   /* IRQ Mode */
    LDR     sp, =__irq_stack0_top__   /* IRQ Stack*/

    MSR     CPSR_c, 0xD3 /* Supervisor Mode*/
    LDR     sp, =__svc_stack0_top__  /* SVC Stack*/
    MOV     pc, r12
.endfunc

.end