[ARM] 3785/1: S3C2412: Fix idle code as default uses wrong clocks

Patch from Ben Dooks

Fix the idle code on the s3c2412 as the default
code is using bits in the CLKCON register that are
no-longer there.

Provide an override for the idle code, and ensure
that the power configuration is set to allow idle
instead of stop or sleep.

Signed-off-by: Ben Dooks <ben-linux@fluff.org>
Signed-off-by: Russell King <rmk+kernel@arm.linux.org.uk>

1 files changed, 29 insertions, 0 deletions

diff --git a/arch/arm/mach-s3c2410/s3c2412.c b/arch/arm/mach-s3c2410/s3c2412.c
index e24ffd5e478b..2d163f7600be 100644
--- a/arch/arm/mach-s3c2410/s3c2412.c
+++ b/arch/arm/mach-s3c2410/s3c2412.c
@@ -35,11 +35,15 @@
 #include <asm/mach/irq.h>
 
 #include <asm/hardware.h>
+#include <asm/proc-fns.h>
 #include <asm/io.h>
 #include <asm/irq.h>
 
+#include <asm/arch/idle.h>
+
 #include <asm/arch/regs-clock.h>
 #include <asm/arch/regs-serial.h>
+#include <asm/arch/regs-power.h>
 #include <asm/arch/regs-gpio.h>
 #include <asm/arch/regs-gpioj.h>
 #include <asm/arch/regs-dsc.h>
@@ -75,6 +79,27 @@ void __init s3c2412_init_uarts(struct s3c2410_uartcfg *cfg, int no)
         s3c_device_nand.name = "s3c2412-nand";
 }
 
+/* s3c2412_idle
+ *
+ * use the standard idle call by ensuring the idle mode
+ * in power config, then issuing the idle co-processor
+ * instruction
+*/
+
+static void s3c2412_idle(void)
+{
+        unsigned long tmp;
+
+        /* ensure our idle mode is to go to idle */
+
+        tmp = __raw_readl(S3C2412_PWRCFG);
+        tmp &= ~S3C2412_PWRCFG_STANDBYWFI_MASK;
+        tmp |= S3C2412_PWRCFG_STANDBYWFI_IDLE;
+        __raw_writel(tmp, S3C2412_PWRCFG);
+
+        cpu_do_idle();
+}
+
 /* s3c2412_map_io
  *
  * register the standard cpu IO areas, and any passed in from the
@@ -87,6 +112,10 @@ void __init s3c2412_map_io(struct map_desc *mach_desc, int mach_size)
 
         s3c24xx_va_gpio2 = S3C24XX_VA_GPIO + 0x10;
 
+        /* set our idle function */
+
+        s3c24xx_idle = s3c2412_idle;
+
         /* register our io-tables */
 
         iotable_init(s3c2412_iodesc, ARRAY_SIZE(s3c2412_iodesc));