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