Index: firmware/export/config-ipodnano2g.h
===================================================================
--- firmware/export/config-ipodnano2g.h	(revision 23338)
+++ firmware/export/config-ipodnano2g.h	(working copy)
@@ -169,7 +169,7 @@
 #define HAVE_WHEEL_POSITION
 
 /* Define this if you have adjustable CPU frequency */
-/* #define HAVE_ADJUSTABLE_CPU_FREQ - not yet */
+#define HAVE_ADJUSTABLE_CPU_FREQ
 
 #define BOOTFILE_EXT "ipod"
 #define BOOTFILE "rockbox." BOOTFILE_EXT
Index: firmware/target/arm/s5l8700/ipodnano2g/pmu-nano2g.c
===================================================================
--- firmware/target/arm/s5l8700/ipodnano2g/pmu-nano2g.c	(revision 23338)
+++ firmware/target/arm/s5l8700/ipodnano2g/pmu-nano2g.c	(working copy)
@@ -24,7 +24,6 @@
 #include "i2c-s5l8700.h"
 
 static struct mutex pmu_adc_mutex;
-int pmu_initialized = 0;
 
 void pmu_read_multiple(int address, int count, unsigned char* buffer)
 {
@@ -52,15 +51,12 @@
 
 void pmu_init(void)
 {
-    if (pmu_initialized) return;
     mutex_init(&pmu_adc_mutex);
-    pmu_initialized = 1;
 }
 
 int pmu_read_adc(unsigned int adc)
 {
     int data = 0;
-    if (!pmu_initialized) pmu_init();
     mutex_lock(&pmu_adc_mutex);
     pmu_write(0x54, 5 | (adc << 4));
     while ((data & 0x80) == 0)
Index: firmware/target/arm/s5l8700/system-s5l8700.c
===================================================================
--- firmware/target/arm/s5l8700/system-s5l8700.c	(revision 23338)
+++ firmware/target/arm/s5l8700/system-s5l8700.c	(working copy)
@@ -24,6 +24,7 @@
 #include "panic.h"
 #ifdef IPOD_NANO2G
 #include "storage.h"
+#include "pmu-target.h"
 #endif
 
 #define default_interrupt(name) \
@@ -153,6 +154,8 @@
 
 void system_init(void)
 {
+    pmu_init();
+    pmu_write(0x1e, 15);  /* Vcore = 1.000V */
 }
 
 void system_reboot(void)
@@ -180,7 +183,7 @@
 
 void system_exception_wait(void)
 {
-    while (1);
+    while(1);
 }
 
 int system_memory_guard(int newmode)
@@ -195,27 +198,52 @@
 {
     if (cpu_frequency == frequency)
         return;
-    
-    /* CPU/COP frequencies can be scaled between Fbus (min) and Fsys (max).
-       Fbus should not be set below ~32Mhz with LCD enabled or the display
-       will be garbled. */
+
+    int oldlevel = disable_irq_save();
+
+#if 1
     if (frequency == CPUFREQ_MAX)
     {
+        /* FCLK_CPU = PLL0, HCLK = PLL0 / 2 */
+        CLKCON = (CLKCON & ~0xFF00FF00) | 0x20003100;
+        CLKCON2 |= 0x200;  /* PCLK = HCLK / 2 */
     }
-    else if (frequency == CPUFREQ_NORMAL)
+    else
     {
+        /* FCLK_CPU = PLL0 / 4, HCLK = PLL0 / 4 */
+        CLKCON = (CLKCON & ~0xFF00FF00) | 0x33003300;
+        CLKCON2 &= ~0x200;  /* PCLK = HCLK */
     }
+
+#else  /* Alternative: Also clock down the PLL. Doesn't seem to save much
+                       current, but results in high switching latency. */
+
+    if (frequency == CPUFREQ_MAX)
+    {
+        CLKCON &= ~0xFF00FF00;  /* Everything back to the OSC */
+        PLLCON &= ~1;  /* Power down PLL0 */
+        PLL0PMS = 0x021200;  /* 192 MHz */
+        PLL0LCNT = 8100;
+        PLLCON |= 1;  /* Power up PLL0 */
+        while (!(PLLLOCK & 1));  /* Wait for PLL to lock */
+        CLKCON2 |= 0x200;  /* PCLK = HCLK / 2 */
+        CLKCON |= 0x20003100;  /* FCLK_CPU = PLL0, PCLK = PLL0 / 2 */
+    }
     else
     {
+        CLKCON &= ~0xFF00FF00;  /* Everything back to the OSC */
+        CLKCON2 &= ~0x200;  /* PCLK = HCLK */
+        PLLCON &= ~1;  /* Power down PLL0 */
+        PLL0PMS = 0x000500;  /* 48 MHz */
+        PLL0LCNT = 8100;
+        PLLCON |= 1;  /* Power up PLL0 */
+        while (!(PLLLOCK & 1));  /* Wait for PLL to lock */
+        CLKCON |= 0x20002000;  /* FCLK_CPU = PLL0, PCLK = PLL0 */
     }
+#endif
 
-    asm volatile (
-        "nop      \n\t"
-        "nop      \n\t"
-        "nop      \n\t"
-    );
-
     cpu_frequency = frequency;
+    restore_irq(oldlevel);
 }
 
 #endif