sergiotarxz/nuttx
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

armv7-a/r: use SRS and RFE for exception handler

Browse Source
This commit is contained in:
zhangyuan21 2022-09-09 12:10:28 +08:00 committed by Masayuki Ishikawa
parent 8b8534c4b4
commit 3b889d820f
4 changed files with 162 additions and 224 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
arch/arm/include/armv7-a/irq.h
View File

@ -66,10 +66,8 @@
#define REG_R12 (12) #define REG_R12 (12)
#define REG_R13 (13) #define REG_R13 (13)
#define REG_R14 (14) #define REG_R14 (14)
#define REG_R15 (15)
#define REG_CPSR (16)
#define ARM_CONTEXT_REGS (17) #define ARM_CONTEXT_REGS (15)
/* If the MCU supports a floating point unit, then it will be necessary /* If the MCU supports a floating point unit, then it will be necessary
* to save the state of the FPU status register and data registers on * to save the state of the FPU status register and data registers on
@ -162,9 +160,12 @@
# define FPU_CONTEXT_REGS (0) # define FPU_CONTEXT_REGS (0)
#endif #endif
#define REG_R15 (ARM_CONTEXT_REGS + FPU_CONTEXT_REGS)
#define REG_CPSR (REG_R15 + 1)
/* The total number of registers saved by software */ /* The total number of registers saved by software */
#define XCPTCONTEXT_REGS (ARM_CONTEXT_REGS + FPU_CONTEXT_REGS) #define XCPTCONTEXT_REGS (REG_CPSR + 1)
#define XCPTCONTEXT_SIZE (4 * XCPTCONTEXT_REGS) #define XCPTCONTEXT_SIZE (4 * XCPTCONTEXT_REGS)
/* Friendly register names */ /* Friendly register names */

9
arch/arm/include/armv7-r/irq.h
View File

@ -66,10 +66,8 @@
#define REG_R12 (12) #define REG_R12 (12)
#define REG_R13 (13) #define REG_R13 (13)
#define REG_R14 (14) #define REG_R14 (14)
#define REG_R15 (15)
#define REG_CPSR (16)
#define ARM_CONTEXT_REGS (17) #define ARM_CONTEXT_REGS (15)
/* If the MCU supports a floating point unit, then it will be necessary /* If the MCU supports a floating point unit, then it will be necessary
* to save the state of the FPU status register and data registers on * to save the state of the FPU status register and data registers on
@ -162,9 +160,12 @@
# define FPU_CONTEXT_REGS (0) # define FPU_CONTEXT_REGS (0)
#endif #endif
#define REG_R15 (ARM_CONTEXT_REGS + FPU_CONTEXT_REGS)
#define REG_CPSR (REG_R15 + 1)
/* The total number of registers saved by software */ /* The total number of registers saved by software */
#define XCPTCONTEXT_REGS (ARM_CONTEXT_REGS + FPU_CONTEXT_REGS) #define XCPTCONTEXT_REGS (REG_CPSR + 1)
#define XCPTCONTEXT_SIZE (4 * XCPTCONTEXT_REGS) #define XCPTCONTEXT_SIZE (4 * XCPTCONTEXT_REGS)
/* Friendly register names */ /* Friendly register names */

184
arch/arm/src/armv7-a/arm_vectors.S
View File

