Support CFG_TEE_CORE_NB_CORE > 8

Remove the stack_tmp_top[] array. Instead, compute the stack top for each CPU in the assembly code: sp = stack_tmp + (cpu_id + 1) * stack_tmp_stride stack_tmp and stack_tmp_stride are exported by thread.c. Signed-off-by: Jerome Forissier <[email protected]> Reviewed-by: Jens Wiklander <[email protected]> Reviewed-by: Etienne Carriere <[email protected]>
OP-TEE · Jul 7, 2016 · 3aa8bf4 · 3aa8bf4
1 parent 2cdf0c8
commit 3aa8bf4
Show file tree

Hide file tree

Showing 6 changed files with 76 additions and 101 deletions.
diff --git a/core/arch/arm/kernel/generic_entry_a32.S b/core/arch/arm/kernel/generic_entry_a32.S
@@ -198,6 +198,23 @@ UNWIND(	.cantunwind)
 UNWIND(	.fnend)
 END_FUNC reset
 
+	/*
+	 * Setup sp to point to the top of the tmp stack for the current CPU:
+	 * sp is assigned stack_tmp + (cpu_id + 1) * stack_tmp_stride
+	 */
+	.macro set_sp
+		bl	get_core_pos
+		cmp	r0, #CFG_TEE_CORE_NB_CORE
+		/* Unsupported CPU, park it before it breaks something */
+		bge	unhandled_cpu
+		add	r0, r0, #1
+		ldr	r2, =stack_tmp_stride
+		ldr	r1, [r2]
+		mul	r2, r0, r1
+		ldr	r1, =stack_tmp
+		add	sp, r1, r2
+	.endm
+
 LOCAL_FUNC reset_primary , :
 UNWIND(	.fnstart)
 UNWIND(	.cantunwind)
@@ -233,13 +250,7 @@ copy_init:
 	bgt	copy_init
 #endif
 
-	bl	get_core_pos
-	cmp	r0, #CFG_TEE_CORE_NB_CORE
-	/* Unsupported CPU, park it before it breaks something */
-	bge	unhandled_cpu
-	lsl	r0, #2
-	ldr	r1, =stack_tmp_top
-	ldr	sp, [r1, r0]
+	set_sp
 
 	/* complete ARM secure MP common configuration */
 	bl	plat_cpu_reset_late
@@ -387,13 +398,7 @@ UNWIND(	.cantunwind)
 	write_vbar r0
 
 	mov	r4, lr
-	bl	get_core_pos
-	cmp	r0, #CFG_TEE_CORE_NB_CORE
-	/* Unsupported CPU, park it before it breaks something */
-	bge	unhandled_cpu
-	lsl	r0, #2
-	ldr	r1, =stack_tmp_top
-	ldr	sp, [r1, r0]
+	set_sp
 
 	bl	core_init_mmu_regs
 	bl	cpu_mmu_enable
@@ -415,13 +420,7 @@ UNWIND(	.fnstart)
 UNWIND(	.cantunwind)
 	bl	wait_primary
 
-	bl	get_core_pos
-	cmp	r0, #CFG_TEE_CORE_NB_CORE
-	/* Unsupported CPU, park it before it breaks something */
-	bge	unhandled_cpu
-	lsl	r0, #2
-	ldr	r1, =stack_tmp_top
-	ldr	sp, [r1, r0]
+	set_sp
 
 	bl	plat_cpu_reset_late
 

diff --git a/core/arch/arm/kernel/generic_entry_a64.S b/core/arch/arm/kernel/generic_entry_a64.S
@@ -33,6 +33,30 @@
 #include <sm/teesmc_opteed_macros.h>
 #include <sm/teesmc_opteed.h>
 
+	/*
+	 * Setup SP_EL0 and SPEL1, SP will be set to SP_EL0.
+	 * SP_EL0 is assigned stack_tmp + (cpu_id + 1) * stack_tmp_stride
+	 * SP_EL1 is assigned thread_core_local[cpu_id]
+	 */
+	.macro set_sp
+		bl	get_core_pos
+		cmp	x0, #CFG_TEE_CORE_NB_CORE
+		/* Unsupported CPU, park it before it breaks something */
+		bge	unhandled_cpu
+		add	x0, x0, #1
+		adr	x2, stack_tmp_stride
+		ldr	w1, [x2]
+		mul	x2, x0, x1
+		adr	x1, stack_tmp
+		add	x1, x1, x2
+		msr	spsel, #0
+		mov	sp, x1
+		bl	thread_get_core_local
+		msr	spsel, #1
+		mov	sp, x0
+		msr	spsel, #0
+	.endm
+
 .section .text.boot
 FUNC _start , :
 	mov	x19, x0		/* Save pagable part address */
