[arm64][hypervisor] Add El2CpuState.

Add code to manage EL2 CPU state. Currently this does the very minimum
and sets the EL2 stack for each CPU.

Next, I'll start setting up more of the EL2 state and also guest
physical address space.

Change-Id: I18b7f9d00b236e52cdc317dffe3b42fcffbcb8fe
diff --git a/kernel/arch/arm64/asm.S b/kernel/arch/arm64/asm.S
index ed094f3..5f7ed1a 100644
--- a/kernel/arch/arm64/asm.S
+++ b/kernel/arch/arm64/asm.S
@@ -65,7 +65,6 @@
     orr x9, x9, #SCR_EL3_RW
     msr scr_el3, x9
 
-
     adr x9, .Ltarget
     msr elr_el3, x9
 
@@ -74,6 +73,10 @@
     b   .confEL1
 
 .inEL2:
+    /* Set the vector base for EL2 */
+    adr_global x9, arm64_el2_exception_base
+    msr vbar_el2, x9
+
     /* Ensure EL1 timers are properly configured, disable EL2 trapping of
         EL1 access to timer control registers.  Also clear virtual offset.
     */
@@ -136,5 +139,4 @@
     ldr     x0, [x11, #8]
     add     x1, x11, #32
     ret
-
 END_FUNCTION(arm64_get_secondary_sp)
diff --git a/kernel/arch/arm64/hypervisor/el2.S b/kernel/arch/arm64/hypervisor/el2.S
new file mode 100644
index 0000000..5074859
--- /dev/null
+++ b/kernel/arch/arm64/hypervisor/el2.S
@@ -0,0 +1,138 @@
+// Copyright 2017 The Fuchsia Authors
+//
+// Use of this source code is governed by a MIT-style
+// license that can be found in the LICENSE file or at
+// https://opensource.org/licenses/MIT
+
+#include <arch/arm64/mmu.h>
+#include <arch/asm_macros.h>
+#include <asm.h>
+#include <magenta/errors.h>
+
+#define ESR_EL2_EC_MASK     0xfc000000
+#define ESR_EL2_ISS_MASK    0x01ffffff
+#define HVC_MAX_INDEX       1
+
+.section .text.el2,"ax",@progbits
+.align 12
+
+// EL2 functions
+LOCAL_FUNCTION(el2_set_stack)
+    mov sp, x0
+    mov x0, #MX_OK
+    eret
+END_FUNCTION(el2_set_stack)
+
+.section .text.boot.vectab.el2,"ax",@progbits
+.align 12
+
+.macro invalid_exception
+    // TODO(abdulla): Check VMID from VTTBR_EL2. ERET to host with error. If
+    // VMID was not 0, terminate guest.
+    eret
+.endm
+
+.macro sync_exception
+    mrs x10, esr_el2
+    and x10, x10, #ESR_EL2_ISS_MASK
+    cmp x10, #HVC_MAX_INDEX
+    b.ge out_of_range
+
+    lsl x10, x10, #2
+    adr x9, table
+    add x9, x9, x10
+    br x9
+
+table:
+    b el2_set_stack
+
+out_of_range:
+    mov x0, MX_ERR_OUT_OF_RANGE
+    eret
+.endm
+
+FUNCTION_LABEL(arm64_el2_exception_base)
+
+/* exceptions from current EL, using SP0 */
+.org 0x000
+LOCAL_FUNCTION(arm64_el2_sync_exc_current_el_SP0)
+    invalid_exception
+END_FUNCTION(arm64_el2_sync_exc_current_el_SP0)
+
+.org 0x080
+LOCAL_FUNCTION(arm64_el2_irq_current_el_SP0)
+    invalid_exception
+END_FUNCTION(arm64_el2_irq_current_el_SP0)
+
+.org 0x100
+LOCAL_FUNCTION(arm64_el2_fiq_current_el_SP0)
+    invalid_exception
+END_FUNCTION(arm64_el2_fiq_current_el_SP0)
+
+.org 0x180
+LOCAL_FUNCTION(arm64_el2_err_exc_current_el_SP0)
+    invalid_exception
+END_FUNCTION(arm64_el2_err_exc_current_el_SP0)
+
+/* exceptions from current EL, using SPx */
+.org 0x200
+LOCAL_FUNCTION(arm64_el2_sync_exc_current_el_SPx)
+    invalid_exception
+END_FUNCTION(arm64_el2_sync_exc_current_el_SPx)
+
+.org 0x280
+LOCAL_FUNCTION(arm64_el2_irq_current_el_SPx)
+    invalid_exception
+END_FUNCTION(arm64_el2_irq_current_el_SPx)
+
+.org 0x300
+LOCAL_FUNCTION(arm64_el2_fiq_current_el_SPx)
+    invalid_exception
+END_FUNCTION(arm64_el2_fiq_current_el_SPx)
+
+.org 0x380
+LOCAL_FUNCTION(arm64_el2_err_exc_current_el_SPx)
+    invalid_exception
+END_FUNCTION(arm64_el2_err_exc_current_el_SPx)
+
+/* exceptions from lower EL, running arm64 */
+.org 0x400
+LOCAL_FUNCTION(arm64_el2_sync_exc_lower_el_64)
+    sync_exception
+END_FUNCTION(arm64_el2_sync_exc_lower_el_64)
+
+.org 0x480
+LOCAL_FUNCTION(arm64_el2_irq_lower_el_64)
+    invalid_exception
+END_FUNCTION(arm64_el2_irq_lower_el_64)
+
+.org 0x500
+LOCAL_FUNCTION(arm64_el2_fiq_lower_el_64)
+    invalid_exception
+END_FUNCTION(arm64_el2_fiq_lower_el_64)
+
+.org 0x580
+LOCAL_FUNCTION(arm64_el2_err_exc_lower_el_64)
+    invalid_exception
+END_FUNCTION(arm64_el2_err_exc_lower_el_64)
+
+/* exceptions from lower EL, running arm32 */
+.org 0x600
+LOCAL_FUNCTION(arm64_el2_sync_exc_lower_el_32)
+    invalid_exception
+END_FUNCTION(arm64_el2_sync_exc_lower_el_32)
+
+.org 0x680
+LOCAL_FUNCTION(arm64_el2_irq_lower_el_32)
+    invalid_exception
+END_FUNCTION(arm64_el2_irq_lower_el_32)
+
+.org 0x700
+LOCAL_FUNCTION(arm64_el2_fiq_lower_el_32)
+    invalid_exception
+END_FUNCTION(arm64_el2_fiq_lower_el_32)
+
+.org 0x780
+LOCAL_FUNCTION(arm64_el2_err_exc_lower_el_32)
+    invalid_exception
+END_FUNCTION(arm64_el2_err_exc_lower_el_32)
diff --git a/kernel/arch/arm64/hypervisor/el2_cpu_state.cpp b/kernel/arch/arm64/hypervisor/el2_cpu_state.cpp
new file mode 100644
index 0000000..925fa3a
--- /dev/null
+++ b/kernel/arch/arm64/hypervisor/el2_cpu_state.cpp
@@ -0,0 +1,114 @@
+// Copyright 2017 The Fuchsia Authors
+//
+// Use of this source code is governed by a MIT-style
+// license that can be found in the LICENSE file or at
+// https://opensource.org/licenses/MIT
+
+#include "el2_cpu_state_priv.h"
+
+#include <fbl/auto_lock.h>
+#include <fbl/mutex.h>
+#include <vm/pmm.h>
+
+static fbl::Mutex el2_mutex;
+static size_t num_guests TA_GUARDED(el2_mutex) = 0;
+static fbl::unique_ptr<El2CpuState> el2_cpu_state TA_GUARDED(el2_mutex);
+
+static mx_status_t el2_set_stack(mx_paddr_t stack_top) {
+    register mx_status_t status asm("x0") = MX_OK;
+    __asm__ volatile("hvc #0" ::: "x0");
+    return status;
+}
+
+El2Stack::~El2Stack() {
+    if (stack_paddr_ != 0)
+        pmm_free_kpages(paddr_to_kvaddr(stack_paddr_), ARCH_DEFAULT_STACK_SIZE / PAGE_SIZE);
+}
+
+mx_status_t El2Stack::Alloc() {
+    pmm_alloc_kpages(ARCH_DEFAULT_STACK_SIZE / PAGE_SIZE, nullptr, &stack_paddr_);
+    return stack_paddr_ != 0 ? MX_OK : MX_ERR_NO_MEMORY;
+}
+
+mx_paddr_t El2Stack::Top() const {
+    return stack_paddr_ + ARCH_DEFAULT_STACK_SIZE;
+}
+
+static mx_status_t el2_on_task(void* context, uint cpu_num) {
+    auto stacks = static_cast<fbl::Array<El2Stack>*>(context);
+    El2Stack& stack = (*stacks)[cpu_num];
+
+    mx_status_t status = el2_set_stack(stack.Top());
+    if (status != MX_OK) {
+        dprintf(CRITICAL, "Failed to set EL2 stack for CPU %u\n", cpu_num);
+        return status;
+    }
+
+    return MX_OK;
+}
+
+static void el2_off_task(void* arg) {
+    mx_status_t status = el2_set_stack(0);
+    if (status != MX_OK)
+        dprintf(CRITICAL, "Failed to clear EL2 stack for CPU %u\n", arch_curr_cpu_num());
+}
+
+// static
+mx_status_t El2CpuState::Create(fbl::unique_ptr<El2CpuState>* out) {
+    fbl::AllocChecker ac;
+    fbl::unique_ptr<El2CpuState> el2_cpu_state(new (&ac) El2CpuState);
+    if (!ac.check())
+        return MX_ERR_NO_MEMORY;
+    mx_status_t status = el2_cpu_state->Init();
+    if (status != MX_OK)
+        return status;
+
+    // Allocate EL2 stack for each CPU.
+    size_t num_cpus = arch_max_num_cpus();
+    El2Stack* stacks = new (&ac) El2Stack[num_cpus];
+    if (!ac.check())
+        return MX_ERR_NO_MEMORY;
+    fbl::Array<El2Stack> el2_stacks(stacks, num_cpus);
+    for (auto& stack : el2_stacks) {
+        mx_status_t status = stack.Alloc();
+        if (status != MX_OK)
+            return status;
+    }
+
+    // Setup EL2 for all online CPUs.
+    mp_cpu_mask_t cpu_mask = percpu_exec(el2_on_task, &el2_stacks);
+    if (cpu_mask != mp_get_online_mask()) {
+        mp_sync_exec(MP_IPI_TARGET_MASK, cpu_mask, el2_off_task, nullptr);
+        return MX_ERR_NOT_SUPPORTED;
+    }
+
+    el2_cpu_state->el2_stacks_ = fbl::move(el2_stacks);
+    *out = fbl::move(el2_cpu_state);
+    return MX_OK;
+}
+
+El2CpuState::~El2CpuState() {
+    mp_sync_exec(MP_IPI_TARGET_ALL, 0, el2_off_task, nullptr);
+}
+
+mx_status_t alloc_vmid(uint8_t* vmid) {
+    fbl::AutoLock lock(&el2_mutex);
+    if (num_guests == 0) {
+        mx_status_t status = El2CpuState::Create(&el2_cpu_state);
+        if (status != MX_OK)
+            return status;
+    }
+    num_guests++;
+    return el2_cpu_state->AllocId(vmid);
+}
+
+mx_status_t free_vmid(uint8_t vmid) {
+    fbl::AutoLock lock(&el2_mutex);
+    mx_status_t status = el2_cpu_state->FreeId(vmid);
+    if (status != MX_OK)
+        return status;
+    num_guests--;
+    if (num_guests == 0)
+        el2_cpu_state.reset();
+    return MX_OK;
+}
diff --git a/kernel/arch/arm64/hypervisor/el2_cpu_state_priv.h b/kernel/arch/arm64/hypervisor/el2_cpu_state_priv.h
new file mode 100644
index 0000000..640a33a
--- /dev/null
+++ b/kernel/arch/arm64/hypervisor/el2_cpu_state_priv.h
@@ -0,0 +1,40 @@
+// Copyright 2017 The Fuchsia Authors
+//
+// Use of this source code is governed by a MIT-style
+// license that can be found in the LICENSE file or at
+// https://opensource.org/licenses/MIT
+
+#pragma once
+
+#include <fbl/array.h>
+#include <fbl/unique_ptr.h>
+#include <hypervisor/cpu_state.h>
+
+/* Represents a stack for use with EL2. */
+class El2Stack {
+public:
+    El2Stack() = default;
+    ~El2Stack();
+    DISALLOW_COPY_ASSIGN_AND_MOVE(El2Stack);
+
+    mx_status_t Alloc();
+    mx_paddr_t Top() const;
+
+private:
+    mx_paddr_t stack_paddr_ = 0;
+};
+
+/* Maintains the EL2 state for each CPU. */
+class El2CpuState : public hypervisor::CpuState<uint8_t, 64> {
+public:
+    static mx_status_t Create(fbl::unique_ptr<El2CpuState>* out);
+    ~El2CpuState();
+
+private:
+    fbl::Array<El2Stack> el2_stacks_;
+
+    El2CpuState() = default;
+};
+
+mx_status_t alloc_vmid(uint8_t* vmid);
+mx_status_t free_vmid(uint8_t vmid);
diff --git a/kernel/arch/arm64/hypervisor/guest.cpp b/kernel/arch/arm64/hypervisor/guest.cpp
index d0cc9e3..29e851b 100644
--- a/kernel/arch/arm64/hypervisor/guest.cpp
+++ b/kernel/arch/arm64/hypervisor/guest.cpp
@@ -5,14 +5,39 @@
 // https://opensource.org/licenses/MIT
 
 #include <arch/hypervisor.h>
+#include <fbl/auto_call.h>
 #include <magenta/errors.h>
 #include <vm/vm_object.h>
 
+#include "el2_cpu_state_priv.h"
+
 // static
 mx_status_t Guest::Create(fbl::RefPtr<VmObject> physmem, fbl::unique_ptr<Guest>* out) {
+    uint8_t vmid;
+    mx_status_t status = alloc_vmid(&vmid);
+    if (status != MX_OK)
+        return status;
+    auto auto_call = fbl::MakeAutoCall([=]() { free_vmid(vmid); });
+
+    fbl::AllocChecker ac;
+    fbl::unique_ptr<Guest> guest(new (&ac) Guest(vmid));
+    if (!ac.check())
+        return MX_ERR_NO_MEMORY;
+
+    auto_call.cancel();
+    *out = fbl::move(guest);
+    // TODO(abdulla): We intentionally return MX_ERR_NOT_SUPPORTED, as the guest
+    // physical address space has not been wired up yet.
     return MX_ERR_NOT_SUPPORTED;
 }
 
+Guest::Guest(uint8_t vmid)
+    : vmid_(vmid) {}
+
+Guest::~Guest() {
+    free_vmid(vmid_);
+}
+
 mx_status_t arch_guest_create(fbl::RefPtr<VmObject> physmem, fbl::unique_ptr<Guest>* guest) {
     if (arm64_get_boot_el() < 2)
         return MX_ERR_NOT_SUPPORTED;
diff --git a/kernel/arch/arm64/hypervisor/rules.mk b/kernel/arch/arm64/hypervisor/rules.mk
index 3320b2e..c731d72 100644
--- a/kernel/arch/arm64/hypervisor/rules.mk
+++ b/kernel/arch/arm64/hypervisor/rules.mk
@@ -9,6 +9,8 @@
 MODULE := $(LOCAL_DIR)
 
 MODULE_SRCS := \
+	$(LOCAL_DIR)/el2.S \
+	$(LOCAL_DIR)/el2_cpu_state.cpp \
 	$(LOCAL_DIR)/guest.cpp \
 	$(LOCAL_DIR)/vcpu.cpp \
 
diff --git a/kernel/arch/arm64/include/arch/hypervisor.h b/kernel/arch/arm64/include/arch/hypervisor.h
index e7f2057..4544ba9 100644
--- a/kernel/arch/arm64/include/arch/hypervisor.h
+++ b/kernel/arch/arm64/include/arch/hypervisor.h
@@ -20,14 +20,16 @@
 class Guest {
 public:
     static status_t Create(fbl::RefPtr<VmObject> physmem, fbl::unique_ptr<Guest>* out);
-    ~Guest() = default;
+    ~Guest();
     DISALLOW_COPY_ASSIGN_AND_MOVE(Guest);
 
     GuestPhysicalAddressSpace* AddressSpace() const { return nullptr; }
     const PacketMux* Mux() const { return nullptr; }
 
 private:
-    Guest() = default;
+    const uint8_t vmid_;
+
+    explicit Guest(uint8_t vmid);
 };
 
 class Vcpu {};