3 files changed, 47 insertions, 10 deletions

diff --git a/firmware/target/arm/pp/system-pp5002.c b/firmware/target/arm/pp/system-pp5002.c
index 3186d3739a..388f962fce 100644
--- a/firmware/target/arm/pp/system-pp5002.c
+++ b/firmware/target/arm/pp/system-pp5002.c
@@ -24,6 +24,11 @@
 #include "adc-target.h"
 #include "button-target.h"
 
+#if defined(HAVE_ADJUSTABLE_CPU_FREQ) && (NUM_CORES > 1)
+#include "corelock.h"
+static struct corelock cpufreq_cl SHAREDBSS_ATTR;
+#endif
+
 extern void TIMER1(void);
 extern void TIMER2(void);
 
@@ -122,6 +127,18 @@ static void ipod_init_cache(void)
 }
     
 #ifdef HAVE_ADJUSTABLE_CPU_FREQ
+#if NUM_CORES > 1
+void set_cpu_frequency__lock(void)
+{
+    corelock_lock(&cpufreq_cl);
+}
+
+void set_cpu_frequency__unlock(void)
+{
+    corelock_unlock(&cpufreq_cl);
+}
+#endif /* NUM_CORES > 1 */
+
 void set_cpu_frequency(long frequency)
 #else
 static void pp_set_cpu_frequency(long frequency)
@@ -193,7 +210,7 @@ void system_init(void)
 
 #ifdef HAVE_ADJUSTABLE_CPU_FREQ
 #if NUM_CORES > 1
-        cpu_boost_init();
+        corelock_init(&cpufreq_cl);
 #endif
 #else
         pp_set_cpu_frequency(CPUFREQ_MAX);
diff --git a/firmware/target/arm/pp/system-pp502x.c b/firmware/target/arm/pp/system-pp502x.c
index 99b536e132..102cfd8fea 100644
--- a/firmware/target/arm/pp/system-pp502x.c
+++ b/firmware/target/arm/pp/system-pp502x.c
@@ -308,16 +308,24 @@ void scale_suspend_core(bool suspend)
 }
 
 #ifdef HAVE_ADJUSTABLE_CPU_FREQ
+#if NUM_CORES > 1
+void set_cpu_frequency__lock(void)
+{
+    corelock_lock(&cpufreq_cl);
+}
+
+void set_cpu_frequency__unlock(void)
+{
+    corelock_unlock(&cpufreq_cl);
+}
+#endif /* NUM_CORES > 1 */
+
 void set_cpu_frequency(long frequency) ICODE_ATTR;
 void set_cpu_frequency(long frequency)
 #else
 static void pp_set_cpu_frequency(long frequency)
 #endif
 {
-#if defined(HAVE_ADJUSTABLE_CPU_FREQ) && (NUM_CORES > 1)
-    corelock_lock(&cpufreq_cl);
-#endif
-
     switch (frequency)
     {
       /* Note1: The PP5022 PLL must be run at >= 96MHz
@@ -424,10 +432,6 @@ static void pp_set_cpu_frequency(long frequency)
         DEV_INIT2    &= ~INIT_PLL;   /* disable PLL power */
         break;
     }
-
-#if defined(HAVE_ADJUSTABLE_CPU_FREQ) && (NUM_CORES > 1)
-    corelock_unlock(&cpufreq_cl);
-#endif
 }
 #endif /* !BOOTLOADER || (SANSA_E200 || SANSA_C200 || PHILIPS_SA9200) */
 
@@ -544,7 +548,6 @@ void system_init(void)
 #ifdef HAVE_ADJUSTABLE_CPU_FREQ
 #if NUM_CORES > 1
         corelock_init(&cpufreq_cl);
-        cpu_boost_init();
 #endif
 #else
         pp_set_cpu_frequency(CPUFREQ_MAX);
diff --git a/firmware/target/arm/pp/system-target.h b/firmware/target/arm/pp/system-target.h
index d372b65502..1e947195bd 100644
--- a/firmware/target/arm/pp/system-target.h
+++ b/firmware/target/arm/pp/system-target.h
@@ -199,4 +199,21 @@ void system_prepare_fw_start(void);
 
 #endif /* BOOTLOADER */
 
+#if defined(HAVE_ADJUSTABLE_CPU_FREQ) && (NUM_CORES > 1)
+#define CPU_BOOST_LOCK_DEFINED
+
+static inline bool cpu_boost_lock(void)
+{
+    void set_cpu_frequency__lock(void);
+    set_cpu_frequency__lock();
+    return true;
+}
+
+static inline void cpu_boost_unlock(void)
+{
+    void set_cpu_frequency__unlock(void);
+    set_cpu_frequency__unlock();
+}
+#endif /* HAVE_ADJUSTABLE_CPU_FREQ && NUM_CORES > 1 */
+
 #endif /* SYSTEM_TARGET_H */