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