Apply errata workarounds only when major/minor revisions match.

Prior to this patch, the errata workarounds were applied for any version
of the CPU in the release build and in the debug build an assert
failure resulted when the revision did not match. This patch applies
errata workarounds in the Cortex-A57 reset handler only if the 'variant'
and 'revision' fields read from the MIDR_EL1 match. In the debug build,
a warning message is printed for each errata workaround which is not
applied.

The patch modifies the register usage in 'reset_handler` so
as to adhere to ARM procedure calling standards.

Fixes ARM-software/tf-issues#242

Change-Id: I51b1f876474599db885afa03346e38a476f84c29

2 files changed, 105 insertions, 24 deletions

diff --git a/lib/cpus/aarch64/cortex_a57.S b/lib/cpus/aarch64/cortex_a57.S
index 3e552978..e7774974 100644
--- a/lib/cpus/aarch64/cortex_a57.S
+++ b/lib/cpus/aarch64/cortex_a57.S
@@ -81,33 +81,79 @@ func cortex_a57_disable_ext_debug
 	dsb	sy
 	ret
 
-func cortex_a57_reset_func
-#if ERRATA_A57_806969 || ERRATA_A57_813420
-	/* ---------------------------------------------
-	 * Ensure that the following errata is only
-	 * applied on r0p0 parts.
-	 * ---------------------------------------------
+	/* --------------------------------------------------
+	 * Errata Workaround for Cortex A57 Errata #806969.
+	 * This applies only to revision r0p0 of Cortex A57.
+	 * Inputs:
+	 * x0: variant[4:7] and revision[0:3] of current cpu.
+	 * --------------------------------------------------
 	 */
-#if ASM_ASSERTION
-	mrs	x0, midr_el1
-	ubfx	x1, x0, #MIDR_VAR_SHIFT, #4
-	ubfx	x2, x0, #MIDR_REV_SHIFT, #4
-	orr	x0, x1, x2
-	cmp	x0, #0
-	ASM_ASSERT(eq)
+func errata_a57_806969_wa
+	/*
+	 * Compare x0 against revision r0p0
+	 */
+	cbz	x0, apply_806969
+#if DEBUG
+	b	print_revision_warning
+#else
+	ret
 #endif
-	mov	x1, xzr
-#if ERRATA_A57_806969
+apply_806969:
+	mrs	x1, CPUACTLR_EL1
 	orr	x1, x1, #CPUACTLR_NO_ALLOC_WBWA
+	msr	CPUACTLR_EL1, x1
+	ret
+
+
+	/* ---------------------------------------------------
+	 * Errata Workaround for Cortex A57 Errata #813420.
+	 * This applies only to revision r0p0 of Cortex A57.
+	 * Inputs:
+	 * x0: variant[4:7] and revision[0:3] of current cpu.
+	 * ---------------------------------------------------
+	 */
+func errata_a57_813420_wa
+	/*
+	 * Compare x0 against revision r0p0
+	 */
+	cbz	x0, apply_813420
+#if DEBUG
+	b	print_revision_warning
+#else
+	ret
 #endif
-#if ERRATA_A57_813420
+apply_813420:
+	mrs	x1, CPUACTLR_EL1
 	orr	x1, x1, #CPUACTLR_DCC_AS_DCCI
-#endif
-	mrs	x0, CPUACTLR_EL1
-	orr	x0, x0, x1
-	msr	CPUACTLR_EL1, x0
+	msr	CPUACTLR_EL1, x1
+	ret
+
+	/* -------------------------------------------------
+	 * The CPU Ops reset function for Cortex-A57.
+	 * -------------------------------------------------
+	 */
+func cortex_a57_reset_func
+	mov	x19, x30
+	mrs	x0, midr_el1
+
+	/*
+	 * Extract the variant[20:23] and revision[0:3] from x0
+	 * and pack it in x20[0:7] as variant[4:7] and revision[0:3].
+	 * First extract x0[16:23] to x20[0:7] and zero fill the rest.
+	 * Then extract x0[0:3] into x20[0:3] retaining other bits.
+	 */
+	ubfx	x20, x0, #(MIDR_VAR_SHIFT - MIDR_REV_BITS), #(MIDR_REV_BITS + MIDR_VAR_BITS)
+	bfxil	x20, x0, #MIDR_REV_SHIFT, #MIDR_REV_BITS
+
+#if ERRATA_A57_806969
+	mov	x0, x20
+	bl	errata_a57_806969_wa
 #endif
 
+#if ERRATA_A57_813420
+	mov	x0, x20
+	bl	errata_a57_813420_wa
+#endif
 	/* ---------------------------------------------
 	 * As a bare minimum enable the SMP bit.
 	 * ---------------------------------------------
@@ -116,8 +162,12 @@ func cortex_a57_reset_func
 	orr	x0, x0, #CPUECTLR_SMP_BIT
 	msr	CPUECTLR_EL1, x0
 	isb
-	ret
+	ret	x19
 
+	/* ----------------------------------------------------
+	 * The CPU Ops core power down function for Cortex-A57.
+	 * ----------------------------------------------------
+	 */
 func cortex_a57_core_pwr_dwn
 	mov	x18, x30
 
@@ -153,6 +203,10 @@ func cortex_a57_core_pwr_dwn
 	mov	x30, x18
 	b	cortex_a57_disable_ext_debug
 
+	/* -------------------------------------------------------
+	 * The CPU Ops cluster power down function for Cortex-A57.
+	 * -------------------------------------------------------
+	 */
 func cortex_a57_cluster_pwr_dwn
 	mov	x18, x30
 
diff --git a/lib/cpus/aarch64/cpu_helpers.S b/lib/cpus/aarch64/cpu_helpers.S
index 46584b3f..f053d44f 100644
--- a/lib/cpus/aarch64/cpu_helpers.S
+++ b/lib/cpus/aarch64/cpu_helpers.S
@@ -45,7 +45,7 @@
 	 */
 	.globl	reset_handler
 func reset_handler
-	mov	x10, x30
+	mov	x19, x30
 
 	bl	plat_reset_handler
 
@@ -58,10 +58,11 @@ func reset_handler
 
 	/* Get the cpu_ops reset handler */
 	ldr	x2, [x0, #CPU_RESET_FUNC]
+	mov	x30, x19
 	cbz	x2, 1f
-	blr	x2
+	br	x2
 1:
-	ret	x10
+	ret
 
 #endif /* IMAGE_BL1 || (IMAGE_BL31 && RESET_TO_BL31) */
 
@@ -191,3 +192,29 @@ func get_cpu_ops_ptr
 	sub	x0, x4, #(CPU_OPS_SIZE + CPU_MIDR)
 error_exit:
 	ret
+
+#if DEBUG
+	/*
+	 * This function prints a warning message to the crash console
+	 * if the CPU revision/part number does not match the errata
+	 * workaround enabled in the build.
+	 * Clobber: x30, x0 - x5
+	 */
+.section .rodata.rev_warn_str, "aS"
+rev_warn_str:
+	.asciz "Warning: Skipping Errata workaround for non matching CPU revision number.\n"
+
+	.globl	print_revision_warning
+func print_revision_warning
+	mov	x5, x30
+	/* Ensure the console is initialized */
+	bl	plat_crash_console_init
+	/* Check if the console is initialized */
+	cbz	x0, 1f
+	/* The console is initialized */
+	adr	x4, rev_warn_str
+	bl	asm_print_str
+1:
+	ret	x5
+#endif
+