diff --git a/arch/arm/core/cortex_m/prep_c.c b/arch/arm/core/cortex_m/prep_c.c index 41b651659f4..3b05338304f 100644 --- a/arch/arm/core/cortex_m/prep_c.c +++ b/arch/arm/core/cortex_m/prep_c.c @@ -84,6 +84,44 @@ static inline void relocate_vector_table(void) } #endif +#ifdef CONFIG_FLOAT +static inline void enable_floating_point(void) +{ + /* + * Upon reset, the Co-Processor Access Control Register is 0x00000000. + * Enable CP10 and CP11 coprocessors to enable floating point. + */ + __scs.cpacr.val = (_SCS_CPACR_CP10_FULL_ACCESS | + _SCS_CPACR_CP11_FULL_ACCESS); + + /* + * Upon reset, the FPU Context Control Register is 0xC0000000 + * (both Automatic and Lazy state preservation is enabled). + * Disable lazy state preservation so the volatile FP registers are + * always saved on exception. + */ + __scs.fpu.ccr.val = (_SCS_FPU_CCR_ASPEN_ENABLE | + _SCS_FPU_CCR_LSPEN_DISABLE); + + /* + * Although automatic state preservation is enabled, the processor + * does not automatically save the volatile FP registers until they + * have first been touched. Perform a dummy move operation so that + * the stack frames are created as expected before any task or fiber + * context switching can occur. + */ + __asm__ volatile( + "vmov s0, s0;\n\t" + "dsb;\n\t" + "isb;\n\t" + ); +} +#else +static inline void enable_floating_point(void) +{ +} +#endif + extern FUNC_NORETURN void _Cstart(void); /** * @@ -97,6 +135,7 @@ extern FUNC_NORETURN void _Cstart(void); void _PrepC(void) { relocate_vector_table(); + enable_floating_point(); bssZero(); dataCopy(); _Cstart();