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