From bd6705c7da22f5bf0443619f6f98637892515704 Mon Sep 17 00:00:00 2001
From: Sascha Hauer <s.hauer@pengutronix.de>
Date: Fri, 20 Dec 2019 12:32:23 +0100
Subject: of: Read dma_offset from device tree

This reads the dma-ranges property from the device tree and sets
dma_offset in the devices accordingly. The code is mostly taken
from the Kernel as of v5.5-rc1. of_dma_configure() is trimmed down
to the cases we want to support currently.

Signed-off-by: Sascha Hauer <s.hauer@pengutronix.de>
---
 drivers/of/address.c  | 109 ++++++++++++++++++++++++++++++++++++++++++++++++++
 drivers/of/platform.c |  18 +++++++++
 2 files changed, 127 insertions(+)

(limited to 'drivers/of')

diff --git a/drivers/of/address.c b/drivers/of/address.c
index 4e12522a0a..2020f5b7b1 100644
--- a/drivers/of/address.c
+++ b/drivers/of/address.c
@@ -458,6 +458,33 @@ bool of_can_translate_address(struct device_node *dev)
 }
 EXPORT_SYMBOL(of_can_translate_address);
 
+static struct device_node *__of_get_dma_parent(struct device_node *np)
+{
+	struct of_phandle_args args;
+	int ret, index;
+
+	index = of_property_match_string(np, "interconnect-names", "dma-mem");
+	if (index < 0)
+		return of_get_parent(np);
+
+	ret = of_parse_phandle_with_args(np, "interconnects",
+					 "#interconnect-cells",
+					 index, &args);
+	if (ret < 0)
+		return of_get_parent(np);
+
+	return args.np;
+}
+
+static struct device_node *of_get_next_dma_parent(struct device_node *np)
+{
+	struct device_node *parent;
+
+	parent = __of_get_dma_parent(np);
+
+	return parent;
+}
+
 const __be32 *of_get_address(struct device_node *dev, int index, u64 *size,
 		    unsigned int *flags)
 {
@@ -586,3 +613,85 @@ void __iomem *of_iomap(struct device_node *np, int index)
 	return IOMEM(res.start);
 }
 EXPORT_SYMBOL(of_iomap);
+
+/**
+ * of_dma_get_range - Get DMA range info
+ * @np:		device node to get DMA range info
+ * @dma_addr:	pointer to store initial DMA address of DMA range
+ * @paddr:	pointer to store initial CPU address of DMA range
+ * @size:	pointer to store size of DMA range
+ *
+ * Look in bottom up direction for the first "dma-ranges" property
+ * and parse it.
+ *  dma-ranges format:
+ *	DMA addr (dma_addr)	: naddr cells
+ *	CPU addr (phys_addr_t)	: pna cells
+ *	size			: nsize cells
+ *
+ * It returns -ENODEV if "dma-ranges" property was not found
+ * for this device in DT.
+ */
+int of_dma_get_range(struct device_node *np, u64 *dma_addr, u64 *paddr, u64 *size)
+{
+	struct device_node *node = np;
+	const __be32 *ranges = NULL;
+	int len, naddr, nsize, pna;
+	int ret = 0;
+	bool found_dma_ranges = false;
+	u64 dmaaddr;
+
+	while (node) {
+		ranges = of_get_property(node, "dma-ranges", &len);
+
+		/* Ignore empty ranges, they imply no translation required */
+		if (ranges && len > 0)
+			break;
+
+		/* Once we find 'dma-ranges', then a missing one is an error */
+		if (found_dma_ranges && !ranges) {
+			ret = -ENODEV;
+			goto out;
+		}
+		found_dma_ranges = true;
+
+		node = of_get_next_dma_parent(node);
+	}
+
+	if (!node || !ranges) {
+		pr_debug("no dma-ranges found for node(%pOF)\n", np);
+		ret = -ENODEV;
+		goto out;
+	}
+
+	naddr = of_bus_n_addr_cells(node);
+	nsize = of_bus_n_size_cells(node);
+	pna = of_n_addr_cells(node);
+	if ((len / sizeof(__be32)) % (pna + naddr + nsize)) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/* dma-ranges format:
+	 * DMA addr	: naddr cells
+	 * CPU addr	: pna cells
+	 * size		: nsize cells
+	 */
+	dmaaddr = of_read_number(ranges, naddr);
+	*paddr = of_translate_dma_address(node, ranges + naddr);
+	if (*paddr == OF_BAD_ADDR) {
+		pr_err("translation of DMA address(%llx) to CPU address failed node(%pOF)\n",
+		       dmaaddr, np);
+		ret = -EINVAL;
+		goto out;
+	}
+	*dma_addr = dmaaddr;
+
+	*size = of_read_number(ranges + naddr + pna, nsize);
+
+	pr_debug("dma_addr(%llx) cpu_addr(%llx) size(%llx)\n",
+		 *dma_addr, *paddr, *size);
+
+out:
+
+	return ret;
+}
diff --git a/drivers/of/platform.c b/drivers/of/platform.c
index d3795d799a..b1a7eb6730 100644
--- a/drivers/of/platform.c
+++ b/drivers/of/platform.c
@@ -74,6 +74,22 @@ static void of_device_make_bus_id(struct device_d *dev)
 	}
 }
 
+static void of_dma_configure(struct device_d *dev, struct device_node *np)
+{
+	u64 dma_addr, paddr, size = 0;
+	unsigned long offset;
+	int ret;
+
+	ret = of_dma_get_range(np, &dma_addr, &paddr, &size);
+	if (ret < 0) {
+		dma_addr = offset = 0;
+	} else {
+		offset = paddr - dma_addr;
+	}
+
+	dev->dma_offset = offset;
+}
+
 /**
  * of_platform_device_create - Alloc, initialize and register an of_device
  * @np: pointer to node to create device for
@@ -148,6 +164,8 @@ struct device_d *of_platform_device_create(struct device_node *np,
 	dev->num_resources = num_reg;
 	of_device_make_bus_id(dev);
 
+	of_dma_configure(dev, np);
+
 	resinval = (-1);
 
 	debug("%s: register device %s, io=%pa\n",
-- 
cgit v1.2.3