Merge branch 'glados/deepidle'

7 files changed, 741 insertions, 12 deletions

diff --git a/arch/arm/mach-s5pv210/Kconfig b/arch/arm/mach-s5pv210/Kconfig
index b564c24..26672b0 100644
--- a/arch/arm/mach-s5pv210/Kconfig
+++ b/arch/arm/mach-s5pv210/Kconfig
@@ -18,6 +18,13 @@ config CPU_S5PV210
 	help
 	  Enable S5PV210 CPU support
 
+config CPU_DIDLE
+        bool "Enable DEEP IDLE support for S5PV210 CPU"
+        depends on CPU_IDLE
+        default n
+        help
+          Enable DEEP IDLE support for S5PV210 CPU.
+
 config S5PV210_SETUP_I2C1
 	bool
 	default	y
diff --git a/arch/arm/mach-s5pv210/Makefile b/arch/arm/mach-s5pv210/Makefile
index ea207cb..1c79fc9 100644
--- a/arch/arm/mach-s5pv210/Makefile
+++ b/arch/arm/mach-s5pv210/Makefile
@@ -59,3 +59,4 @@ obj-$(CONFIG_S5PV210_SETUP_FIMC2)	+= setup-fimc2.o
 
 obj-$(CONFIG_CPU_IDLE)		+= cpuidle.o
 obj-$(CONFIG_CPU_FREQ)		+= dev-cpufreq.o
+obj-$(CONFIG_CPU_DIDLE)		+= didle.o
diff --git a/arch/arm/mach-s5pv210/cpuidle.c b/arch/arm/mach-s5pv210/cpuidle.c
index d7d532a..742bcfa 100644
--- a/arch/arm/mach-s5pv210/cpuidle.c
+++ b/arch/arm/mach-s5pv210/cpuidle.c
@@ -2,6 +2,7 @@
  * arch/arm/mach-s5pv210/cpuidle.c
  *
  * Copyright (c) Samsung Electronics Co. Ltd
+ *           (c) 2011 Ezekeel <notezekeel@googlemail.com>
  *
  * CPU idle driver for S5PV210
  *
@@ -21,21 +22,381 @@
 #include <mach/map.h>
 #include <mach/regs-irq.h>
 #include <mach/regs-clock.h>
+#include <mach/regs-gpio.h>
 #include <plat/pm.h>
 #include <plat/devs.h>
 
