From e5a10ba0c5252b61a43886c47b70bbbf06cb15af Mon Sep 17 00:00:00 2001
From: gdisirio <gdisirio@35acf78f-673a-0410-8e92-d51de3d6d3f4>
Date: Sun, 1 Apr 2012 12:33:00 +0000
Subject: Fixed bug 3513897.

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@4068 35acf78f-673a-0410-8e92-d51de3d6d3f4
---
 os/ports/GCC/ARMCMx/chcore_v7m.c | 22 ----------------------
 os/ports/GCC/ARMCMx/crt0.c       | 27 ++++++++++++++++++++-------
 2 files changed, 20 insertions(+), 29 deletions(-)

(limited to 'os')

diff --git a/os/ports/GCC/ARMCMx/chcore_v7m.c b/os/ports/GCC/ARMCMx/chcore_v7m.c
index 13f78bb60..8c650a202 100644
--- a/os/ports/GCC/ARMCMx/chcore_v7m.c
+++ b/os/ports/GCC/ARMCMx/chcore_v7m.c
@@ -114,28 +114,6 @@ void _port_init(void) {
   SCB_VTOR = CORTEX_VTOR_INIT;
   SCB_AIRCR = AIRCR_VECTKEY | AIRCR_PRIGROUP(0);
 
-#if CORTEX_USE_FPU
-  {
-    uint32_t reg;
-
-    /* Initializing the FPU context save in lazy mode.*/
-    SCB_FPCCR = FPCCR_ASPEN | FPCCR_LSPEN;
-
-    /* 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;
-  }
-#endif
-
   /* Initialization of the system vectors used by the port.*/
   nvicSetSystemHandlerPriority(HANDLER_SVCALL,
     CORTEX_PRIORITY_MASK(CORTEX_PRIORITY_SVCALL));
diff --git a/os/ports/GCC/ARMCMx/crt0.c b/os/ports/GCC/ARMCMx/crt0.c
index fbff66fb0..bb8d3830d 100644
--- a/os/ports/GCC/ARMCMx/crt0.c
+++ b/os/ports/GCC/ARMCMx/crt0.c
@@ -26,10 +26,7 @@
  * @{
  */
 
-#include <stdint.h>
-
-#define FALSE   0
-#define TRUE    (!FALSE)
+#include "ch.h"
 
 typedef void (*funcp_t)(void);
 typedef funcp_t * funcpp_t;
@@ -250,7 +247,7 @@ static void fill32(uint32_t *start, uint32_t *end, uint32_t filler) {
 __attribute__((naked))
 #endif
 void ResetHandler(void) {
-  uint32_t psp, ctl;
+  uint32_t psp, reg;
 
   /* Process Stack initialization, it is allocated starting from the
      symbol __process_stack_end__ and its lower limit is the symbol
@@ -259,9 +256,25 @@ void ResetHandler(void) {
   psp = SYMVAL(__process_stack_end__);
   asm volatile ("msr     PSP, %0" : : "r" (psp));
 
+#if CORTEX_USE_FPU
+  /* Initializing the FPU context save in lazy mode.*/
+  SCB_FPCCR = FPCCR_ASPEN | FPCCR_LSPEN;
+
+  /* CP10 and CP11 set to full access.*/
+  SCB_CPACR |= 0x00F00000;
+
+  /* FPSCR and FPDSCR initially zero.*/
+  reg = 0;
+  asm volatile ("vmsr    FPSCR, %0" : : "r" (reg) : "memory");
+  SCB_FPDSCR = reg;
+
+  /* CPU mode initialization, enforced FPCA bit.*/
+  reg = CRT0_CONTROL_INIT | 4;
+#else
   /* CPU mode initialization.*/
-  ctl = CRT0_CONTROL_INIT;
-  asm volatile ("msr     CONTROL, %0" : : "r" (ctl));
+  reg = CRT0_CONTROL_INIT;
+#endif
+  asm volatile ("msr     CONTROL, %0" : : "r" (reg));
   asm volatile ("isb");
 
   /* Early initialization hook invocation.*/
-- 
cgit v1.2.3