arch/arm64/imx9: Fix cntrfrq_el0 to correct value

Read frequency from system counter and write it to arm register. Signed-off-by: Jouni Ukkonen <[email protected]>
tiiuae · Sep 5, 2024 · 6eb7f5c · 6eb7f5c
1 parent f58914d
commit 6eb7f5c
Showing 1 changed file with 6 additions and 1 deletion.
diff --git a/arch/arm64/src/imx9/imx9_boot.c b/arch/arm64/src/imx9/imx9_boot.c
@@ -95,7 +95,12 @@ void arm64_el_init(void)
 #if (CONFIG_ARCH_ARM64_EXCEPTION_LEVEL == 3)
   /* At EL3, cntfrq_el0 is uninitialized. It must be set. */
 
-  write_sysreg(CONFIG_BOOTLOADER_SYS_CLOCK, cntfrq_el0);
+  uint32_t freq;
+
+  freq = getreg32(IMX9_SYS_CTR_CONTROL_BASE + SYS_CTR_CNTFID0);
+  write_sysreg(freq, cntfrq_el0);
+  modifyreg32(IMX9_SYS_CTR_CONTROL_BASE + SYS_CTR_CNTCR, SC_CNTCR_FREQ0 | SC_CNTCR_FREQ1,
+    SC_CNTCR_FREQ0 | SC_CNTCR_ENABLE | SC_CNTCR_HDBG);
 #endif
 }