diff options
Diffstat (limited to 'ArmPkg')
| -rw-r--r-- | ArmPkg/Library/ArmExceptionLib/AArch64/ExceptionSupport.S | 235 |
1 files changed, 109 insertions, 126 deletions
diff --git a/ArmPkg/Library/ArmExceptionLib/AArch64/ExceptionSupport.S b/ArmPkg/Library/ArmExceptionLib/AArch64/ExceptionSupport.S index 0fd304db2d..ff1f5fc813 100644 --- a/ArmPkg/Library/ArmExceptionLib/AArch64/ExceptionSupport.S +++ b/ArmPkg/Library/ArmExceptionLib/AArch64/ExceptionSupport.S @@ -107,54 +107,6 @@ GCC_ASM_EXPORT(CommonCExceptionHandler) #define FP_CONTEXT_SIZE (32 * 16)
#define SYS_CONTEXT_SIZE ( 6 * 8) // 5 SYS regs + Alignment requirement (ie: the stack must be aligned on 0x10)
-// Cannot str x31 directly
-#define ALL_GP_REGS \
- REG_PAIR (x0, x1, 0x000, GP_CONTEXT_SIZE); \
- REG_PAIR (x2, x3, 0x010, GP_CONTEXT_SIZE); \
- REG_PAIR (x4, x5, 0x020, GP_CONTEXT_SIZE); \
- REG_PAIR (x6, x7, 0x030, GP_CONTEXT_SIZE); \
- REG_PAIR (x8, x9, 0x040, GP_CONTEXT_SIZE); \
- REG_PAIR (x10, x11, 0x050, GP_CONTEXT_SIZE); \
- REG_PAIR (x12, x13, 0x060, GP_CONTEXT_SIZE); \
- REG_PAIR (x14, x15, 0x070, GP_CONTEXT_SIZE); \
- REG_PAIR (x16, x17, 0x080, GP_CONTEXT_SIZE); \
- REG_PAIR (x18, x19, 0x090, GP_CONTEXT_SIZE); \
- REG_PAIR (x20, x21, 0x0a0, GP_CONTEXT_SIZE); \
- REG_PAIR (x22, x23, 0x0b0, GP_CONTEXT_SIZE); \
- REG_PAIR (x24, x25, 0x0c0, GP_CONTEXT_SIZE); \
- REG_PAIR (x26, x27, 0x0d0, GP_CONTEXT_SIZE); \
- REG_PAIR (x28, x29, 0x0e0, GP_CONTEXT_SIZE); \
- REG_ONE (x30, 0x0f0, GP_CONTEXT_SIZE);
-
-// In order to save the SP we need to put it somewhere else first.
-// STR only works with XZR/WZR directly
-#define SAVE_SP \
- add x1, sp, #(FP_CONTEXT_SIZE + SYS_CONTEXT_SIZE); \
- REG_ONE (x1, 0x0f8, GP_CONTEXT_SIZE);
-
-#define ALL_FP_REGS \
- REG_PAIR (q0, q1, 0x000, FP_CONTEXT_SIZE); \
- REG_PAIR (q2, q3, 0x020, FP_CONTEXT_SIZE); \
- REG_PAIR (q4, q5, 0x040, FP_CONTEXT_SIZE); \
- REG_PAIR (q6, q7, 0x060, FP_CONTEXT_SIZE); \
- REG_PAIR (q8, q9, 0x080, FP_CONTEXT_SIZE); \
- REG_PAIR (q10, q11, 0x0a0, FP_CONTEXT_SIZE); \
- REG_PAIR (q12, q13, 0x0c0, FP_CONTEXT_SIZE); \
- REG_PAIR (q14, q15, 0x0e0, FP_CONTEXT_SIZE); \
- REG_PAIR (q16, q17, 0x100, FP_CONTEXT_SIZE); \
- REG_PAIR (q18, q19, 0x120, FP_CONTEXT_SIZE); \
- REG_PAIR (q20, q21, 0x140, FP_CONTEXT_SIZE); \
- REG_PAIR (q22, q23, 0x160, FP_CONTEXT_SIZE); \
- REG_PAIR (q24, q25, 0x180, FP_CONTEXT_SIZE); \
- REG_PAIR (q26, q27, 0x1a0, FP_CONTEXT_SIZE); \
- REG_PAIR (q28, q29, 0x1c0, FP_CONTEXT_SIZE); \
- REG_PAIR (q30, q31, 0x1e0, FP_CONTEXT_SIZE);
-
-#define ALL_SYS_REGS \
- REG_PAIR (x1, x2, 0x000, SYS_CONTEXT_SIZE); \
- REG_PAIR (x3, x4, 0x010, SYS_CONTEXT_SIZE); \
- REG_ONE (x5, 0x020, SYS_CONTEXT_SIZE);
-
//
// There are two methods for installing AArch64 exception vectors:
// 1. Install a copy of the vectors to a location specified by a PCD
@@ -170,18 +122,35 @@ ASM_PFX(ExceptionHandlersStart): VECTOR_BASE(ExceptionHandlersStart)
#endif
-#undef REG_PAIR
-#undef REG_ONE
-#define REG_PAIR(REG1, REG2, OFFSET, CONTEXT_SIZE) stp REG1, REG2, [sp, #(OFFSET-CONTEXT_SIZE)]
-#define REG_ONE(REG1, OFFSET, CONTEXT_SIZE) stur REG1, [sp, #(OFFSET-CONTEXT_SIZE)]
-
.macro ExceptionEntry, val
// Move the stackpointer so we can reach our structure with the str instruction.
sub sp, sp, #(FP_CONTEXT_SIZE + SYS_CONTEXT_SIZE)
- // Save all the General regs before touching x0 and x1.
- // This does not save r31(SP) as it is special. We do that later.
- ALL_GP_REGS
+ // Push some GP registers so we can record the exception context
+ stp x0, x1, [sp, #-GP_CONTEXT_SIZE]!
+ stp x2, x3, [sp, #0x10]
+ stp x4, x5, [sp, #0x20]
+ stp x6, x7, [sp, #0x30]
+
+ EL1_OR_EL2_OR_EL3(x1)
+1:mrs x2, elr_el1 // Exception Link Register
+ mrs x3, spsr_el1 // Saved Processor Status Register 32bit
+ mrs x5, esr_el1 // EL1 Exception syndrome register 32bit
+ mrs x6, far_el1 // EL1 Fault Address Register
+ b 4f
+
+2:mrs x2, elr_el2 // Exception Link Register
+ mrs x3, spsr_el2 // Saved Processor Status Register 32bit
+ mrs x5, esr_el2 // EL2 Exception syndrome register 32bit
+ mrs x6, far_el2 // EL2 Fault Address Register
+ b 4f
+
+3:mrs x2, elr_el3 // Exception Link Register
+ mrs x3, spsr_el3 // Saved Processor Status Register 32bit
+ mrs x5, esr_el3 // EL3 Exception syndrome register 32bit
+ mrs x6, far_el3 // EL3 Fault Address Register
+
+4:mrs x4, fpsr // Floating point Status Register 32bit
// Record the type of exception that occurred.
mov x0, #\val
@@ -278,49 +247,44 @@ ASM_PFX(ExceptionHandlersEnd): ASM_PFX(CommonExceptionEntry):
- /* NOTE:
- We have to break up the save code because the immediate value to be used
- with the SP is too big to do it all in one step so we need to shuffle the SP
- along as we go. (we only have 9bits of immediate to work with) */
-
- // Save the current Stack pointer before we start modifying it.
- SAVE_SP
-
- // Preserve the stack pointer we came in with before we modify it
- EL1_OR_EL2_OR_EL3(x1)
-1:mrs x1, elr_el1 // Exception Link Register
- mrs x2, spsr_el1 // Saved Processor Status Register 32bit
- mrs x4, esr_el1 // EL1 Exception syndrome register 32bit
- mrs x5, far_el1 // EL1 Fault Address Register
- b 4f
-2:mrs x1, elr_el2 // Exception Link Register
- mrs x2, spsr_el2 // Saved Processor Status Register 32bit
- mrs x4, esr_el2 // EL2 Exception syndrome register 32bit
- mrs x5, far_el2 // EL2 Fault Address Register
- b 4f
-
-3:mrs x1, elr_el3 // Exception Link Register
- mrs x2, spsr_el3 // Saved Processor Status Register 32bit
- mrs x4, esr_el3 // EL3 Exception syndrome register 32bit
- mrs x5, far_el3 // EL3 Fault Address Register
-
-4:mrs x3, fpsr // Floating point Status Register 32bit
-
- // Adjust SP to save next set
- add sp, sp, #FP_CONTEXT_SIZE
-
- // Push FP regs to Stack.
- ALL_FP_REGS
-
- // Adjust SP to save next set
- add sp, sp, #SYS_CONTEXT_SIZE
+ // Stack the remaining GP registers
+ stp x8, x9, [sp, #0x40]
+ stp x10, x11, [sp, #0x50]
+ stp x12, x13, [sp, #0x60]
+ stp x14, x15, [sp, #0x70]
+ stp x16, x17, [sp, #0x80]
+ stp x18, x19, [sp, #0x90]
+ stp x20, x21, [sp, #0xa0]
+ stp x22, x23, [sp, #0xb0]
+ stp x24, x25, [sp, #0xc0]
+ stp x26, x27, [sp, #0xd0]
+ stp x28, x29, [sp, #0xe0]
+ add x28, sp, #GP_CONTEXT_SIZE + FP_CONTEXT_SIZE + SYS_CONTEXT_SIZE
+ stp x30, x28, [sp, #0xf0]
// Save the SYS regs
- ALL_SYS_REGS
+ stp x2, x3, [x28, #-SYS_CONTEXT_SIZE]!
+ stp x4, x5, [x28, #0x10]
+ str x6, [x28, #0x20]
- // Point to top of struct after all regs saved
- sub sp, sp, #(GP_CONTEXT_SIZE + FP_CONTEXT_SIZE + SYS_CONTEXT_SIZE)
+ // Push FP regs to Stack.
+ stp q0, q1, [x28, #-FP_CONTEXT_SIZE]!
+ stp q2, q3, [x28, #0x20]
+ stp q4, q5, [x28, #0x40]
+ stp q6, q7, [x28, #0x60]
+ stp q8, q9, [x28, #0x80]
+ stp q10, q11, [x28, #0xa0]
+ stp q12, q13, [x28, #0xc0]
+ stp q14, q15, [x28, #0xe0]
+ stp q16, q17, [x28, #0x100]
+ stp q18, q19, [x28, #0x120]
+ stp q20, q21, [x28, #0x140]
+ stp q22, q23, [x28, #0x160]
+ stp q24, q25, [x28, #0x180]
+ stp q26, q27, [x28, #0x1a0]
+ stp q28, q29, [x28, #0x1c0]
+ stp q30, q31, [x28, #0x1e0]
// x0 still holds the exception type.
// Set x1 to point to the top of our struct on the Stack
@@ -337,13 +301,44 @@ ASM_PFX(CommonExceptionEntry): // We do not try to recover.
bl ASM_PFX(CommonCExceptionHandler) // Call exception handler
-
-// Defines for popping from stack
-
-#undef REG_PAIR
-#undef REG_ONE
-#define REG_PAIR(REG1, REG2, OFFSET, CONTEXT_SIZE) ldp REG1, REG2, [sp, #(OFFSET-CONTEXT_SIZE)]
-#define REG_ONE(REG1, OFFSET, CONTEXT_SIZE) ldur REG1, [sp, #(OFFSET-CONTEXT_SIZE)]
+ // Pop as many GP regs as we can before entering the critical section below
+ ldp x2, x3, [sp, #0x10]
+ ldp x4, x5, [sp, #0x20]
+ ldp x6, x7, [sp, #0x30]
+ ldp x8, x9, [sp, #0x40]
+ ldp x10, x11, [sp, #0x50]
+ ldp x12, x13, [sp, #0x60]
+ ldp x14, x15, [sp, #0x70]
+ ldp x16, x17, [sp, #0x80]
+ ldp x18, x19, [sp, #0x90]
+ ldp x20, x21, [sp, #0xa0]
+ ldp x22, x23, [sp, #0xb0]
+ ldp x24, x25, [sp, #0xc0]
+ ldp x26, x27, [sp, #0xd0]
+ ldp x0, x1, [sp], #0xe0
+
+ // Pop FP regs from Stack.
+ ldp q2, q3, [x28, #0x20]
+ ldp q4, q5, [x28, #0x40]
+ ldp q6, q7, [x28, #0x60]
+ ldp q8, q9, [x28, #0x80]
+ ldp q10, q11, [x28, #0xa0]
+ ldp q12, q13, [x28, #0xc0]
+ ldp q14, q15, [x28, #0xe0]
+ ldp q16, q17, [x28, #0x100]
+ ldp q18, q19, [x28, #0x120]
+ ldp q20, q21, [x28, #0x140]
+ ldp q22, q23, [x28, #0x160]
+ ldp q24, q25, [x28, #0x180]
+ ldp q26, q27, [x28, #0x1a0]
+ ldp q28, q29, [x28, #0x1c0]
+ ldp q30, q31, [x28, #0x1e0]
+ ldp q0, q1, [x28], #FP_CONTEXT_SIZE
+
+ // Pop the SYS regs we need
+ ldp x29, x30, [x28]
+ ldr x28, [x28, #0x10]
+ msr fpsr, x28
//
// Disable interrupt(IRQ and FIQ) before restoring context,
@@ -353,35 +348,23 @@ ASM_PFX(CommonExceptionEntry): msr daifset, #3
isb
- // Adjust SP to pop system registers
- add sp, sp, #(GP_CONTEXT_SIZE + FP_CONTEXT_SIZE + SYS_CONTEXT_SIZE)
- ALL_SYS_REGS
-
- EL1_OR_EL2_OR_EL3(x6)
-1:msr elr_el1, x1 // Exception Link Register
- msr spsr_el1,x2 // Saved Processor Status Register 32bit
+ EL1_OR_EL2_OR_EL3(x28)
+1:msr elr_el1, x29 // Exception Link Register
+ msr spsr_el1, x30 // Saved Processor Status Register 32bit
b 4f
-2:msr elr_el2, x1 // Exception Link Register
- msr spsr_el2,x2 // Saved Processor Status Register 32bit
+2:msr elr_el2, x29 // Exception Link Register
+ msr spsr_el2, x30 // Saved Processor Status Register 32bit
b 4f
-3:msr elr_el3, x1 // Exception Link Register
- msr spsr_el3,x2 // Saved Processor Status Register 32bit
-4:msr fpsr, x3 // Floating point Status Register 32bit
-
- // pop all regs and return from exception.
- sub sp, sp, #(FP_CONTEXT_SIZE + SYS_CONTEXT_SIZE)
- ALL_GP_REGS
+3:msr elr_el3, x29 // Exception Link Register
+ msr spsr_el3, x30 // Saved Processor Status Register 32bit
+4:
- // Adjust SP to pop next set
- add sp, sp, #FP_CONTEXT_SIZE
- // Pop FP regs to Stack.
- ALL_FP_REGS
+ // pop remaining GP regs and return from exception.
+ ldr x30, [sp, #0xf0 - 0xe0]
+ ldp x28, x29, [sp], #GP_CONTEXT_SIZE - 0xe0
// Adjust SP to be where we started from when we came into the handler.
// The handler can not change the SP.
- add sp, sp, #SYS_CONTEXT_SIZE
+ add sp, sp, #FP_CONTEXT_SIZE + SYS_CONTEXT_SIZE
eret
-
-#undef REG_PAIR
-#undef REG_ONE
|