6 files changed, 228 insertions, 1 deletions

diff --git a/arch/arm/mach-w90x900/Makefile b/arch/arm/mach-w90x900/Makefile
index d50c94f4dbd..3ccd625455c 100644
--- a/arch/arm/mach-w90x900/Makefile
+++ b/arch/arm/mach-w90x900/Makefile
@@ -5,7 +5,7 @@
 # Object file lists.
 
 obj-y                           := irq.o time.o mfp-w90p910.o gpio.o clock.o
-
+obj-y                           += clksel.o
 # W90X900 CPU support files
 
 obj-$(CONFIG_CPU_W90P910)       += w90p910.o
diff --git a/arch/arm/mach-w90x900/clksel.c b/arch/arm/mach-w90x900/clksel.c
new file mode 100644
index 00000000000..5a77eb91cb1
--- /dev/null
+++ b/arch/arm/mach-w90x900/clksel.c
@@ -0,0 +1,91 @@
+/*
+ * linux/arch/arm/mach-w90x900/clksel.c
+ *
+ * Copyright (c) 2008 Nuvoton technology corporation
+ *
+ * Wan ZongShun <mcuos.com@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation;version 2 of the License.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/list.h>
+#include <linux/errno.h>
+#include <linux/err.h>
+#include <linux/string.h>
+#include <linux/clk.h>
+#include <linux/mutex.h>
+#include <linux/io.h>
+
+#include <mach/hardware.h>
+#include <mach/regs-clock.h>
+
+#define PLL0            0x00
+#define PLL1            0x01
+#define OTHER           0x02
+#define EXT             0x03
+#define MSOFFSET        0x0C
+#define ATAOFFSET       0x0a
+#define LCDOFFSET       0x06
+#define AUDOFFSET       0x04
+#define CPUOFFSET       0x00
+
+static DEFINE_MUTEX(clksel_sem);
+
+static void clock_source_select(const char *dev_id, unsigned int clkval)
+{
+        unsigned int clksel, offset;
+
+        clksel = __raw_readl(REG_CLKSEL);
+
+        if (strcmp(dev_id, "w90p910-ms") == 0)
+                offset = MSOFFSET;
+        else if (strcmp(dev_id, "w90p910-atapi") == 0)
+                offset = ATAOFFSET;
+        else if (strcmp(dev_id, "w90p910-lcd") == 0)
+                offset = LCDOFFSET;
+        else if (strcmp(dev_id, "w90p910-audio") == 0)
+                offset = AUDOFFSET;
+        else
+                offset = CPUOFFSET;
+
+        clksel &= ~(0x03 << offset);
+        clksel |= (clkval << offset);
+
+        __raw_writel(clksel, REG_CLKSEL);
+}
+
+void w90p910_clock_source(struct device *dev, unsigned char *src)
+{
+        unsigned int clkval;
+        const char *dev_id;
+
+        BUG_ON(!src);
+        clkval = 0;
+
+        mutex_lock(&clksel_sem);
+
+        if (dev)
+                dev_id = dev_name(dev);
+        else
+                dev_id = "cpufreq";
+
+        if (strcmp(src, "pll0") == 0)
+                clkval = PLL0;
+        else if (strcmp(src, "pll1") == 0)
+                clkval = PLL1;
+        else if (strcmp(src, "ext") == 0)
+                clkval = EXT;
+        else if (strcmp(src, "oth") == 0)
+                clkval = OTHER;
+
+        clock_source_select(dev_id, clkval);
+
+        mutex_unlock(&clksel_sem);
+}
+EXPORT_SYMBOL(w90p910_clock_source);
+
diff --git a/arch/arm/mach-w90x900/cpu.h b/arch/arm/mach-w90x900/cpu.h
index 57b5dbabeb4..ddde959d898 100644
--- a/arch/arm/mach-w90x900/cpu.h
+++ b/arch/arm/mach-w90x900/cpu.h
@@ -45,6 +45,7 @@ extern void w90p910_init_clocks(void);
 extern void w90p910_map_io(struct map_desc *mach_desc, int size);
 extern struct platform_device w90p910_serial_device;
 extern struct sys_timer w90x900_timer;
+extern void w90p910_clock_source(struct device *dev, unsigned char *src);
 
 #define W90X900_8250PORT(name)                                  \
 {                                                               \
diff --git a/arch/arm/mach-w90x900/include/mach/regs-clock.h b/arch/arm/mach-w90x900/include/mach/regs-clock.h
index f10b6a8dc06..516d6b477b6 100644
--- a/arch/arm/mach-w90x900/include/mach/regs-clock.h
+++ b/arch/arm/mach-w90x900/include/mach/regs-clock.h
@@ -28,4 +28,26 @@
 #define REG_CLKEN1      (CLK_BA + 0x24)
 #define REG_CLKDIV1     (CLK_BA + 0x28)
 
+/* Define PLL freq setting */
+#define PLL_DISABLE             0x12B63
+#define PLL_66MHZ               0x2B63
+#define PLL_100MHZ              0x4F64
+#define PLL_120MHZ              0x4F63
+#define PLL_166MHZ              0x4124
+#define PLL_200MHZ              0x4F24
+
+/* Define AHB:CPUFREQ ratio */
+#define AHB_CPUCLK_1_1          0x00
+#define AHB_CPUCLK_1_2          0x01
+#define AHB_CPUCLK_1_4          0x02
+#define AHB_CPUCLK_1_8          0x03
+
+/* Define APB:AHB ratio */
+#define APB_AHB_1_2             0x01
+#define APB_AHB_1_4             0x02
+#define APB_AHB_1_8             0x03
+
+/* Define clock skew */
+#define DEFAULTSKEW             0x48
+
 #endif /*  __ASM_ARCH_REGS_CLOCK_H */
