tools/kwbimage.c

   1 /*
   2  * Image manipulator for Marvell SoCs
   3  *  supports Kirkwood, Dove, Armada 370, and Armada XP
   4  *
   5  * (C) Copyright 2013 Thomas Petazzoni
   6  * <thomas.petazzoni@free-electrons.com>
   7  *
   8  * SPDX-License-Identifier:     GPL-2.0+
   9  *
  10  * Not implemented: support for the register headers and secure
  11  * headers in v1 images
  12  */
  13
  14 #include "imagetool.h"
  15 #include <limits.h>
  16 #include <image.h>
  17 #include <stdint.h>
  18 #include "kwbimage.h"
  19
  20 #define ALIGN_SUP(x, a) (((x) + (a - 1)) & ~(a - 1))
  21
  22 /* Structure of the main header, version 0 (Kirkwood, Dove) */
  23 struct main_hdr_v0 {
  24         uint8_t  blockid;               /*0     */
  25         uint8_t  nandeccmode;           /*1     */
  26         uint16_t nandpagesize;          /*2-3   */
  27         uint32_t blocksize;             /*4-7   */
  28         uint32_t rsvd1;                 /*8-11  */
  29         uint32_t srcaddr;               /*12-15 */
  30         uint32_t destaddr;              /*16-19 */
  31         uint32_t execaddr;              /*20-23 */
  32         uint8_t  satapiomode;           /*24    */
  33         uint8_t  rsvd3;                 /*25    */
  34         uint16_t ddrinitdelay;          /*26-27 */
  35         uint16_t rsvd2;                 /*28-29 */
  36         uint8_t  ext;                   /*30    */
  37         uint8_t  checksum;              /*31    */
  38 };
  39
  40 struct ext_hdr_v0_reg {
  41         uint32_t raddr;
  42         uint32_t rdata;
  43 };
  44
  45 #define EXT_HDR_V0_REG_COUNT ((0x1dc - 0x20) / sizeof(struct ext_hdr_v0_reg))
  46
  47 struct ext_hdr_v0 {
  48         uint32_t              offset;
  49         uint8_t               reserved[0x20 - sizeof(uint32_t)];
  50         struct ext_hdr_v0_reg rcfg[EXT_HDR_V0_REG_COUNT];
  51         uint8_t               reserved2[7];
  52         uint8_t               checksum;
  53 };
  54
  55 /* Structure of the main header, version 1 (Armada 370, Armada XP) */
  56 struct main_hdr_v1 {
  57         uint8_t  blockid;               /* 0 */
  58         uint8_t  reserved1;             /* 1 */
  59         uint16_t reserved2;             /* 2-3 */
  60         uint32_t blocksize;             /* 4-7 */
  61         uint8_t  version;               /* 8 */
  62         uint8_t  headersz_msb;          /* 9 */
  63         uint16_t headersz_lsb;          /* A-B */
  64         uint32_t srcaddr;               /* C-F */
  65         uint32_t destaddr;              /* 10-13 */
  66         uint32_t execaddr;              /* 14-17 */
  67         uint8_t  reserved3;             /* 18 */
  68         uint8_t  nandblocksize;         /* 19 */
  69         uint8_t  nandbadblklocation;    /* 1A */
  70         uint8_t  reserved4;             /* 1B */
  71         uint16_t reserved5;             /* 1C-1D */
  72         uint8_t  ext;                   /* 1E */
  73         uint8_t  checksum;              /* 1F */
  74 };
  75
  76 /*
  77  * Header for the optional headers, version 1 (Armada 370, Armada XP)
  78  */
  79 struct opt_hdr_v1 {
  80         uint8_t  headertype;
  81         uint8_t  headersz_msb;
  82         uint16_t headersz_lsb;
  83         char     data[0];
  84 };
  85
  86 /*
  87  * Various values for the opt_hdr_v1->headertype field, describing the
  88  * different types of optional headers. The "secure" header contains
  89  * informations related to secure boot (encryption keys, etc.). The
  90  * "binary" header contains ARM binary code to be executed prior to
  91  * executing the main payload (usually the bootloader). This is
  92  * typically used to execute DDR3 training code. The "register" header
  93  * allows to describe a set of (address, value) tuples that are
  94  * generally used to configure the DRAM controller.
  95  */
  96 #define OPT_HDR_V1_SECURE_TYPE   0x1
  97 #define OPT_HDR_V1_BINARY_TYPE   0x2
  98 #define OPT_HDR_V1_REGISTER_TYPE 0x3
  99
 100 #define KWBHEADER_V1_SIZE(hdr) \
 101         (((hdr)->headersz_msb << 16) | (hdr)->headersz_lsb)
 102
 103 static struct image_cfg_element *image_cfg;
 104 static int cfgn;
 105
 106 struct boot_mode {
 107         unsigned int id;
 108         const char *name;
 109 };
 110
 111 struct boot_mode boot_modes[] = {
 112         { 0x4D, "i2c"  },
 113         { 0x5A, "spi"  },
 114         { 0x8B, "nand" },
 115         { 0x78, "sata" },
 116         { 0x9C, "pex"  },
 117         { 0x69, "uart" },
 118         {},
 119 };
 120
 121 struct nand_ecc_mode {
 122         unsigned int id;
 123         const char *name;
 124 };
 125
 126 struct nand_ecc_mode nand_ecc_modes[] = {
 127         { 0x00, "default" },
 128         { 0x01, "hamming" },
 129         { 0x02, "rs" },
 130         { 0x03, "disabled" },
 131         {},
 132 };
 133
 134 /* Used to identify an undefined execution or destination address */
 135 #define ADDR_INVALID ((uint32_t)-1)
 136
 137 #define BINARY_MAX_ARGS 8
 138
 139 /* In-memory representation of a line of the configuration file */
 140 struct image_cfg_element {
 141         enum {
 142                 IMAGE_CFG_VERSION = 0x1,
 143                 IMAGE_CFG_BOOT_FROM,
 144                 IMAGE_CFG_DEST_ADDR,
 145                 IMAGE_CFG_EXEC_ADDR,
 146                 IMAGE_CFG_NAND_BLKSZ,
 147                 IMAGE_CFG_NAND_BADBLK_LOCATION,
 148                 IMAGE_CFG_NAND_ECC_MODE,
 149                 IMAGE_CFG_NAND_PAGESZ,
 150                 IMAGE_CFG_BINARY,
 151                 IMAGE_CFG_PAYLOAD,
 152                 IMAGE_CFG_DATA,
 153         } type;
 154         union {
 155                 unsigned int version;
 156                 unsigned int bootfrom;
 157                 struct {
 158                         const char *file;
 159                         unsigned int args[BINARY_MAX_ARGS];
 160                         unsigned int nargs;
 161                 } binary;
 162                 const char *payload;
 163                 unsigned int dstaddr;
 164                 unsigned int execaddr;
 165                 unsigned int nandblksz;
 166                 unsigned int nandbadblklocation;
 167                 unsigned int nandeccmode;
 168                 unsigned int nandpagesz;
 169                 struct ext_hdr_v0_reg regdata;
 170         };
 171 };
 172
 173 #define IMAGE_CFG_ELEMENT_MAX 256
 174
 175 /*
 176  * Byte 8 of the image header contains the version number. In the v0
 177  * header, byte 8 was reserved, and always set to 0. In the v1 header,
 178  * byte 8 has been changed to a proper field, set to 1.
 179  */
 180 static unsigned int image_version(void *header)
 181 {
 182         unsigned char *ptr = header;
 183         return ptr[8];
 184 }
 185
 186 /*
 187  * Utility functions to manipulate boot mode and ecc modes (convert
 188  * them back and forth between description strings and the
 189  * corresponding numerical identifiers).
 190  */
 191
 192 static const char *image_boot_mode_name(unsigned int id)
 193 {
 194         int i;
 195         for (i = 0; boot_modes[i].name; i++)
 196                 if (boot_modes[i].id == id)
 197                         return boot_modes[i].name;
 198         return NULL;
 199 }
 200
 201 int image_boot_mode_id(const char *boot_mode_name)
 202 {
 203         int i;
 204         for (i = 0; boot_modes[i].name; i++)
 205                 if (!strcmp(boot_modes[i].name, boot_mode_name))
 206                         return boot_modes[i].id;
 207
 208         return -1;
 209 }
 210
 211 int image_nand_ecc_mode_id(const char *nand_ecc_mode_name)
 212 {
 213         int i;
 214         for (i = 0; nand_ecc_modes[i].name; i++)
 215                 if (!strcmp(nand_ecc_modes[i].name, nand_ecc_mode_name))
 216                         return nand_ecc_modes[i].id;
 217         return -1;
 218 }
 219
 220 static struct image_cfg_element *
 221 image_find_option(unsigned int optiontype)
 222 {
 223         int i;
 224
 225         for (i = 0; i < cfgn; i++) {
 226                 if (image_cfg[i].type == optiontype)
 227                         return &image_cfg[i];
 228         }
 229
 230         return NULL;
 231 }
 232
 233 static unsigned int
 234 image_count_options(unsigned int optiontype)
 235 {
 236         int i;
 237         unsigned int count = 0;
 238
 239         for (i = 0; i < cfgn; i++)
 240                 if (image_cfg[i].type == optiontype)
 241                         count++;
 242
 243         return count;
 244 }
 245
 246 /*
 247  * Compute a 8-bit checksum of a memory area. This algorithm follows
 248  * the requirements of the Marvell SoC BootROM specifications.
 249  */
 250 static uint8_t image_checksum8(void *start, uint32_t len)
 251 {
 252         uint8_t csum = 0;
 253         uint8_t *p = start;
 254
 255         /* check len and return zero checksum if invalid */
 256         if (!len)
 257                 return 0;
 258
 259         do {
 260                 csum += *p;
 261                 p++;
 262         } while (--len);
 263
 264         return csum;
 265 }
 266
 267 static uint32_t image_checksum32(void *start, uint32_t len)
 268 {
 269         uint32_t csum = 0;
 270         uint32_t *p = start;
 271
 272         /* check len and return zero checksum if invalid */
 273         if (!len)
 274                 return 0;
 275
 276         if (len % sizeof(uint32_t)) {
 277                 fprintf(stderr, "Length %d is not in multiple of %zu\n",
 278                         len, sizeof(uint32_t));
 279                 return 0;
 280         }
 281
 282         do {
 283                 csum += *p;
 284                 p++;
 285                 len -= sizeof(uint32_t);
 286         } while (len > 0);
 287
 288         return csum;
 289 }
 290
 291 static void *image_create_v0(size_t *imagesz, struct image_tool_params *params,
 292                              int payloadsz)
 293 {
 294         struct image_cfg_element *e;
 295         size_t headersz;
 296         struct main_hdr_v0 *main_hdr;
 297         struct ext_hdr_v0 *ext_hdr;
 298         void *image;
 299         int has_ext = 0;
 300
 301         /*
 302          * Calculate the size of the header and the size of the
 303          * payload
 304          */
 305         headersz  = sizeof(struct main_hdr_v0);
 306
 307         if (image_count_options(IMAGE_CFG_DATA) > 0) {
 308                 has_ext = 1;
 309                 headersz += sizeof(struct ext_hdr_v0);
 310         }
 311
 312         if (image_count_options(IMAGE_CFG_PAYLOAD) > 1) {
 313                 fprintf(stderr, "More than one payload, not possible\n");
 314                 return NULL;
 315         }
 316
 317         image = malloc(headersz);
 318         if (!image) {
 319                 fprintf(stderr, "Cannot allocate memory for image\n");
 320                 return NULL;
 321         }
 322
 323         memset(image, 0, headersz);
 324
 325         main_hdr = image;
 326
 327         /* Fill in the main header */
 328         main_hdr->blocksize = payloadsz + sizeof(uint32_t) - headersz;
 329         main_hdr->srcaddr   = headersz;
 330         main_hdr->ext       = has_ext;
 331         main_hdr->destaddr  = params->addr;
 332         main_hdr->execaddr  = params->ep;
 333
 334         e = image_find_option(IMAGE_CFG_BOOT_FROM);
 335         if (e)
 336                 main_hdr->blockid = e->bootfrom;
 337         e = image_find_option(IMAGE_CFG_NAND_ECC_MODE);
 338         if (e)
 339                 main_hdr->nandeccmode = e->nandeccmode;
 340         e = image_find_option(IMAGE_CFG_NAND_PAGESZ);
 341         if (e)
 342                 main_hdr->nandpagesize = e->nandpagesz;
 343         main_hdr->checksum = image_checksum8(image,
 344                                              sizeof(struct main_hdr_v0));
 345
 346         /* Generate the ext header */
 347         if (has_ext) {
 348                 int cfgi, datai;
 349
 350                 ext_hdr = image + sizeof(struct main_hdr_v0);
 351                 ext_hdr->offset = 0x40;
 352
 353                 for (cfgi = 0, datai = 0; cfgi < cfgn; cfgi++) {
 354                         e = &image_cfg[cfgi];
 355                         if (e->type != IMAGE_CFG_DATA)
 356                                 continue;
 357
 358                         ext_hdr->rcfg[datai].raddr = e->regdata.raddr;
 359                         ext_hdr->rcfg[datai].rdata = e->regdata.rdata;
 360                         datai++;
 361                 }
 362
 363                 ext_hdr->checksum = image_checksum8(ext_hdr,
 364                                                     sizeof(struct ext_hdr_v0));
 365         }
 366
 367         *imagesz = headersz;
 368         return image;
 369 }
 370
 371 static size_t image_headersz_v1(struct image_tool_params *params,
 372                                 int *hasext)
 373 {
 374         struct image_cfg_element *binarye;
 375         size_t headersz;
 376         int ret;
 377
 378         /*
 379          * Calculate the size of the header and the size of the
 380          * payload
 381          */
 382         headersz = sizeof(struct main_hdr_v1);
 383
 384         if (image_count_options(IMAGE_CFG_BINARY) > 1) {
 385                 fprintf(stderr, "More than one binary blob, not supported\n");
 386                 return 0;
 387         }
 388
 389         if (image_count_options(IMAGE_CFG_PAYLOAD) > 1) {
 390                 fprintf(stderr, "More than one payload, not possible\n");
 391                 return 0;
 392         }
 393
 394         binarye = image_find_option(IMAGE_CFG_BINARY);
 395         if (binarye) {
 396                 struct stat s;
 397
 398                 ret = stat(binarye->binary.file, &s);
 399                 if (ret < 0) {
 400                         char cwd[PATH_MAX];
 401                         char *dir = cwd;
 402
 403                         memset(cwd, 0, sizeof(cwd));
 404                         if (!getcwd(cwd, sizeof(cwd))) {
 405                                 dir = "current working directory";
 406                                 perror("getcwd() failed");
 407                         }
 408
 409                         fprintf(stderr,
 410                                 "Didn't find the file '%s' in '%s' which is mandatory to generate the image\n"
 411                                 "This file generally contains the DDR3 training code, and should be extracted from an existing bootable\n"
 412                                 "image for your board. See 'kwbimage -x' to extract it from an existing image.\n",
 413                                 binarye->binary.file, dir);
 414                         return 0;
 415                 }
 416
 417                 headersz += s.st_size +
 418                         binarye->binary.nargs * sizeof(unsigned int);
 419                 if (hasext)
 420                         *hasext = 1;
 421         }
 422
 423         /*
 424          * The payload should be aligned on some reasonable
 425          * boundary
 426          */
 427         return ALIGN_SUP(headersz, 4096);
 428 }
 429
 430 static void *image_create_v1(size_t *imagesz, struct image_tool_params *params,
 431                              int payloadsz)
 432 {
 433         struct image_cfg_element *e, *binarye;
 434         struct main_hdr_v1 *main_hdr;
 435         size_t headersz;
 436         void *image, *cur;
 437         int hasext = 0;
 438         int ret;
 439
 440         /*
 441          * Calculate the size of the header and the size of the
 442          * payload
 443          */
 444         headersz = image_headersz_v1(params, &hasext);
 445         if (headersz == 0)
 446                 return NULL;
 447
 448         image = malloc(headersz);
 449         if (!image) {
 450                 fprintf(stderr, "Cannot allocate memory for image\n");
 451                 return NULL;
 452         }
 453
 454         memset(image, 0, headersz);
 455
 456         cur = main_hdr = image;
 457         cur += sizeof(struct main_hdr_v1);
 458
 459         /* Fill the main header */
 460         main_hdr->blocksize    = payloadsz - headersz + sizeof(uint32_t);
 461         main_hdr->headersz_lsb = headersz & 0xFFFF;
 462         main_hdr->headersz_msb = (headersz & 0xFFFF0000) >> 16;
 463         main_hdr->destaddr     = params->addr;
 464         main_hdr->execaddr     = params->ep;
 465         main_hdr->srcaddr      = headersz;
 466         main_hdr->ext          = hasext;
 467         main_hdr->version      = 1;
 468         e = image_find_option(IMAGE_CFG_BOOT_FROM);
 469         if (e)
 470                 main_hdr->blockid = e->bootfrom;
 471         e = image_find_option(IMAGE_CFG_NAND_BLKSZ);
 472         if (e)
 473                 main_hdr->nandblocksize = e->nandblksz / (64 * 1024);
 474         e = image_find_option(IMAGE_CFG_NAND_BADBLK_LOCATION);
 475         if (e)
 476                 main_hdr->nandbadblklocation = e->nandbadblklocation;
 477
 478         binarye = image_find_option(IMAGE_CFG_BINARY);
 479         if (binarye) {
 480                 struct opt_hdr_v1 *hdr = cur;
 481                 unsigned int *args;
 482                 size_t binhdrsz;
 483                 struct stat s;
 484                 int argi;
 485                 FILE *bin;
 486
 487                 hdr->headertype = OPT_HDR_V1_BINARY_TYPE;
 488
 489                 bin = fopen(binarye->binary.file, "r");
 490                 if (!bin) {
 491                         fprintf(stderr, "Cannot open binary file %s\n",
 492                                 binarye->binary.file);
 493                         return NULL;
 494                 }
 495
 496                 fstat(fileno(bin), &s);
 497
 498                 binhdrsz = sizeof(struct opt_hdr_v1) +
 499                         (binarye->binary.nargs + 1) * sizeof(unsigned int) +
 500                         s.st_size;
 501                 binhdrsz = ALIGN_SUP(binhdrsz, 32);
 502                 hdr->headersz_lsb = binhdrsz & 0xFFFF;
 503                 hdr->headersz_msb = (binhdrsz & 0xFFFF0000) >> 16;
 504
 505                 cur += sizeof(struct opt_hdr_v1);
 506
 507                 args = cur;
 508                 *args = binarye->binary.nargs;
 509                 args++;
 510                 for (argi = 0; argi < binarye->binary.nargs; argi++)
 511                         args[argi] = binarye->binary.args[argi];
 512
 513                 cur += (binarye->binary.nargs + 1) * sizeof(unsigned int);
 514
 515                 ret = fread(cur, s.st_size, 1, bin);
 516                 if (ret != 1) {
 517                         fprintf(stderr,
 518                                 "Could not read binary image %s\n",
 519                                 binarye->binary.file);
 520                         return NULL;
 521                 }
 522
 523                 fclose(bin);
 524
 525                 cur += s.st_size;
 526
 527                 /*
 528                  * For now, we don't support more than one binary
 529                  * header, and no other header types are
 530                  * supported. So, the binary header is necessarily the
 531                  * last one
 532                  */
 533                 *((unsigned char *)cur) = 0;
 534
 535                 cur += sizeof(uint32_t);
 536         }
 537
 538         /* Calculate and set the header checksum */
 539         main_hdr->checksum = image_checksum8(main_hdr, headersz);
 540
 541         *imagesz = headersz;
 542         return image;
 543 }
 544
 545 static int image_create_config_parse_oneline(char *line,
 546                                              struct image_cfg_element *el)
 547 {
 548         char *keyword, *saveptr;
 549         char deliminiters[] = " \t";
 550
 551         keyword = strtok_r(line, deliminiters, &saveptr);
 552         if (!strcmp(keyword, "VERSION")) {
 553                 char *value = strtok_r(NULL, deliminiters, &saveptr);
 554                 el->type = IMAGE_CFG_VERSION;
 555                 el->version = atoi(value);
 556         } else if (!strcmp(keyword, "BOOT_FROM")) {
 557                 char *value = strtok_r(NULL, deliminiters, &saveptr);
 558                 int ret = image_boot_mode_id(value);
 559                 if (ret < 0) {
 560                         fprintf(stderr,
 561                                 "Invalid boot media '%s'\n", value);
 562                         return -1;
 563                 }
 564                 el->type = IMAGE_CFG_BOOT_FROM;
 565                 el->bootfrom = ret;
 566         } else if (!strcmp(keyword, "NAND_BLKSZ")) {
 567                 char *value = strtok_r(NULL, deliminiters, &saveptr);
 568                 el->type = IMAGE_CFG_NAND_BLKSZ;
 569                 el->nandblksz = strtoul(value, NULL, 16);
 570         } else if (!strcmp(keyword, "NAND_BADBLK_LOCATION")) {
 571                 char *value = strtok_r(NULL, deliminiters, &saveptr);
 572                 el->type = IMAGE_CFG_NAND_BADBLK_LOCATION;
 573                 el->nandbadblklocation =
 574                         strtoul(value, NULL, 16);
 575         } else if (!strcmp(keyword, "NAND_ECC_MODE")) {
 576                 char *value = strtok_r(NULL, deliminiters, &saveptr);
 577                 int ret = image_nand_ecc_mode_id(value);
 578                 if (ret < 0) {
 579                         fprintf(stderr,
 580                                 "Invalid NAND ECC mode '%s'\n", value);
 581                         return -1;
 582                 }
 583                 el->type = IMAGE_CFG_NAND_ECC_MODE;
 584                 el->nandeccmode = ret;
 585         } else if (!strcmp(keyword, "NAND_PAGE_SIZE")) {
 586                 char *value = strtok_r(NULL, deliminiters, &saveptr);
 587                 el->type = IMAGE_CFG_NAND_PAGESZ;
 588                 el->nandpagesz = strtoul(value, NULL, 16);
 589         } else if (!strcmp(keyword, "BINARY")) {
 590                 char *value = strtok_r(NULL, deliminiters, &saveptr);
 591                 int argi = 0;
 592
 593                 el->type = IMAGE_CFG_BINARY;
 594                 el->binary.file = strdup(value);
 595                 while (1) {
 596                         value = strtok_r(NULL, deliminiters, &saveptr);
 597                         if (!value)
 598                                 break;
 599                         el->binary.args[argi] = strtoul(value, NULL, 16);
 600                         argi++;
 601                         if (argi >= BINARY_MAX_ARGS) {
 602                                 fprintf(stderr,
 603                                         "Too many argument for binary\n");
 604                                 return -1;
 605                         }
 606                 }
 607                 el->binary.nargs = argi;
 608         } else if (!strcmp(keyword, "DATA")) {
 609                 char *value1 = strtok_r(NULL, deliminiters, &saveptr);
 610                 char *value2 = strtok_r(NULL, deliminiters, &saveptr);
 611
 612                 if (!value1 || !value2) {
 613                         fprintf(stderr,
 614                                 "Invalid number of arguments for DATA\n");
 615                         return -1;
 616                 }
 617
 618                 el->type = IMAGE_CFG_DATA;
 619                 el->regdata.raddr = strtoul(value1, NULL, 16);
 620                 el->regdata.rdata = strtoul(value2, NULL, 16);
 621         } else {
 622                 fprintf(stderr, "Ignoring unknown line '%s'\n", line);
 623         }
 624
 625         return 0;
 626 }
 627
 628 /*
 629  * Parse the configuration file 'fcfg' into the array of configuration
 630  * elements 'image_cfg', and return the number of configuration
 631  * elements in 'cfgn'.
 632  */
 633 static int image_create_config_parse(FILE *fcfg)
 634 {
 635         int ret;
 636         int cfgi = 0;
 637
 638         /* Parse the configuration file */
 639         while (!feof(fcfg)) {
 640                 char *line;
 641                 char buf[256];
 642
 643                 /* Read the current line */
 644                 memset(buf, 0, sizeof(buf));
 645                 line = fgets(buf, sizeof(buf), fcfg);
 646                 if (!line)
 647                         break;
 648
 649                 /* Ignore useless lines */
 650                 if (line[0] == '\n' || line[0] == '#')
 651                         continue;
 652
 653                 /* Strip final newline */
 654                 if (line[strlen(line) - 1] == '\n')
 655                         line[strlen(line) - 1] = 0;
 656
 657                 /* Parse the current line */
 658                 ret = image_create_config_parse_oneline(line,
 659                                                         &image_cfg[cfgi]);
 660                 if (ret)
 661                         return ret;
 662
 663                 cfgi++;
 664
 665                 if (cfgi >= IMAGE_CFG_ELEMENT_MAX) {
 666                         fprintf(stderr,
 667                                 "Too many configuration elements in .cfg file\n");
 668                         return -1;
 669                 }
 670         }
 671
 672         cfgn = cfgi;
 673         return 0;
 674 }
 675
 676 static int image_get_version(void)
 677 {
 678         struct image_cfg_element *e;
 679
 680         e = image_find_option(IMAGE_CFG_VERSION);
 681         if (!e)
 682                 return -1;
 683
 684         return e->version;
 685 }
 686
 687 static int image_version_file(const char *input)
 688 {
 689         FILE *fcfg;
 690         int version;
 691         int ret;
 692
 693         fcfg = fopen(input, "r");
 694         if (!fcfg) {
 695                 fprintf(stderr, "Could not open input file %s\n", input);
 696                 return -1;
 697         }
 698
 699         image_cfg = malloc(IMAGE_CFG_ELEMENT_MAX *
 700                            sizeof(struct image_cfg_element));
 701         if (!image_cfg) {
 702                 fprintf(stderr, "Cannot allocate memory\n");
 703                 fclose(fcfg);
 704                 return -1;
 705         }
 706
 707         memset(image_cfg, 0,
 708                IMAGE_CFG_ELEMENT_MAX * sizeof(struct image_cfg_element));
 709         rewind(fcfg);
 710
 711         ret = image_create_config_parse(fcfg);
 712         fclose(fcfg);
 713         if (ret) {
 714                 free(image_cfg);
 715                 return -1;
 716         }
 717
 718         version = image_get_version();
 719         /* Fallback to version 0 is no version is provided in the cfg file */
 720         if (version == -1)
 721                 version = 0;
 722
 723         free(image_cfg);
 724
 725         return version;
 726 }
 727
 728 static void kwbimage_set_header(void *ptr, struct stat *sbuf, int ifd,
 729                                 struct image_tool_params *params)
 730 {
 731         FILE *fcfg;
 732         void *image = NULL;
 733         int version;
 734         size_t headersz = 0;
 735         uint32_t checksum;
 736         int ret;
 737         int size;
 738
 739         fcfg = fopen(params->imagename, "r");
 740         if (!fcfg) {
 741                 fprintf(stderr, "Could not open input file %s\n",
 742                         params->imagename);
 743                 exit(EXIT_FAILURE);
 744         }
 745
 746         image_cfg = malloc(IMAGE_CFG_ELEMENT_MAX *
 747                            sizeof(struct image_cfg_element));
 748         if (!image_cfg) {
 749                 fprintf(stderr, "Cannot allocate memory\n");
 750                 fclose(fcfg);
 751                 exit(EXIT_FAILURE);
 752         }
 753
 754         memset(image_cfg, 0,
 755                IMAGE_CFG_ELEMENT_MAX * sizeof(struct image_cfg_element));
 756         rewind(fcfg);
 757
 758         ret = image_create_config_parse(fcfg);
 759         fclose(fcfg);
 760         if (ret) {
 761                 free(image_cfg);
 762                 exit(EXIT_FAILURE);
 763         }
 764
 765         version = image_get_version();
 766         switch (version) {
 767                 /*
 768                  * Fallback to version 0 if no version is provided in the
 769                  * cfg file
 770                  */
 771         case -1:
 772         case 0:
 773                 image = image_create_v0(&headersz, params, sbuf->st_size);
 774                 break;
 775
 776         case 1:
 777                 image = image_create_v1(&headersz, params, sbuf->st_size);
 778                 break;
 779
 780         default:
 781                 fprintf(stderr, "Unsupported version %d\n", version);
 782                 free(image_cfg);
 783                 exit(EXIT_FAILURE);
 784         }
 785
 786         if (!image) {
 787                 fprintf(stderr, "Could not create image\n");
 788                 free(image_cfg);
 789                 exit(EXIT_FAILURE);
 790         }
 791
 792         free(image_cfg);
 793
 794         /* Build and add image checksum header */
 795         checksum = image_checksum32((uint32_t *)ptr, sbuf->st_size);
 796         size = write(ifd, &checksum, sizeof(uint32_t));
 797         if (size != sizeof(uint32_t)) {
 798                 fprintf(stderr, "Error:%s - Checksum write %d bytes %s\n",
 799                         params->cmdname, size, params->imagefile);
 800                 exit(EXIT_FAILURE);
 801         }
 802
 803         sbuf->st_size += sizeof(uint32_t);
 804
 805         /* Finally copy the header into the image area */
 806         memcpy(ptr, image, headersz);
 807
 808         free(image);
 809 }
 810
 811 static void kwbimage_print_header(const void *ptr)
 812 {
 813         struct main_hdr_v0 *mhdr = (struct main_hdr_v0 *)ptr;
 814
 815         printf("Image Type:   MVEBU Boot from %s Image\n",
 816                image_boot_mode_name(mhdr->blockid));
 817         printf("Image version:%d\n", image_version((void *)ptr));
 818         printf("Data Size:    ");
 819         genimg_print_size(mhdr->blocksize - sizeof(uint32_t));
 820         printf("Load Address: %08x\n", mhdr->destaddr);
 821         printf("Entry Point:  %08x\n", mhdr->execaddr);
 822 }
 823
 824 static int kwbimage_check_image_types(uint8_t type)
 825 {
 826         if (type == IH_TYPE_KWBIMAGE)
 827                 return EXIT_SUCCESS;
 828         else
 829                 return EXIT_FAILURE;
 830 }
 831
 832 static int kwbimage_verify_header(unsigned char *ptr, int image_size,
 833                                   struct image_tool_params *params)
 834 {
 835         struct main_hdr_v0 *main_hdr;
 836         struct ext_hdr_v0 *ext_hdr;
 837         uint8_t checksum;
 838
 839         main_hdr = (void *)ptr;
 840         checksum = image_checksum8(ptr,
 841                                    sizeof(struct main_hdr_v0)
 842                                    - sizeof(uint8_t));
 843         if (checksum != main_hdr->checksum)
 844                 return -FDT_ERR_BADSTRUCTURE;
 845
 846         /* Only version 0 extended header has checksum */
 847         if (image_version((void *)ptr) == 0) {
 848                 ext_hdr = (void *)ptr + sizeof(struct main_hdr_v0);
 849                 checksum = image_checksum8(ext_hdr,
 850                                            sizeof(struct ext_hdr_v0)
 851                                            - sizeof(uint8_t));
 852                 if (checksum != ext_hdr->checksum)
 853                         return -FDT_ERR_BADSTRUCTURE;
 854         }
 855
 856         return 0;
 857 }
 858
 859 static int kwbimage_generate(struct image_tool_params *params,
 860                              struct image_type_params *tparams)
 861 {
 862         int alloc_len;
 863         void *hdr;
 864         int version = 0;
 865
 866         version = image_version_file(params->imagename);
 867         if (version == 0) {
 868                 alloc_len = sizeof(struct main_hdr_v0) +
 869                         sizeof(struct ext_hdr_v0);
 870         } else {
 871                 alloc_len = image_headersz_v1(params, NULL);
 872 #if defined(CONFIG_SYS_SPI_U_BOOT_OFFS)
 873                 if (alloc_len > CONFIG_SYS_SPI_U_BOOT_OFFS) {
 874                         fprintf(stderr, "Error: Image header (incl. SPL image) too big!\n");
 875                         fprintf(stderr, "header=0x%x CONFIG_SYS_SPI_U_BOOT_OFFS=0x%x!\n",
 876                                 alloc_len, CONFIG_SYS_SPI_U_BOOT_OFFS);
 877                         fprintf(stderr, "Increase CONFIG_SYS_SPI_U_BOOT_OFFS!\n");
 878                 } else {
 879                         alloc_len = CONFIG_SYS_SPI_U_BOOT_OFFS;
 880                 }
 881 #endif
 882         }
 883
 884         hdr = malloc(alloc_len);
 885         if (!hdr) {
 886                 fprintf(stderr, "%s: malloc return failure: %s\n",
 887                         params->cmdname, strerror(errno));
 888                 exit(EXIT_FAILURE);
 889         }
 890
 891         memset(hdr, 0, alloc_len);
 892         tparams->header_size = alloc_len;
 893         tparams->hdr = hdr;
 894
 895         return 0;
 896 }
 897
 898 /*
 899  * Report Error if xflag is set in addition to default
 900  */
 901 static int kwbimage_check_params(struct image_tool_params *params)
 902 {
 903         if (!strlen(params->imagename)) {
 904                 fprintf(stderr, "Error:%s - Configuration file not specified, "
 905                         "it is needed for kwbimage generation\n",
 906                         params->cmdname);
 907                 return CFG_INVALID;
 908         }
 909
 910         return (params->dflag && (params->fflag || params->lflag)) ||
 911                 (params->fflag && (params->dflag || params->lflag)) ||
 912                 (params->lflag && (params->dflag || params->fflag)) ||
 913                 (params->xflag) || !(strlen(params->imagename));
 914 }
 915
 916 /*
 917  * kwbimage type parameters definition
 918  */
 919 U_BOOT_IMAGE_TYPE(
 920         kwbimage,
 921         "Marvell MVEBU Boot Image support",
 922         0,
 923         NULL,
 924         kwbimage_check_params,
 925         kwbimage_verify_header,
 926         kwbimage_print_header,
 927         kwbimage_set_header,
 928         NULL,
 929         kwbimage_check_image_types,
 930         NULL,
 931         kwbimage_generate
 932 );