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` is not used here.
111	 */
112	adr	r4, _suspend_sp
113	ldr	r5, [r4]
114	add	r4, r4, r5
115
116	mov_imm r1, THREAD_CORE_LOCAL_SIZE
117	mla	r0, r0, r1, r4
118
119	ldr	r0, [r0, #THREAD_CORE_LOCAL_SM_PM_CTX_PHYS]
120	/* Need to use r0!, because sm_pm_cpu_do_resume needs it */
121	ldmia	r0!, {sp, pc}
122END_FUNC sm_pm_cpu_resume
123
124/*
125 * The following will be located in text section whose attribute is
126 * marked as readonly, but we only need to read here
127 * _suspend_sp stores the offset between thread_core_local to _suspend_sp.
128 * _core_pos stores the offset between __get_core_pos to _core_pos.
129 */
130.align 2
131.extern thread_core_local
132_suspend_sp:
133	.long	thread_core_local - .
134.extern __get_core_pos
135_core_pos:
136	.long	__get_core_pos - .
137
138/*
139 * void sm_pm_cpu_do_resume(paddr suspend_regs) __noreturn;
140 * Restore the registers stored when sm_pm_cpu_do_suspend
141 * r0 points to the physical base address of the suspend_regs
142 * field of struct sm_pm_ctx.
143 */
144FUNC sm_pm_cpu_do_resume, :
145UNWIND(	.cantunwind)
146	read_midr r4
147	ubfx	r5, r4, #4, #12
148	ldr     r4, =CORTEX_A5_PART_NUM
149	cmp	r5, r4
150	beq	a5_a7_resume
151	ldr     r4, =CORTEX_A7_PART_NUM
152	cmp	r5, r4
153	beq	a5_a7_resume
154
155	/*
156	 * A9 needs PCR/DIAG
157	 */
158	ldmia   r0!, {r4 - r5}
159	write_pcr r4
160	write_diag r5
161
162a5_a7_resume:
163	/* v7 resume */
164	mov	ip, #0
165	/* Invalidate icache to PoU */
166	write_iciallu
167	/* set reserved context */
168	write_contextidr ip
169	ldmia	r0!, {r4 - r5}
170	write_fcseidr r4
171	write_tpidruro r5
172	ldmia	r0!, {r4 - r11}
173	/* Invalidate entire TLB */
174	write_tlbiall
175	write_dacr r4
176#ifdef CFG_WITH_LPAE
177#error "Not supported -"
178#else
179	write_ttbr0 r5
180	write_ttbr1 r6
181	write_ttbcr r7
182#endif
183
184	ldmia	r0, {r4 - r7}
185	write_prrr r4
186	write_nmrr r5
187	write_vbar r6
188	write_nsacr r7
189
190	write_actlr r9
191	write_cpacr r10
192	write_mvbar r11
193	write_bpiall
194	isb
195	dsb
196	/* MMU will be enabled here */
197	write_sctlr r8
198	isb
199	mov	r0, #0
200	b	suspend_return
201END_FUNC sm_pm_cpu_do_resume
202
203