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         fstat(fileno(bin), &s);
 996
 997         binhdrsz = sizeof(struct opt_hdr_v1) +
 998                 (binarye->binary.nargs + 2) * sizeof(uint32_t) +
 999                 s.st_size;
1000
1001         /*
1002          * The size includes the binary image size, rounded
1003          * up to a 4-byte boundary. Plus 4 bytes for the
1004          * next-header byte and 3-byte alignment at the end.
1005          */
1006         binhdrsz = ALIGN_SUP(binhdrsz, 4) + 4;
1007         hdr->headersz_lsb = cpu_to_le16(binhdrsz & 0xFFFF);
1008         hdr->headersz_msb = (binhdrsz & 0xFFFF0000) >> 16;
1009
1010         cur += sizeof(struct opt_hdr_v1);
1011
1012         args = (uint32_t *)cur;
1013         *args = cpu_to_le32(binarye->binary.nargs);
1014         args++;
1015         for (argi = 0; argi < binarye->binary.nargs; argi++)
1016                 args[argi] = cpu_to_le32(binarye->binary.args[argi]);
1017
1018         cur += (binarye->binary.nargs + 1) * sizeof(uint32_t);
1019
1020         ret = fread(cur, s.st_size, 1, bin);
1021         if (ret != 1) {
1022                 fprintf(stderr,
1023                         "Could not read binary image %s\n",
1024                         binarye->binary.file);
1025                 return -1;
1026         }
1027
1028         fclose(bin);
1029
1030         cur += ALIGN_SUP(s.st_size, 4);
1031
1032         /*
1033          * For now, we don't support more than one binary
1034          * header, and no other header types are
1035          * supported. So, the binary header is necessarily the
1036          * last one
1037          */
1038         *((uint32_t *)cur) = 0x00000000;
1039
1040         cur += sizeof(uint32_t);
1041
1042         return 0;
1043 }
1044
1045 #if defined(CONFIG_KWB_SECURE)
1046
1047 int export_pub_kak_hash(RSA *kak, struct secure_hdr_v1 *secure_hdr)
1048 {
1049         FILE *hashf;
1050         int res;
1051
1052         hashf = fopen("pub_kak_hash.txt", "w");
1053
1054         res = kwb_export_pubkey(kak, &secure_hdr->kak, hashf, "KAK");
1055
1056         fclose(hashf);
1057
1058         return res < 0 ? 1 : 0;
1059 }
1060
1061 int kwb_sign_csk_with_kak(struct image_tool_params *params,
1062                           struct secure_hdr_v1 *secure_hdr, RSA *csk)
1063 {
1064         RSA *kak = NULL;
1065         RSA *kak_pub = NULL;
1066         int csk_idx = image_get_csk_index();
1067         struct sig_v1 tmp_sig;
1068
1069         if (csk_idx >= 16) {
1070                 fprintf(stderr, "Invalid CSK index %d\n", csk_idx);
1071                 return 1;
1072         }
1073
1074         if (kwb_load_kak(params, &kak) < 0)
1075                 return 1;
1076
1077         if (export_pub_kak_hash(kak, secure_hdr))
1078                 return 1;
1079
1080         if (kwb_import_pubkey(&kak_pub, &secure_hdr->kak, "KAK") < 0)
1081                 return 1;
1082
1083         if (kwb_export_pubkey(csk, &secure_hdr->csk[csk_idx], NULL, "CSK") < 0)
1084                 return 1;
1085
1086         if (kwb_sign_and_verify(kak, &secure_hdr->csk,
1087                                 sizeof(secure_hdr->csk) +
1088                                 sizeof(secure_hdr->csksig),
1089                                 &tmp_sig, "CSK") < 0)
1090                 return 1;
1091
1092         if (kwb_verify(kak_pub, &secure_hdr->csk,
1093                        sizeof(secure_hdr->csk) +
1094                        sizeof(secure_hdr->csksig),
1095                        &tmp_sig, "CSK (2)") < 0)
1096                 return 1;
1097
1098         secure_hdr->csksig = tmp_sig;
1099
1100         return 0;
1101 }
1102
1103 int add_secure_header_v1(struct image_tool_params *params, uint8_t *ptr,
1104                          int payloadsz, size_t headersz, uint8_t *image,
1105                          struct secure_hdr_v1 *secure_hdr)
1106 {
1107         struct image_cfg_element *e_jtagdelay;
1108         struct image_cfg_element *e_boxid;
1109         struct image_cfg_element *e_flashid;
1110         RSA *csk = NULL;
1111         unsigned char *image_ptr;
1112         size_t image_size;
1113         struct sig_v1 tmp_sig;
1114         bool specialized_img = image_get_spezialized_img();
1115
1116         kwb_msg("Create secure header content\n");
1117
1118         e_jtagdelay = image_find_option(IMAGE_CFG_JTAG_DELAY);
1119         e_boxid = image_find_option(IMAGE_CFG_BOX_ID);
1120         e_flashid = image_find_option(IMAGE_CFG_FLASH_ID);
1121
1122         if (kwb_load_csk(params, &csk) < 0)
1123                 return 1;
1124
1125         secure_hdr->headertype = OPT_HDR_V1_SECURE_TYPE;
1126         secure_hdr->headersz_msb = 0;
1127         secure_hdr->headersz_lsb = cpu_to_le16(sizeof(struct secure_hdr_v1));
1128         if (e_jtagdelay)
1129                 secure_hdr->jtag_delay = e_jtagdelay->jtag_delay;
1130         if (e_boxid && specialized_img)
1131                 secure_hdr->boxid = cpu_to_le32(e_boxid->boxid);
1132         if (e_flashid && specialized_img)
1133                 secure_hdr->flashid = cpu_to_le32(e_flashid->flashid);
1134
1135         if (kwb_sign_csk_with_kak(params, secure_hdr, csk))
1136                 return 1;
1137
1138         image_ptr = ptr + headersz;
1139         image_size = payloadsz - headersz;
1140
1141         if (kwb_sign_and_verify(csk, image_ptr, image_size,
1142                                 &secure_hdr->imgsig, "image") < 0)
1143                 return 1;
1144
1145         if (kwb_sign_and_verify(csk, image, headersz, &tmp_sig, "header") < 0)
1146                 return 1;
1147
1148         secure_hdr->hdrsig = tmp_sig;
1149
1150         kwb_dump_fuse_cmds(secure_hdr);
1151
1152         return 0;
1153 }
1154 #endif
1155
1156 static void *image_create_v1(size_t *imagesz, struct image_tool_params *params,
1157                              uint8_t *ptr, int payloadsz)
1158 {
1159         struct image_cfg_element *e;
1160         struct main_hdr_v1 *main_hdr;
1161 #if defined(CONFIG_KWB_SECURE)
1162         struct secure_hdr_v1 *secure_hdr = NULL;
1163 #endif
1164         size_t headersz;
1165         uint8_t *image, *cur;
1166         int hasext = 0;
1167         uint8_t *next_ext = NULL;
1168
1169         /*
1170          * Calculate the size of the header and the size of the
1171          * payload
1172          */
1173         headersz = image_headersz_v1(&hasext);
1174         if (headersz == 0)
1175                 return NULL;
1176
1177         image = malloc(headersz);
1178         if (!image) {
1179                 fprintf(stderr, "Cannot allocate memory for image\n");
1180                 return NULL;
1181         }
1182
1183         memset(image, 0, headersz);
1184
1185         main_hdr = (struct main_hdr_v1 *)image;
1186         cur = image;
1187         cur += sizeof(struct main_hdr_v1);
1188         next_ext = &main_hdr->ext;
1189
1190         /* Fill the main header */
1191         main_hdr->blocksize    =
1192                 cpu_to_le32(payloadsz - headersz + sizeof(uint32_t));
1193         main_hdr->headersz_lsb = cpu_to_le16(headersz & 0xFFFF);
1194         main_hdr->headersz_msb = (headersz & 0xFFFF0000) >> 16;
1195         main_hdr->destaddr     = cpu_to_le32(params->addr)
1196                                  - sizeof(image_header_t);
1197         main_hdr->execaddr     = cpu_to_le32(params->ep);
1198         main_hdr->srcaddr      = cpu_to_le32(headersz);
1199         main_hdr->ext          = hasext;
1200         main_hdr->version      = 1;
1201         e = image_find_option(IMAGE_CFG_BOOT_FROM);
1202         if (e)
1203                 main_hdr->blockid = e->bootfrom;
1204         e = image_find_option(IMAGE_CFG_NAND_BLKSZ);
1205         if (e)
1206                 main_hdr->nandblocksize = e->nandblksz / (64 * 1024);
1207         e = image_find_option(IMAGE_CFG_NAND_BADBLK_LOCATION);
1208         if (e)
1209                 main_hdr->nandbadblklocation = e->nandbadblklocation;
1210         e = image_find_option(IMAGE_CFG_BAUDRATE);
1211         if (e)
1212                 main_hdr->options = baudrate_to_option(e->baudrate);
1213         e = image_find_option(IMAGE_CFG_DEBUG);
1214         if (e)
1215                 main_hdr->flags = e->debug ? 0x1 : 0;
1216
1217 #if defined(CONFIG_KWB_SECURE)
1218         if (image_get_csk_index() >= 0) {
1219                 /*
1220                  * only reserve the space here; we fill the header later since
1221                  * we need the header to be complete to compute the signatures
1222                  */
1223                 secure_hdr = (struct secure_hdr_v1 *)cur;
1224                 cur += sizeof(struct secure_hdr_v1);
1225                 next_ext = &secure_hdr->next;
1226         }
1227 #endif
1228         *next_ext = 1;
1229
1230         if (add_binary_header_v1(cur))
1231                 return NULL;
1232
1233 #if defined(CONFIG_KWB_SECURE)
1234         if (secure_hdr && add_secure_header_v1(params, ptr, payloadsz,
1235                                                headersz, image, secure_hdr))
1236                 return NULL;
1237 #endif
1238
1239         /* Calculate and set the header checksum */
1240         main_hdr->checksum = image_checksum8(main_hdr, headersz);
1241
1242         *imagesz = headersz;
1243         return image;
1244 }
1245
1246 int recognize_keyword(char *keyword)
1247 {
1248         int kw_id;
1249
1250         for (kw_id = 1; kw_id < IMAGE_CFG_COUNT; ++kw_id)
1251                 if (!strcmp(keyword, id_strs[kw_id]))
1252                         return kw_id;
1253
1254         return 0;
1255 }
1256
1257 static int image_create_config_parse_oneline(char *line,
1258                                              struct image_cfg_element *el)
1259 {
1260         char *keyword, *saveptr, *value1, *value2;
1261         char delimiters[] = " \t";
1262         int keyword_id, ret, argi;
1263         char *unknown_msg = "Ignoring unknown line '%s'\n";
1264
1265         keyword = strtok_r(line, delimiters, &saveptr);
1266         keyword_id = recognize_keyword(keyword);
1267
1268         if (!keyword_id) {
1269                 fprintf(stderr, unknown_msg, line);
1270                 return 0;
1271         }
1272
1273         el->type = keyword_id;
1274
1275         value1 = strtok_r(NULL, delimiters, &saveptr);
1276
1277         if (!value1) {
1278                 fprintf(stderr, "Parameter missing in line '%s'\n", line);
1279                 return -1;
1280         }
1281
1282         switch (keyword_id) {
1283         case IMAGE_CFG_VERSION:
1284                 el->version = atoi(value1);
1285                 break;
1286         case IMAGE_CFG_BOOT_FROM:
1287                 ret = image_boot_mode_id(value1);
1288
1289                 if (ret < 0) {
1290                         fprintf(stderr, "Invalid boot media '%s'\n", value1);
1291                         return -1;
1292                 }
1293                 el->bootfrom = ret;
1294                 break;
1295         case IMAGE_CFG_NAND_BLKSZ:
1296                 el->nandblksz = strtoul(value1, NULL, 16);
1297                 break;
1298         case IMAGE_CFG_NAND_BADBLK_LOCATION:
1299                 el->nandbadblklocation = strtoul(value1, NULL, 16);
1300                 break;
1301         case IMAGE_CFG_NAND_ECC_MODE:
1302                 ret = image_nand_ecc_mode_id(value1);
1303
1304                 if (ret < 0) {
1305                         fprintf(stderr, "Invalid NAND ECC mode '%s'\n", value1);
1306                         return -1;
1307                 }
1308                 el->nandeccmode = ret;
1309                 break;
1310         case IMAGE_CFG_NAND_PAGESZ:
1311                 el->nandpagesz = strtoul(value1, NULL, 16);
1312                 break;
1313         case IMAGE_CFG_BINARY:
1314                 argi = 0;
1315
1316                 el->binary.file = strdup(value1);
1317                 while (1) {
1318                         char *value = strtok_r(NULL, delimiters, &saveptr);
1319
1320                         if (!value)
1321                                 break;
1322                         el->binary.args[argi] = strtoul(value, NULL, 16);
1323                         argi++;
1324                         if (argi >= BINARY_MAX_ARGS) {
1325                                 fprintf(stderr,
1326                                         "Too many arguments for BINARY\n");
1327                                 return -1;
1328                         }
1329                 }
1330                 el->binary.nargs = argi;
1331                 break;
1332         case IMAGE_CFG_DATA:
1333                 value2 = strtok_r(NULL, delimiters, &saveptr);
1334
1335                 if (!value1 || !value2) {
1336                         fprintf(stderr,
1337                                 "Invalid number of arguments for DATA\n");
1338                         return -1;
1339                 }
1340
1341                 el->regdata.raddr = strtoul(value1, NULL, 16);
1342                 el->regdata.rdata = strtoul(value2, NULL, 16);
1343                 break;
1344         case IMAGE_CFG_BAUDRATE:
1345                 el->baudrate = strtoul(value1, NULL, 10);
1346                 break;
1347         case IMAGE_CFG_DEBUG:
1348                 el->debug = strtoul(value1, NULL, 10);
1349                 break;
1350         case IMAGE_CFG_KAK:
1351                 el->key_name = strdup(value1);
1352                 break;
1353         case IMAGE_CFG_CSK:
1354                 el->key_name = strdup(value1);
1355                 break;
1356         case IMAGE_CFG_CSK_INDEX:
1357                 el->csk_idx = strtol(value1, NULL, 0);
1358                 break;
1359         case IMAGE_CFG_JTAG_DELAY:
1360                 el->jtag_delay = strtoul(value1, NULL, 0);
1361                 break;
1362         case IMAGE_CFG_BOX_ID:
1363                 el->boxid = strtoul(value1, NULL, 0);
1364                 break;
1365         case IMAGE_CFG_FLASH_ID:
1366                 el->flashid = strtoul(value1, NULL, 0);
1367                 break;
1368         case IMAGE_CFG_SEC_SPECIALIZED_IMG:
1369                 el->sec_specialized_img = true;
1370                 break;
1371         case IMAGE_CFG_SEC_COMMON_IMG:
1372                 el->sec_specialized_img = false;
1373                 break;
1374         case IMAGE_CFG_SEC_BOOT_DEV:
1375                 el->sec_boot_dev = strtoul(value1, NULL, 0);
1376                 break;
1377         case IMAGE_CFG_SEC_FUSE_DUMP:
1378                 el->name = strdup(value1);
1379                 break;
1380         default:
1381                 fprintf(stderr, unknown_msg, line);
1382         }
1383
1384         return 0;
1385 }
1386
1387 /*
1388  * Parse the configuration file 'fcfg' into the array of configuration
1389  * elements 'image_cfg', and return the number of configuration
1390  * elements in 'cfgn'.
1391  */
1392 static int image_create_config_parse(FILE *fcfg)
1393 {
1394         int ret;
1395         int cfgi = 0;
1396
1397         /* Parse the configuration file */
1398         while (!feof(fcfg)) {
1399                 char *line;
1400                 char buf[256];
1401
1402                 /* Read the current line */
1403                 memset(buf, 0, sizeof(buf));
1404                 line = fgets(buf, sizeof(buf), fcfg);
1405                 if (!line)
1406                         break;
1407
1408                 /* Ignore useless lines */
1409                 if (line[0] == '\n' || line[0] == '#')
1410                         continue;
1411
1412                 /* Strip final newline */
1413                 if (line[strlen(line) - 1] == '\n')
1414                         line[strlen(line) - 1] = 0;
1415
1416                 /* Parse the current line */
1417                 ret = image_create_config_parse_oneline(line,
1418                                                         &image_cfg[cfgi]);
1419                 if (ret)
1420                         return ret;
1421
1422                 cfgi++;
1423
1424                 if (cfgi >= IMAGE_CFG_ELEMENT_MAX) {
1425                         fprintf(stderr,
1426                                 "Too many configuration elements in .cfg file\n");
1427                         return -1;
1428                 }
1429         }
1430
1431         cfgn = cfgi;
1432         return 0;
1433 }
1434
1435 static int image_get_version(void)
1436 {
1437         struct image_cfg_element *e;
1438
1439         e = image_find_option(IMAGE_CFG_VERSION);
1440         if (!e)
1441                 return -1;
1442
1443         return e->version;
1444 }
1445
1446 static int image_version_file(const char *input)
1447 {
1448         FILE *fcfg;
1449         int version;
1450         int ret;
1451
1452         fcfg = fopen(input, "r");
1453         if (!fcfg) {
1454                 fprintf(stderr, "Could not open input file %s\n", input);
1455                 return -1;
1456         }
1457
1458         image_cfg = malloc(IMAGE_CFG_ELEMENT_MAX *
1459                            sizeof(struct image_cfg_element));
1460         if (!image_cfg) {
1461                 fprintf(stderr, "Cannot allocate memory\n");
1462                 fclose(fcfg);
1463                 return -1;
1464         }
1465
1466         memset(image_cfg, 0,
1467                IMAGE_CFG_ELEMENT_MAX * sizeof(struct image_cfg_element));
1468         rewind(fcfg);
1469
1470         ret = image_create_config_parse(fcfg);
1471         fclose(fcfg);
1472         if (ret) {
1473                 free(image_cfg);
1474                 return -1;
1475         }
1476
1477         version = image_get_version();
1478         /* Fallback to version 0 is no version is provided in the cfg file */
1479         if (version == -1)
1480                 version = 0;
1481
1482         free(image_cfg);
1483
1484         return version;
1485 }
1486
1487 static void kwbimage_set_header(void *ptr, struct stat *sbuf, int ifd,
1488                                 struct image_tool_params *params)
1489 {
1490         FILE *fcfg;
1491         void *image = NULL;
1492         int version;
1493         size_t headersz = 0;
1494         uint32_t checksum;
1495         int ret;
1496         int size;
1497
1498         fcfg = fopen(params->imagename, "r");
1499         if (!fcfg) {
1500                 fprintf(stderr, "Could not open input file %s\n",
1501                         params->imagename);
1502                 exit(EXIT_FAILURE);
1503         }
1504
1505         image_cfg = malloc(IMAGE_CFG_ELEMENT_MAX *
1506                            sizeof(struct image_cfg_element));
1507         if (!image_cfg) {
1508                 fprintf(stderr, "Cannot allocate memory\n");
1509                 fclose(fcfg);
1510                 exit(EXIT_FAILURE);
1511         }
1512
1513         memset(image_cfg, 0,
1514                IMAGE_CFG_ELEMENT_MAX * sizeof(struct image_cfg_element));
1515         rewind(fcfg);
1516
1517         ret = image_create_config_parse(fcfg);
1518         fclose(fcfg);
1519         if (ret) {
1520                 free(image_cfg);
1521                 exit(EXIT_FAILURE);
1522         }
1523
1524         /* The MVEBU BootROM does not allow non word aligned payloads */
1525         sbuf->st_size = ALIGN_SUP(sbuf->st_size, 4);
1526
1527         version = image_get_version();
1528         switch (version) {
1529                 /*
1530                  * Fallback to version 0 if no version is provided in the
1531                  * cfg file
1532                  */
1533         case -1:
1534         case 0:
1535                 image = image_create_v0(&headersz, params, sbuf->st_size);
1536                 break;
1537
1538         case 1:
1539                 image = image_create_v1(&headersz, params, ptr, sbuf->st_size);
1540                 break;
1541
1542         default:
1543                 fprintf(stderr, "Unsupported version %d\n", version);
1544                 free(image_cfg);
1545                 exit(EXIT_FAILURE);
1546         }
1547
1548         if (!image) {
1549                 fprintf(stderr, "Could not create image\n");
1550                 free(image_cfg);
1551                 exit(EXIT_FAILURE);
1552         }
1553
1554         free(image_cfg);
1555
1556         /* Build and add image checksum header */
1557         checksum =
1558                 cpu_to_le32(image_checksum32((uint32_t *)ptr, sbuf->st_size));
1559         size = write(ifd, &checksum, sizeof(uint32_t));
1560         if (size != sizeof(uint32_t)) {
1561                 fprintf(stderr, "Error:%s - Checksum write %d bytes %s\n",
1562                         params->cmdname, size, params->imagefile);
1563                 exit(EXIT_FAILURE);
1564         }
1565
1566         sbuf->st_size += sizeof(uint32_t);
1567
1568         /* Finally copy the header into the image area */
1569         memcpy(ptr, image, headersz);
1570
1571         free(image);
1572 }
1573
1574 static void kwbimage_print_header(const void *ptr)
1575 {
1576         struct main_hdr_v0 *mhdr = (struct main_hdr_v0 *)ptr;
1577
1578         printf("Image Type:   MVEBU Boot from %s Image\n",
1579                image_boot_mode_name(mhdr->blockid));
1580         printf("Image version:%d\n", image_version((void *)ptr));
1581         printf("Data Size:    ");
1582         genimg_print_size(mhdr->blocksize - sizeof(uint32_t));
1583         printf("Load Address: %08x\n", mhdr->destaddr);
1584         printf("Entry Point:  %08x\n", mhdr->execaddr);
1585 }
1586
1587 static int kwbimage_check_image_types(uint8_t type)
1588 {
1589         if (type == IH_TYPE_KWBIMAGE)
1590                 return EXIT_SUCCESS;
1591
1592         return EXIT_FAILURE;
1593 }
1594
1595 static int kwbimage_verify_header(unsigned char *ptr, int image_size,
1596                                   struct image_tool_params *params)
1597 {
1598         struct main_hdr_v0 *main_hdr;
1599         uint8_t checksum;
1600
1601         main_hdr = (struct main_hdr_v0 *)ptr;
1602         checksum = image_checksum8(ptr,
1603                                    sizeof(struct main_hdr_v0)
1604                                    - sizeof(uint8_t));
1605         if (checksum != main_hdr->checksum)
1606                 return -FDT_ERR_BADSTRUCTURE;
1607
1608         /* Only version 0 extended header has checksum */
1609         if (image_version((void *)ptr) == 0) {
1610                 struct ext_hdr_v0 *ext_hdr;
1611
1612                 ext_hdr = (struct ext_hdr_v0 *)
1613                                 (ptr + sizeof(struct main_hdr_v0));
1614                 checksum = image_checksum8(ext_hdr,
1615                                            sizeof(struct ext_hdr_v0)
1616                                            - sizeof(uint8_t));
1617                 if (checksum != ext_hdr->checksum)
1618                         return -FDT_ERR_BADSTRUCTURE;
1619         }
1620
1621         return 0;
1622 }
1623
1624 static int kwbimage_generate(struct image_tool_params *params,
1625                              struct image_type_params *tparams)
1626 {
1627         int alloc_len;
1628         void *hdr;
1629         int version = 0;
1630
1631         version = image_version_file(params->imagename);
1632         if (version == 0) {
1633                 alloc_len = sizeof(struct main_hdr_v0) +
1634                         sizeof(struct ext_hdr_v0);
1635         } else {
1636                 alloc_len = image_headersz_v1(NULL);
1637         }
1638
1639         hdr = malloc(alloc_len);
1640         if (!hdr) {
1641                 fprintf(stderr, "%s: malloc return failure: %s\n",
1642                         params->cmdname, strerror(errno));
1643                 exit(EXIT_FAILURE);
1644         }
1645
1646         memset(hdr, 0, alloc_len);
1647         tparams->header_size = alloc_len;
1648         tparams->hdr = hdr;
1649
1650         /*
1651          * The resulting image needs to be 4-byte aligned. At least
1652          * the Marvell hdrparser tool complains if its unaligned.
1653          * By returning 1 here in this function, called via
1654          * tparams->vrec_header() in mkimage.c, mkimage will
1655          * automatically pad the the resulting image to a 4-byte
1656          * size if necessary.
1657          */
1658         return 1;
1659 }
1660
1661 /*
1662  * Report Error if xflag is set in addition to default
1663  */
1664 static int kwbimage_check_params(struct image_tool_params *params)
1665 {
1666         if (!strlen(params->imagename)) {
1667                 char *msg = "Configuration file for kwbimage creation omitted";
1668
1669                 fprintf(stderr, "Error:%s - %s\n", params->cmdname, msg);
1670                 return CFG_INVALID;
1671         }
1672
1673         return (params->dflag && (params->fflag || params->lflag)) ||
1674                 (params->fflag && (params->dflag || params->lflag)) ||
1675                 (params->lflag && (params->dflag || params->fflag)) ||
1676                 (params->xflag) || !(strlen(params->imagename));
1677 }
1678
1679 /*
1680  * kwbimage type parameters definition
1681  */
1682 U_BOOT_IMAGE_TYPE(
1683         kwbimage,
1684         "Marvell MVEBU Boot Image support",
1685         0,
1686         NULL,
1687         kwbimage_check_params,
1688         kwbimage_verify_header,
1689         kwbimage_print_header,
1690         kwbimage_set_header,
1691         NULL,
1692         kwbimage_check_image_types,
1693         NULL,
1694         kwbimage_generate
1695 );