tools/kwbimage.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * Image manipulator for Marvell SoCs
   4  *  supports Kirkwood, Dove, Armada 370, Armada XP, and Armada 38x
   5  *
   6  * (C) Copyright 2013 Thomas Petazzoni
   7  * <thomas.petazzoni@free-electrons.com>
   8  */
   9
  10 #include "imagetool.h"
  11 #include <limits.h>
  12 #include <image.h>
  13 #include <stdarg.h>
  14 #include <stdint.h>
  15 #include "kwbimage.h"
  16
  17 #include <openssl/bn.h>
  18 #include <openssl/rsa.h>
  19 #include <openssl/pem.h>
  20 #include <openssl/err.h>
  21 #include <openssl/evp.h>
  22
  23 #if OPENSSL_VERSION_NUMBER < 0x10100000L || \
  24     (defined(LIBRESSL_VERSION_NUMBER) && LIBRESSL_VERSION_NUMBER < 0x2070000fL)
  25 static void RSA_get0_key(const RSA *r,
  26                  const BIGNUM **n, const BIGNUM **e, const BIGNUM **d)
  27 {
  28    if (n != NULL)
  29        *n = r->n;
  30    if (e != NULL)
  31        *e = r->e;
  32    if (d != NULL)
  33        *d = r->d;
  34 }
  35
  36 #elif !defined(LIBRESSL_VERSION_NUMBER)
  37 void EVP_MD_CTX_cleanup(EVP_MD_CTX *ctx)
  38 {
  39         EVP_MD_CTX_reset(ctx);
  40 }
  41 #endif
  42
  43 static struct image_cfg_element *image_cfg;
  44 static int cfgn;
  45 static int verbose_mode;
  46
  47 struct boot_mode {
  48         unsigned int id;
  49         const char *name;
  50 };
  51
  52 /*
  53  * SHA2-256 hash
  54  */
  55 struct hash_v1 {
  56         uint8_t hash[32];
  57 };
  58
  59 struct boot_mode boot_modes[] = {
  60         { IBR_HDR_I2C_ID, "i2c"  },
  61         { IBR_HDR_SPI_ID, "spi"  },
  62         { IBR_HDR_NAND_ID, "nand" },
  63         { IBR_HDR_SATA_ID, "sata" },
  64         { IBR_HDR_PEX_ID, "pex"  },
  65         { IBR_HDR_UART_ID, "uart" },
  66         { IBR_HDR_SDIO_ID, "sdio" },
  67         {},
  68 };
  69
  70 struct nand_ecc_mode {
  71         unsigned int id;
  72         const char *name;
  73 };
  74
  75 struct nand_ecc_mode nand_ecc_modes[] = {
  76         { IBR_HDR_ECC_DEFAULT, "default" },
  77         { IBR_HDR_ECC_FORCED_HAMMING, "hamming" },
  78         { IBR_HDR_ECC_FORCED_RS, "rs" },
  79         { IBR_HDR_ECC_DISABLED, "disabled" },
  80         {},
  81 };
  82
  83 /* Used to identify an undefined execution or destination address */
  84 #define ADDR_INVALID ((uint32_t)-1)
  85
  86 #define BINARY_MAX_ARGS 255
  87
  88 /* In-memory representation of a line of the configuration file */
  89
  90 enum image_cfg_type {
  91         IMAGE_CFG_VERSION = 0x1,
  92         IMAGE_CFG_BOOT_FROM,
  93         IMAGE_CFG_DEST_ADDR,
  94         IMAGE_CFG_EXEC_ADDR,
  95         IMAGE_CFG_NAND_BLKSZ,
  96         IMAGE_CFG_NAND_BADBLK_LOCATION,
  97         IMAGE_CFG_NAND_ECC_MODE,
  98         IMAGE_CFG_NAND_PAGESZ,
  99         IMAGE_CFG_BINARY,
 100         IMAGE_CFG_DATA,
 101         IMAGE_CFG_DATA_DELAY,
 102         IMAGE_CFG_BAUDRATE,
 103         IMAGE_CFG_DEBUG,
 104         IMAGE_CFG_KAK,
 105         IMAGE_CFG_CSK,
 106         IMAGE_CFG_CSK_INDEX,
 107         IMAGE_CFG_JTAG_DELAY,
 108         IMAGE_CFG_BOX_ID,
 109         IMAGE_CFG_FLASH_ID,
 110         IMAGE_CFG_SEC_COMMON_IMG,
 111         IMAGE_CFG_SEC_SPECIALIZED_IMG,
 112         IMAGE_CFG_SEC_BOOT_DEV,
 113         IMAGE_CFG_SEC_FUSE_DUMP,
 114
 115         IMAGE_CFG_COUNT
 116 } type;
 117
 118 static const char * const id_strs[] = {
 119         [IMAGE_CFG_VERSION] = "VERSION",
 120         [IMAGE_CFG_BOOT_FROM] = "BOOT_FROM",
 121         [IMAGE_CFG_DEST_ADDR] = "DEST_ADDR",
 122         [IMAGE_CFG_EXEC_ADDR] = "EXEC_ADDR",
 123         [IMAGE_CFG_NAND_BLKSZ] = "NAND_BLKSZ",
 124         [IMAGE_CFG_NAND_BADBLK_LOCATION] = "NAND_BADBLK_LOCATION",
 125         [IMAGE_CFG_NAND_ECC_MODE] = "NAND_ECC_MODE",
 126         [IMAGE_CFG_NAND_PAGESZ] = "NAND_PAGE_SIZE",
 127         [IMAGE_CFG_BINARY] = "BINARY",
 128         [IMAGE_CFG_DATA] = "DATA",
 129         [IMAGE_CFG_DATA_DELAY] = "DATA_DELAY",
 130         [IMAGE_CFG_BAUDRATE] = "BAUDRATE",
 131         [IMAGE_CFG_DEBUG] = "DEBUG",
 132         [IMAGE_CFG_KAK] = "KAK",
 133         [IMAGE_CFG_CSK] = "CSK",
 134         [IMAGE_CFG_CSK_INDEX] = "CSK_INDEX",
 135         [IMAGE_CFG_JTAG_DELAY] = "JTAG_DELAY",
 136         [IMAGE_CFG_BOX_ID] = "BOX_ID",
 137         [IMAGE_CFG_FLASH_ID] = "FLASH_ID",
 138         [IMAGE_CFG_SEC_COMMON_IMG] = "SEC_COMMON_IMG",
 139         [IMAGE_CFG_SEC_SPECIALIZED_IMG] = "SEC_SPECIALIZED_IMG",
 140         [IMAGE_CFG_SEC_BOOT_DEV] = "SEC_BOOT_DEV",
 141         [IMAGE_CFG_SEC_FUSE_DUMP] = "SEC_FUSE_DUMP"
 142 };
 143
 144 struct image_cfg_element {
 145         enum image_cfg_type type;
 146         union {
 147                 unsigned int version;
 148                 unsigned int bootfrom;
 149                 struct {
 150                         const char *file;
 151                         unsigned int args[BINARY_MAX_ARGS];
 152                         unsigned int nargs;
 153                 } binary;
 154                 unsigned int dstaddr;
 155                 unsigned int execaddr;
 156                 unsigned int nandblksz;
 157                 unsigned int nandbadblklocation;
 158                 unsigned int nandeccmode;
 159                 unsigned int nandpagesz;
 160                 struct ext_hdr_v0_reg regdata;
 161                 unsigned int regdata_delay;
 162                 unsigned int baudrate;
 163                 unsigned int debug;
 164                 const char *key_name;
 165                 int csk_idx;
 166                 uint8_t jtag_delay;
 167                 uint32_t boxid;
 168                 uint32_t flashid;
 169                 bool sec_specialized_img;
 170                 unsigned int sec_boot_dev;
 171                 const char *name;
 172         };
 173 };
 174
 175 #define IMAGE_CFG_ELEMENT_MAX 256
 176
 177 /*
 178  * Utility functions to manipulate boot mode and ecc modes (convert
 179  * them back and forth between description strings and the
 180  * corresponding numerical identifiers).
 181  */
 182
 183 static const char *image_boot_mode_name(unsigned int id)
 184 {
 185         int i;
 186
 187         for (i = 0; boot_modes[i].name; i++)
 188                 if (boot_modes[i].id == id)
 189                         return boot_modes[i].name;
 190         return NULL;
 191 }
 192
 193 int image_boot_mode_id(const char *boot_mode_name)
 194 {
 195         int i;
 196
 197         for (i = 0; boot_modes[i].name; i++)
 198                 if (!strcmp(boot_modes[i].name, boot_mode_name))
 199                         return boot_modes[i].id;
 200
 201         return -1;
 202 }
 203
 204 int image_nand_ecc_mode_id(const char *nand_ecc_mode_name)
 205 {
 206         int i;
 207
 208         for (i = 0; nand_ecc_modes[i].name; i++)
 209                 if (!strcmp(nand_ecc_modes[i].name, nand_ecc_mode_name))
 210                         return nand_ecc_modes[i].id;
 211         return -1;
 212 }
 213
 214 static struct image_cfg_element *
 215 image_find_option(unsigned int optiontype)
 216 {
 217         int i;
 218
 219         for (i = 0; i < cfgn; i++) {
 220                 if (image_cfg[i].type == optiontype)
 221                         return &image_cfg[i];
 222         }
 223
 224         return NULL;
 225 }
 226
 227 static unsigned int
 228 image_count_options(unsigned int optiontype)
 229 {
 230         int i;
 231         unsigned int count = 0;
 232
 233         for (i = 0; i < cfgn; i++)
 234                 if (image_cfg[i].type == optiontype)
 235                         count++;
 236
 237         return count;
 238 }
 239
 240 static int image_get_csk_index(void)
 241 {
 242         struct image_cfg_element *e;
 243
 244         e = image_find_option(IMAGE_CFG_CSK_INDEX);
 245         if (!e)
 246                 return -1;
 247
 248         return e->csk_idx;
 249 }
 250
 251 static bool image_get_spezialized_img(void)
 252 {
 253         struct image_cfg_element *e;
 254
 255         e = image_find_option(IMAGE_CFG_SEC_SPECIALIZED_IMG);
 256         if (!e)
 257                 return false;
 258
 259         return e->sec_specialized_img;
 260 }
 261
 262 /*
 263  * Compute a 8-bit checksum of a memory area. This algorithm follows
 264  * the requirements of the Marvell SoC BootROM specifications.
 265  */
 266 static uint8_t image_checksum8(void *start, uint32_t len)
 267 {
 268         uint8_t csum = 0;
 269         uint8_t *p = start;
 270
 271         /* check len and return zero checksum if invalid */
 272         if (!len)
 273                 return 0;
 274
 275         do {
 276                 csum += *p;
 277                 p++;
 278         } while (--len);
 279
 280         return csum;
 281 }
 282
 283 size_t kwbimage_header_size(unsigned char *ptr)
 284 {
 285         if (image_version((void *)ptr) == 0)
 286                 return sizeof(struct main_hdr_v0);
 287         else
 288                 return KWBHEADER_V1_SIZE((struct main_hdr_v1 *)ptr);
 289 }
 290
 291 /*
 292  * Verify checksum over a complete header that includes the checksum field.
 293  * Return 1 when OK, otherwise 0.
 294  */
 295 static int main_hdr_checksum_ok(void *hdr)
 296 {
 297         /* Offsets of checksum in v0 and v1 headers are the same */
 298         struct main_hdr_v0 *main_hdr = (struct main_hdr_v0 *)hdr;
 299         uint8_t checksum;
 300
 301         checksum = image_checksum8(hdr, kwbimage_header_size(hdr));
 302         /* Calculated checksum includes the header checksum field. Compensate
 303          * for that.
 304          */
 305         checksum -= main_hdr->checksum;
 306
 307         return checksum == main_hdr->checksum;
 308 }
 309
 310 static uint32_t image_checksum32(void *start, uint32_t len)
 311 {
 312         uint32_t csum = 0;
 313         uint32_t *p = start;
 314
 315         /* check len and return zero checksum if invalid */
 316         if (!len)
 317                 return 0;
 318
 319         if (len % sizeof(uint32_t)) {
 320                 fprintf(stderr, "Length %d is not in multiple of %zu\n",
 321                         len, sizeof(uint32_t));
 322                 return 0;
 323         }
 324
 325         do {
 326                 csum += *p;
 327                 p++;
 328                 len -= sizeof(uint32_t);
 329         } while (len > 0);
 330
 331         return csum;
 332 }
 333
 334 static uint8_t baudrate_to_option(unsigned int baudrate)
 335 {
 336         switch (baudrate) {
 337         case 2400:
 338                 return MAIN_HDR_V1_OPT_BAUD_2400;
 339         case 4800:
 340                 return MAIN_HDR_V1_OPT_BAUD_4800;
 341         case 9600:
 342                 return MAIN_HDR_V1_OPT_BAUD_9600;
 343         case 19200:
 344                 return MAIN_HDR_V1_OPT_BAUD_19200;
 345         case 38400:
 346                 return MAIN_HDR_V1_OPT_BAUD_38400;
 347         case 57600:
 348                 return MAIN_HDR_V1_OPT_BAUD_57600;
 349         case 115200:
 350                 return MAIN_HDR_V1_OPT_BAUD_115200;
 351         default:
 352                 return MAIN_HDR_V1_OPT_BAUD_DEFAULT;
 353         }
 354 }
 355
 356 static void kwb_msg(const char *fmt, ...)
 357 {
 358         if (verbose_mode) {
 359                 va_list ap;
 360
 361                 va_start(ap, fmt);
 362                 vfprintf(stdout, fmt, ap);
 363                 va_end(ap);
 364         }
 365 }
 366
 367 static int openssl_err(const char *msg)
 368 {
 369         unsigned long ssl_err = ERR_get_error();
 370
 371         fprintf(stderr, "%s", msg);
 372         fprintf(stderr, ": %s\n",
 373                 ERR_error_string(ssl_err, 0));
 374
 375         return -1;
 376 }
 377
 378 static int kwb_load_rsa_key(const char *keydir, const char *name, RSA **p_rsa)
 379 {
 380         char path[PATH_MAX];
 381         RSA *rsa;
 382         FILE *f;
 383
 384         if (!keydir)
 385                 keydir = ".";
 386
 387         snprintf(path, sizeof(path), "%s/%s.key", keydir, name);
 388         f = fopen(path, "r");
 389         if (!f) {
 390                 fprintf(stderr, "Couldn't open RSA private key: '%s': %s\n",
 391                         path, strerror(errno));
 392                 return -ENOENT;
 393         }
 394
 395         rsa = PEM_read_RSAPrivateKey(f, 0, NULL, "");
 396         if (!rsa) {
 397                 openssl_err("Failure reading private key");
 398                 fclose(f);
 399                 return -EPROTO;
 400         }
 401         fclose(f);
 402         *p_rsa = rsa;
 403
 404         return 0;
 405 }
 406
 407 static int kwb_load_cfg_key(struct image_tool_params *params,
 408                             unsigned int cfg_option, const char *key_name,
 409                             RSA **p_key)
 410 {
 411         struct image_cfg_element *e_key;
 412         RSA *key;
 413         int res;
 414
 415         *p_key = NULL;
 416
 417         e_key = image_find_option(cfg_option);
 418         if (!e_key) {
 419                 fprintf(stderr, "%s not configured\n", key_name);
 420                 return -ENOENT;
 421         }
 422
 423         res = kwb_load_rsa_key(params->keydir, e_key->key_name, &key);
 424         if (res < 0) {
 425                 fprintf(stderr, "Failed to load %s\n", key_name);
 426                 return -ENOENT;
 427         }
 428
 429         *p_key = key;
 430
 431         return 0;
 432 }
 433
 434 static int kwb_load_kak(struct image_tool_params *params, RSA **p_kak)
 435 {
 436         return kwb_load_cfg_key(params, IMAGE_CFG_KAK, "KAK", p_kak);
 437 }
 438
 439 static int kwb_load_csk(struct image_tool_params *params, RSA **p_csk)
 440 {
 441         return kwb_load_cfg_key(params, IMAGE_CFG_CSK, "CSK", p_csk);
 442 }
 443
 444 static int kwb_compute_pubkey_hash(struct pubkey_der_v1 *pk,
 445                                    struct hash_v1 *hash)
 446 {
 447         EVP_MD_CTX *ctx;
 448         unsigned int key_size;
 449         unsigned int hash_size;
 450         int ret = 0;
 451
 452         if (!pk || !hash || pk->key[0] != 0x30 || pk->key[1] != 0x82)
 453                 return -EINVAL;
 454
 455         key_size = (pk->key[2] << 8) + pk->key[3] + 4;
 456
 457         ctx = EVP_MD_CTX_create();
 458         if (!ctx)
 459                 return openssl_err("EVP context creation failed");
 460
 461         EVP_MD_CTX_init(ctx);
 462         if (!EVP_DigestInit(ctx, EVP_sha256())) {
 463                 ret = openssl_err("Digest setup failed");
 464                 goto hash_err_ctx;
 465         }
 466
 467         if (!EVP_DigestUpdate(ctx, pk->key, key_size)) {
 468                 ret = openssl_err("Hashing data failed");
 469                 goto hash_err_ctx;
 470         }
 471
 472         if (!EVP_DigestFinal(ctx, hash->hash, &hash_size)) {
 473                 ret = openssl_err("Could not obtain hash");
 474                 goto hash_err_ctx;
 475         }
 476
 477         EVP_MD_CTX_cleanup(ctx);
 478
 479 hash_err_ctx:
 480         EVP_MD_CTX_destroy(ctx);
 481         return ret;
 482 }
 483
 484 static int kwb_import_pubkey(RSA **key, struct pubkey_der_v1 *src, char *keyname)
 485 {
 486         RSA *rsa;
 487         const unsigned char *ptr;
 488
 489         if (!key || !src)
 490                 goto fail;
 491
 492         ptr = src->key;
 493         rsa = d2i_RSAPublicKey(key, &ptr, sizeof(src->key));
 494         if (!rsa) {
 495                 openssl_err("error decoding public key");
 496                 goto fail;
 497         }
 498
 499         return 0;
 500 fail:
 501         fprintf(stderr, "Failed to decode %s pubkey\n", keyname);
 502         return -EINVAL;
 503 }
 504
 505 static int kwb_export_pubkey(RSA *key, struct pubkey_der_v1 *dst, FILE *hashf,
 506                              char *keyname)
 507 {
 508         int size_exp, size_mod, size_seq;
 509         const BIGNUM *key_e, *key_n;
 510         uint8_t *cur;
 511         char *errmsg = "Failed to encode %s\n";
 512
 513         RSA_get0_key(key, NULL, &key_e, NULL);
 514         RSA_get0_key(key, &key_n, NULL, NULL);
 515
 516         if (!key || !key_e || !key_n || !dst) {
 517                 fprintf(stderr, "export pk failed: (%p, %p, %p, %p)",
 518                         key, key_e, key_n, dst);
 519                 fprintf(stderr, errmsg, keyname);
 520                 return -EINVAL;
 521         }
 522
 523         /*
 524          * According to the specs, the key should be PKCS#1 DER encoded.
 525          * But unfortunately the really required encoding seems to be different;
 526          * it violates DER...! (But it still conformes to BER.)
 527          * (Length always in long form w/ 2 byte length code; no leading zero
 528          * when MSB of first byte is set...)
 529          * So we cannot use the encoding func provided by OpenSSL and have to
 530          * do the encoding manually.
 531          */
 532
 533         size_exp = BN_num_bytes(key_e);
 534         size_mod = BN_num_bytes(key_n);
 535         size_seq = 4 + size_mod + 4 + size_exp;
 536
 537         if (size_mod > 256) {
 538                 fprintf(stderr, "export pk failed: wrong mod size: %d\n",
 539                         size_mod);
 540                 fprintf(stderr, errmsg, keyname);
 541                 return -EINVAL;
 542         }
 543
 544         if (4 + size_seq > sizeof(dst->key)) {
 545                 fprintf(stderr, "export pk failed: seq too large (%d, %lu)\n",
 546                         4 + size_seq, sizeof(dst->key));
 547                 fprintf(stderr, errmsg, keyname);
 548                 return -ENOBUFS;
 549         }
 550
 551         cur = dst->key;
 552
 553         /* PKCS#1 (RFC3447) RSAPublicKey structure */
 554         *cur++ = 0x30;          /* SEQUENCE */
 555         *cur++ = 0x82;
 556         *cur++ = (size_seq >> 8) & 0xFF;
 557         *cur++ = size_seq & 0xFF;
 558         /* Modulus */
 559         *cur++ = 0x02;          /* INTEGER */
 560         *cur++ = 0x82;
 561         *cur++ = (size_mod >> 8) & 0xFF;
 562         *cur++ = size_mod & 0xFF;
 563         BN_bn2bin(key_n, cur);
 564         cur += size_mod;
 565         /* Exponent */
 566         *cur++ = 0x02;          /* INTEGER */
 567         *cur++ = 0x82;
 568         *cur++ = (size_exp >> 8) & 0xFF;
 569         *cur++ = size_exp & 0xFF;
 570         BN_bn2bin(key_e, cur);
 571
 572         if (hashf) {
 573                 struct hash_v1 pk_hash;
 574                 int i;
 575                 int ret = 0;
 576
 577                 ret = kwb_compute_pubkey_hash(dst, &pk_hash);
 578                 if (ret < 0) {
 579                         fprintf(stderr, errmsg, keyname);
 580                         return ret;
 581                 }
 582
 583                 fprintf(hashf, "SHA256 = ");
 584                 for (i = 0 ; i < sizeof(pk_hash.hash); ++i)
 585                         fprintf(hashf, "%02X", pk_hash.hash[i]);
 586                 fprintf(hashf, "\n");
 587         }
 588
 589         return 0;
 590 }
 591
 592 int kwb_sign(RSA *key, void *data, int datasz, struct sig_v1 *sig, char *signame)
 593 {
 594         EVP_PKEY *evp_key;
 595         EVP_MD_CTX *ctx;
 596         unsigned int sig_size;
 597         int size;
 598         int ret = 0;
 599
 600         evp_key = EVP_PKEY_new();
 601         if (!evp_key)
 602                 return openssl_err("EVP_PKEY object creation failed");
 603
 604         if (!EVP_PKEY_set1_RSA(evp_key, key)) {
 605                 ret = openssl_err("EVP key setup failed");
 606                 goto err_key;
 607         }
 608
 609         size = EVP_PKEY_size(evp_key);
 610         if (size > sizeof(sig->sig)) {
 611                 fprintf(stderr, "Buffer to small for signature (%d bytes)\n",
 612                         size);
 613                 ret = -ENOBUFS;
 614                 goto err_key;
 615         }
 616
 617         ctx = EVP_MD_CTX_create();
 618         if (!ctx) {
 619                 ret = openssl_err("EVP context creation failed");
 620                 goto err_key;
 621         }
 622         EVP_MD_CTX_init(ctx);
 623         if (!EVP_SignInit(ctx, EVP_sha256())) {
 624                 ret = openssl_err("Signer setup failed");
 625                 goto err_ctx;
 626         }
 627
 628         if (!EVP_SignUpdate(ctx, data, datasz)) {
 629                 ret = openssl_err("Signing data failed");
 630                 goto err_ctx;
 631         }
 632
 633         if (!EVP_SignFinal(ctx, sig->sig, &sig_size, evp_key)) {
 634                 ret = openssl_err("Could not obtain signature");
 635                 goto err_ctx;
 636         }
 637
 638         EVP_MD_CTX_cleanup(ctx);
 639         EVP_MD_CTX_destroy(ctx);
 640         EVP_PKEY_free(evp_key);
 641
 642         return 0;
 643
 644 err_ctx:
 645         EVP_MD_CTX_destroy(ctx);
 646 err_key:
 647         EVP_PKEY_free(evp_key);
 648         fprintf(stderr, "Failed to create %s signature\n", signame);
 649         return ret;
 650 }
 651
 652 int kwb_verify(RSA *key, void *data, int datasz, struct sig_v1 *sig,
 653                char *signame)
 654 {
 655         EVP_PKEY *evp_key;
 656         EVP_MD_CTX *ctx;
 657         int size;
 658         int ret = 0;
 659
 660         evp_key = EVP_PKEY_new();
 661         if (!evp_key)
 662                 return openssl_err("EVP_PKEY object creation failed");
 663
 664         if (!EVP_PKEY_set1_RSA(evp_key, key)) {
 665                 ret = openssl_err("EVP key setup failed");
 666                 goto err_key;
 667         }
 668
 669         size = EVP_PKEY_size(evp_key);
 670         if (size > sizeof(sig->sig)) {
 671                 fprintf(stderr, "Invalid signature size (%d bytes)\n",
 672                         size);
 673                 ret = -EINVAL;
 674                 goto err_key;
 675         }
 676
 677         ctx = EVP_MD_CTX_create();
 678         if (!ctx) {
 679                 ret = openssl_err("EVP context creation failed");
 680                 goto err_key;
 681         }
 682         EVP_MD_CTX_init(ctx);
 683         if (!EVP_VerifyInit(ctx, EVP_sha256())) {
 684                 ret = openssl_err("Verifier setup failed");
 685                 goto err_ctx;
 686         }
 687
 688         if (!EVP_VerifyUpdate(ctx, data, datasz)) {
 689                 ret = openssl_err("Hashing data failed");
 690                 goto err_ctx;
 691         }
 692
 693         if (EVP_VerifyFinal(ctx, sig->sig, sizeof(sig->sig), evp_key) != 1) {
 694                 ret = openssl_err("Could not verify signature");
 695                 goto err_ctx;
 696         }
 697
 698         EVP_MD_CTX_cleanup(ctx);
 699         EVP_MD_CTX_destroy(ctx);
 700         EVP_PKEY_free(evp_key);
 701
 702         return 0;
 703
 704 err_ctx:
 705         EVP_MD_CTX_destroy(ctx);
 706 err_key:
 707         EVP_PKEY_free(evp_key);
 708         fprintf(stderr, "Failed to verify %s signature\n", signame);
 709         return ret;
 710 }
 711
 712 int kwb_sign_and_verify(RSA *key, void *data, int datasz, struct sig_v1 *sig,
 713                         char *signame)
 714 {
 715         if (kwb_sign(key, data, datasz, sig, signame) < 0)
 716                 return -1;
 717
 718         if (kwb_verify(key, data, datasz, sig, signame) < 0)
 719                 return -1;
 720
 721         return 0;
 722 }
 723
 724
 725 int kwb_dump_fuse_cmds_38x(FILE *out, struct secure_hdr_v1 *sec_hdr)
 726 {
 727         struct hash_v1 kak_pub_hash;
 728         struct image_cfg_element *e;
 729         unsigned int fuse_line;
 730         int i, idx;
 731         uint8_t *ptr;
 732         uint32_t val;
 733         int ret = 0;
 734
 735         if (!out || !sec_hdr)
 736                 return -EINVAL;
 737
 738         ret = kwb_compute_pubkey_hash(&sec_hdr->kak, &kak_pub_hash);
 739         if (ret < 0)
 740                 goto done;
 741
 742         fprintf(out, "# burn KAK pub key hash\n");
 743         ptr = kak_pub_hash.hash;
 744         for (fuse_line = 26; fuse_line <= 30; ++fuse_line) {
 745                 fprintf(out, "fuse prog -y %u 0 ", fuse_line);
 746
 747                 for (i = 4; i-- > 0;)
 748                         fprintf(out, "%02hx", (ushort)ptr[i]);
 749                 ptr += 4;
 750                 fprintf(out, " 00");
 751
 752                 if (fuse_line < 30) {
 753                         for (i = 3; i-- > 0;)
 754                                 fprintf(out, "%02hx", (ushort)ptr[i]);
 755                         ptr += 3;
 756                 } else {
 757                         fprintf(out, "000000");
 758                 }
 759
 760                 fprintf(out, " 1\n");
 761         }
 762
 763         fprintf(out, "# burn CSK selection\n");
 764
 765         idx = image_get_csk_index();
 766         if (idx < 0 || idx > 15) {
 767                 ret = -EINVAL;
 768                 goto done;
 769         }
 770         if (idx > 0) {
 771                 for (fuse_line = 31; fuse_line < 31 + idx; ++fuse_line)
 772                         fprintf(out, "fuse prog -y %u 0 00000001 00000000 1\n",
 773                                 fuse_line);
 774         } else {
 775                 fprintf(out, "# CSK index is 0; no mods needed\n");
 776         }
 777
 778         e = image_find_option(IMAGE_CFG_BOX_ID);
 779         if (e) {
 780                 fprintf(out, "# set box ID\n");
 781                 fprintf(out, "fuse prog -y 48 0 %08x 00000000 1\n", e->boxid);
 782         }
 783
 784         e = image_find_option(IMAGE_CFG_FLASH_ID);
 785         if (e) {
 786                 fprintf(out, "# set flash ID\n");
 787                 fprintf(out, "fuse prog -y 47 0 %08x 00000000 1\n", e->flashid);
 788         }
 789
 790         fprintf(out, "# enable secure mode ");
 791         fprintf(out, "(must be the last fuse line written)\n");
 792
 793         val = 1;
 794         e = image_find_option(IMAGE_CFG_SEC_BOOT_DEV);
 795         if (!e) {
 796                 fprintf(stderr, "ERROR: secured mode boot device not given\n");
 797                 ret = -EINVAL;
 798                 goto done;
 799         }
 800
 801         if (e->sec_boot_dev > 0xff) {
 802                 fprintf(stderr, "ERROR: secured mode boot device invalid\n");
 803                 ret = -EINVAL;
 804                 goto done;
 805         }
 806
 807         val |= (e->sec_boot_dev << 8);
 808
 809         fprintf(out, "fuse prog -y 24 0 %08x 0103e0a9 1\n", val);
 810
 811         fprintf(out, "# lock (unused) fuse lines (0-23)s\n");
 812         for (fuse_line = 0; fuse_line < 24; ++fuse_line)
 813                 fprintf(out, "fuse prog -y %u 2 1\n", fuse_line);
 814
 815         fprintf(out, "# OK, that's all :-)\n");
 816
 817 done:
 818         return ret;
 819 }
 820
 821 static int kwb_dump_fuse_cmds(struct secure_hdr_v1 *sec_hdr)
 822 {
 823         int ret = 0;
 824         struct image_cfg_element *e;
 825
 826         e = image_find_option(IMAGE_CFG_SEC_FUSE_DUMP);
 827         if (!e)
 828                 return 0;
 829
 830         if (!strcmp(e->name, "a38x")) {
 831                 FILE *out = fopen("kwb_fuses_a38x.txt", "w+");
 832
 833                 if (!out) {
 834                         fprintf(stderr, "Couldn't open eFuse settings: '%s': %s\n",
 835                                 "kwb_fuses_a38x.txt", strerror(errno));
 836                         return -ENOENT;
 837                 }
 838
 839                 kwb_dump_fuse_cmds_38x(out, sec_hdr);
 840                 fclose(out);
 841                 goto done;
 842         }
 843
 844         ret = -ENOSYS;
 845
 846 done:
 847         return ret;
 848 }
 849
 850 static void *image_create_v0(size_t *imagesz, struct image_tool_params *params,
 851                              int payloadsz)
 852 {
 853         struct image_cfg_element *e;
 854         size_t headersz;
 855         struct main_hdr_v0 *main_hdr;
 856         uint8_t *image;
 857         int has_ext = 0;
 858
 859         /*
 860          * Calculate the size of the header and the size of the
 861          * payload
 862          */
 863         headersz  = sizeof(struct main_hdr_v0);
 864
 865         if (image_count_options(IMAGE_CFG_DATA) > 0) {
 866                 has_ext = 1;
 867                 headersz += sizeof(struct ext_hdr_v0);
 868         }
 869
 870         image = malloc(headersz);
 871         if (!image) {
 872                 fprintf(stderr, "Cannot allocate memory for image\n");
 873                 return NULL;
 874         }
 875
 876         memset(image, 0, headersz);
 877
 878         main_hdr = (struct main_hdr_v0 *)image;
 879
 880         /* Fill in the main header */
 881         main_hdr->blocksize =
 882                 cpu_to_le32(payloadsz - headersz);
 883         main_hdr->srcaddr   = cpu_to_le32(headersz);
 884         main_hdr->ext       = has_ext;
 885         main_hdr->destaddr  = cpu_to_le32(params->addr);
 886         main_hdr->execaddr  = cpu_to_le32(params->ep);
 887
 888         e = image_find_option(IMAGE_CFG_BOOT_FROM);
 889         if (e)
 890                 main_hdr->blockid = e->bootfrom;
 891         e = image_find_option(IMAGE_CFG_NAND_ECC_MODE);
 892         if (e)
 893                 main_hdr->nandeccmode = e->nandeccmode;
 894         e = image_find_option(IMAGE_CFG_NAND_PAGESZ);
 895         if (e)
 896                 main_hdr->nandpagesize = cpu_to_le16(e->nandpagesz);
 897         main_hdr->checksum = image_checksum8(image,
 898                                              sizeof(struct main_hdr_v0));
 899
 900         /* Generate the ext header */
 901         if (has_ext) {
 902                 struct ext_hdr_v0 *ext_hdr;
 903                 int cfgi, datai;
 904
 905                 ext_hdr = (struct ext_hdr_v0 *)
 906                                 (image + sizeof(struct main_hdr_v0));
 907                 ext_hdr->offset = cpu_to_le32(0x40);
 908
 909                 for (cfgi = 0, datai = 0; cfgi < cfgn; cfgi++) {
 910                         e = &image_cfg[cfgi];
 911                         if (e->type != IMAGE_CFG_DATA)
 912                                 continue;
 913
 914                         ext_hdr->rcfg[datai].raddr =
 915                                 cpu_to_le32(e->regdata.raddr);
 916                         ext_hdr->rcfg[datai].rdata =
 917                                 cpu_to_le32(e->regdata.rdata);
 918                         datai++;
 919                 }
 920
 921                 ext_hdr->checksum = image_checksum8(ext_hdr,
 922                                                     sizeof(struct ext_hdr_v0));
 923         }
 924
 925         *imagesz = headersz;
 926         return image;
 927 }
 928
 929 static size_t image_headersz_v1(int *hasext)
 930 {
 931         struct image_cfg_element *binarye;
 932         unsigned int count;
 933         size_t headersz;
 934         int cfgi;
 935
 936         /*
 937          * Calculate the size of the header and the size of the
 938          * payload
 939          */
 940         headersz = sizeof(struct main_hdr_v1);
 941
 942         count = image_count_options(IMAGE_CFG_DATA);
 943         if (count > 0)
 944                 headersz += sizeof(struct register_set_hdr_v1) + 8 * count + 4;
 945
 946         for (cfgi = 0; cfgi < cfgn; cfgi++) {
 947                 int ret;
 948                 struct stat s;
 949
 950                 binarye = &image_cfg[cfgi];
 951                 if (binarye->type != IMAGE_CFG_BINARY)
 952                         continue;
 953
 954                 ret = stat(binarye->binary.file, &s);
 955                 if (ret < 0) {
 956                         char cwd[PATH_MAX];
 957                         char *dir = cwd;
 958
 959                         memset(cwd, 0, sizeof(cwd));
 960                         if (!getcwd(cwd, sizeof(cwd))) {
 961                                 dir = "current working directory";
 962                                 perror("getcwd() failed");
 963                         }
 964
 965                         fprintf(stderr,
 966                                 "Didn't find the file '%s' in '%s' which is mandatory to generate the image\n"
 967                                 "This file generally contains the DDR3 training code, and should be extracted from an existing bootable\n"
 968                                 "image for your board. Use 'dumpimage -T kwbimage -p 0' to extract it from an existing image.\n",
 969                                 binarye->binary.file, dir);
 970                         return 0;
 971                 }
 972
 973                 headersz += sizeof(struct opt_hdr_v1) +
 974                         ALIGN(s.st_size, 4) +
 975                         (binarye->binary.nargs + 2) * sizeof(uint32_t);
 976                 if (hasext)
 977                         *hasext = 1;
 978         }
 979
 980         if (image_get_csk_index() >= 0) {
 981                 headersz += sizeof(struct secure_hdr_v1);
 982                 if (hasext)
 983                         *hasext = 1;
 984         }
 985
 986         /*
 987          * The payload should be aligned on some reasonable
 988          * boundary
 989          */
 990         return ALIGN(headersz, 4096);
 991 }
 992
 993 int add_binary_header_v1(uint8_t **cur, uint8_t **next_ext,
 994                          struct image_cfg_element *binarye)
 995 {
 996         struct opt_hdr_v1 *hdr = (struct opt_hdr_v1 *)*cur;
 997         uint32_t *args;
 998         size_t binhdrsz;
 999         struct stat s;
1000         int argi;
1001         FILE *bin;
1002         int ret;
1003
1004         hdr->headertype = OPT_HDR_V1_BINARY_TYPE;
1005
1006         bin = fopen(binarye->binary.file, "r");
1007         if (!bin) {
1008                 fprintf(stderr, "Cannot open binary file %s\n",
1009                         binarye->binary.file);
1010                 return -1;
1011         }
1012
1013         if (fstat(fileno(bin), &s)) {
1014                 fprintf(stderr, "Cannot stat binary file %s\n",
1015                         binarye->binary.file);
1016                 goto err_close;
1017         }
1018
1019         binhdrsz = sizeof(struct opt_hdr_v1) +
1020                 (binarye->binary.nargs + 2) * sizeof(uint32_t) +
1021                 ALIGN(s.st_size, 4);
1022         hdr->headersz_lsb = cpu_to_le16(binhdrsz & 0xFFFF);
1023         hdr->headersz_msb = (binhdrsz & 0xFFFF0000) >> 16;
1024
1025         *cur += sizeof(struct opt_hdr_v1);
1026
1027         args = (uint32_t *)*cur;
1028         *args = cpu_to_le32(binarye->binary.nargs);
1029         args++;
1030         for (argi = 0; argi < binarye->binary.nargs; argi++)
1031                 args[argi] = cpu_to_le32(binarye->binary.args[argi]);
1032
1033         *cur += (binarye->binary.nargs + 1) * sizeof(uint32_t);
1034
1035         ret = fread(*cur, s.st_size, 1, bin);
1036         if (ret != 1) {
1037                 fprintf(stderr,
1038                         "Could not read binary image %s\n",
1039                         binarye->binary.file);
1040                 goto err_close;
1041         }
1042
1043         fclose(bin);
1044
1045         *cur += ALIGN(s.st_size, 4);
1046
1047         *((uint32_t *)*cur) = 0x00000000;
1048         **next_ext = 1;
1049         *next_ext = *cur;
1050
1051         *cur += sizeof(uint32_t);
1052
1053         return 0;
1054
1055 err_close:
1056         fclose(bin);
1057
1058         return -1;
1059 }
1060
1061 int export_pub_kak_hash(RSA *kak, struct secure_hdr_v1 *secure_hdr)
1062 {
1063         FILE *hashf;
1064         int res;
1065
1066         hashf = fopen("pub_kak_hash.txt", "w");
1067         if (!hashf) {
1068                 fprintf(stderr, "Couldn't open hash file: '%s': %s\n",
1069                         "pub_kak_hash.txt", strerror(errno));
1070                 return 1;
1071         }
1072
1073         res = kwb_export_pubkey(kak, &secure_hdr->kak, hashf, "KAK");
1074
1075         fclose(hashf);
1076
1077         return res < 0 ? 1 : 0;
1078 }
1079
1080 int kwb_sign_csk_with_kak(struct image_tool_params *params,
1081                           struct secure_hdr_v1 *secure_hdr, RSA *csk)
1082 {
1083         RSA *kak = NULL;
1084         RSA *kak_pub = NULL;
1085         int csk_idx = image_get_csk_index();
1086         struct sig_v1 tmp_sig;
1087
1088         if (csk_idx < 0 || csk_idx > 15) {
1089                 fprintf(stderr, "Invalid CSK index %d\n", csk_idx);
1090                 return 1;
1091         }
1092
1093         if (kwb_load_kak(params, &kak) < 0)
1094                 return 1;
1095
1096         if (export_pub_kak_hash(kak, secure_hdr))
1097                 return 1;
1098
1099         if (kwb_import_pubkey(&kak_pub, &secure_hdr->kak, "KAK") < 0)
1100                 return 1;
1101
1102         if (kwb_export_pubkey(csk, &secure_hdr->csk[csk_idx], NULL, "CSK") < 0)
1103                 return 1;
1104
1105         if (kwb_sign_and_verify(kak, &secure_hdr->csk,
1106                                 sizeof(secure_hdr->csk) +
1107                                 sizeof(secure_hdr->csksig),
1108                                 &tmp_sig, "CSK") < 0)
1109                 return 1;
1110
1111         if (kwb_verify(kak_pub, &secure_hdr->csk,
1112                        sizeof(secure_hdr->csk) +
1113                        sizeof(secure_hdr->csksig),
1114                        &tmp_sig, "CSK (2)") < 0)
1115                 return 1;
1116
1117         secure_hdr->csksig = tmp_sig;
1118
1119         return 0;
1120 }
1121
1122 int add_secure_header_v1(struct image_tool_params *params, uint8_t *ptr,
1123                          int payloadsz, size_t headersz, uint8_t *image,
1124                          struct secure_hdr_v1 *secure_hdr)
1125 {
1126         struct image_cfg_element *e_jtagdelay;
1127         struct image_cfg_element *e_boxid;
1128         struct image_cfg_element *e_flashid;
1129         RSA *csk = NULL;
1130         unsigned char *image_ptr;
1131         size_t image_size;
1132         struct sig_v1 tmp_sig;
1133         bool specialized_img = image_get_spezialized_img();
1134
1135         kwb_msg("Create secure header content\n");
1136
1137         e_jtagdelay = image_find_option(IMAGE_CFG_JTAG_DELAY);
1138         e_boxid = image_find_option(IMAGE_CFG_BOX_ID);
1139         e_flashid = image_find_option(IMAGE_CFG_FLASH_ID);
1140
1141         if (kwb_load_csk(params, &csk) < 0)
1142                 return 1;
1143
1144         secure_hdr->headertype = OPT_HDR_V1_SECURE_TYPE;
1145         secure_hdr->headersz_msb = 0;
1146         secure_hdr->headersz_lsb = cpu_to_le16(sizeof(struct secure_hdr_v1));
1147         if (e_jtagdelay)
1148                 secure_hdr->jtag_delay = e_jtagdelay->jtag_delay;
1149         if (e_boxid && specialized_img)
1150                 secure_hdr->boxid = cpu_to_le32(e_boxid->boxid);
1151         if (e_flashid && specialized_img)
1152                 secure_hdr->flashid = cpu_to_le32(e_flashid->flashid);
1153
1154         if (kwb_sign_csk_with_kak(params, secure_hdr, csk))
1155                 return 1;
1156
1157         image_ptr = ptr + headersz;
1158         image_size = payloadsz - headersz;
1159
1160         if (kwb_sign_and_verify(csk, image_ptr, image_size,
1161                                 &secure_hdr->imgsig, "image") < 0)
1162                 return 1;
1163
1164         if (kwb_sign_and_verify(csk, image, headersz, &tmp_sig, "header") < 0)
1165                 return 1;
1166
1167         secure_hdr->hdrsig = tmp_sig;
1168
1169         kwb_dump_fuse_cmds(secure_hdr);
1170
1171         return 0;
1172 }
1173
1174 static void *image_create_v1(size_t *imagesz, struct image_tool_params *params,
1175                              uint8_t *ptr, int payloadsz)
1176 {
1177         struct image_cfg_element *e;
1178         struct main_hdr_v1 *main_hdr;
1179         struct register_set_hdr_v1 *register_set_hdr;
1180         struct secure_hdr_v1 *secure_hdr = NULL;
1181         size_t headersz;
1182         uint8_t *image, *cur;
1183         int hasext = 0;
1184         uint8_t *next_ext = NULL;
1185         int cfgi, datai, size;
1186
1187         /*
1188          * Calculate the size of the header and the size of the
1189          * payload
1190          */
1191         headersz = image_headersz_v1(&hasext);
1192         if (headersz == 0)
1193                 return NULL;
1194
1195         image = malloc(headersz);
1196         if (!image) {
1197                 fprintf(stderr, "Cannot allocate memory for image\n");
1198                 return NULL;
1199         }
1200
1201         memset(image, 0, headersz);
1202
1203         main_hdr = (struct main_hdr_v1 *)image;
1204         cur = image;
1205         cur += sizeof(struct main_hdr_v1);
1206         next_ext = &main_hdr->ext;
1207
1208         /* Fill the main header */
1209         main_hdr->blocksize    =
1210                 cpu_to_le32(payloadsz - headersz);
1211         main_hdr->headersz_lsb = cpu_to_le16(headersz & 0xFFFF);
1212         main_hdr->headersz_msb = (headersz & 0xFFFF0000) >> 16;
1213         main_hdr->destaddr     = cpu_to_le32(params->addr);
1214         main_hdr->execaddr     = cpu_to_le32(params->ep);
1215         main_hdr->srcaddr      = cpu_to_le32(headersz);
1216         main_hdr->ext          = hasext;
1217         main_hdr->version      = 1;
1218         e = image_find_option(IMAGE_CFG_BOOT_FROM);
1219         if (e)
1220                 main_hdr->blockid = e->bootfrom;
1221         e = image_find_option(IMAGE_CFG_NAND_BLKSZ);
1222         if (e)
1223                 main_hdr->nandblocksize = e->nandblksz / (64 * 1024);
1224         e = image_find_option(IMAGE_CFG_NAND_BADBLK_LOCATION);
1225         if (e)
1226                 main_hdr->nandbadblklocation = e->nandbadblklocation;
1227         e = image_find_option(IMAGE_CFG_BAUDRATE);
1228         if (e)
1229                 main_hdr->options = baudrate_to_option(e->baudrate);
1230         e = image_find_option(IMAGE_CFG_DEBUG);
1231         if (e)
1232                 main_hdr->flags = e->debug ? 0x1 : 0;
1233
1234         /*
1235          * For SATA srcaddr is specified in number of sectors starting from
1236          * sector 0. The main header is stored at sector number 1.
1237          * This expects the sector size to be 512 bytes.
1238          * Header size is already aligned.
1239          */
1240         if (main_hdr->blockid == IBR_HDR_SATA_ID)
1241                 main_hdr->srcaddr = cpu_to_le32(headersz / 512 + 1);
1242
1243         /*
1244          * For SDIO srcaddr is specified in number of sectors starting from
1245          * sector 0. The main header is stored at sector number 0.
1246          * This expects sector size to be 512 bytes.
1247          * Header size is already aligned.
1248          */
1249         if (main_hdr->blockid == IBR_HDR_SDIO_ID)
1250                 main_hdr->srcaddr = cpu_to_le32(headersz / 512);
1251
1252         /* For PCIe srcaddr is not used and must be set to 0xFFFFFFFF. */
1253         if (main_hdr->blockid == IBR_HDR_PEX_ID)
1254                 main_hdr->srcaddr = cpu_to_le32(0xFFFFFFFF);
1255
1256         if (image_get_csk_index() >= 0) {
1257                 /*
1258                  * only reserve the space here; we fill the header later since
1259                  * we need the header to be complete to compute the signatures
1260                  */
1261                 secure_hdr = (struct secure_hdr_v1 *)cur;
1262                 cur += sizeof(struct secure_hdr_v1);
1263                 *next_ext = 1;
1264                 next_ext = &secure_hdr->next;
1265         }
1266
1267         datai = 0;
1268         register_set_hdr = (struct register_set_hdr_v1 *)cur;
1269         for (cfgi = 0; cfgi < cfgn; cfgi++) {
1270                 e = &image_cfg[cfgi];
1271                 if (e->type != IMAGE_CFG_DATA &&
1272                     e->type != IMAGE_CFG_DATA_DELAY)
1273                         continue;
1274                 if (e->type == IMAGE_CFG_DATA_DELAY) {
1275                         size = sizeof(struct register_set_hdr_v1) + 8 * datai + 4;
1276                         register_set_hdr->headertype = OPT_HDR_V1_REGISTER_TYPE;
1277                         register_set_hdr->headersz_lsb = cpu_to_le16(size & 0xFFFF);
1278                         register_set_hdr->headersz_msb = size >> 16;
1279                         register_set_hdr->data[datai].last_entry.delay = e->regdata_delay;
1280                         cur += size;
1281                         *next_ext = 1;
1282                         next_ext = &register_set_hdr->data[datai].last_entry.next;
1283                         datai = 0;
1284                         continue;
1285                 }
1286                 register_set_hdr->data[datai].entry.address =
1287                         cpu_to_le32(e->regdata.raddr);
1288                 register_set_hdr->data[datai].entry.value =
1289                         cpu_to_le32(e->regdata.rdata);
1290                 datai++;
1291         }
1292         if (datai != 0) {
1293                 size = sizeof(struct register_set_hdr_v1) + 8 * datai + 4;
1294                 register_set_hdr->headertype = OPT_HDR_V1_REGISTER_TYPE;
1295                 register_set_hdr->headersz_lsb = cpu_to_le16(size & 0xFFFF);
1296                 register_set_hdr->headersz_msb = size >> 16;
1297                 /* Set delay to the smallest possible value 1ms. */
1298                 register_set_hdr->data[datai].last_entry.delay = 1;
1299                 cur += size;
1300                 *next_ext = 1;
1301                 next_ext = &register_set_hdr->data[datai].last_entry.next;
1302         }
1303
1304         for (cfgi = 0; cfgi < cfgn; cfgi++) {
1305                 e = &image_cfg[cfgi];
1306                 if (e->type != IMAGE_CFG_BINARY)
1307                         continue;
1308
1309                 if (add_binary_header_v1(&cur, &next_ext, e))
1310                         return NULL;
1311         }
1312
1313         if (secure_hdr && add_secure_header_v1(params, ptr, payloadsz,
1314                                                headersz, image, secure_hdr))
1315                 return NULL;
1316
1317         /* Calculate and set the header checksum */
1318         main_hdr->checksum = image_checksum8(main_hdr, headersz);
1319
1320         *imagesz = headersz;
1321         return image;
1322 }
1323
1324 int recognize_keyword(char *keyword)
1325 {
1326         int kw_id;
1327
1328         for (kw_id = 1; kw_id < IMAGE_CFG_COUNT; ++kw_id)
1329                 if (!strcmp(keyword, id_strs[kw_id]))
1330                         return kw_id;
1331
1332         return 0;
1333 }
1334
1335 static int image_create_config_parse_oneline(char *line,
1336                                              struct image_cfg_element *el)
1337 {
1338         char *keyword, *saveptr, *value1, *value2;
1339         char delimiters[] = " \t";
1340         int keyword_id, ret, argi;
1341         char *unknown_msg = "Ignoring unknown line '%s'\n";
1342
1343         keyword = strtok_r(line, delimiters, &saveptr);
1344         keyword_id = recognize_keyword(keyword);
1345
1346         if (!keyword_id) {
1347                 fprintf(stderr, unknown_msg, line);
1348                 return 0;
1349         }
1350
1351         el->type = keyword_id;
1352
1353         value1 = strtok_r(NULL, delimiters, &saveptr);
1354
1355         if (!value1) {
1356                 fprintf(stderr, "Parameter missing in line '%s'\n", line);
1357                 return -1;
1358         }
1359
1360         switch (keyword_id) {
1361         case IMAGE_CFG_VERSION:
1362                 el->version = atoi(value1);
1363                 break;
1364         case IMAGE_CFG_BOOT_FROM:
1365                 ret = image_boot_mode_id(value1);
1366
1367                 if (ret < 0) {
1368                         fprintf(stderr, "Invalid boot media '%s'\n", value1);
1369                         return -1;
1370                 }
1371                 el->bootfrom = ret;
1372                 break;
1373         case IMAGE_CFG_NAND_BLKSZ:
1374                 el->nandblksz = strtoul(value1, NULL, 16);
1375                 break;
1376         case IMAGE_CFG_NAND_BADBLK_LOCATION:
1377                 el->nandbadblklocation = strtoul(value1, NULL, 16);
1378                 break;
1379         case IMAGE_CFG_NAND_ECC_MODE:
1380                 ret = image_nand_ecc_mode_id(value1);
1381
1382                 if (ret < 0) {
1383                         fprintf(stderr, "Invalid NAND ECC mode '%s'\n", value1);
1384                         return -1;
1385                 }
1386                 el->nandeccmode = ret;
1387                 break;
1388         case IMAGE_CFG_NAND_PAGESZ:
1389                 el->nandpagesz = strtoul(value1, NULL, 16);
1390                 break;
1391         case IMAGE_CFG_BINARY:
1392                 argi = 0;
1393
1394                 el->binary.file = strdup(value1);
1395                 while (1) {
1396                         char *value = strtok_r(NULL, delimiters, &saveptr);
1397
1398                         if (!value)
1399                                 break;
1400                         el->binary.args[argi] = strtoul(value, NULL, 16);
1401                         argi++;
1402                         if (argi >= BINARY_MAX_ARGS) {
1403                                 fprintf(stderr,
1404                                         "Too many arguments for BINARY\n");
1405                                 return -1;
1406                         }
1407                 }
1408                 el->binary.nargs = argi;
1409                 break;
1410         case IMAGE_CFG_DATA:
1411                 value2 = strtok_r(NULL, delimiters, &saveptr);
1412
1413                 if (!value1 || !value2) {
1414                         fprintf(stderr,
1415                                 "Invalid number of arguments for DATA\n");
1416                         return -1;
1417                 }
1418
1419                 el->regdata.raddr = strtoul(value1, NULL, 16);
1420                 el->regdata.rdata = strtoul(value2, NULL, 16);
1421                 break;
1422         case IMAGE_CFG_DATA_DELAY:
1423                 if (!strcmp(value1, "SDRAM_SETUP"))
1424                         el->regdata_delay = REGISTER_SET_HDR_OPT_DELAY_SDRAM_SETUP;
1425                 else
1426                         el->regdata_delay = REGISTER_SET_HDR_OPT_DELAY_MS(strtoul(value1, NULL, 10));
1427                 break;
1428         case IMAGE_CFG_BAUDRATE:
1429                 el->baudrate = strtoul(value1, NULL, 10);
1430                 break;
1431         case IMAGE_CFG_DEBUG:
1432                 el->debug = strtoul(value1, NULL, 10);
1433                 break;
1434         case IMAGE_CFG_KAK:
1435                 el->key_name = strdup(value1);
1436                 break;
1437         case IMAGE_CFG_CSK:
1438                 el->key_name = strdup(value1);
1439                 break;
1440         case IMAGE_CFG_CSK_INDEX:
1441                 el->csk_idx = strtol(value1, NULL, 0);
1442                 break;
1443         case IMAGE_CFG_JTAG_DELAY:
1444                 el->jtag_delay = strtoul(value1, NULL, 0);
1445                 break;
1446         case IMAGE_CFG_BOX_ID:
1447                 el->boxid = strtoul(value1, NULL, 0);
1448                 break;
1449         case IMAGE_CFG_FLASH_ID:
1450                 el->flashid = strtoul(value1, NULL, 0);
1451                 break;
1452         case IMAGE_CFG_SEC_SPECIALIZED_IMG:
1453                 el->sec_specialized_img = true;
1454                 break;
1455         case IMAGE_CFG_SEC_COMMON_IMG:
1456                 el->sec_specialized_img = false;
1457                 break;
1458         case IMAGE_CFG_SEC_BOOT_DEV:
1459                 el->sec_boot_dev = strtoul(value1, NULL, 0);
1460                 break;
1461         case IMAGE_CFG_SEC_FUSE_DUMP:
1462                 el->name = strdup(value1);
1463                 break;
1464         default:
1465                 fprintf(stderr, unknown_msg, line);
1466         }
1467
1468         return 0;
1469 }
1470
1471 /*
1472  * Parse the configuration file 'fcfg' into the array of configuration
1473  * elements 'image_cfg', and return the number of configuration
1474  * elements in 'cfgn'.
1475  */
1476 static int image_create_config_parse(FILE *fcfg)
1477 {
1478         int ret;
1479         int cfgi = 0;
1480
1481         /* Parse the configuration file */
1482         while (!feof(fcfg)) {
1483                 char *line;
1484                 char buf[256];
1485
1486                 /* Read the current line */
1487                 memset(buf, 0, sizeof(buf));
1488                 line = fgets(buf, sizeof(buf), fcfg);
1489                 if (!line)
1490                         break;
1491
1492                 /* Ignore useless lines */
1493                 if (line[0] == '\n' || line[0] == '#')
1494                         continue;
1495
1496                 /* Strip final newline */
1497                 if (line[strlen(line) - 1] == '\n')
1498                         line[strlen(line) - 1] = 0;
1499
1500                 /* Parse the current line */
1501                 ret = image_create_config_parse_oneline(line,
1502                                                         &image_cfg[cfgi]);
1503                 if (ret)
1504                         return ret;
1505
1506                 cfgi++;
1507
1508                 if (cfgi >= IMAGE_CFG_ELEMENT_MAX) {
1509                         fprintf(stderr,
1510                                 "Too many configuration elements in .cfg file\n");
1511                         return -1;
1512                 }
1513         }
1514
1515         cfgn = cfgi;
1516         return 0;
1517 }
1518
1519 static int image_get_version(void)
1520 {
1521         struct image_cfg_element *e;
1522
1523         e = image_find_option(IMAGE_CFG_VERSION);
1524         if (!e)
1525                 return -1;
1526
1527         return e->version;
1528 }
1529
1530 static int image_get_bootfrom(void)
1531 {
1532         struct image_cfg_element *e;
1533
1534         e = image_find_option(IMAGE_CFG_BOOT_FROM);
1535         if (!e)
1536                 return -1;
1537
1538         return e->bootfrom;
1539 }
1540
1541 static void kwbimage_set_header(void *ptr, struct stat *sbuf, int ifd,
1542                                 struct image_tool_params *params)
1543 {
1544         FILE *fcfg;
1545         void *image = NULL;
1546         int version;
1547         size_t headersz = 0;
1548         uint32_t checksum;
1549         int ret;
1550
1551         fcfg = fopen(params->imagename, "r");
1552         if (!fcfg) {
1553                 fprintf(stderr, "Could not open input file %s\n",
1554                         params->imagename);
1555                 exit(EXIT_FAILURE);
1556         }
1557
1558         image_cfg = malloc(IMAGE_CFG_ELEMENT_MAX *
1559                            sizeof(struct image_cfg_element));
1560         if (!image_cfg) {
1561                 fprintf(stderr, "Cannot allocate memory\n");
1562                 fclose(fcfg);
1563                 exit(EXIT_FAILURE);
1564         }
1565
1566         memset(image_cfg, 0,
1567                IMAGE_CFG_ELEMENT_MAX * sizeof(struct image_cfg_element));
1568         rewind(fcfg);
1569
1570         ret = image_create_config_parse(fcfg);
1571         fclose(fcfg);
1572         if (ret) {
1573                 free(image_cfg);
1574                 exit(EXIT_FAILURE);
1575         }
1576
1577         version = image_get_version();
1578         switch (version) {
1579                 /*
1580                  * Fallback to version 0 if no version is provided in the
1581                  * cfg file
1582                  */
1583         case -1:
1584         case 0:
1585                 image = image_create_v0(&headersz, params, sbuf->st_size);
1586                 break;
1587
1588         case 1:
1589                 image = image_create_v1(&headersz, params, ptr, sbuf->st_size);
1590                 break;
1591
1592         default:
1593                 fprintf(stderr, "Unsupported version %d\n", version);
1594                 free(image_cfg);
1595                 exit(EXIT_FAILURE);
1596         }
1597
1598         if (!image) {
1599                 fprintf(stderr, "Could not create image\n");
1600                 free(image_cfg);
1601                 exit(EXIT_FAILURE);
1602         }
1603
1604         free(image_cfg);
1605
1606         /* Build and add image checksum header */
1607         checksum = cpu_to_le32(image_checksum32((uint8_t *)ptr + headersz,
1608                                 sbuf->st_size - headersz - sizeof(uint32_t)));
1609         memcpy((uint8_t *)ptr + sbuf->st_size - sizeof(uint32_t), &checksum,
1610                 sizeof(uint32_t));
1611
1612         /* Finally copy the header into the image area */
1613         memcpy(ptr, image, headersz);
1614
1615         free(image);
1616 }
1617
1618 static void kwbimage_print_header(const void *ptr)
1619 {
1620         struct main_hdr_v0 *mhdr = (struct main_hdr_v0 *)ptr;
1621
1622         printf("Image Type:   MVEBU Boot from %s Image\n",
1623                image_boot_mode_name(mhdr->blockid));
1624         printf("Image version:%d\n", image_version((void *)ptr));
1625         if (image_version((void *)ptr) == 1) {
1626                 struct main_hdr_v1 *mhdr = (struct main_hdr_v1 *)ptr;
1627
1628                 if (mhdr->ext & 0x1) {
1629                         struct opt_hdr_v1 *ohdr = (struct opt_hdr_v1 *)
1630                                                   ((uint8_t *)ptr +
1631                                                    sizeof(*mhdr));
1632
1633                         while (1) {
1634                                 uint32_t ohdr_size;
1635
1636                                 ohdr_size = (ohdr->headersz_msb << 16) |
1637                                             le16_to_cpu(ohdr->headersz_lsb);
1638                                 if (ohdr->headertype == OPT_HDR_V1_BINARY_TYPE) {
1639                                         printf("BIN Hdr Size: ");
1640                                         genimg_print_size(ohdr_size - 12 - 4 * ohdr->data[0]);
1641                                 }
1642                                 if (!(*((uint8_t *)ohdr + ohdr_size - 4) & 0x1))
1643                                         break;
1644                                 ohdr = (struct opt_hdr_v1 *)((uint8_t *)ohdr +
1645                                                              ohdr_size);
1646                         }
1647                 }
1648         }
1649         printf("Data Size:    ");
1650         genimg_print_size(mhdr->blocksize - sizeof(uint32_t));
1651         printf("Load Address: %08x\n", mhdr->destaddr);
1652         printf("Entry Point:  %08x\n", mhdr->execaddr);
1653 }
1654
1655 static int kwbimage_check_image_types(uint8_t type)
1656 {
1657         if (type == IH_TYPE_KWBIMAGE)
1658                 return EXIT_SUCCESS;
1659
1660         return EXIT_FAILURE;
1661 }
1662
1663 static int kwbimage_verify_header(unsigned char *ptr, int image_size,
1664                                   struct image_tool_params *params)
1665 {
1666         uint8_t checksum;
1667         size_t header_size = kwbimage_header_size(ptr);
1668
1669         if (header_size > image_size)
1670                 return -FDT_ERR_BADSTRUCTURE;
1671
1672         if (!main_hdr_checksum_ok(ptr))
1673                 return -FDT_ERR_BADSTRUCTURE;
1674
1675         /* Only version 0 extended header has checksum */
1676         if (image_version((void *)ptr) == 0) {
1677                 struct main_hdr_v0 *mhdr = (struct main_hdr_v0 *)ptr;
1678
1679                 if (mhdr->ext & 0x1) {
1680                         struct ext_hdr_v0 *ext_hdr;
1681
1682                         if (header_size + sizeof(*ext_hdr) > image_size)
1683                                 return -FDT_ERR_BADSTRUCTURE;
1684
1685                         ext_hdr = (struct ext_hdr_v0 *)
1686                                 (ptr + sizeof(struct main_hdr_v0));
1687                         checksum = image_checksum8(ext_hdr,
1688                                                    sizeof(struct ext_hdr_v0)
1689                                                    - sizeof(uint8_t));
1690                         if (checksum != ext_hdr->checksum)
1691                                 return -FDT_ERR_BADSTRUCTURE;
1692                 }
1693         } else if (image_version((void *)ptr) == 1) {
1694                 struct main_hdr_v1 *mhdr = (struct main_hdr_v1 *)ptr;
1695                 uint32_t offset;
1696                 uint32_t size;
1697
1698                 if (mhdr->ext & 0x1) {
1699                         uint32_t ohdr_size;
1700                         struct opt_hdr_v1 *ohdr = (struct opt_hdr_v1 *)
1701                                                   (ptr + sizeof(*mhdr));
1702
1703                         while (1) {
1704                                 if ((uint8_t *)ohdr + sizeof(*ohdr) >
1705                                     (uint8_t *)mhdr + header_size)
1706                                         return -FDT_ERR_BADSTRUCTURE;
1707
1708                                 ohdr_size = (ohdr->headersz_msb << 16) |
1709                                             le16_to_cpu(ohdr->headersz_lsb);
1710
1711                                 if (ohdr_size < 8 ||
1712                                     (uint8_t *)ohdr + ohdr_size >
1713                                     (uint8_t *)mhdr + header_size)
1714                                         return -FDT_ERR_BADSTRUCTURE;
1715
1716                                 if (!(*((uint8_t *)ohdr + ohdr_size - 4) & 0x1))
1717                                         break;
1718                                 ohdr = (struct opt_hdr_v1 *)((uint8_t *)ohdr +
1719                                                              ohdr_size);
1720                         }
1721                 }
1722
1723                 offset = le32_to_cpu(mhdr->srcaddr);
1724
1725                 /*
1726                  * For SATA srcaddr is specified in number of sectors.
1727                  * The main header is must be stored at sector number 1.
1728                  * This expects that sector size is 512 bytes and recalculates
1729                  * data offset to bytes relative to the main header.
1730                  */
1731                 if (mhdr->blockid == IBR_HDR_SATA_ID) {
1732                         if (offset < 1)
1733                                 return -FDT_ERR_BADSTRUCTURE;
1734                         offset -= 1;
1735                         offset *= 512;
1736                 }
1737
1738                 /*
1739                  * For SDIO srcaddr is specified in number of sectors.
1740                  * This expects that sector size is 512 bytes and recalculates
1741                  * data offset to bytes.
1742                  */
1743                 if (mhdr->blockid == IBR_HDR_SDIO_ID)
1744                         offset *= 512;
1745
1746                 /*
1747                  * For PCIe srcaddr is always set to 0xFFFFFFFF.
1748                  * This expects that data starts after all headers.
1749                  */
1750                 if (mhdr->blockid == IBR_HDR_PEX_ID && offset == 0xFFFFFFFF)
1751                         offset = header_size;
1752
1753                 if (offset > image_size || offset % 4 != 0)
1754                         return -FDT_ERR_BADSTRUCTURE;
1755
1756                 size = le32_to_cpu(mhdr->blocksize);
1757                 if (size < 4 || offset + size > image_size || size % 4 != 0)
1758                         return -FDT_ERR_BADSTRUCTURE;
1759
1760                 if (image_checksum32(ptr + offset, size - 4) !=
1761                     *(uint32_t *)(ptr + offset + size - 4))
1762                         return -FDT_ERR_BADSTRUCTURE;
1763         } else {
1764                 return -FDT_ERR_BADSTRUCTURE;
1765         }
1766
1767         return 0;
1768 }
1769
1770 static int kwbimage_generate(struct image_tool_params *params,
1771                              struct image_type_params *tparams)
1772 {
1773         FILE *fcfg;
1774         struct stat s;
1775         int alloc_len;
1776         int bootfrom;
1777         int version;
1778         void *hdr;
1779         int ret;
1780
1781         fcfg = fopen(params->imagename, "r");
1782         if (!fcfg) {
1783                 fprintf(stderr, "Could not open input file %s\n",
1784                         params->imagename);
1785                 exit(EXIT_FAILURE);
1786         }
1787
1788         if (stat(params->datafile, &s)) {
1789                 fprintf(stderr, "Could not stat data file %s: %s\n",
1790                         params->datafile, strerror(errno));
1791                 exit(EXIT_FAILURE);
1792         }
1793
1794         image_cfg = malloc(IMAGE_CFG_ELEMENT_MAX *
1795                            sizeof(struct image_cfg_element));
1796         if (!image_cfg) {
1797                 fprintf(stderr, "Cannot allocate memory\n");
1798                 fclose(fcfg);
1799                 exit(EXIT_FAILURE);
1800         }
1801
1802         memset(image_cfg, 0,
1803                IMAGE_CFG_ELEMENT_MAX * sizeof(struct image_cfg_element));
1804         rewind(fcfg);
1805
1806         ret = image_create_config_parse(fcfg);
1807         fclose(fcfg);
1808         if (ret) {
1809                 free(image_cfg);
1810                 exit(EXIT_FAILURE);
1811         }
1812
1813         bootfrom = image_get_bootfrom();
1814         version = image_get_version();
1815         switch (version) {
1816                 /*
1817                  * Fallback to version 0 if no version is provided in the
1818                  * cfg file
1819                  */
1820         case -1:
1821         case 0:
1822                 alloc_len = sizeof(struct main_hdr_v0) +
1823                         sizeof(struct ext_hdr_v0);
1824                 break;
1825
1826         case 1:
1827                 alloc_len = image_headersz_v1(NULL);
1828                 break;
1829
1830         default:
1831                 fprintf(stderr, "Unsupported version %d\n", version);
1832                 free(image_cfg);
1833                 exit(EXIT_FAILURE);
1834         }
1835
1836         free(image_cfg);
1837
1838         hdr = malloc(alloc_len);
1839         if (!hdr) {
1840                 fprintf(stderr, "%s: malloc return failure: %s\n",
1841                         params->cmdname, strerror(errno));
1842                 exit(EXIT_FAILURE);
1843         }
1844
1845         memset(hdr, 0, alloc_len);
1846         tparams->header_size = alloc_len;
1847         tparams->hdr = hdr;
1848
1849         /*
1850          * The resulting image needs to be 4-byte aligned. At least
1851          * the Marvell hdrparser tool complains if its unaligned.
1852          * After the image data is stored 4-byte checksum.
1853          * Final SPI and NAND images must be aligned to 256 bytes.
1854          * Final SATA and SDIO images must be aligned to 512 bytes.
1855          */
1856         if (bootfrom == IBR_HDR_SPI_ID || bootfrom == IBR_HDR_NAND_ID)
1857                 return 4 + (256 - (alloc_len + s.st_size + 4) % 256) % 256;
1858         else if (bootfrom == IBR_HDR_SATA_ID || bootfrom == IBR_HDR_SDIO_ID)
1859                 return 4 + (512 - (alloc_len + s.st_size + 4) % 512) % 512;
1860         else
1861                 return 4 + (4 - s.st_size % 4) % 4;
1862 }
1863
1864 static int kwbimage_extract_subimage(void *ptr, struct image_tool_params *params)
1865 {
1866         struct main_hdr_v1 *mhdr = (struct main_hdr_v1 *)ptr;
1867         size_t header_size = kwbimage_header_size(ptr);
1868         int idx = params->pflag;
1869         int cur_idx = 0;
1870         uint32_t offset;
1871         ulong image;
1872         ulong size;
1873
1874         if (image_version((void *)ptr) == 1 && (mhdr->ext & 0x1)) {
1875                 struct opt_hdr_v1 *ohdr = (struct opt_hdr_v1 *)
1876                                           ((uint8_t *)ptr +
1877                                            sizeof(*mhdr));
1878
1879                 while (1) {
1880                         uint32_t ohdr_size = (ohdr->headersz_msb << 16) |
1881                                              le16_to_cpu(ohdr->headersz_lsb);
1882
1883                         if (ohdr->headertype == OPT_HDR_V1_BINARY_TYPE) {
1884                                 if (idx == cur_idx) {
1885                                         image = (ulong)&ohdr->data[4 +
1886                                                  4 * ohdr->data[0]];
1887                                         size = ohdr_size - 12 -
1888                                                4 * ohdr->data[0];
1889                                         goto extract;
1890                                 }
1891                                 ++cur_idx;
1892                         }
1893                         if (!(*((uint8_t *)ohdr + ohdr_size - 4) & 0x1))
1894                                 break;
1895                         ohdr = (struct opt_hdr_v1 *)((uint8_t *)ohdr +
1896                                                      ohdr_size);
1897                 }
1898         }
1899
1900         if (idx != cur_idx) {
1901                 printf("Image %d is not present\n", idx);
1902                 return -1;
1903         }
1904
1905         offset = le32_to_cpu(mhdr->srcaddr);
1906
1907         if (mhdr->blockid == IBR_HDR_SATA_ID) {
1908                 offset -= 1;
1909                 offset *= 512;
1910         }
1911
1912         if (mhdr->blockid == IBR_HDR_SDIO_ID)
1913                 offset *= 512;
1914
1915         if (mhdr->blockid == IBR_HDR_PEX_ID && offset == 0xFFFFFFFF)
1916                 offset = header_size;
1917
1918         image = (ulong)((uint8_t *)ptr + offset);
1919         size = le32_to_cpu(mhdr->blocksize) - 4;
1920
1921 extract:
1922         return imagetool_save_subimage(params->outfile, image, size);
1923 }
1924
1925 /*
1926  * Report Error if xflag is set in addition to default
1927  */
1928 static int kwbimage_check_params(struct image_tool_params *params)
1929 {
1930         if (!params->iflag && (!params->imagename || !strlen(params->imagename))) {
1931                 char *msg = "Configuration file for kwbimage creation omitted";
1932
1933                 fprintf(stderr, "Error:%s - %s\n", params->cmdname, msg);
1934                 return CFG_INVALID;
1935         }
1936
1937         return (params->dflag && (params->fflag || params->lflag)) ||
1938                 (params->fflag && (params->dflag || params->lflag)) ||
1939                 (params->lflag && (params->dflag || params->fflag)) ||
1940                 (params->xflag);
1941 }
1942
1943 /*
1944  * kwbimage type parameters definition
1945  */
1946 U_BOOT_IMAGE_TYPE(
1947         kwbimage,
1948         "Marvell MVEBU Boot Image support",
1949         0,
1950         NULL,
1951         kwbimage_check_params,
1952         kwbimage_verify_header,
1953         kwbimage_print_header,
1954         kwbimage_set_header,
1955         kwbimage_extract_subimage,
1956         kwbimage_check_image_types,
1957         NULL,
1958         kwbimage_generate
1959 );