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 #if defined(CONFIG_SYS_SPI_U_BOOT_OFFS)
 424         if (headersz > CONFIG_SYS_SPI_U_BOOT_OFFS) {
 425                 fprintf(stderr, "Error: Image header (incl. SPL image) too big!\n");
 426                 fprintf(stderr, "header=0x%x CONFIG_SYS_SPI_U_BOOT_OFFS=0x%x!\n",
 427                         (int)headersz, CONFIG_SYS_SPI_U_BOOT_OFFS);
 428                 fprintf(stderr, "Increase CONFIG_SYS_SPI_U_BOOT_OFFS!\n");
 429                 return 0;
 430         } else {
 431                 headersz = CONFIG_SYS_SPI_U_BOOT_OFFS;
 432         }
 433 #endif
 434
 435         /*
 436          * The payload should be aligned on some reasonable
 437          * boundary
 438          */
 439         return ALIGN_SUP(headersz, 4096);
 440 }
 441
 442 static void *image_create_v1(size_t *imagesz, struct image_tool_params *params,
 443                              int payloadsz)
 444 {
 445         struct image_cfg_element *e, *binarye;
 446         struct main_hdr_v1 *main_hdr;
 447         size_t headersz;
 448         void *image, *cur;
 449         int hasext = 0;
 450         int ret;
 451
 452         /*
 453          * Calculate the size of the header and the size of the
 454          * payload
 455          */
 456         headersz = image_headersz_v1(params, &hasext);
 457         if (headersz == 0)
 458                 return NULL;
 459
 460         image = malloc(headersz);
 461         if (!image) {
 462                 fprintf(stderr, "Cannot allocate memory for image\n");
 463                 return NULL;
 464         }
 465
 466         memset(image, 0, headersz);
 467
 468         cur = main_hdr = image;
 469         cur += sizeof(struct main_hdr_v1);
 470
 471         /* Fill the main header */
 472         main_hdr->blocksize    = payloadsz - headersz + sizeof(uint32_t);
 473         main_hdr->headersz_lsb = headersz & 0xFFFF;
 474         main_hdr->headersz_msb = (headersz & 0xFFFF0000) >> 16;
 475         main_hdr->destaddr     = params->addr;
 476         main_hdr->execaddr     = params->ep;
 477         main_hdr->srcaddr      = headersz;
 478         main_hdr->ext          = hasext;
 479         main_hdr->version      = 1;
 480         e = image_find_option(IMAGE_CFG_BOOT_FROM);
 481         if (e)
 482                 main_hdr->blockid = e->bootfrom;
 483         e = image_find_option(IMAGE_CFG_NAND_BLKSZ);
 484         if (e)
 485                 main_hdr->nandblocksize = e->nandblksz / (64 * 1024);
 486         e = image_find_option(IMAGE_CFG_NAND_BADBLK_LOCATION);
 487         if (e)
 488                 main_hdr->nandbadblklocation = e->nandbadblklocation;
 489
 490         binarye = image_find_option(IMAGE_CFG_BINARY);
 491         if (binarye) {
 492                 struct opt_hdr_v1 *hdr = cur;
 493                 unsigned int *args;
 494                 size_t binhdrsz;
 495                 struct stat s;
 496                 int argi;
 497                 FILE *bin;
 498
 499                 hdr->headertype = OPT_HDR_V1_BINARY_TYPE;
 500
 501                 bin = fopen(binarye->binary.file, "r");
 502                 if (!bin) {
 503                         fprintf(stderr, "Cannot open binary file %s\n",
 504                                 binarye->binary.file);
 505                         return NULL;
 506                 }
 507
 508                 fstat(fileno(bin), &s);
 509
 510                 binhdrsz = sizeof(struct opt_hdr_v1) +
 511                         (binarye->binary.nargs + 1) * sizeof(unsigned int) +
 512                         s.st_size;
 513                 binhdrsz = ALIGN_SUP(binhdrsz, 32);
 514                 hdr->headersz_lsb = binhdrsz & 0xFFFF;
 515                 hdr->headersz_msb = (binhdrsz & 0xFFFF0000) >> 16;
 516
 517                 cur += sizeof(struct opt_hdr_v1);
 518
 519                 args = cur;
 520                 *args = binarye->binary.nargs;
 521                 args++;
 522                 for (argi = 0; argi < binarye->binary.nargs; argi++)
 523                         args[argi] = binarye->binary.args[argi];
 524
 525                 cur += (binarye->binary.nargs + 1) * sizeof(unsigned int);
 526
 527                 ret = fread(cur, s.st_size, 1, bin);
 528                 if (ret != 1) {
 529                         fprintf(stderr,
 530                                 "Could not read binary image %s\n",
 531                                 binarye->binary.file);
 532                         return NULL;
 533                 }
 534
 535                 fclose(bin);
 536
 537                 cur += s.st_size;
 538
 539                 /*
 540                  * For now, we don't support more than one binary
 541                  * header, and no other header types are
 542                  * supported. So, the binary header is necessarily the
 543                  * last one
 544                  */
 545                 *((unsigned char *)cur) = 0;
 546
 547                 cur += sizeof(uint32_t);
 548         }
 549
 550         /* Calculate and set the header checksum */
 551         main_hdr->checksum = image_checksum8(main_hdr, headersz);
 552
 553         *imagesz = headersz;
 554         return image;
 555 }
 556
 557 static int image_create_config_parse_oneline(char *line,
 558                                              struct image_cfg_element *el)
 559 {
 560         char *keyword, *saveptr;
 561         char deliminiters[] = " \t";
 562
 563         keyword = strtok_r(line, deliminiters, &saveptr);
 564         if (!strcmp(keyword, "VERSION")) {
 565                 char *value = strtok_r(NULL, deliminiters, &saveptr);
 566                 el->type = IMAGE_CFG_VERSION;
 567                 el->version = atoi(value);
 568         } else if (!strcmp(keyword, "BOOT_FROM")) {
 569                 char *value = strtok_r(NULL, deliminiters, &saveptr);
 570                 int ret = image_boot_mode_id(value);
 571                 if (ret < 0) {
 572                         fprintf(stderr,
 573                                 "Invalid boot media '%s'\n", value);
 574                         return -1;
 575                 }
 576                 el->type = IMAGE_CFG_BOOT_FROM;
 577                 el->bootfrom = ret;
 578         } else if (!strcmp(keyword, "NAND_BLKSZ")) {
 579                 char *value = strtok_r(NULL, deliminiters, &saveptr);
 580                 el->type = IMAGE_CFG_NAND_BLKSZ;
 581                 el->nandblksz = strtoul(value, NULL, 16);
 582         } else if (!strcmp(keyword, "NAND_BADBLK_LOCATION")) {
 583                 char *value = strtok_r(NULL, deliminiters, &saveptr);
 584                 el->type = IMAGE_CFG_NAND_BADBLK_LOCATION;
 585                 el->nandbadblklocation =
 586                         strtoul(value, NULL, 16);
 587         } else if (!strcmp(keyword, "NAND_ECC_MODE")) {
 588                 char *value = strtok_r(NULL, deliminiters, &saveptr);
 589                 int ret = image_nand_ecc_mode_id(value);
 590                 if (ret < 0) {
 591                         fprintf(stderr,
 592                                 "Invalid NAND ECC mode '%s'\n", value);
 593                         return -1;
 594                 }
 595                 el->type = IMAGE_CFG_NAND_ECC_MODE;
 596                 el->nandeccmode = ret;
 597         } else if (!strcmp(keyword, "NAND_PAGE_SIZE")) {
 598                 char *value = strtok_r(NULL, deliminiters, &saveptr);
 599                 el->type = IMAGE_CFG_NAND_PAGESZ;
 600                 el->nandpagesz = strtoul(value, NULL, 16);
 601         } else if (!strcmp(keyword, "BINARY")) {
 602                 char *value = strtok_r(NULL, deliminiters, &saveptr);
 603                 int argi = 0;
 604
 605                 el->type = IMAGE_CFG_BINARY;
 606                 el->binary.file = strdup(value);
 607                 while (1) {
 608                         value = strtok_r(NULL, deliminiters, &saveptr);
 609                         if (!value)
 610                                 break;
 611                         el->binary.args[argi] = strtoul(value, NULL, 16);
 612                         argi++;
 613                         if (argi >= BINARY_MAX_ARGS) {
 614                                 fprintf(stderr,
 615                                         "Too many argument for binary\n");
 616                                 return -1;
 617                         }
 618                 }
 619                 el->binary.nargs = argi;
 620         } else if (!strcmp(keyword, "DATA")) {
 621                 char *value1 = strtok_r(NULL, deliminiters, &saveptr);
 622                 char *value2 = strtok_r(NULL, deliminiters, &saveptr);
 623
 624                 if (!value1 || !value2) {
 625                         fprintf(stderr,
 626                                 "Invalid number of arguments for DATA\n");
 627                         return -1;
 628                 }
 629
 630                 el->type = IMAGE_CFG_DATA;
 631                 el->regdata.raddr = strtoul(value1, NULL, 16);
 632                 el->regdata.rdata = strtoul(value2, NULL, 16);
 633         } else {
 634                 fprintf(stderr, "Ignoring unknown line '%s'\n", line);
 635         }
 636
 637         return 0;
 638 }
 639
 640 /*
 641  * Parse the configuration file 'fcfg' into the array of configuration
 642  * elements 'image_cfg', and return the number of configuration
 643  * elements in 'cfgn'.
 644  */
 645 static int image_create_config_parse(FILE *fcfg)
 646 {
 647         int ret;
 648         int cfgi = 0;
 649
 650         /* Parse the configuration file */
 651         while (!feof(fcfg)) {
 652                 char *line;
 653                 char buf[256];
 654
 655                 /* Read the current line */
 656                 memset(buf, 0, sizeof(buf));
 657                 line = fgets(buf, sizeof(buf), fcfg);
 658                 if (!line)
 659                         break;
 660
 661                 /* Ignore useless lines */
 662                 if (line[0] == '\n' || line[0] == '#')
 663                         continue;
 664
 665                 /* Strip final newline */
 666                 if (line[strlen(line) - 1] == '\n')
 667                         line[strlen(line) - 1] = 0;
 668
 669                 /* Parse the current line */
 670                 ret = image_create_config_parse_oneline(line,
 671                                                         &image_cfg[cfgi]);
 672                 if (ret)
 673                         return ret;
 674
 675                 cfgi++;
 676
 677                 if (cfgi >= IMAGE_CFG_ELEMENT_MAX) {
 678                         fprintf(stderr,
 679                                 "Too many configuration elements in .cfg file\n");
 680                         return -1;
 681                 }
 682         }
 683
 684         cfgn = cfgi;
 685         return 0;
 686 }
 687
 688 static int image_get_version(void)
 689 {
 690         struct image_cfg_element *e;
 691
 692         e = image_find_option(IMAGE_CFG_VERSION);
 693         if (!e)
 694                 return -1;
 695
 696         return e->version;
 697 }
 698
 699 static int image_version_file(const char *input)
 700 {
 701         FILE *fcfg;
 702         int version;
 703         int ret;
 704
 705         fcfg = fopen(input, "r");
 706         if (!fcfg) {
 707                 fprintf(stderr, "Could not open input file %s\n", input);
 708                 return -1;
 709         }
 710
 711         image_cfg = malloc(IMAGE_CFG_ELEMENT_MAX *
 712                            sizeof(struct image_cfg_element));
 713         if (!image_cfg) {
 714                 fprintf(stderr, "Cannot allocate memory\n");
 715                 fclose(fcfg);
 716                 return -1;
 717         }
 718
 719         memset(image_cfg, 0,
 720                IMAGE_CFG_ELEMENT_MAX * sizeof(struct image_cfg_element));
 721         rewind(fcfg);
 722
 723         ret = image_create_config_parse(fcfg);
 724         fclose(fcfg);
 725         if (ret) {
 726                 free(image_cfg);
 727                 return -1;
 728         }
 729
 730         version = image_get_version();
 731         /* Fallback to version 0 is no version is provided in the cfg file */
 732         if (version == -1)
 733                 version = 0;
 734
 735         free(image_cfg);
 736
 737         return version;
 738 }
 739
 740 static void kwbimage_set_header(void *ptr, struct stat *sbuf, int ifd,
 741                                 struct image_tool_params *params)
 742 {
 743         FILE *fcfg;
 744         void *image = NULL;
 745         int version;
 746         size_t headersz = 0;
 747         uint32_t checksum;
 748         int ret;
 749         int size;
 750
 751         fcfg = fopen(params->imagename, "r");
 752         if (!fcfg) {
 753                 fprintf(stderr, "Could not open input file %s\n",
 754                         params->imagename);
 755                 exit(EXIT_FAILURE);
 756         }
 757
 758         image_cfg = malloc(IMAGE_CFG_ELEMENT_MAX *
 759                            sizeof(struct image_cfg_element));
 760         if (!image_cfg) {
 761                 fprintf(stderr, "Cannot allocate memory\n");
 762                 fclose(fcfg);
 763                 exit(EXIT_FAILURE);
 764         }
 765
 766         memset(image_cfg, 0,
 767                IMAGE_CFG_ELEMENT_MAX * sizeof(struct image_cfg_element));
 768         rewind(fcfg);
 769
 770         ret = image_create_config_parse(fcfg);
 771         fclose(fcfg);
 772         if (ret) {
 773                 free(image_cfg);
 774                 exit(EXIT_FAILURE);
 775         }
 776
 777         version = image_get_version();
 778         switch (version) {
 779                 /*
 780                  * Fallback to version 0 if no version is provided in the
 781                  * cfg file
 782                  */
 783         case -1:
 784         case 0:
 785                 image = image_create_v0(&headersz, params, sbuf->st_size);
 786                 break;
 787
 788         case 1:
 789                 image = image_create_v1(&headersz, params, sbuf->st_size);
 790                 break;
 791
 792         default:
 793                 fprintf(stderr, "Unsupported version %d\n", version);
 794                 free(image_cfg);
 795                 exit(EXIT_FAILURE);
 796         }
 797
 798         if (!image) {
 799                 fprintf(stderr, "Could not create image\n");
 800                 free(image_cfg);
 801                 exit(EXIT_FAILURE);
 802         }
 803
 804         free(image_cfg);
 805
 806         /* Build and add image checksum header */
 807         checksum = image_checksum32((uint32_t *)ptr, sbuf->st_size);
 808         size = write(ifd, &checksum, sizeof(uint32_t));
 809         if (size != sizeof(uint32_t)) {
 810                 fprintf(stderr, "Error:%s - Checksum write %d bytes %s\n",
 811                         params->cmdname, size, params->imagefile);
 812                 exit(EXIT_FAILURE);
 813         }
 814
 815         sbuf->st_size += sizeof(uint32_t);
 816
 817         /* Finally copy the header into the image area */
 818         memcpy(ptr, image, headersz);
 819
 820         free(image);
 821 }
 822
 823 static void kwbimage_print_header(const void *ptr)
 824 {
 825         struct main_hdr_v0 *mhdr = (struct main_hdr_v0 *)ptr;
 826
 827         printf("Image Type:   MVEBU Boot from %s Image\n",
 828                image_boot_mode_name(mhdr->blockid));
 829         printf("Image version:%d\n", image_version((void *)ptr));
 830         printf("Data Size:    ");
 831         genimg_print_size(mhdr->blocksize - sizeof(uint32_t));
 832         printf("Load Address: %08x\n", mhdr->destaddr);
 833         printf("Entry Point:  %08x\n", mhdr->execaddr);
 834 }
 835
 836 static int kwbimage_check_image_types(uint8_t type)
 837 {
 838         if (type == IH_TYPE_KWBIMAGE)
 839                 return EXIT_SUCCESS;
 840         else
 841                 return EXIT_FAILURE;
 842 }
 843
 844 static int kwbimage_verify_header(unsigned char *ptr, int image_size,
 845                                   struct image_tool_params *params)
 846 {
 847         struct main_hdr_v0 *main_hdr;
 848         struct ext_hdr_v0 *ext_hdr;
 849         uint8_t checksum;
 850
 851         main_hdr = (void *)ptr;
 852         checksum = image_checksum8(ptr,
 853                                    sizeof(struct main_hdr_v0)
 854                                    - sizeof(uint8_t));
 855         if (checksum != main_hdr->checksum)
 856                 return -FDT_ERR_BADSTRUCTURE;
 857
 858         /* Only version 0 extended header has checksum */
 859         if (image_version((void *)ptr) == 0) {
 860                 ext_hdr = (void *)ptr + sizeof(struct main_hdr_v0);
 861                 checksum = image_checksum8(ext_hdr,
 862                                            sizeof(struct ext_hdr_v0)
 863                                            - sizeof(uint8_t));
 864                 if (checksum != ext_hdr->checksum)
 865                         return -FDT_ERR_BADSTRUCTURE;
 866         }
 867
 868         return 0;
 869 }
 870
 871 static int kwbimage_generate(struct image_tool_params *params,
 872                              struct image_type_params *tparams)
 873 {
 874         int alloc_len;
 875         void *hdr;
 876         int version = 0;
 877
 878         version = image_version_file(params->imagename);
 879         if (version == 0) {
 880                 alloc_len = sizeof(struct main_hdr_v0) +
 881                         sizeof(struct ext_hdr_v0);
 882         } else {
 883                 alloc_len = image_headersz_v1(params, NULL);
 884         }
 885
 886         hdr = malloc(alloc_len);
 887         if (!hdr) {
 888                 fprintf(stderr, "%s: malloc return failure: %s\n",
 889                         params->cmdname, strerror(errno));
 890                 exit(EXIT_FAILURE);
 891         }
 892
 893         memset(hdr, 0, alloc_len);
 894         tparams->header_size = alloc_len;
 895         tparams->hdr = hdr;
 896
 897         return 0;
 898 }
 899
 900 /*
 901  * Report Error if xflag is set in addition to default
 902  */
 903 static int kwbimage_check_params(struct image_tool_params *params)
 904 {
 905         if (!strlen(params->imagename)) {
 906                 fprintf(stderr, "Error:%s - Configuration file not specified, "
 907                         "it is needed for kwbimage generation\n",
 908                         params->cmdname);
 909                 return CFG_INVALID;
 910         }
 911
 912         return (params->dflag && (params->fflag || params->lflag)) ||
 913                 (params->fflag && (params->dflag || params->lflag)) ||
 914                 (params->lflag && (params->dflag || params->fflag)) ||
 915                 (params->xflag) || !(strlen(params->imagename));
 916 }
 917
 918 /*
 919  * kwbimage type parameters definition
 920  */
 921 U_BOOT_IMAGE_TYPE(
 922         kwbimage,
 923         "Marvell MVEBU Boot Image support",
 924         0,
 925         NULL,
 926         kwbimage_check_params,
 927         kwbimage_verify_header,
 928         kwbimage_print_header,
 929         kwbimage_set_header,
 930         NULL,
 931         kwbimage_check_image_types,
 932         NULL,
 933         kwbimage_generate
 934 );