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