imxtools: rework sb file production

The old code had some annoying way of dealing with padding by adding explicit
instructions to the stream, which is 1) ugly 2) not in par with freescale
tools. The trick, which this new version implements, is to put the useful length
of the section in the section header, and the actual (with padding) length in
the boot tag. This way the tools can just ignore padding instruction by
reading the section header, and the bootloader can still load the image because
it uses the boot tags.
Also correctly handle the case where the first section does not start right
after the header (there is a bug in freescale tools for this case by the way).
There is an ambiguity in the way the padding instructions should be encrypted:
the bootloader should logically treat them as regular instruction of the section
stream, but it appears the freescale tools do not generate them as part of the
stream and instead encrypt them like boot tags, which is stupid because there
is no way the bootloader could decrypt them, and anyway we don't care because
the bootloader doesn't decrypt them at all.

Change-Id: Iabdc1d1f9f82d374779bf03efb75c2c3998f5b5d

2 files changed, 82 insertions, 73 deletions

diff --git a/utils/imxtools/sbtools/sb.c b/utils/imxtools/sbtools/sb.c
index 385c5e5ded..183434efad 100644
--- a/utils/imxtools/sbtools/sb.c
+++ b/utils/imxtools/sbtools/sb.c
@@ -27,6 +27,9 @@
 #include "crypto.h"
 #include "sb.h"
 
