1 files changed, 59 insertions, 19 deletions

diff --git a/arch/arm/kvm/init.S b/arch/arm/kvm/init.S
index 9f37a79b880b..f048338135f7 100644
--- a/arch/arm/kvm/init.S
+++ b/arch/arm/kvm/init.S
@@ -21,13 +21,33 @@
 #include <asm/asm-offsets.h>
 #include <asm/kvm_asm.h>
 #include <asm/kvm_arm.h>
+#include <asm/kvm_mmu.h>
 
 /********************************************************************
  * Hypervisor initialization
  *   - should be called with:
- *       r0,r1 = Hypervisor pgd pointer
- *       r2 = top of Hyp stack (kernel VA)
- *       r3 = pointer to hyp vectors
+ *       r0 = top of Hyp stack (kernel VA)
+ *       r1 = pointer to hyp vectors
+ *       r2,r3 = Hypervisor pgd pointer
+ *
+ * The init scenario is:
+ * - We jump in HYP with four parameters: boot HYP pgd, runtime HYP pgd,
+ *   runtime stack, runtime vectors
+ * - Enable the MMU with the boot pgd
+ * - Jump to a target into the trampoline page (remember, this is the same
+ *   physical page!)
+ * - Now switch to the runtime pgd (same VA, and still the same physical
+ *   page!)
+ * - Invalidate TLBs
+ * - Set stack and vectors
+ * - Profit! (or eret, if you only care about the code).
+ *
+ * As we only have four registers available to pass parameters (and we
+ * need six), we split the init in two phases:
+ * - Phase 1: r0 = 0, r1 = 0, r2,r3 contain the boot PGD.
+ *   Provides the basic HYP init, and enable the MMU.
+ * - Phase 2: r0 = ToS, r1 = vectors, r2,r3 contain the runtime PGD.
+ *   Switches to the runtime PGD, set stack and vectors.
  */
 
         .text
@@ -47,22 +67,25 @@ __kvm_hyp_init:
         W(b)    .
 
 __do_hyp_init:
+        cmp     r0, #0                  @ We have a SP?
+        bne     phase2                  @ Yes, second stage init
+
         @ Set the HTTBR to point to the hypervisor PGD pointer passed
-        mcrr    p15, 4, r0, r1, c2
+        mcrr    p15, 4, r2, r3, c2
 
         @ Set the HTCR and VTCR to the same shareability and cacheability
         @ settings as the non-secure TTBCR and with T0SZ == 0.
         mrc     p15, 4, r0, c2, c0, 2   @ HTCR
-        ldr     r12, =HTCR_MASK
-        bic     r0, r0, r12
+        ldr     r2, =HTCR_MASK
+        bic     r0, r0, r2
         mrc     p15, 0, r1, c2, c0, 2   @ TTBCR
         and     r1, r1, #(HTCR_MASK & ~TTBCR_T0SZ)
         orr     r0, r0, r1
         mcr     p15, 4, r0, c2, c0, 2   @ HTCR
 
         mrc     p15, 4, r1, c2, c1, 2   @ VTCR
-        ldr     r12, =VTCR_MASK
-        bic     r1, r1, r12
+        ldr     r2, =VTCR_MASK
+        bic     r1, r1, r2
         bic     r0, r0, #(~VTCR_HTCR_SH)        @ clear non-reusable HTCR bits
         orr     r1, r0, r1
         orr     r1, r1, #(KVM_VTCR_SL0 | KVM_VTCR_T0SZ | KVM_VTCR_S)
@@ -85,24 +108,41 @@ __do_hyp_init:
         @  - Memory alignment checks: enabled
         @  - MMU: enabled (this code must be run from an identity mapping)
         mrc     p15, 4, r0, c1, c0, 0   @ HSCR
-        ldr     r12, =HSCTLR_MASK
-        bic     r0, r0, r12
+        ldr     r2, =HSCTLR_MASK
+        bic     r0, r0, r2
         mrc     p15, 0, r1, c1, c0, 0   @ SCTLR
-        ldr     r12, =(HSCTLR_EE | HSCTLR_FI | HSCTLR_I | HSCTLR_C)
-        and     r1, r1, r12
- ARM(   ldr     r12, =(HSCTLR_M | HSCTLR_A)                     )
- THUMB( ldr     r12, =(HSCTLR_M | HSCTLR_A | HSCTLR_TE)         )
-        orr     r1, r1, r12
+        ldr     r2, =(HSCTLR_EE | HSCTLR_FI | HSCTLR_I | HSCTLR_C)
+        and     r1, r1, r2
+ ARM(   ldr     r2, =(HSCTLR_M | HSCTLR_A)                      )
+ THUMB( ldr     r2, =(HSCTLR_M | HSCTLR_A | HSCTLR_TE)          )
+        orr     r1, r1, r2
         orr     r0, r0, r1
         isb
         mcr     p15, 4, r0, c1, c0, 0   @ HSCR
-        isb
 
-        @ Set stack pointer and return to the kernel
-        mov     sp, r2
+        @ End of init phase-1
+        eret
+
+phase2:
+        @ Set stack pointer
+        mov     sp, r0
 
         @ Set HVBAR to point to the HYP vectors
-        mcr     p15, 4, r3, c12, c0, 0  @ HVBAR
+        mcr     p15, 4, r1, c12, c0, 0  @ HVBAR
+
+        @ Jump to the trampoline page
+        ldr     r0, =TRAMPOLINE_VA
+        adr     r1, target
+        bfi     r0, r1, #0, #PAGE_SHIFT
+        mov     pc, r0
+
+target: @ We're now in the trampoline code, switch page tables
+        mcrr    p15, 4, r2, r3, c2
+        isb
+
+        @ Invalidate the old TLBs
+        mcr     p15, 4, r0, c8, c7, 0   @ TLBIALLH
+        dsb
 
         eret