From e74cea4172ee8564bef5f8500c5dd07512257d99 Mon Sep 17 00:00:00 2001
From: Sebastian Huber <sebastian.huber@embedded-brains.de>
Date: Thu, 2 May 2024 13:49:03 +0200
Subject: [PATCH] arm: Fix Armv7-M floating-point support

The _ARMV7M_Pendable_service_call() and _ARMV7M_Supervisor_call() work
as a team. The --ef and ++ef is there to preserve the original exception
frame across the jump to and from _ARMV7M_Thread_dispatch().

void _ARMV7M_Pendable_service_call( void )
{
   Per_CPU_Control *cpu_self = _Per_CPU_Get();

   /*
    * We must check here if a thread dispatch is allowed.  Right after a
    * "msr basepri_max, %[basepri]" instruction an interrupt service may
still
    * take place.  However, pendable service calls that are activated during
    * this interrupt service may be delayed until interrupts are enable
again.
    */
   if (
     ( cpu_self->isr_nest_level |
cpu_self->thread_dispatch_disable_level ) == 0
   ) {
     ARMV7M_Exception_frame *ef;

     cpu_self->isr_nest_level = 1;

     _ARMV7M_SCB->icsr = ARMV7M_SCB_ICSR_PENDSVCLR;
     _ARMV7M_Trigger_lazy_floating_point_context_save();

At this point, the floating point context should be saved on the
exception frame.  The FPCCR.LSPACT bit should be 0, to indicate that lazy
state preservation is no longer active.

     ef = (ARMV7M_Exception_frame *) _ARMV7M_Get_PSP();
     --ef;
     _ARMV7M_Set_PSP( (uint32_t) ef );

This new exception frame is just there to jump to
_ARMV7M_Thread_dispatch(). Here was the problem, that FPCCR.LSPACT was
not set to 1.  This resulted in a floating-point context from
uninitialized memory which could corrupt the floating-point state.  See
also:

https://developer.arm.com/documentation/ddi0403/d/System-Level-Architecture/System-Level-Programmers--Model/ARMv7-M-exception-model/Exception-return-behavior?lang=en

     /*
      * According to "ARMv7-M Architecture Reference Manual" section B1.5.6
      * "Exception entry behavior" the return address is half-word aligned.
      */
     ef->register_pc = (void *)
       ((uintptr_t) _ARMV7M_Thread_dispatch & ~((uintptr_t) 1));

     ef->register_xpsr = 0x01000000U;
   }
}

Close #4923.
---
 cpukit/score/cpu/arm/armv7m-isr-dispatch.c | 13 ++++++++++++-
 1 file changed, 12 insertions(+), 1 deletion(-)

diff --git a/cpukit/score/cpu/arm/armv7m-isr-dispatch.c b/cpukit/score/cpu/arm/armv7m-isr-dispatch.c
index ea168969ba..3b664083be 100644
--- a/cpukit/score/cpu/arm/armv7m-isr-dispatch.c
+++ b/cpukit/score/cpu/arm/armv7m-isr-dispatch.c
@@ -58,6 +58,7 @@ static void _ARMV7M_Trigger_lazy_floating_point_context_save( void )
 #ifdef ARM_MULTILIB_VFP
   __asm__ volatile (
     "vmov.f32 s0, s0\n"
+    : : : "memory"
   );
 #endif
 }
@@ -75,12 +76,22 @@ void _ARMV7M_Pendable_service_call( void )
   if (
     ( cpu_self->isr_nest_level | cpu_self->thread_dispatch_disable_level ) == 0
   ) {
+    volatile ARMV7M_SCB *scb = _ARMV7M_SCB;
     ARMV7M_Exception_frame *ef;
 
     cpu_self->isr_nest_level = 1;
 
-    _ARMV7M_SCB->icsr = ARMV7M_SCB_ICSR_PENDSVCLR;
+    scb->icsr = ARMV7M_SCB_ICSR_PENDSVCLR;
     _ARMV7M_Trigger_lazy_floating_point_context_save();
+#ifdef ARM_MULTILIB_VFP
+    /*
+     * Set FPCCR[LSPACT] to mark the lazy state preservation as active.  This
+     * prevents that a floating-point context is restored from the
+     * uninitialized exception frame below in the return to
+     * _ARMV7M_Thread_dispatch().
+     */
+    scb->fpccr |= 0x1;
+#endif
 
     ef = (ARMV7M_Exception_frame *) _ARMV7M_Get_PSP();
     --ef;