From 26dc203d6525851d03c3d373f49895b984ff588f Mon Sep 17 00:00:00 2001 From: gdisirio Date: Sun, 11 Dec 2011 12:58:51 +0000 Subject: [PATCH] FPU support for CM4 (not working yet). git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@3591 35acf78f-673a-0410-8e92-d51de3d6d3f4 --- demos/ARMCM4-STM32F407-DISCOVERY/Makefile | 4 +- os/ports/GCC/ARMCMx/chcore_v7m.c | 103 ++++++++++++++++++++-- os/ports/GCC/ARMCMx/chcore_v7m.h | 55 +++++++++--- os/ports/GCC/ARMCMx/nvic.h | 69 ++++++++++++--- 4 files changed, 200 insertions(+), 31 deletions(-) diff --git a/demos/ARMCM4-STM32F407-DISCOVERY/Makefile b/demos/ARMCM4-STM32F407-DISCOVERY/Makefile index d6c23ea1f..0c614db80 100644 --- a/demos/ARMCM4-STM32F407-DISCOVERY/Makefile +++ b/demos/ARMCM4-STM32F407-DISCOVERY/Makefile @@ -5,7 +5,7 @@ # Compiler options here. ifeq ($(USE_OPT),) - USE_OPT = -O2 -ggdb -fomit-frame-pointer + USE_OPT = -O2 -ggdb -fomit-frame-pointer -mhard-float endif # C specific options here (added to USE_OPT). @@ -120,7 +120,7 @@ INCDIR = $(PORTINC) $(KERNINC) $(TESTINC) \ # Compiler settings # -MCU = cortex-m3 +MCU = cortex-m4 #TRGT = arm-elf- TRGT = arm-none-eabi- diff --git a/os/ports/GCC/ARMCMx/chcore_v7m.c b/os/ports/GCC/ARMCMx/chcore_v7m.c index e8a1ff16e..aedaeea10 100644 --- a/os/ports/GCC/ARMCMx/chcore_v7m.c +++ b/os/ports/GCC/ARMCMx/chcore_v7m.c @@ -31,18 +31,36 @@ /** * @brief Internal context stacking. */ +#if CORTEX_USE_FPU || defined(__DOXYGEN__) +#define PUSH_CONTEXT() { \ + asm volatile ("vpush {s16-s31}" \ + : : : "memory"); \ + asm volatile ("push {r4, r5, r6, r7, r8, r9, r10, r11, lr}" \ + : : : "memory"); \ +} +#else /* !CORTEX_USE_FPU */ #define PUSH_CONTEXT() { \ asm volatile ("push {r4, r5, r6, r7, r8, r9, r10, r11, lr}" \ : : : "memory"); \ } +#endif /* !CORTEX_USE_FPU */ /** * @brief Internal context unstacking. */ +#if CORTEX_USE_FPU || defined(__DOXYGEN__) +#define POP_CONTEXT() { \ + asm volatile ("pop {r4, r5, r6, r7, r8, r9, r10, r11, pc}" \ + : : : "memory"); \ + asm volatile ("vpop {s16-s31}" \ + : : : "memory"); \ +} +#else /* !CORTEX_USE_FPU */ #define POP_CONTEXT() { \ asm volatile ("pop {r4, r5, r6, r7, r8, r9, r10, r11, pc}" \ : : : "memory"); \ } +#endif /* !CORTEX_USE_FPU */ #if !CH_OPTIMIZE_SPEED void _port_lock(void) { @@ -80,12 +98,21 @@ CH_IRQ_HANDLER(SysTickVector) { * @note The PendSV vector is only used in advanced kernel mode. */ void SVCallVector(void) { + uint32_t *psp; register struct extctx *ctxp; + /* Current PSP value.*/ + asm volatile ("mrs %0, PSP" : "=r" (psp) : : "memory"); + +#if CORTEX_USE_FPU + /* Restoring the special registers SCB_FPCCR and FPCAR.*/ + SCB_FPCAR = *psp++; + SCB_FPCCR = *psp++; +#endif + /* Discarding the current exception context and positioning the stack to point to the real one.*/ - asm volatile ("mrs %0, PSP" : "=r" (ctxp) : : "memory"); - ctxp++; + ctxp = (struct extctx *)psp + 1; asm volatile ("msr PSP, %0" : : "r" (ctxp) : "memory"); port_unlock_from_isr(); } @@ -99,16 +126,61 @@ void SVCallVector(void) { * @note The PendSV vector is only used in compact kernel mode. */ void PendSVVector(void) { + uint32_t *psp; register struct extctx *ctxp; + /* Current PSP value.*/ + asm volatile ("mrs %0, PSP" : "=r" (psp) : : "memory"); + +#if CORTEX_USE_FPU + /* Restoring the special registers SCB_FPCCR and FPCAR.*/ + SCB_FPCAR = *psp++; + SCB_FPCCR = *psp++; +#endif + /* Discarding the current exception context and positioning the stack to point to the real one.*/ - asm volatile ("mrs %0, PSP" : "=r" (ctxp) : : "memory"); - ctxp++; + ctxp = (struct extctx *)psp + 1; asm volatile ("msr PSP, %0" : : "r" (ctxp) : "memory"); } #endif /* CORTEX_SIMPLIFIED_PRIORITY */ +/** + * @brief Port-related initialization code. + */ +void _port_init(void) { + uint32_t reg; + + /* Initialization of the vector table and priority related settings.*/ + SCB_VTOR = CORTEX_VTOR_INIT; + SCB_AIRCR = AIRCR_VECTKEY | AIRCR_PRIGROUP(0); + +#if CORTEX_USE_FPU + /* CP10 and CP11 set to full access.*/ + SCB_CPACR |= 0x00F00000; + + /* Enables FPU context save/restore on exception entry/exit (FPCA bit).*/ + asm volatile ("mrs %0, CONTROL" : "=r" (reg) : : "memory"); + reg |= 4; + asm volatile ("msr CONTROL, %0" : : "r" (reg) : "memory"); + + /* FPSCR and FPDSCR initially zero.*/ + reg = 0; + asm volatile ("vmsr FPSCR, %0" : : "r" (reg) : "memory"); + SCB_FPDSCR = reg; + + /* Initializing the FPU context save in lazy mode.*/ + SCB_FPCCR = FPCCR_LSPEN; +#endif + + /* Initialization of the system vectors used by the port.*/ + NVICSetSystemHandlerPriority(HANDLER_SVCALL, + CORTEX_PRIORITY_MASK(CORTEX_PRIORITY_SVCALL)); + NVICSetSystemHandlerPriority(HANDLER_PENDSV, + CORTEX_PRIORITY_MASK(CORTEX_PRIORITY_PENDSV)); + NVICSetSystemHandlerPriority(HANDLER_SYSTICK, + CORTEX_PRIORITY_MASK(CORTEX_PRIORITY_SYSTICK)); +} /** * @brief Exception exit redirection to _port_switch_from_isr(). */ @@ -116,12 +188,29 @@ void _port_irq_epilogue(void) { port_lock_from_isr(); if ((SCB_ICSR & ICSR_RETTOBASE)) { - register struct extctx *ctxp; + uint32_t *psp; + struct extctx *ctxp; + + /* Current PSP value.*/ + asm volatile ("mrs %0, PSP" : "=r" (psp) : : "memory"); + +#if CORTEX_USE_FPU + { + uint32_t fpccr; + + /* Saving the special registers SCB_FPCCR and FPCAR as extra context.*/ + *--psp = fpccr = SCB_FPCCR; + *--psp = SCB_FPCAR; + + /* Now the FPCCR is modified in order to not restore the FPU status + from the artificial return context.*/ + SCB_FPCCR = fpccr | FPCCR_LSPACT; + } +#endif /* Adding an artificial exception return context, there is no need to populate it fully.*/ - asm volatile ("mrs %0, PSP" : "=r" (ctxp) : : "memory"); - ctxp--; + ctxp = (struct extctx *)psp - 1; asm volatile ("msr PSP, %0" : : "r" (ctxp) : "memory"); ctxp->pc = _port_switch_from_isr; ctxp->xpsr = (regarm_t)0x01000000; diff --git a/os/ports/GCC/ARMCMx/chcore_v7m.h b/os/ports/GCC/ARMCMx/chcore_v7m.h index 14fca144c..3cd2b9e18 100644 --- a/os/ports/GCC/ARMCMx/chcore_v7m.h +++ b/os/ports/GCC/ARMCMx/chcore_v7m.h @@ -129,8 +129,12 @@ #elif (CORTEX_MODEL == CORTEX_M4) #define CH_ARCHITECTURE_ARM_v7ME #define CH_ARCHITECTURE_NAME "ARMv7-ME" +#if CORTEX_USE_FPU +#define CH_CORE_VARIANT_NAME "Cortex-M4F" +#else #define CH_CORE_VARIANT_NAME "Cortex-M4" #endif +#endif /** * @brief Port-specific information string. @@ -162,6 +166,27 @@ struct extctx { regarm_t lr_thd; regarm_t pc; regarm_t xpsr; +#if CORTEX_USE_FPU || defined(__DOXYGEN__) + regarm_t s0; + regarm_t s1; + regarm_t s2; + regarm_t s3; + regarm_t s4; + regarm_t s5; + regarm_t s6; + regarm_t s7; + regarm_t s8; + regarm_t s9; + regarm_t s10; + regarm_t s11; + regarm_t s12; + regarm_t s13; + regarm_t s14; + regarm_t s15; + regarm_t s16; + regarm_t fpscr; + regarm_t reserved; +#endif /* CORTEX_USE_FPU */ }; struct intctx { @@ -174,6 +199,24 @@ struct intctx { regarm_t r10; regarm_t r11; regarm_t lr; +#if CORTEX_USE_FPU || defined(__DOXYGEN__) + regarm_t s16; + regarm_t s17; + regarm_t s18; + regarm_t s19; + regarm_t s20; + regarm_t s21; + regarm_t s22; + regarm_t s23; + regarm_t s24; + regarm_t s25; + regarm_t s26; + regarm_t s27; + regarm_t s28; + regarm_t s29; + regarm_t s30; + regarm_t s31; +#endif /* CORTEX_USE_FPU */ }; #endif @@ -208,16 +251,7 @@ struct intctx { /** * @brief Port-related initialization code. */ -#define port_init() { \ - SCB_VTOR = CORTEX_VTOR_INIT; \ - SCB_AIRCR = AIRCR_VECTKEY | AIRCR_PRIGROUP(0); \ - NVICSetSystemHandlerPriority(HANDLER_SVCALL, \ - CORTEX_PRIORITY_MASK(CORTEX_PRIORITY_SVCALL)); \ - NVICSetSystemHandlerPriority(HANDLER_PENDSV, \ - CORTEX_PRIORITY_MASK(CORTEX_PRIORITY_PENDSV)); \ - NVICSetSystemHandlerPriority(HANDLER_SYSTICK, \ - CORTEX_PRIORITY_MASK(CORTEX_PRIORITY_SYSTICK)); \ -} +#define port_init() _port_init() /** * @brief Kernel-lock action. @@ -357,6 +391,7 @@ struct intctx { extern "C" { #endif void port_halt(void); + void _port_init(void); void _port_switch(Thread *ntp, Thread *otp); void _port_irq_epilogue(void); void _port_switch_from_isr(void); diff --git a/os/ports/GCC/ARMCMx/nvic.h b/os/ports/GCC/ARMCMx/nvic.h index 02f010c4a..b7cc249b5 100644 --- a/os/ports/GCC/ARMCMx/nvic.h +++ b/os/ports/GCC/ARMCMx/nvic.h @@ -53,11 +53,6 @@ typedef volatile uint32_t IOREG32; /**< 32 bits I/O register type. */ */ #define NVIC_ITCR (*((IOREG32 *)0xE000E004)) -/** - * @brief NVIC STIR register. - */ -#define NVIC_STIR (*((IOREG32 *)0xE000EF00)) - /** * @brief Structure representing the SYSTICK I/O space. */ @@ -66,12 +61,12 @@ typedef struct { IOREG32 RVR; IOREG32 CVR; IOREG32 CBVR; -} CM3_ST; +} CMx_ST; /** * @brief SYSTICK peripheral base address. */ -#define STBase ((CM3_ST *)0xE000E010) +#define STBase ((CMx_ST *)0xE000E010) #define ST_CSR (STBase->CSR) #define ST_RVR (STBase->RVR) #define ST_CVR (STBase->CVR) @@ -111,18 +106,21 @@ typedef struct { IOREG32 IABR[8]; IOREG32 unused5[56]; IOREG32 IPR[60]; -} CM3_NVIC; + IOREG32 unused6[644]; + IOREG32 STIR; +} CMx_NVIC; /** * @brief NVIC peripheral base address. */ -#define NVICBase ((CM3_NVIC *)0xE000E100) +#define NVICBase ((CMx_NVIC *)0xE000E100) #define NVIC_ISER(n) (NVICBase->ISER[n]) #define NVIC_ICER(n) (NVICBase->ICER[n]) #define NVIC_ISPR(n) (NVICBase->ISPR[n]) #define NVIC_ICPR(n) (NVICBase->ICPR[n]) #define NVIC_IABR(n) (NVICBase->IABR[n]) #define NVIC_IPR(n) (NVICBase->IPR[n]) +#define NVIC_STIR (NVICBase->STIR) /** * @brief Structure representing the System Control Block I/O space. @@ -142,12 +140,19 @@ typedef struct { IOREG32 MMFAR; IOREG32 BFAR; IOREG32 AFSR; -} CM3_SCB; + IOREG32 PFR[2]; + IOREG32 DFR; + IOREG32 ADR; + IOREG32 MMFR[4]; + IOREG32 SAR[5]; + IOREG32 unused1[5]; + IOREG32 CPACR; +} CMx_SCB; /** * @brief SCB peripheral base address. */ -#define SCBBase ((CM3_SCB *)0xE000ED00) +#define SCBBase ((CMx_SCB *)0xE000ED00) #define SCB_CPUID (SCBBase->CPUID) #define SCB_ICSR (SCBBase->ICSR) #define SCB_VTOR (SCBBase->VTOR) @@ -162,6 +167,12 @@ typedef struct { #define SCB_MMFAR (SCBBase->MMFAR) #define SCB_BFAR (SCBBase->BFAR) #define SCB_AFSR (SCBBase->AFSR) +#define SCB_PFR(n) (SCBBase->PFR[n]) +#define SCB_DFR (SCBBase->DFR) +#define SCB_ADR (SCBBase->ADR) +#define SCB_MMFR(n) (SCBBase->MMFR[n]) +#define SCB_SAR(n) (SCBBase->SAR[n]) +#define SCB_CPACR (SCBBase->CPACR) #define ICSR_VECTACTIVE_MASK (0x1FF << 0) #define ICSR_RETTOBASE (0x1 << 11) @@ -172,12 +183,46 @@ typedef struct { #define ICSR_PENDSTSET (0x1 << 26) #define ICSR_PENDSVCLR (0x1 << 27) #define ICSR_PENDSVSET (0x1 << 28) -#define ICSR_NMIPENDSET (0x1 << 31) +#define ICSR_NMIPENDSET (0x1U << 31) #define AIRCR_VECTKEY 0x05FA0000 #define AIRCR_PRIGROUP_MASK (0x7 << 8) #define AIRCR_PRIGROUP(n) ((n) << 8) +typedef struct { + IOREG32 unused1[1]; + IOREG32 FPCCR; + IOREG32 FPCAR; + IOREG32 FPDSCR; + IOREG32 MVFR0; + IOREG32 MVFR1; +} CMx_FPU; + +/** + * @brief FPU peripheral base address. + */ +#define FPUBase ((CMx_FPU *)0xE000EF30L) +#define SCB_FPCCR (FPUBase->FPCCR) +#define SCB_FPCAR (FPUBase->FPCAR) +#define SCB_FPDSCR (FPUBase->FPDSCR) +#define SCB_MVFR0 (FPUBase->MVFR0) +#define SCB_MVFR1 (FPUBase->MVFR1) + +#define FPCCR_ASPEN (0x1U << 31) +#define FPCCR_LSPEN (0x1U << 30) +#define FPCCR_MONRDY (0x1U << 8) +#define FPCCR_BFRDY (0x1U << 6) +#define FPCCR_MMRDY (0x1U << 5) +#define FPCCR_HFRDY (0x1U << 4) +#define FPCCR_THREAD (0x1U << 3) +#define FPCCR_USER (0x1U << 1) +#define FPCCR_LSPACT (0x1U << 0) + +#define FPDSCR_AHP (0x1U << 26) +#define FPDSCR_DN (0x1U << 25) +#define FPDSCR_FZ (0x1U << 24) +#define FPDSCR_RMODE(n) ((n) << 22) + #ifdef __cplusplus extern "C" { #endif