ARM: tegra: power: Save CPU context to non-cacheable stack
Scott Williams [Thu, 4 Aug 2011 20:32:10 +0000 (13:32 -0700)]
The standard cpu_suspend does not work if there is an exernal
L2 cache in the system individual CPUs are suspending without
shutting down the whole CPU complex. As a workaround for this
problem, we must save the CPU context to a non-cacheable region
of memory.

Change-Id: I2fffbc77ed4f17fe9710307aaacda80836bacee8
Signed-off-by: Scott Williams <scwilliams@nvidia.com>

Rebase-Id: R7328c032c2a13775aa09432e119ea845ded85930

arch/arm/mach-tegra/headsmp.S
arch/arm/mach-tegra/pm.c
arch/arm/mach-tegra/sleep-t20.S
arch/arm/mach-tegra/sleep.S
arch/arm/mach-tegra/sleep.h

index c16a7a5..07e0410 100644 (file)
@@ -113,7 +113,7 @@ ENTRY(tegra_resume)
 
        cpu_id  r0
        cmp     r0, #0                          @ CPU0?
-       bne     cpu_resume                      @ no
+       bne     tegra_cpu_resume_phys           @ no
 
 #ifndef CONFIG_ARCH_TEGRA_2x_SOC
        @ Clear the flow controller flags for this CPU.
@@ -131,7 +131,7 @@ ENTRY(tegra_resume)
        orr     r1, r1, #1
        str     r1, [r0]
 
-       b       cpu_resume
+       b       tegra_cpu_resume_phys
 ENDPROC(tegra_resume)
 #endif
 
index eba64bb..1c9f652 100644 (file)
@@ -91,7 +91,7 @@ struct suspend_context {
 };
 
 #ifdef CONFIG_PM_SLEEP
-#ifdef CONFIG_CACHE_L2X0
+#if USE_TEGRA_CPU_SUSPEND
 void *tegra_cpu_context;       /* non-cacheable page for CPU context */
 #endif
 phys_addr_t tegra_pgd_phys;    /* pgd used by hotplug & LP2 bootup */
