ANDROID: KVM: arm64: Set up S2MPU Memory Protection Table

S2MPU Second-level Memory Protection Table is a PA-contiguous buffer
containing an array of 2-bit read/write entries at given granularity
for a given gigabyte physical address space region. The size of SMPT
varies per granularity but at the finest 4K granularity it is 64KB
PA-contiguous, aligned to 64KB.

Allocate sufficient number of SMPT buffers for the S2MPU driver assuming
4K granularity for 4K/16K PAGE_SIZE, and 64K granularity for 64K
PAGE_SIZE. We also assume that all S2MPUs share SMPTs for a given
gigabyte region. There are 34 gigabyte regions that can be set by the
driver (GBs 4-33 always block all traffic).

Hyp takes ownership of the memory in s2mpu_init and assigns pointers to
the buffers to L1ENTRY_L2TABLE_ADDR registers on init and power-on
events. The pointers remain static as the driver will only change
granularity between 1G and 4K/64K (depending on PAGE_SIZE).

Test: builds, boots
Bug: 190463801
Change-Id: I3fcad8b3ce5d194a987b09d042bd56d59bb35e5e
Signed-off-by: David Brazdil <dbrazdil@google.com>
(cherry picked from commit f0e1de52ef)
Signed-off-by: Mostafa Saleh <smostafa@google.com>

arch/arm64/include/asm/kvm_s2mpu.h

@@ -29,6 +29,7 @@
 #define REG_NS_FAULT_PA_LOW(vid)		(0x2004 + ((vid) * 0x20))
 #define REG_NS_FAULT_PA_HIGH(vid)		(0x2008 + ((vid) * 0x20))
 #define REG_NS_FAULT_INFO(vid)			(0x2010 + ((vid) * 0x20))
+#define REG_NS_L1ENTRY_L2TABLE_ADDR(vid, gb)	(0x4000 + ((vid) * 0x200) + ((gb) * 0x8))
 #define REG_NS_L1ENTRY_ATTR(vid, gb)		(0x4004 + ((vid) * 0x200) + ((gb) * 0x8))
 
 #define CTRL0_ENABLE				BIT(0)
@@ -76,12 +77,40 @@
 #define FAULT_INFO_LEN_MASK			GENMASK(19, 16)
 #define FAULT_INFO_ID_MASK			GENMASK(15, 0)
 
+#define L1ENTRY_L2TABLE_ADDR(pa)		((pa) >> 4)
+
+#define L1ENTRY_ATTR_L2TABLE_EN			BIT(0)
+#define L1ENTRY_ATTR_GRAN_4K			0x0
+#define L1ENTRY_ATTR_GRAN_64K			0x1
+#define L1ENTRY_ATTR_GRAN_2M			0x2
 #define L1ENTRY_ATTR_PROT(prot)			FIELD_PREP(GENMASK(2, 1), prot)
+#define L1ENTRY_ATTR_GRAN(gran)			FIELD_PREP(GENMASK(5, 4), gran)
 #define L1ENTRY_ATTR_1G(prot)			L1ENTRY_ATTR_PROT(prot)
+#define L1ENTRY_ATTR_L2(gran)			(L1ENTRY_ATTR_GRAN(gran) | \
+						 L1ENTRY_ATTR_L2TABLE_EN)
 
 #define NR_GIGABYTES				64
 #define RO_GIGABYTES_FIRST			4
 #define RO_GIGABYTES_LAST			33
