Sansa AMS: Split set_cpu_frequency() into 2 separate functions for as3525v1/v2 as the code is quite different for each model.

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

1 files changed, 37 insertions, 26 deletions

diff --git a/firmware/target/arm/as3525/system-as3525.c b/firmware/target/arm/as3525/system-as3525.c
index fb51dde..c51d844 100644
--- a/firmware/target/arm/as3525/system-as3525.c
+++ b/firmware/target/arm/as3525/system-as3525.c
@@ -347,6 +347,8 @@ int system_memory_guard(int newmode)
 
 #ifndef BOOTLOADER
 #ifdef HAVE_ADJUSTABLE_CPU_FREQ
+
+#if CONFIG_CPU == AS3525
 void set_cpu_frequency(long frequency)
 {
     if(frequency == CPUFREQ_MAX)
@@ -360,7 +362,6 @@ void set_cpu_frequency(long frequency)
         while(adc_read(ADC_CVDD) < 470); /* 470 * .0025 = 1.175V */
 #endif  /*  HAVE_ADJUSTABLE_CPU_VOLTAGE */
 
-#if CONFIG_CPU == AS3525    /* only in arm922tdmi */
         asm volatile(
             "mrc p15, 0, r0, c1, c0  \n"
 
@@ -373,42 +374,17 @@ void set_cpu_frequency(long frequency)
 
             "mcr p15, 0, r0, c1, c0  \n"
             : : : "r0" );
-#else
-    /* AS3525v2 */
-    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);
-
-    CGU_PROC = ((AS3525_FCLK_POSTDIV << 4) |
-                (AS3525_FCLK_PREDIV  << 2) |
-                 AS3525_FCLK_SEL);
-    restore_irq(oldstatus);
-#endif /* CONFIG_CPU == AS3525 */
 
         cpu_frequency = CPUFREQ_MAX;
     }
     else
     {
-#if CONFIG_CPU == AS3525    /* only in arm922tdmi */
         asm volatile(
             "mrc p15, 0, r0, c1, c0  \n"
             "bic r0, r0, #3<<30      \n"     /* fastbus clocking */
             "mcr p15, 0, r0, c1, c0  \n"
             : : : "r0" );
-#else
-    /* AS3525v2 */
-    int oldstatus = disable_irq_save();
 
-    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 & ~(0x1F << 2)) | (AS3525_PCLK_DIV0_UNBOOSTED << 2);
-
-    restore_irq(oldstatus);
-#endif /* CONFIG_CPU == AS3525 */
 
 #ifdef HAVE_ADJUSTABLE_CPU_VOLTAGE
         /* Decreasing frequency so reduce voltage after change */
@@ -418,5 +394,40 @@ void set_cpu_frequency(long frequency)
         cpu_frequency = CPUFREQ_NORMAL;
     }
 }
+#else   /* as3525v2  */
+void set_cpu_frequency(long frequency)
+{
+    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);
+
+        CGU_PROC = ((AS3525_FCLK_POSTDIV << 4) |
+                    (AS3525_FCLK_PREDIV  << 2) |
+                    AS3525_FCLK_SEL);
+
+        restore_irq(oldstatus);
+
+        cpu_frequency = CPUFREQ_MAX;
+    }
+    else
+    {
+        int oldstatus = disable_irq_save();
+
+        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 & ~(0x1F << 2)) | (AS3525_PCLK_DIV0_UNBOOSTED << 2);
+
+        restore_irq(oldstatus);
+
+        cpu_frequency = CPUFREQ_NORMAL;
+    }
+}
+#endif
+
 #endif /* HAVE_ADJUSTABLE_CPU_FREQ */
 #endif /* !BOOTLOADER */