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