init

main.c

@@ -0,0 +1,213 @@
+#include <stdio.h>
+#include <libfdt.h>
+#include <fdt.h>
+#include <sys/stat.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+#if 0
+#define debug(...) printf(__VA_ARGS__)
+#else
+#define debug(...)
+#endif
+
+static int firmware_fdt_read_blob(const char *filename, char **buffp, int *len)
+{
+	int fd = 0;	/* assume stdin */
+	char *buf = NULL;
+	int ret = 0;
+	*len = 0;
+
+	*buffp = NULL;
+	if (strcmp(filename, "-") != 0)
+	{
+		fd = open(filename, O_RDONLY);
+		if (fd < 0)
+			return errno;
+	}
+
+	struct stat st;
+	stat(filename, &st);
+	size_t bufsize = st.st_size;
+
+	/* Loop until we have read everything */
+	buf = malloc(bufsize);
+	if (!buf)
+	{
+		printf("Error\n");
+		ret = -1;
+		return ret;
+	}
+
+	off_t offset = 0;
+	do {
+		ret = read(fd, buf + offset, bufsize - offset);
+		if (ret < 0)
+		{
+			ret = errno;
+			break;
+		}
+		*len += ret;
+	} while (ret != 0);
+
+	/* Clean up, including closing stdin; return errno on error */
+	close(fd);
+	if (ret)
+		free(buf);
+	else
+		*buffp = buf;
+	return ret;
+}
+
+int a()
+{
+	int len = 0;
+	char *buf = NULL;
+	const char *filename = "/tmp/esr1x.firmware";
+
+	firmware_fdt_read_blob(filename, &buf, &len);
+
+	printf("A:\n");
+
+	int node;
+	node = fdt_path_offset(buf, "/images/sbi@1");
+	printf("fdt_path_offset of sbi@1: %x\n", node);
+	int d = 0;
+#if 0
+	node = fdt_next_node(buf, 0, &d);
+	node = fdt_next_node(buf, node, &d);
+	node = fdt_next_node(buf, node, &d);
+	node = fdt_next_node(buf, node, &d);
+	node = fdt_next_node(buf, node, &d);
+	node = fdt_next_node(buf, node, &d);
+	node = fdt_next_node(buf, node, &d);
+	node = fdt_next_node(buf, node, &d);
+	printf("node: %x\n", node);
+#endif
+
+	return 0;
+}
+
+static inline const void *fdt_offset_ptr_(const void *fdt, int offset)
+{
+	return (const char *)fdt + fdt_off_dt_struct(fdt) + offset;
+}
+
+#define FDT_ALIGN(x, a)		(((x) + (a) - 1) & ~((a) - 1))
+#define FDT_TAGALIGN(x)		(FDT_ALIGN((x), FDT_TAGSIZE))
+
+/* Do not touch any forward data, caller will validate it later */
+int guess_next_tag(char * buf, int offset)
+{
+	const unsigned * val = fdt_offset_ptr_(buf, offset);
+	unsigned tag = fdt32_to_cpu(*val);
+	char * ptr = NULL;
+
+	switch (tag) {
+		case FDT_BEGIN_NODE:
+			ptr = (char *)val;
+			ptr += FDT_TAGSIZE;
+			debug("Starts: '%s'\n", ptr);
+			while (*ptr != '\0')
+				ptr++;
+			offset += ptr - (char *)val + 1;
+			debug("skipped %d chars\n", ptr - (char *)val + 1);
+			break;
+		case FDT_PROP:
+			ptr = (char *)val;
+			ptr += FDT_TAGSIZE;
+			int len = fdt32_to_cpu(*((int *)ptr));
+			debug("It's prop: len %d\n", len);
+			offset += sizeof(struct fdt_property) + len;
+			break;
+		case FDT_END:
+		case FDT_END_NODE:
+		case FDT_NOP:
+			offset += FDT_TAGSIZE;
+			break;
+		default:
+			printf("Unknown label: %x\n", tag);
+			return -1;
+	}
+
+	offset = FDT_TAGALIGN(offset);
+	debug("Next tag offset: %x\n", offset);
+	return offset;
+}
+
+int is_offset_valid(char * fdt, int offset, int len)
+{
+	unsigned int uoffset = offset;
+	unsigned int absoffset = offset + fdt_off_dt_struct(fdt);
+
+	if (offset < 0)
+		return 0;
+
+	if ((absoffset < uoffset)
+	    || ((absoffset + len) < absoffset)
+	    || (absoffset + len) > fdt_totalsize(fdt))
+		return 0;
+
+	if (fdt_version(fdt) >= 0x11)
+		if (((uoffset + len) < uoffset)
+		    || ((offset + len) > fdt_size_dt_struct(fdt)))
+			return 0;
+
+	return 1;
+}
+
+int b()
+{
+	int len = 0;
+	char *buf = NULL;
+	const char *filename = "/tmp/esr1x.firmware";
+
+	firmware_fdt_read_blob(filename, &buf, &len);
+
+	printf("B:\n");
+	int offset = 0;
+	struct { const char * name; int offset; } node_offsets[] = {
+		{ "sbi@1", 0 },
+		{ "uboot-spi@1", 0 },
+		{ "uboot-nand@1", 0 }
+	};
+	do {
+		if (FDT_BEGIN_NODE == fdt32_to_cpu(*(int *)(fdt_offset_ptr_(buf, offset))))
+		{
+			for (int i = 0; i < sizeof(node_offsets) / sizeof(*node_offsets); i++)
+			{
+				if (!strcmp(node_offsets[i].name,
+					    (char *)(fdt_offset_ptr_(buf, offset + FDT_TAGSIZE))))
+				{
+					node_offsets[i].offset = offset;
+				}
+
+			}
+		}
+
+		int nextoffset = guess_next_tag(buf, offset);
+		int valid = is_offset_valid(buf, offset, nextoffset - offset);
+		if (!valid)
+		{
+			printf("next tag is not valid! (%d %d)\n", offset, nextoffset);
+			break;
+		}
+		offset = nextoffset;
+	} while (FDT_END != fdt32_to_cpu(*(int *)(fdt_offset_ptr_(buf, offset))));
+
+	debug("Yay\n");
+
+	for (int i = 0; i < sizeof(node_offsets) / sizeof(*node_offsets); i++)
+		printf("%s offset: %x\n", node_offsets[i].name, node_offsets[i].offset);
+
+	return 0;
+}
+
+
+int main(int argc, char *argv[])
+{
+	a();
+	b();
+	return 0;
+}