diff --git a/main.c b/main.c index f31b7e6..26baadc 100644 --- a/main.c +++ b/main.c @@ -5,6 +5,7 @@ #include #include #include +#include #if 0 #define debug(...) printf(__VA_ARGS__) @@ -13,54 +14,7 @@ #endif static int reads_counter = 0; - -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; -} +static int fit_offset = 0; static int firmware_fdt_read_chunk(const char *filename, char *buf, int offset, int len) { @@ -71,6 +25,8 @@ static int firmware_fdt_read_chunk(const char *filename, char *buf, int offset, if (fd < 0) return errno; + offset += fit_offset; + struct stat st; stat(filename, &st); size_t file_size = st.st_size; @@ -100,29 +56,13 @@ static int firmware_fdt_read_chunk(const char *filename, char *buf, int offset, /* Clean up, including closing stdin; return errno on error */ close(fd); + debug("Read %d bytes from %x (%x)\n", readed, offset - fit_offset, offset); + memset(buf + readed, 0, len - readed); reads_counter++; return readed; } -int a() -{ - int len = 0; - char *buf = NULL; - const char *filename = "/tmp/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; - - return 0; -} - static inline const void *fdt_offset_ptr_(const void *fdt, const void *buf, int buf_offset) { debug("fdt_offset_ptr_: %p + %x\n", buf, buf_offset); @@ -201,7 +141,7 @@ int is_offset_valid(char * fdt, int offset, int len) return 1; } -#define WINDOW_SIZE (1024 * 100) +#define WINDOW_SIZE (1024 * 2) int buf_offset_left_after_tag(int offset) { @@ -327,13 +267,90 @@ static int fdt_dump_node_data(char * fdt, char * strings, const char *outname, c #endif } -int b() +#define IH_NMLEN 32 /* Image Name Length */ +#define IH_MAGIC 0x27051956 /* Image Magic Number */ + +/* + * all data in network byte order (aka natural aka bigendian) + */ + +typedef struct image_header { + uint32_t ih_magic; /* Image Header Magic Number */ + uint32_t ih_hcrc; /* Image Header CRC Checksum */ + uint32_t ih_time; /* Image Creation Timestamp */ + uint32_t ih_size; /* Image Data Size */ + uint32_t ih_load; /* Data Load Address */ + uint32_t ih_ep; /* Entry Point Address */ + uint32_t ih_dcrc; /* Image Data CRC Checksum */ + uint8_t ih_os; /* Operating System */ + uint8_t ih_arch; /* CPU architecture */ + uint8_t ih_type; /* Image Type */ + uint8_t ih_comp; /* Compression Type */ + uint8_t ih_name[IH_NMLEN]; /* Image Name */ +} image_header_t; + + +int b(int argc, const char * argv[]) { int len = 0; - const char *filename = "/tmp/firmware"; + if (argc < 2) + { + printf("Gimme path to FIT!\n"); + return -1; + } + const char * filename = argv[1]; + argc--; + + int fdt_start = 0; + char * milf = malloc(WINDOW_SIZE); + if (!milf) + { + printf("<%s> malloc failed", __func__); + return -1; + } + + int readed = firmware_fdt_read_chunk(filename, milf, 0, WINDOW_SIZE); + if (readed < 0) + { + free(milf); + return -1; + } + + fit_offset = 0; + if (IH_MAGIC != ntohl(*(uint32_t *)milf)) + goto skip_milf; + + fdt_start += sizeof(image_header_t); + char * subimage_len_ptr = milf + sizeof(image_header_t); + int subimage_len = 0; + int subimage_idx = 0; + do + { + subimage_len = ntohl(*(uint32_t *)subimage_len_ptr); + subimage_len_ptr += sizeof(uint32_t); + debug("milf%d: `%x`\n", subimage_idx, subimage_len); + fdt_start += sizeof(uint32_t); + if (subimage_idx++ < 3) + /* Images is aligned for 4 bytes, round up to get offset */ + fdt_start += (subimage_len + 3) & ~0b11; + } while (subimage_len); + + /* No FIT in firmware */ + if (3 > subimage_idx) + { + /* Clean up useless */ + printf("Not enough idxs\n"); + free(milf); + return -1; + } - char *fdt = malloc(WINDOW_SIZE); - int readed = firmware_fdt_read_chunk(filename, fdt, 0, WINDOW_SIZE); + fit_offset = fdt_start; + debug("Calculated fit_offset: %x\n", fit_offset); + + char *fdt; +skip_milf: + fdt = malloc(WINDOW_SIZE); + readed = firmware_fdt_read_chunk(filename, fdt, 0, WINDOW_SIZE); if (readed <= 0) return -1; @@ -345,11 +362,21 @@ int b() printf("B:\n"); int offset = 0; - struct { const char * name; int offset; } node_offsets[] = { - { "bdk@1", 0 }, - { "bl1@1", 0 }, - { "uboot@1", 0 } - }; + struct { const char * name; int offset; } node_offsets[10] = { 0 }; + + int node_idx = 0; + while (--argc) + { + node_offsets[node_idx].name = argv[2 + node_idx]; + debug("Remember argv[%d]: '%s'\n", 2 + node_idx, argv[2 + node_idx]); + node_idx++; + } + debug("Search these nodes:\n"); + for (int i = 0; node_offsets[i].name; i++) + { + debug("%s\n", node_offsets[i].name); + } + int buf_offset = 0; do { int offset_adjust = guess_next_tag(fdt, offset, buf, buf_offset); @@ -376,7 +403,7 @@ int b() if (FDT_BEGIN_NODE == fdt32_to_cpu(*(int *)(fdt_offset_ptr_(fdt, buf, buf_offset)))) { debug("Search starts\n"); - for (int i = 0; i < sizeof(node_offsets) / sizeof(*node_offsets); i++) + for (int i = 0; node_offsets[i].name; i++) { if (!strcmp(node_offsets[i].name, (char *)(fdt_offset_ptr_(fdt, buf, @@ -397,11 +424,12 @@ int b() if (readed <= 0) return -1; - for (int i = 0; i < sizeof(node_offsets) / sizeof(*node_offsets); i++) + for (int i = 0; node_offsets[i].name; i++) { printf("%s offset: %x\n", node_offsets[i].name, node_offsets[i].offset); int writed = fdt_dump_node_data(fdt, strings, node_offsets[i].name, filename, node_offsets[i].offset); - printf("Written %d bytes\n", writed); + if (writed) + printf("Written %d bytes\n", writed); } return 0; @@ -410,7 +438,7 @@ int b() int main(int argc, char *argv[]) { - b(); + b(argc, argv); printf("Counter: %d\n", reads_counter); return 0; }