rework general purpose registers save and restore

The runtime exception handling assembler code used magic numbers for
saving and restoring the general purpose register context on stack
memory. The memory is interpreted as a 'gp_regs' structure and the
magic numbers are offsets to members of this structure. This patch
replaces the magic number offsets with constants. It also adds compile
time assertions to prevent an incorrect assembler view of this
structure.

Change-Id: Ibf125bfdd62ba3a33e58c5f1d71f8c229720781c

5 files changed, 114 insertions, 48 deletions

diff --git a/bl31/aarch64/runtime_exceptions.S b/bl31/aarch64/runtime_exceptions.S
index 6d086e0..52163b8 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 0067ca6..786c52a 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 3504d04..0af98d6 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 3ccbe88..61c2d27 100644
--- a/include/runtime_svc.h
+++ b/include/runtime_svc.h
@@ -31,6 +31,7 @@
 #ifndef __RUNTIME_SVC_H__
 #define __RUNTIME_SVC_H__
 #include <psci.h>
+#include <bl_common.h>
 
 /*******************************************************************************
  * 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 08a568e..c951d0e 100644
--- a/lib/arch/aarch64/misc_helpers.S
+++ b/lib/arch/aarch64/misc_helpers.S
@@ -29,6 +29,10 @@
  */
 
 #include <arch_helpers.h>
+#include <runtime_svc.h>
+
+	.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