ARM: 5634/1: Add static setting cpu frequence for w90p910 platform

Add static setting cpu frequence for w90p910 platform.

Signed-off-by: Wan ZongShun <mcuos.com@gmail.com>
Signed-off-by: Russell King <rmk+kernel@arm.linux.org.uk>

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

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);
+

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)					\
 {								\

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 */

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 */

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)