1 files changed, 6 insertions, 11 deletions

diff --git a/arch/arm/mach-davinci/psc.c b/arch/arm/mach-davinci/psc.c
index e44544ac2b16..84171abf5f7b 100644
--- a/arch/arm/mach-davinci/psc.c
+++ b/arch/arm/mach-davinci/psc.c
@@ -39,6 +39,7 @@
 #define MDSTAT          0x800
 #define MDCTL           0xA00
 
+#define MDSTAT_STATE_MASK 0x1f
 
 /* Return nonzero iff the domain's clock is active */
 int __init davinci_psc_is_clk_active(unsigned int id)
@@ -53,14 +54,13 @@ int __init davinci_psc_is_clk_active(unsigned int id)
 /* Enable or disable a PSC domain */
 void davinci_psc_config(unsigned int domain, unsigned int id, char enable)
 {
-        u32 epcpr, ptcmd, ptstat, pdstat, pdctl1, mdstat, mdctl, mdstat_mask;
+        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 */
 
         mdctl = __raw_readl(psc_base + MDCTL + 4 * id);
-        if (enable)
-                mdctl |= 0x00000003;    /* Enable Module */
-        else
-                mdctl &= 0xFFFFFFE2;    /* Disable Module */
+        mdctl &= ~MDSTAT_STATE_MASK;
+        mdctl |= next_state;
         __raw_writel(mdctl, psc_base + MDCTL + 4 * id);
 
         pdstat = __raw_readl(psc_base + PDSTAT);
@@ -93,12 +93,7 @@ void davinci_psc_config(unsigned int domain, unsigned int id, char enable)
                 } while (!(((ptstat >> domain) & 1) == 0));
         }
 
-        if (enable)
-                mdstat_mask = 0x3;
-        else
-                mdstat_mask = 0x2;
-
         do {
                 mdstat = __raw_readl(psc_base + MDSTAT + 4 * id);
-        } while (!((mdstat & 0x0000001F) == mdstat_mask));
+        } while (!((mdstat & MDSTAT_STATE_MASK) == next_state));
 }