4 files changed, 125 insertions, 13 deletions

diff --git a/arch/sh/include/asm/suspend.h b/arch/sh/include/asm/suspend.h
index b1b995370e7..5c8ea28ff7a 100644
--- a/arch/sh/include/asm/suspend.h
+++ b/arch/sh/include/asm/suspend.h
@@ -10,6 +10,15 @@ struct swsusp_arch_regs {
         struct pt_regs user_regs;
         unsigned long bank1_regs[8];
 };
+
+void sh_mobile_call_standby(unsigned long mode);
+
+#ifdef CONFIG_CPU_IDLE
+void sh_mobile_setup_cpuidle(void);
+#else
+static inline void sh_mobile_setup_cpuidle(void) {}
+#endif
+
 #endif
 
 /* flags passed to assembly suspend code */
diff --git a/arch/sh/kernel/cpu/shmobile/Makefile b/arch/sh/kernel/cpu/shmobile/Makefile
index 08bfa7c7db2..e8a5111e848 100644
--- a/arch/sh/kernel/cpu/shmobile/Makefile
+++ b/arch/sh/kernel/cpu/shmobile/Makefile
@@ -4,3 +4,4 @@
 
 # Power Management & Sleep mode
 obj-$(CONFIG_PM)        += pm.o sleep.o
+obj-$(CONFIG_CPU_IDLE)  += cpuidle.o
diff --git a/arch/sh/kernel/cpu/shmobile/cpuidle.c b/arch/sh/kernel/cpu/shmobile/cpuidle.c
new file mode 100644
index 00000000000..4afdd975cc6
--- /dev/null
+++ b/arch/sh/kernel/cpu/shmobile/cpuidle.c
@@ -0,0 +1,102 @@
+/*
+ * arch/sh/kernel/cpu/shmobile/cpuidle.c
+ *
+ * Cpuidle support code for SuperH Mobile
+ *
+ *  Copyright (C) 2009 Magnus Damm
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
+ */
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/io.h>
+#include <linux/suspend.h>
+#include <linux/cpuidle.h>
+#include <asm/suspend.h>
+#include <asm/uaccess.h>
+#include <asm/hwblk.h>
+
+static unsigned long cpuidle_mode[] = {
+        SUSP_SH_SLEEP, /* regular sleep mode */
+        SUSP_SH_SLEEP | SUSP_SH_SF, /* sleep mode + self refresh */
+};
+
+static int cpuidle_sleep_enter(struct cpuidle_device *dev,
+                               struct cpuidle_state *state)
+{
+        unsigned long allowed_mode = arch_hwblk_sleep_mode();
+        ktime_t before, after;
+        int requested_state = state - &dev->states[0];
+        int allowed_state;
+        int k;
+
+        /* convert allowed mode to allowed state */
+        for (k = ARRAY_SIZE(cpuidle_mode) - 1; k > 0; k--)
+                if (cpuidle_mode[k] == allowed_mode)
+                        break;
+
+        allowed_state = k;
+
+        /* take the following into account for sleep mode selection:
+         * - allowed_state: best mode allowed by hardware (clock deps)
+         * - requested_state: best mode allowed by software (latencies)
+         */
+        k = min_t(int, allowed_state, requested_state);
+
+        dev->last_state = &dev->states[k];
+        before = ktime_get();
+        sh_mobile_call_standby(cpuidle_mode[k]);
+        after = ktime_get();
+        return ktime_to_ns(ktime_sub(after, before)) >> 10;
+}
+
+static struct cpuidle_device cpuidle_dev;
+static struct cpuidle_driver cpuidle_driver = {
+        .name =         "sh_idle",
+        .owner =        THIS_MODULE,
+};
+
+void sh_mobile_setup_cpuidle(void)
+{
+        struct cpuidle_device *dev = &cpuidle_dev;
+        struct cpuidle_state *state;
+        int i;
+
+        cpuidle_register_driver(&cpuidle_driver);
+
+        for (i = 0; i < CPUIDLE_STATE_MAX; i++) {
+                dev->states[i].name[0] = '\0';
+                dev->states[i].desc[0] = '\0';
+        }
+
+        i = CPUIDLE_DRIVER_STATE_START;
+
+        state = &dev->states[i++];
+        snprintf(state->name, CPUIDLE_NAME_LEN, "C0");
+        strncpy(state->desc, "SuperH Sleep Mode", CPUIDLE_DESC_LEN);
+        state->exit_latency = 1;
+        state->target_residency = 1 * 2;
+        state->power_usage = 3;
+        state->flags = 0;
+        state->flags |= CPUIDLE_FLAG_SHALLOW;
+        state->flags |= CPUIDLE_FLAG_TIME_VALID;
+        state->enter = cpuidle_sleep_enter;
+
+        dev->safe_state = state;
+
+        state = &dev->states[i++];
+        snprintf(state->name, CPUIDLE_NAME_LEN, "C1");
+        strncpy(state->desc, "SuperH Sleep Mode [SF]", CPUIDLE_DESC_LEN);
+        state->exit_latency = 100;
+        state->target_residency = 1 * 2;
+        state->power_usage = 1;
+        state->flags = 0;
+        state->flags |= CPUIDLE_FLAG_TIME_VALID;
+        state->enter = cpuidle_sleep_enter;
+
+        dev->state_count = i;
+
+        cpuidle_register_device(dev);
+}
diff --git a/arch/sh/kernel/cpu/shmobile/pm.c b/arch/sh/kernel/cpu/shmobile/pm.c
index 8c067adf683..de078d24ce5 100644
--- a/arch/sh/kernel/cpu/shmobile/pm.c
+++ b/arch/sh/kernel/cpu/shmobile/pm.c
@@ -1,5 +1,5 @@
 /*
- * arch/sh/kernel/cpu/sh4a/pm-sh_mobile.c
+ * arch/sh/kernel/cpu/shmobile/pm.c
  *
  * Power management support code for SuperH Mobile
  *
@@ -32,20 +32,17 @@
  *
  * R-standby mode is unsupported, but will be added in the future
  * U-standby mode is low priority since it needs bootloader hacks
- *
- * All modes should be tied in with cpuidle. But before that can
- * happen we need to keep track of enabled hardware blocks so we
- * can avoid entering sleep modes that stop clocks to hardware
- * blocks that are in use even though the cpu core is idle.
  */
 
