diff --git a/arch/Kconfig b/arch/Kconfig index 91f4a4ee3e5..1d087f9c584 100644 --- a/arch/Kconfig +++ b/arch/Kconfig @@ -414,6 +414,12 @@ config CPU_HAS_TEE Execution Environment (e.g. when it has a security attribution unit). +config CPU_HAS_DCLS + bool + help + This option is enabled when the processor hardware is configured in + Dual-redundant Core Lock-step (DCLS) topology. + config CPU_HAS_FPU bool help diff --git a/arch/arm/core/aarch32/cortex_r/reset.S b/arch/arm/core/aarch32/cortex_r/reset.S index d88a9e2ecad..9c1650be0de 100644 --- a/arch/arm/core/aarch32/cortex_r/reset.S +++ b/arch/arm/core/aarch32/cortex_r/reset.S @@ -1,5 +1,6 @@ /* * Copyright (c) 2013-2014 Wind River Systems, Inc. + * Copyright (c) 2019 Stephanos Ioannidis * * SPDX-License-Identifier: Apache-2.0 */ @@ -26,17 +27,17 @@ GDATA(z_arm_sys_stack) GDATA(z_arm_fiq_stack) GDATA(z_arm_abort_stack) GDATA(z_arm_undef_stack) - -#define STACK_MARGIN 4 - +#if defined(CONFIG_PLATFORM_SPECIFIC_INIT) +GTEXT(z_platform_init) +#endif /** * * @brief Reset vector * - * Ran when the system comes out of reset. The processor is in thread mode with - * privileged level. At this point, the main stack pointer (MSP) is already - * pointing to a valid area in SRAM. + * Ran when the system comes out of reset. The processor is in Supervisor mode + * and interrupts are disabled. The processor architectural registers are in + * an indeterminate state. * * When these steps are completed, jump to z_arm_prep_c(), which will finish * setting up the system for running C code. @@ -44,59 +45,143 @@ GDATA(z_arm_undef_stack) * @return N/A */ SECTION_SUBSEC_FUNC(TEXT, _reset_section, z_arm_reset) -SECTION_SUBSEC_FUNC(TEXT,_reset_section,__start) - 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 - mov r14, #0 +SECTION_SUBSEC_FUNC(TEXT, _reset_section, __start) - /* lock interrupts: will get unlocked when switch to main task */ - cpsid if +#if defined(CONFIG_CPU_HAS_DCLS) + /* + * Initialise CPU registers to a defined state if the processor is + * configured as Dual-redundant Core Lock-step (DCLS). This is required + * for state convergence of the two parallel executing cores. + */ - /* Setup FIQ stack */ - msr CPSR_c, #(MODE_FIQ | I_BIT | F_BIT) - ldr sp, =(z_arm_fiq_stack + CONFIG_ARMV7_FIQ_STACK_SIZE - STACK_MARGIN) + /* Common and SVC mode registers */ + 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 + mov r13, #0 /* r13_svc */ + mov r14, #0 /* r14_svc */ + mrs r0, cpsr + msr spsr_cxsf, r0 /* spsr_svc */ - /* Setup IRQ stack */ - msr CPSR_c, #(MODE_IRQ | I_BIT | F_BIT) - ldr sp, =(_interrupt_stack + CONFIG_ISR_STACK_SIZE - STACK_MARGIN) + /* FIQ mode registers */ + cps #MODE_FIQ + mov r8, #0 /* r8_fiq */ + mov r9, #0 /* r9_fiq */ + mov r10, #0 /* r10_fiq */ + mov r11, #0 /* r11_fiq */ + mov r12, #0 /* r12_fiq */ + mov r13, #0 /* r13_fiq */ + mov r14, #0 /* r14_fiq */ + mrs r0, cpsr + msr spsr_cxsf, r0 /* spsr_fiq */ - /* Setup data abort stack */ - msr CPSR_c, #(MODE_ABT | I_BIT | F_BIT) - ldr sp, =(z_arm_abort_stack + CONFIG_ARMV7_EXCEPTION_STACK_SIZE - \ - STACK_MARGIN) + /* IRQ mode registers */ + cps #MODE_IRQ + mov r13, #0 /* r13_irq */ + mov r14, #0 /* r14_irq */ + mrs r0, cpsr + msr spsr_cxsf, r0 /* spsr_irq */ - /* Setup undefined mode stack */ - msr CPSR_c, #(MODE_UDF | I_BIT | F_BIT) - ldr sp, =(z_arm_undef_stack + CONFIG_ARMV7_EXCEPTION_STACK_SIZE - \ - STACK_MARGIN) + /* ABT mode registers */ + cps #MODE_ABT + mov r13, #0 /* r13_abt */ + mov r14, #0 /* r14_abt */ + mrs r0, cpsr + msr spsr_cxsf, r0 /* spsr_abt */ - /* Setup SVC mode stack */ - msr CPSR_c, #(MODE_SVC | I_BIT | F_BIT) - ldr sp, =(z_arm_svc_stack + CONFIG_ARMV7_SVC_STACK_SIZE - STACK_MARGIN) + /* UND mode registers */ + cps #MODE_UND + mov r13, #0 /* r13_und */ + mov r14, #0 /* r14_und */ + mrs r0, cpsr + msr spsr_cxsf, r0 /* spsr_und */ - /* Setup System mode stack */ - msr CPSR_c, #(MODE_SYS | I_BIT | F_BIT) - ldr sp, =(z_arm_sys_stack + CONFIG_ARMV7_SYS_STACK_SIZE - STACK_MARGIN) + /* SYS mode registers */ + cps #MODE_SYS + mov r13, #0 /* r13_sys */ + mov r14, #0 /* r14_sys */ - /* Setup system control register */ - mrc p15, 0, r0, c1, c0, 0 /* SCTLR */ - bic r0, r0, #HIVECS /* Exception vectors from 0-0x1c */ - mcr p15, 0, r0, c1, c0, 0 +#if defined(CONFIG_FLOAT) + /* + * Initialise FPU registers to a defined state. + */ -#if defined(CONFIG_WDOG_INIT) - /* board-specific watchdog initialization is necessary */ - bl z_arm_watchdog_init + /* Allow VFP coprocessor access */ + mrc p15, 0, r0, c1, c0, 2 + orr r0, r0, #(CPACR_CP10(CPACR_FA) | CPACR_CP11(CPACR_FA)) + mcr p15, 0, r0, c1, c0, 2 + + /* Enable VFP */ + mov r0, #FPEXC_EN + fmxr fpexc, r0 + + /* Initialise VFP registers */ + fmdrr d0, r1, r1 + fmdrr d1, r1, r1 + fmdrr d2, r1, r1 + fmdrr d3, r1, r1 + fmdrr d4, r1, r1 + fmdrr d5, r1, r1 + fmdrr d6, r1, r1 + fmdrr d7, r1, r1 + fmdrr d8, r1, r1 + fmdrr d9, r1, r1 + fmdrr d10, r1, r1 + fmdrr d11, r1, r1 + fmdrr d12, r1, r1 + fmdrr d13, r1, r1 + fmdrr d14, r1, r1 + fmdrr d15, r1, r1 +#endif /* CONFIG_FLOAT */ + +#endif /* CONFIG_CPU_HAS_DCLS */ + + /* + * Configure stack. + */ + + /* FIQ mode stack */ + msr CPSR_c, #(MODE_FIQ | I_BIT | F_BIT) + ldr sp, =(z_arm_fiq_stack + CONFIG_ARMV7_FIQ_STACK_SIZE) + + /* IRQ mode stack */ + msr CPSR_c, #(MODE_IRQ | I_BIT | F_BIT) + ldr sp, =(_interrupt_stack + CONFIG_ISR_STACK_SIZE) + + /* ABT mode stack */ + msr CPSR_c, #(MODE_ABT | I_BIT | F_BIT) + ldr sp, =(z_arm_abort_stack + CONFIG_ARMV7_EXCEPTION_STACK_SIZE) + + /* UND mode stack */ + msr CPSR_c, #(MODE_UND | I_BIT | F_BIT) + ldr sp, =(z_arm_undef_stack + CONFIG_ARMV7_EXCEPTION_STACK_SIZE) + + /* SVC mode stack */ + msr CPSR_c, #(MODE_SVC | I_BIT | F_BIT) + ldr sp, =(z_arm_svc_stack + CONFIG_ARMV7_SVC_STACK_SIZE) + + /* SYS mode stack */ + msr CPSR_c, #(MODE_SYS | I_BIT | F_BIT) + ldr sp, =(z_arm_sys_stack + CONFIG_ARMV7_SYS_STACK_SIZE) + +#if defined(CONFIG_PLATFORM_SPECIFIC_INIT) + /* Execute platform-specific initialisation if applicable */ + bl z_platform_init #endif - b z_arm_prep_c +#if defined(CONFIG_WDOG_INIT) + /* board-specific watchdog initialization is necessary */ + bl z_arm_watchdog_init +#endif + + b z_arm_prep_c diff --git a/include/arch/arm/aarch32/cortex_r/cpu.h b/include/arch/arm/aarch32/cortex_r/cpu.h index 9ce788a511b..28a072b908f 100644 --- a/include/arch/arm/aarch32/cortex_r/cpu.h +++ b/include/arch/arm/aarch32/cortex_r/cpu.h @@ -12,7 +12,7 @@ #define MODE_IRQ 0x12 #define MODE_SVC 0x13 #define MODE_ABT 0x17 -#define MODE_UDF 0x1b +#define MODE_UND 0x1b #define MODE_SYS 0x1f #define MODE_MASK 0x1f @@ -23,6 +23,14 @@ #define HIVECS (1 << 13) +#define CPACR_NA (0U) +#define CPACR_FA (3U) + +#define CPACR_CP10(r) (r << 20) +#define CPACR_CP11(r) (r << 22) + +#define FPEXC_EN (1 << 30) + #define RET_FROM_SVC 0 #define RET_FROM_IRQ 1