imx clocksource: add frequency change support

Signed-off-by: Sascha Hauer <s.hauer@pengutronix.de>

1 files changed, 13 insertions, 0 deletions

diff --git a/arch/arm/mach-imx/clocksource.c b/arch/arm/mach-imx/clocksource.c
index 1abe65ebde..6c5cc11410 100644
--- a/arch/arm/mach-imx/clocksource.c
+++ b/arch/arm/mach-imx/clocksource.c
@@ -32,6 +32,7 @@
 #include <common.h>
 #include <init.h>
 #include <clock.h>
+#include <notifier.h>
 #include <asm/arch/imx-regs.h>
 #include <asm/arch/clock.h>
 
@@ -48,6 +49,16 @@ static struct clocksource cs = {
 	.shift	= 10,
 };
 
+static int imx_clocksource_clock_change(struct notifier_block *nb, unsigned long event, void *data)
+{
+	cs.mult = clocksource_hz2mult(imx_get_gptclk(), cs.shift);
+	return 0;
+}
+
+static struct notifier_block imx_clock_notifier = {
+	.notifier_call = imx_clocksource_clock_change,
+};
+
 static int clocksource_init (void)
 {
 	int i;
@@ -74,6 +85,8 @@ static int clocksource_init (void)
 
 	init_clock(&cs);
 
+	clock_register_client(&imx_clock_notifier);
+
 	return 0;
 }