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