8 files changed, 57 insertions, 14 deletions

diff --git a/arch/arm/mach-davinci/clock.c b/arch/arm/mach-davinci/clock.c
index f0baaa15a57e..39bf321d70a2 100644
--- a/arch/arm/mach-davinci/clock.c
+++ b/arch/arm/mach-davinci/clock.c
@@ -42,7 +42,8 @@ static void __clk_enable(struct clk *clk)
         if (clk->parent)
                 __clk_enable(clk->parent);
         if (clk->usecount++ == 0 && (clk->flags & CLK_PSC))
-                davinci_psc_config(psc_domain(clk), clk->lpsc, 1);
+                davinci_psc_config(psc_domain(clk), clk->psc_ctlr,
+                                clk->lpsc, 1);
 }
 
 static void __clk_disable(struct clk *clk)
@@ -50,7 +51,8 @@ static void __clk_disable(struct clk *clk)
         if (WARN_ON(clk->usecount == 0))
                 return;
         if (--clk->usecount == 0 && !(clk->flags & CLK_PLL))
-                davinci_psc_config(psc_domain(clk), clk->lpsc, 0);
+                davinci_psc_config(psc_domain(clk), clk->psc_ctlr,
+                                clk->lpsc, 0);
         if (clk->parent)
                 __clk_disable(clk->parent);
 }
@@ -164,11 +166,11 @@ static int __init clk_disable_unused(void)
                         continue;
 
                 /* ignore if in Disabled or SwRstDisable states */
-                if (!davinci_psc_is_clk_active(ck->lpsc))
+                if (!davinci_psc_is_clk_active(ck->psc_ctlr, ck->lpsc))
                         continue;
 
                 pr_info("Clocks: disable unused %s\n", ck->name);
-                davinci_psc_config(psc_domain(ck), ck->lpsc, 0);
+                davinci_psc_config(psc_domain(ck), ck->psc_ctlr, ck->lpsc, 0);
         }
         spin_unlock_irq(&clockfw_lock);
 
diff --git a/arch/arm/mach-davinci/clock.h b/arch/arm/mach-davinci/clock.h
index 584494a32070..27233cb4a2fb 100644
--- a/arch/arm/mach-davinci/clock.h
+++ b/arch/arm/mach-davinci/clock.h
@@ -67,6 +67,7 @@ struct clk {
         u8                      usecount;
         u8                      flags;
         u8                      lpsc;
+        u8                      psc_ctlr;
         struct clk              *parent;
         struct pll_data         *pll_data;
         u32                     div_reg;
diff --git a/arch/arm/mach-davinci/dm355.c b/arch/arm/mach-davinci/dm355.c
index e93840a814ed..37f20a7214be 100644
--- a/arch/arm/mach-davinci/dm355.c
+++ b/arch/arm/mach-davinci/dm355.c
@@ -545,6 +545,10 @@ static struct davinci_id dm355_ids[] = {
         },
 };
 
+static void __iomem *dm355_psc_bases[] = {
+        IO_ADDRESS(DAVINCI_PWR_SLEEP_CNTRL_BASE),
+};
+
 static struct davinci_soc_info davinci_soc_info_dm355 = {
         .io_desc                = dm355_io_desc,
         .io_desc_num            = ARRAY_SIZE(dm355_io_desc),
@@ -552,6 +556,8 @@ static struct davinci_soc_info davinci_soc_info_dm355 = {
         .ids                    = dm355_ids,
         .ids_num                = ARRAY_SIZE(dm355_ids),
         .cpu_clks               = dm355_clks,
+        .psc_bases              = dm355_psc_bases,
+        .psc_bases_num          = ARRAY_SIZE(dm355_psc_bases),
 };
 
 void __init dm355_init(void)
diff --git a/arch/arm/mach-davinci/dm644x.c b/arch/arm/mach-davinci/dm644x.c
index 648160c2960d..7b15faba56ed 100644
--- a/arch/arm/mach-davinci/dm644x.c
+++ b/arch/arm/mach-davinci/dm644x.c
@@ -485,6 +485,10 @@ static struct davinci_id dm644x_ids[] = {
         },
 };
 
+static void __iomem *dm644x_psc_bases[] = {
+        IO_ADDRESS(DAVINCI_PWR_SLEEP_CNTRL_BASE),
+};
+
 static struct davinci_soc_info davinci_soc_info_dm644x = {
         .io_desc                = dm644x_io_desc,
         .io_desc_num            = ARRAY_SIZE(dm644x_io_desc),
@@ -492,6 +496,8 @@ static struct davinci_soc_info davinci_soc_info_dm644x = {
         .ids                    = dm644x_ids,
         .ids_num                = ARRAY_SIZE(dm644x_ids),
         .cpu_clks               = dm644x_clks,
+        .psc_bases              = dm644x_psc_bases,
+        .psc_bases_num          = ARRAY_SIZE(dm644x_psc_bases),
 };
 
 void __init dm644x_init(void)
diff --git a/arch/arm/mach-davinci/dm646x.c b/arch/arm/mach-davinci/dm646x.c
index 7cf9705be9ce..3c61543c7cfa 100644
--- a/arch/arm/mach-davinci/dm646x.c
+++ b/arch/arm/mach-davinci/dm646x.c
@@ -465,6 +465,10 @@ static struct davinci_id dm646x_ids[] = {
         },
 };
 
