10 files changed, 98 insertions, 59 deletions

diff --git a/apps/debug_menu.c b/apps/debug_menu.c
index 329ba7d7ab..0d34b232c3 100644
--- a/apps/debug_menu.c
+++ b/apps/debug_menu.c
@@ -271,7 +271,8 @@ bool dbg_audio_thread(void)
         snprintf(buf, sizeof(buf), "track count: %d", track_count);
         lcd_puts(0, line++, buf);
 
-        snprintf(buf, sizeof(buf), "cpu freq: %dMHz", (int)FREQ/1000000+1);
+        snprintf(buf, sizeof(buf), "cpu freq: %dMHz",
+                 (int)((FREQ + 500000) / 1000000));
         lcd_puts(0, line++, buf);
 
         snprintf(buf, sizeof(buf), "boost ratio: %d%%",
@@ -682,7 +683,7 @@ bool dbg_spdif(void)
         valnogood = (interruptstat & 0x01000000)?true:false;
         symbolerr = (interruptstat & 0x00800000)?true:false;
         parityerr = (interruptstat & 0x00400000)?true:false;
-        
+
         snprintf(buf, sizeof(buf), "Val: %s Sym: %s Par: %s",
                  valnogood?"--":"OK",
                  symbolerr?"--":"OK",
diff --git a/apps/plugins/grayscale.c b/apps/plugins/grayscale.c
index cdb3f880e5..192dcc1738 100644
--- a/apps/plugins/grayscale.c
+++ b/apps/plugins/grayscale.c
@@ -164,6 +164,9 @@ int main(void)
     rb->lcd_puts(0, 0, pbuf);
     rb->lcd_update();
 
+#if !defined(SIMULATOR) && defined(HAVE_ADJUSTABLE_CPU_FREQ)
+    rb->cpu_boost(true);
+#endif
     gray_show(true);          /* switch on greyscale overlay */
 
     time = *rb->current_tick; /* start time measurement */
@@ -223,6 +226,9 @@ int main(void)
                  time / 100, time % 100);
     rb->lcd_puts(0, 0, pbuf);
     gray_deferred_lcd_update();       /* schedule an lcd_update() */
+#if !defined(SIMULATOR) && defined(HAVE_ADJUSTABLE_CPU_FREQ)
+    rb->cpu_boost(false);
+#endif
 
     /* drawing is now finished, play around with scrolling 
      * until you press OFF or connect USB
diff --git a/apps/plugins/lib/gray_core.c b/apps/plugins/lib/gray_core.c
index 02477b5b82..7fcd3dac80 100644
--- a/apps/plugins/lib/gray_core.c
+++ b/apps/plugins/lib/gray_core.c
@@ -226,38 +226,23 @@ void gray_release(void)
    lcd_set_invert_display(), lcd_set_flip(), lcd_roll() */
 void gray_show(bool enable)
 {
-#if (CONFIG_CPU == SH7034) && (CONFIG_LCD == LCD_SSD1815)
-    if (enable)
-    {
-        _gray_info.flags |= _GRAY_RUNNING;
-        _gray_rb->timer_register(1, NULL, FREQ / 67, 1, _timer_isr);
-        _gray_rb->screen_dump_set_hook(gray_screendump_hook);
-    }
-    else
-    {
-        _gray_rb->timer_unregister();
-        _gray_info.flags &= ~_GRAY_RUNNING;
-        _gray_rb->screen_dump_set_hook(NULL);
-        _gray_rb->lcd_update(); /* restore whatever there was before */
-    }
-#elif defined(CPU_COLDFIRE) && (CONFIG_LCD == LCD_S1D15E06)
     if (enable && !(_gray_info.flags & _GRAY_RUNNING))
     {
         _gray_info.flags |= _GRAY_RUNNING;
-        _gray_rb->cpu_boost(true);  /* run at 120 MHz to avoid freq changes */
-        _gray_rb->timer_register(1, NULL, *_gray_rb->cpu_frequency / 70, 1,
-                                 _timer_isr);
+#if CONFIG_LCD == LCD_SSD1815
+        _gray_rb->timer_register(1, NULL, CPU_FREQ / 67, 1, _timer_isr);
+#elif CONFIG_LCD == LCD_S1D15E06
+        _gray_rb->timer_register(1, NULL, CPU_FREQ / 70, 1, _timer_isr);
+#endif
         _gray_rb->screen_dump_set_hook(gray_screendump_hook);
     }
     else if (!enable && (_gray_info.flags & _GRAY_RUNNING))
     {
         _gray_rb->timer_unregister();
-        _gray_rb->cpu_boost(false);
         _gray_info.flags &= ~_GRAY_RUNNING;
         _gray_rb->screen_dump_set_hook(NULL);
         _gray_rb->lcd_update(); /* restore whatever there was before */
     }
-#endif
 }
 
 /* Update a rectangular area of the greyscale overlay */
diff --git a/apps/plugins/mandelbrot.c b/apps/plugins/mandelbrot.c
index 88f48d3f2b..8830d7ab33 100644
--- a/apps/plugins/mandelbrot.c
+++ b/apps/plugins/mandelbrot.c
@@ -370,6 +370,9 @@ enum plugin_status plugin_start(struct plugin_api* api, void* parameter)
     /* main loop */
     while (true) {
         if (redraw > REDRAW_NONE) {
+#if !defined(SIMULATOR) && defined(HAVE_ADJUSTABLE_CPU_FREQ)
+            rb->cpu_boost(true);
+#endif
             if (redraw == REDRAW_FULL)
                 gray_ub_clear_display();
 
@@ -378,6 +381,9 @@ enum plugin_status plugin_start(struct plugin_api* api, void* parameter)
             else
                 calc_mandelbrot_32();
 
+#if !defined(SIMULATOR) && defined(HAVE_ADJUSTABLE_CPU_FREQ)
+            rb->cpu_boost(false);
+#endif
             px_min = 0;
             px_max = LCD_WIDTH;
             py_min = 0;
diff --git a/firmware/backlight.c b/firmware/backlight.c
index 675237157b..ecd4403d88 100644
--- a/firmware/backlight.c
+++ b/firmware/backlight.c
@@ -81,7 +81,7 @@ static void backlight_isr(void)
     int timer_period;
     bool idle = false;
     
-    timer_period = FREQ / 1000 * BL_PWM_INTERVAL / 1000;
+    timer_period = CPU_FREQ / 1000 * BL_PWM_INTERVAL / 1000;
     switch (bl_dim_state) 
     {
       /* New cycle */
diff --git a/firmware/export/system.h b/firmware/export/system.h
index 58e6570d3a..c2246e1e70 100644
--- a/firmware/export/system.h
+++ b/firmware/export/system.h
@@ -257,14 +257,17 @@ static inline unsigned long SWAB32(unsigned long value)
 static inline void invalidate_icache(void)
 {
    asm volatile ("move.l #0x01000000,%d0\n"
-                 "movec.l %d0,%cacr\n"
+                 "movec.l %d0,%cacr\n"
                  "move.l #0x80000000,%d0\n"
-                 "movec.l %d0,%cacr");
+                 "movec.l %d0,%cacr");
 }
-
-#define CPUFREQ_DEFAULT CPU_FREQ
-#define CPUFREQ_NORMAL 47980800
-#define CPUFREQ_MAX 119952000
+            
+#define CPUFREQ_DEFAULT_MULT 1
+#define CPUFREQ_DEFAULT      (CPUFREQ_DEFAULT_MULT * CPU_FREQ)
+#define CPUFREQ_NORMAL_MULT  4
+#define CPUFREQ_NORMAL       (CPUFREQ_NORMAL_MULT * CPU_FREQ)
+#define CPUFREQ_MAX_MULT     11
+#define CPUFREQ_MAX          (CPUFREQ_MAX_MULT * CPU_FREQ)
 
 #elif CONFIG_CPU == TCC730
 
diff --git a/firmware/export/timer.h b/firmware/export/timer.h
index 73936ca28c..afd60ac66c 100644
--- a/firmware/export/timer.h
+++ b/firmware/export/timer.h
@@ -28,6 +28,9 @@
 bool timer_register(int reg_prio, void (*unregister_callback)(void),
                     long cycles, int int_prio, void (*timer_callback)(void));
 bool timer_set_period(long cycles);
+#ifdef CPU_COLDFIRE
+void timers_adjust_prescale(int multiplier, bool enable_irq);
+#endif
 void timer_unregister(void);
 
 #endif /* !SIMULATOR */
diff --git a/firmware/kernel.c b/firmware/kernel.c
index 44927cd1c3..ee4e37e50d 100644
--- a/firmware/kernel.c
+++ b/firmware/kernel.c
@@ -156,11 +156,11 @@ int queue_broadcast(long id, void *data)
 #if CONFIG_CPU == SH7034
 void tick_start(unsigned int interval_in_ms)
 {
-    unsigned int count;
+    unsigned long count;
 
-    count = FREQ * interval_in_ms / 1000 / 8;
+    count = CPU_FREQ * interval_in_ms / 1000 / 8;
 
-    if(count > 0xffff)
+    if(count > 0x10000)
     {
         panicf("Error! The tick interval is too long (%d ms)\n",
                interval_in_ms);
@@ -174,7 +174,7 @@ void tick_start(unsigned int interval_in_ms)
     TMDR &= ~0x01; /* Operate normally */
 
     TCNT0 = 0;   /* Start counting at 0 */
-    GRA0 = count;
+    GRA0 = (unsigned short)(count - 1);
     TCR0 = 0x23; /* Clear at GRA match, sysclock/8 */
 
     /* Enable interrupt on level 1 */
@@ -186,7 +186,7 @@ void tick_start(unsigned int interval_in_ms)
     TSTR |= 0x01; /* Start timer 1 */
 }
 
-#pragma interrupt
+void IMIA0(void) __attribute__ ((interrupt_handler));
 void IMIA0(void)
 {
     int i;
@@ -208,22 +208,28 @@ void IMIA0(void)
 #elif defined(CPU_COLDFIRE)
 void tick_start(unsigned int interval_in_ms)
 {
-    unsigned int count;
+    unsigned long count;
+    int prescale;
 
-    count = FREQ/2 * interval_in_ms / 1000 / 16;
+    count = CPU_FREQ/2 * interval_in_ms / 1000 / 16;
 
-    if(count > 0xffff)
+    if(count > 0x10000)
     {
         panicf("Error! The tick interval is too long (%d ms)\n",
                interval_in_ms);
         return;
     }
+
+    prescale = cpu_frequency / CPU_FREQ;
+    /* Note: The prescaler is later adjusted on-the-fly on CPU frequency
+       changes within timer.c */
     
     /* We are using timer 0 */
 
-    TRR0 = count; /* The reference count */
+    TRR0 = (unsigned short)(count - 1); /* The reference count */
     TCN0 = 0; /* reset the timer */
-    TMR0 = 0x001d; /* no prescaler, restart, CLK/16, enabled */
+    TMR0 = 0x001d | ((unsigned short)(prescale - 1) << 8); 
+           /* restart, CLK/16, enabled, prescaler */
 
     TER0 = 0xff; /* Clear all events */
 
diff --git a/firmware/system.c b/firmware/system.c
index 20dc7c5241..e2b4efca21 100644
--- a/firmware/system.c
+++ b/firmware/system.c
@@ -23,6 +23,7 @@
 #include "font.h"
 #include "system.h"
 #include "kernel.h"
+#include "timer.h"
 
 #ifndef SIMULATOR
 long cpu_frequency = CPU_FREQ;
@@ -503,11 +504,13 @@ void system_init(void)
 }
 
 #ifdef IRIVER_H100
-#define MAX_REFRESH_TIMER 56
-#define NORMAL_REFRESH_TIMER 20
+#define MAX_REFRESH_TIMER     59
+#define NORMAL_REFRESH_TIMER  21
+#define DEFAULT_REFRESH_TIMER 4
 #else
-#define MAX_REFRESH_TIMER 28
-#define NORMAL_REFRESH_TIMER 10
+#define MAX_REFRESH_TIMER     29
+#define NORMAL_REFRESH_TIMER  10
+#define DEFAULT_REFRESH_TIMER 1
 #endif
 
 void set_cpu_frequency (long) __attribute__ ((section (".icode")));
@@ -516,44 +519,46 @@ void set_cpu_frequency(long frequency)
     switch(frequency)
     {
     case CPUFREQ_MAX:
-        DCR = (DCR & ~0x01ff) | 1;   /* Refresh timer for bypass
-                                            frequency */
+        DCR = (DCR & ~0x01ff) | DEFAULT_REFRESH_TIMER;  
+              /* Refresh timer for bypass frequency */
         PLLCR &= ~1;  /* Bypass mode */
-        PLLCR = 0x11853005;
+        timers_adjust_prescale(CPUFREQ_DEFAULT_MULT, false);
+        PLLCR = 0x11856005;
         CSCR0 = 0x00000980; /* Flash: 2 wait state */
         CSCR1 = 0x00000980; /* LCD: 2 wait states */
         while(!(PLLCR & 0x80000000)) {}; /* Wait until the PLL has locked.
                                             This may take up to 10ms! */
+        timers_adjust_prescale(CPUFREQ_MAX_MULT, true);
         DCR = (DCR & ~0x01ff) | MAX_REFRESH_TIMER;   /* Refresh timer */
         cpu_frequency = CPUFREQ_MAX;
-        tick_start(1000/HZ);
         IDECONFIG1 = 0x106000 | (5 << 10); /* BUFEN2 enable + CS2Pre/CS2Post */
         IDECONFIG2 = 0x40000 | (1 << 8); /* TA enable + CS2wait */
         break;
         
     case CPUFREQ_NORMAL:
-        DCR = (DCR & ~0x01ff) | 1;   /* Refresh timer for bypass
-                                            frequency */
+        DCR = (DCR & ~0x01ff) | DEFAULT_REFRESH_TIMER;
+              /* Refresh timer for bypass frequency */
         PLLCR &= ~1;  /* Bypass mode */
-        PLLCR = 0x10886001;
+        timers_adjust_prescale(CPUFREQ_DEFAULT_MULT, false);
+        PLLCR = 0x1385e005;
         CSCR0 = 0x00000180; /* Flash: 0 wait states */
         CSCR1 = 0x00000180; /* LCD: 0 wait states */
         while(!(PLLCR & 0x80000000)) {}; /* Wait until the PLL has locked.
                                             This may take up to 10ms! */
+        timers_adjust_prescale(CPUFREQ_NORMAL_MULT, true);
         DCR = (DCR & ~0x01ff) | NORMAL_REFRESH_TIMER;   /* Refresh timer */
         cpu_frequency = CPUFREQ_NORMAL;
-        tick_start(1000/HZ);
         IDECONFIG1 = 0x106000 | (5 << 10); /* BUFEN2 enable + CS2Pre/CS2Post */
         IDECONFIG2 = 0x40000 | (0 << 8); /* TA enable + CS2wait */
         break;
     default:
-        DCR = (DCR & ~0x01ff) | 1;   /* Refresh timer for bypass
-                                            frequency */
+        DCR = (DCR & ~0x01ff) | DEFAULT_REFRESH_TIMER;  
+              /* Refresh timer for bypass frequency */
         PLLCR = 0x00000000;  /* Bypass mode */
+        timers_adjust_prescale(CPUFREQ_DEFAULT_MULT, true);
         CSCR0 = 0x00000180; /* Flash: 0 wait states */
         CSCR1 = 0x00000180; /* LCD: 0 wait states */
-        cpu_frequency = CPU_FREQ;
-        tick_start(1000/HZ);
+        cpu_frequency = CPUFREQ_DEFAULT;
         IDECONFIG1 = 0x106000 | (1 << 10); /* BUFEN2 enable + CS2Pre/CS2Post */
         IDECONFIG2 = 0x40000 | (0 << 8); /* TA enable + CS2wait */
         break;
diff --git a/firmware/timer.c b/firmware/timer.c
index 3e524ace35..8aff4eb6ee 100644
--- a/firmware/timer.c
+++ b/firmware/timer.c
@@ -28,7 +28,9 @@
 static int timer_prio = -1;
 static void (*pfn_timer)(void) = NULL;      /* timer callback */
 static void (*pfn_unregister)(void) = NULL; /* unregister callback */
-
+#ifdef CPU_COLDFIRE
+static int base_prescale;
+#endif
 
 /* interrupt handler */
 #if CONFIG_CPU == SH7034
@@ -93,16 +95,19 @@ static bool timer_set(long cycles, bool start)
     and_b(~0x01, &TSR4); /* clear an eventual interrupt */
 
 #elif defined CPU_COLDFIRE
-    if (prescale > 4096)
+    if (prescale > 4096/CPUFREQ_MAX_MULT)
         return false;
 
-    if (prescale > 256)
+    if (prescale > 256/CPUFREQ_MAX_MULT)
     {
         phi = 0x05;      /* prescale sysclk/16, timer enabled */
         prescale >>= 4;
     }
     else
         phi = 0x03;      /* prescale sysclk, timer enabled */
+        
+    base_prescale = prescale;
+    prescale *= (cpu_frequency / CPU_FREQ);
 
     if (start)
     {
@@ -125,7 +130,26 @@ static bool timer_set(long cycles, bool start)
     return true;
 }
 
-/* Register a user timer, called every <count> CPU cycles */
+#ifdef CPU_COLDFIRE
+void timers_adjust_prescale(int multiplier, bool enable_irq)
+{
+    /* tick timer */
+    TMR0 = (TMR0 & 0x00ef)
+         | ((unsigned short)(multiplier - 1) << 8)
+         | (enable_irq ? 0x10 : 0);
+
+    if (pfn_timer)
+    {
+        /* user timer */
+        int prescale = base_prescale * multiplier;
+        TMR1 = (TMR1 & 0x00ef)
+             | ((unsigned short)(prescale - 1) << 8)
+             | (enable_irq ? 0x10 : 0);
+    }
+}
+#endif
+
+/* Register a user timer, called every <cycles> CPU_FREQ cycles */
 bool timer_register(int reg_prio, void (*unregister_callback)(void),
                     long cycles, int int_prio, void (*timer_callback)(void))
 {