; ; 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. ; /* * gas does not handle all vmsr/vmrs forms correctly, so use the obsolete * fmrx/rmxr mnemonics instead (these are strictly not correct though, and * are not accepted by armasm). */ #if !BUILD_RVCT #define vmrs fmrx #define vmsr fmxr #endif 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 |.text.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 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