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