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

3 files changed, 198 insertions, 29 deletions

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,29 +126,91 @@ 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().

  */

 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
@@ -54,11 +54,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.

  */

 typedef struct {

@@ -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