From 81e70911d6022b3af8918b273c2b4743f37b267d Mon Sep 17 00:00:00 2001
From: David Brazdil <dbrazdil@google.com>
Date: Wed, 13 Oct 2021 16:17:21 +0100
Subject: [PATCH] ANDROID: KVM: arm64: Implement MMIO handler in S2MPU driver

The host should not have access to the vast majority of S2MPU MMIO
registers. Currently it only needs access to fault information, in
the future maybe also performance registers.

Implement an MMIO trap handler for the S2MPU, allowing read-only
access to FAULT_* registers, and a write-only access to
INTERRUPT_CLEAR.

Test: builds, boots
Bug: 190463801
Signed-off-by: David Brazdil <dbrazdil@google.com>
Change-Id: I14d37bb0d5ad591ac3ac372701afd5597e0b3435
---
 arch/arm64/include/asm/kvm_s2mpu.h    |  3 ++
 arch/arm64/kvm/hyp/nvhe/iommu/s2mpu.c | 66 +++++++++++++++++++++++++++
 2 files changed, 69 insertions(+)

diff --git a/arch/arm64/include/asm/kvm_s2mpu.h b/arch/arm64/include/asm/kvm_s2mpu.h
index 6904ae20a99f..fa10bc754f03 100644
--- a/arch/arm64/include/asm/kvm_s2mpu.h
+++ b/arch/arm64/include/asm/kvm_s2mpu.h
@@ -54,6 +54,9 @@
 /* For use with hi_lo_readq_relaxed(). */
 #define REG_NS_FAULT_PA_HIGH_LOW(vid)		REG_NS_FAULT_PA_LOW(vid)
 
+/* Mask used for extracting VID from FAULT_* register offset. */
+#define REG_NS_FAULT_VID_MASK			GENMASK(7, 5)
+
 #define VERSION_MAJOR_ARCH_VER_MASK		GENMASK(31, 28)
 #define VERSION_MINOR_ARCH_VER_MASK		GENMASK(27, 24)
 #define VERSION_REV_ARCH_VER_MASK		GENMASK(23, 16)
diff --git a/arch/arm64/kvm/hyp/nvhe/iommu/s2mpu.c b/arch/arm64/kvm/hyp/nvhe/iommu/s2mpu.c
index bbe4490b7173..f83546b7d586 100644
--- a/arch/arm64/kvm/hyp/nvhe/iommu/s2mpu.c
+++ b/arch/arm64/kvm/hyp/nvhe/iommu/s2mpu.c
@@ -324,6 +324,71 @@ static bool s2mpu_host_smc_handler(struct kvm_cpu_context *host_ctxt)
 	return true;  /* SMC handled */
 }
 
+static struct s2mpu *find_s2mpu_by_addr(phys_addr_t addr)
+{
+	struct s2mpu *dev;
+
+	for_each_s2mpu(dev) {
+		if (dev->pa <= addr && addr < (dev->pa + S2MPU_MMIO_SIZE))
+			return dev;
+	}
+	return NULL;
+}
+
+static u32 host_mmio_reg_access_mask(size_t off, bool is_write)
+{
+	const u32 no_access  = 0;
+	const u32 read_write = (u32)(-1);
+	const u32 read_only  = is_write ? no_access  : read_write;
+	const u32 write_only = is_write ? read_write : no_access;
+	u32 masked_off;
+
+	/* IRQ handler can clear interrupts. */
+	if (off == REG_NS_INTERRUPT_CLEAR)
+		return write_only & ALL_VIDS_BITMAP;
+
+	/* IRQ handler can read bitmap of pending interrupts. */
+	if (off == REG_NS_FAULT_STATUS)
+		return read_only & ALL_VIDS_BITMAP;
+
+	/* IRQ handler can read fault information. */
+	masked_off = off & ~REG_NS_FAULT_VID_MASK;
+	if ((masked_off == REG_NS_FAULT_PA_LOW(0)) ||
+	    (masked_off == REG_NS_FAULT_PA_HIGH(0)) ||
+	    (masked_off == REG_NS_FAULT_INFO(0)))
+		return read_only;
+
+	return no_access;
+}
+
+static bool s2mpu_host_mmio_dabt_handler(struct kvm_cpu_context *host_ctxt,
+					 phys_addr_t fault_pa, unsigned int len,
+					 bool is_write, int rd)
+{
+	struct s2mpu *dev;
+	size_t off;
+	u32 mask;
+
+	/* Only handle MMIO access with u32 size and alignment. */
+	if ((len != sizeof(u32)) || (fault_pa & (sizeof(u32) - 1)))
+		return false;
+
+	dev = find_s2mpu_by_addr(fault_pa);
+	if (!dev || !is_powered_on(dev))
+		return false;
+
+	off = fault_pa - dev->pa;
+	mask = host_mmio_reg_access_mask(off, is_write);
+	if (!mask)
+		return false;
+
+	if (is_write)
+		writel_relaxed(cpu_reg(host_ctxt, rd) & mask, dev->va + off);
+	else
+		cpu_reg(host_ctxt, rd) = readl_relaxed(dev->va + off) & mask;
+	return true;
+}
+
 static int s2mpu_init(void)
 {
 	struct s2mpu *dev;
@@ -371,5 +436,6 @@ static int s2mpu_init(void)
 const struct kvm_iommu_ops kvm_s2mpu_ops = (struct kvm_iommu_ops){
 	.init = s2mpu_init,
 	.host_smc_handler = s2mpu_host_smc_handler,
+	.host_mmio_dabt_handler = s2mpu_host_mmio_dabt_handler,
 	.host_stage2_set_owner = s2mpu_host_stage2_set_owner,
 };