clocksource: msc313e: WIP changes

arch/arm/boot/dts/mstar-v7.dtsi

@@ -130,14 +130,14 @@
 				clocks = <&xtal_div2>;
 				interrupts-extended = <&intc_fiq GIC_SPI 1 IRQ_TYPE_LEVEL_HIGH>;
 			};
-/*
+
 			timer2: timer@60c0 {
 				compatible = "mstar,msc313e-timer";
 				reg = <0x60c0 0x40>;
 				clocks = <&xtal_div2>;
 				interrupts-extended = <&intc_fiq GIC_SPI 12 IRQ_TYPE_LEVEL_HIGH>;
 			};
-*/
+
 			intc_fiq: interrupt-controller@201310 {
 				compatible = "mstar,mst-intc";
 				reg = <0x201310 0x40>;

drivers/clocksource/timer-msc313e.c

@@ -16,6 +16,7 @@
 #include <linux/of.h>
 #include <linux/of_address.h>
 #include <linux/of_irq.h>
+#include <linux/delay.h>
 
 #include "timer-of.h"
 
@@ -36,8 +37,14 @@ struct msc313e_clksrc {
 	struct clocksource clksrc;
 };
 
+struct msc313e_delay {
+	void __iomem * base;
+	struct delay_timer delay;
+};
+
 static struct timer_of *msc313e_clkevt;
 static void __iomem *msc313e_clksrc;
+static struct msc313e_delay *msc313e_delay;
 
 #define to_msc313e_timer(ptr) container_of(ptr, struct msc313e_timer, clksrc)
 
@@ -123,6 +130,17 @@ static u64 msc313e_timer_read(struct clocksource *cs)
 	return result & cs->mask;
 }
 
+static unsigned long msc313e_read_delay_timer_read(void)
+{
+	unsigned long result;
+
+	result = readw(msc313e_delay->base + REG_COUNTER_LOW);
+	result |= readw(msc313e_delay->base + REG_COUNTER_HIGH) << 16;
+
+	return result;
+}
+
+
 #if 0
 static u64 msc313e_timer_sched_clock_read(void)
 {
@@ -185,7 +203,6 @@ static int __init msc313e_clkevt_init(struct device_node *np)
 
 	clockevents_config_and_register(&msc313e_clkevt->clkevt, timer_of_rate(msc313e_clkevt),
 					TIMER_SYNC_TICKS, 0xffffffff);
-	pr_info("%s: OK\n", __func__);
 	return 0;
 }
 
@@ -204,9 +221,38 @@ static int __init msc313e_clksrc_init(struct device_node *np)
 	reg = readw(msc313e_clksrc + REG_CTRL);
 	reg |= CTRL_ENABLE;
 	writew(reg, msc313e_clksrc + REG_CTRL);
+
 	return clocksource_mmio_init(timer_of_base(&to), np->name, timer_of_rate(&to), 200, 32, msc313e_timer_read);
 }
 
+static int __init msc313e_delay_init(struct device_node *np)
+{
+	struct timer_of to = { 0 };
+	int ret;
+	u16 reg;
+
+	to.flags = TIMER_OF_BASE | TIMER_OF_CLOCK;
+	ret = timer_of_init(np, &to);
+	if (ret)
+		return ret;
+
+	msc313e_delay = kzalloc(sizeof(struct msc313e_delay), GFP_KERNEL);
+	if (!msc313e_delay)
+		return -ENOMEM;
+
+	msc313e_delay->base = timer_of_base(&to);
+	msc313e_delay->delay.read_current_timer = msc313e_read_delay_timer_read;
+	msc313e_delay->delay.freq = timer_of_rate(&to);
+
+	reg = readw(msc313e_delay->base + REG_CTRL);
+	reg |= CTRL_ENABLE;
+	writew(reg, msc313e_delay->base + REG_CTRL);
+
+	register_current_timer_delay(&msc313e_delay->delay);
+
+	return 0;
+}
+
 #if 0
 static int __init msc313e_timer_probe(struct device_node *node)
 {
@@ -278,10 +324,10 @@ static int __init msc313e_timer_init(struct device_node *np)
 
 	if (!msc313e_clksrc)
 		return msc313e_clksrc_init(np);
-#if 0
+
 	if (!msc313e_delay)
 		return msc313e_delay_init(np);
-#endif
+
 	return -EINVAL;
 }