4 files changed, 65 insertions, 26 deletions

diff --git a/firmware/export/config/ipodnano2g.h b/firmware/export/config/ipodnano2g.h
index be7129b690..851c59f529 100644
--- a/firmware/export/config/ipodnano2g.h
+++ b/firmware/export/config/ipodnano2g.h
@@ -171,7 +171,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
diff --git a/firmware/target/arm/s5l8700/ipodnano2g/pmu-nano2g.c b/firmware/target/arm/s5l8700/ipodnano2g/pmu-nano2g.c
index be026a8cf4..aea9b79b55 100644
--- a/firmware/target/arm/s5l8700/ipodnano2g/pmu-nano2g.c
+++ b/firmware/target/arm/s5l8700/ipodnano2g/pmu-nano2g.c
@@ -24,16 +24,15 @@
 #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)
+int pmu_read_multiple(int address, int count, unsigned char* buffer)
 {
-    i2c_read(0xe6, address, count, buffer);
+    return i2c_read(0xe6, address, count, buffer);
 }
 
-void pmu_write_multiple(int address, int count, unsigned char* buffer)
+int pmu_write_multiple(int address, int count, unsigned char* buffer)
 {
-    i2c_write(0xe6, address, count, buffer);
+    return i2c_write(0xe6, address, count, buffer);
 }
 
 unsigned char pmu_read(int address)
@@ -45,22 +44,19 @@ unsigned char pmu_read(int address)
     return tmp;
 }
 
-void pmu_write(int address, unsigned char val)
+int pmu_write(int address, unsigned char val)
 {
-    pmu_write_multiple(address, 1, &val);
+    return pmu_write_multiple(address, 1, &val);
 }
 
 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)
diff --git a/firmware/target/arm/s5l8700/ipodnano2g/pmu-target.h b/firmware/target/arm/s5l8700/ipodnano2g/pmu-target.h
index 53f4dacc1b..40618c4447 100644
--- a/firmware/target/arm/s5l8700/ipodnano2g/pmu-target.h
+++ b/firmware/target/arm/s5l8700/ipodnano2g/pmu-target.h
@@ -25,9 +25,9 @@
 #include "config.h"
 
 unsigned char pmu_read(int address);
-void pmu_write(int address, unsigned char val);
-void pmu_read_multiple(int address, int count, unsigned char* buffer);
-void pmu_write_multiple(int address, int count, unsigned char* buffer);
+int pmu_write(int address, unsigned char val);
+int pmu_read_multiple(int address, int count, unsigned char* buffer);
+int pmu_write_multiple(int address, int count, unsigned char* buffer);
 int pmu_read_adc(unsigned int adc);
 int pmu_read_battery_voltage(void);
 int pmu_read_battery_current(void);
diff --git a/firmware/target/arm/s5l8700/system-s5l8700.c b/firmware/target/arm/s5l8700/system-s5l8700.c
index fe0fb9ff7c..68ec3826a8 100644
--- a/firmware/target/arm/s5l8700/system-s5l8700.c
+++ b/firmware/target/arm/s5l8700/system-s5l8700.c
@@ -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,7 @@ void fiq_dummy(void)
 
 void system_init(void)
 {
+    pmu_init();
 }
 
 void system_reboot(void)
@@ -180,7 +182,7 @@ void system_reboot(void)
 
 void system_exception_wait(void)
 {
-    while (1);
+    while(1);
 }
 
 int system_memory_guard(int newmode)
@@ -195,27 +197,68 @@ void set_cpu_frequency(long frequency)
 {
     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;
+        /* PCLK = HCLK / 2 */
+        CLKCON2 |= 0x200;
+        /* Switch to ASYNCHRONOUS mode */
+        asm volatile(
+            "mrc     p15, 0, r0,c1,c0    \n\t"
+            "orr     r0, r0, #0xc0000000 \n\t"
+            "mcr     p15, 0, r0,c1,c0    \n\t"
+            ::: "r0"
+          );
+    }
+    else
+    {
+        /* Switch to FASTBUS mode */
+        asm volatile(
+            "mrc     p15, 0, r0,c1,c0    \n\t"
+            "bic     r0, r0, #0xc0000000 \n\t"
+            "mcr     p15, 0, r0,c1,c0    \n\t"
+            ::: "r0"
+          );
+        /* PCLK = HCLK */
+        CLKCON2 &= ~0x200;
+        /* FCLK_CPU = OFF, HCLK = PLL0 / 4 */
+        CLKCON = (CLKCON & ~0xFF00FF00) | 0x80003300;
     }
-    else if (frequency == CPUFREQ_NORMAL)
+
+#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 */
     }
-
-    asm volatile (
-        "nop      \n\t"
-        "nop      \n\t"
-        "nop      \n\t"
-    );
+#endif
 
     cpu_frequency = frequency;
+    restore_irq(oldlevel);
 }
 
 #endif