/*
* Copyright (c) 2006-2022, RT-Thread Development Team
*
* SPDX-License-Identifier: Apache-2.0
*
* Change Logs:
* Date Author Notes
* 2020-03-19 WangHuachen first version
* 2021-05-11 WangHuachen Added call to Xil_InitializeExistingMPURegConfig to
* initialize the MPU configuration table with the MPU
* configurations already set in Init_Mpu function.
*/
.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
.equ UND_Stack_Size, 0x00000000
.equ SVC_Stack_Size, 0x00000000
.equ ABT_Stack_Size, 0x00000000
.equ FIQ_Stack_Size, 0x00000200
.equ IRQ_Stack_Size, 0x00000200
.equ USR_Stack_Size, 0x00000000
.set RPU_GLBL_CNTL, 0xFF9A0000
.set RPU_ERR_INJ, 0xFF9A0020
.set RPU_0_CFG, 0xFF9A0100
.set RPU_1_CFG, 0xFF9A0200
.set RST_LPD_DBG, 0xFF5E0240
.set BOOT_MODE_USER, 0xFF5E0200
.set fault_log_enable, 0x101
#define ISR_Stack_Size (UND_Stack_Size + SVC_Stack_Size + ABT_Stack_Size + \
FIQ_Stack_Size + IRQ_Stack_Size)
.section .data.share.isr
/* stack */
.globl stack_start
.globl stack_top
.align 3
.bss
stack_start:
.rept ISR_Stack_Size
.long 0
.endr
stack_top:
.section .boot,"axS"
/* reset entry */
.globl _reset
_reset:
/* Initialize processor registers to 0 */
mov r0,#0
mov r1,#0
mov r2,#0
mov r3,#0
mov r4,#0
mov r5,#0
mov r6,#0
mov r7,#0
mov r8,#0
mov r9,#0
mov r10,#0
mov r11,#0
mov r12,#0
/* set the cpu to SVC32 mode and disable interrupt */
cpsid if, #Mode_SVC
/* setup stack */
bl stack_setup
/*
* Enable access to VFP by enabling access to Coprocessors 10 and 11.
* Enables Full Access i.e. in both privileged and non privileged modes
*/
mrc p15, 0, r0, c1, c0, 2 /* Read Coprocessor Access Control Register (CPACR) */
orr r0, r0, #(0xF << 20) /* Enable access to CP 10 & 11 */
mcr p15, 0, r0, c1, c0, 2 /* Write Coprocessor Access Control Register (CPACR) */
isb
/* enable fpu access */
vmrs r3, FPEXC
orr r1, r3, #(1<<30)
vmsr FPEXC, r1
/* clear the floating point register*/
mov r1,#0
vmov d0,r1,r1
vmov d1,r1,r1
vmov d2,r1,r1
vmov d3,r1,r1
vmov d4,r1,r1
vmov d5,r1,r1
vmov d6,r1,r1
vmov d7,r1,r1
vmov d8,r1,r1
vmov d9,r1,r1
vmov d10,r1,r1
vmov d11,r1,r1
vmov d12,r1,r1
vmov d13,r1,r1
vmov d14,r1,r1
vmov d15,r1,r1
#ifdef __SOFTFP__
/* Disable the FPU if SOFTFP is defined*/
vmsr FPEXC,r3
#endif
/* Disable MPU and caches */
mrc p15, 0, r0, c1, c0, 0 /* Read CP15 Control Register*/
bic r0, r0, #0x05 /* Disable MPU (M bit) and data cache (C bit) */
bic r0, r0, #0x1000 /* Disable instruction cache (I bit) */
dsb /* Ensure all previous loads/stores have completed */
mcr p15, 0, r0, c1, c0, 0 /* Write CP15 Control Register */
isb /* Ensure subsequent insts execute wrt new MPU settings */
/* Disable Branch prediction, TCM ECC checks */
mrc p15, 0, r0, c1, c0, 1 /* Read ACTLR */
orr r0, r0, #(0x1 << 17) /* Enable RSDIS bit 17 to disable the return stack */
orr r0, r0, #(0x1 << 16) /* Clear BP bit 15 and set BP bit 16:*/
bic r0, r0, #(0x1 << 15) /* Branch always not taken and history table updates disabled*/
orr r0, r0, #(0x1 << 27) /* Enable B1TCM ECC check */
orr r0, r0, #(0x1 << 26) /* Enable B0TCM ECC check */
orr r0, r0, #(0x1 << 25) /* Enable ATCM ECC check */
bic r0, r0, #(0x1 << 5) /* Generate abort on parity errors, with [5:3]=b 000*/
bic r0, r0, #(0x1 << 4)
bic r0, r0, #(0x1 << 3)
mcr p15, 0, r0, c1, c0, 1 /* Write ACTLR*/
dsb /* Complete all outstanding explicit memory operations*/
/* Invalidate caches */
mov r0,#0 /* r0 = 0 */
dsb
mcr p15, 0, r0, c7, c5, 0 /* invalidate icache */
mcr p15, 0, r0, c15, c5, 0 /* Invalidate entire data cache*/
isb
/* enable fault log for lock step */
ldr r0,=RPU_GLBL_CNTL
ldr r1, [r0]
ands r1, r1, #0x8
/* branch to initialization if split mode*/
bne init
/* check for boot mode if in lock step, branch to init if JTAG boot mode*/
ldr r0,=BOOT_MODE_USER
ldr r1, [r0]
ands r1, r1, #0xF
beq init
/* reset the debug logic */
ldr r0,=RST_LPD_DBG
ldr r1, [r0]
orr r1, r1, #(0x1 << 4)
orr r1, r1, #(0x1 << 5)
str r1, [r0]
/* enable fault log */
ldr r0,=RPU_ERR_INJ
ldr r1,=fault_log_enable
ldr r2, [r0]
orr r2, r2, r1
str r2, [r0]
nop
nop
init:
bl Init_MPU /* Initialize MPU */
/* Enable Branch prediction */
mrc p15, 0, r0, c1, c0, 1 /* Read ACTLR*/
bic r0, r0, #(0x1 << 17) /* Clear RSDIS bit 17 to enable return stack*/
bic r0, r0, #(0x1 << 16) /* Clear BP bit 15 and BP bit 16:*/
bic r0, r0, #(0x1 << 15) /* Normal operation, BP is taken from the global history table.*/
orr r0, r0, #(0x1 << 14) /* Disable DBWR for errata 780125 */
mcr p15, 0, r0, c1, c0, 1 /* Write ACTLR*/
/* Enable icahce and dcache */
mrc p15,0,r1,c1,c0,0
ldr r0, =0x1005
orr r1,r1,r0
dsb
mcr p15,0,r1,c1,c0,0 /* Enable cache */
isb /* isb flush prefetch buffer */
/* Set vector table in TCM/LOVEC */
mrc p15, 0, r0, c1, c0, 0
mvn r1, #0x2000
and r0, r0, r1
mcr p15, 0, r0, c1, c0, 0
/* Clear VINITHI to enable LOVEC on reset */
#if 1
ldr r0, =RPU_0_CFG
#else
ldr r0, =RPU_1_CFG
#endif
ldr r1, [r0]
bic r1, r1, #(0x1 << 2)
str r1, [r0]
/* enable asynchronous abort exception */
mrs r0, cpsr
bic r0, r0, #0x100
msr cpsr_xsf, r0
/* clear .bss */
mov r0,#0 /* get a zero */
ldr r1,=__bss_start /* bss start */
ldr r2,=__bss_end /* bss end */
bss_loop:
cmp r1,r2 /* check if data to clear */
strlo r0,[r1],#4 /* clear 4 bytes */
blo bss_loop /* loop until done */
/* call C++ constructors of global objects */
ldr r0, =__ctors_start__
ldr r1, =__ctors_end__
ctor_loop:
cmp r0, r1
beq ctor_end
ldr r2, [r0], #4
stmfd sp!, {r0-r1}
mov lr, pc
bx r2
ldmfd sp!, {r0-r1}
b ctor_loop
ctor_end:
bl Xil_InitializeExistingMPURegConfig /* Initialize MPU config */
/* start RT-Thread Kernel */
ldr pc, _entry
_entry:
.word entry
stack_setup:
ldr r0, =stack_top
@ Set the startup stack for svc
mov sp, r0
@ 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
@ Switch back to SVC
msr cpsr_c, #Mode_SVC|I_Bit|F_Bit
bx lr
.section .text.isr, "ax"
/* exception handlers: undef, swi, padt, dabt, resv, irq, fiq */
.align 5
.globl vector_fiq
vector_fiq:
stmfd sp!,{r0-r7,lr}
bl rt_hw_trap_fiq
ldmfd sp!,{r0-r7,lr}
subs pc,lr,#4
.globl rt_interrupt_enter
.globl rt_interrupt_leave
.globl rt_thread_switch_interrupt_flag
.globl rt_interrupt_from_thread
.globl rt_interrupt_to_thread
.align 5
.globl vector_irq
vector_irq:
stmfd sp!, {r0-r12,lr}
#if defined (__VFP_FP__) && !defined(__SOFTFP__)
vstmdb sp!, {d0-d15} /* Store floating point registers */
vmrs r1, FPSCR
stmfd sp!,{r1}
vmrs r1, FPEXC
stmfd sp!,{r1}
#endif
bl rt_interrupt_enter
bl rt_hw_trap_irq
bl rt_interrupt_leave
@ if rt_thread_switch_interrupt_flag set, jump to
@ rt_hw_context_switch_interrupt_do and don't return
ldr r0, =rt_thread_switch_interrupt_flag
ldr r1, [r0]
cmp r1, #1
beq rt_hw_context_switch_interrupt_do
#if defined (__VFP_FP__) && !defined(__SOFTFP__)
ldmfd sp!, {r1} /* Restore floating point registers */
vmsr FPEXC, r1
ldmfd sp!, {r1}
vmsr FPSCR, r1
vldmia sp!, {d0-d15}
#endif
ldmfd sp!, {r0-r12,lr}
subs pc, lr, #4
rt_hw_context_switch_interrupt_do:
mov r1, #0 @ clear flag
str r1, [r0]
#if defined (__VFP_FP__) && !defined(__SOFTFP__)
ldmfd sp!, {r1} /* Restore floating point registers */
vmsr FPEXC, r1
ldmfd sp!, {r1}
vmsr FPSCR, r1
vldmia sp!, {d0-d15}
#endif
mov r1, sp @ r1 point to {r0-r3} in stack
add sp, sp, #4*4
ldmfd sp!, {r4-r12,lr}@ reload saved registers
mrs r0, spsr @ get cpsr of interrupt thread
sub r2, lr, #4 @ save old task's pc to r2
@ Switch to SVC mode with no interrupt.
msr cpsr_c, #I_Bit|F_Bit|Mode_SVC
stmfd sp!, {r2} @ push old task's pc
stmfd sp!, {r4-r12,lr}@ push old task's lr,r12-r4
ldmfd r1, {r1-r4} @ restore r0-r3 of the interrupt thread
stmfd sp!, {r1-r4} @ push old task's r0-r3
stmfd sp!, {r0} @ push old task's cpsr
#if defined (__VFP_FP__) && !defined(__SOFTFP__)
vstmdb sp!, {d0-d15} /* Store floating point registers */
vmrs r1, FPSCR
stmfd sp!,{r1}
vmrs r1, FPEXC
stmfd sp!,{r1}
#endif
ldr r4, =rt_interrupt_from_thread
ldr r5, [r4]
str sp, [r5] @ store sp in preempted tasks's TCB
ldr r6, =rt_interrupt_to_thread
ldr r7, [r6]
ldr sp, [r7] @ get new task's stack pointer
#if defined (__VFP_FP__) && !defined(__SOFTFP__)
ldmfd sp!, {r1} /* Restore floating point registers */
vmsr FPEXC, r1
ldmfd sp!, {r1}
vmsr FPSCR, r1
vldmia sp!, {d0-d15}
#endif
ldmfd sp!, {r4} @ pop new task's cpsr to spsr
msr spsr_cxsf, r4
ldmfd sp!, {r0-r12,lr,pc}^ @ pop new task's r0-r12,lr & pc, copy spsr to cpsr
.macro push_svc_reg
sub sp, sp, #17 * 4 @/* Sizeof(struct rt_hw_exp_stack) */
stmia sp, {r0 - r12} @/* Calling r0-r12 */
mov r0, sp
mrs r6, spsr @/* Save CPSR */
str lr, [r0, #15*4] @/* Push PC */
str r6, [r0, #16*4] @/* Push CPSR */
cps #Mode_SVC
str sp, [r0, #13*4] @/* Save calling SP */
str lr, [r0, #14*4] @/* Save calling PC */
.endm
.align 5
.globl vector_swi
vector_swi:
push_svc_reg
bl rt_hw_trap_swi
b .
.align 5
.globl vector_undef
vector_undef:
push_svc_reg
bl rt_hw_trap_undef
b .
.align 5
.globl vector_pabt
vector_pabt:
push_svc_reg
bl rt_hw_trap_pabt
b .
.align 5
.globl vector_dabt
vector_dabt:
push_svc_reg
bl rt_hw_trap_dabt
b .
.align 5
.globl vector_resv
vector_resv:
push_svc_reg
bl rt_hw_trap_resv
b .