diff --git a/arch/arm/mach-w90x900/include/mach/regs-ebi.h b/arch/arm/mach-w90x900/include/mach/regs-ebi.h
new file mode 100644
index 00000000000..b68455e7f88
--- /dev/null
+++ b/arch/arm/mach-w90x900/include/mach/regs-ebi.h
@@ -0,0 +1,33 @@
+/*
+ * arch/arm/mach-w90x900/include/mach/regs-ebi.h
+ *
+ * Copyright (c) 2009 Nuvoton technology corporation.
+ *
+ * Wan ZongShun <mcuos.com@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation;version 2 of the License.
+ *
+ */
+
+#ifndef __ASM_ARCH_REGS_EBI_H
+#define __ASM_ARCH_REGS_EBI_H
+
+/* EBI Control Registers */
+
+#define EBI_BA          W90X900_VA_EBI
+#define REG_EBICON      (EBI_BA + 0x00)
+#define REG_ROMCON      (EBI_BA + 0x04)
+#define REG_SDCONF0     (EBI_BA + 0x08)
+#define REG_SDCONF1     (EBI_BA + 0x0C)
+#define REG_SDTIME0     (EBI_BA + 0x10)
+#define REG_SDTIME1     (EBI_BA + 0x14)
+#define REG_EXT0CON     (EBI_BA + 0x18)
+#define REG_EXT1CON     (EBI_BA + 0x1C)
+#define REG_EXT2CON     (EBI_BA + 0x20)
+#define REG_EXT3CON     (EBI_BA + 0x24)
+#define REG_EXT4CON     (EBI_BA + 0x28)
+#define REG_CKSKEW      (EBI_BA + 0x2C)
+
+#endif /*  __ASM_ARCH_REGS_EBI_H */
diff --git a/arch/arm/mach-w90x900/w90p910.c b/arch/arm/mach-w90x900/w90p910.c
index 6ebf5cfc78d..8444eababaa 100644
--- a/arch/arm/mach-w90x900/w90p910.c
+++ b/arch/arm/mach-w90x900/w90p910.c
@@ -24,6 +24,7 @@
 #include <linux/platform_device.h>
 #include <linux/io.h>
 #include <linux/serial_8250.h>
+#include <linux/delay.h>
 
 #include <asm/mach/arch.h>
 #include <asm/mach/map.h>
@@ -32,6 +33,8 @@
 
 #include <mach/hardware.h>
 #include <mach/regs-serial.h>
+#include <mach/regs-clock.h>
+#include <mach/regs-ebi.h>
 
 #include "cpu.h"
 #include "clock.h"
@@ -123,6 +126,83 @@ void __init w90p910_map_io(struct map_desc *mach_desc, int mach_size)
                 printk(KERN_ERR "CPU type 0x%08lx is not W90P910\n", idcode);
 }
 
+/*Set W90P910 cpu frequence*/
+static int __init w90p910_set_clkval(unsigned int cpufreq)
+{
+        unsigned int pllclk, ahbclk, apbclk, val;
+
+        pllclk = 0;
+        ahbclk = 0;
+        apbclk = 0;
+
+        switch (cpufreq) {
+        case 66:
+                pllclk = PLL_66MHZ;
+                ahbclk = AHB_CPUCLK_1_1;
+                apbclk = APB_AHB_1_2;
+                break;
+
+        case 100:
+                pllclk = PLL_100MHZ;
+                ahbclk = AHB_CPUCLK_1_1;
+                apbclk = APB_AHB_1_2;
+                break;
+
+        case 120:
+                pllclk = PLL_120MHZ;
+                ahbclk = AHB_CPUCLK_1_2;
+                apbclk = APB_AHB_1_2;
+                break;
+
+        case 166:
+                pllclk = PLL_166MHZ;
+                ahbclk = AHB_CPUCLK_1_2;
+                apbclk = APB_AHB_1_2;
+                break;
+
+        case 200:
+                pllclk = PLL_200MHZ;
+                ahbclk = AHB_CPUCLK_1_2;
+                apbclk = APB_AHB_1_2;
+                break;
+        }
+
+        __raw_writel(pllclk, REG_PLLCON0);
+
+        val = __raw_readl(REG_CLKDIV);
+        val &= ~(0x03 << 24 | 0x03 << 26);
+        val |= (ahbclk << 24 | apbclk << 26);
+        __raw_writel(val, REG_CLKDIV);
+
+        return  0;
+}
+static int __init w90p910_set_cpufreq(char *str)
+{
+        unsigned long cpufreq, val;
+
+        if (!*str)
+                return 0;
+
+        strict_strtoul(str, 0, &cpufreq);
+
+        w90p910_clock_source(NULL, "ext");
+
+        w90p910_set_clkval(cpufreq);
+
+        mdelay(1);
+
+        val = __raw_readl(REG_CKSKEW);
+        val &= ~0xff;
+        val |= DEFAULTSKEW;
+        __raw_writel(val, REG_CKSKEW);
+
+        w90p910_clock_source(NULL, "pll0");
+
+        return 1;
+}
+
+__setup("cpufreq=", w90p910_set_cpufreq);
+
 /*Init W90P910 clock*/
 
 void __init w90p910_init_clocks(void)