sbtoelf: implement sb1 loading and dumping

Implement actual loading of a sb1 file to a structure in full
generality. Also implement dumping for debug purpose

Change-Id: I320035ea628719480a79aaccb05dce9a83256927

6 files changed, 240 insertions, 30 deletions

diff --git a/utils/imxtools/sbtools/crypto.c b/utils/imxtools/sbtools/crypto.c
index d73547de25..d4afc6c816 100644
--- a/utils/imxtools/sbtools/crypto.c
+++ b/utils/imxtools/sbtools/crypto.c
@@ -28,7 +28,6 @@
 
 static enum crypto_method_t cur_method = CRYPTO_NONE;
 static byte key[16];
-static union xorcrypt_key_t xor_key[2];
 static uint16_t usb_vid, usb_pid;
 
 void crypto_setup(enum crypto_method_t method, void *param)
diff --git a/utils/imxtools/sbtools/misc.c b/utils/imxtools/sbtools/misc.c
index ec9b8c2a27..d88b2bc858 100644
--- a/utils/imxtools/sbtools/misc.c
+++ b/utils/imxtools/sbtools/misc.c
@@ -239,6 +239,12 @@ void print_key(struct crypto_key_t *key, bool newline)
         case CRYPTO_NONE:
             printf("none");
             break;
+        case CRYPTO_XOR_KEY:
+            print_hex(&key->u.xor_key[0].key[0], 64, false);
+            print_hex(&key->u.xor_key[1].key[0], 64, false);
+            break;
+        default:
+            printf("unknown");
     }
     if(newline)
         printf("\n");
diff --git a/utils/imxtools/sbtools/sb.c b/utils/imxtools/sbtools/sb.c
index 78f98b5985..2625d770c2 100644
--- a/utils/imxtools/sbtools/sb.c
+++ b/utils/imxtools/sbtools/sb.c
@@ -1138,7 +1138,7 @@ void sb_dump(struct sb_file_t *file, void *u, sb_color_printf cprintf)
     #define TEXT2   BLUE
     #define SEP     OFF
     
-    printf(HEADER, "SB File\n");
+    printf(BLUE, "SB File\n");
     printf(TREE, "+-");
     printf(HEADER, "Version: ");
     printf(TEXT, "1.%d\n", file->minor_version);
diff --git a/utils/imxtools/sbtools/sb1.c b/utils/imxtools/sbtools/sb1.c
index 19f53b0015..22854036c1 100644
--- a/utils/imxtools/sbtools/sb1.c
+++ b/utils/imxtools/sbtools/sb1.c
@@ -26,6 +26,25 @@
 #include "crypto.h"
 #include "sb1.h"
 
+static int sdram_size_table[] = {2, 8, 16, 32, 64};
+
+#define NR_SDRAM_ENTRIES    (int)(sizeof(sdram_size_table) / sizeof(sdram_size_table[0]))
+
+int sb1_sdram_size_by_index(int index)
+{
+    if(index < 0 || index >= NR_SDRAM_ENTRIES)
+        return -1;
+    return sdram_size_table[index];
+}
+
+int sb1_sdram_index_by_size(int size)
+{
+    for(int i = 0; i < NR_SDRAM_ENTRIES; i++)
+        if(sdram_size_table[i] == size)
+            return i;
+    return -1;
+}
+
 static uint16_t swap16(uint16_t t)
 {
     return (t << 8) | (t >> 8);
@@ -96,23 +115,34 @@ static const char *sb1_cmd_name(int cmd)
     }
 }
 
