Keil CM4 FPU support added.

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@3675 35acf78f-673a-0410-8e92-d51de3d6d3f4

8 files changed, 397 insertions, 195 deletions

diff --git a/demos/ARMCM4-STM32F407-DISCOVERY/keil/ch.uvproj b/demos/ARMCM4-STM32F407-DISCOVERY/keil/ch.uvproj
index 24588192f..c6aa767d3 100644
--- a/demos/ARMCM4-STM32F407-DISCOVERY/keil/ch.uvproj
+++ b/demos/ARMCM4-STM32F407-DISCOVERY/keil/ch.uvproj
@@ -204,7 +204,7 @@
             <hadIRAM>1</hadIRAM>
             <hadXRAM>0</hadXRAM>
             <uocXRam>0</uocXRam>
-            <RvdsVP>1</RvdsVP>
+            <RvdsVP>2</RvdsVP>
             <hadIRAM2>1</hadIRAM2>
             <hadIROM2>0</hadIROM2>
             <StupSel>8</StupSel>
@@ -346,7 +346,7 @@
               <MiscControls></MiscControls>
               <Define>__heap_base__=Image$$RW_IRAM1$$ZI$$Limit __heap_end__=Image$$RW_IRAM2$$Base</Define>
               <Undefine></Undefine>
-              <IncludePath>..\;..\..\..\os\kernel\include;..\..\..\os\ports\RVCT\ARMCMx;..\..\..\os\ports\RVCT\ARMCMx\STM32F4xx;..\..\..\os\hal\include;..\..\..\os\hal\platforms\STM32;..\..\..\os\hal\platforms\STM32\GPIOv2;..\..\..\os\hal\platforms\STM32F4xx;..\..\..\boards\ST_STM32F4_DISCOVERY;..\..\..\test</IncludePath>
+              <IncludePath>..\;..\..\..\os\kernel\include;..\..\..\os\ports\common\ARMCMx;..\..\..\os\ports\common\ARMCMx\CMSIS\include;..\..\..\os\ports\RVCT\ARMCMx;..\..\..\os\ports\RVCT\ARMCMx\STM32F4xx;..\..\..\os\hal\include;..\..\..\os\hal\platforms\STM32;..\..\..\os\hal\platforms\STM32\GPIOv2;..\..\..\os\hal\platforms\STM32F4xx;..\..\..\boards\ST_STM32F4_DISCOVERY;..\..\..\test</IncludePath>
             </VariousControls>
           </Cads>
           <Aads>
@@ -422,11 +422,6 @@
               <FilePath>..\..\..\os\ports\RVCT\ARMCMx\chcore_v7m.c</FilePath>
             </File>
             <File>
-              <FileName>nvic.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\..\..\os\ports\RVCT\ARMCMx\nvic.c</FilePath>
-            </File>
-            <File>
               <FileName>chcore.h</FileName>
               <FileType>5</FileType>
               <FilePath>..\..\..\os\ports\RVCT\ARMCMx\chcore.h</FilePath>
@@ -442,11 +437,6 @@
               <FilePath>..\..\..\os\ports\RVCT\ARMCMx\chtypes.h</FilePath>
             </File>
             <File>
-              <FileName>nvic.h</FileName>
-              <FileType>5</FileType>
-              <FilePath>..\..\..\os\ports\RVCT\ARMCMx\nvic.h</FilePath>
-            </File>
-            <File>
               <FileName>cmparams.h</FileName>
               <FileType>5</FileType>
               <FilePath>..\..\..\os\ports\RVCT\ARMCMx\STM32F4xx\cmparams.h</FilePath>
@@ -456,6 +446,16 @@
               <FileType>2</FileType>
               <FilePath>..\..\..\os\ports\RVCT\ARMCMx\STM32F4xx\vectors.s</FilePath>
             </File>
+            <File>
+              <FileName>nvic.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\..\..\os\ports\common\ARMCMx\nvic.c</FilePath>
+            </File>
+            <File>
+              <FileName>nvic.h</FileName>
+              <FileType>5</FileType>
+              <FilePath>..\..\..\os\ports\common\ARMCMx\nvic.h</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>
diff --git a/docs/reports/STM32F407-168-RVCT.txt b/docs/reports/STM32F407-168-RVCT.txt
index 40a194098..cbd3796d0 100644
--- a/docs/reports/STM32F407-168-RVCT.txt
+++ b/docs/reports/STM32F407-168-RVCT.txt
@@ -6,8 +6,8 @@ Compiler: RealView C/C++ Compiler V4.1.0.791 [Evaluation].
 

 *** ChibiOS/RT test suite

 ***

