tools/kwbimage.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * Image manipulator for Marvell SoCs
   4  *  supports Kirkwood, Dove, Armada 370, Armada XP, and Armada 38x
   5  *
   6  * (C) Copyright 2013 Thomas Petazzoni
   7  * <thomas.petazzoni@free-electrons.com>
   8  *
   9  * Not implemented: support for the register headers in v1 images
  10  */
  11
  12 #include "imagetool.h"
  13 #include <limits.h>
  14 #include <image.h>
  15 #include <stdarg.h>
  16 #include <stdint.h>
  17 #include "kwbimage.h"
  18
  19 #ifdef CONFIG_KWB_SECURE
  20 #include <openssl/bn.h>
  21 #include <openssl/rsa.h>
  22 #include <openssl/pem.h>
  23 #include <openssl/err.h>
  24 #include <openssl/evp.h>
  25
  26 #if OPENSSL_VERSION_NUMBER < 0x10100000L || \
  27     (defined(LIBRESSL_VERSION_NUMBER) && LIBRESSL_VERSION_NUMBER < 0x2070000fL)
  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 #elif !defined(LIBRESSL_VERSION_NUMBER)
  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) != 1) {
 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(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(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(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         e = image_find_option(IMAGE_CFG_BINARY);
1277         if (e) {
1278                 char *s = strrchr(e->binary.file, '/');
1279
1280                 if (strcmp(s, "/binary.0") == 0)
1281                         main_hdr->destaddr = cpu_to_le32(params->addr);
1282         }
1283
1284 #if defined(CONFIG_KWB_SECURE)
1285         if (image_get_csk_index() >= 0) {
1286                 /*
1287                  * only reserve the space here; we fill the header later since
1288                  * we need the header to be complete to compute the signatures
1289                  */
1290                 secure_hdr = (struct secure_hdr_v1 *)cur;
1291                 cur += sizeof(struct secure_hdr_v1);
1292                 next_ext = &secure_hdr->next;
1293         }
1294 #endif
1295         *next_ext = 1;
1296
1297         if (add_binary_header_v1(cur))
1298                 return NULL;
1299
1300 #if defined(CONFIG_KWB_SECURE)
1301         if (secure_hdr && add_secure_header_v1(params, ptr, payloadsz,
1302                                                headersz, image, secure_hdr))
1303                 return NULL;
1304 #endif
1305
1306         /* Calculate and set the header checksum */
1307         main_hdr->checksum = image_checksum8(main_hdr, headersz);
1308
1309         *imagesz = headersz;
1310         return image;
1311 }
1312
1313 int recognize_keyword(char *keyword)
1314 {
1315         int kw_id;
1316
1317         for (kw_id = 1; kw_id < IMAGE_CFG_COUNT; ++kw_id)
1318                 if (!strcmp(keyword, id_strs[kw_id]))
1319                         return kw_id;
1320
1321         return 0;
1322 }
1323
1324 static int image_create_config_parse_oneline(char *line,
1325                                              struct image_cfg_element *el)
1326 {
1327         char *keyword, *saveptr, *value1, *value2;
1328         char delimiters[] = " \t";
1329         int keyword_id, ret, argi;
1330         char *unknown_msg = "Ignoring unknown line '%s'\n";
1331
1332         keyword = strtok_r(line, delimiters, &saveptr);
1333         keyword_id = recognize_keyword(keyword);
1334
1335         if (!keyword_id) {
1336                 fprintf(stderr, unknown_msg, line);
1337                 return 0;
1338         }
1339
1340         el->type = keyword_id;
1341
1342         value1 = strtok_r(NULL, delimiters, &saveptr);
1343
1344         if (!value1) {
1345                 fprintf(stderr, "Parameter missing in line '%s'\n", line);
1346                 return -1;
1347         }
1348
1349         switch (keyword_id) {
1350         case IMAGE_CFG_VERSION:
1351                 el->version = atoi(value1);
1352                 break;
1353         case IMAGE_CFG_BOOT_FROM:
1354                 ret = image_boot_mode_id(value1);
1355
1356                 if (ret < 0) {
1357                         fprintf(stderr, "Invalid boot media '%s'\n", value1);
1358                         return -1;
1359                 }
1360                 el->bootfrom = ret;
1361                 break;
1362         case IMAGE_CFG_NAND_BLKSZ:
1363                 el->nandblksz = strtoul(value1, NULL, 16);
1364                 break;
1365         case IMAGE_CFG_NAND_BADBLK_LOCATION:
1366                 el->nandbadblklocation = strtoul(value1, NULL, 16);
1367                 break;
1368         case IMAGE_CFG_NAND_ECC_MODE:
1369                 ret = image_nand_ecc_mode_id(value1);
1370
1371                 if (ret < 0) {
1372                         fprintf(stderr, "Invalid NAND ECC mode '%s'\n", value1);
1373                         return -1;
1374                 }
1375                 el->nandeccmode = ret;
1376                 break;
1377         case IMAGE_CFG_NAND_PAGESZ:
1378                 el->nandpagesz = strtoul(value1, NULL, 16);
1379                 break;
1380         case IMAGE_CFG_BINARY:
1381                 argi = 0;
1382
1383                 el->binary.file = strdup(value1);
1384                 while (1) {
1385                         char *value = strtok_r(NULL, delimiters, &saveptr);
1386
1387                         if (!value)
1388                                 break;
1389                         el->binary.args[argi] = strtoul(value, NULL, 16);
1390                         argi++;
1391                         if (argi >= BINARY_MAX_ARGS) {
1392                                 fprintf(stderr,
1393                                         "Too many arguments for BINARY\n");
1394                                 return -1;
1395                         }
1396                 }
1397                 el->binary.nargs = argi;
1398                 break;
1399         case IMAGE_CFG_DATA:
1400                 value2 = strtok_r(NULL, delimiters, &saveptr);
1401
1402                 if (!value1 || !value2) {
1403                         fprintf(stderr,
1404                                 "Invalid number of arguments for DATA\n");
1405                         return -1;
1406                 }
1407
1408                 el->regdata.raddr = strtoul(value1, NULL, 16);
1409                 el->regdata.rdata = strtoul(value2, NULL, 16);
1410                 break;
1411         case IMAGE_CFG_BAUDRATE:
1412                 el->baudrate = strtoul(value1, NULL, 10);
1413                 break;
1414         case IMAGE_CFG_DEBUG:
1415                 el->debug = strtoul(value1, NULL, 10);
1416                 break;
1417         case IMAGE_CFG_KAK:
1418                 el->key_name = strdup(value1);
1419                 break;
1420         case IMAGE_CFG_CSK:
1421                 el->key_name = strdup(value1);
1422                 break;
1423         case IMAGE_CFG_CSK_INDEX:
1424                 el->csk_idx = strtol(value1, NULL, 0);
1425                 break;
1426         case IMAGE_CFG_JTAG_DELAY:
1427                 el->jtag_delay = strtoul(value1, NULL, 0);
1428                 break;
1429         case IMAGE_CFG_BOX_ID:
1430                 el->boxid = strtoul(value1, NULL, 0);
1431                 break;
1432         case IMAGE_CFG_FLASH_ID:
1433                 el->flashid = strtoul(value1, NULL, 0);
1434                 break;
1435         case IMAGE_CFG_SEC_SPECIALIZED_IMG:
1436                 el->sec_specialized_img = true;
1437                 break;
1438         case IMAGE_CFG_SEC_COMMON_IMG:
1439                 el->sec_specialized_img = false;
1440                 break;
1441         case IMAGE_CFG_SEC_BOOT_DEV:
1442                 el->sec_boot_dev = strtoul(value1, NULL, 0);
1443                 break;
1444         case IMAGE_CFG_SEC_FUSE_DUMP:
1445                 el->name = strdup(value1);
1446                 break;
1447         default:
1448                 fprintf(stderr, unknown_msg, line);
1449         }
1450
1451         return 0;
1452 }
1453
1454 /*
1455  * Parse the configuration file 'fcfg' into the array of configuration
1456  * elements 'image_cfg', and return the number of configuration
1457  * elements in 'cfgn'.
1458  */
1459 static int image_create_config_parse(FILE *fcfg)
1460 {
1461         int ret;
1462         int cfgi = 0;
1463
1464         /* Parse the configuration file */
1465         while (!feof(fcfg)) {
1466                 char *line;
1467                 char buf[256];
1468
1469                 /* Read the current line */
1470                 memset(buf, 0, sizeof(buf));
1471                 line = fgets(buf, sizeof(buf), fcfg);
1472                 if (!line)
1473                         break;
1474
1475                 /* Ignore useless lines */
1476                 if (line[0] == '\n' || line[0] == '#')
1477                         continue;
1478
1479                 /* Strip final newline */
1480                 if (line[strlen(line) - 1] == '\n')
1481                         line[strlen(line) - 1] = 0;
1482
1483                 /* Parse the current line */
1484                 ret = image_create_config_parse_oneline(line,
1485                                                         &image_cfg[cfgi]);
1486                 if (ret)
1487                         return ret;
1488
1489                 cfgi++;
1490
1491                 if (cfgi >= IMAGE_CFG_ELEMENT_MAX) {
1492                         fprintf(stderr,
1493                                 "Too many configuration elements in .cfg file\n");
1494                         return -1;
1495                 }
1496         }
1497
1498         cfgn = cfgi;
1499         return 0;
1500 }
1501
1502 static int image_get_version(void)
1503 {
1504         struct image_cfg_element *e;
1505
1506         e = image_find_option(IMAGE_CFG_VERSION);
1507         if (!e)
1508                 return -1;
1509
1510         return e->version;
1511 }
1512
1513 static void kwbimage_set_header(void *ptr, struct stat *sbuf, int ifd,
1514                                 struct image_tool_params *params)
1515 {
1516         FILE *fcfg;
1517         void *image = NULL;
1518         int version;
1519         size_t headersz = 0;
1520         uint32_t checksum;
1521         int ret;
1522         int size;
1523
1524         fcfg = fopen(params->imagename, "r");
1525         if (!fcfg) {
1526                 fprintf(stderr, "Could not open input file %s\n",
1527                         params->imagename);
1528                 exit(EXIT_FAILURE);
1529         }
1530
1531         image_cfg = malloc(IMAGE_CFG_ELEMENT_MAX *
1532                            sizeof(struct image_cfg_element));
1533         if (!image_cfg) {
1534                 fprintf(stderr, "Cannot allocate memory\n");
1535                 fclose(fcfg);
1536                 exit(EXIT_FAILURE);
1537         }
1538
1539         memset(image_cfg, 0,
1540                IMAGE_CFG_ELEMENT_MAX * sizeof(struct image_cfg_element));
1541         rewind(fcfg);
1542
1543         ret = image_create_config_parse(fcfg);
1544         fclose(fcfg);
1545         if (ret) {
1546                 free(image_cfg);
1547                 exit(EXIT_FAILURE);
1548         }
1549
1550         /* The MVEBU BootROM does not allow non word aligned payloads */
1551         sbuf->st_size = ALIGN(sbuf->st_size, 4);
1552
1553         version = image_get_version();
1554         switch (version) {
1555                 /*
1556                  * Fallback to version 0 if no version is provided in the
1557                  * cfg file
1558                  */
1559         case -1:
1560         case 0:
1561                 image = image_create_v0(&headersz, params, sbuf->st_size);
1562                 break;
1563
1564         case 1:
1565                 image = image_create_v1(&headersz, params, ptr, sbuf->st_size);
1566                 break;
1567
1568         default:
1569                 fprintf(stderr, "Unsupported version %d\n", version);
1570                 free(image_cfg);
1571                 exit(EXIT_FAILURE);
1572         }
1573
1574         if (!image) {
1575                 fprintf(stderr, "Could not create image\n");
1576                 free(image_cfg);
1577                 exit(EXIT_FAILURE);
1578         }
1579
1580         free(image_cfg);
1581
1582         /* Build and add image checksum header */
1583         checksum =
1584                 cpu_to_le32(image_checksum32((uint32_t *)ptr, sbuf->st_size));
1585         size = write(ifd, &checksum, sizeof(uint32_t));
1586         if (size != sizeof(uint32_t)) {
1587                 fprintf(stderr, "Error:%s - Checksum write %d bytes %s\n",
1588                         params->cmdname, size, params->imagefile);
1589                 exit(EXIT_FAILURE);
1590         }
1591
1592         sbuf->st_size += sizeof(uint32_t);
1593
1594         /* Finally copy the header into the image area */
1595         memcpy(ptr, image, headersz);
1596
1597         free(image);
1598 }
1599
1600 static void kwbimage_print_header(const void *ptr)
1601 {
1602         struct main_hdr_v0 *mhdr = (struct main_hdr_v0 *)ptr;
1603
1604         printf("Image Type:   MVEBU Boot from %s Image\n",
1605                image_boot_mode_name(mhdr->blockid));
1606         printf("Image version:%d\n", image_version((void *)ptr));
1607         printf("Data Size:    ");
1608         genimg_print_size(mhdr->blocksize - sizeof(uint32_t));
1609         printf("Load Address: %08x\n", mhdr->destaddr);
1610         printf("Entry Point:  %08x\n", mhdr->execaddr);
1611 }
1612
1613 static int kwbimage_check_image_types(uint8_t type)
1614 {
1615         if (type == IH_TYPE_KWBIMAGE)
1616                 return EXIT_SUCCESS;
1617
1618         return EXIT_FAILURE;
1619 }
1620
1621 static int kwbimage_verify_header(unsigned char *ptr, int image_size,
1622                                   struct image_tool_params *params)
1623 {
1624         uint8_t checksum;
1625         size_t header_size = kwbimage_header_size(ptr);
1626
1627         if (header_size > image_size)
1628                 return -FDT_ERR_BADSTRUCTURE;
1629
1630         if (!main_hdr_checksum_ok(ptr))
1631                 return -FDT_ERR_BADSTRUCTURE;
1632
1633         /* Only version 0 extended header has checksum */
1634         if (image_version((void *)ptr) == 0) {
1635                 struct ext_hdr_v0 *ext_hdr;
1636
1637                 ext_hdr = (struct ext_hdr_v0 *)
1638                                 (ptr + sizeof(struct main_hdr_v0));
1639                 checksum = image_checksum8(ext_hdr,
1640                                            sizeof(struct ext_hdr_v0)
1641                                            - sizeof(uint8_t));
1642                 if (checksum != ext_hdr->checksum)
1643                         return -FDT_ERR_BADSTRUCTURE;
1644         }
1645
1646         return 0;
1647 }
1648
1649 static int kwbimage_generate(struct image_tool_params *params,
1650                              struct image_type_params *tparams)
1651 {
1652         FILE *fcfg;
1653         int alloc_len;
1654         int version;
1655         void *hdr;
1656         int ret;
1657
1658         fcfg = fopen(params->imagename, "r");
1659         if (!fcfg) {
1660                 fprintf(stderr, "Could not open input file %s\n",
1661                         params->imagename);
1662                 exit(EXIT_FAILURE);
1663         }
1664
1665         image_cfg = malloc(IMAGE_CFG_ELEMENT_MAX *
1666                            sizeof(struct image_cfg_element));
1667         if (!image_cfg) {
1668                 fprintf(stderr, "Cannot allocate memory\n");
1669                 fclose(fcfg);
1670                 exit(EXIT_FAILURE);
1671         }
1672
1673         memset(image_cfg, 0,
1674                IMAGE_CFG_ELEMENT_MAX * sizeof(struct image_cfg_element));
1675         rewind(fcfg);
1676
1677         ret = image_create_config_parse(fcfg);
1678         fclose(fcfg);
1679         if (ret) {
1680                 free(image_cfg);
1681                 exit(EXIT_FAILURE);
1682         }
1683
1684         version = image_get_version();
1685         switch (version) {
1686                 /*
1687                  * Fallback to version 0 if no version is provided in the
1688                  * cfg file
1689                  */
1690         case -1:
1691         case 0:
1692                 alloc_len = sizeof(struct main_hdr_v0) +
1693                         sizeof(struct ext_hdr_v0);
1694                 break;
1695
1696         case 1:
1697                 alloc_len = image_headersz_v1(NULL);
1698                 break;
1699
1700         default:
1701                 fprintf(stderr, "Unsupported version %d\n", version);
1702                 free(image_cfg);
1703                 exit(EXIT_FAILURE);
1704         }
1705
1706         free(image_cfg);
1707
1708         hdr = malloc(alloc_len);
1709         if (!hdr) {
1710                 fprintf(stderr, "%s: malloc return failure: %s\n",
1711                         params->cmdname, strerror(errno));
1712                 exit(EXIT_FAILURE);
1713         }
1714
1715         memset(hdr, 0, alloc_len);
1716         tparams->header_size = alloc_len;
1717         tparams->hdr = hdr;
1718
1719         /*
1720          * The resulting image needs to be 4-byte aligned. At least
1721          * the Marvell hdrparser tool complains if its unaligned.
1722          * By returning 1 here in this function, called via
1723          * tparams->vrec_header() in mkimage.c, mkimage will
1724          * automatically pad the the resulting image to a 4-byte
1725          * size if necessary.
1726          */
1727         return 1;
1728 }
1729
1730 /*
1731  * Report Error if xflag is set in addition to default
1732  */
1733 static int kwbimage_check_params(struct image_tool_params *params)
1734 {
1735         if (!strlen(params->imagename)) {
1736                 char *msg = "Configuration file for kwbimage creation omitted";
1737
1738                 fprintf(stderr, "Error:%s - %s\n", params->cmdname, msg);
1739                 return CFG_INVALID;
1740         }
1741
1742         return (params->dflag && (params->fflag || params->lflag)) ||
1743                 (params->fflag && (params->dflag || params->lflag)) ||
1744                 (params->lflag && (params->dflag || params->fflag)) ||
1745                 (params->xflag) || !(strlen(params->imagename));
1746 }
1747
1748 /*
1749  * kwbimage type parameters definition
1750  */
1751 U_BOOT_IMAGE_TYPE(
1752         kwbimage,
1753         "Marvell MVEBU Boot Image support",
1754         0,
1755         NULL,
1756         kwbimage_check_params,
1757         kwbimage_verify_header,
1758         kwbimage_print_header,
1759         kwbimage_set_header,
1760         NULL,
1761         kwbimage_check_image_types,
1762         NULL,
1763         kwbimage_generate
1764 );