You are viewing a plain text version of this content. The canonical link for it is here.
Posted to commits@nuttx.apache.org by xi...@apache.org on 2022/04/14 14:33:16 UTC
[incubator-nuttx] branch master updated: arm/armv7-[a|r]: move fpu save/restore to assembly handler
This is an automated email from the ASF dual-hosted git repository.
xiaoxiang pushed a commit to branch master
in repository https://gitbox.apache.org/repos/asf/incubator-nuttx.git
The following commit(s) were added to refs/heads/master by this push:
new dc961baaea arm/armv7-[a|r]: move fpu save/restore to assembly handler
dc961baaea is described below
commit dc961baaea17107109d94575e9bb5d9fb725b801
Author: chao.an <an...@xiaomi.com>
AuthorDate: Thu Apr 14 18:07:14 2022 +0800
arm/armv7-[a|r]: move fpu save/restore to assembly handler
Save/Restore FPU registers in C environment is dangerous practive,
which cannot guarantee the compiler won't generate the assembly code
with float point registers, especially in interrupt handling
Signed-off-by: chao.an <an...@xiaomi.com>
---
arch/arm/src/arm/arm_doirq.c | 10 +--
arch/arm/src/arm/arm_vectors.S | 138 ++++++++++++++++++++++++++++++-
arch/arm/src/armv7-a/arm_doirq.c | 15 +---
arch/arm/src/armv7-a/arm_syscall.c | 3 -
arch/arm/src/armv7-a/arm_vectors.S | 144 +++++++++++++++++++++++++++++++++
arch/arm/src/armv7-r/arm_doirq.c | 16 ----
arch/arm/src/armv7-r/arm_syscall.c | 3 -
arch/arm/src/armv7-r/arm_vectors.S | 144 +++++++++++++++++++++++++++++++++
arch/arm/src/dm320/dm320_decodeirq.c | 16 +---
arch/arm/src/imx1/imx_decodeirq.c | 16 +---
arch/arm/src/lpc31xx/lpc31_decodeirq.c | 16 +---
11 files changed, 438 insertions(+), 83 deletions(-)
diff --git a/arch/arm/src/arm/arm_doirq.c b/arch/arm/src/arm/arm_doirq.c
index 03d58ec743..6b5a97986f 100644
--- a/arch/arm/src/arm/arm_doirq.c
+++ b/arch/arm/src/arm/arm_doirq.c
@@ -79,7 +79,7 @@ void arm_doirq(int irq, uint32_t *regs)
irq_dispatch(irq, regs);
-#if defined(CONFIG_ARCH_FPU) || defined(CONFIG_ARCH_ADDRENV)
+#ifdef CONFIG_ARCH_ADDRENV
/* Check for a context switch. If a context switch occurred, then
* CURRENT_REGS will have a different value than it did on entry. If an
* interrupt level context switch has occurred, then restore the floating
@@ -89,13 +89,6 @@ void arm_doirq(int irq, uint32_t *regs)
if (regs != CURRENT_REGS)
{
-#ifdef CONFIG_ARCH_FPU
- /* Restore floating point registers */
-
- arm_restorefpu((uint32_t *)CURRENT_REGS);
-#endif
-
-#ifdef CONFIG_ARCH_ADDRENV
/* Make sure that the address environment for the previously
* running task is closed down gracefully (data caches dump,
* MMU flushed) and set up the address environment for the new
@@ -103,7 +96,6 @@ void arm_doirq(int irq, uint32_t *regs)
*/
group_addrenv(NULL);
-#endif
}
#endif
diff --git a/arch/arm/src/arm/arm_vectors.S b/arch/arm/src/arm/arm_vectors.S
index 73a8ed8de4..fa30eed4dc 100644
--- a/arch/arm/src/arm/arm_vectors.S
+++ b/arch/arm/src/arm/arm_vectors.S
@@ -41,6 +41,66 @@
* Assembly Macros
****************************************************************************/
+/****************************************************************************
+ * Name: savefpu
+ *
+ * Description:
+ * Save the state of the floating point registers.
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_ARCH_FPU
+ .macro savefpu, out, tmp
+ /* Store all floating point registers. Registers are stored in numeric order,
+ * s0, s1, ... in increasing address order.
+ */
+
+#ifdef CONFIG_ARM_DPFPU32
+ vstmia.64 \out!, {d0-d15} /* Save the full FP context */
+ vstmia.64 \out!, {d16-d31}
+#else
+ vstmia \out!, {s0-s31} /* Save the full FP context */
+#endif
+
+ /* Store the floating point control and status register. At the end of the
+ * vstmia, r1 will point to the FPSCR storage location.
+ */
+
+ vmrs \tmp, fpscr /* Fetch the FPSCR */
+ str \tmp, [\out], #4 /* Save the floating point control and status register */
+ .endm
+#endif
+
+/****************************************************************************
+ * Name: restorefpu
+ *
+ * Description:
+ * Restore the state of the floating point registers.
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_ARCH_FPU
+ .macro restorefpu, in, tmp
+ /* Load all floating point registers. Registers are loaded in numeric order,
+ * s0, s1, ... in increasing address order.
+ */
+
+#ifdef CONFIG_ARM_DPFPU32
+ vldmia.64 \in!, {d0-d15} /* Restore the full FP context */
+ vldmia.64 \in!, {d16-d31}
+#else
+ vldmia \in!, {s0-s31} /* Restore the full FP context */
+#endif
+
+ /* Load the floating point control and status register. At the end of the
+ * vstmia, \in will point to the FPSCR storage location.
+ */
+
+ ldr \tmp, [\in], #4 /* Fetch the floating point control and status register */
+ vmsr fpscr, \tmp /* Restore the FPSCR */
+ .endm
+#endif
+
/****************************************************************************
* Private Functions
****************************************************************************/
@@ -101,6 +161,13 @@ arm_vectorirq:
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4}
+#ifdef CONFIG_ARCH_FPU
+ /* Save the state of the floating point registers. */
+
+ add r0, sp, #(4*REG_S0) /* R1=Address of FP register storage */
+ savefpu r0, r1
+#endif
+
/* Then call the IRQ handler with interrupts disabled. */
mov fp, #0 /* Init frame pointer */
@@ -116,7 +183,7 @@ arm_vectorirq:
#else
/* Call arm_decodeirq() on the user stack */
- mov r4, sp /* Save the SP in a preserved register */
+ mov r4, sp /* Save the SP in a preserved register */
/* If the interrupt stack is disabled, reserve xcpcontext to ensure
* that signal processing can have a separate xcpcontext to handle
@@ -131,9 +198,16 @@ arm_vectorirq:
sub sp, sp, #XCPTCONTEXT_SIZE /* Reserve signal context */
- bic sp, sp, #7 /* Force 8-byte alignment */
- bl arm_decodeirq /* Call the handler */
- mov sp, r4 /* Restore the possibly unaligned stack pointer */
+ bic sp, sp, #7 /* Force 8-byte alignment */
+ bl arm_decodeirq /* Call the handler */
+ mov sp, r4 /* Restore the possibly unaligned stack pointer */
+#endif
+
+#ifdef CONFIG_ARCH_FPU
+ /* Restore the state of the floating point registers. */
+
+ add r1, r0, #(4*REG_S0) /* R1=Address of FP register storage */
+ restorefpu r1, r2
#endif
/* Switch back IRQ mode and return with shadow SPSR */
@@ -212,6 +286,13 @@ arm_vectorsvc:
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4}
+#ifdef CONFIG_ARCH_FPU
+ /* Save the state of the floating point registers. */
+
+ add r0, sp, #(4*REG_S0) /* R1=Address of FP register storage */
+ savefpu r0, r1
+#endif
+
/* Then call the SVC handler with interrupts disabled.
* void arm_syscall(struct xcptcontext *xcp)
*/
@@ -220,6 +301,13 @@ arm_vectorsvc:
mov r0, sp /* Get r0=xcp */
bl arm_syscall /* Call the handler */
+#ifdef CONFIG_ARCH_FPU
+ /* Restore the state of the floating point registers. */
+
+ add r1, r0, #(4*REG_S0) /* R1=Address of FP register storage */
+ restorefpu r1, r2
+#endif
+
/* Switch back SVC mode and return with shadow SPSR */
mov r4, #(PSR_MODE_SVC | PSR_I_BIT)
@@ -294,6 +382,13 @@ arm_vectordata:
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4}
+#ifdef CONFIG_ARCH_FPU
+ /* Save the state of the floating point registers. */
+
+ add r0, sp, #(4*REG_S0) /* R1=Address of FP register storage */
+ savefpu r0, r1
+#endif
+
/* Then call the data abort handler with interrupts disabled.
* void arm_dataabort(struct xcptcontext *xcp)
*/
@@ -306,6 +401,13 @@ arm_vectordata:
#endif
bl arm_dataabort /* Call the handler */
+#ifdef CONFIG_ARCH_FPU
+ /* Restore the state of the floating point registers. */
+
+ add r1, r0, #(4*REG_S0) /* R1=Address of FP register storage */
+ restorefpu r1, r2
+#endif
+
/* Switch back ABT mode and return with shadow SPSR */
mov r4, #(PSR_MODE_ABT | PSR_I_BIT)
@@ -380,6 +482,13 @@ arm_vectorprefetch:
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4}
+#ifdef CONFIG_ARCH_FPU
+ /* Save the state of the floating point registers. */
+
+ add r0, sp, #(4*REG_S0) /* R1=Address of FP register storage */
+ savefpu r0, r1
+#endif
+
/* Then call the prefetch abort handler with interrupts disabled.
* void arm_prefetchabort(struct xcptcontext *xcp)
*/
@@ -388,6 +497,13 @@ arm_vectorprefetch:
mov r0, sp /* Get r0=xcp */
bl arm_prefetchabort /* Call the handler */
+#ifdef CONFIG_ARCH_FPU
+ /* Restore the state of the floating point registers. */
+
+ add r1, r0, #(4*REG_S0) /* R1=Address of FP register storage */
+ restorefpu r1, r2
+#endif
+
/* Switch back ABT mode and return with shadow SPSR */
mov r4, #(PSR_MODE_ABT | PSR_I_BIT)
@@ -460,6 +576,13 @@ arm_vectorundefinsn:
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4}
+#ifdef CONFIG_ARCH_FPU
+ /* Save the state of the floating point registers. */
+
+ add r0, sp, #(4*REG_S0) /* R1=Address of FP register storage */
+ savefpu r0, r1
+#endif
+
/* Then call the undef insn handler with interrupts disabled.
* void arm_undefinedinsn(struct xcptcontext *xcp)
*/
@@ -468,6 +591,13 @@ arm_vectorundefinsn:
mov r0, sp /* Get r0=xcp */
bl arm_undefinedinsn /* Call the handler */
+#ifdef CONFIG_ARCH_FPU
+ /* Restore the state of the floating point registers. */
+
+ add r1, r0, #(4*REG_S0) /* R1=Address of FP register storage */
+ restorefpu r1, r2
+#endif
+
/* Switch back UND mode and return with shadow SPSR */
mov r4, #(PSR_MODE_UND | PSR_I_BIT)
diff --git a/arch/arm/src/armv7-a/arm_doirq.c b/arch/arm/src/armv7-a/arm_doirq.c
index b4a0b5b3ea..e8c7e1c66c 100644
--- a/arch/arm/src/armv7-a/arm_doirq.c
+++ b/arch/arm/src/armv7-a/arm_doirq.c
@@ -70,23 +70,15 @@ uint32_t *arm_doirq(int irq, uint32_t *regs)
irq_dispatch(irq, regs);
-#if defined(CONFIG_ARCH_FPU) || defined(CONFIG_ARCH_ADDRENV)
+#ifdef CONFIG_ARCH_ADDRENV
/* Check for a context switch. If a context switch occurred, then
* CURRENT_REGS will have a different value than it did on entry. If an
- * interrupt level context switch has occurred, then restore the floating
- * point state and the establish the correct address environment before
- * returning from the interrupt.
+ * interrupt level context switch has occurred, then establish the correct
+ * address environment before returning from the interrupt.
*/
if (regs != CURRENT_REGS)
{
-#ifdef CONFIG_ARCH_FPU
- /* Restore floating point registers */
-
- arm_restorefpu((uint32_t *)CURRENT_REGS);
-#endif
-
-#ifdef CONFIG_ARCH_ADDRENV
/* Make sure that the address environment for the previously
* running task is closed down gracefully (data caches dump,
* MMU flushed) and set up the address environment for the new
@@ -94,7 +86,6 @@ uint32_t *arm_doirq(int irq, uint32_t *regs)
*/
group_addrenv(NULL);
-#endif
}
#endif
diff --git a/arch/arm/src/armv7-a/arm_syscall.c b/arch/arm/src/armv7-a/arm_syscall.c
index 0ac7c8c636..80e06f55fc 100644
--- a/arch/arm/src/armv7-a/arm_syscall.c
+++ b/arch/arm/src/armv7-a/arm_syscall.c
@@ -262,7 +262,6 @@ uint32_t *arm_syscall(uint32_t *regs)
* set will determine the restored context.
*/
- arm_restorefpu((uint32_t *)regs[REG_R1]);
regs = (uint32_t *)regs[REG_R1];
DEBUGASSERT(regs);
}
@@ -288,8 +287,6 @@ uint32_t *arm_syscall(uint32_t *regs)
case SYS_switch_context:
{
DEBUGASSERT(regs[REG_R1] != 0 && regs[REG_R2] != 0);
- arm_savefpu(regs);
- arm_restorefpu((uint32_t *)regs[REG_R2]);
*(uint32_t **)regs[REG_R1] = regs;
regs = (uint32_t *)regs[REG_R2];
}
diff --git a/arch/arm/src/armv7-a/arm_vectors.S b/arch/arm/src/armv7-a/arm_vectors.S
index 605e765243..220e61c9ac 100644
--- a/arch/arm/src/armv7-a/arm_vectors.S
+++ b/arch/arm/src/armv7-a/arm_vectors.S
@@ -88,6 +88,66 @@
.endm
#endif
+/****************************************************************************
+ * Name: savefpu
+ *
+ * Description:
+ * Save the state of the floating point registers.
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_ARCH_FPU
+ .macro savefpu, out, tmp
+ /* Store all floating point registers. Registers are stored in numeric order,
+ * s0, s1, ... in increasing address order.
+ */
+
+#ifdef CONFIG_ARM_DPFPU32
+ vstmia.64 \out!, {d0-d15} /* Save the full FP context */
+ vstmia.64 \out!, {d16-d31}
+#else
+ vstmia \out!, {s0-s31} /* Save the full FP context */
+#endif
+
+ /* Store the floating point control and status register. At the end of the
+ * vstmia, r1 will point to the FPSCR storage location.
+ */
+
+ vmrs \tmp, fpscr /* Fetch the FPSCR */
+ str \tmp, [\out], #4 /* Save the floating point control and status register */
+ .endm
+#endif
+
+/****************************************************************************
+ * Name: restorefpu
+ *
+ * Description:
+ * Restore the state of the floating point registers.
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_ARCH_FPU
+ .macro restorefpu, in, tmp
+ /* Load all floating point registers. Registers are loaded in numeric order,
+ * s0, s1, ... in increasing address order.
+ */
+
+#ifdef CONFIG_ARM_DPFPU32
+ vldmia.64 \in!, {d0-d15} /* Restore the full FP context */
+ vldmia.64 \in!, {d16-d31}
+#else
+ vldmia \in!, {s0-s31} /* Restore the full FP context */
+#endif
+
+ /* Load the floating point control and status register. At the end of the
+ * vstmia, \in will point to the FPSCR storage location.
+ */
+
+ ldr \tmp, [\in], #4 /* Fetch the floating point control and status register */
+ vmsr fpscr, \tmp /* Restore the FPSCR */
+ .endm
+#endif
+
/****************************************************************************
* Private Functions
****************************************************************************/
@@ -160,6 +220,13 @@ arm_vectorirq:
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4}
+#ifdef CONFIG_ARCH_FPU
+ /* Save the state of the floating point registers. */
+
+ add r0, sp, #(4*REG_S0) /* R1=Address of FP register storage */
+ savefpu r0, r1
+#endif
+
/* Then call the IRQ handler with interrupts disabled. */
mov fp, #0 /* Init frame pointer */
@@ -196,6 +263,13 @@ arm_vectorirq:
mov sp, r4 /* Restore the possibly unaligned stack pointer */
#endif
+#ifdef CONFIG_ARCH_FPU
+ /* Restore the state of the floating point registers. */
+
+ add r1, r0, #(4*REG_S0) /* R1=Address of FP register storage */
+ restorefpu r1, r2
+#endif
+
/* Switch back IRQ mode and return with shadow SPSR */
#ifdef CONFIG_ARMV7A_DECODEFIQ
@@ -289,6 +363,13 @@ arm_vectorsvc:
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4}
+#ifdef CONFIG_ARCH_FPU
+ /* Save the state of the floating point registers. */
+
+ add r0, sp, #(4*REG_S0) /* R1=Address of FP register storage */
+ savefpu r0, r1
+#endif
+
/* Then call the SVC handler with interrupts disabled.
* void arm_syscall(struct xcptcontext *xcp)
*/
@@ -327,6 +408,13 @@ arm_vectorsvc:
mov sp, r4 /* Restore the possibly unaligned stack pointer */
#endif
+#ifdef CONFIG_ARCH_FPU
+ /* Restore the state of the floating point registers. */
+
+ add r1, r0, #(4*REG_S0) /* R1=Address of FP register storage */
+ restorefpu r1, r2
+#endif
+
/* Switch back SVC mode and return with shadow SPSR */
#ifdef CONFIG_ARMV7A_DECODEFIQ
@@ -423,6 +511,13 @@ arm_vectordata:
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4}
+#ifdef CONFIG_ARCH_FPU
+ /* Save the state of the floating point registers. */
+
+ add r0, sp, #(4*REG_S0) /* R1=Address of FP register storage */
+ savefpu r0, r1
+#endif
+
/* Then call the data abort handler with interrupts disabled.
* void arm_dataabort(struct xcptcontext *xcp)
*/
@@ -436,6 +531,13 @@ arm_vectordata:
bl arm_dataabort /* Call the handler */
mov sp, r4 /* Restore the possibly unaligned stack pointer */
+#ifdef CONFIG_ARCH_FPU
+ /* Restore the state of the floating point registers. */
+
+ add r1, r0, #(4*REG_S0) /* R1=Address of FP register storage */
+ restorefpu r1, r2
+#endif
+
/* Switch back ABT mode and return with shadow SPSR */
#ifdef CONFIG_ARMV7A_DECODEFIQ
@@ -520,6 +622,13 @@ arm_vectorprefetch:
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4}
+#ifdef CONFIG_ARCH_FPU
+ /* Save the state of the floating point registers. */
+
+ add r0, sp, #(4*REG_S0) /* R1=Address of FP register storage */
+ savefpu r0, r1
+#endif
+
/* Then call the prefetch abort handler with interrupts disabled.
* void arm_prefetchabort(struct xcptcontext *xcp)
*/
@@ -533,6 +642,13 @@ arm_vectorprefetch:
bl arm_prefetchabort /* Call the handler */
mov sp, r4 /* Restore the possibly unaligned stack pointer */
+#ifdef CONFIG_ARCH_FPU
+ /* Restore the state of the floating point registers. */
+
+ add r1, r0, #(4*REG_S0) /* R1=Address of FP register storage */
+ restorefpu r1, r2
+#endif
+
/* Switch back ABT mode and return with shadow SPSR */
#ifdef CONFIG_ARMV7A_DECODEFIQ
@@ -615,6 +731,13 @@ arm_vectorundefinsn:
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4}
+#ifdef CONFIG_ARCH_FPU
+ /* Save the state of the floating point registers. */
+
+ add r0, sp, #(4*REG_S0) /* R1=Address of FP register storage */
+ savefpu r0, r1
+#endif
+
/* Then call the undef insn handler with interrupts disabled.
* void arm_undefinedinsn(struct xcptcontext *xcp)
*/
@@ -626,6 +749,13 @@ arm_vectorundefinsn:
bl arm_undefinedinsn /* Call the handler */
mov sp, r4 /* Restore the possibly unaligned stack pointer */
+#ifdef CONFIG_ARCH_FPU
+ /* Restore the state of the floating point registers. */
+
+ add r1, r0, #(4*REG_S0) /* R1=Address of FP register storage */
+ restorefpu r1, r2
+#endif
+
/* Switch back UND mode and return with shadow SPSR */
#ifdef CONFIG_ARMV7A_DECODEFIQ
@@ -709,6 +839,13 @@ arm_vectorfiq:
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4}
+#ifdef CONFIG_ARCH_FPU
+ /* Save the state of the floating point registers. */
+
+ add r0, sp, #(4*REG_S0) /* R1=Address of FP register storage */
+ savefpu r0, r1
+#endif
+
/* Then call the FIQ handler with interrupts disabled. */
mov fp, #0 /* Init frame pointer */
@@ -732,6 +869,13 @@ arm_vectorfiq:
mov sp, r4 /* Restore the possibly unaligned stack pointer */
#endif
+#ifdef CONFIG_ARCH_FPU
+ /* Restore the state of the floating point registers. */
+
+ add r1, r0, #(4*REG_S0) /* R1=Address of FP register storage */
+ restorefpu r1, r2
+#endif
+
/* Switch back FIQ mode and return with shadow SPSR */
#ifdef CONFIG_ARMV7A_DECODEFIQ
diff --git a/arch/arm/src/armv7-r/arm_doirq.c b/arch/arm/src/armv7-r/arm_doirq.c
index c5c0b8ff59..6d59fc035d 100644
--- a/arch/arm/src/armv7-r/arm_doirq.c
+++ b/arch/arm/src/armv7-r/arm_doirq.c
@@ -60,22 +60,6 @@ uint32_t *arm_doirq(int irq, uint32_t *regs)
irq_dispatch(irq, regs);
-#ifdef CONFIG_ARCH_FPU
- /* Check for a context switch. If a context switch occurred, then
- * CURRENT_REGS will have a different value than it did on entry. If an
- * interrupt level context switch has occurred, then restore the floating
- * point state and the establish the correct address environment before
- * returning from the interrupt.
- */
-
- if (regs != CURRENT_REGS)
- {
- /* Restore floating point registers */
-
- arm_restorefpu((uint32_t *)CURRENT_REGS);
- }
-#endif
-
/* Set CURRENT_REGS to NULL to indicate that we are no longer in an
* interrupt handler.
*/
diff --git a/arch/arm/src/armv7-r/arm_syscall.c b/arch/arm/src/armv7-r/arm_syscall.c
index 4a52d0191b..4a576a4d01 100644
--- a/arch/arm/src/armv7-r/arm_syscall.c
+++ b/arch/arm/src/armv7-r/arm_syscall.c
@@ -259,7 +259,6 @@ uint32_t *arm_syscall(uint32_t *regs)
* set will determine the restored context.
*/
- arm_restorefpu((uint32_t *)regs[REG_R1]);
regs = (uint32_t *)regs[REG_R1];
DEBUGASSERT(regs);
}
@@ -285,8 +284,6 @@ uint32_t *arm_syscall(uint32_t *regs)
case SYS_switch_context:
{
DEBUGASSERT(regs[REG_R1] != 0 && regs[REG_R2] != 0);
- arm_savefpu(regs);
- arm_restorefpu((uint32_t *)regs[REG_R2]);
*(uint32_t **)regs[REG_R1] = regs;
regs = (uint32_t *)regs[REG_R2];
}
diff --git a/arch/arm/src/armv7-r/arm_vectors.S b/arch/arm/src/armv7-r/arm_vectors.S
index d131962fdf..4114ecae46 100644
--- a/arch/arm/src/armv7-r/arm_vectors.S
+++ b/arch/arm/src/armv7-r/arm_vectors.S
@@ -42,6 +42,66 @@
* Assembly Macros
****************************************************************************/
+/****************************************************************************
+ * Name: savefpu
+ *
+ * Description:
+ * Save the state of the floating point registers.
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_ARCH_FPU
+ .macro savefpu, out, tmp
+ /* Store all floating point registers. Registers are stored in numeric order,
+ * s0, s1, ... in increasing address order.
+ */
+
+#ifdef CONFIG_ARM_DPFPU32
+ vstmia.64 \out!, {d0-d15} /* Save the full FP context */
+ vstmia.64 \out!, {d16-d31}
+#else
+ vstmia \out!, {s0-s31} /* Save the full FP context */
+#endif
+
+ /* Store the floating point control and status register. At the end of the
+ * vstmia, r1 will point to the FPSCR storage location.
+ */
+
+ vmrs \tmp, fpscr /* Fetch the FPSCR */
+ str \tmp, [\out], #4 /* Save the floating point control and status register */
+ .endm
+#endif
+
+/****************************************************************************
+ * Name: restorefpu
+ *
+ * Description:
+ * Restore the state of the floating point registers.
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_ARCH_FPU
+ .macro restorefpu, in, tmp
+ /* Load all floating point registers. Registers are loaded in numeric order,
+ * s0, s1, ... in increasing address order.
+ */
+
+#ifdef CONFIG_ARM_DPFPU32
+ vldmia.64 \in!, {d0-d15} /* Restore the full FP context */
+ vldmia.64 \in!, {d16-d31}
+#else
+ vldmia \in!, {s0-s31} /* Restore the full FP context */
+#endif
+
+ /* Load the floating point control and status register. At the end of the
+ * vstmia, \in will point to the FPSCR storage location.
+ */
+
+ ldr \tmp, [\in], #4 /* Fetch the floating point control and status register */
+ vmsr fpscr, \tmp /* Restore the FPSCR */
+ .endm
+#endif
+
/****************************************************************************
* Private Functions
****************************************************************************/
@@ -114,6 +174,13 @@ arm_vectorirq:
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4}
+#ifdef CONFIG_ARCH_FPU
+ /* Save the state of the floating point registers. */
+
+ add r0, sp, #(4*REG_S0) /* R1=Address of FP register storage */
+ savefpu r0, r1
+#endif
+
/* Then call the IRQ handler with interrupts disabled. */
mov fp, #0 /* Init frame pointer */
@@ -150,6 +217,13 @@ arm_vectorirq:
mov sp, r4 /* Restore the possibly unaligned stack pointer */
#endif
+#ifdef CONFIG_ARCH_FPU
+ /* Restore the state of the floating point registers. */
+
+ add r1, r0, #(4*REG_S0) /* R1=Address of FP register storage */
+ restorefpu r1, r2
+#endif
+
/* Switch back IRQ mode and return with shadow SPSR */
#ifdef CONFIG_ARMV7A_DECODEFIQ
@@ -243,6 +317,13 @@ arm_vectorsvc:
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4}
+#ifdef CONFIG_ARCH_FPU
+ /* Save the state of the floating point registers. */
+
+ add r0, sp, #(4*REG_S0) /* R1=Address of FP register storage */
+ savefpu r0, r1
+#endif
+
/* Then call the SVC handler with interrupts disabled.
* void arm_syscall(struct xcptcontext *xcp)
*/
@@ -281,6 +362,13 @@ arm_vectorsvc:
mov sp, r4 /* Restore the possibly unaligned stack pointer */
#endif
+#ifdef CONFIG_ARCH_FPU
+ /* Restore the state of the floating point registers. */
+
+ add r1, r0, #(4*REG_S0) /* R1=Address of FP register storage */
+ restorefpu r1, r2
+#endif
+
/* Switch back SVC mode and return with shadow SPSR */
#ifdef CONFIG_ARMV7A_DECODEFIQ
@@ -377,6 +465,13 @@ arm_vectordata:
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4}
+#ifdef CONFIG_ARCH_FPU
+ /* Save the state of the floating point registers. */
+
+ add r0, sp, #(4*REG_S0) /* R1=Address of FP register storage */
+ savefpu r0, r1
+#endif
+
/* Then call the data abort handler with interrupts disabled.
* void arm_dataabort(struct xcptcontext *xcp)
*/
@@ -390,6 +485,13 @@ arm_vectordata:
bl arm_dataabort /* Call the handler */
mov sp, r4 /* Restore the possibly unaligned stack pointer */
+#ifdef CONFIG_ARCH_FPU
+ /* Restore the state of the floating point registers. */
+
+ add r1, r0, #(4*REG_S0) /* R1=Address of FP register storage */
+ restorefpu r1, r2
+#endif
+
/* Switch back ABT mode and return with shadow SPSR */
#ifdef CONFIG_ARMV7A_DECODEFIQ
@@ -474,6 +576,13 @@ arm_vectorprefetch:
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4}
+#ifdef CONFIG_ARCH_FPU
+ /* Save the state of the floating point registers. */
+
+ add r0, sp, #(4*REG_S0) /* R1=Address of FP register storage */
+ savefpu r0, r1
+#endif
+
/* Then call the prefetch abort handler with interrupts disabled.
* void arm_prefetchabort(struct xcptcontext *xcp)
*/
@@ -487,6 +596,13 @@ arm_vectorprefetch:
bl arm_prefetchabort /* Call the handler */
mov sp, r4 /* Restore the possibly unaligned stack pointer */
+#ifdef CONFIG_ARCH_FPU
+ /* Restore the state of the floating point registers. */
+
+ add r1, r0, #(4*REG_S0) /* R1=Address of FP register storage */
+ restorefpu r1, r2
+#endif
+
/* Switch back ABT mode and return with shadow SPSR */
#ifdef CONFIG_ARMV7A_DECODEFIQ
@@ -569,6 +685,13 @@ arm_vectorundefinsn:
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4}
+#ifdef CONFIG_ARCH_FPU
+ /* Save the state of the floating point registers. */
+
+ add r0, sp, #(4*REG_S0) /* R1=Address of FP register storage */
+ savefpu r0, r1
+#endif
+
/* Then call the undef insn handler with interrupts disabled.
* void arm_undefinedinsn(struct xcptcontext *xcp)
*/
@@ -580,6 +703,13 @@ arm_vectorundefinsn:
bl arm_undefinedinsn /* Call the handler */
mov sp, r4 /* Restore the possibly unaligned stack pointer */
+#ifdef CONFIG_ARCH_FPU
+ /* Restore the state of the floating point registers. */
+
+ add r1, r0, #(4*REG_S0) /* R1=Address of FP register storage */
+ restorefpu r1, r2
+#endif
+
/* Switch back UND mode and return with shadow SPSR */
#ifdef CONFIG_ARMV7A_DECODEFIQ
@@ -663,6 +793,13 @@ arm_vectorfiq:
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4}
+#ifdef CONFIG_ARCH_FPU
+ /* Save the state of the floating point registers. */
+
+ add r0, sp, #(4*REG_S0) /* R1=Address of FP register storage */
+ savefpu r0, r1
+#endif
+
/* Then call the FIQ handler with interrupts disabled. */
mov fp, #0 /* Init frame pointer */
@@ -686,6 +823,13 @@ arm_vectorfiq:
mov sp, r4 /* Restore the possibly unaligned stack pointer */
#endif
+#ifdef CONFIG_ARCH_FPU
+ /* Restore the state of the floating point registers. */
+
+ add r1, r0, #(4*REG_S0) /* R1=Address of FP register storage */
+ restorefpu r1, r2
+#endif
+
/* Switch back FIQ mode and return with shadow SPSR */
#ifdef CONFIG_ARMV7A_DECODEFIQ
diff --git a/arch/arm/src/dm320/dm320_decodeirq.c b/arch/arm/src/dm320/dm320_decodeirq.c
index 0745770f9e..8e48ad2c56 100644
--- a/arch/arm/src/dm320/dm320_decodeirq.c
+++ b/arch/arm/src/dm320/dm320_decodeirq.c
@@ -83,23 +83,16 @@ uint32_t *arm_decodeirq(uint32_t *regs)
irq_dispatch(irq, regs);
-#if defined(CONFIG_ARCH_FPU) || defined(CONFIG_ARCH_ADDRENV)
+#ifdef CONFIG_ARCH_ADDRENV
/* Check for a context switch. If a context switch occurred, then
* CURRENT_REGS will have a different value than it did on entry.
- * If an interrupt level context switch has occurred, then restore
- * the floating point state and the establish the correct address
- * environment before returning from the interrupt.
+ * If an interrupt level context switch has occurred, then
+ * establish the correct address environment before returning
+ * from the interrupt.
*/
if (regs != CURRENT_REGS)
{
-#ifdef CONFIG_ARCH_FPU
- /* Restore floating point registers */
-
- arm_restorefpu((uint32_t *)CURRENT_REGS);
-#endif
-
-#ifdef CONFIG_ARCH_ADDRENV
/* Make sure that the address environment for the previously
* running task is closed down gracefully (data caches dump,
* MMU flushed) and set up the address environment for the new
@@ -107,7 +100,6 @@ uint32_t *arm_decodeirq(uint32_t *regs)
*/
group_addrenv(NULL);
-#endif
}
#endif
diff --git a/arch/arm/src/imx1/imx_decodeirq.c b/arch/arm/src/imx1/imx_decodeirq.c
index 74b355b5c2..3f634cb9a9 100644
--- a/arch/arm/src/imx1/imx_decodeirq.c
+++ b/arch/arm/src/imx1/imx_decodeirq.c
@@ -99,23 +99,16 @@ uint32_t *arm_decodeirq(uint32_t *regs)
irq_dispatch(irq, regs);
-#if defined(CONFIG_ARCH_FPU) || defined(CONFIG_ARCH_ADDRENV)
+#ifdef CONFIG_ARCH_ADDRENV
/* Check for a context switch. If a context switch occurred, then
* CURRENT_REGS will have a different value than it did on entry.
- * If an interrupt level context switch has occurred, then restore
- * the floating point state and the establish the correct address
- * environment before returning from the interrupt.
+ * If an interrupt level context switch has occurred, then
+ * establish the correct address environment before returning
+ * from the interrupt.
*/
if (regs != CURRENT_REGS)
{
-#ifdef CONFIG_ARCH_FPU
- /* Restore floating point registers */
-
- arm_restorefpu((uint32_t *)CURRENT_REGS);
-#endif
-
-#ifdef CONFIG_ARCH_ADDRENV
/* Make sure that the address environment for the previously
* running task is closed down gracefully (data caches dump,
* MMU flushed) and set up the address environment for the new
@@ -123,7 +116,6 @@ uint32_t *arm_decodeirq(uint32_t *regs)
*/
group_addrenv(NULL);
-#endif
}
#endif
}
diff --git a/arch/arm/src/lpc31xx/lpc31_decodeirq.c b/arch/arm/src/lpc31xx/lpc31_decodeirq.c
index 7120714636..8a504fd675 100644
--- a/arch/arm/src/lpc31xx/lpc31_decodeirq.c
+++ b/arch/arm/src/lpc31xx/lpc31_decodeirq.c
@@ -89,23 +89,16 @@ uint32_t *arm_decodeirq(uint32_t *regs)
irq_dispatch(irq, regs);
-#if defined(CONFIG_ARCH_FPU) || defined(CONFIG_ARCH_ADDRENV)
+#ifdef CONFIG_ARCH_ADDRENV
/* Check for a context switch. If a context switch occurred, then
* CURRENT_REGS will have a different value than it did on entry.
- * If an interrupt level context switch has occurred, then restore
- * the floating point state and the establish the correct address
- * environment before returning from the interrupt.
+ * If an interrupt level context switch has occurred, then
+ * establish the correct address environment before returning
+ * from the interrupt.
*/
if (regs != CURRENT_REGS)
{
-#ifdef CONFIG_ARCH_FPU
- /* Restore floating point registers */
-
- arm_restorefpu((uint32_t *)CURRENT_REGS);
-#endif
-
-#ifdef CONFIG_ARCH_ADDRENV
/* Make sure that the address environment for the previously
* running task is closed down gracefully (data caches dump,
* MMU flushed) and set up the address environment for the new
@@ -113,7 +106,6 @@ uint32_t *arm_decodeirq(uint32_t *regs)
*/
group_addrenv(NULL);
-#endif
}
#endif