D60115.1794513115.diff
No OneTemporary
Actions

Authored By

Unknown

Size

61 KB

Referenced Files

None

Subscribers

None

D60115.1794513115.diff
View Options

	diff --git a/sys/arm64/arm64/gicv5.c b/sys/arm64/arm64/gicv5.c
	--- a/sys/arm64/arm64/gicv5.c
	+++ b/sys/arm64/arm64/gicv5.c
	@@ -69,9 +69,6 @@
	#include "gicv5var.h"
	#include "gic_v3_var.h" /* For GICV3_IVAR_NIRQS */

	-#define GICV5_PPIS_PER_REG 64
	-#define GICV5_PPI_COUNT 128
	-
	#define LPI_IPI_BASE 0
	#define LPI_IPI_LIMIT (LPI_IPI_BASE + (mp_maxid + 1) * INTR_IPI_COUNT)
	#define LPI_IS_IPI(lpi) ((lpi) < LPI_ITS_BASE)
	diff --git a/sys/arm64/arm64/gicv5reg.h b/sys/arm64/arm64/gicv5reg.h
	--- a/sys/arm64/arm64/gicv5reg.h
	+++ b/sys/arm64/arm64/gicv5reg.h
	@@ -662,6 +662,19 @@
	#define ICH_HFGRTR_EL2_CRn 12
	#define ICH_HFGRTR_EL2_CRm 9
	#define ICH_HFGRTR_EL2_op2 4
	+#define ICH_HFGRTR_EL2_ICC_PPI_ACTIVERn_EL1 (0x1ul << 20)
	+#define ICH_HFGRTR_EL2_ICC_PPI_PRIORITYn_EL1 (0x1ul << 19)
	+#define ICH_HFGRTR_EL2_ICC_PPI_PENDRn_EL1 (0x1ul << 18)
	+#define ICH_HFGRTR_EL2_ICC_PPI_ENABLERn_EL1 (0x1ul << 17)
	+#define ICH_HFGRTR_EL2_ICC_PPI_HMRn_EL1 (0x1ul << 16)
	+#define ICH_HFGRTR_EL2_ICC_IAFFIDR_EL1 (0x1ul << 7)
	+#define ICH_HFGRTR_EL2_ICC_ICSR_EL1 (0x1ul << 6)
	+#define ICH_HFGRTR_EL2_ICC_PCR_EL1 (0x1ul << 5)
	+#define ICH_HFGRTR_EL2_ICC_HPPIR_EL1 (0x1ul << 4)
	+#define ICH_HFGRTR_EL2_ICC_HAPR_EL1 (0x1ul << 3)
	+#define ICH_HFGRTR_EL2_ICC_CR0_EL1 (0x1ul << 2)
	+#define ICH_HFGRTR_EL2_ICC_IDRn_EL1 (0x1ul << 1)
	+#define ICH_HFGRTR_EL2_ICC_APR_EL1 (0x1ul << 0)

	/*
	* ICH_HFGWTR_EL2
	@@ -673,6 +686,14 @@
	#define ICH_HFGWTR_EL2_CRn 12
	#define ICH_HFGWTR_EL2_CRm 9
	#define ICH_HFGWTR_EL2_op2 6
	+#define ICH_HFGWTR_EL2_ICC_PPI_ACTIVERn_EL1 (0x1ul << 20)
	+#define ICH_HFGWTR_EL2_ICC_PPI_PRIORITYn_EL1 (0x1ul << 19)
	+#define ICH_HFGWTR_EL2_ICC_PPI_PENDRn_EL1 (0x1ul << 18)
	+#define ICH_HFGWTR_EL2_ICC_PPI_ENABLERn_EL1 (0x1ul << 17)
	+#define ICH_HFGWTR_EL2_ICC_ICSR_EL1 (0x1ul << 6)
	+#define ICH_HFGWTR_EL2_ICC_PCR_EL1 (0x1ul << 5)
	+#define ICH_HFGWTR_EL2_ICC_CR0_EL1 (0x1ul << 2)
	+#define ICH_HFGWTR_EL2_ICC_APR_EL1 (0x1ul << 0)

	/*
	* ICH_HPPIR_EL2
	@@ -858,7 +879,26 @@
	#define IRS_IDR7 0x001c
	#define IRS_IDR7_SPI_BASE 0x00ffffff
	#define IRS_IIDR 0x0040
	+#define IRS_IIDR_PRODUCT_ID_SHIFT 20
	+#define IRS_IIDR_PRODUCT_ID_MASK (0xfffu << IRS_IIDR_PRODUCT_ID_SHIFT)
	+#define IRS_IIDR_VARIANT_SHIFT 16
	+#define IRS_IIDR_VARIANT_MASK (0xfu << IRS_IIDR_VARIANT_SHIFT)
	+#define IRS_IIDR_REVISION_SHIFT 12
	+#define IRS_IIDR_REVISION_MASK (0xfu << IRS_IIDR_REVISION_SHIFT)
	+#define IRS_IIDR_IMPLEMENTER_SHIFT 0
	+#define IRS_IIDR_IMPLEMENTER_MASK (0xfffu << IRS_IIDR_IMPLEMENTED_SHIFT)
	#define IRS_AIDR 0x0044
	+#define IRS_AIDR_COMPONENT_SHIFT 0
	+#define IRS_AIDR_COMPONENT_IRS (0x0u << IRS_AIDR_COMPONENT_SHIFT)
	+#define IRS_AIDR_COMPONENT_ITS (0x1u << IRS_AIDR_COMPONENT_SHIFT)
	+#define IRS_AIDR_COMPONENT_IWB (0x2u << IRS_AIDR_COMPONENT_SHIFT)
	+#define IRS_AIDR_COMPONENT_MASK (0xfu << IRS_AIDR_COMPONENT_SHIFT)
	+#define IRS_AIDR_ARCH_MAJOR_REV_SHIFT 4
	+#define IRS_AIDR_ARCH_MAJOR_REV_V5 (0x0u << IRS_AIDR_ARCH_MAJOR_REV_SHIFT)
	+#define IRS_AIDR_ARCH_MAJOR_REV_MASK (0xfu << IRS_AIDR_ARCH_MAJOR_REV_SHIFT)
	+#define IRS_AIDR_ARCH_MINOR_REV_SHIFT 0
	+#define IRS_AIDR_ARCH_MINOR_REV_0 (0x0u << IRS_AIDR_ARCH_MINOR_REV_SHIFT)
	+#define IRS_AIDR_ARCH_MINOR_REV_MASK (0xfu << IRS_AIDR_ARCH_MINOR_REV_SHIFT)
	#define IRS_CR0 0x0080
	#define IRS_CR0_IDLE (0x1u << 1)
	#define IRS_CR0_IRSEN (0x1u << 0)
	@@ -904,6 +944,7 @@
	#define IRS_SYNC_STATUSR 0x00c4
	#define IRS_SPI_VMR 0x0100
	#define IRS_SPI_SELR 0x0108
	+#define IRS_SPI_SELR_ID_MASK (0xffffffu << 0)
	#define IRS_SPI_DOMAINR 0x010c
	#define IRS_SPI_RESAMPLER 0x0110
	#define IRS_SPI_CFGR 0x0114
	@@ -913,8 +954,14 @@
	#define IRS_SPI_STATUSR_V (0x1u << 1)
	#define IRS_SPI_STATUSR_IDLE (0x1u << 0)
	#define IRS_PE_SELR 0x0140
	+#define IRS_PE_SELR_IAFFID_SHIFT 0
	+#define IRS_PE_SELR_IAFFID_MASK (0xffffu << IRS_PE_SELR_IAFFID_SHIFT)
	#define IRS_PE_STATUSR 0x0144
	+#define IRS_PE_CR0_ONLINE (0x1 << 2)
	+#define IRS_PE_CR0_V (0x1 << 1)
	+#define IRS_PE_CR0_IDLE (0x1 << 0)
	#define IRS_PE_CR0 0x0148
	+#define IRS_PE_CR0_DPS (0x1 << 0)
	#define IRS_IST_BASER 0x0180
	#define IRS_IST_BASER_ADDR_LIMIT 0x0100000000000000ul
	#define IRS_IST_BASER_ADDR_MASK 0x00ffffffffffffc0ul
	diff --git a/sys/arm64/arm64/gicv5var.h b/sys/arm64/arm64/gicv5var.h
	--- a/sys/arm64/arm64/gicv5var.h
	+++ b/sys/arm64/arm64/gicv5var.h
	@@ -60,6 +60,9 @@
	struct gicv5_irs *di_irs;
	};

	+#define GICV5_PPIS_PER_REG 64
	+#define GICV5_PPI_COUNT 128
	+
	#define GICV5_IVAR_LPI_START 5000

	__BUS_ACCESSOR(gicv5, lpi_start, GICV5, LPI_START, u_int);
	diff --git a/sys/arm64/vmm/arm64.h b/sys/arm64/vmm/arm64.h
	--- a/sys/arm64/vmm/arm64.h
	+++ b/sys/arm64/vmm/arm64.h
	@@ -36,8 +36,8 @@
	#include "io/vgic_v3.h"
	#include "io/vtimer.h"

	-struct vgic_v3;
	-struct vgic_v3_cpu;
	+struct vgic;
	+struct vgic_cpu;

	/*
	Encode the index of a given register within VNCR into the enum value.
	@@ -149,6 +149,40 @@
	HOST_CNTHCTL_EL2,
	HOST_CNTVOFF_EL2,

	+
	+ /* GICv5 registers */
	+ HOST_ICH_PPI_ACTIVER0_EL2,
	+ HOST_ICH_PPI_ACTIVER1_EL2,
	+ HOST_ICH_PPI_DVIR0_EL2,
	+ HOST_ICH_PPI_DVIR1_EL2,
	+ HOST_ICH_PPI_ENABLER0_EL2,
	+ HOST_ICH_PPI_ENABLER1_EL2,
	+ HOST_ICH_PPI_PENDR0_EL2,
	+ HOST_ICH_PPI_PENDR1_EL2,
	+ HOST_ICH_PPI_PRIORITYR0_EL2,
	+ HOST_ICH_PPI_PRIORITYR1_EL2,
	+ HOST_ICH_PPI_PRIORITYR2_EL2,
	+ HOST_ICH_PPI_PRIORITYR3_EL2,
	+ HOST_ICH_PPI_PRIORITYR4_EL2,
	+ HOST_ICH_PPI_PRIORITYR5_EL2,
	+ HOST_ICH_PPI_PRIORITYR6_EL2,
	+ HOST_ICH_PPI_PRIORITYR7_EL2,
	+ HOST_ICH_PPI_PRIORITYR8_EL2,
	+ HOST_ICH_PPI_PRIORITYR9_EL2,
	+ HOST_ICH_PPI_PRIORITYR10_EL2,
	+ HOST_ICH_PPI_PRIORITYR11_EL2,
	+ HOST_ICH_PPI_PRIORITYR12_EL2,
	+ HOST_ICH_PPI_PRIORITYR13_EL2,
	+ HOST_ICH_PPI_PRIORITYR14_EL2,
	+ HOST_ICH_PPI_PRIORITYR15_EL2,
	+ HOST_ICH_HFGITR_EL2,
	+ HOST_ICH_HFGRTR_EL2,
	+ HOST_ICH_HFGWTR_EL2,
	+ HOST_ICH_VCTLR_EL2,
	+ HOST_ICH_HPPIR_EL2,
	+ HOST_ICH_APR_EL2,
	+ HOST_ICH_APR_RING_EL2,
	+
	NR_NON_VNCR_REGS,

	/* VNCR Registers */
	@@ -285,7 +319,8 @@
	uint16_t ich_lr_num;
	uint16_t ich_apr_num;
	} vgic_v3;
	- struct vgic_v3_cpu *vgic_cpu;
	+ struct vgic_cpu *vgic_cpu;
	+
	bool has_exception;
	bool dbg_oslock;

	@@ -346,12 +381,15 @@
	uint64_t cntvoff_el2; /* VM-wide virtual timer offset */
	uint64_t el2_addr; /* The address of this in el2 space */
	uint64_t feats; /* Which features are enabled */
	-#define HYP_FEAT_HCX (0x1ul << 0)
	-#define HYP_FEAT_ECV_POFF (0x1ul << 1)
	-#define HYP_FEAT_FGT (0x1ul << 2)
	-#define HYP_FEAT_FGT2 (0x1ul << 3)
	+#define HYP_FEAT_HCX (0x1ul << 0)
	+#define HYP_FEAT_ECV_POFF (0x1ul << 1)
	+#define HYP_FEAT_FGT (0x1ul << 2)
	+#define HYP_FEAT_FGT2 (0x1ul << 3)
	+#define HYP_FEAT_GICV3_OR_LEGACY (0x1ul << 4)
	+#define HYP_FEAT_GICV5 (0x1ul << 5)
	bool vgic_attached;
	- struct vgic_v3 *vgic;
	+ struct vgic *vgic;
	+ int vgic_ver;
	struct hypctx *ctx[];
	};

	@@ -365,5 +403,6 @@

	struct hypctx *arm64_get_active_vcpu(void);
	void raise_data_insn_abort(struct hypctx *, uint64_t, bool, int);
	+int vmm_get_max_ipa_bits(void);

	#endif /* !_VMM_ARM64_H_ */
	diff --git a/sys/arm64/vmm/io/vgic.h b/sys/arm64/vmm/io/vgic.h
	--- a/sys/arm64/vmm/io/vgic.h
	+++ b/sys/arm64/vmm/io/vgic.h
	@@ -36,6 +36,7 @@

	bool vgic_present(void);
	void vgic_init(void);
	+int vgic_get_version(void);
	int vgic_attach_to_vm(struct hyp hyp, struct vm_vgic_descr descr);
	void vgic_detach_from_vm(struct hyp *hyp);
	void vgic_vminit(struct hyp *hyp);
	diff --git a/sys/arm64/vmm/io/vgic.c b/sys/arm64/vmm/io/vgic.c
	--- a/sys/arm64/vmm/io/vgic.c
	+++ b/sys/arm64/vmm/io/vgic.c
	@@ -29,6 +29,11 @@
	#include <sys/types.h>
	#include <sys/systm.h>
	#include <sys/bus.h>
	+#include <sys/malloc.h>
	+
	+#include <dev/vmm/vmm_vm.h>
	+
	+#include <arm64/vmm/arm64.h>

	#include "vgic.h"
	#include "vgic_if.h"
	@@ -124,10 +129,34 @@
	VGIC_INIT(vgic_dev);
	}

	+int
	+vgic_get_version(void)
	+{
	+ return (VGIC_GET_VERSION(vgic_dev));
	+}
	+
	int
	vgic_attach_to_vm(struct hyp hyp, struct vm_vgic_descr descr)
	{
	- return (VGIC_ATTACH_TO_VM(vgic_dev, hyp, descr));
	+ int rv;
	+
	+ rv = VGIC_ATTACH_TO_VM(vgic_dev, hyp, descr);
	+ if (rv != 0)
	+ return (rv);
	+
	+ for (u_int cpu = 0; cpu < vm_get_maxcpus(hyp->vm); cpu++) {
	+ struct hypctx *hypctx;
	+
	+ hypctx = hyp->ctx[cpu];
	+ if (hypctx == NULL)
	+ continue;
	+ MPASS(hypctx->vgic_cpu != NULL);
	+ VGIC_CPUINIT(vgic_dev, hypctx);
	+ }
	+
	+ hyp->vgic_ver = hyp->vgic->vgic_ver;
	+
	+ return (0);
	}

	void
	@@ -139,25 +168,29 @@
	void
	vgic_vminit(struct hyp *hyp)
	{
	+ hyp->vgic = malloc(sizeof(*hyp->vgic), M_DEVBUF, M_WAITOK \| M_ZERO);
	VGIC_VMINIT(vgic_dev, hyp);
	}

	void
	vgic_cpuinit(struct hypctx *hypctx)
	{
	- VGIC_CPUINIT(vgic_dev, hypctx);
	+ hypctx->vgic_cpu = malloc(sizeof(*hypctx->vgic_cpu), M_DEVBUF,
	+ M_WAITOK \| M_ZERO);
	}

	void
	vgic_cpucleanup(struct hypctx *hypctx)
	{
	VGIC_CPUCLEANUP(vgic_dev, hypctx);
	+ free(hypctx->vgic_cpu, M_DEVBUF);
	}

	void
	vgic_vmcleanup(struct hyp *hyp)
	{
	VGIC_VMCLEANUP(vgic_dev, hyp);
	+ free(hyp->vgic, M_DEVBUF);
	}

	int
	diff --git a/sys/arm64/vmm/io/vgic_if.m b/sys/arm64/vmm/io/vgic_if.m
	--- a/sys/arm64/vmm/io/vgic_if.m
	+++ b/sys/arm64/vmm/io/vgic_if.m
	@@ -102,3 +102,7 @@
	device_t dev;
	struct hypctx *hypctx;
	}
	+
	+METHOD int get_version {
	+ device_t dev;
	+}
	diff --git a/sys/arm64/vmm/io/vgic_internal.h b/sys/arm64/vmm/io/vgic_internal.h
	--- a/sys/arm64/vmm/io/vgic_internal.h
	+++ b/sys/arm64/vmm/io/vgic_internal.h
	@@ -30,6 +30,9 @@
	#ifndef _VGIC_INTERNAL_H_
	#define _VGIC_INTERNAL_H_

	+struct hyp;
	+struct hypctx;
	+
	struct vgic_v3_irq {
	/* List of IRQs that are active or pending */
	TAILQ_ENTRY(vgic_v3_irq) act_pend_list;
	@@ -80,6 +83,48 @@
	u_int ich_lr_used;
	};

	+/* Global data not needed by EL2 */
	+struct vgic_v5 {
	+ struct mtx irs_mtx;
	+ uint64_t irs_start;
	+ size_t irs_end;
	+
	+ bool irs_en;
	+ uint32_t irs_cr1;
	+
	+ uint64_t irs_ist_baser;
	+ uint32_t irs_ist_cfgr;
	+
	+ int irs_pe_selr;
	+
	+ uint32_t irs_spi_statusr;
	+ uint32_t irs_spi_selr;
	+
	+ int max_cpu_count;
	+ int vm_id;
	+};
	+
	+/* Per-CPU data not needed by EL2 */
	+struct vgic_v5_cpu {
	+ bool dps;
	+};
	+
	+struct vgic {
	+ u_int vgic_ver;
	+
	+ union {
	+ struct vgic_v3 v3;
	+ struct vgic_v5 v5;
	+ };
	+};
	+
	+struct vgic_cpu {
	+ union {
	+ struct vgic_v3_cpu v3;
	+ struct vgic_v5_cpu v5;
	+ };
	+};
	+
	typedef void (register_read)(struct hypctx , u_int, uint64_t , void *);
	typedef void (register_write)(struct hypctx *, u_int, u_int, u_int,
	uint64_t, void *);
	@@ -88,7 +133,7 @@
	register_write vgic_ignore_write;

	#define VGIC_8_BIT (1 << 0)
	-/* (1 << 1) is reserved for 16 bit accesses */
	+#define VGIC_16_BIT (1 << 1)
	#define VGIC_32_BIT (1 << 2)
	#define VGIC_64_BIT (1 << 3)

	@@ -129,4 +174,18 @@
	bool vgic_register_write(struct hypctx , struct vgic_register , u_int, u_int,
	u_int, uint64_t, void *);

	+void vgic_v3_vminit(device_t, struct hyp *);
	+void vgic_v3_cpuinit(device_t, struct hypctx *);
	+void vgic_v3_cpucleanup(device_t, struct hypctx *);
	+void vgic_v3_vmcleanup(device_t, struct hyp *);
	+int vgic_v3_max_cpu_count(device_t, struct hyp *);
	+int vgic_v3_attach_to_vm(device_t, struct hyp , struct vm_vgic_descr );
	+void vgic_v3_detach_from_vm(device_t, struct hyp *);
	+bool vgic_v3_has_pending_irq(device_t, struct hypctx *);
	+int vgic_v3_inject_irq(device_t, struct hyp *, int, uint32_t, bool);
	+int vgic_v3_inject_msi(device_t dev, struct hyp *, uint64_t, uint64_t);
	+void vgic_v3_flush_hwstate(device_t, struct hypctx *);
	+void vgic_v3_sync_hwstate(device_t, struct hypctx *);
	+void vgic_v3_init(device_t);
	+
	#endif /* _VGIC_INTERNAL_H_ */
	diff --git a/sys/arm64/vmm/io/vgic_v3.c b/sys/arm64/vmm/io/vgic_v3.c
	--- a/sys/arm64/vmm/io/vgic_v3.c
	+++ b/sys/arm64/vmm/io/vgic_v3.c
	@@ -93,17 +93,11 @@
	size_t ich_apr_num;
	};

	-
	/* How many IRQs we support (SGIs + PPIs + SPIs). Not including LPIs */
	#define VGIC_NIRQS 1023
	/* Pretend to be an Arm design */
	#define VGIC_IIDR 0x43b

	-static vgic_inject_irq_t vgic_v3_inject_irq;
	-static vgic_inject_msi_t vgic_v3_inject_msi;
	-
	-static int vgic_v3_max_cpu_count(device_t dev, struct hyp *hyp);
	-
	#define INJECT_IRQ(hyp, vcpuid, irqid, level) \
	vgic_v3_inject_irq(NULL, (hyp), (vcpuid), (irqid), (level))

	@@ -346,40 +340,19 @@
	return (-1);
	}

	-static void
	+void
	vgic_v3_vminit(device_t dev, struct hyp *hyp)
	{
	- struct vgic_v3 *vgic;
	-
	- hyp->vgic = malloc(sizeof(*hyp->vgic), M_VGIC_V3,
	- M_WAITOK \| M_ZERO);
	- vgic = hyp->vgic;
	-
	- /*
	- * Configure the Distributor control register. The register resets to an
	- * architecturally UNKNOWN value, so we reset to 0 to disable all
	- * functionality controlled by the register.
	- *
	- * The exception is GICD_CTLR.DS, which is RA0/WI when the Distributor
	- * supports one security state (ARM GIC Architecture Specification for
	- * GICv3 and GICv4, p. 4-464)
	- */
	- vgic->gicd_ctlr = 0;
	-
	- mtx_init(&vgic->dist_mtx, "VGICv3 Distributor lock", NULL,
	- MTX_SPIN);
	}

	-static void
	+void
	vgic_v3_cpuinit(device_t dev, struct hypctx *hypctx)
	{
	struct vgic_v3_cpu *vgic_cpu;
	struct vgic_v3_irq *irq;
	int i, irqid;

	- hypctx->vgic_cpu = malloc(sizeof(*hypctx->vgic_cpu),
	- M_VGIC_V3, M_WAITOK \| M_ZERO);
	- vgic_cpu = hypctx->vgic_cpu;
	+ vgic_cpu = &hypctx->vgic_cpu->v3;

	mtx_init(&vgic_cpu->lr_mtx, "VGICv3 ICH_LR_EL2 lock", NULL, MTX_SPIN);

	@@ -447,39 +420,36 @@
	hypctx->vgic_v3.ich_apr_num = virt_features.ich_apr_num;
	}

	-static void
	+void
	vgic_v3_cpucleanup(device_t dev, struct hypctx *hypctx)
	{
	struct vgic_v3_cpu *vgic_cpu;
	struct vgic_v3_irq *irq;
	int irqid;

	- vgic_cpu = hypctx->vgic_cpu;
	+ vgic_cpu = &hypctx->vgic_cpu->v3;
	for (irqid = 0; irqid < VGIC_PRV_I_NUM; irqid++) {
	irq = &vgic_cpu->private_irqs[irqid];
	mtx_destroy(&irq->irq_spinmtx);
	}

	mtx_destroy(&vgic_cpu->lr_mtx);
	- free(hypctx->vgic_cpu->private_irqs, M_VGIC_V3);
	- free(hypctx->vgic_cpu, M_VGIC_V3);
	+ free(vgic_cpu->private_irqs, M_VGIC_V3);
	}

	-static void
	+void
	vgic_v3_vmcleanup(device_t dev, struct hyp *hyp)
	{
	- mtx_destroy(&hyp->vgic->dist_mtx);
	- free(hyp->vgic, M_VGIC_V3);
	}

	-static int
	+int
	vgic_v3_max_cpu_count(device_t dev, struct hyp *hyp)
	{
	struct vgic_v3 *vgic;
	size_t count;
	int16_t max_count;

	- vgic = hyp->vgic;
	+ vgic = &hyp->vgic->v3;
	max_count = vm_get_maxcpus(hyp->vm);

	/* No registers, assume the maximum CPUs */
	@@ -659,7 +629,7 @@
	target_hypctx = hyp->ctx[target_vcpu];
	if (target_hypctx == NULL)
	goto next_irq;
	- vgic_cpu = target_hypctx->vgic_cpu;
	+ vgic_cpu = &target_hypctx->vgic_cpu->v3;

	if (!set) {
	/* pending -> not pending */
	@@ -735,7 +705,7 @@
	target_hypctx = hyp->ctx[target_vcpu];
	if (target_hypctx == NULL)
	goto next_irq;
	- vgic_cpu = target_hypctx->vgic_cpu;
	+ vgic_cpu = &target_hypctx->vgic_cpu->v3;

	if (!set) {
	/* active -> not active */
	@@ -895,16 +865,20 @@
	{
	struct hyp *hyp;
	struct vgic_v3 *vgic;
	+ uint64_t val;

	hyp = hypctx->hyp;
	- vgic = hyp->vgic;
	+ vgic = &hyp->vgic->v3;

	- mtx_lock_spin(&vgic->dist_mtx);
	- *rval = vgic->gicd_ctlr;
	- mtx_unlock_spin(&vgic->dist_mtx);
	+ val = atomic_load_32(&vgic->gicd_ctlr);
	+
	+ /* No GICv2 emulation, so this is RAO */
	+ val \|= GICD_CTLR_ARE_NS;

	/* Writes are never pending */
	- *rval &= ~GICD_CTLR_RWP;
	+ val &= ~GICD_CTLR_RWP;
	+
	+ *rval = val;
	}

	static void
	@@ -915,7 +889,7 @@

	MPASS(offset == 0);
	MPASS(size == 4);
	- vgic = hypctx->hyp->vgic;
	+ vgic = &hypctx->hyp->vgic->v3;

	/*
	* GICv2 backwards compatibility is not implemented so
	@@ -929,10 +903,8 @@
	*/
	wval &= GICD_CTLR_G1A;
	wval \|= GICD_CTLR_ARE_NS;
	- mtx_lock_spin(&vgic->dist_mtx);
	- vgic->gicd_ctlr = wval;
	+ atomic_store_32(&vgic->gicd_ctlr, wval);
	/* TODO: Wake any vcpus that have interrupts pending */
	- mtx_unlock_spin(&vgic->dist_mtx);
	}

	/* GICD_TYPER */
	@@ -1216,7 +1188,7 @@

	hypctx = vcpu_get_cookie(vcpu);
	hyp = hypctx->hyp;
	- vgic = hyp->vgic;
	+ vgic = &hyp->vgic->v3;

	/* Check the register is one of ours and is the correct size */
	if (fault_ipa < vgic->dist_start \|\| fault_ipa + size > vgic->dist_end) {
	@@ -1254,7 +1226,7 @@

	hypctx = vcpu_get_cookie(vcpu);
	hyp = hypctx->hyp;
	- vgic = hyp->vgic;
	+ vgic = &hyp->vgic->v3;

	/* Check the register is one of ours and is the correct size */
	if (fault_ipa < vgic->dist_start \|\| fault_ipa + size > vgic->dist_end) {
	@@ -1461,7 +1433,7 @@
	/* Find the current vcpu ctx to get the vgic struct */
	hypctx = vcpu_get_cookie(vcpu);
	hyp = hypctx->hyp;
	- vgic = hyp->vgic;
	+ vgic = &hyp->vgic->v3;

	/* Check the register is one of ours and is the correct size */
	if (fault_ipa < vgic->redist_start \|\|
	@@ -1539,7 +1511,7 @@
	/* Find the current vcpu ctx to get the vgic struct */
	hypctx = vcpu_get_cookie(vcpu);
	hyp = hypctx->hyp;
	- vgic = hyp->vgic;
	+ vgic = &hyp->vgic->v3;

	/* Check the register is one of ours and is the correct size */
	if (fault_ipa < vgic->redist_start \|\|
	@@ -1675,7 +1647,7 @@
	int i;

	/* Allocate memory for the SPIs */
	- vgic = hyp->vgic;
	+ vgic = &hyp->vgic->v3;
	vgic->irqs = malloc((VGIC_NIRQS - VGIC_PRV_I_NUM) *
	sizeof(*vgic->irqs), M_VGIC_V3, M_WAITOK \| M_ZERO);

	@@ -1696,7 +1668,7 @@
	struct vgic_v3_irq *irq;
	int i;

	- vgic = hyp->vgic;
	+ vgic = &hyp->vgic->v3;
	for (i = 0; i < VGIC_NIRQS - VGIC_PRV_I_NUM; i++) {
	irq = &vgic->irqs[i];

	@@ -1706,7 +1678,7 @@
	free(vgic->irqs, M_VGIC_V3);
	}

	-static int
	+int
	vgic_v3_attach_to_vm(device_t dev, struct hyp hyp, struct vm_vgic_descr descr)
	{
	struct vm *vm;
	@@ -1741,7 +1713,8 @@
	if (cpu_count > vm_get_maxcpus(vm))
	return (EINVAL);

	- vgic = hyp->vgic;
	+ hyp->vgic->vgic_ver = 3;
	+ vgic = &hyp->vgic->v3;

	/* Set the distributor address and size for trapping guest access. */
	vgic->dist_start = descr->v3_regs.dist_start;
	@@ -1767,7 +1740,7 @@
	return (0);
	}

	-static void
	+void
	vgic_v3_detach_from_vm(device_t dev, struct hyp *hyp)
	{
	if (hyp->vgic_attached) {
	@@ -1789,13 +1762,13 @@
	hypctx = hyp->ctx[vcpuid];
	if (hypctx == NULL)
	return (NULL);
	- vgic_cpu = hypctx->vgic_cpu;
	+ vgic_cpu = &hypctx->vgic_cpu->v3;
	irq = &vgic_cpu->private_irqs[irqid];
	} else if (irqid <= GIC_LAST_SPI) {
	irqid -= VGIC_PRV_I_NUM;
	if (irqid >= VGIC_NIRQS)
	return (NULL);
	- irq = &hyp->vgic->irqs[irqid];
	+ irq = &hyp->vgic->v3.irqs[irqid];
	} else if (irqid < GIC_FIRST_LPI) {
	return (NULL);
	} else {
	@@ -1814,13 +1787,13 @@
	mtx_unlock_spin(&irq->irq_spinmtx);
	}

	-static bool
	+bool
	vgic_v3_has_pending_irq(device_t dev, struct hypctx *hypctx)
	{
	struct vgic_v3_cpu *vgic_cpu;
	bool empty;

	- vgic_cpu = hypctx->vgic_cpu;
	+ vgic_cpu = &hypctx->vgic_cpu->v3;
	mtx_lock_spin(&vgic_cpu->lr_mtx);
	empty = TAILQ_EMPTY(&vgic_cpu->irq_act_pend);
	mtx_unlock_spin(&vgic_cpu->lr_mtx);
	@@ -1848,7 +1821,7 @@
	return (false);
	}

	-static int
	+int
	vgic_v3_inject_irq(device_t dev, struct hyp *hyp, int vcpuid, uint32_t irqid,
	bool level)
	{
	@@ -1894,7 +1867,7 @@
	}

	notify = false;
	- vgic_cpu = hypctx->vgic_cpu;
	+ vgic_cpu = &hypctx->vgic_cpu->v3;

	mtx_lock_spin(&vgic_cpu->lr_mtx);

	@@ -1919,13 +1892,13 @@
	return (0);
	}

	-static int
	+int
	vgic_v3_inject_msi(device_t dev, struct hyp *hyp, uint64_t msg, uint64_t addr)
	{
	struct vgic_v3 *vgic;
	uint64_t reg;

	- vgic = hyp->vgic;
	+ vgic = &hyp->vgic->v3;

	/* This is a 4 byte register */
	if (addr < vgic->dist_start \|\| addr + 4 > vgic->dist_end) {
	@@ -1939,14 +1912,14 @@
	return (INJECT_IRQ(hyp, -1, msg, true));
	}

	-static void
	+void
	vgic_v3_flush_hwstate(device_t dev, struct hypctx *hypctx)
	{
	struct vgic_v3_cpu *vgic_cpu;
	struct vgic_v3_irq *irq;
	int i;

	- vgic_cpu = hypctx->vgic_cpu;
	+ vgic_cpu = &hypctx->vgic_cpu->v3;

	/*
	* All Distributor writes have been executed at this point, do not
	@@ -2021,7 +1994,7 @@
	mtx_unlock_spin(&vgic_cpu->lr_mtx);
	}

	-static void
	+void
	vgic_v3_sync_hwstate(device_t dev, struct hypctx *hypctx)
	{
	struct vgic_v3_cpu *vgic_cpu;
	@@ -2029,7 +2002,7 @@
	uint64_t lr;
	int i;

	- vgic_cpu = hypctx->vgic_cpu;
	+ vgic_cpu = &hypctx->vgic_cpu->v3;

	/* Exit early if there are no buffered interrupts */
	if (vgic_cpu->ich_lr_used == 0)
	@@ -2093,7 +2066,7 @@
	vgic_cpu->ich_lr_used = 0;
	}

	-static void
	+void
	vgic_v3_init(device_t dev)
	{
	uint64_t ich_vtr_el2;
	@@ -2139,11 +2112,14 @@
	static int
	vgic_v3_probe(device_t dev)
	{
	+ u_int rev;
	+
	if (!gic_get_vgic(dev))
	return (EINVAL);

	- /* We currently only support the GICv3 */
	- if (gic_get_hw_rev(dev) < 3)
	+ /* We currently only support the GICv3/v4 */
	+ rev = gic_get_hw_rev(dev);
	+ if (rev < 3 \|\| rev > 4)
	return (EINVAL);

	device_set_desc(dev, "Virtual GIC v3");
	@@ -2164,6 +2140,12 @@
	return (0);
	}

	+static int
	+vgic_v3_get_version(device_t dev)
	+{
	+ return (3);
	+}
	+
	static device_method_t vgic_v3_methods[] = {
	/* Device interface */
	DEVMETHOD(device_probe, vgic_v3_probe),
	@@ -2184,6 +2166,7 @@
	DEVMETHOD(vgic_inject_msi, vgic_v3_inject_msi),
	DEVMETHOD(vgic_flush_hwstate, vgic_v3_flush_hwstate),
	DEVMETHOD(vgic_sync_hwstate, vgic_v3_sync_hwstate),
	+ DEVMETHOD(vgic_get_version, vgic_v3_get_version),

	/* End */
	DEVMETHOD_END
	diff --git a/sys/arm64/vmm/io/vgic_v5.c b/sys/arm64/vmm/io/vgic_v5.c
	new file mode 100644
	--- /dev/null
	+++ b/sys/arm64/vmm/io/vgic_v5.c
	@@ -0,0 +1,906 @@
	+/*-
	+ * SPDX-License-Identifier: BSD-2-Clause
	+ *
	+ * Copyright (C) 2018 Alexandru Elisei <alexandru.elisei@gmail.com>
	+ * Copyright (C) 2020-2022 Andrew Turner
	+ * Copyright (C) 2023 Arm Ltd
	+ *
	+ * 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 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 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/types.h>
	+#include <sys/errno.h>
	+#include <sys/systm.h>
	+#include <sys/bitstring.h>
	+#include <sys/bus.h>
	+#include <sys/kernel.h>
	+#include <sys/lock.h>
	+#include <sys/malloc.h>
	+#include <sys/module.h>
	+#include <sys/mutex.h>
	+#include <sys/rman.h>
	+#include <sys/smp.h>
	+#include <sys/intr.h>
	+
	+#include <vm/vm.h>
	+#include <vm/pmap.h>
	+
	+#include <dev/ofw/openfirm.h>
	+
	+#include <machine/atomic.h>
	+#include <machine/bus.h>
	+#include <machine/cpufunc.h>
	+#include <machine/cpu.h>
	+#include <machine/machdep.h>
	+#include <machine/param.h>
	+#include <machine/pmap.h>
	+#include <machine/vmparam.h>
	+#include <machine/intr.h>
	+#include <machine/vmm.h>
	+#include <machine/vmm_instruction_emul.h>
	+
	+#include <arm/arm/gic_common.h>
	+
	+#include <arm64/arm64/gicv5reg.h>
	+#include <arm64/arm64/gicv5var.h>
	+
	+#include <arm64/vmm/hyp.h>
	+#include <arm64/vmm/mmu.h>
	+#include <arm64/vmm/arm64.h>
	+#include <arm64/vmm/vmm_handlers.h>
	+
	+#include <dev/vmm/vmm_dev.h>
	+#include <dev/vmm/vmm_vm.h>
	+
	+#include "vgic.h"
	+#include "vgic_internal.h"
	+
	+#include "vgic_if.h"
	+
	+MALLOC_DEFINE(M_VGIC_V5, "ARM VMM VGIC V5", "ARM VMM VGIC V5");
	+
	+#define SPI_COUNT 4096
	+
	+#define VGIC_IAFFID_BITS 16
	+#define VGIC_INTID_BITS 12
	+#define VGIC_PRI_BITS 5
	+#define VGIC_MIN_LPI_BITS 8
	+
	+static int vgic_v5_max_cpu_count(device_t dev, struct hyp *hyp);
	+
	+static bool
	+has_gcie_legacy(void)
	+{
	+ uint64_t idr0;
	+
	+ get_kernel_reg(ICC_IDR0_EL1, &idr0);
	+
	+ return ((idr0 & ICC_IDR0_GCIE_LEGACY_MASK) ? true : false);
	+}
	+
	+static void
	+vgic_v5_vminit(device_t dev, struct hyp *hyp)
	+{
	+ struct vgic_v5 *vgic;
	+
	+ vgic = &hyp->vgic->v5;
	+ vgic->max_cpu_count = vgic_v5_max_cpu_count(dev, hyp);
	+}
	+
	+static void
	+vgic_v5_cpuinit(device_t dev, struct hypctx *hypctx)
	+{
	+ if (hypctx->hyp->vgic->vgic_ver == 3) {
	+ vgic_v3_cpuinit(dev, hypctx);
	+ return;
	+ }
	+
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_HFGITR_EL2,
	+ ICH_HFGITR_EL2_GICCDPEND \|
	+ ICH_HFGITR_EL2_GICCDEN \|
	+ ICH_HFGITR_EL2_GICCDAFF \|
	+ ICH_HFGITR_EL2_GICCDPRI \|
	+ ICH_HFGITR_EL2_GICRCDIA \|
	+ ICH_HFGITR_EL2_GICCDDI \|
	+ ICH_HFGITR_EL2_GICCDDIS \|
	+ ICH_HFGITR_EL2_GICCDEOI \|
	+ ICH_HFGITR_EL2_GICCDHM \|
	+ ICH_HFGITR_EL2_GICCDRCFG);
	+
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_HFGWTR_EL2,
	+ ICH_HFGWTR_EL2_ICC_PPI_ACTIVERn_EL1 \|
	+ ICH_HFGWTR_EL2_ICC_PPI_PRIORITYn_EL1 \|
	+ ICH_HFGWTR_EL2_ICC_PPI_PENDRn_EL1 \|
	+ ICH_HFGWTR_EL2_ICC_PPI_ENABLERn_EL1 \|
	+ ICH_HFGWTR_EL2_ICC_ICSR_EL1 \|
	+ ICH_HFGWTR_EL2_ICC_PCR_EL1 \|
	+ ICH_HFGWTR_EL2_ICC_CR0_EL1);
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_HFGRTR_EL2,
	+ hypctx_read_sys_reg(hypctx, HOST_ICH_HFGWTR_EL2) \|
	+ ICH_HFGRTR_EL2_ICC_PPI_HMRn_EL1);
	+}
	+
	+static void
	+vgic_v5_cpucleanup(device_t dev, struct hypctx *hypctx)
	+{
	+ if (hypctx->hyp->vgic->vgic_ver == 3) {
	+ vgic_v3_cpucleanup(dev, hypctx);
	+ return;
	+ }
	+}
	+
	+static void
	+vgic_v5_vmcleanup(device_t dev, struct hyp *hyp)
	+{
	+}
	+
	+static int
	+vgic_v5_max_cpu_count(device_t dev, struct hyp *hyp)
	+{
	+ switch (hyp->vgic->vgic_ver) {
	+ case 0:
	+ /* No GIC attached, return the maximum */
	+ return (vm_get_maxcpus(hyp->vm));
	+ case 3:
	+ return (vgic_v3_max_cpu_count(dev, hyp));
	+ case 5:
	+ return (gicv5_get_max_vpe_count(device_get_parent(dev)));
	+ default:
	+ panic("%s: Invalid GIC version %u", __func__,
	+ hyp->vgic->vgic_ver);
	+ }
	+}
	+
	+static int
	+vgic_v5_icc_iaffidr_read(struct vcpu vcpu, uint64_t rval, void *arg)
	+{
	+ *rval = vcpu_vcpuid(vcpu);
	+ return (0);
	+}
	+
	+static int
	+vgic_v5_icc_idr0_read(struct vcpu vcpu, uint64_t rval, void *arg)
	+{
	+ *rval = ICC_IDR0_ID_BITS_16 \| ICC_IDR0_PRI_BITS_5 \|
	+ ICC_IDR0_GCIE_LEGACY_NONE;
	+ return (0);
	+}
	+
	+static int
	+vgic_v5_write_ignore(struct vcpu vcpu, uint64_t rval, void arg)
	+{
	+ return (0);
	+}
	+
	+/* IRS_IDR0..7 */
	+static register_read irs_idr_read;
	+/* IRS_IIDR */
	+static register_read irs_iidr_read;
	+/* IRS_AIDR */
	+static register_read irs_aidr_read;
	+/* IRS_CR0 */
	+static register_read irs_cr0_read;
	+static register_write irs_cr0_write;
	+/* IRS_CR1 */
	+static register_read irs_cr1_read;
	+static register_write irs_cr1_write;
	+/* IRS_SPI_SELR */
	+static register_write irs_spi_selr_write;
	+/* IRS_SPI_CFGR */
	+static register_read irs_spi_cfgr_read;
	+static register_write irs_spi_cfgr_write;
	+/* IRS_SPI_STATUSR */
	+static register_read irs_spi_statusr_read;
	+/* IRS_PE_SELR */
	+static register_write irs_pe_selr_write;
	+/* IRS_PE_STATUSR */
	+static register_read irs_pe_statusr_read;
	+/* IRS_PE_CR0 */
	+static register_read irs_pe_cr0_read;
	+static register_write irs_pe_cr0_write;
	+/* IRS_IST_BASER */
	+static register_read irs_ist_baser_read;
	+static register_write irs_ist_baser_write;
	+/* IRS_IST_CFGR */
	+static register_read irs_ist_cfgr_read;
	+static register_write irs_ist_cfgr_write;
	+/* IRS_IST_STATUSR */
	+static register_read irs_ist_statusr_read;
	+
	+static struct vgic_register irs_registers[] = {
	+ VGIC_REGISTER_RANGE(IRS_IDR0, IRS_IDR7 + 4, 4, VGIC_32_BIT,
	+ irs_idr_read, vgic_ignore_write),
	+
	+ VGIC_REGISTER(IRS_IIDR, 4, VGIC_32_BIT, irs_iidr_read,
	+ vgic_ignore_write),
	+ VGIC_REGISTER(IRS_AIDR, 4, VGIC_32_BIT, irs_aidr_read,
	+ vgic_ignore_write),
	+
	+ VGIC_REGISTER(IRS_CR0, 4, VGIC_32_BIT, irs_cr0_read, irs_cr0_write),
	+ VGIC_REGISTER(IRS_CR1, 4, VGIC_32_BIT, irs_cr1_read, irs_cr1_write),
	+ VGIC_REGISTER(IRS_SPI_SELR, 4, VGIC_32_BIT, vgic_zero_read,
	+ irs_spi_selr_write),
	+ VGIC_REGISTER(IRS_SPI_CFGR, 4, VGIC_32_BIT, irs_spi_cfgr_read,
	+ irs_spi_cfgr_write),
	+ VGIC_REGISTER(IRS_SPI_STATUSR, 4, VGIC_32_BIT, irs_spi_statusr_read,
	+ vgic_ignore_write),
	+
	+ VGIC_REGISTER(IRS_PE_SELR, 4, VGIC_32_BIT, vgic_zero_read,
	+ irs_pe_selr_write),
	+ VGIC_REGISTER(IRS_PE_STATUSR, 4, VGIC_32_BIT, irs_pe_statusr_read,
	+ vgic_ignore_write),
	+ VGIC_REGISTER(IRS_PE_CR0, 4, VGIC_32_BIT, irs_pe_cr0_read,
	+ irs_pe_cr0_write),
	+
	+
	+ VGIC_REGISTER(IRS_IST_BASER, 8, VGIC_64_BIT \| VGIC_32_BIT,
	+ irs_ist_baser_read, irs_ist_baser_write),
	+ VGIC_REGISTER(IRS_IST_CFGR, 4, VGIC_32_BIT, irs_ist_cfgr_read,
	+ irs_ist_cfgr_write),
	+ VGIC_REGISTER(IRS_IST_STATUSR, 4, VGIC_32_BIT, irs_ist_statusr_read,
	+ vgic_ignore_write),
	+};
	+
	+static int
	+irs_read(struct vcpu vcpu, uint64_t fault_ipa, uint64_t rval,
	+ int size, void *arg)
	+{
	+ struct hyp *hyp;
	+ struct hypctx *hypctx;
	+ struct vgic_v5 *vgic;
	+ uint64_t reg;
	+
	+ hypctx = vcpu_get_cookie(vcpu);
	+ hyp = hypctx->hyp;
	+ vgic = &hyp->vgic->v5;
	+
	+ /* Check the register is one of ours and is the correct size */
	+ if (fault_ipa < vgic->irs_start \|\| fault_ipa + size > vgic->irs_end) {
	+ return (EINVAL);
	+ }
	+
	+ reg = fault_ipa - vgic->irs_start;
	+
	+ /*
	+ * Misaligned reads are an illegal access. As per RVXDYH they are RAZ.
	+ */
	+ if ((reg & (size - 1)) != 0) {
	+ *rval = 0;
	+ return (0);
	+ }
	+
	+ if (vgic_register_read(hypctx, irs_registers, nitems(irs_registers),
	+ reg, size, rval, NULL))
	+ return (0);
	+
	+ panic("%s: TODO %lx\n", __func__, reg);
	+
	+ /* Other register addresses are RAZ */
	+ *rval = 0;
	+
	+ return (0);
	+}
	+
	+static int
	+irs_write(struct vcpu *vcpu, uint64_t fault_ipa, uint64_t wval,
	+ int size, void *arg)
	+{
	+ struct hyp *hyp;
	+ struct hypctx *hypctx;
	+ struct vgic_v5 *vgic;
	+ uint64_t reg;
	+
	+ hypctx = vcpu_get_cookie(vcpu);
	+ hyp = hypctx->hyp;
	+ vgic = &hyp->vgic->v5;
	+
	+ /* Check the register is one of ours and is the correct size */
	+ if (fault_ipa < vgic->irs_start \|\| fault_ipa + size > vgic->irs_end) {
	+ return (EINVAL);
	+ }
	+
	+ reg = fault_ipa - vgic->irs_start;
	+
	+ /*
	+ * Misaligned writes are an illegal access. As per RVXDYH they are WI.
	+ */
	+ if ((reg & (size - 1)) != 0)
	+ return (0);
	+
	+ if (vgic_register_write(hypctx, irs_registers, nitems(irs_registers),
	+ reg, size, wval, NULL))
	+ return (0);
	+
	+ panic("%s: TODO %lx\n", __func__, reg);
	+
	+ /* Other register addresses are WI */
	+ return (0);
	+}
	+
	+static uint32_t
	+get_pa_range(int ipa_bits)
	+{
	+ switch (ipa_bits) {
	+ case 32:
	+ return (IRS_IDR0_PA_RANGE_4G);
	+ case 36:
	+ return (IRS_IDR0_PA_RANGE_64G);
	+ case 40:
	+ return (IRS_IDR0_PA_RANGE_1T);
	+ case 42:
	+ return (IRS_IDR0_PA_RANGE_4T);
	+ case 44:
	+ return (IRS_IDR0_PA_RANGE_16T);
	+ case 48:
	+ return (IRS_IDR0_PA_RANGE_256T);
	+ case 52:
	+ return (IRS_IDR0_PA_RANGE_4P);
	+ case 56:
	+ return (IRS_IDR0_PA_RANGE_64P);
	+ }
	+
	+ panic("%s: unsupported IPA size %u bits\n", __func__, ipa_bits);
	+
	+ return (0);
	+}
	+
	+/*
	+ * IRS registers
	+ */
	+static void
	+irs_idr_read(struct hypctx hypctx, u_int reg, uint64_t rval, void *arg)
	+{
	+ struct hyp *hyp;
	+ int ipa_bits;
	+ int n;
	+
	+ hyp = hypctx->hyp;
	+
	+ n = (reg - IRS_IDR0) / 4;
	+ switch (n) {
	+ case 0:
	+ /* Match PA range in use by the VM. */
	+ ipa_bits = vmm_get_max_ipa_bits();
	+ *rval = get_pa_range(ipa_bits);
	+ break;
	+ case 1:
	+ *rval = ((VGIC_PRI_BITS - 1) << IRS_IDR0_PRI_BITS_SHIFT) \|
	+ ((VGIC_IAFFID_BITS - 1) << IRS_IDR0_IAFFID_BITS_SHIFT) \|
	+ (vm_get_maxcpus(hyp->vm) << IRS_IDR0_PE_CNT_SHIFT);
	+ break;
	+ case 2:
	+ *rval = IRS_IDR2_NO_ISTMD \|
	+ IRS_IDR2_IST_LEVELS_SINGLE \|
	+ (VGIC_MIN_LPI_BITS << IRS_IDR2_MIN_LPI_ID_BITS_SHIFT) \|
	+ IRS_IDR2_LPI \|
	+ (VGIC_INTID_BITS << IRS_IDR2_ID_BITS_SHIFT);
	+ break;
	+ case 3:
	+ case 4:
	+ /* RES0 when IRS_IDR0.VIRT != 1 */
	+ *rval = 0;
	+ break;
	+ case 5: /* SPI range */
	+ case 6: /* SPI range on this IRS */
	+ *rval = SPI_COUNT;
	+ break;
	+ case 7: /* SPI base on this IRS */
	+ *rval = 0;
	+ break;
	+ default:
	+ panic("%s: Invalid register IRS_IDR%d", __func__, n);
	+ }
	+}
	+
	+static void
	+irs_iidr_read(struct hypctx hypctx, u_int reg, uint64_t rval, void *arg)
	+{
	+ *rval = 0;
	+}
	+
	+static void
	+irs_aidr_read(struct hypctx hypctx, u_int reg, uint64_t rval, void *arg)
	+{
	+ *rval = IRS_AIDR_COMPONENT_IRS \| IRS_AIDR_ARCH_MAJOR_REV_V5 \|
	+ IRS_AIDR_ARCH_MINOR_REV_0;
	+}
	+
	+static void
	+irs_cr0_read(struct hypctx hypctx, u_int reg, uint64_t rval, void *arg)
	+{
	+ struct vgic_v5 *vgic;
	+ uint64_t val;
	+
	+ vgic = &hypctx->hyp->vgic->v5;
	+ val = IRS_CR0_IDLE;
	+ if (vgic->irs_en)
	+ val \|= IRS_CR0_IRSEN;
	+ *rval = val;
	+}
	+
	+void
	+irs_cr0_write(struct hypctx *hypctx, u_int reg, u_int offset, u_int size,
	+ uint64_t wval, void *arg)
	+{
	+ struct vgic_v5 *vgic;
	+
	+ vgic = &hypctx->hyp->vgic->v5;
	+ vgic->irs_en = (wval & IRS_CR0_IRSEN) != 0;
	+}
	+
	+static void
	+irs_cr1_read(struct hypctx hypctx, u_int reg, uint64_t rval, void *arg)
	+{
	+ struct vgic_v5 *vgic;
	+
	+ vgic = &hypctx->hyp->vgic->v5;
	+ *rval = atomic_load_32(&vgic->irs_cr1);
	+}
	+
	+void
	+irs_cr1_write(struct hypctx *hypctx, u_int reg, u_int offset, u_int size,
	+ uint64_t wval, void *arg)
	+{
	+ struct vgic_v5 *vgic;
	+
	+ vgic = &hypctx->hyp->vgic->v5;
	+ if (!(atomic_load_64(&vgic->irs_ist_baser) & IRS_IST_BASER_VALID))
	+ atomic_store_32(&vgic->irs_cr1, wval);
	+}
	+
	+void
	+irs_spi_selr_write(struct hypctx *hypctx, u_int reg, u_int offset, u_int size,
	+ uint64_t wval, void *arg)
	+{
	+ struct vgic_v5 *vgic;
	+
	+ vgic = &hypctx->hyp->vgic->v5;
	+ if ((wval & IRS_SPI_SELR_ID_MASK) >= SPI_COUNT) {
	+ atomic_store_32(&vgic->irs_spi_statusr, ~IRS_SPI_STATUSR_V);
	+ wval = SPI_COUNT;
	+ } else {
	+ atomic_store_32(&vgic->irs_spi_statusr, IRS_SPI_STATUSR_V);
	+ }
	+
	+ atomic_store_32(&vgic->irs_spi_selr, wval);
	+}
	+
	+void
	+irs_spi_cfgr_write(struct hypctx *hypctx, u_int reg, u_int offset, u_int size,
	+ uint64_t wval, void *arg)
	+{
	+}
	+
	+static void
	+irs_spi_cfgr_read(struct hypctx hypctx, u_int reg, uint64_t rval, void *arg)
	+{
	+ *rval = 0;
	+}
	+
	+static void
	+irs_spi_statusr_read(struct hypctx hypctx, u_int reg, uint64_t rval,
	+ void *arg)
	+{
	+ struct vgic_v5 *vgic;
	+
	+ vgic = &hypctx->hyp->vgic->v5;
	+ *rval = atomic_load_32(&vgic->irs_spi_statusr) \| IRS_SPI_STATUSR_IDLE;
	+}
	+
	+static void
	+irs_pe_cr0_read(struct hypctx hypctx, u_int reg, uint64_t rval, void *arg)
	+{
	+ struct hypctx *target_hypctx;
	+ struct vgic_v5 *vgic;
	+
	+ vgic = &hypctx->hyp->vgic->v5;
	+ if (vgic->irs_pe_selr < vgic->max_cpu_count) {
	+ target_hypctx = hypctx->hyp->ctx[vgic->irs_pe_selr];
	+ *rval = target_hypctx->vgic_cpu->v5.dps ? IRS_PE_CR0_DPS : 0;
	+ } else
	+ *rval = 0;
	+}
	+
	+void
	+irs_pe_cr0_write(struct hypctx *hypctx, u_int reg, u_int offset, u_int size,
	+ uint64_t wval, void *arg)
	+{
	+ struct hypctx *target_hypctx;
	+ struct vgic_v5 *vgic;
	+
	+ vgic = &hypctx->hyp->vgic->v5;
	+ if (vgic->irs_pe_selr < vgic->max_cpu_count) {
	+ target_hypctx = hypctx->hyp->ctx[vgic->irs_pe_selr];
	+ target_hypctx->vgic_cpu->v5.dps = wval & IRS_PE_CR0_DPS;
	+ }
	+}
	+
	+void
	+irs_pe_selr_write(struct hypctx *hypctx, u_int reg, u_int offset, u_int size,
	+ uint64_t wval, void *arg)
	+{
	+ struct vgic_v5 *vgic;
	+
	+ vgic = &hypctx->hyp->vgic->v5;
	+ vgic->irs_pe_selr = wval & IRS_PE_SELR_IAFFID_MASK;
	+}
	+
	+static void
	+irs_pe_statusr_read(struct hypctx hypctx, u_int reg, uint64_t rval, void *arg)
	+{
	+ struct vgic_v5 *vgic;
	+
	+ vgic = &hypctx->hyp->vgic->v5;
	+
	+ *rval = IRS_PE_CR0_IDLE;
	+ if (vgic->irs_pe_selr < vgic->max_cpu_count)
	+ *rval \|= IRS_PE_CR0_V;
	+}
	+
	+static void
	+irs_ist_baser_read(struct hypctx hypctx, u_int reg, uint64_t rval, void *arg)
	+{
	+ struct vgic_v5 *vgic;
	+
	+ vgic = &hypctx->hyp->vgic->v5;
	+ *rval = atomic_load_64(&vgic->irs_ist_baser);
	+}
	+
	+void
	+irs_ist_baser_write(struct hypctx *hypctx, u_int reg, u_int offset, u_int size,
	+ uint64_t wval, void *arg)
	+{
	+ struct vgic_v5 *vgic;
	+ uint64_t nval, mask, val;
	+ u_int shift;
	+
	+ vgic = &hypctx->hyp->vgic->v5;
	+ if (size == 8) {
	+ atomic_store_64(&vgic->irs_ist_baser, wval);
	+ return;
	+ }
	+
	+ MPASS(size == 4);
	+
	+ val = atomic_load_64(&vgic->irs_ist_baser);
	+ if (offset == 0) {
	+ /* Set the upper 32 bits */
	+ shift = 32;
	+ } else {
	+ MPASS(offset == 4);
	+ shift = 0;
	+ }
	+
	+
	+ mask = 0xfffffffful << shift;
	+ nval = (wval & 0xfffffffful) << shift;
	+ while (!atomic_fcmpset_64(&vgic->irs_ist_baser, &val,
	+ (val & ~mask) \| nval))
	+ cpu_spinwait();
	+}
	+
	+static void
	+irs_ist_cfgr_read(struct hypctx hypctx, u_int reg, uint64_t rval, void *arg)
	+{
	+ struct vgic_v5 *vgic;
	+
	+ vgic = &hypctx->hyp->vgic->v5;
	+ *rval = atomic_load_32(&vgic->irs_ist_cfgr);
	+}
	+
	+void
	+irs_ist_cfgr_write(struct hypctx *hypctx, u_int reg, u_int offset, u_int size,
	+ uint64_t wval, void *arg)
	+{
	+ struct vgic_v5 *vgic;
	+
	+ vgic = &hypctx->hyp->vgic->v5;
	+ if (!(atomic_load_64(&vgic->irs_ist_baser) & IRS_IST_BASER_VALID))
	+ atomic_store_32(&vgic->irs_ist_cfgr, wval);
	+}
	+
	+static void
	+irs_ist_statusr_read(struct hypctx hypctx, u_int reg, uint64_t rval,
	+ void *arg)
	+{
	+ *rval = IRS_IST_STATUSR_IDLE;
	+}
	+
	+static int
	+vgic_v5_attach_to_vm(device_t dev, struct hyp hyp, struct vm_vgic_descr descr)
	+{
	+ struct vm *vm;
	+ struct vgic_v5 *vgic;
	+ device_t parent;
	+
	+ switch (descr->ver.version) {
	+ case 3:
	+ if (!has_gcie_legacy())
	+ return (EINVAL);
	+ return (vgic_v3_attach_to_vm(dev, hyp, descr));
	+ case 5:
	+ break;
	+ default:
	+ return (EINVAL);
	+ }
	+
	+ vm = hyp->vm;
	+ vgic = &hyp->vgic->v5;
	+
	+ hyp->vgic->vgic_ver = 5;
	+
	+ parent = device_get_parent(dev);
	+ vgic->vm_id = gicv5_alloc_vm(parent, vm_get_maxcpus(hyp->vm));
	+
	+ vgic->irs_start = descr->v3_regs.dist_start;
	+ vgic->irs_end = descr->v3_regs.dist_start + descr->v3_regs.dist_size;
	+
	+ /*
	+ * IRS_CR0.IDLE resets to 1
	+ * IRS_CR0.IRSEN resets to 0
	+ *
	+ * IRS_IST_BASER.VALID resets to 0
	+ *
	+ * IRS_IST_STATUSR.IDLE resets to 1
	+ *
	+ * IRS_MPAM_PARTID_R.IDLE resets to 1
	+ *
	+ * IRS_PE_STATUSR.V resets to 0
	+ * IRS_PE_STATUSR.IDLE resets to 1
	+ *
	+ * IRS_SAVE_VM_STATUSR.IDLE resets to 1
	+ *
	+ * IRS_SPI_STATUSR.V resets to 0
	+ * IRS_SPI_STATUSR.IDLE resets to 1
	+ *
	+ * IRS_SYNC_STATUSR.IDLE resets to 1
	+ *
	+ * IRS_VM_STATUSR.V resets to 0
	+ * IRS_VM_STATUSR.IDLE resets to 1
	+ *
	+ * IRS_VMT_BASER.VALID resets to 0
	+ *
	+ * IRS_VMT_STATUSR.IDLE resets to 1
	+ *
	+ * IRS_VPE_STATUSR.V resets to 0
	+ * IRS_VPE_STATUSR.IDLE resets to 1
	+ */
	+ vgic->irs_spi_statusr = IRS_SPI_STATUSR_IDLE;
	+
	+ /* TODO: Use a GICv5 entry */
	+ vm_register_inst_handler(vm, descr->v3_regs.dist_start,
	+ descr->v3_regs.dist_size, irs_read, irs_write);
	+
	+ vm_register_reg_handler(vm, ISS_MSR_REG(ICC_IAFFIDR_EL1),
	+ ISS_MSR_REG_MASK, vgic_v5_icc_iaffidr_read, vgic_v5_write_ignore,
	+ NULL);
	+ vm_register_reg_handler(vm, ISS_MSR_REG(ICC_IDR0_EL1),
	+ ISS_MSR_REG_MASK, vgic_v5_icc_idr0_read, vgic_v5_write_ignore,
	+ NULL);
	+
	+ hyp->vgic_attached = true;
	+
	+ return (0);
	+}
	+
	+static void
	+vgic_v5_detach_from_vm(device_t dev, struct hyp *hyp)
	+{
	+ device_t parent;
	+
	+ switch (hyp->vgic->vgic_ver) {
	+ case 0:
	+ /* No vgic was attached */
	+ return;
	+ case 3:
	+ vgic_v3_detach_from_vm(dev, hyp);
	+ return;
	+ case 5:
	+ hyp->vgic_attached = false;
	+ parent = device_get_parent(dev);
	+ gicv5_free_vm(parent, hyp->vgic->v5.vm_id);
	+ return;
	+ default:
	+ /* Shouldn't happen */
	+ MPASS(false);
	+ break;
	+ }
	+}
	+
	+static bool
	+vgic_v5_has_pending_irq(device_t dev, struct hypctx *hypctx)
	+{
	+ if (hypctx->hyp->vgic->vgic_ver == 3) {
	+ return (vgic_v3_has_pending_irq(dev, hypctx));
	+ }
	+
	+ if (hypctx_read_sys_reg(hypctx, HOST_ICH_PPI_PENDR0_EL2) != 0 \|\|
	+ hypctx_read_sys_reg(hypctx, HOST_ICH_PPI_PENDR1_EL2) != 0)
	+ return (true);
	+
	+ if (hypctx_read_sys_reg(hypctx, HOST_ICH_HPPIR_EL2) &
	+ ICH_HPPIR_EL2_HPPIV)
	+ return (true);
	+
	+ return (false);
	+}
	+
	+static int
	+vgic_v5_inject_irq(device_t dev, struct hyp *hyp, int vcpuid, uint32_t irqid,
	+ bool level)
	+{
	+ struct hypctx *hypctx;
	+ uint64_t reg;
	+ u_int irq;
	+
	+ if (hyp->vgic->vgic_ver == 3) {
	+ return (vgic_v3_inject_irq(dev, hyp, vcpuid, irqid, level));
	+ }
	+
	+ KASSERT((irqid & GSI_INT_TYPE_MASK) != 0,
	+ ("%s: No type set for irqid %x\n", __func__, irqid));
	+
	+ irq = irqid & GSI_INT_ID_MASK;
	+ switch ((irqid & GSI_INT_TYPE_MASK) >> GSI_INT_TYPE_SHIFT) {
	+ case 1: /* PPI */
	+ if (vcpuid < 0 \|\| vcpuid >= vm_get_maxcpus(hyp->vm))
	+ return (EINVAL);
	+
	+ hypctx = hyp->ctx[vcpuid];
	+ if (irq < GICV5_PPIS_PER_REG)
	+ reg = hypctx_read_sys_reg(hypctx,
	+ HOST_ICH_PPI_PENDR0_EL2);
	+ else if (irq < GICV5_PPI_COUNT)
	+ reg = hypctx_read_sys_reg(hypctx,
	+ HOST_ICH_PPI_PENDR1_EL2);
	+ else
	+ return (EINVAL);
	+ if (level)
	+ reg \|= 1ul << (irq % GICV5_PPIS_PER_REG);
	+ else
	+ reg &= ~(1ul << (irq % GICV5_PPIS_PER_REG));
	+ if (irq < GICV5_PPIS_PER_REG)
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_PPI_PENDR0_EL2,
	+ reg);
	+ else if (irq < GICV5_PPI_COUNT)
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_PPI_PENDR1_EL2,
	+ reg);
	+ break;
	+ case 3: /* SPI */
	+ if (level)
	+ WRITE_SPECIALREG(GIC_VDPEND, irq \|
	+ (((uint64_t)hyp->vgic->v5.vm_id <<
	+ GIC_VDPEND_VM_SHIFT) & GIC_VDPEND_VM_MASK) \|
	+ GIC_VDPEND_TYPE_SPI \| GIC_VDPEND_PENDING);
	+ else
	+ WRITE_SPECIALREG(GIC_VDPEND, irq \| GIC_VDPEND_TYPE_SPI \|
	+ (((uint64_t)hyp->vgic->v5.vm_id <<
	+ GIC_VDPEND_VM_SHIFT) & GIC_VDPEND_VM_MASK));
	+
	+ break;
	+ }
	+
	+ if (vcpuid != -1)
	+ vcpu_notify_event(vm_vcpu(hyp->vm, vcpuid));
	+
	+ return (0);
	+}
	+
	+static int
	+vgic_v5_inject_msi(device_t dev, struct hyp *hyp, uint64_t msg, uint64_t addr)
	+{
	+ if (hyp->vgic->vgic_ver == 3) {
	+ return (vgic_v3_inject_msi(dev, hyp, msg, addr));
	+ }
	+ return (EINVAL);
	+}
	+
	+static void
	+vgic_v5_flush_hwstate(device_t dev, struct hypctx *hypctx)
	+{
	+ if (hypctx->hyp->vgic->vgic_ver == 3) {
	+ vgic_v3_flush_hwstate(dev, hypctx);
	+ return;
	+ }
	+}
	+
	+static void
	+vgic_v5_sync_hwstate(device_t dev, struct hypctx *hypctx)
	+{
	+ if (hypctx->hyp->vgic->vgic_ver == 3) {
	+ vgic_v3_sync_hwstate(dev, hypctx);
	+ return;
	+ }
	+}
	+
	+static void
	+vgic_v5_init(device_t dev)
	+{
	+ if (has_gcie_legacy()) {
	+ vgic_v3_init(dev);
	+ }
	+}
	+
	+static int
	+vgic_v5_probe(device_t dev)
	+{
	+ if (!gic_get_vgic(dev))
	+ return (EINVAL);
	+
	+ /* We currently only support the GICv5 */
	+ if (gic_get_hw_rev(dev) < 5)
	+ return (EINVAL);
	+
	+ device_set_desc(dev, "Virtual GIC v5");
	+ return (BUS_PROBE_DEFAULT);
	+}
	+
	+static int
	+vgic_v5_attach(device_t dev)
	+{
	+ vgic_dev = dev;
	+ return (0);
	+}
	+
	+static int
	+vgic_v5_detach(device_t dev)
	+{
	+ vgic_dev = NULL;
	+ return (0);
	+}
	+
	+static int
	+vgic_v5_get_version(device_t dev)
	+{
	+ return (5);
	+}
	+
	+static device_method_t vgic_v5_methods[] = {
	+ /* Device interface */
	+ DEVMETHOD(device_probe, vgic_v5_probe),
	+ DEVMETHOD(device_attach, vgic_v5_attach),
	+ DEVMETHOD(device_detach, vgic_v5_detach),
	+
	+ /* VGIC interface */
	+ DEVMETHOD(vgic_init, vgic_v5_init),
	+ DEVMETHOD(vgic_attach_to_vm, vgic_v5_attach_to_vm),
	+ DEVMETHOD(vgic_detach_from_vm, vgic_v5_detach_from_vm),
	+ DEVMETHOD(vgic_vminit, vgic_v5_vminit),
	+ DEVMETHOD(vgic_cpuinit, vgic_v5_cpuinit),
	+ DEVMETHOD(vgic_cpucleanup, vgic_v5_cpucleanup),
	+ DEVMETHOD(vgic_vmcleanup, vgic_v5_vmcleanup),
	+ DEVMETHOD(vgic_max_cpu_count, vgic_v5_max_cpu_count),
	+ DEVMETHOD(vgic_has_pending_irq, vgic_v5_has_pending_irq),
	+ DEVMETHOD(vgic_inject_irq, vgic_v5_inject_irq),
	+ DEVMETHOD(vgic_inject_msi, vgic_v5_inject_msi),
	+ DEVMETHOD(vgic_flush_hwstate, vgic_v5_flush_hwstate),
	+ DEVMETHOD(vgic_sync_hwstate, vgic_v5_sync_hwstate),
	+ DEVMETHOD(vgic_get_version, vgic_v5_get_version),
	+
	+ /* End */
	+ DEVMETHOD_END
	+};
	+
	+/* TODO: Create a vgic base class? */
	+DEFINE_CLASS_0(vgic, vgic_v5_driver, vgic_v5_methods, 0);
	+
	+DRIVER_MODULE(vgic_v5, gic, vgic_v5_driver, 0, 0);
	diff --git a/sys/arm64/vmm/io/vtimer.c b/sys/arm64/vmm/io/vtimer.c
	--- a/sys/arm64/vmm/io/vtimer.c
	+++ b/sys/arm64/vmm/io/vtimer.c
	@@ -46,10 +46,12 @@
	#include <machine/vmm.h>

	#include <arm64/vmm/arm64.h>
	+#include <arm64/arm64/gicv5reg.h>

	#include <dev/vmm/vmm_vm.h>

	#include "vgic.h"
	+#include "vgic_internal.h"
	#include "vtimer.h"

	#define RES1 0xffffffffffffffffUL
	@@ -71,6 +73,18 @@

	static void vtimer_schedule_irq(struct hypctx *hypctx, bool phys);

	+static uint32_t
	+get_irqid(struct hypctx *hypctx, uint32_t irqid)
	+{
	+ struct hyp *hyp;
	+
	+ hyp = hypctx->hyp;
	+
	+ if (hyp->vgic->vgic_ver == 5)
	+ return (irqid \| GSI_INT_TYPE_PPI);
	+ return (irqid);
	+}
	+
	static int
	vtimer_virtual_timer_intr(void *arg)
	{
	@@ -100,7 +114,8 @@
	hypctx_read_sys_reg(hypctx, HOST_CNTVOFF_EL2);
	if (hypctx->vtimer_cpu.virt_timer.cntx_cval_el0 < cntpct_el0)
	vgic_inject_irq(hypctx->hyp, vcpu_vcpuid(hypctx->vcpu),
	- GT_VIRT_IRQ, true);
	+ get_irqid(hypctx, hypctx->vtimer_cpu.virt_timer.irqid),
	+ true);

	cntv_ctl = hypctx->vtimer_cpu.virt_timer.cntx_ctl_el0;

	@@ -261,15 +276,19 @@
	vtime_sync_timer(struct hypctx hypctx, struct vtimer_timer timer,
	uint64_t cntpct_el0)
	{
	+ uint32_t irqid;
	+
	+ irqid = get_irqid(hypctx, timer->irqid);
	+
	if (!timer_enabled(timer->cntx_ctl_el0)) {
	vgic_inject_irq(hypctx->hyp, vcpu_vcpuid(hypctx->vcpu),
	- timer->irqid, false);
	+ irqid, false);
	} else if (timer->cntx_cval_el0 < cntpct_el0) {
	vgic_inject_irq(hypctx->hyp, vcpu_vcpuid(hypctx->vcpu),
	- timer->irqid, true);
	+ irqid, true);
	} else {
	vgic_inject_irq(hypctx->hyp, vcpu_vcpuid(hypctx->vcpu),
	- timer->irqid, false);
	+ irqid, false);
	vtimer_schedule_irq(hypctx, false);
	}
	}
	@@ -296,7 +315,7 @@

	hypctx = context;
	vgic_inject_irq(hypctx->hyp, vcpu_vcpuid(hypctx->vcpu),
	- hypctx->vtimer_cpu.phys_timer.irqid, true);
	+ get_irqid(hypctx, hypctx->vtimer_cpu.phys_timer.irqid), true);
	}

	static void
	@@ -306,7 +325,7 @@

	hypctx = context;
	vgic_inject_irq(hypctx->hyp, vcpu_vcpuid(hypctx->vcpu),
	- hypctx->vtimer_cpu.virt_timer.irqid, true);
	+ get_irqid(hypctx, hypctx->vtimer_cpu.virt_timer.irqid), true);
	}

	static void
	@@ -326,7 +345,7 @@
	if (timer->cntx_cval_el0 < cntpct_el0) {
	/* Timer set in the past, trigger interrupt */
	vgic_inject_irq(hypctx->hyp, vcpu_vcpuid(hypctx->vcpu),
	- timer->irqid, true);
	+ get_irqid(hypctx, timer->irqid), true);
	} else {
	diff = timer->cntx_cval_el0 - cntpct_el0;
	time = diff * SBT_1S / tmr_frq;
	@@ -355,7 +374,8 @@
	* the CNTP_CTL_EL0.IMASK bit instead of reading the IAR register.
	* Masking the interrupt doesn't remove it from the list registers.
	*/
	- vgic_inject_irq(hypctx->hyp, vcpu_vcpuid(vcpu), timer->irqid, false);
	+ vgic_inject_irq(hypctx->hyp, vcpu_vcpuid(vcpu),
	+ get_irqid(hypctx, timer->irqid), false);
	}

	/*
	diff --git a/sys/arm64/vmm/vmm.c b/sys/arm64/vmm/vmm.c
	--- a/sys/arm64/vmm/vmm.c
	+++ b/sys/arm64/vmm/vmm.c
	@@ -91,6 +91,7 @@
	uint64_t id_aa64mmfr2;
	uint64_t id_aa64pfr0;
	uint64_t id_aa64pfr1;
	+ uint64_t id_aa64pfr2;
	};

	static const struct vmm_regs vmm_arch_regs_masks = {
	@@ -127,6 +128,8 @@
	ID_AA64PFR0_EL2_64 \|
	ID_AA64PFR0_EL1_64 \|
	ID_AA64PFR0_EL0_64,
	+ .id_aa64pfr2 =
	+ ID_AA64PFR2_GCIE_NONE
	};

	/* Host registers masked by vmm_arch_regs_masks. */
	@@ -169,6 +172,7 @@
	_FETCH_KERN_REG(ID_AA64MMFR2_EL1, id_aa64mmfr2);
	_FETCH_KERN_REG(ID_AA64PFR0_EL1, id_aa64pfr0);
	_FETCH_KERN_REG(ID_AA64PFR1_EL1, id_aa64pfr1);
	+ _FETCH_KERN_REG(ID_AA64PFR2_EL1, id_aa64pfr2);
	#undef _FETCH_KERN_REG
	return (0);
	}
	@@ -425,6 +429,40 @@
	return (0);
	}

	+static int
	+vmm_read_id_aa64pfr0_el1(struct vcpu vcpu, uint64_t rval, void *arg)
	+{
	+ struct hypctx *hypctx;
	+
	+ hypctx = vcpu_get_cookie(vcpu);
	+
	+ if (hypctx->hyp->vgic_ver == 3)
	+ *rval = (vmm_arch_regs.id_aa64pfr0 & ~ID_AA64PFR0_GIC_MASK) \|
	+ ID_AA64PFR0_GIC_CPUIF_EN;
	+ else
	+ *rval = (vmm_arch_regs.id_aa64pfr0 & ~ID_AA64PFR0_GIC_MASK) \|
	+ ID_AA64PFR0_GIC_CPUIF_NONE;
	+
	+ return (0);
	+}
	+
	+static int
	+vmm_read_id_aa64pfr2_el1(struct vcpu vcpu, uint64_t rval, void *arg)
	+{
	+ struct hypctx *hypctx;
	+
	+ hypctx = vcpu_get_cookie(vcpu);
	+
	+ if (hypctx->hyp->vgic_ver == 5)
	+ *rval = (vmm_arch_regs.id_aa64pfr2 & ~ID_AA64PFR2_GCIE_MASK) \|
	+ ID_AA64PFR2_GCIE_IMPL;
	+ else
	+ *rval = (vmm_arch_regs.id_aa64pfr2 & ~ID_AA64PFR2_GCIE_MASK) \|
	+ ID_AA64PFR2_GCIE_NONE;
	+
	+ return (0);
	+}
	+
	static int
	vmm_write_oslar_el1(struct vcpu vcpu, uint64_t wval, void arg)
	{
	@@ -479,7 +517,9 @@
	}

	/* ID registers */
	- ID_SPECIAL_REG(ID_AA64PFR0_EL1, id_aa64pfr0),
	+ SPECIAL_REG(ID_AA64PFR0_EL1, vmm_read_id_aa64pfr0_el1, vmm_reg_wi),
	+ SPECIAL_REG(ID_AA64PFR2_EL1, vmm_read_id_aa64pfr2_el1, vmm_reg_wi),
	+
	ID_SPECIAL_REG(ID_AA64DFR0_EL1, id_aa64dfr0),
	ID_SPECIAL_REG(ID_AA64ISAR0_EL1, id_aa64isar0),
	ID_SPECIAL_REG(ID_AA64MMFR0_EL1, id_aa64mmfr0),
	diff --git a/sys/arm64/vmm/vmm_arm64.c b/sys/arm64/vmm/vmm_arm64.c
	--- a/sys/arm64/vmm/vmm_arm64.c
	+++ b/sys/arm64/vmm/vmm_arm64.c
	@@ -59,6 +59,8 @@
	#include <dev/vmm/vmm_mem.h>
	#include <dev/vmm/vmm_vm.h>

	+#include <arm64/arm64/gicv5reg.h>
	+
	#include "mmu.h"
	#include "arm64.h"
	#include "hyp.h"
	@@ -542,6 +544,19 @@
	if (ID_AA64MMFR1_HCX_VAL(idreg) >= ID_AA64MMFR1_HCX_IMPL)
	hyp->feats \|= HYP_FEAT_HCX;

	+ get_kernel_reg(ID_AA64PFR2_EL1, &idreg);
	+ if (ID_AA64PFR2_GCIE_VAL(idreg) >= ID_AA64PFR2_GCIE_IMPL) {
	+ hyp->feats \|= HYP_FEAT_GICV5;
	+
	+ get_kernel_reg(ICC_IDR0_EL1, &idreg);
	+ if (idreg & ICC_IDR0_GCIE_LEGACY_MASK)
	+ hyp->feats \|= HYP_FEAT_GICV3_OR_LEGACY;
	+ } else {
	+ get_kernel_reg(ID_AA64PFR0_EL1, &idreg);
	+ if (ID_AA64PFR0_GIC_VAL(idreg) >= ID_AA64PFR0_GIC_CPUIF_EN)
	+ hyp->feats \|= HYP_FEAT_GICV3_OR_LEGACY;
	+ }
	+
	hyp->cntvoff_el2 = READ_CNTPCT();
	vgic_vminit(hyp);

	@@ -1465,3 +1480,9 @@

	return (ret);
	}
	+
	+int
	+vmm_get_max_ipa_bits(void)
	+{
	+ return (vmm_max_ipa_bits);
	+}
	diff --git a/sys/arm64/vmm/vmm_dev_machdep.c b/sys/arm64/vmm/vmm_dev_machdep.c
	--- a/sys/arm64/vmm/vmm_dev_machdep.c
	+++ b/sys/arm64/vmm/vmm_dev_machdep.c
	@@ -109,8 +109,7 @@
	struct vm_vgic_version *vgv;

	vgv = (struct vm_vgic_version *)data;
	- /* TODO: Query the vgic driver for this */
	- vgv->version = 3;
	+ vgv->version = vgic_get_version();
	vgv->flags = 0;
	error = 0;
	break;
	diff --git a/sys/arm64/vmm/vmm_hyp.c b/sys/arm64/vmm/vmm_hyp.c
	--- a/sys/arm64/vmm/vmm_hyp.c
	+++ b/sys/arm64/vmm/vmm_hyp.c
	@@ -32,9 +32,13 @@
	#include <sys/types.h>
	#include <sys/proc.h>

	+#include <dev/vmm/vmm_vm.h>

	#include "arm64.h"
	#include "hyp.h"
	+#include "io/vgic_internal.h"
	+
	+#include <arm64/arm64/gicv5reg.h>

	struct hypctx;

	@@ -172,22 +176,14 @@
	}

	static void
	-vmm_hyp_reg_store_vgic(struct hypctx *hypctx, bool guest)
	+vmm_hyp_reg_store_vgic_v3(struct hypctx hypctx, struct hyp hyp)
	{
	- hypctx_write_sys_reg(hypctx, HOST_ICH_HCR_EL2,
	- READ_SPECIALREG(ich_hcr_el2));
	- hypctx_write_sys_reg(hypctx, HOST_ICH_VMCR_EL2,
	- READ_SPECIALREG(ich_vmcr_el2));
	-
	- if (!guest)
	- return;
	-
	/* Store the GICv3 registers */
	- hypctx_write_sys_reg(hypctx, HOST_ICH_EISR_EL2,
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_EISR_EL2,
	READ_SPECIALREG(ich_eisr_el2));
	- hypctx_write_sys_reg(hypctx, HOST_ICH_ELRSR_EL2,
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_ELRSR_EL2,
	READ_SPECIALREG(ich_elrsr_el2));
	- hypctx_write_sys_reg(hypctx, HOST_ICH_MISR_EL2,
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_MISR_EL2,
	READ_SPECIALREG(ich_misr_el2));

	switch (hypctx->vgic_v3.ich_lr_num - 1) {
	@@ -210,7 +206,7 @@
	STORE_LR(3);
	STORE_LR(2);
	STORE_LR(1);
	- default:
	+default:
	STORE_LR(0);
	#undef STORE_LR
	}
	@@ -225,12 +221,95 @@
	STORE_APR(3);
	STORE_APR(2);
	STORE_APR(1);
	- default:
	+default:
	STORE_APR(0);
	#undef STORE_APR
	}
	}

	+static void
	+vmm_hyp_reg_store_vgic_v5(struct hypctx hypctx, struct hyp hyp)
	+{
	+ /* Store the GICv5 registers */
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_PPI_ACTIVER0_EL2,
	+ READ_SPECIALREG(ICH_PPI_ACTIVER0_EL2));
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_PPI_ACTIVER1_EL2,
	+ READ_SPECIALREG(ICH_PPI_ACTIVER1_EL2));
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_PPI_ENABLER0_EL2,
	+ READ_SPECIALREG(ICH_PPI_ENABLER0_EL2));
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_PPI_ENABLER1_EL2,
	+ READ_SPECIALREG(ICH_PPI_ENABLER1_EL2));
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_PPI_PENDR0_EL2,
	+ READ_SPECIALREG(ICH_PPI_PENDR0_EL2));
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_PPI_PENDR1_EL2,
	+ READ_SPECIALREG(ICH_PPI_PENDR1_EL2));
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_PPI_PRIORITYR0_EL2,
	+ READ_SPECIALREG(ICH_PPI_PRIORITYR0_EL2));
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_PPI_PRIORITYR1_EL2,
	+ READ_SPECIALREG(ICH_PPI_PRIORITYR1_EL2));
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_PPI_PRIORITYR2_EL2,
	+ READ_SPECIALREG(ICH_PPI_PRIORITYR2_EL2));
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_PPI_PRIORITYR3_EL2,
	+ READ_SPECIALREG(ICH_PPI_PRIORITYR3_EL2));
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_PPI_PRIORITYR4_EL2,
	+ READ_SPECIALREG(ICH_PPI_PRIORITYR4_EL2));
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_PPI_PRIORITYR5_EL2,
	+ READ_SPECIALREG(ICH_PPI_PRIORITYR5_EL2));
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_PPI_PRIORITYR6_EL2,
	+ READ_SPECIALREG(ICH_PPI_PRIORITYR6_EL2));
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_PPI_PRIORITYR7_EL2,
	+ READ_SPECIALREG(ICH_PPI_PRIORITYR7_EL2));
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_PPI_PRIORITYR8_EL2,
	+ READ_SPECIALREG(ICH_PPI_PRIORITYR8_EL2));
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_PPI_PRIORITYR9_EL2,
	+ READ_SPECIALREG(ICH_PPI_PRIORITYR9_EL2));
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_PPI_PRIORITYR10_EL2,
	+ READ_SPECIALREG(ICH_PPI_PRIORITYR10_EL2));
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_PPI_PRIORITYR11_EL2,
	+ READ_SPECIALREG(ICH_PPI_PRIORITYR11_EL2));
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_PPI_PRIORITYR12_EL2,
	+ READ_SPECIALREG(ICH_PPI_PRIORITYR12_EL2));
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_PPI_PRIORITYR13_EL2,
	+ READ_SPECIALREG(ICH_PPI_PRIORITYR13_EL2));
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_PPI_PRIORITYR14_EL2,
	+ READ_SPECIALREG(ICH_PPI_PRIORITYR14_EL2));
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_PPI_PRIORITYR15_EL2,
	+ READ_SPECIALREG(ICH_PPI_PRIORITYR15_EL2));
	+
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_VMCR_EL2,
	+ READ_SPECIALREG(ich_vmcr_el2));
	+}
	+
	+static void
	+vmm_hyp_reg_store_vgic(struct hypctx hypctx, struct hyp hyp, bool guest)
	+{
	+ if (guest && (hyp->feats & HYP_FEAT_GICV5)) {
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_HPPIR_EL2,
	+ READ_SPECIALREG(ICH_HPPIR_EL2));
	+ WRITE_SPECIALREG(ICH_CONTEXTR_EL2, 0); /* No VPE resident */
	+ }
	+
	+ if (hyp->feats & HYP_FEAT_GICV3_OR_LEGACY) {
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_HCR_EL2,
	+ READ_SPECIALREG(ich_hcr_el2));
	+ }
	+
	+ hypctx_write_sys_reg(hypctx, HOST_ICH_VMCR_EL2,
	+ READ_SPECIALREG(ich_vmcr_el2));
	+
	+ if (!guest)
	+ return;
	+
	+ if (hyp->feats & HYP_FEAT_GICV3_OR_LEGACY)
	+ vmm_hyp_reg_store_vgic_v3(hypctx, hyp);
	+
	+ if ((hyp->feats & HYP_FEAT_GICV5) && (hyp->vgic_ver == 5))
	+ vmm_hyp_reg_store_vgic_v5(hypctx, hyp);
	+
	+ if (hyp->feats & HYP_FEAT_GICV5)
	+ WRITE_SPECIALREG(ICH_VCTLR_EL2, 0);
	+}
	+
	static void
	vmm_hyp_reg_store_timer(struct hypctx hypctx, struct hyp hyp, bool guest)
	{
	@@ -630,17 +709,8 @@
	}

	static void
	-vmm_hyp_reg_restore_vgic(struct hypctx *hypctx, bool guest)
	+vmm_hyp_reg_restore_vgic_v3(struct hypctx hypctx, struct hyp hyp)
	{
	- /* Load the GICv3 registers */
	- WRITE_SPECIALREG(ich_hcr_el2,
	- hypctx_read_sys_reg(hypctx, HOST_ICH_HCR_EL2));
	- WRITE_SPECIALREG(ich_vmcr_el2,
	- hypctx_read_sys_reg(hypctx, HOST_ICH_VMCR_EL2));
	-
	- if (!guest)
	- return;
	-
	switch (hypctx->vgic_v3.ich_lr_num - 1) {
	#define LOAD_LR(x) \
	case x: \
	@@ -661,7 +731,7 @@
	LOAD_LR(3);
	LOAD_LR(2);
	LOAD_LR(1);
	- default:
	+default:
	LOAD_LR(0);
	#undef LOAD_LR
	}
	@@ -676,16 +746,119 @@
	LOAD_APR(3);
	LOAD_APR(2);
	LOAD_APR(1);
	- default:
	+default:
	LOAD_APR(0);
	#undef LOAD_APR
	}
	}

	+static void
	+vmm_hyp_reg_restore_vgic_v5(struct hypctx hypctx, struct hyp hyp)
	+{
	+ /* Store the GICv5 registers */
	+ WRITE_SPECIALREG(ICH_PPI_ACTIVER0_EL2,
	+ hypctx_read_sys_reg(hypctx, HOST_ICH_PPI_ACTIVER0_EL2));
	+ WRITE_SPECIALREG(ICH_PPI_ACTIVER1_EL2,
	+ hypctx_read_sys_reg(hypctx, HOST_ICH_PPI_ACTIVER1_EL2));
	+
	+ WRITE_SPECIALREG(ICH_PPI_DVIR0_EL2, 0x1ul << 27); // CNTV irq
	+ WRITE_SPECIALREG(ICH_PPI_DVIR1_EL2, 0);
	+
	+ WRITE_SPECIALREG(ICH_PPI_ENABLER0_EL2,
	+ hypctx_read_sys_reg(hypctx, HOST_ICH_PPI_ENABLER0_EL2));
	+ WRITE_SPECIALREG(ICH_PPI_ENABLER1_EL2,
	+ hypctx_read_sys_reg(hypctx, HOST_ICH_PPI_ENABLER1_EL2));
	+
	+ WRITE_SPECIALREG(ICH_PPI_PENDR0_EL2,
	+ hypctx_read_sys_reg(hypctx, HOST_ICH_PPI_PENDR0_EL2));
	+ WRITE_SPECIALREG(ICH_PPI_PENDR1_EL2,
	+ hypctx_read_sys_reg(hypctx, HOST_ICH_PPI_PENDR1_EL2));
	+
	+ WRITE_SPECIALREG(ICH_PPI_PRIORITYR0_EL2,
	+ hypctx_read_sys_reg(hypctx, HOST_ICH_PPI_PRIORITYR0_EL2));
	+ WRITE_SPECIALREG(ICH_PPI_PRIORITYR1_EL2,
	+ hypctx_read_sys_reg(hypctx, HOST_ICH_PPI_PRIORITYR1_EL2));
	+ WRITE_SPECIALREG(ICH_PPI_PRIORITYR2_EL2,
	+ hypctx_read_sys_reg(hypctx, HOST_ICH_PPI_PRIORITYR2_EL2));
	+ WRITE_SPECIALREG(ICH_PPI_PRIORITYR3_EL2,
	+ hypctx_read_sys_reg(hypctx, HOST_ICH_PPI_PRIORITYR3_EL2));
	+ WRITE_SPECIALREG(ICH_PPI_PRIORITYR4_EL2,
	+ hypctx_read_sys_reg(hypctx, HOST_ICH_PPI_PRIORITYR4_EL2));
	+ WRITE_SPECIALREG(ICH_PPI_PRIORITYR5_EL2,
	+ hypctx_read_sys_reg(hypctx, HOST_ICH_PPI_PRIORITYR5_EL2));
	+ WRITE_SPECIALREG(ICH_PPI_PRIORITYR6_EL2,
	+ hypctx_read_sys_reg(hypctx, HOST_ICH_PPI_PRIORITYR6_EL2));
	+ WRITE_SPECIALREG(ICH_PPI_PRIORITYR7_EL2,
	+ hypctx_read_sys_reg(hypctx, HOST_ICH_PPI_PRIORITYR7_EL2));
	+ WRITE_SPECIALREG(ICH_PPI_PRIORITYR8_EL2,
	+ hypctx_read_sys_reg(hypctx, HOST_ICH_PPI_PRIORITYR8_EL2));
	+ WRITE_SPECIALREG(ICH_PPI_PRIORITYR9_EL2,
	+ hypctx_read_sys_reg(hypctx, HOST_ICH_PPI_PRIORITYR9_EL2));
	+ WRITE_SPECIALREG(ICH_PPI_PRIORITYR10_EL2,
	+ hypctx_read_sys_reg(hypctx, HOST_ICH_PPI_PRIORITYR10_EL2));
	+ WRITE_SPECIALREG(ICH_PPI_PRIORITYR11_EL2,
	+ hypctx_read_sys_reg(hypctx, HOST_ICH_PPI_PRIORITYR11_EL2));
	+ WRITE_SPECIALREG(ICH_PPI_PRIORITYR12_EL2,
	+ hypctx_read_sys_reg(hypctx, HOST_ICH_PPI_PRIORITYR12_EL2));
	+ WRITE_SPECIALREG(ICH_PPI_PRIORITYR13_EL2,
	+ hypctx_read_sys_reg(hypctx, HOST_ICH_PPI_PRIORITYR13_EL2));
	+ WRITE_SPECIALREG(ICH_PPI_PRIORITYR14_EL2,
	+ hypctx_read_sys_reg(hypctx, HOST_ICH_PPI_PRIORITYR14_EL2));
	+ WRITE_SPECIALREG(ICH_PPI_PRIORITYR15_EL2,
	+ hypctx_read_sys_reg(hypctx, HOST_ICH_PPI_PRIORITYR15_EL2));
	+
	+ WRITE_SPECIALREG(ICH_HFGITR_EL2,
	+ hypctx_read_sys_reg(hypctx, HOST_ICH_HFGITR_EL2));
	+ WRITE_SPECIALREG(ICH_HFGRTR_EL2,
	+ hypctx_read_sys_reg(hypctx, HOST_ICH_HFGRTR_EL2));
	+ WRITE_SPECIALREG(ICH_HFGWTR_EL2,
	+ hypctx_read_sys_reg(hypctx, HOST_ICH_HFGWTR_EL2));
	+
	+ WRITE_SPECIALREG(ICH_VCTLR_EL2, ICH_VCTLR_EN);
	+}
	+
	+static void
	+vmm_hyp_reg_restore_vgic(struct hypctx hypctx, struct hyp hyp, bool guest)
	+{
	+ int vcpuid;
	+
	+ if (hyp->feats & HYP_FEAT_GICV3_OR_LEGACY) {
	+ /* Load the GICv3 registers */
	+ WRITE_SPECIALREG(ich_hcr_el2,
	+ hypctx_read_sys_reg(hypctx, HOST_ICH_HCR_EL2));
	+ }
	+
	+ WRITE_SPECIALREG(ich_vmcr_el2,
	+ hypctx_read_sys_reg(hypctx, HOST_ICH_VMCR_EL2));
	+
	+ if (!guest)
	+ return;
	+
	+ if (hyp->feats & HYP_FEAT_GICV3_OR_LEGACY)
	+ vmm_hyp_reg_restore_vgic_v3(hypctx, hyp);
	+
	+ if (hyp->feats & HYP_FEAT_GICV5) {
	+ if (hyp->vgic_ver == 3) {
	+ /* Enable GICv3 support */
	+ WRITE_SPECIALREG(ICH_VCTLR_EL2, ICH_VCTLR_V3);
	+ WRITE_SPECIALREG(ICH_VCTLR_EL2, ICH_VCTLR_V3 \| ICH_VCTLR_EN);
	+ } else
	+ vmm_hyp_reg_restore_vgic_v5(hypctx, hyp);
	+
	+ vcpuid = vcpu_vcpuid(hypctx->vcpu);
	+
	+ WRITE_SPECIALREG(ICH_CONTEXTR_EL2, ICH_CONTEXTR_EL2_V \|
	+ (((uint64_t)vcpuid << ICH_CONTEXTR_EL2_VPE_SHIFT) &
	+ ICH_CONTEXTR_EL2_VPE_MASK) \|
	+ ((hyp->vgic->v5.vm_id << ICH_CONTEXTR_EL2_VM_SHIFT) &
	+ ICH_CONTEXTR_EL2_VM_MASK));
	+ }
	+}
	+
	static void
	vmm_hyp_reg_store(struct hypctx hypctx, struct hyp hyp, bool guest)
	{
	- vmm_hyp_reg_store_vgic(hypctx, guest);
	+ vmm_hyp_reg_store_vgic(hypctx, hyp, guest);
	vmm_hyp_reg_store_timer(hypctx, hyp, guest);
	vmm_hyp_reg_store_pmu_debug(hypctx, guest);
	vmm_hyp_reg_store_special(hypctx, hyp, guest);
	@@ -697,7 +870,7 @@
	vmm_hyp_reg_restore_special(hypctx, hyp, guest);
	vmm_hyp_reg_restore_pmu_debug(hypctx);
	vmm_hyp_reg_restore_timer(hypctx, hyp, guest);
	- vmm_hyp_reg_restore_vgic(hypctx, guest);
	+ vmm_hyp_reg_restore_vgic(hypctx, hyp, guest);
	}

	static void
	@@ -762,6 +935,7 @@
	__vmm_hyp_call_guest(struct hypctx hypctx, struct hypctx host_hypctx)
	{
	uint64_t ret;
	+
	WRITE_SPECIALREG(vttbr_el2, hypctx_read_sys_reg(hypctx, HOST_VTTBR_EL2));
	WRITE_SPECIALREG(mdcr_el2, hypctx_read_sys_reg(hypctx, HOST_MDCR_EL2));
	/* Call into the guest */
	diff --git a/sys/arm64/vmm/vmm_hyp_exception.S b/sys/arm64/vmm/vmm_hyp_exception.S
	--- a/sys/arm64/vmm/vmm_hyp_exception.S
	+++ b/sys/arm64/vmm/vmm_hyp_exception.S
	@@ -31,8 +31,10 @@


	#include <sys/elf_common.h>
	+#include <machine/armreg.h>
	#include <machine/asm.h>
	#include <machine/hypervisor.h>
	+#include "../arm64/gicv5reg.h"

	#include "assym.inc"
	#include "hyp_assym.h"
	@@ -268,14 +270,21 @@
	LENTRY(handle_el2_el1_irq64)
	do_world_switch_to_host
	str x9, [sp, #-16]!
	+ mrs x9, id_aa64pfr2_el1
	+ tst x9, #ID_AA64PFR2_GCIE_MASK
	+ beq 1f
	+ mrs x9, ICC_IDR0_EL1
	+ tst x9, #ICC_IDR0_GCIE_LEGACY_MASK
	+ beq 3f
	+1:
	mrs x9, ich_misr_el2
	cmp x9, xzr
	- beq 1f
	+ beq 2f
	mov x0, #EXCP_TYPE_MAINT_IRQ
	- b 2f
	-1:
	- mov x0, #EXCP_TYPE_EL1_IRQ
	+ b 3f
	2:
	+ mov x0, #EXCP_TYPE_EL1_IRQ
	+3:
	ldr x9, [sp], #16
	ret
	LEND(handle_el2_el1_irq64)
	diff --git a/sys/conf/files.arm64 b/sys/conf/files.arm64
	--- a/sys/conf/files.arm64
	+++ b/sys/conf/files.arm64
	@@ -179,6 +179,7 @@
	dependency "$S/arm64/vmm/vmm_hyp_exception.S"
	arm64/vmm/io/vgic.c optional vmm
	arm64/vmm/io/vgic_v3.c optional vmm
	+arm64/vmm/io/vgic_v5.c optional vmm
	arm64/vmm/io/vgic_if.m optional vmm
	arm64/vmm/io/vtimer.c optional vmm
	dev/vmm/vmm_dev.c optional vmm
	diff --git a/sys/modules/vmm/Makefile b/sys/modules/vmm/Makefile
	--- a/sys/modules/vmm/Makefile
	+++ b/sys/modules/vmm/Makefile
	@@ -40,6 +40,7 @@
	vgic_if.h \
	vgic_if.c \
	vgic_v3.c \
	+ vgic_v5.c \
	vtimer.c

	CLEANFILES+= vmm_nvhe_exception.o vmm_nvhe.o

File Metadata

Mime Type: text/plain
Expires: Thu, Nov 12, 7:51 PM (10 h, 24 m)
Storage Engine: blob
Storage Format: Raw Data
Storage Handle: 41682667
Default Alt Text: D60115.1794513115.diff (61 KB)

D60115.1794513115.diffNo OneTemporaryActions

D60115.1794513115.diffView Options

File Metadata

Event Timeline

D60115.1794513115.diff
No OneTemporary
Actions

D60115.1794513115.diff
View Options