// SPDX-License-Identifier: GPL-2.0-only
/*
 * Fault injection for both 32 and 64bit guests.
 *
 * Copyright (C) 2012,2013 - ARM Ltd
 * Author: Marc Zyngier <marc.zyngier@arm.com>
 *
 * Based on arch/arm/kvm/emulate.c
 * Copyright (C) 2012 - Virtual Open Systems and Columbia University
 * Author: Christoffer Dall <c.dall@virtualopensystems.com>
 */

#include <hyp/adjust_pc.h>
#include <linux/kvm_host.h>
#include <asm/kvm_emulate.h>
#include <asm/kvm_mmu.h>
#include <asm/kvm_nested.h>

#if !defined (__KVM_NVHE_HYPERVISOR__) && !defined (__KVM_VHE_HYPERVISOR__)
#error Hypervisor code only!
#endif

static inline u64 __vcpu_read_sys_reg(const struct kvm_vcpu *vcpu, int reg)
{
        if (has_vhe())
                return vcpu_read_sys_reg(vcpu, reg);

        return __vcpu_sys_reg(vcpu, reg);
}

static inline void __vcpu_write_sys_reg(struct kvm_vcpu *vcpu, u64 val, int reg)
{
        if (has_vhe())
                vcpu_write_sys_reg(vcpu, val, reg);
        else
                __vcpu_assign_sys_reg(vcpu, reg, val);
}

static void __vcpu_write_spsr(struct kvm_vcpu *vcpu, unsigned long target_mode,
                              u64 val)
{
        if (has_vhe()) {
                if (target_mode == PSR_MODE_EL1h)
                        vcpu_write_sys_reg(vcpu, val, SPSR_EL1);
                else
                        vcpu_write_sys_reg(vcpu, val, SPSR_EL2);
        } else {
                __vcpu_assign_sys_reg(vcpu, SPSR_EL1, val);
        }
}

static void __vcpu_write_spsr_abt(struct kvm_vcpu *vcpu, u64 val)
{
        if (has_vhe() && vcpu_get_flag(vcpu, SYSREGS_ON_CPU))
                write_sysreg(val, spsr_abt);
        else
                vcpu->arch.ctxt.spsr_abt = val;
}

static void __vcpu_write_spsr_und(struct kvm_vcpu *vcpu, u64 val)
{
        if (has_vhe() && vcpu_get_flag(vcpu, SYSREGS_ON_CPU))
                write_sysreg(val, spsr_und);
        else
                vcpu->arch.ctxt.spsr_und = val;
}

/*
 * This performs the exception entry at a given EL (@target_mode), stashing PC
 * and PSTATE into ELR and SPSR respectively, and compute the new PC/PSTATE.
 * The EL passed to this function *must* be a non-secure, privileged mode with
 * bit 0 being set (PSTATE.SP == 1).
 *
 * When an exception is taken, most PSTATE fields are left unchanged in the
 * handler. However, some are explicitly overridden (e.g. M[4:0]). Luckily all
 * of the inherited bits have the same position in the AArch64/AArch32 SPSR_ELx
 * layouts, so we don't need to shuffle these for exceptions from AArch32 EL0.
 *
 * For the SPSR_ELx layout for AArch64, see ARM DDI 0487E.a page C5-429.
 * For the SPSR_ELx layout for AArch32, see ARM DDI 0487E.a page C5-426.
 *
 * Here we manipulate the fields in order of the AArch64 SPSR_ELx layout, from
 * MSB to LSB.
 */
static void enter_exception64(struct kvm_vcpu *vcpu, unsigned long target_mode,
                              enum exception_type type)
{
        unsigned long sctlr, vbar, old, new, mode;
        u64 exc_offset;

        mode = *vcpu_cpsr(vcpu) & (PSR_MODE_MASK | PSR_MODE32_BIT);

