; ; Copyright (c) 2012, 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} 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,5 ; PMon: Event Counter Selection Register mrc p15,0,r10,c9,c12,1 ; PMon: Count Enable Set Reg stm r0!, {r8-r10} mrc p15,0,r8,c9,c12,2 ; PMon: Count Enable Clear Register mrc p15,0,r9,c9,c13,0 ; PMon: Cycle Counter Register mrc p15,0,r10,c9,c12,3 ; PMon: Overflow flag Status Register stm r0!, {r8-r10} mrc p15,0,r8,c9,c14,1 ; PMon: Interrupt Enable Set Registern mrc p15,0,r9,c9,c14,2 ; PMon: Interrupt Enable Clear Register stm r0!, {r8-r9} mrc p15,0,r8,c9,c12,0 ; Read PMon Control Register ubfx r9,r8,#11,#5 ; extract # of event counters, N tst r9, r9 beq %f1 mov r8,#0 0 mcr p15,0,r8,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} add r8,r8,#1 ; increment index cmps r8,r9 bne %b0 1 pop {r4, r8, r9, r10} bx lr ENDFUNC restore_performance_monitors FUNCTION push {r4-r6, r8-r10, lr} ; NOTE: all counters disabled by PMCR<0> == 0 on reset ldr r8,[r0] ; r8 = PMCR add r1,r0,#20 ; r1 now points to saved PMOVSR ldr r9,[r1] ; r9 = PMOVSR 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 add r1,r0,#32 ; r1 now points to the 1st saved event counter ;; Restore counters mov r6,#0 10 mcr p15,0,r6,c9,c12,5 ; PMon: select CounterN isb ldm r1!, {r3,r4} ; Read saved data mcr p15,0,r3,c9,c13,1 ; PMon: restore Event Type Register mcr p15,0,r4,c9,c13,2 ; PMon: restore Event Counter Register add r6,r6,#1 ; increment index cmps r6,r12 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, #1 mcr p15,0,r3,c9,c12,0 ; set the PMCR global enable bit 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 mov r1, #0 mcr p15,0,r1,c9,c12,0 ; clear the PMCR global enable bit isb ;; Restore left regs but PMCR add r1,r0,#4 ; r1 now points to the PMSELR ldm r1!,{r3,r4} mcr p15,0,r3,c9,c12,5 ; PMon: Event Counter Selection Reg mcr p15,0,r4,c9,c12,1 ; PMon: Count Enable Set Reg ldm r1!, {r3,r4} mcr p15,0,r4,c9,c13,0 ; PMon: Cycle Counter Register ldm r1!,{r3,r4} mcr p15,0,r3,c9,c14,2 ; PMon: Interrupt Enable Clear Reg mcr p15,0,r4,c9,c14,1 ; PMon: Interrupt Enable Set Reg ldr r3,[r1] isb ldr r0,[r0] mcr p15,0,r0,c9,c12,0 ; restore the PM Control Register isb pop {r4-r6, 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