1 --- a/arch/arm/mach-cns3xxx/core.c
2 +++ b/arch/arm/mach-cns3xxx/core.c
3 @@ -103,12 +103,13 @@ static void cns3xxx_timer_set_mode(enum
6 case CLOCK_EVT_MODE_PERIODIC:
7 - reload = pclk * 20 / (3 * HZ) * 0x25000;
8 + reload = pclk * 1000000 / HZ;
9 writel(reload, cns3xxx_tmr1 + TIMER1_AUTO_RELOAD_OFFSET);
10 ctrl |= (1 << 0) | (1 << 2) | (1 << 9);
12 case CLOCK_EVT_MODE_ONESHOT:
13 /* period set, and timer enabled in 'next_event' hook */
14 + writel(0, cns3xxx_tmr1 + TIMER1_AUTO_RELOAD_OFFSET);
15 ctrl |= (1 << 2) | (1 << 9);
17 case CLOCK_EVT_MODE_UNUSED:
18 @@ -136,7 +137,7 @@ static struct clock_event_device cns3xxx
19 .features = CLOCK_EVT_FEAT_PERIODIC | CLOCK_EVT_FEAT_ONESHOT,
20 .set_mode = cns3xxx_timer_set_mode,
21 .set_next_event = cns3xxx_timer_set_next_event,
24 .cpumask = cpu_all_mask,
27 @@ -183,6 +184,35 @@ static void __init cns3xxx_init_twd(void
31 +static cycle_t cns3xxx_get_cycles(struct clocksource *cs)
35 + val = readl(cns3xxx_tmr1 + TIMER_FREERUN_CONTROL_OFFSET);
38 + return ((val << 32) | readl(cns3xxx_tmr1 + TIMER_FREERUN_OFFSET));
41 +static struct clocksource clocksource_cns3xxx = {
44 + .read = cns3xxx_get_cycles,
45 + .mask = CLOCKSOURCE_MASK(48),
47 + .flags = CLOCK_SOURCE_IS_CONTINUOUS,
50 +static void __init cns3xxx_clocksource_init(void)
52 + /* Reset the FreeRunning counter */
53 + writel((1 << 16), cns3xxx_tmr1 + TIMER_FREERUN_CONTROL_OFFSET);
55 + clocksource_cns3xxx.mult =
56 + clocksource_khz2mult(100, clocksource_cns3xxx.shift);
57 + clocksource_register(&clocksource_cns3xxx);
61 * Set up the clock source and clock events devices
63 @@ -200,13 +230,12 @@ static void __init __cns3xxx_timer_init(
64 /* stop free running timer3 */
65 writel(0, cns3xxx_tmr1 + TIMER_FREERUN_CONTROL_OFFSET);
68 - writel(0x5C800, cns3xxx_tmr1 + TIMER1_COUNTER_OFFSET);
69 - writel(0x5C800, cns3xxx_tmr1 + TIMER1_AUTO_RELOAD_OFFSET);
71 writel(0, cns3xxx_tmr1 + TIMER1_MATCH_V1_OFFSET);
72 writel(0, cns3xxx_tmr1 + TIMER1_MATCH_V2_OFFSET);
74 + val = (cns3xxx_cpu_clock() >> 3) * 1000000 / HZ;
75 + writel(val, cns3xxx_tmr1 + TIMER1_COUNTER_OFFSET);
77 /* mask irq, non-mask timer1 overflow */
78 irq_mask = readl(cns3xxx_tmr1 + TIMER1_2_INTERRUPT_MASK_OFFSET);
79 irq_mask &= ~(1 << 2);
80 @@ -218,23 +247,9 @@ static void __init __cns3xxx_timer_init(
82 writel(val, cns3xxx_tmr1 + TIMER1_2_CONTROL_OFFSET);
85 - writel(0, cns3xxx_tmr1 + TIMER2_MATCH_V1_OFFSET);
86 - writel(0, cns3xxx_tmr1 + TIMER2_MATCH_V2_OFFSET);
89 - irq_mask = readl(cns3xxx_tmr1 + TIMER1_2_INTERRUPT_MASK_OFFSET);
90 - irq_mask |= ((1 << 3) | (1 << 4) | (1 << 5));
91 - writel(irq_mask, cns3xxx_tmr1 + TIMER1_2_INTERRUPT_MASK_OFFSET);
94 - val = readl(cns3xxx_tmr1 + TIMER1_2_CONTROL_OFFSET);
96 - writel(val, cns3xxx_tmr1 + TIMER1_2_CONTROL_OFFSET);
98 - /* Make irqs happen for the system timer */
99 setup_irq(timer_irq, &cns3xxx_timer_irq);
101 + cns3xxx_clocksource_init();
102 cns3xxx_clockevents_init(timer_irq);