tools/mxsimage.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * Freescale i.MX23/i.MX28 SB image generator
   4  *
   5  * Copyright (C) 2012-2013 Marek Vasut <marex@denx.de>
   6  */
   7
   8 #ifdef CONFIG_MXS
   9
  10 #include <errno.h>
  11 #include <fcntl.h>
  12 #include <stdio.h>
  13 #include <string.h>
  14 #include <unistd.h>
  15 #include <limits.h>
  16
  17 #include <openssl/evp.h>
  18
  19 #include "imagetool.h"
  20 #include "mxsimage.h"
  21 #include "pbl_crc32.h"
  22 #include <image.h>
  23
  24 /*
  25  * OpenSSL 1.1.0 and newer compatibility functions:
  26  * https://wiki.openssl.org/index.php/1.1_API_Changes
  27  */
  28 #if OPENSSL_VERSION_NUMBER < 0x10100000L || \
  29     (defined(LIBRESSL_VERSION_NUMBER) && LIBRESSL_VERSION_NUMBER < 0x2070000fL)
  30 static void *OPENSSL_zalloc(size_t num)
  31 {
  32         void *ret = OPENSSL_malloc(num);
  33
  34         if (ret != NULL)
  35                 memset(ret, 0, num);
  36         return ret;
  37 }
  38
  39 EVP_MD_CTX *EVP_MD_CTX_new(void)
  40 {
  41         return OPENSSL_zalloc(sizeof(EVP_MD_CTX));
  42 }
  43
  44 void EVP_MD_CTX_free(EVP_MD_CTX *ctx)
  45 {
  46         EVP_MD_CTX_cleanup(ctx);
  47         OPENSSL_free(ctx);
  48 }
  49
  50 int EVP_CIPHER_CTX_reset(EVP_CIPHER_CTX *ctx)
  51 {
  52         return EVP_CIPHER_CTX_cleanup(ctx);
  53 }
  54 #endif
  55
  56 /*
  57  * DCD block
  58  * |-Write to address command block
  59  * |  0xf00 == 0xf33d
  60  * |  0xba2 == 0xb33f
  61  * |-ORR address with mask command block
  62  * |  0xf00 |= 0x1337
  63  * |-Write to address command block
  64  * |  0xba2 == 0xd00d
  65  * :
  66  */
  67 #define SB_HAB_DCD_WRITE        0xccUL
  68 #define SB_HAB_DCD_CHECK        0xcfUL
  69 #define SB_HAB_DCD_NOOP         0xc0UL
  70 #define SB_HAB_DCD_MASK_BIT     (1 << 3)
  71 #define SB_HAB_DCD_SET_BIT      (1 << 4)
  72
  73 /* Addr.n = Value.n */
  74 #define SB_DCD_WRITE    \
  75         (SB_HAB_DCD_WRITE << 24)
  76 /* Addr.n &= ~Value.n */
  77 #define SB_DCD_ANDC     \
  78         ((SB_HAB_DCD_WRITE << 24) | SB_HAB_DCD_SET_BIT)
  79 /* Addr.n |= Value.n */
  80 #define SB_DCD_ORR      \
  81         ((SB_HAB_DCD_WRITE << 24) | SB_HAB_DCD_SET_BIT | SB_HAB_DCD_MASK_BIT)
  82 /* (Addr.n & Value.n) == 0 */
  83 #define SB_DCD_CHK_EQZ  \
  84         (SB_HAB_DCD_CHECK << 24)
  85 /* (Addr.n & Value.n) == Value.n */
  86 #define SB_DCD_CHK_EQ   \
  87         ((SB_HAB_DCD_CHECK << 24) | SB_HAB_DCD_SET_BIT)
  88 /* (Addr.n & Value.n) != Value.n */
  89 #define SB_DCD_CHK_NEQ  \
  90         ((SB_HAB_DCD_CHECK << 24) | SB_HAB_DCD_MASK_BIT)
  91 /* (Addr.n & Value.n) != 0 */
  92 #define SB_DCD_CHK_NEZ  \
  93         ((SB_HAB_DCD_CHECK << 24) | SB_HAB_DCD_SET_BIT | SB_HAB_DCD_MASK_BIT)
  94 /* NOP */
  95 #define SB_DCD_NOOP     \
  96         (SB_HAB_DCD_NOOP << 24)
  97
  98 struct sb_dcd_ctx {
  99         struct sb_dcd_ctx               *dcd;
 100
 101         uint32_t                        id;
 102
 103         /* The DCD block. */
 104         uint32_t                        *payload;
 105         /* Size of the whole DCD block. */
 106         uint32_t                        size;
 107
 108         /* Pointer to previous DCD command block. */
 109         uint32_t                        *prev_dcd_head;
 110 };
 111
 112 /*
 113  * IMAGE
 114  *   |-SECTION
 115  *   |    |-CMD
 116  *   |    |-CMD
 117  *   |    `-CMD
 118  *   |-SECTION
 119  *   |    |-CMD
 120  *   :    :
 121  */
 122 struct sb_cmd_list {
 123         char                            *cmd;
 124         size_t                          len;
 125         unsigned int                    lineno;
 126 };
 127
 128 struct sb_cmd_ctx {
 129         uint32_t                        size;
 130
 131         struct sb_cmd_ctx               *cmd;
 132
 133         uint8_t                         *data;
 134         uint32_t                        length;
 135
 136         struct sb_command               payload;
 137         struct sb_command               c_payload;
 138 };
 139
 140 struct sb_section_ctx {
 141         uint32_t                        size;
 142
 143         /* Section flags */
 144         unsigned int                    boot:1;
 145
 146         struct sb_section_ctx           *sect;
 147
 148         struct sb_cmd_ctx               *cmd_head;
 149         struct sb_cmd_ctx               *cmd_tail;
 150
 151         struct sb_sections_header       payload;
 152 };
 153
 154 struct sb_image_ctx {
 155         unsigned int                    in_section:1;
 156         unsigned int                    in_dcd:1;
 157         /* Image configuration */
 158         unsigned int                    display_progress:1;
 159         unsigned int                    silent_dump:1;
 160         char                            *input_filename;
 161         char                            *output_filename;
 162         char                            *cfg_filename;
 163         uint8_t                         image_key[16];
 164
 165         /* Number of section in the image */
 166         unsigned int                    sect_count;
 167         /* Bootable section */
 168         unsigned int                    sect_boot;
 169         unsigned int                    sect_boot_found:1;
 170
 171         struct sb_section_ctx           *sect_head;
 172         struct sb_section_ctx           *sect_tail;
 173
 174         struct sb_dcd_ctx               *dcd_head;
 175         struct sb_dcd_ctx               *dcd_tail;
 176
 177         EVP_CIPHER_CTX                  *cipher_ctx;
 178         EVP_MD_CTX                      *md_ctx;
 179         uint8_t                         digest[32];
 180         struct sb_key_dictionary_key    sb_dict_key;
 181
 182         struct sb_boot_image_header     payload;
 183 };
 184
 185 /*
 186  * Instruction semantics:
 187  * NOOP
 188  * TAG [LAST]
 189  * LOAD       address file
 190  * LOAD  IVT  address IVT_entry_point
 191  * FILL address pattern length
 192  * JUMP [HAB] address [r0_arg]
 193  * CALL [HAB] address [r0_arg]
 194  * MODE mode
 195  *      For i.MX23, mode = USB/I2C/SPI1_FLASH/SPI2_FLASH/NAND_BCH
 196  *                         JTAG/SPI3_EEPROM/SD_SSP0/SD_SSP1
 197  *      For i.MX28, mode = USB/I2C/SPI2_FLASH/SPI3_FLASH/NAND_BCH
 198  *                         JTAG/SPI2_EEPROM/SD_SSP0/SD_SSP1
 199  */
 200
 201 /*
 202  * AES libcrypto
 203  */
 204 static int sb_aes_init(struct sb_image_ctx *ictx, uint8_t *iv, int enc)
 205 {
 206         EVP_CIPHER_CTX *ctx;
 207         int ret;
 208
 209         /* If there is no init vector, init vector is all zeroes. */
 210         if (!iv)
 211                 iv = ictx->image_key;
 212
 213         ctx = EVP_CIPHER_CTX_new();
 214         ret = EVP_CipherInit(ctx, EVP_aes_128_cbc(), ictx->image_key, iv, enc);
 215         if (ret == 1) {
 216                 EVP_CIPHER_CTX_set_padding(ctx, 0);
 217                 ictx->cipher_ctx = ctx;
 218         }
 219         return ret;
 220 }
 221
 222 static int sb_aes_crypt(struct sb_image_ctx *ictx, uint8_t *in_data,
 223                         uint8_t *out_data, int in_len)
 224 {
 225         EVP_CIPHER_CTX *ctx = ictx->cipher_ctx;
 226         int ret, outlen;
 227         uint8_t *outbuf;
 228
 229         outbuf = malloc(in_len);
 230         if (!outbuf)
 231                 return -ENOMEM;
 232         memset(outbuf, 0, sizeof(in_len));
 233
 234         ret = EVP_CipherUpdate(ctx, outbuf, &outlen, in_data, in_len);
 235         if (!ret) {
 236                 ret = -EINVAL;
 237                 goto err;
 238         }
 239
 240         if (out_data)
 241                 memcpy(out_data, outbuf, outlen);
 242
 243 err:
 244         free(outbuf);
 245         return ret;
 246 }
 247
 248 static int sb_aes_deinit(EVP_CIPHER_CTX *ctx)
 249 {
 250         return EVP_CIPHER_CTX_reset(ctx);
 251 }
 252
 253 static int sb_aes_reinit(struct sb_image_ctx *ictx, int enc)
 254 {
 255         int ret;
 256         EVP_CIPHER_CTX *ctx = ictx->cipher_ctx;
 257         struct sb_boot_image_header *sb_header = &ictx->payload;
 258         uint8_t *iv = sb_header->iv;
 259
 260         ret = sb_aes_deinit(ctx);
 261         if (!ret)
 262                 return ret;
 263         return sb_aes_init(ictx, iv, enc);
 264 }
 265
 266 /*
 267  * Debug
 268  */
 269 static void soprintf(struct sb_image_ctx *ictx, const char *fmt, ...)
 270 {
 271         va_list ap;
 272
 273         if (ictx->silent_dump)
 274                 return;
 275
 276         va_start(ap, fmt);
 277         vfprintf(stdout, fmt, ap);
 278         va_end(ap);
 279 }
 280
 281 /*
 282  * Code
 283  */
 284 static time_t sb_get_timestamp(void)
 285 {
 286         struct tm time_2000 = {
 287                 .tm_yday        = 1,    /* Jan. 1st */
 288                 .tm_year        = 100,  /* 2000 */
 289         };
 290         time_t seconds_to_2000 = mktime(&time_2000);
 291         time_t seconds_to_now = time(NULL);
 292
 293         return seconds_to_now - seconds_to_2000;
 294 }
 295
 296 static int sb_get_time(time_t time, struct tm *tm)
 297 {
 298         struct tm time_2000 = {
 299                 .tm_yday        = 1,    /* Jan. 1st */
 300                 .tm_year        = 0,    /* 1900 */
 301         };
 302         const time_t seconds_to_2000 = mktime(&time_2000);
 303         const time_t seconds_to_now = seconds_to_2000 + time;
 304         struct tm *ret;
 305         ret = gmtime_r(&seconds_to_now, tm);
 306         return ret ? 0 : -EINVAL;
 307 }
 308
 309 static void sb_encrypt_sb_header(struct sb_image_ctx *ictx)
 310 {
 311         EVP_MD_CTX *md_ctx = ictx->md_ctx;
 312         struct sb_boot_image_header *sb_header = &ictx->payload;
 313         uint8_t *sb_header_ptr = (uint8_t *)sb_header;
 314
 315         /* Encrypt the header, compute the digest. */
 316         sb_aes_crypt(ictx, sb_header_ptr, NULL, sizeof(*sb_header));
 317         EVP_DigestUpdate(md_ctx, sb_header_ptr, sizeof(*sb_header));
 318 }
 319
 320 static void sb_encrypt_sb_sections_header(struct sb_image_ctx *ictx)
 321 {
 322         EVP_MD_CTX *md_ctx = ictx->md_ctx;
 323         struct sb_section_ctx *sctx = ictx->sect_head;
 324         struct sb_sections_header *shdr;
 325         uint8_t *sb_sections_header_ptr;
 326         const int size = sizeof(*shdr);
 327
 328         while (sctx) {
 329                 shdr = &sctx->payload;
 330                 sb_sections_header_ptr = (uint8_t *)shdr;
 331
 332                 sb_aes_crypt(ictx, sb_sections_header_ptr,
 333                              ictx->sb_dict_key.cbc_mac, size);
 334                 EVP_DigestUpdate(md_ctx, sb_sections_header_ptr, size);
 335
 336                 sctx = sctx->sect;
 337         };
 338 }
 339
 340 static void sb_encrypt_key_dictionary_key(struct sb_image_ctx *ictx)
 341 {
 342         EVP_MD_CTX *md_ctx = ictx->md_ctx;
 343
 344         sb_aes_crypt(ictx, ictx->image_key, ictx->sb_dict_key.key,
 345                      sizeof(ictx->sb_dict_key.key));
 346         EVP_DigestUpdate(md_ctx, &ictx->sb_dict_key, sizeof(ictx->sb_dict_key));
 347 }
 348
 349 static void sb_decrypt_key_dictionary_key(struct sb_image_ctx *ictx)
 350 {
 351         EVP_MD_CTX *md_ctx = ictx->md_ctx;
 352
 353         EVP_DigestUpdate(md_ctx, &ictx->sb_dict_key, sizeof(ictx->sb_dict_key));
 354         sb_aes_crypt(ictx, ictx->sb_dict_key.key, ictx->image_key,
 355                      sizeof(ictx->sb_dict_key.key));
 356 }
 357
 358 static void sb_encrypt_tag(struct sb_image_ctx *ictx,
 359                 struct sb_cmd_ctx *cctx)
 360 {
 361         EVP_MD_CTX *md_ctx = ictx->md_ctx;
 362         struct sb_command *cmd = &cctx->payload;
 363
 364         sb_aes_crypt(ictx, (uint8_t *)cmd,
 365                      (uint8_t *)&cctx->c_payload, sizeof(*cmd));
 366         EVP_DigestUpdate(md_ctx, &cctx->c_payload, sizeof(*cmd));
 367 }
 368
 369 static int sb_encrypt_image(struct sb_image_ctx *ictx)
 370 {
 371         /* Start image-wide crypto. */
 372         ictx->md_ctx = EVP_MD_CTX_new();
 373         EVP_DigestInit(ictx->md_ctx, EVP_sha1());
 374
 375         /*
 376          * SB image header.
 377          */
 378         sb_aes_init(ictx, NULL, 1);
 379         sb_encrypt_sb_header(ictx);
 380
 381         /*
 382          * SB sections header.
 383          */
 384         sb_encrypt_sb_sections_header(ictx);
 385
 386         /*
 387          * Key dictionary.
 388          */
 389         sb_aes_reinit(ictx, 1);
 390         sb_encrypt_key_dictionary_key(ictx);
 391
 392         /*
 393          * Section tags.
 394          */
 395         struct sb_cmd_ctx *cctx;
 396         struct sb_command *ccmd;
 397         struct sb_section_ctx *sctx = ictx->sect_head;
 398
 399         while (sctx) {
 400                 cctx = sctx->cmd_head;
 401
 402                 sb_aes_reinit(ictx, 1);
 403
 404                 while (cctx) {
 405                         ccmd = &cctx->payload;
 406
 407                         sb_encrypt_tag(ictx, cctx);
 408
 409                         if (ccmd->header.tag == ROM_TAG_CMD) {
 410                                 sb_aes_reinit(ictx, 1);
 411                         } else if (ccmd->header.tag == ROM_LOAD_CMD) {
 412                                 sb_aes_crypt(ictx, cctx->data, cctx->data,
 413                                              cctx->length);
 414                                 EVP_DigestUpdate(ictx->md_ctx, cctx->data,
 415                                                  cctx->length);
 416                         }
 417
 418                         cctx = cctx->cmd;
 419                 }
 420
 421                 sctx = sctx->sect;
 422         };
 423
 424         /*
 425          * Dump the SHA1 of the whole image.
 426          */
 427         sb_aes_reinit(ictx, 1);
 428
 429         EVP_DigestFinal(ictx->md_ctx, ictx->digest, NULL);
 430         EVP_MD_CTX_free(ictx->md_ctx);
 431         sb_aes_crypt(ictx, ictx->digest, ictx->digest, sizeof(ictx->digest));
 432
 433         /* Stop the encryption session. */
 434         sb_aes_deinit(ictx->cipher_ctx);
 435
 436         return 0;
 437 }
 438
 439 static int sb_load_file(struct sb_cmd_ctx *cctx, char *filename)
 440 {
 441         long real_size, roundup_size;
 442         uint8_t *data;
 443         long ret;
 444         unsigned long size;
 445         FILE *fp;
 446
 447         if (!filename) {
 448                 fprintf(stderr, "ERR: Missing filename!\n");
 449                 return -EINVAL;
 450         }
 451
 452         fp = fopen(filename, "r");
 453         if (!fp)
 454                 goto err_open;
 455
 456         ret = fseek(fp, 0, SEEK_END);
 457         if (ret < 0)
 458                 goto err_file;
 459
 460         real_size = ftell(fp);
 461         if (real_size < 0)
 462                 goto err_file;
 463
 464         ret = fseek(fp, 0, SEEK_SET);
 465         if (ret < 0)
 466                 goto err_file;
 467
 468         roundup_size = roundup(real_size, SB_BLOCK_SIZE);
 469         data = calloc(1, roundup_size);
 470         if (!data)
 471                 goto err_file;
 472
 473         size = fread(data, 1, real_size, fp);
 474         if (size != (unsigned long)real_size)
 475                 goto err_alloc;
 476
 477         cctx->data = data;
 478         cctx->length = roundup_size;
 479
 480         fclose(fp);
 481         return 0;
 482
 483 err_alloc:
 484         free(data);
 485 err_file:
 486         fclose(fp);
 487 err_open:
 488         fprintf(stderr, "ERR: Failed to load file \"%s\"\n", filename);
 489         return -EINVAL;
 490 }
 491
 492 static uint8_t sb_command_checksum(struct sb_command *inst)
 493 {
 494         uint8_t *inst_ptr = (uint8_t *)inst;
 495         uint8_t csum = 0;
 496         unsigned int i;
 497
 498         for (i = 0; i < sizeof(struct sb_command); i++)
 499                 csum += inst_ptr[i];
 500
 501         return csum;
 502 }
 503
 504 static int sb_token_to_long(char *tok, uint32_t *rid)
 505 {
 506         char *endptr;
 507         unsigned long id;
 508
 509         if (tok[0] != '0' || tok[1] != 'x') {
 510                 fprintf(stderr, "ERR: Invalid hexadecimal number!\n");
 511                 return -EINVAL;
 512         }
 513
 514         tok += 2;
 515
 516         errno = 0;
 517         id = strtoul(tok, &endptr, 16);
 518         if ((errno == ERANGE && id == ULONG_MAX) || (errno != 0 && id == 0)) {
 519                 fprintf(stderr, "ERR: Value can't be decoded!\n");
 520                 return -EINVAL;
 521         }
 522
 523         /* Check for 32-bit overflow. */
 524         if (id > 0xffffffff) {
 525                 fprintf(stderr, "ERR: Value too big!\n");
 526                 return -EINVAL;
 527         }
 528
 529         if (endptr == tok) {
 530                 fprintf(stderr, "ERR: Deformed value!\n");
 531                 return -EINVAL;
 532         }
 533
 534         *rid = (uint32_t)id;
 535         return 0;
 536 }
 537
 538 static int sb_grow_dcd(struct sb_dcd_ctx *dctx, unsigned int inc_size)
 539 {
 540         uint32_t *tmp;
 541
 542         if (!inc_size)
 543                 return 0;
 544
 545         dctx->size += inc_size;
 546         tmp = realloc(dctx->payload, dctx->size);
 547         if (!tmp)
 548                 return -ENOMEM;
 549
 550         dctx->payload = tmp;
 551
 552         /* Assemble and update the HAB DCD header. */
 553         dctx->payload[0] = htonl((SB_HAB_DCD_TAG << 24) |
 554                                  (dctx->size << 8) |
 555                                  SB_HAB_VERSION);
 556
 557         return 0;
 558 }
 559
 560 static int sb_build_dcd(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd)
 561 {
 562         struct sb_dcd_ctx *dctx;
 563
 564         char *tok;
 565         uint32_t id;
 566         int ret;
 567
 568         dctx = calloc(1, sizeof(*dctx));
 569         if (!dctx)
 570                 return -ENOMEM;
 571
 572         ret = sb_grow_dcd(dctx, 4);
 573         if (ret)
 574                 goto err_dcd;
 575
 576         /* Read DCD block number. */
 577         tok = strtok(cmd->cmd, " ");
 578         if (!tok) {
 579                 fprintf(stderr, "#%i ERR: DCD block without number!\n",
 580                         cmd->lineno);
 581                 ret = -EINVAL;
 582                 goto err_dcd;
 583         }
 584
 585         /* Parse the DCD block number. */
 586         ret = sb_token_to_long(tok, &id);
 587         if (ret) {
 588                 fprintf(stderr, "#%i ERR: Malformed DCD block number!\n",
 589                         cmd->lineno);
 590                 goto err_dcd;
 591         }
 592
 593         dctx->id = id;
 594
 595         /*
 596          * The DCD block is now constructed. Append it to the list.
 597          * WARNING: The DCD size is still not computed and will be
 598          * updated while parsing it's commands.
 599          */
 600         if (!ictx->dcd_head) {
 601                 ictx->dcd_head = dctx;
 602                 ictx->dcd_tail = dctx;
 603         } else {
 604                 ictx->dcd_tail->dcd = dctx;
 605                 ictx->dcd_tail = dctx;
 606         }
 607
 608         return 0;
 609
 610 err_dcd:
 611         free(dctx->payload);
 612         free(dctx);
 613         return ret;
 614 }
 615
 616 static int sb_build_dcd_block(struct sb_image_ctx *ictx,
 617                               struct sb_cmd_list *cmd,
 618                               uint32_t type)
 619 {
 620         char *tok;
 621         uint32_t address, value, length;
 622         int ret;
 623
 624         struct sb_dcd_ctx *dctx = ictx->dcd_tail;
 625         uint32_t *dcd;
 626
 627         if (dctx->prev_dcd_head && (type != SB_DCD_NOOP) &&
 628             ((dctx->prev_dcd_head[0] & 0xff0000ff) == type)) {
 629                 /* Same instruction as before, just append it. */
 630                 ret = sb_grow_dcd(dctx, 8);
 631                 if (ret)
 632                         return ret;
 633         } else if (type == SB_DCD_NOOP) {
 634                 ret = sb_grow_dcd(dctx, 4);
 635                 if (ret)
 636                         return ret;
 637
 638                 /* Update DCD command block pointer. */
 639                 dctx->prev_dcd_head = dctx->payload +
 640                                 dctx->size / sizeof(*dctx->payload) - 1;
 641
 642                 /* NOOP has only 4 bytes and no payload. */
 643                 goto noop;
 644         } else {
 645                 /*
 646                  * Either a different instruction block started now
 647                  * or this is the first instruction block.
 648                  */
 649                 ret = sb_grow_dcd(dctx, 12);
 650                 if (ret)
 651                         return ret;
 652
 653                 /* Update DCD command block pointer. */
 654                 dctx->prev_dcd_head = dctx->payload +
 655                                 dctx->size / sizeof(*dctx->payload) - 3;
 656         }
 657
 658         dcd = dctx->payload + dctx->size / sizeof(*dctx->payload) - 2;
 659
 660         /*
 661          * Prepare the command.
 662          */
 663         tok = strtok(cmd->cmd, " ");
 664         if (!tok) {
 665                 fprintf(stderr, "#%i ERR: Missing DCD address!\n",
 666                         cmd->lineno);
 667                 ret = -EINVAL;
 668                 goto err;
 669         }
 670
 671         /* Read DCD destination address. */
 672         ret = sb_token_to_long(tok, &address);
 673         if (ret) {
 674                 fprintf(stderr, "#%i ERR: Incorrect DCD address!\n",
 675                         cmd->lineno);
 676                 goto err;
 677         }
 678
 679         tok = strtok(NULL, " ");
 680         if (!tok) {
 681                 fprintf(stderr, "#%i ERR: Missing DCD value!\n",
 682                         cmd->lineno);
 683                 ret = -EINVAL;
 684                 goto err;
 685         }
 686
 687         /* Read DCD operation value. */
 688         ret = sb_token_to_long(tok, &value);
 689         if (ret) {
 690                 fprintf(stderr, "#%i ERR: Incorrect DCD value!\n",
 691                         cmd->lineno);
 692                 goto err;
 693         }
 694
 695         /* Fill in the new DCD entry. */
 696         dcd[0] = htonl(address);
 697         dcd[1] = htonl(value);
 698
 699 noop:
 700         /* Update the DCD command block. */
 701         length = dctx->size -
 702                  ((dctx->prev_dcd_head - dctx->payload) *
 703                  sizeof(*dctx->payload));
 704         dctx->prev_dcd_head[0] = htonl(type | (length << 8));
 705
 706 err:
 707         return ret;
 708 }
 709
 710 static int sb_build_section(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd)
 711 {
 712         struct sb_section_ctx *sctx;
 713         struct sb_sections_header *shdr;
 714         char *tok;
 715         uint32_t bootable = 0;
 716         uint32_t id;
 717         int ret;
 718
 719         sctx = calloc(1, sizeof(*sctx));
 720         if (!sctx)
 721                 return -ENOMEM;
 722
 723         /* Read section number. */
 724         tok = strtok(cmd->cmd, " ");
 725         if (!tok) {
 726                 fprintf(stderr, "#%i ERR: Section without number!\n",
 727                         cmd->lineno);
 728                 ret = -EINVAL;
 729                 goto err_sect;
 730         }
 731
 732         /* Parse the section number. */
 733         ret = sb_token_to_long(tok, &id);
 734         if (ret) {
 735                 fprintf(stderr, "#%i ERR: Malformed section number!\n",
 736                         cmd->lineno);
 737                 goto err_sect;
 738         }
 739
 740         /* Read section's BOOTABLE flag. */
 741         tok = strtok(NULL, " ");
 742         if (tok && (strlen(tok) == 8) && !strncmp(tok, "BOOTABLE", 8))
 743                 bootable = SB_SECTION_FLAG_BOOTABLE;
 744
 745         sctx->boot = bootable;
 746
 747         shdr = &sctx->payload;
 748         shdr->section_number = id;
 749         shdr->section_flags = bootable;
 750
 751         /*
 752          * The section is now constructed. Append it to the list.
 753          * WARNING: The section size is still not computed and will
 754          * be updated while parsing it's commands.
 755          */
 756         ictx->sect_count++;
 757
 758         /* Mark that this section is bootable one. */
 759         if (bootable) {
 760                 if (ictx->sect_boot_found) {
 761                         fprintf(stderr,
 762                                 "#%i WARN: Multiple bootable section!\n",
 763                                 cmd->lineno);
 764                 } else {
 765                         ictx->sect_boot = id;
 766                         ictx->sect_boot_found = 1;
 767                 }
 768         }
 769
 770         if (!ictx->sect_head) {
 771                 ictx->sect_head = sctx;
 772                 ictx->sect_tail = sctx;
 773         } else {
 774                 ictx->sect_tail->sect = sctx;
 775                 ictx->sect_tail = sctx;
 776         }
 777
 778         return 0;
 779
 780 err_sect:
 781         free(sctx);
 782         return ret;
 783 }
 784
 785 static int sb_build_command_nop(struct sb_image_ctx *ictx)
 786 {
 787         struct sb_section_ctx *sctx = ictx->sect_tail;
 788         struct sb_cmd_ctx *cctx;
 789         struct sb_command *ccmd;
 790
 791         cctx = calloc(1, sizeof(*cctx));
 792         if (!cctx)
 793                 return -ENOMEM;
 794
 795         ccmd = &cctx->payload;
 796
 797         /*
 798          * Construct the command.
 799          */
 800         ccmd->header.checksum   = 0x5a;
 801         ccmd->header.tag        = ROM_NOP_CMD;
 802
 803         cctx->size = sizeof(*ccmd);
 804
 805         /*
 806          * Append the command to the last section.
 807          */
 808         if (!sctx->cmd_head) {
 809                 sctx->cmd_head = cctx;
 810                 sctx->cmd_tail = cctx;
 811         } else {
 812                 sctx->cmd_tail->cmd = cctx;
 813                 sctx->cmd_tail = cctx;
 814         }
 815
 816         return 0;
 817 }
 818
 819 static int sb_build_command_tag(struct sb_image_ctx *ictx,
 820                                 struct sb_cmd_list *cmd)
 821 {
 822         struct sb_section_ctx *sctx = ictx->sect_tail;
 823         struct sb_cmd_ctx *cctx;
 824         struct sb_command *ccmd;
 825         char *tok;
 826
 827         cctx = calloc(1, sizeof(*cctx));
 828         if (!cctx)
 829                 return -ENOMEM;
 830
 831         ccmd = &cctx->payload;
 832
 833         /*
 834          * Prepare the command.
 835          */
 836         /* Check for the LAST keyword. */
 837         tok = strtok(cmd->cmd, " ");
 838         if (tok && !strcmp(tok, "LAST"))
 839                 ccmd->header.flags = ROM_TAG_CMD_FLAG_ROM_LAST_TAG;
 840
 841         /*
 842          * Construct the command.
 843          */
 844         ccmd->header.checksum   = 0x5a;
 845         ccmd->header.tag        = ROM_TAG_CMD;
 846
 847         cctx->size = sizeof(*ccmd);
 848
 849         /*
 850          * Append the command to the last section.
 851          */
 852         if (!sctx->cmd_head) {
 853                 sctx->cmd_head = cctx;
 854                 sctx->cmd_tail = cctx;
 855         } else {
 856                 sctx->cmd_tail->cmd = cctx;
 857                 sctx->cmd_tail = cctx;
 858         }
 859
 860         return 0;
 861 }
 862
 863 static int sb_build_command_load(struct sb_image_ctx *ictx,
 864                                  struct sb_cmd_list *cmd)
 865 {
 866         struct sb_section_ctx *sctx = ictx->sect_tail;
 867         struct sb_cmd_ctx *cctx;
 868         struct sb_command *ccmd;
 869         char *tok;
 870         int ret, is_ivt = 0, is_dcd = 0;
 871         uint32_t dest, dcd = 0;
 872
 873         cctx = calloc(1, sizeof(*cctx));
 874         if (!cctx)
 875                 return -ENOMEM;
 876
 877         ccmd = &cctx->payload;
 878
 879         /*
 880          * Prepare the command.
 881          */
 882         tok = strtok(cmd->cmd, " ");
 883         if (!tok) {
 884                 fprintf(stderr, "#%i ERR: Missing LOAD address or 'IVT'!\n",
 885                         cmd->lineno);
 886                 ret = -EINVAL;
 887                 goto err;
 888         }
 889
 890         /* Check for "IVT" flag. */
 891         if (!strcmp(tok, "IVT"))
 892                 is_ivt = 1;
 893         if (!strcmp(tok, "DCD"))
 894                 is_dcd = 1;
 895         if (is_ivt || is_dcd) {
 896                 tok = strtok(NULL, " ");
 897                 if (!tok) {
 898                         fprintf(stderr, "#%i ERR: Missing LOAD address!\n",
 899                                 cmd->lineno);
 900                         ret = -EINVAL;
 901                         goto err;
 902                 }
 903         }
 904
 905         /* Read load destination address. */
 906         ret = sb_token_to_long(tok, &dest);
 907         if (ret) {
 908                 fprintf(stderr, "#%i ERR: Incorrect LOAD address!\n",
 909                         cmd->lineno);
 910                 goto err;
 911         }
 912
 913         /* Read filename or IVT entrypoint or DCD block ID. */
 914         tok = strtok(NULL, " ");
 915         if (!tok) {
 916                 fprintf(stderr,
 917                         "#%i ERR: Missing LOAD filename or IVT ep or DCD block ID!\n",
 918                         cmd->lineno);
 919                 ret = -EINVAL;
 920                 goto err;
 921         }
 922
 923         if (is_ivt) {
 924                 /* Handle IVT. */
 925                 struct sb_ivt_header *ivt;
 926                 uint32_t ivtep;
 927                 ret = sb_token_to_long(tok, &ivtep);
 928
 929                 if (ret) {
 930                         fprintf(stderr,
 931                                 "#%i ERR: Incorrect IVT entry point!\n",
 932                                 cmd->lineno);
 933                         goto err;
 934                 }
 935
 936                 ivt = calloc(1, sizeof(*ivt));
 937                 if (!ivt) {
 938                         ret = -ENOMEM;
 939                         goto err;
 940                 }
 941
 942                 ivt->header = sb_hab_ivt_header();
 943                 ivt->entry = ivtep;
 944                 ivt->self = dest;
 945
 946                 cctx->data = (uint8_t *)ivt;
 947                 cctx->length = sizeof(*ivt);
 948         } else if (is_dcd) {
 949                 struct sb_dcd_ctx *dctx = ictx->dcd_head;
 950                 uint32_t dcdid;
 951                 uint8_t *payload;
 952                 uint32_t asize;
 953                 ret = sb_token_to_long(tok, &dcdid);
 954
 955                 if (ret) {
 956                         fprintf(stderr,
 957                                 "#%i ERR: Incorrect DCD block ID!\n",
 958                                 cmd->lineno);
 959                         goto err;
 960                 }
 961
 962                 while (dctx) {
 963                         if (dctx->id == dcdid)
 964                                 break;
 965                         dctx = dctx->dcd;
 966                 }
 967
 968                 if (!dctx) {
 969                         fprintf(stderr, "#%i ERR: DCD block %08x not found!\n",
 970                                 cmd->lineno, dcdid);
 971                         goto err;
 972                 }
 973
 974                 asize = roundup(dctx->size, SB_BLOCK_SIZE);
 975                 payload = calloc(1, asize);
 976                 if (!payload) {
 977                         ret = -ENOMEM;
 978                         goto err;
 979                 }
 980
 981                 memcpy(payload, dctx->payload, dctx->size);
 982
 983                 cctx->data = payload;
 984                 cctx->length = asize;
 985
 986                 /* Set the Load DCD flag. */
 987                 dcd = ROM_LOAD_CMD_FLAG_DCD_LOAD;
 988         } else {
 989                 /* Regular LOAD of a file. */
 990                 ret = sb_load_file(cctx, tok);
 991                 if (ret) {
 992                         fprintf(stderr, "#%i ERR: Cannot load '%s'!\n",
 993                                 cmd->lineno, tok);
 994                         goto err;
 995                 }
 996         }
 997
 998         if (cctx->length & (SB_BLOCK_SIZE - 1)) {
 999                 fprintf(stderr, "#%i ERR: Unaligned payload!\n",
1000                         cmd->lineno);
1001         }
1002
1003         /*
1004          * Construct the command.
1005          */
1006         ccmd->header.checksum   = 0x5a;
1007         ccmd->header.tag        = ROM_LOAD_CMD;
1008         ccmd->header.flags      = dcd;
1009
1010         ccmd->load.address      = dest;
1011         ccmd->load.count        = cctx->length;
1012         ccmd->load.crc32        = pbl_crc32(0,
1013                                             (const char *)cctx->data,
1014                                             cctx->length);
1015
1016         cctx->size = sizeof(*ccmd) + cctx->length;
1017
1018         /*
1019          * Append the command to the last section.
1020          */
1021         if (!sctx->cmd_head) {
1022                 sctx->cmd_head = cctx;
1023                 sctx->cmd_tail = cctx;
1024         } else {
1025                 sctx->cmd_tail->cmd = cctx;
1026                 sctx->cmd_tail = cctx;
1027         }
1028
1029         return 0;
1030
1031 err:
1032         free(cctx);
1033         return ret;
1034 }
1035
1036 static int sb_build_command_fill(struct sb_image_ctx *ictx,
1037                                  struct sb_cmd_list *cmd)
1038 {
1039         struct sb_section_ctx *sctx = ictx->sect_tail;
1040         struct sb_cmd_ctx *cctx;
1041         struct sb_command *ccmd;
1042         char *tok;
1043         uint32_t address, pattern, length;
1044         int ret;
1045
1046         cctx = calloc(1, sizeof(*cctx));
1047         if (!cctx)
1048                 return -ENOMEM;
1049
1050         ccmd = &cctx->payload;
1051
1052         /*
1053          * Prepare the command.
1054          */
1055         tok = strtok(cmd->cmd, " ");
1056         if (!tok) {
1057                 fprintf(stderr, "#%i ERR: Missing FILL address!\n",
1058                         cmd->lineno);
1059                 ret = -EINVAL;
1060                 goto err;
1061         }
1062
1063         /* Read fill destination address. */
1064         ret = sb_token_to_long(tok, &address);
1065         if (ret) {
1066                 fprintf(stderr, "#%i ERR: Incorrect FILL address!\n",
1067                         cmd->lineno);
1068                 goto err;
1069         }
1070
1071         tok = strtok(NULL, " ");
1072         if (!tok) {
1073                 fprintf(stderr, "#%i ERR: Missing FILL pattern!\n",
1074                         cmd->lineno);
1075                 ret = -EINVAL;
1076                 goto err;
1077         }
1078
1079         /* Read fill pattern address. */
1080         ret = sb_token_to_long(tok, &pattern);
1081         if (ret) {
1082                 fprintf(stderr, "#%i ERR: Incorrect FILL pattern!\n",
1083                         cmd->lineno);
1084                 goto err;
1085         }
1086
1087         tok = strtok(NULL, " ");
1088         if (!tok) {
1089                 fprintf(stderr, "#%i ERR: Missing FILL length!\n",
1090                         cmd->lineno);
1091                 ret = -EINVAL;
1092                 goto err;
1093         }
1094
1095         /* Read fill pattern address. */
1096         ret = sb_token_to_long(tok, &length);
1097         if (ret) {
1098                 fprintf(stderr, "#%i ERR: Incorrect FILL length!\n",
1099                         cmd->lineno);
1100                 goto err;
1101         }
1102
1103         /*
1104          * Construct the command.
1105          */
1106         ccmd->header.checksum   = 0x5a;
1107         ccmd->header.tag        = ROM_FILL_CMD;
1108
1109         ccmd->fill.address      = address;
1110         ccmd->fill.count        = length;
1111         ccmd->fill.pattern      = pattern;
1112
1113         cctx->size = sizeof(*ccmd);
1114
1115         /*
1116          * Append the command to the last section.
1117          */
1118         if (!sctx->cmd_head) {
1119                 sctx->cmd_head = cctx;
1120                 sctx->cmd_tail = cctx;
1121         } else {
1122                 sctx->cmd_tail->cmd = cctx;
1123                 sctx->cmd_tail = cctx;
1124         }
1125
1126         return 0;
1127
1128 err:
1129         free(cctx);
1130         return ret;
1131 }
1132
1133 static int sb_build_command_jump_call(struct sb_image_ctx *ictx,
1134                                       struct sb_cmd_list *cmd,
1135                                       unsigned int is_call)
1136 {
1137         struct sb_section_ctx *sctx = ictx->sect_tail;
1138         struct sb_cmd_ctx *cctx;
1139         struct sb_command *ccmd;
1140         char *tok;
1141         uint32_t dest, arg = 0x0;
1142         uint32_t hab = 0;
1143         int ret;
1144         const char *cmdname = is_call ? "CALL" : "JUMP";
1145
1146         cctx = calloc(1, sizeof(*cctx));
1147         if (!cctx)
1148                 return -ENOMEM;
1149
1150         ccmd = &cctx->payload;
1151
1152         /*
1153          * Prepare the command.
1154          */
1155         tok = strtok(cmd->cmd, " ");
1156         if (!tok) {
1157                 fprintf(stderr,
1158                         "#%i ERR: Missing %s address or 'HAB'!\n",
1159                         cmd->lineno, cmdname);
1160                 ret = -EINVAL;
1161                 goto err;
1162         }
1163
1164         /* Check for "HAB" flag. */
1165         if (!strcmp(tok, "HAB")) {
1166                 hab = is_call ? ROM_CALL_CMD_FLAG_HAB : ROM_JUMP_CMD_FLAG_HAB;
1167                 tok = strtok(NULL, " ");
1168                 if (!tok) {
1169                         fprintf(stderr, "#%i ERR: Missing %s address!\n",
1170                                 cmd->lineno, cmdname);
1171                         ret = -EINVAL;
1172                         goto err;
1173                 }
1174         }
1175         /* Read load destination address. */
1176         ret = sb_token_to_long(tok, &dest);
1177         if (ret) {
1178                 fprintf(stderr, "#%i ERR: Incorrect %s address!\n",
1179                         cmd->lineno, cmdname);
1180                 goto err;
1181         }
1182
1183         tok = strtok(NULL, " ");
1184         if (tok) {
1185                 ret = sb_token_to_long(tok, &arg);
1186                 if (ret) {
1187                         fprintf(stderr,
1188                                 "#%i ERR: Incorrect %s argument!\n",
1189                                 cmd->lineno, cmdname);
1190                         goto err;
1191                 }
1192         }
1193
1194         /*
1195          * Construct the command.
1196          */
1197         ccmd->header.checksum   = 0x5a;
1198         ccmd->header.tag        = is_call ? ROM_CALL_CMD : ROM_JUMP_CMD;
1199         ccmd->header.flags      = hab;
1200
1201         ccmd->call.address      = dest;
1202         ccmd->call.argument     = arg;
1203
1204         cctx->size = sizeof(*ccmd);
1205
1206         /*
1207          * Append the command to the last section.
1208          */
1209         if (!sctx->cmd_head) {
1210                 sctx->cmd_head = cctx;
1211                 sctx->cmd_tail = cctx;
1212         } else {
1213                 sctx->cmd_tail->cmd = cctx;
1214                 sctx->cmd_tail = cctx;
1215         }
1216
1217         return 0;
1218
1219 err:
1220         free(cctx);
1221         return ret;
1222 }
1223
1224 static int sb_build_command_jump(struct sb_image_ctx *ictx,
1225                                  struct sb_cmd_list *cmd)
1226 {
1227         return sb_build_command_jump_call(ictx, cmd, 0);
1228 }
1229
1230 static int sb_build_command_call(struct sb_image_ctx *ictx,
1231                                  struct sb_cmd_list *cmd)
1232 {
1233         return sb_build_command_jump_call(ictx, cmd, 1);
1234 }
1235
1236 static int sb_build_command_mode(struct sb_image_ctx *ictx,
1237                                  struct sb_cmd_list *cmd)
1238 {
1239         struct sb_section_ctx *sctx = ictx->sect_tail;
1240         struct sb_cmd_ctx *cctx;
1241         struct sb_command *ccmd;
1242         char *tok;
1243         int ret;
1244         unsigned int i;
1245         uint32_t mode = 0xffffffff;
1246
1247         cctx = calloc(1, sizeof(*cctx));
1248         if (!cctx)
1249                 return -ENOMEM;
1250
1251         ccmd = &cctx->payload;
1252
1253         /*
1254          * Prepare the command.
1255          */
1256         tok = strtok(cmd->cmd, " ");
1257         if (!tok) {
1258                 fprintf(stderr, "#%i ERR: Missing MODE boot mode argument!\n",
1259                         cmd->lineno);
1260                 ret = -EINVAL;
1261                 goto err;
1262         }
1263
1264         for (i = 0; i < ARRAY_SIZE(modetable); i++) {
1265                 if (!strcmp(tok, modetable[i].name)) {
1266                         mode = modetable[i].mode;
1267                         break;
1268                 }
1269
1270                 if (!modetable[i].altname)
1271                         continue;
1272
1273                 if (!strcmp(tok, modetable[i].altname)) {
1274                         mode = modetable[i].mode;
1275                         break;
1276                 }
1277         }
1278
1279         if (mode == 0xffffffff) {
1280                 fprintf(stderr, "#%i ERR: Invalid MODE boot mode argument!\n",
1281                         cmd->lineno);
1282                 ret = -EINVAL;
1283                 goto err;
1284         }
1285
1286         /*
1287          * Construct the command.
1288          */
1289         ccmd->header.checksum   = 0x5a;
1290         ccmd->header.tag        = ROM_MODE_CMD;
1291
1292         ccmd->mode.mode         = mode;
1293
1294         cctx->size = sizeof(*ccmd);
1295
1296         /*
1297          * Append the command to the last section.
1298          */
1299         if (!sctx->cmd_head) {
1300                 sctx->cmd_head = cctx;
1301                 sctx->cmd_tail = cctx;
1302         } else {
1303                 sctx->cmd_tail->cmd = cctx;
1304                 sctx->cmd_tail = cctx;
1305         }
1306
1307         return 0;
1308
1309 err:
1310         free(cctx);
1311         return ret;
1312 }
1313
1314 static int sb_prefill_image_header(struct sb_image_ctx *ictx)
1315 {
1316         struct sb_boot_image_header *hdr = &ictx->payload;
1317
1318         /* Fill signatures */
1319         memcpy(hdr->signature1, "STMP", 4);
1320         memcpy(hdr->signature2, "sgtl", 4);
1321
1322         /* SB Image version 1.1 */
1323         hdr->major_version = SB_VERSION_MAJOR;
1324         hdr->minor_version = SB_VERSION_MINOR;
1325
1326         /* Boot image major version */
1327         hdr->product_version.major = htons(0x999);
1328         hdr->product_version.minor = htons(0x999);
1329         hdr->product_version.revision = htons(0x999);
1330         /* Boot image major version */
1331         hdr->component_version.major = htons(0x999);
1332         hdr->component_version.minor = htons(0x999);
1333         hdr->component_version.revision = htons(0x999);
1334
1335         /* Drive tag must be 0x0 for i.MX23 */
1336         hdr->drive_tag = 0;
1337
1338         hdr->header_blocks =
1339                 sizeof(struct sb_boot_image_header) / SB_BLOCK_SIZE;
1340         hdr->section_header_size =
1341                 sizeof(struct sb_sections_header) / SB_BLOCK_SIZE;
1342         hdr->timestamp_us = sb_get_timestamp() * 1000000;
1343
1344         hdr->flags = ictx->display_progress ?
1345                 SB_IMAGE_FLAG_DISPLAY_PROGRESS : 0;
1346
1347         /* FIXME -- We support only default key */
1348         hdr->key_count = 1;
1349
1350         return 0;
1351 }
1352
1353 static int sb_postfill_image_header(struct sb_image_ctx *ictx)
1354 {
1355         struct sb_boot_image_header *hdr = &ictx->payload;
1356         struct sb_section_ctx *sctx = ictx->sect_head;
1357         uint32_t kd_size, sections_blocks;
1358         EVP_MD_CTX *md_ctx;
1359
1360         /* The main SB header size in blocks. */
1361         hdr->image_blocks = hdr->header_blocks;
1362
1363         /* Size of the key dictionary, which has single zero entry. */
1364         kd_size = hdr->key_count * sizeof(struct sb_key_dictionary_key);
1365         hdr->image_blocks += kd_size / SB_BLOCK_SIZE;
1366
1367         /* Now count the payloads. */
1368         hdr->section_count = ictx->sect_count;
1369         while (sctx) {
1370                 hdr->image_blocks += sctx->size / SB_BLOCK_SIZE;
1371                 sctx = sctx->sect;
1372         }
1373
1374         if (!ictx->sect_boot_found) {
1375                 fprintf(stderr, "ERR: No bootable section selected!\n");
1376                 return -EINVAL;
1377         }
1378         hdr->first_boot_section_id = ictx->sect_boot;
1379
1380         /* The n * SB section size in blocks. */
1381         sections_blocks = hdr->section_count * hdr->section_header_size;
1382         hdr->image_blocks += sections_blocks;
1383
1384         /* Key dictionary offset. */
1385         hdr->key_dictionary_block = hdr->header_blocks + sections_blocks;
1386
1387         /* Digest of the whole image. */
1388         hdr->image_blocks += 2;
1389
1390         /* Pointer past the dictionary. */
1391         hdr->first_boot_tag_block =
1392                 hdr->key_dictionary_block + kd_size / SB_BLOCK_SIZE;
1393
1394         /* Compute header digest. */
1395         md_ctx = EVP_MD_CTX_new();
1396
1397         EVP_DigestInit(md_ctx, EVP_sha1());
1398         EVP_DigestUpdate(md_ctx, hdr->signature1,
1399                          sizeof(struct sb_boot_image_header) -
1400                          sizeof(hdr->digest));
1401         EVP_DigestFinal(md_ctx, hdr->digest, NULL);
1402         EVP_MD_CTX_free(md_ctx);
1403
1404         return 0;
1405 }
1406
1407 static int sb_fixup_sections_and_tags(struct sb_image_ctx *ictx)
1408 {
1409         /* Fixup the placement of sections. */
1410         struct sb_boot_image_header *ihdr = &ictx->payload;
1411         struct sb_section_ctx *sctx = ictx->sect_head;
1412         struct sb_sections_header *shdr;
1413         struct sb_cmd_ctx *cctx;
1414         struct sb_command *ccmd;
1415         uint32_t offset = ihdr->first_boot_tag_block;
1416
1417         while (sctx) {
1418                 shdr = &sctx->payload;
1419
1420                 /* Fill in the section TAG offset. */
1421                 shdr->section_offset = offset + 1;
1422                 offset += shdr->section_size;
1423
1424                 /* Section length is measured from the TAG block. */
1425                 shdr->section_size--;
1426
1427                 /* Fixup the TAG command. */
1428                 cctx = sctx->cmd_head;
1429                 while (cctx) {
1430                         ccmd = &cctx->payload;
1431                         if (ccmd->header.tag == ROM_TAG_CMD) {
1432                                 ccmd->tag.section_number = shdr->section_number;
1433                                 ccmd->tag.section_length = shdr->section_size;
1434                                 ccmd->tag.section_flags = shdr->section_flags;
1435                         }
1436
1437                         /* Update the command checksum. */
1438                         ccmd->header.checksum = sb_command_checksum(ccmd);
1439
1440                         cctx = cctx->cmd;
1441                 }
1442
1443                 sctx = sctx->sect;
1444         }
1445
1446         return 0;
1447 }
1448
1449 static int sb_parse_line(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd)
1450 {
1451         char *tok;
1452         char *line = cmd->cmd;
1453         char *rptr = NULL;
1454         int ret;
1455
1456         /* Analyze the identifier on this line first. */
1457         tok = strtok_r(line, " ", &rptr);
1458         if (!tok || (strlen(tok) == 0)) {
1459                 fprintf(stderr, "#%i ERR: Invalid line!\n", cmd->lineno);
1460                 return -EINVAL;
1461         }
1462
1463         cmd->cmd = rptr;
1464
1465         /* set DISPLAY_PROGRESS flag */
1466         if (!strcmp(tok, "DISPLAYPROGRESS")) {
1467                 ictx->display_progress = 1;
1468                 return 0;
1469         }
1470
1471         /* DCD */
1472         if (!strcmp(tok, "DCD")) {
1473                 ictx->in_section = 0;
1474                 ictx->in_dcd = 1;
1475                 sb_build_dcd(ictx, cmd);
1476                 return 0;
1477         }
1478
1479         /* Section */
1480         if (!strcmp(tok, "SECTION")) {
1481                 ictx->in_section = 1;
1482                 ictx->in_dcd = 0;
1483                 sb_build_section(ictx, cmd);
1484                 return 0;
1485         }
1486
1487         if (!ictx->in_section && !ictx->in_dcd) {
1488                 fprintf(stderr, "#%i ERR: Data outside of a section!\n",
1489                         cmd->lineno);
1490                 return -EINVAL;
1491         }
1492
1493         if (ictx->in_section) {
1494                 /* Section commands */
1495                 if (!strcmp(tok, "NOP")) {
1496                         ret = sb_build_command_nop(ictx);
1497                 } else if (!strcmp(tok, "TAG")) {
1498                         ret = sb_build_command_tag(ictx, cmd);
1499                 } else if (!strcmp(tok, "LOAD")) {
1500                         ret = sb_build_command_load(ictx, cmd);
1501                 } else if (!strcmp(tok, "FILL")) {
1502                         ret = sb_build_command_fill(ictx, cmd);
1503                 } else if (!strcmp(tok, "JUMP")) {
1504                         ret = sb_build_command_jump(ictx, cmd);
1505                 } else if (!strcmp(tok, "CALL")) {
1506                         ret = sb_build_command_call(ictx, cmd);
1507                 } else if (!strcmp(tok, "MODE")) {
1508                         ret = sb_build_command_mode(ictx, cmd);
1509                 } else {
1510                         fprintf(stderr,
1511                                 "#%i ERR: Unsupported instruction '%s'!\n",
1512                                 cmd->lineno, tok);
1513                         return -ENOTSUP;
1514                 }
1515         } else if (ictx->in_dcd) {
1516                 char *lptr;
1517                 uint32_t ilen = '1';
1518
1519                 tok = strtok_r(tok, ".", &lptr);
1520                 if (!tok || (strlen(tok) == 0) || (lptr && strlen(lptr) != 1)) {
1521                         fprintf(stderr, "#%i ERR: Invalid line!\n",
1522                                 cmd->lineno);
1523                         return -EINVAL;
1524                 }
1525
1526                 if (lptr &&
1527                     (lptr[0] != '1' && lptr[0] != '2' && lptr[0] != '4')) {
1528                         fprintf(stderr, "#%i ERR: Invalid instruction width!\n",
1529                                 cmd->lineno);
1530                         return -EINVAL;
1531                 }
1532
1533                 if (lptr)
1534                         ilen = lptr[0] - '1';
1535
1536                 /* DCD commands */
1537                 if (!strcmp(tok, "WRITE")) {
1538                         ret = sb_build_dcd_block(ictx, cmd,
1539                                                  SB_DCD_WRITE | ilen);
1540                 } else if (!strcmp(tok, "ANDC")) {
1541                         ret = sb_build_dcd_block(ictx, cmd,
1542                                                  SB_DCD_ANDC | ilen);
1543                 } else if (!strcmp(tok, "ORR")) {
1544                         ret = sb_build_dcd_block(ictx, cmd,
1545                                                  SB_DCD_ORR | ilen);
1546                 } else if (!strcmp(tok, "EQZ")) {
1547                         ret = sb_build_dcd_block(ictx, cmd,
1548                                                  SB_DCD_CHK_EQZ | ilen);
1549                 } else if (!strcmp(tok, "EQ")) {
1550                         ret = sb_build_dcd_block(ictx, cmd,
1551                                                  SB_DCD_CHK_EQ | ilen);
1552                 } else if (!strcmp(tok, "NEQ")) {
1553                         ret = sb_build_dcd_block(ictx, cmd,
1554                                                  SB_DCD_CHK_NEQ | ilen);
1555                 } else if (!strcmp(tok, "NEZ")) {
1556                         ret = sb_build_dcd_block(ictx, cmd,
1557                                                  SB_DCD_CHK_NEZ | ilen);
1558                 } else if (!strcmp(tok, "NOOP")) {
1559                         ret = sb_build_dcd_block(ictx, cmd, SB_DCD_NOOP);
1560                 } else {
1561                         fprintf(stderr,
1562                                 "#%i ERR: Unsupported instruction '%s'!\n",
1563                                 cmd->lineno, tok);
1564                         return -ENOTSUP;
1565                 }
1566         } else {
1567                 fprintf(stderr, "#%i ERR: Unsupported instruction '%s'!\n",
1568                         cmd->lineno, tok);
1569                 return -ENOTSUP;
1570         }
1571
1572         /*
1573          * Here we have at least one section with one command, otherwise we
1574          * would have failed already higher above.
1575          *
1576          * FIXME -- should the updating happen here ?
1577          */
1578         if (ictx->in_section && !ret) {
1579                 ictx->sect_tail->size += ictx->sect_tail->cmd_tail->size;
1580                 ictx->sect_tail->payload.section_size =
1581                         ictx->sect_tail->size / SB_BLOCK_SIZE;
1582         }
1583
1584         return ret;
1585 }
1586
1587 static int sb_load_cmdfile(struct sb_image_ctx *ictx)
1588 {
1589         struct sb_cmd_list cmd;
1590         int lineno = 1;
1591         FILE *fp;
1592         char *line = NULL;
1593         ssize_t rlen;
1594         size_t len;
1595
1596         fp = fopen(ictx->cfg_filename, "r");
1597         if (!fp)
1598                 goto err_file;
1599
1600         while ((rlen = getline(&line, &len, fp)) > 0) {
1601                 memset(&cmd, 0, sizeof(cmd));
1602
1603                 /* Strip the trailing newline. */
1604                 line[rlen - 1] = '\0';
1605
1606                 cmd.cmd = line;
1607                 cmd.len = rlen;
1608                 cmd.lineno = lineno++;
1609
1610                 sb_parse_line(ictx, &cmd);
1611         }
1612
1613         free(line);
1614
1615         fclose(fp);
1616
1617         return 0;
1618
1619 err_file:
1620         fclose(fp);
1621         fprintf(stderr, "ERR: Failed to load file \"%s\"\n",
1622                 ictx->cfg_filename);
1623         return -EINVAL;
1624 }
1625
1626 static int sb_build_tree_from_cfg(struct sb_image_ctx *ictx)
1627 {
1628         int ret;
1629
1630         ret = sb_load_cmdfile(ictx);
1631         if (ret)
1632                 return ret;
1633
1634         ret = sb_prefill_image_header(ictx);
1635         if (ret)
1636                 return ret;
1637
1638         ret = sb_postfill_image_header(ictx);
1639         if (ret)
1640                 return ret;
1641
1642         ret = sb_fixup_sections_and_tags(ictx);
1643         if (ret)
1644                 return ret;
1645
1646         return 0;
1647 }
1648
1649 static int sb_verify_image_header(struct sb_image_ctx *ictx,
1650                                   FILE *fp, long fsize)
1651 {
1652         /* Verify static fields in the image header. */
1653         struct sb_boot_image_header *hdr = &ictx->payload;
1654         const char *stat[2] = { "[PASS]", "[FAIL]" };
1655         struct tm tm;
1656         int sz, ret = 0;
1657         unsigned char digest[20];
1658         EVP_MD_CTX *md_ctx;
1659         unsigned long size;
1660
1661         /* Start image-wide crypto. */
1662         ictx->md_ctx = EVP_MD_CTX_new();
1663         EVP_DigestInit(ictx->md_ctx, EVP_sha1());
1664
1665         soprintf(ictx, "---------- Verifying SB Image Header ----------\n");
1666
1667         size = fread(&ictx->payload, 1, sizeof(ictx->payload), fp);
1668         if (size != sizeof(ictx->payload)) {
1669                 fprintf(stderr, "ERR: SB image header too short!\n");
1670                 return -EINVAL;
1671         }
1672
1673         /* Compute header digest. */
1674         md_ctx = EVP_MD_CTX_new();
1675         EVP_DigestInit(md_ctx, EVP_sha1());
1676         EVP_DigestUpdate(md_ctx, hdr->signature1,
1677                          sizeof(struct sb_boot_image_header) -
1678                          sizeof(hdr->digest));
1679         EVP_DigestFinal(md_ctx, digest, NULL);
1680         EVP_MD_CTX_free(md_ctx);
1681
1682         sb_aes_init(ictx, NULL, 1);
1683         sb_encrypt_sb_header(ictx);
1684
1685         if (memcmp(digest, hdr->digest, 20))
1686                 ret = -EINVAL;
1687         soprintf(ictx, "%s Image header checksum:        %s\n", stat[!!ret],
1688                  ret ? "BAD" : "OK");
1689         if (ret)
1690                 return ret;
1691
1692         if (memcmp(hdr->signature1, "STMP", 4) ||
1693             memcmp(hdr->signature2, "sgtl", 4))
1694                 ret = -EINVAL;
1695         soprintf(ictx, "%s Signatures:                   '%.4s' '%.4s'\n",
1696                  stat[!!ret], hdr->signature1, hdr->signature2);
1697         if (ret)
1698                 return ret;
1699
1700         if ((hdr->major_version != SB_VERSION_MAJOR) ||
1701             ((hdr->minor_version != 1) && (hdr->minor_version != 2)))
1702                 ret = -EINVAL;
1703         soprintf(ictx, "%s Image version:                v%i.%i\n", stat[!!ret],
1704                  hdr->major_version, hdr->minor_version);
1705         if (ret)
1706                 return ret;
1707
1708         ret = sb_get_time(hdr->timestamp_us / 1000000, &tm);
1709         soprintf(ictx,
1710                  "%s Creation time:                %02i:%02i:%02i %02i/%02i/%04i\n",
1711                  stat[!!ret], tm.tm_hour, tm.tm_min, tm.tm_sec,
1712                  tm.tm_mday, tm.tm_mon, tm.tm_year + 2000);
1713         if (ret)
1714                 return ret;
1715
1716         soprintf(ictx, "%s Product version:              %x.%x.%x\n", stat[0],
1717                  ntohs(hdr->product_version.major),
1718                  ntohs(hdr->product_version.minor),
1719                  ntohs(hdr->product_version.revision));
1720         soprintf(ictx, "%s Component version:            %x.%x.%x\n", stat[0],
1721                  ntohs(hdr->component_version.major),
1722                  ntohs(hdr->component_version.minor),
1723                  ntohs(hdr->component_version.revision));
1724
1725         if (hdr->flags & ~SB_IMAGE_FLAGS_MASK)
1726                 ret = -EINVAL;
1727         soprintf(ictx, "%s Image flags:                  %s\n", stat[!!ret],
1728                  hdr->flags & SB_IMAGE_FLAG_DISPLAY_PROGRESS ?
1729                  "Display_progress" : "");
1730         if (ret)
1731                 return ret;
1732
1733         if (hdr->drive_tag != 0)
1734                 ret = -EINVAL;
1735         soprintf(ictx, "%s Drive tag:                    %i\n", stat[!!ret],
1736                  hdr->drive_tag);
1737         if (ret)
1738                 return ret;
1739
1740         sz = sizeof(struct sb_boot_image_header) / SB_BLOCK_SIZE;
1741         if (hdr->header_blocks != sz)
1742                 ret = -EINVAL;
1743         soprintf(ictx, "%s Image header size (blocks):   %i\n", stat[!!ret],
1744                  hdr->header_blocks);
1745         if (ret)
1746                 return ret;
1747
1748         sz = sizeof(struct sb_sections_header) / SB_BLOCK_SIZE;
1749         if (hdr->section_header_size != sz)
1750                 ret = -EINVAL;
1751         soprintf(ictx, "%s Section header size (blocks): %i\n", stat[!!ret],
1752                  hdr->section_header_size);
1753         if (ret)
1754                 return ret;
1755
1756         soprintf(ictx, "%s Sections count:               %i\n", stat[!!ret],
1757                  hdr->section_count);
1758         soprintf(ictx, "%s First bootable section        %i\n", stat[!!ret],
1759                  hdr->first_boot_section_id);
1760
1761         if (hdr->image_blocks != fsize / SB_BLOCK_SIZE)
1762                 ret = -EINVAL;
1763         soprintf(ictx, "%s Image size (blocks):          %i\n", stat[!!ret],
1764                  hdr->image_blocks);
1765         if (ret)
1766                 return ret;
1767
1768         sz = hdr->header_blocks + hdr->section_header_size * hdr->section_count;
1769         if (hdr->key_dictionary_block != sz)
1770                 ret = -EINVAL;
1771         soprintf(ictx, "%s Key dict offset (blocks):     %i\n", stat[!!ret],
1772                  hdr->key_dictionary_block);
1773         if (ret)
1774                 return ret;
1775
1776         if (hdr->key_count != 1)
1777                 ret = -EINVAL;
1778         soprintf(ictx, "%s Number of encryption keys:    %i\n", stat[!!ret],
1779                  hdr->key_count);
1780         if (ret)
1781                 return ret;
1782
1783         sz = hdr->header_blocks + hdr->section_header_size * hdr->section_count;
1784         sz += hdr->key_count *
1785                 sizeof(struct sb_key_dictionary_key) / SB_BLOCK_SIZE;
1786         if (hdr->first_boot_tag_block != (unsigned)sz)
1787                 ret = -EINVAL;
1788         soprintf(ictx, "%s First TAG block (blocks):     %i\n", stat[!!ret],
1789                  hdr->first_boot_tag_block);
1790         if (ret)
1791                 return ret;
1792
1793         return 0;
1794 }
1795
1796 static void sb_decrypt_tag(struct sb_image_ctx *ictx,
1797                 struct sb_cmd_ctx *cctx)
1798 {
1799         EVP_MD_CTX *md_ctx = ictx->md_ctx;
1800         struct sb_command *cmd = &cctx->payload;
1801
1802         sb_aes_crypt(ictx, (uint8_t *)&cctx->c_payload,
1803                      (uint8_t *)&cctx->payload, sizeof(*cmd));
1804         EVP_DigestUpdate(md_ctx, &cctx->c_payload, sizeof(*cmd));
1805 }
1806
1807 static int sb_verify_command(struct sb_image_ctx *ictx,
1808                              struct sb_cmd_ctx *cctx, FILE *fp,
1809                              unsigned long *tsize)
1810 {
1811         struct sb_command *ccmd = &cctx->payload;
1812         unsigned long size, asize;
1813         char *csum, *flag = "";
1814         int ret;
1815         unsigned int i;
1816         uint8_t csn, csc = ccmd->header.checksum;
1817         ccmd->header.checksum = 0x5a;
1818         csn = sb_command_checksum(ccmd);
1819         ccmd->header.checksum = csc;
1820
1821         if (csc == csn)
1822                 ret = 0;
1823         else
1824                 ret = -EINVAL;
1825         csum = ret ? "checksum BAD" : "checksum OK";
1826
1827         switch (ccmd->header.tag) {
1828         case ROM_NOP_CMD:
1829                 soprintf(ictx, " NOOP # %s\n", csum);
1830                 return ret;
1831         case ROM_TAG_CMD:
1832                 if (ccmd->header.flags & ROM_TAG_CMD_FLAG_ROM_LAST_TAG)
1833                         flag = "LAST";
1834                 soprintf(ictx, " TAG %s # %s\n", flag, csum);
1835                 sb_aes_reinit(ictx, 0);
1836                 return ret;
1837         case ROM_LOAD_CMD:
1838                 soprintf(ictx, " LOAD addr=0x%08x length=0x%08x # %s\n",
1839                          ccmd->load.address, ccmd->load.count, csum);
1840
1841                 cctx->length = ccmd->load.count;
1842                 asize = roundup(cctx->length, SB_BLOCK_SIZE);
1843                 cctx->data = malloc(asize);
1844                 if (!cctx->data)
1845                         return -ENOMEM;
1846
1847                 size = fread(cctx->data, 1, asize, fp);
1848                 if (size != asize) {
1849                         fprintf(stderr,
1850                                 "ERR: SB LOAD command payload too short!\n");
1851                         return -EINVAL;
1852                 }
1853
1854                 *tsize += size;
1855
1856                 EVP_DigestUpdate(ictx->md_ctx, cctx->data, asize);
1857                 sb_aes_crypt(ictx, cctx->data, cctx->data, asize);
1858
1859                 if (ccmd->load.crc32 != pbl_crc32(0,
1860                                                   (const char *)cctx->data,
1861                                                   asize)) {
1862                         fprintf(stderr,
1863                                 "ERR: SB LOAD command payload CRC32 invalid!\n");
1864                         return -EINVAL;
1865                 }
1866                 return 0;
1867         case ROM_FILL_CMD:
1868                 soprintf(ictx,
1869                          " FILL addr=0x%08x length=0x%08x pattern=0x%08x # %s\n",
1870                          ccmd->fill.address, ccmd->fill.count,
1871                          ccmd->fill.pattern, csum);
1872                 return 0;
1873         case ROM_JUMP_CMD:
1874                 if (ccmd->header.flags & ROM_JUMP_CMD_FLAG_HAB)
1875                         flag = " HAB";
1876                 soprintf(ictx,
1877                          " JUMP%s addr=0x%08x r0_arg=0x%08x # %s\n",
1878                          flag, ccmd->fill.address, ccmd->jump.argument, csum);
1879                 return 0;
1880         case ROM_CALL_CMD:
1881                 if (ccmd->header.flags & ROM_CALL_CMD_FLAG_HAB)
1882                         flag = " HAB";
1883                 soprintf(ictx,
1884                          " CALL%s addr=0x%08x r0_arg=0x%08x # %s\n",
1885                          flag, ccmd->fill.address, ccmd->jump.argument, csum);
1886                 return 0;
1887         case ROM_MODE_CMD:
1888                 for (i = 0; i < ARRAY_SIZE(modetable); i++) {
1889                         if (ccmd->mode.mode == modetable[i].mode) {
1890                                 soprintf(ictx, " MODE %s # %s\n",
1891                                          modetable[i].name, csum);
1892                                 break;
1893                         }
1894                 }
1895                 fprintf(stderr, " MODE !INVALID! # %s\n", csum);
1896                 return 0;
1897         }
1898
1899         return ret;
1900 }
1901
1902 static int sb_verify_commands(struct sb_image_ctx *ictx,
1903                               struct sb_section_ctx *sctx, FILE *fp)
1904 {
1905         unsigned long size, tsize = 0;
1906         struct sb_cmd_ctx *cctx;
1907         int ret;
1908
1909         sb_aes_reinit(ictx, 0);
1910
1911         while (tsize < sctx->size) {
1912                 cctx = calloc(1, sizeof(*cctx));
1913                 if (!cctx)
1914                         return -ENOMEM;
1915                 if (!sctx->cmd_head) {
1916                         sctx->cmd_head = cctx;
1917                         sctx->cmd_tail = cctx;
1918                 } else {
1919                         sctx->cmd_tail->cmd = cctx;
1920                         sctx->cmd_tail = cctx;
1921                 }
1922
1923                 size = fread(&cctx->c_payload, 1, sizeof(cctx->c_payload), fp);
1924                 if (size != sizeof(cctx->c_payload)) {
1925                         fprintf(stderr, "ERR: SB command header too short!\n");
1926                         return -EINVAL;
1927                 }
1928
1929                 tsize += size;
1930
1931                 sb_decrypt_tag(ictx, cctx);
1932
1933                 ret = sb_verify_command(ictx, cctx, fp, &tsize);
1934                 if (ret)
1935                         return -EINVAL;
1936         }
1937
1938         return 0;
1939 }
1940
1941 static int sb_verify_sections_cmds(struct sb_image_ctx *ictx, FILE *fp)
1942 {
1943         struct sb_boot_image_header *hdr = &ictx->payload;
1944         struct sb_sections_header *shdr;
1945         unsigned int i;
1946         int ret;
1947         struct sb_section_ctx *sctx;
1948         unsigned long size;
1949         char *bootable = "";
1950
1951         soprintf(ictx, "----- Verifying  SB Sections and Commands -----\n");
1952
1953         for (i = 0; i < hdr->section_count; i++) {
1954                 sctx = calloc(1, sizeof(*sctx));
1955                 if (!sctx)
1956                         return -ENOMEM;
1957                 if (!ictx->sect_head) {
1958                         ictx->sect_head = sctx;
1959                         ictx->sect_tail = sctx;
1960                 } else {
1961                         ictx->sect_tail->sect = sctx;
1962                         ictx->sect_tail = sctx;
1963                 }
1964
1965                 size = fread(&sctx->payload, 1, sizeof(sctx->payload), fp);
1966                 if (size != sizeof(sctx->payload)) {
1967                         fprintf(stderr, "ERR: SB section header too short!\n");
1968                         return -EINVAL;
1969                 }
1970         }
1971
1972         size = fread(&ictx->sb_dict_key, 1, sizeof(ictx->sb_dict_key), fp);
1973         if (size != sizeof(ictx->sb_dict_key)) {
1974                 fprintf(stderr, "ERR: SB key dictionary too short!\n");
1975                 return -EINVAL;
1976         }
1977
1978         sb_encrypt_sb_sections_header(ictx);
1979         sb_aes_reinit(ictx, 0);
1980         sb_decrypt_key_dictionary_key(ictx);
1981
1982         sb_aes_reinit(ictx, 0);
1983
1984         sctx = ictx->sect_head;
1985         while (sctx) {
1986                 shdr = &sctx->payload;
1987
1988                 if (shdr->section_flags & SB_SECTION_FLAG_BOOTABLE) {
1989                         sctx->boot = 1;
1990                         bootable = " BOOTABLE";
1991                 }
1992
1993                 sctx->size = (shdr->section_size * SB_BLOCK_SIZE) +
1994                              sizeof(struct sb_command);
1995                 soprintf(ictx, "SECTION 0x%x%s # size = %i bytes\n",
1996                          shdr->section_number, bootable, sctx->size);
1997
1998                 if (shdr->section_flags & ~SB_SECTION_FLAG_BOOTABLE)
1999                         fprintf(stderr, " WARN: Unknown section flag(s) %08x\n",
2000                                 shdr->section_flags);
2001
2002                 if ((shdr->section_flags & SB_SECTION_FLAG_BOOTABLE) &&
2003                     (hdr->first_boot_section_id != shdr->section_number)) {
2004                         fprintf(stderr,
2005                                 " WARN: Bootable section does ID not match image header ID!\n");
2006                 }
2007
2008                 ret = sb_verify_commands(ictx, sctx, fp);
2009                 if (ret)
2010                         return ret;
2011
2012                 sctx = sctx->sect;
2013         }
2014
2015         /*
2016          * FIXME IDEA:
2017          * check if the first TAG command is at sctx->section_offset
2018          */
2019         return 0;
2020 }
2021
2022 static int sb_verify_image_end(struct sb_image_ctx *ictx,
2023                                FILE *fp, off_t filesz)
2024 {
2025         uint8_t digest[32];
2026         unsigned long size;
2027         off_t pos;
2028         int ret;
2029
2030         soprintf(ictx, "------------- Verifying image end -------------\n");
2031
2032         size = fread(digest, 1, sizeof(digest), fp);
2033         if (size != sizeof(digest)) {
2034                 fprintf(stderr, "ERR: SB key dictionary too short!\n");
2035                 return -EINVAL;
2036         }
2037
2038         pos = ftell(fp);
2039         if (pos != filesz) {
2040                 fprintf(stderr, "ERR: Trailing data past the image!\n");
2041                 return -EINVAL;
2042         }
2043
2044         /* Check the image digest. */
2045         EVP_DigestFinal(ictx->md_ctx, ictx->digest, NULL);
2046         EVP_MD_CTX_free(ictx->md_ctx);
2047
2048         /* Decrypt the image digest from the input image. */
2049         sb_aes_reinit(ictx, 0);
2050         sb_aes_crypt(ictx, digest, digest, sizeof(digest));
2051
2052         /* Check all of 20 bytes of the SHA1 hash. */
2053         ret = memcmp(digest, ictx->digest, 20) ? -EINVAL : 0;
2054
2055         if (ret)
2056                 soprintf(ictx, "[FAIL] Full-image checksum:          BAD\n");
2057         else
2058                 soprintf(ictx, "[PASS] Full-image checksum:          OK\n");
2059
2060         return ret;
2061 }
2062
2063
2064 static int sb_build_tree_from_img(struct sb_image_ctx *ictx)
2065 {
2066         long filesize;
2067         int ret;
2068         FILE *fp;
2069
2070         if (!ictx->input_filename) {
2071                 fprintf(stderr, "ERR: Missing filename!\n");
2072                 return -EINVAL;
2073         }
2074
2075         fp = fopen(ictx->input_filename, "r");
2076         if (!fp)
2077                 goto err_open;
2078
2079         ret = fseek(fp, 0, SEEK_END);
2080         if (ret < 0)
2081                 goto err_file;
2082
2083         filesize = ftell(fp);
2084         if (filesize < 0)
2085                 goto err_file;
2086
2087         ret = fseek(fp, 0, SEEK_SET);
2088         if (ret < 0)
2089                 goto err_file;
2090
2091         if (filesize < (signed)sizeof(ictx->payload)) {
2092                 fprintf(stderr, "ERR: File too short!\n");
2093                 goto err_file;
2094         }
2095
2096         if (filesize & (SB_BLOCK_SIZE - 1)) {
2097                 fprintf(stderr, "ERR: The file is not aligned!\n");
2098                 goto err_file;
2099         }
2100
2101         /* Load and verify image header */
2102         ret = sb_verify_image_header(ictx, fp, filesize);
2103         if (ret)
2104                 goto err_verify;
2105
2106         /* Load and verify sections and commands */
2107         ret = sb_verify_sections_cmds(ictx, fp);
2108         if (ret)
2109                 goto err_verify;
2110
2111         ret = sb_verify_image_end(ictx, fp, filesize);
2112         if (ret)
2113                 goto err_verify;
2114
2115         ret = 0;
2116
2117 err_verify:
2118         soprintf(ictx, "-------------------- Result -------------------\n");
2119         soprintf(ictx, "Verification %s\n", ret ? "FAILED" : "PASSED");
2120
2121         /* Stop the encryption session. */
2122         sb_aes_deinit(ictx->cipher_ctx);
2123
2124         fclose(fp);
2125         return ret;
2126
2127 err_file:
2128         fclose(fp);
2129 err_open:
2130         fprintf(stderr, "ERR: Failed to load file \"%s\"\n",
2131                 ictx->input_filename);
2132         return -EINVAL;
2133 }
2134
2135 static void sb_free_image(struct sb_image_ctx *ictx)
2136 {
2137         struct sb_section_ctx *sctx = ictx->sect_head, *s_head;
2138         struct sb_dcd_ctx *dctx = ictx->dcd_head, *d_head;
2139         struct sb_cmd_ctx *cctx, *c_head;
2140
2141         while (sctx) {
2142                 s_head = sctx;
2143                 c_head = sctx->cmd_head;
2144
2145                 while (c_head) {
2146                         cctx = c_head;
2147                         c_head = c_head->cmd;
2148                         if (cctx->data)
2149                                 free(cctx->data);
2150                         free(cctx);
2151                 }
2152
2153                 sctx = sctx->sect;
2154                 free(s_head);
2155         }
2156
2157         while (dctx) {
2158                 d_head = dctx;
2159                 dctx = dctx->dcd;
2160                 free(d_head->payload);
2161                 free(d_head);
2162         }
2163 }
2164
2165 /*
2166  * MXSSB-MKIMAGE glue code.
2167  */
2168 static int mxsimage_check_image_types(uint8_t type)
2169 {
2170         if (type == IH_TYPE_MXSIMAGE)
2171                 return EXIT_SUCCESS;
2172         else
2173                 return EXIT_FAILURE;
2174 }
2175
2176 static void mxsimage_set_header(void *ptr, struct stat *sbuf, int ifd,
2177                                 struct image_tool_params *params)
2178 {
2179 }
2180
2181 int mxsimage_check_params(struct image_tool_params *params)
2182 {
2183         if (!params)
2184                 return -1;
2185         if (!strlen(params->imagename)) {
2186                 fprintf(stderr,
2187                         "Error: %s - Configuration file not specified, it is needed for mxsimage generation\n",
2188                         params->cmdname);
2189                 return -1;
2190         }
2191
2192         /*
2193          * Check parameters:
2194          * XIP is not allowed and verify that incompatible
2195          * parameters are not sent at the same time
2196          * For example, if list is required a data image must not be provided
2197          */
2198         return  (params->dflag && (params->fflag || params->lflag)) ||
2199                 (params->fflag && (params->dflag || params->lflag)) ||
2200                 (params->lflag && (params->dflag || params->fflag)) ||
2201                 (params->xflag) || !(strlen(params->imagename));
2202 }
2203
2204 static int mxsimage_verify_print_header(char *file, int silent)
2205 {
2206         int ret;
2207         struct sb_image_ctx ctx;
2208
2209         memset(&ctx, 0, sizeof(ctx));
2210
2211         ctx.input_filename = file;
2212         ctx.silent_dump = silent;
2213
2214         ret = sb_build_tree_from_img(&ctx);
2215         sb_free_image(&ctx);
2216
2217         return ret;
2218 }
2219
2220 char *imagefile;
2221 static int mxsimage_verify_header(unsigned char *ptr, int image_size,
2222                         struct image_tool_params *params)
2223 {
2224         struct sb_boot_image_header *hdr;
2225
2226         if (!ptr)
2227                 return -EINVAL;
2228
2229         hdr = (struct sb_boot_image_header *)ptr;
2230
2231         /*
2232          * Check if the header contains the MXS image signatures,
2233          * if so, do a full-image verification.
2234          */
2235         if (memcmp(hdr->signature1, "STMP", 4) ||
2236             memcmp(hdr->signature2, "sgtl", 4))
2237                 return -EINVAL;
2238
2239         imagefile = params->imagefile;
2240
2241         return mxsimage_verify_print_header(params->imagefile, 1);
2242 }
2243
2244 static void mxsimage_print_header(const void *hdr)
2245 {
2246         if (imagefile)
2247                 mxsimage_verify_print_header(imagefile, 0);
2248 }
2249
2250 static int sb_build_image(struct sb_image_ctx *ictx,
2251                           struct image_type_params *tparams)
2252 {
2253         struct sb_boot_image_header *sb_header = &ictx->payload;
2254         struct sb_section_ctx *sctx;
2255         struct sb_cmd_ctx *cctx;
2256         struct sb_command *ccmd;
2257         struct sb_key_dictionary_key *sb_dict_key = &ictx->sb_dict_key;
2258
2259         uint8_t *image, *iptr;
2260
2261         /* Calculate image size. */
2262         uint32_t size = sizeof(*sb_header) +
2263                 ictx->sect_count * sizeof(struct sb_sections_header) +
2264                 sizeof(*sb_dict_key) + sizeof(ictx->digest);
2265
2266         sctx = ictx->sect_head;
2267         while (sctx) {
2268                 size += sctx->size;
2269                 sctx = sctx->sect;
2270         };
2271
2272         image = malloc(size);
2273         if (!image)
2274                 return -ENOMEM;
2275         iptr = image;
2276
2277         memcpy(iptr, sb_header, sizeof(*sb_header));
2278         iptr += sizeof(*sb_header);
2279
2280         sctx = ictx->sect_head;
2281         while (sctx) {
2282                 memcpy(iptr, &sctx->payload, sizeof(struct sb_sections_header));
2283                 iptr += sizeof(struct sb_sections_header);
2284                 sctx = sctx->sect;
2285         };
2286
2287         memcpy(iptr, sb_dict_key, sizeof(*sb_dict_key));
2288         iptr += sizeof(*sb_dict_key);
2289
2290         sctx = ictx->sect_head;
2291         while (sctx) {
2292                 cctx = sctx->cmd_head;
2293                 while (cctx) {
2294                         ccmd = &cctx->payload;
2295
2296                         memcpy(iptr, &cctx->c_payload, sizeof(cctx->payload));
2297                         iptr += sizeof(cctx->payload);
2298
2299                         if (ccmd->header.tag == ROM_LOAD_CMD) {
2300                                 memcpy(iptr, cctx->data, cctx->length);
2301                                 iptr += cctx->length;
2302                         }
2303
2304                         cctx = cctx->cmd;
2305                 }
2306
2307                 sctx = sctx->sect;
2308         };
2309
2310         memcpy(iptr, ictx->digest, sizeof(ictx->digest));
2311         iptr += sizeof(ictx->digest);
2312
2313         /* Configure the mkimage */
2314         tparams->hdr = image;
2315         tparams->header_size = size;
2316
2317         return 0;
2318 }
2319
2320 static int mxsimage_generate(struct image_tool_params *params,
2321         struct image_type_params *tparams)
2322 {
2323         int ret;
2324         struct sb_image_ctx ctx;
2325
2326         /* Do not copy the U-Boot image! */
2327         params->skipcpy = 1;
2328
2329         memset(&ctx, 0, sizeof(ctx));
2330
2331         ctx.cfg_filename = params->imagename;
2332         ctx.output_filename = params->imagefile;
2333
2334         ret = sb_build_tree_from_cfg(&ctx);
2335         if (ret)
2336                 goto fail;
2337
2338         ret = sb_encrypt_image(&ctx);
2339         if (!ret)
2340                 ret = sb_build_image(&ctx, tparams);
2341
2342 fail:
2343         sb_free_image(&ctx);
2344
2345         return ret;
2346 }
2347
2348 /*
2349  * mxsimage parameters
2350  */
2351 U_BOOT_IMAGE_TYPE(
2352         mxsimage,
2353         "Freescale MXS Boot Image support",
2354         0,
2355         NULL,
2356         mxsimage_check_params,
2357         mxsimage_verify_header,
2358         mxsimage_print_header,
2359         mxsimage_set_header,
2360         NULL,
2361         mxsimage_check_image_types,
2362         NULL,
2363         mxsimage_generate
2364 );
2365 #endif