-*** Kernel:       2.3.4unstable

-*** Compiled:     Nov 26 2011 - 22:18:53

+*** Kernel:       2.3.5unstable

+*** Compiled:     Dec 28 2011 - 13:36:15

 *** Compiler:     RVCT

 *** Architecture: ARMv7-ME

 *** Core Variant: Cortex-M4

@@ -101,51 +101,51 @@ Compiler: RealView C/C++ Compiler V4.1.0.791 [Evaluation].
 --- Result: SUCCESS

 ----------------------------------------------------------------------------

 --- Test Case 11.1 (Benchmark, messages #1)

---- Score : 711240 msgs/S, 1422480 ctxswc/S

+--- Score : 711270 msgs/S, 1422540 ctxswc/S

 --- Result: SUCCESS

 ----------------------------------------------------------------------------

 --- Test Case 11.2 (Benchmark, messages #2)

---- Score : 610371 msgs/S, 1220742 ctxswc/S

+--- Score : 610399 msgs/S, 1220798 ctxswc/S

 --- Result: SUCCESS

 ----------------------------------------------------------------------------

 --- Test Case 11.3 (Benchmark, messages #3)

---- Score : 610371 msgs/S, 1220742 ctxswc/S

+--- Score : 610399 msgs/S, 1220798 ctxswc/S

 --- Result: SUCCESS

 ----------------------------------------------------------------------------

 --- Test Case 11.4 (Benchmark, context switch)

---- Score : 2376704 ctxswc/S

+--- Score : 2393744 ctxswc/S

 --- Result: SUCCESS

 ----------------------------------------------------------------------------

 --- Test Case 11.5 (Benchmark, threads, full cycle)

---- Score : 448805 threads/S

+--- Score : 447628 threads/S

 --- Result: SUCCESS

 ----------------------------------------------------------------------------

 --- Test Case 11.6 (Benchmark, threads, create only)

---- Score : 640666 threads/S

+--- Score : 638254 threads/S

 --- Result: SUCCESS

 ----------------------------------------------------------------------------

 --- Test Case 11.7 (Benchmark, mass reschedule, 5 threads)

---- Score : 205448 reschedules/S, 1232688 ctxswc/S

+--- Score : 205457 reschedules/S, 1232742 ctxswc/S

 --- Result: SUCCESS

 ----------------------------------------------------------------------------

 --- Test Case 11.8 (Benchmark, round robin context switching)

---- Score : 1434640 ctxswc/S

+--- Score : 1434680 ctxswc/S

 --- Result: SUCCESS

 ----------------------------------------------------------------------------

 --- Test Case 11.9 (Benchmark, I/O Queues throughput)

---- Score : 1724868 bytes/S

+--- Score : 1724960 bytes/S

 --- Result: SUCCESS

 ----------------------------------------------------------------------------

 --- Test Case 11.10 (Benchmark, virtual timers set/reset)

---- Score : 2223216 timers/S

+--- Score : 2223332 timers/S

 --- Result: SUCCESS

 ----------------------------------------------------------------------------

 --- Test Case 11.11 (Benchmark, semaphores wait/signal)

---- Score : 3197168 wait+signal/S

+--- Score : 3197328 wait+signal/S

 --- Result: SUCCESS

 ----------------------------------------------------------------------------

 --- Test Case 11.12 (Benchmark, mutexes lock/unlock)

---- Score : 1929332 lock+unlock/S

+--- Score : 1929428 lock+unlock/S

 --- Result: SUCCESS

 ----------------------------------------------------------------------------

 --- Test Case 11.13 (Benchmark, RAM footprint)

diff --git a/os/ports/RVCT/ARMCMx/chcore.h b/os/ports/RVCT/ARMCMx/chcore.h
index 08b3a2ccb..a7ecc8ec0 100644
--- a/os/ports/RVCT/ARMCMx/chcore.h
+++ b/os/ports/RVCT/ARMCMx/chcore.h
@@ -53,7 +53,7 @@
 #if (CORTEX_MODEL == CORTEX_M0) || (CORTEX_MODEL == CORTEX_M3) ||           \

     (CORTEX_MODEL == CORTEX_M4)

 #elif (CORTEX_MODEL == CORTEX_M1)

-#warning "untested Cortex-M model"

+#error "untested Cortex-M model"

 #else

 #error "unknown or unsupported Cortex-M model"

 #endif

@@ -105,7 +105,7 @@
  *          a stack frame when compiling without optimizations. You may

  *          reduce this value to zero when compiling with optimizations.

  */

-#ifndef PORT_IDLE_THREAD_STACK_SIZE

+#if !defined(PORT_IDLE_THREAD_STACK_SIZE)

 #define PORT_IDLE_THREAD_STACK_SIZE     16

 #endif

 

@@ -120,14 +120,14 @@
  *          @p chSchDoReschedule() can have a stack frame, expecially with

  *          compiler optimizations disabled.

  */

-#ifndef PORT_INT_REQUIRED_STACK

+#if !defined(PORT_INT_REQUIRED_STACK)

 #define PORT_INT_REQUIRED_STACK         16

 #endif

 

 /**

  * @brief   Enables the use of the WFI instruction in the idle thread loop.

  */

-#ifndef CORTEX_ENABLE_WFI_IDLE

+#if !defined(CORTEX_ENABLE_WFI_IDLE)

 #define CORTEX_ENABLE_WFI_IDLE          FALSE

 #endif

 

@@ -136,25 +136,12 @@
  * @note    The default SYSTICK handler priority is calculated as the priority

  *          level in the middle of the numeric priorities range.

  */

-#ifndef CORTEX_PRIORITY_SYSTICK

+#if !defined(CORTEX_PRIORITY_SYSTICK)

 #define CORTEX_PRIORITY_SYSTICK         (CORTEX_PRIORITY_LEVELS >> 1)

-#else

+#elif !CORTEX_IS_VALID_PRIORITY(CORTEX_PRIORITY_SYSTICK)

 /* If it is externally redefined then better perform a validity check on it.*/

-#if !CORTEX_IS_VALID_PRIORITY(CORTEX_PRIORITY_SYSTICK)

 #error "invalid priority level specified for CORTEX_PRIORITY_SYSTICK"

 #endif

-#endif

-

-/**

- * @brief   Stack alignment enforcement.

- * @note    The default value is 64 in order to comply with EABI, reducing

- *          the value to 32 can save some RAM space if you don't care about

- *          binary compatibility with EABI compiled libraries.

- * @note    Allowed values are 32 or 64.

- */

-#ifndef CORTEX_STACK_ALIGNMENT

-#define CORTEX_STACK_ALIGNMENT          64

-#endif

 

 /*===========================================================================*/

 /* Port derived parameters (common).                                         */

@@ -189,86 +176,51 @@
 

 #include "nvic.h"

 

+/* The following declarations are there just for Doxygen documentation, the

+   real declarations are inside the sub-headers.*/

+#if defined(__DOXYGEN__)

+

 /**

  * @brief   Stack and memory alignment enforcement.

+ * @note    In this architecture the stack alignment is enforced to 64 bits,

+ *          32 bits alignment is supported by hardware but deprecated by ARM,

+ *          the implementation choice is to not offer the option.

  */

-#if (CORTEX_STACK_ALIGNMENT == 64) || defined(__DOXYGEN__)

-#if defined(__DOXYGEN__)

-/* Dummy declaration, for Doxygen only.*/

 typedef uint64_t stkalign_t;

-#else

-typedef uint64_t stkalign_t __attribute__ ((aligned (8)));

-#endif

-#elif CORTEX_STACK_ALIGNMENT == 32

-typedef uint32_t stkalign_t __attribute__ ((aligned (4)));

-#else

-#error "invalid stack alignment selected"

-#endif

 

-#if defined(__DOXYGEN__)

 /**

  * @brief   Interrupt saved context.

  * @details This structure represents the stack frame saved during a

  *          preemption-capable interrupt handler.

  * @note    It is implemented to match the Cortex-Mx exception context.

  */

-struct extctx {

-  /* Dummy definition, just for Doxygen.*/

-};

+struct extctx {};

 

 /**

  * @brief   System saved context.

  * @details This structure represents the inner stack frame during a context

  *          switching.

  */

-struct intctx {

-  /* Dummy definition, just for Doxygen.*/

-};

-#endif

-

-/**

- * @brief   Platform dependent part of the @p Thread structure.

-

- * @details In this port the structure just holds a pointer to the @p intctx

- *          structure representing the stack pointer at context switch time.

- */

-struct context {

-  struct intctx *r13;

-};

-

-/**

- * @brief   Platform dependent part of the @p chThdCreateI() API.

- * @details This code usually setup the context switching frame represented

- *          by an @p intctx structure.

- */

-#define SETUP_CONTEXT(workspace, wsize, pf, arg) {                          \

-  tp->p_ctx.r13 = (struct intctx *)((uint8_t *)workspace +                  \

-                                     wsize -                                \

-                                     sizeof(struct intctx));                \

-  tp->p_ctx.r13->r4 = (void *)pf;                                           \

-  tp->p_ctx.r13->r5 = (void *)arg;                                          \

-  tp->p_ctx.r13->lr = (void *)_port_thread_start;                           \

-}

+struct intctx {};

 

-/**

- * @brief   Enforces a correct alignment for a stack area size value.

- */

-#define STACK_ALIGN(n) ((((n) - 1) | (sizeof(stkalign_t) - 1)) + 1)

+#endif /* defined(__DOXYGEN__) */

 

 /**

- * @brief   Computes the thread working area global size.

+ * @brief   Excludes the default @p chSchIsPreemptionRequired()implementation.

  */

-#define THD_WA_SIZE(n) STACK_ALIGN(sizeof(Thread) +                         \

-                                   sizeof(struct intctx) +                  \

-                                   sizeof(struct extctx) +                  \

-                                   (n) + (PORT_INT_REQUIRED_STACK))

+#define PORT_OPTIMIZED_ISPREEMPTIONREQUIRED

 

+#if (CH_TIME_QUANTUM > 0) || defined(__DOXYGEN__)

 /**

- * @brief   Static working area allocation.

- * @details This macro is used to allocate a static thread working area

- *          aligned as both position and size.

+ * @brief   Inlineable version of this kernel function.

  */

-#define WORKING_AREA(s, n) stkalign_t s[THD_WA_SIZE(n) / sizeof(stkalign_t)]

+#define chSchIsPreemptionRequired()                                         \

+  (rlist.r_preempt ? firstprio(&rlist.r_queue) > currp->p_prio :            \

+                     firstprio(&rlist.r_queue) >= currp->p_prio)

+#else /* CH_TIME_QUANTUM == 0 */

+#define chSchIsPreemptionRequired()                                         \

+  (firstprio(&rlist.r_queue) > currp->p_prio)

+#endif /* CH_TIME_QUANTUM == 0 */

 

 #endif /* _FROM_ASM_ */

 

diff --git a/os/ports/RVCT/ARMCMx/chcore_v7m.c b/os/ports/RVCT/ARMCMx/chcore_v7m.c
index 7ed226255..336552107 100644
--- a/os/ports/RVCT/ARMCMx/chcore_v7m.c
+++ b/os/ports/RVCT/ARMCMx/chcore_v7m.c
@@ -28,6 +28,10 @@
 

 #include "ch.h"

 

+/*===========================================================================*/

+/* Port interrupt handlers.                                                  */

+/*===========================================================================*/

+

 /**

  * @brief   System Timer vector.

  * @details This interrupt is used as system tick.

@@ -44,4 +48,158 @@ CH_IRQ_HANDLER(SysTickVector) {
   CH_IRQ_EPILOGUE();

 }

 

+#if !CORTEX_SIMPLIFIED_PRIORITY || defined(__DOXYGEN__)

+/**

+ * @brief   SVC vector.

+ * @details The SVC vector is used for exception mode re-entering after a

+ *          context switch.

+ * @note    The PendSV vector is only used in advanced kernel mode.

+ */

+void SVCallVector(void) {

+  struct extctx *ctxp;

+  register uint32_t psp __asm("psp");

+

+  /* Current PSP value.*/

+  ctxp = (struct extctx *)psp;

+

+  /* Discarding the current exception context and positioning the stack to

+     point to the real one.*/

+  ctxp++;

+

+#if CORTEX_USE_FPU

+  /* Restoring the special register SCB_FPCCR.*/

+  SCB_FPCCR = (uint32_t)ctxp->fpccr;

+  SCB_FPCAR = SCB_FPCAR + sizeof (struct extctx);

+#endif

+  psp = (uint32_t)ctxp;

+  port_unlock_from_isr();

+}

+#endif /* !CORTEX_SIMPLIFIED_PRIORITY */

+

+#if CORTEX_SIMPLIFIED_PRIORITY || defined(__DOXYGEN__)

+/**

+ * @brief   PendSV vector.

+ * @details The PendSV vector is used for exception mode re-entering after a

+ *          context switch.

+ * @note    The PendSV vector is only used in compact kernel mode.

+ */

+void PendSVVector(void) {

+  struct extctx *ctxp;

+  register uint32_t psp __asm("psp");

+

+  /* Current PSP value.*/

+  ctxp = (struct extctx *)psp;

+

+  /* Discarding the current exception context and positioning the stack to

+     point to the real one.*/

+  ctxp++;

+

+#if CORTEX_USE_FPU

+  /* Restoring the special register SCB_FPCCR.*/

+  SCB_FPCCR = (uint32_t)ctxp->fpccr;

+  SCB_FPCAR = SCB_FPCAR + sizeof (struct extctx);

+#endif

+  psp = (uint32_t)ctxp;

+}

+#endif /* CORTEX_SIMPLIFIED_PRIORITY */

+

+/*===========================================================================*/

+/* Port exported functions.                                                  */

+/*===========================================================================*/

+

+/**

+ * @brief   Port-related initialization code.

+ */

+void _port_init(void) {

+

+  /* 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

+  {

+    register uint32_t control __asm("control");

+    register uint32_t fpscr __asm("fpscr");

+

+    /* Initializing the FPU context save in lazy mode.*/

+    SCB_FPCCR = FPCCR_ASPEN | FPCCR_LSPEN;

+

+    /* CP10 and CP11 set to full access in the startup code.*/

+/*    SCB_CPACR |= 0x00F00000;*/

+

+    /* Enables FPU context save/restore on exception entry/exit (FPCA bit).*/

+    control |= 4;

+

+    /* FPSCR and FPDSCR initially zero.*/

+    fpscr = 0;

+    SCB_FPDSCR = 0;

+  }

+#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) != 0) {

+    struct extctx *ctxp;

+    register uint32_t psp __asm("psp");

+

+    /* Current PSP value.*/

+    ctxp = (struct extctx *)psp;

+

+    /* Adding an artificial exception return context, there is no need to

+       populate it fully.*/

+    ctxp--;

+    psp = (uint32_t)ctxp;

+    ctxp->xpsr = (regarm_t)0x01000000;

+

+    /* The exit sequence is different depending on if a preemption is

+       required or not.*/

+    if (chSchIsPreemptionRequired()) {

+#if CORTEX_USE_FPU

+      /* Triggering a lazy FPU state save.*/

+      register volatile uint32_t fpscr __asm("fpscr");

+      (void)fpscr;

+#endif

+      /* Preemption is required we need to enforce a context switch.*/

+      ctxp->pc = (regarm_t)_port_switch_from_isr;

+    }

+    else {

+      /* Preemption not required, we just need to exit the exception

+         atomically.*/

+      ctxp->pc = (regarm_t)_port_exit_from_isr;

+    }

+

+#if CORTEX_USE_FPU

+    {

+      uint32_t fpccr;

+

+      /* Saving the special register SCB_FPCCR into the reserved offset of

+         the Cortex-M4 exception frame.*/

+      (ctxp + 1)->fpccr = (regarm_t)(fpccr = SCB_FPCCR);

+

+      /* Now the FPCCR is modified in order to not restore the FPU status

+         from the artificial return context.*/

+      SCB_FPCCR = fpccr | FPCCR_LSPACT;

+    }

+#endif

+

+    /* Note, returning without unlocking is intentional, this is done in

+       order to keep the rest of the context switching atomic.*/

+    return;

+  }

