diff options
| -rw-r--r-- | firmware/target/arm/as3525/system-as3525.c | 20 |
1 files changed, 13 insertions, 7 deletions
diff --git a/firmware/target/arm/as3525/system-as3525.c b/firmware/target/arm/as3525/system-as3525.c index 5a7d25a..90535fc 100644 --- a/firmware/target/arm/as3525/system-as3525.c +++ b/firmware/target/arm/as3525/system-as3525.c @@ -433,35 +433,41 @@ void set_cpu_frequency(long frequency) #else /* as3525v2 */ void set_cpu_frequency(long frequency) { + int oldstatus = disable_irq_save(); + int delay; + if(frequency == CPUFREQ_MAX) { - int oldstatus = disable_irq_save(); /* Change PCLK while FCLK is low, so it doesn't go too high */ CGU_PERI = (CGU_PERI & ~(0x1F << 2)) | (AS3525_PCLK_DIV0 << 2); + delay = 40; while(delay--) ; + CGU_PROC = ((AS3525_FCLK_POSTDIV << 4) | (AS3525_FCLK_PREDIV << 2) | AS3525_FCLK_SEL); - restore_irq(oldstatus); + delay = 40; while(delay--) ; - cpu_frequency = CPUFREQ_MAX; } else { - int oldstatus = disable_irq_save(); + frequency = CPUFREQ_NORMAL; /* We only have 2 settings */ CGU_PROC = ((AS3525_FCLK_POSTDIV_UNBOOSTED << 4) | (AS3525_FCLK_PREDIV << 2) | AS3525_FCLK_SEL); + delay = 40; while(delay--) ; + /* Change PCLK after FCLK is low, so it doesn't go too high */ CGU_PERI = (CGU_PERI & ~(0x1F << 2)) | (AS3525_PCLK_DIV0_UNBOOSTED << 2); + delay = 40; while(delay--) ; + } - restore_irq(oldstatus); + cpu_frequency = frequency; - cpu_frequency = CPUFREQ_NORMAL; - } + restore_irq(oldstatus); } #endif |