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