Implement workaround for AT speculative behaviour

During context switching from higher EL (EL2 or higher)
to lower EL can cause incorrect translation in TLB due to
speculative execution of AT instruction using out-of-context
translation regime.

Workaround is implemented as below during EL's (EL1 or EL2)
"context_restore" operation:
1. Disable page table walk using SCTLR.M and TCR.EPD0 & EPD1
   bits for EL1 or EL2 (stage1 and stage2 disabled)
2. Save all system registers except TCR and SCTLR (for EL1 and EL2)
3. Do memory barrier operation (isb) to ensure all
   system register writes are done.
4. Restore TCR and SCTLR registers (for EL1 and EL2)

Errata details are available for various CPUs as below:
Cortex-A76: 1165522
Cortex-A72: 1319367
Cortex-A57: 1319537
Cortex-A55: 1530923
Cortex-A53: 1530924

More details can be found in mail-chain:
https://lists.trustedfirmware.org/pipermail/tf-a/2020-April/000445.html

Currently, Workaround is implemented as build option which is default
disabled.

Signed-off-by: Manish V Badarkhe <Manish.Badarkhe@arm.com>
Change-Id: If8545e61f782cb0c2dda7ffbaf50681c825bd2f0
diff --git a/lib/el3_runtime/aarch64/context.S b/lib/el3_runtime/aarch64/context.S
index 221f33e..984468a 100644
--- a/lib/el3_runtime/aarch64/context.S
+++ b/lib/el3_runtime/aarch64/context.S
@@ -234,6 +234,21 @@
  */
 func el2_sysregs_context_restore
 
+#if ERRATA_SPECULATIVE_AT
+/* Clear EPD0 and EPD1 bit and M bit to disable PTW */
+	mrs	x9, hcr_el2
+	tst	x9, #HCR_E2H_BIT
+	bne	1f
+	mrs	x9, tcr_el2
+	orr	x9, x9, #TCR_EPD0_BIT
+	orr	x9, x9, #TCR_EPD1_BIT
+	msr	tcr_el2, x9
+1:	mrs	x9, sctlr_el2
+	bic	x9, x9, #SCTLR_M_BIT
+	msr	sctlr_el2, x9
+	isb
+#endif
+
 	ldp	x9, x10, [x0, #CTX_ACTLR_EL2]
 	msr	actlr_el2, x9
 	msr	afsr0_el2, x10
@@ -282,17 +297,15 @@
 	msr	mair_el2, x15
 	msr	mdcr_el2, x16
 
-	ldp	x17, x9, [x0, #CTX_PMSCR_EL2]
+	ldr	x17, [x0, #CTX_PMSCR_EL2]
 	msr	PMSCR_EL2, x17
-	msr	sctlr_el2, x9
 
 	ldp	x10, x11, [x0, #CTX_SPSR_EL2]
 	msr	spsr_el2, x10
 	msr	sp_el2, x11
 
-	ldp	x12, x13, [x0, #CTX_TCR_EL2]
-	msr	tcr_el2, x12
-	msr	tpidr_el2, x13
+	ldr	x12, [x0, #CTX_TPIDR_EL2]
+	msr	tpidr_el2, x12
 
 	ldp	x14, x15, [x0, #CTX_TTBR0_EL2]
 	msr	ttbr0_el2, x14
@@ -404,6 +417,19 @@
 	msr	scxtnum_el2, x9
 #endif
 
+#if ERRATA_SPECULATIVE_AT
+/*
+ * Make sure all registers are stored successfully except
+ * SCTLR_EL2 and TCR_EL2
+ */
+	isb
+#endif
+
+	ldr	x9, [x0, #CTX_SCTLR_EL2]
+	msr	sctlr_el2, x9
+	ldr	x9, [x0, #CTX_TCR_EL2]
+	msr	tcr_el2, x9
+
 	ret
 endfunc el2_sysregs_context_restore
 
@@ -515,12 +541,22 @@
  */
 func el1_sysregs_context_restore
 
+#if ERRATA_SPECULATIVE_AT
+	mrs	x9, tcr_el1
+	orr	x9, x9, #TCR_EPD0_BIT
+	orr	x9, x9, #TCR_EPD1_BIT
+	msr	tcr_el1, x9
+	mrs	x9, sctlr_el1
+	bic	x9, x9, #SCTLR_M_BIT
+	msr	sctlr_el1, x9
+	isb
+#endif
+
 	ldp	x9, x10, [x0, #CTX_SPSR_EL1]
 	msr	spsr_el1, x9
 	msr	elr_el1, x10
 
-	ldp	x15, x16, [x0, #CTX_SCTLR_EL1]
-	msr	sctlr_el1, x15
+	ldr	x16, [x0, #CTX_ACTLR_EL1]
 	msr	actlr_el1, x16
 
 	ldp	x17, x9, [x0, #CTX_CPACR_EL1]
@@ -539,9 +575,8 @@
 	msr	mair_el1, x14
 	msr	amair_el1, x15
 
-	ldp	x16, x17, [x0, #CTX_TCR_EL1]
-	msr	tcr_el1, x16
-	msr	tpidr_el1, x17
+	ldr	x16,[x0, #CTX_TPIDR_EL1]
+	msr	tpidr_el1, x16
 
 	ldp	x9, x10, [x0, #CTX_TPIDR_EL0]
 	msr	tpidr_el0, x9
@@ -597,6 +632,19 @@
 	msr	GCR_EL1, x14
 #endif
 
+#if ERRATA_SPECULATIVE_AT
+/*
+ * Make sure all registers are stored successfully except
+ * SCTLR_EL1 and TCR_EL1
+ */
+	isb
+#endif
+
+	ldr	x9, [x0, #CTX_SCTLR_EL1]
+	msr	sctlr_el1, x9
+	ldr	x9, [x0, #CTX_TCR_EL1]
+	msr	tcr_el1, x9
+
 	/* No explict ISB required here as ERET covers it */
 	ret
 endfunc el1_sysregs_context_restore