tools/kwbimage.c

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