[ARM] Separate ICS525 VCO calculation code.

The ICS525 clock chip is used in several different parts of the
Integrator platform.  Rather than duplicate the code, separate it
out so everyone can use it.

arch/arm/Kconfig

@@ -483,6 +483,11 @@ config PCI_HOST_VIA82C505
 	depends on PCI && ARCH_SHARK
 	default y
 
+config ICST525
+	bool
+	depends on ARCH_INTEGRATOR
+	default y
+
 config ISA
 	bool
 	depends on FOOTBRIDGE_HOST || ARCH_SHARK || ARCH_CLPS7500 || ARCH_EBSA110 || ARCH_CDB89712 || ARCH_EDB7211 || ARCH_SA1100
@@ -582,7 +587,7 @@ endif
 
 config CPU_FREQ_INTEGRATOR
 	tristate "CPUfreq driver for ARM Integrator CPUs"
-	depends on ARCH_INTEGRATOR && CPU_FREQ
+	depends on ARCH_INTEGRATOR && ICST525 && CPU_FREQ
 	default y
 	help
 	  This enables the CPUfreq driver for ARM Integrator CPUs.

arch/arm/common/Makefile

@@ -3,6 +3,7 @@
 #
 
 obj-y				+= platform.o
+obj-$(CONFIG_ICST525)		+= icst525.o
 obj-$(CONFIG_SA1111)		+= sa1111.o sa1111-pcibuf.o sa1111-pcipool.o
 obj-$(CONFIG_PCI_HOST_PLX90X0)	+= plx90x0.o
 obj-$(CONFIG_PCI_HOST_VIA82C505) += via82c505.o

arch/arm/common/icst525.c