        if      (mode == target_mode)
                exc_offset = CURRENT_EL_SP_ELx_VECTOR;
        else if ((mode | PSR_MODE_THREAD_BIT) == target_mode)
                exc_offset = CURRENT_EL_SP_EL0_VECTOR;
        else if (!(mode & PSR_MODE32_BIT))
                exc_offset = LOWER_EL_AArch64_VECTOR;
        else
                exc_offset = LOWER_EL_AArch32_VECTOR;

        switch (target_mode) {
        case PSR_MODE_EL1h:
                vbar = __vcpu_read_sys_reg(vcpu, VBAR_EL1);
                sctlr = __vcpu_read_sys_reg(vcpu, SCTLR_EL1);
                __vcpu_write_sys_reg(vcpu, *vcpu_pc(vcpu), ELR_EL1);
                break;
        case PSR_MODE_EL2h:
                vbar = __vcpu_read_sys_reg(vcpu, VBAR_EL2);
                sctlr = __vcpu_read_sys_reg(vcpu, SCTLR_EL2);
                __vcpu_write_sys_reg(vcpu, *vcpu_pc(vcpu), ELR_EL2);
                break;
        default:
                /* Don't do that */
                BUG();
        }

        *vcpu_pc(vcpu) = vbar + exc_offset + type;

        old = *vcpu_cpsr(vcpu);
        new = 0;

        new |= (old & PSR_N_BIT);
        new |= (old & PSR_Z_BIT);
        new |= (old & PSR_C_BIT);
        new |= (old & PSR_V_BIT);

        if (kvm_has_mte(kern_hyp_va(vcpu->kvm)))
                new |= PSR_TCO_BIT;

        new |= (old & PSR_DIT_BIT);

        // PSTATE.UAO is set to zero upon any exception to AArch64
        // See ARM DDI 0487E.a, page D5-2579.

        // PSTATE.PAN is unchanged unless SCTLR_ELx.SPAN == 0b0
        // SCTLR_ELx.SPAN is RES1 when ARMv8.1-PAN is not implemented
        // See ARM DDI 0487E.a, page D5-2578.
        new |= (old & PSR_PAN_BIT);
        if (!(sctlr & SCTLR_EL1_SPAN))
                new |= PSR_PAN_BIT;

        // PSTATE.SS is set to zero upon any exception to AArch64
        // See ARM DDI 0487E.a, page D2-2452.

        // PSTATE.IL is set to zero upon any exception to AArch64
        // See ARM DDI 0487E.a, page D1-2306.

        // PSTATE.SSBS is set to SCTLR_ELx.DSSBS upon any exception to AArch64
        // See ARM DDI 0487E.a, page D13-3258
        if (sctlr & SCTLR_ELx_DSSBS)
                new |= PSR_SSBS_BIT;

        // PSTATE.BTYPE is set to zero upon any exception to AArch64
        // See ARM DDI 0487E.a, pages D1-2293 to D1-2294.

        new |= PSR_D_BIT;
        new |= PSR_A_BIT;
        new |= PSR_I_BIT;
        new |= PSR_F_BIT;

        new |= target_mode;

        *vcpu_cpsr(vcpu) = new;
        __vcpu_write_spsr(vcpu, target_mode, old);
}

/*
 * When an exception is taken, most CPSR fields are left unchanged in the
 * handler. However, some are explicitly overridden (e.g. M[4:0]).
 *
 * The SPSR/SPSR_ELx layouts differ, and the below is intended to work with
 * either format. Note: SPSR.J bit doesn't exist in SPSR_ELx, but this bit was
 * obsoleted by the ARMv7 virtualization extensions and is RES0.
 *
 * For the SPSR layout seen from AArch32, see:
 * - ARM DDI 0406C.d, page B1-1148
 * - ARM DDI 0487E.a, page G8-6264
 *
 * For the SPSR_ELx layout for AArch32 seen from AArch64, see:
 * - ARM DDI 0487E.a, page C5-426
 *
 * Here we manipulate the fields in order of the AArch32 SPSR_ELx layout, from
 * MSB to LSB.
 */
