2 files changed, 23 insertions, 1 deletions

diff --git a/firmware/export/system.h b/firmware/export/system.h
index 56fee6bdab..f379b325d3 100644
--- a/firmware/export/system.h
+++ b/firmware/export/system.h
@@ -33,9 +33,11 @@ extern long cpu_frequency;
 #define FREQ cpu_frequency
 void set_cpu_frequency(long frequency);
 void cpu_boost(bool on_off);
+void cpu_idle_mode(bool on_off);
 #else
 #define FREQ CPU_FREQ
 #define cpu_boost(on_off)
+#define cpu_idle_mode(on_off)
 #endif
 
 #define BAUDRATE 9600
diff --git a/firmware/system.c b/firmware/system.c
index 66bd90f71b..c01d1270d7 100644
--- a/firmware/system.c
+++ b/firmware/system.c
@@ -30,6 +30,7 @@ long cpu_frequency = CPU_FREQ;
 
 #ifdef HAVE_ADJUSTABLE_CPU_FREQ
 int boost_counter = 0;
+bool cpu_idle = false;
 void cpu_boost(bool on_off)
 {
     if(on_off)
@@ -45,7 +46,10 @@ void cpu_boost(bool on_off)
         /* Lower the frequency if the counter reaches 0 */
         if(--boost_counter == 0)
         {
-            set_cpu_frequency(CPUFREQ_NORMAL);
+            if(cpu_idle)
+                set_cpu_frequency(CPUFREQ_DEFAULT);
+            else
+                set_cpu_frequency(CPUFREQ_NORMAL);
         }
 
         /* Safety measure */
@@ -53,6 +57,22 @@ void cpu_boost(bool on_off)
             boost_counter = 0;
     }
 }
+
+void cpu_idle_mode(bool on_off)
+{
+    cpu_idle = on_off;
+
+    /* We need to adjust the frequency immediately if the CPU
+       isn't boosted */
+    if(boost_counter == 0)
+    {
+        if(cpu_idle)
+            set_cpu_frequency(CPUFREQ_DEFAULT);
+        else
+            set_cpu_frequency(CPUFREQ_NORMAL);
+    }
+}
+
 #endif
 
 #if CONFIG_CPU == TCC730