Enable iPod Nano 2G boosting (without undervolting for now)

git-svn-id: svn://svn.rockbox.org/rockbox/trunk@24721 a1c6a512-1295-4272-9138-f99709370657

1 files changed, 55 insertions, 12 deletions

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