Index: firmware/system.c
===================================================================
RCS file: /cvsroot/rockbox/firmware/system.c,v
retrieving revision 1.91
diff -u -r1.91 system.c
--- firmware/system.c 27 Feb 2006 12:35:05 -0000 1.91
+++ firmware/system.c 10 Apr 2006 18:11:56 -0000
@@ -1378,6 +1378,239 @@
interrupt_vector[n + 1] = handler;
}
+static struct
+{
+ unsigned char freq;
+ unsigned char freq_idx;
+ unsigned char div;
+ unsigned char mem_conf[9];
+}
+perf_modes[3] =
+ {
+ {12, 3, 4, {2, 1, 1, 1, 1, 1, 1, 1, 1}},
+ {48, 3, 1, {5, 4, 1, 4, 4, 1, 3, 3, 1}},
+ {60, 4, 1, {6, 5, 1, 5, 5, 1, 4, 3, 1}}
+ };
+
+static unsigned char group_first_clkout[12] =
+ {0, 28, 37, 41, 57, 58, 59, 60, 61, 62, 71, 72};
+
+static struct
+{
+ unsigned char first;
+ unsigned char n;
+}
+clkout_group_div_conns[12] =
+ {
+ {0, 7},
+ {7, 2},
+ {9, 1},
+ {10, 1},
+ {0, 0},
+ {0, 0},
+ {0, 0},
+ {0, 0},
+ {11, 1},
+ {12, 6},
+ {0, 0},
+ {0, 0}
+ };
+
+static unsigned char bus_divider;
+static unsigned char performance_mode;
+
+static int clkout_group(int n)
+{
+ int g = 0;
+ while (g < 11 && group_first_clkout[g + 1] <= n)
+ g++;
+ return g;
+}
+
+static int clkout_conn_reg(int n)
+{
+ if (n <= 56)
+ return n;
+ else if (n <= 60)
+ return -1;
+ else if (n <= 70)
+ return n - 4;
+ else
+ return -1;
+}
+
+static int clocks_divider_size(int n)
+{
+ return (n == 16) ? 10 : 8;
+}
+
+static void clocks_set_divider(int n, int a, int b)
+{
+ if (a == b)
+ CLK_DIVIDER_REG(n) &= ~1;
+ else
+ {
+ int c = -a;
+ int d = b - a;
+ int div_size = clocks_divider_size(n);
+ int max_n = 1 << div_size;
+ while ((d << 1) < max_n && (c << 1) >= -max_n)
+ {
+ d <<= 1;
+ c <<= 1;
+ }
+ CLK_DIVIDER_REG(n) = 5 | ((c << div_size) | d) << 3;
+ }
+}
+
+static int clocks_clkout_connected(int n)
+{
+ int r = clkout_conn_reg(n);
+ if (r >= 0)
+ return CLK_CLKOUT_CONN_REG(r) & 1;
+ else
+ return 0;
+}
+
+static int clocks_clkout_divider(int n)
+{
+ int r = clkout_conn_reg(n);
+ if (r >= 0)
+ return (CLK_CLKOUT_CONN_REG(r) >> 1)
+ + clkout_group_div_conns[clkout_group(n)].first;
+ else
+ return -1;
+}
+
+static void clocks_set_arm_perf_mode(int mode)
+{
+ MEM_CLK_REG(0x0c) = perf_modes[mode].mem_conf[0];
+ MEM_CLK_REG(0x14) = perf_modes[mode].mem_conf[1];
+ MEM_CLK_REG(0x2c) = perf_modes[mode].mem_conf[3];
+ MEM_CLK_REG(0x34) = perf_modes[mode].mem_conf[4];
+ MEM_CLK_REG(0x4c) = perf_modes[mode].mem_conf[6];
+ MEM_CLK_REG(0x54) = perf_modes[mode].mem_conf[7];
+ MEM_CLK_REG(0x08) = perf_modes[mode].mem_conf[2];
+ MEM_CLK_REG(0x28) = perf_modes[mode].mem_conf[5];
+ MEM_CLK_REG(0x48) = perf_modes[mode].mem_conf[8];
+}
+
+static void clocks_set_reg30_60(int n, int a)
+{
+ int r90 = CLKREG90(n);
+ int t = CLKREG0(n);
+ if (r90 & 1)
+ {
+ CLKREG60(n) = a;
+ CLKREG0(n) = (t & 4) | 2;
+ }
+ else
+ {
+ CLKREG30(n) = a;
+ CLKREG0(n) = (t & 4) | 1;
+ }
+}
+
+static void nop_f(void) {}
+
+static void clocks_busy_wait(int n)
+{
+ while (n > 0)
+ {
+ nop_f();
+ n--;
+ }
+}
+
+static void clocks_set_bus_freq(int i)
+{
+ clocks_set_reg30_60(0, 1);
+ clocks_set_reg30_60(3, 1);
+
+ BUS_PLL_REG(4) = 1;
+ BUS_PLL_REG(0) = 1;
+ BUS_PLL_REG(8) = 0;
+ BUS_PLL_REG(0x10) = 0;
+ BUS_PLL_REG(0x18) = 1;
+ BUS_PLL_REG(0x14) = i;
+ BUS_PLL_REG(4) = 0;
+
+ clocks_busy_wait(1000);
+ if (i == 4) {
+ CLK_DIVIDER_REG(1) = 0xffffc385;
+ CLK_DIVIDER_REG(1) = 0xffffc384;
+ CLK_CLKOUT_CONN_REG(0x11) = 3;
+ CLK_DIVIDER_REG(1) = 0xffffc385;
+ CLKREGC0(0x11) = 1;
+ CLKREG0(0) = 1;
+ (*(volatile unsigned long *)0x80004414) = 1;
+ clocks_set_reg30_60(0, 9);
+ }
+
+ clocks_set_reg30_60(0, 9);
+ clocks_set_reg30_60(3, 9);
+}
+
+static void clocks_set_bus_div(int div)
+{
+ clocks_set_divider(bus_divider, 1, div);
+}
+
+static void clocks_set_performance_mode(int mode)
+{
+ int old = performance_mode;
+ if (perf_modes[old].freq < perf_modes[mode].freq)
+ {
+ clocks_set_arm_perf_mode(mode);
+ if (perf_modes[old].freq_idx != perf_modes[mode].freq_idx)
+ clocks_set_bus_freq(perf_modes[mode].freq_idx);
+ if (perf_modes[old].div != perf_modes[mode].div)
+ clocks_set_bus_div(perf_modes[mode].div);
+ }
+ else if (perf_modes[old].freq > perf_modes[mode].freq)
+ {
+ if (perf_modes[old].freq_idx != perf_modes[mode].freq_idx)
+ clocks_set_bus_freq(perf_modes[mode].freq_idx);
+ if (perf_modes[old].div != perf_modes[mode].div)
+ clocks_set_bus_div(perf_modes[mode].div);
+ clocks_set_arm_perf_mode(mode);
+ }
+ performance_mode = mode;
+}
+
+static void clocks_init(void)
+{
+ bus_divider = clocks_clkout_divider(0);
+ performance_mode = 0;
+ clocks_set_performance_mode(2);
+
+ (*(volatile unsigned long *)0x80105064) = 1;
+ CLKREGC0(0x14) = 1;
+ CLKREG0(0) = 1;
+ (*(volatile unsigned long *)0x80004414) = 1;
+ clocks_set_reg30_60(0, 9);
+
+}
+
+#ifdef HAVE_ADJUSTABLE_CPU_FREQ
+void set_cpu_frequency(long frequency)
+{
+ switch (frequency)
+ {
+ case CPUFREQ_MAX:
+ clocks_set_performance_mode(2);
+ break;
+ case CPUFREQ_NORMAL:
+ clocks_set_performance_mode(1);
+ break;
+ case CPUFREQ_DEFAULT:
+ clocks_set_performance_mode(0);
+ break;
+ }
+
+}
+#endif
+
void system_init(void)
{
int i;
@@ -1400,6 +1633,8 @@
interrupt_vector[i + 1] = undefined_int;
}
interrupt_vector[0] = undefined_int;
+
+ clocks_init();
}
Index: firmware/export/pnx0101.h
===================================================================
RCS file: /cvsroot/rockbox/firmware/export/pnx0101.h,v
retrieving revision 1.1
diff -u -r1.1 pnx0101.h
--- firmware/export/pnx0101.h 12 Jan 2006 00:35:50 -0000 1.1
+++ firmware/export/pnx0101.h 10 Apr 2006 18:11:56 -0000
@@ -67,1 +80,14 @@
+
+#define CLK_DIVIDER_REG(n) (*(volatile unsigned long *)(0x80004420 + 4 * (n)))
+#define CLK_CLKOUT_CONN_REG(n) (*(volatile unsigned long *)(0x80004308 \
+ + 4 * (n)))
+#define MEM_CLK_REG(n) (*(volatile unsigned long *)(0x80008200 + (n)))
+#define BUS_PLL_REG(n) (*(volatile unsigned long *)(0x80004ce4 + (n)))
+
+#define CLKREG0(n) (*(volatile unsigned long *)(0x80004000 + 4 * (n)))
+#define CLKREG30(n) (*(volatile unsigned long *)(0x80004030 + 4 * (n)))
+#define CLKREG60(n) (*(volatile unsigned long *)(0x80004060 + 4 * (n)))
+#define CLKREG90(n) (*(volatile unsigned long *)(0x80004090 + 4 * (n)))
+#define CLKREGC0(n) (*(volatile unsigned long *)(0x800040C0 + 4 * (n)))
+
#endif