[PATCH 3/5] soc: qcom: mdt_loader: add qcom_mdt_pas_map/unmap_devmem_rscs()

[Mukesh Ojha <mukesh.ojha@oss.qualcomm.com>]

Add qcom_mdt_pas_map_devmem_rscs() and qcom_mdt_pas_unmap_devmem_rscs()
to IOMMU map/unmap RSC_DEVMEM resource table entries on SoCs where Linux
runs as hypervisor at EL2 and owns the IOMMU mappings for remote
processors.

The map function fetches the resource table via
qcom_scm_pas_get_rsc_table() and iterates over RSC_DEVMEM entries using
rsc_table_for_each_entry(), calling iommu_map() for each. Mapped entries
are tracked in a per-PAS linked list stored in a hash table keyed by
pas_id for later lookup by the unmap function.

Signed-off-by: Mukesh Ojha <mukesh.ojha@oss.qualcomm.com>
---
 drivers/soc/qcom/mdt_loader.c       | 189 ++++++++++++++++++++++++++++
 include/linux/soc/qcom/mdt_loader.h |  22 ++++
 2 files changed, 211 insertions(+)

diff --git a/drivers/soc/qcom/mdt_loader.c b/drivers/soc/qcom/mdt_loader.c
index ed882dd43587..dd84d0eba152 100644
--- a/drivers/soc/qcom/mdt_loader.c
+++ b/drivers/soc/qcom/mdt_loader.c
@@ -11,14 +11,34 @@
 #include <linux/device.h>
 #include <linux/elf.h>
 #include <linux/firmware.h>
+#include <linux/hashtable.h>
 #include <linux/io.h>
+#include <linux/iommu.h>
 #include <linux/kernel.h>
+#include <linux/list.h>
 #include <linux/module.h>
 #include <linux/firmware/qcom/qcom_scm.h>
+#include <linux/rsc_table.h>
 #include <linux/sizes.h>
 #include <linux/slab.h>
 #include <linux/soc/qcom/mdt_loader.h>
 
