tools/kwbimage.c

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