DEFINE(NVHE_INIT_HCR_EL2,    offsetof(struct kvm_nvhe_init_params, hcr_el2));
   DEFINE(NVHE_INIT_VTTBR,      offsetof(struct kvm_nvhe_init_params, vttbr));
   DEFINE(NVHE_INIT_VTCR,       offsetof(struct kvm_nvhe_init_params, vtcr));
+  DEFINE(NVHE_INIT_TMP,                offsetof(struct kvm_nvhe_init_params, tmp));
 #endif
 #ifdef CONFIG_CPU_PM
   DEFINE(CPU_CTX_SP,           offsetof(struct cpu_suspend_ctx, sp));
 
        .align  11
 
 SYM_CODE_START(__kvm_hyp_init)
-       ventry  __invalid               // Synchronous EL2t
-       ventry  __invalid               // IRQ EL2t
-       ventry  __invalid               // FIQ EL2t
-       ventry  __invalid               // Error EL2t
+       ventry  .                       // Synchronous EL2t
+       ventry  .                       // IRQ EL2t
+       ventry  .                       // FIQ EL2t
+       ventry  .                       // Error EL2t
 
-       ventry  __invalid               // Synchronous EL2h
-       ventry  __invalid               // IRQ EL2h
-       ventry  __invalid               // FIQ EL2h
-       ventry  __invalid               // Error EL2h
+       ventry  .                       // Synchronous EL2h
+       ventry  .                       // IRQ EL2h
+       ventry  .                       // FIQ EL2h
+       ventry  .                       // Error EL2h
 
        ventry  __do_hyp_init           // Synchronous 64-bit EL1
-       ventry  __invalid               // IRQ 64-bit EL1
-       ventry  __invalid               // FIQ 64-bit EL1
-       ventry  __invalid               // Error 64-bit EL1
+       ventry  .                       // IRQ 64-bit EL1
+       ventry  .                       // FIQ 64-bit EL1
+       ventry  .                       // Error 64-bit EL1
 
-       ventry  __invalid               // Synchronous 32-bit EL1
-       ventry  __invalid               // IRQ 32-bit EL1
-       ventry  __invalid               // FIQ 32-bit EL1
-       ventry  __invalid               // Error 32-bit EL1
-
-__invalid:
-       b       .
+       ventry  .                       // Synchronous 32-bit EL1
+       ventry  .                       // IRQ 32-bit EL1
+       ventry  .                       // FIQ 32-bit EL1
+       ventry  .                       // Error 32-bit EL1
 
        /*
         * Only uses x0..x3 so as to not clobber callee-saved SMCCC registers.
        eret
 SYM_CODE_END(__kvm_hyp_init)
 
+SYM_CODE_START_LOCAL(__kvm_init_el2_state)
+       /* Initialize EL2 CPU state to sane values. */
+       init_el2_state                          // Clobbers x0..x2
+       finalise_el2_state
+       ret
+SYM_CODE_END(__kvm_init_el2_state)
+
 /*
  * Initialize the hypervisor in EL2.
  *
        // TPIDR_EL2 is used to preserve x0 across the macro maze...
        isb
        msr     tpidr_el2, x0
-       init_el2_state
-       finalise_el2_state
+       str     lr, [x0, #NVHE_INIT_TMP]
+
+       bl      __kvm_init_el2_state
+
        mrs     x0, tpidr_el2
+       ldr     lr, [x0, #NVHE_INIT_TMP]
 
 1:
        ldr     x1, [x0, #NVHE_INIT_TPIDR_EL2]
 
 2:     msr     SPsel, #1                       // We want to use SP_EL{1,2}
 
-       /* Initialize EL2 CPU state to sane values. */
-       init_el2_state                          // Clobbers x0..x2
-       finalise_el2_state
+       bl      __kvm_init_el2_state
+
        __init_el2_nvhe_prepare_eret
 
        /* Enable MMU, set vectors and stack. */