core: select workaround vector in C

Replace the two assembly implementations for selecting the exception
vector with a common C version.

Reviewed-by: Volodymyr Babchuk <vlad.babchuk@gmail.com>
Tested-by: Jens Wiklander <jens.wiklander@linaro.org> (Hikey, QEMU)
Signed-off-by: Jens Wiklander <jens.wiklander@linaro.org>

4 files changed, 46 insertions, 64 deletions

diff --git a/core/arch/arm/kernel/thread.c b/core/arch/arm/kernel/thread.c
index 1f375491..3b5618e0 100644
--- a/core/arch/arm/kernel/thread.c
+++ b/core/arch/arm/kernel/thread.c
@@ -929,6 +929,48 @@ static void init_sec_mon(size_t pos __maybe_unused)
 #endif
 }
 
+static uint32_t __maybe_unused get_midr_implementer(uint32_t midr)
+{
+	return (midr >> MIDR_IMPLEMENTER_SHIFT) & MIDR_IMPLEMENTER_MASK;
+}
+
+static uint32_t __maybe_unused get_midr_primary_part(uint32_t midr)
+{
+	return (midr >> MIDR_PRIMARY_PART_NUM_SHIFT) &
+	       MIDR_PRIMARY_PART_NUM_MASK;
+}
+
+static vaddr_t get_excp_vect(void)
+{
+#ifdef CFG_CORE_WORKAROUND_SPECTRE_BP_SEC
+	uint32_t midr = read_midr();
+
+	if (get_midr_implementer(midr) != MIDR_IMPLEMENTER_ARM)
+		return (vaddr_t)thread_excp_vect;
+
+	switch (get_midr_primary_part(midr)) {
+#ifdef ARM32
+	case CORTEX_A8_PART_NUM:
+	case CORTEX_A9_PART_NUM:
+	case CORTEX_A17_PART_NUM:
+#endif
+	case CORTEX_A57_PART_NUM:
+	case CORTEX_A72_PART_NUM:
+	case CORTEX_A73_PART_NUM:
+	case CORTEX_A75_PART_NUM:
+		return (vaddr_t)thread_excp_vect_workaround;
+#ifdef ARM32
+	case CORTEX_A15_PART_NUM:
+		return (vaddr_t)thread_excp_vect_workaround_a15;
+#endif
+	default:
+		return (vaddr_t)thread_excp_vect;
+	}
+#endif /*CFG_CORE_WORKAROUND_SPECTRE_BP_SEC*/
+
+	return (vaddr_t)thread_excp_vect;
+}
+
 void thread_init_per_cpu(void)
 {
 	size_t pos = get_core_pos();
@@ -939,7 +981,7 @@ void thread_init_per_cpu(void)
 	set_tmp_stack(l, GET_STACK(stack_tmp[pos]) - STACK_TMP_OFFS);
 	set_abt_stack(l, GET_STACK(stack_abt[pos]));
 
-	thread_init_vbar();
+	thread_init_vbar(get_excp_vect());
 }
 
 struct thread_specific_data *thread_get_tsd(void)
diff --git a/core/arch/arm/kernel/thread_a32.S b/core/arch/arm/kernel/thread_a32.S
index 4bf638b8..bb07348e 100644
--- a/core/arch/arm/kernel/thread_a32.S
+++ b/core/arch/arm/kernel/thread_a32.S
@@ -460,44 +460,7 @@ KEEP_PAGER thread_rpc
 FUNC thread_init_vbar , :
 UNWIND(	.fnstart)
 	/* Set vector (VBAR) */
