feat(armv8r): handle different timer init cases

Signed-off-by: Jose Martins <[email protected]>

Author: josecm

src/arch/armv8/armv8-r/vmm.c

@@ -15,24 +15,38 @@ static uint32_t timer_freq = 0;
 void vmm_arch_profile_init()
 {
     if (cpu_is_master()) {
-        /**
-         * Since there is no firmware in cortex-r platforms, we need to initialize the system
-         * counter.
-         */
-        volatile struct generic_timer_cntctrl* timer_ctl;
-        timer_ctl = (struct generic_timer_cntctrl*)mem_alloc_map_dev(&cpu()->as, SEC_HYP_PRIVATE,
-            platform.arch.generic_timer.base_addr, platform.arch.generic_timer.base_addr,
-            sizeof(struct generic_timer_cntctrl));
-
-        timer_ctl->CNTCR |= GENERIC_TIMER_CNTCTL_CNTCR_EN;
-        fence_ord_write();
-
-        timer_freq = timer_ctl->CNTDIF0;
-
-        mem_unmap(&cpu()->as, (vaddr_t)timer_ctl, sizeof(struct generic_timer_cntctrl), false);
+        unsigned long cur_cntfrq = sysreg_cntfrq_el0_read();
+        if (cur_cntfrq != 0UL) {
+            timer_freq = (uint32_t)cur_cntfrq;
+        } else {
+#ifdef PLAT_GENERIC_TIMER_FREQ_HZ
+            timer_freq = (uint32_t)PLAT_GENERIC_TIMER_FREQ_HZ;
+#else
+            if (platform.arch.generic_timer.base_addr == 0) {
+                ERROR("generic timer base_addr undefined; cannot init system counter");
+            } else {
+                volatile struct generic_timer_cntctrl* timer_ctl;
+                timer_ctl = (struct generic_timer_cntctrl*)mem_alloc_map_dev(&cpu()->as,
+                    SEC_HYP_PRIVATE, platform.arch.generic_timer.base_addr,
+                    platform.arch.generic_timer.base_addr, sizeof(struct generic_timer_cntctrl));
+
+                timer_ctl->CNTCR |= GENERIC_TIMER_CNTCTL_CNTCR_EN;
+                fence_ord_write();
+
+                timer_freq = (uint32_t)timer_ctl->CNTDIF0;
+
+                mem_unmap(&cpu()->as, (vaddr_t)timer_ctl, sizeof(struct generic_timer_cntctrl),
+                    false);
+            }
+#endif
+        }
     }
 
     cpu_sync_barrier(&cpu_glb_sync);
 
+    /* Program CNTFRQ_EL0 and verify. */
     sysreg_cntfrq_el0_write(timer_freq);
+    if (sysreg_cntfrq_el0_read() != (unsigned long)timer_freq) {
+        ERROR("failed to program CNTFRQ_EL0 to %u Hz", timer_freq);
+    }
 }