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