diff --git a/core/arch/arm/include/arm32_macros.S b/core/arch/arm/include/arm32_macros.S index 8385762d5d6..1cd04b013ff 100644 --- a/core/arch/arm/include/arm32_macros.S +++ b/core/arch/arm/include/arm32_macros.S @@ -27,6 +27,10 @@ /* Please keep them sorted based on the CRn register */ + .macro read_midr reg + mrc p15, 0, \reg, c0, c0, 0 + .endm + .macro read_ctr reg mrc p15, 0, \reg, c0, c0, 1 .endm diff --git a/core/arch/arm/include/sm/pm.h b/core/arch/arm/include/sm/pm.h new file mode 100644 index 00000000000..05bab8d2b12 --- /dev/null +++ b/core/arch/arm/include/sm/pm.h @@ -0,0 +1,49 @@ +/* + * Copyright 2017 NXP + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef SM_PM_H +#define SM_PM_H +#include +#include + +struct sm_pm_ctx { + uint32_t sp; + paddr_t cpu_resume_addr; + uint32_t suspend_regs[16]; +}; + +/* suspend/resume core functions */ +void cpu_suspend_init(void); +void cpu_suspend_save(struct sm_pm_ctx *ptr, uint32_t save_sz, uint32_t sp); +void cpu_do_suspend(uint32_t *ptr); +void cpu_do_resume(void); + +/* + * Exported to platform suspend, arg will be passed to fn as r0 + */ +int cpu_suspend(uint32_t arg, int (*fn)(uint32_t)); +#endif diff --git a/core/arch/arm/kernel/asm-defines.c b/core/arch/arm/kernel/asm-defines.c index 1e3ed4037e4..fba92e17bad 100644 --- a/core/arch/arm/kernel/asm-defines.c +++ b/core/arch/arm/kernel/asm-defines.c @@ -26,6 +26,7 @@ */ #include +#include #include #include #include "thread_private.h" @@ -55,6 +56,14 @@ DEFINES /* struct thread_core_local */ DEFINE(THREAD_CORE_LOCAL_R0, offsetof(struct thread_core_local, r[0])); + DEFINE(THREAD_CORE_LOCAL_PM_CTX_PHYS, + offsetof(struct thread_core_local, sm_pm_ctx_phys)); + DEFINE(THREAD_CORE_LOCAL_SIZE, sizeof(struct thread_core_local)); + + DEFINE(PM_CTX_SP, offsetof(struct sm_pm_ctx, sp)); + DEFINE(PM_CTX_RESUME_ADDR, offsetof(struct sm_pm_ctx, cpu_resume_addr)); + DEFINE(PM_CTX_SUSPEND_REGS, offsetof(struct sm_pm_ctx, suspend_regs)); + DEFINE(PM_CTX_SIZE, sizeof(struct sm_pm_ctx)); #endif /*ARM32*/ #ifdef ARM64 diff --git a/core/arch/arm/kernel/thread.c b/core/arch/arm/kernel/thread.c index aad91a7caa6..c4346baeb70 100644 --- a/core/arch/arm/kernel/thread.c +++ b/core/arch/arm/kernel/thread.c @@ -89,7 +89,7 @@ struct thread_ctx threads[CFG_NUM_THREADS]; -static struct thread_core_local thread_core_local[CFG_TEE_CORE_NB_CORE]; +struct thread_core_local thread_core_local[CFG_TEE_CORE_NB_CORE]; #ifdef CFG_WITH_STACK_CANARIES #ifdef ARM32 diff --git a/core/arch/arm/kernel/thread_private.h b/core/arch/arm/kernel/thread_private.h index 0f78cca13df..6272beaac65 100644 --- a/core/arch/arm/kernel/thread_private.h +++ b/core/arch/arm/kernel/thread_private.h @@ -134,6 +134,7 @@ struct thread_core_local { uint32_t flags; vaddr_t abt_stack_va_end; #ifdef ARM32 + paddr_t sm_pm_ctx_phys; uint32_t r[2]; #endif #ifdef ARM64 diff --git a/core/arch/arm/sm/pm.c b/core/arch/arm/sm/pm.c new file mode 100644 index 00000000000..ea9af1a1766 --- /dev/null +++ b/core/arch/arm/sm/pm.c @@ -0,0 +1,86 @@ +/* + * Copyright 2017 NXP + * + * Peng Fan + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "../kernel/thread_private.h" + +#if CFG_TEE_CORE_NB_CORE > 4 +#error "Max support 4 cores in one cluster now" +#endif + +void cpu_suspend_init(void) +{ + /* Place holer for now */ +} + +void cpu_suspend_save(struct sm_pm_ctx *ctx, uint32_t save_sz __unused, + uint32_t sp) +{ + union { + void (*resume_fun)(void); + void *resume_addr; + } u; + + struct thread_core_local *p = thread_get_core_local(); + + p->sm_pm_ctx_phys = virt_to_phys((void *)ctx); + + u.resume_fun = cpu_do_resume; + /* + * The content will be passed to cpu_do_resume + * as register r1 and sp + */ + ctx->sp = sp; + ctx->cpu_resume_addr = virt_to_phys(u.resume_addr); + + cpu_do_suspend(&ctx->suspend_regs[0]); + + dcache_op_level1(DCACHE_OP_CLEAN_INV); + +#ifdef CFG_PL310 + arm_cl2_cleanbyway(core_mmu_get_va(PL310_BASE, MEM_AREA_IO_SEC)); +#endif +} diff --git a/core/arch/arm/sm/sub.mk b/core/arch/arm/sm/sub.mk index fef4b43ba0c..2a45fd03336 100644 --- a/core/arch/arm/sm/sub.mk +++ b/core/arch/arm/sm/sub.mk @@ -1,3 +1,3 @@ srcs-y += sm_a32.S srcs-y += sm.c -srcs-$(CFG_PSCI_ARM32) += std_smc.c psci.c psci-helper.S +srcs-$(CFG_PSCI_ARM32) += std_smc.c psci.c pm.c psci-helper.S suspend.S diff --git a/core/arch/arm/sm/suspend.S b/core/arch/arm/sm/suspend.S new file mode 100644 index 00000000000..bf45ba52744 --- /dev/null +++ b/core/arch/arm/sm/suspend.S @@ -0,0 +1,231 @@ +/* + * Copyright 2017 NXP + * + * Peng Fan + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +.section .text +FUNC cpu_suspend, : +UNWIND( .fnstart) + push {r4 - r12, lr} +UNWIND(.save {r4 - r12, lr}) + mov r5, sp + sub sp, sp, #PM_CTX_SIZE + push {r0, r1} + + mov r2, r5 + mov r1, #PM_CTX_SIZE + add r0, sp, #8 + blx cpu_suspend_save + adr lr, aborted + /* Jump to arch specific suspend*/ + pop {r0, pc} +aborted: + /* cpu not suspended */ + add sp, sp, #PM_CTX_SIZE + /* Return -1 to the caller */ + mov r0, #(-1) + pop {r4 - r12, pc} +UNWIND( .fnend) +END_FUNC cpu_suspend + +FUNC cpu_v7_suspend, : +UNWIND(.fnstart) + push {r4 - r12, lr} + read_fcseidr r4 + read_tpidruro r5 + stmia r0!, {r4 - r5} + read_dacr r4 +#ifdef CFG_WITH_LPAE +#error "Not supported" +#else + read_ttbr0 r5 + read_ttbr1 r6 + read_ttbcr r7 +#endif + read_sctlr r8 + read_actlr r9 + read_cpacr r10 + read_mvbar r11 + stmia r0!, {r4 - r11} + read_prrr r4 + read_nmrr r5 + read_vbar r6 + read_nsacr r7 + stmia r0, {r4 - r7} + pop {r4 - r12, pc} +UNWIND( .fnend) +END_FUNC cpu_v7_suspend + +FUNC cpu_do_suspend, : +UNWIND(.fnstart) + push {r4, r5} + read_midr r4 + ubfx r5, r4, #4, #12 + ldr r4, =0xc07 + cmp r5, r4 + beq a7_suspend + /* + * TODO: add more Cortex A[x] check? + * + * A9 needs PCR/DIAG + */ + read_pcr r4 + read_diag r5 + stmia r0!, {r4 - r5} +a7_suspend: + pop {r4, r5} + b cpu_v7_suspend +UNWIND( .fnend) +END_FUNC cpu_do_suspend + +FUNC cpu_resume, : +UNWIND(.fnstart) + /* Disable IRQ/FIQ */ + mrs r1, cpsr + orr r1, r1, #(CPSR_I | CPSR_F) + msr cpsr, r1 + /* Assmue max 4 cpus and 1 cluster */ + read_mpidr r1 + and r1, r1, #0x3 + + adr r0, _suspend_sp + ldr r2, [r0] + /* Get the physical address of thread_core_local */ + add r0, r0, r2 + + cmp r1, #0 + beq 2f +1: + add r0, r0, #THREAD_CORE_LOCAL_SIZE + subs r1, r1, #1 + bne 1b +2: + ldr r0, [r0, #THREAD_CORE_LOCAL_PM_CTX_PHYS] + /* Need to use r0!, because cpu_do_resume needs it */ + ldmia r0!, {sp, pc} +UNWIND( .fnend) +END_FUNC cpu_resume + +FUNC cpu_resume_after_mmu, : +UNWIND(.fnstart) + mov r0, #0 + pop {r4 - r12, pc} +UNWIND( .fnend) +END_FUNC cpu_resume_after_mmu + +FUNC cpu_resume_mmu, : +UNWIND(.fnstart) + ldr r3, =cpu_resume_after_mmu + write_tlbiall + isb + write_sctlr r0 + dsb + isb + nop + nop + nop + bx r3 +UNWIND( .fnend) +END_FUNC cpu_resume_mmu + +/* + * Restore the registers stored when cpu_do_suspend + */ +FUNC cpu_do_resume, : +UNWIND(.fnstart) + read_midr r4 + ubfx r5, r4, #4, #12 + ldr r4, =0xc07 + cmp r5, r4 + beq a7_resume + + /* + * A9 needs PCR/DIAG + */ + ldmia r0!, {r4 - r5} + write_pcr r4 + write_diag r5 + +a7_resume: + /* v7 resume */ + mov ip, #0 + /* Invalidate icache to PoU */ + write_iciallu + /* set reserved context */ + write_contextidr ip + ldmia r0!, {r4 - r5} + write_fcseidr r4 + write_tpidruro r5 + ldmia r0!, {r4 - r11} + /* Invalidate entire TLB */ + write_tlbiall + write_dacr r4 +#ifdef CFG_WITH_LPAE +#error "Not supported -" +#else + write_ttbr0 r5 + write_ttbr1 r6 + write_ttbcr r7 +#endif + + ldmia r0, {r4 - r7} + write_prrr r4 + write_nmrr r5 + write_vbar r6 + write_nsacr r7 + + read_actlr r4 + teq r4, r9 + mcrne p15, 0, r9, c1, c0, 1 + write_cpacr r10 + write_mvbar r11 + isb + dsb + mov r0, r8 + /* Pass sctlr to cpu_resume_mmu */ + b cpu_resume_mmu +UNWIND( .fnend) +END_FUNC cpu_do_resume + +/* + * The following will be located in text section whose attribute is + * marked as readonly, but we only need to read here + */ +.align 2 +.extern thread_core_local +_suspend_sp: + .long thread_core_local - .