tools/kwbimage.c

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