[PATCH v3] ARMv7M: Fix exception handler for supporting FPU

[Martin Galvan]

On exception entry, _ARMV7M_Exception_default stores the previous Stack Pointer
in a CPU_Exception_frame. The SP can be MSP or PSP, depending on the mode
in which the exception was taken. To know this, we must check the value of LR.

Right now the code checks whether it should store MSP or PSP by comparing LR to
-3. However, this doesn't work if we're using an FPU. This patch fixes that
by checking bit 2 of LR, which indicates which SP we were using.

Thanks to Sudarshan Rajagopalan for finding the bug and proposing a first
version of the fix.

Closes #2401.

---
 cpukit/score/cpu/arm/armv7m-exception-default.c | 27 +++++++++++++++++--------
 1 file changed, 19 insertions(+), 8 deletions(-)

diff --git a/cpukit/score/cpu/arm/armv7m-exception-default.c b/cpukit/score/cpu/arm/armv7m-exception-default.c
index e890cdf..d04dbea 100644
--- a/cpukit/score/cpu/arm/armv7m-exception-default.c
+++ b/cpukit/score/cpu/arm/armv7m-exception-default.c
@@ -22,16 +22,27 @@
 
 void __attribute__((naked)) _ARMV7M_Exception_default( void )
 {
+    /* On exception entry, ARMv7M saves context state onto a stack pointed to
+     * by either MSP or PSP. The value stored in LR indicates whether we were
+     * in Thread or Handler mode, whether we were using the FPU (if any),
+     * and which stack pointer we were using.
+     * In particular, bit 2 of LR will be 0 if we were using MSP.
+     *
+     * For a more detailed explanation, see the Exception Entry Behavior
+     * section of the ARMv7M Architecture Reference Manual.
+     */
+
+    /* As we're in Handler mode here, we'll always operate on MSP.
+     * However, we need to store the right SP in our CPU_Exception_frame.
+     */
   __asm__ volatile (
-    "sub sp, %[cpufsz]\n"
+    "sub sp, %[cpufsz]\n" /* Allocate space for a CPU_Exception_frame. */
     "stm sp, {r0-r12}\n"
-    "mov r2, lr\n"
-    "mrs r1, msp\n"
-    "mrs r0, psp\n"
-    "cmn r2, #3\n"
-    "itt ne\n"
-    "movne r0, r1\n"
-    "addne r0, %[cpufsz]\n"
+    "tst lr, #4\n"       /* Check if bit 2 of LR is zero. (If so, PSR.Z = 1) */
+    "itte eq\n"          /* IF bit 2 of LR is zero... (PSR.Z == 1) */
+    "mrseq r0, msp\n"    /* THEN we were using MSP. */
+    "addeq r0, %[cpufsz]\n" /* THEN, set r0 = old MSP. */
+    "mrsne r0, psp\n"    /* ELSE it's not zero; we were using PSP. */
     "add r2, r0, %[v7mlroff]\n"
     "add r1, sp, %[cpulroff]\n"
     "ldm r2, {r3-r5}\n"
-- 
1.9.1