static unsigned long get_except32_cpsr(struct kvm_vcpu *vcpu, u32 mode)
{
        u32 sctlr = __vcpu_read_sys_reg(vcpu, SCTLR_EL1);
        unsigned long old, new;

        old = *vcpu_cpsr(vcpu);
        new = 0;

        new |= (old & PSR_AA32_N_BIT);
        new |= (old & PSR_AA32_Z_BIT);
        new |= (old & PSR_AA32_C_BIT);
        new |= (old & PSR_AA32_V_BIT);
        new |= (old & PSR_AA32_Q_BIT);

        // CPSR.IT[7:0] are set to zero upon any exception
        // See ARM DDI 0487E.a, section G1.12.3
        // See ARM DDI 0406C.d, section B1.8.3

        new |= (old & PSR_AA32_DIT_BIT);

        // CPSR.SSBS is set to SCTLR.DSSBS upon any exception
        // See ARM DDI 0487E.a, page G8-6244
        if (sctlr & BIT(31))
                new |= PSR_AA32_SSBS_BIT;

        // CPSR.PAN is unchanged unless SCTLR.SPAN == 0b0
        // SCTLR.SPAN is RES1 when ARMv8.1-PAN is not implemented
        // See ARM DDI 0487E.a, page G8-6246
        new |= (old & PSR_AA32_PAN_BIT);
        if (!(sctlr & BIT(23)))
                new |= PSR_AA32_PAN_BIT;

        // SS does not exist in AArch32, so ignore

        // CPSR.IL is set to zero upon any exception
        // See ARM DDI 0487E.a, page G1-5527

        new |= (old & PSR_AA32_GE_MASK);

        // CPSR.IT[7:0] are set to zero upon any exception
        // See prior comment above

        // CPSR.E is set to SCTLR.EE upon any exception
        // See ARM DDI 0487E.a, page G8-6245
        // See ARM DDI 0406C.d, page B4-1701
        if (sctlr & BIT(25))
                new |= PSR_AA32_E_BIT;

        // CPSR.A is unchanged upon an exception to Undefined, Supervisor
        // CPSR.A is set upon an exception to other modes
        // See ARM DDI 0487E.a, pages G1-5515 to G1-5516
        // See ARM DDI 0406C.d, page B1-1182
        new |= (old & PSR_AA32_A_BIT);
        if (mode != PSR_AA32_MODE_UND && mode != PSR_AA32_MODE_SVC)
                new |= PSR_AA32_A_BIT;

        // CPSR.I is set upon any exception
        // See ARM DDI 0487E.a, pages G1-5515 to G1-5516
        // See ARM DDI 0406C.d, page B1-1182
        new |= PSR_AA32_I_BIT;

        // CPSR.F is set upon an exception to FIQ
        // CPSR.F is unchanged upon an exception to other modes
        // See ARM DDI 0487E.a, pages G1-5515 to G1-5516
        // See ARM DDI 0406C.d, page B1-1182
        new |= (old & PSR_AA32_F_BIT);
        if (mode == PSR_AA32_MODE_FIQ)
                new |= PSR_AA32_F_BIT;

        // CPSR.T is set to SCTLR.TE upon any exception
        // See ARM DDI 0487E.a, page G8-5514
        // See ARM DDI 0406C.d, page B1-1181
        if (sctlr & BIT(30))
                new |= PSR_AA32_T_BIT;

        new |= mode;

        return new;
}

/*
 * Table taken from ARMv8 ARM DDI0487B-B, table G1-10.
 */
static const u8 return_offsets[8][2] = {
        [0] = { 0, 0 },         /* Reset, unused */
        [1] = { 4, 2 },         /* Undefined */
        [2] = { 0, 0 },         /* SVC, unused */
        [3] = { 4, 4 },         /* Prefetch abort */
        [4] = { 8, 8 },         /* Data abort */
        [5] = { 0, 0 },         /* HVC, unused */
        [6] = { 4, 4 },         /* IRQ, unused */
        [7] = { 4, 4 },         /* FIQ, unused */
};

