[PATCH v2 5/5] firmware: stratix10-svc: enable Agilex5 SMMU support in probe

[Adrian Ng Ho Yin]

Wire up the Agilex5-specific path in stratix10_svc_drv_probe():

Add intel,agilex5-svc to the of_device_id match table with
SVC_FLAG_USE_DMA_MEM set in .data. The flag is read at probe via
of_device_get_match_data() to select the DMA coherent path without
hardcoding a compatible string check in the probe function.

On Agilex5, DDR starts at 0x8000_0000 which is outside the SDM's
addressable range, making the SMMU mandatory. Fail probe with -ENODEV
if no IOMMU domain is attached to the device.

Issue INTEL_SIP_SMC_SDM_REMAPPER_CONFIG/BYPASS to disable the hardware
address remapper present in Agilex5 REV B. Without this, the remapper
adds an extra offset on top of the SMMU IOVA translation, producing an
incorrect address. Bypassing it keeps behaviour consistent across all
Agilex5 silicon revisions.

Register svc_data_mem_cleanup() as a devm action on the DMA path to
free any buffers leaked by service clients on driver unbind. Guard
err_destroy_pool against NULL genpool for the early-exit DMA path.

Signed-off-by: Adrian Ng Ho Yin <adrian.ho.yin.ng@xxxxxxxxxx>
---
 drivers/firmware/stratix10-svc.c | 86 +++++++++++++++++++++++++++-----
 1 file changed, 74 insertions(+), 12 deletions(-)

diff --git a/drivers/firmware/stratix10-svc.c b/drivers/firmware/stratix10-svc.c
index 4a2071d57399..4b96e795ffd0 100644
--- a/drivers/firmware/stratix10-svc.c
+++ b/drivers/firmware/stratix10-svc.c
@@ -62,6 +62,9 @@
 #define SVC_SDM_DMA_ADDR_BITS			29
 #define SVC_SDM_DMA_ADDR_OFFSET			0x80000000UL
 
+/* Per-compatible feature flags stored in of_device_id.data */
+#define SVC_FLAG_USE_DMA_MEM			BIT(0)
+
 /* stratix10 service layer clients */
 #define STRATIX10_RSU				"stratix10-rsu"
 
@@ -2006,6 +2009,7 @@ EXPORT_SYMBOL_GPL(stratix10_svc_free_memory);
 static const struct of_device_id stratix10_svc_drv_match[] = {
 	{.compatible = "intel,stratix10-svc"},
 	{.compatible = "intel,agilex-svc"},
+	{.compatible = "intel,agilex5-svc", .data = (void *)SVC_FLAG_USE_DMA_MEM},
 	{},
 };
 
@@ -2016,13 +2020,39 @@ static const char * const chan_names[SVC_NUM_CHANNEL] = {
 	SVC_CLIENT_HWMON
 };
 
+static void svc_data_mem_cleanup(void *data)
+{
+	struct stratix10_svc_controller *ctrl = data;
+	struct stratix10_svc_data_mem *pmem, *tmp;
+
+	guard(mutex)(&svc_mem_lock);
+
+	list_for_each_entry_safe(pmem, tmp, &svc_data_mem, node) {
+		dev_warn(ctrl->dev, "leaked svc buffer %p, freeing on unbind\n",
+			 pmem->vaddr);
+		if (ctrl->use_dma_mem) {
+			dma_free_coherent(ctrl->dev, pmem->size,
+					  pmem->vaddr, pmem->dma_addr);
+			list_del(&pmem->node);
+			kfree(pmem);
+		} else {
+			gen_pool_free(ctrl->genpool,
+				      (unsigned long)pmem->vaddr, pmem->size);
+			pmem->vaddr = NULL;
+			list_del(&pmem->node);
+		}
+	}
+}
+
 static int stratix10_svc_drv_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
 	struct stratix10_svc_controller *controller;