+#define ILRAM_BASE 0xe5200000
+
 extern const unsigned char sh_mobile_standby[];
 extern const unsigned int sh_mobile_standby_size;
 
-static void sh_mobile_call_standby(unsigned long mode)
+void sh_mobile_call_standby(unsigned long mode)
 {
         extern void *vbr_base;
-        void *onchip_mem = (void *)0xe5200000; /* ILRAM */
+        void *onchip_mem = (void *)ILRAM_BASE;
         void (*standby_onchip_mem)(unsigned long) = onchip_mem;
 
         /* Note: Wake up from sleep may generate exceptions!
@@ -55,11 +52,6 @@ static void sh_mobile_call_standby(unsigned long mode)
         if (mode & SUSP_SH_SF)
                 asm volatile("ldc %0, vbr" : : "r" (onchip_mem) : "memory");
 
-        /* Copy the assembly snippet to the otherwise ununsed ILRAM */
-        memcpy(onchip_mem, sh_mobile_standby, sh_mobile_standby_size);
-        wmb();
-        ctrl_barrier();
-
         /* Let assembly snippet in on-chip memory handle the rest */
         standby_onchip_mem(mode);
 
@@ -85,7 +77,15 @@ static struct platform_suspend_ops sh_pm_ops = {
 
 static int __init sh_pm_init(void)
 {
+        void *onchip_mem = (void *)ILRAM_BASE;
+
+        /* Copy the assembly snippet to the otherwise ununsed ILRAM */
+        memcpy(onchip_mem, sh_mobile_standby, sh_mobile_standby_size);
+        wmb();
+        ctrl_barrier();
+
         suspend_set_ops(&sh_pm_ops);
+        sh_mobile_setup_cpuidle();
         return 0;
 }