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