4 files changed, 72 insertions, 9 deletions

diff --git a/arch/arm/mach-omap2/clkt2xxx_apll.c b/arch/arm/mach-omap2/clkt2xxx_apll.c
index f51cffd1fc53..b19a1f7234ae 100644
--- a/arch/arm/mach-omap2/clkt2xxx_apll.c
+++ b/arch/arm/mach-omap2/clkt2xxx_apll.c
@@ -78,6 +78,26 @@ static int omap2_clk_apll54_enable(struct clk *clk)
         return omap2_clk_apll_enable(clk, OMAP24XX_ST_54M_APLL_MASK);
 }
 
+static void _apll96_allow_idle(struct clk *clk)
+{
+        omap2xxx_cm_set_apll96_auto_low_power_stop();
+}
+
+static void _apll96_deny_idle(struct clk *clk)
+{
+        omap2xxx_cm_set_apll96_disable_autoidle();
+}
+
+static void _apll54_allow_idle(struct clk *clk)
+{
+        omap2xxx_cm_set_apll54_auto_low_power_stop();
+}
+
+static void _apll54_deny_idle(struct clk *clk)
+{
+        omap2xxx_cm_set_apll54_disable_autoidle();
+}
+
 /* Stop APLL */
 static void omap2_clk_apll_disable(struct clk *clk)
 {
@@ -93,11 +113,15 @@ static void omap2_clk_apll_disable(struct clk *clk)
 const struct clkops clkops_apll96 = {
         .enable         = omap2_clk_apll96_enable,
         .disable        = omap2_clk_apll_disable,
+        .allow_idle     = _apll96_allow_idle,
+        .deny_idle      = _apll96_deny_idle,
 };
 
 const struct clkops clkops_apll54 = {
         .enable         = omap2_clk_apll54_enable,
         .disable        = omap2_clk_apll_disable,
+        .allow_idle     = _apll54_allow_idle,
+        .deny_idle      = _apll54_deny_idle,
 };
 
 /* Public functions */
diff --git a/arch/arm/mach-omap2/cm2xxx_3xxx.c b/arch/arm/mach-omap2/cm2xxx_3xxx.c
index 6b0c7c85ef53..9d0dec806e92 100644
--- a/arch/arm/mach-omap2/cm2xxx_3xxx.c
+++ b/arch/arm/mach-omap2/cm2xxx_3xxx.c
@@ -25,10 +25,14 @@
 #include "cm-regbits-24xx.h"
 #include "cm-regbits-34xx.h"
 
-/* CM_AUTOIDLE_PLL.AUTO_* bit values */
+/* CM_AUTOIDLE_PLL.AUTO_* bit values for DPLLs */
 #define DPLL_AUTOIDLE_DISABLE                           0x0
 #define OMAP2XXX_DPLL_AUTOIDLE_LOW_POWER_STOP           0x3
 
+/* CM_AUTOIDLE_PLL.AUTO_* bit values for APLLs (OMAP2xxx only) */
+#define OMAP2XXX_APLL_AUTOIDLE_DISABLE                  0x0
+#define OMAP2XXX_APLL_AUTOIDLE_LOW_POWER_STOP           0x3
+
 static const u8 cm_idlest_offs[] = {
         CM_IDLEST1, CM_IDLEST2, OMAP2430_CM_IDLEST3
 };
@@ -154,6 +158,44 @@ void omap2xxx_cm_set_dpll_auto_low_power_stop(void)
 }
 
 /*
+ * APLL autoidle control
+ */
+
+static void _omap2xxx_set_apll_autoidle(u8 m, u32 mask)
+{
+        u32 v;
+
+        v = omap2_cm_read_mod_reg(PLL_MOD, CM_AUTOIDLE);
+        v &= ~mask;
+        v |= m << __ffs(mask);
+        omap2_cm_write_mod_reg(v, PLL_MOD, CM_AUTOIDLE);
+}
+
+void omap2xxx_cm_set_apll54_disable_autoidle(void)
+{
+        _omap2xxx_set_apll_autoidle(OMAP2XXX_APLL_AUTOIDLE_LOW_POWER_STOP,
+                                    OMAP24XX_AUTO_54M_MASK);
+}
+
+void omap2xxx_cm_set_apll54_auto_low_power_stop(void)
+{
+        _omap2xxx_set_apll_autoidle(OMAP2XXX_APLL_AUTOIDLE_DISABLE,
+                                    OMAP24XX_AUTO_54M_MASK);
+}
+
+void omap2xxx_cm_set_apll96_disable_autoidle(void)
+{
+        _omap2xxx_set_apll_autoidle(OMAP2XXX_APLL_AUTOIDLE_LOW_POWER_STOP,
+                                    OMAP24XX_AUTO_96M_MASK);
+}
+
+void omap2xxx_cm_set_apll96_auto_low_power_stop(void)
+{
+        _omap2xxx_set_apll_autoidle(OMAP2XXX_APLL_AUTOIDLE_DISABLE,
+                                    OMAP24XX_AUTO_96M_MASK);
+}
+
+/*
  *
  */
 