+/*
+ *  linux/arch/arm/common/icst525.c
+ *
+ *  Copyright (C) 2003 Deep Blue Solutions, Ltd, All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ *  Support functions for calculating clocks/divisors for the ICST525
+ *  clock generators.  See http://www.icst.com/ for more information
+ *  on these devices.
+ */
+#include <linux/module.h>
+#include <linux/kernel.h>
+
+#include <asm/hardware/icst525.h>
+
+/*
+ * Divisors for each OD setting.
+ */
+static unsigned char s2div[8] = { 10, 2, 8, 4, 5, 7, 9, 6 };
+
+unsigned long icst525_khz(const struct icst525_params *p, struct icst525_vco vco)
+{
+	return p->ref * 2 * (vco.v + 8) / ((vco.r + 2) * s2div[vco.s]);
+}
+
+EXPORT_SYMBOL(icst525_khz);
+
+/*
+ * Ascending divisor S values.
+ */
+static unsigned char idx2s[] = { 1, 3, 4, 7, 5, 2, 6, 0 };
+
+struct icst525_vco
+icst525_khz_to_vco(const struct icst525_params *p, unsigned long freq)
+{
+	struct icst525_vco vco = { .s = 1, .v = p->vd_max, .r = p->rd_max };
+	unsigned long f;
+	unsigned int i = 0, rd, best = (unsigned int)-1;
+
+	/*
+	 * First, find the PLL output divisor such
+	 * that the PLL output is within spec.
+	 */
+	do {
+		f = freq * s2div[idx2s[i]];
+
+		/*
+		 * f must be between 10MHz and
+		 *  320MHz (5V) or 200MHz (3V)
+		 */
+		if (f > 10000 && f <= p->vco_max)
+			break;
+	} while (i < ARRAY_SIZE(idx2s));
+
+	if (i > ARRAY_SIZE(idx2s))
+		return vco;
+
+	vco.s = idx2s[i];
+
+	/*
+	 * Now find the closest divisor combination
+	 * which gives a PLL output of 'f'.
+	 */
+	for (rd = p->rd_min; rd <= p->rd_max; rd++) {
+		unsigned long fref_div, f_pll;
+		unsigned int vd;
+		int f_diff;
+
+		fref_div = (2 * p->ref) / rd;
+
+		vd = (f + fref_div / 2) / fref_div;
+		if (vd < p->vd_min || vd > p->vd_max)
+			continue;
+
+		f_pll = fref_div * vd;
+		f_diff = f_pll - f;
+		if (f_diff < 0)
+			f_diff = -f_diff;
+
+		if ((unsigned)f_diff < best) {
+			vco.v = vd - 8;
+			vco.r = rd - 2;
+			if (f_diff == 0)
+				break;
+			best = f_diff;
+		}
+	}
+
+	return vco;
+}
+
+EXPORT_SYMBOL(icst525_khz_to_vco);
+
+struct icst525_vco
+icst525_ps_to_vco(const struct icst525_params *p, unsigned long period)
+{
+	struct icst525_vco vco = { .s = 1, .v = p->vd_max, .r = p->rd_max };
+	unsigned long f, ps;
+	unsigned int i = 0, rd, best = (unsigned int)-1;
+
+	ps = 1000000000UL / p->vco_max;
+
+	/*
+	 * First, find the PLL output divisor such
+	 * that the PLL output is within spec.
+	 */
+	do {
+		f = period / s2div[idx2s[i]];
+
+		/*
+		 * f must be between 10MHz and
+		 *  320MHz (5V) or 200MHz (3V)
+		 */
+		if (f >= ps && f < 100000)
+			break;
+	} while (i < ARRAY_SIZE(idx2s));
+
+	if (i > ARRAY_SIZE(idx2s))
+		return vco;
+
+	vco.s = idx2s[i];
+
+	ps = 500000000UL / p->ref;
+
+	/*
+	 * Now find the closest divisor combination
+	 * which gives a PLL output of 'f'.
+	 */
+	for (rd = p->rd_min; rd <= p->rd_max; rd++) {
+		unsigned long f_in_div, f_pll;
+		unsigned int vd;
+		int f_diff;
+
+		f_in_div = ps * rd;
+
+		vd = (f_in_div + f / 2) / f;
+		if (vd < p->vd_min || vd > p->vd_max)
+			continue;
+
+		f_pll = (f_in_div + vd / 2) / vd;
+		f_diff = f_pll - f;
+		if (f_diff < 0)
+			f_diff = -f_diff;
+
+		if ((unsigned)f_diff < best) {
+			vco.v = vd - 8;
+			vco.r = rd - 2;
+			if (f_diff == 0)
+				break;
+			best = f_diff;
+		}
+	}
+
+	return vco;
+}
+
+EXPORT_SYMBOL(icst525_ps_to_vco);

arch/arm/mach-integrator/cpu.c

@@ -23,6 +23,7 @@
 
 #include <asm/hardware.h>
 #include <asm/io.h>
+#include <asm/hardware/icst525.h>
 
 static struct cpufreq_driver integrator_driver;
 
@@ -31,75 +32,40 @@ static struct cpufreq_driver integrator_driver;
 #define CM_STAT (IO_ADDRESS(INTEGRATOR_HDR_BASE)+INTEGRATOR_HDR_STAT_OFFSET)
 #define CM_LOCK (IO_ADDRESS(INTEGRATOR_HDR_BASE)+INTEGRATOR_HDR_LOCK_OFFSET)
 
