arch/arm64/imx9: Fix cntfrq_el0 to correct value

Read base frequency from system counter0 and write it to arm core register. This corrects timers to work properly Then enable counting. Signed-off-by: Jouni Ukkonen <[email protected]>
apache · Nov 21, 2024 · 0858306 · 0858306
1 parent 1d04884
commit 0858306
Showing 1 changed file with 8 additions and 1 deletion.
diff --git a/arch/arm64/src/imx9/imx9_boot.c b/arch/arm64/src/imx9/imx9_boot.c
@@ -103,7 +103,14 @@ 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;
+
+  /* Read Frequency ID0 (24MHz) and write it to arm core */
+
+  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_FREQ1,
+              SC_CNTCR_FREQ0 | SC_CNTCR_ENABLE | SC_CNTCR_HDBG);
 #endif
 }