@@ -72,24 +96,8 @@ copy_init:
 	b.lt	copy_init
 #endif
 
-	/*
-	 * Setup SP_EL0 and SPEL1, SP will be set to SP_EL0.
-	 * SP_EL0 is assigned stack_tmp_top[cpu_id]
-	 * SP_EL1 is assigned thread_core_local[cpu_id]
-	 */
-	bl	get_core_pos
-	cmp	x0, #CFG_TEE_CORE_NB_CORE
-	/* Unsupported CPU, park it before it breaks something */
-	bge	unhandled_cpu
-	lsl	x2, x0, #3
-	adr	x1, stack_tmp_top
-	ldr	x1, [x1, x2]
-	msr	spsel, #0
-	mov	sp, x1
-	bl	thread_get_core_local
-	msr	spsel, #1
-	mov	sp, x0
-	msr	spsel, #0
+	/* Setup SP_EL0 and SP_EL1, SP will be set to SP_EL0 */
+	set_sp
 
 	/* Enable aborts now that we can receive exceptions */
 	msr	daifclr, #DAIFBIT_ABT
@@ -164,24 +172,8 @@ FUNC cpu_on_handler , :
 	msr	sctlr_el1, x0
 	isb
 
-	/*
-	 * Setup SP_EL0 and SPEL1, SP will be set to SP_EL0.
-	 * SP_EL0 is assigned stack_tmp_top[cpu_id]
-	 * SP_EL1 is assigned thread_core_local[cpu_id]
-	 */
-	bl	get_core_pos
-	cmp	x0, #CFG_TEE_CORE_NB_CORE
-	/* Unsupported CPU, park it before it breaks something */
-	bge	unhandled_cpu
-	lsl	x2, x0, #3
-	adr	x1, stack_tmp_top
-	ldr	x1, [x1, x2]
-	msr	spsel, #0
-	mov	sp, x1
-	bl	thread_get_core_local
-	msr	spsel, #1
-	mov	sp, x0
-	msr	spsel, #0
+	/* Setup SP_EL0 and SP_EL1, SP will be set to SP_EL0 */
+	set_sp
 
 	/* Enable aborts now that we can receive exceptions */
 	msr	daifclr, #DAIFBIT_ABT

diff --git a/core/arch/arm/kernel/link.mk b/core/arch/arm/kernel/link.mk
@@ -28,7 +28,6 @@ link-script-cppflags := -DASM=1 \
 entries-unpaged += thread_init_vbar
 entries-unpaged += sm_init
 entries-unpaged += core_init_mmu_regs
-entries-unpaged += stack_tmp_top
 entries-unpaged += sem_cpu_sync
 entries-unpaged += generic_boot_get_handlers
 

diff --git a/core/arch/arm/kernel/thread.c b/core/arch/arm/kernel/thread.c
@@ -47,6 +47,7 @@
 #include <util.h>
 #include <trace.h>
 #include <assert.h>
+#include <keep.h>
 
 #ifdef ARM32
 #define STACK_TMP_SIZE		1024
@@ -93,52 +94,31 @@ static struct thread_core_local thread_core_local[CFG_TEE_CORE_NB_CORE];
 #define STACK_CANARY_SIZE	0
 #endif
 
