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 if (cpu_sheeva) {
1105                         headersz = ALIGN(headersz, 16);
1106                 } else {
1107                         headersz = ALIGN(headersz, 4);
1108                 }
1109
1110                 headersz += ALIGN(s.st_size, 4) + sizeof(uint32_t);
1111                 if (hasext)
1112                         *hasext = 1;
1113         }
1114
1115         if (count > 0)
1116                 headersz += sizeof(struct register_set_hdr_v1) + 8 * count + 4;
1117
1118         return image_headersz_align(headersz, image_get_bootfrom());
1119 }
1120
1121 static int add_binary_header_v1(uint8_t **cur, uint8_t **next_ext,
1122                                 struct image_cfg_element *binarye,
1123                                 struct main_hdr_v1 *main_hdr)
1124 {
1125         struct opt_hdr_v1 *hdr = (struct opt_hdr_v1 *)*cur;
1126         uint32_t base_addr;
1127         uint32_t add_args;
1128         uint32_t offset;
1129         uint32_t *args;
1130         size_t binhdrsz;
1131         int cpu_sheeva;
1132         struct stat s;
1133         int argi;
1134         FILE *bin;
1135         int ret;
1136
1137         hdr->headertype = OPT_HDR_V1_BINARY_TYPE;
1138
1139         bin = fopen(binarye->binary.file, "r");
1140         if (!bin) {
1141                 fprintf(stderr, "Cannot open binary file %s\n",
1142                         binarye->binary.file);
1143                 return -1;
1144         }
1145
1146         if (fstat(fileno(bin), &s)) {
1147                 fprintf(stderr, "Cannot stat binary file %s\n",
1148                         binarye->binary.file);
1149                 goto err_close;
1150         }
1151
1152         *cur += sizeof(struct opt_hdr_v1);
1153
1154         args = (uint32_t *)*cur;
1155         *args = cpu_to_le32(binarye->binary.nargs);
1156         args++;
1157         for (argi = 0; argi < binarye->binary.nargs; argi++)
1158                 args[argi] = cpu_to_le32(binarye->binary.args[argi]);
1159
1160         *cur += (binarye->binary.nargs + 1) * sizeof(uint32_t);
1161
1162         /*
1163          * ARM executable code inside the BIN header on platforms with Sheeva
1164          * CPU (A370 and AXP) must always be aligned with the 128-bit boundary.
1165          * In the case when this code is not position independent (e.g. ARM
1166          * SPL), it must be placed at fixed load and execute address.
1167          * This requirement can be met by inserting dummy arguments into
1168          * BIN header, if needed.
1169          */
1170         cpu_sheeva = image_is_cpu_sheeva();
1171         base_addr = cpu_sheeva ? 0x40004000 : 0x40000000;
1172         offset = *cur - (uint8_t *)main_hdr;
1173         if (binarye->binary.loadaddr)
1174                 add_args = (binarye->binary.loadaddr - base_addr - offset) / sizeof(uint32_t);
1175         else if (cpu_sheeva)
1176                 add_args = ((16 - offset % 16) % 16) / sizeof(uint32_t);
1177         else
1178                 add_args = 0;
1179         if (add_args) {
1180                 *(args - 1) = cpu_to_le32(binarye->binary.nargs + add_args);
1181                 *cur += add_args * sizeof(uint32_t);
1182         }
1183
1184         ret = fread(*cur, s.st_size, 1, bin);
1185         if (ret != 1) {
1186                 fprintf(stderr,
1187                         "Could not read binary image %s\n",
1188                         binarye->binary.file);
1189                 goto err_close;
1190         }
1191
1192         fclose(bin);
1193
1194         *cur += ALIGN(s.st_size, 4);
1195
1196         *((uint32_t *)*cur) = 0x00000000;
1197         **next_ext = 1;
1198         *next_ext = *cur;
1199
1200         *cur += sizeof(uint32_t);
1201
1202         binhdrsz = sizeof(struct opt_hdr_v1) +
1203                 (binarye->binary.nargs + add_args + 2) * sizeof(uint32_t) +
1204                 ALIGN(s.st_size, 4);
1205         hdr->headersz_lsb = cpu_to_le16(binhdrsz & 0xFFFF);
1206         hdr->headersz_msb = (binhdrsz & 0xFFFF0000) >> 16;
1207
1208         return 0;
1209
1210 err_close:
1211         fclose(bin);
1212
1213         return -1;
1214 }
1215
1216 static int export_pub_kak_hash(RSA *kak, struct secure_hdr_v1 *secure_hdr)
1217 {
1218         FILE *hashf;
1219         int res;
1220
1221         hashf = fopen("pub_kak_hash.txt", "w");
1222         if (!hashf) {
1223                 fprintf(stderr, "Couldn't open hash file: '%s': %s\n",
1224                         "pub_kak_hash.txt", strerror(errno));
1225                 return 1;
1226         }
1227
1228         res = kwb_export_pubkey(kak, &secure_hdr->kak, hashf, "KAK");
1229
1230         fclose(hashf);
1231
1232         return res < 0 ? 1 : 0;
1233 }
1234
1235 static int kwb_sign_csk_with_kak(struct image_tool_params *params,
1236                                  struct secure_hdr_v1 *secure_hdr, RSA *csk)
1237 {
1238         RSA *kak = NULL;
1239         RSA *kak_pub = NULL;
1240         int csk_idx = image_get_csk_index();
1241         struct sig_v1 tmp_sig;
1242
1243         if (csk_idx < 0 || csk_idx > 15) {
1244                 fprintf(stderr, "Invalid CSK index %d\n", csk_idx);
1245                 return 1;
1246         }
1247
1248         if (kwb_load_kak(params, &kak) < 0)
1249                 return 1;
1250
1251         if (export_pub_kak_hash(kak, secure_hdr))
1252                 return 1;
1253
1254         if (kwb_import_pubkey(&kak_pub, &secure_hdr->kak, "KAK") < 0)
1255                 return 1;
1256
1257         if (kwb_export_pubkey(csk, &secure_hdr->csk[csk_idx], NULL, "CSK") < 0)
1258                 return 1;
1259
1260         if (kwb_sign_and_verify(kak, &secure_hdr->csk,
1261                                 sizeof(secure_hdr->csk) +
1262                                 sizeof(secure_hdr->csksig),
1263                                 &tmp_sig, "CSK") < 0)
1264                 return 1;
1265
1266         if (kwb_verify(kak_pub, &secure_hdr->csk,
1267                        sizeof(secure_hdr->csk) +
1268                        sizeof(secure_hdr->csksig),
1269                        &tmp_sig, "CSK (2)") < 0)
1270                 return 1;
1271
1272         secure_hdr->csksig = tmp_sig;
1273
1274         return 0;
1275 }
1276
1277 static int add_secure_header_v1(struct image_tool_params *params, uint8_t *ptr,
1278                                 int payloadsz, size_t headersz, uint8_t *image,
1279                                 struct secure_hdr_v1 *secure_hdr)
1280 {
1281         struct image_cfg_element *e_jtagdelay;
1282         struct image_cfg_element *e_boxid;
1283         struct image_cfg_element *e_flashid;
1284         RSA *csk = NULL;
1285         unsigned char *image_ptr;
1286         size_t image_size;
1287         struct sig_v1 tmp_sig;
1288         bool specialized_img = image_get_spezialized_img();
1289
1290         kwb_msg("Create secure header content\n");
1291
1292         e_jtagdelay = image_find_option(IMAGE_CFG_JTAG_DELAY);
1293         e_boxid = image_find_option(IMAGE_CFG_BOX_ID);
1294         e_flashid = image_find_option(IMAGE_CFG_FLASH_ID);
1295
1296         if (kwb_load_csk(params, &csk) < 0)
1297                 return 1;
1298
1299         secure_hdr->headertype = OPT_HDR_V1_SECURE_TYPE;
1300         secure_hdr->headersz_msb = 0;
1301         secure_hdr->headersz_lsb = cpu_to_le16(sizeof(struct secure_hdr_v1));
1302         if (e_jtagdelay)
1303                 secure_hdr->jtag_delay = e_jtagdelay->jtag_delay;
1304         if (e_boxid && specialized_img)
1305                 secure_hdr->boxid = cpu_to_le32(e_boxid->boxid);
1306         if (e_flashid && specialized_img)
1307                 secure_hdr->flashid = cpu_to_le32(e_flashid->flashid);
1308
1309         if (kwb_sign_csk_with_kak(params, secure_hdr, csk))
1310                 return 1;
1311
1312         image_ptr = ptr + headersz;
1313         image_size = payloadsz - headersz;
1314
1315         if (kwb_sign_and_verify(csk, image_ptr, image_size,
1316                                 &secure_hdr->imgsig, "image") < 0)
1317                 return 1;
1318
1319         if (kwb_sign_and_verify(csk, image, headersz, &tmp_sig, "header") < 0)
1320                 return 1;
1321
1322         secure_hdr->hdrsig = tmp_sig;
1323
1324         kwb_dump_fuse_cmds(secure_hdr);
1325
1326         return 0;
1327 }
1328
1329 static void finish_register_set_header_v1(uint8_t **cur, uint8_t **next_ext,
1330                                           struct register_set_hdr_v1 *register_set_hdr,
1331                                           int *datai, uint8_t delay)
1332 {
1333         int size = sizeof(struct register_set_hdr_v1) + 8 * (*datai) + 4;
1334
1335         register_set_hdr->headertype = OPT_HDR_V1_REGISTER_TYPE;
1336         register_set_hdr->headersz_lsb = cpu_to_le16(size & 0xFFFF);
1337         register_set_hdr->headersz_msb = size >> 16;
1338         register_set_hdr->data[*datai].last_entry.delay = delay;
1339         *cur += size;
1340         **next_ext = 1;
1341         *next_ext = &register_set_hdr->data[*datai].last_entry.next;
1342         *datai = 0;
1343 }
1344
1345 static void *image_create_v1(size_t *imagesz, struct image_tool_params *params,
1346                              uint8_t *ptr, int payloadsz)
1347 {
1348         struct image_cfg_element *e;
1349         struct main_hdr_v1 *main_hdr;
1350         struct opt_hdr_v1 *ohdr;
1351         struct register_set_hdr_v1 *register_set_hdr;
1352         struct secure_hdr_v1 *secure_hdr = NULL;
1353         size_t headersz;
1354         uint8_t *image, *cur;
1355         int hasext = 0;
1356         uint8_t *next_ext = NULL;
1357         int cfgi, datai;
1358         uint8_t delay;
1359
1360         /*
1361          * Calculate the size of the header and the size of the
1362          * payload
1363          */
1364         headersz = image_headersz_v1(&hasext);
1365         if (headersz == 0)
1366                 return NULL;
1367
1368         image = malloc(headersz);
1369         if (!image) {
1370                 fprintf(stderr, "Cannot allocate memory for image\n");
1371                 return NULL;
1372         }
1373
1374         memset(image, 0, headersz);
1375
1376         main_hdr = (struct main_hdr_v1 *)image;
1377         cur = image;
1378         cur += sizeof(struct main_hdr_v1);
1379         next_ext = &main_hdr->ext;
1380
1381         /* Fill the main header */
1382         main_hdr->blocksize    =
1383                 cpu_to_le32(payloadsz);
1384         main_hdr->headersz_lsb = cpu_to_le16(headersz & 0xFFFF);
1385         main_hdr->headersz_msb = (headersz & 0xFFFF0000) >> 16;
1386         main_hdr->destaddr     = cpu_to_le32(params->addr);
1387         main_hdr->execaddr     = cpu_to_le32(params->ep);
1388         main_hdr->srcaddr      = cpu_to_le32(headersz);
1389         main_hdr->ext          = hasext;
1390         main_hdr->version      = 1;
1391         main_hdr->blockid      = image_get_bootfrom();
1392
1393         e = image_find_option(IMAGE_CFG_NAND_BLKSZ);
1394         if (e)
1395                 main_hdr->nandblocksize = e->nandblksz / (64 * 1024);
1396         e = image_find_option(IMAGE_CFG_NAND_PAGESZ);
1397         if (e)
1398                 main_hdr->nandpagesize = cpu_to_le16(e->nandpagesz);
1399         e = image_find_option(IMAGE_CFG_NAND_BADBLK_LOCATION);
1400         if (e)
1401                 main_hdr->nandbadblklocation = e->nandbadblklocation;
1402         e = image_find_option(IMAGE_CFG_BAUDRATE);
1403         if (e)
1404                 main_hdr->options |= baudrate_to_option(e->baudrate);
1405         e = image_find_option(IMAGE_CFG_UART_PORT);
1406         if (e)
1407                 main_hdr->options |= (e->uart_port & 3) << 3;
1408         e = image_find_option(IMAGE_CFG_UART_MPP);
1409         if (e)
1410                 main_hdr->options |= (e->uart_mpp & 7) << 5;
1411         e = image_find_option(IMAGE_CFG_DEBUG);
1412         if (e)
1413                 main_hdr->flags = e->debug ? 0x1 : 0;
1414
1415         /*
1416          * For SATA srcaddr is specified in number of sectors starting from
1417          * sector 0. The main header is stored at sector number 1.
1418          * This expects the sector size to be 512 bytes.
1419          * Header size is already aligned.
1420          */
1421         if (main_hdr->blockid == IBR_HDR_SATA_ID)
1422                 main_hdr->srcaddr = cpu_to_le32(headersz / 512 + 1);
1423
1424         /*
1425          * For SDIO srcaddr is specified in number of sectors starting from
1426          * sector 0. The main header is stored at sector number 0.
1427          * This expects sector size to be 512 bytes.
1428          * Header size is already aligned.
1429          */
1430         if (main_hdr->blockid == IBR_HDR_SDIO_ID)
1431                 main_hdr->srcaddr = cpu_to_le32(headersz / 512);
1432
1433         /* For PCIe srcaddr is not used and must be set to 0xFFFFFFFF. */
1434         if (main_hdr->blockid == IBR_HDR_PEX_ID)
1435                 main_hdr->srcaddr = cpu_to_le32(0xFFFFFFFF);
1436
1437         if (image_get_csk_index() >= 0) {
1438                 /*
1439                  * only reserve the space here; we fill the header later since
1440                  * we need the header to be complete to compute the signatures
1441                  */
1442                 secure_hdr = (struct secure_hdr_v1 *)cur;
1443                 cur += sizeof(struct secure_hdr_v1);
1444                 *next_ext = 1;
1445                 next_ext = &secure_hdr->next;
1446         }
1447
1448         datai = 0;
1449         for (cfgi = 0; cfgi < cfgn; cfgi++) {
1450                 e = &image_cfg[cfgi];
1451                 if (e->type != IMAGE_CFG_DATA &&
1452                     e->type != IMAGE_CFG_DATA_DELAY &&
1453                     e->type != IMAGE_CFG_BINARY)
1454                         continue;
1455
1456                 if (datai == 0)
1457                         register_set_hdr = (struct register_set_hdr_v1 *)cur;
1458
1459                 /* If delay is not specified, use the smallest possible value. */
1460                 if (e->type == IMAGE_CFG_DATA_DELAY)
1461                         delay = e->regdata_delay;
1462                 else
1463                         delay = REGISTER_SET_HDR_OPT_DELAY_MS(0);
1464
1465                 /*
1466                  * DATA_DELAY command is the last entry in the register set
1467                  * header and BINARY command inserts new binary header.
1468                  * Therefore BINARY command requires to finish register set
1469                  * header if some DATA command was specified. And DATA_DELAY
1470                  * command automatically finish register set header even when
1471                  * there was no DATA command.
1472                  */
1473                 if (e->type == IMAGE_CFG_DATA_DELAY ||
1474                     (e->type == IMAGE_CFG_BINARY && datai != 0))
1475                         finish_register_set_header_v1(&cur, &next_ext, register_set_hdr,
1476                                                       &datai, delay);
1477
1478                 if (e->type == IMAGE_CFG_DATA) {
1479                         register_set_hdr->data[datai].entry.address =
1480                                 cpu_to_le32(e->regdata.raddr);
1481                         register_set_hdr->data[datai].entry.value =
1482                                 cpu_to_le32(e->regdata.rdata);
1483                         datai++;
1484                 }
1485
1486                 if (e->type == IMAGE_CFG_BINARY) {
1487                         if (add_binary_header_v1(&cur, &next_ext, e, main_hdr))
1488                                 return NULL;
1489                 }
1490         }
1491         if (datai != 0) {
1492                 /* Set delay to the smallest possible value. */
1493                 delay = REGISTER_SET_HDR_OPT_DELAY_MS(0);
1494                 finish_register_set_header_v1(&cur, &next_ext, register_set_hdr,
1495                                               &datai, delay);
1496         }
1497
1498         if (secure_hdr && add_secure_header_v1(params, ptr, payloadsz + headersz,
1499                                                headersz, image, secure_hdr))
1500                 return NULL;
1501
1502         *imagesz = headersz;
1503
1504         /* Fill the real header size without padding into the main header */
1505         headersz = sizeof(*main_hdr);
1506         for_each_opt_hdr_v1 (ohdr, main_hdr)
1507                 headersz += opt_hdr_v1_size(ohdr);
1508         main_hdr->headersz_lsb = cpu_to_le16(headersz & 0xFFFF);
1509         main_hdr->headersz_msb = (headersz & 0xFFFF0000) >> 16;
1510
1511         /* Calculate and set the header checksum */
1512         main_hdr->checksum = image_checksum8(main_hdr, headersz);
1513
1514         return image;
1515 }
1516
1517 static int recognize_keyword(char *keyword)
1518 {
1519         int kw_id;
1520
1521         for (kw_id = 1; kw_id < IMAGE_CFG_COUNT; ++kw_id)
1522                 if (!strcmp(keyword, id_strs[kw_id]))
1523                         return kw_id;
1524
1525         return 0;
1526 }
1527
1528 static int image_create_config_parse_oneline(char *line,
1529                                              struct image_cfg_element *el)
1530 {
1531         char *keyword, *saveptr, *value1, *value2;
1532         char delimiters[] = " \t";
1533         int keyword_id, ret, argi;
1534         char *unknown_msg = "Ignoring unknown line '%s'\n";
1535
1536         keyword = strtok_r(line, delimiters, &saveptr);
1537         keyword_id = recognize_keyword(keyword);
1538
1539         if (!keyword_id) {
1540                 fprintf(stderr, unknown_msg, line);
1541                 return 0;
1542         }
1543
1544         el->type = keyword_id;
1545
1546         value1 = strtok_r(NULL, delimiters, &saveptr);
1547
1548         if (!value1) {
1549                 fprintf(stderr, "Parameter missing in line '%s'\n", line);
1550                 return -1;
1551         }
1552
1553         switch (keyword_id) {
1554         case IMAGE_CFG_VERSION:
1555                 el->version = atoi(value1);
1556                 break;
1557         case IMAGE_CFG_CPU:
1558                 if (strcmp(value1, "FEROCEON") == 0)
1559                         el->cpu_sheeva = 0;
1560                 else if (strcmp(value1, "SHEEVA") == 0)
1561                         el->cpu_sheeva = 1;
1562                 else if (strcmp(value1, "A9") == 0)
1563                         el->cpu_sheeva = 0;
1564                 else {
1565                         fprintf(stderr, "Invalid CPU %s\n", value1);
1566                         return -1;
1567                 }
1568                 break;
1569         case IMAGE_CFG_BOOT_FROM:
1570                 ret = image_boot_mode_id(value1);
1571
1572                 if (ret < 0) {
1573                         fprintf(stderr, "Invalid boot media '%s'\n", value1);
1574                         return -1;
1575                 }
1576                 el->bootfrom = ret;
1577                 break;
1578         case IMAGE_CFG_NAND_BLKSZ:
1579                 el->nandblksz = strtoul(value1, NULL, 16);
1580                 break;
1581         case IMAGE_CFG_NAND_BADBLK_LOCATION:
1582                 el->nandbadblklocation = strtoul(value1, NULL, 16);
1583                 break;
1584         case IMAGE_CFG_NAND_ECC_MODE:
1585                 ret = image_nand_ecc_mode_id(value1);
1586
1587                 if (ret < 0) {
1588                         fprintf(stderr, "Invalid NAND ECC mode '%s'\n", value1);
1589                         return -1;
1590                 }
1591                 el->nandeccmode = ret;
1592                 break;
1593         case IMAGE_CFG_NAND_PAGESZ:
1594                 el->nandpagesz = strtoul(value1, NULL, 16);
1595                 break;
1596         case IMAGE_CFG_BINARY:
1597                 argi = 0;
1598
1599                 el->binary.file = strdup(value1);
1600                 while (1) {
1601                         char *value = strtok_r(NULL, delimiters, &saveptr);
1602                         char *endptr;
1603
1604                         if (!value)
1605                                 break;
1606
1607                         if (!strcmp(value, "LOAD_ADDRESS")) {
1608                                 value = strtok_r(NULL, delimiters, &saveptr);
1609                                 if (!value) {
1610                                         fprintf(stderr,
1611                                                 "Missing address argument for BINARY LOAD_ADDRESS\n");
1612                                         return -1;
1613                                 }
1614                                 el->binary.loadaddr = strtoul(value, &endptr, 16);
1615                                 if (*endptr) {
1616                                         fprintf(stderr,
1617                                                 "Invalid argument '%s' for BINARY LOAD_ADDRESS\n",
1618                                                 value);
1619                                         return -1;
1620                                 }
1621                                 value = strtok_r(NULL, delimiters, &saveptr);
1622                                 if (value) {
1623                                         fprintf(stderr,
1624                                                 "Unexpected argument '%s' after BINARY LOAD_ADDRESS\n",
1625                                                 value);
1626                                         return -1;
1627                                 }
1628                                 break;
1629                         }
1630
1631                         el->binary.args[argi] = strtoul(value, &endptr, 16);
1632                         if (*endptr) {
1633                                 fprintf(stderr, "Invalid argument '%s' for BINARY\n", value);
1634                                 return -1;
1635                         }
1636                         argi++;
1637                         if (argi >= BINARY_MAX_ARGS) {
1638                                 fprintf(stderr,
1639                                         "Too many arguments for BINARY\n");
1640                                 return -1;
1641                         }
1642                 }
1643                 el->binary.nargs = argi;
1644                 break;
1645         case IMAGE_CFG_DATA:
1646                 value2 = strtok_r(NULL, delimiters, &saveptr);
1647
1648                 if (!value1 || !value2) {
1649                         fprintf(stderr,
1650                                 "Invalid number of arguments for DATA\n");
1651                         return -1;
1652                 }
1653
1654                 el->regdata.raddr = strtoul(value1, NULL, 16);
1655                 el->regdata.rdata = strtoul(value2, NULL, 16);
1656                 break;
1657         case IMAGE_CFG_DATA_DELAY:
1658                 if (!strcmp(value1, "SDRAM_SETUP"))
1659                         el->regdata_delay = REGISTER_SET_HDR_OPT_DELAY_SDRAM_SETUP;
1660                 else
1661                         el->regdata_delay = REGISTER_SET_HDR_OPT_DELAY_MS(strtoul(value1, NULL, 10));
1662                 break;
1663         case IMAGE_CFG_BAUDRATE:
1664                 el->baudrate = strtoul(value1, NULL, 10);
1665                 break;
1666         case IMAGE_CFG_UART_PORT:
1667                 el->uart_port = strtoul(value1, NULL, 16);
1668                 break;
1669         case IMAGE_CFG_UART_MPP:
1670                 el->uart_mpp = strtoul(value1, NULL, 16);
1671                 break;
1672         case IMAGE_CFG_DEBUG:
1673                 el->debug = strtoul(value1, NULL, 10);
1674                 break;
1675         case IMAGE_CFG_KAK:
1676                 el->key_name = strdup(value1);
1677                 break;
1678         case IMAGE_CFG_CSK:
1679                 el->key_name = strdup(value1);
1680                 break;
1681         case IMAGE_CFG_CSK_INDEX:
1682                 el->csk_idx = strtol(value1, NULL, 0);
1683                 break;
1684         case IMAGE_CFG_JTAG_DELAY:
1685                 el->jtag_delay = strtoul(value1, NULL, 0);
1686                 break;
1687         case IMAGE_CFG_BOX_ID:
1688                 el->boxid = strtoul(value1, NULL, 0);
1689                 break;
1690         case IMAGE_CFG_FLASH_ID:
1691                 el->flashid = strtoul(value1, NULL, 0);
1692                 break;
1693         case IMAGE_CFG_SEC_SPECIALIZED_IMG:
1694                 el->sec_specialized_img = true;
1695                 break;
1696         case IMAGE_CFG_SEC_COMMON_IMG:
1697                 el->sec_specialized_img = false;
1698                 break;
1699         case IMAGE_CFG_SEC_BOOT_DEV:
1700                 el->sec_boot_dev = strtoul(value1, NULL, 0);
1701                 break;
1702         case IMAGE_CFG_SEC_FUSE_DUMP:
1703                 el->name = strdup(value1);
1704                 break;
1705         default:
1706                 fprintf(stderr, unknown_msg, line);
1707         }
1708
1709         return 0;
1710 }
1711
1712 /*
1713  * Parse the configuration file 'fcfg' into the array of configuration
1714  * elements 'image_cfg', and return the number of configuration
1715  * elements in 'cfgn'.
1716  */
1717 static int image_create_config_parse(FILE *fcfg)
1718 {
1719         int ret;
1720         int cfgi = 0;
1721
1722         /* Parse the configuration file */
1723         while (!feof(fcfg)) {
1724                 char *line;
1725                 char buf[256];
1726
1727                 /* Read the current line */
1728                 memset(buf, 0, sizeof(buf));
1729                 line = fgets(buf, sizeof(buf), fcfg);
1730                 if (!line)
1731                         break;
1732
1733                 /* Ignore useless lines */
1734                 if (line[0] == '\n' || line[0] == '#')
1735                         continue;
1736
1737                 /* Strip final newline */
1738                 if (line[strlen(line) - 1] == '\n')
1739                         line[strlen(line) - 1] = 0;
1740
1741                 /* Parse the current line */
1742                 ret = image_create_config_parse_oneline(line,
1743                                                         &image_cfg[cfgi]);
1744                 if (ret)
1745                         return ret;
1746
1747                 cfgi++;
1748
1749                 if (cfgi >= IMAGE_CFG_ELEMENT_MAX) {
1750                         fprintf(stderr,
1751                                 "Too many configuration elements in .cfg file\n");
1752                         return -1;
1753                 }
1754         }
1755
1756         cfgn = cfgi;
1757         return 0;
1758 }
1759
1760 static int image_get_version(void)
1761 {
1762         struct image_cfg_element *e;
1763
1764         e = image_find_option(IMAGE_CFG_VERSION);
1765         if (!e)
1766                 return -1;
1767
1768         return e->version;
1769 }
1770
1771 static void kwbimage_set_header(void *ptr, struct stat *sbuf, int ifd,
1772                                 struct image_tool_params *params)
1773 {
1774         FILE *fcfg;
1775         void *image = NULL;
1776         int version;
1777         size_t headersz = 0;
1778         size_t datasz;
1779         uint32_t checksum;
1780         struct stat s;
1781         int ret;
1782
1783         /*
1784          * Do not use sbuf->st_size as it contains size with padding.
1785          * We need original image data size, so stat original file.
1786          */
1787         if (stat(params->datafile, &s)) {
1788                 fprintf(stderr, "Could not stat data file %s: %s\n",
1789                         params->datafile, strerror(errno));
1790                 exit(EXIT_FAILURE);
1791         }
1792         datasz = ALIGN(s.st_size, 4);
1793
1794         fcfg = fopen(params->imagename, "r");
1795         if (!fcfg) {
1796                 fprintf(stderr, "Could not open input file %s\n",
1797                         params->imagename);
1798                 exit(EXIT_FAILURE);
1799         }
1800
1801         image_cfg = malloc(IMAGE_CFG_ELEMENT_MAX *
1802                            sizeof(struct image_cfg_element));
1803         if (!image_cfg) {
1804                 fprintf(stderr, "Cannot allocate memory\n");
1805                 fclose(fcfg);
1806                 exit(EXIT_FAILURE);
1807         }
1808
1809         memset(image_cfg, 0,
1810                IMAGE_CFG_ELEMENT_MAX * sizeof(struct image_cfg_element));
1811         rewind(fcfg);
1812
1813         ret = image_create_config_parse(fcfg);
1814         fclose(fcfg);
1815         if (ret) {
1816                 free(image_cfg);
1817                 exit(EXIT_FAILURE);
1818         }
1819
1820         version = image_get_version();
1821         switch (version) {
1822                 /*
1823                  * Fallback to version 0 if no version is provided in the
1824                  * cfg file
1825                  */
1826         case -1:
1827         case 0:
1828                 image = image_create_v0(&headersz, params, datasz + 4);
1829                 break;
1830
1831         case 1:
1832                 image = image_create_v1(&headersz, params, ptr, datasz + 4);
1833                 break;
1834
1835         default:
1836                 fprintf(stderr, "Unsupported version %d\n", version);
1837                 free(image_cfg);
1838                 exit(EXIT_FAILURE);
1839         }
1840
1841         if (!image) {
1842                 fprintf(stderr, "Could not create image\n");
1843                 free(image_cfg);
1844                 exit(EXIT_FAILURE);
1845         }
1846
1847         free(image_cfg);
1848
1849         /* Build and add image data checksum */
1850         checksum = cpu_to_le32(image_checksum32((uint8_t *)ptr + headersz,
1851                                                 datasz));
1852         memcpy((uint8_t *)ptr + headersz + datasz, &checksum, sizeof(uint32_t));
1853
1854         /* Finally copy the header into the image area */
1855         memcpy(ptr, image, headersz);
1856
1857         free(image);
1858 }
1859
1860 static void kwbimage_print_header(const void *ptr)
1861 {
1862         struct main_hdr_v0 *mhdr = (struct main_hdr_v0 *)ptr;
1863         struct opt_hdr_v1 *ohdr;
1864
1865         printf("Image Type:   MVEBU Boot from %s Image\n",
1866                image_boot_mode_name(mhdr->blockid));
1867         printf("Image version:%d\n", kwbimage_version(ptr));
1868
1869         for_each_opt_hdr_v1 (ohdr, mhdr) {
1870                 if (ohdr->headertype == OPT_HDR_V1_BINARY_TYPE) {
1871                         printf("BIN Hdr Size: ");
1872                         genimg_print_size(opt_hdr_v1_size(ohdr) - 12 -
1873                                           4 * ohdr->data[0]);
1874                 }
1875         }
1876
1877         printf("Data Size:    ");
1878         genimg_print_size(mhdr->blocksize - sizeof(uint32_t));
1879         printf("Load Address: %08x\n", mhdr->destaddr);
1880         printf("Entry Point:  %08x\n", mhdr->execaddr);
1881 }
1882
1883 static int kwbimage_check_image_types(uint8_t type)
1884 {
1885         if (type == IH_TYPE_KWBIMAGE)
1886                 return EXIT_SUCCESS;
1887
1888         return EXIT_FAILURE;
1889 }
1890
1891 static int kwbimage_verify_header(unsigned char *ptr, int image_size,
1892                                   struct image_tool_params *params)
1893 {
1894         size_t header_size = kwbheader_size(ptr);
1895         uint8_t blockid;
1896         uint32_t offset;
1897         uint32_t size;
1898         uint8_t csum;
1899
1900         if (header_size > image_size)
1901                 return -FDT_ERR_BADSTRUCTURE;
1902
1903         if (!main_hdr_checksum_ok(ptr))
1904                 return -FDT_ERR_BADSTRUCTURE;
1905
1906         /* Only version 0 extended header has checksum */
1907         if (kwbimage_version(ptr) == 0) {
1908                 struct main_hdr_v0 *mhdr = (struct main_hdr_v0 *)ptr;
1909
1910                 if (mhdr->ext & 0x1) {
1911                         struct ext_hdr_v0 *ext_hdr = (void *)(mhdr + 1);
1912
1913                         csum = image_checksum8(ext_hdr, sizeof(*ext_hdr) - 1);
1914                         if (csum != ext_hdr->checksum)
1915                                 return -FDT_ERR_BADSTRUCTURE;
1916                 }
1917
1918                 blockid = mhdr->blockid;
1919                 offset = le32_to_cpu(mhdr->srcaddr);
1920                 size = le32_to_cpu(mhdr->blocksize);
1921         } else if (kwbimage_version(ptr) == 1) {
1922                 struct main_hdr_v1 *mhdr = (struct main_hdr_v1 *)ptr;
1923                 const uint8_t *mhdr_end;
1924                 struct opt_hdr_v1 *ohdr;
1925
1926                 mhdr_end = (uint8_t *)mhdr + header_size;
1927                 for_each_opt_hdr_v1 (ohdr, ptr)
1928                         if (!opt_hdr_v1_valid_size(ohdr, mhdr_end))
1929                                 return -FDT_ERR_BADSTRUCTURE;
1930
1931                 blockid = mhdr->blockid;
1932                 offset = le32_to_cpu(mhdr->srcaddr);
1933                 size = le32_to_cpu(mhdr->blocksize);
1934         } else {
1935                 return -FDT_ERR_BADSTRUCTURE;
1936         }
1937
1938         /*
1939          * For SATA srcaddr is specified in number of sectors.
1940          * The main header is must be stored at sector number 1.
1941          * This expects that sector size is 512 bytes and recalculates
1942          * data offset to bytes relative to the main header.
1943          */
1944         if (blockid == IBR_HDR_SATA_ID) {
1945                 if (offset < 1)
1946                         return -FDT_ERR_BADSTRUCTURE;
1947                 offset -= 1;
1948                 offset *= 512;
1949         }
1950
1951         /*
1952          * For SDIO srcaddr is specified in number of sectors.
1953          * This expects that sector size is 512 bytes and recalculates
1954          * data offset to bytes.
1955          */
1956         if (blockid == IBR_HDR_SDIO_ID)
1957                 offset *= 512;
1958
1959         /*
1960          * For PCIe srcaddr is always set to 0xFFFFFFFF.
1961          * This expects that data starts after all headers.
1962          */
1963         if (blockid == IBR_HDR_PEX_ID && offset == 0xFFFFFFFF)
1964                 offset = header_size;
1965
1966         if (offset > image_size || offset % 4 != 0)
1967                 return -FDT_ERR_BADSTRUCTURE;
1968
1969         if (size < 4 || offset + size > image_size || size % 4 != 0)
1970                 return -FDT_ERR_BADSTRUCTURE;
1971
1972         if (image_checksum32(ptr + offset, size - 4) !=
1973             *(uint32_t *)(ptr + offset + size - 4))
1974                 return -FDT_ERR_BADSTRUCTURE;
1975
1976         return 0;
1977 }
1978
1979 static int kwbimage_generate(struct image_tool_params *params,
1980                              struct image_type_params *tparams)
1981 {
1982         FILE *fcfg;
1983         struct stat s;
1984         int alloc_len;
1985         int bootfrom;
1986         int version;
1987         void *hdr;
1988         int ret;
1989
1990         fcfg = fopen(params->imagename, "r");
1991         if (!fcfg) {
1992                 fprintf(stderr, "Could not open input file %s\n",
1993                         params->imagename);
1994                 exit(EXIT_FAILURE);
1995         }
1996
1997         if (stat(params->datafile, &s)) {
1998                 fprintf(stderr, "Could not stat data file %s: %s\n",
1999                         params->datafile, strerror(errno));
2000                 exit(EXIT_FAILURE);
2001         }
2002
2003         image_cfg = malloc(IMAGE_CFG_ELEMENT_MAX *
2004                            sizeof(struct image_cfg_element));
2005         if (!image_cfg) {
2006                 fprintf(stderr, "Cannot allocate memory\n");
2007                 fclose(fcfg);
2008                 exit(EXIT_FAILURE);
2009         }
2010
2011         memset(image_cfg, 0,
2012                IMAGE_CFG_ELEMENT_MAX * sizeof(struct image_cfg_element));
2013         rewind(fcfg);
2014
2015         ret = image_create_config_parse(fcfg);
2016         fclose(fcfg);
2017         if (ret) {
2018                 free(image_cfg);
2019                 exit(EXIT_FAILURE);
2020         }
2021
2022         bootfrom = image_get_bootfrom();
2023         version = image_get_version();
2024         switch (version) {
2025                 /*
2026                  * Fallback to version 0 if no version is provided in the
2027                  * cfg file
2028                  */
2029         case -1:
2030         case 0:
2031                 alloc_len = image_headersz_v0(NULL);
2032                 break;
2033
2034         case 1:
2035                 alloc_len = image_headersz_v1(NULL);
2036                 if (!alloc_len) {
2037                         free(image_cfg);
2038                         exit(EXIT_FAILURE);
2039                 }
2040                 if (alloc_len > 192*1024) {
2041                         fprintf(stderr, "Header is too big (%u bytes), maximal kwbimage header size is %u bytes\n", alloc_len, 192*1024);
2042                         free(image_cfg);
2043                         exit(EXIT_FAILURE);
2044                 }
2045                 break;
2046
2047         default:
2048                 fprintf(stderr, "Unsupported version %d\n", version);
2049                 free(image_cfg);
2050                 exit(EXIT_FAILURE);
2051         }
2052
2053         free(image_cfg);
2054
2055         hdr = malloc(alloc_len);
2056         if (!hdr) {
2057                 fprintf(stderr, "%s: malloc return failure: %s\n",
2058                         params->cmdname, strerror(errno));
2059                 exit(EXIT_FAILURE);
2060         }
2061
2062         memset(hdr, 0, alloc_len);
2063         tparams->header_size = alloc_len;
2064         tparams->hdr = hdr;
2065
2066         /*
2067          * The resulting image needs to be 4-byte aligned. At least
2068          * the Marvell hdrparser tool complains if its unaligned.
2069          * After the image data is stored 4-byte checksum.
2070          * Final UART image must be aligned to 128 bytes.
2071          * Final SPI and NAND images must be aligned to 256 bytes.
2072          * Final SATA and SDIO images must be aligned to 512 bytes.
2073          */
2074         if (bootfrom == IBR_HDR_SPI_ID || bootfrom == IBR_HDR_NAND_ID)
2075                 return 4 + (256 - (alloc_len + s.st_size + 4) % 256) % 256;
2076         else if (bootfrom == IBR_HDR_SATA_ID || bootfrom == IBR_HDR_SDIO_ID)
2077                 return 4 + (512 - (alloc_len + s.st_size + 4) % 512) % 512;
2078         else if (bootfrom == IBR_HDR_UART_ID)
2079                 return 4 + (128 - (alloc_len + s.st_size + 4) % 128) % 128;
2080         else
2081                 return 4 + (4 - s.st_size % 4) % 4;
2082 }
2083
2084 static int kwbimage_extract_subimage(void *ptr, struct image_tool_params *params)
2085 {
2086         struct main_hdr_v1 *mhdr = (struct main_hdr_v1 *)ptr;
2087         size_t header_size = kwbheader_size(ptr);
2088         struct opt_hdr_v1 *ohdr;
2089         int idx = params->pflag;
2090         int cur_idx = 0;
2091         uint32_t offset;
2092         ulong image;
2093         ulong size;
2094
2095         for_each_opt_hdr_v1 (ohdr, ptr) {
2096                 if (ohdr->headertype != OPT_HDR_V1_BINARY_TYPE)
2097                         continue;
2098
2099                 if (idx == cur_idx) {
2100                         image = (ulong)&ohdr->data[4 + 4 * ohdr->data[0]];
2101                         size = opt_hdr_v1_size(ohdr) - 12 - 4 * ohdr->data[0];
2102                         goto extract;
2103                 }
2104
2105                 ++cur_idx;
2106         }
2107
2108         if (idx != cur_idx) {
2109                 printf("Image %d is not present\n", idx);
2110                 return -1;
2111         }
2112
2113         offset = le32_to_cpu(mhdr->srcaddr);
2114
2115         if (mhdr->blockid == IBR_HDR_SATA_ID) {
2116                 offset -= 1;
2117                 offset *= 512;
2118         }
2119
2120         if (mhdr->blockid == IBR_HDR_SDIO_ID)
2121                 offset *= 512;
2122
2123         if (mhdr->blockid == IBR_HDR_PEX_ID && offset == 0xFFFFFFFF)
2124                 offset = header_size;
2125
2126         image = (ulong)((uint8_t *)ptr + offset);
2127         size = le32_to_cpu(mhdr->blocksize) - 4;
2128
2129 extract:
2130         return imagetool_save_subimage(params->outfile, image, size);
2131 }
2132
2133 /*
2134  * Report Error if xflag is set in addition to default
2135  */
2136 static int kwbimage_check_params(struct image_tool_params *params)
2137 {
2138         if (!params->iflag && (!params->imagename || !strlen(params->imagename))) {
2139                 char *msg = "Configuration file for kwbimage creation omitted";
2140
2141                 fprintf(stderr, "Error:%s - %s\n", params->cmdname, msg);
2142                 return 1;
2143         }
2144
2145         return (params->dflag && (params->fflag || params->lflag)) ||
2146                 (params->fflag && (params->dflag || params->lflag)) ||
2147                 (params->lflag && (params->dflag || params->fflag)) ||
2148                 (params->xflag);
2149 }
2150
2151 /*
2152  * kwbimage type parameters definition
2153  */
2154 U_BOOT_IMAGE_TYPE(
2155         kwbimage,
2156         "Marvell MVEBU Boot Image support",
2157         0,
2158         NULL,
2159         kwbimage_check_params,
2160         kwbimage_verify_header,
2161         kwbimage_print_header,
2162         kwbimage_set_header,
2163         kwbimage_extract_subimage,
2164         kwbimage_check_image_types,
2165         NULL,
2166         kwbimage_generate
2167 );