ARM: 8985/1: efi/decompressor: deal with HYP mode boot gracefully

[linux-block.git] / arch / arm / boot / compressed / head.S

diff --git a/arch/arm/boot/compressed/head.S b/arch/arm/boot/compressed/head.S
index c79db44ba1284bdcdbcfb188d301ef5144e4c43c..434a16982e344fe43d62f45b14bdf9ab4c780800 100644 (file)
--- a/arch/arm/boot/compressed/head.S
+++ b/arch/arm/boot/compressed/head.S
@@ -1410,7 +1410,11 @@ memdump: mov     r12, r0
 __hyp_reentry_vectors:
                W(b)    .                       @ reset
                W(b)    .                       @ undef
+#ifdef CONFIG_EFI_STUB
+               W(b)    __enter_kernel_from_hyp @ hvc from HYP
+#else
                W(b)    .                       @ svc
+#endif
                W(b)    .                       @ pabort
                W(b)    .                       @ dabort
                W(b)    __enter_kernel          @ hyp
@@ -1429,14 +1433,72 @@ __enter_kernel:
 reloc_code_end:
 
 #ifdef CONFIG_EFI_STUB
+__enter_kernel_from_hyp:
+               mrc     p15, 4, r0, c1, c0, 0   @ read HSCTLR
+               bic     r0, r0, #0x5            @ disable MMU and caches
+               mcr     p15, 4, r0, c1, c0, 0   @ write HSCTLR
+               isb
+               b       __enter_kernel
+
 ENTRY(efi_enter_kernel)
                mov     r4, r0                  @ preserve image base
                mov     r8, r1                  @ preserve DT pointer
 
+ ARM(          adrl    r0, call_cache_fn       )
+ THUMB(                adr     r0, call_cache_fn       )
+               adr     r1, 0f                  @ clean the region of code we
+               bl      cache_clean_flush       @ may run with the MMU off
+
+#ifdef CONFIG_ARM_VIRT_EXT
+               @
+               @ The EFI spec does not support booting on ARM in HYP mode,
+               @ since it mandates that the MMU and caches are on, with all
+               @ 32-bit addressable DRAM mapped 1:1 using short descriptors.
+               @
+               @ While the EDK2 reference implementation adheres to this,
+               @ U-Boot might decide to enter the EFI stub in HYP mode
+               @ anyway, with the MMU and caches either on or off.
+               @
+               mrs     r0, cpsr                @ get the current mode
+               msr     spsr_cxsf, r0           @ record boot mode
+               and     r0, r0, #MODE_MASK      @ are we running in HYP mode?
+               cmp     r0, #HYP_MODE
+               bne     .Lefi_svc
+
+               mrc     p15, 4, r1, c1, c0, 0   @ read HSCTLR
+               tst     r1, #0x1                @ MMU enabled at HYP?
+               beq     1f
+
+               @
+               @ When running in HYP mode with the caches on, we're better
+               @ off just carrying on using the cached 1:1 mapping that the
+               @ firmware provided. Set up the HYP vectors so HVC instructions
+               @ issued from HYP mode take us to the correct handler code. We
+               @ will disable the MMU before jumping to the kernel proper.
+               @
+               adr     r0, __hyp_reentry_vectors
+               mcr     p15, 4, r0, c12, c0, 0  @ set HYP vector base (HVBAR)
+               isb
+               b       .Lefi_hyp
+
+               @
+               @ When running in HYP mode with the caches off, we need to drop
+               @ into SVC mode now, and let the decompressor set up its cached
+               @ 1:1 mapping as usual.
+               @
+1:             mov     r9, r4                  @ preserve image base
+               bl      __hyp_stub_install      @ install HYP stub vectors
+               safe_svcmode_maskall    r1      @ drop to SVC mode
+               msr     spsr_cxsf, r0           @ record boot mode
+               orr     r4, r9, #1              @ restore image base and set LSB
+               b       .Lefi_hyp
+.Lefi_svc:
+#endif
                mrc     p15, 0, r0, c1, c0, 0   @ read SCTLR
                tst     r0, #0x1                @ MMU enabled?
                orreq   r4, r4, #1              @ set LSB if not
 
+.Lefi_hyp:
                mov     r0, r8                  @ DT start
                add     r1, r8, r2              @ DT end
                bl      cache_clean_flush