diff options
| -rw-r--r-- | common/aarch32/debug.S | 16 | ||||
| -rw-r--r-- | include/common/aarch32/asm_macros.S | 10 | ||||
| -rw-r--r-- | include/lib/aarch32/smcc_macros.S | 80 | ||||
| -rw-r--r-- | make_helpers/armv7-a-cpus.mk | 4 |
4 files changed, 109 insertions, 1 deletions
diff --git a/common/aarch32/debug.S b/common/aarch32/debug.S index 583ee4a5..f5063569 100644 --- a/common/aarch32/debug.S +++ b/common/aarch32/debug.S @@ -71,7 +71,15 @@ endfunc report_exception assert_msg1: .asciz "ASSERT: File " assert_msg2: +#if ARM_ARCH_MAJOR == 7 && !defined(ARMV7_SUPPORTS_VIRTUALIZATION) + /****************************************************************** + * Virtualization comes with the UDIV/SDIV instructions. If missing + * write file line number in hexadecimal format. + ******************************************************************/ + .asciz " Line 0x" +#else .asciz " Line " +#endif /* --------------------------------------------------------------------------- * Assertion support in assembly. @@ -113,6 +121,13 @@ func asm_assert bne 1f mov r4, r6 +#if ARM_ARCH_MAJOR == 7 && !defined(ARMV7_SUPPORTS_VIRTUALIZATION) + /****************************************************************** + * Virtualization comes with the UDIV/SDIV instructions. If missing + * write file line number in hexadecimal format. + ******************************************************************/ + bl asm_print_hex +#else /* Print line number in decimal */ mov r6, #10 /* Divide by 10 after every loop iteration */ ldr r5, =MAX_DEC_DIVISOR @@ -124,6 +139,7 @@ dec_print_loop: udiv r5, r5, r6 /* Reduce divisor */ cmp r5, #0 bne dec_print_loop +#endif bl plat_crash_console_flush diff --git a/include/common/aarch32/asm_macros.S b/include/common/aarch32/asm_macros.S index 0d1a37d1..74322228 100644 --- a/include/common/aarch32/asm_macros.S +++ b/include/common/aarch32/asm_macros.S @@ -79,6 +79,16 @@ ldr r0, =(\_name + \_size) .endm +#if ARM_ARCH_MAJOR == 7 && !defined(ARMV7_SUPPORTS_VIRTUALIZATION) + /* + * ARMv7 cores without Virtualization extension do not support the + * eret instruction. + */ + .macro eret + movs pc, lr + .endm +#endif + #if (ARM_ARCH_MAJOR == 7) /* ARMv7 does not support stl instruction */ .macro stl _reg, _write_lock diff --git a/include/lib/aarch32/smcc_macros.S b/include/lib/aarch32/smcc_macros.S index cf26175d..93f211f7 100644 --- a/include/lib/aarch32/smcc_macros.S +++ b/include/lib/aarch32/smcc_macros.S @@ -22,6 +22,44 @@ mov r0, sp add r0, r0, #SMC_CTX_SP_USR +#if ARM_ARCH_MAJOR == 7 && !defined(ARMV7_SUPPORTS_VIRTUALIZATION) + /* Must be in secure state to restore Monitor mode */ + ldcopr r4, SCR + bic r2, r4, #SCR_NS_BIT + stcopr r2, SCR + isb + + cps #MODE32_sys + stm r0!, {sp, lr} + + cps #MODE32_irq + mrs r2, spsr + stm r0!, {r2, sp, lr} + + cps #MODE32_fiq + mrs r2, spsr + stm r0!, {r2, sp, lr} + + cps #MODE32_svc + mrs r2, spsr + stm r0!, {r2, sp, lr} + + cps #MODE32_abt + mrs r2, spsr + stm r0!, {r2, sp, lr} + + cps #MODE32_und + mrs r2, spsr + stm r0!, {r2, sp, lr} + + /* lr_mon is already saved by caller */ + cps #MODE32_mon + mrs r2, spsr + stm r0!, {r2} + + stcopr r4, SCR + isb +#else /* Save the banked registers including the current SPSR and LR */ mrs r4, sp_usr mrs r5, lr_usr @@ -44,9 +82,10 @@ mrs r11, lr_und mrs r12, spsr stm r0!, {r4-r12} - /* lr_mon is already saved by caller */ + ldcopr r4, SCR +#endif str r4, [sp, #SMC_CTX_SCR] ldcopr r4, PMCR str r4, [sp, #SMC_CTX_PMCR] @@ -82,6 +121,44 @@ /* Restore the banked registers including the current SPSR */ add r1, r0, #SMC_CTX_SP_USR + +#if ARM_ARCH_MAJOR == 7 && !defined(ARMV7_SUPPORTS_VIRTUALIZATION) + /* Must be in secure state to restore Monitor mode */ + ldcopr r4, SCR + bic r2, r4, #SCR_NS_BIT + stcopr r2, SCR + isb + + cps #MODE32_sys + ldm r1!, {sp, lr} + + cps #MODE32_irq + ldm r1!, {r2, sp, lr} + msr spsr_fsxc, r2 + + cps #MODE32_fiq + ldm r1!, {r2, sp, lr} + msr spsr_fsxc, r2 + + cps #MODE32_svc + ldm r1!, {r2, sp, lr} + msr spsr_fsxc, r2 + + cps #MODE32_abt + ldm r1!, {r2, sp, lr} + msr spsr_fsxc, r2 + + cps #MODE32_und + ldm r1!, {r2, sp, lr} + msr spsr_fsxc, r2 + + cps #MODE32_mon + ldm r1!, {r2} + msr spsr_fsxc, r2 + + stcopr r4, SCR + isb +#else ldm r1!, {r4-r12} msr sp_usr, r4 msr lr_usr, r5 @@ -109,6 +186,7 @@ * f->[31:24] and c->[7:0] bits of SPSR. */ msr spsr_fsxc, r12 +#endif /* Restore the LR */ ldr lr, [r0, #SMC_CTX_LR_MON] diff --git a/make_helpers/armv7-a-cpus.mk b/make_helpers/armv7-a-cpus.mk index c6491aa8..120b36c7 100644 --- a/make_helpers/armv7-a-cpus.mk +++ b/make_helpers/armv7-a-cpus.mk @@ -36,7 +36,11 @@ endif # # ARMV7_SUPPORTS_LARGE_PAGE_ADDRESSING # Defined if core supports the Large Page Addressing extension. +# +# ARMV7_SUPPORTS_VIRTUALIZATION +# Defined if ARMv7 core supports the Virtualization extension. ifeq ($(filter yes,$(ARM_CORTEX_A7) $(ARM_CORTEX_A12) $(ARM_CORTEX_A15) $(ARM_CORTEX_A17)),yes) $(eval $(call add_define,ARMV7_SUPPORTS_LARGE_PAGE_ADDRESSING)) +$(eval $(call add_define,ARMV7_SUPPORTS_VIRTUALIZATION)) endif |