summaryrefslogtreecommitdiff
path: root/arch/arm/plat-mxc/entry-pm.S
blob: 4a3af0e16191e395c06f80605e9057c8f17d766a (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
/*
 * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
 */

/*
 * The code contained herein is licensed under the GNU General Public
 * License. You may obtain a copy of the GNU General Public License
 * Version 2 or later at the following locations:
 *
 * http://www.opensource.org/licenses/gpl-license.html
 * http://www.gnu.org/copyleft/gpl.html
 */

/*!
 * @file plat-mxc/entry-pm.S
 *
 * @brief This file contains common pm entry .
 *
 * @ingroup MXC_PM
 */

#include <asm/assembler.h>
#include <asm/ptrace.h>
#include <asm/memory.h>
#include <asm/system.h>
#include <mach/hardware.h>
#include <asm/asm-offsets.h>
#include <asm/thread_info.h>
#include <asm/proc-fns.h>
#include <asm/vfpmacros.h>

#define WAIT_MODE               111
#define DOZE_MODE               112
#define STOP_MODE               113
#define DSM_MODE                114

#define PM_XLOAD_SIZE		0x04
#define PM_XLOAD_ENTRY		0x08
#define PM_XLOAD_SUSPEND_MODE	0x0C
#define PM_XLOAD_CORE_SP	0x10

#define PROCINFO_PROC_FNS	36
#define PROC_FIN_FN		12
#define PROC_IDLE_FN		20

#ifdef CONFIG_FIQ
#define ARM_CONTEXT_SIZE 12
#else
#define ARM_CONTEXT_SIZE 8
#endif

#ifdef CONFIG_PM_VERBOSE
resume_str:
	.string "Resume from DSM..."
	.size resume_str, . - resume_str

.macro show_resume_str
	ldr r0, =resume_str
	bl printk
.endm

#else
.macro show_resume_str
.endm
#endif

	.data
	.align 3
arm_core_context:
	.rept ARM_CONTEXT_SIZE
	.long 0
	.endr

#ifdef CONFIG_VFP
	.text
	.align 5
arm_vfp_save:
	mov ip, sp
	stmdb sp!, {r0-r8, fp, ip, lr, pc}
	sub fp, ip, #4
	mov r1, #THREAD_SIZE
	sub r1, r1, #1
	bic r0, sp, r1
	ldr r8, [r0, #TI_CPU]
	add r4, r0, #TI_VFPSTATE

	ldr r3, =last_VFP_context
	VFPFMRX	r2, FPEXC
	tst r2, #FPEXC_EN
	bne 1f

	ldr r4, [r3, r8, lsl #2]
	cmp r4, #0
	beq dead_vfp
1:
	bic r1, r2, #FPEXC_EN
	VFPFMXR FPEXC, r1
	/*TODO: SMP */
	VFPFSTMIA r4, r1
	VFPFMRX	r5, FPSCR
	tst r2, #FPEXC_EX
	VFPFMRX r6, FPINST, NE
	tstne r2, #FPEXC_FP2V
	VFPFMRX r7, FPINST2, NE
	stmia r4, {r2, r5, r6, r7}

	mov r1, #0
	str r1, [r3, r8, lsl #2]
dead_vfp:
	ldmia sp, {r0-r8, fp, sp, pc}
#endif
/*
 * The function just be called in this file
 * Current r0 ~r4 are not saved.
 * Otherwise, the working registers should be saved
 */
	.text
	.align 5
arm_core_save:
	mov ip, sp
	stmdb sp!, {r8, r9, sl, fp, ip, lr, pc}
	sub fp, ip, #4
	ldr r0, =arm_core_context
	mov r3, r0
	/* SVC mode */
	mrs r1, spsr	@Save spsr
	mrs r2, cpsr	@Save cpsr
	stmia r0!, {r1, r2}
	/* Abort mode */
	msr cpsr_c, #PSR_F_BIT | PSR_I_BIT | ABT_MODE
	stmia r0!, {sp}		@Save stack pointer for abort mode
	msr cpsr_c, #PSR_F_BIT | PSR_I_BIT | UND_MODE
	stmia r0!, {sp}		@Save stack pointer for undefine mode
	msr cpsr_c, #PSR_F_BIT | PSR_I_BIT | IRQ_MODE
	stmia r0!, {sp}		@Save stack pointer for irq mode
#ifdef CONFIG_FIQ
	msr cpsr_c, #PSR_F_BIT | PSR_I_BIT | FIQ_MODE
	/*Save general register and sp for fiq mode*/
	stmia r0!, {r8-r9, sl, fp, ip, sp}
#endif
	ldr r0, [r3, #4]
	msr cpsr_c, r0
	ldmia sp, {r8-r9, sl, fp, sp, pc}

/*
 * The function just be called in this file
 * Current r0 ~r4 are not saved.
 * Otherwise, the working registers should be saved
 */
arm_core_restore:
	mov ip, sp
	stmdb sp!, {fp, ip, lr, pc}
	sub fp, ip, #4
	ldr r0, =arm_core_context
	mov r3, r0
	/* SVC mode */
	add r0, r0, #8		@skip svc mode
	/* Abort mode */
	msr cpsr_c, #PSR_F_BIT | PSR_I_BIT | ABT_MODE
	ldmia r0!, {sp}		@restore stack pointer for abort mode
	msr cpsr_c, #PSR_F_BIT | PSR_I_BIT | UND_MODE
	ldmia r0!, {sp}		@restore stack pointer for undefine mode
	msr cpsr_c, #PSR_F_BIT | PSR_I_BIT | IRQ_MODE
	ldmia r0!, {sp}		@restore stack pointer for irq mode
#ifdef CONFIG_FIQ
	msr cpsr_c, #PSR_F_BIT | PSR_I_BIT | FIQ_MODE
	/*Save general register and sp for fiq mode*/
	ldmia r0!, {r8-r9, sl, fp, ip, sp}
#endif
	ldmia r3!, {r1, r2}
	msr cpsr, r2		@restore cpsr
	msr spsr, r1		@restore spsr
	ldmia sp, {fp, sp, pc}

mxc_cp15_context:
	.rept 16
	.long 0
	.endr

	.align 5
mxc_cp15_restore:
	/* Physical address */
	adr r0, mxc_cp15_context
	ldmia r0, {r1-r9}
#ifndef CONFIG_PM_DEBUG
	@Add dynamic check to skip this block when debug
	sub lr, lr, #PHYS_OFFSET
	add lr, lr, #PAGE_OFFSET
#endif
	mcr p15, 0, r3, c1, c0, 2	@CP Access Register
	mcr p15, 0, r2, c1, c0, 1	@Aux Control register

#ifndef CONFIG_PM_DEBUG
	mcr p15, 0, r0, c7, c5, 6	@flush BTAC/BTB
	mcr p15, 0, r0, c7, c7, 0	@invalidate both caches
	mcr p15, 0, r0, c8, c7, 0	@Inval TLBs
#endif

	mcr p15, 0, r4, c13, c0, 0	@PID
	mcr p15, 0, r5, c13, c0, 1	@Context ID

	mcr p15, 0, r6, c3, c0, 0	@Domain Access Register
	mcr p15, 0, r7, c2, c0, 0	@TTB0
	mcr p15, 0, r8, c2, c0, 1	@TTB1
	mcr p15, 0, r9, c2, c0, 2	@TTBC

	mcr p15, 0, r1, c1, c0, 0	@Control Register
	/* mcu enabled */
	mrc p15, 0, r0, c2, c0, 0

	mov pc, lr
	nop
	nop
	nop
	nop
	nop
	nop
	nop
	nop

mxc_cp15_save:
	mov ip, sp
	stmdb sp!, {r8-r9, fp, ip, lr, pc}
	sub fp, ip, #4
	ldr r0, =mxc_cp15_context
/* System Control Registers */
	mrc p15, 0, r1, c1, c0, 0	@Control Register
	mrc p15, 0, r2, c1, c0, 1	@Aux Control Register
	mrc p15, 0, r3, c1, c0, 2	@CP access Register

/* Memory management Registers */
	mrc p15, 0, r4, c13, c0, 0	@PID
	mrc p15, 0, r5, c13, c0, 1	@Context ID

	mrc p15, 0, r6, c3, c0, 0	@Domain Access Register

	mrc p15, 0, r7, c2, c0, 0	@TTB0
	mrc p15, 0, r8, c2, c0, 1	@TTB1
	mrc p15, 0, r9, c2, c0, 2	@TTBC
	stmia r0, {r1-r9}
	ldmia sp, {r8, r9, fp, sp, pc}

/*
 * int __mxc_pm_arch_entry(u32 entry, u32 size)
 */
	.align 5
	.globl mxc_pm_arch_entry
mxc_pm_arch_entry:
	mov ip, sp
	stmdb sp!, {r4-r9, sl, fp, ip, lr, pc}
	sub fp, ip, #4
	sub sp, sp, #4
	mov r8, r0	@save entry
	mov r9, r1	@save entry size
#ifdef CONFIG_VFP
	bl arm_vfp_save
#endif
	/* r0 ~r3, ip is dirty*/
	bl arm_core_save	@save arm context
	bl mxc_cp15_save
	mov r0, sp
	mov r1, r8	@restore entry
	mov r2, r9	@restore entry size
	bl __mxc_pm_xload_setup
1:	bl cpu_v6_proc_fin
	bl cpu_v6_do_idle
	nop
	nop
	nop
	nop
__mxc_pm_arch_leave:
	adr r0, __mxc_pm_xload_info
	ldr sp, [r0, #PM_XLOAD_CORE_SP]

#ifndef CONFIG_PM_DEBUG
	sub sp, sp, #PAGE_OFFSET
	add sp, sp, #PHYS_OFFSET
#endif
	bl mxc_cp15_restore
#ifndef CONFIG_PM_DEBUG
	sub sp, sp, #PHYS_OFFSET
	add sp, sp, #PAGE_OFFSET
#endif
	show_resume_str
	bl arm_core_restore
	ldmib sp, {r4-r9, sl, fp, sp, pc}

__mxc_pm_xload_info:
	adr pc, __mxc_pm_xload_entry		@Jump instruction
	.long	__mxc_pm_xload_end - __mxc_pm_xload_info	@loader size
	.long 	(__mxc_pm_arch_leave - PAGE_OFFSET + PHYS_OFFSET) @resume entry
	.long   0 		@suspend state
	.long   0		@Core Stack pointer
__mxc_pm_xload_entry:
	adr r0, __mxc_pm_xload_info
	ldr pc, [r0, #PM_XLOAD_ENTRY]
__mxc_pm_xload_end:

/*
 * __mxc_pm_xload_setup(u32 sp, u32 entry, u32 size)
 * r0~r6 is dirty
 */
__mxc_pm_xload_setup:
	ldr r3, =__mxc_pm_xload_info
	str r0, [r3, #PM_XLOAD_CORE_SP]
	ldr r4, [r3, #PM_XLOAD_SIZE]
	cmp r2, r4
	blo 2f
1:	ldr r5, [r3], #4
	str r5, [r1], #4
	subs r4, r4, #4
	bhi 1b
	b 3f
2:	str r3, [r1]
3:	mov pc, lr