+  port_unlock_from_isr();

+}

+

 /** @} */

diff --git a/os/ports/RVCT/ARMCMx/chcore_v7m.h b/os/ports/RVCT/ARMCMx/chcore_v7m.h
index 5a7b6eb0e..612eeb814 100644
--- a/os/ports/RVCT/ARMCMx/chcore_v7m.h
+++ b/os/ports/RVCT/ARMCMx/chcore_v7m.h
@@ -39,14 +39,77 @@
 #define CORTEX_BASEPRI_DISABLED         0

 

 /*===========================================================================*/

+/* Port macros.                                                              */

+/*===========================================================================*/

+

+/*===========================================================================*/

 /* Port configurable parameters.                                             */

 /*===========================================================================*/

 

 /**

+ * @brief   Stack size for the system idle thread.

+ * @details This size depends on the idle thread implementation, usually

+ *          the idle thread should take no more space than those reserved

+ *          by @p PORT_INT_REQUIRED_STACK.

+ * @note    In this port it is set to 16 because the idle thread does have

+ *          a stack frame when compiling without optimizations. You may

+ *          reduce this value to zero when compiling with optimizations.

+ */

+#if !defined(PORT_IDLE_THREAD_STACK_SIZE)

+#define PORT_IDLE_THREAD_STACK_SIZE     16

