--- a/arch/arm/mach-cns3xxx/core.c
+++ b/arch/arm/mach-cns3xxx/core.c
@@ -138,6 +138,7 @@ static int cns3xxx_set_oneshot(struct cl
 
 	/* period set, and timer enabled in 'next_event' hook */
 	ctrl |= (1 << 2) | (1 << 9);
+	writel(0, cns3xxx_tmr1 + TIMER1_AUTO_RELOAD_OFFSET);
 	writel(ctrl, cns3xxx_tmr1 + TIMER1_2_CONTROL_OFFSET);
 	return 0;
 }
@@ -148,7 +149,7 @@ static int cns3xxx_set_periodic(struct c
 	int pclk = cns3xxx_cpu_clock() / 8;
 	int reload;
 
-	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);
 	writel(ctrl, cns3xxx_tmr1 + TIMER1_2_CONTROL_OFFSET);
@@ -175,8 +176,8 @@ static struct clock_event_device cns3xxx
 	.set_state_oneshot	= cns3xxx_set_oneshot,
 	.tick_resume		= cns3xxx_shutdown,
 	.set_next_event		= cns3xxx_timer_set_next_event,
-	.rating			= 350,
-	.cpumask		= cpu_all_mask,
+	.rating			= 300,
+	.cpumask		= cpu_possible_mask,
 };
 
 static void __init cns3xxx_clockevents_init(unsigned int timer_irq)
@@ -220,6 +221,32 @@ static void __init cns3xxx_init_twd(void
 	twd_local_timer_register(&cns3xx_twd_local_timer);
 }
 
+static u64 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),
+	.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_register_khz(&clocksource_cns3xxx, 100);
+}
+
 /*
  * Set up the clock source and clock events devices
  */
@@ -237,13 +264,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);
@@ -255,23 +281,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);
 	cns3xxx_init_twd();
 }