4 files changed, 50 insertions, 10 deletions

diff --git a/arch/arm/mach-pxa/include/mach/pm.h b/arch/arm/mach-pxa/include/mach/pm.h
index 261e5bc958d..83342469aca 100644
--- a/arch/arm/mach-pxa/include/mach/pm.h
+++ b/arch/arm/mach-pxa/include/mach/pm.h
@@ -15,6 +15,8 @@ struct pxa_cpu_pm_fns {
 	void	(*restore)(unsigned long *);
 	int	(*valid)(suspend_state_t state);
 	void	(*enter)(suspend_state_t state);
+	int	(*prepare)(void);
+	void	(*finish)(void);
 };
 
 extern struct pxa_cpu_pm_fns *pxa_cpu_pm_fns;
diff --git a/arch/arm/mach-pxa/pm.c b/arch/arm/mach-pxa/pm.c
index 1b539e67557..164eb0bb632 100644
--- a/arch/arm/mach-pxa/pm.c
+++ b/arch/arm/mach-pxa/pm.c
@@ -86,9 +86,27 @@ static int pxa_pm_valid(suspend_state_t state)
 	return -EINVAL;
 }
 
+static int pxa_pm_prepare(void)
+{
+	int ret = 0;
+
+	if (pxa_cpu_pm_fns && pxa_cpu_pm_fns->prepare)
+		ret = pxa_cpu_pm_fns->prepare();
+
+	return ret;
+}
+
+static void pxa_pm_finish(void)
+{
+	if (pxa_cpu_pm_fns && pxa_cpu_pm_fns->finish)
+		pxa_cpu_pm_fns->finish();
+}
+
 static struct platform_suspend_ops pxa_pm_ops = {
 	.valid		= pxa_pm_valid,
 	.enter		= pxa_pm_enter,
+	.prepare	= pxa_pm_prepare,
+	.finish		= pxa_pm_finish,
 };
 
 static int __init pxa_pm_init(void)
diff --git a/arch/arm/mach-pxa/pxa25x.c b/arch/arm/mach-pxa/pxa25x.c
index 305452b56e9..db7be22ccd1 100644
--- a/arch/arm/mach-pxa/pxa25x.c
+++ b/arch/arm/mach-pxa/pxa25x.c
@@ -234,9 +234,6 @@ static void pxa25x_cpu_pm_save(unsigned long *sleep_save)
 
 static void pxa25x_cpu_pm_restore(unsigned long *sleep_save)
 {
-	/* ensure not to come back here if it wasn't intended */
-	PSPR = 0;
-
 	/* restore registers */
 	RESTORE(GAFR0_L); RESTORE(GAFR0_U);
 	RESTORE(GAFR1_L); RESTORE(GAFR1_U);
@@ -256,19 +253,32 @@ static void pxa25x_cpu_pm_enter(suspend_state_t state)
 
 	switch (state) {
 	case PM_SUSPEND_MEM:
-		/* set resume return address */
-		PSPR = virt_to_phys(pxa_cpu_resume);
 		pxa25x_cpu_suspend(PWRMODE_SLEEP);
 		break;
 	}
 }
 
+static int pxa25x_cpu_pm_prepare(void)
+{
+	/* set resume return address */
+	PSPR = virt_to_phys(pxa_cpu_resume);
+	return 0;
+}
+
+static void pxa25x_cpu_pm_finish(void)
+{
+	/* ensure not to come back here if it wasn't intended */
+	PSPR = 0;
+}
+
 static struct pxa_cpu_pm_fns pxa25x_cpu_pm_fns = {
 	.save_count	= SLEEP_SAVE_COUNT,
 	.valid		= suspend_valid_only_mem,
 	.save		= pxa25x_cpu_pm_save,
 	.restore	= pxa25x_cpu_pm_restore,
 	.enter		= pxa25x_cpu_pm_enter,
+	.prepare	= pxa25x_cpu_pm_prepare,
+	.finish		= pxa25x_cpu_pm_finish,
 };
 
 static void __init pxa25x_init_pm(void)
diff --git a/arch/arm/mach-pxa/pxa27x.c b/arch/arm/mach-pxa/pxa27x.c
index c33cf6ac8c8..4835207c142 100644
--- a/arch/arm/mach-pxa/pxa27x.c
+++ b/arch/arm/mach-pxa/pxa27x.c
@@ -220,9 +220,6 @@ void pxa27x_cpu_pm_save(unsigned long *sleep_save)
 
 void pxa27x_cpu_pm_restore(unsigned long *sleep_save)
 {
-	/* ensure not to come back here if it wasn't intended */
-	PSPR = 0;
-
 	/* restore registers */
 	RESTORE(GAFR0_L); RESTORE(GAFR0_U);
 	RESTORE(GAFR1_L); RESTORE(GAFR1_U);
@@ -259,8 +256,6 @@ void pxa27x_cpu_pm_enter(suspend_state_t state)
 		pxa_cpu_standby();
 		break;
 	case PM_SUSPEND_MEM:
-		/* set resume return address */
-		PSPR = virt_to_phys(pxa_cpu_resume);
 		pxa27x_cpu_suspend(PWRMODE_SLEEP);
 		break;
 	}
@@ -271,12 +266,27 @@ static int pxa27x_cpu_pm_valid(suspend_state_t state)
 	return state == PM_SUSPEND_MEM || state == PM_SUSPEND_STANDBY;
 }
 
+static int pxa27x_cpu_pm_prepare(void)
+{
+	/* set resume return address */
+	PSPR = virt_to_phys(pxa_cpu_resume);
+	return 0;
+}
+
+static void pxa27x_cpu_pm_finish(void)
+{
+	/* ensure not to come back here if it wasn't intended */
+	PSPR = 0;
+}
+
 static struct pxa_cpu_pm_fns pxa27x_cpu_pm_fns = {
 	.save_count	= SLEEP_SAVE_COUNT,
 	.save		= pxa27x_cpu_pm_save,
 	.restore	= pxa27x_cpu_pm_restore,
 	.valid		= pxa27x_cpu_pm_valid,
 	.enter		= pxa27x_cpu_pm_enter,
+	.prepare	= pxa27x_cpu_pm_prepare,
+	.finish		= pxa27x_cpu_pm_finish,
 };
 
 static void __init pxa27x_init_pm(void)