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