1 files changed, 214 insertions, 33 deletions

diff --git a/arch/arm/plat-s3c24xx/pm.c b/arch/arm/plat-s3c24xx/pm.c
index 4fdb3117744f..bf5581a9aeea 100644
--- a/arch/arm/plat-s3c24xx/pm.c
+++ b/arch/arm/plat-s3c24xx/pm.c
@@ -83,38 +83,39 @@ static struct sleep_save core_save[] = {
 	SAVE_ITEM(S3C2410_REFRESH),
 };
 
-static struct sleep_save gpio_save[] = {
-	SAVE_ITEM(S3C2410_GPACON),
-	SAVE_ITEM(S3C2410_GPADAT),
-
-	SAVE_ITEM(S3C2410_GPBCON),
-	SAVE_ITEM(S3C2410_GPBDAT),
-	SAVE_ITEM(S3C2410_GPBUP),
-
-	SAVE_ITEM(S3C2410_GPCCON),
-	SAVE_ITEM(S3C2410_GPCDAT),
-	SAVE_ITEM(S3C2410_GPCUP),
-
-	SAVE_ITEM(S3C2410_GPDCON),
-	SAVE_ITEM(S3C2410_GPDDAT),
-	SAVE_ITEM(S3C2410_GPDUP),
-
-	SAVE_ITEM(S3C2410_GPECON),
-	SAVE_ITEM(S3C2410_GPEDAT),
-	SAVE_ITEM(S3C2410_GPEUP),
-
-	SAVE_ITEM(S3C2410_GPFCON),
-	SAVE_ITEM(S3C2410_GPFDAT),
-	SAVE_ITEM(S3C2410_GPFUP),
-
-	SAVE_ITEM(S3C2410_GPGCON),
-	SAVE_ITEM(S3C2410_GPGDAT),
-	SAVE_ITEM(S3C2410_GPGUP),
-
-	SAVE_ITEM(S3C2410_GPHCON),
-	SAVE_ITEM(S3C2410_GPHDAT),
-	SAVE_ITEM(S3C2410_GPHUP),
+static struct gpio_sleep {
+	void __iomem	*base;
+	unsigned int	 gpcon;
+	unsigned int	 gpdat;
+	unsigned int	 gpup;
+} gpio_save[] = {
+	[0] = {
+		.base	= S3C2410_GPACON,
+	},
+	[1] = {
+		.base	= S3C2410_GPBCON,
+	},
+	[2] = {
+		.base	= S3C2410_GPCCON,
+	},
+	[3] = {
+		.base	= S3C2410_GPDCON,
+	},
+	[4] = {
+		.base	= S3C2410_GPECON,
+	},
+	[5] = {
+		.base	= S3C2410_GPFCON,
+	},
+	[6] = {
+		.base	= S3C2410_GPGCON,
+	},
+	[7] = {
+		.base	= S3C2410_GPHCON,
+	},
+};
 
+static struct sleep_save misc_save[] = {
 	SAVE_ITEM(S3C2410_DCLKCON),
 };
 
@@ -486,6 +487,184 @@ static void s3c2410_pm_configure_extint(void)
 	}
 }
 
