5 files changed, 103 insertions, 0 deletions

diff --git a/firmware/export/config-gmini120.h b/firmware/export/config-gmini120.h
index 7b6af81a6b..473f1cfa7f 100644
--- a/firmware/export/config-gmini120.h
+++ b/firmware/export/config-gmini120.h
@@ -56,4 +56,7 @@
 
 #define HAVE_LED
 
+/* Define this if you have adjustable CPU frequency */
+#define HAVE_ADJUSTABLE_CPU_FREQ
+
 #endif
diff --git a/firmware/export/config-gminisp.h b/firmware/export/config-gminisp.h
index d85240bdd1..431cf2f3ef 100644
--- a/firmware/export/config-gminisp.h
+++ b/firmware/export/config-gminisp.h
@@ -50,4 +50,7 @@
 
 #define HAVE_LED
 
+/* Define this if you have adjustable CPU frequency */
+#define HAVE_ADJUSTABLE_CPU_FREQ
+
 #endif
diff --git a/firmware/export/config-h100.h b/firmware/export/config-h100.h
index 0083842770..9c7ba6c1d3 100644
--- a/firmware/export/config-h100.h
+++ b/firmware/export/config-h100.h
@@ -53,4 +53,7 @@
 /* Define this if you have a software controlled poweroff */
 #define HAVE_SW_POWEROFF
 
+/* Define this if you have adjustable CPU frequency */
+#define HAVE_ADJUSTABLE_CPU_FREQ
+
 #endif
diff --git a/firmware/export/system.h b/firmware/export/system.h
index f792a132e7..442072ba51 100644
--- a/firmware/export/system.h
+++ b/firmware/export/system.h
@@ -22,11 +22,18 @@
 
 #include "cpu.h"
 #include "config.h"
+#include "stdbool.h"
 
 extern void system_reboot (void);
 extern void system_init(void);
 
+extern long cpu_frequency;
+
+#ifdef HAVE_ADJUSTABLE_CPU_FREQ
+#define FREQ cpu_frequency
+#else
 #define FREQ CPU_FREQ
+#endif
 #define BAUDRATE 9600
 
 #ifndef NULL
@@ -188,6 +195,13 @@ static inline void invalidate_icache(void)
                  "movec.l %d0,%cacr");
 }
 
+#define CPUFREQ_DEFAULT CPU_FREQ
+#define CPUFREQ_NORMAL 47980800
+#define CPUFREQ_MAX 95961600
+
+void set_cpu_frequency(long frequency);
+void cpu_boost(bool on_off);
+
 #elif CONFIG_CPU == TCC730
 
 extern int smsc_version(void);
diff --git a/firmware/system.c b/firmware/system.c
index 258fcd8827..382a7568a0 100644
--- a/firmware/system.c
+++ b/firmware/system.c
@@ -22,6 +22,11 @@
 #include "lcd.h"
 #include "font.h"
 #include "system.h"
+#include "kernel.h"
+
+#ifndef SIMULATOR
+long cpu_frequency = CPU_FREQ;
+#endif
 
 #if CONFIG_CPU == TCC730
 
@@ -416,6 +421,81 @@ void system_init(void)
 {
 }
 
+void set_cpu_frequency (long) __attribute__ ((section (".icode")));
+void set_cpu_frequency(long frequency)
+{
+    switch(frequency)
+    {
+    case CPUFREQ_MAX:
+        DCR = (DCR & ~0x000001ff) | 1;   /* Refresh timer for bypass
+                                            frequency */
+        PLLCR &= ~1;  /* Bypass mode */
+        PLLCR = 0x11c8600d;
+        CSCR0 = 0x00000580; /* Flash: 1 wait state */
+        CSCR1 = 0x00001180; /* LCD: 4 wait states */
+        while(!(PLLCR & 0x80000000)) {}; /* Wait until the PLL has locked.
+                                            This may take up to 10ms! */
+        DCR = (DCR & ~0x000001ff) | 33;   /* Refresh timer */
+        cpu_frequency = CPUFREQ_MAX;
+        tick_start(1000/HZ);
+        IDECONFIG1 = (IDECONFIG1 & ~(7 << 10)) | (5 << 10); /* CS2Pre,Post */
+        IDECONFIG2 = (IDECONFIG2 & ~0x0000ff00) | (0 << 8); /* CS2wait */
+        break;
+        
+    case CPUFREQ_NORMAL:
+        DCR = (DCR & ~0x000001ff) | 1;   /* Refresh timer for bypass
+                                            frequency */
+        PLLCR &= ~1;  /* Bypass mode */
+        PLLCR = 0x10c86801;
+        CSCR0 = 0x00000180; /* Flash: 0 wait states */
+        CSCR1 = 0x00000980; /* LCD: 2 wait states */
+        while(!(PLLCR & 0x80000000)) {}; /* Wait until the PLL has locked.
+                                            This may take up to 10ms! */
+        DCR = (DCR & ~0x000001ff) | 10;   /* Refresh timer */
+        cpu_frequency = CPUFREQ_NORMAL;
+        tick_start(1000/HZ);
+        IDECONFIG1 = (IDECONFIG1 & ~(7 << 10)) | (5 << 10); /* CS2Pre,Post */
+        IDECONFIG2 = (IDECONFIG2 & ~0x0000ff00) | (0 << 8); /* CS2wait */
+        break;
+    default:
+        DCR = (DCR & ~0x000001ff) | 1;   /* Refresh timer for bypass
+                                            frequency */
+        PLLCR &= ~1;  /* Bypass mode */
+        CSCR0 = 0x00000180; /* Flash: 0 wait states */
+        CSCR1 = 0x00000180; /* LCD: 0 wait states */
+        cpu_frequency = CPU_FREQ;
+        tick_start(1000/HZ);
+        IDECONFIG1 = (IDECONFIG1 & ~(7 << 10)) | (1 << 10); /* CS2Pre,Post */
+        IDECONFIG2 = (IDECONFIG2 & ~0x0000ff00) | (0 << 8); /* CS2wait */
+        break;
+    }
+}
+
+void cpu_boost(bool on_off)
+{
+    static int counter = 0;
+    if(on_off)
+    {
+        /* Boost the frequency if not already boosted */
+        if(counter++ == 0)
+        {
+            set_cpu_frequency(CPUFREQ_MAX);
+        }
+    }
+    else
+    {
+        /* Lower the frequency if the counter reaches 0 */
+        if(--counter == 0)
+        {
+            set_cpu_frequency(CPUFREQ_NORMAL);
+        }
+
+        /* Safety measure */
+        if(counter < 0)
+            counter = 0;
+    }
+}
+
 #elif CONFIG_CPU == SH7034
 #include "led.h"
 #include "system.h"