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 <anchao@xiaomi.com>
This commit is contained in:
chao.an
Xiang Xiao
parent
f14eed6432
commit
dc961baaea
11 changed files with 438 additions and 83 deletions
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
@ -136,6 +203,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 */
|
||||
|
||||
mov r4, #(PSR_MODE_IRQ | PSR_I_BIT)
|
||||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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];
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -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];
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
Loading…
Reference in a new issue