summaryrefslogtreecommitdiff
path: root/ANDROID_3.4.5/arch/arm/mach-wmt/sleep.S
blob: 07d52325ec006f1b34bb13b2148b1fc571c91481 (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
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
/*++
linux/arch/arm/mach-wmt/sleep.s

Copyright (c) 2008  WonderMedia Technologies, Inc.

This program is free software: you can redistribute it and/or modify it under the
terms of the GNU General Public License as published by the Free Software Foundation,
either version 2 of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
PARTICULAR PURPOSE.  See the GNU General Public License for more details.
You should have received a copy of the GNU General Public License along with
this program.  If not, see <http://www.gnu.org/licenses/>.

WonderMedia Technologies, Inc.
10F, 529, Chung-Cheng Road, Hsin-Tien, Taipei 231, R.O.C.
--*/
#include <linux/linkage.h>
#include <asm/assembler.h>
#include <mach/wmt_secure.h>



/*
 * Registers access definitions
 */
/* Standard definitions of mode bits and interrupt (I & F) flags in PSRs */
#define	MODE_USR	0x10
#define MODE_FIQ	0x11
#define MODE_IRQ	0x12
#define MODE_SVC	0x13
#define MODE_ABT	0x17
#define MODE_UND	0x1B
#define MODE_SYS	0x1F

#define I_BIT		0x80
#define F_BIT		0x40

#define DO_POWER_OFF_SUSPEND	0xFFFFFFC4

	.arm
	.arch_extension sec

	.text

ENTRY(wmt_assem_suspend)
//suspend
	mrs	r3, cpsr
	bic r3, r3, #0x1f
	orr r3, r3, #MODE_SVC
	msr	cpsr_c, r3			@ ensure we are in SVC mode
	mov	r2, sp                                  @ using svc stack pointer as the global one

	mrs	r3, cpsr
	bic r3, r3, #0x1f
	orr r3, r3, #MODE_FIQ
	msr	cpsr_c, r3			@ store FIQ bank registers
	mrs	r3, spsr
	stmfd	r2!, {r3, r8 - r14}

	mrs	r3, cpsr
	bic r3, r3, #0x1f
	orr r3, r3, #MODE_IRQ
	msr	cpsr_c, r3			@ store IRQ bank registers
	mrs	r3, spsr
	stmfd	r2!, {r3, r13, r14}

	mrs	r3, cpsr
	bic r3, r3, #0x1f
	orr r3, r3, #MODE_UND
	msr	cpsr_c, r3			@ store UND bank registers
	mrs	r3, spsr
	stmfd	r2!, {r3, r13, r14}

	mrs	r3, cpsr
	bic r3, r3, #0x1f
	orr r3, r3, #MODE_ABT
	msr	cpsr_c, r3			@ store ABT bank registers
	mrs	r3, spsr
	stmfd	r2!, {r3, r13, r14}

	mrs	r3, cpsr
	bic r3, r3, #0x1f
	orr r3, r3, #MODE_SYS
	msr	cpsr_c, r3                       @ store SYS bank registers
	mrs	r3, spsr
	stmfd	r2!, {r3 - r14}

	mrs	r3, cpsr
	bic r3, r3, #0x1f
	orr r3, r3, #MODE_SVC
	msr     cpsr_c, r3		@ come back to SVC mode
	mov	sp, r2			                @ restore the updated and used stack pointer

	ldr r2, =0xfe000000
	
	mrs	r0, cpsr
	mrs	r1, spsr
	stmfd   sp!, {r0, r1, r4 - r12, lr}            	@ save current mode (SVC) registers on stack

	mrc	p15, 0, r4, c13, c0, 0	@ FCSE/PID
	mrc	p15, 0, r5, c13, c0, 1	@ Context ID
	mrc	p15, 0, r6, c13, c0, 3	@ User r/o thread ID
	
	stmfd	sp!, {r4 - r6}
	mrc	p15, 0, r6, c3, c0, 0	@ Domain ID
	mrc	p15, 0, r7, c2, c0, 0	@ TTB 0
	mrc	p15, 0, r8, c2, c0, 1	@ TTB 1
	mrc	p15, 0, r9, c1, c0, 0	@ Control register
	mrc	p15, 0, r10, c1, c0, 1	@ Auxiliary control register
	mrc	p15, 0, r11, c1, c0, 2	@ Co-processor access control
	stmfd	sp!, {r6 - r11}

	ldr r1, =0x130000
	add r5, r2, r1

	/* Store physcial address of stack to HSP2_REG */
	ldr	r1, =0xc0000000				@ PAGE_OFFSET, define in memory.h
	mov	r0, sp
	sub	r0, r0, r1                              @ __virt_to_phys(x)	((x) - PAGE_OFFSET + PHYS_OFFSET)
	str	r0, [r5, #0x34]				@ save sp to HSPR2

	/* Flush all data */

	dmb					@ ensure ordering with previous memory accesses
	mrc	p15, 1, r0, c0, c0, 1		@ read clidr
	ands	r3, r0, #0x7000000		@ extract loc from clidr
	mov	r3, r3, lsr #23			@ left align loc bit field
	beq	finished			@ if loc is 0, then no need to clean
	mov	r10, #0				@ start clean at cache level 0
loop1:
	add	r2, r10, r10, lsr #1		@ work out 3x current cache level
	mov	r1, r0, lsr r2			@ extract cache type bits from clidr
	and	r1, r1, #7			@ mask of the bits for current cache only
	cmp	r1, #2				@ see what cache we have at this level
	blt	skip				@ skip if no cache, or just i-cache
	mcr	p15, 2, r10, c0, c0, 0		@ select current cache level in cssr
	isb					@ isb to sych the new cssr&csidr
	mrc	p15, 1, r1, c0, c0, 0		@ read the new csidr
	and	r2, r1, #7			@ extract the length of the cache lines
	add	r2, r2, #4			@ add 4 (line length offset)
	ldr	r4, =0x3ff
	ands	r4, r4, r1, lsr #3		@ find maximum number on the way size
	clz	r5, r4				@ find bit position of way size increment
	ldr	r7, =0x7fff
	ands	r7, r7, r1, lsr #13		@ extract max number of the index size
loop2:
	mov	r9, r4				@ create working copy of max way size
loop3:
 ARM(	orr	r11, r10, r9, lsl r5	)	@ factor way and cache number into r11
 THUMB(	lsl	r6, r9, r5		)
 THUMB(	orr	r11, r10, r6		)	@ factor way and cache number into r11
 ARM(	orr	r11, r11, r7, lsl r2	)	@ factor index number into r11
 THUMB(	lsl	r6, r7, r2		)
 THUMB(	orr	r11, r11, r6		)	@ factor index number into r11
	mcr	p15, 0, r11, c7, c14, 2		@ clean & invalidate by set/way
	subs	r9, r9, #1			@ decrement the way
	bge	loop3
	subs	r7, r7, #1			@ decrement the index
	bge	loop2
skip:
	add	r10, r10, #2			@ increment cache number
	cmp	r3, r10
	bgt	loop1
finished:
	mov	r10, #0				@ swith back to cache level 0
	mcr	p15, 2, r10, c0, c0, 0		@ select current cache level in cssr


	
	dsb
	isb


	/* Save physical entry address */
	ldr     r1, =DO_POWER_OFF_SUSPEND
	
	/* Turn off MMU and disable caches */
	mrc	p15, 0, r0, c1, c0, 0	                @ read control reg
	bic	r0, r0, #0x05		                @ clear D-cache, turn off MMU
	bic	r0, r0, #0x1000		                @ clear I-cache
	mcr	p15, 0, r0, c1, c0, 0                   @ turn off ...
	nop
	//mov     pc, r1
	blx		r1
	nop
	nop
	nop
	nop
	

	ldmia   sp!, {r0, r1, r4 - r12, lr}

	msr	cpsr, r0
	msr	spsr, r1

	mov	r1, r2

	mov	r2, sp	                                @ using svc stack pointer as the global one

	mrs	r3, cpsr
	bic r3, r3, #0x1f
	orr r3, r3, #MODE_SYS
	msr	cpsr_c, r3
	ldmia	r2!, {r3 - r14}
	msr	spsr, r3

	mrs	r3, cpsr
	bic r3, r3, #0x1f
	orr r3, r3, #MODE_ABT
	msr	cpsr_c, r3			@ restore ABT bank registers
	ldmia	r2!, {r3, r13, r14}
	msr	spsr, r3

	mrs	r3, cpsr
	bic r3, r3, #0x1f
	orr r3, r3, #MODE_UND
	msr	cpsr_c, r3			@ restore UND bank registers
	ldmia	r2!, {r3, r13, r14}
	msr	spsr, r3

	mrs	r3, cpsr
	bic r3, r3, #0x1f
	orr r3, r3, #MODE_IRQ
	msr	cpsr_c, r3			@ restore IRQ bank registers
	ldmia	r2!, {r3, r13, r14}
	msr	spsr, r3

	mrs	r3, cpsr
	bic r3, r3, #0x1f
	orr r3, r3, #MODE_FIQ
	msr	cpsr_c, r3			@ restore FIQ bank registers
	ldmia	r2!, {r3, r8 - r14}
	msr	spsr, r3

	mrs	r3, cpsr
	bic r3, r3, #0x1f
	orr r3, r3, #MODE_SVC
	msr	cpsr_c,  r3 		@ ensure we are in SVC mode
	mov	sp, r2


	mov	pc, lr					@ return to caller, saved when get into suspend
      nop     	
      nop     	
      nop     
      nop      	

ENDPROC(wmt_assem_suspend)


ENTRY(wmt_assem_secure_suspend)
//suspend
	mrs	r3, cpsr
	bic r3, r3, #0x1f
	orr r3, r3, #MODE_SVC
	msr	cpsr_c, r3			@ ensure we are in SVC mode
	mov	r2, sp              @ using svc stack pointer as the global one

	mrs	r3, cpsr
	bic r3, r3, #0x1f
	orr r3, r3, #MODE_FIQ
	msr	cpsr_c, r3			@ store FIQ bank registers
	mrs	r3, spsr
	stmfd	r2!, {r3, r8 - r14}

	mrs	r3, cpsr
	bic r3, r3, #0x1f
	orr r3, r3, #MODE_IRQ
	msr	cpsr_c, r3			@ store IRQ bank registers
	mrs	r3, spsr
	stmfd	r2!, {r3, r13, r14}

	mrs	r3, cpsr
	bic r3, r3, #0x1f
	orr r3, r3, #MODE_UND
	msr	cpsr_c, r3			@ store UND bank registers
	mrs	r3, spsr
	stmfd	r2!, {r3, r13, r14}

	mrs	r3, cpsr
	bic r3, r3, #0x1f
	orr r3, r3, #MODE_ABT
	msr	cpsr_c, r3			@ store ABT bank registers
	mrs	r3, spsr
	stmfd	r2!, {r3, r13, r14}

	mrs	r3, cpsr
	bic r3, r3, #0x1f
	orr r3, r3, #MODE_SYS
	msr	cpsr_c, r3          @ store SYS bank registers
	mrs	r3, spsr
	stmfd	r2!, {r3 - r14}

	mrs	r3, cpsr
	bic r3, r3, #0x1f
	orr r3, r3, #MODE_SVC
	msr     cpsr_c, r3		@ come back to SVC mode
	mov	sp, r2			    @ restore the updated and used stack pointer

	ldr r2, =0xfe000000
	
	mrs	r0, cpsr
	mrs	r1, spsr
	stmfd   sp!, {r0, r1, r4 - r12, lr}            	@ save current mode (SVC) registers on stack

	mrc	p15, 0, r4, c13, c0, 0	@ FCSE/PID
	mrc	p15, 0, r5, c13, c0, 1	@ Context ID
	mrc	p15, 0, r6, c13, c0, 3	@ User r/o thread ID
	
	stmfd	sp!, {r4 - r6}
	mrc	p15, 0, r6, c3, c0, 0	@ Domain ID
	mrc	p15, 0, r7, c2, c0, 0	@ TTB 0
	mrc	p15, 0, r8, c2, c0, 1	@ TTB 1
	mrc	p15, 0, r9, c1, c0, 0	@ Control register
	mrc	p15, 0, r10, c1, c0, 1	@ Auxiliary control register
	mrc	p15, 0, r11, c1, c0, 2	@ Co-processor access control
	stmfd	sp!, {r6 - r11}

	ldr r1, =0x130000
	add r5, r2, r1

	/* Store physcial address of stack to HSP1_REG */
	ldr	r1, =0xc0000000				@ PAGE_OFFSET, define in memory.h
	mov	r0, sp
	sub	r0, r0, r1                              @ __virt_to_phys(x)	((x) - PAGE_OFFSET + PHYS_OFFSET)
	str	r0, [r5, #0x34]				@ save sp to HSPR1


	/* Flush all data */
	dmb					@ ensure ordering with previous memory accesses
	mrc	p15, 1, r0, c0, c0, 1		@ read clidr
	ands	r3, r0, #0x7000000		@ extract loc from clidr
	mov	r3, r3, lsr #23			@ left align loc bit field
	beq	sec_finished			@ if loc is 0, then no need to clean
	mov	r10, #0				@ start clean at cache level 0
sec_loop1:
	add	r2, r10, r10, lsr #1		@ work out 3x current cache level
	mov	r1, r0, lsr r2			@ extract cache type bits from clidr
	and	r1, r1, #7			@ mask of the bits for current cache only
	cmp	r1, #2				@ see what cache we have at this level
	blt	sec_skip				@ skip if no cache, or just i-cache
	mcr	p15, 2, r10, c0, c0, 0		@ select current cache level in cssr
	isb					@ isb to sych the new cssr&csidr
	mrc	p15, 1, r1, c0, c0, 0		@ read the new csidr
	and	r2, r1, #7			@ extract the length of the cache lines
	add	r2, r2, #4			@ add 4 (line length offset)
	ldr	r4, =0x3ff
	ands	r4, r4, r1, lsr #3		@ find maximum number on the way size
	clz	r5, r4				@ find bit position of way size increment
	ldr	r7, =0x7fff
	ands	r7, r7, r1, lsr #13		@ extract max number of the index size
sec_loop2:
	mov	r9, r4				@ create working copy of max way size
sec_loop3:
 ARM(	orr	r11, r10, r9, lsl r5	)	@ factor way and cache number into r11
 THUMB(	lsl	r6, r9, r5		)
 THUMB(	orr	r11, r10, r6		)	@ factor way and cache number into r11
 ARM(	orr	r11, r11, r7, lsl r2	)	@ factor index number into r11
 THUMB(	lsl	r6, r7, r2		)
 THUMB(	orr	r11, r11, r6		)	@ factor index number into r11
	mcr	p15, 0, r11, c7, c14, 2		@ clean & invalidate by set/way
	subs	r9, r9, #1			@ decrement the way
	bge	sec_loop3
	subs	r7, r7, #1			@ decrement the index
	bge	sec_loop2
sec_skip:
	add	r10, r10, #2			@ increment cache number
	cmp	r3, r10
	bgt	sec_loop1
sec_finished:
	mov	r10, #0						@ swith back to cache level 0
	mcr	p15, 2, r10, c0, c0, 0		@ select current cache level in cssr


	
	dsb
	isb


	
	/* Turn off MMU and disable caches */
	mrc	p15, 0, r0, c1, c0, 0	            @ read control reg
	bic	r0, r0, #0x05		                @ clear D-cache, turn off MMU
	bic	r0, r0, #0x1000		                @ clear I-cache
	mcr	p15, 0, r0, c1, c0, 0               @ turn off ...
	nop
	//mov     pc, r1
	//blx		r1
	ldr     r0, =WMT_SMC_CMD_SECURE_SUSPEND
	dsb
	smc	#0                                  @switch to monitor mode
	nop
	nop
	nop
	nop
	

	ldmia   sp!, {r0, r1, r4 - r12, lr}

	msr	cpsr, r0
	msr	spsr, r1

	mov	r1, r2

	mov	r2, sp	                                @ using svc stack pointer as the global one

	mrs	r3, cpsr
	bic r3, r3, #0x1f
	orr r3, r3, #MODE_SYS
	msr	cpsr_c, r3
	ldmia	r2!, {r3 - r14}
	msr	spsr, r3

	mrs	r3, cpsr
	bic r3, r3, #0x1f
	orr r3, r3, #MODE_ABT
	msr	cpsr_c, r3			@ restore ABT bank registers
	ldmia	r2!, {r3, r13, r14}
	msr	spsr, r3

	mrs	r3, cpsr
	bic r3, r3, #0x1f
	orr r3, r3, #MODE_UND
	msr	cpsr_c, r3			@ restore UND bank registers
	ldmia	r2!, {r3, r13, r14}
	msr	spsr, r3

	mrs	r3, cpsr
	bic r3, r3, #0x1f
	orr r3, r3, #MODE_IRQ
	msr	cpsr_c, r3			@ restore IRQ bank registers
	ldmia	r2!, {r3, r13, r14}
	msr	spsr, r3

	mrs	r3, cpsr
	bic r3, r3, #0x1f
	orr r3, r3, #MODE_FIQ
	msr	cpsr_c, r3			@ restore FIQ bank registers
	ldmia	r2!, {r3, r8 - r14}
	msr	spsr, r3

	mrs	r3, cpsr
	bic r3, r3, #0x1f
	orr r3, r3, #MODE_SVC
	msr	cpsr_c,  r3 		@ ensure we are in SVC mode
	mov	sp, r2


	mov	pc, lr					@ return to caller, saved when get into suspend
      nop     	
      nop     	
      nop     
      nop      	

ENDPROC(wmt_assem_secure_suspend)