powerpc/mpc85xx: Set SYSCLK to the required frequency

For ICS307-02, there is one general expression to generate SYSCLK:
CLK1Frequency = InputFrequency * 2 * (VDW + 8) / ((RDW + 2) * OD)

If we want the required frequency for SYSCLK, we must find one solution
to generate this frequency, this solution includes VDW, RDW and OD.
For OD, there are only eight option value: 10, 2, 8, 4, 5, 7, 3, 6.
For RDW, the range is 1 to 127.
For VDW, the range is 4 to 511.

First, we use one OD, RDW and required SYSCLK to calculate the VDW,
if VDW is in it's range, we will calculate the CLK1Frequency with
the OD, RDW and VDW calculated, and we will check this percent
(CLK1Frequency / required SYSCLK), and the precision is 1/1000.
if the percent is less than 1/1000, we think the CLK1Frequency is we want.
Otherwise, We will continue to calculate it with the next OD and RDW.

Signed-off-by: Jerry Huang <Chang-Ming.Huang@freescale.com>
Signed-off-by: Kumar Gala <galak@kernel.crashing.org>
diff --git a/board/freescale/common/ics307_clk.c b/board/freescale/common/ics307_clk.c
index 89d8810..6acbc36 100644
--- a/board/freescale/common/ics307_clk.c
+++ b/board/freescale/common/ics307_clk.c
@@ -31,12 +31,81 @@
 #include "pixis.h"
 #endif
 
+/* define for SYS CLK or CLK1Frequency */
+#define TTL		1
+#define CLK2		0
+#define CRYSTAL		0
+#define MAX_VDW		(511 + 8)
+#define MAX_RDW		(127 + 2)
+#define MIN_VDW		(4 + 8)
+#define MIN_RDW		(1 + 2)
+#define NUM_OD_SETTING	8
+/*
+ * These defines cover the industrial temperature range part,
+ * for commercial, change below to 400000 and 55000, respectively
+ */
+#define MAX_VCO		360000
+#define MIN_VCO		60000
+
 /* decode S[0-2] to Output Divider (OD) */
 static u8 ics307_s_to_od[] = {
 	10, 2, 8, 4, 5, 7, 3, 6
 };
 
 /*
+ * Find one solution to generate required frequency for SYSCLK
+ * out_freq: KHz, required frequency to the SYSCLK
+ * the result will be retuned with component RDW, VDW, OD, TTL,
+ * CLK2 and crystal
+ */
+unsigned long ics307_sysclk_calculator(unsigned long out_freq)
+{
+	const unsigned long input_freq = CONFIG_ICS307_REFCLK_HZ;
+	unsigned long vdw, rdw, odp, s_vdw = 0, s_rdw = 0, s_odp = 0, od;
+	unsigned long tmp_out, diff, result = 0;
+	int found = 0;
+
+	for (odp = 0; odp < NUM_OD_SETTING; odp++) {
+		od = ics307_s_to_od[odp];
+		if (od * out_freq < MIN_VCO || od * out_freq > MAX_VCO)
+			continue;
+		for (rdw = MIN_RDW; rdw <= MAX_RDW; rdw++) {
+			/* Calculate the VDW */
+			vdw = out_freq * 1000 * od * rdw / (input_freq * 2);
+			if (vdw > MAX_VDW)
+				vdw = MAX_VDW;
+			if (vdw < MIN_VDW)
+				continue;
+			/* Calculate the temp out frequency */
+			tmp_out = input_freq * 2 * vdw / (rdw * od * 1000);
+			diff = MAX(out_freq, tmp_out) - MIN(out_freq, tmp_out);
+			/*
+			 * calculate the percent, the precision is 1/1000
+			 * If greater than 1/1000, continue
+			 * otherwise, we think the solution is we required
+			 */
+			if (diff * 1000 / out_freq > 1)
+				continue;
+			else {
+				s_vdw = vdw;
+				s_rdw = rdw;
+				s_odp = odp;
+				found = 1;
+				break;
+			}
+		}
+	}
+
+	if (found)
+		result = (s_rdw - 2) | (s_vdw - 8) << 7 | s_odp << 16 |
+			CLK2 << 19 | TTL << 21 | CRYSTAL << 22;
+
+	debug("ICS307-02: RDW: %ld, VDW: %ld, OD: %d\n", s_rdw - 2, s_vdw - 8,
+			ics307_s_to_od[s_odp]);
+	return result;
+}
+
+/*
  * Calculate frequency being generated by ICS307-02 clock chip based upon
  * the control bytes being programmed into it.
  */
diff --git a/board/freescale/common/ics307_clk.h b/board/freescale/common/ics307_clk.h
index db3dbc4..3757912 100644
--- a/board/freescale/common/ics307_clk.h
+++ b/board/freescale/common/ics307_clk.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2010 Freescale Semiconductor, Inc.
+ * Copyright 2010-2011 Freescale Semiconductor, Inc.
  *
  * See file CREDITS for list of people who contributed to this
  * project.
@@ -23,8 +23,10 @@
 #define __ICS_CLK_H_	1
 
 #ifndef __ASSEMBLY__
+
 extern unsigned long get_board_sys_clk(void);
 extern unsigned long get_board_ddr_clk(void);
+extern unsigned long ics307_sysclk_calculator(unsigned long out_freq);
 #endif
 
 #endif	/* __ICS_CLK_H_ */
diff --git a/board/freescale/common/ngpixis.c b/board/freescale/common/ngpixis.c
index 765f035..276ae3c 100644
--- a/board/freescale/common/ngpixis.c
+++ b/board/freescale/common/ngpixis.c
@@ -156,9 +156,29 @@
 }
 #endif
 
+void pixis_sysclk_set(unsigned long sysclk)
+{
+	unsigned long freq_word;
+	u8 sclk0, sclk1, sclk2;
+
+	freq_word = ics307_sysclk_calculator(sysclk);
+	sclk2 = freq_word & 0xff;
+	sclk1 = (freq_word >> 8) & 0xff;
+	sclk0 = (freq_word >> 16) & 0xff;
+
+	/* set SYSCLK enable bit */
+	PIXIS_WRITE(vcfgen0, 0x01);
+
+	/* SYSCLK to required frequency */
+	PIXIS_WRITE(sclk[0], sclk0);
+	PIXIS_WRITE(sclk[1], sclk1);
+	PIXIS_WRITE(sclk[2], sclk2);
+}
+
 int pixis_reset_cmd(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 {
 	unsigned int i;
+	unsigned long sysclk;
 	char *p_altbank = NULL;
 #ifdef DEBUG
 	char *p_dump = NULL;
@@ -182,6 +202,12 @@
 			continue;
 		}
 #endif
+		if (strcmp(argv[i], "sysclk") == 0) {
+			sysclk = simple_strtoul(argv[i + 1], NULL, 0);
+			i += 1;
+			pixis_sysclk_set(sysclk);
+			continue;
+		}
 
 		unknown_param = argv[i];
 	}
@@ -219,4 +245,5 @@
 #ifdef DEBUG
 	"pixis_reset dump - display the PIXIS registers\n"
 #endif
+	"pixis_reset sysclk <SYSCLK_freq> - reset with SYSCLK frequency(KHz)\n"
 	);