3 files changed, 119 insertions, 2 deletions

diff --git a/rbutil/mkimxboot/main.c b/rbutil/mkimxboot/main.c
index 68387c25c0..ed88d3e1ff 100644
--- a/rbutil/mkimxboot/main.c
+++ b/rbutil/mkimxboot/main.c
@@ -53,6 +53,7 @@ static void usage(void)
     printf("  -t <type>\tSet type (dualboot, singleboot, recovery)\n");
     printf("  -v <v>\tSet variant\n");
     printf("  -x\t\tDump device informations\n");
+    printf("  -w\tExtract the original firmware\n");
     printf("Supported variants: (default is standard)\n");
     printf("  ");
     for(size_t i = 0; i < NR_VARIANTS; i++)
@@ -74,6 +75,7 @@ int main(int argc, char *argv[])
     enum imx_firmware_variant_t variant = VARIANT_DEFAULT;
     enum imx_output_type_t type = IMX_DUALBOOT;
     bool debug = false;
+    bool extract_of = false;
 
     if(argc == 1)
         usage();
@@ -93,7 +95,7 @@ int main(int argc, char *argv[])
             {0, 0, 0, 0}
         };
 
-        int c = getopt_long(argc, argv, "?di:o:b:t:v:x", long_options, NULL);
+        int c = getopt_long(argc, argv, "?di:o:b:t:v:xw", long_options, NULL);
         if(c == -1)
             break;
         switch(c)
@@ -150,6 +152,9 @@ int main(int argc, char *argv[])
                 for(int i = 0; i < sizeof(imx_variants) / sizeof(imx_variants[0]); i++)
                     printf("  %s -> variant=%d\n", imx_variants[i].name, imx_variants[i].variant);
                 break;
+            case 'w':
+                extract_of = true;
+                break;
             default:
                 abort();
         }
@@ -165,7 +170,7 @@ int main(int argc, char *argv[])
         printf("You must specify an output file\n");
         return 1;
     }
-    if(!bootfile)
+    if(!bootfile && !extract_of)
     {
         printf("You must specify an boot file\n");
         return 1;
@@ -176,6 +181,13 @@ int main(int argc, char *argv[])
         return 1;
     }
 
+    if(extract_of)
+    {
+        enum imx_error_t err = extract_firmware(infile, variant, outfile);
+        printf("Result: %d\n", err);
+        return 0;
+    }
+
     struct imx_option_t opt;
     memset(&opt, 0, sizeof(opt));
     opt.debug = debug;
diff --git a/rbutil/mkimxboot/mkimxboot.c b/rbutil/mkimxboot/mkimxboot.c
index b79361dd63..9e43bbd61c 100644
--- a/rbutil/mkimxboot/mkimxboot.c
+++ b/rbutil/mkimxboot/mkimxboot.c
@@ -495,3 +495,106 @@ enum imx_error_t mkimxboot(const char *infile, const char *bootfile,
     sb_free(sb_file);
     return ret;
 }
+
+enum imx_error_t extract_firmware(const char *infile,
+    enum imx_firmware_variant_t fw_variant, const char *outfile)
+{
+    /* Dump tables */
+    if(fw_variant > VARIANT_COUNT) {
+        return IMX_ERROR;
+    }
+    dump_imx_dev_info("[INFO] ");
+    /* compute MD5 sum of the file */
+    uint8_t file_md5sum[16];
+    FILE *f = fopen(infile, "rb");
+    if(f == NULL)
+    {
+        printf("[ERR] Cannot open input file\n");
+        return IMX_OPEN_ERROR;
+    }
+    fseek(f, 0, SEEK_END);
+    size_t sz = ftell(f);
+    fseek(f, 0, SEEK_SET);
+    void *buf = xmalloc(sz);
+    if(fread(buf, sz, 1, f) != 1)
+    {
+        fclose(f);
+        free(buf);
+        printf("[ERR] Cannot read file\n");
+        return IMX_READ_ERROR;
+    }
+    md5_context ctx;
+    md5_starts(&ctx);
+    md5_update(&ctx, buf, sz);
+    md5_finish(&ctx, file_md5sum);
+    fclose(f);
+    
+    printf("[INFO] MD5 sum of the file: ");
+    print_hex(file_md5sum, 16, true);
+    /* find model */
+    enum imx_model_t model;
+    int md5_idx;
+    do
+    {
+        int i = 0;
+        while(i < NR_IMX_SUMS)
+        {
+            uint8_t md5[20];
+            if(strlen(imx_sums[i].md5sum) != 32)
+            {
+                printf("[INFO] Invalid MD5 sum in imx_sums\n");
+                return IMX_ERROR;
+            }
+            for(int j = 0; j < 16; j++)
+            {
+                byte a, b;
+                if(convxdigit(imx_sums[i].md5sum[2 * j], &a) || convxdigit(imx_sums[i].md5sum[2 * j + 1], &b))
+                {
+                    printf("[ERR][INTERNAL] Bad checksum format: %s\n", imx_sums[i].md5sum);
+                    free(buf);
+                    return IMX_ERROR;
+                }
+                md5[j] = (a << 4) | b;
+            }
+            if(memcmp(file_md5sum, md5, 16) == 0)
+                break;
+            i++;
+        }
+        if(i == NR_IMX_SUMS)
+        {
+            printf("[ERR] MD5 sum doesn't match any known file\n");
+            return IMX_NO_MATCH;
+        }
+        model = imx_sums[i].model;
+        md5_idx = i;
+    }while(0);
+    printf("[INFO] File is for model %d (%s, version %s)\n", model,
+        imx_models[model].model_name, imx_sums[md5_idx].version);
+
+    if(imx_sums[md5_idx].fw_variants[fw_variant].size == 0)
+    {
+        printf("[ERR] Input file does not contain variant '%s'\n", imx_fw_variant[fw_variant]);
+        free(buf);
+        return IMX_VARIANT_MISMATCH;
+    }
+
+    f = fopen(outfile, "wb");
+    if(f == NULL)
+    {
+        printf("[ERR] Cannot open input file\n");
+        free(buf);
+        return IMX_OPEN_ERROR;
+    }
+    enum imx_error_t ret = IMX_SUCCESS;
+
+    if(fwrite(buf + imx_sums[md5_idx].fw_variants[fw_variant].offset,
+            imx_sums[md5_idx].fw_variants[fw_variant].size, 1, f) != 1)
+    {
+        printf("[ERR] Cannot write file\n");
+        ret = IMX_ERROR;
+    }
+    fclose(f);
+    free(buf);
+
+    return ret;
+}
diff --git a/rbutil/mkimxboot/mkimxboot.h b/rbutil/mkimxboot/mkimxboot.h
index 5be87b31e0..1c3711023c 100644
--- a/rbutil/mkimxboot/mkimxboot.h
+++ b/rbutil/mkimxboot/mkimxboot.h
@@ -85,6 +85,8 @@ struct imx_option_t
 void dump_imx_dev_info(const char *prefix);
 enum imx_error_t mkimxboot(const char *infile, const char *bootfile,
     const char *outfile, struct imx_option_t opt);
+enum imx_error_t extract_firmware(const char *infile,
+    enum imx_firmware_variant_t fw_variant, const char *outfile);
 
 #ifdef __cplusplus
 }