tools/kwbimage.c

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