tools/kwbimage.c

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