diff --git a/os/ports/GCC/ARMCMx/chcore_v7m.c b/os/ports/GCC/ARMCMx/chcore_v7m.c
index 04f9ab7e8..e4ea3a72e 100644
--- a/os/ports/GCC/ARMCMx/chcore_v7m.c
+++ b/os/ports/GCC/ARMCMx/chcore_v7m.c
@@ -120,6 +120,9 @@ void _port_init(void) {
   {
     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;
 
@@ -132,9 +135,6 @@ void _port_init(void) {
     reg = 0;
     asm volatile ("vmsr    FPSCR, %0" : : "r" (reg) : "memory");
     SCB_FPDSCR = reg;
-
-    /* Initializing the FPU context save in lazy mode.*/
-    SCB_FPCCR = FPCCR_ASPEN | FPCCR_LSPEN;
   }
 #endif
 
@@ -150,6 +150,9 @@ void _port_init(void) {
  * @brief   Exception exit redirection to _port_switch_from_isr().
  */
 void _port_irq_epilogue(void) {
+#if CORTEX_USE_FPU
+  uint32_t fpccr;
+#endif
 
   port_lock_from_isr();
   if ((SCB_ICSR & ICSR_RETTOBASE)) {
@@ -182,16 +185,14 @@ void _port_irq_epilogue(void) {
     }
 
 #if CORTEX_USE_FPU
-    {
-      uint32_t fpccr;
-
-      /* Saving the special register SCB_FPCCR.*/
-      ctxp->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;
-    }
+    /* Saving the special register SCB_FPCCR into the reserved offset of
+       the Cortex-M4 exception frame.*/
+    (ctxp + 1)->fpccr = (regarm_t)(fpccr = SCB_FPCCR);
+#endif
+#if CORTEX_USE_FPU
+    /* 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
diff --git a/testhal/STM32F4xx/IRQ_STORM_FPU/main.c b/testhal/STM32F4xx/IRQ_STORM_FPU/main.c
index 47194e090..73c98bf6f 100644
--- a/testhal/STM32F4xx/IRQ_STORM_FPU/main.c
+++ b/testhal/STM32F4xx/IRQ_STORM_FPU/main.c
@@ -75,11 +75,11 @@ static msg_t WorkerThread(void *arg) {
  * GPT2 callback.
  */
 static void gpt2cb(GPTDriver *gptp) {
-/*  float f1, f2, f3, f4, f5;*/
+/*  float f1, f2, f3, f4, f5;
 
   (void)gptp;
 
-/*  f1 = ff1(2);
+  f1 = ff1(2);
   f2 = ff1(3);
   f3 = ff1(4);
   f5 = f1 + f2 + f3;
@@ -87,14 +87,13 @@ static void gpt2cb(GPTDriver *gptp) {
   f5 = ff2(f5, f4, f5, f4);
   if (f5 != 196)
     chSysHalt();*/
-  volatile float f1 = ff1(67);
 }
 
 /*
  * GPT3 callback.
  */
 static void gpt3cb(GPTDriver *gptp) {
-  float f1, f2, f3, f4, f5;
+/*  float f1, f2, f3, f4, f5;
 
   (void)gptp;
 
@@ -105,7 +104,8 @@ static void gpt3cb(GPTDriver *gptp) {
   f4 = ff1(4);
   f5 = ff2(f5, f4, f5, f4);
   if (f5 != 100)
-    chSysHalt();
+    chSysHalt();*/
+  volatile float f1 = ff1(1);
 }
 
 /*