+#endif

+

+/**

+ * @brief   Per-thread stack overhead for interrupts servicing.

+ * @details This constant is used in the calculation of the correct working

+ *          area size.

+ *          This value can be zero on those architecture where there is a

+ *          separate interrupt stack and the stack space between @p intctx and

+ *          @p extctx is known to be zero.

+ * @note    In this port it is conservatively set to 16 because the function

+ *          @p chSchDoReschedule() can have a stack frame, expecially with

+ *          compiler optimizations disabled.

+ */

+#if !defined(PORT_INT_REQUIRED_STACK)

+#define PORT_INT_REQUIRED_STACK         16

+#endif

+

+/**

+ * @brief   Enables the use of the WFI instruction in the idle thread loop.

+ */

+#if !defined(CORTEX_ENABLE_WFI_IDLE)

+#define CORTEX_ENABLE_WFI_IDLE          FALSE

+#endif

+

+/**

+ * @brief   SYSTICK handler priority.

+ * @note    The default SYSTICK handler priority is calculated as the priority

+ *          level in the middle of the numeric priorities range.

+ */

+#if !defined(CORTEX_PRIORITY_SYSTICK)

+#define CORTEX_PRIORITY_SYSTICK         (CORTEX_PRIORITY_LEVELS >> 1)

+#elif !CORTEX_IS_VALID_PRIORITY(CORTEX_PRIORITY_SYSTICK)