-#ifdef CFG_CORE_WORKAROUND_SPECTRE_BP_SEC
-	/*
-	 * For unrecognized CPUs we fall back to the vector used for
-	 * unaffected CPUs. Cortex A-15 has special treatment compared to
-	 * the other affected Cortex CPUs.
-	 */
-	read_midr r1
-	ubfx	r2, r1, #MIDR_IMPLEMENTER_SHIFT, #MIDR_IMPLEMENTER_WIDTH
-	cmp	r2, #MIDR_IMPLEMENTER_ARM
-	bne	1f
-
-	ubfx	r2, r1, #MIDR_PRIMARY_PART_NUM_SHIFT, \
-			#MIDR_PRIMARY_PART_NUM_WIDTH
-
-	movw	r3, #CORTEX_A8_PART_NUM
-	cmp	r2, r3
-	movwne	r3, #CORTEX_A9_PART_NUM
-	cmpne	r2, r3
-	movwne	r3, #CORTEX_A17_PART_NUM
-	cmpne	r2, r3
-	movwne	r3, #CORTEX_A57_PART_NUM
-	cmpne	r2, r3
-	movwne	r3, #CORTEX_A72_PART_NUM
-	cmpne	r2, r3
-	movwne	r3, #CORTEX_A73_PART_NUM
-	cmpne	r2, r3
-	movwne	r3, #CORTEX_A75_PART_NUM
-	cmpne	r2, r3
-	ldreq	r0, =thread_excp_vect_workaround
-	beq	2f
-
-	movw	r3, #CORTEX_A15_PART_NUM
-	cmp	r2, r3
-	ldreq	r0, =thread_excp_vect_workaround_a15
-	beq	2f
-#endif
-1:	ldr	r0, =thread_excp_vect
-2:	write_vbar r0
+	write_vbar r0
 	bx	lr
 UNWIND(	.fnend)
 END_FUNC thread_init_vbar
diff --git a/core/arch/arm/kernel/thread_a64.S b/core/arch/arm/kernel/thread_a64.S
index 19503797..771d7fd1 100644
--- a/core/arch/arm/kernel/thread_a64.S
+++ b/core/arch/arm/kernel/thread_a64.S
@@ -254,30 +254,7 @@ END_FUNC thread_rpc
 KEEP_PAGER thread_rpc
 
 FUNC thread_init_vbar , :
-#ifdef CFG_CORE_WORKAROUND_SPECTRE_BP_SEC
-	/*
-	 * For unrecognized CPUs we fall back to the vector used for
-	 * unaffected CPUs.
-	 */
-	mrs	x1, midr_el1
-	ubfx	x2, x1, #MIDR_IMPLEMENTER_SHIFT, #MIDR_IMPLEMENTER_WIDTH
-	cmp	x2, #MIDR_IMPLEMENTER_ARM
-	b.ne	1f
-
-	adr	x0, thread_excp_vect_workaround
-	ubfx	x2, x1, #MIDR_PRIMARY_PART_NUM_SHIFT, \
-			#MIDR_PRIMARY_PART_NUM_WIDTH
-	cmp	x2, #CORTEX_A57_PART_NUM
-	b.eq	2f
-	cmp	x2, #CORTEX_A72_PART_NUM
-	b.eq	2f
-	cmp	x2, #CORTEX_A73_PART_NUM
-	b.eq	2f
-	cmp	x2, #CORTEX_A75_PART_NUM
-	b.eq	2f
-#endif
-1:	adr	x0, thread_excp_vect
-2:	msr	vbar_el1, x0
+	msr	vbar_el1, x0
 	ret
 END_FUNC thread_init_vbar
 KEEP_PAGER thread_init_vbar
diff --git a/core/arch/arm/kernel/thread_private.h b/core/arch/arm/kernel/thread_private.h
index 9877e9db..6e736de0 100644
--- a/core/arch/arm/kernel/thread_private.h
+++ b/core/arch/arm/kernel/thread_private.h
@@ -168,7 +168,7 @@ extern long thread_user_kcode_offset;
 /*
  * Initializes VBAR for current CPU (called by thread_init_per_cpu()
  */
-void thread_init_vbar(void);
+void thread_init_vbar(vaddr_t addr);
 
 void thread_excp_vect(void);
 void thread_excp_vect_workaround(void);