6 files changed, 74 insertions, 52 deletions

diff --git a/firmware/target/arm/tcc780x/cowond2/lcd-cowond2.c b/firmware/target/arm/tcc780x/cowond2/lcd-cowond2.c
index 7b64493c0a..d0b7e9214e 100644
--- a/firmware/target/arm/tcc780x/cowond2/lcd-cowond2.c
+++ b/firmware/target/arm/tcc780x/cowond2/lcd-cowond2.c
@@ -116,33 +116,11 @@ static void lcd_write_reg(unsigned char reg, unsigned short val)
     restore_irq(level);
 }
 
-
-/*
-   TEMP: Rough millisecond delay routine used by the LCD panel init sequence.
-   PCK_TCT must first have been initialised to 2Mhz by calling clock_init().
-*/
-static void sleep_ms(unsigned int ms)
-{
-    /* disable timer */
-    TCFG1 = 0;
-
-    /* set Timer1 reference value based on 125kHz tick */
-    TREF1 = ms * 125;
-
-    /* single count, zero the counter, divider = 16 [2^(3+1)], enable */
-    TCFG1 = (1<<9) | (1<<8) | (3<<4) | 1;
-
-    /* wait until Timer1 ref reached */
-    while (!(TIREQ & TF1)) {};
-}
-
-
 static void lcd_display_on(void)
 {
     /* power on sequence as per the D2 firmware */
     GPIOA_SET = (1<<16);
-    
-    sleep_ms(10);
+    udelay(10000);
     
     lcd_write_reg(1,  0x1D);
     lcd_write_reg(2,  0x0);
@@ -164,14 +142,14 @@ static void lcd_display_on(void)
     lcd_write_reg(23, 0x0);
     lcd_write_reg(24, 0x0);
     lcd_write_reg(25, 0x0);
-    sleep_ms(10);
+    udelay(10000);
     
     lcd_write_reg(9,  0x4055);
     lcd_write_reg(10, 0x0);
-    sleep_ms(40);
+    udelay(40000);
     
     lcd_write_reg(10, 0x2000);
-    sleep_ms(40);
+    udelay(40000);
     
     lcd_write_reg(1,  0xC01D);
     lcd_write_reg(2,  0x204);
@@ -191,11 +169,11 @@ static void lcd_display_on(void)
     lcd_write_reg(23, 0x406);
     lcd_write_reg(24, 0x2);
     lcd_write_reg(25, 0x0);
-    sleep_ms(60);
+    udelay(60000);
     
     lcd_write_reg(9,  0xA55);
     lcd_write_reg(10, 0x111F);
-    sleep_ms(10);
+    udelay(10000);
 
     /* tell that we're on now */
     display_on = true;
@@ -210,10 +188,10 @@ static void lcd_display_off(void)
     lcd_write_reg(9,  0x55);
     lcd_write_reg(10, 0x1417);
     lcd_write_reg(5,  0x4003);
-    sleep_ms(10);
+    udelay(10000);
     
     lcd_write_reg(9, 0x0);
-    sleep_ms(10);
+    udelay(10000);
     
     /* kill power to LCD panel (unconfirmed) */
     GPIOA_CLEAR = (1<<16);
diff --git a/firmware/target/arm/tcc780x/kernel-tcc780x.c b/firmware/target/arm/tcc780x/kernel-tcc780x.c
index e5d96d288a..dee5e040e2 100644
--- a/firmware/target/arm/tcc780x/kernel-tcc780x.c
+++ b/firmware/target/arm/tcc780x/kernel-tcc780x.c
@@ -29,16 +29,13 @@
 void tick_start(unsigned int interval_in_ms)
 {
     /* disable Timer0 */
-    TCFG0 &= ~1;
+    TCFG(0) &= ~TCFG_EN;
 
     /* set counter reference value based on 1Mhz tick */
-    TREF0 = interval_in_ms * 1000;
+    TREF(0) = interval_in_ms * 1000;
 
     /* Timer0 = reset to 0, divide=2, IRQ enable, enable (continuous) */
-    TCFG0 = (1<<8) | (0<<4) | (1<<3) | 1;
-
-    /* Unmask timer IRQ */
-    IEN |= TIMER0_IRQ_MASK;
+    TCFG(0) = TCFG_CLEAR | (0 << TCFG_SEL) | TCFG_IEN | TCFG_EN;
 }
 
 /* NB: Since we are using a single timer IRQ, tick tasks are dispatched as
diff --git a/firmware/target/arm/tcc780x/system-target.h b/firmware/target/arm/tcc780x/system-target.h
index 0802bb92e0..6e2e7be980 100644
--- a/firmware/target/arm/tcc780x/system-target.h
+++ b/firmware/target/arm/tcc780x/system-target.h
@@ -35,10 +35,12 @@
 #define outw(a,b) (*(volatile unsigned short *) (b) = (a))
 
 /* TC32 is configured to 1MHz in clock_init() */
+#define USEC_TIMER  TC32MCNT
+
 static inline void udelay(unsigned usecs)
 {
-    unsigned stop = TC32MCNT + usecs;
-    while (TIME_BEFORE(TC32MCNT, stop));
+    unsigned stop = USEC_TIMER + usecs;
+    while (TIME_BEFORE(USEC_TIMER, stop));
 }
 
 #endif /* SYSTEM_TARGET_H */
diff --git a/firmware/target/arm/tcc780x/system-tcc780x.c b/firmware/target/arm/tcc780x/system-tcc780x.c
index 77ae3a4cd9..ab8a6cf218 100644
--- a/firmware/target/arm/tcc780x/system-tcc780x.c
+++ b/firmware/target/arm/tcc780x/system-tcc780x.c
@@ -229,11 +229,14 @@ static void clock_init(void)
         "nop      \n\t"
     );
     
-    /* configure PCK_TCT to 2Mhz (clock source 4 (Xin) divided by 6) */
+    /* Configure PCK_TCT to 2Mhz (Xin divided by 6) */
     PCLK_TCT = PCK_EN | (CKSEL_XIN<<24) | 5;
     
-    /* set TC32 timer to XIN divided by 12 (1MHz) */
+    /* Set TC32 timer to be our USEC_TIMER (Xin divided by 12 = 1MHz) */
     TC32EN = (1<<24) | 11;
+    
+    /* Unmask common timer IRQ (shared by tick and user timer) */
+    IEN |= TIMER0_IRQ_MASK;
 }
 #endif
 
diff --git a/firmware/target/arm/tcc780x/timer-target.h b/firmware/target/arm/tcc780x/timer-target.h
index 991d949447..10090ceddc 100644
--- a/firmware/target/arm/tcc780x/timer-target.h
+++ b/firmware/target/arm/tcc780x/timer-target.h
@@ -21,8 +21,8 @@
 #ifndef TIMER_TARGET_H
 #define TIMER_TARGET_H
 
-/* timers are based on XIN (12Mhz) */
-#define TIMER_FREQ (12000000)
+/* Timer is based on PCK_TCT (set to 2Mhz in system.c) */
+#define TIMER_FREQ (2000000)
 
 bool __timer_set(long cycles, bool set);
 bool __timer_register(void);
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;
     }
 }