+/* If it is externally redefined then better perform a validity check on it.*/

+#error "invalid priority level specified for CORTEX_PRIORITY_SYSTICK"

+#endif

+

+/**

+ * @brief   FPU support in context switch.

+ * @details Activating this option activates the FPU support in the kernel.

+ */

+#if !defined(CORTEX_USE_FPU)

+#define CORTEX_USE_FPU                  CORTEX_HAS_FPU

+#elif CORTEX_USE_FPU && !CORTEX_HAS_FPU

+/* This setting requires an FPU presence check in case it is externally

+   redefined.*/

+#error "the selected core does not have an FPU"

+#endif

+

+/**

  * @brief   Simplified priority handling flag.

- * @details Activating this option will make the Kernel work in compact mode.

+ * @details Activating this option makes the Kernel work in compact mode.

  */

-#ifndef CORTEX_SIMPLIFIED_PRIORITY

+#if !defined(CORTEX_SIMPLIFIED_PRIORITY)

 #define CORTEX_SIMPLIFIED_PRIORITY      FALSE

 #endif

 

@@ -57,14 +120,12 @@
  *          @p CORTEX_MAXIMUM_PRIORITY priority level as fast interrupts

  *          priority level.

  */

-#ifndef CORTEX_PRIORITY_SVCALL