diff --git a/arch/arm/mach-omap2/cm2xxx_3xxx.h b/arch/arm/mach-omap2/cm2xxx_3xxx.h
index 5f4df1ceafad..088bbad73db5 100644
--- a/arch/arm/mach-omap2/cm2xxx_3xxx.h
+++ b/arch/arm/mach-omap2/cm2xxx_3xxx.h
@@ -125,6 +125,11 @@ extern void omap3xxx_cm_clkdm_force_wakeup(s16 module, u32 mask);
 extern void omap2xxx_cm_set_dpll_disable_autoidle(void);
 extern void omap2xxx_cm_set_dpll_auto_low_power_stop(void);
 
+extern void omap2xxx_cm_set_apll54_disable_autoidle(void);
+extern void omap2xxx_cm_set_apll54_auto_low_power_stop(void);
+extern void omap2xxx_cm_set_apll96_disable_autoidle(void);
+extern void omap2xxx_cm_set_apll96_auto_low_power_stop(void);
+
 #endif
 
 /* CM register bits shared between 24XX and 3430 */
diff --git a/arch/arm/mach-omap2/pm24xx.c b/arch/arm/mach-omap2/pm24xx.c
index 297bb21061b8..abe08f49b2ec 100644
--- a/arch/arm/mach-omap2/pm24xx.c
+++ b/arch/arm/mach-omap2/pm24xx.c
@@ -378,7 +378,6 @@ static void __init prcm_setup_regs(void)
 {
         int i, num_mem_banks;
         struct powerdomain *pwrdm;
-        u32 v;
 
         /* Enable autoidle */
         omap2_prm_write_mod_reg(OMAP24XX_AUTOIDLE_MASK, OCP_MOD,
@@ -469,13 +468,6 @@ static void __init prcm_setup_regs(void)
         omap2_cm_write_mod_reg(OMAP2420_AUTO_DSP_IPI_MASK, OMAP24XX_DSP_MOD,
                                CM_AUTOIDLE);
 
-        /* Put both APLLs into autoidle mode */
-        v = omap2_cm_read_mod_reg(PLL_MOD, CM_AUTOIDLE);
-        v &= ~(OMAP24XX_AUTO_96M_MASK | OMAP24XX_AUTO_54M_SHIFT);
-        v |= (0x03 << OMAP24XX_AUTO_96M_SHIFT) |
-                (0x03 << OMAP24XX_AUTO_54M_SHIFT);
-        omap2_cm_write_mod_reg(v, PLL_MOD, CM_AUTOIDLE);
-
         omap2_cm_write_mod_reg(OMAP24XX_AUTO_OMAPCTRL_MASK |
                                OMAP24XX_AUTO_WDT1_MASK |
                                OMAP24XX_AUTO_MPU_WDT_MASK |