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
diff --git a/firmware/target/arm/as3525/system-as3525.c b/firmware/target/arm/as3525/system-as3525.c
index feaf06a..651e43e 100644
--- a/firmware/target/arm/as3525/system-as3525.c
+++ b/firmware/target/arm/as3525/system-as3525.c
@@ -349,6 +349,23 @@ int system_memory_guard(int newmode)
 #ifdef HAVE_ADJUSTABLE_CPU_FREQ
 void set_cpu_frequency(long frequency)
 {
+/* XXX : for 24<->240MHz only ! */
+#define SWITCH_STEPS 6
+    static const unsigned char dividers[SWITCH_STEPS][2] = {
+    /* { CGU_PERI divider - 1 , CGU_PROC divider - 1 } */
+    /*   boosted
+     *     ||
+     *     \/
+     *  unboosted
+     */
+        { 9, 0 }, /* pclk = 240/(9+1) = 24MHz, fclk = 240(0+1) = 240MHz */
+        { 8, 1 }, /* pclk = 240/(8+1) = 26.66MHz, fclk = 240/(1+1) = 120MHz, pclk = 120/(8+1) = 13.3MHz */
+        { 4, 2 }, /* pclk = 120/(4+1) = 24MHz, fclk = 240/(2+1) = 80MHz, pclk =80/(4+1) = 20MHz */
+        { 2, 4 }, /* pclk = 80/(2+1) = 26.66MHz, fclk = 240/(4+1) = 48MHz, pclk = 48/(2+1) = 16MHz */
+        { 1, 7 }, /* pclk = 48/(1+1) = 24MHz, fclk = 240/(7+1) = 30MHz, pclk = 30/(1+1) = 15MHz */
+        { 0, 9 }, /* pclk = 30/(0+1) = 30MHz, fclk = 240/(9+1) = 24MHz, pclk = 24MHz */
+    };
+
     if(frequency == CPUFREQ_MAX)
     {
 #ifdef HAVE_ADJUSTABLE_CPU_VOLTAGE
@@ -377,12 +394,15 @@ void set_cpu_frequency(long frequency)
     /* 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);
+    int i;
+    for (i = 1; i < SWITCH_STEPS; i++)
+    {
+        CGU_PERI = (CGU_PERI & ~(0x1F << 2)) | (dividers[i][0] << 2);

-    CGU_PROC = ((AS3525_FCLK_POSTDIV << 4) |
-                (AS3525_FCLK_PREDIV  << 2) |
-                 AS3525_FCLK_SEL);
+        CGU_PROC = ((AS3525_FCLK_POSTDIV << 4) |
+                    (dividers[i][1]<< 2) |
+                     AS3525_FCLK_SEL);
+    }
     restore_irq(oldstatus);
 #endif /* CONFIG_CPU == AS3525 */

@@ -400,12 +420,15 @@ void set_cpu_frequency(long frequency)
     /* AS3525v2 */
     int oldstatus = disable_irq_save();

-    CGU_PROC = ((AS3525_FCLK_POSTDIV_UNBOOSTED << 4) |
-                (AS3525_FCLK_PREDIV  << 2) |
-                 AS3525_FCLK_SEL);
+    int i;
+    for (i = SWITCH_STEPS; i > 0; --i)
+    {
+        CGU_PROC = ((AS3525_FCLK_POSTDIV << 4) |
+                    (dividers[i][1]<< 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);
+        CGU_PERI = (CGU_PERI & ~(0x1F << 2)) | (dividers[i][0] << 2);
+    }

     restore_irq(oldstatus);
 #endif /* CONFIG_CPU == AS3525 */