+#if !defined(CORTEX_PRIORITY_SVCALL)

 #define CORTEX_PRIORITY_SVCALL          (CORTEX_MAXIMUM_PRIORITY + 1)

-#else

+#elif !CORTEX_IS_VALID_PRIORITY(CORTEX_PRIORITY_SVCALL)

 /* If it is externally redefined then better perform a validity check on it.*/

-#if !CORTEX_IS_VALID_PRIORITY(CORTEX_PRIORITY_SVCALL)

 #error "invalid priority level specified for CORTEX_PRIORITY_SVCALL"

 #endif

-#endif

 

 /**

  * @brief   NVIC VTOR initialization expression.

@@ -119,8 +180,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.

@@ -142,7 +207,18 @@
  */

 typedef void *regarm_t;

 

+/**

+ * @brief   Stack and memory alignment enforcement.

+ * @note    In this architecture the stack alignment is enforced to 64 bits,

+ *          32 bits alignment is supported by hardware but deprecated by ARM,

+ *          the implementation choice is to not offer the option.

+ */

+typedef uint64_t stkalign_t;

+

+/* The documentation of the following declarations is in chconf.h in order

+   to not have duplicated structure names into the documentation.*/

 #if !defined(__DOXYGEN__)

+

 struct extctx {

   regarm_t      r0;

   regarm_t      r1;

@@ -152,9 +228,47 @@ struct extctx {
   regarm_t      lr_thd;

   regarm_t      pc;

   regarm_t      xpsr;

+#if CORTEX_USE_FPU

+  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      fpscr;

+  regarm_t      fpccr;

+#endif /* CORTEX_USE_FPU */

 };

 

 struct intctx {

+#if CORTEX_USE_FPU

+  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 */

   regarm_t      r4;

   regarm_t      r5;

   regarm_t      r6;

@@ -165,7 +279,51 @@ struct intctx {
   regarm_t      r11;

   regarm_t      lr;

 };

-#endif

+

+#endif /* !defined(__DOXYGEN__) */

+

+/**

+ * @brief   Platform dependent part of the @p Thread structure.

+ * @details In this port the structure just holds a pointer to the @p intctx

+ *          structure representing the stack pointer at context switch time.

+ */

+struct context {

+  struct intctx *r13;

+};

+

+/**

+ * @brief   Platform dependent part of the @p chThdCreateI() API.

+ * @details This code usually setup the context switching frame represented

+ *          by an @p intctx structure.

+ */

+#define SETUP_CONTEXT(workspace, wsize, pf, arg) {                          \

+  tp->p_ctx.r13 = (struct intctx *)((uint8_t *)workspace +                  \

+                                     wsize -                                \

+                                     sizeof(struct intctx));                \

+  tp->p_ctx.r13->r4 = (regarm_t)pf;                                         \

+  tp->p_ctx.r13->r5 = (regarm_t)arg;                                        \

+  tp->p_ctx.r13->lr = (regarm_t)_port_thread_start;                         \

+}

+

+/**

+ * @brief   Enforces a correct alignment for a stack area size value.

+ */

+#define STACK_ALIGN(n) ((((n) - 1) | (sizeof(stkalign_t) - 1)) + 1)

+

+/**

+ * @brief   Computes the thread working area global size.

+ */

+#define THD_WA_SIZE(n) STACK_ALIGN(sizeof(Thread) +                         \

+                                   sizeof(struct intctx) +                  \

+                                   sizeof(struct extctx) +                  \

+                                   (n) + (PORT_INT_REQUIRED_STACK))

