rk27xx: implement frequency scalling

Implemented scheme:
        ARM AHB APB
Normal   50  50  50 MHz
Max     200 100  50 MHz

Frequency scaling is disabled on rk27generic due to too
slow lcd updates when running with 50MHz AHB.

battery_bench shows ~1h runtime improvement on hifiman.

Change-Id: I2c6f8acf6d4570c4e14f5bcc72280b51ce13c408

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 */