diff --git a/bl31/aarch64/runtime_exceptions.S b/bl31/aarch64/runtime_exceptions.S index 6d086e005..52163b8d1 100644 --- a/bl31/aarch64/runtime_exceptions.S +++ b/bl31/aarch64/runtime_exceptions.S @@ -202,47 +202,3 @@ serror_aarch32: .align 7 -save_regs:; .type save_regs, %function - sub sp, sp, #0x100 - stp x0, x1, [sp, #0x0] - stp x2, x3, [sp, #0x10] - stp x4, x5, [sp, #0x20] - stp x6, x7, [sp, #0x30] - 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] - mrs x0, sp_el0 - stp x28, x0, [sp, #0xe0] - mrs x0, spsr_el3 - str x0, [sp, #0xf0] - ret - - -restore_regs:; .type restore_regs, %function - ldr x9, [sp, #0xf0] - msr spsr_el3, x9 - ldp x28, x9, [sp, #0xe0] - msr sp_el0, x9 - ldp x26, x27, [sp, #0xd0] - ldp x24, x25, [sp, #0xc0] - ldp x22, x23, [sp, #0xb0] - ldp x20, x21, [sp, #0xa0] - ldp x18, x19, [sp, #0x90] - ldp x16, x17, [sp, #0x80] - ldp x14, x15, [sp, #0x70] - ldp x12, x13, [sp, #0x60] - ldp x10, x11, [sp, #0x50] - ldp x8, x9, [sp, #0x40] - ldp x6, x7, [sp, #0x30] - ldp x4, x5, [sp, #0x20] - ldp x2, x3, [sp, #0x10] - ldp x0, x1, [sp, #0x0] - add sp, sp, #0x100 - ret diff --git a/include/asm_macros.S b/include/asm_macros.S index 0067ca610..786c52a24 100644 --- a/include/asm_macros.S +++ b/include/asm_macros.S @@ -55,14 +55,14 @@ .macro exception_entry func - stp x29, x30, [sp, #-0x10]! + stp x29, x30, [sp, #-(SIZEOF_GPREGS - GPREGS_FP_OFF)]! bl \func .endm .macro exception_exit func bl \func - ldp x29, x30, [sp], #0x10 + ldp x29, x30, [sp], #(SIZEOF_GPREGS - GPREGS_FP_OFF) .endm diff --git a/include/bl_common.h b/include/bl_common.h index 3504d0410..0af98d689 100644 --- a/include/bl_common.h +++ b/include/bl_common.h @@ -63,6 +63,13 @@ ******************************************************************************/ #define EL_CHANGE_MEM_SIZE (sizeof(meminfo) + sizeof(el_change_info)) +/******************************************************************************* + * Macro to flag a compile time assertion. It uses the preprocessor to generate + * an invalid C construct if 'cond' evaluates to false. + * The following compilation error is triggered if the assertion fails: + * "error: size of array 'msg' is negative" + ******************************************************************************/ +#define CASSERT(cond, msg) typedef char msg[(cond) ? 0 : -1] #ifndef __ASSEMBLY__ /******************************************************************************* diff --git a/include/runtime_svc.h b/include/runtime_svc.h index 3ccbe8811..61c2d2752 100644 --- a/include/runtime_svc.h +++ b/include/runtime_svc.h @@ -31,6 +31,7 @@ #ifndef __RUNTIME_SVC_H__ #define __RUNTIME_SVC_H__ #include +#include /******************************************************************************* * Bit definitions inside the function id as per the SMC calling convention @@ -72,6 +73,45 @@ #define FIQ_AARCH32 0xe #define SERROR_AARCH32 0xf +/******************************************************************************* + * Constants that allow assembler code to access members of the 'gp_regs' + * structure at their correct offsets. + ******************************************************************************/ +#define SIZEOF_GPREGS 0x110 +#define GPREGS_X0_OFF 0x0 +#define GPREGS_X1_OFF 0x8 +#define GPREGS_X2_OFF 0x10 +#define GPREGS_X3_OFF 0x18 +#define GPREGS_X4_OFF 0x20 +#define GPREGS_X5_OFF 0x28 +#define GPREGS_X6_OFF 0x30 +#define GPREGS_X7_OFF 0x38 +#define GPREGS_X8_OFF 0x40 +#define GPREGS_X9_OFF 0x48 +#define GPREGS_X10_OFF 0x50 +#define GPREGS_X11_OFF 0x58 +#define GPREGS_X12_OFF 0x60 +#define GPREGS_X13_OFF 0x68 +#define GPREGS_X14_OFF 0x70 +#define GPREGS_X15_OFF 0x78 +#define GPREGS_X16_OFF 0x80 +#define GPREGS_X17_OFF 0x88 +#define GPREGS_X18_OFF 0x90 +#define GPREGS_X19_OFF 0x98 +#define GPREGS_X20_OFF 0xA0 +#define GPREGS_X21_OFF 0xA8 +#define GPREGS_X22_OFF 0xB0 +#define GPREGS_X23_OFF 0xB8 +#define GPREGS_X24_OFF 0xC0 +#define GPREGS_X25_OFF 0xC8 +#define GPREGS_X26_OFF 0xD0 +#define GPREGS_X27_OFF 0xD8 +#define GPREGS_X28_OFF 0xE0 +#define GPREGS_SP_EL0_OFF 0xE8 +#define GPREGS_SPSR_OFF 0xF0 +#define GPREGS_FP_OFF 0x100 +#define GPREGS_LR_OFF 0x108 + #ifndef __ASSEMBLY__ typedef struct { @@ -106,10 +146,24 @@ typedef struct { unsigned long x28; unsigned long sp_el0; unsigned long spsr; - unsigned long fp; + /* + * Alignment constraint which allows save & restore of fp & lr on the + * stack during exception handling + */ + unsigned long fp __attribute__((__aligned__(16))); unsigned long lr; -} gp_regs; +} __attribute__((__aligned__(16))) gp_regs; +/******************************************************************************* + * Compile time assertions to ensure that: + * 1) the assembler code's view of the size of the 'gp_regs' data structure is + * the same as the actual size of this data structure. + * 2) the assembler code's view of the offset of the frame pointer member of + * the 'gp_regs' structure is the same as the actual offset of this member. + ******************************************************************************/ +CASSERT((sizeof(gp_regs) == SIZEOF_GPREGS), assert_sizeof_gpregs_mismatch); +CASSERT(GPREGS_FP_OFF == __builtin_offsetof(gp_regs, fp), \ + assert_gpregs_fp_offset_mismatch); /******************************************************************************* * Function & variable prototypes diff --git a/lib/arch/aarch64/misc_helpers.S b/lib/arch/aarch64/misc_helpers.S index 08a568ef0..c951d0eff 100644 --- a/lib/arch/aarch64/misc_helpers.S +++ b/lib/arch/aarch64/misc_helpers.S @@ -29,6 +29,10 @@ */ #include +#include + + .globl save_regs + .globl restore_regs .globl enable_irq .globl disable_irq @@ -80,6 +84,51 @@ .section .text, "ax" +save_regs:; .type save_regs, %function + sub sp, sp, #GPREGS_FP_OFF + stp x0, x1, [sp, #GPREGS_X0_OFF] + stp x2, x3, [sp, #GPREGS_X2_OFF] + stp x4, x5, [sp, #GPREGS_X4_OFF] + stp x6, x7, [sp, #GPREGS_X6_OFF] + stp x8, x9, [sp, #GPREGS_X8_OFF] + stp x10, x11, [sp, #GPREGS_X10_OFF] + stp x12, x13, [sp, #GPREGS_X12_OFF] + stp x14, x15, [sp, #GPREGS_X14_OFF] + stp x16, x17, [sp, #GPREGS_X16_OFF] + stp x18, x19, [sp, #GPREGS_X18_OFF] + stp x20, x21, [sp, #GPREGS_X20_OFF] + stp x22, x23, [sp, #GPREGS_X22_OFF] + stp x24, x25, [sp, #GPREGS_X24_OFF] + stp x26, x27, [sp, #GPREGS_X26_OFF] + mrs x0, sp_el0 + stp x28, x0, [sp, #GPREGS_X28_OFF] + mrs x0, spsr_el3 + str x0, [sp, #GPREGS_SPSR_OFF] + ret + + +restore_regs:; .type restore_regs, %function + ldr x9, [sp, #GPREGS_SPSR_OFF] + msr spsr_el3, x9 + ldp x28, x9, [sp, #GPREGS_X28_OFF] + msr sp_el0, x9 + ldp x26, x27, [sp, #GPREGS_X26_OFF] + ldp x24, x25, [sp, #GPREGS_X24_OFF] + ldp x22, x23, [sp, #GPREGS_X22_OFF] + ldp x20, x21, [sp, #GPREGS_X20_OFF] + ldp x18, x19, [sp, #GPREGS_X18_OFF] + ldp x16, x17, [sp, #GPREGS_X16_OFF] + ldp x14, x15, [sp, #GPREGS_X14_OFF] + ldp x12, x13, [sp, #GPREGS_X12_OFF] + ldp x10, x11, [sp, #GPREGS_X10_OFF] + ldp x8, x9, [sp, #GPREGS_X8_OFF] + ldp x6, x7, [sp, #GPREGS_X6_OFF] + ldp x4, x5, [sp, #GPREGS_X4_OFF] + ldp x2, x3, [sp, #GPREGS_X2_OFF] + ldp x0, x1, [sp, #GPREGS_X0_OFF] + add sp, sp, #GPREGS_FP_OFF + ret + get_afflvl_shift:; .type get_afflvl_shift, %function cmp x0, #3 cinc x0, x0, eq