-#include <mach/dma.h>
-#include <mach/regs-gpio.h>
+#ifdef CONFIG_CPU_DIDLE
+#include <linux/dma-mapping.h>
+#include <linux/deep_idle.h>
+
+#include <plat/regs-otg.h>
+#include <mach/cpuidle.h>
+#include <mach/power-domain.h>
+
+extern bool suspend_ongoing(void);
+extern bool bt_is_running(void);
+extern bool gps_is_running(void);
+extern bool vibrator_is_running(void);
+
+/*
+ * For saving & restoring VIC register before entering
+ * didle mode
+ */
+static unsigned long vic_regs[4];
+static unsigned long *regs_save;
+static dma_addr_t phy_regs_save;
+
+#define MAX_CHK_DEV	0xf
+
+/*
+ * Specific device list for checking before entering
+ * didle mode
+ */
+struct check_device_op {
+	void __iomem		*base;
+	struct platform_device	*pdev;
+};
+
+/* Array of checking devices list */
+static struct check_device_op chk_dev_op[] = {
+#if defined(CONFIG_S3C_DEV_HSMMC)
+	{.base = 0, .pdev = &s3c_device_hsmmc0},
+#endif
+#if defined(CONFIG_S3C_DEV_HSMMC1)
+	{.base = 0, .pdev = &s3c_device_hsmmc1},
+#endif
+#if 0
+	{.base = 0, .pdev = &s3c_device_hsmmc2},
+#endif
+#if defined(CONFIG_S3C_DEV_HSMMC3)
+	{.base = 0, .pdev = &s3c_device_hsmmc3},
+#endif
+	{.base = 0, .pdev = NULL},
+};
+
+#define S3C_HSMMC_PRNSTS	(0x24)
+#define S3C_HSMMC_CLKCON	(0x2c)
+#define S3C_HSMMC_CMD_INHIBIT	0x00000001
+#define S3C_HSMMC_DATA_INHIBIT	0x00000002
+#define S3C_HSMMC_CLOCK_CARD_EN	0x0004
+
+static int sdmmc_dev_num;
+/* If SD/MMC interface is working: return = 1 or not 0 */
+static int check_sdmmc_op(unsigned int ch)
+{
+	unsigned int reg1, reg2;
+	void __iomem *base_addr;
+
+	if (unlikely(ch > sdmmc_dev_num)) {
+		printk(KERN_ERR "Invalid ch[%d] for SD/MMC\n", ch);
+		return 0;
+	}
+
+	base_addr = chk_dev_op[ch].base;
+	/* Check CMDINHDAT[1] and CMDINHCMD [0] */
+	reg1 = readl(base_addr + S3C_HSMMC_PRNSTS);
+	/* Check CLKCON [2]: ENSDCLK */
+	reg2 = readl(base_addr + S3C_HSMMC_CLKCON);
+
+	return !!(reg1 & (S3C_HSMMC_CMD_INHIBIT | S3C_HSMMC_DATA_INHIBIT)) ||
+	       (reg2 & (S3C_HSMMC_CLOCK_CARD_EN));
+}
+
+/* Check all sdmmc controller */
+static int loop_sdmmc_check(void)
+{
+	unsigned int iter;
+
+	for (iter = 0; iter < sdmmc_dev_num + 1; iter++) {
+		if (check_sdmmc_op(iter))
+			return 1;
+	}
+	return 0;
+}
+
+/*
+ * Check USBOTG is working or not
+ * GOTGCTL(0xEC000000)
+ * BSesVld (Indicates the Device mode transceiver status)
+ * BSesVld =	1b : B-session is valid
+ *		0b : B-session is not valid
+ */
+static int check_usbotg_op(void)
+{
+	unsigned int val;
+
+	val = __raw_readl(S3C_UDC_OTG_GOTGCTL);
+
+	return val & (B_SESSION_VALID);
+}
+
+/*
+ * Check power gating : LCD, CAM, TV, MFC, G3D
+ * Check clock gating : DMA, USBHOST, I2C
+ */
+extern volatile int s5p_rp_is_running;
+extern int s5p_rp_get_op_level(void);
+
+static int check_power_clock_gating(void)
+{
+	unsigned long val;
+
+	/* check power gating */
+	val = __raw_readl(S5P_NORMAL_CFG);
+	if (val & (S5PV210_PD_LCD | S5PV210_PD_CAM | S5PV210_PD_TV
+				  | S5PV210_PD_MFC | S5PV210_PD_G3D))
+		return 1;
+
+#ifdef CONFIG_SND_S5P_RP
+	if (s5p_rp_get_op_level())
+		return 1;
+#endif
+	/* check clock gating */
+	val = __raw_readl(S5P_CLKGATE_IP0);
+	if (val & (S5P_CLKGATE_IP0_MDMA | S5P_CLKGATE_IP0_PDMA0
+					| S5P_CLKGATE_IP0_PDMA1))
+		return 1;
+
+	val = __raw_readl(S5P_CLKGATE_IP1);
+	if (val & S5P_CLKGATE_IP1_USBHOST)
+		return 1;
+
+	val = __raw_readl(S5P_CLKGATE_IP3);
+	if (val & (S5P_CLKGATE_IP3_I2C0 | S5P_CLKGATE_IP3_I2C_HDMI_DDC
+					| S5P_CLKGATE_IP3_I2C2))
+		return 1;
+
+	return 0;
+}
+
+/*
+ * Skipping enter the didle mode when RTC & I2S interrupts be issued
+ * during critical section of entering didle mode (around 20ms).
+ */
+#ifdef CONFIG_S5P_INTERNAL_DMA
+static int check_idmapos(void)
+{
+	dma_addr_t src;
+
+	i2sdma_getpos(&src);
+	src = src & 0x3FFF;
+	src = 0x4000 - src;
+
+	return src < 0x150;
+}
+#endif
+
+static int check_rtcint(void)
+{
+	unsigned int current_cnt = get_rtc_cnt();
+
+	return current_cnt < 0x40;
+}
+
+/*
+ * Before entering, didle mode GPIO Powe Down Mode
+ * Configuration register has to be set with same state
+ * in Normal Mode
+ */
+#define GPIO_OFFSET		0x20
+#define GPIO_CON_PDN_OFFSET	0x10
+#define GPIO_PUD_PDN_OFFSET	0x14
+#define GPIO_PUD_OFFSET		0x08
+
+static unsigned int pud_pdn[(S5PV210_MP28_BASE - S5PV210_GPA0_BASE) / GPIO_OFFSET + 1];
+static unsigned int con_pdn[(S5PV210_MP28_BASE - S5PV210_GPA0_BASE) / GPIO_OFFSET + 1];
+
+static void s5p_gpio_pdn_conf(void)
+{
+	void __iomem *gpio_base = S5PV210_GPA0_BASE;
+	unsigned int val;
+	int i = 0;
 
-#define S5PC110_MAX_STATES	1
+	do {
+		/* Save power down control state */
+		con_pdn[i] = __raw_readl(gpio_base + GPIO_CON_PDN_OFFSET);
+		/* Keep the previous state in didle mode */
+		__raw_writel(0xffff, gpio_base + GPIO_CON_PDN_OFFSET);
+
+		/* Save power down pull up-down state */
+		pud_pdn[i] = __raw_readl(gpio_base + GPIO_PUD_PDN_OFFSET);
+		/* Pull up-down state in didle is same as normal */
+		val = __raw_readl(gpio_base + GPIO_PUD_OFFSET);
+		__raw_writel(val, gpio_base + GPIO_PUD_PDN_OFFSET);
+
+		gpio_base += GPIO_OFFSET;
+		i++;
+
+	} while (gpio_base <= S5PV210_MP28_BASE);
+
+	return;
+}
+
+static void s5p_gpio_restore_conf(void)
+{
+	void __iomem *gpio_base = S5PV210_GPA0_BASE;
+	int i = 0;
+
+	do {
+		/* Restore power down control state */
+		__raw_writel(con_pdn[i], gpio_base + GPIO_CON_PDN_OFFSET);
+
+		/* Restore power down pull up-down state */
+		__raw_writel(pud_pdn[i], gpio_base + GPIO_PUD_PDN_OFFSET);
+
+		gpio_base += GPIO_OFFSET;
+		i++;
+
+	} while (gpio_base <= S5PV210_MP28_BASE);
+
+	return;
+}
+
+static void s5p_enter_didle(bool top_on)
+{
+	unsigned long tmp;
+	unsigned long save_eint_mask;
+
+	/* store the physical address of the register recovery block */
+	__raw_writel(phy_regs_save, S5P_INFORM2);
+
+	/* ensure at least INFORM0 has the resume address */
+	__raw_writel(virt_to_phys(s5pv210_didle_resume), S5P_INFORM0);
+
+	/* Save current VIC_INT_ENABLE register*/
+	vic_regs[0] = __raw_readl(S5P_VIC0REG(VIC_INT_ENABLE));
+	vic_regs[1] = __raw_readl(S5P_VIC1REG(VIC_INT_ENABLE));
+	vic_regs[2] = __raw_readl(S5P_VIC2REG(VIC_INT_ENABLE));
+	vic_regs[3] = __raw_readl(S5P_VIC3REG(VIC_INT_ENABLE));
+
+	/* Disable all interrupt through VIC */
+	__raw_writel(0xffffffff, S5P_VIC0REG(VIC_INT_ENABLE_CLEAR));
+	__raw_writel(0xffffffff, S5P_VIC1REG(VIC_INT_ENABLE_CLEAR));
+	__raw_writel(0xffffffff, S5P_VIC2REG(VIC_INT_ENABLE_CLEAR));
+	__raw_writel(0xffffffff, S5P_VIC3REG(VIC_INT_ENABLE_CLEAR));
+
+	if (!top_on) {
+	    /* GPIO Power Down Control */
+	    s5p_gpio_pdn_conf();
+	}
+
+	/*
+	 * Configure external interrupt wakeup mask
+	 * We use the same wakeup mask as for sleep state plus make sure 
+	 * that at least XEINT[22] = GPH2[6] = GPIO_nPOWER = GPIO_N_POWER
+	 * and XEINT[29] = GPH3[5] = GPIO_OK_KEY are enabled
+	 */
+	save_eint_mask = __raw_readl(S5P_EINT_WAKEUP_MASK);
+	tmp = s3c_irqwake_eintmask;
+	tmp &= ~((1<<22) | (1<<29));
+	__raw_writel(tmp, S5P_EINT_WAKEUP_MASK);
+
+	/* Clear wakeup status register */
+	tmp = __raw_readl(S5P_WAKEUP_STAT);
+	__raw_writel(tmp, S5P_WAKEUP_STAT);
+
+	/*
+	 * Wakeup source configuration for didle
+	 * We use the same wakeup mask as for sleep state plus make
+	 * sure that at least RTC ALARM, RTC TICK, KEY, I2S and ST are
+	 * enabled as wakeup sources
+	 */
+	tmp = s3c_irqwake_intmask;
+	tmp &= ~((1<<1) | (1<<2) | (1<<5) | (1<<13) | (1<<14));
+	__raw_writel(tmp, S5P_WAKEUP_MASK);
+
+	tmp = __raw_readl(S5P_IDLE_CFG);
+	tmp &= ~(0x3fU << 26);
+	if (top_on) {
+	    /*
+	     * IDLE config register set
+	     * TOP_LOGIC = ON
+	     * TOP_MEMORY = ON
+	     * ARM_L2CACHE = Retention
+	     * CFG_DIDLE = DEEP
+	     */
+	    tmp |= ((2<<30) | (2<<28) | (1<<26) | (1<<0));
+	} else {
+	    /*
+	     * IDLE config register set
+	     * TOP_LOGIC = Retention
+	     * TOP_MEMORY = Retention
+	     * ARM_L2CACHE = Retention
+	     * CFG_DIDLE = DEEP
+	     */
+	    tmp |= ((1<<30) | (1<<28) | (1<<26) | (1<<0));
+	}
+	__raw_writel(tmp, S5P_IDLE_CFG);
+
+	/* Power mode Config setting */
+	tmp = __raw_readl(S5P_PWR_CFG);
+	tmp &= S5P_CFG_WFI_CLEAN;
+	tmp |= S5P_CFG_WFI_IDLE;
+	__raw_writel(tmp, S5P_PWR_CFG);
+
+	/* To check VIC Status register before enter didle mode */
+	if ((__raw_readl(S5P_VIC0REG(VIC_RAW_STATUS)) & vic_regs[0]) |
+	    (__raw_readl(S5P_VIC1REG(VIC_RAW_STATUS)) & vic_regs[1]) |
+	    (__raw_readl(S5P_VIC2REG(VIC_RAW_STATUS)) & vic_regs[2]) |
+	    (__raw_readl(S5P_VIC3REG(VIC_RAW_STATUS)) & vic_regs[3]))
+		goto skipped_didle;
+
+	/* SYSCON_INT_DISABLE */
+	tmp = __raw_readl(S5P_OTHERS);
+	tmp |= S5P_OTHER_SYSC_INTOFF;
+	__raw_writel(tmp, S5P_OTHERS);
+
+	/*
+	 * s5pv210_didle_save will also act as our return point from when
+	 * we resume as it saves its own register state and restore it
+	 * during the resume.
+	 */
+	s5pv210_didle_save(regs_save);
+
+	/* restore the cpu state using the kernel's cpu init code. */
+	cpu_init();
+
+skipped_didle:
+	__raw_writel(save_eint_mask, S5P_EINT_WAKEUP_MASK);
+
+	tmp = __raw_readl(S5P_IDLE_CFG);
+	tmp &= ~((3<<30) | (3<<28) | (3<<26) | (1<<0));
+	tmp |= ((2<<30) | (2<<28));
+	__raw_writel(tmp, S5P_IDLE_CFG);
+
+	/* Power mode Config setting */
+	tmp = __raw_readl(S5P_PWR_CFG);
+	tmp &= S5P_CFG_WFI_CLEAN;
+	__raw_writel(tmp, S5P_PWR_CFG);
+
+	if (!top_on) {
+	    /* Release retention GPIO/CF/MMC/UART IO */
+	    tmp = __raw_readl(S5P_OTHERS);
+	    tmp |= (S5P_OTHERS_RET_IO | S5P_OTHERS_RET_CF |	\
+		    S5P_OTHERS_RET_MMC | S5P_OTHERS_RET_UART);
+	    __raw_writel(tmp, S5P_OTHERS);
+	}
+
+	if (!top_on) {
+	    /* Restore GPIO Power Down Configuration */
+	    s5p_gpio_restore_conf();
+	}
+
+	__raw_writel(vic_regs[0], S5P_VIC0REG(VIC_INT_ENABLE));
+	__raw_writel(vic_regs[1], S5P_VIC1REG(VIC_INT_ENABLE));
+	__raw_writel(vic_regs[2], S5P_VIC2REG(VIC_INT_ENABLE));
+	__raw_writel(vic_regs[3], S5P_VIC3REG(VIC_INT_ENABLE));
+}
+#endif
 
 static void s5p_enter_idle(void)
 {
 	unsigned long tmp;
 
 	tmp = __raw_readl(S5P_IDLE_CFG);
-	tmp &= ~((3<<30)|(3<<28)|(1<<0));
-	tmp |= ((2<<30)|(2<<28));
+	tmp &= ~((3U<<30)|(3<<28)|(1<<0));
+	tmp |= ((2U<<30)|(2<<28));
 	__raw_writel(tmp, S5P_IDLE_CFG);
 
 	tmp = __raw_readl(S5P_PWR_CFG);
@@ -46,21 +407,43 @@ static void s5p_enter_idle(void)
 }
 
 /* Actual code that puts the SoC in different idle states */
