diff --git a/firmware/target/arm/as3525/system-as3525.c b/firmware/target/arm/as3525/system-as3525.c
index 51fcd97..a1af059 100644
--- a/firmware/target/arm/as3525/system-as3525.c
+++ b/firmware/target/arm/as3525/system-as3525.c
@@ -457,6 +457,15 @@ void set_cpu_frequency(long frequency)
{
int oldstatus = disable_irq_save();

+ invalidate_idcache();
+ asm volatile(
+ "mrc p15, 0, r0, c1, c0 \n"
+ "bic r0, r0, #1 \n" /* disable mmu */
+ "@bic r0, r0, #(1<<12) \n" /* icache */
+ "@bic r0, r0, #(1<<2) \n" /* dcache */
+ "mcr p15, 0, r0, c1, c0 \n"
+ : : : "r0" );
+
/* We only have 2 settings */
cpu_frequency = (frequency == CPUFREQ_MAX) ? frequency : CPUFREQ_NORMAL;

@@ -479,6 +488,8 @@ void set_cpu_frequency(long frequency)
CGU_PERI = (CGU_PERI & ~(0xF << 2)) | (AS3525_PCLK_DIV0_UNBOOSTED << 2);
}

+ enable_mmu();
+
restore_irq(oldstatus);
}
#endif