3 files changed, 99 insertions, 24 deletions

diff --git a/cpu/arm926ejs/davinci/cpu.c b/cpu/arm926ejs/davinci/cpu.c
index 390cab8a2..fc3551c30 100644
--- a/cpu/arm926ejs/davinci/cpu.c
+++ b/cpu/arm926ejs/davinci/cpu.c
@@ -23,7 +23,7 @@
 #include <common.h>
 #include <netdev.h>
 #include <asm/arch/hardware.h>
-
+#include <asm/io.h>
 
 /* offsets from PLL controller base */
 #define PLLC_PLLCTL	0x100
@@ -60,6 +60,54 @@
 #define DDR_PLLDIV	PLLC_PLLDIV1
 #endif
 
+#ifdef CONFIG_SOC_DA8XX
+const dv_reg * const sysdiv[7] = {
+	&davinci_pllc_regs->plldiv1, &davinci_pllc_regs->plldiv2,
+	&davinci_pllc_regs->plldiv3, &davinci_pllc_regs->plldiv4,
+	&davinci_pllc_regs->plldiv5, &davinci_pllc_regs->plldiv6,
+	&davinci_pllc_regs->plldiv7
+};
+
+int clk_get(enum davinci_clk_ids id)
+{
+	int pre_div;
+	int pllm;
+	int post_div;
+	int pll_out;
+
+	pll_out = CONFIG_SYS_OSCIN_FREQ;
+
+	if (id == DAVINCI_AUXCLK_CLKID)
+		goto out;
+
+	/*
+	 * Lets keep this simple. Combining operations can result in
+	 * unexpected approximations
+	 */
+	pre_div = (readl(&davinci_pllc_regs->prediv) &
+		   DAVINCI_PLLC_DIV_MASK) + 1;
+	pllm = readl(&davinci_pllc_regs->pllm) + 1;
+
+	pll_out /= pre_div;
+	pll_out *= pllm;
+
+	if (id == DAVINCI_PLLM_CLKID)
+		goto out;
+
+	post_div = (readl(&davinci_pllc_regs->postdiv) &
+		    DAVINCI_PLLC_DIV_MASK) + 1;
+
+	pll_out /= post_div;
+
+	if (id == DAVINCI_PLLC_CLKID)
+		goto out;
+
+	pll_out /= (readl(sysdiv[id - 1]) & DAVINCI_PLLC_DIV_MASK) + 1;
+
+out:
+	return pll_out;
+}
+#endif /* CONFIG_SOC_DA8XX */
 
 #ifdef CONFIG_DISPLAY_CPUINFO
 
diff --git a/cpu/arm926ejs/davinci/psc.c b/cpu/arm926ejs/davinci/psc.c
index 5bb972f18..8273a7fae 100644
--- a/cpu/arm926ejs/davinci/psc.c
+++ b/cpu/arm926ejs/davinci/psc.c
@@ -25,6 +25,7 @@
 
 #include <common.h>
 #include <asm/arch/hardware.h>
