#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;
}