tools/mxsimage.c

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