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 255
  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 - 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         int cfgi;
 946
 947         /*
 948          * Calculate the size of the header and the size of the
 949          * payload
 950          */
 951         headersz = sizeof(struct main_hdr_v1);
 952
 953         if (image_count_options(IMAGE_CFG_PAYLOAD) > 1) {
 954                 fprintf(stderr, "More than one payload, not possible\n");
 955                 return 0;
 956         }
 957
 958         for (cfgi = 0; cfgi < cfgn; cfgi++) {
 959                 int ret;
 960                 struct stat s;
 961
 962                 binarye = &image_cfg[cfgi];
 963                 if (binarye->type != IMAGE_CFG_BINARY)
 964                         continue;
 965
 966                 ret = stat(binarye->binary.file, &s);
 967                 if (ret < 0) {
 968                         char cwd[PATH_MAX];
 969                         char *dir = cwd;
 970
 971                         memset(cwd, 0, sizeof(cwd));
 972                         if (!getcwd(cwd, sizeof(cwd))) {
 973                                 dir = "current working directory";
 974                                 perror("getcwd() failed");
 975                         }
 976
 977                         fprintf(stderr,
 978                                 "Didn't find the file '%s' in '%s' which is mandatory to generate the image\n"
 979                                 "This file generally contains the DDR3 training code, and should be extracted from an existing bootable\n"
 980                                 "image for your board. See 'kwbimage -x' to extract it from an existing image.\n",
 981                                 binarye->binary.file, dir);
 982                         return 0;
 983                 }
 984
 985                 headersz += sizeof(struct opt_hdr_v1) +
 986                         ALIGN(s.st_size, 4) +
 987                         (binarye->binary.nargs + 2) * sizeof(uint32_t);
 988                 if (hasext)
 989                         *hasext = 1;
 990         }
 991
 992 #if defined(CONFIG_KWB_SECURE)
 993         if (image_get_csk_index() >= 0) {
 994                 headersz += sizeof(struct secure_hdr_v1);
 995                 if (hasext)
 996                         *hasext = 1;
 997         }
 998 #endif
 999
