1/* SPDX-License-Identifier: BSD-2-Clause */ 2/* 3 * Copyright 2017 NXP 4 * 5 * Peng Fan <peng.fan@nxp.com> 6 */ 7 8#include <arm32_macros_cortex_a9.S> 9#include <arm32_macros.S> 10#include <arm.h> 11#include <asm.S> 12#include <generated/asm-defines.h> 13#include <keep.h> 14#include <kernel/asan.h> 15#include <platform_config.h> 16 17.section .text 18 19/* 20 * int sm_pm_cpu_suspend(uint32_t arg, int (*fn)(uint32_t)) 21 * @arg will be passed to fn as argument 22 * return value: 0 - cpu resumed from suspended state. 23 * -1 - cpu not suspended. 24 */ 25FUNC sm_pm_cpu_suspend, : 26UNWIND( .fnstart) 27UNWIND( .cantunwind) 28 push {r4 - r12, lr} 29 mov r5, sp 30 sub sp, sp, #SM_PM_CTX_SIZE 31 push {r0, r1} 32 33 mov r1, r5 34 add r0, sp, #8 35 blx sm_pm_cpu_suspend_save 36 adr lr, aborted 37 /* Jump to arch specific suspend */ 38 pop {r0, pc} 39aborted: 40 /* cpu not suspended */ 41 add sp, sp, #SM_PM_CTX_SIZE 42 /* Return -1 to the caller */ 43 mov r0, #(-1) 44suspend_return: 45 pop {r4 - r12, pc} 46UNWIND( .fnend) 47END_FUNC sm_pm_cpu_suspend 48 49FUNC sm_pm_cpu_do_suspend, : 50UNWIND( .fnstart) 51UNWIND( .cantunwind) 52 push {r4 - r11} 53 read_midr r4 54 ubfx r5, r4, #4, #12 55 ldr r4, =CORTEX_A7_PART_NUM 56 cmp r5, r4 57 beq a7_suspend 58 ldr r4, =CORTEX_A9_PART_NUM 59 cmp r5, r4 60 beq a9_suspend 61 /* cpu not supported */ 62 b . 63 /* A9 needs PCR/DIAG */ 64a9_suspend: 65 read_pcr r4 66 read_diag r5 67 stmia r0!, {r4 - r5} 68a7_suspend: 69 read_fcseidr r4 70 read_tpidruro r5 71 stmia r0!, {r4 - r5} 72 read_dacr r4 73#ifdef CFG_WITH_LPAE 74#error "Not supported" 75#else 76 read_ttbr0 r5 77 read_ttbr1 r6 78 read_ttbcr r7 79#endif 80 read_sctlr r8 81 read_actlr r9 82 read_cpacr r10 83 read_mvbar r11 84 stmia r0!, {r4 - r11} 85 read_prrr r4 86 read_nmrr r5 87 read_vbar r6 88 read_nsacr r7 89 stmia r0, {r4 - r7} 90 pop {r4 - r11} 91 bx lr 92UNWIND( .fnend) 93END_FUNC sm_pm_cpu_do_suspend 94 95FUNC sm_pm_cpu_resume, : 96UNWIND( .fnstart) 97UNWIND( .cantunwind) 98 cpsid aif 99 100 /* Call into the runtime address of __get_core_pos */ 101 adr r0, _core_pos 102 ldr r1, [r0] 103 add r0, r0, r1 104 blx r0 105 106 /* 107 * At this point, MMU is not enabled now. 108 * 1. Get the runtime physical address of _suspend_sp 109 * 2. Get the offset from _suspend_sp to &thread_core_local 110 * 3. Get the runtime physical address of thread_core_local 111 * Since moving towards non-linear mapping, 112 * `ldr r0, =thread_core_local` is not used here. 113 */ 114 adr r4, _suspend_sp 115 ldr r5, [r4] 116 add r4, r4, r5 117 118 mov_imm r1, THREAD_CORE_LOCAL_SIZE 119 mla r0, r0, r1, r4 120 121 ldr r0, [r0, #THREAD_CORE_LOCAL_SM_PM_CTX_PHYS] 122 /* Need to use r0!, because sm_pm_cpu_do_resume needs it */ 123 ldmia r0!, {sp, pc} 124UNWIND( .fnend) 125END_FUNC sm_pm_cpu_resume 126 127/* 128 * The following will be located in text section whose attribute is 129 * marked as readonly, but we only need to read here 130 * _suspend_sp stores the offset between thread_core_local to _suspend_sp. 131 * _core_pos stores the offset between __get_core_pos to _core_pos. 132 */ 133.align 2 134.extern thread_core_local 135_suspend_sp: 136 .long thread_core_local - . 137.extern __get_core_pos 138_core_pos: 139 .long __get_core_pos - . 140 141/* 142 * void sm_pm_cpu_do_resume(paddr suspend_regs) __noreturn; 143 * Restore the registers stored when sm_pm_cpu_do_suspend 144 * r0 points to the physical base address of the suspend_regs 145 * field of struct sm_pm_ctx. 146 */ 147FUNC sm_pm_cpu_do_resume, : 148UNWIND( .fnstart) 149UNWIND( .cantunwind) 150 read_midr r4 151 ubfx r5, r4, #4, #12 152 ldr r4, =CORTEX_A7_PART_NUM 153 cmp r5, r4 154 beq a7_resume 155 156 /* 157 * A9 needs PCR/DIAG 158 */ 159 ldmia r0!, {r4 - r5} 160 write_pcr r4 161 write_diag r5 162 163a7_resume: 164 /* v7 resume */ 165 mov ip, #0 166 /* Invalidate icache to PoU */ 167 write_iciallu 168 /* set reserved context */ 169 write_contextidr ip 170 ldmia r0!, {r4 - r5} 171 write_fcseidr r4 172 write_tpidruro r5 173 ldmia r0!, {r4 - r11} 174 /* Invalidate entire TLB */ 175 write_tlbiall 176 write_dacr r4 177#ifdef CFG_WITH_LPAE 178#error "Not supported -" 179#else 180 write_ttbr0 r5 181 write_ttbr1 r6 182 write_ttbcr r7 183#endif 184 185 ldmia r0, {r4 - r7} 186 write_prrr r4 187 write_nmrr r5 188 write_vbar r6 189 write_nsacr r7 190 191 write_actlr r9 192 write_cpacr r10 193 write_mvbar r11 194 write_bpiall 195 isb 196 dsb 197 /* MMU will be enabled here */ 198 write_sctlr r8 199 isb 200 mov r0, #0 201 b suspend_return 202UNWIND( .fnend) 203END_FUNC sm_pm_cpu_do_resume 204 205