1 files changed, 22 insertions, 30 deletions

diff --git a/firmware/target/arm/tms320dm320/timer-dm320.c b/firmware/target/arm/tms320dm320/timer-dm320.c
index 21449ed19f..482fef9f12 100644
--- a/firmware/target/arm/tms320dm320/timer-dm320.c
+++ b/firmware/target/arm/tms320dm320/timer-dm320.c
@@ -32,32 +32,16 @@ void TIMER0(void)
     IO_INTC_IRQ0 |= 1<<IRQ_TIMER0;
 }
 
-static void stop_timer(void)
-{
-    IO_INTC_EINT0 &= ~(1<<IRQ_TIMER0);
-
-    IO_INTC_IRQ0 |= 1<<IRQ_TIMER0;
-    
-    IO_TIMER0_TMMD = CONFIG_TIMER0_TMMD_STOP;
-}
-
 bool __timer_set(long cycles, bool start)
 {
     int oldlevel;
-    unsigned int divider;
-    /* taken from linux/arch/arm/mach-itdm320-20/time.c and timer-meg-fx.c */
+    unsigned int divider=cycles, prescaler=0;
 
-        /* Turn off all timers */
-    IO_TIMER0_TMMD = CONFIG_TIMER0_TMMD_STOP;
-        IO_TIMER1_TMMD = CONFIG_TIMER1_TMMD_STOP;
-        IO_TIMER2_TMMD = CONFIG_TIMER2_TMMD_STOP;
-        IO_TIMER3_TMMD = CONFIG_TIMER3_TMMD_STOP;
+    if(cycles<1)
+        return false;
 
-    /* Find the minimum factor that puts the counter in range 1-65535 */
-    unsigned int prescaler = (cycles + 65534) / 65535;
+    IO_TIMER0_TMMD = CONFIG_TIMER0_TMMD_STOP;
 
-    /* Test this by writing 1's to registers to see how many bits we have */
-    /* Maximum divider setting is x / 1024 / 65536 = x / 67108864 */
     if (start && pfn_unregister != NULL)
     {
         pfn_unregister();
@@ -66,13 +50,14 @@ bool __timer_set(long cycles, bool start)
 
     oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL);
 
-    /* Max prescale is 1023+1 */
-    for (divider = 0; prescaler > 1024; prescaler >>= 1, divider++);
+    /* Increase prescale values starting from 0 to make the cycle count fit */
+    while(divider>65535 && prescaler<1024)
+    {
+        prescaler++;
+        divider=cycles/(prescaler+1);
+    }
 
-    /* Setup the Prescalar */
     IO_TIMER0_TMPRSCL = prescaler;
-
-    /* Setup the Divisor */
     IO_TIMER0_TMDIV = divider;
 
     set_irq_level(oldlevel);
@@ -80,22 +65,29 @@ bool __timer_set(long cycles, bool start)
     return true;
 }
 
-bool __timer_register(void)
+static void stop_timer(void)
 {
-    bool retval = true;
+    IO_INTC_EINT0 &= ~(1<<IRQ_TIMER0);
+
+    IO_INTC_IRQ0 |= 1<<IRQ_TIMER0;
+    
+    IO_TIMER0_TMMD = CONFIG_TIMER0_TMMD_STOP;
+}
 
+bool __timer_register(void)
+{
     int oldstatus = set_interrupt_status(IRQ_FIQ_DISABLED, IRQ_FIQ_STATUS);
 
     stop_timer();
 
     /* Turn Timer0 to Free Run mode */
-        IO_TIMER0_TMMD = CONFIG_TIMER0_TMMD_FREE_RUN;
+    IO_TIMER0_TMMD = CONFIG_TIMER0_TMMD_FREE_RUN;
 
-        IO_INTC_EINT0 |= 1<<IRQ_TIMER0;
+    IO_INTC_EINT0 |= 1<<IRQ_TIMER0;
 
     set_interrupt_status(oldstatus, IRQ_FIQ_STATUS);
 
-    return retval;
+    return true;
 }
 
 void __timer_unregister(void)