4 files changed, 577 insertions, 0 deletions

diff --git a/arch/arm/kvm/arm.c b/arch/arm/kvm/arm.c
index d3506b4001aa..2c6b780e78a7 100644
--- a/arch/arm/kvm/arm.c
+++ b/arch/arm/kvm/arm.c
@@ -34,11 +34,21 @@
 #include <asm/ptrace.h>
 #include <asm/mman.h>
 #include <asm/cputype.h>
+#include <asm/tlbflush.h>
+#include <asm/virt.h>
+#include <asm/kvm_arm.h>
+#include <asm/kvm_asm.h>
+#include <asm/kvm_mmu.h>
 
 #ifdef REQUIRES_VIRT
 __asm__(".arch_extension        virt");
 #endif
 
+static DEFINE_PER_CPU(unsigned long, kvm_arm_hyp_stack_page);
+static struct vfp_hard_struct __percpu *kvm_host_vfp_state;
+static unsigned long hyp_default_vectors;
+
+
 int kvm_arch_hardware_enable(void *garbage)
 {
         return 0;
@@ -331,9 +341,171 @@ long kvm_arch_vm_ioctl(struct file *filp,
         return -EINVAL;
 }
 
+static void cpu_init_hyp_mode(void *vector)
+{
+        unsigned long long pgd_ptr;
+        unsigned long pgd_low, pgd_high;
+        unsigned long hyp_stack_ptr;
+        unsigned long stack_page;
+        unsigned long vector_ptr;
+
+        /* Switch from the HYP stub to our own HYP init vector */
+        __hyp_set_vectors((unsigned long)vector);
+
+        pgd_ptr = (unsigned long long)kvm_mmu_get_httbr();
+        pgd_low = (pgd_ptr & ((1ULL << 32) - 1));
+        pgd_high = (pgd_ptr >> 32ULL);
+        stack_page = __get_cpu_var(kvm_arm_hyp_stack_page);
+        hyp_stack_ptr = stack_page + PAGE_SIZE;
+        vector_ptr = (unsigned long)__kvm_hyp_vector;
+
+        /*
+         * Call initialization code, and switch to the full blown
+         * HYP code. The init code doesn't need to preserve these registers as
+         * r1-r3 and r12 are already callee save according to the AAPCS.
+         * Note that we slightly misuse the prototype by casing the pgd_low to
+         * a void *.
+         */
+        kvm_call_hyp((void *)pgd_low, pgd_high, hyp_stack_ptr, vector_ptr);
+}
+
+/**
+ * Inits Hyp-mode on all online CPUs
+ */
+static int init_hyp_mode(void)
+{
+        phys_addr_t init_phys_addr;
+        int cpu;
+        int err = 0;
+
+        /*
+         * Allocate Hyp PGD and setup Hyp identity mapping
+         */
+        err = kvm_mmu_init();
+        if (err)
+                goto out_err;
+
+        /*
+         * It is probably enough to obtain the default on one
+         * CPU. It's unlikely to be different on the others.
+         */
+        hyp_default_vectors = __hyp_get_vectors();
+
+        /*
+         * Allocate stack pages for Hypervisor-mode
+         */
+        for_each_possible_cpu(cpu) {
+                unsigned long stack_page;
+
+                stack_page = __get_free_page(GFP_KERNEL);
+                if (!stack_page) {
+                        err = -ENOMEM;
+                        goto out_free_stack_pages;
+                }
+
+                per_cpu(kvm_arm_hyp_stack_page, cpu) = stack_page;
+        }
+
+        /*
+         * Execute the init code on each CPU.
+         *
+         * Note: The stack is not mapped yet, so don't do anything else than
+         * initializing the hypervisor mode on each CPU using a local stack
+         * space for temporary storage.
+         */
+        init_phys_addr = virt_to_phys(__kvm_hyp_init);
+        for_each_online_cpu(cpu) {
+                smp_call_function_single(cpu, cpu_init_hyp_mode,
+                                         (void *)(long)init_phys_addr, 1);
+        }
+
+        /*
+         * Unmap the identity mapping
+         */
+        kvm_clear_hyp_idmap();
+
+        /*
+         * Map the Hyp-code called directly from the host
+         */
+        err = create_hyp_mappings(__kvm_hyp_code_start, __kvm_hyp_code_end);
+        if (err) {
+                kvm_err("Cannot map world-switch code\n");
+                goto out_free_mappings;
+        }
+
+        /*
+         * Map the Hyp stack pages
+         */
+        for_each_possible_cpu(cpu) {
+                char *stack_page = (char *)per_cpu(kvm_arm_hyp_stack_page, cpu);
+                err = create_hyp_mappings(stack_page, stack_page + PAGE_SIZE);
+
+                if (err) {
+                        kvm_err("Cannot map hyp stack\n");
+                        goto out_free_mappings;
+                }
+        }
+
+        /*
+         * Map the host VFP structures
+         */
+        kvm_host_vfp_state = alloc_percpu(struct vfp_hard_struct);
+        if (!kvm_host_vfp_state) {
+                err = -ENOMEM;
+                kvm_err("Cannot allocate host VFP state\n");
+                goto out_free_mappings;
+        }
+
+        for_each_possible_cpu(cpu) {
+                struct vfp_hard_struct *vfp;
+
+                vfp = per_cpu_ptr(kvm_host_vfp_state, cpu);
+                err = create_hyp_mappings(vfp, vfp + 1);
+
+                if (err) {
+                        kvm_err("Cannot map host VFP state: %d\n", err);
+                        goto out_free_vfp;
+                }
+        }
+
+        kvm_info("Hyp mode initialized successfully\n");
+        return 0;
+out_free_vfp:
+        free_percpu(kvm_host_vfp_state);
+out_free_mappings:
+        free_hyp_pmds();
+out_free_stack_pages:
+        for_each_possible_cpu(cpu)
+                free_page(per_cpu(kvm_arm_hyp_stack_page, cpu));
+out_err:
+        kvm_err("error initializing Hyp mode: %d\n", err);
+        return err;
+}
+
+/**
+ * Initialize Hyp-mode and memory mappings on all CPUs.
+ */
 int kvm_arch_init(void *opaque)
 {
+        int err;
+
+        if (!is_hyp_mode_available()) {
+                kvm_err("HYP mode not available\n");
+                return -ENODEV;
+        }
+
+        if (kvm_target_cpu() < 0) {
+                kvm_err("Target CPU not supported!\n");
+                return -ENODEV;
+        }
+
+        err = init_hyp_mode();
+        if (err)
+                goto out_err;
+
         return 0;
+out_err:
+        return err;
 }
 
 /* NOP: Compiling as a module not supported */
diff --git a/arch/arm/kvm/init.S b/arch/arm/kvm/init.S
index 1dc8926e26d2..9f37a79b880b 100644
--- a/arch/arm/kvm/init.S
+++ b/arch/arm/kvm/init.S
@@ -15,5 +15,100 @@
  * along with this program; if not, write to the Free Software
  * Foundation, 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  */
+
+#include <linux/linkage.h>
+#include <asm/unified.h>
 #include <asm/asm-offsets.h>
 #include <asm/kvm_asm.h>
+#include <asm/kvm_arm.h>
+
+/********************************************************************
+ * Hypervisor initialization
+ *   - should be called with:
+ *       r0,r1 = Hypervisor pgd pointer
+ *       r2 = top of Hyp stack (kernel VA)
+ *       r3 = pointer to hyp vectors
+ */
+
+        .text
+        .pushsection    .hyp.idmap.text,"ax"
+        .align 5
+__kvm_hyp_init:
+        .globl __kvm_hyp_init
+
+        @ Hyp-mode exception vector
+        W(b)    .
+        W(b)    .
+        W(b)    .
+        W(b)    .
+        W(b)    .
+        W(b)    __do_hyp_init
+        W(b)    .
+        W(b)    .
+
+__do_hyp_init:
+        @ Set the HTTBR to point to the hypervisor PGD pointer passed
+        mcrr    p15, 4, r0, r1, 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
+        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
+        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)
+        mcr     p15, 4, r1, c2, c1, 2   @ VTCR
+
+        @ Use the same memory attributes for hyp. accesses as the kernel
+        @ (copy MAIRx ro HMAIRx).
+        mrc     p15, 0, r0, c10, c2, 0
+        mcr     p15, 4, r0, c10, c2, 0
+        mrc     p15, 0, r0, c10, c2, 1
+        mcr     p15, 4, r0, c10, c2, 1
+
+        @ Set the HSCTLR to:
+        @  - ARM/THUMB exceptions: Kernel config (Thumb-2 kernel)
+        @  - Endianness: Kernel config
+        @  - Fast Interrupt Features: Kernel config
+        @  - Write permission implies XN: disabled
+        @  - Instruction cache: enabled
+        @  - Data/Unified cache: enabled
+        @  - 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
+        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
+        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
+
+        @ Set HVBAR to point to the HYP vectors
+        mcr     p15, 4, r3, c12, c0, 0  @ HVBAR
+
+        eret
+
+        .ltorg
+
+        .globl __kvm_hyp_init_end
+__kvm_hyp_init_end:
+
+        .popsection
diff --git a/arch/arm/kvm/interrupts.S b/arch/arm/kvm/interrupts.S
index 1dc8926e26d2..d10a8075409a 100644
--- a/arch/arm/kvm/interrupts.S
+++ b/arch/arm/kvm/interrupts.S
@@ -15,5 +15,67 @@
  * along with this program; if not, write to the Free Software
  * Foundation, 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  */
