arch/arm64/kvm/hyp/exception.c - linux - Git at Google

 // 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>

 #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)
 {
 	u64 val;

 	if (__vcpu_read_sys_reg_from_cpu(reg, &val))
 		return val;

 	return __vcpu_sys_reg(vcpu, reg);
 }

 static inline void __vcpu_write_sys_reg(struct kvm_vcpu *vcpu, u64 val, int reg)
 {
 	if (__vcpu_write_sys_reg_to_cpu(val, reg))
 		return;

 	 __vcpu_sys_reg(vcpu, reg) = val;
 }

 static void __vcpu_write_spsr(struct kvm_vcpu *vcpu, u64 val)
 {
 	if (has_vhe())
 		write_sysreg_el1(val, SYS_SPSR);
 	else
 		__vcpu_sys_reg(vcpu, SPSR_EL1) = val;
 }

 static void __vcpu_write_spsr_abt(struct kvm_vcpu *vcpu, u64 val)
 {
 	if (has_vhe())
 		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())
 		write_sysreg(val, spsr_und);
 	else
 		vcpu->arch.ctxt.spsr_und = val;
 }

 unsigned long get_except64_offset(unsigned long psr, unsigned long target_mode,
 				  enum exception_type type)
 {
 	u64 mode = psr & (PSR_MODE_MASK | PSR_MODE32_BIT);
 	u64 exc_offset;

 	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;

 	return exc_offset + type;
 }

 /*
  * 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.
  */
 unsigned long get_except64_cpsr(unsigned long old, bool has_mte,
 				unsigned long sctlr, unsigned long target_mode)
 {
 	u64 new = 0;

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

 	if (has_mte)
 		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;

 	return new;
 }

 /*
  * 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).
  */
 static void enter_exception64(struct kvm_vcpu *vcpu, unsigned long target_mode,
 			      enum exception_type type)
 {
 	u64 offset = get_except64_offset(*vcpu_cpsr(vcpu), target_mode, type);
 	unsigned long sctlr, vbar, old, new;

 	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;
 	default:
 		/* Don't do that */
 		BUG();
 	}

 	*vcpu_pc(vcpu) = vbar + offset;

 	old = *vcpu_cpsr(vcpu);
 	new = get_except64_cpsr(old, kvm_has_mte(kern_hyp_va(vcpu->kvm)), sctlr, target_mode);
 	*vcpu_cpsr(vcpu) = new;
 	__vcpu_write_spsr(vcpu, 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;
 		default:
 			/*
 			 * Only EL1_SYNC makes sense so far, EL2_{SYNC,IRQ}
 			 * will be implemented at some point. 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);
 	}
 }
	// 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>

	#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)
	{
	u64 val;

	if (__vcpu_read_sys_reg_from_cpu(reg, &val))
	return val;

	return __vcpu_sys_reg(vcpu, reg);
	}

	static inline void __vcpu_write_sys_reg(struct kvm_vcpu *vcpu, u64 val, int reg)
	{
	if (__vcpu_write_sys_reg_to_cpu(val, reg))
	return;

	__vcpu_sys_reg(vcpu, reg) = val;
	}

	static void __vcpu_write_spsr(struct kvm_vcpu *vcpu, u64 val)
	{
	if (has_vhe())
	write_sysreg_el1(val, SYS_SPSR);
	else
	__vcpu_sys_reg(vcpu, SPSR_EL1) = val;
	}

	static void __vcpu_write_spsr_abt(struct kvm_vcpu *vcpu, u64 val)
	{
	if (has_vhe())
	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())
	write_sysreg(val, spsr_und);
	else
	vcpu->arch.ctxt.spsr_und = val;
	}

	unsigned long get_except64_offset(unsigned long psr, unsigned long target_mode,
	enum exception_type type)
	{
	u64 mode = psr & (PSR_MODE_MASK \| PSR_MODE32_BIT);
	u64 exc_offset;

	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;

	return exc_offset + type;
	}

	/*
	* 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.
	*/
	unsigned long get_except64_cpsr(unsigned long old, bool has_mte,
	unsigned long sctlr, unsigned long target_mode)
	{
	u64 new = 0;

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

	if (has_mte)
	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;

	return new;
	}

	/*
	* 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).
	*/
	static void enter_exception64(struct kvm_vcpu *vcpu, unsigned long target_mode,
	enum exception_type type)
	{
	u64 offset = get_except64_offset(*vcpu_cpsr(vcpu), target_mode, type);
	unsigned long sctlr, vbar, old, new;

	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;
	default:
	/* Don't do that */
	BUG();
	}

	*vcpu_pc(vcpu) = vbar + offset;

	old = *vcpu_cpsr(vcpu);
	new = get_except64_cpsr(old, kvm_has_mte(kern_hyp_va(vcpu->kvm)), sctlr, target_mode);
	*vcpu_cpsr(vcpu) = new;
	__vcpu_write_spsr(vcpu, 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;
	default:
	/*
	* Only EL1_SYNC makes sense so far, EL2_{SYNC,IRQ}
	* will be implemented at some point. 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);
	}
	}