3 files changed, 39 insertions, 2 deletions

diff --git a/firmware/export/config.h b/firmware/export/config.h
index 1b756cc6bd..6a3091de30 100644
--- a/firmware/export/config.h
+++ b/firmware/export/config.h
@@ -222,6 +222,7 @@
 
 #if (CONFIG_CODEC == SWCODEC) && !defined(SIMULATOR) && !defined(BOOTLOADER)
 #define HAVE_PRIORITY_SCHEDULING
+#define HAVE_SCHEDULER_BOOSTCTRL
 #endif
 
 /* define for all cpus from coldfire family */
diff --git a/firmware/export/thread.h b/firmware/export/thread.h
index cd532c8968..17e6e3aa88 100644
--- a/firmware/export/thread.h
+++ b/firmware/export/thread.h
@@ -131,12 +131,18 @@ struct thread_entry*
                           const char *name
                           IF_PRIO(, int priority));
 
+#ifdef HAVE_SCHEDULER_BOOSTCTRL
+void trigger_cpu_boost(void);
+#else
+#define trigger_cpu_boost()
+#endif
+
 void remove_thread(struct thread_entry *thread);
 void switch_thread(bool save_context, struct thread_entry **blocked_list);
 void sleep_thread(int ticks);
 void block_thread(struct thread_entry **thread, int timeout);
 void wakeup_thread(struct thread_entry **thread);
-void thread_set_priority(struct thread_entry *thread, int priority);
+int thread_set_priority(struct thread_entry *thread, int priority);
 void init_threads(void);
 int thread_stack_usage(const struct thread_entry *thread);
 int thread_get_status(const struct thread_entry *thread);
diff --git a/firmware/thread.c b/firmware/thread.c
index 205375a44d..6a94a52333 100644
--- a/firmware/thread.c
+++ b/firmware/thread.c
@@ -35,6 +35,9 @@ struct core_entry cores[NUM_CORES] IBSS_ATTR;
 #ifdef HAVE_PRIORITY_SCHEDULING
 static unsigned short highest_priority IBSS_ATTR;
 #endif
+#ifdef HAVE_SCHEDULER_BOOSTCTRL
+static bool cpu_boosted IBSS_ATTR;
+#endif
 
 /* Define to enable additional checks for blocking violations etc. */
 // #define THREAD_EXTRA_CHECKS
@@ -332,6 +335,14 @@ static inline void sleep_core(void)
         if (cores[CURRENT_CORE].running != NULL)
             break;
 
+#ifdef HAVE_SCHEDULER_BOOSTCTRL
+        if (cpu_boosted)
+        {
+            cpu_boost(false);
+            cpu_boosted = false;
+        }
+#endif
+        
         /* Enter sleep mode to reduce power usage, woken up on interrupt */
 #ifdef CPU_COLDFIRE
         asm volatile ("stop #0x2000");
@@ -646,6 +657,17 @@ struct thread_entry*
     return thread;
 }
 
+#ifdef HAVE_SCHEDULER_BOOSTCTRL
+void trigger_cpu_boost(void)
+{
+    if (!cpu_boosted)
+    {
+        cpu_boost(true);
+        cpu_boosted = true;
+    }
+}
+#endif
+
 /*---------------------------------------------------------------------------
  * Remove a thread on the current core from the scheduler.
  * Parameter is the ID as returned from create_thread().
@@ -676,13 +698,18 @@ void remove_thread(struct thread_entry *thread)
 }
 
 #ifdef HAVE_PRIORITY_SCHEDULING
-void thread_set_priority(struct thread_entry *thread, int priority)
+int thread_set_priority(struct thread_entry *thread, int priority)
 {
+    int old_priority;
+    
     if (thread == NULL)
         thread = cores[CURRENT_CORE].running;
     
+    old_priority = thread->priority;
     thread->priority = priority;
     highest_priority = 100;
+    
+    return old_priority;
 }
 #endif
 
@@ -699,6 +726,9 @@ void init_threads(void)
     cores[core].threads[0].priority = PRIORITY_USER_INTERFACE;
     highest_priority = 100;
 #endif
+#ifdef HAVE_SCHEDULER_BOOSTCTRL
+    cpu_boosted = false;
+#endif
     add_to_list(&cores[core].running, &cores[core].threads[0]);
     
     /* In multiple core setups, each core has a different stack.  There is probably