[components/drivers]update hwtimer (#8565)

1 files changed, 73 insertions, 7 deletions

diff --git a/libcpu/aarch64/common/setup.c b/libcpu/aarch64/common/setup.c
index 3ecddbc523..c74976b499 100644
--- a/libcpu/aarch64/common/setup.c
+++ b/libcpu/aarch64/common/setup.c
@@ -24,11 +24,6 @@
 #include <ioremap.h>
 #include <rtdevice.h>
 
-#define rt_sysreg_write(sysreg, val) \
-    __asm__ volatile ("msr "RT_STRINGIFY(sysreg)", %0"::"r"((rt_uint64_t)(val)))
-
-#define rt_sysreg_read(sysreg, val) \
-    __asm__ volatile ("mrs %0, "RT_STRINGIFY(sysreg)"":"=r"((val)))
 #define SIZE_KB  1024
 #define SIZE_MB (1024 * SIZE_KB)
 #define SIZE_GB (1024 * SIZE_MB)
@@ -73,6 +68,68 @@ void rt_hw_fdt_install_early(void *fdt)
     }
 }
 
+#ifdef RT_USING_HWTIMER
+static rt_ubase_t loops_per_tick[RT_CPUS_NR];
+
+static rt_ubase_t cpu_get_cycles(void)
+{
+    rt_ubase_t cycles;
+
+    rt_hw_sysreg_read(cntpct_el0, cycles);
+
+    return cycles;
+}
+
+static void cpu_loops_per_tick_init(void)
+{
+    rt_ubase_t offset;
+    volatile rt_ubase_t freq, step, cycles_end1, cycles_end2;
+    volatile rt_uint32_t cycles_count1 = 0, cycles_count2 = 0;
+
+    rt_hw_sysreg_read(cntfrq_el0, freq);
+    step = freq / RT_TICK_PER_SECOND;
+
+    cycles_end1 = cpu_get_cycles() + step;
+
+    while (cpu_get_cycles() < cycles_end1)
+    {
+        __asm__ volatile ("nop");
+        __asm__ volatile ("add %0, %0, #1":"=r"(cycles_count1));
+    }
+
+    cycles_end2 = cpu_get_cycles() + step;
+
+    while (cpu_get_cycles() < cycles_end2)
+    {
+        __asm__ volatile ("add %0, %0, #1":"=r"(cycles_count2));
+    }
+
+    if ((rt_int32_t)(cycles_count2 - cycles_count1) > 0)
+    {
+        offset = cycles_count2 - cycles_count1;
+    }
+    else
+    {
+        /* Impossible, but prepared for any eventualities */
+        offset = cycles_count2 / 4;
+    }
+
+    loops_per_tick[rt_hw_cpu_id()] = offset;
+}
+
+static void cpu_us_delay(rt_uint32_t us)
+{
+    volatile rt_base_t start = cpu_get_cycles(), cycles;
+
+    cycles = ((us * 0x10c7UL) * loops_per_tick[rt_hw_cpu_id()] * RT_TICK_PER_SECOND) >> 32;
+
+    while ((cpu_get_cycles() - start) < cycles)
+    {
+        rt_hw_cpu_relax();
+    }
+}
+#endif /* RT_USING_HWTIMER */
+
 rt_weak void rt_hw_idle_wfi(void)
 {
     __asm__ volatile ("wfi");
@@ -90,7 +147,7 @@ rt_inline void cpu_info_init(void)
     struct rt_ofw_node *np;
 
     /* get boot cpu info */
-    rt_sysreg_read(mpidr_el1, mpidr);
+    rt_hw_sysreg_read(mpidr_el1, mpidr);
 
     rt_ofw_foreach_cpu_node(np)
     {
@@ -128,6 +185,15 @@ rt_inline void cpu_info_init(void)
     }
 
     rt_hw_cpu_dcache_ops(RT_HW_CACHE_FLUSH, rt_cpu_mpidr_table, sizeof(rt_cpu_mpidr_table));
+
+#ifdef RT_USING_HWTIMER
+    cpu_loops_per_tick_init();
+
+    if (!rt_device_hwtimer_us_delay)
+    {
+        rt_device_hwtimer_us_delay = &cpu_us_delay;
+    }
+#endif /* RT_USING_HWTIMER */
 }
 
 rt_inline rt_bool_t is_kernel_aspace(const char *name)
@@ -421,7 +487,7 @@ rt_weak void rt_hw_secondary_cpu_bsp_start(void)
     rt_hw_spin_lock(&_cpus_lock);
 
     /* Save all mpidr */
-    rt_sysreg_read(mpidr_el1, rt_cpu_mpidr_table[cpu_id]);
+    rt_hw_sysreg_read(mpidr_el1, rt_cpu_mpidr_table[cpu_id]);
 
     rt_hw_mmu_ktbl_set((unsigned long)MMUTable);