Moved some functions to common/

2 files changed, 56 insertions, 0 deletions

diff --git a/common/debug.c b/common/debug.c
index c82377c..523ae7b 100644
--- a/common/debug.c
+++ b/common/debug.c
@@ -1,6 +1,8 @@
 #include <apos/types.h>
 #include <apos/debug.h>
 #include <apos/bits.h>
+#include <apos/pmem.h>
+#include <libfdt.h>
 #include <stdarg.h>
 
 #ifdef DEBUG
@@ -56,6 +58,36 @@ static void __putchar(char c)
 	port->data = c;
 }
 
+static enum serial_dev_t serial_dev_enum(const char *dev_name)
+{
+	if (strncmp("ns16550", dev_name, 7) == 0)
+		return NS16550A;
+
+	return -1;
+}
+
+struct dbg_info_t dbg_from_fdt(void *fdt)
+{
+	int chosen_offset = fdt_path_offset(fdt, "/chosen");
+	const char *stdout = fdt_getprop(fdt, chosen_offset, "stdout-path", NULL);
+
+	int stdout_offset = fdt_path_offset(fdt, stdout);
+
+	/* get serial device type */
+	const char *dev_name = (const char *)fdt_getprop(fdt, stdout_offset,
+			"compatible", NULL);
+
+	enum serial_dev_t dev = serial_dev_enum(dev_name);
+
+	/* get serial device address */
+	struct cell_info_t ci = get_reginfo(fdt, stdout);
+	void *reg_ptr = (void *)fdt_getprop(fdt, stdout_offset, "reg", NULL);
+
+	void *dbg_ptr = (void *)(paddr_t)fdt_load_int_ptr(ci.addr_cells, reg_ptr);
+
+	return (struct dbg_info_t){dbg_ptr, dev};
+}
+
 void dbg_init(void *pt, enum serial_dev_t dev)
 {
 	switch (dev) {
diff --git a/common/fdt.c b/common/fdt.c
new file mode 100644
index 0000000..c554b25
--- /dev/null
+++ b/common/fdt.c
@@ -0,0 +1,24 @@
+#include <libfdt.h>
+
+struct cell_info_t get_cellinfo(void *fdt, int offset)
+{
+	return (struct cell_info_t){
+		fdt_size_cells(fdt, offset),
+		fdt_address_cells(fdt, offset)
+	};
+}
+
+/* how "reg" is interpreted depends on the parent node */
+struct cell_info_t get_reginfo(void *fdt, const char *path)
+{
+	const char *i = strrchr(path, '/');
+	if(!i)
+		return (struct cell_info_t){0, 0};
+
+	size_t baselen = i - path;
+	if(i == 0)
+		/* root node */
+		baselen = 1;
+
+	return get_cellinfo(fdt, fdt_path_offset_namelen(fdt, path, baselen));
+}