1 files changed, 97 insertions, 1 deletions

diff --git a/drivers/firmware/kvm-hyp-services.c b/drivers/firmware/kvm-hyp-services.c
index bfb5ab5fba1..2e62f16aa50 100644
--- a/drivers/firmware/kvm-hyp-services.c
+++ b/drivers/firmware/kvm-hyp-services.c
@@ -5,8 +5,14 @@
 
 #include <common.h>
 #include <dm.h>
+#include <malloc.h>
+#include <virtio_types.h>
+#include <virtio.h>
+#include <asm/io.h>
 #include <dm/device_compat.h>
 #include <linux/arm-smccc.h>
+#include <linux/compat.h>
+#include <linux/psci.h>
 
 #define DRIVER_NAME	"kvm-hyp-services"
 
@@ -16,6 +22,23 @@ static bool kvm_hyp_services_is_supported(void (*invoke_fn)(unsigned long a0, un
 							    unsigned long a6, unsigned long a7,
 							    struct arm_smccc_res *res))
 {
+	u32 smccc_version = ARM_SMCCC_VERSION_1_0;
+	struct arm_smccc_res res;
+
+	smccc_version = invoke_psci_fn(ARM_SMCCC_VERSION_FUNC_ID, 0, 0, 0);
+
+	if (smccc_version < ARM_SMCCC_VERSION_1_1)
+		return false;
+
+	(*invoke_fn)(ARM_SMCCC_VENDOR_HYP_CALL_UID_FUNC_ID,
+		     0, 0, 0, 0, 0, 0, 0, &res);
+
+	if (res.a0 != ARM_SMCCC_VENDOR_HYP_UID_KVM_REG_0 ||
+	    res.a1 != ARM_SMCCC_VENDOR_HYP_UID_KVM_REG_1 ||
+	    res.a2 != ARM_SMCCC_VENDOR_HYP_UID_KVM_REG_2 ||
+	    res.a3 != ARM_SMCCC_VENDOR_HYP_UID_KVM_REG_3)
+		return false;
+
 	return true;
 }
 
@@ -24,11 +47,84 @@ ARM_SMCCC_FEATURE_DRIVER(kvm_hyp_services) = {
 	.is_supported = kvm_hyp_services_is_supported,
 };
 
-static int kvm_hyp_services_probe(struct udevice *dev)
+extern struct virtio_iommu_platform_ops *virtio_iommu_platform_ops;
+
+static int kvm_hyp_mem_share(struct udevice *udev, void *addr, u32 npages)
+{
+	struct psci_plat_data *smccc = dev_get_parent_plat(udev);
+
+	while (npages--) {
+		struct arm_smccc_res res;
+		phys_addr_t phys = virt_to_phys(addr);
+
+		smccc->invoke_fn(ARM_SMCCC_VENDOR_HYP_KVM_MEM_SHARE_FUNC_ID,
+				 phys, 0, 0, 0, 0, 0, 0, &res);
+		if (res.a0 != SMCCC_RET_SUCCESS)
+			return -EPERM;
+
+		addr += PAGE_SIZE;
+	}
+
+	return 0;
+}
+
+static int kvm_hyp_mem_unshare(struct udevice *udev, void *addr, u32 npages)
+{
+	struct psci_plat_data *smccc = dev_get_parent_plat(udev);
+
+	while (npages--) {
+		struct arm_smccc_res res;
+		phys_addr_t phys = virt_to_phys(addr);
+
+		smccc->invoke_fn(ARM_SMCCC_VENDOR_HYP_KVM_MEM_UNSHARE_FUNC_ID,
+				 phys, 0, 0, 0, 0, 0, 0, &res);
+		if (res.a0 != SMCCC_RET_SUCCESS)
+			return -EPERM;
+
+		addr += PAGE_SIZE;
+	}
+
+	return 0;
+}
+
+static int kvm_hyp_memshare_init(unsigned long features, struct psci_plat_data *smccc)
 {
+	static struct virtio_iommu_platform_ops ops = {
+		.map	= kvm_hyp_mem_share,
+		.unmap	= kvm_hyp_mem_unshare,
+	};
+	struct arm_smccc_res res;
+
+	smccc->invoke_fn(ARM_SMCCC_VENDOR_HYP_KVM_HYP_MEMINFO_FUNC_ID,
+			 0, 0, 0, 0, 0, 0, 0, &res);
+
+	if (res.a0 != PAGE_SIZE)
+		return -ENXIO;
+
+	virtio_iommu_platform_ops = &ops;
 	return 0;
 }
 
+static int kvm_hyp_services_probe(struct udevice *dev)
+{
+	int ret = 0;
+	struct psci_plat_data *smccc = dev_get_parent_plat(dev);
+	struct arm_smccc_res res;
+
+	if (!(kvm_hyp_services_is_supported(smccc->invoke_fn)))
+		return -ENXIO;
+
+	memset(&res, 0, sizeof(res));
+	smccc->invoke_fn(ARM_SMCCC_VENDOR_HYP_KVM_FEATURES_FUNC_ID,
+			 0, 0, 0, 0, 0, 0, 0, &res);
+
+	if (res.a0 & BIT(ARM_SMCCC_KVM_FUNC_HYP_MEMINFO))
+		ret = kvm_hyp_memshare_init(res.a0, smccc);
+
+	pr_debug("Probed KVM hypervisor services: 0x%08x\n", (u32)res.a0);
+	return ret;
+}
+
 U_BOOT_DRIVER(kvm_hyp_services) = {
 	.name = DRIVER_NAME,
 	.id = UCLASS_FIRMWARE,