@@ -259,7 +259,7 @@ static __init int create_suspend_pgtable(void)
  */
 static __init int alloc_suspend_context(void)
 {
-#ifdef CONFIG_CACHE_L2X0
+#if USE_TEGRA_CPU_SUSPEND
        pgprot_t prot = __pgprot_modify(pgprot_kernel, L_PTE_MT_MASK,
                                         L_PTE_MT_UNCACHED | L_PTE_XN);
        struct page *ctx_page;
index aa4be11..ba6a79d 100644 (file)
@@ -187,9 +187,10 @@ ENTRY(tegra2_sleep_wfi)
 
        /*
         * cpu may be reset while in wfi, which will return through
-        * tegra_resume to cpu_resume to tegra_cpu_resume
+        * tegra_resume to tegra_cpu_resume_phys to tegra_cpu_resume
         * or interrupt may wake wfi, which will return here
-        * cpu state is unchanged - MMU is on, cache is on, coherency is off
+        * cpu state is unchanged - MMU is on, cache is on, coherency
+        * is off, and the data cache is off
         *
         * r11 contains the original actlr
         */
@@ -204,6 +205,12 @@ ENTRY(tegra2_sleep_wfi)
 
        bl      tegra_pen_unlock
 
+       /* Enable the data cache and SMP coherency */
+       mrc     p15, 0, r10, c1, c0, 0
+       orr     r10, r10, #CR_C
+       dsb
+       mcr     p15, 0, r10, c1, c0, 0
+       isb
        mcr     p15, 0, r11, c1, c0, 1  @ reenable coherency
 
        @ the cpu was running with coherency disabled, caches may be out of date
index 60d5dc7..9b81c9a 100644 (file)
@@ -115,9 +115,60 @@ ENTRY(tegra_cpu_exit_coherency)
 ENDPROC(tegra_cpu_exit_coherency)
 
 /*
+ * Restore CPU state for a suspend
+ *
+ * NOTE: This is a copy of cpu_resume in arch/arm/sleep.S that has been
+ *      modified to work with an L2 cache.
+ */
+       .align  L1_CACHE_SHIFT
+ENTRY(tegra_cpu_resume_phys)
+#if USE_TEGRA_CPU_SUSPEND
+#ifdef CONFIG_SMP
+       adr     r0, tegra_phys_sleep_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, tegra_phys_sleep_sp         @ stack phys addr
+#endif
+       setmode PSR_I_BIT | PSR_F_BIT | SVC_MODE, r1  @ set SVC, irqs off
+#ifdef MULTI_CPU
+       @ load v:p, stack, return fn, resume fn
+  ARM( ldmia   r0!, {r1, sp, lr, pc}   )
+THUMB( ldmia   r0!, {r1, r2, r3, r4}   )
+THUMB( mov     sp, r2                  )
+THUMB( mov     lr, r3                  )
+THUMB( bx      r4                      )
+#else
+       @ load v:p, stack, return fn
+  ARM( ldmia   r0!, {r1, sp, lr}       )
+THUMB( ldmia   r0!, {r1, r2, lr}       )
+THUMB( mov     sp, r2                  )
+       b       cpu_do_resume
+#endif
+#else
+       /* Use the standard cpu_resume. */
+       b       cpu_resume
+#endif
+ENDPROC(tegra_cpu_resume_phys)
+
+#if USE_TEGRA_CPU_SUSPEND
+       .align  L1_CACHE_SHIFT
+       .globl  tegra_phys_sleep_sp
+tegra_phys_sleep_sp:
+       .rept   CONFIG_NR_CPUS
+       .long   0               @ preserve stack phys ptr here
+       .endr
+       .align  L1_CACHE_SHIFT  @ nothing else must be in this cache line
+#endif
+
+/*
  * tegra_cpu_suspend
  *
  * Save CPU suspend state
+ * NOTE: This is a copy of cpu_suspend in arch/arm/sleep.S that has been
+ *      modified to work with an L2 cache.
  *
  * Input:
  *     r1 = v:p offset
@@ -126,12 +177,102 @@ ENDPROC(tegra_cpu_exit_coherency)
  *     sp is decremented to allocate space for CPU state on stack
  * r0-r3,r8-r10,lr corrupted
  */
-
+       .align  L1_CACHE_SHIFT
 ENTRY(tegra_cpu_suspend)
+#if USE_TEGRA_CPU_SUSPEND
+       mov     r9, lr
+#ifdef MULTI_CPU
+       mov32   r10, processor
+       mov     r2, sp                  @ current virtual SP
+       ldr     r0, [r10, #CPU_SLEEP_SIZE] @ size of CPU sleep state
+       ldr     ip, [r10, #CPU_DO_RESUME] @ virtual resume function
+       sub     sp, sp, r0              @ allocate CPU state on stack
+       mov     r0, sp                  @ save pointer
+       add     ip, ip, r1              @ convert resume fn to phys
+       stmfd   sp!, {r1, r2, r3, ip}   @ save v:p, virt SP, retfn, phys resume fn
+       mov     lr, pc
+       ldr     pc, [r10, #CPU_DO_SUSPEND] @ save CPU state
+#else
+       mov     r2, sp                  @ current virtual SP
+       mov32   r0, cpu_suspend_size
+       sub     sp, sp, r0              @ allocate CPU state on stack
+       mov     r0, sp                  @ save pointer
+       stmfd   sp!, {r1, r2, r3}       @ save v:p, virt SP, return fn
+       bl      cpu_do_suspend
+#endif
+
+       /* Disable the data cache */
+       mrc     p15, 0, r10, c1, c0, 0
+       bic     r10, r10, #CR_C
+       dsb
+       mcr     p15, 0, r10, c1, c0, 0
+       isb
+
+       /* Flush data cache */
+#ifdef MULTI_CACHE
+       mov32   r10, cpu_cache
+       mov     lr, pc
+       ldr     pc, [r10, #CACHE_FLUSH_KERN_ALL]
+#else
+       bl      __cpuc_flush_kern_all
+#endif
+
+       /* Invalidate the TLBs & BTAC */
+       mov     r1, #0
+       mcr     p15, 0, r1, c8, c3, 0   @ invalidate shared TLBs
+       mcr     p15, 0, r1, c7, c1, 6   @ invalidate shared BTAC
+       dsb
+       isb
+
+       /* Turn off SMP coherency */
+       exit_smp r1, r2
+
+       /* Convert SP from virtual to physical address. */
+       movw    r1, #0xFFF
+       bic     r2, sp, r1              @ VA & 0xFFFFF000
+       mcr     p15, 0, r2, c7, c8, 0   @ V2PPRPC
+       mrc     p15, 0, r2, c7, c4, 0   @ PAR
+       bic     r2, r2, r1              @ PA & 0xFFFFF000
+       and     r0, sp, r1              @ VA & 0x00000FFF
+       orr     r2, r0, r2              @ (PA & 0xFFFFF000) | (VA & 0x00000FFF)
+
+       mov32   r3, tegra_phys_sleep_sp @ per-CPU phys SP save area
+
+#ifdef CONFIG_SMP
+       ALT_SMP(mrc p15, 0, lr, c0, c0, 5)
+       ALT_UP(mov lr, #0)
+       and     lr, lr, #15
+#else
+       mov     lr, #0
+#endif
+
+       /* Save the normal PRRR value */
+       mrc     p15, 0, r0, c10, c2, 0  @ PRRR
+
+       /* Override all remappings to strongly ordered */
+       mov     r1, #0
+       mcr     p15, 0, r1, c10, c2, 0  @ PRRR
+       mcr     p15, 0, r1, c8, c7, 0   @ invalidate local TLBs
+       dsb
+       isb
+
+       /* Save the physical stack pointer */
+       str     r2, [r3, lr, lsl #2]    @ save phys SP
+
+       /* Restore the regular remappings */
+       mcr     p15, 0, r0, c10, c2, 0  @ PRRR
+       mcr     p15, 0, r1, c8, c7, 0   @ invalidate local TLBs
+       dsb
+       isb
+
+       mov     pc, r9
+#else
+       /* Use the standard cpu_suspend. */
        mov     r8, lr
        bl      cpu_suspend
        exit_smp r0, r2
        mov     pc, r8
+#endif
 ENDPROC(tegra_cpu_suspend)
 
 /*
@@ -145,7 +286,7 @@ ENDPROC(tegra_cpu_suspend)
  *     r0 = v:p offset
  *     r7 = SP after saving the registers but before cpu_suspend, suitable
  *          for restoring an aborted suspend
- *     sp = SP after cpu_suspend (the 'real' SP)
+ *     sp = SP after tegra_cpu_suspend (the 'real' SP)
  * Saves r4-r11 on the stack
  * Corrupts r1, r3-r10
  */
@@ -155,7 +296,20 @@ ENTRY(tegra_cpu_save)
 
        adr     r3, tegra_cpu_resume
 
-       mov     r7, sp
+       mov     r7, sp                  @ SP after reg save, before suspend
+
+#if USE_TEGRA_CPU_SUSPEND
+       cpu_id  r4
+       mov32   r5, tegra_cpu_context   @ address of non-cacheable context page
+       ldr     r5, [r5]                @ non-cacheable context save area
+       mov     r6, #0x400              @ size of one CPU context stack area
+       add     r4, r4, #1
+       smlabb  sp, r6, r4, r5          @ context area for this CPU
+       push_stack_token r4             @ debug check word
+       stmfd   sp!, {r7}               @ save the real stack pointer
+       push_stack_token r4             @ debug check word
+#endif
+
        mov     r4, r12
        mov     r5, r0
        mov     r6, r2
@@ -189,6 +343,7 @@ ENDPROC(tegra_sleep_cpu)
  * tegra_cpu_resume
  *
  * reloads the volatile CPU state from the context area
+ * initializes the processor mode stacks
  * the mmu should be on and the CPU should be coherent before this is called
  */
        .align L1_CACHE_SHIFT
@@ -200,6 +355,13 @@ tegra_cpu_resume:
        dsb
        isb
 
+#if USE_TEGRA_CPU_SUSPEND
+       pop_stack_token r4, r5          @ check stack debug token
+       ldmfd   sp!, {r0}               @ get the real stack pointer
+       pop_stack_token r4, r5          @ check stack debug token
+       mov     sp, r0                  @ switch to the real stack pointer
+#endif
+
        bl      cpu_init
 
        pop_ctx_regs r1, r2             @ restore context registers
index cba062e..93ab2bb 100644 (file)
 
 #include "iomap.h"
 
+#ifdef CONFIG_CACHE_L2X0
+#define USE_TEGRA_CPU_SUSPEND  1
+#else
+#define USE_TEGRA_CPU_SUSPEND  0
+#endif
+
 #define TEGRA_POWER_SDRAM_SELFREFRESH  (1 << 26) /* SDRAM is in self-refresh */
 #define TEGRA_POWER_HOTPLUG_SHUTDOWN   (1 << 27) /* Hotplug shutdown */
 #define TEGRA_POWER_CLUSTER_G          (1 << 28) /* G CPU */