/*-
 * SPDX-License-Identifier: BSD-2-Clause
 *
 * Copyright (c) 2021 Andrew Turner
 *
 * This work was supported by Innovate UK project 105694, "Digital Security
 * by Design (DSbD) Technology Platform Prototype".
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 */

#include <sys/cdefs.h>
#include <sys/types.h>
#include <sys/proc.h>


#include "arm64.h"
#include "hyp.h"

struct hypctx;

uint64_t VMM_HYP_FUNC(do_call_guest)(struct hypctx *);

static void
vmm_hyp_reg_store(struct hypctx *hypctx, struct hyp *hyp, bool guest,
    bool ecv_poff)
{
        uint64_t dfr0;

        if (guest) {
                /* Store the timer registers */
                hypctx->vtimer_cpu.cntkctl_el1 =
                    READ_SPECIALREG(EL1_REG(CNTKCTL));
                hypctx->vtimer_cpu.virt_timer.cntx_cval_el0 =
                    READ_SPECIALREG(EL0_REG(CNTV_CVAL));
                hypctx->vtimer_cpu.virt_timer.cntx_ctl_el0 =
                    READ_SPECIALREG(EL0_REG(CNTV_CTL));
        }
        if (guest_or_nonvhe(guest) && ecv_poff) {
                /*
                 * If we have ECV then the guest could modify these registers.
                 * If VHE is enabled then the kernel will see a different view
                 * of the registers, so doesn't need to handle them.
                 */
                hypctx->vtimer_cpu.phys_timer.cntx_cval_el0 =
                    READ_SPECIALREG(EL0_REG(CNTP_CVAL));
                hypctx->vtimer_cpu.phys_timer.cntx_ctl_el0 =
                    READ_SPECIALREG(EL0_REG(CNTP_CTL));
        }

        if (guest) {
                /* Store the GICv3 registers */
                hypctx->vgic_v3_regs.ich_eisr_el2 =
                    READ_SPECIALREG(ich_eisr_el2);
                hypctx->vgic_v3_regs.ich_elrsr_el2 =
                    READ_SPECIALREG(ich_elrsr_el2);
                hypctx->vgic_v3_regs.ich_hcr_el2 =
                    READ_SPECIALREG(ich_hcr_el2);
                hypctx->vgic_v3_regs.ich_misr_el2 =
                    READ_SPECIALREG(ich_misr_el2);
                hypctx->vgic_v3_regs.ich_vmcr_el2 =
                    READ_SPECIALREG(ich_vmcr_el2);
                switch (hypctx->vgic_v3_regs.ich_lr_num - 1) {
#define STORE_LR(x)                                     \
        case x:                                         \
                hypctx->vgic_v3_regs.ich_lr_el2[x] =    \
                    READ_SPECIALREG(ich_lr ## x ##_el2)
                STORE_LR(15);
                STORE_LR(14);
                STORE_LR(13);
                STORE_LR(12);
                STORE_LR(11);
                STORE_LR(10);
                STORE_LR(9);
                STORE_LR(8);
                STORE_LR(7);
                STORE_LR(6);
                STORE_LR(5);
                STORE_LR(4);
                STORE_LR(3);
                STORE_LR(2);
                STORE_LR(1);
                default:
                STORE_LR(0);
#undef STORE_LR
                }

                switch (hypctx->vgic_v3_regs.ich_apr_num - 1) {
#define STORE_APR(x)                                            \
        case x:                                                 \
                hypctx->vgic_v3_regs.ich_ap0r_el2[x] =          \
                    READ_SPECIALREG(ich_ap0r ## x ##_el2);      \
                hypctx->vgic_v3_regs.ich_ap1r_el2[x] =          \
                    READ_SPECIALREG(ich_ap1r ## x ##_el2)
                STORE_APR(3);
                STORE_APR(2);
                STORE_APR(1);
                default:
                STORE_APR(0);
#undef STORE_APR
                }
        }

        hypctx->dbgclaimset_el1 = READ_SPECIALREG(dbgclaimset_el1);

        dfr0 = READ_SPECIALREG(id_aa64dfr0_el1);
        switch (ID_AA64DFR0_BRPs_VAL(dfr0) - 1) {
#define STORE_DBG_BRP(x)                                                \
        case x:                                                         \
                hypctx->dbgbcr_el1[x] =                                 \
                    READ_SPECIALREG(dbgbcr ## x ## _el1);               \
                hypctx->dbgbvr_el1[x] =                                 \
                    READ_SPECIALREG(dbgbvr ## x ## _el1)
        STORE_DBG_BRP(15);
        STORE_DBG_BRP(14);
        STORE_DBG_BRP(13);
        STORE_DBG_BRP(12);
        STORE_DBG_BRP(11);
        STORE_DBG_BRP(10);
        STORE_DBG_BRP(9);
        STORE_DBG_BRP(8);
        STORE_DBG_BRP(7);
        STORE_DBG_BRP(6);
        STORE_DBG_BRP(5);
        STORE_DBG_BRP(4);
        STORE_DBG_BRP(3);
        STORE_DBG_BRP(2);
        STORE_DBG_BRP(1);
        default:
        STORE_DBG_BRP(0);
#undef STORE_DBG_BRP
        }

        switch (ID_AA64DFR0_WRPs_VAL(dfr0) - 1) {
#define STORE_DBG_WRP(x)                                                \
        case x:                                                         \
                hypctx->dbgwcr_el1[x] =                                 \
                    READ_SPECIALREG(dbgwcr ## x ## _el1);               \
                hypctx->dbgwvr_el1[x] =                                 \
                    READ_SPECIALREG(dbgwvr ## x ## _el1)
        STORE_DBG_WRP(15);
        STORE_DBG_WRP(14);
        STORE_DBG_WRP(13);
        STORE_DBG_WRP(12);
        STORE_DBG_WRP(11);
        STORE_DBG_WRP(10);
        STORE_DBG_WRP(9);
        STORE_DBG_WRP(8);
        STORE_DBG_WRP(7);
        STORE_DBG_WRP(6);
        STORE_DBG_WRP(5);
        STORE_DBG_WRP(4);
        STORE_DBG_WRP(3);
        STORE_DBG_WRP(2);
        STORE_DBG_WRP(1);
        default:
        STORE_DBG_WRP(0);
#undef STORE_DBG_WRP
        }

        /* Store the PMU registers */
        hypctx->pmcr_el0 = READ_SPECIALREG(pmcr_el0);
        hypctx->pmccntr_el0 = READ_SPECIALREG(pmccntr_el0);
        hypctx->pmccfiltr_el0 = READ_SPECIALREG(pmccfiltr_el0);
        hypctx->pmuserenr_el0 = READ_SPECIALREG(pmuserenr_el0);
        hypctx->pmselr_el0 = READ_SPECIALREG(pmselr_el0);
        hypctx->pmxevcntr_el0 = READ_SPECIALREG(pmxevcntr_el0);
        hypctx->pmcntenset_el0 = READ_SPECIALREG(pmcntenset_el0);
        hypctx->pmintenset_el1 = READ_SPECIALREG(pmintenset_el1);
        hypctx->pmovsset_el0 = READ_SPECIALREG(pmovsset_el0);

        switch ((hypctx->pmcr_el0 & PMCR_N_MASK) >> PMCR_N_SHIFT) {
#define STORE_PMU(x)                                                    \
        case (x + 1):                                                   \
                hypctx->pmevcntr_el0[x] =                               \
                    READ_SPECIALREG(pmevcntr ## x ## _el0);             \
                hypctx->pmevtyper_el0[x] =                              \
                    READ_SPECIALREG(pmevtyper ## x ## _el0)
        STORE_PMU(30);
        STORE_PMU(29);
        STORE_PMU(28);
        STORE_PMU(27);
        STORE_PMU(26);
        STORE_PMU(25);
        STORE_PMU(24);
        STORE_PMU(23);
        STORE_PMU(22);
        STORE_PMU(21);
        STORE_PMU(20);
        STORE_PMU(19);
        STORE_PMU(18);
        STORE_PMU(17);
        STORE_PMU(16);
        STORE_PMU(15);
        STORE_PMU(14);
        STORE_PMU(13);
        STORE_PMU(12);
        STORE_PMU(11);
        STORE_PMU(10);
        STORE_PMU(9);
        STORE_PMU(8);
        STORE_PMU(7);
        STORE_PMU(6);
        STORE_PMU(5);
        STORE_PMU(4);
        STORE_PMU(3);
        STORE_PMU(2);
        STORE_PMU(1);
        STORE_PMU(0);
        default:                /* N == 0 when only PMCCNTR_EL0 is available */
                break;
#undef STORE_PMU
        }

        /* Store the special to from the trapframe */
        hypctx->tf.tf_sp = READ_SPECIALREG(sp_el1);
        hypctx->tf.tf_elr = READ_SPECIALREG(elr_el2);
        hypctx->tf.tf_spsr = READ_SPECIALREG(spsr_el2);
        if (guest) {
                hypctx->tf.tf_esr = READ_SPECIALREG(esr_el2);
                hypctx->par_el1 = READ_SPECIALREG(par_el1);
        }

        /* Store the guest special registers */
        hypctx->sp_el0 = READ_SPECIALREG(sp_el0);
        hypctx->tpidr_el0 = READ_SPECIALREG(tpidr_el0);
        hypctx->tpidrro_el0 = READ_SPECIALREG(tpidrro_el0);
        hypctx->tpidr_el1 = READ_SPECIALREG(tpidr_el1);

        hypctx->actlr_el1 = READ_SPECIALREG(actlr_el1);
        hypctx->csselr_el1 = READ_SPECIALREG(csselr_el1);
        hypctx->mdccint_el1 = READ_SPECIALREG(mdccint_el1);
        hypctx->mdscr_el1 = READ_SPECIALREG(mdscr_el1);

        if (guest_or_nonvhe(guest)) {
                hypctx->elr_el1 = READ_SPECIALREG(EL1_REG(ELR));
                hypctx->vbar_el1 = READ_SPECIALREG(EL1_REG(VBAR));

                hypctx->afsr0_el1 = READ_SPECIALREG(EL1_REG(AFSR0));
                hypctx->afsr1_el1 = READ_SPECIALREG(EL1_REG(AFSR1));
                hypctx->amair_el1 = READ_SPECIALREG(EL1_REG(AMAIR));
                hypctx->contextidr_el1 = READ_SPECIALREG(EL1_REG(CONTEXTIDR));
                hypctx->cpacr_el1 = READ_SPECIALREG(EL1_REG(CPACR));
                hypctx->esr_el1 = READ_SPECIALREG(EL1_REG(ESR));
                hypctx->far_el1 = READ_SPECIALREG(EL1_REG(FAR));
                hypctx->mair_el1 = READ_SPECIALREG(EL1_REG(MAIR));
                hypctx->sctlr_el1 = READ_SPECIALREG(EL1_REG(SCTLR));
                hypctx->spsr_el1 = READ_SPECIALREG(EL1_REG(SPSR));
                hypctx->tcr_el1 = READ_SPECIALREG(EL1_REG(TCR));
                /* TODO: Support when this is not res0 */
                hypctx->tcr2_el1 = 0;
                hypctx->ttbr0_el1 = READ_SPECIALREG(EL1_REG(TTBR0));
                hypctx->ttbr1_el1 = READ_SPECIALREG(EL1_REG(TTBR1));
        }

        hypctx->cptr_el2 = READ_SPECIALREG(cptr_el2);
        hypctx->hcr_el2 = READ_SPECIALREG(hcr_el2);
        hypctx->vpidr_el2 = READ_SPECIALREG(vpidr_el2);
        hypctx->vmpidr_el2 = READ_SPECIALREG(vmpidr_el2);
}

static void
vmm_hyp_reg_restore(struct hypctx *hypctx, struct hyp *hyp, bool guest,
    bool ecv_poff)
{
        uint64_t dfr0;

        /* Restore the special registers */
        WRITE_SPECIALREG(hcr_el2, hypctx->hcr_el2);

        if (guest) {
                if ((hyp->feats & HYP_FEAT_HCX) != 0)
                        WRITE_SPECIALREG(HCRX_EL2_REG, hypctx->hcrx_el2);
        }
        isb();

#ifdef VMM_VHE
        if (guest) {
                /* Fine-grained trap controls */
                if ((hyp->feats & HYP_FEAT_FGT) != 0) {
                        WRITE_SPECIALREG(HDFGWTR_EL2_REG, hypctx->hdfgwtr_el2);
                        WRITE_SPECIALREG(HFGITR_EL2_REG, hypctx->hfgitr_el2);
                        WRITE_SPECIALREG(HFGRTR_EL2_REG, hypctx->hfgrtr_el2);
                        WRITE_SPECIALREG(HFGWTR_EL2_REG, hypctx->hfgwtr_el2);
                }

                if ((hyp->feats & HYP_FEAT_FGT2) != 0) {
                        WRITE_SPECIALREG(HDFGRTR2_EL2_REG,
                            hypctx->hdfgrtr2_el2);
                        WRITE_SPECIALREG(HDFGWTR2_EL2_REG,
                            hypctx->hdfgwtr2_el2);
                        WRITE_SPECIALREG(HFGITR2_EL2_REG, hypctx->hfgitr2_el2);
                        WRITE_SPECIALREG(HFGRTR2_EL2_REG, hypctx->hfgrtr2_el2);
                        WRITE_SPECIALREG(HFGWTR2_EL2_REG, hypctx->hfgwtr2_el2);
                }
        }
#endif

        WRITE_SPECIALREG(sp_el0, hypctx->sp_el0);
        WRITE_SPECIALREG(tpidr_el0, hypctx->tpidr_el0);
        WRITE_SPECIALREG(tpidrro_el0, hypctx->tpidrro_el0);
        WRITE_SPECIALREG(tpidr_el1, hypctx->tpidr_el1);

        WRITE_SPECIALREG(actlr_el1, hypctx->actlr_el1);
        WRITE_SPECIALREG(csselr_el1, hypctx->csselr_el1);
        WRITE_SPECIALREG(mdccint_el1, hypctx->mdccint_el1);
        WRITE_SPECIALREG(mdscr_el1, hypctx->mdscr_el1);

        if (guest_or_nonvhe(guest)) {
                WRITE_SPECIALREG(EL1_REG(ELR), hypctx->elr_el1);
                WRITE_SPECIALREG(EL1_REG(VBAR), hypctx->vbar_el1);

                WRITE_SPECIALREG(EL1_REG(AFSR0), hypctx->afsr0_el1);
                WRITE_SPECIALREG(EL1_REG(AFSR1), hypctx->afsr1_el1);
                WRITE_SPECIALREG(EL1_REG(AMAIR), hypctx->amair_el1);
                WRITE_SPECIALREG(EL1_REG(CONTEXTIDR), hypctx->contextidr_el1);
                WRITE_SPECIALREG(EL1_REG(CPACR), hypctx->cpacr_el1);
                WRITE_SPECIALREG(EL1_REG(ESR), hypctx->esr_el1);
                WRITE_SPECIALREG(EL1_REG(FAR), hypctx->far_el1);
                WRITE_SPECIALREG(EL1_REG(MAIR), hypctx->mair_el1); //

                WRITE_SPECIALREG(EL1_REG(SCTLR), hypctx->sctlr_el1);
                WRITE_SPECIALREG(EL1_REG(SPSR), hypctx->spsr_el1);
                WRITE_SPECIALREG(EL1_REG(TCR), hypctx->tcr_el1);
                /* TODO: tcr2_el1 */
                WRITE_SPECIALREG(EL1_REG(TTBR0), hypctx->ttbr0_el1);
                WRITE_SPECIALREG(EL1_REG(TTBR1), hypctx->ttbr1_el1);
        }

        if (guest) {
                WRITE_SPECIALREG(par_el1, hypctx->par_el1);
        }

        WRITE_SPECIALREG(cptr_el2, hypctx->cptr_el2);
        WRITE_SPECIALREG(vpidr_el2, hypctx->vpidr_el2);
        WRITE_SPECIALREG(vmpidr_el2, hypctx->vmpidr_el2);

        /* Load the special regs from the trapframe */
        WRITE_SPECIALREG(sp_el1, hypctx->tf.tf_sp);
        WRITE_SPECIALREG(elr_el2, hypctx->tf.tf_elr);
        WRITE_SPECIALREG(spsr_el2, hypctx->tf.tf_spsr);

        /* Restore the PMU registers */
        WRITE_SPECIALREG(pmcr_el0, hypctx->pmcr_el0);
        WRITE_SPECIALREG(pmccntr_el0, hypctx->pmccntr_el0);
        WRITE_SPECIALREG(pmccfiltr_el0, hypctx->pmccfiltr_el0);
        WRITE_SPECIALREG(pmuserenr_el0, hypctx->pmuserenr_el0);
        WRITE_SPECIALREG(pmselr_el0, hypctx->pmselr_el0);
        WRITE_SPECIALREG(pmxevcntr_el0, hypctx->pmxevcntr_el0);
        /* Clear all events/interrupts then enable them */
        WRITE_SPECIALREG(pmcntenclr_el0, ~0ul);
        WRITE_SPECIALREG(pmcntenset_el0, hypctx->pmcntenset_el0);
        WRITE_SPECIALREG(pmintenclr_el1, ~0ul);
        WRITE_SPECIALREG(pmintenset_el1, hypctx->pmintenset_el1);
        WRITE_SPECIALREG(pmovsclr_el0, ~0ul);
        WRITE_SPECIALREG(pmovsset_el0, hypctx->pmovsset_el0);

        switch ((hypctx->pmcr_el0 & PMCR_N_MASK) >> PMCR_N_SHIFT) {
#define LOAD_PMU(x)                                                     \
        case (x + 1):                                                   \
                WRITE_SPECIALREG(pmevcntr ## x ## _el0,                 \
                    hypctx->pmevcntr_el0[x]);                           \
                WRITE_SPECIALREG(pmevtyper ## x ## _el0,                \
                    hypctx->pmevtyper_el0[x])
        LOAD_PMU(30);
        LOAD_PMU(29);
        LOAD_PMU(28);
        LOAD_PMU(27);
        LOAD_PMU(26);
        LOAD_PMU(25);
        LOAD_PMU(24);
        LOAD_PMU(23);
        LOAD_PMU(22);
        LOAD_PMU(21);
        LOAD_PMU(20);
        LOAD_PMU(19);
        LOAD_PMU(18);
        LOAD_PMU(17);
        LOAD_PMU(16);
        LOAD_PMU(15);
        LOAD_PMU(14);
        LOAD_PMU(13);
        LOAD_PMU(12);
        LOAD_PMU(11);
        LOAD_PMU(10);
        LOAD_PMU(9);
        LOAD_PMU(8);
        LOAD_PMU(7);
        LOAD_PMU(6);
        LOAD_PMU(5);
        LOAD_PMU(4);
        LOAD_PMU(3);
        LOAD_PMU(2);
        LOAD_PMU(1);
        LOAD_PMU(0);
        default:                /* N == 0 when only PMCCNTR_EL0 is available */
                break;
#undef LOAD_PMU
        }

        WRITE_SPECIALREG(dbgclaimclr_el1, ~0ul);
        WRITE_SPECIALREG(dbgclaimclr_el1, hypctx->dbgclaimset_el1);

        dfr0 = READ_SPECIALREG(id_aa64dfr0_el1);
        switch (ID_AA64DFR0_BRPs_VAL(dfr0) - 1) {
#define LOAD_DBG_BRP(x)                                                 \
        case x:                                                         \
                WRITE_SPECIALREG(dbgbcr ## x ## _el1,                   \
                    hypctx->dbgbcr_el1[x]);                             \
                WRITE_SPECIALREG(dbgbvr ## x ## _el1,                   \
                    hypctx->dbgbvr_el1[x])
        LOAD_DBG_BRP(15);
        LOAD_DBG_BRP(14);
        LOAD_DBG_BRP(13);
        LOAD_DBG_BRP(12);
        LOAD_DBG_BRP(11);
        LOAD_DBG_BRP(10);
        LOAD_DBG_BRP(9);
        LOAD_DBG_BRP(8);
        LOAD_DBG_BRP(7);
        LOAD_DBG_BRP(6);
        LOAD_DBG_BRP(5);
        LOAD_DBG_BRP(4);
        LOAD_DBG_BRP(3);
        LOAD_DBG_BRP(2);
        LOAD_DBG_BRP(1);
        default:
        LOAD_DBG_BRP(0);
#undef LOAD_DBG_BRP
        }

        switch (ID_AA64DFR0_WRPs_VAL(dfr0) - 1) {
#define LOAD_DBG_WRP(x)                                                 \
        case x:                                                         \
                WRITE_SPECIALREG(dbgwcr ## x ## _el1,                   \
                    hypctx->dbgwcr_el1[x]);                             \
                WRITE_SPECIALREG(dbgwvr ## x ## _el1,                   \
                    hypctx->dbgwvr_el1[x])
        LOAD_DBG_WRP(15);
        LOAD_DBG_WRP(14);
        LOAD_DBG_WRP(13);
        LOAD_DBG_WRP(12);
        LOAD_DBG_WRP(11);
        LOAD_DBG_WRP(10);
        LOAD_DBG_WRP(9);
        LOAD_DBG_WRP(8);
        LOAD_DBG_WRP(7);
        LOAD_DBG_WRP(6);
        LOAD_DBG_WRP(5);
        LOAD_DBG_WRP(4);
        LOAD_DBG_WRP(3);
        LOAD_DBG_WRP(2);
        LOAD_DBG_WRP(1);
        default:
        LOAD_DBG_WRP(0);
#undef LOAD_DBG_WRP
        }

        if (guest) {
                /* Load the timer registers */
                WRITE_SPECIALREG(EL1_REG(CNTKCTL),
                    hypctx->vtimer_cpu.cntkctl_el1);
                WRITE_SPECIALREG(EL0_REG(CNTV_CVAL),
                    hypctx->vtimer_cpu.virt_timer.cntx_cval_el0);
                WRITE_SPECIALREG(EL0_REG(CNTV_CTL),
                    hypctx->vtimer_cpu.virt_timer.cntx_ctl_el0);
                WRITE_SPECIALREG(cnthctl_el2, hyp->vtimer.cnthctl_el2);
                WRITE_SPECIALREG(cntvoff_el2, hyp->vtimer.cntvoff_el2);

                if (ecv_poff) {
                        /*
                         * Load the same offset as the virtual timer
                         * to keep in sync.
                         */
                        WRITE_SPECIALREG(CNTPOFF_EL2_REG,
                            hyp->vtimer.cntvoff_el2);
                        isb();
                }
        }
        if (guest_or_nonvhe(guest) && ecv_poff) {
                /*
                 * If we have ECV then the guest could modify these registers.
                 * If VHE is enabled then the kernel will see a different view
                 * of the registers, so doesn't need to handle them.
                 */
                WRITE_SPECIALREG(EL0_REG(CNTP_CVAL),
                    hypctx->vtimer_cpu.phys_timer.cntx_cval_el0);
                WRITE_SPECIALREG(EL0_REG(CNTP_CTL),
                    hypctx->vtimer_cpu.phys_timer.cntx_ctl_el0);
        }

        if (guest) {
                /* Load the GICv3 registers */
                WRITE_SPECIALREG(ich_hcr_el2, hypctx->vgic_v3_regs.ich_hcr_el2);
                WRITE_SPECIALREG(ich_vmcr_el2,
                    hypctx->vgic_v3_regs.ich_vmcr_el2);
                switch (hypctx->vgic_v3_regs.ich_lr_num - 1) {
#define LOAD_LR(x)                                      \
        case x:                                         \
                WRITE_SPECIALREG(ich_lr ## x ##_el2,    \
                    hypctx->vgic_v3_regs.ich_lr_el2[x])
                LOAD_LR(15);
                LOAD_LR(14);
                LOAD_LR(13);
                LOAD_LR(12);
                LOAD_LR(11);
                LOAD_LR(10);
                LOAD_LR(9);
                LOAD_LR(8);
                LOAD_LR(7);
                LOAD_LR(6);
                LOAD_LR(5);
                LOAD_LR(4);
                LOAD_LR(3);
                LOAD_LR(2);
                LOAD_LR(1);
                default:
                LOAD_LR(0);
#undef LOAD_LR
                }

                switch (hypctx->vgic_v3_regs.ich_apr_num - 1) {
#define LOAD_APR(x)                                             \
        case x:                                                 \
                WRITE_SPECIALREG(ich_ap0r ## x ##_el2,          \
                    hypctx->vgic_v3_regs.ich_ap0r_el2[x]);              \
                WRITE_SPECIALREG(ich_ap1r ## x ##_el2,          \
                    hypctx->vgic_v3_regs.ich_ap1r_el2[x])
                LOAD_APR(3);
                LOAD_APR(2);
                LOAD_APR(1);
                default:
                LOAD_APR(0);
#undef LOAD_APR
                }
        }
}

static uint64_t
vmm_hyp_call_guest(struct hyp *hyp, struct hypctx *hypctx)
{
        struct hypctx host_hypctx;
        uint64_t cntvoff_el2;
        uint64_t ich_hcr_el2, ich_vmcr_el2, cnthctl_el2, cntkctl_el1;
#ifndef VMM_VHE
        uint64_t hcrx_el2;
#endif
        uint64_t ret;
        uint64_t s1e1r, hpfar_el2;
        bool ecv_poff, hpfar_valid;

        ecv_poff = (hyp->vtimer.cnthctl_el2 & CNTHCTL_ECV_EN) != 0;
        vmm_hyp_reg_store(&host_hypctx, NULL, false, ecv_poff);
#ifndef VMM_VHE
        if ((hyp->feats & HYP_FEAT_HCX) != 0)
                hcrx_el2 = READ_SPECIALREG(MRS_REG_ALT_NAME(HCRX_EL2));
#endif

        /* Save the host special registers */
        cnthctl_el2 = READ_SPECIALREG(cnthctl_el2);
        cntkctl_el1 = READ_SPECIALREG(cntkctl_el1);
        cntvoff_el2 = READ_SPECIALREG(cntvoff_el2);

        ich_hcr_el2 = READ_SPECIALREG(ich_hcr_el2);
        ich_vmcr_el2 = READ_SPECIALREG(ich_vmcr_el2);

        vmm_hyp_reg_restore(hypctx, hyp, true, ecv_poff);

        /* Load the common hypervisor registers */
        WRITE_SPECIALREG(vttbr_el2, hyp->vttbr_el2);

        host_hypctx.mdcr_el2 = READ_SPECIALREG(mdcr_el2);
        WRITE_SPECIALREG(mdcr_el2, hypctx->mdcr_el2);

        /* Call into the guest */
        ret = VMM_HYP_FUNC(do_call_guest)(hypctx);

        WRITE_SPECIALREG(mdcr_el2, host_hypctx.mdcr_el2);
        isb();

        /* Store the exit info */
        hypctx->exit_info.far_el2 = READ_SPECIALREG(far_el2);
        vmm_hyp_reg_store(hypctx, hyp, true, ecv_poff);

        hpfar_valid = true;
        if (ret == EXCP_TYPE_EL1_SYNC) {
                switch (ESR_ELx_EXCEPTION(hypctx->tf.tf_esr)) {
                case EXCP_INSN_ABORT_L:
                case EXCP_DATA_ABORT_L:
                        /*
                         * The hpfar_el2 register is valid for:
                         *  - Translation and Access faults.
                         *  - Translation, Access, and permission faults on
                         *    the translation table walk on the stage 1 tables.
                         *  - A stage 2 Address size fault.
                         *
                         * As we only need it in the first 2 cases we can just
                         * exclude it on permission faults that are not from
                         * the stage 1 table walk.
                         *
                         * TODO: Add a case for Arm erratum 834220.
                         */
                        if ((hypctx->tf.tf_esr & ISS_DATA_S1PTW) != 0)
                                break;
                        switch (hypctx->tf.tf_esr & ISS_DATA_DFSC_MASK) {
                        case ISS_DATA_DFSC_PF_L1:
                        case ISS_DATA_DFSC_PF_L2:
                        case ISS_DATA_DFSC_PF_L3:
                                hpfar_valid = false;
                                break;
                        }
                        break;
                }
        }
        if (hpfar_valid) {
                hypctx->exit_info.hpfar_el2 = READ_SPECIALREG(hpfar_el2);
        } else {
                /*
                 * TODO: There is a risk the at instruction could cause an
                 * exception here. We should handle it & return a failure.
                 */
                s1e1r =
                    arm64_address_translate_s1e1r(hypctx->exit_info.far_el2);
                if (PAR_SUCCESS(s1e1r)) {
                        hpfar_el2 = (s1e1r & PAR_PA_MASK) >> PAR_PA_SHIFT;
                        hpfar_el2 <<= HPFAR_EL2_FIPA_SHIFT;
                        hypctx->exit_info.hpfar_el2 = hpfar_el2;
                } else {
                        ret = EXCP_TYPE_REENTER;
                }
        }

        vmm_hyp_reg_restore(&host_hypctx, NULL, false, ecv_poff);

#ifndef VMM_VHE
        if ((hyp->feats & HYP_FEAT_HCX) != 0)
                WRITE_SPECIALREG(MRS_REG_ALT_NAME(HCRX_EL2), hcrx_el2);
#endif

        /* Restore the host special registers */
        WRITE_SPECIALREG(ich_hcr_el2, ich_hcr_el2);
        WRITE_SPECIALREG(ich_vmcr_el2, ich_vmcr_el2);

        WRITE_SPECIALREG(cnthctl_el2, cnthctl_el2);
        WRITE_SPECIALREG(cntkctl_el1, cntkctl_el1);
        WRITE_SPECIALREG(cntvoff_el2, cntvoff_el2);

        return (ret);
}

VMM_STATIC uint64_t
VMM_HYP_FUNC(enter_guest)(struct hyp *hyp, struct hypctx *hypctx)
{
        uint64_t ret;

        do {
                ret = vmm_hyp_call_guest(hyp, hypctx);
        } while (ret == EXCP_TYPE_REENTER);

        return (ret);
}

VMM_STATIC uint64_t
VMM_HYP_FUNC(read_reg)(uint64_t reg)
{
        switch (reg) {
        case HYP_REG_ICH_VTR:
                return (READ_SPECIALREG(ich_vtr_el2));
        }

        return (0);
}

VMM_STATIC void
VMM_HYP_FUNC(clean_s2_tlbi)(void)
{
        dsb(ishst);
        __asm __volatile("tlbi alle1is");
        dsb(ish);
}

VMM_STATIC void
VMM_HYP_FUNC(s2_tlbi_range)(uint64_t vttbr, vm_offset_t sva, vm_offset_t eva,
    bool final_only)
{
        uint64_t end, r, start;
        uint64_t host_vttbr;
#ifdef VMM_VHE
        uint64_t host_tcr;
#endif

#ifdef VMM_VHE
        dsb(ishst);
#endif

#define TLBI_VA_SHIFT                   12
#define TLBI_VA_MASK                    ((1ul << 44) - 1)
#define TLBI_VA(addr)                   (((addr) >> TLBI_VA_SHIFT) & TLBI_VA_MASK)
#define TLBI_VA_L3_INCR                 (L3_SIZE >> TLBI_VA_SHIFT)

        /* Switch to the guest vttbr */
        /* TODO: Handle Cortex-A57/A72 erratum 131936 */
        host_vttbr = READ_SPECIALREG(vttbr_el2);
        WRITE_SPECIALREG(vttbr_el2, vttbr);
        isb();

#ifdef VMM_VHE
        host_tcr = READ_SPECIALREG(tcr_el2);
        WRITE_SPECIALREG(tcr_el2, host_tcr & ~HCR_TGE);
        isb();
#endif

        /*
         * The CPU can cache the stage 1 + 2 combination so we need to ensure
         * the stage 2 is invalidated first, then when this has completed we
         * invalidate the stage 1 TLB. As we don't know which stage 1 virtual
         * addresses point at the stage 2 IPA we need to invalidate the entire
         * stage 1 TLB.
         */

        start = TLBI_VA(sva);
        end = TLBI_VA(eva);
        for (r = start; r < end; r += TLBI_VA_L3_INCR) {
                /* Invalidate the stage 2 TLB entry */
                if (final_only)
                        __asm __volatile("tlbi  ipas2le1is, %0" : : "r"(r));
                else
                        __asm __volatile("tlbi  ipas2e1is, %0" : : "r"(r));
        }
        /* Ensure the entry has been invalidated */
        dsb(ish);
        /* Invalidate the stage 1 TLB. */
        __asm __volatile("tlbi vmalle1is");
        dsb(ish);
        isb();

#ifdef VMM_VHE
        WRITE_SPECIALREG(tcr_el2, host_tcr);
        isb();
#endif

        /* Switch back to the host vttbr */
        WRITE_SPECIALREG(vttbr_el2, host_vttbr);
        isb();
}

VMM_STATIC void
VMM_HYP_FUNC(s2_tlbi_all)(uint64_t vttbr)
{
        uint64_t host_vttbr;

#ifdef VMM_VHE
        dsb(ishst);
#endif

        /* Switch to the guest vttbr */
        /* TODO: Handle Cortex-A57/A72 erratum 131936 */
        host_vttbr = READ_SPECIALREG(vttbr_el2);
        WRITE_SPECIALREG(vttbr_el2, vttbr);
        isb();

        __asm __volatile("tlbi vmalls12e1is");
        dsb(ish);
        isb();

        /* Switch back t othe host vttbr */
        WRITE_SPECIALREG(vttbr_el2, host_vttbr);
        isb();
}