2 files changed, 59 insertions, 23 deletions

diff --git a/apps/debug_menu.c b/apps/debug_menu.c
index 7b81869e69..fcd7e01181 100644
--- a/apps/debug_menu.c
+++ b/apps/debug_menu.c
@@ -691,24 +691,6 @@ bool dbg_ports(void)
 
         switch(button)
         {
-        case BUTTON_UP:
-            cpu_boost(true);
-            snprintf(buf, sizeof(buf), "freq: %ld, IDECONFIG1: %08lx, IDECONFIG2: %08lx", FREQ, IDECONFIG1, IDECONFIG2);
-            splash(HZ, false, buf);
-            break;
-            
-        case BUTTON_DOWN:
-            cpu_boost(false);
-            snprintf(buf, sizeof(buf), "freq: %ld, IDECONFIG1: %08lx, IDECONFIG2: %08lx", FREQ, IDECONFIG1, IDECONFIG2);
-            splash(HZ, false, buf);
-            break;
-            
-        case BUTTON_SELECT:
-            set_cpu_frequency(CPUFREQ_DEFAULT);
-            snprintf(buf, sizeof(buf), "freq: %ld, IDECONFIG1: %08lx, IDECONFIG2: %08lx", FREQ, IDECONFIG1, IDECONFIG2);
-            splash(HZ, false, buf);
-            break;
-            
             case SETTINGS_CANCEL:
                 return false;
         }
@@ -811,6 +793,57 @@ bool dbg_ports(void)
 }
 #endif
 
+#ifdef HAVE_ADJUSTABLE_CPU_FREQ
+extern int boost_counter;
+bool dbg_cpufreq(void)
+{
+    char buf[128];
+    int line;
+    int button;
+
+#ifdef HAVE_LCD_BITMAP
+    lcd_setmargins(0, 0);
+#endif
+    lcd_clear_display();
+    lcd_setfont(FONT_SYSFIXED);
+
+    while(1)
+    {
+        line = 0;
+        
+        snprintf(buf, sizeof(buf), "Frequency: %ld", FREQ);
+        lcd_puts(0, line++, buf);
+
+        snprintf(buf, sizeof(buf), "boost_counter: %d", boost_counter);
+        lcd_puts(0, line++, buf);
+
+        lcd_update();
+        button = button_get_w_tmo(HZ/10);
+
+        switch(button)
+        {
+        case BUTTON_UP:
+            cpu_boost(true);
+            break;
+            
+        case BUTTON_DOWN:
+            cpu_boost(false);
+            break;
+            
+        case BUTTON_SELECT:
+            set_cpu_frequency(CPUFREQ_DEFAULT);
+            boost_counter = 0;
+            break;
+            
+        case SETTINGS_CANCEL:
+            return false;
+        }
+    }
+
+    return false;
+}
+#endif
+
 #ifdef HAVE_RTC
 /* Read RTC RAM contents and display them */
 bool dbg_rtc(void)
@@ -1754,6 +1787,9 @@ bool debug_menu(void)
 #if CONFIG_CPU == SH7034 || CONFIG_CPU == MCF5249
         { "View I/O ports", dbg_ports },
 #endif
+#ifdef HAVE_ADJUSTABLE_CPU_FREQ
+        { "CPU frequency", dbg_cpufreq },
+#endif
 #if CONFIG_CPU == SH7034
 #ifdef HAVE_LCD_BITMAP
 #ifdef HAVE_RTC
diff --git a/firmware/system.c b/firmware/system.c
index 1cafc8f096..03414924ca 100644
--- a/firmware/system.c
+++ b/firmware/system.c
@@ -29,13 +29,13 @@ long cpu_frequency = CPU_FREQ;
 #endif
 
 #ifdef HAVE_ADJUSTABLE_CPU_FREQ
+int boost_counter = 0;
 void cpu_boost(bool on_off)
 {
-    static int counter = 0;
     if(on_off)
     {
         /* Boost the frequency if not already boosted */
-        if(counter++ == 0)
+        if(boost_counter++ == 0)
         {
             set_cpu_frequency(CPUFREQ_MAX);
         }
@@ -43,14 +43,14 @@ void cpu_boost(bool on_off)
     else
     {
         /* Lower the frequency if the counter reaches 0 */
-        if(--counter == 0)
+        if(--boost_counter == 0)
         {
             set_cpu_frequency(CPUFREQ_NORMAL);
         }
 
         /* Safety measure */
-        if(counter < 0)
-            counter = 0;
+        if(boost_counter < 0)
+            boost_counter = 0;
     }
 }
 #endif