+static void __iomem *dm646x_psc_bases[] = {
+        IO_ADDRESS(DAVINCI_PWR_SLEEP_CNTRL_BASE),
+};
+
 static struct davinci_soc_info davinci_soc_info_dm646x = {
         .io_desc                = dm646x_io_desc,
         .io_desc_num            = ARRAY_SIZE(dm646x_io_desc),
@@ -472,6 +476,8 @@ static struct davinci_soc_info davinci_soc_info_dm646x = {
         .ids                    = dm646x_ids,
         .ids_num                = ARRAY_SIZE(dm646x_ids),
         .cpu_clks               = dm646x_clks,
+        .psc_bases              = dm646x_psc_bases,
+        .psc_bases_num          = ARRAY_SIZE(dm646x_psc_bases),
 };
 
 void __init dm646x_init(void)
diff --git a/arch/arm/mach-davinci/include/mach/common.h b/arch/arm/mach-davinci/include/mach/common.h
index 97782a765882..7851d5680c13 100644
--- a/arch/arm/mach-davinci/include/mach/common.h
+++ b/arch/arm/mach-davinci/include/mach/common.h
@@ -34,6 +34,8 @@ struct davinci_soc_info {
         struct davinci_id               *ids;
         unsigned long                   ids_num;
         struct davinci_clk              *cpu_clks;
+        void __iomem                    **psc_bases;
+        unsigned long                   psc_bases_num;
 };
 
 extern struct davinci_soc_info davinci_soc_info;
diff --git a/arch/arm/mach-davinci/include/mach/psc.h b/arch/arm/mach-davinci/include/mach/psc.h
index 55a90d419fac..ab8a2586d1cc 100644
--- a/arch/arm/mach-davinci/include/mach/psc.h
+++ b/arch/arm/mach-davinci/include/mach/psc.h
@@ -27,6 +27,8 @@
 #ifndef __ASM_ARCH_PSC_H
 #define __ASM_ARCH_PSC_H
 
+#define DAVINCI_PWR_SLEEP_CNTRL_BASE    0x01C41000
+
 /* Power and Sleep Controller (PSC) Domains */
 #define DAVINCI_GPSC_ARMDOMAIN      0
 #define DAVINCI_GPSC_DSPDOMAIN      1
@@ -116,8 +118,8 @@
 #define DM646X_LPSC_TIMER1         35
 #define DM646X_LPSC_ARM_INTC       45
 
-extern int davinci_psc_is_clk_active(unsigned int id);
-extern void davinci_psc_config(unsigned int domain, unsigned int id,
-                               char enable);
+extern int davinci_psc_is_clk_active(unsigned int ctlr, unsigned int id);
+extern void davinci_psc_config(unsigned int domain, unsigned int ctlr,
+                unsigned int id, char enable);
 
 #endif /* __ASM_ARCH_PSC_H */
diff --git a/arch/arm/mach-davinci/psc.c b/arch/arm/mach-davinci/psc.c
index 84171abf5f7b..a78b657e916e 100644
--- a/arch/arm/mach-davinci/psc.c
+++ b/arch/arm/mach-davinci/psc.c
@@ -28,8 +28,6 @@
 #include <mach/psc.h>
 #include <mach/mux.h>
 
-#define DAVINCI_PWR_SLEEP_CNTRL_BASE 0x01C41000
-
 /* PSC register offsets */
 #define EPCPR           0x070
 #define PTCMD           0x120
@@ -42,22 +40,42 @@
 #define MDSTAT_STATE_MASK 0x1f
 
 /* 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_warning("PSC: Bad psc data: 0x%x[%d]\n",
+                                (int)soc_info->psc_bases, ctlr);
+                return 0;
+        }
+
+        psc_base = soc_info->psc_bases[ctlr];
+        mdstat = __raw_readl(psc_base + MDSTAT + 4 * id);
 
         /* if clocked, state can be "Enable" or "SyncReset" */
         return mdstat & BIT(12);
 }
 
 /* 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, char enable)
 {
         u32 epcpr, ptcmd, ptstat, pdstat, pdctl1, mdstat, mdctl;
-        void __iomem *psc_base = IO_ADDRESS(DAVINCI_PWR_SLEEP_CNTRL_BASE);
+        void __iomem *psc_base;
+        struct davinci_soc_info *soc_info = &davinci_soc_info;
         u32 next_state = enable ? 0x3 : 0x2; /* 0x3 enables, 0x2 disables */
 
+        if (!soc_info->psc_bases || (ctlr >= soc_info->psc_bases_num)) {
+                pr_warning("PSC: Bad psc data: 0x%x[%d]\n",
+                                (int)soc_info->psc_bases, ctlr);
+                return;
+        }
+
+        psc_base = soc_info->psc_bases[ctlr];
+
         mdctl = __raw_readl(psc_base + MDCTL + 4 * id);
         mdctl &= ~MDSTAT_STATE_MASK;
         mdctl |= next_state;