-	struct gen_pool *genpool;
+	struct gen_pool *genpool = NULL;
 	struct stratix10_svc_sh_memory *sh_memory;
 	struct stratix10_svc *svc = NULL;
+	struct arm_smccc_res res;
+	bool use_dma_mem = false;
 
 	svc_invoke_fn *invoke_fn;
 	size_t fifo_size;
@@ -2033,18 +2063,41 @@ static int stratix10_svc_drv_probe(struct platform_device *pdev)
 	if (IS_ERR(invoke_fn))
 		return -EINVAL;
 
-	sh_memory = devm_kzalloc(dev, sizeof(*sh_memory), GFP_KERNEL);
-	if (!sh_memory)
-		return -ENOMEM;
+	use_dma_mem = ((unsigned long)of_device_get_match_data(dev) &
+			 SVC_FLAG_USE_DMA_MEM);
 
-	sh_memory->invoke_fn = invoke_fn;
-	ret = svc_get_sh_memory(pdev, sh_memory);
-	if (ret)
-		return ret;
+	if (use_dma_mem) {
+		if (!iommu_get_domain_for_dev(dev)) {
+			dev_err(dev,
+				"SMMU is required for agilex5-svc but no IOMMU domain found\n");
+			dev_err(dev,
+				"Ensure the SMMU node is enabled in the device tree and 'iommus' is set for this node\n");
+			return -ENODEV;
+		}
 
-	genpool = svc_create_memory_pool(pdev, sh_memory);
-	if (IS_ERR(genpool))
-		return PTR_ERR(genpool);
+		invoke_fn(INTEL_SIP_SMC_SDM_REMAPPER_CONFIG,
+			  INTEL_SIP_SMC_SDM_REMAPPER_BYPASS,
+			  0, 0, 0, 0, 0, 0, &res);
+	}
+
+	if (use_dma_mem) {
+		ret = svc_setup_dma_memory(pdev);
+		if (ret)
+			return ret;
+	} else {
+		sh_memory = devm_kzalloc(dev, sizeof(*sh_memory), GFP_KERNEL);
+		if (!sh_memory)
+			return -ENOMEM;
+
+		sh_memory->invoke_fn = invoke_fn;
+		ret = svc_get_sh_memory(pdev, sh_memory);
+		if (ret)
+			return ret;
+
+		genpool = svc_create_memory_pool(pdev, sh_memory);
+		if (IS_ERR(genpool))
+			return PTR_ERR(genpool);
+	}
 
 	/* allocate service controller and supporting channel */
 	controller = devm_kzalloc(dev, struct_size(controller, chans, SVC_NUM_CHANNEL),
@@ -2059,9 +2112,17 @@ static int stratix10_svc_drv_probe(struct platform_device *pdev)
 	controller->num_active_client = 0;
 	controller->genpool = genpool;
 	controller->invoke_fn = invoke_fn;
+	controller->use_dma_mem = use_dma_mem;
+	controller->dma_addr_offset = use_dma_mem ? SVC_SDM_DMA_ADDR_OFFSET : 0;
 	INIT_LIST_HEAD(&controller->node);
 	init_completion(&controller->complete_status);
 
+	if (use_dma_mem) {
+		ret = devm_add_action_or_reset(dev, svc_data_mem_cleanup, controller);
+		if (ret)
+			goto err_destroy_pool;
+	}
+
 	ret = stratix10_svc_async_init(controller);
 	if (ret) {
 		dev_dbg(dev, "Intel Service Layer Driver: Error on stratix10_svc_async_init %d\n",
@@ -2129,7 +2190,8 @@ static int stratix10_svc_drv_probe(struct platform_device *pdev)
 		kfifo_free(&controller->chans[i].svc_fifo);
 	stratix10_svc_async_exit(controller);
 err_destroy_pool:
-	gen_pool_destroy(genpool);
+	if (genpool)
+		gen_pool_destroy(genpool);
 
 	return ret;
 }
-- 
2.49.GIT