ARM: versatile: Map local timers using Device Tree when possible

Try to map TWD registers basing on a "arm,*-twd-timer" Device Tree
node. This overrides existing twd_base value.

Signed-off-by: Pawel Moll <pawel.moll@arm.com>

1 files changed, 26 insertions, 0 deletions

diff --git a/arch/arm/plat-versatile/localtimer.c b/arch/arm/plat-versatile/localtimer.c
index 0fb3961999b5..e15668793159 100644
--- a/arch/arm/plat-versatile/localtimer.c
+++ b/arch/arm/plat-versatile/localtimer.c
@@ -11,16 +11,42 @@
 #include <linux/init.h>
 #include <linux/smp.h>
 #include <linux/clockchips.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
 
 #include <asm/smp_twd.h>
 #include <asm/localtimer.h>
 #include <mach/irqs.h>
 
+const static struct of_device_id twd_of_match[] __initconst = {
+       { .compatible = "arm,cortex-a9-twd-timer",      },
+       { .compatible = "arm,cortex-a5-twd-timer",      },
+       { .compatible = "arm,arm11mp-twd-timer",        },
+       { },
+};
+
 /*
  * Setup the local clock events for a CPU.
  */
 int __cpuinit local_timer_setup(struct clock_event_device *evt)
 {
+#if defined(CONFIG_OF)
+        static int dt_node_probed;
+
+        /* Look for TWD node only once */
+        if (!dt_node_probed) {
+                struct device_node *node = of_find_matching_node(NULL,
+                                twd_of_match);
+
+                if (node)
+                        twd_base = of_iomap(node, 0);
+
+                dt_node_probed = 1;
+        }
+#endif
+        if (!twd_base)
+                return -ENXIO;
+
         evt->irq = IRQ_LOCALTIMER;
         twd_timer_setup(evt);
         return 0;