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

    .text
    .code 32
    .global _start
    .global _end
    .global _get_cpu_id
    .global _invalidate_l1_it_cache
    .global _invalidate_l1_data_cache
    .global _enable_I_cache
    .global _enable_cache
    .global _disable_cache

    .extern vApplicationIRQHandler
    .extern vSWIHandler
    .extern prefetchAbortInterrupt
    .extern dataAbortInterrupt
    .extern FIQInterrupt

    .set SYS_MODE,	0x1f
    .set SVC_MODE,	0x13
    .set IRQ_MODE,	0x12

    /* Hardware registers. */
    .extern ulICCIAR
    .extern ulICCEOIR
    .extern ulICCPMR

/************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
/**********************************************************************/

    .macro portSAVE_CONTEXT

	/* Save the LR and SPSR onto the system mode stack before switching to
	system mode to save the remaining system mode registers. */
	SRSDB	sp!, #SYS_MODE
	CPS		#SYS_MODE
	PUSH	{R0-R12, R14}
    .endm

    .macro portRESTORE_CONTEXT

        /* Restore all system mode registers other than the SP (which is already
	being used). */
	POP		{R0-R12, R14}

	/* Return to the task code, loading CPSR on the way. */
	RFEIA	sp!

    .endm

.func _start
/*===================================================================
/* Entry point
/*=================================================================== */
_start:
    B       _reset                  /* Reset_Handler                   */
    B       _irq_undefined          /* Undefined_Handler               */
    B       _swiHandler             /* SWI_Handler                     */
    B       _prefetchAbortHandler   /* Prefetch_Handler                */
    B       _dataAbortHandler       /* Data_Handler                    */
    NOP                             /* Reserved vector                 */
    B       _irqHandler             /* IRQ_Handler                     */
    B       _FIQHandler             /* 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

/* Clear .bss */
    .extern _fbss
    .extern __end__

    LDR     r0, =0x0
    LDR     r1, =_fbss
    LDR     r2, =__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
    BX		r12
.endfunc

.align 4
_irq_undefined:				/* Undefined handler */
    B		.

.align 4
_swiHandler:
    nop
    nop

.align 4
_irqHandler:
    /* Return to the interrupted instruction. */
	SUB		lr, lr, #4

	/* Push the return address and SPSR. */
	PUSH	{lr}
	MRS		lr, SPSR
	PUSH	{lr}

	/* Change to supervisor mode to allow reentry. */
	CPS		#SVC_MODE

	/* Push used registers. */
	PUSH	{r0-r4, r12}


	/* Read value from the interrupt acknowledge register, which is stored in r0
	for future parameter and interrupt clearing use. */
	LDR 	r2, ulICCIARConst
	LDR		r2, [r2]
	LDR		r0, [r2]

	/* Ensure bit 2 of the stack pointer is clear.  r2 holds the bit 2 value for
	future use. */
	MOV		r2, sp
	AND		r2, r2, #4
	SUB		sp, sp, r2

	/* Call the interrupt handler. */
	PUSH	{r0-r3, lr}
	LDR		r1, vApplicationIRQHandlerConst
	BLX		r1
	POP		{r0-r3, lr}
	ADD		sp, sp, r2

	CPSID	i
	DSB
	ISB

	/* Write the value read from ICCIAR to ICCEOIR. */
	LDR 	r4, ulICCEOIRConst
	LDR		r4, [r4]
	STR		r0, [r4]

exit_without_switch:
	/* No context switch.  Restore used registers, LR_irq and SPSR before
	returning. */
	POP		{r0-r4, r12}
	CPS		#IRQ_MODE
	POP		{LR}
	MSR		SPSR_cxsf, LR
	POP		{LR}
	MOVS            PC, LR
        nop
        nop

.align 4
_prefetchAbortHandler:	/* Prefetch Abort handler */
	dsb
	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
	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 _get_cpu_id
/*===================================================================
/* Get number of core
/*=================================================================== */
_get_cpu_id:
    MRC     p15, 0, r0, c0, c0, 5      /* Read CPU ID register*/
    ANDS    r0, r0, #0x03              /* Mask off, leaving the CPU ID field*/
    MOV     pc, lr
.endfunc

.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

ulICCIARConst:	.word ulICCIAR
ulICCEOIRConst:	.word ulICCEOIR
ulICCPMRConst: .word ulICCPMR
vApplicationIRQHandlerConst: .word vApplicationIRQHandler
.end