1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
 firmware/target/arm/as3525/system-as3525.c |   63 ++++++++++++++++-----------
 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 */