diff --git a/firmware/export/config/sansaclipplus.h b/firmware/export/config/sansaclipplus.h
index c52d803..e4c43d9 100644
--- a/firmware/export/config/sansaclipplus.h
+++ b/firmware/export/config/sansaclipplus.h
@@ -187,7 +187,7 @@
#define CONFIG_LED LED_VIRTUAL

/* Define this if you have adjustable CPU frequency */
-//#define HAVE_ADJUSTABLE_CPU_FREQ
+#define HAVE_ADJUSTABLE_CPU_FREQ

#define BOOTFILE_EXT "sansa"
#define BOOTFILE "rockbox." BOOTFILE_EXT
diff --git a/firmware/export/config/sansaclipv2.h b/firmware/export/config/sansaclipv2.h
index eb6fb6c..c308f76 100644
--- a/firmware/export/config/sansaclipv2.h
+++ b/firmware/export/config/sansaclipv2.h
@@ -181,7 +181,7 @@
#define CONFIG_LED LED_VIRTUAL

/* Define this if you have adjustable CPU frequency */
-//#define HAVE_ADJUSTABLE_CPU_FREQ
+#define HAVE_ADJUSTABLE_CPU_FREQ

#define BOOTFILE_EXT "sansa"
#define BOOTFILE "rockbox." BOOTFILE_EXT
diff --git a/firmware/export/config/sansafuzev2.h b/firmware/export/config/sansafuzev2.h
index b56c8a5..ebee66a 100644
--- a/firmware/export/config/sansafuzev2.h
+++ b/firmware/export/config/sansafuzev2.h
@@ -196,7 +196,7 @@
#define USB_PRODUCT_ID 0x74c3 /* MSC = 0x74c3, MTP = 0x74c2 */

/* Define this if you have adjustable CPU frequency */
-//#define HAVE_ADJUSTABLE_CPU_FREQ
+#define HAVE_ADJUSTABLE_CPU_FREQ

#define BOOTFILE_EXT "sansa"
#define BOOTFILE "rockbox." BOOTFILE_EXT
diff --git a/firmware/target/arm/as3525/clock-target.h b/firmware/target/arm/as3525/clock-target.h
index 1689c59..295b0d8 100644
--- a/firmware/target/arm/as3525/clock-target.h
+++ b/firmware/target/arm/as3525/clock-target.h
@@ -70,8 +70,8 @@
* - bit 12 = unknown (always set to 1)
* Fpll = Fin * F / (R * OD), where Fin = 12 MHz
*/
-#define AS3525_PLLA_FREQ 240000000
-#define AS3525_PLLA_SETTING 0x113B
+#define AS3525_PLLA_FREQ 248000000
+#define AS3525_PLLA_SETTING 0x113D

#define AS3525_PLLB_FREQ 192000000
#define AS3525_PLLB_SETTING 0x155F
@@ -87,12 +87,10 @@
*/

#ifdef SANSA_FUZEV2
-/* display is unbearably slow at 24MHz
- * 34285715 HZ works ok but 40MHz works even better*/
-#define AS3525_DRAM_FREQ 40000000 /* Initial DRAM frequency */
-#else
-#define AS3525_DRAM_FREQ 24000000 /* Initial DRAM frequency */
-#endif /* SANSA_FUZEV2 */
+#define AS3525_DRAM_FREQ 41333334 /* Initial DRAM frequency */
+#else /* Clipv2 / Clip+ */
+#define AS3525_DRAM_FREQ 24800000 /* Initial DRAM frequency */
+#endif

