[coreboot-gerrit] Patch set updated for coreboot: 94ff9d5 arm64: save and restore cntfrq for secondary cpus

[Patrick Georgi (pgeorgi@google.com)]

Patrick Georgi (pgeorgi at google.com) just uploaded a new patch set to gerrit, which you can find at http://review.coreboot.org/9921

-gerrit

commit 94ff9d5d6923a993bacd6256d0c665d72524272c
Author: Jimmy Huang <jimmy.huang at mediatek.com>
Date:   Wed Apr 1 18:27:12 2015 +0800

    arm64: save and restore cntfrq for secondary cpus
    
    CNTFRQ_EL0 can only be set in highest implemented exception level.
    Save and restore CNTFRQ_EL0 for secondary cpus in coreboot.
    
    This patch fix the error below:
    
    SANITY CHECK: Unexpected variation in cntfrq. Boot CPU:
    0x00000000c65d40, CPU1: 0x00000000000000
    
    BRANCH=none
    BUG=none
    TEST=boot to kernel on oak board and check secondary cpu's cntfrq.
         confirmed cpu1's cntfrq is same as boot cpu's.
    
    Change-Id: I9fbc3c82c2544f0b59ec34b1d631dadf4b9d40eb
    Signed-off-by: Patrick Georgi <pgeorgi at chromium.org>
    Original-Commit-Id: b47e4e649efc7f79f016522c7d8a240f98225598
    Original-Signed-off-by: Jimmy Huang <jimmy.huang at mediatek.com>
    Original-Change-Id: I2d71b0ccfe42e8a30cd1367d10b0f8993431ef8c
    Original-Reviewed-on: https://chromium-review.googlesource.com/264914
    Original-Reviewed-by: Aaron Durbin <adurbin at chromium.org>
---
 src/arch/arm64/armv8/lib/sysctrl.c              | 15 +++++++++++++++
 src/arch/arm64/include/arch/startup.h           |  3 ++-
 src/arch/arm64/include/armv8/arch/lib_helpers.h | 12 ++++++++++++
 src/arch/arm64/stage_entry.S                    |  3 +++
 src/arch/arm64/startup.c                        |  1 +
 5 files changed, 33 insertions(+), 1 deletion(-)

diff --git a/src/arch/arm64/armv8/lib/sysctrl.c b/src/arch/arm64/armv8/lib/sysctrl.c
index d783509..2b5390b 100644
--- a/src/arch/arm64/armv8/lib/sysctrl.c
+++ b/src/arch/arm64/armv8/lib/sysctrl.c
@@ -1051,3 +1051,18 @@ void raw_write_vbar(uint64_t vbar, uint32_t el)
 {
 	SWITCH_CASE_WRITE(raw_write_vbar, vbar, el);
 }
+
+uint32_t raw_read_cntfrq_el0(void)
+{
+	uint32_t cntfrq_el0;
+
+	__asm__ __volatile__("mrs %0, CNTFRQ_EL0\n\t" : "=r" (cntfrq_el0) : :
+			     "memory");
+	return cntfrq_el0;
+}
+
+void raw_write_cntfrq_el0(uint32_t cntfrq_el0)
+{
+	__asm__ __volatile__("msr CNTFRQ_EL0, %0\n\t" : : "r" (cntfrq_el0) :
+			     "memory");
+}
diff --git a/src/arch/arm64/include/arch/startup.h b/src/arch/arm64/include/arch/startup.h
index bb3a863..671c13e 100644
--- a/src/arch/arm64/include/arch/startup.h
+++ b/src/arch/arm64/include/arch/startup.h
@@ -29,8 +29,9 @@
 #define TTBR0_INDEX			2
 #define SCR_INDEX			3
 #define VBAR_INDEX			4
+#define CNTFRQ_INDEX			5
 /* IMPORTANT!!! If any new element is added please update NUM_ELEMENTS */
-#define NUM_ELEMENTS			5
+#define NUM_ELEMENTS			6
 
 #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 1e04d48..bb7300f 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 el0 register */
+.macro read_el0 xreg sysreg
+	mrs	\xreg, \sysreg\()_el0
+.endm
+
+/* Macro to write to an el0 register */
+.macro write_el0 sysreg xreg temp
+	msr	\sysreg\()_el0, \xreg
+.endm
+
 /* Macro to invalidate all stage 1 TLB entries for current EL */
 .macro tlbiall_current temp
 	switch_el \temp, 401f, 402f, 403f
@@ -547,6 +557,8 @@ uint64_t raw_read_vbar_current(void);
 void raw_write_vbar_current(uint64_t vbar);
 uint64_t raw_read_vbar(uint32_t el);
 void raw_write_vbar(uint64_t vbar, uint32_t el);
+uint32_t raw_read_cntfrq_el0(void);
+void raw_write_cntfrq_el0(uint32_t cntfrq_el0);
 
 /* Cache maintenance system instructions */
 void dccisw(uint64_t cisw);
diff --git a/src/arch/arm64/stage_entry.S b/src/arch/arm64/stage_entry.S
index fa2064e..6fdd20e 100644
--- a/src/arch/arm64/stage_entry.S
+++ b/src/arch/arm64/stage_entry.S
@@ -190,6 +190,9 @@ ENDPROC(__rmodule_entry)
 	get_element_addr VBAR_INDEX
 	write_current vbar, x0, x1
 
+	get_element_addr CNTFRQ_INDEX
+	write_el0 cntfrq, x0, x1
+
 	dsb sy
 	isb
 
diff --git a/src/arch/arm64/startup.c b/src/arch/arm64/startup.c
index d2b7691..6a85940 100644
--- a/src/arch/arm64/startup.c
+++ b/src/arch/arm64/startup.c
@@ -45,6 +45,7 @@ void startup_save_cpu_data(void)
 	save_element(TCR_INDEX, raw_read_tcr_current());
 	save_element(TTBR0_INDEX, raw_read_ttbr0_current());
 	save_element(VBAR_INDEX, raw_read_vbar_current());
+	save_element(CNTFRQ_INDEX, raw_read_cntfrq_el0());
 
 	if (get_current_el() == EL3)
 		save_element(SCR_INDEX, raw_read_scr_el3());