3 files changed, 62 insertions, 0 deletions

diff --git a/arch/powerpc/sysdev/6xx-suspend.S b/arch/powerpc/sysdev/6xx-suspend.S
new file mode 100644
index 000000000000..21cda085d926
--- /dev/null
+++ b/arch/powerpc/sysdev/6xx-suspend.S
@@ -0,0 +1,52 @@
+/*
+ * Enter and leave sleep state on chips with 6xx-style HID0
+ * power management bits, which don't leave sleep state via reset.
+ *
+ * Author: Scott Wood <scottwood@freescale.com>
+ *
+ * Copyright (c) 2006-2007 Freescale Semiconductor, Inc.
+ *
+ * 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.
+ */
+
+#include <asm/ppc_asm.h>
+#include <asm/reg.h>
+#include <asm/thread_info.h>
+#include <asm/asm-offsets.h>
+
+_GLOBAL(mpc6xx_enter_standby)
+        mflr    r4
+
+        mfspr   r5, SPRN_HID0
+        rlwinm  r5, r5, 0, ~(HID0_DOZE | HID0_NAP)
+        oris    r5, r5, HID0_SLEEP@h
+        mtspr   SPRN_HID0, r5
+        isync
+
+        lis     r5, ret_from_standby@h
+        ori     r5, r5, ret_from_standby@l
+        mtlr    r5
+
+        rlwinm  r5, r1, 0, 0, 31-THREAD_SHIFT
+        lwz     r6, TI_LOCAL_FLAGS(r5)
+        ori     r6, r6, _TLF_SLEEPING
+        stw     r6, TI_LOCAL_FLAGS(r5)
+
+        mfmsr   r5
+        ori     r5, r5, MSR_EE
+        oris    r5, r5, MSR_POW@h
+        sync
+        mtmsr   r5
+        isync
+
+1:      b       1b
+
+ret_from_standby:
+        mfspr   r5, SPRN_HID0
+        rlwinm  r5, r5, 0, ~HID0_SLEEP
+        mtspr   SPRN_HID0, r5
+
+        mtlr    r4
+        blr
diff --git a/arch/powerpc/sysdev/Makefile b/arch/powerpc/sysdev/Makefile
index 6d386d0071a0..2cc50520a698 100644
--- a/arch/powerpc/sysdev/Makefile
+++ b/arch/powerpc/sysdev/Makefile
@@ -44,3 +44,7 @@ obj-$(CONFIG_PPC_DCR)		+= dcr.o
 obj-$(CONFIG_8xx)               += mpc8xx_pic.o cpm1.o
 obj-$(CONFIG_UCODE_PATCH)       += micropatch.o
 endif
+
+ifeq ($(CONFIG_SUSPEND),y)
+obj-$(CONFIG_6xx)               += 6xx-suspend.o
+endif
diff --git a/include/asm-powerpc/mpc6xx.h b/include/asm-powerpc/mpc6xx.h
new file mode 100644
index 000000000000..effc2291beb2
--- /dev/null
+++ b/include/asm-powerpc/mpc6xx.h
@@ -0,0 +1,6 @@
+#ifndef __ASM_POWERPC_MPC6xx_H
+#define __ASM_POWERPC_MPC6xx_H
+
+void mpc6xx_enter_standby(void);
+
+#endif