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         if (image_get_csk_index() >= 0) {
 936                 headersz += sizeof(struct secure_hdr_v1);
 937                 if (hasext)
 938                         *hasext = 1;
 939         }
 940
 941         count = image_count_options(IMAGE_CFG_DATA);
 942         if (count > 0)
 943                 headersz += sizeof(struct register_set_hdr_v1) + 8 * count + 4;
 944
 945         for (cfgi = 0; cfgi < cfgn; cfgi++) {
 946                 int ret;
 947                 struct stat s;
 948
 949                 binarye = &image_cfg[cfgi];
 950                 if (binarye->type != IMAGE_CFG_BINARY)
 951                         continue;
 952
 953                 ret = stat(binarye->binary.file, &s);
 954                 if (ret < 0) {
 955                         char cwd[PATH_MAX];
 956                         char *dir = cwd;
 957
 958                         memset(cwd, 0, sizeof(cwd));
 959                         if (!getcwd(cwd, sizeof(cwd))) {
 960                                 dir = "current working directory";
 961                                 perror("getcwd() failed");
 962                         }
 963
 964                         fprintf(stderr,
 965                                 "Didn't find the file '%s' in '%s' which is mandatory to generate the image\n"
 966                                 "This file generally contains the DDR3 training code, and should be extracted from an existing bootable\n"
 967                                 "image for your board. Use 'dumpimage -T kwbimage -p 0' to extract it from an existing image.\n",
 968                                 binarye->binary.file, dir);
 969                         return 0;
 970                 }
 971
 972                 headersz += sizeof(struct opt_hdr_v1) + sizeof(uint32_t) +
 973                         (binarye->binary.nargs) * sizeof(uint32_t);
 974                 headersz = ALIGN(headersz, 16);
 975                 headersz += ALIGN(s.st_size, 4) + sizeof(uint32_t);
 976                 if (hasext)
 977                         *hasext = 1;
 978         }
 979
 980         /*
 981          * The payload should be aligned on some reasonable
 982          * boundary
 983          */
 984         return ALIGN(headersz, 4096);
 985 }
 986
 987 int add_binary_header_v1(uint8_t **cur, uint8_t **next_ext,
 988                          struct image_cfg_element *binarye,
 989                          struct main_hdr_v1 *main_hdr)
 990 {
 991         struct opt_hdr_v1 *hdr = (struct opt_hdr_v1 *)*cur;
 992         uint32_t add_args;
 993         uint32_t offset;
 994         uint32_t *args;
 995         size_t binhdrsz;
 996         struct stat s;
 997         int argi;
 998         FILE *bin;
 999         int ret;
1000
1001         hdr->headertype = OPT_HDR_V1_BINARY_TYPE;
1002
1003         bin = fopen(binarye->binary.file, "r");
1004         if (!bin) {
1005                 fprintf(stderr, "Cannot open binary file %s\n",
1006                         binarye->binary.file);
1007                 return -1;
1008         }
1009
1010         if (fstat(fileno(bin), &s)) {
1011                 fprintf(stderr, "Cannot stat binary file %s\n",
1012                         binarye->binary.file);
1013                 goto err_close;
1014         }
1015
1016         *cur += sizeof(struct opt_hdr_v1);
1017
1018         args = (uint32_t *)*cur;
1019         *args = cpu_to_le32(binarye->binary.nargs);
1020         args++;
1021         for (argi = 0; argi < binarye->binary.nargs; argi++)
1022                 args[argi] = cpu_to_le32(binarye->binary.args[argi]);
1023
1024         *cur += (binarye->binary.nargs + 1) * sizeof(uint32_t);
1025
1026         /*
1027          * ARM executable code inside the BIN header on some mvebu platforms
1028          * (e.g. A370, AXP) must always be aligned with the 128-bit boundary.
1029          * This requirement can be met by inserting dummy arguments into
1030          * BIN header, if needed.
1031          */
1032         offset = *cur - (uint8_t *)main_hdr;
1033         add_args = ((16 - offset % 16) % 16) / sizeof(uint32_t);
1034         if (add_args) {
1035                 *(args - 1) = cpu_to_le32(binarye->binary.nargs + add_args);
1036                 *cur += add_args * sizeof(uint32_t);
1037         }
1038
1039         ret = fread(*cur, s.st_size, 1, bin);
1040         if (ret != 1) {
1041                 fprintf(stderr,
1042                         "Could not read binary image %s\n",
1043                         binarye->binary.file);
1044                 goto err_close;
1045         }
1046
1047         fclose(bin);
1048
1049         *cur += ALIGN(s.st_size, 4);
1050
1051         *((uint32_t *)*cur) = 0x00000000;
1052         **next_ext = 1;
1053         *next_ext = *cur;
1054
1055         *cur += sizeof(uint32_t);
1056
1057         binhdrsz = sizeof(struct opt_hdr_v1) +
1058                 (binarye->binary.nargs + add_args + 2) * sizeof(uint32_t) +
1059                 ALIGN(s.st_size, 4);
1060         hdr->headersz_lsb = cpu_to_le16(binhdrsz & 0xFFFF);
1061         hdr->headersz_msb = (binhdrsz & 0xFFFF0000) >> 16;
1062
1063         return 0;
1064
1065 err_close:
1066         fclose(bin);
1067
1068         return -1;
1069 }
1070
1071 int export_pub_kak_hash(RSA *kak, struct secure_hdr_v1 *secure_hdr)
1072 {
1073         FILE *hashf;
1074         int res;
1075
1076         hashf = fopen("pub_kak_hash.txt", "w");
1077         if (!hashf) {
1078                 fprintf(stderr, "Couldn't open hash file: '%s': %s\n",
1079                         "pub_kak_hash.txt", strerror(errno));
1080                 return 1;
1081         }
1082
1083         res = kwb_export_pubkey(kak, &secure_hdr->kak, hashf, "KAK");
1084
1085         fclose(hashf);
1086
1087         return res < 0 ? 1 : 0;
1088 }
1089
1090 int kwb_sign_csk_with_kak(struct image_tool_params *params,
1091                           struct secure_hdr_v1 *secure_hdr, RSA *csk)
1092 {
1093         RSA *kak = NULL;
1094         RSA *kak_pub = NULL;
1095         int csk_idx = image_get_csk_index();
1096         struct sig_v1 tmp_sig;
1097
1098         if (csk_idx < 0 || csk_idx > 15) {
1099                 fprintf(stderr, "Invalid CSK index %d\n", csk_idx);
1100                 return 1;
1101         }
1102
1103         if (kwb_load_kak(params, &kak) < 0)
1104                 return 1;
1105
1106         if (export_pub_kak_hash(kak, secure_hdr))
1107                 return 1;
1108
1109         if (kwb_import_pubkey(&kak_pub, &secure_hdr->kak, "KAK") < 0)
1110                 return 1;
1111
1112         if (kwb_export_pubkey(csk, &secure_hdr->csk[csk_idx], NULL, "CSK") < 0)
1113                 return 1;
1114
1115         if (kwb_sign_and_verify(kak, &secure_hdr->csk,
1116                                 sizeof(secure_hdr->csk) +
1117                                 sizeof(secure_hdr->csksig),
1118                                 &tmp_sig, "CSK") < 0)
1119                 return 1;
1120
1121         if (kwb_verify(kak_pub, &secure_hdr->csk,
1122                        sizeof(secure_hdr->csk) +
1123                        sizeof(secure_hdr->csksig),
1124                        &tmp_sig, "CSK (2)") < 0)
1125                 return 1;
1126
1127         secure_hdr->csksig = tmp_sig;
1128
1129         return 0;
1130 }
1131
1132 int add_secure_header_v1(struct image_tool_params *params, uint8_t *ptr,
1133                          int payloadsz, size_t headersz, uint8_t *image,
1134                          struct secure_hdr_v1 *secure_hdr)
1135 {
1136         struct image_cfg_element *e_jtagdelay;
1137         struct image_cfg_element *e_boxid;
1138         struct image_cfg_element *e_flashid;
1139         RSA *csk = NULL;
1140         unsigned char *image_ptr;
1141         size_t image_size;
1142         struct sig_v1 tmp_sig;
1143         bool specialized_img = image_get_spezialized_img();
1144
1145         kwb_msg("Create secure header content\n");
1146
1147         e_jtagdelay = image_find_option(IMAGE_CFG_JTAG_DELAY);
1148         e_boxid = image_find_option(IMAGE_CFG_BOX_ID);
1149         e_flashid = image_find_option(IMAGE_CFG_FLASH_ID);
1150
1151         if (kwb_load_csk(params, &csk) < 0)
1152                 return 1;
1153
1154         secure_hdr->headertype = OPT_HDR_V1_SECURE_TYPE;
1155         secure_hdr->headersz_msb = 0;
1156         secure_hdr->headersz_lsb = cpu_to_le16(sizeof(struct secure_hdr_v1));
1157         if (e_jtagdelay)
1158                 secure_hdr->jtag_delay = e_jtagdelay->jtag_delay;
1159         if (e_boxid && specialized_img)
1160                 secure_hdr->boxid = cpu_to_le32(e_boxid->boxid);
1161         if (e_flashid && specialized_img)
1162                 secure_hdr->flashid = cpu_to_le32(e_flashid->flashid);
1163
1164         if (kwb_sign_csk_with_kak(params, secure_hdr, csk))
1165                 return 1;
1166
1167         image_ptr = ptr + headersz;
1168         image_size = payloadsz - headersz;
1169
1170         if (kwb_sign_and_verify(csk, image_ptr, image_size,
1171                                 &secure_hdr->imgsig, "image") < 0)
1172                 return 1;
1173
1174         if (kwb_sign_and_verify(csk, image, headersz, &tmp_sig, "header") < 0)
1175                 return 1;
1176
1177         secure_hdr->hdrsig = tmp_sig;
1178
1179         kwb_dump_fuse_cmds(secure_hdr);
1180
1181         return 0;
1182 }
1183
1184 static void *image_create_v1(size_t *imagesz, struct image_tool_params *params,
1185                              uint8_t *ptr, int payloadsz)
1186 {
1187         struct image_cfg_element *e;
1188         struct main_hdr_v1 *main_hdr;
1189         struct register_set_hdr_v1 *register_set_hdr;
1190         struct secure_hdr_v1 *secure_hdr = NULL;
1191         size_t headersz;
1192         uint8_t *image, *cur;
1193         int hasext = 0;
1194         uint8_t *next_ext = NULL;
1195         int cfgi, datai, size;
1196
1197         /*
1198          * Calculate the size of the header and the size of the
1199          * payload
1200          */
1201         headersz = image_headersz_v1(&hasext);
1202         if (headersz == 0)
1203                 return NULL;
1204
1205         image = malloc(headersz);
1206         if (!image) {
1207                 fprintf(stderr, "Cannot allocate memory for image\n");
1208                 return NULL;
1209         }
1210
1211         memset(image, 0, headersz);
1212
1213         main_hdr = (struct main_hdr_v1 *)image;
1214         cur = image;
1215         cur += sizeof(struct main_hdr_v1);
1216         next_ext = &main_hdr->ext;
1217
1218         /* Fill the main header */
1219         main_hdr->blocksize    =
1220                 cpu_to_le32(payloadsz - headersz);
1221         main_hdr->headersz_lsb = cpu_to_le16(headersz & 0xFFFF);
1222         main_hdr->headersz_msb = (headersz & 0xFFFF0000) >> 16;
1223         main_hdr->destaddr     = cpu_to_le32(params->addr);
1224         main_hdr->execaddr     = cpu_to_le32(params->ep);
1225         main_hdr->srcaddr      = cpu_to_le32(headersz);
1226         main_hdr->ext          = hasext;
1227         main_hdr->version      = 1;
1228         e = image_find_option(IMAGE_CFG_BOOT_FROM);
1229         if (e)
1230                 main_hdr->blockid = e->bootfrom;
1231         e = image_find_option(IMAGE_CFG_NAND_BLKSZ);
1232         if (e)
1233                 main_hdr->nandblocksize = e->nandblksz / (64 * 1024);
1234         e = image_find_option(IMAGE_CFG_NAND_PAGESZ);
1235         if (e)
1236                 main_hdr->nandpagesize = cpu_to_le16(e->nandpagesz);
1237         e = image_find_option(IMAGE_CFG_NAND_BADBLK_LOCATION);
1238         if (e)
1239                 main_hdr->nandbadblklocation = e->nandbadblklocation;
1240         e = image_find_option(IMAGE_CFG_BAUDRATE);
1241         if (e)
1242                 main_hdr->options = baudrate_to_option(e->baudrate);
1243         e = image_find_option(IMAGE_CFG_DEBUG);
1244         if (e)
1245                 main_hdr->flags = e->debug ? 0x1 : 0;
1246
1247         /*
1248          * For SATA srcaddr is specified in number of sectors starting from
1249          * sector 0. The main header is stored at sector number 1.
1250          * This expects the sector size to be 512 bytes.
1251          * Header size is already aligned.
1252          */
1253         if (main_hdr->blockid == IBR_HDR_SATA_ID)
1254                 main_hdr->srcaddr = cpu_to_le32(headersz / 512 + 1);
1255
1256         /*
1257          * For SDIO srcaddr is specified in number of sectors starting from
1258          * sector 0. The main header is stored at sector number 0.
1259          * This expects sector size to be 512 bytes.
1260          * Header size is already aligned.
1261          */
1262         if (main_hdr->blockid == IBR_HDR_SDIO_ID)
1263                 main_hdr->srcaddr = cpu_to_le32(headersz / 512);
1264
1265         /* For PCIe srcaddr is not used and must be set to 0xFFFFFFFF. */
1266         if (main_hdr->blockid == IBR_HDR_PEX_ID)
1267                 main_hdr->srcaddr = cpu_to_le32(0xFFFFFFFF);
1268
1269         if (image_get_csk_index() >= 0) {
1270                 /*
1271                  * only reserve the space here; we fill the header later since
1272                  * we need the header to be complete to compute the signatures
1273                  */
1274                 secure_hdr = (struct secure_hdr_v1 *)cur;
1275                 cur += sizeof(struct secure_hdr_v1);
1276                 *next_ext = 1;
1277                 next_ext = &secure_hdr->next;
1278         }
1279
1280         datai = 0;
1281         register_set_hdr = (struct register_set_hdr_v1 *)cur;
1282         for (cfgi = 0; cfgi < cfgn; cfgi++) {
1283                 e = &image_cfg[cfgi];
1284                 if (e->type != IMAGE_CFG_DATA &&
1285                     e->type != IMAGE_CFG_DATA_DELAY)
1286                         continue;
1287                 if (e->type == IMAGE_CFG_DATA_DELAY) {
1288                         size = sizeof(struct register_set_hdr_v1) + 8 * datai + 4;
1289                         register_set_hdr->headertype = OPT_HDR_V1_REGISTER_TYPE;
1290                         register_set_hdr->headersz_lsb = cpu_to_le16(size & 0xFFFF);
1291                         register_set_hdr->headersz_msb = size >> 16;
1292                         register_set_hdr->data[datai].last_entry.delay = e->regdata_delay;
1293                         cur += size;
1294                         *next_ext = 1;
1295                         next_ext = &register_set_hdr->data[datai].last_entry.next;
1296                         datai = 0;
1297                         continue;
1298                 }
1299                 register_set_hdr->data[datai].entry.address =
1300                         cpu_to_le32(e->regdata.raddr);
1301                 register_set_hdr->data[datai].entry.value =
1302                         cpu_to_le32(e->regdata.rdata);
1303                 datai++;
1304         }
1305         if (datai != 0) {
1306                 size = sizeof(struct register_set_hdr_v1) + 8 * datai + 4;
1307                 register_set_hdr->headertype = OPT_HDR_V1_REGISTER_TYPE;
1308                 register_set_hdr->headersz_lsb = cpu_to_le16(size & 0xFFFF);
1309                 register_set_hdr->headersz_msb = size >> 16;
1310                 /* Set delay to the smallest possible value 1ms. */
1311                 register_set_hdr->data[datai].last_entry.delay = 1;
1312                 cur += size;
1313                 *next_ext = 1;
1314                 next_ext = &register_set_hdr->data[datai].last_entry.next;
1315         }
1316
1317         for (cfgi = 0; cfgi < cfgn; cfgi++) {
1318                 e = &image_cfg[cfgi];
1319                 if (e->type != IMAGE_CFG_BINARY)
1320                         continue;
1321
1322                 if (add_binary_header_v1(&cur, &next_ext, e, main_hdr))
1323                         return NULL;
1324         }
1325
1326         if (secure_hdr && add_secure_header_v1(params, ptr, payloadsz,
1327                                                headersz, image, secure_hdr))
1328                 return NULL;
1329
1330         /* Calculate and set the header checksum */
1331         main_hdr->checksum = image_checksum8(main_hdr, headersz);
1332
1333         *imagesz = headersz;
1334         return image;
1335 }
1336
1337 int recognize_keyword(char *keyword)
1338 {
1339         int kw_id;
1340
1341         for (kw_id = 1; kw_id < IMAGE_CFG_COUNT; ++kw_id)
1342                 if (!strcmp(keyword, id_strs[kw_id]))
1343                         return kw_id;
1344
1345         return 0;
1346 }
1347
1348 static int image_create_config_parse_oneline(char *line,
1349                                              struct image_cfg_element *el)
1350 {
1351         char *keyword, *saveptr, *value1, *value2;
1352         char delimiters[] = " \t";
1353         int keyword_id, ret, argi;
1354         char *unknown_msg = "Ignoring unknown line '%s'\n";
1355
1356         keyword = strtok_r(line, delimiters, &saveptr);
1357         keyword_id = recognize_keyword(keyword);
1358
1359         if (!keyword_id) {
1360                 fprintf(stderr, unknown_msg, line);
1361                 return 0;
1362         }
1363
1364         el->type = keyword_id;
1365
1366         value1 = strtok_r(NULL, delimiters, &saveptr);
1367
1368         if (!value1) {
1369                 fprintf(stderr, "Parameter missing in line '%s'\n", line);
1370                 return -1;
1371         }
1372
1373         switch (keyword_id) {
1374         case IMAGE_CFG_VERSION:
1375                 el->version = atoi(value1);
1376                 break;
1377         case IMAGE_CFG_BOOT_FROM:
1378                 ret = image_boot_mode_id(value1);
1379
1380                 if (ret < 0) {
1381                         fprintf(stderr, "Invalid boot media '%s'\n", value1);
1382                         return -1;
1383                 }
1384                 el->bootfrom = ret;
1385                 break;
1386         case IMAGE_CFG_NAND_BLKSZ:
1387                 el->nandblksz = strtoul(value1, NULL, 16);
1388                 break;
1389         case IMAGE_CFG_NAND_BADBLK_LOCATION:
1390                 el->nandbadblklocation = strtoul(value1, NULL, 16);
1391                 break;
1392         case IMAGE_CFG_NAND_ECC_MODE:
1393                 ret = image_nand_ecc_mode_id(value1);
1394
1395                 if (ret < 0) {
1396                         fprintf(stderr, "Invalid NAND ECC mode '%s'\n", value1);
1397                         return -1;
1398                 }
1399                 el->nandeccmode = ret;
1400                 break;
1401         case IMAGE_CFG_NAND_PAGESZ:
1402                 el->nandpagesz = strtoul(value1, NULL, 16);
1403                 break;
1404         case IMAGE_CFG_BINARY:
1405                 argi = 0;
1406
1407                 el->binary.file = strdup(value1);
1408                 while (1) {
1409                         char *value = strtok_r(NULL, delimiters, &saveptr);
1410
1411                         if (!value)
1412                                 break;
1413                         el->binary.args[argi] = strtoul(value, NULL, 16);
1414                         argi++;
1415                         if (argi >= BINARY_MAX_ARGS) {
1416                                 fprintf(stderr,
1417                                         "Too many arguments for BINARY\n");
1418                                 return -1;
1419                         }
1420                 }
1421                 el->binary.nargs = argi;
1422                 break;
1423         case IMAGE_CFG_DATA:
1424                 value2 = strtok_r(NULL, delimiters, &saveptr);
1425
1426                 if (!value1 || !value2) {
1427                         fprintf(stderr,
1428                                 "Invalid number of arguments for DATA\n");
1429                         return -1;
1430                 }
1431
1432                 el->regdata.raddr = strtoul(value1, NULL, 16);
1433                 el->regdata.rdata = strtoul(value2, NULL, 16);
1434                 break;
1435         case IMAGE_CFG_DATA_DELAY:
1436                 if (!strcmp(value1, "SDRAM_SETUP"))
1437                         el->regdata_delay = REGISTER_SET_HDR_OPT_DELAY_SDRAM_SETUP;
1438                 else
1439                         el->regdata_delay = REGISTER_SET_HDR_OPT_DELAY_MS(strtoul(value1, NULL, 10));
1440                 break;
1441         case IMAGE_CFG_BAUDRATE:
1442                 el->baudrate = strtoul(value1, NULL, 10);
1443                 break;
1444         case IMAGE_CFG_DEBUG:
1445                 el->debug = strtoul(value1, NULL, 10);
1446                 break;
1447         case IMAGE_CFG_KAK:
1448                 el->key_name = strdup(value1);
1449                 break;
1450         case IMAGE_CFG_CSK:
1451                 el->key_name = strdup(value1);
1452                 break;
1453         case IMAGE_CFG_CSK_INDEX:
1454                 el->csk_idx = strtol(value1, NULL, 0);
1455                 break;
1456         case IMAGE_CFG_JTAG_DELAY:
1457                 el->jtag_delay = strtoul(value1, NULL, 0);
1458                 break;
1459         case IMAGE_CFG_BOX_ID:
1460                 el->boxid = strtoul(value1, NULL, 0);
1461                 break;
1462         case IMAGE_CFG_FLASH_ID:
1463                 el->flashid = strtoul(value1, NULL, 0);
1464                 break;
1465         case IMAGE_CFG_SEC_SPECIALIZED_IMG:
1466                 el->sec_specialized_img = true;
1467                 break;
1468         case IMAGE_CFG_SEC_COMMON_IMG:
1469                 el->sec_specialized_img = false;
1470                 break;
1471         case IMAGE_CFG_SEC_BOOT_DEV:
1472                 el->sec_boot_dev = strtoul(value1, NULL, 0);
1473                 break;
1474         case IMAGE_CFG_SEC_FUSE_DUMP:
1475                 el->name = strdup(value1);
1476                 break;
1477         default:
1478                 fprintf(stderr, unknown_msg, line);
1479         }
1480
1481         return 0;
1482 }
1483
1484 /*
1485  * Parse the configuration file 'fcfg' into the array of configuration
1486  * elements 'image_cfg', and return the number of configuration
1487  * elements in 'cfgn'.
1488  */
1489 static int image_create_config_parse(FILE *fcfg)
1490 {
1491         int ret;
1492         int cfgi = 0;
1493
1494         /* Parse the configuration file */
1495         while (!feof(fcfg)) {
1496                 char *line;
1497                 char buf[256];
1498
1499                 /* Read the current line */
1500                 memset(buf, 0, sizeof(buf));
1501                 line = fgets(buf, sizeof(buf), fcfg);
1502                 if (!line)
1503                         break;
1504
1505                 /* Ignore useless lines */
1506                 if (line[0] == '\n' || line[0] == '#')
1507                         continue;
1508
1509                 /* Strip final newline */
1510                 if (line[strlen(line) - 1] == '\n')
1511                         line[strlen(line) - 1] = 0;
1512
1513                 /* Parse the current line */
1514                 ret = image_create_config_parse_oneline(line,
1515                                                         &image_cfg[cfgi]);
1516                 if (ret)
1517                         return ret;
1518
1519                 cfgi++;
1520
1521                 if (cfgi >= IMAGE_CFG_ELEMENT_MAX) {
1522                         fprintf(stderr,
1523                                 "Too many configuration elements in .cfg file\n");
1524                         return -1;
1525                 }
1526         }
1527
1528         cfgn = cfgi;
1529         return 0;
1530 }
1531
1532 static int image_get_version(void)
1533 {
1534         struct image_cfg_element *e;
1535
1536         e = image_find_option(IMAGE_CFG_VERSION);
1537         if (!e)
1538                 return -1;
1539
1540         return e->version;
1541 }
1542
1543 static int image_get_bootfrom(void)
1544 {
1545         struct image_cfg_element *e;
1546
1547         e = image_find_option(IMAGE_CFG_BOOT_FROM);
1548         if (!e)
1549                 return -1;
1550
1551         return e->bootfrom;
1552 }
1553
1554 static void kwbimage_set_header(void *ptr, struct stat *sbuf, int ifd,
1555                                 struct image_tool_params *params)
1556 {
1557         FILE *fcfg;
1558         void *image = NULL;
1559         int version;
1560         size_t headersz = 0;
1561         uint32_t checksum;
1562         int ret;
1563
1564         fcfg = fopen(params->imagename, "r");
1565         if (!fcfg) {
1566                 fprintf(stderr, "Could not open input file %s\n",
1567                         params->imagename);
1568                 exit(EXIT_FAILURE);
1569         }
1570
1571         image_cfg = malloc(IMAGE_CFG_ELEMENT_MAX *
1572                            sizeof(struct image_cfg_element));
1573         if (!image_cfg) {
1574                 fprintf(stderr, "Cannot allocate memory\n");
1575                 fclose(fcfg);
1576                 exit(EXIT_FAILURE);
1577         }
1578
1579         memset(image_cfg, 0,
1580                IMAGE_CFG_ELEMENT_MAX * sizeof(struct image_cfg_element));
1581         rewind(fcfg);
1582
1583         ret = image_create_config_parse(fcfg);
1584         fclose(fcfg);
1585         if (ret) {
1586                 free(image_cfg);
1587                 exit(EXIT_FAILURE);
1588         }
1589
1590         version = image_get_version();
1591         switch (version) {
1592                 /*
1593                  * Fallback to version 0 if no version is provided in the
1594                  * cfg file
1595                  */
1596         case -1:
1597         case 0:
1598                 image = image_create_v0(&headersz, params, sbuf->st_size);
1599                 break;
1600
1601         case 1:
1602                 image = image_create_v1(&headersz, params, ptr, sbuf->st_size);
1603                 break;
1604
1605         default:
1606                 fprintf(stderr, "Unsupported version %d\n", version);
1607                 free(image_cfg);
1608                 exit(EXIT_FAILURE);
1609         }
1610
1611         if (!image) {
1612                 fprintf(stderr, "Could not create image\n");
1613                 free(image_cfg);
1614                 exit(EXIT_FAILURE);
1615         }
1616
1617         free(image_cfg);
1618
1619         /* Build and add image checksum header */
1620         checksum = cpu_to_le32(image_checksum32((uint8_t *)ptr + headersz,
1621                                 sbuf->st_size - headersz - sizeof(uint32_t)));
1622         memcpy((uint8_t *)ptr + sbuf->st_size - sizeof(uint32_t), &checksum,
1623                 sizeof(uint32_t));
1624
1625         /* Finally copy the header into the image area */
1626         memcpy(ptr, image, headersz);
1627
1628         free(image);
1629 }
1630
1631 static void kwbimage_print_header(const void *ptr)
1632 {
1633         struct main_hdr_v0 *mhdr = (struct main_hdr_v0 *)ptr;
1634         struct opt_hdr_v1 *ohdr;
1635
1636         printf("Image Type:   MVEBU Boot from %s Image\n",
1637                image_boot_mode_name(mhdr->blockid));
1638         printf("Image version:%d\n", kwbimage_version(ptr));
1639
1640         for_each_opt_hdr_v1 (ohdr, mhdr) {
1641                 if (ohdr->headertype == OPT_HDR_V1_BINARY_TYPE) {
1642                         printf("BIN Hdr Size: ");
1643                         genimg_print_size(opt_hdr_v1_size(ohdr) - 12 -
1644                                           4 * ohdr->data[0]);
1645                 }
1646         }
1647
1648         printf("Data Size:    ");
1649         genimg_print_size(mhdr->blocksize - sizeof(uint32_t));
1650         printf("Load Address: %08x\n", mhdr->destaddr);
1651         printf("Entry Point:  %08x\n", mhdr->execaddr);
1652 }
1653
1654 static int kwbimage_check_image_types(uint8_t type)
1655 {
1656         if (type == IH_TYPE_KWBIMAGE)
1657                 return EXIT_SUCCESS;
1658
1659         return EXIT_FAILURE;
1660 }
1661
1662 static int kwbimage_verify_header(unsigned char *ptr, int image_size,
1663                                   struct image_tool_params *params)
1664 {
1665         size_t header_size = kwbheader_size(ptr);
1666         uint8_t csum;
1667
1668         if (header_size > image_size)
1669                 return -FDT_ERR_BADSTRUCTURE;
1670
1671         if (!main_hdr_checksum_ok(ptr))
1672                 return -FDT_ERR_BADSTRUCTURE;
1673
1674         /* Only version 0 extended header has checksum */
1675         if (kwbimage_version(ptr) == 0) {
1676                 struct main_hdr_v0 *mhdr = (struct main_hdr_v0 *)ptr;
1677
1678                 if (mhdr->ext & 0x1) {
1679                         struct ext_hdr_v0 *ext_hdr = (void *)(mhdr + 1);
1680
1681                         csum = image_checksum8(ext_hdr, sizeof(*ext_hdr) - 1);
1682                         if (csum != ext_hdr->checksum)
1683                                 return -FDT_ERR_BADSTRUCTURE;
1684                 }
1685         } else if (kwbimage_version(ptr) == 1) {
1686                 struct main_hdr_v1 *mhdr = (struct main_hdr_v1 *)ptr;
1687                 const uint8_t *mhdr_end;
1688                 struct opt_hdr_v1 *ohdr;
1689                 uint32_t offset;
1690                 uint32_t size;
1691
1692                 mhdr_end = (uint8_t *)mhdr + header_size;
1693                 for_each_opt_hdr_v1 (ohdr, ptr)
1694                         if (!opt_hdr_v1_valid_size(ohdr, mhdr_end))
1695                                 return -FDT_ERR_BADSTRUCTURE;
1696
1697                 offset = le32_to_cpu(mhdr->srcaddr);
1698
1699                 /*
1700                  * For SATA srcaddr is specified in number of sectors.
1701                  * The main header is must be stored at sector number 1.
1702                  * This expects that sector size is 512 bytes and recalculates
1703                  * data offset to bytes relative to the main header.
1704                  */
1705                 if (mhdr->blockid == IBR_HDR_SATA_ID) {
1706                         if (offset < 1)
1707                                 return -FDT_ERR_BADSTRUCTURE;
1708                         offset -= 1;
1709                         offset *= 512;
1710                 }
1711
1712                 /*
1713                  * For SDIO srcaddr is specified in number of sectors.
1714                  * This expects that sector size is 512 bytes and recalculates
1715                  * data offset to bytes.
1716                  */
1717                 if (mhdr->blockid == IBR_HDR_SDIO_ID)
1718                         offset *= 512;
1719
1720                 /*
1721                  * For PCIe srcaddr is always set to 0xFFFFFFFF.
1722                  * This expects that data starts after all headers.
1723                  */
1724                 if (mhdr->blockid == IBR_HDR_PEX_ID && offset == 0xFFFFFFFF)
1725                         offset = header_size;
1726
1727                 if (offset > image_size || offset % 4 != 0)
1728                         return -FDT_ERR_BADSTRUCTURE;
1729
1730                 size = le32_to_cpu(mhdr->blocksize);
1731                 if (size < 4 || offset + size > image_size || size % 4 != 0)
1732                         return -FDT_ERR_BADSTRUCTURE;
1733
1734                 if (image_checksum32(ptr + offset, size - 4) !=
1735                     *(uint32_t *)(ptr + offset + size - 4))
1736                         return -FDT_ERR_BADSTRUCTURE;
1737         } else {
1738                 return -FDT_ERR_BADSTRUCTURE;
1739         }
1740
1741         return 0;
1742 }
1743
1744 static int kwbimage_generate(struct image_tool_params *params,
1745                              struct image_type_params *tparams)
1746 {
1747         FILE *fcfg;
1748         struct stat s;
1749         int alloc_len;
1750         int bootfrom;
1751         int version;
1752         void *hdr;
1753         int ret;
1754
1755         fcfg = fopen(params->imagename, "r");
1756         if (!fcfg) {
1757                 fprintf(stderr, "Could not open input file %s\n",
1758                         params->imagename);
1759                 exit(EXIT_FAILURE);
1760         }
1761
1762         if (stat(params->datafile, &s)) {
1763                 fprintf(stderr, "Could not stat data file %s: %s\n",
1764                         params->datafile, strerror(errno));
1765                 exit(EXIT_FAILURE);
1766         }
1767
1768         image_cfg = malloc(IMAGE_CFG_ELEMENT_MAX *
1769                            sizeof(struct image_cfg_element));
1770         if (!image_cfg) {
1771                 fprintf(stderr, "Cannot allocate memory\n");
1772                 fclose(fcfg);
1773                 exit(EXIT_FAILURE);
1774         }
1775
1776         memset(image_cfg, 0,
1777                IMAGE_CFG_ELEMENT_MAX * sizeof(struct image_cfg_element));
1778         rewind(fcfg);
1779
1780         ret = image_create_config_parse(fcfg);
1781         fclose(fcfg);
1782         if (ret) {
1783                 free(image_cfg);
1784                 exit(EXIT_FAILURE);
1785         }
1786
1787         bootfrom = image_get_bootfrom();
1788         version = image_get_version();
1789         switch (version) {
1790                 /*
1791                  * Fallback to version 0 if no version is provided in the
1792                  * cfg file
1793                  */
1794         case -1:
1795         case 0:
1796                 alloc_len = sizeof(struct main_hdr_v0) +
1797                         sizeof(struct ext_hdr_v0);
1798                 break;
1799
1800         case 1:
1801                 alloc_len = image_headersz_v1(NULL);
1802                 break;
1803
1804         default:
1805                 fprintf(stderr, "Unsupported version %d\n", version);
1806                 free(image_cfg);
1807                 exit(EXIT_FAILURE);
1808         }
1809
1810         free(image_cfg);
1811
1812         hdr = malloc(alloc_len);
1813         if (!hdr) {
1814                 fprintf(stderr, "%s: malloc return failure: %s\n",
1815                         params->cmdname, strerror(errno));
1816                 exit(EXIT_FAILURE);
1817         }
1818
1819         memset(hdr, 0, alloc_len);
1820         tparams->header_size = alloc_len;
1821         tparams->hdr = hdr;
1822
1823         /*
1824          * The resulting image needs to be 4-byte aligned. At least
1825          * the Marvell hdrparser tool complains if its unaligned.
1826          * After the image data is stored 4-byte checksum.
1827          * Final SPI and NAND images must be aligned to 256 bytes.
1828          * Final SATA and SDIO images must be aligned to 512 bytes.
1829          */
1830         if (bootfrom == IBR_HDR_SPI_ID || bootfrom == IBR_HDR_NAND_ID)
1831                 return 4 + (256 - (alloc_len + s.st_size + 4) % 256) % 256;
1832         else if (bootfrom == IBR_HDR_SATA_ID || bootfrom == IBR_HDR_SDIO_ID)
1833                 return 4 + (512 - (alloc_len + s.st_size + 4) % 512) % 512;
1834         else
1835                 return 4 + (4 - s.st_size % 4) % 4;
1836 }
1837
1838 static int kwbimage_extract_subimage(void *ptr, struct image_tool_params *params)
1839 {
1840         struct main_hdr_v1 *mhdr = (struct main_hdr_v1 *)ptr;
1841         size_t header_size = kwbheader_size(ptr);
1842         struct opt_hdr_v1 *ohdr;
1843         int idx = params->pflag;
1844         int cur_idx = 0;
1845         uint32_t offset;
1846         ulong image;
1847         ulong size;
1848
1849         for_each_opt_hdr_v1 (ohdr, ptr) {
1850                 if (ohdr->headertype != OPT_HDR_V1_BINARY_TYPE)
1851                         continue;
1852
1853                 if (idx == cur_idx) {
1854                         image = (ulong)&ohdr->data[4 + 4 * ohdr->data[0]];
1855                         size = opt_hdr_v1_size(ohdr) - 12 - 4 * ohdr->data[0];
1856                         goto extract;
1857                 }
1858
1859                 ++cur_idx;
1860         }
1861
1862         if (idx != cur_idx) {
1863                 printf("Image %d is not present\n", idx);
1864                 return -1;
1865         }
1866
1867         offset = le32_to_cpu(mhdr->srcaddr);
1868
1869         if (mhdr->blockid == IBR_HDR_SATA_ID) {
1870                 offset -= 1;
1871                 offset *= 512;
1872         }
1873
1874         if (mhdr->blockid == IBR_HDR_SDIO_ID)
1875                 offset *= 512;
1876
1877         if (mhdr->blockid == IBR_HDR_PEX_ID && offset == 0xFFFFFFFF)
1878                 offset = header_size;
1879
1880         image = (ulong)((uint8_t *)ptr + offset);
1881         size = le32_to_cpu(mhdr->blocksize) - 4;
1882
1883 extract:
1884         return imagetool_save_subimage(params->outfile, image, size);
1885 }
1886
1887 /*
1888  * Report Error if xflag is set in addition to default
1889  */
1890 static int kwbimage_check_params(struct image_tool_params *params)
1891 {
1892         if (!params->iflag && (!params->imagename || !strlen(params->imagename))) {
1893                 char *msg = "Configuration file for kwbimage creation omitted";
1894
1895                 fprintf(stderr, "Error:%s - %s\n", params->cmdname, msg);
1896                 return CFG_INVALID;
1897         }
1898
1899         return (params->dflag && (params->fflag || params->lflag)) ||
1900                 (params->fflag && (params->dflag || params->lflag)) ||
1901                 (params->lflag && (params->dflag || params->fflag)) ||
1902                 (params->xflag);
1903 }
1904
1905 /*
1906  * kwbimage type parameters definition
1907  */
1908 U_BOOT_IMAGE_TYPE(
1909         kwbimage,
1910         "Marvell MVEBU Boot Image support",
1911         0,
1912         NULL,
1913         kwbimage_check_params,
1914         kwbimage_verify_header,
1915         kwbimage_print_header,
1916         kwbimage_set_header,
1917         kwbimage_extract_subimage,
1918         kwbimage_check_image_types,
1919         NULL,
1920         kwbimage_generate
1921 );