+#define NR_RO_GIGABYTES				(RO_GIGABYTES_LAST - RO_GIGABYTES_FIRST + 1)
+#define NR_RW_GIGABYTES				(NR_GIGABYTES - NR_RO_GIGABYTES)
+
+#ifdef CONFIG_ARM64_64K_PAGES
+#define SMPT_GRAN				SZ_64K
+#define SMPT_GRAN_ATTR				L1ENTRY_ATTR_GRAN_64K
+#else
+#define SMPT_GRAN				SZ_4K
+#define SMPT_GRAN_ATTR				L1ENTRY_ATTR_GRAN_4K
+#endif
+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_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)
+#define SMPT_ORDER				get_order(SMPT_SIZE)
 
 /*
  * Iterate over S2MPU gigabyte regions. Skip those that cannot be modified
@@ -123,10 +152,23 @@ enum mpt_prot {
 	MPT_PROT_MASK	= MPT_PROT_RW,
 };
 
+struct fmpt {
+	u32 *smpt;
+	bool gran_1g;
+	enum mpt_prot prot;
+};
+
+struct mpt {
+	struct fmpt fmpt[NR_GIGABYTES];
+};
+
 extern size_t kvm_nvhe_sym(kvm_hyp_nr_s2mpus);
 #define kvm_hyp_nr_s2mpus kvm_nvhe_sym(kvm_hyp_nr_s2mpus)
 
 extern struct s2mpu *kvm_nvhe_sym(kvm_hyp_s2mpus);
 #define kvm_hyp_s2mpus 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)
+
 #endif /* __ARM64_KVM_S2MPU_H__ */

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

@@ -13,6 +13,7 @@
 
 #include <linux/arm-smccc.h>
 
+#include <nvhe/memory.h>
 #include <nvhe/mm.h>
 #include <nvhe/spinlock.h>
 #include <nvhe/trap_handler.h>
@@ -28,6 +29,7 @@
 
 size_t __ro_after_init		kvm_hyp_nr_s2mpus;
 struct s2mpu __ro_after_init	*kvm_hyp_s2mpus;
+struct mpt			kvm_hyp_host_mpt;
 
 static hyp_spinlock_t		s2mpu_lock;
 
@@ -122,6 +124,26 @@ static void __set_l1entry_attr_with_prot(struct s2mpu *dev, unsigned int gb,
 		       dev->va + REG_NS_L1ENTRY_ATTR(vid, gb));
 }
 
