tools/kwbimage.c

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