1 files changed, 52 insertions, 10 deletions

diff --git a/firmware/target/arm/tcc780x/timer-tcc780x.c b/firmware/target/arm/tcc780x/timer-tcc780x.c
index 17956131c0..ca6613a3ff 100644
--- a/firmware/target/arm/tcc780x/timer-tcc780x.c
+++ b/firmware/target/arm/tcc780x/timer-tcc780x.c
@@ -25,40 +25,82 @@
 #include "timer.h"
 #include "logf.h"
 
-/* Use the TC32 counter [sourced by Xin:12Mhz] for this timer, as it's the
-   only one that allows a 32-bit counter (Timer0-5 are 16/20 bit only). */
+static const int prescale_shifts[] = {1, 2, 3, 4, 5, 10, 12};
 
 bool __timer_set(long cycles, bool start)
 {
-    (void)cycles;
-    (void)start;
-    return false;
+    bool found = false;
+ 
+    int prescale_option = 0;
+    int actual_cycles = 0;
+ 
+    /* Use the first prescale that fits Timer4's 20-bit counter */
+    while (!found && prescale_option < 7)
+    {
+        actual_cycles = cycles >> prescale_shifts[prescale_option];
+ 
+        if (actual_cycles < 0x100000)
+            found = true;
+        else
+            prescale_option++;
+    }
+ 
+    if (!found)
+        return false;
+
+    /* Stop the timer and set new prescale & ref count */
+    TCFG(4) &= ~TCFG_EN;
+    TCFG(4) = prescale_option << TCFG_SEL;
+    TREF(4) = actual_cycles;
+    
+    if (start && pfn_unregister != NULL)
+    {
+        pfn_unregister();
+        pfn_unregister = NULL;
+    }
+    
+    return true;
 }
 
 bool __timer_register(void)
 {
-    return false;
+    int oldstatus = disable_interrupt_save(IRQ_STATUS);
+    
+    TCFG(4) |= TCFG_CLEAR | TCFG_IEN | TCFG_EN;
+    
+    restore_interrupt(oldstatus);
+    
+    return true;
 }
 
 void __timer_unregister(void)
 {
+    int oldstatus = disable_interrupt_save(IRQ_STATUS);
+    
+    TCFG(4) &= ~TCFG_EN;
+    
+    restore_interrupt(oldstatus);
 }
 
 
 /* Timer interrupt processing - all timers (inc. tick) have a single IRQ */
 void TIMER0(void)
 {
-    if (TIREQ & TF0)    /* Timer0 reached ref value */
+    if (TIREQ & TIREQ_TF0)    /* Timer0 reached ref value */
     {
         /* Run through the list of tick tasks */
         call_tick_tasks();
         
         /* reset Timer 0 IRQ & ref flags */
-        TIREQ |= TI0 | TF0;
+        TIREQ = TIREQ_TI0 | TIREQ_TF0;
     }
 
-    if (TC32IRQ & (1<<3))    /* end of TC32 prescale */
+    if (TIREQ & TIREQ_TF4)    /* Timer4 reached ref value */
     {
-        /* dispatch timer */
+        /* dispatch user timer */
+        if (pfn_timer != NULL)
+            pfn_timer();
+
+        TIREQ = TIREQ_TI4 | TIREQ_TF4;
     }
 }