static void enter_exception32(struct kvm_vcpu *vcpu, u32 mode, u32 vect_offset)
{
        unsigned long spsr = *vcpu_cpsr(vcpu);
        bool is_thumb = (spsr & PSR_AA32_T_BIT);
        u32 sctlr = __vcpu_read_sys_reg(vcpu, SCTLR_EL1);
        u32 return_address;

        *vcpu_cpsr(vcpu) = get_except32_cpsr(vcpu, mode);
        return_address   = *vcpu_pc(vcpu);
        return_address  += return_offsets[vect_offset >> 2][is_thumb];

        /* KVM only enters the ABT and UND modes, so only deal with those */
        switch(mode) {
        case PSR_AA32_MODE_ABT:
                __vcpu_write_spsr_abt(vcpu, host_spsr_to_spsr32(spsr));
                vcpu_gp_regs(vcpu)->compat_lr_abt = return_address;
                break;

        case PSR_AA32_MODE_UND:
                __vcpu_write_spsr_und(vcpu, host_spsr_to_spsr32(spsr));
                vcpu_gp_regs(vcpu)->compat_lr_und = return_address;
                break;
        }

        /* Branch to exception vector */
        if (sctlr & (1 << 13))
                vect_offset += 0xffff0000;
        else /* always have security exceptions */
                vect_offset += __vcpu_read_sys_reg(vcpu, VBAR_EL1);

        *vcpu_pc(vcpu) = vect_offset;
}

static void kvm_inject_exception(struct kvm_vcpu *vcpu)
{
        if (vcpu_el1_is_32bit(vcpu)) {
                switch (vcpu_get_flag(vcpu, EXCEPT_MASK)) {
                case unpack_vcpu_flag(EXCEPT_AA32_UND):
                        enter_exception32(vcpu, PSR_AA32_MODE_UND, 4);
                        break;
                case unpack_vcpu_flag(EXCEPT_AA32_IABT):
                        enter_exception32(vcpu, PSR_AA32_MODE_ABT, 12);
                        break;
                case unpack_vcpu_flag(EXCEPT_AA32_DABT):
                        enter_exception32(vcpu, PSR_AA32_MODE_ABT, 16);
                        break;
                default:
                        /* Err... */
                        break;
                }
        } else {
                switch (vcpu_get_flag(vcpu, EXCEPT_MASK)) {
                case unpack_vcpu_flag(EXCEPT_AA64_EL1_SYNC):
                        enter_exception64(vcpu, PSR_MODE_EL1h, except_type_sync);
                        break;

                case unpack_vcpu_flag(EXCEPT_AA64_EL1_SERR):
                        enter_exception64(vcpu, PSR_MODE_EL1h, except_type_serror);
                        break;

                case unpack_vcpu_flag(EXCEPT_AA64_EL2_SYNC):
                        enter_exception64(vcpu, PSR_MODE_EL2h, except_type_sync);
                        break;

                case unpack_vcpu_flag(EXCEPT_AA64_EL2_IRQ):
                        enter_exception64(vcpu, PSR_MODE_EL2h, except_type_irq);
                        break;

                case unpack_vcpu_flag(EXCEPT_AA64_EL2_SERR):
                        enter_exception64(vcpu, PSR_MODE_EL2h, except_type_serror);
                        break;

                default:
                        /*
                         * Only EL1_{SYNC,SERR} and EL2_{SYNC,IRQ,SERR} makes
                         * sense so far. Everything else gets silently
                         * ignored.
                         */
                        break;
                }
        }
}

/*
 * Adjust the guest PC (and potentially exception state) depending on
 * flags provided by the emulation code.
 */
void __kvm_adjust_pc(struct kvm_vcpu *vcpu)
{
        if (vcpu_get_flag(vcpu, PENDING_EXCEPTION)) {
                kvm_inject_exception(vcpu);
                vcpu_clear_flag(vcpu, PENDING_EXCEPTION);
                vcpu_clear_flag(vcpu, EXCEPT_MASK);
        } else if (vcpu_get_flag(vcpu, INCREMENT_PC)) {
                kvm_skip_instr(vcpu);
                vcpu_clear_flag(vcpu, INCREMENT_PC);
        }
}