+
+#include <linux/linkage.h>
+#include <linux/const.h>
+#include <asm/unified.h>
+#include <asm/page.h>
 #include <asm/asm-offsets.h>
 #include <asm/kvm_asm.h>
+#include <asm/kvm_arm.h>
+
+        .text
+
+__kvm_hyp_code_start:
+        .globl __kvm_hyp_code_start
+
+/********************************************************************
+ * Flush per-VMID TLBs
+ */
+ENTRY(__kvm_flush_vm_context)
+        bx      lr
+ENDPROC(__kvm_flush_vm_context)
+
+/********************************************************************
+ *  Hypervisor world-switch code
+ */
+ENTRY(__kvm_vcpu_run)
+        bx      lr
+
+/********************************************************************
+ *  Call function in Hyp mode
+ *
+ *
+ * u64 kvm_call_hyp(void *hypfn, ...);
+ *
+ * This is not really a variadic function in the classic C-way and care must
+ * be taken when calling this to ensure parameters are passed in registers
+ * only, since the stack will change between the caller and the callee.
+ *
+ * Call the function with the first argument containing a pointer to the
+ * function you wish to call in Hyp mode, and subsequent arguments will be
+ * passed as r0, r1, and r2 (a maximum of 3 arguments in addition to the
+ * function pointer can be passed).  The function being called must be mapped
+ * in Hyp mode (see init_hyp_mode in arch/arm/kvm/arm.c).  Return values are
+ * passed in r0 and r1.
+ *
+ * The calling convention follows the standard AAPCS:
+ *   r0 - r3: caller save
+ *   r12:     caller save
+ *   rest:    callee save
+ */
+ENTRY(kvm_call_hyp)
+        hvc     #0
+        bx      lr
+
+/********************************************************************
+ * Hypervisor exception vector and handlers
+ */
+
+        .align 5
+__kvm_hyp_vector:
+        .globl __kvm_hyp_vector
+        nop
+
+__kvm_hyp_code_end:
+        .globl  __kvm_hyp_code_end
diff --git a/arch/arm/kvm/mmu.c b/arch/arm/kvm/mmu.c
index 10ed4643269f..4decdb618019 100644
--- a/arch/arm/kvm/mmu.c
+++ b/arch/arm/kvm/mmu.c
@@ -15,3 +15,251 @@
  * along with this program; if not, write to the Free Software
  * Foundation, 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  */
