import PULS_20160108

[GitHub/mt8127/android_kernel_alcatel_ttab.git] / arch / arm / kernel / sleep.S

diff --git a/arch/arm/kernel/sleep.S b/arch/arm/kernel/sleep.S
index 987dcf33415c420f70dc08852da45bd654a21ea2..ea3ec1d84c33d7954aaac96f4481fee6e3dd193a 100644 (file)
--- a/arch/arm/kernel/sleep.S
+++ b/arch/arm/kernel/sleep.S
@@ -4,6 +4,7 @@
 #include <asm/assembler.h>
 #include <asm/glue-cache.h>
 #include <asm/glue-proc.h>
+#include "entry-header.S"
        .text
 
 /*
@@ -30,9 +31,8 @@ ENTRY(__cpu_suspend)
        mov     r2, r5                  @ virtual SP
        ldr     r3, =sleep_save_sp
 #ifdef CONFIG_SMP
-       ALT_SMP(mrc p15, 0, lr, c0, c0, 5)
-       ALT_UP(mov lr, #0)
-       and     lr, lr, #15
+       get_thread_info r5
+       ldr     lr, [r5, #TI_CPU]       @ cpu logical index
        add     r3, r3, lr, lsl #2
 #endif
        bl      __cpu_suspend_save
@@ -81,11 +81,15 @@ ENDPROC(cpu_resume_after_mmu)
        .data
        .align
 ENTRY(cpu_resume)
+        bl      v7_invalidate_l1
 #ifdef CONFIG_SMP
+       mov     r1, #0                  @ fall-back logical index for UP
+       ALT_SMP(mrc p15, 0, r0, c0, c0, 5)
+       ALT_UP_B(1f)
+       bic     r0, #0xff000000
+       bl      cpu_logical_index       @ return logical index in r1
+1:
        adr     r0, sleep_save_sp
-       ALT_SMP(mrc p15, 0, r1, c0, c0, 5)
-       ALT_UP(mov r1, #0)
-       and     r1, r1, #15
        ldr     r0, [r0, r1, lsl #2]    @ stack phys addr
 #else
        ldr     r0, sleep_save_sp       @ stack phys addr
@@ -102,3 +106,20 @@ sleep_save_sp:
        .rept   CONFIG_NR_CPUS
        .long   0                               @ preserve stack phys ptr here
        .endr
+
+#ifdef CONFIG_SMP
+cpu_logical_index:
+       adr     r3, cpu_map_ptr
+       ldr     r2, [r3]
+       add     r3, r3, r2              @ virt_to_phys(__cpu_logical_map)
+       mov     r1, #0
+1:
+       ldr     r2, [r3, r1, lsl #2]
+       cmp     r2, r0
+       moveq   pc, lr
+       add     r1, r1, #1
+       b       1b
+
+cpu_map_ptr:
+       .long __cpu_logical_map - .
+#endif