#else
/* AS3525v1 */
diff --git a/firmware/target/arm/as3525/system-as3525.c b/firmware/target/arm/as3525/system-as3525.c
index 2e4747e..c92af22 100644
--- a/firmware/target/arm/as3525/system-as3525.c
+++ b/firmware/target/arm/as3525/system-as3525.c
@@ -403,32 +403,81 @@ void set_cpu_frequency(long frequency)
}
}
#else /* as3525v2 */
-/* FIXME : disabled for now, seems to cause buggy memory accesses
- * Disabling MMU or putting the function in uncached memory seems to help? */
+
+static inline void clk_wait(void)
+{
+#if 0
+ unsigned i = 40;
+ do {
+ nop;
+ } while(--i);
+#endif
+}
+
void set_cpu_frequency(long frequency)
{
int oldstatus = disable_irq_save();
+ unsigned long cgu_peri = CGU_PERI & ~(0xF << 2);
+
+#if AS3525_PLLA_FREQ != 248000000
+# error only suited for PLLA_FREQ == CPUFREQ_MAX == 248MHz
+#endif
+
+#if AS3525_PCLK_FREQ == 24800000 /* Clipv2 / Clip+ */
+ /* fclk 240MHz -> 24MHz */
+ static const uint8_t divider[][2] = {
+ /* -> change fclk -> tmp pclk -> change pclk -> new pclk */
+ /* fclk div pclk div fclk tmp pclk new pclk */
+ { 1-1, 10-1 }, // 240 24 24
+ { 2-1, 5-1 }, // 120 48 24
+ { 3-1, 2-1 }, // 80 20 40
+ { 5-1, 1-1 }, // 48 24 48
+ { 10-1, 1-1 }, // 24 24 24
+ };
+#elif AS3525_PCLK_FREQ == 41333334 /* Fuzev2 */
+ /* fclk 240MHz -> 40MHz */
+ static const uint8_t divider[][2] = {
+ /* -> change fclk -> tmp pclk -> change pclk -> new pclk */
+ /* fclk div pclk div fclk tmp pclk new pclk */
+ { 1-1, 6-1 }, // 240 40 40
+ { 2-1, 3-1 }, // 120 20 40
+ { 4-1, 2-1 }, // 60 20 30
+ { 6-1, 1-1 }, // 40 20 40
+ };
+#endif
+
+ const size_t n_divs = sizeof(divider)/sizeof(divider[0]);

/* We only have 2 settings */
cpu_frequency = (frequency == CPUFREQ_MAX) ? frequency : CPUFREQ_NORMAL;

if(frequency == CPUFREQ_MAX)
{
- /* Change PCLK while FCLK is low, so it doesn't go too high */
- CGU_PERI = (CGU_PERI & ~(0xF << 2)) | (AS3525_PCLK_DIV0 << 2);
-
- CGU_PROC = ((AS3525_FCLK_POSTDIV << 4) |
- (AS3525_FCLK_PREDIV << 2) |
- AS3525_FCLK_SEL);
+ unsigned i = n_divs - 1;
+ do
+ {
+ CGU_PERI = cgu_peri | (divider[i][1] << 2);
+ clk_wait();
+
+ CGU_PROC = (divider[i][0]<< 4)
+ | (AS3525_FCLK_PREDIV << 2)
+ | AS3525_FCLK_SEL;
+ clk_wait();
+ } while(i--);
}
else
{
- CGU_PROC = ((AS3525_FCLK_POSTDIV_UNBOOSTED << 4) |
- (AS3525_FCLK_PREDIV << 2) |
- AS3525_FCLK_SEL);
-
- /* Change PCLK after FCLK is low, so it doesn't go too high */
- CGU_PERI = (CGU_PERI & ~(0xF << 2)) | (AS3525_PCLK_DIV0_UNBOOSTED << 2);
+ unsigned i = 1;
+ do
+ {
+ CGU_PROC = (divider[i][0] << 4)
+ | (AS3525_FCLK_PREDIV << 2)
+ | AS3525_FCLK_SEL;
+ clk_wait();
+
+ CGU_PERI = cgu_peri | (divider[i][1] << 2);
+ clk_wait();
+ } while(++i < n_divs);
}

restore_irq(oldstatus);