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