[rtems-libbsd commit] linux/of_address.h: Add of_translate_address()

[Sebastian Huber]

Module:    rtems-libbsd
Branch:    master
Commit:    0f1d2f6174999b574b57a4fc268ce9a3c618d55f
Changeset: http://git.rtems.org/rtems-libbsd/commit/?id=0f1d2f6174999b574b57a4fc268ce9a3c618d55f

Author:    Sebastian Huber <sebastian.huber at embedded-brains.de>
Date:      Wed Jan 10 13:18:05 2018 +0100

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 e91baaa..7be4557 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 928b7e5..e82c538 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 c0fdebb..8113eb2 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)