+#define RSC_TABLE_HASH_BITS	5  /* 32 buckets */
+
+static DEFINE_HASHTABLE(qcom_pas_rsc_table_map, RSC_TABLE_HASH_BITS);
+
+struct qcom_pas_devmem_rsc {
+	struct fw_rsc_devmem *devmem;
+	struct list_head node;
+};
+
+struct qcom_pas_rsc_table_info {
+	struct resource_table *rsc_table;
+	struct list_head devmem_list;
+	struct hlist_node hnode;
+	int pas_id;
+};
+
 static bool mdt_header_valid(const struct firmware *fw)
 {
 	const struct elf32_hdr *ehdr;
@@ -507,5 +527,174 @@ int qcom_mdt_pas_load(struct qcom_scm_pas_context *ctx, const struct firmware *f
 }
 EXPORT_SYMBOL_GPL(qcom_mdt_pas_load);
 
+static void __qcom_mdt_unmap_devmem_rscs(struct qcom_pas_rsc_table_info *info,
+					 struct iommu_domain *domain)
+{
+	struct qcom_pas_devmem_rsc *entry, *tmp;
+
+	list_for_each_entry_safe(entry, tmp, &info->devmem_list, node) {
+		iommu_unmap(domain, entry->devmem->da, entry->devmem->len);
+		list_del(&entry->node);
+		kfree(entry);
+	}
+}
+
+/**
+ * qcom_mdt_pas_unmap_devmem_rscs() - IOMMU unmap device memory resources
+ *					for a given Peripheral
+ * @ctx:	pas context data structure
+ * @domain:	IOMMU domain
+ *
+ * Looks up the resource table info previously stored by
+ * qcom_mdt_pas_map_devmem_rscs(), unmaps all RSC_DEVMEM entries from the
+ * IOMMU domain, and releases the associated memory.
+ */
+void qcom_mdt_pas_unmap_devmem_rscs(struct qcom_scm_pas_context *ctx,
+				    struct iommu_domain *domain)
+{
+	struct qcom_pas_rsc_table_info *info;
+	struct hlist_node *tmp;
+
+	if (!ctx || !domain)
+		return;
+
+	hash_for_each_possible_safe(qcom_pas_rsc_table_map, info, tmp, hnode, ctx->pas_id) {
+		if (info->pas_id != ctx->pas_id)
+			continue;
+
+		__qcom_mdt_unmap_devmem_rscs(info, domain);
+		hash_del(&info->hnode);
+		kfree(info->rsc_table);
+		kfree(info);
+		break;
+	}
+}
+EXPORT_SYMBOL_GPL(qcom_mdt_pas_unmap_devmem_rscs);
+
+static int __qcom_mdt_map_devmem_rscs(struct device *dev, void *ptr, int avail,
+				      struct iommu_domain *domain,
+				      struct qcom_pas_rsc_table_info *info)
+{
+	struct qcom_pas_devmem_rsc *devmem_info;
+	struct fw_rsc_devmem *rsc = ptr;
+	int ret;
+
+	if (sizeof(*rsc) > avail) {
+		dev_err(dev, "devmem rsc is truncated\n");
+		return -EINVAL;
+	}
+
+	if (rsc->reserved) {
+		dev_err(dev, "devmem rsc has non zero reserved bytes\n");
+		return -EINVAL;
+	}
+
+	devmem_info = kzalloc(sizeof(*devmem_info), GFP_KERNEL);
+	if (!devmem_info)
+		return -ENOMEM;
+
+	ret = iommu_map(domain, rsc->da, rsc->pa, rsc->len, rsc->flags, GFP_KERNEL);
+	if (ret) {
+		dev_err(dev, "failed to map devmem: %d\n", ret);
+		kfree(devmem_info);
+		return ret;
+	}
+
+	devmem_info->devmem = rsc;
+	list_add_tail(&devmem_info->node, &info->devmem_list);
+
+	dev_dbg(dev, "mapped devmem pa 0x%x, da 0x%x, len 0x%x\n",
+		rsc->pa, rsc->da, rsc->len);
+
+	return 0;
+}
+
+struct qcom_mdt_map_cb_data {
+	struct device *dev;
+	struct iommu_domain *domain;
+	struct qcom_pas_rsc_table_info *info;
+};
+
+static int qcom_mdt_map_devmem_cb(u32 type, void *rsc, int offset,
+				  int avail, void *data)
+{
+	struct qcom_mdt_map_cb_data *d = data;
+
+	if (type != RSC_DEVMEM)
+		return 0;
+
+	return __qcom_mdt_map_devmem_rscs(d->dev, rsc, avail, d->domain,
+					  d->info);
+}
+
+/**
+ * qcom_mdt_pas_map_devmem_rscs() - IOMMU map device memory resources for
+ *				     a given Peripheral
+ *
+ * This routine should be called when it is known that the SoC is running
+ * with Linux as hypervisor at EL2 where it is in control of the IOMMU map
+ * of the resources for the remote processors.
+ *
+ * @ctx:	    pas context data structure
+ * @domain:	    IOMMU domain
+ * @input_rt:	    input resource table buffer when resource table is part of
+ *		    firmware binary; pass NULL if not available
+ * @input_rt_size:  size of @input_rt; pass zero if @input_rt is NULL
+ *
+ * Returns 0 on success, negative errno otherwise.
+ */
+int qcom_mdt_pas_map_devmem_rscs(struct qcom_scm_pas_context *ctx,
+				 struct iommu_domain *domain,
+				 void *input_rt, size_t input_rt_size)
+{
+	struct qcom_mdt_map_cb_data map_data;
+	size_t output_rt_size = QCOM_MDT_MAX_RSCTABLE_SIZE;
+	struct qcom_pas_rsc_table_info *info;
+	struct resource_table *rsc_table;
+	int ret;
+
+	if (!ctx || !domain)
+		return -EINVAL;
+
+	rsc_table = qcom_scm_pas_get_rsc_table(ctx, input_rt, input_rt_size,
+					       &output_rt_size);
+	if (IS_ERR(rsc_table)) {
+		ret = PTR_ERR(rsc_table);
+		dev_err(ctx->dev, "error %d getting resource_table\n", ret);
+		return ret;
+	}
+
+	info = kzalloc(sizeof(*info), GFP_KERNEL);
+	if (!info) {
+		ret = -ENOMEM;
+		goto free_rsc_table;
+	}
+
+	map_data.dev = ctx->dev;
+	map_data.domain = domain;
+	map_data.info = info;
+	info->pas_id = ctx->pas_id;
+	info->rsc_table = rsc_table;
+	INIT_LIST_HEAD(&info->devmem_list);
+
+	ret = rsc_table_for_each_entry(rsc_table, output_rt_size, ctx->dev,
+				       qcom_mdt_map_devmem_cb, &map_data);
+	if (ret)
+		goto undo_mapping;
+
+	hash_add(qcom_pas_rsc_table_map, &info->hnode, ctx->pas_id);
+
+	return 0;
+
+undo_mapping:
+	__qcom_mdt_unmap_devmem_rscs(info, domain);
+	kfree(info);
+free_rsc_table:
+	kfree(rsc_table);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(qcom_mdt_pas_map_devmem_rscs);
+
 MODULE_DESCRIPTION("Firmware parser for Qualcomm MDT format");
 MODULE_LICENSE("GPL v2");
diff --git a/include/linux/soc/qcom/mdt_loader.h b/include/linux/soc/qcom/mdt_loader.h
index 7c551b98e182..304aebb2b9cd 100644
--- a/include/linux/soc/qcom/mdt_loader.h
+++ b/include/linux/soc/qcom/mdt_loader.h
@@ -8,8 +8,11 @@
 #define QCOM_MDT_TYPE_HASH	(2 << 24)
 #define QCOM_MDT_RELOCATABLE	BIT(27)
 
+#define QCOM_MDT_MAX_RSCTABLE_SIZE	SZ_16K
+
 struct device;
 struct firmware;
+struct iommu_domain;
 struct qcom_scm_pas_context;
 
 #if IS_ENABLED(CONFIG_QCOM_MDT_LOADER)
@@ -23,6 +26,12 @@ int qcom_mdt_load(struct device *dev, const struct firmware *fw,
 int qcom_mdt_pas_load(struct qcom_scm_pas_context *ctx, const struct firmware *fw,
 		      const char *firmware, phys_addr_t *reloc_base);
 
+int qcom_mdt_pas_map_devmem_rscs(struct qcom_scm_pas_context *ctx,
+				 struct iommu_domain *domain,
+				 void *input_rt, size_t input_rt_size);
+void qcom_mdt_pas_unmap_devmem_rscs(struct qcom_scm_pas_context *ctx,
+				    struct iommu_domain *domain);
+
 int qcom_mdt_load_no_init(struct device *dev, const struct firmware *fw,
 			  const char *fw_name, void *mem_region,
 			  phys_addr_t mem_phys, size_t mem_size,
@@ -52,6 +61,19 @@ static inline int qcom_mdt_pas_load(struct qcom_scm_pas_context *ctx,
 	return -ENODEV;
 }
 
+static inline int qcom_mdt_pas_map_devmem_rscs(struct qcom_scm_pas_context *ctx,
+					       struct iommu_domain *domain,
+					       void *input_rt,
+					       size_t input_rt_size)
+{
+	return -ENODEV;
+}
+
+static inline void qcom_mdt_pas_unmap_devmem_rscs(struct qcom_scm_pas_context *ctx,
+						  struct iommu_domain *domain)
+{
+}
+
 static inline int qcom_mdt_load_no_init(struct device *dev,
 					const struct firmware *fw,
 					const char *fw_name, void *mem_region,
-- 
2.53.0