1 files changed, 45 insertions, 6 deletions

diff --git a/arch/arm/mach-shmobile/setup-r8a7790.c b/arch/arm/mach-shmobile/setup-r8a7790.c
index 6acddc139d88..4c96dad21195 100644
--- a/arch/arm/mach-shmobile/setup-r8a7790.c
+++ b/arch/arm/mach-shmobile/setup-r8a7790.c
@@ -210,14 +210,53 @@ u32 __init r8a7790_read_mode_pins(void)
         return mode;
 }
 
+#define CNTCR 0
+#define CNTFID0 0x20
+
 void __init r8a7790_timer_init(void)
 {
-        void __iomem *cntcr;
-
-        /* make sure arch timer is started by setting bit 0 of CNTCT */
-        cntcr = ioremap(0xe6080000, PAGE_SIZE);
-        iowrite32(1, cntcr);
-        iounmap(cntcr);
+#ifdef CONFIG_ARM_ARCH_TIMER
+        u32 mode = r8a7790_read_mode_pins();
+        void __iomem *base;
+        int extal_mhz = 0;
+        u32 freq;
+
+        /* At Linux boot time the r8a7790 arch timer comes up
+         * with the counter disabled. Moreover, it may also report
+         * a potentially incorrect fixed 13 MHz frequency. To be
+         * correct these registers need to be updated to use the
+         * frequency EXTAL / 2 which can be determined by the MD pins.
+         */
+
+        switch (mode & (MD(14) | MD(13))) {
+        case 0:
+                extal_mhz = 15;
+                break;
+        case MD(13):
+                extal_mhz = 20;
+                break;
+        case MD(14):
+                extal_mhz = 26;
+                break;
+        case MD(13) | MD(14):
+                extal_mhz = 30;
+                break;
+        }
+
+        /* The arch timer frequency equals EXTAL / 2 */
+        freq = extal_mhz * (1000000 / 2);
+
+        /* Remap "armgcnt address map" space */
+        base = ioremap(0xe6080000, PAGE_SIZE);
+
+        /* Update registers with correct frequency */
+        iowrite32(freq, base + CNTFID0);
+        asm volatile("mcr p15, 0, %0, c14, c0, 0" : : "r" (freq));
+
+        /* make sure arch timer is started by setting bit 0 of CNTCR */
+        iowrite32(1, base + CNTCR);
+        iounmap(base);
+#endif /* CONFIG_ARM_ARCH_TIMER */
 
         shmobile_timer_init();
 }