1000 #if defined(CONFIG_SYS_U_BOOT_OFFS)
1001         if (headersz > CONFIG_SYS_U_BOOT_OFFS) {
1002                 fprintf(stderr,
1003                         "Error: Image header (incl. SPL image) too big!\n");
1004                 fprintf(stderr, "header=0x%x CONFIG_SYS_U_BOOT_OFFS=0x%x!\n",
1005                         (int)headersz, CONFIG_SYS_U_BOOT_OFFS);
1006                 fprintf(stderr, "Increase CONFIG_SYS_U_BOOT_OFFS!\n");
1007                 return 0;
1008         }
1009
1010         headersz = CONFIG_SYS_U_BOOT_OFFS;
1011 #endif
1012
1013         /*
1014          * The payload should be aligned on some reasonable
1015          * boundary
1016          */
1017         return ALIGN(headersz, 4096);
1018 }
1019
1020 int add_binary_header_v1(uint8_t **cur, uint8_t **next_ext,
1021                          struct image_cfg_element *binarye)
1022 {
1023         struct opt_hdr_v1 *hdr = (struct opt_hdr_v1 *)*cur;
1024         uint32_t *args;
1025         size_t binhdrsz;
1026         struct stat s;
1027         int argi;
1028         FILE *bin;
1029         int ret;
1030
1031         hdr->headertype = OPT_HDR_V1_BINARY_TYPE;
1032
1033         bin = fopen(binarye->binary.file, "r");
1034         if (!bin) {
1035                 fprintf(stderr, "Cannot open binary file %s\n",
1036                         binarye->binary.file);
1037                 return -1;
1038         }
1039
1040         if (fstat(fileno(bin), &s)) {
1041                 fprintf(stderr, "Cannot stat binary file %s\n",
1042                         binarye->binary.file);
1043                 goto err_close;
1044         }
1045
1046         binhdrsz = sizeof(struct opt_hdr_v1) +
1047                 (binarye->binary.nargs + 2) * sizeof(uint32_t) +
1048                 ALIGN(s.st_size, 4);
1049         hdr->headersz_lsb = cpu_to_le16(binhdrsz & 0xFFFF);
1050         hdr->headersz_msb = (binhdrsz & 0xFFFF0000) >> 16;
1051
1052         *cur += sizeof(struct opt_hdr_v1);
1053
1054         args = (uint32_t *)*cur;
1055         *args = cpu_to_le32(binarye->binary.nargs);
1056         args++;
1057         for (argi = 0; argi < binarye->binary.nargs; argi++)
1058                 args[argi] = cpu_to_le32(binarye->binary.args[argi]);
1059
1060         *cur += (binarye->binary.nargs + 1) * sizeof(uint32_t);
1061
1062         ret = fread(*cur, s.st_size, 1, bin);
1063         if (ret != 1) {
1064                 fprintf(stderr,
1065                         "Could not read binary image %s\n",
1066                         binarye->binary.file);
1067                 goto err_close;
1068         }
1069
1070         fclose(bin);
1071
1072         *cur += ALIGN(s.st_size, 4);
1073
1074         *((uint32_t *)*cur) = 0x00000000;
1075         **next_ext = 1;
1076         *next_ext = *cur;
1077
1078         *cur += sizeof(uint32_t);
1079
1080         return 0;
1081
1082 err_close:
1083         fclose(bin);
1084
1085         return -1;
1086 }
1087
1088 #if defined(CONFIG_KWB_SECURE)
1089
1090 int export_pub_kak_hash(RSA *kak, struct secure_hdr_v1 *secure_hdr)
1091 {
1092         FILE *hashf;
1093         int res;
1094
1095         hashf = fopen("pub_kak_hash.txt", "w");
1096
1097         res = kwb_export_pubkey(kak, &secure_hdr->kak, hashf, "KAK");
1098
1099         fclose(hashf);
1100
1101         return res < 0 ? 1 : 0;
1102 }
1103
1104 int kwb_sign_csk_with_kak(struct image_tool_params *params,
1105                           struct secure_hdr_v1 *secure_hdr, RSA *csk)
1106 {
1107         RSA *kak = NULL;
1108         RSA *kak_pub = NULL;
1109         int csk_idx = image_get_csk_index();
1110         struct sig_v1 tmp_sig;
1111
1112         if (csk_idx >= 16) {
1113                 fprintf(stderr, "Invalid CSK index %d\n", csk_idx);
1114                 return 1;
1115         }
1116
1117         if (kwb_load_kak(params, &kak) < 0)
1118                 return 1;
1119
1120         if (export_pub_kak_hash(kak, secure_hdr))
1121                 return 1;
1122
1123         if (kwb_import_pubkey(&kak_pub, &secure_hdr->kak, "KAK") < 0)
1124                 return 1;
1125
1126         if (kwb_export_pubkey(csk, &secure_hdr->csk[csk_idx], NULL, "CSK") < 0)
1127                 return 1;
1128
1129         if (kwb_sign_and_verify(kak, &secure_hdr->csk,
1130                                 sizeof(secure_hdr->csk) +
1131                                 sizeof(secure_hdr->csksig),
1132                                 &tmp_sig, "CSK") < 0)
1133                 return 1;
1134
1135         if (kwb_verify(kak_pub, &secure_hdr->csk,
1136                        sizeof(secure_hdr->csk) +
1137                        sizeof(secure_hdr->csksig),
1138                        &tmp_sig, "CSK (2)") < 0)
1139                 return 1;
1140
1141         secure_hdr->csksig = tmp_sig;
1142
1143         return 0;
1144 }
1145
1146 int add_secure_header_v1(struct image_tool_params *params, uint8_t *ptr,
1147                          int payloadsz, size_t headersz, uint8_t *image,
1148                          struct secure_hdr_v1 *secure_hdr)
1149 {
1150         struct image_cfg_element *e_jtagdelay;
1151         struct image_cfg_element *e_boxid;
1152         struct image_cfg_element *e_flashid;
1153         RSA *csk = NULL;
1154         unsigned char *image_ptr;
1155         size_t image_size;
1156         struct sig_v1 tmp_sig;
1157         bool specialized_img = image_get_spezialized_img();
1158
1159         kwb_msg("Create secure header content\n");
1160
1161         e_jtagdelay = image_find_option(IMAGE_CFG_JTAG_DELAY);
1162         e_boxid = image_find_option(IMAGE_CFG_BOX_ID);
1163         e_flashid = image_find_option(IMAGE_CFG_FLASH_ID);
1164
1165         if (kwb_load_csk(params, &csk) < 0)
1166                 return 1;
1167
1168         secure_hdr->headertype = OPT_HDR_V1_SECURE_TYPE;
1169         secure_hdr->headersz_msb = 0;
1170         secure_hdr->headersz_lsb = cpu_to_le16(sizeof(struct secure_hdr_v1));
1171         if (e_jtagdelay)
1172                 secure_hdr->jtag_delay = e_jtagdelay->jtag_delay;
1173         if (e_boxid && specialized_img)
1174                 secure_hdr->boxid = cpu_to_le32(e_boxid->boxid);
1175         if (e_flashid && specialized_img)
1176                 secure_hdr->flashid = cpu_to_le32(e_flashid->flashid);
1177
1178         if (kwb_sign_csk_with_kak(params, secure_hdr, csk))
1179                 return 1;
1180
1181         image_ptr = ptr + headersz;
1182         image_size = payloadsz - headersz;
1183
1184         if (kwb_sign_and_verify(csk, image_ptr, image_size,
1185                                 &secure_hdr->imgsig, "image") < 0)
1186                 return 1;
1187
1188         if (kwb_sign_and_verify(csk, image, headersz, &tmp_sig, "header") < 0)
1189                 return 1;
1190
1191         secure_hdr->hdrsig = tmp_sig;
1192
1193         kwb_dump_fuse_cmds(secure_hdr);
1194
1195         return 0;
1196 }
1197 #endif
1198
1199 static void *image_create_v1(size_t *imagesz, struct image_tool_params *params,
1200                              uint8_t *ptr, int payloadsz)
1201 {
1202         struct image_cfg_element *e;
1203         struct main_hdr_v1 *main_hdr;
1204 #if defined(CONFIG_KWB_SECURE)
1205         struct secure_hdr_v1 *secure_hdr = NULL;
1206 #endif
1207         size_t headersz;
1208         uint8_t *image, *cur;
1209         int hasext = 0;
1210         uint8_t *next_ext = NULL;
1211         int cfgi;
1212
1213         /*
1214          * Calculate the size of the header and the size of the
1215          * payload
1216          */
1217         headersz = image_headersz_v1(&hasext);
1218         if (headersz == 0)
1219                 return NULL;
1220
1221         image = malloc(headersz);
1222         if (!image) {
1223                 fprintf(stderr, "Cannot allocate memory for image\n");
1224                 return NULL;
1225         }
1226
1227         memset(image, 0, headersz);
1228
1229         main_hdr = (struct main_hdr_v1 *)image;
1230         cur = image;
1231         cur += sizeof(struct main_hdr_v1);
1232         next_ext = &main_hdr->ext;
1233
1234         /* Fill the main header */
1235         main_hdr->blocksize    =
1236                 cpu_to_le32(payloadsz - headersz);
1237         main_hdr->headersz_lsb = cpu_to_le16(headersz & 0xFFFF);
1238         main_hdr->headersz_msb = (headersz & 0xFFFF0000) >> 16;
1239         main_hdr->destaddr     = cpu_to_le32(params->addr);
1240         main_hdr->execaddr     = cpu_to_le32(params->ep);
1241         main_hdr->srcaddr      = cpu_to_le32(headersz);
1242         main_hdr->ext          = hasext;
1243         main_hdr->version      = 1;
1244         e = image_find_option(IMAGE_CFG_BOOT_FROM);
1245         if (e)
1246                 main_hdr->blockid = e->bootfrom;
1247         e = image_find_option(IMAGE_CFG_NAND_BLKSZ);
1248         if (e)
1249                 main_hdr->nandblocksize = e->nandblksz / (64 * 1024);
1250         e = image_find_option(IMAGE_CFG_NAND_BADBLK_LOCATION);
1251         if (e)
1252                 main_hdr->nandbadblklocation = e->nandbadblklocation;
1253         e = image_find_option(IMAGE_CFG_BAUDRATE);
1254         if (e)
1255                 main_hdr->options = baudrate_to_option(e->baudrate);
1256         e = image_find_option(IMAGE_CFG_DEBUG);
1257         if (e)
1258                 main_hdr->flags = e->debug ? 0x1 : 0;
1259
1260         /*
1261          * For SATA srcaddr is specified in number of sectors starting from
1262          * sector 0. The main header is stored at sector number 1.
1263          * This expects the sector size to be 512 bytes.
1264          * Header size is already aligned.
1265          */
1266         if (main_hdr->blockid == IBR_HDR_SATA_ID)
1267                 main_hdr->srcaddr = cpu_to_le32(headersz / 512 + 1);
1268
1269         /*
1270          * For SDIO srcaddr is specified in number of sectors starting from
1271          * sector 0. The main header is stored at sector number 0.
1272          * This expects sector size to be 512 bytes.
1273          * Header size is already aligned.
1274          */
1275         if (main_hdr->blockid == IBR_HDR_SDIO_ID)
1276                 main_hdr->srcaddr = cpu_to_le32(headersz / 512);
1277
1278         /* For PCIe srcaddr is not used and must be set to 0xFFFFFFFF. */
1279         if (main_hdr->blockid == IBR_HDR_PEX_ID)
1280                 main_hdr->srcaddr = cpu_to_le32(0xFFFFFFFF);
1281
1282 #if defined(CONFIG_KWB_SECURE)
1283         if (image_get_csk_index() >= 0) {
1284                 /*
1285                  * only reserve the space here; we fill the header later since
1286                  * we need the header to be complete to compute the signatures
1287                  */
1288                 secure_hdr = (struct secure_hdr_v1 *)cur;
1289                 cur += sizeof(struct secure_hdr_v1);
1290                 *next_ext = 1;
1291                 next_ext = &secure_hdr->next;
1292         }
1293 #endif
1294
1295         for (cfgi = 0; cfgi < cfgn; cfgi++) {
1296                 e = &image_cfg[cfgi];
1297                 if (e->type != IMAGE_CFG_BINARY)
1298                         continue;
1299
1300                 if (add_binary_header_v1(&cur, &next_ext, e))
1301                         return NULL;
1302         }
1303
1304 #if defined(CONFIG_KWB_SECURE)
1305         if (secure_hdr && add_secure_header_v1(params, ptr, payloadsz,
1306                                                headersz, image, secure_hdr))
1307                 return NULL;
1308 #endif
1309
1310         /* Calculate and set the header checksum */
1311         main_hdr->checksum = image_checksum8(main_hdr, headersz);
1312
1313         *imagesz = headersz;
1314         return image;
1315 }
1316
1317 int recognize_keyword(char *keyword)
1318 {
1319         int kw_id;
1320
1321         for (kw_id = 1; kw_id < IMAGE_CFG_COUNT; ++kw_id)
1322                 if (!strcmp(keyword, id_strs[kw_id]))
1323                         return kw_id;
1324
1325         return 0;
1326 }
1327
1328 static int image_create_config_parse_oneline(char *line,
1329                                              struct image_cfg_element *el)
1330 {
1331         char *keyword, *saveptr, *value1, *value2;
1332         char delimiters[] = " \t";
1333         int keyword_id, ret, argi;
1334         char *unknown_msg = "Ignoring unknown line '%s'\n";
1335
1336         keyword = strtok_r(line, delimiters, &saveptr);
1337         keyword_id = recognize_keyword(keyword);
1338
1339         if (!keyword_id) {
1340                 fprintf(stderr, unknown_msg, line);
1341                 return 0;
1342         }
1343
1344         el->type = keyword_id;
1345
1346         value1 = strtok_r(NULL, delimiters, &saveptr);
1347
1348         if (!value1) {
1349                 fprintf(stderr, "Parameter missing in line '%s'\n", line);
1350                 return -1;
1351         }
1352
1353         switch (keyword_id) {
1354         case IMAGE_CFG_VERSION:
1355                 el->version = atoi(value1);
1356                 break;
1357         case IMAGE_CFG_BOOT_FROM:
1358                 ret = image_boot_mode_id(value1);
1359
1360                 if (ret < 0) {
1361                         fprintf(stderr, "Invalid boot media '%s'\n", value1);
1362                         return -1;
1363                 }
1364                 el->bootfrom = ret;
1365                 break;
1366         case IMAGE_CFG_NAND_BLKSZ:
1367                 el->nandblksz = strtoul(value1, NULL, 16);
1368                 break;
1369         case IMAGE_CFG_NAND_BADBLK_LOCATION:
1370                 el->nandbadblklocation = strtoul(value1, NULL, 16);
1371                 break;
1372         case IMAGE_CFG_NAND_ECC_MODE:
1373                 ret = image_nand_ecc_mode_id(value1);
1374
1375                 if (ret < 0) {
1376                         fprintf(stderr, "Invalid NAND ECC mode '%s'\n", value1);
1377                         return -1;
1378                 }
1379                 el->nandeccmode = ret;
1380                 break;
1381         case IMAGE_CFG_NAND_PAGESZ:
1382                 el->nandpagesz = strtoul(value1, NULL, 16);
1383                 break;
1384         case IMAGE_CFG_BINARY:
1385                 argi = 0;
1386
1387                 el->binary.file = strdup(value1);
1388                 while (1) {
1389                         char *value = strtok_r(NULL, delimiters, &saveptr);
1390
1391                         if (!value)
1392                                 break;
1393                         el->binary.args[argi] = strtoul(value, NULL, 16);
1394                         argi++;
1395                         if (argi >= BINARY_MAX_ARGS) {
1396                                 fprintf(stderr,
1397                                         "Too many arguments for BINARY\n");
1398                                 return -1;
1399                         }
1400                 }
1401                 el->binary.nargs = argi;
1402                 break;
1403         case IMAGE_CFG_DATA:
1404                 value2 = strtok_r(NULL, delimiters, &saveptr);
1405
1406                 if (!value1 || !value2) {
1407                         fprintf(stderr,
1408                                 "Invalid number of arguments for DATA\n");
1409                         return -1;
1410                 }
1411
1412                 el->regdata.raddr = strtoul(value1, NULL, 16);
1413                 el->regdata.rdata = strtoul(value2, NULL, 16);
1414                 break;
1415         case IMAGE_CFG_BAUDRATE:
1416                 el->baudrate = strtoul(value1, NULL, 10);
1417                 break;
1418         case IMAGE_CFG_DEBUG:
1419                 el->debug = strtoul(value1, NULL, 10);
1420                 break;
1421         case IMAGE_CFG_KAK:
1422                 el->key_name = strdup(value1);
1423                 break;
1424         case IMAGE_CFG_CSK:
1425                 el->key_name = strdup(value1);
1426                 break;
1427         case IMAGE_CFG_CSK_INDEX:
1428                 el->csk_idx = strtol(value1, NULL, 0);
1429                 break;
1430         case IMAGE_CFG_JTAG_DELAY:
1431                 el->jtag_delay = strtoul(value1, NULL, 0);
1432                 break;
1433         case IMAGE_CFG_BOX_ID:
1434                 el->boxid = strtoul(value1, NULL, 0);
1435                 break;
1436         case IMAGE_CFG_FLASH_ID:
1437                 el->flashid = strtoul(value1, NULL, 0);
1438                 break;
1439         case IMAGE_CFG_SEC_SPECIALIZED_IMG:
1440                 el->sec_specialized_img = true;
1441                 break;
1442         case IMAGE_CFG_SEC_COMMON_IMG:
1443                 el->sec_specialized_img = false;
1444                 break;
1445         case IMAGE_CFG_SEC_BOOT_DEV:
1446                 el->sec_boot_dev = strtoul(value1, NULL, 0);
1447                 break;
1448         case IMAGE_CFG_SEC_FUSE_DUMP:
1449                 el->name = strdup(value1);
1450                 break;
1451         default:
1452                 fprintf(stderr, unknown_msg, line);
1453         }
1454
1455         return 0;
1456 }
1457
1458 /*
1459  * Parse the configuration file 'fcfg' into the array of configuration
1460  * elements 'image_cfg', and return the number of configuration
1461  * elements in 'cfgn'.
1462  */
1463 static int image_create_config_parse(FILE *fcfg)
1464 {
1465         int ret;
1466         int cfgi = 0;
1467
1468         /* Parse the configuration file */
1469         while (!feof(fcfg)) {
1470                 char *line;
1471                 char buf[256];
1472
1473                 /* Read the current line */
1474                 memset(buf, 0, sizeof(buf));
1475                 line = fgets(buf, sizeof(buf), fcfg);
1476                 if (!line)
1477                         break;
1478
1479                 /* Ignore useless lines */
1480                 if (line[0] == '\n' || line[0] == '#')
1481                         continue;
1482
1483                 /* Strip final newline */
1484                 if (line[strlen(line) - 1] == '\n')
1485                         line[strlen(line) - 1] = 0;
1486
1487                 /* Parse the current line */
1488                 ret = image_create_config_parse_oneline(line,
1489                                                         &image_cfg[cfgi]);
1490                 if (ret)
1491                         return ret;
1492
1493                 cfgi++;
1494
1495                 if (cfgi >= IMAGE_CFG_ELEMENT_MAX) {
1496                         fprintf(stderr,
1497                                 "Too many configuration elements in .cfg file\n");
1498                         return -1;
1499                 }
1500         }
1501
1502         cfgn = cfgi;
1503         return 0;
1504 }
1505
1506 static int image_get_version(void)
1507 {
1508         struct image_cfg_element *e;
1509
1510         e = image_find_option(IMAGE_CFG_VERSION);
1511         if (!e)
1512                 return -1;
1513
1514         return e->version;
1515 }
1516
1517 static int image_get_bootfrom(void)
1518 {
1519         struct image_cfg_element *e;
1520
1521         e = image_find_option(IMAGE_CFG_BOOT_FROM);
1522         if (!e)
1523                 return -1;
1524
1525         return e->bootfrom;
1526 }
1527
1528 static void kwbimage_set_header(void *ptr, struct stat *sbuf, int ifd,
1529                                 struct image_tool_params *params)
1530 {
1531         FILE *fcfg;
1532         void *image = NULL;
1533         int version;
1534         size_t headersz = 0;
1535         uint32_t checksum;
1536         int ret;
1537
1538         fcfg = fopen(params->imagename, "r");
1539         if (!fcfg) {
1540                 fprintf(stderr, "Could not open input file %s\n",
1541                         params->imagename);
1542                 exit(EXIT_FAILURE);
1543         }
1544
1545         image_cfg = malloc(IMAGE_CFG_ELEMENT_MAX *
1546                            sizeof(struct image_cfg_element));
1547         if (!image_cfg) {
1548                 fprintf(stderr, "Cannot allocate memory\n");
1549                 fclose(fcfg);
1550                 exit(EXIT_FAILURE);
1551         }
1552
1553         memset(image_cfg, 0,
1554                IMAGE_CFG_ELEMENT_MAX * sizeof(struct image_cfg_element));
1555         rewind(fcfg);
1556
1557         ret = image_create_config_parse(fcfg);
1558         fclose(fcfg);
1559         if (ret) {
1560                 free(image_cfg);
1561                 exit(EXIT_FAILURE);
1562         }
1563
1564         version = image_get_version();
1565         switch (version) {
1566                 /*
1567                  * Fallback to version 0 if no version is provided in the
1568                  * cfg file
1569                  */
1570         case -1:
1571         case 0:
1572                 image = image_create_v0(&headersz, params, sbuf->st_size);
1573                 break;
1574
1575         case 1:
1576                 image = image_create_v1(&headersz, params, ptr, sbuf->st_size);
1577                 break;
1578
1579         default:
1580                 fprintf(stderr, "Unsupported version %d\n", version);
1581                 free(image_cfg);
1582                 exit(EXIT_FAILURE);
1583         }
1584
1585         if (!image) {
1586                 fprintf(stderr, "Could not create image\n");
1587                 free(image_cfg);
1588                 exit(EXIT_FAILURE);
1589         }
1590
1591         free(image_cfg);
1592
1593         /* Build and add image checksum header */
1594         checksum = cpu_to_le32(image_checksum32((uint8_t *)ptr + headersz,
1595                                 sbuf->st_size - headersz - sizeof(uint32_t)));
1596         memcpy((uint8_t *)ptr + sbuf->st_size - sizeof(uint32_t), &checksum,
1597                 sizeof(uint32_t));
1598
1599         /* Finally copy the header into the image area */
1600         memcpy(ptr, image, headersz);
1601
1602         free(image);
1603 }
1604
1605 static void kwbimage_print_header(const void *ptr)
1606 {
1607         struct main_hdr_v0 *mhdr = (struct main_hdr_v0 *)ptr;
1608
1609         printf("Image Type:   MVEBU Boot from %s Image\n",
1610                image_boot_mode_name(mhdr->blockid));
1611         printf("Image version:%d\n", image_version((void *)ptr));
1612         if (image_version((void *)ptr) == 1) {
1613                 struct main_hdr_v1 *mhdr = (struct main_hdr_v1 *)ptr;
1614
1615                 if (mhdr->ext & 0x1) {
1616                         struct opt_hdr_v1 *ohdr = (struct opt_hdr_v1 *)
1617                                                   ((uint8_t *)ptr +
1618                                                    sizeof(*mhdr));
1619
1620                         while (1) {
1621                                 uint32_t ohdr_size;
1622
1623                                 ohdr_size = (ohdr->headersz_msb << 16) |
1624                                             le16_to_cpu(ohdr->headersz_lsb);
1625                                 if (ohdr->headertype == OPT_HDR_V1_BINARY_TYPE) {
1626                                         printf("BIN Hdr Size: ");
1627                                         genimg_print_size(ohdr_size - 12 - 4 * ohdr->data[0]);
1628                                 }
1629                                 if (!(*((uint8_t *)ohdr + ohdr_size - 4) & 0x1))
1630                                         break;
1631                                 ohdr = (struct opt_hdr_v1 *)((uint8_t *)ohdr +
1632                                                              ohdr_size);
1633                         }
1634                 }
1635         }
1636         printf("Data Size:    ");
1637         genimg_print_size(mhdr->blocksize - sizeof(uint32_t));
1638         printf("Load Address: %08x\n", mhdr->destaddr);
1639         printf("Entry Point:  %08x\n", mhdr->execaddr);
1640 }
1641
1642 static int kwbimage_check_image_types(uint8_t type)
1643 {
1644         if (type == IH_TYPE_KWBIMAGE)
1645                 return EXIT_SUCCESS;
1646
1647         return EXIT_FAILURE;
1648 }
1649
1650 static int kwbimage_verify_header(unsigned char *ptr, int image_size,
1651                                   struct image_tool_params *params)
1652 {
1653         uint8_t checksum;
1654         size_t header_size = kwbimage_header_size(ptr);
1655
1656         if (header_size > image_size)
1657                 return -FDT_ERR_BADSTRUCTURE;
1658
1659         if (!main_hdr_checksum_ok(ptr))
1660                 return -FDT_ERR_BADSTRUCTURE;
1661
1662         /* Only version 0 extended header has checksum */
1663         if (image_version((void *)ptr) == 0) {
1664                 struct main_hdr_v0 *mhdr = (struct main_hdr_v0 *)ptr;
1665
1666                 if (mhdr->ext & 0x1) {
1667                         struct ext_hdr_v0 *ext_hdr;
1668
1669                         ext_hdr = (struct ext_hdr_v0 *)
1670                                 (ptr + sizeof(struct main_hdr_v0));
1671                         checksum = image_checksum8(ext_hdr,
1672                                                    sizeof(struct ext_hdr_v0)
1673                                                    - sizeof(uint8_t));
1674                         if (checksum != ext_hdr->checksum)
1675                                 return -FDT_ERR_BADSTRUCTURE;
1676                 }
1677         }
1678
1679         if (image_version((void *)ptr) == 1) {
1680                 struct main_hdr_v1 *mhdr = (struct main_hdr_v1 *)ptr;
1681                 uint32_t offset;
1682                 uint32_t size;
1683
1684                 if (mhdr->ext & 0x1) {
1685                         uint32_t ohdr_size;
1686                         struct opt_hdr_v1 *ohdr = (struct opt_hdr_v1 *)
1687                                                   (ptr + sizeof(*mhdr));
1688
1689                         while (1) {
1690                                 if ((uint8_t *)ohdr + sizeof(*ohdr) >
1691                                     (uint8_t *)mhdr + header_size)
1692                                         return -FDT_ERR_BADSTRUCTURE;
1693
1694                                 ohdr_size = (ohdr->headersz_msb << 16) |
1695                                             le16_to_cpu(ohdr->headersz_lsb);
1696
1697                                 if (ohdr_size < 8 ||
1698                                     (uint8_t *)ohdr + ohdr_size >
1699                                     (uint8_t *)mhdr + header_size)
1700                                         return -FDT_ERR_BADSTRUCTURE;
1701
1702                                 if (!(*((uint8_t *)ohdr + ohdr_size - 4) & 0x1))
1703                                         break;
1704                                 ohdr = (struct opt_hdr_v1 *)((uint8_t *)ohdr +
1705                                                              ohdr_size);
1706                         }
1707                 }
1708
1709                 offset = le32_to_cpu(mhdr->srcaddr);
1710
1711                 /*
1712                  * For SATA srcaddr is specified in number of sectors.
1713                  * The main header is must be stored at sector number 1.
1714                  * This expects that sector size is 512 bytes and recalculates
1715                  * data offset to bytes relative to the main header.
1716                  */
1717                 if (mhdr->blockid == IBR_HDR_SATA_ID) {
1718                         if (offset < 1)
1719                                 return -FDT_ERR_BADSTRUCTURE;
1720                         offset -= 1;
1721                         offset *= 512;
1722                 }
1723
1724                 /*
1725                  * For SDIO srcaddr is specified in number of sectors.
1726                  * This expects that sector size is 512 bytes and recalculates
1727                  * data offset to bytes.
1728                  */
1729                 if (mhdr->blockid == IBR_HDR_SDIO_ID)
1730                         offset *= 512;
1731
1732                 /*
1733                  * For PCIe srcaddr is always set to 0xFFFFFFFF.
1734                  * This expects that data starts after all headers.
1735                  */
1736                 if (mhdr->blockid == IBR_HDR_PEX_ID && offset == 0xFFFFFFFF)
1737                         offset = header_size;
1738
1739                 if (offset > image_size || offset % 4 != 0)
1740                         return -FDT_ERR_BADSTRUCTURE;
1741
1742                 size = le32_to_cpu(mhdr->blocksize);
1743                 if (offset + size > image_size || size % 4 != 0)
1744                         return -FDT_ERR_BADSTRUCTURE;
1745
1746                 if (image_checksum32(ptr + offset, size - 4) !=
1747                     *(uint32_t *)(ptr + offset + size - 4))
1748                         return -FDT_ERR_BADSTRUCTURE;
1749         }
1750
1751         return 0;
1752 }
1753
1754 static int kwbimage_generate(struct image_tool_params *params,
1755                              struct image_type_params *tparams)
1756 {
1757         FILE *fcfg;
1758         struct stat s;
1759         int alloc_len;
1760         int bootfrom;
1761         int version;
1762         void *hdr;
1763         int ret;
1764
1765         fcfg = fopen(params->imagename, "r");
1766         if (!fcfg) {
1767                 fprintf(stderr, "Could not open input file %s\n",
1768                         params->imagename);
1769                 exit(EXIT_FAILURE);
1770         }
1771
1772         if (stat(params->datafile, &s)) {
1773                 fprintf(stderr, "Could not stat data file %s: %s\n",
1774                         params->datafile, strerror(errno));
1775                 exit(EXIT_FAILURE);
1776         }
1777
1778         image_cfg = malloc(IMAGE_CFG_ELEMENT_MAX *
1779                            sizeof(struct image_cfg_element));
1780         if (!image_cfg) {
1781                 fprintf(stderr, "Cannot allocate memory\n");
1782                 fclose(fcfg);
1783                 exit(EXIT_FAILURE);
1784         }
1785
1786         memset(image_cfg, 0,
1787                IMAGE_CFG_ELEMENT_MAX * sizeof(struct image_cfg_element));
1788         rewind(fcfg);
1789
1790         ret = image_create_config_parse(fcfg);
1791         fclose(fcfg);
1792         if (ret) {
1793                 free(image_cfg);
1794                 exit(EXIT_FAILURE);
1795         }
1796
1797         bootfrom = image_get_bootfrom();
1798         version = image_get_version();
1799         switch (version) {
1800                 /*
1801                  * Fallback to version 0 if no version is provided in the
1802                  * cfg file
1803                  */
1804         case -1:
1805         case 0:
1806                 alloc_len = sizeof(struct main_hdr_v0) +
1807                         sizeof(struct ext_hdr_v0);
1808                 break;
1809
1810         case 1:
1811                 alloc_len = image_headersz_v1(NULL);
1812                 break;
1813
1814         default:
1815                 fprintf(stderr, "Unsupported version %d\n", version);
1816                 free(image_cfg);
1817                 exit(EXIT_FAILURE);
1818         }
1819
1820         free(image_cfg);
1821
1822         hdr = malloc(alloc_len);
1823         if (!hdr) {
1824                 fprintf(stderr, "%s: malloc return failure: %s\n",
1825                         params->cmdname, strerror(errno));
1826                 exit(EXIT_FAILURE);
1827         }
1828
1829         memset(hdr, 0, alloc_len);
1830         tparams->header_size = alloc_len;
1831         tparams->hdr = hdr;
1832
1833         /*
1834          * The resulting image needs to be 4-byte aligned. At least
1835          * the Marvell hdrparser tool complains if its unaligned.
1836          * After the image data is stored 4-byte checksum.
1837          * Final SPI and NAND images must be aligned to 256 bytes.
1838          * Final SATA and SDIO images must be aligned to 512 bytes.
1839          */
1840         if (bootfrom == IBR_HDR_SPI_ID || bootfrom == IBR_HDR_NAND_ID)
1841                 return 4 + (256 - (alloc_len + s.st_size + 4) % 256) % 256;
1842         else if (bootfrom == IBR_HDR_SATA_ID || bootfrom == IBR_HDR_SDIO_ID)
1843                 return 4 + (512 - (alloc_len + s.st_size + 4) % 512) % 512;
1844         else
1845                 return 4 + (4 - s.st_size % 4) % 4;
1846 }
1847
1848 /*
1849  * Report Error if xflag is set in addition to default
1850  */
1851 static int kwbimage_check_params(struct image_tool_params *params)
1852 {
1853         if (!strlen(params->imagename)) {
1854                 char *msg = "Configuration file for kwbimage creation omitted";
1855
1856                 fprintf(stderr, "Error:%s - %s\n", params->cmdname, msg);
1857                 return CFG_INVALID;
1858         }
1859
1860         return (params->dflag && (params->fflag || params->lflag)) ||
1861                 (params->fflag && (params->dflag || params->lflag)) ||
1862                 (params->lflag && (params->dflag || params->fflag)) ||
1863                 (params->xflag) || !(strlen(params->imagename));
1864 }
1865
1866 /*
1867  * kwbimage type parameters definition
1868  */
1869 U_BOOT_IMAGE_TYPE(
1870         kwbimage,
1871         "Marvell MVEBU Boot Image support",
1872         0,
1873         NULL,
1874         kwbimage_check_params,
1875         kwbimage_verify_header,
1876         kwbimage_print_header,
1877         kwbimage_set_header,
1878         NULL,
1879         kwbimage_check_image_types,
1880         NULL,
1881         kwbimage_generate
1882 );