-struct vco {
-	unsigned char vdw;
-	unsigned char od;
+static const struct icst525_params lclk_params = {
+	.ref		= 24000,
+	.vco_max	= 320000,
+	.vd_min		= 8,
+	.vd_max		= 132,
+	.rd_min		= 24,
+	.rd_max		= 24,
 };
 
-/*
- * Divisors for each OD setting.
- */
-static unsigned char cc_divisor[8] = { 10, 2, 8, 4, 5, 7, 9, 6 };
-
-static unsigned int vco_to_freq(struct vco vco, int factor)
-{
-	return 2000 * (vco.vdw + 8) / cc_divisor[vco.od] / factor;
-}
-
-/*
- * Divisor indexes in ascending divisor order
- */
-static unsigned char s2od[] = { 1, 3, 4, 7, 5, 2, 6, 0 };
-
-static struct vco freq_to_vco(unsigned int freq_khz, int factor)
-{
-	struct vco vco = {0, 0};
-	unsigned int i, f;
-
-	freq_khz *= factor;
-
-	for (i = 0; i < 8; i++) {
-		f = freq_khz * cc_divisor[s2od[i]];
-		/* f must be between 10MHz and 320MHz */
-		if (f > 10000 && f <= 320000)
-			break;
-	}
-
-	vco.od  = s2od[i];
-	vco.vdw = f / 2000 - 8;
-
-	return vco;
-}
-
+static const struct icst525_params cclk_params = {
+	.ref		= 24000,
+	.vco_max	= 320000,
+	.vd_min		= 12,
+	.vd_max		= 160,
+	.rd_min		= 24,
+	.rd_max		= 24,
+};
 
 /*
  * Validate the speed policy.
  */
 static int integrator_verify_policy(struct cpufreq_policy *policy)
 {
-	struct vco vco;
+	struct icst525_vco vco;
 
 	cpufreq_verify_within_limits(policy, 
 				     policy->cpuinfo.min_freq, 
 				     policy->cpuinfo.max_freq);
 
-	vco = freq_to_vco(policy->max, 1);
-
-	if (vco.vdw < 4)
-		vco.vdw = 4;
-	if (vco.vdw > 152)
-		vco.vdw = 152;
-
-	policy->max = vco_to_freq(vco, 1);
-
-	vco = freq_to_vco(policy->min, 1);
-
-	if (vco.vdw < 4)
-		vco.vdw = 4;
-	if (vco.vdw > 152)
-		vco.vdw = 152;
+	vco = icst525_khz_to_vco(&cclk_params, policy->max);
+	policy->max = icst525_khz(&cclk_params, vco);
 
-	policy->min = vco_to_freq(vco, 1);
+	vco = icst525_khz_to_vco(&cclk_params, policy->min);
+	policy->min = icst525_khz(&cclk_params, vco);
 
 	cpufreq_verify_within_limits(policy, 
 				     policy->cpuinfo.min_freq, 
@@ -115,7 +81,7 @@ static int integrator_set_target(struct cpufreq_policy *policy,
 {
 	unsigned long cpus_allowed;
 	int cpu = policy->cpu;
-	struct vco vco;
+	struct icst525_vco vco;
 	struct cpufreq_freqs freqs;
 	u_int cm_osc;
 
@@ -133,19 +99,20 @@ static int integrator_set_target(struct cpufreq_policy *policy,
 
 	/* get current setting */
 	cm_osc = __raw_readl(CM_OSC);
-	vco.od = (cm_osc >> 8) & 7;
-	vco.vdw = cm_osc & 255;
-	freqs.old = vco_to_freq(vco, 1);
+	vco.s = (cm_osc >> 8) & 7;
+	vco.v = cm_osc & 255;
+	vco.r = 22;
+	freqs.old = icst525_khz(&cclk_params, vco);
 
-	/* freq_to_vco rounds down -- so we need the next larger freq in
-	 * case of CPUFREQ_RELATION_L.
+	/* icst525_khz_to_vco rounds down -- so we need the next
+	 * larger freq in case of CPUFREQ_RELATION_L.
 	 */
 	if (relation == CPUFREQ_RELATION_L)
 		target_freq += 1999;
 	if (target_freq > policy->max)
 		target_freq = policy->max;
-	vco = freq_to_vco(target_freq, 1);
-	freqs.new = vco_to_freq(vco, 1);
+	vco = icst525_khz_to_vco(&cclk_params, target_freq);
+	freqs.new = icst525_khz(&cclk_params, vco);
 
 	freqs.cpu = policy->cpu;
 
@@ -158,7 +125,7 @@ static int integrator_set_target(struct cpufreq_policy *policy,
 
 	cm_osc = __raw_readl(CM_OSC);
 	cm_osc &= 0xfffff800;
-	cm_osc |= vco.vdw | vco.od << 8;
+	cm_osc |= vco.v | vco.s << 8;
 
 	__raw_writel(0xa05f, CM_LOCK);
 	__raw_writel(cm_osc, CM_OSC);
@@ -179,7 +146,7 @@ static int integrator_cpufreq_init(struct cpufreq_policy *policy)
 	unsigned long cpus_allowed;
 	unsigned int cpu = policy->cpu;
 	u_int cm_osc, cm_stat, mem_freq_khz;
-	struct vco vco;
+	struct icst525_vco vco;
 
 	cpus_allowed = current->cpus_allowed;
 
@@ -189,23 +156,26 @@ static int integrator_cpufreq_init(struct cpufreq_policy *policy)
 	/* detect memory etc. */
 	cm_stat = __raw_readl(CM_STAT);
 	cm_osc = __raw_readl(CM_OSC);
-	vco.od  = (cm_osc >> 20) & 7;
-	vco.vdw = (cm_osc >> 12) & 255;
-	mem_freq_khz = vco_to_freq(vco, 2);
+	vco.s = (cm_osc >> 20) & 7;
+	vco.v = (cm_osc >> 12) & 255;
+	vco.r = 22;
+	mem_freq_khz = icst525_khz(&lclk_params, vco) / 2;
 
 	printk(KERN_INFO "CPU%d: Module id: %d\n", cpu, cm_stat & 255);
 	printk(KERN_INFO "CPU%d: Memory clock = %d.%03d MHz\n",
 	       cpu, mem_freq_khz / 1000, mem_freq_khz % 1000);
 
-	vco.od = (cm_osc >> 8) & 7;
-	vco.vdw = cm_osc & 255;
+	vco.s = (cm_osc >> 8) & 7;
+	vco.v = cm_osc & 255;
+	vco.r = 22;
 
 	/* set default policy and cpuinfo */
 	policy->policy = CPUFREQ_POLICY_PERFORMANCE;
 	policy->cpuinfo.max_freq = 160000;
 	policy->cpuinfo.min_freq = 12000;
 	policy->cpuinfo.transition_latency = 1000; /* 1 ms, assumed */
-	policy->cur = policy->min = policy->max = vco_to_freq(vco, 1); /* current freq */
+	policy->cur = policy->min = policy->max =
+		icst525_khz(&cclk_params, vco); /* current freq */
 
 	set_cpus_allowed(current, cpus_allowed);
 

include/asm-arm/hardware/icst525.h

+/*
+ *  linux/include/asm-arm/hardware/icst525.h
+ *
+ *  Copyright (C) 2003 Deep Blue Solutions, Ltd, All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ *  Support functions for calculating clocks/divisors for the ICST525
+ *  clock generators.  See http://www.icst.com/ for more information
+ *  on these devices.
+ */
+#ifndef ASMARM_HARDWARE_ICST525_H
+#define ASMARM_HARDWARE_ICST525_H
+
+struct icst525_params {
+	unsigned long	ref;
+	unsigned long	vco_max;	/* inclusive */
+	unsigned short	vd_min;		/* inclusive */
+	unsigned short	vd_max;		/* inclusive */
+	unsigned char	rd_min;		/* inclusive */
+	unsigned char	rd_max;		/* inclusive */
+};
+
+struct icst525_vco {
+	unsigned short	v;
+	unsigned char	r;
+	unsigned char	s;
+};
+
+unsigned long icst525_khz(const struct icst525_params *p, struct icst525_vco vco);
+struct icst525_vco icst525_khz_to_vco(const struct icst525_params *p, unsigned long freq);
+struct icst525_vco icst525_ps_to_vco(const struct icst525_params *p, unsigned long period);
+
+#endif