-#define DECLARE_STACK(name, num_stacks, stack_size) \
-	static uint32_t name[num_stacks][ \
-		ROUNDUP(stack_size + STACK_CANARY_SIZE, STACK_ALIGNMENT) / \
+#define DECLARE_STACK(name, num_stacks, stack_size, linkage) \
+linkage uint32_t name[num_stacks] \
+		[ROUNDUP(stack_size + STACK_CANARY_SIZE, STACK_ALIGNMENT) / \
 		sizeof(uint32_t)] \
 		__attribute__((section(".nozi.stack"), \
 			       aligned(STACK_ALIGNMENT)))
 
+#define STACK_SIZE(stack) (sizeof(stack) - STACK_CANARY_SIZE / 2)
+
 #define GET_STACK(stack) \
-	((vaddr_t)(stack) + sizeof(stack) - STACK_CANARY_SIZE / 2)
+	((vaddr_t)(stack) + STACK_SIZE(stack))
 
-DECLARE_STACK(stack_tmp,	CFG_TEE_CORE_NB_CORE,	STACK_TMP_SIZE);
-DECLARE_STACK(stack_abt,	CFG_TEE_CORE_NB_CORE,	STACK_ABT_SIZE);
+DECLARE_STACK(stack_tmp, CFG_TEE_CORE_NB_CORE, STACK_TMP_SIZE, /* global */);
+DECLARE_STACK(stack_abt, CFG_TEE_CORE_NB_CORE, STACK_ABT_SIZE, static);
 #if !defined(CFG_WITH_ARM_TRUSTED_FW)
-DECLARE_STACK(stack_sm,		CFG_TEE_CORE_NB_CORE,	SM_STACK_SIZE);
+DECLARE_STACK(stack_sm, CFG_TEE_CORE_NB_CORE, SM_STACK_SIZE, static);
 #endif
 #ifndef CFG_WITH_PAGER
-DECLARE_STACK(stack_thread,	CFG_NUM_THREADS,	STACK_THREAD_SIZE);
+DECLARE_STACK(stack_thread, CFG_NUM_THREADS, STACK_THREAD_SIZE, static);
 #endif
 
-const vaddr_t stack_tmp_top[CFG_TEE_CORE_NB_CORE] = {
-	GET_STACK(stack_tmp[0]),
-#if CFG_TEE_CORE_NB_CORE > 1
-	GET_STACK(stack_tmp[1]),
-#endif
-#if CFG_TEE_CORE_NB_CORE > 2
-	GET_STACK(stack_tmp[2]),
-#endif
-#if CFG_TEE_CORE_NB_CORE > 3
-	GET_STACK(stack_tmp[3]),
-#endif
-#if CFG_TEE_CORE_NB_CORE > 4
-	GET_STACK(stack_tmp[4]),
-#endif
-#if CFG_TEE_CORE_NB_CORE > 5
-	GET_STACK(stack_tmp[5]),
-#endif
-#if CFG_TEE_CORE_NB_CORE > 6
-	GET_STACK(stack_tmp[6]),
-#endif
-#if CFG_TEE_CORE_NB_CORE > 7
-	GET_STACK(stack_tmp[7]),
-#endif
-#if CFG_TEE_CORE_NB_CORE > 8
-#error "Top of tmp stacks aren't defined for more than 8 CPUS"
-#endif
-};
+const uint32_t stack_tmp_stride = STACK_SIZE(stack_tmp[0]);
+
+KEEP_PAGER(stack_tmp);
+KEEP_PAGER(stack_tmp_stride);
 
 thread_smc_handler_t thread_std_smc_handler_ptr;
 static thread_smc_handler_t thread_fast_smc_handler_ptr;
@@ -769,7 +749,6 @@ static void init_handlers(const struct thread_handlers *handlers)
 	thread_system_reset_handler_ptr = handlers->system_reset;
 }
 
-
 #ifdef CFG_WITH_PAGER
 static void init_thread_stacks(void)
 {

diff --git a/core/arch/arm/plat-sunxi/entry.S b/core/arch/arm/plat-sunxi/entry.S
@@ -60,9 +60,12 @@ UNWIND(	.cantunwind)
 
 	mov	r4, r1
 	bl	get_core_pos
-	lsl	r0, #2
-	ldr	r1, =stack_tmp_top
-	ldr	sp, [r1, r0]
+	add	r0, r0, #1
+	ldr	r2, =stack_tmp_stride
+	ldr	r1, [r2]
+	mul	r2, r0, r1
+	ldr	r1, =stack_tmp
+	ldr	sp, [r1, r2]
 
     	/* NSACR configuration */
     	read_nsacr  r1

diff --git a/core/arch/arm/plat-sunxi/smp_boot.S b/core/arch/arm/plat-sunxi/smp_boot.S
@@ -58,10 +58,13 @@ UNWIND(	.cantunwind)
 	write_vbar r0
 
 	/* Setup tmp stack */
-	bl	get_core_pos
-	lsl	r0, #2
-	ldr	r1, =stack_tmp_top
-	ldr	sp, [r1, r0]
+        bl      get_core_pos
+        add     r0, r0, #1
+        ldr     r2, =stack_tmp_stride
+        ldr     r1, [r2]
+        mul     r2, r0, r1
+        ldr     r1, =stack_tmp
+        ldr     sp, [r1, r2]
 
         /* NSACR configuration */
     	read_nsacr  r1