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