+#include <asm/io.h>
 
 /*
  * The PSC manages three inputs to a "module" which may be a peripheral or
@@ -47,21 +48,45 @@
 /* Works on Always On power domain only (no PD argument) */
 void lpsc_on(unsigned int id)
 {
-	dv_reg_p mdstat, mdctl;
+	dv_reg_p mdstat, mdctl, ptstat, ptcmd;
+#ifdef CONFIG_SOC_DA8XX
+	struct davinci_psc_regs *psc_regs;
+#endif
 
+#ifndef CONFIG_SOC_DA8XX
 	if (id >= DAVINCI_LPSC_GEM)
 		return;			/* Don't work on DSP Power Domain */
 
 	mdstat = REG_P(PSC_MDSTAT_BASE + (id * 4));
 	mdctl = REG_P(PSC_MDCTL_BASE + (id * 4));
+	ptstat = REG_P(PSC_PTSTAT);
+	ptcmd = REG_P(PSC_PTCMD);
+#else
+	if (id < DAVINCI_LPSC_PSC1_BASE) {
+		if (id >= PSC_PSC0_MODULE_ID_CNT)
+			return;
+		psc_regs = davinci_psc0_regs;
+		mdstat = &psc_regs->psc0.mdstat[id];
+		mdctl = &psc_regs->psc0.mdctl[id];
+	} else {
+		id -= DAVINCI_LPSC_PSC1_BASE;
+		if (id >= PSC_PSC1_MODULE_ID_CNT)
+			return;
+		psc_regs = davinci_psc1_regs;
+		mdstat = &psc_regs->psc1.mdstat[id];
+		mdctl = &psc_regs->psc1.mdctl[id];
+	}
+	ptstat = &psc_regs->ptstat;
+	ptcmd = &psc_regs->ptcmd;
+#endif
 
-	while (REG(PSC_PTSTAT) & 0x01)
+	while (readl(ptstat) & 0x01)
 		continue;
 
-	if ((*mdstat & 0x1f) == 0x03)
-		return;			/* Already on and enabled */
+	if ((readl(mdstat) & 0x1f) == 0x03)
+		return; /* Already on and enabled */
 
-	*mdctl |= 0x03;
+	writel(readl(mdctl) | 0x03, mdctl);
 
 	switch (id) {
 #ifdef CONFIG_SOC_DM644X
@@ -80,16 +105,16 @@ void lpsc_on(unsigned int id)
 	case DAVINCI_LPSC_MEMSTICK:
 	case DAVINCI_LPSC_McBSP:
 	case DAVINCI_LPSC_GPIO:
-		*mdctl |= 0x200;
+		writel(readl(mdctl) | 0x200, mdctl);
 		break;
 #endif
 	}
 
-	REG(PSC_PTCMD) = 0x01;
+	writel(0x01, ptcmd);
 
-	while (REG(PSC_PTSTAT) & 0x03)
+	while (readl(ptstat) & 0x01)
 		continue;
-	while ((*mdstat & 0x1f) != 0x03)	/* Probably an overkill... */
+	while ((readl(mdstat) & 0x1f) != 0x03)
 		continue;
 }
 
diff --git a/cpu/arm926ejs/davinci/timer.c b/cpu/arm926ejs/davinci/timer.c
index 80751add8..9da7443f3 100644
--- a/cpu/arm926ejs/davinci/timer.c
+++ b/cpu/arm926ejs/davinci/timer.c
@@ -38,8 +38,9 @@
  */
 
 #include <common.h>
+#include <asm/io.h>
 
-typedef volatile struct {
+struct davinci_timer {
 	u_int32_t	pid12;
 	u_int32_t	emumgt;
 	u_int32_t	na1;
@@ -51,9 +52,10 @@ typedef volatile struct {
 	u_int32_t	tcr;
 	u_int32_t	tgcr;
 	u_int32_t	wdtcr;
-} davinci_timer;
+};
 
-davinci_timer		*timer = (davinci_timer *)CONFIG_SYS_TIMERBASE;
+static struct davinci_timer * const timer =
+	(struct davinci_timer *)CONFIG_SYS_TIMERBASE;
 
 #define TIMER_LOAD_VAL	(CONFIG_SYS_HZ_CLOCK / CONFIG_SYS_HZ)
 #define TIM_CLK_DIV	16
@@ -64,30 +66,30 @@ static ulong lastinc;
 int timer_init(void)
 {
 	/* We are using timer34 in unchained 32-bit mode, full speed */
-	timer->tcr = 0x0;
-	timer->tgcr = 0x0;
-	timer->tgcr = 0x06 | ((TIM_CLK_DIV - 1) << 8);
-	timer->tim34 = 0x0;
-	timer->prd34 = TIMER_LOAD_VAL;
+	writel(0x0, &timer->tcr);
+	writel(0x0, &timer->tgcr);
+	writel(0x06 | ((TIM_CLK_DIV - 1) << 8), &timer->tgcr);
+	writel(0x0, &timer->tim34);
+	writel(TIMER_LOAD_VAL, &timer->prd34);
 	lastinc = 0;
 	timestamp = 0;
-	timer->tcr = 2 << 22;
+	writel(2 << 22, &timer->tcr);
 
 	return(0);
 }
 
 void reset_timer(void)
 {
-	timer->tcr = 0x0;
-	timer->tim34 = 0;
+	writel(0x0, &timer->tcr);
+	writel(0x0, &timer->tim34);
 	lastinc = 0;
 	timestamp = 0;
-	timer->tcr = 2 << 22;
+	writel(2 << 22, &timer->tcr);
 }
 
 static ulong get_timer_raw(void)
 {
-	ulong now = timer->tim34;
+	ulong now = readl(&timer->tim34);
 
 	if (now >= lastinc) {
 		/* normal mode */
@@ -110,7 +112,7 @@ void set_timer(ulong t)
 	timestamp = t;
 }
 
-void udelay(unsigned long usec)
+void __udelay(unsigned long usec)
 {
 	ulong tmo;
 	ulong endtime;