[Tinyx-devel] [PATCH 2/3] ARM7: LPC2XXX: CLOCK: Implementing clocks for LPC2XXX platform.

[David C. <da...@gm...>]

This patch implements clocks for LPC2XXX platform, using the generic
clock subsystem.

Signed-off-by: David Cohen <da...@gm...>
---
 arch/arm7/mach-lpc21xx/clock.c |   96 +++++++++++++++++++++++++++++++---------
 arch/arm7/mach-lpc21xx/clock.h |   34 +++++++++++++-
 2 files changed, 106 insertions(+), 24 deletions(-)

diff --git a/arch/arm7/mach-lpc21xx/clock.c b/arch/arm7/mach-lpc21xx/clock.c
index 61b00e3..b8003d9 100644
--- a/arch/arm7/mach-lpc21xx/clock.c
+++ b/arch/arm7/mach-lpc21xx/clock.c
@@ -19,37 +19,91 @@
  * along with Tinyx.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <asm/io.h>
+#include <asm/arch/lpc2xxx.h>
+#include <asm/arch/start_lpc2148.h>
+
 #include "clock.h"
 
-unsigned int cclk_set_rate(struct clock *clk, unsigned int rate)
+#define VAL_FEED1	0xAA
+#define VAL_FEED2	0x55
+
+#define FCCO_MIN	156000000
+#define FCCO_MAX	320000000
+
+#define PLLCON_DIS	(0 << 0)	/* PLL Disable */
+#define	PLLCON_EN	(1 << 0)	/* PLL Enable */
+#define PLLCON_CON	(1 << 1)	/* PLL Connect */
+#define PLLSTAT_LOCK	(1 << 10)	/* PLL Lock Status */
+
+static unsigned int psel[4][2] = {
+	{ 1, 0x0 << 5 },
+	{ 2, 0x1 << 5 },
+	{ 4, 0x2 << 5 },
+	{ 8, 0x3 << 5 },
+};
+
+static unsigned int cclk_get_psel(unsigned int msel, unsigned int rate)
 {
-	return 0;
+	unsigned int p = 4;
+	unsigned int fcco = rate * 2 * msel;
+
+	if ((fcco * psel[0][p]) < FCCO_MIN)
+		/* BUG: FCCO out of range */
+		goto end;
+
+	while (p) {
+		if ((fcco * psel[0][p]) > FCCO_MAX)
+			p--;
+		else
+			goto end;
+	}
+
+	/* if ((fcco * psel[0][p]) > FCCO_MAX); BUG: FCCO out of range */
+
+end:
+	return psel[1][p];
 }
 
-unsigned int pclk_set_rate(struct clock *clk, unsigned int rate)
+static void cclk_set_pllcon(unsigned int con)
 {
-	return 0;
+	__raw_writew(con, PLLCON);
+	__raw_writew(VAL_FEED1, PLLFEED);
+	__raw_writew(VAL_FEED2, PLLFEED);
 }
 
-void pclk_propagate(struct clock *clk, struct clock *parent)
+static unsigned int cclk_set_rate(struct clock *clk, unsigned int rate)
 {
-}
+	unsigned int msel;
+	unsigned int psel;
 
-static struct clock pclk = {
-	.id		= PCLK,
-	.num_child	= 0,
-	.child		= NULL,
-	.set_rate	= pclk_set_rate,
-	.propagate	= pclk_propagate,
-};
+	msel = rate / FOSC;
+	msel -= msel ? 1 : 0;
 
-static struct clock cclk = {
-	.id		= CCLK,
-	.num_child	= 1,
-	.child		= &pclk,
-	.set_rate	= cclk_set_rate,
-	.propagate	= NULL,
-};
+	psel = cclk_get_psel(msel, rate);
+
+	/* Disabling PLL */
+	cclk_set_pllcon(PLLCON_DIS);
+
+	/* Configuring PLL */
+	__raw_writew((msel | psel), PLLCFG);
+	cclk_set_pllcon(PLLCON_EN);
+
+	/* Waiting for PLL Lock */
+	while (!(__raw_readw(PLLSTAT) & PLLSTAT_LOCK));
+
+	/* Connecting PLL */
+	cclk_set_pllcon(PLLCON_EN | PLLCON_CON);
+
+	return (msel+1) * FOSC;
+}
 
-struct clock *lpc2xxx_clk_list[] = { &cclk, &pclk };
+static unsigned int pclk_set_rate(struct clock *clk, unsigned int rate)
+{
+	return 0;
+}
+
+static void pclk_propagate(struct clock *clk)
+{
+}
 
diff --git a/arch/arm7/mach-lpc21xx/clock.h b/arch/arm7/mach-lpc21xx/clock.h
index 214fd62..d68f374 100644
--- a/arch/arm7/mach-lpc21xx/clock.h
+++ b/arch/arm7/mach-lpc21xx/clock.h
@@ -25,8 +25,36 @@
 #include <asm/arch-lpc2xxx/clock.h>
 #include <tinyx/kernel.h>
 
-extern unsigned int cclk_set_rate(struct clock *clk, unsigned int rate);
-extern unsigned int pclk_set_rate(struct clock *clk, unsigned int rate);
-extern void pclk_propagate(struct clock *clk, struct clock *parent);
+static unsigned int cclk_set_rate(struct clock *clk, unsigned int rate);
+static unsigned int pclk_set_rate(struct clock *clk, unsigned int rate);
+static void pclk_propagate(struct clock *clk);
+
+static struct clock pclk;
+static struct clock cclk;
+
+static struct clock pclk = {
+	.id		= PCLK,
+	.flags		= CLK_ENABLED,
+	.rate		= 12000000,
+	.parent		= &cclk,
+	.child_list	= NULL,
+	.set_rate	= pclk_set_rate,
+	.propagate	= pclk_propagate,
+	.rate_list	= { 12000000, 0 },
+};
+
+static struct clock *cclk_childs[2] = { &pclk, NULL };
+
+static struct clock cclk = {
+	.id		= CCLK,
+	.flags		= CLK_ENABLED | CLK_PRIMARY,
+	.rate		= 12000000,
+	.child_list	= cclk_childs,
+	.set_rate	= cclk_set_rate,
+	.propagate	= NULL,
+	.rate_list	= { 12000000, 48000000, 0 },
+};
+
+struct clock *lpc2xxx_clk_list[] = { &cclk, &pclk , NULL };
 
 #endif /* __ARCH_MACH_LPC2XXX_CLOCK_H */
-- 
1.5.3.7