+

+/**

+ * @brief   Static working area allocation.

+ * @details This macro is used to allocate a static thread working area

+ *          aligned as both position and size.

+ */

+#define WORKING_AREA(s, n) stkalign_t s[THD_WA_SIZE(n) / sizeof(stkalign_t)]

 

 /**

  * @brief   IRQ prologue code.

@@ -198,16 +356,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.

@@ -242,7 +391,6 @@ struct intctx {
 /**

  * @brief   Kernel-lock action from an interrupt handler.

  * @details This function is invoked before invoking I-class APIs from

-

  *          interrupt handlers. The implementation is architecture dependent,

  *          in its simplest form it is void.

  * @note    Same as @p port_lock() in this port.

@@ -333,9 +481,11 @@ struct intctx {
 extern "C" {

 #endif

   void port_halt(void);

-  void _port_switch(Thread *ntp, Thread *otp);

+  void _port_init(void);

   void _port_irq_epilogue(void);

   void _port_switch_from_isr(void);

+  void _port_exit_from_isr(void);

+  void _port_switch(Thread *ntp, Thread *otp);

   void _port_thread_start(void);

 #ifdef __cplusplus

 }

diff --git a/os/ports/RVCT/ARMCMx/chcoreasm_v7m.s b/os/ports/RVCT/ARMCMx/chcoreasm_v7m.s
index 6be194737..7d2d88fbf 100644
--- a/os/ports/RVCT/ARMCMx/chcoreasm_v7m.s
+++ b/os/ports/RVCT/ARMCMx/chcoreasm_v7m.s
@@ -25,10 +25,8 @@
 #include "chconf.h"

 #include "chcore.h"

 

-EXTCTX_SIZE     EQU     32

 CONTEXT_OFFSET  EQU     12

 SCB_ICSR        EQU     0xE000ED04

-ICSR_RETTOBASE  EQU     0x00000800

 ICSR_PENDSVSET  EQU     0x10000000

 

                 PRESERVE8

@@ -36,7 +34,6 @@ ICSR_PENDSVSET  EQU     0x10000000
                 AREA    |.text|, CODE, READONLY

 

                 IMPORT  chThdExit

-                IMPORT  chSchIsPreemptionRequired

                 IMPORT  chSchDoReschedule

 #if CH_DBG_SYSTEM_STATE_CHECK

                 IMPORT  dbg_check_unlock

@@ -49,8 +46,14 @@ ICSR_PENDSVSET  EQU     0x10000000
                 EXPORT _port_switch

 _port_switch    PROC

                 push    {r4, r5, r6, r7, r8, r9, r10, r11, lr}

+#if CORTEX_USE_FPU

+                vpush   {s16-s31}

+#endif

                 str     sp, [r1, #CONTEXT_OFFSET]                          

                 ldr     sp, [r0, #CONTEXT_OFFSET]

+#if CORTEX_USE_FPU

+                vpop    {s16-s31}

+#endif

                 pop     {r4, r5, r6, r7, r8, r9, r10, r11, pc}

                 ENDP

 

@@ -79,17 +82,16 @@ _port_thread_start PROC
  * Exception handlers return here for context switching.

  */

                 EXPORT  _port_switch_from_isr

