Cortex-A57: Implement workaround for erratum 814670

Change-Id: Ice3dcba8c46cea070fd4ca3ffb32aedc840589ad
Signed-off-by: Ambroise Vincent <ambroise.vincent@arm.com>

3 files changed, 79 insertions, 2 deletions

diff --git a/lib/cpus/aarch32/cortex_a57.S b/lib/cpus/aarch32/cortex_a57.S
index 04942d30..63412766 100644
--- a/lib/cpus/aarch32/cortex_a57.S
+++ b/lib/cpus/aarch32/cortex_a57.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -123,6 +123,35 @@ func check_errata_813420
 	b	cpu_rev_var_ls
 endfunc check_errata_813420
 
+	/* ---------------------------------------------------
+	 * Errata Workaround for Cortex A57 Errata #814670.
+	 * This applies only to revision r0p0 of Cortex A57.
+	 * Inputs:
+	 * r0: variant[4:7] and revision[0:3] of current cpu.
+	 * Shall clobber: r0-r3
+	 * ---------------------------------------------------
+	 */
+func errata_a57_814670_wa
+	/*
+	 * Compare r0 against revision r0p0
+	 */
+	mov		r2, lr
+	bl		check_errata_814670
+	cmp		r0, #ERRATA_NOT_APPLIES
+	beq		1f
+	ldcopr16	r0, r1, CORTEX_A57_CPUACTLR
+	orr64_imm	r0, r1, CORTEX_A57_CPUACTLR_DIS_DMB_NULLIFICATION
+	stcopr16	r0, r1, CORTEX_A57_CPUACTLR
+	isb
+1:
+	bx		r2
+endfunc errata_a57_814670_wa
+
+func check_errata_814670
+	mov	r1, #0x00
+	b	cpu_rev_var_ls
+endfunc check_errata_814670
+
 	/* --------------------------------------------------------------------
 	 * Disable the over-read from the LDNP instruction.
 	 *
@@ -366,6 +395,11 @@ func cortex_a57_reset_func
 	bl	errata_a57_813420_wa
 #endif
 
+#if ERRATA_A57_814670
+	mov	r0, r4
+	bl	errata_a57_814670_wa
+#endif
+
 #if A57_DISABLE_NON_TEMPORAL_HINT
 	mov	r0, r4
 	bl	a57_disable_ldnp_overread
@@ -533,6 +567,7 @@ func cortex_a57_errata_report
 	report_errata ERRATA_A57_806969, cortex_a57, 806969
 	report_errata ERRATA_A57_813419, cortex_a57, 813419
 	report_errata ERRATA_A57_813420, cortex_a57, 813420
+	report_errata ERRATA_A57_814670, cortex_a57, 814670
 	report_errata A57_DISABLE_NON_TEMPORAL_HINT, cortex_a57, \
 		disable_ldnp_overread
 	report_errata ERRATA_A57_826974, cortex_a57, 826974
diff --git a/lib/cpus/aarch64/cortex_a57.S b/lib/cpus/aarch64/cortex_a57.S
index a8626716..6fa8506f 100644
--- a/lib/cpus/aarch64/cortex_a57.S
+++ b/lib/cpus/aarch64/cortex_a57.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2018, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2019, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -132,6 +132,34 @@ func check_errata_813420
 	b	cpu_rev_var_ls
 endfunc check_errata_813420
 
+	/* ---------------------------------------------------
+	 * Errata Workaround for Cortex A57 Errata #814670.
+	 * This applies only to revision r0p0 of Cortex A57.
+	 * Inputs:
+	 * x0: variant[4:7] and revision[0:3] of current cpu.
+	 * Shall clobber: x0-x17
+	 * ---------------------------------------------------
+	 */
+func errata_a57_814670_wa
+	/*
+	 * Compare x0 against revision r0p0
+	 */
+	mov	x17, x30
+	bl	check_errata_814670
+	cbz	x0, 1f
+	mrs	x1, CORTEX_A57_CPUACTLR_EL1
+	orr	x1, x1, #CORTEX_A57_CPUACTLR_EL1_DIS_DMB_NULLIFICATION
+	msr	CORTEX_A57_CPUACTLR_EL1, x1
+	isb
+1:
+	ret	x17
+endfunc errata_a57_814670_wa
+
+func check_errata_814670
+	mov	x1, #0x00
+	b	cpu_rev_var_ls
+endfunc check_errata_814670
+
 	/* --------------------------------------------------------------------
 	 * Disable the over-read from the LDNP instruction.
 	 *
@@ -366,6 +394,11 @@ func cortex_a57_reset_func
 	bl	errata_a57_813420_wa
 #endif
 
+#if ERRATA_A57_814670
+	mov	x0, x18
+	bl	errata_a57_814670_wa
+#endif
+
 #if A57_DISABLE_NON_TEMPORAL_HINT
 	mov	x0, x18
 	bl	a57_disable_ldnp_overread
@@ -537,6 +570,7 @@ func cortex_a57_errata_report
 	report_errata ERRATA_A57_806969, cortex_a57, 806969
 	report_errata ERRATA_A57_813419, cortex_a57, 813419
 	report_errata ERRATA_A57_813420, cortex_a57, 813420
+	report_errata ERRATA_A57_814670, cortex_a57, 814670
 	report_errata A57_DISABLE_NON_TEMPORAL_HINT, cortex_a57, \
 		disable_ldnp_overread
 	report_errata ERRATA_A57_826974, cortex_a57, 826974
diff --git a/lib/cpus/cpu-ops.mk b/lib/cpus/cpu-ops.mk
index 98d59ef5..d8555bc5 100644
--- a/lib/cpus/cpu-ops.mk
+++ b/lib/cpus/cpu-ops.mk
@@ -111,6 +111,10 @@ ERRATA_A57_813419	?=0
 # only to revision r0p0 of the Cortex A57 cpu.
 ERRATA_A57_813420	?=0
 
+# Flag to apply erratum 814670  workaround during reset. This erratum applies
+# only to revision r0p0 of the Cortex A57 cpu.
+ERRATA_A57_814670	?=0
+
 # Flag to apply erratum 826974 workaround during reset. This erratum applies
 # only to revision <= r1p1 of the Cortex A57 cpu.
 ERRATA_A57_826974	?=0
@@ -200,6 +204,10 @@ $(eval $(call add_define,ERRATA_A57_813419))
 $(eval $(call assert_boolean,ERRATA_A57_813420))
 $(eval $(call add_define,ERRATA_A57_813420))
 
+# Process ERRATA_A57_814670 flag
+$(eval $(call assert_boolean,ERRATA_A57_814670))
+$(eval $(call add_define,ERRATA_A57_814670))
+
 # Process ERRATA_A57_826974 flag
 $(eval $(call assert_boolean,ERRATA_A57_826974))
 $(eval $(call add_define,ERRATA_A57_826974))