diff options
| -rw-r--r-- | src/arch/arm64/armv8/lib/sysctrl.c | 15 | ||||
| -rw-r--r-- | src/arch/arm64/include/arch/startup.h | 3 | ||||
| -rw-r--r-- | src/arch/arm64/include/armv8/arch/lib_helpers.h | 12 | ||||
| -rw-r--r-- | src/arch/arm64/stage_entry.S | 3 | ||||
| -rw-r--r-- | src/arch/arm64/startup.c | 1 |
5 files changed, 33 insertions, 1 deletions
diff --git a/src/arch/arm64/armv8/lib/sysctrl.c b/src/arch/arm64/armv8/lib/sysctrl.c index d78350920f..2b5390bd66 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 bb3a863754..671c13e4cb 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 1e04d48cfa..bb7300f2b7 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 fa2064e708..6fdd20e992 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 d2b76913d2..6a85940b1f 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()); |