@ -171,6 +171,12 @@
.type arm_vectorirq, %function .type arm_vectorirq, %function
arm_vectorirq: arm_vectorirq:
/* Save the LR and SPSR onto the SYS mode stack before switch. */
sub lr, lr, #4
srsdb sp, #PSR_MODE_SYS
/* Switch to SYS mode */ /* Switch to SYS mode */
#ifdef CONFIG_ARMV7A_DECODEFIQ #ifdef CONFIG_ARMV7A_DECODEFIQ
@ -186,24 +192,15 @@ arm_vectorirq:
sub sp, sp, #XCPTCONTEXT_SIZE sub sp, sp, #XCPTCONTEXT_SIZE
stmia sp, {r0-r12} /* Save the SYS mode regs */ stmia sp, {r0-r12} /* Save the SYS mode regs */
cps #PSR_MODE_IRQ /* Switch back IRQ mode */
/* Get the values for r15(pc) and CPSR in r3 and r4 */
sub r3, lr, #4
mrs r4, spsr
cps #PSR_MODE_SYS /* Then switch back to SYS mode */
/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */ /* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
add r1, sp, #XCPTCONTEXT_SIZE add r1, sp, #XCPTCONTEXT_SIZE
mov r2, r14 mov r2, r14
/* Save r13(sp), r14(lr), r15(pc), and the CPSR */ /* Save r13(sp), r14(lr) */
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */ add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4} stmia r0, {r1-r2}
#ifdef CONFIG_ARCH_FPU #ifdef CONFIG_ARCH_FPU
/* Save the state of the floating point registers. */ /* Save the state of the floating point registers. */
@ -265,18 +262,16 @@ arm_vectorirq:
* context switch is required. * context switch is required.
*/ */
/* Restore the CPSR, IRQ mode registers and return */
ldr r1, [r0, #(4*REG_CPSR)] /* Fetch the return SPSR */
msr spsr_cxsf, r1 /* Set the return mode SPSR */
/* Life is simple when everything is IRQ mode */ /* Life is simple when everything is IRQ mode */
mov r14, r0 /* (IRQ) r14=Register storage area */ mov r14, r0 /* (IRQ) r14=Register storage area */
ldmia r14!, {r0-r12} /* Restore common r0-r12 */ ldmia r14!, {r0-r12} /* Restore common r0-r12 */
ldmia r14, {r13, r14}^ /* Restore user mode r13/r14 */ ldmia r14, {r13, r14}^ /* Restore user mode r13/r14 */
add r14, r14, #(4*2) /* (IRQ) r14=address of r15 storage */
ldmia r14, {r15}^ /* Return */ /* Restore the CPSR, SYS mode registers and return. */
add r14, r14, #(4*(REG_R15-REG_R13))
rfeia r14
#if !defined(CONFIG_SMP) && CONFIG_ARCH_INTERRUPTSTACK > 7 #if !defined(CONFIG_SMP) && CONFIG_ARCH_INTERRUPTSTACK > 7
.Lirqstacktop: .Lirqstacktop:
@ -299,6 +294,11 @@ arm_vectorirq:
.type arm_vectorsvc, %function .type arm_vectorsvc, %function
arm_vectorsvc: arm_vectorsvc:
/* Save the LR and SPSR onto the SYS mode stack before switch. */
srsdb sp, #PSR_MODE_SYS
/* Switch to SYS mode */ /* Switch to SYS mode */
#ifdef CONFIG_ARMV7A_DECODEFIQ #ifdef CONFIG_ARMV7A_DECODEFIQ
@ -314,24 +314,15 @@ arm_vectorsvc:
sub sp, sp, #XCPTCONTEXT_SIZE sub sp, sp, #XCPTCONTEXT_SIZE
stmia sp, {r0-r12} /* Save the SYS mode regs */ stmia sp, {r0-r12} /* Save the SYS mode regs */
cps #PSR_MODE_SVC /* Switch back SVC mode */
/* Get the values for r15(pc) and CPSR in r3 and r4 */
mov r3, r14 /* Save r14 as the PC as well */
mrs r4, spsr /* Get the saved CPSR */
cps #PSR_MODE_SYS /* Then switch back to SYS mode */
/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */ /* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
add r1, sp, #XCPTCONTEXT_SIZE add r1, sp, #XCPTCONTEXT_SIZE
mov r2, r14 mov r2, r14
/* Save r13(sp), r14(lr), r15(pc), and the CPSR */ /* Save r13(sp), r14(lr) */
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */ add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4} stmia r0, {r1-r2}
#ifdef CONFIG_ARCH_FPU #ifdef CONFIG_ARCH_FPU
/* Save the state of the floating point registers. */ /* Save the state of the floating point registers. */
@ -395,18 +386,16 @@ arm_vectorsvc:
* context switch is required. * context switch is required.
*/ */
/* Restore the CPSR, SYS mode registers and return */
ldr r1, [r0, #(4*REG_CPSR)] /* Fetch the return SPSR */
msr spsr_cxsf, r1 /* Set the return mode SPSR */
/* Life is simple when everything is SVC mode */ /* Life is simple when everything is SVC mode */
mov r14, r0 /* (SVC) r14=Register storage area */ mov r14, r0 /* (SVC) r14=Register storage area */
ldmia r14!, {r0-r12} /* Restore common r0-r12 */ ldmia r14!, {r0-r12} /* Restore common r0-r12 */
ldmia r14, {r13, r14}^ /* Restore user mode r13/r14 */ ldmia r14, {r13, r14}^ /* Restore user mode r13/r14 */
add r14, r14, #(4*2) /* (SVC) r14=address of r15 storage */
ldmia r14, {r15}^ /* Return */ /* Restore the CPSR, SYS mode registers and return. */
add r14, r14, #(4*(REG_R15-REG_R13))
rfeia r14
.size arm_vectorsvc, . - arm_vectorsvc .size arm_vectorsvc, . - arm_vectorsvc
.align 5 .align 5
@ -427,6 +416,12 @@ arm_vectorsvc:
.type arm_vectordata, %function .type arm_vectordata, %function
arm_vectordata: arm_vectordata:
/* Save the LR and SPSR onto the SYS mode stack before switch. */
sub lr, lr, #8
srsdb sp, #PSR_MODE_SYS
/* Switch to SYS mode */ /* Switch to SYS mode */
#ifdef CONFIG_ARMV7A_DECODEFIQ #ifdef CONFIG_ARMV7A_DECODEFIQ
@ -442,24 +437,15 @@ arm_vectordata:
sub sp, sp, #XCPTCONTEXT_SIZE sub sp, sp, #XCPTCONTEXT_SIZE
stmia sp, {r0-r12} /* Save the SYS mode regs */ stmia sp, {r0-r12} /* Save the SYS mode regs */
cps #PSR_MODE_ABT /* Switch back ABT mode */
/* Get the values for r15(pc) and CPSR in r3 and r4 */
sub r3, lr, #8
mrs r4, spsr
cps #PSR_MODE_SYS /* Then switch back to SYS mode */
/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */ /* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
add r1, sp, #XCPTCONTEXT_SIZE add r1, sp, #XCPTCONTEXT_SIZE
mov r2, r14 mov r2, r14
/* Save r13(sp), r14(lr), r15(pc), and the CPSR */ /* Save r13(sp), r14(lr) */
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */ add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4} stmia r0, {r1-r2}
#ifdef CONFIG_ARCH_FPU #ifdef CONFIG_ARCH_FPU
/* Save the state of the floating point registers. */ /* Save the state of the floating point registers. */
@ -498,18 +484,16 @@ arm_vectordata:
* context switch is required. * context switch is required.
*/ */
/* Restore the CPSR, ABT mode registers and return */
ldr r1, [r0, #(4*REG_CPSR)] /* Fetch the return SPSR */
msr spsr_cxsf, r1 /* Set the return mode SPSR */
/* Life is simple when everything is ABT mode */ /* Life is simple when everything is ABT mode */
mov r14, r0 /* (ABT) r14=Register storage area */ mov r14, r0 /* (ABT) r14=Register storage area */
ldmia r14!, {r0-r12} /* Restore common r0-r12 */ ldmia r14!, {r0-r12} /* Restore common r0-r12 */
ldmia r14, {r13, r14}^ /* Restore user mode r13/r14 */ ldmia r14, {r13, r14}^ /* Restore user mode r13/r14 */
add r14, r14, #(4*2) /* (ABT) r14=address of r15 storage */
ldmia r14, {r15}^ /* Return */ /* Restore the CPSR, SYS mode registers and return. */
add r14, r14, #(4*(REG_R15-REG_R13))
rfeia r14
.size arm_vectordata, . - arm_vectordata .size arm_vectordata, . - arm_vectordata
.align 5 .align 5
@ -530,6 +514,12 @@ arm_vectordata:
.type arm_vectorprefetch, %function .type arm_vectorprefetch, %function
arm_vectorprefetch: arm_vectorprefetch:
/* Save the LR and SPSR onto the SYS mode stack before switch. */
sub lr, lr, #4
srsdb sp, #PSR_MODE_SYS
cpsid if, #PSR_MODE_SYS /* Switch to SYS mode */ cpsid if, #PSR_MODE_SYS /* Switch to SYS mode */
/* Create a context structure. First set aside a stack frame /* Create a context structure. First set aside a stack frame
@ -539,24 +529,15 @@ arm_vectorprefetch:
sub sp, sp, #XCPTCONTEXT_SIZE sub sp, sp, #XCPTCONTEXT_SIZE
stmia sp, {r0-r12} /* Save the SYS mode regs */ stmia sp, {r0-r12} /* Save the SYS mode regs */
cps #PSR_MODE_ABT /* Switch back ABT mode */
/* Get the values for r15(pc) and CPSR in r3 and r4 */
sub r3, lr, #4
mrs r4, spsr
cps #PSR_MODE_SYS /* Then switch back to SYS mode */
/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */ /* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
add r1, sp, #XCPTCONTEXT_SIZE add r1, sp, #XCPTCONTEXT_SIZE
mov r2, r14 mov r2, r14
/* Save r13(sp), r14(lr), r15(pc), and the CPSR */ /* Save r13(sp), r14(lr) */
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */ add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4} stmia r0, {r1-r2}
#ifdef CONFIG_ARCH_FPU #ifdef CONFIG_ARCH_FPU
/* Save the state of the floating point registers. */ /* Save the state of the floating point registers. */
@ -595,18 +576,16 @@ arm_vectorprefetch:
* context switch is required. * context switch is required.
*/ */
/* Restore the CPSR, ABT mode registers and return */
ldr r1, [r0, #(4*REG_CPSR)] /* Fetch the return SPSR */
msr spsr_cxsf, r1 /* Set the return mode SPSR */
/* Life is simple when everything is ABT mode */ /* Life is simple when everything is ABT mode */
mov r14, r0 /* (ABT) r14=Register storage area */ mov r14, r0 /* (ABT) r14=Register storage area */
ldmia r14!, {r0-r12} /* Restore common r0-r12 */ ldmia r14!, {r0-r12} /* Restore common r0-r12 */
ldmia r14, {r13, r14}^ /* Restore user mode r13/r14 */ ldmia r14, {r13, r14}^ /* Restore user mode r13/r14 */
add r14, r14, #(4*2) /* (ABT) r14=address of r15 storage */
ldmia r14, {r15}^ /* Return */ /* Restore the CPSR, SYS mode registers and return. */
add r14, r14, #(4*(REG_R15-REG_R13))
rfeia r14
.size arm_vectorprefetch, . - arm_vectorprefetch .size arm_vectorprefetch, . - arm_vectorprefetch
.align 5 .align 5
@ -625,6 +604,11 @@ arm_vectorprefetch:
.type arm_vectorundefinsn, %function .type arm_vectorundefinsn, %function
arm_vectorundefinsn: arm_vectorundefinsn:
/* Save the LR and SPSR onto the SYS mode stack before switch. */
srsdb sp, #PSR_MODE_SYS
cpsid if, #PSR_MODE_SYS /* Switch to SYS mode */ cpsid if, #PSR_MODE_SYS /* Switch to SYS mode */
/* Create a context structure. First set aside a stack frame /* Create a context structure. First set aside a stack frame
@ -634,24 +618,15 @@ arm_vectorundefinsn:
sub sp, sp, #XCPTCONTEXT_SIZE sub sp, sp, #XCPTCONTEXT_SIZE
stmia sp, {r0-r12} /* Save the SYS mode regs */ stmia sp, {r0-r12} /* Save the SYS mode regs */
cps #PSR_MODE_UND /* Switch back UND mode */
/* Get the values for r15(pc) and CPSR in r3 and r4 */
mov r3, lr
mrs r4, spsr
cps #PSR_MODE_SYS /* Then switch back to SYS mode */
/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */ /* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
add r1, sp, #XCPTCONTEXT_SIZE add r1, sp, #XCPTCONTEXT_SIZE
mov r2, r14 mov r2, r14
/* Save r13(sp), r14(lr), r15(pc), and the CPSR */ /* Save r13(sp), r14(lr) */
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */ add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4} stmia r0, {r1-r2}
#ifdef CONFIG_ARCH_FPU #ifdef CONFIG_ARCH_FPU
/* Save the state of the floating point registers. */ /* Save the state of the floating point registers. */
@ -688,18 +663,16 @@ arm_vectorundefinsn:
* context switch is required. * context switch is required.
*/ */
/* Restore the CPSR, UND mode registers and return */
ldr r1, [r0, #(4*REG_CPSR)] /* Fetch the return SPSR */
msr spsr_cxsf, r1 /* Set the return mode SPSR */
/* Life is simple when everything is UND mode */ /* Life is simple when everything is UND mode */
mov r14, r0 /* (UND) r14=Register storage area */ mov r14, r0 /* (UND) r14=Register storage area */
ldmia r14!, {r0-r12} /* Restore common r0-r12 */ ldmia r14!, {r0-r12} /* Restore common r0-r12 */
ldmia r14, {r13, r14}^ /* Restore user mode r13/r14 */ ldmia r14, {r13, r14}^ /* Restore user mode r13/r14 */
add r14, r14, #(4*2) /* (UND) r14=address of r15 storage */
ldmia r14, {r15}^ /* Return */ /* Restore the CPSR, SYS mode registers and return. */
add r14, r14, #(4*(REG_R15-REG_R13))
rfeia r14
.size arm_vectorundefinsn, . - arm_vectorundefinsn .size arm_vectorundefinsn, . - arm_vectorundefinsn
.align 5 .align 5
@ -721,6 +694,12 @@ arm_vectorundefinsn:
arm_vectorfiq: arm_vectorfiq:
#ifdef CONFIG_ARMV7A_DECODEFIQ #ifdef CONFIG_ARMV7A_DECODEFIQ
/* Save the LR and SPSR onto the SYS mode stack before switch. */
sub lr, lr, #4
srsdb sp, #PSR_MODE_SYS
cpsid if, #PSR_MODE_SYS /* Switch to SYS mode */ cpsid if, #PSR_MODE_SYS /* Switch to SYS mode */
/* Create a context structure. First set aside a stack frame /* Create a context structure. First set aside a stack frame
@ -730,24 +709,15 @@ arm_vectorfiq:
sub sp, sp, #XCPTCONTEXT_SIZE sub sp, sp, #XCPTCONTEXT_SIZE
stmia sp, {r0-r12} /* Save the SYS mode regs */ stmia sp, {r0-r12} /* Save the SYS mode regs */
cps #PSR_MODE_FIQ /* Switch back FIQ mode */
/* Get the values for r15(pc) and CPSR in r3 and r4 */
sub r3, lr, #4
mrs r4, spsr
cps #PSR_MODE_SYS /* Then switch back to SYS mode */
/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */ /* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
add r1, sp, #XCPTCONTEXT_SIZE add r1, sp, #XCPTCONTEXT_SIZE
mov r2, r14 mov r2, r14
/* Save r13(sp), r14(lr), r15(pc), and the CPSR */ /* Save r13(sp), r14(lr) */
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */ add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4} stmia r0, {r1-r2}
#ifdef CONFIG_ARCH_FPU #ifdef CONFIG_ARCH_FPU
/* Save the state of the floating point registers. */ /* Save the state of the floating point registers. */
@ -796,18 +766,16 @@ arm_vectorfiq:
* context switch is required. * context switch is required.
*/ */
/* Restore the CPSR, FIQ mode registers and return */
ldr r1, [r0, #(4*REG_CPSR)] /* Fetch the return SPSR */
msr spsr_cxsf, r1 /* Set the return mode SPSR */
/* Life is simple when everything is FIQ mode */ /* Life is simple when everything is FIQ mode */
mov r14, r0 /* (FIQ) r14=Register storage area */ mov r14, r0 /* (FIQ) r14=Register storage area */
ldmia r14!, {r0-r7} /* Restore common r0-r7 */ ldmia r14!, {r0-r7} /* Restore common r0-r7 */
ldmia r14, {r8-r14}^ /* Restore user mode r8-r14 */ ldmia r14, {r8-r14}^ /* Restore user mode r8-r14 */
add r14, r14, #(4*7) /* (FIQ) r14=address of r15 storage */
ldmia r14, {r15}^ /* Return */ /* Restore the CPSR, SYS mode registers and return. */
add r14, r14, #(4*(REG_R15-REG_R13))
rfeia r14
#if !defined(CONFIG_SMP) && CONFIG_ARCH_INTERRUPTSTACK > 7 #if !defined(CONFIG_SMP) && CONFIG_ARCH_INTERRUPTSTACK > 7
.Lfiqstacktop: .Lfiqstacktop:

184
arch/arm/src/armv7-r/arm_vectors.S
View File

@ -125,6 +125,12 @@
.type arm_vectorirq, %function .type arm_vectorirq, %function
arm_vectorirq: arm_vectorirq:
/* Save the LR and SPSR onto the SYS mode stack before switch. */
sub lr, lr, #4
srsdb sp, #PSR_MODE_SYS
/* Switch to SYS mode */ /* Switch to SYS mode */
#ifdef CONFIG_ARMV7A_DECODEFIQ #ifdef CONFIG_ARMV7A_DECODEFIQ
@ -140,24 +146,15 @@ arm_vectorirq:
sub sp, sp, #XCPTCONTEXT_SIZE sub sp, sp, #XCPTCONTEXT_SIZE
stmia sp, {r0-r12} /* Save the SYS mode regs */ stmia sp, {r0-r12} /* Save the SYS mode regs */
cps #PSR_MODE_IRQ /* Switch back IRQ mode */
/* Get the values for r15(pc) and CPSR in r3 and r4 */
sub r3, lr, #4
mrs r4, spsr
cps #PSR_MODE_SYS /* Then switch back to SYS mode */
/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */ /* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
add r1, sp, #XCPTCONTEXT_SIZE add r1, sp, #XCPTCONTEXT_SIZE
mov r2, r14 mov r2, r14
/* Save r13(sp), r14(lr), r15(pc), and the CPSR */ /* Save r13(sp), r14(lr) */
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */ add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4} stmia r0, {r1-r2}
#ifdef CONFIG_ARCH_FPU #ifdef CONFIG_ARCH_FPU
/* Save the state of the floating point registers. */ /* Save the state of the floating point registers. */
@ -219,18 +216,16 @@ arm_vectorirq:
* context switch is required. * context switch is required.
*/ */
/* Restore the CPSR, IRQ mode registers and return */
ldr r1, [r0, #(4*REG_CPSR)] /* Fetch the return SPSR */
msr spsr_cxsf, r1 /* Set the return mode SPSR */
/* Life is simple when everything is IRQ mode */ /* Life is simple when everything is IRQ mode */
mov r14, r0 /* (IRQ) r14=Register storage area */ mov r14, r0 /* (IRQ) r14=Register storage area */
ldmia r14!, {r0-r12} /* Restore common r0-r12 */ ldmia r14!, {r0-r12} /* Restore common r0-r12 */
ldmia r14, {r13, r14}^ /* Restore user mode r13/r14 */ ldmia r14, {r13, r14}^ /* Restore user mode r13/r14 */
add r14, r14, #(4*2) /* (IRQ) r14=address of r15 storage */
ldmia r14, {r15}^ /* Return */ /* Restore the CPSR, SYS mode registers and return. */
add r14, r14, #(4*(REG_R15-REG_R13))
rfeia r14
#if CONFIG_ARCH_INTERRUPTSTACK > 7 #if CONFIG_ARCH_INTERRUPTSTACK > 7
.Lirqstacktop: .Lirqstacktop:
@ -253,6 +248,11 @@ arm_vectorirq:
.type arm_vectorsvc, %function .type arm_vectorsvc, %function
arm_vectorsvc: arm_vectorsvc:
/* Save the LR and SPSR onto the SYS mode stack before switch. */
srsdb sp, #PSR_MODE_SYS
/* Switch to SYS mode */ /* Switch to SYS mode */
#ifdef CONFIG_ARMV7A_DECODEFIQ #ifdef CONFIG_ARMV7A_DECODEFIQ
@ -268,24 +268,15 @@ arm_vectorsvc:
sub sp, sp, #XCPTCONTEXT_SIZE sub sp, sp, #XCPTCONTEXT_SIZE
stmia sp, {r0-r12} /* Save the SYS mode regs */ stmia sp, {r0-r12} /* Save the SYS mode regs */
cps #PSR_MODE_SVC /* Switch back SVC mode */
/* Get the values for r15(pc) and CPSR in r3 and r4 */
mov r3, r14 /* Save r14 as the PC as well */
mrs r4, spsr /* Get the saved CPSR */
cps #PSR_MODE_SYS /* Then switch back to SYS mode */
/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */ /* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
add r1, sp, #XCPTCONTEXT_SIZE add r1, sp, #XCPTCONTEXT_SIZE
mov r2, r14 mov r2, r14
/* Save r13(sp), r14(lr), r15(pc), and the CPSR */ /* Save r13(sp), r14(lr) */
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */ add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4} stmia r0, {r1-r2}
#ifdef CONFIG_ARCH_FPU #ifdef CONFIG_ARCH_FPU
/* Save the state of the floating point registers. */ /* Save the state of the floating point registers. */
@ -349,18 +340,16 @@ arm_vectorsvc:
* context switch is required. * context switch is required.
*/ */
/* Restore the CPSR, SYS mode registers and return */
ldr r1, [r0, #(4*REG_CPSR)] /* Fetch the return SPSR */
msr spsr_cxsf, r1 /* Set the return mode SPSR */
/* Life is simple when everything is SVC mode */ /* Life is simple when everything is SVC mode */
mov r14, r0 /* (SVC) r14=Register storage area */ mov r14, r0 /* (SVC) r14=Register storage area */
ldmia r14!, {r0-r12} /* Restore common r0-r12 */ ldmia r14!, {r0-r12} /* Restore common r0-r12 */
ldmia r14, {r13, r14}^ /* Restore user mode r13/r14 */ ldmia r14, {r13, r14}^ /* Restore user mode r13/r14 */
add r14, r14, #(4*2) /* (SVC) r14=address of r15 storage */
ldmia r14, {r15}^ /* Return */ /* Restore the CPSR, SYS mode registers and return. */
add r14, r14, #(4*(REG_R15-REG_R13))
rfeia r14
.size arm_vectorsvc, . - arm_vectorsvc .size arm_vectorsvc, . - arm_vectorsvc
.align 5 .align 5
@ -381,6 +370,12 @@ arm_vectorsvc:
.type arm_vectordata, %function .type arm_vectordata, %function
arm_vectordata: arm_vectordata:
/* Save the LR and SPSR onto the SYS mode stack before switch. */
sub lr, lr, #8
srsdb sp, #PSR_MODE_SYS
/* Switch to SYS mode */ /* Switch to SYS mode */
#ifdef CONFIG_ARMV7A_DECODEFIQ #ifdef CONFIG_ARMV7A_DECODEFIQ
@ -396,24 +391,15 @@ arm_vectordata:
sub sp, sp, #XCPTCONTEXT_SIZE sub sp, sp, #XCPTCONTEXT_SIZE
stmia sp, {r0-r12} /* Save the SYS mode regs */ stmia sp, {r0-r12} /* Save the SYS mode regs */
cps #PSR_MODE_ABT /* Switch back ABT mode */
/* Get the values for r15(pc) and CPSR in r3 and r4 */
sub r3, lr, #8
mrs r4, spsr
cps #PSR_MODE_SYS /* Then switch back to SYS mode */
/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */ /* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
add r1, sp, #XCPTCONTEXT_SIZE add r1, sp, #XCPTCONTEXT_SIZE
mov r2, r14 mov r2, r14
/* Save r13(sp), r14(lr), r15(pc), and the CPSR */ /* Save r13(sp), r14(lr) */
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */ add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4} stmia r0, {r1-r2}
#ifdef CONFIG_ARCH_FPU #ifdef CONFIG_ARCH_FPU
/* Save the state of the floating point registers. */ /* Save the state of the floating point registers. */
@ -452,18 +438,16 @@ arm_vectordata:
* context switch is required. * context switch is required.
*/ */
/* Restore the CPSR, ABT mode registers and return */
ldr r1, [r0, #(4*REG_CPSR)] /* Fetch the return SPSR */
msr spsr_cxsf, r1 /* Set the return mode SPSR */
/* Life is simple when everything is ABT mode */ /* Life is simple when everything is ABT mode */
mov r14, r0 /* (ABT) r14=Register storage area */ mov r14, r0 /* (ABT) r14=Register storage area */
ldmia r14!, {r0-r12} /* Restore common r0-r12 */ ldmia r14!, {r0-r12} /* Restore common r0-r12 */
ldmia r14, {r13, r14}^ /* Restore user mode r13/r14 */ ldmia r14, {r13, r14}^ /* Restore user mode r13/r14 */
add r14, r14, #(4*2) /* (ABT) r14=address of r15 storage */
ldmia r14, {r15}^ /* Return */ /* Restore the CPSR, SYS mode registers and return. */
add r14, r14, #(4*(REG_R15-REG_R13))
rfeia r14
.size arm_vectordata, . - arm_vectordata .size arm_vectordata, . - arm_vectordata
.align 5 .align 5
@ -484,6 +468,12 @@ arm_vectordata:
.type arm_vectorprefetch, %function .type arm_vectorprefetch, %function
arm_vectorprefetch: arm_vectorprefetch:
/* Save the LR and SPSR onto the SYS mode stack before switch. */
sub lr, lr, #4
srsdb sp, #PSR_MODE_SYS
cpsid if, #PSR_MODE_SYS /* Switch to SYS mode */ cpsid if, #PSR_MODE_SYS /* Switch to SYS mode */
/* Create a context structure. First set aside a stack frame /* Create a context structure. First set aside a stack frame
@ -493,24 +483,15 @@ arm_vectorprefetch:
sub sp, sp, #XCPTCONTEXT_SIZE sub sp, sp, #XCPTCONTEXT_SIZE
stmia sp, {r0-r12} /* Save the SYS mode regs */ stmia sp, {r0-r12} /* Save the SYS mode regs */
cps #PSR_MODE_ABT /* Switch back ABT mode */
/* Get the values for r15(pc) and CPSR in r3 and r4 */
sub r3, lr, #4
mrs r4, spsr
cps #PSR_MODE_SYS /* Then switch back to SYS mode */
/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */ /* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
add r1, sp, #XCPTCONTEXT_SIZE add r1, sp, #XCPTCONTEXT_SIZE
mov r2, r14 mov r2, r14
/* Save r13(sp), r14(lr), r15(pc), and the CPSR */ /* Save r13(sp), r14(lr) */
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */ add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4} stmia r0, {r1-r2}
#ifdef CONFIG_ARCH_FPU #ifdef CONFIG_ARCH_FPU
/* Save the state of the floating point registers. */ /* Save the state of the floating point registers. */
@ -549,18 +530,16 @@ arm_vectorprefetch:
* context switch is required. * context switch is required.
*/ */
/* Restore the CPSR, ABT mode registers and return */
ldr r1, [r0, #(4*REG_CPSR)] /* Fetch the return SPSR */
msr spsr_cxsf, r1 /* Set the return mode SPSR */
/* Life is simple when everything is ABT mode */ /* Life is simple when everything is ABT mode */
mov r14, r0 /* (ABT) r14=Register storage area */ mov r14, r0 /* (ABT) r14=Register storage area */
ldmia r14!, {r0-r12} /* Restore common r0-r12 */ ldmia r14!, {r0-r12} /* Restore common r0-r12 */
ldmia r14, {r13, r14}^ /* Restore user mode r13/r14 */ ldmia r14, {r13, r14}^ /* Restore user mode r13/r14 */
add r14, r14, #(4*2) /* (ABT) r14=address of r15 storage */
ldmia r14, {r15}^ /* Return */ /* Restore the CPSR, SYS mode registers and return. */
add r14, r14, #(4*(REG_R15-REG_R13))
rfeia r14
.size arm_vectorprefetch, . - arm_vectorprefetch .size arm_vectorprefetch, . - arm_vectorprefetch
.align 5 .align 5
@ -579,6 +558,11 @@ arm_vectorprefetch:
.type arm_vectorundefinsn, %function .type arm_vectorundefinsn, %function
arm_vectorundefinsn: arm_vectorundefinsn:
/* Save the LR and SPSR onto the SYS mode stack before switch. */
srsdb sp, #PSR_MODE_SYS
cpsid if, #PSR_MODE_SYS /* Switch to SYS mode */ cpsid if, #PSR_MODE_SYS /* Switch to SYS mode */
/* Create a context structure. First set aside a stack frame /* Create a context structure. First set aside a stack frame
@ -588,24 +572,15 @@ arm_vectorundefinsn:
sub sp, sp, #XCPTCONTEXT_SIZE sub sp, sp, #XCPTCONTEXT_SIZE
stmia sp, {r0-r12} /* Save the SYS mode regs */ stmia sp, {r0-r12} /* Save the SYS mode regs */
cps #PSR_MODE_UND /* Switch back UND mode */
/* Get the values for r15(pc) and CPSR in r3 and r4 */
mov r3, lr
mrs r4, spsr
cps #PSR_MODE_SYS /* Then switch back to SYS mode */
/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */ /* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
add r1, sp, #XCPTCONTEXT_SIZE add r1, sp, #XCPTCONTEXT_SIZE
mov r2, r14 mov r2, r14
/* Save r13(sp), r14(lr), r15(pc), and the CPSR */ /* Save r13(sp), r14(lr) */
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */ add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4} stmia r0, {r1-r2}
#ifdef CONFIG_ARCH_FPU #ifdef CONFIG_ARCH_FPU
/* Save the state of the floating point registers. */ /* Save the state of the floating point registers. */
@ -642,18 +617,16 @@ arm_vectorundefinsn:
* context switch is required. * context switch is required.
*/ */
/* Restore the CPSR, UND mode registers and return */
ldr r1, [r0, #(4*REG_CPSR)] /* Fetch the return SPSR */
msr spsr_cxsf, r1 /* Set the return mode SPSR */
/* Life is simple when everything is UND mode */ /* Life is simple when everything is UND mode */
mov r14, r0 /* (UND) r14=Register storage area */ mov r14, r0 /* (UND) r14=Register storage area */
ldmia r14!, {r0-r12} /* Restore common r0-r12 */ ldmia r14!, {r0-r12} /* Restore common r0-r12 */
ldmia r14, {r13, r14}^ /* Restore user mode r13/r14 */ ldmia r14, {r13, r14}^ /* Restore user mode r13/r14 */
add r14, r14, #(4*2) /* (UND) r14=address of r15 storage */
ldmia r14, {r15}^ /* Return */ /* Restore the CPSR, SYS mode registers and return. */
add r14, r14, #(4*(REG_R15-REG_R13))
rfeia r14
.size arm_vectorundefinsn, . - arm_vectorundefinsn .size arm_vectorundefinsn, . - arm_vectorundefinsn
.align 5 .align 5
@ -675,6 +648,12 @@ arm_vectorundefinsn:
arm_vectorfiq: arm_vectorfiq:
#ifdef CONFIG_ARMV7R_DECODEFIQ #ifdef CONFIG_ARMV7R_DECODEFIQ
/* Save the LR and SPSR onto the SYS mode stack before switch. */
sub lr, lr, #4
srsdb sp, #PSR_MODE_SYS
cpsid if, #PSR_MODE_SYS /* Switch to SYS mode */ cpsid if, #PSR_MODE_SYS /* Switch to SYS mode */
/* Create a context structure. First set aside a stack frame /* Create a context structure. First set aside a stack frame
@ -684,24 +663,15 @@ arm_vectorfiq:
sub sp, sp, #XCPTCONTEXT_SIZE sub sp, sp, #XCPTCONTEXT_SIZE
stmia sp, {r0-r12} /* Save the SYS mode regs */ stmia sp, {r0-r12} /* Save the SYS mode regs */
cps #PSR_MODE_FIQ /* Switch back FIQ mode */
/* Get the values for r15(pc) and CPSR in r3 and r4 */
sub r3, lr, #4
mrs r4, spsr
cps #PSR_MODE_SYS /* Then switch back to SYS mode */
/* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */ /* Get the correct values of USR/SYS r13(sp) and r14(lr) in r1 and r2 */
add r1, sp, #XCPTCONTEXT_SIZE add r1, sp, #XCPTCONTEXT_SIZE
mov r2, r14 mov r2, r14
/* Save r13(sp), r14(lr), r15(pc), and the CPSR */ /* Save r13(sp), r14(lr) */
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */ add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4} stmia r0, {r1-r2}
#ifdef CONFIG_ARCH_FPU #ifdef CONFIG_ARCH_FPU
/* Save the state of the floating point registers. */ /* Save the state of the floating point registers. */
@ -750,18 +720,16 @@ arm_vectorfiq:
* context switch is required. * context switch is required.
*/ */
/* Restore the CPSR, FIQ mode registers and return */
ldr r1, [r0, #(4*REG_CPSR)] /* Fetch the return SPSR */
msr spsr_cxsf, r1 /* Set the return mode SPSR */
/* Life is simple when everything is FIQ mode */ /* Life is simple when everything is FIQ mode */
mov r14, r0 /* (FIQ) r14=Register storage area */ mov r14, r0 /* (FIQ) r14=Register storage area */
ldmia r14!, {r0-r7} /* Restore common r0-r7 */ ldmia r14!, {r0-r7} /* Restore common r0-r7 */
ldmia r14, {r8-r14}^ /* Restore user mode r8-r14 */ ldmia r14, {r8-r14}^ /* Restore user mode r8-r14 */
add r14, r14, #(4*7) /* (FIQ) r14=address of r15 storage */
ldmia r14, {r15}^ /* Return */ /* Restore the CPSR, SYS mode registers and return. */
add r14, r14, #(4*(REG_R15-REG_R13))
rfeia r14
#if CONFIG_ARCH_INTERRUPTSTACK > 7 #if CONFIG_ARCH_INTERRUPTSTACK > 7
.Lfiqstacktop: .Lfiqstacktop: