4 files changed, 60 insertions, 4 deletions

diff --git a/firmware/export/config/hifimanhm60x.h b/firmware/export/config/hifimanhm60x.h
index 11916a4b83..d188d19201 100644
--- a/firmware/export/config/hifimanhm60x.h
+++ b/firmware/export/config/hifimanhm60x.h
@@ -174,7 +174,7 @@
 #define STORAGE_NEEDS_ALIGN
 
 /* Define this if you have adjustable CPU frequency */
-/* #define HAVE_ADJUSTABLE_CPU_FREQ */
+#define HAVE_ADJUSTABLE_CPU_FREQ
 
 #define RKW_FORMAT
 #define BOOTFILE_EXT "rkw"
diff --git a/firmware/export/config/hifimanhm801.h b/firmware/export/config/hifimanhm801.h
index dfa6b8d13d..80b6ccbe41 100644
--- a/firmware/export/config/hifimanhm801.h
+++ b/firmware/export/config/hifimanhm801.h
@@ -162,7 +162,7 @@
 #define STORAGE_NEEDS_ALIGN
 
 /* Define this if you have adjustable CPU frequency */
-/* #define HAVE_ADJUSTABLE_CPU_FREQ */
+#define HAVE_ADJUSTABLE_CPU_FREQ
 
 #define RKW_FORMAT
 #define BOOTFILE_EXT "rkw"
diff --git a/firmware/target/arm/rk27xx/system-rk27xx.c b/firmware/target/arm/rk27xx/system-rk27xx.c
index 1b4505541f..8c75deef07 100644
--- a/firmware/target/arm/rk27xx/system-rk27xx.c
+++ b/firmware/target/arm/rk27xx/system-rk27xx.c
@@ -231,3 +231,58 @@ void commit_discard_dcache_range (const void *base, unsigned int size)
         opcode += 32;
     }
 }
+
+#ifdef HAVE_ADJUSTABLE_CPU_FREQ
+static inline void set_sdram_timing(int ahb_freq)
+{
+    MCSDR_T_REF = (125*ahb_freq/1000000) >> 3;
+    MCSDR_T_RFC = (64*ahb_freq/1000000)/1000;
+}
+
+void set_cpu_frequency(long frequency)
+{
+    if (cpu_frequency == frequency)
+        return;
+
+    set_sdram_timing(12000000);
+
+    if (frequency == CPUFREQ_MAX)
+    {
+        /* PLL set to 200 Mhz
+         * PLL:ARM = 1:1
+         * ARM:AHB = 2:1
+         * AHB:APB = 2:1
+         */
+        SCU_DIVCON1 = (SCU_DIVCON1 &~ 0x1f) | (1<<3)|1;
+        SCU_PLLCON1 = ((1<<24)|(1<<23)|(5<<16)|(49<<4)); /*((24/6)*50)/1*/
+
+        /* wait for PLL lock ~0.3 ms */
+        while (!(SCU_STATUS & 1));
+
+        /* leave SLOW mode */
+        SCU_DIVCON1 &= ~1;
+
+        set_sdram_timing(CPUFREQ_MAX/2);
+    }
+    else
+    {
+        /* PLL set to 100 MHz
+         * PLL:ARM = 2:1
+         * ARM:AHB = 1:1
+         * AHB:APB = 1:1
+         */
+        SCU_DIVCON1 = (SCU_DIVCON1 & ~0x1f) | (1<<2)|1;
+        SCU_PLLCON1 = ((1<<24)|(1<<23)|(5<<16)|(49<<4)|(1<<1)); /*((24/6)*50)/2*/
+
+        /* wait for PLL lock ~0.3 ms */
+        while (!(SCU_STATUS & 1));
+
+        /* leave SLOW mode */
+        SCU_DIVCON1 &= ~1;
+
+        set_sdram_timing(CPUFREQ_NORMAL);
+    }
+
+    cpu_frequency = frequency;
+}
+#endif
diff --git a/firmware/target/arm/rk27xx/system-target.h b/firmware/target/arm/rk27xx/system-target.h
index 730686549d..8a705dd77a 100644
--- a/firmware/target/arm/rk27xx/system-target.h
+++ b/firmware/target/arm/rk27xx/system-target.h
@@ -48,7 +48,8 @@ static inline void commit_dcache(void) {}
 void commit_discard_dcache(void);
 void commit_discard_idcache(void);
 
-#define CPUFREQ_NORMAL 200000000
-#define CPUFREQ_MAX    200000000
+#define CPUFREQ_DEFAULT  50000000
+#define CPUFREQ_NORMAL   50000000
+#define CPUFREQ_MAX     200000000
 
 #endif /* SYSTEM_TARGET_H */