From 7f164a83a9d9cade9917f448c629d85913ff10c6 Mon Sep 17 00:00:00 2001
From: Olivier Deprez <olivier.deprez@arm.com>
Date: Fri, 20 Mar 2020 14:22:05 +0100
Subject: [PATCH] context: TPIDR_EL2 register not saved/restored

TPIDR_EL2 is missing from the EL2 state register save/restore
sequence. This patch adds it to the context save restore routines.

Signed-off-by: Olivier Deprez <olivier.deprez@arm.com>
Change-Id: I35fc5ee82f97b72bcedac57c791312e7b3a45251
---
 include/lib/el3_runtime/aarch64/context.h |  6 ++++--
 lib/el3_runtime/aarch64/context.S         | 18 ++++++++++++------
 2 files changed, 16 insertions(+), 8 deletions(-)

diff --git a/include/lib/el3_runtime/aarch64/context.h b/include/lib/el3_runtime/aarch64/context.h
index e061950c8..0029658ac 100644
--- a/include/lib/el3_runtime/aarch64/context.h
+++ b/include/lib/el3_runtime/aarch64/context.h
@@ -183,7 +183,7 @@
 #define CTX_SPSR_EL2		U(0xd0)
 #define CTX_SP_EL2		U(0xd8)
 #define CTX_TCR_EL2		U(0xe0)
-#define CTX_TRFCR_EL2		U(0xe8)
+#define CTX_TPIDR_EL2		U(0xe8)
 #define CTX_TTBR0_EL2		U(0xf0)
 #define CTX_VBAR_EL2		U(0xf8)
 #define CTX_VMPIDR_EL2		U(0x100)
@@ -234,11 +234,13 @@
 #define CTX_VSESR_EL2		U(0x228)
 #define CTX_VSTCR_EL2		U(0x230)
 #define CTX_VSTTBR_EL2		U(0x238)
+#define CTX_TRFCR_EL2		U(0x240)
 
 // Starting with Armv8.5
-#define CTX_SCXTNUM_EL2		U(0x240)
+#define CTX_SCXTNUM_EL2		U(0x248)
 /* Align to the next 16 byte boundary */
 #define CTX_EL2_SYSREGS_END	U(0x250)
+
 #endif /* CTX_INCLUDE_EL2_REGS */
 
 /*******************************************************************************
diff --git a/lib/el3_runtime/aarch64/context.S b/lib/el3_runtime/aarch64/context.S
index 30ad7b7d1..221f33e06 100644
--- a/lib/el3_runtime/aarch64/context.S
+++ b/lib/el3_runtime/aarch64/context.S
@@ -100,7 +100,7 @@ func el2_sysregs_context_save
 	stp	x10, x11, [x0, #CTX_SPSR_EL2]
 
 	mrs	x12, tcr_el2
-	mrs	x13, TRFCR_EL2
+	mrs	x13, tpidr_el2
 	stp	x12, x13, [x0, #CTX_TCR_EL2]
 
 	mrs	x14, ttbr0_el2
@@ -204,11 +204,14 @@ func el2_sysregs_context_save
 
 	mrs	x16, vsttbr_el2
 	str	x16, [x0, #CTX_VSTTBR_EL2]
+
+	mrs	x17, TRFCR_EL2
+	str	x17, [x0, #CTX_TRFCR_EL2]
 #endif
 
 #if ARM_ARCH_AT_LEAST(8, 5)
-	mrs	x17, scxtnum_el2
-	str	x17, [x0, #CTX_SCXTNUM_EL2]
+	mrs	x9, scxtnum_el2
+	str	x9, [x0, #CTX_SCXTNUM_EL2]
 #endif
 
 	ret
@@ -289,7 +292,7 @@ func el2_sysregs_context_restore
 
 	ldp	x12, x13, [x0, #CTX_TCR_EL2]
 	msr	tcr_el2, x12
-	msr	TRFCR_EL2, x13
+	msr	tpidr_el2, x13
 
 	ldp	x14, x15, [x0, #CTX_TTBR0_EL2]
 	msr	ttbr0_el2, x14
@@ -391,11 +394,14 @@ func el2_sysregs_context_restore
 
 	ldr	x16, [x0, #CTX_VSTTBR_EL2]
 	msr	vsttbr_el2, x16
+
+	ldr	x17, [x0, #CTX_TRFCR_EL2]
+	msr	TRFCR_EL2, x17
 #endif
 
 #if ARM_ARCH_AT_LEAST(8, 5)
-	ldr	x17, [x0, #CTX_SCXTNUM_EL2]
-	msr	scxtnum_el2, x17
+	ldr	x9, [x0, #CTX_SCXTNUM_EL2]
+	msr	scxtnum_el2, x9
 #endif
 
 	ret