From 0f1d2f6174999b574b57a4fc268ce9a3c618d55f Mon Sep 17 00:00:00 2001
From: Sebastian Huber <sebastian.huber@embedded-brains.de>
Date: Wed, 10 Jan 2018 13:18:05 +0100
Subject: linux/of_address.h: Add of_translate_address()

Update #3277.
---
 rtemsbsd/powerpc/include/linux/of.h         |  4 +-
 rtemsbsd/powerpc/include/linux/of_address.h |  4 +-
 rtemsbsd/sys/powerpc/compat.c               | 76 ++++++++++++++++++++++++++++-
 3 files changed, 81 insertions(+), 3 deletions(-)

diff --git a/rtemsbsd/powerpc/include/linux/of.h b/rtemsbsd/powerpc/include/linux/of.h
index e91baaaf..7be45576 100644
--- a/rtemsbsd/powerpc/include/linux/of.h
+++ b/rtemsbsd/powerpc/include/linux/of.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, 2017 embedded brains GmbH
+ * Copyright (c) 2015, 2018 embedded brains GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -35,6 +35,8 @@
 extern "C" {
 #endif /* __cplusplus */
 
+#define	OF_BAD_ADDR ((uint64_t)-1)
+
 typedef uint32_t phandle;
 
 struct device_node {
diff --git a/rtemsbsd/powerpc/include/linux/of_address.h b/rtemsbsd/powerpc/include/linux/of_address.h
index 928b7e58..e82c538b 100644
--- a/rtemsbsd/powerpc/include/linux/of_address.h
+++ b/rtemsbsd/powerpc/include/linux/of_address.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015 embedded brains GmbH
+ * Copyright (c) 2015, 2018 embedded brains GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -34,6 +34,8 @@
 extern "C" {
 #endif /* __cplusplus */
 
+uint64_t of_translate_address(struct device_node *dn, const uint32_t *addr);
+
 int of_address_to_resource(struct device_node *dn, int index,
     struct resource *res);
 
diff --git a/rtemsbsd/sys/powerpc/compat.c b/rtemsbsd/sys/powerpc/compat.c
index c0fdebbd..8113eb20 100644
--- a/rtemsbsd/sys/powerpc/compat.c
+++ b/rtemsbsd/sys/powerpc/compat.c
@@ -2,7 +2,7 @@
 #include <rtems/bsd/local/opt_dpaa.h>
 
 /*
- * Copyright (c) 2015 embedded brains GmbH
+ * Copyright (c) 2015, 2018 embedded brains GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -250,6 +250,80 @@ of_n_size_cells(struct device_node *dn)
 	return (get_size_cells(bsp_fdt_get(), dn->offset));
 }
 
+static uint64_t
+translate_address(const char *fdt, int node, int ac, int sc,
+    const uint32_t *addr)
+{
+	int pac;
+	int psc;
+	uint64_t taddr;
+
+	taddr = of_read_number(addr, ac);
+
+	node = fdt_parent_offset(fdt, node);
+	if (node < 0)
+		return (OF_BAD_ADDR);
+
+	for (;;) {
+		int len;
+		int parent;
+		const uint32_t *ranges;
+		uint64_t offset;
+
+		parent = fdt_parent_offset(fdt, node);
+		if (parent < 0)
+			break;
+
+		pac = get_address_cells(fdt, parent);
+		if (pac < 0)
+			return (OF_BAD_ADDR);
+
+		psc = get_size_cells(fdt, parent);
+		if (psc < 0)
+			return (OF_BAD_ADDR);
+
+		ranges = fdt_getprop(fdt, node, "ranges", &len);
+		if (ranges == NULL || len == 0)
+			break;
+
+		if (len != (ac + pac + sc) * 4)
+			return (OF_BAD_ADDR);
+
+		if (of_read_number(&ranges[0], ac) != 0)
+			return (OF_BAD_ADDR);
+
+		offset = of_read_number(&ranges[ac], pac);
+		taddr += offset;
+
+		node = parent;
+		ac = pac;
+		sc = psc;
+	}
+
+	return (taddr);
+}
+
+uint64_t
+of_translate_address(struct device_node *dn, const uint32_t *addr)
+{
+	const void *fdt = bsp_fdt_get();
+	int node;
+	int ac;
+	int sc;
+
+	node = dn->offset;
+
+	ac = get_address_cells(fdt, node);
+	if (ac < 0)
+		return (OF_BAD_ADDR);
+
+	sc = get_size_cells(fdt, node);
+	if (sc < 0)
+		return (OF_BAD_ADDR);
+
+	return (translate_address(fdt, node, ac, sc, addr));
+}
+
 int
 of_address_to_resource(struct device_node *dn, int index,
     struct resource *res)
-- 
cgit v1.2.3