+/* offsets for CON/DAT/UP registers */
+
+#define OFFS_CON	(S3C2410_GPACON - S3C2410_GPACON)
+#define OFFS_DAT	(S3C2410_GPADAT - S3C2410_GPACON)
+#define OFFS_UP		(S3C2410_GPBUP  - S3C2410_GPBCON)
+
+/* s3c2410_pm_save_gpios()
+ *
+ * Save the state of the GPIOs
+ */
+
+static void s3c2410_pm_save_gpios(void)
+{
+	struct gpio_sleep *gps = gpio_save;
+	unsigned int gpio;
+
+	for (gpio = 0; gpio < ARRAY_SIZE(gpio_save); gpio++, gps++) {
+		void __iomem *base = gps->base;
+
+		gps->gpcon = __raw_readl(base + OFFS_CON);
+		gps->gpdat = __raw_readl(base + OFFS_DAT);
+
+		if (gpio > 0)
+			gps->gpup = __raw_readl(base + OFFS_UP);
+
+	}
+}
+
+/* Test whether the given masked+shifted bits of an GPIO configuration
+ * are one of the SFN (special function) modes. */
+
+static inline int is_sfn(unsigned long con)
+{
+	return (con == 2 || con == 3);
+}
+
+/* Test if the given masked+shifted GPIO configuration is an input */
+
+static inline int is_in(unsigned long con)
+{
+	return con == 0;
+}
+
+/* Test if the given masked+shifted GPIO configuration is an output */
+
+static inline int is_out(unsigned long con)
+{
+	return con == 1;
+}
+
+/* s3c2410_pm_restore_gpio()
+ *
+ * Restore one of the GPIO banks that was saved during suspend. This is
+ * not as simple as once thought, due to the possibility of glitches
+ * from the order that the CON and DAT registers are set in.
+ *
+ * The three states the pin can be are {IN,OUT,SFN} which gives us 9
+ * combinations of changes to check. Three of these, if the pin stays
+ * in the same configuration can be discounted. This leaves us with
+ * the following:
+ *
+ * { IN => OUT }  Change DAT first
+ * { IN => SFN }  Change CON first
+ * { OUT => SFN } Change CON first, so new data will not glitch
+ * { OUT => IN }  Change CON first, so new data will not glitch
+ * { SFN => IN }  Change CON first
+ * { SFN => OUT } Change DAT first, so new data will not glitch [1]
+ *
+ * We do not currently deal with the UP registers as these control
+ * weak resistors, so a small delay in change should not need to bring
+ * these into the calculations.
+ *
+ * [1] this assumes that writing to a pin DAT whilst in SFN will set the
+ *     state for when it is next output.
+ */
+
+static void s3c2410_pm_restore_gpio(int index, struct gpio_sleep *gps)
+{
+	void __iomem *base = gps->base;
+	unsigned long gps_gpcon = gps->gpcon;
+	unsigned long gps_gpdat = gps->gpdat;
+	unsigned long old_gpcon;
+	unsigned long old_gpdat;
+	unsigned long old_gpup = 0x0;
+	unsigned long gpcon;
+	int nr;
+
+	old_gpcon = __raw_readl(base + OFFS_CON);
+	old_gpdat = __raw_readl(base + OFFS_DAT);
+
+	if (base == S3C2410_GPACON) {
+		/* GPACON only has one bit per control / data and no PULLUPs.
+		 * GPACON[x] = 0 => Output, 1 => SFN */
+
+		/* first set all SFN bits to SFN */
+
+		gpcon = old_gpcon | gps->gpcon;
+		__raw_writel(gpcon, base + OFFS_CON);
+
+		/* now set all the other bits */
+
+		__raw_writel(gps_gpdat, base + OFFS_DAT);
+		__raw_writel(gps_gpcon, base + OFFS_CON);
+	} else {
+		unsigned long old, new, mask;
+		unsigned long change_mask = 0x0;
+
+		old_gpup = __raw_readl(base + OFFS_UP);
+
+		/* Create a change_mask of all the items that need to have
+		 * their CON value changed before their DAT value, so that
+		 * we minimise the work between the two settings.
+		 */
+
+		for (nr = 0, mask = 0x03; nr < 32; nr += 2, mask <<= 2) {
+			old = (old_gpcon & mask) >> nr;
+			new = (gps_gpcon & mask) >> nr;
+
+			/* If there is no change, then skip */
+
+			if (old == new)
+				continue;
+
+			/* If both are special function, then skip */
+
+			if (is_sfn(old) && is_sfn(new))
+				continue;
+
+			/* Change is IN => OUT, do not change now */
+
+			if (is_in(old) && is_out(new))
+				continue;
+
+			/* Change is SFN => OUT, do not change now */
+
+			if (is_sfn(old) && is_out(new))
+				continue;
+
+			/* We should now be at the case of IN=>SFN,
+			 * OUT=>SFN, OUT=>IN, SFN=>IN. */
+
+			change_mask |= mask;
+		}
+
+		/* Write the new CON settings */
+
+		gpcon = old_gpcon & ~change_mask;
+		gpcon |= gps_gpcon & change_mask;
+
+		__raw_writel(gpcon, base + OFFS_CON);
+
+		/* Now change any items that require DAT,CON */
+
+		__raw_writel(gps_gpdat, base + OFFS_DAT);
+		__raw_writel(gps_gpcon, base + OFFS_CON);
+		__raw_writel(gps->gpup, base + OFFS_UP);
+	}
+
+	DBG("GPIO[%d] CON %08lx => %08lx, DAT %08lx => %08lx\n",
+	    index, old_gpcon, gps_gpcon, old_gpdat, gps_gpdat);
+}
+
+
+/** s3c2410_pm_restore_gpios()
+ *
+ * Restore the state of the GPIOs
+ */
+
+static void s3c2410_pm_restore_gpios(void)
+{
+	struct gpio_sleep *gps = gpio_save;
+	int gpio;
+
+	for (gpio = 0; gpio < ARRAY_SIZE(gpio_save); gpio++, gps++) {
+		s3c2410_pm_restore_gpio(gpio, gps);
+	}
+}
+
 void (*pm_cpu_prep)(void);
 void (*pm_cpu_sleep)(void);
 
@@ -535,7 +714,8 @@ static int s3c2410_pm_enter(suspend_state_t state)
 
 	/* save all necessary core registers not covered by the drivers */
 
-	s3c2410_pm_do_save(gpio_save, ARRAY_SIZE(gpio_save));
+	s3c2410_pm_save_gpios();
+	s3c2410_pm_do_save(misc_save, ARRAY_SIZE(misc_save));
 	s3c2410_pm_do_save(core_save, ARRAY_SIZE(core_save));
 	s3c2410_pm_do_save(uart_save, ARRAY_SIZE(uart_save));
 
@@ -585,8 +765,9 @@ static int s3c2410_pm_enter(suspend_state_t state)
 	/* restore the system state */
 
 	s3c2410_pm_do_restore_core(core_save, ARRAY_SIZE(core_save));
-	s3c2410_pm_do_restore(gpio_save, ARRAY_SIZE(gpio_save));
+	s3c2410_pm_do_restore(misc_save, ARRAY_SIZE(misc_save));
 	s3c2410_pm_do_restore(uart_save, ARRAY_SIZE(uart_save));
+	s3c2410_pm_restore_gpios();
 
 	s3c2410_pm_debug_init();