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