openwrt-xburst/target/linux/cns3xxx/patches-2.6.39/102-cns3xxx_timers.patch

--- a/arch/arm/mach-cns3xxx/core.c
+++ b/arch/arm/mach-cns3xxx/core.c
@@ -117,12 +117,13 @@ static void cns3xxx_timer_set_mode(enum
 
 	switch (mode) {
 	case CLOCK_EVT_MODE_PERIODIC:
-		reload = pclk * 20 / (3 * HZ) * 0x25000;
+		reload = pclk * 1000000 / HZ;
 		writel(reload, cns3xxx_tmr1 + TIMER1_AUTO_RELOAD_OFFSET);
 		ctrl |= (1 << 0) | (1 << 2) | (1 << 9);
 		break;
 	case CLOCK_EVT_MODE_ONESHOT:
 		/* period set, and timer enabled in 'next_event' hook */
+		writel(0, cns3xxx_tmr1 + TIMER1_AUTO_RELOAD_OFFSET);
 		ctrl |= (1 << 2) | (1 << 9);
 		break;
 	case CLOCK_EVT_MODE_UNUSED:
@@ -147,11 +148,11 @@ static int cns3xxx_timer_set_next_event(
 
 static struct clock_event_device cns3xxx_tmr1_clockevent = {
 	.name		= "cns3xxx timer1",
-	.shift		= 8,
+	.shift		= 32,
 	.features       = CLOCK_EVT_FEAT_PERIODIC | CLOCK_EVT_FEAT_ONESHOT,
 	.set_mode	= cns3xxx_timer_set_mode,
 	.set_next_event	= cns3xxx_timer_set_next_event,
-	.rating		= 350,
+	.rating		= 300,
 	.cpumask	= cpu_all_mask,
 };
 
@@ -193,6 +194,35 @@ static struct irqaction cns3xxx_timer_ir
 	.handler	= cns3xxx_timer_interrupt,
 };
 
+static cycle_t cns3xxx_get_cycles(struct clocksource *cs)
+{
+  u64 val;
+
+  val = readl(cns3xxx_tmr1 + TIMER_FREERUN_CONTROL_OFFSET);
+  val &= 0xffff;
+
+  return ((val << 32) | readl(cns3xxx_tmr1 + TIMER_FREERUN_OFFSET));
+}
+
+static struct clocksource clocksource_cns3xxx = {
+	.name = "freerun",
+	.rating = 200,
+	.read = cns3xxx_get_cycles,
+	.mask = CLOCKSOURCE_MASK(48),
+	.shift  = 16,
+	.flags  = CLOCK_SOURCE_IS_CONTINUOUS,
+};
+
+static void __init cns3xxx_clocksource_init(void)
+{
+	/* Reset the FreeRunning counter */
+	writel((1 << 16), cns3xxx_tmr1 + TIMER_FREERUN_CONTROL_OFFSET);
+
+	clocksource_cns3xxx.mult =
+		clocksource_khz2mult(100, clocksource_cns3xxx.shift);
+	clocksource_register(&clocksource_cns3xxx);
+}
+
 /*
  * Set up the clock source and clock events devices
  */
@@ -210,13 +240,12 @@ static void __init __cns3xxx_timer_init(
 	/* stop free running timer3 */
 	writel(0, cns3xxx_tmr1 + TIMER_FREERUN_CONTROL_OFFSET);
 
-	/* timer1 */
-	writel(0x5C800, cns3xxx_tmr1 + TIMER1_COUNTER_OFFSET);
-	writel(0x5C800, cns3xxx_tmr1 + TIMER1_AUTO_RELOAD_OFFSET);
-
 	writel(0, cns3xxx_tmr1 + TIMER1_MATCH_V1_OFFSET);
 	writel(0, cns3xxx_tmr1 + TIMER1_MATCH_V2_OFFSET);
 
+	val = (cns3xxx_cpu_clock() >> 3) * 1000000 / HZ;
+	writel(val, cns3xxx_tmr1 + TIMER1_COUNTER_OFFSET);
+
 	/* mask irq, non-mask timer1 overflow */
 	irq_mask = readl(cns3xxx_tmr1 + TIMER1_2_INTERRUPT_MASK_OFFSET);
 	irq_mask &= ~(1 << 2);
@@ -228,23 +257,9 @@ static void __init __cns3xxx_timer_init(
 	val |= (1 << 9);
 	writel(val, cns3xxx_tmr1 + TIMER1_2_CONTROL_OFFSET);
 
-	/* timer2 */
-	writel(0, cns3xxx_tmr1 + TIMER2_MATCH_V1_OFFSET);
-	writel(0, cns3xxx_tmr1 + TIMER2_MATCH_V2_OFFSET);
-
-	/* mask irq */
-	irq_mask = readl(cns3xxx_tmr1 + TIMER1_2_INTERRUPT_MASK_OFFSET);
-	irq_mask |= ((1 << 3) | (1 << 4) | (1 << 5));
-	writel(irq_mask, cns3xxx_tmr1 + TIMER1_2_INTERRUPT_MASK_OFFSET);
-
-	/* down counter */
-	val = readl(cns3xxx_tmr1 + TIMER1_2_CONTROL_OFFSET);
-	val |= (1 << 10);
-	writel(val, cns3xxx_tmr1 + TIMER1_2_CONTROL_OFFSET);
-
-	/* Make irqs happen for the system timer */
 	setup_irq(timer_irq, &cns3xxx_timer_irq);
 
+	cns3xxx_clocksource_init();
 	cns3xxx_clockevents_init(timer_irq);
 }