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 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 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 int kwb_sign(RSA *key, void *data, int datasz, struct sig_v1 *sig, char *signame)
 606 {
 607         EVP_PKEY *evp_key;
 608         EVP_MD_CTX *ctx;
 609         unsigned int sig_size;
 610         int size;
 611         int ret = 0;
 612
 613         evp_key = EVP_PKEY_new();
 614         if (!evp_key)
 615                 return openssl_err("EVP_PKEY object creation failed");
 616
 617         if (!EVP_PKEY_set1_RSA(evp_key, key)) {
 618                 ret = openssl_err("EVP key setup failed");
 619                 goto err_key;
 620         }
 621
 622         size = EVP_PKEY_size(evp_key);
 623         if (size > sizeof(sig->sig)) {
 624                 fprintf(stderr, "Buffer to small for signature (%d bytes)\n",
 625                         size);
 626                 ret = -ENOBUFS;
 627                 goto err_key;
 628         }
 629
 630         ctx = EVP_MD_CTX_create();
 631         if (!ctx) {
 632                 ret = openssl_err("EVP context creation failed");
 633                 goto err_key;
 634         }
 635         EVP_MD_CTX_init(ctx);
 636         if (!EVP_SignInit(ctx, EVP_sha256())) {
 637                 ret = openssl_err("Signer setup failed");
 638                 goto err_ctx;
 639         }
 640
 641         if (!EVP_SignUpdate(ctx, data, datasz)) {
 642                 ret = openssl_err("Signing data failed");
 643                 goto err_ctx;
 644         }
 645
 646         if (!EVP_SignFinal(ctx, sig->sig, &sig_size, evp_key)) {
 647                 ret = openssl_err("Could not obtain signature");
 648                 goto err_ctx;
 649         }
 650
 651         EVP_MD_CTX_cleanup(ctx);
 652         EVP_MD_CTX_destroy(ctx);
 653         EVP_PKEY_free(evp_key);
 654
 655         return 0;
 656
 657 err_ctx:
 658         EVP_MD_CTX_destroy(ctx);
 659 err_key:
 660         EVP_PKEY_free(evp_key);
 661         fprintf(stderr, "Failed to create %s signature\n", signame);
 662         return ret;
 663 }
 664
 665 int kwb_verify(RSA *key, void *data, int datasz, struct sig_v1 *sig,
 666                char *signame)
 667 {
 668         EVP_PKEY *evp_key;
 669         EVP_MD_CTX *ctx;
 670         int size;
 671         int ret = 0;
 672
 673         evp_key = EVP_PKEY_new();
 674         if (!evp_key)
 675                 return openssl_err("EVP_PKEY object creation failed");
 676
 677         if (!EVP_PKEY_set1_RSA(evp_key, key)) {
 678                 ret = openssl_err("EVP key setup failed");
 679                 goto err_key;
 680         }
 681
 682         size = EVP_PKEY_size(evp_key);
 683         if (size > sizeof(sig->sig)) {
 684                 fprintf(stderr, "Invalid signature size (%d bytes)\n",
 685                         size);
 686                 ret = -EINVAL;
 687                 goto err_key;
 688         }
 689
 690         ctx = EVP_MD_CTX_create();
 691         if (!ctx) {
 692                 ret = openssl_err("EVP context creation failed");
 693                 goto err_key;
 694         }
 695         EVP_MD_CTX_init(ctx);
 696         if (!EVP_VerifyInit(ctx, EVP_sha256())) {
 697                 ret = openssl_err("Verifier setup failed");
 698                 goto err_ctx;
 699         }
 700
 701         if (!EVP_VerifyUpdate(ctx, data, datasz)) {
 702                 ret = openssl_err("Hashing data failed");
 703                 goto err_ctx;
 704         }
 705
 706         if (EVP_VerifyFinal(ctx, sig->sig, sizeof(sig->sig), evp_key) != 1) {
 707                 ret = openssl_err("Could not verify signature");
 708                 goto err_ctx;
 709         }
 710
 711         EVP_MD_CTX_cleanup(ctx);
 712         EVP_MD_CTX_destroy(ctx);
 713         EVP_PKEY_free(evp_key);
 714
 715         return 0;
 716
 717 err_ctx:
 718         EVP_MD_CTX_destroy(ctx);
 719 err_key:
 720         EVP_PKEY_free(evp_key);
 721         fprintf(stderr, "Failed to verify %s signature\n", signame);
 722         return ret;
 723 }
 724
 725 int kwb_sign_and_verify(RSA *key, void *data, int datasz, struct sig_v1 *sig,
 726                         char *signame)
 727 {
 728         if (kwb_sign(key, data, datasz, sig, signame) < 0)
 729                 return -1;
 730
 731         if (kwb_verify(key, data, datasz, sig, signame) < 0)
 732                 return -1;
 733
 734         return 0;
 735 }
 736
 737
 738 int kwb_dump_fuse_cmds_38x(FILE *out, struct secure_hdr_v1 *sec_hdr)
 739 {
 740         struct hash_v1 kak_pub_hash;
 741         struct image_cfg_element *e;
 742         unsigned int fuse_line;
 743         int i, idx;
 744         uint8_t *ptr;
 745         uint32_t val;
 746         int ret = 0;
 747
 748         if (!out || !sec_hdr)
 749                 return -EINVAL;
 750
 751         ret = kwb_compute_pubkey_hash(&sec_hdr->kak, &kak_pub_hash);
 752         if (ret < 0)
 753                 goto done;
 754
 755         fprintf(out, "# burn KAK pub key hash\n");
 756         ptr = kak_pub_hash.hash;
 757         for (fuse_line = 26; fuse_line <= 30; ++fuse_line) {
 758                 fprintf(out, "fuse prog -y %u 0 ", fuse_line);
 759
 760                 for (i = 4; i-- > 0;)
 761                         fprintf(out, "%02hx", (ushort)ptr[i]);
 762                 ptr += 4;
 763                 fprintf(out, " 00");
 764
 765                 if (fuse_line < 30) {
 766                         for (i = 3; i-- > 0;)
 767                                 fprintf(out, "%02hx", (ushort)ptr[i]);
 768                         ptr += 3;
 769                 } else {
 770                         fprintf(out, "000000");
 771                 }
 772
 773                 fprintf(out, " 1\n");
 774         }
 775
 776         fprintf(out, "# burn CSK selection\n");
 777
 778         idx = image_get_csk_index();
 779         if (idx < 0 || idx > 15) {
 780                 ret = -EINVAL;
 781                 goto done;
 782         }
 783         if (idx > 0) {
 784                 for (fuse_line = 31; fuse_line < 31 + idx; ++fuse_line)
 785                         fprintf(out, "fuse prog -y %u 0 00000001 00000000 1\n",
 786                                 fuse_line);
 787         } else {
 788                 fprintf(out, "# CSK index is 0; no mods needed\n");
 789         }
 790
 791         e = image_find_option(IMAGE_CFG_BOX_ID);
 792         if (e) {
 793                 fprintf(out, "# set box ID\n");
 794                 fprintf(out, "fuse prog -y 48 0 %08x 00000000 1\n", e->boxid);
 795         }
 796
 797         e = image_find_option(IMAGE_CFG_FLASH_ID);
 798         if (e) {
 799                 fprintf(out, "# set flash ID\n");
 800                 fprintf(out, "fuse prog -y 47 0 %08x 00000000 1\n", e->flashid);
 801         }
 802
 803         fprintf(out, "# enable secure mode ");
 804         fprintf(out, "(must be the last fuse line written)\n");
 805
 806         val = 1;
 807         e = image_find_option(IMAGE_CFG_SEC_BOOT_DEV);
 808         if (!e) {
 809                 fprintf(stderr, "ERROR: secured mode boot device not given\n");
 810                 ret = -EINVAL;
 811                 goto done;
 812         }
 813
 814         if (e->sec_boot_dev > 0xff) {
 815                 fprintf(stderr, "ERROR: secured mode boot device invalid\n");
 816                 ret = -EINVAL;
 817                 goto done;
 818         }
 819
 820         val |= (e->sec_boot_dev << 8);
 821
 822         fprintf(out, "fuse prog -y 24 0 %08x 0103e0a9 1\n", val);
 823
 824         fprintf(out, "# lock (unused) fuse lines (0-23)s\n");
 825         for (fuse_line = 0; fuse_line < 24; ++fuse_line)
 826                 fprintf(out, "fuse prog -y %u 2 1\n", fuse_line);
 827
 828         fprintf(out, "# OK, that's all :-)\n");
 829
 830 done:
 831         return ret;
 832 }
 833
 834 static int kwb_dump_fuse_cmds(struct secure_hdr_v1 *sec_hdr)
 835 {
 836         int ret = 0;
 837         struct image_cfg_element *e;
 838
 839         e = image_find_option(IMAGE_CFG_SEC_FUSE_DUMP);
 840         if (!e)
 841                 return 0;
 842
 843         if (!strcmp(e->name, "a38x")) {
 844                 FILE *out = fopen("kwb_fuses_a38x.txt", "w+");
 845
 846                 if (!out) {
 847                         fprintf(stderr, "Couldn't open eFuse settings: '%s': %s\n",
 848                                 "kwb_fuses_a38x.txt", strerror(errno));
 849                         return -ENOENT;
 850                 }
 851
 852                 kwb_dump_fuse_cmds_38x(out, sec_hdr);
 853                 fclose(out);
 854                 goto done;
 855         }
 856
 857         ret = -ENOSYS;
 858
 859 done:
 860         return ret;
 861 }
 862
 863 static size_t image_headersz_align(size_t headersz, uint8_t blockid)
 864 {
 865         /*
 866          * Header needs to be 4-byte aligned, which is already ensured by code
 867          * above. Moreover UART images must have header aligned to 128 bytes
 868          * (xmodem block size), NAND images to 256 bytes (ECC calculation),
 869          * and SATA and SDIO images to 512 bytes (storage block size).
 870          * Note that SPI images do not have to have header size aligned
 871          * to 256 bytes because it is possible to read from SPI storage from
 872          * any offset (read offset does not have to be aligned to block size).
 873          */
 874         if (blockid == IBR_HDR_UART_ID)
 875                 return ALIGN(headersz, 128);
 876         else if (blockid == IBR_HDR_NAND_ID)
 877                 return ALIGN(headersz, 256);
 878         else if (blockid == IBR_HDR_SATA_ID || blockid == IBR_HDR_SDIO_ID)
 879                 return ALIGN(headersz, 512);
 880         else
 881                 return headersz;
 882 }
 883
 884 static size_t image_headersz_v0(int *hasext)
 885 {
 886         size_t headersz;
 887
 888         headersz = sizeof(struct main_hdr_v0);
 889         if (image_count_options(IMAGE_CFG_DATA) > 0) {
 890                 headersz += sizeof(struct ext_hdr_v0);
 891                 if (hasext)
 892                         *hasext = 1;
 893         }
 894
 895         return image_headersz_align(headersz, image_get_bootfrom());
 896 }
 897
 898 static void *image_create_v0(size_t *imagesz, struct image_tool_params *params,
 899                              int payloadsz)
 900 {
 901         struct image_cfg_element *e;
 902         size_t headersz;
 903         struct main_hdr_v0 *main_hdr;
 904         uint8_t *image;
 905         int has_ext = 0;
 906
 907         /*
 908          * Calculate the size of the header and the size of the
 909          * payload
 910          */
 911         headersz = image_headersz_v0(&has_ext);
 912
 913         image = malloc(headersz);
 914         if (!image) {
 915                 fprintf(stderr, "Cannot allocate memory for image\n");
 916                 return NULL;
 917         }
 918
 919         memset(image, 0, headersz);
 920
 921         main_hdr = (struct main_hdr_v0 *)image;
 922
 923         /* Fill in the main header */
 924         main_hdr->blocksize =
 925                 cpu_to_le32(payloadsz);
 926         main_hdr->srcaddr   = cpu_to_le32(headersz);
 927         main_hdr->ext       = has_ext;
 928         main_hdr->version   = 0;
 929         main_hdr->destaddr  = cpu_to_le32(params->addr);
 930         main_hdr->execaddr  = cpu_to_le32(params->ep);
 931         main_hdr->blockid   = image_get_bootfrom();
 932
 933         e = image_find_option(IMAGE_CFG_NAND_ECC_MODE);
 934         if (e)
 935                 main_hdr->nandeccmode = e->nandeccmode;
 936         e = image_find_option(IMAGE_CFG_NAND_PAGESZ);
 937         if (e)
 938                 main_hdr->nandpagesize = cpu_to_le16(e->nandpagesz);
 939         main_hdr->checksum = image_checksum8(image,
 940                                              sizeof(struct main_hdr_v0));
 941
 942         /*
 943          * For SATA srcaddr is specified in number of sectors starting from
 944          * sector 0. The main header is stored at sector number 1.
 945          * This expects the sector size to be 512 bytes.
 946          * Header size is already aligned.
 947          */
 948         if (main_hdr->blockid == IBR_HDR_SATA_ID)
 949                 main_hdr->srcaddr = cpu_to_le32(headersz / 512 + 1);
 950
 951         /*
 952          * For SDIO srcaddr is specified in number of sectors starting from
 953          * sector 0. The main header is stored at sector number 0.
 954          * This expects sector size to be 512 bytes.
 955          * Header size is already aligned.
 956          */
 957         if (main_hdr->blockid == IBR_HDR_SDIO_ID)
 958                 main_hdr->srcaddr = cpu_to_le32(headersz / 512);
 959
 960         /* For PCIe srcaddr is not used and must be set to 0xFFFFFFFF. */
 961         if (main_hdr->blockid == IBR_HDR_PEX_ID)
 962                 main_hdr->srcaddr = cpu_to_le32(0xFFFFFFFF);
 963
 964         /* Generate the ext header */
 965         if (has_ext) {
 966                 struct ext_hdr_v0 *ext_hdr;
 967                 int cfgi, datai;
 968
 969                 ext_hdr = (struct ext_hdr_v0 *)
 970                                 (image + sizeof(struct main_hdr_v0));
 971                 ext_hdr->offset = cpu_to_le32(0x40);
 972
 973                 for (cfgi = 0, datai = 0; cfgi < cfgn; cfgi++) {
 974                         e = &image_cfg[cfgi];
 975                         if (e->type != IMAGE_CFG_DATA)
 976                                 continue;
 977
 978                         ext_hdr->rcfg[datai].raddr =
 979                                 cpu_to_le32(e->regdata.raddr);
 980                         ext_hdr->rcfg[datai].rdata =
 981                                 cpu_to_le32(e->regdata.rdata);
 982                         datai++;
 983                 }
 984
 985                 ext_hdr->checksum = image_checksum8(ext_hdr,
 986                                                     sizeof(struct ext_hdr_v0));
 987         }
 988
 989         *imagesz = headersz;
 990         return image;
 991 }
 992
 993 static size_t image_headersz_v1(int *hasext)
 994 {
 995         struct image_cfg_element *binarye;
 996         unsigned int count;
 997         size_t headersz;
 998         int cfgi;
 999
1000         /*
1001          * Calculate the size of the header and the size of the
1002          * payload
1003          */
1004         headersz = sizeof(struct main_hdr_v1);
1005
1006         if (image_get_csk_index() >= 0) {
1007                 headersz += sizeof(struct secure_hdr_v1);
1008                 if (hasext)
1009                         *hasext = 1;
1010         }
1011
1012         count = image_count_options(IMAGE_CFG_DATA);
1013         if (count > 0)
1014                 headersz += sizeof(struct register_set_hdr_v1) + 8 * count + 4;
1015
1016         for (cfgi = 0; cfgi < cfgn; cfgi++) {
1017                 int ret;
1018                 struct stat s;
1019
1020                 binarye = &image_cfg[cfgi];
1021                 if (binarye->type != IMAGE_CFG_BINARY)
1022                         continue;
1023
1024                 ret = stat(binarye->binary.file, &s);
1025                 if (ret < 0) {
1026                         char cwd[PATH_MAX];
1027                         char *dir = cwd;
1028
1029                         memset(cwd, 0, sizeof(cwd));
1030                         if (!getcwd(cwd, sizeof(cwd))) {
1031                                 dir = "current working directory";
1032                                 perror("getcwd() failed");
1033                         }
1034
1035                         fprintf(stderr,
1036                                 "Didn't find the file '%s' in '%s' which is mandatory to generate the image\n"
1037                                 "This file generally contains the DDR3 training code, and should be extracted from an existing bootable\n"
1038                                 "image for your board. Use 'dumpimage -T kwbimage -p 0' to extract it from an existing image.\n",
1039                                 binarye->binary.file, dir);
1040                         return 0;
1041                 }
1042
1043                 headersz += sizeof(struct opt_hdr_v1) + sizeof(uint32_t) +
1044                         (binarye->binary.nargs) * sizeof(uint32_t);
1045                 headersz = ALIGN(headersz, 16);
1046                 headersz += ALIGN(s.st_size, 4) + sizeof(uint32_t);
1047                 if (hasext)
1048                         *hasext = 1;
1049         }
1050
1051         return image_headersz_align(headersz, image_get_bootfrom());
1052 }
1053
1054 int add_binary_header_v1(uint8_t **cur, uint8_t **next_ext,
1055                          struct image_cfg_element *binarye,
1056                          struct main_hdr_v1 *main_hdr)
1057 {
1058         struct opt_hdr_v1 *hdr = (struct opt_hdr_v1 *)*cur;
1059         uint32_t add_args;
1060         uint32_t offset;
1061         uint32_t *args;
1062         size_t binhdrsz;
1063         struct stat s;
1064         int argi;
1065         FILE *bin;
1066         int ret;
1067
1068         hdr->headertype = OPT_HDR_V1_BINARY_TYPE;
1069
1070         bin = fopen(binarye->binary.file, "r");
1071         if (!bin) {
1072                 fprintf(stderr, "Cannot open binary file %s\n",
1073                         binarye->binary.file);
1074                 return -1;
1075         }
1076
1077         if (fstat(fileno(bin), &s)) {
1078                 fprintf(stderr, "Cannot stat binary file %s\n",
1079                         binarye->binary.file);
1080                 goto err_close;
1081         }
1082
1083         *cur += sizeof(struct opt_hdr_v1);
1084
1085         args = (uint32_t *)*cur;
1086         *args = cpu_to_le32(binarye->binary.nargs);
1087         args++;
1088         for (argi = 0; argi < binarye->binary.nargs; argi++)
1089                 args[argi] = cpu_to_le32(binarye->binary.args[argi]);
1090
1091         *cur += (binarye->binary.nargs + 1) * sizeof(uint32_t);
1092
1093         /*
1094          * ARM executable code inside the BIN header on some mvebu platforms
1095          * (e.g. A370, AXP) must always be aligned with the 128-bit boundary.
1096          * This requirement can be met by inserting dummy arguments into
1097          * BIN header, if needed.
1098          */
1099         offset = *cur - (uint8_t *)main_hdr;
1100         add_args = ((16 - offset % 16) % 16) / sizeof(uint32_t);
1101         if (add_args) {
1102                 *(args - 1) = cpu_to_le32(binarye->binary.nargs + add_args);
1103                 *cur += add_args * sizeof(uint32_t);
1104         }
1105
1106         ret = fread(*cur, s.st_size, 1, bin);
1107         if (ret != 1) {
1108                 fprintf(stderr,
1109                         "Could not read binary image %s\n",
1110                         binarye->binary.file);
1111                 goto err_close;
1112         }
1113
1114         fclose(bin);
1115
1116         *cur += ALIGN(s.st_size, 4);
1117
1118         *((uint32_t *)*cur) = 0x00000000;
1119         **next_ext = 1;
1120         *next_ext = *cur;
1121
1122         *cur += sizeof(uint32_t);
1123
1124         binhdrsz = sizeof(struct opt_hdr_v1) +
1125                 (binarye->binary.nargs + add_args + 2) * sizeof(uint32_t) +
1126                 ALIGN(s.st_size, 4);
1127         hdr->headersz_lsb = cpu_to_le16(binhdrsz & 0xFFFF);
1128         hdr->headersz_msb = (binhdrsz & 0xFFFF0000) >> 16;
1129
1130         return 0;
1131
1132 err_close:
1133         fclose(bin);
1134
1135         return -1;
1136 }
1137
1138 int export_pub_kak_hash(RSA *kak, struct secure_hdr_v1 *secure_hdr)
1139 {
1140         FILE *hashf;
1141         int res;
1142
1143         hashf = fopen("pub_kak_hash.txt", "w");
1144         if (!hashf) {
1145                 fprintf(stderr, "Couldn't open hash file: '%s': %s\n",
1146                         "pub_kak_hash.txt", strerror(errno));
1147                 return 1;
1148         }
1149
1150         res = kwb_export_pubkey(kak, &secure_hdr->kak, hashf, "KAK");
1151
1152         fclose(hashf);
1153
1154         return res < 0 ? 1 : 0;
1155 }
1156
1157 int kwb_sign_csk_with_kak(struct image_tool_params *params,
1158                           struct secure_hdr_v1 *secure_hdr, RSA *csk)
1159 {
1160         RSA *kak = NULL;
1161         RSA *kak_pub = NULL;
1162         int csk_idx = image_get_csk_index();
1163         struct sig_v1 tmp_sig;
1164
1165         if (csk_idx < 0 || csk_idx > 15) {
1166                 fprintf(stderr, "Invalid CSK index %d\n", csk_idx);
1167                 return 1;
1168         }
1169
1170         if (kwb_load_kak(params, &kak) < 0)
1171                 return 1;
1172
1173         if (export_pub_kak_hash(kak, secure_hdr))
1174                 return 1;
1175
1176         if (kwb_import_pubkey(&kak_pub, &secure_hdr->kak, "KAK") < 0)
1177                 return 1;
1178
1179         if (kwb_export_pubkey(csk, &secure_hdr->csk[csk_idx], NULL, "CSK") < 0)
1180                 return 1;
1181
1182         if (kwb_sign_and_verify(kak, &secure_hdr->csk,
1183                                 sizeof(secure_hdr->csk) +
1184                                 sizeof(secure_hdr->csksig),
1185                                 &tmp_sig, "CSK") < 0)
1186                 return 1;
1187
1188         if (kwb_verify(kak_pub, &secure_hdr->csk,
1189                        sizeof(secure_hdr->csk) +
1190                        sizeof(secure_hdr->csksig),
1191                        &tmp_sig, "CSK (2)") < 0)
1192                 return 1;
1193
1194         secure_hdr->csksig = tmp_sig;
1195
1196         return 0;
1197 }
1198
1199 int add_secure_header_v1(struct image_tool_params *params, uint8_t *ptr,
1200                          int payloadsz, size_t headersz, uint8_t *image,
1201                          struct secure_hdr_v1 *secure_hdr)
1202 {
1203         struct image_cfg_element *e_jtagdelay;
1204         struct image_cfg_element *e_boxid;
1205         struct image_cfg_element *e_flashid;
1206         RSA *csk = NULL;
1207         unsigned char *image_ptr;
1208         size_t image_size;
1209         struct sig_v1 tmp_sig;
1210         bool specialized_img = image_get_spezialized_img();
1211
1212         kwb_msg("Create secure header content\n");
1213
1214         e_jtagdelay = image_find_option(IMAGE_CFG_JTAG_DELAY);
1215         e_boxid = image_find_option(IMAGE_CFG_BOX_ID);
1216         e_flashid = image_find_option(IMAGE_CFG_FLASH_ID);
1217
1218         if (kwb_load_csk(params, &csk) < 0)
1219                 return 1;
1220
1221         secure_hdr->headertype = OPT_HDR_V1_SECURE_TYPE;
1222         secure_hdr->headersz_msb = 0;
1223         secure_hdr->headersz_lsb = cpu_to_le16(sizeof(struct secure_hdr_v1));
1224         if (e_jtagdelay)
1225                 secure_hdr->jtag_delay = e_jtagdelay->jtag_delay;
1226         if (e_boxid && specialized_img)
1227                 secure_hdr->boxid = cpu_to_le32(e_boxid->boxid);
1228         if (e_flashid && specialized_img)
1229                 secure_hdr->flashid = cpu_to_le32(e_flashid->flashid);
1230
1231         if (kwb_sign_csk_with_kak(params, secure_hdr, csk))
1232                 return 1;
1233
1234         image_ptr = ptr + headersz;
1235         image_size = payloadsz - headersz;
1236
1237         if (kwb_sign_and_verify(csk, image_ptr, image_size,
1238                                 &secure_hdr->imgsig, "image") < 0)
1239                 return 1;
1240
1241         if (kwb_sign_and_verify(csk, image, headersz, &tmp_sig, "header") < 0)
1242                 return 1;
1243
1244         secure_hdr->hdrsig = tmp_sig;
1245
1246         kwb_dump_fuse_cmds(secure_hdr);
1247
1248         return 0;
1249 }
1250
1251 static void *image_create_v1(size_t *imagesz, struct image_tool_params *params,
1252                              uint8_t *ptr, int payloadsz)
1253 {
1254         struct image_cfg_element *e;
1255         struct main_hdr_v1 *main_hdr;
1256         struct opt_hdr_v1 *ohdr;
1257         struct register_set_hdr_v1 *register_set_hdr;
1258         struct secure_hdr_v1 *secure_hdr = NULL;
1259         size_t headersz;
1260         uint8_t *image, *cur;
1261         int hasext = 0;
1262         uint8_t *next_ext = NULL;
1263         int cfgi, datai, size;
1264
1265         /*
1266          * Calculate the size of the header and the size of the
1267          * payload
1268          */
1269         headersz = image_headersz_v1(&hasext);
1270         if (headersz == 0)
1271                 return NULL;
1272
1273         image = malloc(headersz);
1274         if (!image) {
1275                 fprintf(stderr, "Cannot allocate memory for image\n");
1276                 return NULL;
1277         }
1278
1279         memset(image, 0, headersz);
1280
1281         main_hdr = (struct main_hdr_v1 *)image;
1282         cur = image;
1283         cur += sizeof(struct main_hdr_v1);
1284         next_ext = &main_hdr->ext;
1285
1286         /* Fill the main header */
1287         main_hdr->blocksize    =
1288                 cpu_to_le32(payloadsz);
1289         main_hdr->headersz_lsb = cpu_to_le16(headersz & 0xFFFF);
1290         main_hdr->headersz_msb = (headersz & 0xFFFF0000) >> 16;
1291         main_hdr->destaddr     = cpu_to_le32(params->addr);
1292         main_hdr->execaddr     = cpu_to_le32(params->ep);
1293         main_hdr->srcaddr      = cpu_to_le32(headersz);
1294         main_hdr->ext          = hasext;
1295         main_hdr->version      = 1;
1296         main_hdr->blockid      = image_get_bootfrom();
1297
1298         e = image_find_option(IMAGE_CFG_NAND_BLKSZ);
1299         if (e)
1300                 main_hdr->nandblocksize = e->nandblksz / (64 * 1024);
1301         e = image_find_option(IMAGE_CFG_NAND_PAGESZ);
1302         if (e)
1303                 main_hdr->nandpagesize = cpu_to_le16(e->nandpagesz);
1304         e = image_find_option(IMAGE_CFG_NAND_BADBLK_LOCATION);
1305         if (e)
1306                 main_hdr->nandbadblklocation = e->nandbadblklocation;
1307         e = image_find_option(IMAGE_CFG_BAUDRATE);
1308         if (e)
1309                 main_hdr->options |= baudrate_to_option(e->baudrate);
1310         e = image_find_option(IMAGE_CFG_UART_PORT);
1311         if (e)
1312                 main_hdr->options |= (e->uart_port & 3) << 3;
1313         e = image_find_option(IMAGE_CFG_UART_MPP);
1314         if (e)
1315                 main_hdr->options |= (e->uart_mpp & 7) << 5;
1316         e = image_find_option(IMAGE_CFG_DEBUG);
1317         if (e)
1318                 main_hdr->flags = e->debug ? 0x1 : 0;
1319
1320         /*
1321          * For SATA srcaddr is specified in number of sectors starting from
1322          * sector 0. The main header is stored at sector number 1.
1323          * This expects the sector size to be 512 bytes.
1324          * Header size is already aligned.
1325          */
1326         if (main_hdr->blockid == IBR_HDR_SATA_ID)
1327                 main_hdr->srcaddr = cpu_to_le32(headersz / 512 + 1);
1328
1329         /*
1330          * For SDIO srcaddr is specified in number of sectors starting from
1331          * sector 0. The main header is stored at sector number 0.
1332          * This expects sector size to be 512 bytes.
1333          * Header size is already aligned.
1334          */
1335         if (main_hdr->blockid == IBR_HDR_SDIO_ID)
1336                 main_hdr->srcaddr = cpu_to_le32(headersz / 512);
1337
1338         /* For PCIe srcaddr is not used and must be set to 0xFFFFFFFF. */
1339         if (main_hdr->blockid == IBR_HDR_PEX_ID)
1340                 main_hdr->srcaddr = cpu_to_le32(0xFFFFFFFF);
1341
1342         if (image_get_csk_index() >= 0) {
1343                 /*
1344                  * only reserve the space here; we fill the header later since
1345                  * we need the header to be complete to compute the signatures
1346                  */
1347                 secure_hdr = (struct secure_hdr_v1 *)cur;
1348                 cur += sizeof(struct secure_hdr_v1);
1349                 *next_ext = 1;
1350                 next_ext = &secure_hdr->next;
1351         }
1352
1353         datai = 0;
1354         register_set_hdr = (struct register_set_hdr_v1 *)cur;
1355         for (cfgi = 0; cfgi < cfgn; cfgi++) {
1356                 e = &image_cfg[cfgi];
1357                 if (e->type != IMAGE_CFG_DATA &&
1358                     e->type != IMAGE_CFG_DATA_DELAY)
1359                         continue;
1360                 if (e->type == IMAGE_CFG_DATA_DELAY) {
1361                         size = sizeof(struct register_set_hdr_v1) + 8 * datai + 4;
1362                         register_set_hdr->headertype = OPT_HDR_V1_REGISTER_TYPE;
1363                         register_set_hdr->headersz_lsb = cpu_to_le16(size & 0xFFFF);
1364                         register_set_hdr->headersz_msb = size >> 16;
1365                         register_set_hdr->data[datai].last_entry.delay = e->regdata_delay;
1366                         cur += size;
1367                         *next_ext = 1;
1368                         next_ext = &register_set_hdr->data[datai].last_entry.next;
1369                         datai = 0;
1370                         continue;
1371                 }
1372                 register_set_hdr->data[datai].entry.address =
1373                         cpu_to_le32(e->regdata.raddr);
1374                 register_set_hdr->data[datai].entry.value =
1375                         cpu_to_le32(e->regdata.rdata);
1376                 datai++;
1377         }
1378         if (datai != 0) {
1379                 size = sizeof(struct register_set_hdr_v1) + 8 * datai + 4;
1380                 register_set_hdr->headertype = OPT_HDR_V1_REGISTER_TYPE;
1381                 register_set_hdr->headersz_lsb = cpu_to_le16(size & 0xFFFF);
1382                 register_set_hdr->headersz_msb = size >> 16;
1383                 /* Set delay to the smallest possible value 1ms. */
1384                 register_set_hdr->data[datai].last_entry.delay = 1;
1385                 cur += size;
1386                 *next_ext = 1;
1387                 next_ext = &register_set_hdr->data[datai].last_entry.next;
1388         }
1389
1390         for (cfgi = 0; cfgi < cfgn; cfgi++) {
1391                 e = &image_cfg[cfgi];
1392                 if (e->type != IMAGE_CFG_BINARY)
1393                         continue;
1394
1395                 if (add_binary_header_v1(&cur, &next_ext, e, main_hdr))
1396                         return NULL;
1397         }
1398
1399         if (secure_hdr && add_secure_header_v1(params, ptr, payloadsz + headersz,
1400                                                headersz, image, secure_hdr))
1401                 return NULL;
1402
1403         *imagesz = headersz;
1404
1405         /* Fill the real header size without padding into the main header */
1406         headersz = sizeof(*main_hdr);
1407         for_each_opt_hdr_v1 (ohdr, main_hdr)
1408                 headersz += opt_hdr_v1_size(ohdr);
1409         main_hdr->headersz_lsb = cpu_to_le16(headersz & 0xFFFF);
1410         main_hdr->headersz_msb = (headersz & 0xFFFF0000) >> 16;
1411
1412         /* Calculate and set the header checksum */
1413         main_hdr->checksum = image_checksum8(main_hdr, headersz);
1414
1415         return image;
1416 }
1417
1418 int recognize_keyword(char *keyword)
1419 {
1420         int kw_id;
1421
1422         for (kw_id = 1; kw_id < IMAGE_CFG_COUNT; ++kw_id)
1423                 if (!strcmp(keyword, id_strs[kw_id]))
1424                         return kw_id;
1425
1426         return 0;
1427 }
1428
1429 static int image_create_config_parse_oneline(char *line,
1430                                              struct image_cfg_element *el)
1431 {
1432         char *keyword, *saveptr, *value1, *value2;
1433         char delimiters[] = " \t";
1434         int keyword_id, ret, argi;
1435         char *unknown_msg = "Ignoring unknown line '%s'\n";
1436
1437         keyword = strtok_r(line, delimiters, &saveptr);
1438         keyword_id = recognize_keyword(keyword);
1439
1440         if (!keyword_id) {
1441                 fprintf(stderr, unknown_msg, line);
1442                 return 0;
1443         }
1444
1445         el->type = keyword_id;
1446
1447         value1 = strtok_r(NULL, delimiters, &saveptr);
1448
1449         if (!value1) {
1450                 fprintf(stderr, "Parameter missing in line '%s'\n", line);
1451                 return -1;
1452         }
1453
1454         switch (keyword_id) {
1455         case IMAGE_CFG_VERSION:
1456                 el->version = atoi(value1);
1457                 break;
1458         case IMAGE_CFG_BOOT_FROM:
1459                 ret = image_boot_mode_id(value1);
1460
1461                 if (ret < 0) {
1462                         fprintf(stderr, "Invalid boot media '%s'\n", value1);
1463                         return -1;
1464                 }
1465                 el->bootfrom = ret;
1466                 break;
1467         case IMAGE_CFG_NAND_BLKSZ:
1468                 el->nandblksz = strtoul(value1, NULL, 16);
1469                 break;
1470         case IMAGE_CFG_NAND_BADBLK_LOCATION:
1471                 el->nandbadblklocation = strtoul(value1, NULL, 16);
1472                 break;
1473         case IMAGE_CFG_NAND_ECC_MODE:
1474                 ret = image_nand_ecc_mode_id(value1);
1475
1476                 if (ret < 0) {
1477                         fprintf(stderr, "Invalid NAND ECC mode '%s'\n", value1);
1478                         return -1;
1479                 }
1480                 el->nandeccmode = ret;
1481                 break;
1482         case IMAGE_CFG_NAND_PAGESZ:
1483                 el->nandpagesz = strtoul(value1, NULL, 16);
1484                 break;
1485         case IMAGE_CFG_BINARY:
1486                 argi = 0;
1487
1488                 el->binary.file = strdup(value1);
1489                 while (1) {
1490                         char *value = strtok_r(NULL, delimiters, &saveptr);
1491
1492                         if (!value)
1493                                 break;
1494                         el->binary.args[argi] = strtoul(value, NULL, 16);
1495                         argi++;
1496                         if (argi >= BINARY_MAX_ARGS) {
1497                                 fprintf(stderr,
1498                                         "Too many arguments for BINARY\n");
1499                                 return -1;
1500                         }
1501                 }
1502                 el->binary.nargs = argi;
1503                 break;
1504         case IMAGE_CFG_DATA:
1505                 value2 = strtok_r(NULL, delimiters, &saveptr);
1506
1507                 if (!value1 || !value2) {
1508                         fprintf(stderr,
1509                                 "Invalid number of arguments for DATA\n");
1510                         return -1;
1511                 }
1512
1513                 el->regdata.raddr = strtoul(value1, NULL, 16);
1514                 el->regdata.rdata = strtoul(value2, NULL, 16);
1515                 break;
1516         case IMAGE_CFG_DATA_DELAY:
1517                 if (!strcmp(value1, "SDRAM_SETUP"))
1518                         el->regdata_delay = REGISTER_SET_HDR_OPT_DELAY_SDRAM_SETUP;
1519                 else
1520                         el->regdata_delay = REGISTER_SET_HDR_OPT_DELAY_MS(strtoul(value1, NULL, 10));
1521                 break;
1522         case IMAGE_CFG_BAUDRATE:
1523                 el->baudrate = strtoul(value1, NULL, 10);
1524                 break;
1525         case IMAGE_CFG_UART_PORT:
1526                 el->uart_port = strtoul(value1, NULL, 16);
1527                 break;
1528         case IMAGE_CFG_UART_MPP:
1529                 el->uart_mpp = strtoul(value1, NULL, 16);
1530                 break;
1531         case IMAGE_CFG_DEBUG:
1532                 el->debug = strtoul(value1, NULL, 10);
1533                 break;
1534         case IMAGE_CFG_KAK:
1535                 el->key_name = strdup(value1);
1536                 break;
1537         case IMAGE_CFG_CSK:
1538                 el->key_name = strdup(value1);
1539                 break;
1540         case IMAGE_CFG_CSK_INDEX:
1541                 el->csk_idx = strtol(value1, NULL, 0);
1542                 break;
1543         case IMAGE_CFG_JTAG_DELAY:
1544                 el->jtag_delay = strtoul(value1, NULL, 0);
1545                 break;
1546         case IMAGE_CFG_BOX_ID:
1547                 el->boxid = strtoul(value1, NULL, 0);
1548                 break;
1549         case IMAGE_CFG_FLASH_ID:
1550                 el->flashid = strtoul(value1, NULL, 0);
1551                 break;
1552         case IMAGE_CFG_SEC_SPECIALIZED_IMG:
1553                 el->sec_specialized_img = true;
1554                 break;
1555         case IMAGE_CFG_SEC_COMMON_IMG:
1556                 el->sec_specialized_img = false;
1557                 break;
1558         case IMAGE_CFG_SEC_BOOT_DEV:
1559                 el->sec_boot_dev = strtoul(value1, NULL, 0);
1560                 break;
1561         case IMAGE_CFG_SEC_FUSE_DUMP:
1562                 el->name = strdup(value1);
1563                 break;
1564         default:
1565                 fprintf(stderr, unknown_msg, line);
1566         }
1567
1568         return 0;
1569 }
1570
1571 /*
1572  * Parse the configuration file 'fcfg' into the array of configuration
1573  * elements 'image_cfg', and return the number of configuration
1574  * elements in 'cfgn'.
1575  */
1576 static int image_create_config_parse(FILE *fcfg)
1577 {
1578         int ret;
1579         int cfgi = 0;
1580
1581         /* Parse the configuration file */
1582         while (!feof(fcfg)) {
1583                 char *line;
1584                 char buf[256];
1585
1586                 /* Read the current line */
1587                 memset(buf, 0, sizeof(buf));
1588                 line = fgets(buf, sizeof(buf), fcfg);
1589                 if (!line)
1590                         break;
1591
1592                 /* Ignore useless lines */
1593                 if (line[0] == '\n' || line[0] == '#')
1594                         continue;
1595
1596                 /* Strip final newline */
1597                 if (line[strlen(line) - 1] == '\n')
1598                         line[strlen(line) - 1] = 0;
1599
1600                 /* Parse the current line */
1601                 ret = image_create_config_parse_oneline(line,
1602                                                         &image_cfg[cfgi]);
1603                 if (ret)
1604                         return ret;
1605
1606                 cfgi++;
1607
1608                 if (cfgi >= IMAGE_CFG_ELEMENT_MAX) {
1609                         fprintf(stderr,
1610                                 "Too many configuration elements in .cfg file\n");
1611                         return -1;
1612                 }
1613         }
1614
1615         cfgn = cfgi;
1616         return 0;
1617 }
1618
1619 static int image_get_version(void)
1620 {
1621         struct image_cfg_element *e;
1622
1623         e = image_find_option(IMAGE_CFG_VERSION);
1624         if (!e)
1625                 return -1;
1626
1627         return e->version;
1628 }
1629
1630 static void kwbimage_set_header(void *ptr, struct stat *sbuf, int ifd,
1631                                 struct image_tool_params *params)
1632 {
1633         FILE *fcfg;
1634         void *image = NULL;
1635         int version;
1636         size_t headersz = 0;
1637         size_t datasz;
1638         uint32_t checksum;
1639         struct stat s;
1640         int ret;
1641
1642         /*
1643          * Do not use sbuf->st_size as it contains size with padding.
1644          * We need original image data size, so stat original file.
1645          */
1646         if (stat(params->datafile, &s)) {
1647                 fprintf(stderr, "Could not stat data file %s: %s\n",
1648                         params->datafile, strerror(errno));
1649                 exit(EXIT_FAILURE);
1650         }
1651         datasz = ALIGN(s.st_size, 4);
1652
1653         fcfg = fopen(params->imagename, "r");
1654         if (!fcfg) {
1655                 fprintf(stderr, "Could not open input file %s\n",
1656                         params->imagename);
1657                 exit(EXIT_FAILURE);
1658         }
1659
1660         image_cfg = malloc(IMAGE_CFG_ELEMENT_MAX *
1661                            sizeof(struct image_cfg_element));
1662         if (!image_cfg) {
1663                 fprintf(stderr, "Cannot allocate memory\n");
1664                 fclose(fcfg);
1665                 exit(EXIT_FAILURE);
1666         }
1667
1668         memset(image_cfg, 0,
1669                IMAGE_CFG_ELEMENT_MAX * sizeof(struct image_cfg_element));
1670         rewind(fcfg);
1671
1672         ret = image_create_config_parse(fcfg);
1673         fclose(fcfg);
1674         if (ret) {
1675                 free(image_cfg);
1676                 exit(EXIT_FAILURE);
1677         }
1678
1679         version = image_get_version();
1680         switch (version) {
1681                 /*
1682                  * Fallback to version 0 if no version is provided in the
1683                  * cfg file
1684                  */
1685         case -1:
1686         case 0:
1687                 image = image_create_v0(&headersz, params, datasz + 4);
1688                 break;
1689
1690         case 1:
1691                 image = image_create_v1(&headersz, params, ptr, datasz + 4);
1692                 break;
1693
1694         default:
1695                 fprintf(stderr, "Unsupported version %d\n", version);
1696                 free(image_cfg);
1697                 exit(EXIT_FAILURE);
1698         }
1699
1700         if (!image) {
1701                 fprintf(stderr, "Could not create image\n");
1702                 free(image_cfg);
1703                 exit(EXIT_FAILURE);
1704         }
1705
1706         free(image_cfg);
1707
1708         /* Build and add image data checksum */
1709         checksum = cpu_to_le32(image_checksum32((uint8_t *)ptr + headersz,
1710                                                 datasz));
1711         memcpy((uint8_t *)ptr + headersz + datasz, &checksum, sizeof(uint32_t));
1712
1713         /* Finally copy the header into the image area */
1714         memcpy(ptr, image, headersz);
1715
1716         free(image);
1717 }
1718
1719 static void kwbimage_print_header(const void *ptr)
1720 {
1721         struct main_hdr_v0 *mhdr = (struct main_hdr_v0 *)ptr;
1722         struct opt_hdr_v1 *ohdr;
1723
1724         printf("Image Type:   MVEBU Boot from %s Image\n",
1725                image_boot_mode_name(mhdr->blockid));
1726         printf("Image version:%d\n", kwbimage_version(ptr));
1727
1728         for_each_opt_hdr_v1 (ohdr, mhdr) {
1729                 if (ohdr->headertype == OPT_HDR_V1_BINARY_TYPE) {
1730                         printf("BIN Hdr Size: ");
1731                         genimg_print_size(opt_hdr_v1_size(ohdr) - 12 -
1732                                           4 * ohdr->data[0]);
1733                 }
1734         }
1735
1736         printf("Data Size:    ");
1737         genimg_print_size(mhdr->blocksize - sizeof(uint32_t));
1738         printf("Load Address: %08x\n", mhdr->destaddr);
1739         printf("Entry Point:  %08x\n", mhdr->execaddr);
1740 }
1741
1742 static int kwbimage_check_image_types(uint8_t type)
1743 {
1744         if (type == IH_TYPE_KWBIMAGE)
1745                 return EXIT_SUCCESS;
1746
1747         return EXIT_FAILURE;
1748 }
1749
1750 static int kwbimage_verify_header(unsigned char *ptr, int image_size,
1751                                   struct image_tool_params *params)
1752 {
1753         size_t header_size = kwbheader_size(ptr);
1754         uint8_t blockid;
1755         uint32_t offset;
1756         uint32_t size;
1757         uint8_t csum;
1758
1759         if (header_size > image_size)
1760                 return -FDT_ERR_BADSTRUCTURE;
1761
1762         if (!main_hdr_checksum_ok(ptr))
1763                 return -FDT_ERR_BADSTRUCTURE;
1764
1765         /* Only version 0 extended header has checksum */
1766         if (kwbimage_version(ptr) == 0) {
1767                 struct main_hdr_v0 *mhdr = (struct main_hdr_v0 *)ptr;
1768
1769                 if (mhdr->ext & 0x1) {
1770                         struct ext_hdr_v0 *ext_hdr = (void *)(mhdr + 1);
1771
1772                         csum = image_checksum8(ext_hdr, sizeof(*ext_hdr) - 1);
1773                         if (csum != ext_hdr->checksum)
1774                                 return -FDT_ERR_BADSTRUCTURE;
1775                 }
1776
1777                 blockid = mhdr->blockid;
1778                 offset = le32_to_cpu(mhdr->srcaddr);
1779                 size = le32_to_cpu(mhdr->blocksize);
1780         } else if (kwbimage_version(ptr) == 1) {
1781                 struct main_hdr_v1 *mhdr = (struct main_hdr_v1 *)ptr;
1782                 const uint8_t *mhdr_end;
1783                 struct opt_hdr_v1 *ohdr;
1784
1785                 mhdr_end = (uint8_t *)mhdr + header_size;
1786                 for_each_opt_hdr_v1 (ohdr, ptr)
1787                         if (!opt_hdr_v1_valid_size(ohdr, mhdr_end))
1788                                 return -FDT_ERR_BADSTRUCTURE;
1789
1790                 blockid = mhdr->blockid;
1791                 offset = le32_to_cpu(mhdr->srcaddr);
1792                 size = le32_to_cpu(mhdr->blocksize);
1793         } else {
1794                 return -FDT_ERR_BADSTRUCTURE;
1795         }
1796
1797         /*
1798          * For SATA srcaddr is specified in number of sectors.
1799          * The main header is must be stored at sector number 1.
1800          * This expects that sector size is 512 bytes and recalculates
1801          * data offset to bytes relative to the main header.
1802          */
1803         if (blockid == IBR_HDR_SATA_ID) {
1804                 if (offset < 1)
1805                         return -FDT_ERR_BADSTRUCTURE;
1806                 offset -= 1;
1807                 offset *= 512;
1808         }
1809
1810         /*
1811          * For SDIO srcaddr is specified in number of sectors.
1812          * This expects that sector size is 512 bytes and recalculates
1813          * data offset to bytes.
1814          */
1815         if (blockid == IBR_HDR_SDIO_ID)
1816                 offset *= 512;
1817
1818         /*
1819          * For PCIe srcaddr is always set to 0xFFFFFFFF.
1820          * This expects that data starts after all headers.
1821          */
1822         if (blockid == IBR_HDR_PEX_ID && offset == 0xFFFFFFFF)
1823                 offset = header_size;
1824
1825         if (offset > image_size || offset % 4 != 0)
1826                 return -FDT_ERR_BADSTRUCTURE;
1827
1828         if (size < 4 || offset + size > image_size || size % 4 != 0)
1829                 return -FDT_ERR_BADSTRUCTURE;
1830
1831         if (image_checksum32(ptr + offset, size - 4) !=
1832             *(uint32_t *)(ptr + offset + size - 4))
1833                 return -FDT_ERR_BADSTRUCTURE;
1834
1835         return 0;
1836 }
1837
1838 static int kwbimage_generate(struct image_tool_params *params,
1839                              struct image_type_params *tparams)
1840 {
1841         FILE *fcfg;
1842         struct stat s;
1843         int alloc_len;
1844         int bootfrom;
1845         int version;
1846         void *hdr;
1847         int ret;
1848
1849         fcfg = fopen(params->imagename, "r");
1850         if (!fcfg) {
1851                 fprintf(stderr, "Could not open input file %s\n",
1852                         params->imagename);
1853                 exit(EXIT_FAILURE);
1854         }
1855
1856         if (stat(params->datafile, &s)) {
1857                 fprintf(stderr, "Could not stat data file %s: %s\n",
1858                         params->datafile, strerror(errno));
1859                 exit(EXIT_FAILURE);
1860         }
1861
1862         image_cfg = malloc(IMAGE_CFG_ELEMENT_MAX *
1863                            sizeof(struct image_cfg_element));
1864         if (!image_cfg) {
1865                 fprintf(stderr, "Cannot allocate memory\n");
1866                 fclose(fcfg);
1867                 exit(EXIT_FAILURE);
1868         }
1869
1870         memset(image_cfg, 0,
1871                IMAGE_CFG_ELEMENT_MAX * sizeof(struct image_cfg_element));
1872         rewind(fcfg);
1873
1874         ret = image_create_config_parse(fcfg);
1875         fclose(fcfg);
1876         if (ret) {
1877                 free(image_cfg);
1878                 exit(EXIT_FAILURE);
1879         }
1880
1881         bootfrom = image_get_bootfrom();
1882         version = image_get_version();
1883         switch (version) {
1884                 /*
1885                  * Fallback to version 0 if no version is provided in the
1886                  * cfg file
1887                  */
1888         case -1:
1889         case 0:
1890                 alloc_len = image_headersz_v0(NULL);
1891                 break;
1892
1893         case 1:
1894                 alloc_len = image_headersz_v1(NULL);
1895                 break;
1896
1897         default:
1898                 fprintf(stderr, "Unsupported version %d\n", version);
1899                 free(image_cfg);
1900                 exit(EXIT_FAILURE);
1901         }
1902
1903         free(image_cfg);
1904
1905         hdr = malloc(alloc_len);
1906         if (!hdr) {
1907                 fprintf(stderr, "%s: malloc return failure: %s\n",
1908                         params->cmdname, strerror(errno));
1909                 exit(EXIT_FAILURE);
1910         }
1911
1912         memset(hdr, 0, alloc_len);
1913         tparams->header_size = alloc_len;
1914         tparams->hdr = hdr;
1915
1916         /*
1917          * The resulting image needs to be 4-byte aligned. At least
1918          * the Marvell hdrparser tool complains if its unaligned.
1919          * After the image data is stored 4-byte checksum.
1920          * Final UART image must be aligned to 128 bytes.
1921          * Final SPI and NAND images must be aligned to 256 bytes.
1922          * Final SATA and SDIO images must be aligned to 512 bytes.
1923          */
1924         if (bootfrom == IBR_HDR_SPI_ID || bootfrom == IBR_HDR_NAND_ID)
1925                 return 4 + (256 - (alloc_len + s.st_size + 4) % 256) % 256;
1926         else if (bootfrom == IBR_HDR_SATA_ID || bootfrom == IBR_HDR_SDIO_ID)
1927                 return 4 + (512 - (alloc_len + s.st_size + 4) % 512) % 512;
1928         else if (bootfrom == IBR_HDR_UART_ID)
1929                 return 4 + (128 - (alloc_len + s.st_size + 4) % 128) % 128;
1930         else
1931                 return 4 + (4 - s.st_size % 4) % 4;
1932 }
1933
1934 static int kwbimage_extract_subimage(void *ptr, struct image_tool_params *params)
1935 {
1936         struct main_hdr_v1 *mhdr = (struct main_hdr_v1 *)ptr;
1937         size_t header_size = kwbheader_size(ptr);
1938         struct opt_hdr_v1 *ohdr;
1939         int idx = params->pflag;
1940         int cur_idx = 0;
1941         uint32_t offset;
1942         ulong image;
1943         ulong size;
1944
1945         for_each_opt_hdr_v1 (ohdr, ptr) {
1946                 if (ohdr->headertype != OPT_HDR_V1_BINARY_TYPE)
1947                         continue;
1948
1949                 if (idx == cur_idx) {
1950                         image = (ulong)&ohdr->data[4 + 4 * ohdr->data[0]];
1951                         size = opt_hdr_v1_size(ohdr) - 12 - 4 * ohdr->data[0];
1952                         goto extract;
1953                 }
1954
1955                 ++cur_idx;
1956         }
1957
1958         if (idx != cur_idx) {
1959                 printf("Image %d is not present\n", idx);
1960                 return -1;
1961         }
1962
1963         offset = le32_to_cpu(mhdr->srcaddr);
1964
1965         if (mhdr->blockid == IBR_HDR_SATA_ID) {
1966                 offset -= 1;
1967                 offset *= 512;
1968         }
1969
1970         if (mhdr->blockid == IBR_HDR_SDIO_ID)
1971                 offset *= 512;
1972
1973         if (mhdr->blockid == IBR_HDR_PEX_ID && offset == 0xFFFFFFFF)
1974                 offset = header_size;
1975
1976         image = (ulong)((uint8_t *)ptr + offset);
1977         size = le32_to_cpu(mhdr->blocksize) - 4;
1978
1979 extract:
1980         return imagetool_save_subimage(params->outfile, image, size);
1981 }
1982
1983 /*
1984  * Report Error if xflag is set in addition to default
1985  */
1986 static int kwbimage_check_params(struct image_tool_params *params)
1987 {
1988         if (!params->iflag && (!params->imagename || !strlen(params->imagename))) {
1989                 char *msg = "Configuration file for kwbimage creation omitted";
1990
1991                 fprintf(stderr, "Error:%s - %s\n", params->cmdname, msg);
1992                 return 1;
1993         }
1994
1995         return (params->dflag && (params->fflag || params->lflag)) ||
1996                 (params->fflag && (params->dflag || params->lflag)) ||
1997                 (params->lflag && (params->dflag || params->fflag)) ||
1998                 (params->xflag);
1999 }
2000
2001 /*
2002  * kwbimage type parameters definition
2003  */
2004 U_BOOT_IMAGE_TYPE(
2005         kwbimage,
2006         "Marvell MVEBU Boot Image support",
2007         0,
2008         NULL,
2009         kwbimage_check_params,
2010         kwbimage_verify_header,
2011         kwbimage_print_header,
2012         kwbimage_set_header,
2013         kwbimage_extract_subimage,
2014         kwbimage_check_image_types,
2015         NULL,
2016         kwbimage_generate
2017 );