diff --git a/main.c b/main.c index f2de48c..ac7cbf2 100644 --- a/main.c +++ b/main.c @@ -60,6 +60,48 @@ static int firmware_fdt_read_blob(const char *filename, char **buffp, int *len) return ret; } +static int firmware_fdt_read_chunk(const char *filename, char *buf, int len, int offset) +{ + int fd = 0; /* assume stdin */ + int ret = 0; + + fd = open(filename, O_RDONLY); + if (fd < 0) + return errno; + + struct stat st; + stat(filename, &st); + size_t file_size = st.st_size; + + if (file_size < offset || offset < 0) + { + printf("Bad offset: %d/%ld\n", offset, file_size); + return -1; + } + if (file_size < offset + len) + len = file_size - offset; + + if (offset) + lseek(fd, offset, SEEK_SET); + /* Loop until we have read everything */ + size_t readed = 0; + do { + ret = read(fd, buf + readed, len - readed); + if (ret < 0) + { + ret = errno; + break; + } + readed += ret; + } while (ret != 0 && readed < len); + + /* Clean up, including closing stdin; return errno on error */ + close(fd); + + memset(buf + readed, 0, len - readed); + return readed; +} + int a() { int len = 0; @@ -89,18 +131,19 @@ int a() return 0; } -static inline const void *fdt_offset_ptr_(const void *fdt, int offset) +static inline const void *fdt_offset_ptr_(const void *fdt, const void *buf, int buf_offset) { - return (const char *)fdt + fdt_off_dt_struct(fdt) + offset; + debug("fdt_offset_ptr_: %p + %d + %x\n", buf, fdt_off_dt_struct(fdt), buf_offset); + return (const char *)buf + fdt_off_dt_struct(fdt) + buf_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) +int guess_next_tag(char * fdt, int offset, char * buf) { - const unsigned * val = fdt_offset_ptr_(buf, offset); + const unsigned * val = fdt_offset_ptr_(fdt, buf, 0); unsigned tag = fdt32_to_cpu(*val); char * ptr = NULL; @@ -157,13 +200,113 @@ int is_offset_valid(char * fdt, int offset, int len) return 1; } +#define WINDOW_SIZE 512 + +static int fdt_dump_node_data(const char *outname, const char *filename, int offset) +{ + char *buf = malloc(WINDOW_SIZE); + int readed = firmware_fdt_read_chunk(filename, buf, WINDOW_SIZE, 0); + if (readed <= 0) + return -1; + char *fdt = buf; + + buf = malloc(WINDOW_SIZE); + readed = firmware_fdt_read_chunk(filename, buf, WINDOW_SIZE, offset); + if (readed <= 0) + return -1; + + char * strings = malloc(WINDOW_SIZE); + readed = firmware_fdt_read_chunk(filename, strings, WINDOW_SIZE, fdt_off_dt_strings(fdt)); + if (readed <= 0) + return -1; + + int data_offset = 0; + int data_len = 0; + do { + int nextoffset = guess_next_tag(fdt, offset, buf); + readed = firmware_fdt_read_chunk(filename, buf, WINDOW_SIZE, nextoffset); + debug("DUMP: Read additional %d bytes from offset %x\n", readed, nextoffset); + int valid = is_offset_valid(fdt, offset, nextoffset - offset); + if (!valid) + { + printf("DUMP: next tag is not valid! (%d %d)\n", offset, nextoffset); + break; + } + debug("DUMP: Offset %x is valid\n", nextoffset); + offset = nextoffset; + + if (FDT_PROP == fdt32_to_cpu(*(int *)(fdt_offset_ptr_(fdt, buf, 0)))) + { + const struct fdt_property *prop = fdt_offset_ptr_(fdt, buf, 0); + debug("prop->nameoff: %x (%x)\n", fdt32_to_cpu(prop->nameoff), fdt_off_dt_strings(fdt)); + char * name = strings + fdt32_to_cpu(prop->nameoff); + debug("Property name: '%s'\n", name); + + if (strcmp(name, "data")) + continue; + data_offset = prop->data - (char *)prop + offset; + data_len = fdt32_to_cpu(prop->len); + debug("Property \"data\" at offset %x len %d\n", data_offset, data_len); + break; + } + } while (FDT_END != fdt32_to_cpu(*(int *)(fdt_offset_ptr_(fdt, buf, 0)))); + + char * data = malloc(data_len); + readed = firmware_fdt_read_chunk(filename, data, data_len, + fdt_off_dt_struct(fdt) + data_offset); + if (readed <= 0) + return -1; +#if 1 + int fd = 0; /* assume stdin */ + int ret = 0; + + char out_filename[128] = ""; + snprintf(out_filename, 128, "/tmp/%s", outname); + *strchr(out_filename, '@') = '_'; + debug("Open file '%s'\n", out_filename); + fd = open(out_filename, O_WRONLY | O_CREAT, 0666); + if (fd < 0) + { + printf("Error: %d\n", errno); + return -errno; + } + + size_t writed = 0; + do { + ret = write(fd, data + writed, data_len - writed); + if (ret < 0) + { + ret = -errno; + break; + } + writed += ret; + } while (ret != 0 && writed < data_len); + + /* Clean up, including closing stdin; return errno on error */ + close(fd); + + if (ret < 0) + return ret; + + return writed; +#else + return 0; +#endif +} + int b() { int len = 0; - char *buf = NULL; const char *filename = "/tmp/esr1x.firmware"; - firmware_fdt_read_blob(filename, &buf, &len); + char *buf = malloc(WINDOW_SIZE); + int file_offset = 0; + int readed = firmware_fdt_read_chunk(filename, buf, WINDOW_SIZE, file_offset); + if (readed <= 0) + return -1; + char *fdt = buf; + buf = malloc(WINDOW_SIZE); + memcpy(buf, fdt, WINDOW_SIZE); printf("B:\n"); int offset = 0; @@ -173,33 +316,44 @@ int b() { "uboot-nand@1", 0 } }; do { - if (FDT_BEGIN_NODE == fdt32_to_cpu(*(int *)(fdt_offset_ptr_(buf, offset)))) + if (FDT_BEGIN_NODE == fdt32_to_cpu(*(int *)(fdt_offset_ptr_(fdt, buf, 0)))) { + debug("Search starts\n"); 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)))) + (char *)(fdt_offset_ptr_(fdt, buf, FDT_TAGSIZE)))) { node_offsets[i].offset = offset; } } + debug("Search ends\n"); } - int nextoffset = guess_next_tag(buf, offset); - int valid = is_offset_valid(buf, offset, nextoffset - offset); + int nextoffset = guess_next_tag(fdt, offset, buf); + readed = firmware_fdt_read_chunk(filename, buf, WINDOW_SIZE, nextoffset); + debug("Read additional %d bytes from offset %x\n", readed, nextoffset); + int valid = is_offset_valid(fdt, offset, nextoffset - offset); if (!valid) { printf("next tag is not valid! (%d %d)\n", offset, nextoffset); break; } + debug("Offset %x is valid\n", nextoffset); offset = nextoffset; - } while (FDT_END != fdt32_to_cpu(*(int *)(fdt_offset_ptr_(buf, offset)))); + } while (FDT_END != fdt32_to_cpu(*(int *)(fdt_offset_ptr_(fdt, buf, 0)))); 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); + readed = firmware_fdt_read_chunk(filename, buf, WINDOW_SIZE, + node_offsets[i].offset); + int writed = fdt_dump_node_data(node_offsets[i].name, filename, node_offsets[i].offset); + printf("Written %d bytes\n", writed); + } return 0; }