From d8a5017ee0d47e860148d139bc5329083ac06515 Mon Sep 17 00:00:00 2001
From: Joseph Lo <josephl@nvidia.com>
Date: Fri, 17 Apr 2015 15:31:59 +0800
Subject: arm64: save/restore cptr_el3 and cpacr_el1 registers

CPTR_EL3 and CPACR_EL1 are the registers for controlling the trap level
and access right of the FPU/SIMD instructions. Need to save/restore them
in every power cycle to keep the settings consistent.

BRANCH=none
BUG=none
TEST=boot on smaug/foster, verify the cpu_on/off is ok as well

Change-Id: I96fc0e0d2620e72b6ae2ffe4d073c9328047dc01
Signed-off-by: Patrick Georgi <pgeorgi@chromium.org>
Original-Commit-Id: 73e8cc8f25922e7bc218d24fbf4f7c67e15e3057
Original-Change-Id: I51eed07b1bb8f6eb2715622ec5d5c3f80c3c8bdd
Original-Signed-off-by: Joseph Lo <josephl@nvidia.com>
Original-Reviewed-on: https://chromium-review.googlesource.com/266073
Original-Reviewed-by: Aaron Durbin <adurbin@chromium.org>
Original-Reviewed-by: Benson Leung <bleung@chromium.org>
Reviewed-on: http://review.coreboot.org/9981
Tested-by: build bot (Jenkins)
Reviewed-by: Marc Jones <marc.jones@se-eng.com>
---
 src/arch/arm64/include/arch/startup.h           |  4 +++-
 src/arch/arm64/include/armv8/arch/lib_helpers.h | 10 ++++++++++
 src/arch/arm64/stage_entry.S                    |  6 ++++++
 src/arch/arm64/startup.c                        |  5 ++++-
 4 files changed, 23 insertions(+), 2 deletions(-)

(limited to 'src')

diff --git a/src/arch/arm64/include/arch/startup.h b/src/arch/arm64/include/arch/startup.h
index 671c13e4cb..6e55190e8a 100644
--- a/src/arch/arm64/include/arch/startup.h
+++ b/src/arch/arm64/include/arch/startup.h
@@ -30,8 +30,10 @@
 #define SCR_INDEX			3
 #define VBAR_INDEX			4
 #define CNTFRQ_INDEX			5
+#define CPTR_INDEX			6
+#define CPACR_INDEX			7
 /* IMPORTANT!!! If any new element is added please update NUM_ELEMENTS */
-#define NUM_ELEMENTS			6
+#define NUM_ELEMENTS			8
 
 #ifndef __ASSEMBLY__
 
diff --git a/src/arch/arm64/include/armv8/arch/lib_helpers.h b/src/arch/arm64/include/armv8/arch/lib_helpers.h
index bb7300f2b7..315a0c0b03 100644
--- a/src/arch/arm64/include/armv8/arch/lib_helpers.h
+++ b/src/arch/arm64/include/armv8/arch/lib_helpers.h
@@ -261,6 +261,16 @@
 402:
 .endm
 
+/* Macro to read from an el1 register */
+.macro read_el1 xreg sysreg
+	mrs	\xreg, \sysreg\()_el1
+.endm
+
+/* Macro to write to an el1 register */
+.macro write_el1 sysreg xreg temp
+	msr	\sysreg\()_el1, \xreg
+.endm
+
 /* Macro to read from an el0 register */
 .macro read_el0 xreg sysreg
 	mrs	\xreg, \sysreg\()_el0
diff --git a/src/arch/arm64/stage_entry.S b/src/arch/arm64/stage_entry.S
index cfe8edd1fa..b7dc1b5b33 100644
--- a/src/arch/arm64/stage_entry.S
+++ b/src/arch/arm64/stage_entry.S
@@ -193,6 +193,12 @@ ENDPROC(__rmodule_entry)
 	get_element_addr CNTFRQ_INDEX
 	write_el0 cntfrq, x0, x1
 
+	get_element_addr CPTR_INDEX
+	write_el3 cptr, x0, x1
+
+	get_element_addr CPACR_INDEX
+	write_el1 cpacr, x0, x1
+
 	dsb sy
 	isb
 
diff --git a/src/arch/arm64/startup.c b/src/arch/arm64/startup.c
index 6a85940b1f..ba480bc0a6 100644
--- a/src/arch/arm64/startup.c
+++ b/src/arch/arm64/startup.c
@@ -46,9 +46,12 @@ void startup_save_cpu_data(void)
 	save_element(TTBR0_INDEX, raw_read_ttbr0_current());
 	save_element(VBAR_INDEX, raw_read_vbar_current());
 	save_element(CNTFRQ_INDEX, raw_read_cntfrq_el0());
+	save_element(CPACR_INDEX, raw_read_cpacr_el1());
 
-	if (get_current_el() == EL3)
+	if (get_current_el() == EL3) {
 		save_element(SCR_INDEX, raw_read_scr_el3());
+		save_element(CPTR_INDEX, raw_read_cptr_el3());
+	}
 
 	dcache_clean_by_mva(_arm64_startup_data,
 			    NUM_ELEMENTS * PER_ELEMENT_SIZE_BYTES);
-- 
cgit v1.2.3