ANDROID: KVM: arm64: Modify S2MPU MPT in 'host_stage2_set_owner'

The 'host_stage2_set_owner' callback indicates that a range of
PA-contiguous pages changed owner. With all devices owned by the host,
the driver sets the protection bits in the corresponding FMPT/SMPT to
either MPT_PROT_RW if owned by the host or MPT_PROT_NONE otherwise.

For each gigabyte region, the implementation will select between 1G and
4K/64K (depending on PAGE_SIZE) mappings and populate the L1ENTRY_ATTR
register or SMPT bitmap, respectivelly.

The driver never dynamically switches between two granularities which
both require a SMPT. This is because the L1ENTRY_ATTR and
L1ENTRY_L2TABLE_ADDR registers would need to be set atomically.

Test: builds, boots
Bug: 190463801
Change-Id: Ifb0bdcaa143ef8eb213ba4133ac86d8b610a4bcf
Signed-off-by: David Brazdil <dbrazdil@google.com>
(cherry picked from commit 4475d993aa)
Signed-off-by: Mostafa Saleh <smostafa@google.com>
Signed-off-by: Quentin Perret <qperret@google.com>

arch/arm64/include/asm/kvm_s2mpu.h

@@ -9,6 +9,8 @@
 
 #include <linux/bitfield.h>
 
+#include <asm/kvm_mmu.h>
+
 #define S2MPU_MMIO_SIZE				SZ_64K
 
 #define NR_VIDS					8
@@ -25,6 +27,9 @@
 #define REG_NS_NUM_CONTEXT			0x100
 #define REG_NS_CONTEXT_CFG_VALID_VID		0x104
 #define REG_NS_ALL_INVALIDATION			0x1000
+#define REG_NS_RANGE_INVALIDATION		0x1020
+#define REG_NS_RANGE_INVALIDATION_START_PPN	0x1024
+#define REG_NS_RANGE_INVALIDATION_END_PPN	0x1028
 #define REG_NS_FAULT_STATUS			0x2000
 #define REG_NS_FAULT_PA_LOW(vid)		(0x2004 + ((vid) * 0x20))
 #define REG_NS_FAULT_PA_HIGH(vid)		(0x2008 + ((vid) * 0x20))
@@ -66,6 +71,7 @@
 		FIELD_PREP(GENMASK((4 * (ctx) + 2), 4 * (ctx)), (vid))
 
 #define INVALIDATION_INVALIDATE			BIT(0)
+#define RANGE_INVALIDATION_PPN_SHIFT		12
 
 #define NR_FAULT_INFO_REGS			8
 #define FAULT_INFO_VID_MASK			GENMASK(26, 24)
@@ -107,6 +113,8 @@ static_assert(SMPT_GRAN <= PAGE_SIZE);
 #define MPT_PROT_BITS				2
 #define SMPT_WORD_SIZE				sizeof(u32)
 #define SMPT_ELEMS_PER_BYTE			(BITS_PER_BYTE / MPT_PROT_BITS)
+#define SMPT_ELEMS_PER_WORD			(SMPT_WORD_SIZE * SMPT_ELEMS_PER_BYTE)
+#define SMPT_WORD_BYTE_RANGE			(SMPT_GRAN * SMPT_ELEMS_PER_WORD)
 #define SMPT_NUM_ELEMS				(SZ_1G / SMPT_GRAN)
 #define SMPT_NUM_WORDS				(SMPT_SIZE / SMPT_WORD_SIZE)
 #define SMPT_SIZE				(SMPT_NUM_ELEMS / SMPT_ELEMS_PER_BYTE)
@@ -152,6 +160,13 @@ enum mpt_prot {
 	MPT_PROT_MASK	= MPT_PROT_RW,
 };
 
