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