+                EXPORT  _port_exit_from_isr

 _port_switch_from_isr PROC

 #if CH_DBG_SYSTEM_STATE_CHECK

                 bl      dbg_check_lock

 #endif

-                bl      chSchIsPreemptionRequired

-                cbz     r0, noreschedule

                 bl      chSchDoReschedule

-noreschedule

 #if CH_DBG_SYSTEM_STATE_CHECK

                 bl      dbg_check_unlock

 #endif

+_port_exit_from_isr

 #if CORTEX_SIMPLIFIED_PRIORITY

                 mov     r3, #SCB_ICSR :AND: 0xFFFF

                 movt    r3, #SCB_ICSR :SHR: 16

@@ -102,72 +104,4 @@ waithere        b       waithere
 #endif

                 ENDP

 

-/*

- * Reschedule verification and setup after an IRQ.

- */

-                EXPORT  _port_irq_epilogue

-_port_irq_epilogue PROC

-#if CORTEX_SIMPLIFIED_PRIORITY

-                cpsid   i

-#else

-                movs    r3, #CORTEX_BASEPRI_KERNEL

-                msr     BASEPRI, r3

-#endif

-                mov     r3, #SCB_ICSR :AND: 0xFFFF

-                movt    r3, #SCB_ICSR :SHR: 16

-                ldr     r3, [r3, #0]

-                ands    r3, r3, #ICSR_RETTOBASE

-                bne     skipexit

-#if CORTEX_SIMPLIFIED_PRIORITY

-                cpsie   i

-#else

-                /* Note, R3 is already zero.*/

-                msr     BASEPRI, r3

-#endif

-                bx      lr

-skipexit

-                mrs     r3, PSP

-                subs    r3, r3, #EXTCTX_SIZE

-                msr     PSP, r3

-                ldr     r2, =_port_switch_from_isr

-                str     r2, [r3, #24]

-                mov     r2, #0x01000000

-                str     r2, [r3, #28]

-                bx      lr

-                ENDP

-

-/*

- * SVC vector.

- * Discarding the current exception context and positioning the stack to

- * point to the real one.

- */

-#if !CORTEX_SIMPLIFIED_PRIORITY

-                EXPORT  SVCallVector

-SVCallVector    PROC

-                mrs     r3, PSP

-                adds    r3, r3, #EXTCTX_SIZE

-                msr     PSP, r3

-                movs    r3, #CORTEX_BASEPRI_DISABLED

-                msr     BASEPRI, r3

-                bx      lr

-                nop

-                ENDP

-#endif

-

-/*

- * PendSV vector.

- * Discarding the current exception context and positioning the stack to

- * point to the real one.

- */

-#if CORTEX_SIMPLIFIED_PRIORITY

-                EXPORT  PendSVVector

-PendSVVector    PROC

-                mrs     r3, PSP

-                adds    r3, r3, #EXTCTX_SIZE

-                msr     PSP, r3

-                bx      lr

-                nop

-                ENDP

-#endif

-

                 END

diff --git a/os/ports/RVCT/ARMCMx/cstartup.s b/os/ports/RVCT/ARMCMx/cstartup.s
index 3c0fde7c9..5c770f50c 100644
--- a/os/ports/RVCT/ARMCMx/cstartup.s
+++ b/os/ports/RVCT/ARMCMx/cstartup.s
@@ -75,6 +75,14 @@ Reset_Handler   PROC
                 msr     CONTROL, r0

                 isb

                 bl      __early_init

+

+                IF      {CPU} = "Cortex-M4.fp"

+                LDR     R0, =0xE000ED88           ; Enable CP10,CP11

+                LDR     R1, [R0]

+                ORR     R1, R1, #(0xF << 20)

+                STR     R1, [R0]

+                ENDIF

+

                 ldr     r0, =__main

                 bx      r0

                 ENDP

diff --git a/readme.txt b/readme.txt
index 440f59499..e942168c1 100644
--- a/readme.txt
+++ b/readme.txt
@@ -87,7 +87,7 @@
 - NEW: Added a linker script that demonstrates how to put stacks and other

   critical structures in the CCM RAM instead normal RAM.

 - NEW: Added experimental support for the Cortex-M4 FPU (default when the

-  FPU is present).

+  FPU is present but can be disabled).

 - NEW: Improved I2C driver model and STM32 implementation by Barthess.

 - CHANGE: Removed the option to change the stack alignment in the GCC

   Cortex-Mx ports, now alignment is always 64 bits.