+static const u64 mpt_prot_doubleword[] = {
+	[MPT_PROT_NONE] = 0x0000000000000000,
+	[MPT_PROT_R]    = 0x5555555555555555,
+	[MPT_PROT_W]	= 0xaaaaaaaaaaaaaaaa,
+	[MPT_PROT_RW]   = 0xffffffffffffffff,
+};
+
 struct fmpt {
 	u32 *smpt;
 	bool gran_1g;
@@ -162,6 +177,11 @@ struct mpt {
 	struct fmpt fmpt[NR_GIGABYTES];
 };
 
+enum mpt_update_flags {
+	MPT_UPDATE_L1 = BIT(0),
+	MPT_UPDATE_L2 = BIT(1),
+};
+
 extern size_t kvm_nvhe_sym(kvm_hyp_nr_s2mpus);
 #define kvm_hyp_nr_s2mpus kvm_nvhe_sym(kvm_hyp_nr_s2mpus)
 
@@ -171,4 +191,125 @@ extern struct s2mpu *kvm_nvhe_sym(kvm_hyp_s2mpus);
 extern struct mpt kvm_nvhe_sym(kvm_hyp_host_mpt);
 #define kvm_hyp_host_mpt kvm_nvhe_sym(kvm_hyp_host_mpt)
 
+/* Set protection bits of SMPT in a given range without using memset. */
+static inline void __set_smpt_range_slow(u32 *smpt, size_t start_gb_byte,
+					 size_t end_gb_byte, enum mpt_prot prot)
+{
+	size_t i, start_word_byte, end_word_byte, word_idx, first_elem, last_elem;
+	u32 val;
+
+	/* Iterate over u32 words. */
+	start_word_byte = start_gb_byte;
+	while (start_word_byte < end_gb_byte) {
+		/* Determine the range of bytes covered by this word. */
+		word_idx = start_word_byte / SMPT_WORD_BYTE_RANGE;
+		end_word_byte = min(
+			ALIGN(start_word_byte + 1, SMPT_WORD_BYTE_RANGE),
+			end_gb_byte);
+
+		/* Identify protection bit offsets within the word. */
+		first_elem = (start_word_byte / SMPT_GRAN) % SMPT_ELEMS_PER_WORD;
+		last_elem = ((end_word_byte - 1) / SMPT_GRAN) % SMPT_ELEMS_PER_WORD;
+
+		/* Modify the corresponding word. */
+		val = READ_ONCE(smpt[word_idx]);
+		for (i = first_elem; i <= last_elem; i++) {
+			val &= ~(MPT_PROT_MASK << (i * MPT_PROT_BITS));
+			val |= prot << (i * MPT_PROT_BITS);
+		}
+		WRITE_ONCE(smpt[word_idx], val);
+
+		start_word_byte = end_word_byte;
+	}
+}
+
+/* Set protection bits of SMPT in a given range. */
+static inline void __set_smpt_range(u32 *smpt, size_t start_gb_byte,
+				    size_t end_gb_byte, enum mpt_prot prot)
+{
+	size_t interlude_start, interlude_end, interlude_bytes, word_idx;
+	char prot_byte = (char)mpt_prot_doubleword[prot];
+
+	if (start_gb_byte >= end_gb_byte)
+		return;
+
+	/* Check if range spans at least one full u32 word. */
+	interlude_start = ALIGN(start_gb_byte, SMPT_WORD_BYTE_RANGE);
+	interlude_end = ALIGN_DOWN(end_gb_byte, SMPT_WORD_BYTE_RANGE);
+
+	/* If not, fall back to editing bits in the given range. */
+	if (interlude_start >= interlude_end) {
+		__set_smpt_range_slow(smpt, start_gb_byte, end_gb_byte, prot);
+		return;
+	}
+
+	/* Use bit-editing for prologue/epilogue, memset for interlude. */
+	word_idx = interlude_start / SMPT_WORD_BYTE_RANGE;
+	interlude_bytes = (interlude_end - interlude_start) / SMPT_GRAN / SMPT_ELEMS_PER_BYTE;
+
+	__set_smpt_range_slow(smpt, start_gb_byte, interlude_start, prot);
+	memset(&smpt[word_idx], prot_byte, interlude_bytes);
+	__set_smpt_range_slow(smpt, interlude_end, end_gb_byte, prot);
+}
+
+/* Returns true if all SMPT protection bits match 'prot'. */
+static inline bool __is_smpt_uniform(u32 *smpt, enum mpt_prot prot)
+{
+	size_t i;
+	u64 *doublewords = (u64 *)smpt;
+
+	for (i = 0; i < SMPT_NUM_WORDS / 2; i++) {
+		if (doublewords[i] != mpt_prot_doubleword[prot])
+			return false;
+	}
+	return true;
+}
+
+/**
+ * Set protection bits of FMPT/SMPT in a given range.
+ * Returns flags specifying whether L1/L2 changes need to be made visible
+ * to the device.
+ */
+static inline enum mpt_update_flags
+__set_fmpt_range(struct fmpt *fmpt, size_t start_gb_byte, size_t end_gb_byte,
+		 enum mpt_prot prot)
+{
+	if (start_gb_byte == 0 && end_gb_byte >= SZ_1G) {
+		/* Update covers the entire GB region. */
+		if (fmpt->gran_1g && fmpt->prot == prot)
+			return 0;
+
+		fmpt->gran_1g = true;
+		fmpt->prot = prot;
+		return MPT_UPDATE_L1;
+	}
+
+	if (fmpt->gran_1g) {
+		/* GB region currently uses 1G mapping. */
+		if (fmpt->prot == prot)
+			return 0;
+
+		/*
+		 * Range has different mapping than the rest of the GB.
+		 * Convert to PAGE_SIZE mapping.
+		 */
+		fmpt->gran_1g = false;
+		__set_smpt_range(fmpt->smpt, 0, start_gb_byte, fmpt->prot);
+		__set_smpt_range(fmpt->smpt, start_gb_byte, end_gb_byte, prot);
+		__set_smpt_range(fmpt->smpt, end_gb_byte, SZ_1G, fmpt->prot);
+		return MPT_UPDATE_L1 | MPT_UPDATE_L2;
+	}
+
+	/* GB region currently uses PAGE_SIZE mapping. */
+	__set_smpt_range(fmpt->smpt, start_gb_byte, end_gb_byte, prot);
+
+	/* Check if the entire GB region has the same prot bits. */
+	if (!__is_smpt_uniform(fmpt->smpt, prot))
+		return MPT_UPDATE_L2;
+
+	fmpt->gran_1g = true;
+	fmpt->prot = prot;
+	return MPT_UPDATE_L1;
+}
+
 #endif /* __ARM64_KVM_S2MPU_H__ */

arch/arm64/kvm/hyp/nvhe/iommu/s2mpu.c

@@ -21,6 +21,8 @@
 #define SMC_CMD_PREPARE_PD_ONOFF	0x82000410
 #define SMC_MODE_POWER_UP		1
 
+#define PA_MAX				((phys_addr_t)SZ_1G * NR_GIGABYTES)
+
 #define for_each_s2mpu(i) \
 	for ((i) = &kvm_hyp_s2mpus[0]; (i) != &kvm_hyp_s2mpus[kvm_hyp_nr_s2mpus]; (i)++)
 
@@ -117,6 +119,17 @@ static void __all_invalidation(struct s2mpu *dev)
 		       dev->va + REG_NS_ALL_INVALIDATION);
 }
 
