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