[PATCHv2 5/5] xen/arm: Add OMAP5 architected timer initialization codes.

[Chen Baozi <baozich@gmail.com>]

Signed-off-by: Chen Baozi <baozich@gmail.com>
---
 xen/arch/arm/platforms/omap5.c        | 72 +++++++++++++++++++++++++++++++++++
 xen/arch/arm/time.c                   |  7 +++-
 xen/include/asm-arm/platforms/omap5.h | 12 ++++++
 3 files changed, 90 insertions(+), 1 deletion(-)

diff --git a/xen/arch/arm/platforms/omap5.c b/xen/arch/arm/platforms/omap5.c
index 084e211..4791366 100644
--- a/xen/arch/arm/platforms/omap5.c
+++ b/xen/arch/arm/platforms/omap5.c
@@ -20,6 +20,77 @@
 #include <xen/config.h>
 #include <asm/platform.h>
 #include <asm/platforms/omap5.h>
+#include <xen/mm.h>
+#include <xen/vmap.h>
+
+static uint16_t num_den[8][2] = {
+    {         0,          0 },  /* not used */
+    {  26 *  64,  26 *  125 },  /* 12.0 Mhz */
+    {   2 * 768,   2 * 1625 },  /* 13.0 Mhz */
+    {         0,          0 },  /* not used */
+    { 130 *   8, 130 *   25 },  /* 19.2 Mhz */
+    {   2 * 384,   2 * 1625 },  /* 26.0 Mhz */
+    {   3 * 256,   3 * 1125 },  /* 27.0 Mhz */
+    { 130 *   4, 130 *   25 },  /* 38.4 Mhz */
+};
+
+/*
+ * The realtime counter also called master counter, is a free-running
+ * counter, which is related to real time. It produces the count used
+ * by the CPU local timer peripherals in teh MPU cluster. The timer counts
+ * at a rate of 6.144 MHz. Because the device operates on different clocks
+ * in different power modes, the master counter shifts operation between
+ * clocks, adjusting the increment per clock in hardware accordingly to
+ * maintain a constant count rate.
+ */
+static int omap5_init_time(void)
+{
+    void __iomem *ckgen_prm_base;
+    void __iomem *rt_ct_base;
+    unsigned int sys_clksel;
+    unsigned int num, den, frac1, frac2;
+
+    ckgen_prm_base = ioremap_attr(OMAP5_CKGEN_PRM_BASE,
+                                  0x20, PAGE_HYPERVISOR_NOCACHE);
+    if (!ckgen_prm_base) {
+        dprintk(XENLOG_ERR, "%s: PRM_BASE ioremap failed\n", __func__);
+        return -ENOMEM;
+    }
+
+    sys_clksel = ioreadl(ckgen_prm_base + OMAP5_CM_CLKSEL_SYS) &
+        ~SYS_CLKSEL_MASK;
+
+    iounmap(ckgen_prm_base);
+
+    rt_ct_base = ioremap_attr(REALTIME_COUNTER_BASE,
+                              0x20, PAGE_HYPERVISOR_NOCACHE);
+    if (!rt_ct_base) {
+        dprintk(XENLOG_ERR, "%s: REALTIME_COUNTER_BASE ioremap failed\n", __func__);
+        return -ENOMEM;
+    }
+
+    frac1 = ioreadl(rt_ct_base + INCREMENTER_NUMERATOR_OFFSET);
+    num = frac1 & ~NUMERATOR_DENUMERATOR_MASK;
+    if (num_den[sys_clksel][0] != num) {
+        frac1 &= NUMERATOR_DENUMERATOR_MASK;
+        frac1 |= num_den[sys_clksel][0];
+    }
+
+    frac2 = ioreadl(rt_ct_base + INCREMENTER_DENUMERATOR_RELOAD_OFFSET);
+    den = frac2 & ~NUMERATOR_DENUMERATOR_MASK;
+    if (num_den[sys_clksel][1] != num) {
+        frac2 &= NUMERATOR_DENUMERATOR_MASK;
+        frac2 |= num_den[sys_clksel][1];
+    }
+
+    iowritel(rt_ct_base + INCREMENTER_NUMERATOR_OFFSET, frac1);
+    iowritel(rt_ct_base + INCREMENTER_DENUMERATOR_RELOAD_OFFSET,
+             frac2 | PRM_FRAC_INCREMENTER_DENUMERATOR_RELOAD);
+
+    iounmap(rt_ct_base);
+
+    return 0;
+}
 
 static const char const *omap5_dt_compat[] __initdata =
 {
@@ -29,6 +100,7 @@ static const char const *omap5_dt_compat[] __initdata =
 
 PLATFORM_START(omap5, "TI OMAP5")
     .compatible = omap5_dt_compat,
+    .init_time = omap5_init_time,
 PLATFORM_END
 
 /*
diff --git a/xen/arch/arm/time.c b/xen/arch/arm/time.c
index 4ed7882..fd48a4b 100644
--- a/xen/arch/arm/time.c
+++ b/xen/arch/arm/time.c
@@ -104,6 +104,7 @@ int __init init_xen_time(void)
     struct dt_device_node *dev;
     int res;
     unsigned int i;
+    u32 rate;
 
     dev = dt_find_compatible_node(NULL, NULL, "arm,armv7-timer");
     if ( !dev )
@@ -134,7 +135,11 @@ int __init init_xen_time(void)
     if ( !cpu_has_gentimer )
         panic("CPU does not support the Generic Timer v1 interface.\n");
 
-    cpu_khz = READ_SYSREG32(CNTFRQ_EL0) / 1000;
+    res = dt_property_read_u32(dev, "clock-frequency", &rate);
+    if ( !res )
+	cpu_khz = rate / 1000;
+    else
+        cpu_khz = READ_SYSREG32(CNTFRQ_EL0) / 1000;
 
     boot_count = READ_SYSREG64(CNTPCT_EL0);
     printk("Using generic timer at %lu KHz\n", cpu_khz);
diff --git a/xen/include/asm-arm/platforms/omap5.h b/xen/include/asm-arm/platforms/omap5.h
index ff07d95..c657bae 100644
--- a/xen/include/asm-arm/platforms/omap5.h
+++ b/xen/include/asm-arm/platforms/omap5.h
@@ -1,6 +1,18 @@
 #ifndef __ASM_ARM_PLATFORMS_OMAP5_H
 #define __ASM_ASM_PLATFORMS_OMAP5_H
 
+#define REALTIME_COUNTER_BASE                 0x48243200
+#define INCREMENTER_NUMERATOR_OFFSET          0x10
+#define INCREMENTER_DENUMERATOR_RELOAD_OFFSET 0x14
+#define NUMERATOR_DENUMERATOR_MASK            0xfffff000
+#define PRM_FRAC_INCREMENTER_DENUMERATOR_RELOAD 0x00010000
+
+#define OMAP5_L4_WKUP                         0x4AE00000
+#define OMAP5_PRM_BASE                        (OMAP5_L4_WKUP + 0x6000)
+#define OMAP5_CKGEN_PRM_BASE                  (OMAP5_PRM_BASE + 0x100)
+#define OMAP5_CM_CLKSEL_SYS                   0x10
+#define SYS_CLKSEL_MASK                       0xfffffff8
+
 #endif /* __ASM_ARM_PLATFORMS_OMAP5_H */
 
 /*
-- 
1.8.1.4