+static void __set_l1entry_attr_with_fmpt(struct s2mpu *dev, unsigned int gb,
+					 unsigned int vid, struct fmpt *fmpt)
+{
+	if (fmpt->gran_1g) {
+		__set_l1entry_attr_with_prot(dev, gb, vid, fmpt->prot);
+	} else {
+		/* Order against writes to the SMPT. */
+		writel(L1ENTRY_ATTR_L2(SMPT_GRAN_ATTR),
+		       dev->va + REG_NS_L1ENTRY_ATTR(vid, gb));
+	}
+}
+
+static void __set_l1entry_l2table_addr(struct s2mpu *dev, unsigned int gb,
+				       unsigned int vid, phys_addr_t addr)
+{
+	/* Order against writes to the SMPT. */
+	writel(L1ENTRY_L2TABLE_ADDR(addr),
+	       dev->va + REG_NS_L1ENTRY_L2TABLE_ADDR(vid, gb));
+}
+
 /**
  * Initialize S2MPU device and set all GB regions to 1G granularity with
  * given protection bits.
@@ -141,6 +163,29 @@ static void initialize_with_prot(struct s2mpu *dev, enum mpt_prot prot)
 	__set_control_regs(dev);
 }
 
+/**
+ * Initialize S2MPU device, set L2 table addresses and configure L1TABLE_ATTR
+ * registers according to the given MPT struct.
+ */
+static void initialize_with_mpt(struct s2mpu *dev, struct mpt *mpt)
+{
+	unsigned int gb, vid;
+	struct fmpt *fmpt;
+
+	/* Must write CONTEXT_CFG_VALID_VID before setting L1ENTRY registers. */
+	__set_context_ids(dev);
+
+	for_each_gb_and_vid(gb, vid) {
+		fmpt = &mpt->fmpt[gb];
+		__set_l1entry_l2table_addr(dev, gb, vid, __hyp_pa(fmpt->smpt));
+		__set_l1entry_attr_with_fmpt(dev, gb, vid, fmpt);
+	}
+	__all_invalidation(dev);
+
+	/* Set control registers, enable the S2MPU. */
+	__set_control_regs(dev);
+}
+
 static bool s2mpu_host_smc_handler(struct kvm_cpu_context *host_ctxt)
 {
 	DECLARE_REG(u64, fn, host_ctxt, 0);
@@ -181,7 +226,7 @@ static bool s2mpu_host_smc_handler(struct kvm_cpu_context *host_ctxt)
 
 			if (mode == SMC_MODE_POWER_UP) {
 				dev->power_state = S2MPU_POWER_ON;
-				initialize_with_prot(dev, MPT_PROT_RW);
+				initialize_with_mpt(dev, &kvm_hyp_host_mpt);
 			} else {
 				initialize_with_prot(dev, MPT_PROT_NONE);
 				dev->power_state = S2MPU_POWER_OFF;
@@ -197,6 +242,7 @@ static bool s2mpu_host_smc_handler(struct kvm_cpu_context *host_ctxt)
 static int s2mpu_init(void)
 {
 	struct s2mpu *dev;
+	unsigned int gb;
 	int ret;
 
 	/* Map data structures in EL2 stage-1. */
@@ -206,6 +252,15 @@ static int s2mpu_init(void)
 	if (ret)
 		return ret;
 
+	for_each_gb(gb) {
+		ret = pkvm_create_mappings(
+			kvm_hyp_host_mpt.fmpt[gb].smpt,
+			kvm_hyp_host_mpt.fmpt[gb].smpt + SMPT_NUM_WORDS,
+			PAGE_HYP);
+		if (ret)
+			return ret;
+	}
+
 	/* Map S2MPU MMIO regions in EL2 stage-1. */
 	for_each_s2mpu(dev) {
 		ret = __pkvm_create_private_mapping(
@@ -219,7 +274,7 @@ static int s2mpu_init(void)
 	 * the blocking reset state as the bootloader may have programmed them.
 	 */
 	for_each_powered_s2mpu(dev)
-		initialize_with_prot(dev, MPT_PROT_RW);
+		initialize_with_mpt(dev, &kvm_hyp_host_mpt);
 	return 0;
 }
 

arch/arm64/kvm/iommu/s2mpu.c

@@ -301,9 +301,54 @@ static int create_s2mpu_array(struct s2mpu **array)
 	return 0;
 }
 
+static int alloc_smpts(struct mpt *mpt)
+{
+	unsigned int gb;
+
+	for_each_gb(gb) {
+		/* The returned buffer is aligned to its size, as required. */
+		mpt->fmpt[gb].smpt = (u32 *)__get_free_pages(GFP_KERNEL, SMPT_ORDER);
+		if (!mpt->fmpt[gb].smpt)
+			return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static void free_smpts(struct mpt *mpt)
+{
+	unsigned int gb;
+
+	for_each_gb(gb)
+		free_pages((unsigned long)mpt->fmpt[gb].smpt, SMPT_ORDER);
+}
+
+static int init_host_mpt(struct mpt *mpt)
+{
+	unsigned int gb;
+	int ret;
+
+	ret = alloc_smpts(mpt);
+	if (ret) {
+		kvm_err("Cannot allocate memory for S2MPU host MPT");
+		return ret;
+	}
+
+	/* Initialize the host MPT. Use 1G mappings with RW permissions. */
+	for_each_gb(gb) {
+		kvm_hyp_host_mpt.fmpt[gb] = (struct fmpt){
+			.gran_1g = true,
+			.prot = MPT_PROT_RW,
+			.smpt = kern_hyp_va(mpt->fmpt[gb].smpt),
+		};
+	}
+	return 0;
+}
+
 int kvm_s2mpu_init(void)
 {
 	struct s2mpu *s2mpus = NULL;
+	struct mpt mpt = {};
 	int ret;
 
 	ret = platform_driver_probe(&of_driver, s2mpu_probe);
@@ -314,10 +359,16 @@ int kvm_s2mpu_init(void)
 	if (ret)
 		goto out;
 
+	ret = init_host_mpt(&mpt);
+	if (ret)
+		goto out;
+
 	kvm_info("S2MPU driver initialized\n");
 
 out:
-	if (ret)
+	if (ret) {
 		free_s2mpu_array(s2mpus);
+		free_smpts(&mpt);
+	}
 	return ret;
 }