1 files changed, 42 insertions, 0 deletions

diff --git a/arch/arm/mach-tegra/sleep.S b/arch/arm/mach-tegra/sleep.S
index 91548a77dd95..88f4de986a52 100644
--- a/arch/arm/mach-tegra/sleep.S
+++ b/arch/arm/mach-tegra/sleep.S
@@ -25,6 +25,7 @@
 #include <linux/linkage.h>
 
 #include <asm/assembler.h>
+#include <asm/cache.h>
 #include <asm/cp15.h>
 
 #include "iomap.h"
@@ -59,4 +60,45 @@ ENTRY(tegra_disable_clean_inv_dcache)
         ldmfd   sp!, {r0, r4-r5, r7, r9-r11, pc}
 ENDPROC(tegra_disable_clean_inv_dcache)
 
+/*
+ * tegra_sleep_cpu_finish(unsigned long v2p)
+ *
+ * enters suspend in LP2 by turning off the mmu and jumping to
+ * tegra?_tear_down_cpu
+ */
+ENTRY(tegra_sleep_cpu_finish)
+        /* Flush and disable the L1 data cache */
+        bl      tegra_disable_clean_inv_dcache
+
+        mov32   r6, tegra_tear_down_cpu
+        ldr     r1, [r6]
+        add     r1, r1, r0
+
+        mov32   r3, tegra_shut_off_mmu
+        add     r3, r3, r0
+        mov     r0, r1
+
+        mov     pc, r3
+ENDPROC(tegra_sleep_cpu_finish)
+
+/*
+ * tegra_shut_off_mmu
+ *
+ * r0 = physical address to jump to with mmu off
+ *
+ * called with VA=PA mapping
+ * turns off MMU, icache, dcache and branch prediction
+ */
+        .align  L1_CACHE_SHIFT
+        .pushsection    .idmap.text, "ax"
+ENTRY(tegra_shut_off_mmu)
+        mrc     p15, 0, r3, c1, c0, 0
+        movw    r2, #CR_I | CR_Z | CR_C | CR_M
+        bic     r3, r3, r2
+        dsb
+        mcr     p15, 0, r3, c1, c0, 0
+        isb
+        mov     pc, r0
+ENDPROC(tegra_shut_off_mmu)
+        .popsection
 #endif