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