1 files changed, 77 insertions, 39 deletions

diff --git a/arch/arm/mach-davinci/psc.c b/arch/arm/mach-davinci/psc.c
index 84171abf5f7..82fdc69d572 100644
--- a/arch/arm/mach-davinci/psc.c
+++ b/arch/arm/mach-davinci/psc.c
@@ -19,55 +19,96 @@
  *
  */
 #include <linux/kernel.h>
-#include <linux/module.h>
 #include <linux/init.h>
 #include <linux/io.h>
 
 #include <mach/cputype.h>
-#include <mach/hardware.h>
 #include <mach/psc.h>
-#include <mach/mux.h>
 
-#define DAVINCI_PWR_SLEEP_CNTRL_BASE 0x01C41000
-
-/* PSC register offsets */
-#define EPCPR		0x070
-#define PTCMD		0x120
-#define PTSTAT		0x128
-#define PDSTAT		0x200
-#define PDCTL1		0x304
-#define MDSTAT		0x800
-#define MDCTL		0xA00
-
-#define MDSTAT_STATE_MASK 0x1f
+#include "clock.h"
 
 /* Return nonzero iff the domain's clock is active */
-int __init davinci_psc_is_clk_active(unsigned int id)
+int __init davinci_psc_is_clk_active(unsigned int ctlr, unsigned int id)
 {
-	void __iomem *psc_base = IO_ADDRESS(DAVINCI_PWR_SLEEP_CNTRL_BASE);
-	u32 mdstat = __raw_readl(psc_base + MDSTAT + 4 * id);
+	void __iomem *psc_base;
+	u32 mdstat;
+	struct davinci_soc_info *soc_info = &davinci_soc_info;
+
+	if (!soc_info->psc_bases || (ctlr >= soc_info->psc_bases_num)) {
+		pr_warn("PSC: Bad psc data: 0x%x[%d]\n",
+				(int)soc_info->psc_bases, ctlr);
+		return 0;
+	}
+
+	psc_base = ioremap(soc_info->psc_bases[ctlr], SZ_4K);
+	mdstat = __raw_readl(psc_base + MDSTAT + 4 * id);
+	iounmap(psc_base);
 
 	/* if clocked, state can be "Enable" or "SyncReset" */
 	return mdstat & BIT(12);
 }
 
+/* Control "reset" line associated with PSC domain */
+void davinci_psc_reset(unsigned int ctlr, unsigned int id, bool reset)
+{
+	u32 mdctl;
+	void __iomem *psc_base;
+	struct davinci_soc_info *soc_info = &davinci_soc_info;
+
+	if (!soc_info->psc_bases || (ctlr >= soc_info->psc_bases_num)) {
+		pr_warn("PSC: Bad psc data: 0x%x[%d]\n",
+				(int)soc_info->psc_bases, ctlr);
+		return;
+	}
+
+	psc_base = ioremap(soc_info->psc_bases[ctlr], SZ_4K);
+
+	mdctl = readl(psc_base + MDCTL + 4 * id);
+	if (reset)
+		mdctl &= ~MDCTL_LRST;
+	else
+		mdctl |= MDCTL_LRST;
+	writel(mdctl, psc_base + MDCTL + 4 * id);
+
+	iounmap(psc_base);
+}
+
 /* Enable or disable a PSC domain */
-void davinci_psc_config(unsigned int domain, unsigned int id, char enable)
+void davinci_psc_config(unsigned int domain, unsigned int ctlr,
+		unsigned int id, bool enable, u32 flags)
 {
-	u32 epcpr, ptcmd, ptstat, pdstat, pdctl1, mdstat, mdctl;
-	void __iomem *psc_base = IO_ADDRESS(DAVINCI_PWR_SLEEP_CNTRL_BASE);
-	u32 next_state = enable ? 0x3 : 0x2; /* 0x3 enables, 0x2 disables */
+	u32 epcpr, ptcmd, ptstat, pdstat, pdctl, mdstat, mdctl;
+	void __iomem *psc_base;
+	struct davinci_soc_info *soc_info = &davinci_soc_info;
+	u32 next_state = PSC_STATE_ENABLE;
+
+	if (!soc_info->psc_bases || (ctlr >= soc_info->psc_bases_num)) {
+		pr_warn("PSC: Bad psc data: 0x%x[%d]\n",
+				(int)soc_info->psc_bases, ctlr);
+		return;
+	}
+
+	psc_base = ioremap(soc_info->psc_bases[ctlr], SZ_4K);
+
+	if (!enable) {
+		if (flags & PSC_SWRSTDISABLE)
+			next_state = PSC_STATE_SWRSTDISABLE;
+		else
+			next_state = PSC_STATE_DISABLE;
+	}
 
 	mdctl = __raw_readl(psc_base + MDCTL + 4 * id);
 	mdctl &= ~MDSTAT_STATE_MASK;
 	mdctl |= next_state;
+	if (flags & PSC_FORCE)
+		mdctl |= MDCTL_FORCE;
 	__raw_writel(mdctl, psc_base + MDCTL + 4 * id);
 
-	pdstat = __raw_readl(psc_base + PDSTAT);
-	if ((pdstat & 0x00000001) == 0) {
-		pdctl1 = __raw_readl(psc_base + PDCTL1);
-		pdctl1 |= 0x1;
-		__raw_writel(pdctl1, psc_base + PDCTL1);
+	pdstat = __raw_readl(psc_base + PDSTAT + 4 * domain);
+	if ((pdstat & PDSTAT_STATE_MASK) == 0) {
+		pdctl = __raw_readl(psc_base + PDCTL + 4 * domain);
+		pdctl |= PDCTL_NEXT;
+		__raw_writel(pdctl, psc_base + PDCTL + 4 * domain);
 
 		ptcmd = 1 << domain;
 		__raw_writel(ptcmd, psc_base + PTCMD);
@@ -76,24 +117,21 @@ void davinci_psc_config(unsigned int domain, unsigned int id, char enable)
 			epcpr = __raw_readl(psc_base + EPCPR);
 		} while ((((epcpr >> domain) & 1) == 0));
 
-		pdctl1 = __raw_readl(psc_base + PDCTL1);
-		pdctl1 |= 0x100;
-		__raw_writel(pdctl1, psc_base + PDCTL1);
-
-		do {
-			ptstat = __raw_readl(psc_base +
-					       PTSTAT);
-		} while (!(((ptstat >> domain) & 1) == 0));
+		pdctl = __raw_readl(psc_base + PDCTL + 4 * domain);
+		pdctl |= PDCTL_EPCGOOD;
+		__raw_writel(pdctl, psc_base + PDCTL + 4 * domain);
 	} else {
 		ptcmd = 1 << domain;
 		__raw_writel(ptcmd, psc_base + PTCMD);
-
-		do {
-			ptstat = __raw_readl(psc_base + PTSTAT);
-		} while (!(((ptstat >> domain) & 1) == 0));
 	}
 
 	do {
+		ptstat = __raw_readl(psc_base + PTSTAT);
+	} while (!(((ptstat >> domain) & 1) == 0));
+
+	do {
 		mdstat = __raw_readl(psc_base + MDSTAT + 4 * id);
 	} while (!((mdstat & MDSTAT_STATE_MASK) == next_state));
+
+	iounmap(psc_base);
 }