+static void __range_invalidation(struct s2mpu *dev, phys_addr_t first_byte,
+				 phys_addr_t last_byte)
+{
+	u32 start_ppn = first_byte >> RANGE_INVALIDATION_PPN_SHIFT;
+	u32 end_ppn = last_byte >> RANGE_INVALIDATION_PPN_SHIFT;
+
+	writel_relaxed(start_ppn, dev->va + REG_NS_RANGE_INVALIDATION_START_PPN);
+	writel_relaxed(end_ppn, dev->va + REG_NS_RANGE_INVALIDATION_END_PPN);
+	writel_relaxed(INVALIDATION_INVALIDATE, dev->va + REG_NS_RANGE_INVALIDATION);
+}
+
 static void __set_l1entry_attr_with_prot(struct s2mpu *dev, unsigned int gb,
 					 unsigned int vid, enum mpt_prot prot)
 {
@@ -186,6 +199,77 @@ static void initialize_with_mpt(struct s2mpu *dev, struct mpt *mpt)
 	__set_control_regs(dev);
 }
 
+/**
+ * Set MPT protection bits set to 'prot' in the give byte range (page-aligned).
+ * Update currently powered S2MPUs.
+ */
+static void set_mpt_range_locked(struct mpt *mpt, phys_addr_t first_byte,
+				 phys_addr_t last_byte, enum mpt_prot prot)
+{
+	unsigned int first_gb = first_byte / SZ_1G;
+	unsigned int last_gb = last_byte / SZ_1G;
+	size_t start_gb_byte, end_gb_byte;
+	unsigned int gb, vid;
+	struct s2mpu *dev;
+	struct fmpt *fmpt;
+	enum mpt_update_flags flags;
+
+	for_each_gb_in_range(gb, first_gb, last_gb) {
+		fmpt = &mpt->fmpt[gb];
+		start_gb_byte = (gb == first_gb) ? first_byte % SZ_1G : 0;
+		end_gb_byte = (gb == last_gb) ? (last_byte % SZ_1G) + 1 : SZ_1G;
+
+		flags = __set_fmpt_range(fmpt, start_gb_byte, end_gb_byte, prot);
+
+		if (flags & MPT_UPDATE_L2)
+			kvm_flush_dcache_to_poc(fmpt->smpt, SMPT_SIZE);
+
+		if (flags & MPT_UPDATE_L1) {
+			for_each_powered_s2mpu(dev) {
+				for_each_vid(vid)
+					__set_l1entry_attr_with_fmpt(dev, gb, vid, fmpt);
+			}
+		}
+	}
+
+	/* Invalidate range in all powered S2MPUs. */
+	for_each_powered_s2mpu(dev)
+		__range_invalidation(dev, first_byte, last_byte);
+}
+
+static void s2mpu_host_stage2_set_owner(phys_addr_t addr, size_t size, u32 owner_id)
+{
+	/* Grant access only to the default owner of the page table (ID=0). */
+	enum mpt_prot prot = owner_id ? MPT_PROT_NONE : MPT_PROT_RW;
+
+	/*
+	 * NOTE: The following code refers to 'end' as the exclusive upper
+	 * bound and 'last' as the inclusive one.
+	 */
+
+	/*
+	 * Sanitize inputs with S2MPU-specific physical address space bounds.
+	 * Ownership change requests outside this boundary will be ignored.
+	 * The S2MPU also specifies that the PA region 4-34GB always maps to
+	 * PROT_NONE and the corresponding MMIO registers are read-only.
+	 * Ownership changes in this region will have no effect.
+	 */
+
+	if (addr >= PA_MAX)
+		return;
+
+	size = min(size, (size_t)(PA_MAX - addr));
+	if (size == 0)
+		return;
+
+	hyp_spin_lock(&s2mpu_lock);
+	set_mpt_range_locked(&kvm_hyp_host_mpt,
+			     ALIGN_DOWN(addr, SMPT_GRAN),
+			     ALIGN(addr + size, SMPT_GRAN) - 1,
+			     prot);
+	hyp_spin_unlock(&s2mpu_lock);
+}
+
 static bool s2mpu_host_smc_handler(struct kvm_cpu_context *host_ctxt)
 {
 	DECLARE_REG(u64, fn, host_ctxt, 0);
@@ -281,4 +365,5 @@ 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_stage2_set_owner = s2mpu_host_stage2_set_owner,
 };