+#define ALIGN_DOWN(n, a)    (((n)/(a))*(a))
+#define ALIGN_UP(n, a)      ALIGN_DOWN((n)+((a)-1),a)
+
 static void fill_gaps(struct sb_file_t *sb)
 {
     for(int i = 0; i < sb->nr_sections; i++)
@@ -58,27 +61,33 @@ static void compute_sb_offsets(struct sb_file_t *sb, void *u, generic_printf_t c
     /* sections */
     for(int i = 0; i < sb->nr_sections; i++)
     {
-        /* each section has a preliminary TAG command */
-        sb->image_size += sizeof(struct sb_instruction_tag_t) / BLOCK_SIZE;
-        /* we might need to pad the section so compute next alignment */
-        uint32_t alignment = BLOCK_SIZE;
-        if((i + 1) < sb->nr_sections)
-            alignment = sb->sections[i + 1].alignment;
-        alignment /= BLOCK_SIZE; /* alignment in block sizes */
-
         struct sb_section_t *sec = &sb->sections[i];
+        /* we need to make sure section starts on the right alignment,
+         * and since each section starts with a boot tag, we need to ensure
+         * that the boot tag is at address X such that X+BLOCK_SIZE is a
+         * multiple of the alignment */
+        uint32_t alignment = sb->sections[i].alignment / BLOCK_SIZE;
+        sb->image_size = ALIGN_UP(sb->image_size + 1, alignment) - 1;
+        /* update padding of previous section */
+        if(i > 0)
+            sb->sections[i - 1].pad_size = sb->image_size -
+                sb->sections[i - 1].file_offset - sb->sections[i - 1].sec_size;
+        /* each section has a preliminary TAG command */
+        sb->image_size += 1;
+        sec->file_offset = sb->image_size;
+        /* compute section size */
         sec->sec_size = 0;
+        sec->pad_size = 0;
 
         char name[5];
         sb_fill_section_name(name, sec->identifier);
         printf(BLUE, "%s", sec->is_data ? "Data" : "Boot");
-        printf(GREEN, " Section");
+        printf(GREEN, " Section ");
         printf(YELLOW, "'%s'", name);
         if(sec->is_cleartext)
             printf(RED, " (cleartext)");
         printf(OFF, "\n");
 
-        sec->file_offset = sb->image_size;
         for(int j = 0; j < sec->nr_insts; j++)
         {
             struct sb_inst_t *inst = &sec->insts[j];
@@ -87,7 +96,6 @@ static void compute_sb_offsets(struct sb_file_t *sb, void *u, generic_printf_t c
                 printf(RED, "  %s", inst->inst == SB_INST_CALL ? "CALL" : "JUMP");
                 printf(OFF, " | "); printf(BLUE, "addr=0x%08x", inst->addr);
                 printf(OFF, " | "); printf(GREEN, "arg=0x%08x\n", inst->argument);
-                sb->image_size += sizeof(struct sb_instruction_call_t) / BLOCK_SIZE;
                 sec->sec_size += sizeof(struct sb_instruction_call_t) / BLOCK_SIZE;
             }
             else if(inst->inst == SB_INST_FILL)
@@ -96,7 +104,6 @@ static void compute_sb_offsets(struct sb_file_t *sb, void *u, generic_printf_t c
                 printf(OFF, " | "); printf(BLUE, "addr=0x%08x", inst->addr);
                 printf(OFF, " | "); printf(GREEN, "len=0x%08x", inst->size);
                 printf(OFF, " | "); printf(YELLOW, "pattern=0x%08x\n", inst->pattern);
-                sb->image_size += sizeof(struct sb_instruction_fill_t) / BLOCK_SIZE;
                 sec->sec_size += sizeof(struct sb_instruction_fill_t) / BLOCK_SIZE;
             }
             else if(inst->inst == SB_INST_LOAD)
@@ -105,30 +112,25 @@ static void compute_sb_offsets(struct sb_file_t *sb, void *u, generic_printf_t c
                 printf(OFF, " | "); printf(BLUE, "addr=0x%08x", inst->addr);
                 printf(OFF, " | "); printf(GREEN, "len=0x%08x\n", inst->size);
                 /* load header */
-                sb->image_size += sizeof(struct sb_instruction_load_t) / BLOCK_SIZE;
                 sec->sec_size += sizeof(struct sb_instruction_load_t) / BLOCK_SIZE;
                 /* data + alignment */
-                sb->image_size += (inst->size + inst->padding_size) / BLOCK_SIZE;
                 sec->sec_size += (inst->size + inst->padding_size) / BLOCK_SIZE;
             }
             else if(inst->inst == SB_INST_MODE)
             {
                 printf(RED, "  MODE");
                 printf(OFF, " | "); printf(BLUE, "mod=0x%08x\n", inst->addr);
-                sb->image_size += sizeof(struct sb_instruction_mode_t) / BLOCK_SIZE;
                 sec->sec_size += sizeof(struct sb_instruction_mode_t) / BLOCK_SIZE;
             }
             else if(inst->inst == SB_INST_DATA)
             {
                 printf(RED, "  DATA");
                 printf(OFF, " | "); printf(BLUE, "size=0x%08x\n", inst->size);
-                sb->image_size += ROUND_UP(inst->size, BLOCK_SIZE) / BLOCK_SIZE;
                 sec->sec_size += ROUND_UP(inst->size, BLOCK_SIZE) / BLOCK_SIZE;
             }
             else if(inst->inst == SB_INST_NOP)
             {
                 printf(RED, "  NOOP\n");
-                sb->image_size += sizeof(struct sb_instruction_nop_t) / BLOCK_SIZE;
                 sec->sec_size += sizeof(struct sb_instruction_nop_t) / BLOCK_SIZE;
             }
             else
@@ -136,50 +138,7 @@ static void compute_sb_offsets(struct sb_file_t *sb, void *u, generic_printf_t c
                 cprintf(u, true, GREY, "die on inst %d\n", inst->inst);
             }
         }
-        /* we need to make sure next section starts on the right alignment.
-         * Since each section starts with a boot tag, we thus need to ensure
-         * that this sections ends at adress X such that X+BLOCK_SIZE is
-         * a multiple of the alignment.
-         * For data sections, we just add random data, otherwise we add nops */
-        uint32_t missing_sz = alignment - ((sb->image_size + 1) % alignment);
-        if(missing_sz != alignment)
-        {
-            struct sb_inst_t *aug_insts;
-            int nr_aug_insts = 0;
-
-            if(sb->sections[i].is_data)
-            {
-                nr_aug_insts = 1;
-                aug_insts = xmalloc(sizeof(struct sb_inst_t));
-                memset(aug_insts, 0, sizeof(struct sb_inst_t));
-                aug_insts[0].inst = SB_INST_DATA;
-                aug_insts[0].size = missing_sz * BLOCK_SIZE;
-                aug_insts[0].data = xmalloc(missing_sz * BLOCK_SIZE);
-                generate_random_data(aug_insts[0].data, missing_sz * BLOCK_SIZE);
-                printf(RED, "  DATA");
-                printf(OFF, " | "); printf(BLUE, "size=0x%08x\n", aug_insts[0].size);
-            }
-            else
-            {
-                nr_aug_insts = missing_sz;
-                aug_insts = xmalloc(sizeof(struct sb_inst_t) * nr_aug_insts);
-                memset(aug_insts, 0, sizeof(struct sb_inst_t) * nr_aug_insts);
-                for(int j = 0; j < nr_aug_insts; j++)
-                {
-                    aug_insts[j].inst = SB_INST_NOP;
-                    printf(RED, "  NOOP\n");
-                }
-            }
-
-            sb->sections[i].insts = augment_array(sb->sections[i].insts, sizeof(struct sb_inst_t),
-                sb->sections[i].nr_insts, aug_insts, nr_aug_insts);
-            sb->sections[i].nr_insts += nr_aug_insts;
-            free(aug_insts);
-
-            /* augment image and section size */
-            sb->image_size += missing_sz;
-            sec->sec_size += missing_sz;
-        }
+        sb->image_size += sec->sec_size;
     }
     /* final signature */
     sb->image_size += 2;
@@ -222,14 +181,13 @@ static void produce_sb_header(struct sb_file_t *sb, struct sb_header_t *sb_hdr)
     sb_hdr->flags = sb->flags;
     sb_hdr->image_size = sb->image_size;
     sb_hdr->header_size = sizeof(struct sb_header_t) / BLOCK_SIZE;
-    sb_hdr->first_boot_sec_id = sb->sections[0].identifier;
+    sb_hdr->first_boot_sec_id = sb->sections[sb->first_boot_sec].identifier;
     sb_hdr->nr_keys = g_nr_keys;
     sb_hdr->nr_sections = sb->nr_sections;
     sb_hdr->sec_hdr_size = sizeof(struct sb_section_header_t) / BLOCK_SIZE;
     sb_hdr->key_dict_off = sb_hdr->header_size +
         sb_hdr->sec_hdr_size * sb_hdr->nr_sections;
-    sb_hdr->first_boot_tag_off = sb_hdr->key_dict_off +
-        sizeof(struct sb_key_dictionary_entry_t) * sb_hdr->nr_keys / BLOCK_SIZE;
+    sb_hdr->first_boot_tag_off = sb->sections[sb->first_boot_sec].file_offset - 1;
     generate_random_data(sb_hdr->rand_pad0, sizeof(sb_hdr->rand_pad0));
     generate_random_data(sb_hdr->rand_pad1, sizeof(sb_hdr->rand_pad1));
     /* Version 1.0 has 6 bytes of random padding,
@@ -277,7 +235,11 @@ static void produce_section_tag_cmd(struct sb_section_t *sec,
     tag->hdr.opcode = SB_INST_TAG;
     tag->hdr.flags = is_last ? SB_INST_LAST_TAG : 0;
     tag->identifier = sec->identifier;
-    tag->len = sec->sec_size;
+    /* there is a catch here: in the section header at the beginning of the SB
+     * file, we put the *useful* length of the section (without padding) but
+     * the bootloader will not use those and only use the TAG commande which
+     * need to give the *actual* length (with padding) */
+    tag->len = sec->sec_size + sec->pad_size;
     tag->flags = (sec->is_data ? 0 : SECTION_BOOTABLE)
         | (sec->is_cleartext ? SECTION_CLEARTEXT : 0)
         | sec->other_flags;
@@ -330,15 +292,24 @@ enum sb_error_t sb_write_file(struct sb_file_t *sb, const char *filename, void *
     /* init CBC-MACs */
     for(int i = 0; i < g_nr_keys; i++)
         memset(cbc_macs[i], 0, 16);
-
+    /* fill gaps */
     fill_gaps(sb);
-    if(sb->nr_sections == 0 || sb->sections[0].is_data)
+    /* find first bootable section */
+    sb->first_boot_sec = -1;
+    for(int i = 0; i < sb->nr_sections; i++)
+        if(!sb->sections[i].is_data)
+        {
+            sb->first_boot_sec = i;
+            break;
+        }
+    if(sb->first_boot_sec == -1)
     {
-        cprintf(u, true, GREY, "First section of the image is not bootable, I cannot handle that.\n");
+        cprintf(u, true, GREY, "Image contains no bootable section, I cannot handle that.\n");
         return SB_ERROR;
     }
+    /* compute section offsets */
     compute_sb_offsets(sb, u, cprintf);
-
+    /* generate random real key */
     generate_random_data(real_key.u.key, 16);
 
     /* global SHA-1 */
@@ -406,6 +377,17 @@ enum sb_error_t sb_write_file(struct sb_file_t *sb, const char *filename, void *
             printf(YELLOW, "%02x", crypto_iv[j]);
         printf(OFF, "\n");
     }
+    /* the first section might not start right after the header, pad with
+     * random data */
+    unsigned init_gap = (sb->sections[0].file_offset - 1) * BLOCK_SIZE - (buf_p - buf);
+    if(init_gap > 0)
+    {
+        byte *data = xmalloc(init_gap);
+        generate_random_data(data, init_gap);
+        sha_1_update(&file_sha1, data, init_gap);
+        write(data, init_gap);
+        free(data);
+    }
     /* produce sections data */
     for(int i = 0; i< sb_hdr.nr_sections; i++)
     {
@@ -449,6 +431,31 @@ enum sb_error_t sb_write_file(struct sb_file_t *sb, const char *filename, void *
                 free(data);
             }
         }
+        /* pad section with random data or NOP */
+        uint32_t pad_size = sb->sections[i].pad_size;
+        if(sb->sections[i].is_data)
+        {
+            byte *data = xmalloc(pad_size * BLOCK_SIZE);
+            generate_random_data(data, pad_size * BLOCK_SIZE);
+            sha_1_update(&file_sha1, data, pad_size * BLOCK_SIZE);
+            write(data, pad_size * BLOCK_SIZE);
+            free(data);
+        }
+        else
+        {
+            for(unsigned j = 0; j < pad_size; j++)
+            {
+                struct sb_instruction_nop_t cmd;
+                memset(&cmd, 0, sizeof(cmd));
+                cmd.hdr.opcode = SB_INST_NOP;
+                cmd.hdr.checksum = instruction_checksum(&cmd.hdr);
+                if(g_nr_keys > 0 && !sb->sections[i].is_cleartext)
+                    crypto_cbc((byte *)&cmd, (byte *)&cmd, sizeof(cmd) / BLOCK_SIZE,
+                        &real_key, cur_cbc_mac, &cur_cbc_mac, 1);
+                sha_1_update(&file_sha1, (byte *)&cmd, sizeof(cmd));
+                write(&cmd, sizeof(cmd));
+            }
+        }
     }
     /* write file SHA-1 */
     byte final_sig[32];
@@ -458,11 +465,11 @@ enum sb_error_t sb_write_file(struct sb_file_t *sb, const char *filename, void *
     if(g_nr_keys > 0)
         crypto_cbc(final_sig, final_sig, 2, &real_key, crypto_iv, NULL, 1);
     write(final_sig, 32);
-
     if(buf_p - buf != sb_hdr.image_size * BLOCK_SIZE)
     {
-        if(g_debug)
-            printf(GREY, u, true, "SB image buffer was not entirely filled !\n");
+        printf(GREY, "[ERROR][INTERNAL] SB image buffer was not entirely filled !\n");
+        printf(GREY, "[ERROR][INTERNAL] expected %u blocks, got %u\n",
+            (buf_p - buf) / BLOCK_SIZE, sb_hdr.image_size);
         return SB_ERROR;
     }
 
diff --git a/utils/imxtools/sbtools/sb.h b/utils/imxtools/sbtools/sb.h
index cf826362de..9ab7fe7aba 100644
--- a/utils/imxtools/sbtools/sb.h
+++ b/utils/imxtools/sbtools/sb.h
@@ -195,7 +195,8 @@ struct sb_section_t
     struct sb_inst_t *insts;
     /* for production use */
     uint32_t file_offset; /* in blocks */
-    uint32_t sec_size; /* in blocks */
+    uint32_t sec_size; /* in blocks, without padding */
+    uint32_t pad_size; /* padding size after the section until next section */
 };
 
 struct sb_file_t
@@ -217,6 +218,7 @@ struct sb_file_t
     struct sb_version_t product_ver;
     struct sb_version_t component_ver;
     /* for production use */
+    int first_boot_sec; /* index in sections[] */
     uint32_t image_size; /* in blocks */
 };