4 files changed, 50 insertions, 12 deletions

diff --git a/arch/arm/mach-shmobile/board-ap4evb.c b/arch/arm/mach-shmobile/board-ap4evb.c
index 5168a0338cf4..264340a60f65 100644
--- a/arch/arm/mach-shmobile/board-ap4evb.c
+++ b/arch/arm/mach-shmobile/board-ap4evb.c
@@ -1486,6 +1486,6 @@ MACHINE_START(AP4EVB, "ap4evb")
         .init_irq       = sh7372_init_irq,
         .handle_irq     = shmobile_handle_irq_intc,
         .init_machine   = ap4evb_init,
-        .init_late      = shmobile_init_late,
+        .init_late      = sh7372_pm_init_late,
         .timer          = &shmobile_timer,
 MACHINE_END
diff --git a/arch/arm/mach-shmobile/board-mackerel.c b/arch/arm/mach-shmobile/board-mackerel.c
index d1e8fe83588c..f96e41557d65 100644
--- a/arch/arm/mach-shmobile/board-mackerel.c
+++ b/arch/arm/mach-shmobile/board-mackerel.c
@@ -1654,6 +1654,6 @@ MACHINE_START(MACKEREL, "mackerel")
         .init_irq       = sh7372_init_irq,
         .handle_irq     = shmobile_handle_irq_intc,
         .init_machine   = mackerel_init,
-        .init_late      = shmobile_init_late,
+        .init_late      = sh7372_pm_init_late,
         .timer          = &shmobile_timer,
 MACHINE_END
diff --git a/arch/arm/mach-shmobile/include/mach/sh7372.h b/arch/arm/mach-shmobile/include/mach/sh7372.h
index 40beb796d020..eb98b45c5089 100644
--- a/arch/arm/mach-shmobile/include/mach/sh7372.h
+++ b/arch/arm/mach-shmobile/include/mach/sh7372.h
@@ -489,4 +489,6 @@ extern void __init sh7372_init_pm_domains(void);
 static inline void sh7372_init_pm_domains(void) {}
 #endif
 
+extern void __init sh7372_pm_init_late(void);
+
 #endif /* __ASM_SH7372_H__ */
diff --git a/arch/arm/mach-shmobile/pm-sh7372.c b/arch/arm/mach-shmobile/pm-sh7372.c
index 5cafd35cc411..a7a5e20ae9a0 100644
--- a/arch/arm/mach-shmobile/pm-sh7372.c
+++ b/arch/arm/mach-shmobile/pm-sh7372.c
@@ -339,6 +339,21 @@ static void sh7372_enter_a3sm_common(int pllc0_on)
         sh7372_set_reset_vector(__pa(sh7372_resume_core_standby_sysc));
         sh7372_enter_sysc(pllc0_on, 1 << 12);
 }
+
+static void sh7372_enter_a4s_common(int pllc0_on)
+{
+        sh7372_intca_suspend();
+        sh7372_set_reset_vector(SMFRAM);
+        sh7372_enter_sysc(pllc0_on, 1 << 10);
+        sh7372_intca_resume();
+}
+
+static void sh7372_pm_setup_smfram(void)
+{
+        memcpy((void *)SMFRAM, sh7372_resume_core_standby_sysc, 0x100);
+}
+#else
+static inline void sh7372_pm_setup_smfram(void) {}
 #endif /* CONFIG_SUSPEND || CONFIG_CPU_IDLE */
 
 #ifdef CONFIG_CPU_IDLE
@@ -378,11 +393,24 @@ static int sh7372_enter_a3sm_pll_off(struct cpuidle_device *dev,
         return 3;
 }
 
+static int sh7372_enter_a4s(struct cpuidle_device *dev,
+                            struct cpuidle_driver *drv, int index)
+{
+        unsigned long msk, msk2;
+
+        if (!sh7372_sysc_valid(&msk, &msk2))
+                return sh7372_enter_a3sm_pll_off(dev, drv, index);
+
+        sh7372_setup_sysc(msk, msk2);
+        sh7372_enter_a4s_common(0);
+        return 4;
+}
+
 static struct cpuidle_driver sh7372_cpuidle_driver = {
         .name                   = "sh7372_cpuidle",
         .owner                  = THIS_MODULE,
         .en_core_tk_irqen       = 1,
-        .state_count            = 4,
+        .state_count            = 5,
         .safe_state_index       = 0, /* C1 */
         .states[0] = ARM_CPUIDLE_WFI_STATE,
         .states[0].enter = shmobile_enter_wfi,
@@ -410,6 +438,15 @@ static struct cpuidle_driver sh7372_cpuidle_driver = {
                 .flags = CPUIDLE_FLAG_TIME_VALID,
                 .enter = sh7372_enter_a3sm_pll_off,
         },
+        .states[4] = {
+                .name = "C5",
+                .desc = "A4S PLL OFF",
+                .exit_latency = 240,
+                .target_residency = 30 + 240,
+                .flags = CPUIDLE_FLAG_TIME_VALID,
+                .enter = sh7372_enter_a4s,
+                .disabled = true,
+        },
 };
 
 static void sh7372_cpuidle_init(void)
@@ -421,15 +458,6 @@ static void sh7372_cpuidle_init(void) {}
 #endif
 
 #ifdef CONFIG_SUSPEND
-static void sh7372_enter_a4s_common(int pllc0_on)
-{
-        sh7372_intca_suspend();
-        memcpy((void *)SMFRAM, sh7372_resume_core_standby_sysc, 0x100);
-        sh7372_set_reset_vector(SMFRAM);
-        sh7372_enter_sysc(pllc0_on, 1 << 10);
-        sh7372_intca_resume();
-}
-
 static int sh7372_enter_suspend(suspend_state_t suspend_state)
 {
         unsigned long msk, msk2;
@@ -497,6 +525,14 @@ void __init sh7372_pm_init(void)
         /* do not convert A3SM, A3SP, A3SG, A4R power down into A4S */
         __raw_writel(0, PDNSEL);
 
+        sh7372_pm_setup_smfram();
+
         sh7372_suspend_init();
         sh7372_cpuidle_init();
 }
+
+void __init sh7372_pm_init_late(void)
+{
+        shmobile_init_late();
+        pm_genpd_name_attach_cpuidle("A4S", 4);
+}