From 8a87d158539269ba64c9680b4181a95de3e30d57 Mon Sep 17 00:00:00 2001
From: Iurii Konovalenko <iurii.konovalenko@globallogic.com>
Date: Fri, 22 Jul 2016 21:47:25 +0530
Subject: [PATCH 2/2] arm: rmobile: Add timer initialization

When run Xvisor hypervisor, cores can't setup arch timer because of
modes mismatch. The only place to set up timer is u-boot.

Signed-off-by: Iurii Konovalenko <iurii.konovalenko@globallogic.com>
Signed-off-by: Anup Patel <anup@brainfault.org>
---
 board/renesas/lager/lager.c | 65 ++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 64 insertions(+), 1 deletion(-)

diff --git a/board/renesas/lager/lager.c b/board/renesas/lager/lager.c
index 672f691..9c8dba1 100644
--- a/board/renesas/lager/lager.c
+++ b/board/renesas/lager/lager.c
@@ -89,6 +89,65 @@ void arch_preboot_os(void)
 	mstp_setbits_le32(MSTPSR1, SMSTPCR1, TMU0_MSTP125);
 }
 
+#ifdef CONFIG_ARMV7_VIRT
+#define TIMER_BASE                  0xE6080000
+#define TIMER_CNTCR                 0x0
+#define TIMER_CNTFID0               0x20
+#define MODEMR                      0xE6160060
+#define BIT(x)                      (1 << (x))
+#define MD(nr)                      BIT(nr)
+
+static int shmobile_init_time(void)
+{
+    uint32_t freq;
+    int extal_mhz = 0;
+    unsigned int mode = readl(MODEMR);
+
+    /* 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);
+
+    /*
+     * Update the timer if it is either not running, or is not at the
+     * right frequency. The timer is only configurable in secure mode
+     * so this avoids an abort if the loader started the timer and
+     * entered the kernel in non-secure mode.
+     */
+
+    if ( (readl(TIMER_BASE + TIMER_CNTCR) & 1) == 0 ||
+            readl(TIMER_BASE + TIMER_CNTFID0) != freq ) {
+        /* Update registers with correct frequency */
+        writel(freq, TIMER_BASE + TIMER_CNTFID0);
+        asm volatile("mcr p15, 0, %0, c14, c0, 0" : : "r" (freq));
+
+       /* make sure arch timer is started by setting bit 0 of CNTCR */
+        writel(1, TIMER_BASE + TIMER_CNTCR);
+    }
+    return 0;
+}
+#endif
+
 DECLARE_GLOBAL_DATA_PTR;
 int board_init(void)
 {
@@ -119,6 +178,11 @@ int board_init(void)
 	gpio_set_value(GPIO_GP_5_31, 1);
 	udelay(1);
 
+#ifdef CONFIG_ARMV7_VIRT
+	/* init timer */
+	shmobile_init_time();
+#endif
+
 	return 0;
 }
 
@@ -212,7 +276,6 @@ void reset_cpu(ulong addr)
 #define LAGER_RST_CA7BAR_BAREN                 (1 << 4)
 #define LAGER_RST_CA15RESCNT_OFFSET            0x40
 #define LAGER_RST_CA7RESCNT_OFFSET             0x44
-#define		BIT(x)	(1 << (x))
 #define LAGER_INIT_SECONDARY_START             0xE63C0FFC
 #define LAGER_RST_BASE                         0xE6160000
 #define LAGER_RST_CA15BAR                      0xE6160020
-- 
1.9.1