-static int s5p_enter_idle_normal(struct cpuidle_device *dev,
+static int s5p_enter_idle_state(struct cpuidle_device *dev,
 				struct cpuidle_state *state)
 {
 	struct timeval before, after;
 	int idle_time;
+#ifdef CONFIG_CPU_DIDLE
+	int idle_state = 0;
+#endif
 
 	local_irq_disable();
 	do_gettimeofday(&before);
 
+#ifdef CONFIG_CPU_DIDLE
+#ifdef CONFIG_S5P_INTERNAL_DMA
+	if (!deepidle_is_enabled() || check_power_clock_gating() || suspend_ongoing() || loop_sdmmc_check() || check_usbotg_op() || check_rtcint() || check_idmapos()) {
+#else
+	if (!deepidle_is_enabled() || check_power_clock_gating() || suspend_ongoing() || loop_sdmmc_check() || check_usbotg_op() || check_rtcint()) {
+#endif
+	    s5p_enter_idle();
+	} else if (bt_is_running() || gps_is_running() || vibrator_is_running()) {
+	    s5p_enter_didle(true);
+	    idle_state = 1;
+	} else {
+	    s5p_enter_didle(false);
+	    idle_state = 2;
+	}
+#else   
 	s5p_enter_idle();
+#endif
 
 	do_gettimeofday(&after);
 	local_irq_enable();
 	idle_time = (after.tv_sec - before.tv_sec) * USEC_PER_SEC +
-			(after.tv_usec - before.tv_usec);
+	    (after.tv_usec - before.tv_usec);
+#ifdef CONFIG_CPU_DIDLE
+	report_idle_time(idle_state, idle_time);
+#endif
 	return idle_time;
 }
 
@@ -75,26 +458,94 @@ static struct cpuidle_driver s5p_idle_driver = {
 static int s5p_init_cpuidle(void)
 {
 	struct cpuidle_device *device;
+	int ret;
 
-	cpuidle_register_driver(&s5p_idle_driver);
+#ifdef CONFIG_CPU_DIDLE
+	struct resource *res;
+	struct platform_device *pdev;
+	int i = 0;
+#endif
+
+	ret = cpuidle_register_driver(&s5p_idle_driver);
+	if (ret) {
+		printk(KERN_ERR "%s: Failed registering driver\n", __func__);
+		goto err;
+	}
 
 	device = &per_cpu(s5p_cpuidle_device, smp_processor_id());
 	device->state_count = 1;
 
 	/* Wait for interrupt state */
-	device->states[0].enter = s5p_enter_idle_normal;
+	device->states[0].enter = s5p_enter_idle_state;
 	device->states[0].exit_latency = 1;	/* uS */
 	device->states[0].target_residency = 10000;
 	device->states[0].flags = CPUIDLE_FLAG_TIME_VALID;
+#ifdef CONFIG_CPU_DIDLE
+	strcpy(device->states[0].name, "(DEEP)IDLE");
+	strcpy(device->states[0].desc, "ARM clock/power gating - WFI");
+#else
 	strcpy(device->states[0].name, "IDLE");
 	strcpy(device->states[0].desc, "ARM clock gating - WFI");
+#endif
+
+	ret = cpuidle_register_device(device);
+	if (ret) {
+		printk(KERN_ERR "%s: Failed registering device\n", __func__);
+		goto err_register_driver;
+	}
 
-	if (cpuidle_register_device(device)) {
-		printk(KERN_ERR "s5p_init_cpuidle: Failed registering\n");
-		return -EIO;
+#ifdef CONFIG_CPU_DIDLE
+	regs_save = dma_alloc_coherent(NULL, 4096, &phy_regs_save, GFP_KERNEL);
+	if (regs_save == NULL) {
+		printk(KERN_ERR "%s: DMA alloc error\n", __func__);
+		ret = -ENOMEM;
+		goto err_register_device;
 	}
+	printk(KERN_INFO "cpuidle: phy_regs_save:0x%x\n", phy_regs_save);
+
+	/* Allocate memory region to access IP's directly */
+	for (i = 0 ; i < MAX_CHK_DEV ; i++) {
+
+		pdev = chk_dev_op[i].pdev;
+
+		if (pdev == NULL) {
+			sdmmc_dev_num = i - 1;
+			break;
+		}
+
+		res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+		if (!res) {
+			printk(KERN_ERR "%s: failed to get io memory region\n",
+					__func__);
+			ret = -EINVAL;
+			goto err_alloc;
+		}
+		chk_dev_op[i].base = ioremap_nocache(res->start, 4096);
+
+		if (!chk_dev_op[i].base) {
+			printk(KERN_ERR "failed to remap io region\n");
+			ret = -EINVAL;
+			goto err_resource;
+		}
+	}
+#endif
 
 	return 0;
+
+#ifdef CONFIG_CPU_DIDLE
+err_alloc:
+	while (--i >= 0) {
+		iounmap(chk_dev_op[i].base);
+	}
+err_resource:
+	dma_free_coherent(NULL, 4096, regs_save, phy_regs_save);
+err_register_device:
+	cpuidle_unregister_device(device);
+#endif
+err_register_driver:
+	cpuidle_unregister_driver(&s5p_idle_driver);
+err:
+	return ret;
 }
 
 device_initcall(s5p_init_cpuidle);
diff --git a/arch/arm/mach-s5pv210/didle.S b/arch/arm/mach-s5pv210/didle.S
new file mode 100644
index 0000000..2abaca0
--- /dev/null
+++ b/arch/arm/mach-s5pv210/didle.S
@@ -0,0 +1,218 @@
+/* linux/arch/arm/mach-s5pv210/didle.S
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+*/
+
+#include <linux/linkage.h>
+#include <asm/assembler.h>
+#include <mach/hardware.h>
+#include <mach/map.h>
+#include <asm/asm-offsets.h>
+#include <asm/memory.h>
+#include <asm/system.h>
+
+/*
+ *	v7_flush_l1_dcache()
+ *
+ *	Flush the L1 D-cache.
+ */
+ENTRY(v7_flush_l1_dcache)
+	dmb					@ ensure ordering with previous memory accesses
+	mrc	p15, 1, r0, c0, c0, 1		@ read clidr
+	ands	r3, r0, #0x7000000		@ extract loc from clidr
+	mov	r3, r3, lsr #23			@ left align loc bit field
+	beq	finished			@ if loc is 0, then no need to clean
+	mov	r10, #0				@ start clean at cache level 0
+loop1:
+	add	r2, r10, r10, lsr #1		@ work out 3x current cache level
+	mov	r1, r0, lsr r2			@ extract cache type bits from clidr
+	and	r1, r1, #7			@ mask of the bits for current cache only
+	cmp	r1, #2				@ see what cache we have at this level
+	blt	finished			@ finish if no cache, or just i-cache
+	mcr	p15, 2, r10, c0, c0, 0		@ select current cache level in cssr
+	isb					@ isb to sych the new cssr&csidr
+	mrc	p15, 1, r1, c0, c0, 0		@ read the new csidr
+	and	r2, r1, #7			@ extract the length of the cache lines
+	add	r2, r2, #4			@ add 4 (line length offset)
+	ldr	r4, =0x3ff
+	ands	r4, r4, r1, lsr #3		@ find maximum number on the way size
+	clz	r5, r4				@ find bit position of way size increment
+	ldr	r7, =0x7fff
+	ands	r7, r7, r1, lsr #13		@ extract max number of the index size
+loop2:
+	mov	r9, r4				@ create working copy of max way size
+loop3:
+	orr	r11, r10, r9, lsl r5		@ factor way and cache number into r11
+	orr	r11, r11, r7, lsl r2		@ factor index number into r11
+	mcr	p15, 0, r11, c7, c14, 2		@ clean & invalidate by set/way
+	subs	r9, r9, #1			@ decrement the way
+	bge	loop3
+	subs	r7, r7, #1			@ decrement the index
+	bge	loop2
+finished:
+	mov	r10, #0				@ swith back to cache level 0
+	mcr	p15, 2, r10, c0, c0, 0		@ select current cache level in cssr
+	dsb
+	isb
+	mov	pc, lr
+ENDPROC(v7_flush_l1_dcache)
+
+ENTRY(v7_flush_cache_for_didle)
+	stmfd	sp!, {r4-r5, r7, r9-r11, lr}
+	bl	v7_flush_l1_dcache
+	mov	r0, #0
+	mcr	p15, 0, r0, c7, c5, 0		@ I+BTB cache invalidate
+	ldmfd	sp!, {r4-r5, r7, r9-r11, lr}
+	mov	pc, lr
+ENDPROC(v7_flush_cache_for_didle)
+
+ENTRY(s5pv210_didle)
+	stmfd	sp!, {r4-r5, r7, r9-r11, lr}
+
+	bl	v7_flush_cache_for_didle
+
+	ldmfd	sp!, {r4-r5, r7, r9-r11, lr}
+	dmb
+	dsb
+	wfi
+
+	b	.
+
+	.text
+
+	/* s5pv210_didle_save
+	 *
+	 * entry:
+	 *	r0 = save address (virtual addr of s3c_sleep_save_phys)
+	*/
+
+ENTRY(s5pv210_didle_save)
+
+	stmfd	sp!, { r3 - r12, lr }
+
+	mrc	p15, 0, r4, c13, c0, 0	@ FCSE/PID
+	mrc	p15, 0, r5, c3, c0, 0	@ Domain ID
+	mrc	p15, 0, r6, c2, c0, 0	@ Translation Table BASE0
+	mrc	p15, 0, r7, c2, c0, 1	@ Translation Table BASE1
+	mrc	p15, 0, r8, c2, c0, 2	@ Translation Table Control
+	mrc	p15, 0, r9, c1, c0, 0	@ Control register
+	mrc	p15, 0, r10, c1, c0, 1	@ Auxiliary control register
+	mrc	p15, 0, r11, c1, c0, 2	@ Co-processor access controls
+	mrc	p15, 0, r12, c10, c2, 0	@ Read PRRR
+	mrc	p15, 0, r3, c10, c2, 1	@ READ NMRR
+
+	/* Save CP15 registers */
+	stmia	r0, { r3 - r13 }
+
+	bl s5pv210_didle
+
+	@@ return to the caller, after having the MMU
+	@@ turned on, this restores the last bits from the
+	@@ stack
+resume_with_mmu:
+	mrc     p15, 0, r0, c1, c0, 1	@enable L2 cache
+	orr     r0, r0, #(1<<1)
+	mcr     p15, 0, r0, c1, c0, 1
+
+	mov	r0, #1
+	/* delete added mmu table list */
+	ldr	r9 , =(PAGE_OFFSET - PLAT_PHYS_OFFSET)
+	add	r4, r4, r9
+	str	r12, [r4]
+
+	ldmfd	sp!, { r3 - r12, pc }
+
+	.ltorg
+
+	/* s5pv210_didle_resume
+	 *
+	 * resume code entry for bootloader to call
+	 *
+	 * we must put this code here in the data segment as we have no
+	 * other way of restoring the stack pointer after sleep, and we
+	 * must not write to the code segment (code is read-only)
+	*/
+
+ENTRY(s5pv210_didle_resume)
+	mov	r0, #PSR_I_BIT | PSR_F_BIT | SVC_MODE
+	msr	cpsr_c, r0
+
+	@@ load UART to allow us to print the two characters for
+	@@ resume debug
+
+	mov	r1, #0
+	mcr	p15, 0, r1, c8, c7, 0		@@ invalidate TLBs
+	mcr	p15, 0, r1, c7, c5, 0		@@ invalidate I Cache
+
+	ldr	r1, =0xe010f008		@ Read INFORM2 register
+	ldr	r0, [r1]		@ Load phy_regs_save value
+	ldmia	r0, { r3 - r13 }
+
+	mcr	p15, 0, r4, c13, c0, 0	@ FCSE/PID
+	mcr	p15, 0, r5, c3, c0, 0	@ Domain ID
+
+	mcr	p15, 0, r8, c2, c0, 2	@ Translation Table Control
+	mcr	p15, 0, r7, c2, c0, 1	@ Translation Table BASE1
+	mcr	p15, 0, r6, c2, c0, 0	@ Translation Table BASE0
+
+	bic	r10, r10, #(1<<1)	@ disable L2cache
+	mcr	p15, 0, r10, c1, c0, 1	@ Auxiliary control register
+
+	mov	r0, #0
+	mcr	p15, 0, r0, c8, c7, 0	@ Invalidate I & D TLB
+
+	mov	r0, #0			@ restore copro access controls
+	mcr	p15, 0, r11, c1, c0, 2	@ Co-processor access controls
+	mcr 	p15, 0, r0, c7, c5, 4
+
+	mcr	p15, 0, r12, c10, c2, 0	@ write PRRR
+	mcr	p15, 0, r3, c10, c2, 1	@ write NMRR
+
+	/* calculate first section address into r8 */
+	mov	r4, r6
+	ldr	r5, =0x3fff
+	bic	r4, r4, r5
+	ldr	r11, =0xe010f000
+	ldr	r10, [r11, #0]
+	mov	r10, r10 ,LSR #18
+	bic	r10, r10, #0x3
+	orr	r4, r4, r10
+
+	/* calculate mmu list value into r9 */
+	mov 	r10, r10, LSL #18
+	ldr	r5, =0x40e
+	orr	r10, r10, r5
+
+	/* back up originally data */
+	ldr	r12, [r4]
+
+	/* Added list about mmu */
+	str	r10, [r4]
+
+	ldr	r2, =resume_with_mmu
+	mcr	p15, 0, r9, c1, c0, 0		@ turn on MMU, etc
+
+        nop
+        nop
+        nop
+        nop
+        nop					@ second-to-last before mmu
+
+	mov	pc, r2				@ go back to virtual address
+
+	.ltorg
diff --git a/arch/arm/mach-s5pv210/herring-rfkill.c b/arch/arm/mach-s5pv210/herring-rfkill.c
index 2115d9f..ce7408f 100644
--- a/arch/arm/mach-s5pv210/herring-rfkill.c
+++ b/arch/arm/mach-s5pv210/herring-rfkill.c
@@ -49,6 +49,16 @@ static struct wake_lock rfkill_wake_lock;
 static struct rfkill *bt_rfk;
 static const char bt_name[] = "bcm4329";
 
+#ifdef CONFIG_CPU_DIDLE
+static bool bt_running = false;
+
+bool bt_is_running(void)
+{
+    return bt_running;
+}
+EXPORT_SYMBOL(bt_is_running);
+#endif
+
 static int bluetooth_set_power(void *data, enum rfkill_user_states state)
 {
 	int ret = 0;
@@ -118,6 +128,10 @@ static int bluetooth_set_power(void *data, enum rfkill_user_states state)
 	case RFKILL_USER_STATE_SOFT_BLOCKED:
 		pr_debug("[BT] Device Powering OFF\n");
 
+#ifdef CONFIG_CPU_DIDLE
+		bt_running = false;
+#endif
+
 		ret = disable_irq_wake(irq);
 		if (ret < 0)
 			pr_err("[BT] unset wakeup src failed\n");
@@ -159,6 +173,10 @@ irqreturn_t bt_host_wake_irq_handler(int irq, void *dev_id)
 {
 	pr_debug("[BT] bt_host_wake_irq_handler start\n");
 
+#ifdef CONFIG_CPU_DIDLE
+	bt_running = true;
+#endif
+
 	if (gpio_get_value(GPIO_BT_HOST_WAKE))
 		wake_lock(&rfkill_wake_lock);
 	else
diff --git a/arch/arm/mach-s5pv210/herring-vibrator.c b/arch/arm/mach-s5pv210/herring-vibrator.c
index 323520f..82959f5 100644
--- a/arch/arm/mach-s5pv210/herring-vibrator.c
+++ b/arch/arm/mach-s5pv210/herring-vibrator.c
@@ -43,11 +43,25 @@ static struct vibrator {
 	struct work_struct work;
 } vibdata;
 
+#ifdef CONFIG_CPU_DIDLE
+static bool vibrator_running = false;
+
+bool vibrator_is_running(void)
+{
+    return vibrator_running;
+}
+EXPORT_SYMBOL(vibrator_is_running);
+#endif
+
 static void herring_vibrator_off(void)
 {
 	pwm_disable(vibdata.pwm_dev);
 	gpio_direction_output(GPIO_VIBTONE_EN1, GPIO_LEVEL_LOW);
 	wake_unlock(&vibdata.wklock);
+
+#ifdef CONFIG_CPU_DIDLE
+	vibrator_running = false;
+#endif
 }
 
 static int herring_vibrator_get_time(struct timed_output_dev *dev)
@@ -68,6 +82,10 @@ static void herring_vibrator_enable(struct timed_output_dev *dev, int value)
 	hrtimer_cancel(&vibdata.timer);
 	cancel_work_sync(&vibdata.work);
 	if (value) {
+#ifdef CONFIG_CPU_DIDLE
+		vibrator_running = true;
+#endif
+
 		wake_lock(&vibdata.wklock);
 		pwm_config(vibdata.pwm_dev, PWM_DUTY, PWM_PERIOD);
 		pwm_enable(vibdata.pwm_dev);
diff --git a/arch/arm/mach-s5pv210/include/mach/cpuidle.h b/arch/arm/mach-s5pv210/include/mach/cpuidle.h
new file mode 100644
index 0000000..7454ae4
--- /dev/null
+++ b/arch/arm/mach-s5pv210/include/mach/cpuidle.h
@@ -0,0 +1,16 @@
+/* arch/arm/mach-s5pv210/include/mach/cpuidle.h
+ *
+ * Copyright 2010 Samsung Electronics
+ *	Jaecheol Lee <jc.lee@samsung>
+ *
+ * S5PV210 - CPUIDLE support
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+*/
+
+extern int  s5pv210_didle_save(unsigned long *saveblk);
+extern void s5pv210_didle_resume(void);
+extern void i2sdma_getpos(dma_addr_t *src);
+extern unsigned int get_rtc_cnt(void);