+static const char *sb1_datatype_name(int cmd)
+{
+    switch(cmd)
+    {
+        case SB1_DATATYPE_UINT32: return "uint32";
+        case SB1_DATATYPE_UINT16: return "uint16";
+        case SB1_DATATYPE_UINT8: return "uint8";
+        default: return "unknown";
+    }
+}
+
 struct sb1_file_t *sb1_read_memory(void *_buf, size_t filesize, void *u,
     sb1_color_printf cprintf, enum sb1_error_t *err)
 {
-    struct sb1_file_t *sb1_file = NULL;
+    struct sb1_file_t *file = NULL;
     uint8_t *buf = _buf;
 
     #define printf(c, ...) cprintf(u, false, c, __VA_ARGS__)
     #define fatal(e, ...) \
         do { if(err) *err = e; \
             cprintf(u, true, GREY, __VA_ARGS__); \
-            sb1_free(sb1_file); \
+            sb1_free(file); \
             return NULL; } while(0)
     #define print_hex(c, p, len, nl) \
         do { printf(c, ""); print_hex(p, len, nl); } while(0)
 
-    sb1_file = xmalloc(sizeof(struct sb1_file_t));
-    memset(sb1_file, 0, sizeof(struct sb1_file_t));
+    file = xmalloc(sizeof(struct sb1_file_t));
+    memset(file, 0, sizeof(struct sb1_file_t));
     struct sb1_header_t *header = (struct sb1_header_t *)buf;
 
     if(memcmp(header->signature, "STMP", 4) != 0)
@@ -136,9 +166,6 @@ struct sb1_file_t *sb1_read_memory(void *_buf, size_t filesize, void *u,
     struct sb1_version_t component_ver = header->component_ver;
     fix_version(&component_ver);
 
-    memcpy(&sb1_file->product_ver, &product_ver, sizeof(product_ver));
-    memcpy(&sb1_file->component_ver, &component_ver, sizeof(component_ver));
-
     printf(GREEN, "  Product version: ");
     printf(YELLOW, "%X.%X.%X\n", product_ver.major, product_ver.minor, product_ver.revision);
     printf(GREEN, "  Component version: ");
@@ -147,6 +174,14 @@ struct sb1_file_t *sb1_read_memory(void *_buf, size_t filesize, void *u,
     printf(GREEN, "  Drive tag: ");
     printf(YELLOW, "%x\n", header->drive_tag);
 
+    /* copy rom version, padding and drive tag */
+    /* copy versions */
+    memcpy(&file->product_ver, &product_ver, sizeof(product_ver));
+    memcpy(&file->component_ver, &component_ver, sizeof(component_ver));
+    file->rom_version = header->rom_version;
+    file->pad2 = header->pad2;
+    file->drive_tag = header->drive_tag;
+
     /* reduce size w.r.t to userdata part */
     uint32_t userdata_size = 0;
     if(header->userdata_offset != 0)
@@ -217,6 +252,7 @@ struct sb1_file_t *sb1_read_memory(void *_buf, size_t filesize, void *u,
         if(mark != *(uint32_t *)(ptr + size))
             fatal(SB1_CHECKSUM_ERROR, "Crypto mark mismatch\n");
         memmove(copy_ptr, ptr, size);
+
         ptr += size + 4;
         copy_ptr += size;
         offset = 0;
@@ -236,13 +272,54 @@ struct sb1_file_t *sb1_read_memory(void *_buf, size_t filesize, void *u,
         printf(YELLOW, "    Critical:");
         printf(RED, " %d\n", SB1_CMD_CRITICAL(cmd->cmd));
         printf(YELLOW, "    Data Type:");
-        printf(RED, " %#x\n", SB1_CMD_DATATYPE(cmd->cmd));
+        printf(RED, " %#x ", SB1_CMD_DATATYPE(cmd->cmd));
+        printf(GREEN, "(%s)\n", sb1_datatype_name(SB1_CMD_DATATYPE(cmd->cmd)));
         printf(YELLOW, "    Bytes:");
         printf(RED, " %#x\n", SB1_CMD_BYTES(cmd->cmd));
         printf(YELLOW, "    Boot:");
-        printf(RED, " %#x (%s)\n", SB1_CMD_BOOT(cmd->cmd), sb1_cmd_name(SB1_CMD_BOOT(cmd->cmd)));
+        printf(RED, " %#x ", SB1_CMD_BOOT(cmd->cmd));
+        printf(GREEN, "(%s)\n", sb1_cmd_name(SB1_CMD_BOOT(cmd->cmd)));
         printf(YELLOW, "    Addr:");
-        printf(RED, " %#x\n", cmd->addr);
+        printf(RED, " %#x", cmd->addr);
+
+        if(SB1_CMD_BOOT(cmd->cmd) == SB1_INST_SDRAM)
+            printf(GREEN, " (Chip Select=%d, Size=%d)", SB1_ADDR_SDRAM_CS(cmd->addr),
+                    sb1_sdram_size_by_index(SB1_ADDR_SDRAM_SZ(cmd->addr)));
+        printf(OFF, "\n");
+        if(SB1_CMD_BOOT(cmd->cmd) == SB1_INST_FILL)
+        {
+            printf(YELLOW, "    Pattern:");
+            printf(RED, " %#x\n", *(uint32_t *)(cmd + 1));
+        }
+
+        /* copy command */
+        struct sb1_inst_t inst;
+        memset(&inst, 0, sizeof(inst));
+        inst.cmd = SB1_CMD_BOOT(cmd->cmd);
+        inst.critical = SB1_CMD_CRITICAL(cmd->cmd);
+        inst.datatype = SB1_CMD_DATATYPE(cmd->cmd);
+        inst.size = SB1_CMD_BYTES(cmd->cmd);
+
+        switch(SB1_CMD_BOOT(cmd->cmd))
+        {
+            case SB1_INST_SDRAM:
+                inst.sdram.chip_select = SB1_ADDR_SDRAM_CS(cmd->addr);
+                inst.sdram.size_index = SB1_ADDR_SDRAM_SZ(cmd->addr);
+                break;
+            case SB1_INST_MODE:
+                inst.mode = cmd->addr;
+                break;
+            case SB1_INST_LOAD:
+                inst.data = malloc(inst.size);
+                memcpy(inst.data, cmd + 1, inst.size);
+                /* fallthrough */
+            default:
+                inst.addr = cmd->addr;
+                break;
+        }
+
+        file->insts = augment_array(file->insts, sizeof(inst), file->nr_insts, &inst, 1);
+        file->nr_insts++;
 
         /* last instruction ? */
         if(SB1_CMD_BOOT(cmd->cmd) == SB1_INST_JUMP ||
@@ -252,11 +329,15 @@ struct sb1_file_t *sb1_read_memory(void *_buf, size_t filesize, void *u,
         cmd = (void *)cmd + 4 + 4 * SB1_CMD_SIZE(cmd->cmd);
     }
 
-    sb1_file->data_size = header->image_size - header->header_size;
-    sb1_file->data = malloc(sb1_file->data_size);
-    memcpy(sb1_file->data, header + 1, sb1_file->data_size);
+    /* copy userdata */
+    file->userdata_size = userdata_size;
+    if(userdata_size > 0)
+    {
+        file->userdata = malloc(userdata_size);
+        memcpy(file->userdata, (void *)header + header->userdata_offset, userdata_size);
+    }
 
-    return sb1_file;
+    return file;
     #undef printf
     #undef fatal
     #undef print_hex
@@ -266,7 +347,10 @@ void sb1_free(struct sb1_file_t *file)
 {
     if(!file) return;
 
-    free(file->data);
+    for(int i = 0; i < file->nr_insts; i++)
+        free(file->insts[i].data);
+    free(file->insts);
+    free(file->userdata);
     free(file);
 }
 
@@ -280,8 +364,86 @@ void sb1_dump(struct sb1_file_t *file, void *u, sb1_color_printf cprintf)
     #define HEADER  GREEN
     #define TEXT    YELLOW
     #define TEXT2   BLUE
+    #define TEXT3   RED
     #define SEP     OFF
 
+    printf(BLUE, "SB1 File\n");
+    printf(TREE, "+-");
+    printf(HEADER, "Rom Ver: ");
+    printf(TEXT, "%x\n", file->rom_version);
+    printf(TREE, "+-");
+    printf(HEADER, "Pad: ");
+    printf(TEXT, "%x\n", file->pad2);
+    printf(TREE, "+-");
+    printf(HEADER, "Drive Tag: ");
+    printf(TEXT, "%x\n", file->drive_tag);
+    printf(TREE, "+-");
+    printf(HEADER, "Product Version: ");
+    printf(TEXT, "%X.%X.%X\n", file->product_ver.major, file->product_ver.minor,
+        file->product_ver.revision);
+    printf(TREE, "+-");
+    printf(HEADER, "Component Version: ");
+    printf(TEXT, "%X.%X.%X\n", file->component_ver.major, file->component_ver.minor,
+        file->component_ver.revision);
+
+    for(int j = 0; j < file->nr_insts; j++)
+    {
+        struct sb1_inst_t *inst = &file->insts[j];
+        printf(TREE, "+-");
+        printf(HEADER, "Command\n");
+        printf(TREE, "|  +-");
+        switch(inst->cmd)
+        {
+            case SB1_INST_CALL:
+            case SB1_INST_JUMP:
+                printf(HEADER, "%s", inst->cmd == SB1_INST_CALL ? "CALL" : "JUMP");
+                printf(SEP, " | ");
+                printf(TEXT3, "crit=%d", inst->critical);
+                printf(SEP, " | ");
+                printf(TEXT, "addr=0x%08x\n", inst->addr);
+                break;
+            case SB1_INST_LOAD:
+                printf(HEADER, "LOAD");
+                printf(SEP, " | ");
+                printf(TEXT3, "crit=%d", inst->critical);
+                printf(SEP, " | ");
+                printf(TEXT, "addr=0x%08x", inst->addr);
+                printf(SEP, " | ");
+                printf(TEXT2, "len=0x%08x\n", inst->size);
+                break;
+            case SB1_INST_FILL:
+                printf(HEADER, "FILL");
+                printf(SEP, " | ");
+                printf(TEXT3, "crit=%d", inst->critical);
+                printf(SEP, " | ");
+                printf(TEXT, "addr=0x%08x", inst->addr);
+                printf(SEP, " | ");
+                printf(TEXT2, "len=0x%08x", inst->size);
+                printf(SEP, " | ");
+                printf(TEXT2, "pattern=0x%08x\n", inst->pattern);
+                break;
+            case SB1_INST_MODE:
+                printf(HEADER, "MODE");
+                printf(SEP, " | ");
+                printf(TEXT3, "crit=%d", inst->critical);
+                printf(SEP, " | ");
+                printf(TEXT, "mode=0x%08x\n", inst->addr);
+                break;
+            case SB1_INST_SDRAM:
+                printf(HEADER, "SRAM");
+                printf(SEP, " | ");
+                printf(TEXT3, "crit=%d", inst->critical);
+                printf(SEP, " | ");
+                printf(TEXT, "chip_select=%d", inst->sdram.chip_select);
+                printf(SEP, " | ");
+                printf(TEXT2, "chip_size=%d\n", sb1_sdram_size_by_index(inst->sdram.size_index));
+                break;
+            default:
+                printf(GREY, "[Unknown instruction %x]\n", inst->cmd);
+                break;
+        }
+    }
+
     #undef printf
     #undef print_hex
 }
diff --git a/utils/imxtools/sbtools/sb1.h b/utils/imxtools/sbtools/sb1.h
index f0a7a4ebc8..8d8c2d2296 100644
--- a/utils/imxtools/sbtools/sb1.h
+++ b/utils/imxtools/sbtools/sb1.h
@@ -58,12 +58,20 @@ struct sb1_cmd_header_t
     uint32_t addr;
 } __attribute__((packed));
 
+#define SB1_CMD_MAX_SIZE        0x1ff8
+
 #define SB1_CMD_SIZE(cmd)       ((cmd) >> 21)
 #define SB1_CMD_CRITICAL(cmd)   !!(cmd & (1 << 20))
 #define SB1_CMD_BYTES(cmd)      (((cmd) >> 6) & 0x3fff)
 #define SB1_CMD_DATATYPE(cmd)   (((cmd) >> 4) & 0x3)
 #define SB1_CMD_BOOT(cmd)       ((cmd) & 0xf)
 
+#define SB1_ADDR_SDRAM_CS(addr) ((addr) & 0x3)
+#define SB1_ADDR_SDRAM_SZ(addr) ((addr) >> 16)
+
+int sb1_sdram_size_by_index(int index); // returns - 1 on error
+int sb1_sdram_index_by_size(int size); // returns -1 on error
+
 #define SB1_INST_LOAD   0x1
 #define SB1_INST_FILL   0x2
 #define SB1_INST_JUMP   0x3
@@ -71,12 +79,46 @@ struct sb1_cmd_header_t
 #define SB1_INST_MODE   0x5
 #define SB1_INST_SDRAM  0x6
 
+#define SB1_DATATYPE_UINT32 0
+#define SB1_DATATYPE_UINT16 1
+#define SB1_DATATYPE_UINT8  2
+
+/*******
+ * API *
+ *******/
+
+struct sb1_inst_t
+{
+    uint8_t cmd;
+    uint16_t size;
+    // <union>
+    struct
+    {
+        uint8_t chip_select;
+        uint8_t size_index;
+    }sdram;
+    uint8_t mode;
+    uint32_t addr;
+    // </union>
+    uint8_t datatype;
+    uint8_t critical;
+    // <union>
+    void *data;
+    uint32_t pattern;
+    // </union>
+};
+
 struct sb1_file_t
 {
+    uint32_t rom_version;
+    uint32_t pad2; // unknown meaning but copy it anyway !
+    uint32_t drive_tag;
     struct sb1_version_t product_ver;
     struct sb1_version_t component_ver;
-    void *data;
-    int data_size;
+    int nr_insts;
+    struct sb1_inst_t *insts;
+    void *userdata;
+    int userdata_size;
 };
 
 enum sb1_error_t
diff --git a/utils/imxtools/sbtools/sbtoelf.c b/utils/imxtools/sbtools/sbtoelf.c
index 0445c4a366..01a51cae90 100644
--- a/utils/imxtools/sbtools/sbtoelf.c
+++ b/utils/imxtools/sbtools/sbtoelf.c
@@ -162,7 +162,6 @@ static void extract_sb1_file(struct sb1_file_t *file)
     FILE *f = fopen(g_out_prefix, "wb");
     if(f == NULL)
         bugp("Cannot open %s for writing\n", g_out_prefix);
-    fwrite(file->data, file->data_size, 1, f);
     fclose(f);
 }
 
@@ -228,33 +227,35 @@ enum sb_version_guess_t
 
 enum sb_version_guess_t guess_sb_version(const char *filename)
 {
+#define ret(x) do { fclose(f); return x; } while(0)
     FILE *f = fopen(filename, "rb");
     if(f == NULL)
         bugp("Cannot open file for reading\n");
     // check signature
     uint8_t sig[4];
     if(fseek(f, 20, SEEK_SET))
-        return SB_VERSION_UNK;
+        ret(SB_VERSION_UNK);
     if(fread(sig, 4, 1, f) != 1)
-        return SB_VERSION_UNK;
+        ret(SB_VERSION_UNK);
     if(memcmp(sig, "STMP", 4) != 0)
-        return SB_VERSION_UNK;
+        ret(SB_VERSION_UNK);
     // check header size (v1)
     uint32_t hdr_size;
     if(fseek(f, 8, SEEK_SET))
-        return SB_VERSION_UNK;
+        ret(SB_VERSION_UNK);
     if(fread(&hdr_size, 4, 1, f) != 1)
-        return SB_VERSION_UNK;
+        ret(SB_VERSION_UNK);
     if(hdr_size == 0x34)
-        return SB_VERSION_1;
+        ret(SB_VERSION_1);
     // check header size (v2)
     if(fseek(f, 32, SEEK_SET))
-        return SB_VERSION_UNK;
+        ret(SB_VERSION_UNK);
     if(fread(&hdr_size, 4, 1, f) != 1)
-        return SB_VERSION_UNK;
+        ret(SB_VERSION_UNK);
     if(hdr_size == 0xc)
-        return SB_VERSION_2;
-    return SB_VERSION_UNK;
+        ret(SB_VERSION_2);
+    ret(SB_VERSION_UNK);
+#undef ret
 }
 
 int main(int argc, char **argv)