[PATCH 6/7] arm: mediatek: enable gpt6 on boot up to make arch timer working

[Matthias Brugger <matthias.bgg@gmail.com>]

We enable GTP6 which ungates the arch timer clock. Apart we write the
frequency with which the timer is running in the CNTFREQ register.
In the future this should be done in the bootloader.

Signed-off-by: Matthias Brugger <matthias.bgg@gmail.com>
---
 arch/arm/mach-mediatek/mediatek.c |   27 +++++++++++++++++++++++++++
 1 file changed, 27 insertions(+)

diff --git a/arch/arm/mach-mediatek/mediatek.c b/arch/arm/mach-mediatek/mediatek.c
index f2acf07..b7c5c60 100644
--- a/arch/arm/mach-mediatek/mediatek.c
+++ b/arch/arm/mach-mediatek/mediatek.c
@@ -16,6 +16,32 @@
  */
 #include <linux/init.h>
 #include <asm/mach/arch.h>
+#include <linux/of.h>
+#include <linux/clk-provider.h>
+#include <linux/clocksource.h>
+
+
+#define GPT6_CON_MT65xx 0x10008060
+
+static void __init mediatek_timer_init(void)
+{
+	static void __iomem *gpt_base;
+
+	if (of_machine_is_compatible("mediatek,mt6589")) {
+		/* set cntfreq register which is not done in bootloader */
+		asm volatile("mcr p15, 0, %0, c14, c0, 0" : : "r" (13000000));
+
+		/* turn on GPT6 which ungates arch timer clocks */
+		gpt_base = ioremap(GPT6_CON_MT65xx, 0x04);
+	}
+
+	/* enabel clock and set to free-run */
+	if (gpt_base)
+		writel(0x31, gpt_base);
+
+	of_clk_init(NULL);
+	clocksource_of_init();
+};
 
 static const char * const mediatek_board_dt_compat[] = {
 	"mediatek,mt6589",
@@ -24,4 +50,5 @@ static const char * const mediatek_board_dt_compat[] = {
 
 DT_MACHINE_START(MEDIATEK_DT, "Mediatek Cortex-A7 (Device Tree)")
 	.dt_compat	= mediatek_board_dt_compat,
+	.init_time	= mediatek_timer_init,
 MACHINE_END
-- 
1.7.9.5