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
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
|
;
; Copyright (c) 2011, ARM Limited. All rights reserved.
;
; Redistribution and use in source and binary forms, with
; or without modification, are permitted provided that the
; following conditions are met:
;
; Redistributions of source code must retain the above
; copyright notice, this list of conditions and the
; following disclaimer.
;
; Redistributions in binary form must reproduce the
; above copyright notice, this list of conditions and
; the following disclaimer in the documentation
; and/or other materials provided with the distribution.
;
; Neither the name of ARM nor the names of its
; contributors may be used to endorse or promote products
; derived from this software without specific prior written
; permission.
;
EXPORT save_performance_monitors
EXPORT restore_performance_monitors
EXPORT save_banked_registers
EXPORT restore_banked_registers
EXPORT save_cp15
EXPORT restore_cp15
EXPORT save_control_registers
EXPORT restore_control_registers
EXPORT save_mmu
EXPORT restore_mmu
EXPORT save_mpu
EXPORT restore_mpu
EXPORT save_vfp
EXPORT restore_vfp
EXPORT save_generic_timer
EXPORT restore_generic_timer
EXPORT save_fault_status
EXPORT restore_fault_status
AREA APPF, CODE
; Aliases for mode encodings - do not change
MODE_USR EQU 0x10
MODE_FIQ EQU 0x11
MODE_IRQ EQU 0x12
MODE_SVC EQU 0x13
MODE_MON EQU 0x16 ; A-profile (Security Extensions) only
MODE_ABT EQU 0x17
MODE_UND EQU 0x1B
MODE_SYS EQU 0x1F
MODE_HYP EQU 0x1A
TTBCR_EAE EQU (1<<31) ; Are we using LPAE?
PFR0_THUMB_EE_SUPPORT EQU (1<<12)
save_performance_monitors FUNCTION
push {r4, r8, r9, r10}
; Ignore:
; Count Enable Clear Register
; Software Increment Register
; Interrupt Enable Clear Register
mrc p15,0,r8,c9,c12,0 ; PMon: Control Register
bic r1,r8,#1
mcr p15,0,r1,c9,c12,0 ; disable counter updates from here
isb ; 0b0 => PMCR<0>
mrc p15,0,r9,c9,c12,3 ; PMon: Overflow Flag Status Reg
mrc p15,0,r10,c9,c12,5 ; PMon: Event Counter Selection Reg
stm r0!, {r8-r10}
ubfx r9,r8,#11,#5 ; extract # of event counters, N
tst r9, r9
beq %f1
0 subs r9,r9,#1 ; decrement N
mcr p15,0,r9,c9,c12,5 ; PMon: select CounterN
isb
mrc p15,0,r3,c9,c13,1 ; PMon: save Event Type register
mrc p15,0,r4,c9,c13,2 ; PMon: save Event Counter register
stm r0!, {r3,r4}
bne %b0
1 mrc p15,0,r1,c9,c13,0 ; PMon: Cycle Count Register
mrc p15,0,r2,c9,c14,0 ; PMon: User Enable Register
mrc p15,0,r3,c9,c14,1 ; PMon: Interrupt Enable Set Reg
mrc p15,0,r4,c9,c12,1 ; PMon: Count Enable Set Register
stm r0!, {r1-r4}
pop {r4, r8, r9, r10}
bx lr
ENDFUNC
restore_performance_monitors FUNCTION
push {r4-r5, r8-r10, lr}
; NOTE: all counters disabled by PMCR<0> == 0 on reset
; Restore performance counters
ldm r0!,{r8-r10} ; recover first block of PMon context
; (PMCR, PMOVSR, PMSELR)
mov r1, #0 ; generate register of all 0's
mvn r2, #0 ; generate register of all 1's
mcr p15,0,r2,c9,c14,2 ; disable all counter related interrupts
mcr p15,0,r2,c9,c12,3 ; clear all overflow flags
isb
ubfx r12,r8,#11,#5 ; extract # of event counters, N (0-31)
tst r12, r12
beq %f20
mov r3, r12 ; for N >0, generate a 2nd copy of N
mov r4, #1
lsl r4, r4, r3
sub r4, r4, #1 ; set bits<N-1:0> to all 1's
0 subs r3,r3,#1 ; decrement N
mcr p15,0,r3,c9,c12,5 ; select Event CounterN
isb
mrc p15,0,r5,c9,c13,1 ; read Event Type register
bfc r5,#0,#8
mcr p15,0,r5,c9,c13,1 ; set Event Type to 0x0
mcr p15,0,r2,c9,c13,2 ; set Event Counter to all 1's
isb
bne %b0
mov r3, #1
bic r5, r9, #1<<31
mcr p15,0,r5,c9,c12,1 ; enable Event Counters
; (PMOVSR bits set)
mcr p15,0,r3,c9,c12,0 ; set the PMCR global enable bit
isb
mcr p15,0,r9,c9,c12,4 ; set event count overflow bits
isb
mcr p15,0,r4,c9,c12,2 ; disable Event Counters
; restore the event counters
10 subs r12,r12,#1 ; decrement N
mcr p15,0,r12,c9,c12,5 ; select Event CounterN
isb
ldm r0!,{r3-r4}
mcr p15,0,r3,c9,c13,1 ; restore Event Type
mcr p15,0,r4,c9,c13,2 ; restore Event Counter
isb
bne %b10
20 tst r9, #0x80000000 ; check for cycle count overflow flag
beq %f40
mcr p15,0,r2,c9,c13,0 ; set Cycle Counter to all 1's
isb
mov r3, #0x80000000
mcr p15,0,r3,c9,c12,1 ; enable the Cycle Counter
isb
30 mrc p15,0,r4,c9,c12,3 ; check cycle count overflow now set
movs r4,r4 ; test bit<31>
bpl %b30
mcr p15,0,r3,c9,c12,2 ; disable the Cycle Counter
40 mcr p15,0,r1,c9,c12,0 ; clear the PMCR global enable bit
isb
; restore the remaining PMon registers
ldm r0!,{r1-r4}
mcr p15,0,r1,c9,c13,0 ; restore Cycle Count Register
mcr p15,0,r2,c9,c14,0 ; restore User Enable Register
mcr p15,0,r3,c9,c14,1 ; restore Interrupt Enable Set Reg
mcr p15,0,r4,c9,c12,1 ; restore Count Enable Set Register
mcr p15,0,r10,c9,c12,5 ; restore Event Counter Selection
isb
mcr p15,0,r8,c9,c12,0 ; restore the PM Control Register
isb
pop {r4-r5, r8-r10, pc}
ENDFUNC
save_banked_registers FUNCTION
mrs r2, CPSR ; save current mode
and r3, r2, #0x1f ; If we are in HYP mode then use the virt.
cmp r3, #MODE_HYP ; instructions to save the banked registers
beq save_in_hyp ; without changing the mode
cps #MODE_SYS ; switch to System mode
str sp,[r0], #4 ; save the User SP
str lr,[r0], #4 ; save the User LR
cps #MODE_ABT ; switch to Abort mode
str sp,[r0], #4 ; save the current SP
mrs r3,SPSR
stm r0!,{r3,lr} ; save the current SPSR, LR
cps #MODE_UND ; switch to Undefined mode
str sp,[r0], #4 ; save the current SP
mrs r3,SPSR
stm r0!,{r3,lr} ; save the current SPSR, LR
cps #MODE_IRQ ; switch to IRQ mode
str sp,[r0], #4 ; save the current SP
mrs r3,SPSR
stm r0!,{r3,lr} ; save the current SPSR, LR
cps #MODE_FIQ ; switch to FIQ mode
str SP,[r0], #4 ; save the current SP
mrs r3,SPSR
stm r0!,{r8-r12,lr} ; save the current SPSR,r8-r12,LR
msr CPSR_cxsf, r2 ; switch back to original mode
bx lr
save_in_hyp
mrs r1, SP_usr
stm r0!, {r1}
mrs r1, SP_und
mrs r2, SPSR_und
mrs r3, LR_und
stm r0!, {r1-r3}
mrs r1, SP_abt
mrs r2, SPSR_abt
mrs r3, LR_abt
stm r0!, {r1-r3}
mrs r1, SP_svc
mrs r2, SPSR_svc
mrs r3, LR_svc
stm r0!, {r1-r3}
mrs r1, SP_irq
mrs r2, SPSR_irq
mrs r3, LR_irq
stm r0!, {r1-r3}
mrs r1, SP_fiq
mrs r2, SPSR_fiq
mrs r3, LR_fiq
stm r0!, {r1-r3}
mrs r1, r8_fiq
mrs r2, r9_fiq
mrs r3, r10_fiq
stm r0!, {r1-r3}
mrs r1, r11_fiq
mrs r2, r12_fiq
stm r0!, {r1-r2}
bx lr
ENDFUNC
restore_banked_registers FUNCTION
mrs r2, CPSR ; save current mode
and r3, r2, #0x1f ; If we are in HYP mode then use the virt.
cmp r3, #MODE_HYP ; instructions to restore the banked registers
beq rest_in_hyp ; without changing the mode
cps #MODE_SYS ; switch to System mode
ldr sp,[r0],#4 ; restore the User SP
ldr lr,[r0],#4 ; restore the User LR
cps #MODE_ABT ; switch to Abort mode
ldr sp,[r0],#4 ; restore the current SP
ldm r0!,{r3,lr} ; restore the current LR
msr SPSR_fsxc,r3 ; restore the current SPSR
cps #MODE_UND ; switch to Undefined mode
ldr sp,[r0],#4 ; restore the current SP
ldm r0!,{r3,lr} ; restore the current LR
msr SPSR_fsxc,r3 ; restore the current SPSR
cps #MODE_IRQ ; switch to IRQ mode
ldr sp,[r0],#4 ; restore the current SP
ldm r0!,{r3,lr} ; restore the current LR
msr SPSR_fsxc,r3 ; restore the current SPSR
cps #MODE_FIQ ; switch to FIQ mode
ldr sp,[r0],#4 ; restore the current SP
ldm r0!,{r8-r12,lr} ; restore the current r8-r12,LR
msr SPSR_fsxc,r4 ; restore the current SPSR
msr CPSR_cxsf, r2 ; switch back to original mode
0 bx lr
rest_in_hyp
ldm r0!, {r1}
msr SP_usr, r1
ldm r0!, {r1-r3}
msr SP_und, r1
msr SPSR_und, r2
msr LR_und, r3
ldm r0!, {r1-r3}
msr SP_abt, r1
msr SPSR_abt, r2
msr LR_abt, r3
ldm r0!, {r1-r3}
msr SP_svc, r1
msr SPSR_svc, r2
msr LR_svc, r3
ldm r0!, {r1-r3}
msr SP_irq, r1
msr SPSR_irq, r2
msr LR_irq, r3
ldm r0!, {r1-r3}
msr SP_fiq, r1
msr SPSR_fiq, r2
msr LR_fiq, r3
ldm r0!, {r1-r3}
msr r8_fiq, r1
msr r9_fiq, r2
msr r10_fiq, r3
ldm r0!, {r1-r2}
msr r11_fiq, r1
msr r12_fiq, r2
bx lr
ENDFUNC
save_cp15 FUNCTION
; CSSELR Cache Size Selection Register
mrc p15,2,r3,c0,c0,0
str r3,[r0], #4
; IMPLEMENTATION DEFINED - proprietary features:
; (CP15 register 15, TCM support, lockdown support, etc.)
; NOTE: IMP DEF registers might have save and restore order that relate
; to other CP15 registers or logical grouping requirements and can
; therefore occur at any point in this sequence.
bx lr
ENDFUNC
restore_cp15 FUNCTION
; CSSELR – Cache Size Selection Register
ldr r3,[r0], #4
mcr p15,2,r3,c0,c0,0
bx lr
ENDFUNC
; Function called with two arguments:
; r0 contains address to store control registers
; r1 is non-zero if we are Secure
save_control_registers FUNCTION
cmp r1, #0 ; Are we Secure?
mrc p15,0,r2,c1,c0,1 ; ACTLR - Auxiliary Control Register
mrc p15,0,r3,c1,c0,0 ; SCTLR - System Control Register
mrc p15,0,r12,c1,c0,2 ; CPACR - Coprocessor Access Control Register
stm r0!, {r2-r3, r12}
mrcne p15,0,r1,c12,c0,1 ; MVBAR - Monitor Vector Base Address Register
mrcne p15,0,r2,c1,c1,0 ; Secure Configuration Register
mrcne p15,0,r3,c1,c1,1 ; Secure Debug Enable Register
mrcne p15,0,r12,c1,c1,2 ; Non-Secure Access Control Register
stmne r0!, {r1-r3,r12}
mrc p15,0,r1,c13,c0,1 ; CONTEXTIDR
mrc p15,0,r2,c13,c0,2 ; TPIDRURW
mrc p15,0,r3,c13,c0,3 ; TPIDRURO
mrc p15,0,r12,c13,c0,4 ; TPIDRPRW
stm r0!, {r1-r3,r12}
; The next two registers are only present if ThumbEE is implemented
mrc p15, 0, r1, c0, c1, 0 ; Read ID_PFR0
tst r1, #PFR0_THUMB_EE_SUPPORT
mrcne p14,6,r1,c0,c0,0 ; TEECR
mrcne p14,6,r2,c1,c0,0 ; TEEHBR
stmne r0!, {r1, r2}
mrc p14,7,r1,c1,c0,0 ; JOSCR
mrc p14,7,r2,c2,c0,0 ; JMCR
stm r0!, {r1, r2}
bx lr
ENDFUNC
; Function called with two arguments:
; r0 contains address to read control registers
; r1 is non-zero if we are Secure
restore_control_registers FUNCTION
cmp r1, #0 ; Are we Secure?
ldm r0!, {r2-r3, r12}
mcr p15,0,r2,c1,c0,1 ; ACTLR - Auxiliary Control Register
mcr p15,0,r3,c1,c0,0 ; SCTLR - System Control Register
mcr p15,0,r12,c1,c0,2 ; CPACR - Coprocessor Access Control Register
ldmne r0!, {r1-r3,r12}
mcrne p15,0,r1,c12,c0,1 ; MVBAR - Monitor Vector Base Address Register
mcrne p15,0,r2,c1,c1,0 ; Secure Configuration Register
mcrne p15,0,r3,c1,c1,1 ; Secure Debug Enable Register
mcrne p15,0,r12,c1,c1,2 ; Non-Secure Access Control Register
ldm r0!, {r1-r3,r12}
mcr p15,0,r1,c13,c0,1 ; CONTEXTIDR
mcr p15,0,r2,c13,c0,2 ; TPIDRURW
mcr p15,0,r3,c13,c0,3 ; TPIDRURO
mcr p15,0,r12,c13,c0,4 ; TPIDRPRW
; The next two registers are only present if ThumbEE is implemented
mrc p15, 0, r1, c0, c1, 0 ; Read ID_PFR0
tst r1, #PFR0_THUMB_EE_SUPPORT
ldmne r0!, {r1,r2}
mcrne p14,6,r1,c0,c0,0 ; TEECR
mcrne p14,6,r2,c1,c0,0 ; TEEHBR
ldm r0!, {r1, r2}
mcr p14,7,r1,c1,c0,0 ; JOSCR
mcr p14,7,r2,c2,c0,0 ; JMCR
isb
bx lr
ENDFUNC
save_mmu FUNCTION
push {r4, r5, r6, r7}
; ASSUMPTION: no useful fault address / fault status information
mrc p15,0,r4,c12,c0,0 ; VBAR
mrc p15,0,r5,c2,c0,2 ; TTBCR
tst r5, #TTBCR_EAE ; Are we using LPAE?
; save 32 or 64 bit TTBRs
mrceq p15,0,r6,c2,c0,0 ; 32 bit TTBR0
mrceq p15,0,r7,c2,c0,1 ; 32 bit TTBR1
mrrcne p15,0,r6,r7,c2 ; 64 bit TTBR0
stm r0!, {r4-r7}
mrrcne p15,1,r6,r7,c2 ; 64 bit TTBR1
stmne r0!, {r6-r7}
mrc p15,0,r4,c3,c0,0 ; DACR
mrc p15,0,r5,c7,c4,0 ; PAR
mrc p15,0,r6,c10,c2,0 ; PRRR
mrc p15,0,r7,c10,c2,1 ; NMRR
stm r0!, {r4-r7}
pop {r4, r5, r6, r7}
bx lr
ENDFUNC
restore_mmu FUNCTION
push {r4, r5, r6, r7}
ldm r0!, {r4-r7}
mcr p15,0,r4,c12,c0,0 ; VBAR
mcr p15,0,r5,c2,c0,2 ; TTBCR
tst r5, #TTBCR_EAE ; Are we using LPAE?
; restore 32 or 64 bit TTBRs
mcreq p15,0,r6,c2,c0,0 ; 32 bit TTBR0
mcreq p15,0,r7,c2,c0,1 ; 32 bit TTBR1
mcrrne p15,0,r6,r7,c2 ; 64-bit TTBR0
ldmne r0!, {r6-r7}
mcrrne p15,1,r6,r7,c2 ; 64-bit TTBR1
ldm r0!, {r4-r7}
mcr p15,0,r4,c3,c0,0 ; DACR
mcr p15,0,r5,c7,c4,0 ; PAR
mcr p15,0,r6,c10,c2,0 ; PRRR
mcr p15,0,r7,c10,c2,1 ; NMRR
pop {r4, r5, r6, r7}
bx lr
ENDFUNC
save_mpu FUNCTION
mrc p15, 0, r1, c0, c0, 4 ; Read MPUIR
and r1, r1, #0xff00
lsr r1, r1, #8 ; Extract number of MPU regions
; Loop over the number of regions
10 cmp r1, #0
beq %f20
sub r1, r1, #1
mcr p15, 0, r1, c6, c2, 0 ; Write RGNR
mrc p15, 0, r2, c6, c1, 0 ; Read MPU Region Base Address Register
mrc p15, 0, r3, c6, c1, 2 ; Read Data MPU Region Size and Enable Register
mrc p15, 0, r12, c6, c1, 4 ; Read Region access control Register
stm r0!, {r2, r3, r12}
b %b10
20 bx lr
ENDFUNC
restore_mpu FUNCTION
mrc p15, 0, r1, c0, c0, 4 ; Read MPUIR
and r1, r1, #0xff00
lsr r1, r1, #8 ; Extract number of MPU regions
; Loop over the number of regions
10 cmp r1, #0
beq %f20
ldm r0!, {r2, r3, r12}
sub r1, r1, #1
mcr p15, 0, r1, c6, c2, 0 ; Write RGNR
mcr p15, 0, r2, c6, c1, 0 ; Write MPU Region Base Address Register
mcr p15, 0, r3, c6, c1, 2 ; Write Data MPU Region Size and Enable Register
mcr p15, 0, r12, c6, c1, 4 ; Write Region access control Register
b %b10
20 bx lr
ENDFUNC
save_vfp FUNCTION
; FPU state save/restore.
; FPSID,MVFR0 and MVFR1 don't get serialized/saved (Read Only).
mrc p15,0,r3,c1,c0,2 ; CPACR allows CP10 and CP11 access
ORR r2,r3,#0xF00000
mcr p15,0,r2,c1,c0,2
isb
mrc p15,0,r2,c1,c0,2
and r2,r2,#0xF00000
cmp r2,#0xF00000
beq %f0
movs r2, #0
b %f2
0 ; Save configuration registers and enable.
vmrs r12,FPEXC
str r12,[r0],#4 ; Save the FPEXC
; Enable FPU access to save/restore the other registers.
ldr r2,=0x40000000
vmsr FPEXC,r2
vmrs r2,FPSCR
str r2,[r0],#4 ; Save the FPSCR
; Store the VFP-D16 registers.
vstm r0!, {D0-D15}
; Check for Advanced SIMD/VFP-D32 support
vmrs r2,MVFR0
and r2,r2,#0xF ; extract the A_SIMD bitfield
cmp r2, #0x2
blt %f1
; Store the Advanced SIMD/VFP-D32 additional registers.
vstm r0!, {D16-D31}
; IMPLEMENTATION DEFINED: save any subarchitecture defined state
; NOTE: Don't change the order of the FPEXC and CPACR restores
1
vmsr FPEXC,r12 ; Restore the original En bit of FPU.
2
mcr p15,0,r3,c1,c0,2 ; Restore the original CPACR value.
bx lr
ENDFUNC
restore_vfp FUNCTION
; FPU state save/restore. Obviously FPSID,MVFR0 and MVFR1 don't get
; serialized (RO).
; Modify CPACR to allow CP10 and CP11 access
mrc p15,0,r1,c1,c0,2
ORR r2,r1,#0x00F00000
mcr p15,0,r2,c1,c0,2
; Enable FPU access to save/restore the rest of registers.
ldr r2,=0x40000000
vmsr FPEXC, r2
; Recover FPEXC and FPSCR. These will be restored later.
ldm r0!,{r3,r12}
; Restore the VFP-D16 registers.
vldm r0!, {D0-D15}
; Check for Advanced SIMD/VFP-D32 support
vmrs r2, MVFR0
and r2,r2,#0xF ; extract the A_SIMD bitfield
cmp r2, #0x2
blt %f0
; Store the Advanced SIMD/VFP-D32 additional registers.
vldm r0!, {D16-D31}
; IMPLEMENTATION DEFINED: restore any subarchitecture defined state
0 ; Restore configuration registers and enable.
; Restore FPSCR _before_ FPEXC since FPEXC could disable FPU
; and make setting FPSCR unpredictable.
vmsr FPSCR,r12
vmsr FPEXC,r3 ; Restore FPEXC after FPSCR
; Restore CPACR
mcr p15,0,r1,c1,c0,2
bx lr
ENDFUNC
; If r1 is 0, we assume that the OS is not using the Virtualization extensions,
; and that the warm boot code will set up CNTHCTL correctly. If r1 is non-zero
; then CNTHCTL is saved and restored
; CNTP_CVAL will be preserved as it is in the always-on domain.
save_generic_timer FUNCTION
mrc p15,0,r2,c14,c2,1 ; read CNTP_CTL
mrc p15,0,r3,c14,c2,0 ; read CNTP_TVAL
mrc p15,0,r12,c14,c1,0 ; read CNTKCTL
stm r0!, {r2, r3, r12}
cmp r1, #0
mrcne p15,4,r1,c14,c1,0 ; read CNTHCTL
strne r1, [r0], #4
bx lr
ENDFUNC
restore_generic_timer FUNCTION
ldm r0!, {r2, r3, r12}
mcr p15,0,r3,c14,c2,0 ; write CNTP_TVAL
mcr p15,0,r12,c14,c1,0 ; write CNTKCTL
mcr p15,0,r2,c14,c2,1 ; write CNTP_CTL
cmp r1, #0
ldrne r1, [r0], #4
mcrne p15,4,r1,c14,c1,0 ; write CNTHCTL
bx lr
ENDFUNC
save_fault_status FUNCTION
mrc p15,0,r1,c6,c0,0 ; read DFAR
mrc p15,0,r2,c6,c0,2 ; read IFAR
mrc p15,0,r3,c5,c0,0 ; read DFSR
mrc p15,0,r12,c5,c0,1 ; read IFSR
stm r0!, {r1,r2,r3,r12}
mrc p15,0,r1,c5,c1,0 ; read ADFSR
mrc p15,0,r2,c5,c1,1 ; read AIFSR
stm r0!, {r1,r2}
bx lr
ENDFUNC
restore_fault_status FUNCTION
ldm r0!, {r1,r2,r3,r12}
mcr p15,0,r1,c6,c0,0 ; write DFAR
mcr p15,0,r2,c6,c0,2 ; write IFAR
mcr p15,0,r3,c5,c0,0 ; write DFSR
mcr p15,0,r12,c5,c0,1 ; write IFSR
ldm r0!, {r1,r2}
mcr p15,0,r1,c5,c1,0 ; write ADFSR
mcr p15,0,r2,c5,c1,1 ; write AIFSR
bx lr
ENDFUNC
END
|