+static int add_flash_partition(
+ struct flash_partition_entry *part_list,
+ size_t max_entries,
+ const char *name,
+ unsigned long base,
+ unsigned long size)
+{
+ int ptr;
+ /* check if the list has a free entry */
+ for (ptr = 0; ptr < max_entries; ptr++, part_list++) {
+ if (part_list->name == NULL &&
+ part_list->base == 0 &&
+ part_list->size == 0)
+ break;
+ }
+
+ if (ptr == max_entries) {
+ error(1, 0, "No free flash part entry available.");
+ }
+
+ part_list->name = calloc(1, strlen(name) + 1);
+ memcpy((char *)part_list->name, name, strlen(name));
+ part_list->base = base;
+ part_list->size = size;
+
+ return 0;
+}
+
+/** read the partition table into struct flash_partition_entry */
+static int read_partition_table(
+ FILE *file, long offset,
+ struct flash_partition_entry *entries, size_t max_entries,
+ int type)
+{
+ char buf[2048];
+ char *ptr, *end;
+ const char *parthdr = NULL;
+ const char *fwuphdr = "fwup-ptn";
+ const char *flashhdr = "partition";
+
+ /* TODO: search for the partition table */
+
+ switch(type) {
+ case 0:
+ parthdr = fwuphdr;
+ break;
+ case 1:
+ parthdr = flashhdr;
+ break;
+ default:
+ error(1, 0, "Invalid partition table");
+ }
+
+ if (fseek(file, offset, SEEK_SET) < 0)
+ error(1, errno, "Can not seek in the firmware");
+
+ if (fread(buf, 1, 2048, file) < 0)
+ error(1, errno, "Can not read fwup-ptn from the firmware");
+
+ buf[2047] = '\0';
+
+ /* look for the partition header */
+ if (memcmp(buf, parthdr, strlen(parthdr)) != 0) {
+ fprintf(stderr, "DEBUG: can not find fwuphdr\n");
+ return 1;
+ }
+
+ ptr = buf;
+ end = buf + sizeof(buf);
+ while ((ptr + strlen(parthdr)) < end &&
+ memcmp(ptr, parthdr, strlen(parthdr)) == 0) {
+ char *end_part;
+ char *end_element;
+
+ char name[32] = { 0 };
+ int name_len = 0;
+ unsigned long base = 0;
+ unsigned long size = 0;
+
+ end_part = memchr(ptr, '\n', (end - ptr));
+ if (end_part == NULL) {
+ /* in theory this should never happen, because a partition always ends with 0x09, 0x0D, 0x0A */
+ break;
+ }
+
+ for (int i = 0; i <= 4; i++) {
+ if (end_part <= ptr)
+ break;
+
+ end_element = memchr(ptr, 0x20, (end_part - ptr));
+ if (end_element == NULL) {
+ error(1, errno, "Ignoring the rest of the partition entries.");
+ break;
+ }
+
+ switch (i) {
+ /* partition header */
+ case 0:
+ ptr = end_element + 1;
+ continue;
+ /* name */
+ case 1:
+ name_len = (end_element - ptr) > 31 ? 31 : (end_element - ptr);
+ strncpy(name, ptr, name_len);
+ name[name_len] = '\0';
+ ptr = end_element + 1;
+ continue;
+
+ /* string "base" */
+ case 2:
+ ptr = end_element + 1;
+ continue;
+
+ /* actual base */
+ case 3:
+ base = strtoul(ptr, NULL, 16);
+ ptr = end_element + 1;
+ continue;
+
+ /* string "size" */
+ case 4:
+ ptr = end_element + 1;
+ /* actual size. The last element doesn't have a sepeartor */
+ size = strtoul(ptr, NULL, 16);
+ /* the part ends with 0x09, 0x0d, 0x0a */
+ ptr = end_part + 1;
+ add_flash_partition(entries, max_entries, name, base, size);
+ continue;
+ }
+ }
+ }
+
+ return 0;
+}
+
+static void write_partition(
+ FILE *input_file,
+ size_t firmware_offset,
+ struct flash_partition_entry *entry,
+ FILE *output_file)
+{
+ char buf[4096];
+ size_t offset;
+
+ fseek(input_file, entry->base + firmware_offset, SEEK_SET);
+
+ for (offset = 0; sizeof(buf) + offset <= entry->size; offset += sizeof(buf)) {
+ if (fread(buf, sizeof(buf), 1, input_file) < 0)
+ error(1, errno, "Can not read partition from input_file");
+
+ if (fwrite(buf, sizeof(buf), 1, output_file) < 0)
+ error(1, errno, "Can not write partition to output_file");
+ }
+ /* write last chunk smaller than buffer */
+ if (offset < entry->size) {
+ offset = entry->size - offset;
+ if (fread(buf, offset, 1, input_file) < 0)
+ error(1, errno, "Can not read partition from input_file");
+ if (fwrite(buf, offset, 1, output_file) < 0)
+ error(1, errno, "Can not write partition to output_file");
+ }
+}
+
+static int extract_firmware_partition(FILE *input_file, size_t firmware_offset, struct flash_partition_entry *entry, const char *output_directory)
+{
+ FILE *output_file;
+ char output[PATH_MAX];
+
+ snprintf(output, PATH_MAX, "%s/%s", output_directory, entry->name);
+ output_file = fopen(output, "wb+");
+ if (output_file == NULL) {
+ error(1, errno, "Can not open output file %s", output);
+ }
+
+ write_partition(input_file, firmware_offset, entry, output_file);
+
+ fclose(output_file);
+
+ return 0;
+}
+
+/** extract all partitions from the firmware file */
+static int extract_firmware(const char *input, const char *output_directory)
+{
+ struct flash_partition_entry entries[16] = { 0 };
+ size_t max_entries = 16;
+ size_t firmware_offset = 0x1014;
+ FILE *input_file;
+
+ struct stat statbuf;
+
+ /* check input file */
+ if (stat(input, &statbuf)) {
+ error(1, errno, "Can not read input firmware %s", input);
+ }
+
+ /* check if output directory exists */
+ if (stat(output_directory, &statbuf)) {
+ error(1, errno, "Failed to stat output directory %s", output_directory);
+ }
+
+ if ((statbuf.st_mode & S_IFMT) != S_IFDIR) {
+ error(1, errno, "Given output directory is not a directory %s", output_directory);
+ }
+
+ input_file = fopen(input, "rb");
+
+ if (read_partition_table(input_file, firmware_offset, entries, 16, 0) != 0) {
+ error(1, 0, "Error can not read the partition table (fwup-ptn)");
+ }
+
+ for (int i = 0; i < max_entries; i++) {
+ if (entries[i].name == NULL &&
+ entries[i].base == 0 &&
+ entries[i].size == 0)
+ continue;
+
+ extract_firmware_partition(input_file, firmware_offset, &entries[i], output_directory);
+ }
+
+ return 0;
+}
+
+static struct flash_partition_entry *find_partition(
+ struct flash_partition_entry *entries, size_t max_entries,
+ const char *name, const char *error_msg)
+{
+ for (int i = 0; i < max_entries; i++, entries++) {
+ if (strcmp(entries->name, name) == 0)
+ return entries;
+ }
+
+ error(1, 0, "%s", error_msg);
+ return NULL;
+}
+
+static void write_ff(FILE *output_file, size_t size)
+{
+ char buf[4096];
+ int offset;
+
+ memset(buf, 0xff, sizeof(buf));
+
+ for (offset = 0; offset + sizeof(buf) < size ; offset += sizeof(buf)) {
+ if (fwrite(buf, sizeof(buf), 1, output_file) < 0)
+ error(1, errno, "Can not write 0xff to output_file");
+ }
+
+ /* write last chunk smaller than buffer */
+ if (offset < size) {
+ offset = size - offset;
+ if (fwrite(buf, offset, 1, output_file) < 0)
+ error(1, errno, "Can not write partition to output_file");
+ }
+}
+
+static void convert_firmware(const char *input, const char *output)
+{
+ struct flash_partition_entry fwup[MAX_PARTITIONS] = { 0 };
+ struct flash_partition_entry flash[MAX_PARTITIONS] = { 0 };
+ struct flash_partition_entry *fwup_os_image = NULL, *fwup_file_system = NULL;
+ struct flash_partition_entry *flash_os_image = NULL, *flash_file_system = NULL;
+ struct flash_partition_entry *fwup_partition_table = NULL;
+ size_t firmware_offset = 0x1014;
+ FILE *input_file, *output_file;
+
+ struct stat statbuf;
+
+ /* check input file */
+ if (stat(input, &statbuf)) {
+ error(1, errno, "Can not read input firmware %s", input);
+ }
+
+ input_file = fopen(input, "rb");
+ if (!input_file)
+ error(1, 0, "Can not open input firmware %s", input);
+
+ output_file = fopen(output, "wb");
+ if (!output_file)
+ error(1, 0, "Can not open output firmware %s", output);
+
+ if (read_partition_table(input_file, firmware_offset, fwup, MAX_PARTITIONS, 0) != 0) {
+ error(1, 0, "Error can not read the partition table (fwup-ptn)");
+ }
+
+ fwup_os_image = find_partition(fwup, MAX_PARTITIONS,
+ "os-image", "Error can not find os-image partition (fwup)");
+ fwup_file_system = find_partition(fwup, MAX_PARTITIONS,
+ "file-system", "Error can not find file-system partition (fwup)");
+ fwup_partition_table = find_partition(fwup, MAX_PARTITIONS,
+ "partition-table", "Error can not find partition-table partition");
+
+ /* the flash partition table has a 0x00000004 magic haeder */
+ if (read_partition_table(input_file, firmware_offset + fwup_partition_table->base + 4, flash, MAX_PARTITIONS, 1) != 0)
+ error(1, 0, "Error can not read the partition table (flash)");
+
+ flash_os_image = find_partition(flash, MAX_PARTITIONS,
+ "os-image", "Error can not find os-image partition (flash)");
+ flash_file_system = find_partition(flash, MAX_PARTITIONS,
+ "file-system", "Error can not find file-system partition (flash)");
+
+ /* write os_image to 0x0 */
+ write_partition(input_file, firmware_offset, fwup_os_image, output_file);
+ write_ff(output_file, flash_os_image->size - fwup_os_image->size);
+
+ /* write file-system behind os_image */
+ fseek(output_file, flash_file_system->base - flash_os_image->base, SEEK_SET);
+ write_partition(input_file, firmware_offset, fwup_file_system, output_file);
+ write_ff(output_file, flash_file_system->size - fwup_file_system->size);
+
+ fclose(output_file);
+ fclose(input_file);
+}
+