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