+
+#include <linux/mman.h>
+#include <linux/kvm_host.h>
+#include <linux/io.h>
+#include <asm/idmap.h>
+#include <asm/pgalloc.h>
+#include <asm/kvm_arm.h>
+#include <asm/kvm_mmu.h>
+#include <asm/mach/map.h>
+
+extern char  __hyp_idmap_text_start[], __hyp_idmap_text_end[];
+
+static DEFINE_MUTEX(kvm_hyp_pgd_mutex);
+
+static void kvm_set_pte(pte_t *pte, pte_t new_pte)
+{
+        pte_val(*pte) = new_pte;
+        /*
+         * flush_pmd_entry just takes a void pointer and cleans the necessary
+         * cache entries, so we can reuse the function for ptes.
+         */
+        flush_pmd_entry(pte);
+}
+
+static void free_ptes(pmd_t *pmd, unsigned long addr)
+{
+        pte_t *pte;
+        unsigned int i;
+
+        for (i = 0; i < PTRS_PER_PMD; i++, addr += PMD_SIZE) {
+                if (!pmd_none(*pmd) && pmd_table(*pmd)) {
+                        pte = pte_offset_kernel(pmd, addr);
+                        pte_free_kernel(NULL, pte);
+                }
+                pmd++;
+        }
+}
+
+/**
+ * free_hyp_pmds - free a Hyp-mode level-2 tables and child level-3 tables
+ *
+ * Assumes this is a page table used strictly in Hyp-mode and therefore contains
+ * only mappings in the kernel memory area, which is above PAGE_OFFSET.
+ */
+void free_hyp_pmds(void)
+{
+        pgd_t *pgd;
+        pud_t *pud;
+        pmd_t *pmd;
+        unsigned long addr;
+
+        mutex_lock(&kvm_hyp_pgd_mutex);
+        for (addr = PAGE_OFFSET; addr != 0; addr += PGDIR_SIZE) {
+                pgd = hyp_pgd + pgd_index(addr);
+                pud = pud_offset(pgd, addr);
+
+                if (pud_none(*pud))
+                        continue;
+                BUG_ON(pud_bad(*pud));
+
+                pmd = pmd_offset(pud, addr);
+                free_ptes(pmd, addr);
+                pmd_free(NULL, pmd);
+                pud_clear(pud);
+        }
+        mutex_unlock(&kvm_hyp_pgd_mutex);
+}
+
+static void create_hyp_pte_mappings(pmd_t *pmd, unsigned long start,
+                                    unsigned long end)
+{
+        pte_t *pte;
+        unsigned long addr;
+        struct page *page;
+
+        for (addr = start & PAGE_MASK; addr < end; addr += PAGE_SIZE) {
+                pte = pte_offset_kernel(pmd, addr);
+                BUG_ON(!virt_addr_valid(addr));
+                page = virt_to_page(addr);
+                kvm_set_pte(pte, mk_pte(page, PAGE_HYP));
+        }
+}
+
+static void create_hyp_io_pte_mappings(pmd_t *pmd, unsigned long start,
+                                       unsigned long end,
+                                       unsigned long *pfn_base)
+{
+        pte_t *pte;
+        unsigned long addr;
+
+        for (addr = start & PAGE_MASK; addr < end; addr += PAGE_SIZE) {
+                pte = pte_offset_kernel(pmd, addr);
+                BUG_ON(pfn_valid(*pfn_base));
+                kvm_set_pte(pte, pfn_pte(*pfn_base, PAGE_HYP_DEVICE));
+                (*pfn_base)++;
+        }
+}
+
+static int create_hyp_pmd_mappings(pud_t *pud, unsigned long start,
+                                   unsigned long end, unsigned long *pfn_base)
+{
+        pmd_t *pmd;
+        pte_t *pte;
+        unsigned long addr, next;
+
+        for (addr = start; addr < end; addr = next) {
+                pmd = pmd_offset(pud, addr);
+
+                BUG_ON(pmd_sect(*pmd));
+
+                if (pmd_none(*pmd)) {
+                        pte = pte_alloc_one_kernel(NULL, addr);
+                        if (!pte) {
+                                kvm_err("Cannot allocate Hyp pte\n");
+                                return -ENOMEM;
+                        }
+                        pmd_populate_kernel(NULL, pmd, pte);
+                }
+
+                next = pmd_addr_end(addr, end);
+
+                /*
+                 * If pfn_base is NULL, we map kernel pages into HYP with the
+                 * virtual address. Otherwise, this is considered an I/O
+                 * mapping and we map the physical region starting at
+                 * *pfn_base to [start, end[.
+                 */
+                if (!pfn_base)
+                        create_hyp_pte_mappings(pmd, addr, next);
+                else
+                        create_hyp_io_pte_mappings(pmd, addr, next, pfn_base);
+        }
+
+        return 0;
+}
+
+static int __create_hyp_mappings(void *from, void *to, unsigned long *pfn_base)
+{
+        unsigned long start = (unsigned long)from;
+        unsigned long end = (unsigned long)to;
+        pgd_t *pgd;
+        pud_t *pud;
+        pmd_t *pmd;
+        unsigned long addr, next;
+        int err = 0;
+
+        BUG_ON(start > end);
+        if (start < PAGE_OFFSET)
+                return -EINVAL;
+
+        mutex_lock(&kvm_hyp_pgd_mutex);
+        for (addr = start; addr < end; addr = next) {
+                pgd = hyp_pgd + pgd_index(addr);
+                pud = pud_offset(pgd, addr);
+
+                if (pud_none_or_clear_bad(pud)) {
+                        pmd = pmd_alloc_one(NULL, addr);
+                        if (!pmd) {
+                                kvm_err("Cannot allocate Hyp pmd\n");
+                                err = -ENOMEM;
+                                goto out;
+                        }
+                        pud_populate(NULL, pud, pmd);
+                }
+
+                next = pgd_addr_end(addr, end);
+                err = create_hyp_pmd_mappings(pud, addr, next, pfn_base);
+                if (err)
+                        goto out;
+        }
+out:
+        mutex_unlock(&kvm_hyp_pgd_mutex);
+        return err;
+}
+
+/**
+ * create_hyp_mappings - map a kernel virtual address range in Hyp mode
+ * @from:       The virtual kernel start address of the range
+ * @to:         The virtual kernel end address of the range (exclusive)
+ *
+ * The same virtual address as the kernel virtual address is also used in
+ * Hyp-mode mapping to the same underlying physical pages.
+ *
+ * Note: Wrapping around zero in the "to" address is not supported.
+ */
+int create_hyp_mappings(void *from, void *to)
+{
+        return __create_hyp_mappings(from, to, NULL);
+}
+
+/**
+ * create_hyp_io_mappings - map a physical IO range in Hyp mode
+ * @from:       The virtual HYP start address of the range
+ * @to:         The virtual HYP end address of the range (exclusive)
+ * @addr:       The physical start address which gets mapped
+ */
+int create_hyp_io_mappings(void *from, void *to, phys_addr_t addr)
+{
+        unsigned long pfn = __phys_to_pfn(addr);
+        return __create_hyp_mappings(from, to, &pfn);
+}
+
+int kvm_handle_guest_abort(struct kvm_vcpu *vcpu, struct kvm_run *run)
+{
+        return -EINVAL;
+}
+
+phys_addr_t kvm_mmu_get_httbr(void)
+{
+        VM_BUG_ON(!virt_addr_valid(hyp_pgd));
+        return virt_to_phys(hyp_pgd);
+}
+
+int kvm_mmu_init(void)
+{
+        return hyp_pgd ? 0 : -ENOMEM;
+}
+
+/**
+ * kvm_clear_idmap - remove all idmaps from the hyp pgd
+ *
+ * Free the underlying pmds for all pgds in range and clear the pgds (but
+ * don't free them) afterwards.
+ */
+void kvm_clear_hyp_idmap(void)
+{
+        unsigned long addr, end;
+        unsigned long next;
+        pgd_t *pgd = hyp_pgd;
+        pud_t *pud;
+        pmd_t *pmd;
+
+        addr = virt_to_phys(__hyp_idmap_text_start);
+        end = virt_to_phys(__hyp_idmap_text_end);
+
+        pgd += pgd_index(addr);
+        do {
+                next = pgd_addr_end(addr, end);
+                if (pgd_none_or_clear_bad(pgd))
+                        continue;
+                pud = pud_offset(pgd, addr);
+                pmd = pmd_offset(pud, addr);
+
+                pud_clear(pud);
+                clean_pmd_entry(pmd);
+                pmd_free(NULL, (pmd_t *)((unsigned long)pmd & PAGE_MASK));
+        } while (pgd++, addr = next, addr < end);
+}