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( .cantunwind) 27 push {r4 - r12, lr} 28 mov r5, sp 29 sub sp, sp, #SM_PM_CTX_SIZE 30 push {r0, r1} 31 32 mov r1, r5 33 add r0, sp, #8 34 blx sm_pm_cpu_suspend_save 35 adr lr, aborted 36 /* Jump to arch specific suspend */ 37 pop {r0, pc} 38aborted: 39 /* cpu not suspended */ 40 add sp, sp, #SM_PM_CTX_SIZE 41 /* Return -1 to the caller */ 42 mov r0, #(-1) 43suspend_return: 44 pop {r4 - r12, pc} 45END_FUNC sm_pm_cpu_suspend 46 47FUNC sm_pm_cpu_do_suspend, : 48UNWIND( .cantunwind) 49 push {r4 - r11} 50 read_midr r4 51 ubfx r5, r4, #4, #12 52 ldr r4, =CORTEX_A5_PART_NUM 53 cmp r5, r4 54 beq a5_a7_suspend 55 ldr r4, =CORTEX_A7_PART_NUM 56 cmp r5, r4 57 beq a5_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} 68a5_a7_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 92END_FUNC sm_pm_cpu_do_suspend 93 94FUNC sm_pm_cpu_resume, : 95UNWIND( .cantunwind) 96 cpsid aif 97 98 /* Call into the runtime address of __get_core_pos */ 99 adr r0, _core_pos 100 ldr r1, [r0] 101 add r0, r0, r1 102 blx r0 103 104 /* 105 * At this point, MMU is not enabled now. 106 * 1. Get the runtime physical address of _suspend_sp 107 * 2. Get the offset from _suspend_sp to thread_core_local 108 * 3. Get the runtime physical address of thread_core_local 109 * Since moving towards non-linear mapping, 110 * `ldr r0, =thread_core_local 111 * ldr r0 [r0]` is not used here. 112 */ 113 adr r4, _suspend_sp 114 ldr r5, [r4] 115 add r4, r4, r5 116 ldr r4, [r4] 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} 124END_FUNC sm_pm_cpu_resume 125 126/* 127 * The following will be located in text section whose attribute is 128 * marked as readonly, but we only need to read here 129 * _suspend_sp stores the offset between thread_core_local to _suspend_sp. 130 * _core_pos stores the offset between __get_core_pos to _core_pos. 131 */ 132.align 2 133.extern thread_core_local_pa 134_suspend_sp: 135 .long thread_core_local_pa - . 136.extern __get_core_pos 137_core_pos: 138 .long __get_core_pos - . 139 140/* 141 * void sm_pm_cpu_do_resume(paddr suspend_regs) __noreturn; 142 * Restore the registers stored when sm_pm_cpu_do_suspend 143 * r0 points to the physical base address of the suspend_regs 144 * field of struct sm_pm_ctx. 145 */ 146FUNC sm_pm_cpu_do_resume, : 147UNWIND( .cantunwind) 148 read_midr r4 149 ubfx r5, r4, #4, #12 150 ldr r4, =CORTEX_A5_PART_NUM 151 cmp r5, r4 152 beq a5_a7_resume 153 ldr r4, =CORTEX_A7_PART_NUM 154 cmp r5, r4 155 beq a5_a7_resume 156 157 /* 158 * A9 needs PCR/DIAG 159 */ 160 ldmia r0!, {r4 - r5} 161 write_pcr r4 162 write_diag r5 163 164a5_a7_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 203END_FUNC sm_pm_cpu_do_resume 204 205