| 1 | --- a/arch/arm/mach-cns3xxx/core.c |
| 2 | +++ b/arch/arm/mach-cns3xxx/core.c |
| 3 | @@ -117,12 +117,13 @@ static void cns3xxx_timer_set_mode(enum |
| 4 | |
| 5 | switch (mode) { |
| 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); |
| 11 | break; |
| 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); |
| 16 | break; |
| 17 | case CLOCK_EVT_MODE_UNUSED: |
| 18 | @@ -147,11 +148,11 @@ static int cns3xxx_timer_set_next_event( |
| 19 | |
| 20 | static struct clock_event_device cns3xxx_tmr1_clockevent = { |
| 21 | .name = "cns3xxx timer1", |
| 22 | - .shift = 8, |
| 23 | + .shift = 32, |
| 24 | .features = CLOCK_EVT_FEAT_PERIODIC | CLOCK_EVT_FEAT_ONESHOT, |
| 25 | .set_mode = cns3xxx_timer_set_mode, |
| 26 | .set_next_event = cns3xxx_timer_set_next_event, |
| 27 | - .rating = 350, |
| 28 | + .rating = 300, |
| 29 | .cpumask = cpu_all_mask, |
| 30 | }; |
| 31 | |
| 32 | @@ -193,6 +194,35 @@ static struct irqaction cns3xxx_timer_ir |
| 33 | .handler = cns3xxx_timer_interrupt, |
| 34 | }; |
| 35 | |
| 36 | +static cycle_t cns3xxx_get_cycles(struct clocksource *cs) |
| 37 | +{ |
| 38 | + u64 val; |
| 39 | + |
| 40 | + val = readl(cns3xxx_tmr1 + TIMER_FREERUN_CONTROL_OFFSET); |
| 41 | + val &= 0xffff; |
| 42 | + |
| 43 | + return ((val << 32) | readl(cns3xxx_tmr1 + TIMER_FREERUN_OFFSET)); |
| 44 | +} |
| 45 | + |
| 46 | +static struct clocksource clocksource_cns3xxx = { |
| 47 | + .name = "freerun", |
| 48 | + .rating = 200, |
| 49 | + .read = cns3xxx_get_cycles, |
| 50 | + .mask = CLOCKSOURCE_MASK(48), |
| 51 | + .shift = 16, |
| 52 | + .flags = CLOCK_SOURCE_IS_CONTINUOUS, |
| 53 | +}; |
| 54 | + |
| 55 | +static void __init cns3xxx_clocksource_init(void) |
| 56 | +{ |
| 57 | + /* Reset the FreeRunning counter */ |
| 58 | + writel((1 << 16), cns3xxx_tmr1 + TIMER_FREERUN_CONTROL_OFFSET); |
| 59 | + |
| 60 | + clocksource_cns3xxx.mult = |
| 61 | + clocksource_khz2mult(100, clocksource_cns3xxx.shift); |
| 62 | + clocksource_register(&clocksource_cns3xxx); |
| 63 | +} |
| 64 | + |
| 65 | /* |
| 66 | * Set up the clock source and clock events devices |
| 67 | */ |
| 68 | @@ -210,13 +240,12 @@ static void __init __cns3xxx_timer_init( |
| 69 | /* stop free running timer3 */ |
| 70 | writel(0, cns3xxx_tmr1 + TIMER_FREERUN_CONTROL_OFFSET); |
| 71 | |
| 72 | - /* timer1 */ |
| 73 | - writel(0x5C800, cns3xxx_tmr1 + TIMER1_COUNTER_OFFSET); |
| 74 | - writel(0x5C800, cns3xxx_tmr1 + TIMER1_AUTO_RELOAD_OFFSET); |
| 75 | - |
| 76 | writel(0, cns3xxx_tmr1 + TIMER1_MATCH_V1_OFFSET); |
| 77 | writel(0, cns3xxx_tmr1 + TIMER1_MATCH_V2_OFFSET); |
| 78 | |
| 79 | + val = (cns3xxx_cpu_clock() >> 3) * 1000000 / HZ; |
| 80 | + writel(val, cns3xxx_tmr1 + TIMER1_COUNTER_OFFSET); |
| 81 | + |
| 82 | /* mask irq, non-mask timer1 overflow */ |
| 83 | irq_mask = readl(cns3xxx_tmr1 + TIMER1_2_INTERRUPT_MASK_OFFSET); |
| 84 | irq_mask &= ~(1 << 2); |
| 85 | @@ -228,23 +257,9 @@ static void __init __cns3xxx_timer_init( |
| 86 | val |= (1 << 9); |
| 87 | writel(val, cns3xxx_tmr1 + TIMER1_2_CONTROL_OFFSET); |
| 88 | |
| 89 | - /* timer2 */ |
| 90 | - writel(0, cns3xxx_tmr1 + TIMER2_MATCH_V1_OFFSET); |
| 91 | - writel(0, cns3xxx_tmr1 + TIMER2_MATCH_V2_OFFSET); |
| 92 | - |
| 93 | - /* mask irq */ |
| 94 | - irq_mask = readl(cns3xxx_tmr1 + TIMER1_2_INTERRUPT_MASK_OFFSET); |
| 95 | - irq_mask |= ((1 << 3) | (1 << 4) | (1 << 5)); |
| 96 | - writel(irq_mask, cns3xxx_tmr1 + TIMER1_2_INTERRUPT_MASK_OFFSET); |
| 97 | - |
| 98 | - /* down counter */ |
| 99 | - val = readl(cns3xxx_tmr1 + TIMER1_2_CONTROL_OFFSET); |
| 100 | - val |= (1 << 10); |
| 101 | - writel(val, cns3xxx_tmr1 + TIMER1_2_CONTROL_OFFSET); |
| 102 | - |
| 103 | - /* Make irqs happen for the system timer */ |
| 104 | setup_irq(timer_irq, &cns3xxx_timer_irq); |
| 105 | |
| 106 | + cns3xxx_clocksource_init(); |
| 107 | cns3xxx_clockevents_init(timer_irq); |
| 108 | } |
| 109 | |
| 110 | |
