tools/kwbimage.c

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