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