mxsimage.c

/*
 * Freescale i.MX23/i.MX28 SB image generator
 *
 * Copyright (C) 2012-2013 Marek Vasut <marex@denx.de>
 *
 * SPDX-License-Identifier:	GPL-2.0+
 */

#ifdef CONFIG_MXS

#include <errno.h>
#include <fcntl.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <limits.h>

#include <openssl/evp.h>

#include "imagetool.h"
#include "mxsimage.h"
#include "pbl_crc32.h"
#include <image.h>


/*
 * DCD block
 * |-Write to address command block
 * |  0xf00 == 0xf33d
 * |  0xba2 == 0xb33f
 * |-ORR address with mask command block
 * |  0xf00 |= 0x1337
 * |-Write to address command block
 * |  0xba2 == 0xd00d
 * :
 */
#define SB_HAB_DCD_WRITE	0xccUL
#define SB_HAB_DCD_CHECK	0xcfUL
#define SB_HAB_DCD_NOOP		0xc0UL
#define SB_HAB_DCD_MASK_BIT	(1 << 3)
#define SB_HAB_DCD_SET_BIT	(1 << 4)

/* Addr.n = Value.n */
#define	SB_DCD_WRITE	\
	(SB_HAB_DCD_WRITE << 24)
/* Addr.n &= ~Value.n */
#define	SB_DCD_ANDC	\
	((SB_HAB_DCD_WRITE << 24) | SB_HAB_DCD_SET_BIT)
/* Addr.n |= Value.n */
#define	SB_DCD_ORR	\
	((SB_HAB_DCD_WRITE << 24) | SB_HAB_DCD_SET_BIT | SB_HAB_DCD_MASK_BIT)
/* (Addr.n & Value.n) == 0 */
#define	SB_DCD_CHK_EQZ	\
	(SB_HAB_DCD_CHECK << 24)
/* (Addr.n & Value.n) == Value.n */
#define	SB_DCD_CHK_EQ	\
	((SB_HAB_DCD_CHECK << 24) | SB_HAB_DCD_SET_BIT)
/* (Addr.n & Value.n) != Value.n */
#define	SB_DCD_CHK_NEQ	\
	((SB_HAB_DCD_CHECK << 24) | SB_HAB_DCD_MASK_BIT)
/* (Addr.n & Value.n) != 0 */
#define	SB_DCD_CHK_NEZ	\
	((SB_HAB_DCD_CHECK << 24) | SB_HAB_DCD_SET_BIT | SB_HAB_DCD_MASK_BIT)
/* NOP */
#define	SB_DCD_NOOP	\
	(SB_HAB_DCD_NOOP << 24)

struct sb_dcd_ctx {
	struct sb_dcd_ctx		*dcd;

	uint32_t			id;

	/* The DCD block. */
	uint32_t			*payload;
	/* Size of the whole DCD block. */
	uint32_t			size;

	/* Pointer to previous DCD command block. */
	uint32_t			*prev_dcd_head;
};

/*
 * IMAGE
 *   |-SECTION
 *   |    |-CMD
 *   |    |-CMD
 *   |    `-CMD
 *   |-SECTION
 *   |    |-CMD
 *   :    :
 */
struct sb_cmd_list {
	char				*cmd;
	size_t				len;
	unsigned int			lineno;
};

struct sb_cmd_ctx {
	uint32_t			size;

	struct sb_cmd_ctx		*cmd;

	uint8_t				*data;
	uint32_t			length;

	struct sb_command		payload;
	struct sb_command		c_payload;
};

struct sb_section_ctx {
	uint32_t			size;

	/* Section flags */
	unsigned int			boot:1;

	struct sb_section_ctx		*sect;

	struct sb_cmd_ctx		*cmd_head;
	struct sb_cmd_ctx		*cmd_tail;

	struct sb_sections_header	payload;
};

struct sb_image_ctx {
	unsigned int			in_section:1;
	unsigned int			in_dcd:1;
	/* Image configuration */
	unsigned int			display_progress:1;
	unsigned int			silent_dump:1;
	char				*input_filename;
	char				*output_filename;
	char				*cfg_filename;
	uint8_t				image_key[16];

	/* Number of section in the image */
	unsigned int			sect_count;
	/* Bootable section */
	unsigned int			sect_boot;
	unsigned int			sect_boot_found:1;

	struct sb_section_ctx		*sect_head;
	struct sb_section_ctx		*sect_tail;

	struct sb_dcd_ctx		*dcd_head;
	struct sb_dcd_ctx		*dcd_tail;

	EVP_CIPHER_CTX			cipher_ctx;
	EVP_MD_CTX			md_ctx;
	uint8_t				digest[32];
	struct sb_key_dictionary_key	sb_dict_key;

	struct sb_boot_image_header	payload;
};

/*
 * Instruction semantics:
 * NOOP
 * TAG [LAST]
 * LOAD       address file
 * LOAD  IVT  address IVT_entry_point
 * FILL address pattern length
 * JUMP [HAB] address [r0_arg]
 * CALL [HAB] address [r0_arg]
 * MODE mode
 *      For i.MX23, mode = USB/I2C/SPI1_FLASH/SPI2_FLASH/NAND_BCH
 *                         JTAG/SPI3_EEPROM/SD_SSP0/SD_SSP1
 *      For i.MX28, mode = USB/I2C/SPI2_FLASH/SPI3_FLASH/NAND_BCH
 *                         JTAG/SPI2_EEPROM/SD_SSP0/SD_SSP1
 */

/*
 * AES libcrypto
 */
static int sb_aes_init(struct sb_image_ctx *ictx, uint8_t *iv, int enc)
{
	EVP_CIPHER_CTX *ctx = &ictx->cipher_ctx;
	int ret;

	/* If there is no init vector, init vector is all zeroes. */
	if (!iv)
		iv = ictx->image_key;

	EVP_CIPHER_CTX_init(ctx);
	ret = EVP_CipherInit(ctx, EVP_aes_128_cbc(), ictx->image_key, iv, enc);
	if (ret == 1)
		EVP_CIPHER_CTX_set_padding(ctx, 0);
	return ret;
}

static int sb_aes_crypt(struct sb_image_ctx *ictx, uint8_t *in_data,
			uint8_t *out_data, int in_len)
{
	EVP_CIPHER_CTX *ctx = &ictx->cipher_ctx;
	int ret, outlen;
	uint8_t *outbuf;

	outbuf = malloc(in_len);
	if (!outbuf)
		return -ENOMEM;
	memset(outbuf, 0, sizeof(in_len));

	ret = EVP_CipherUpdate(ctx, outbuf, &outlen, in_data, in_len);
	if (!ret) {
		ret = -EINVAL;
		goto err;
	}

	if (out_data)
		memcpy(out_data, outbuf, outlen);

err:
	free(outbuf);
	return ret;
}

static int sb_aes_deinit(EVP_CIPHER_CTX *ctx)
{
	return EVP_CIPHER_CTX_cleanup(ctx);
}

static int sb_aes_reinit(struct sb_image_ctx *ictx, int enc)
{
	int ret;
	EVP_CIPHER_CTX *ctx = &ictx->cipher_ctx;
	struct sb_boot_image_header *sb_header = &ictx->payload;
	uint8_t *iv = sb_header->iv;

	ret = sb_aes_deinit(ctx);
	if (!ret)
		return ret;
	return sb_aes_init(ictx, iv, enc);
}

/*
 * Debug
 */
static void soprintf(struct sb_image_ctx *ictx, const char *fmt, ...)
{
	va_list ap;

	if (ictx->silent_dump)
		return;

	va_start(ap, fmt);
	vfprintf(stdout, fmt, ap);
	va_end(ap);
}

/*
 * Code
 */
static time_t sb_get_timestamp(void)
{
	struct tm time_2000 = {
		.tm_yday	= 1,	/* Jan. 1st */
		.tm_year	= 100,	/* 2000 */
	};
	time_t seconds_to_2000 = mktime(&time_2000);
	time_t seconds_to_now = time(NULL);

	return seconds_to_now - seconds_to_2000;
}

static int sb_get_time(time_t time, struct tm *tm)
{
	struct tm time_2000 = {
		.tm_yday	= 1,	/* Jan. 1st */
		.tm_year	= 0,	/* 1900 */
	};
	const time_t seconds_to_2000 = mktime(&time_2000);
	const time_t seconds_to_now = seconds_to_2000 + time;
	struct tm *ret;
	ret = gmtime_r(&seconds_to_now, tm);
	return ret ? 0 : -EINVAL;
}

static void sb_encrypt_sb_header(struct sb_image_ctx *ictx)
{
	EVP_MD_CTX *md_ctx = &ictx->md_ctx;
	struct sb_boot_image_header *sb_header = &ictx->payload;
	uint8_t *sb_header_ptr = (uint8_t *)sb_header;

	/* Encrypt the header, compute the digest. */
	sb_aes_crypt(ictx, sb_header_ptr, NULL, sizeof(*sb_header));
	EVP_DigestUpdate(md_ctx, sb_header_ptr, sizeof(*sb_header));
}

static void sb_encrypt_sb_sections_header(struct sb_image_ctx *ictx)
{
	EVP_MD_CTX *md_ctx = &ictx->md_ctx;
	struct sb_section_ctx *sctx = ictx->sect_head;
	struct sb_sections_header *shdr;
	uint8_t *sb_sections_header_ptr;
	const int size = sizeof(*shdr);

	while (sctx) {
		shdr = &sctx->payload;
		sb_sections_header_ptr = (uint8_t *)shdr;

		sb_aes_crypt(ictx, sb_sections_header_ptr,
			     ictx->sb_dict_key.cbc_mac, size);
		EVP_DigestUpdate(md_ctx, sb_sections_header_ptr, size);

		sctx = sctx->sect;
	};
}

static void sb_encrypt_key_dictionary_key(struct sb_image_ctx *ictx)
{
	EVP_MD_CTX *md_ctx = &ictx->md_ctx;

	sb_aes_crypt(ictx, ictx->image_key, ictx->sb_dict_key.key,
		     sizeof(ictx->sb_dict_key.key));
	EVP_DigestUpdate(md_ctx, &ictx->sb_dict_key, sizeof(ictx->sb_dict_key));
}

static void sb_decrypt_key_dictionary_key(struct sb_image_ctx *ictx)
{
	EVP_MD_CTX *md_ctx = &ictx->md_ctx;

	EVP_DigestUpdate(md_ctx, &ictx->sb_dict_key, sizeof(ictx->sb_dict_key));
	sb_aes_crypt(ictx, ictx->sb_dict_key.key, ictx->image_key,
		     sizeof(ictx->sb_dict_key.key));
}

static void sb_encrypt_tag(struct sb_image_ctx *ictx,
		struct sb_cmd_ctx *cctx)
{
	EVP_MD_CTX *md_ctx = &ictx->md_ctx;
	struct sb_command *cmd = &cctx->payload;

	sb_aes_crypt(ictx, (uint8_t *)cmd,
		     (uint8_t *)&cctx->c_payload, sizeof(*cmd));
	EVP_DigestUpdate(md_ctx, &cctx->c_payload, sizeof(*cmd));
}

static int sb_encrypt_image(struct sb_image_ctx *ictx)
{
	/* Start image-wide crypto. */
	EVP_MD_CTX_init(&ictx->md_ctx);
	EVP_DigestInit(&ictx->md_ctx, EVP_sha1());

	/*
	 * SB image header.
	 */
	sb_aes_init(ictx, NULL, 1);
	sb_encrypt_sb_header(ictx);

	/*
	 * SB sections header.
	 */
	sb_encrypt_sb_sections_header(ictx);

	/*
	 * Key dictionary.
	 */
	sb_aes_reinit(ictx, 1);
	sb_encrypt_key_dictionary_key(ictx);

	/*
	 * Section tags.
	 */
	struct sb_cmd_ctx *cctx;
	struct sb_command *ccmd;
	struct sb_section_ctx *sctx = ictx->sect_head;

	while (sctx) {
		cctx = sctx->cmd_head;

		sb_aes_reinit(ictx, 1);

		while (cctx) {
			ccmd = &cctx->payload;

			sb_encrypt_tag(ictx, cctx);

			if (ccmd->header.tag == ROM_TAG_CMD) {
				sb_aes_reinit(ictx, 1);
			} else if (ccmd->header.tag == ROM_LOAD_CMD) {
				sb_aes_crypt(ictx, cctx->data, cctx->data,
					     cctx->length);
				EVP_DigestUpdate(&ictx->md_ctx, cctx->data,
						 cctx->length);
			}

			cctx = cctx->cmd;
		}

		sctx = sctx->sect;
	};

	/*
	 * Dump the SHA1 of the whole image.
	 */
	sb_aes_reinit(ictx, 1);

	EVP_DigestFinal(&ictx->md_ctx, ictx->digest, NULL);
	sb_aes_crypt(ictx, ictx->digest, ictx->digest, sizeof(ictx->digest));

	/* Stop the encryption session. */
	sb_aes_deinit(&ictx->cipher_ctx);

	return 0;
}

static int sb_load_file(struct sb_cmd_ctx *cctx, char *filename)
{
	long real_size, roundup_size;
	uint8_t *data;
	long ret;
	unsigned long size;
	FILE *fp;

	if (!filename) {
		fprintf(stderr, "ERR: Missing filename!\n");
		return -EINVAL;
	}

	fp = fopen(filename, "r");
	if (!fp)
		goto err_open;

	ret = fseek(fp, 0, SEEK_END);
	if (ret < 0)
		goto err_file;

	real_size = ftell(fp);
	if (real_size < 0)
		goto err_file;

	ret = fseek(fp, 0, SEEK_SET);
	if (ret < 0)
		goto err_file;

	roundup_size = roundup(real_size, SB_BLOCK_SIZE);
	data = calloc(1, roundup_size);
	if (!data)
		goto err_file;

	size = fread(data, 1, real_size, fp);
	if (size != (unsigned long)real_size)
		goto err_alloc;

	cctx->data = data;
	cctx->length = roundup_size;

	fclose(fp);
	return 0;

err_alloc:
	free(data);
err_file:
	fclose(fp);
err_open:
	fprintf(stderr, "ERR: Failed to load file \"%s\"\n", filename);
	return -EINVAL;
}

static uint8_t sb_command_checksum(struct sb_command *inst)
{
	uint8_t *inst_ptr = (uint8_t *)inst;
	uint8_t csum = 0;
	unsigned int i;

	for (i = 0; i < sizeof(struct sb_command); i++)
		csum += inst_ptr[i];

	return csum;
}

static int sb_token_to_long(char *tok, uint32_t *rid)
{
	char *endptr;
	unsigned long id;

	if (tok[0] != '0' || tok[1] != 'x') {
		fprintf(stderr, "ERR: Invalid hexadecimal number!\n");
		return -EINVAL;
	}

	tok += 2;

	errno = 0;
	id = strtoul(tok, &endptr, 16);
	if ((errno == ERANGE && id == ULONG_MAX) || (errno != 0 && id == 0)) {
		fprintf(stderr, "ERR: Value can't be decoded!\n");
		return -EINVAL;
	}

	/* Check for 32-bit overflow. */
	if (id > 0xffffffff) {
		fprintf(stderr, "ERR: Value too big!\n");
		return -EINVAL;
	}

	if (endptr == tok) {
		fprintf(stderr, "ERR: Deformed value!\n");
		return -EINVAL;
	}

	*rid = (uint32_t)id;
	return 0;
}

static int sb_grow_dcd(struct sb_dcd_ctx *dctx, unsigned int inc_size)
{
	uint32_t *tmp;

	if (!inc_size)
		return 0;

	dctx->size += inc_size;
	tmp = realloc(dctx->payload, dctx->size);
	if (!tmp)
		return -ENOMEM;

	dctx->payload = tmp;

	/* Assemble and update the HAB DCD header. */
	dctx->payload[0] = htonl((SB_HAB_DCD_TAG << 24) |
				 (dctx->size << 8) |
				 SB_HAB_VERSION);

	return 0;
}

static int sb_build_dcd(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd)
{
	struct sb_dcd_ctx *dctx;

	char *tok;
	uint32_t id;
	int ret;

	dctx = calloc(1, sizeof(*dctx));
	if (!dctx)
		return -ENOMEM;

	ret = sb_grow_dcd(dctx, 4);
	if (ret)
		goto err_dcd;

	/* Read DCD block number. */
	tok = strtok(cmd->cmd, " ");
	if (!tok) {
		fprintf(stderr, "#%i ERR: DCD block without number!\n",
			cmd->lineno);
		ret = -EINVAL;
		goto err_dcd;
	}

	/* Parse the DCD block number. */
	ret = sb_token_to_long(tok, &id);
	if (ret) {
		fprintf(stderr, "#%i ERR: Malformed DCD block number!\n",
			cmd->lineno);
		goto err_dcd;
	}

	dctx->id = id;

	/*
	 * The DCD block is now constructed. Append it to the list.
	 * WARNING: The DCD size is still not computed and will be
	 * updated while parsing it's commands.
	 */
	if (!ictx->dcd_head) {
		ictx->dcd_head = dctx;
		ictx->dcd_tail = dctx;
	} else {
		ictx->dcd_tail->dcd = dctx;
		ictx->dcd_tail = dctx;
	}

	return 0;

err_dcd:
	free(dctx->payload);
	free(dctx);
	return ret;
}

static int sb_build_dcd_block(struct sb_image_ctx *ictx,
			      struct sb_cmd_list *cmd,
			      uint32_t type)
{
	char *tok;
	uint32_t address, value, length;
	int ret;

	struct sb_dcd_ctx *dctx = ictx->dcd_tail;
	uint32_t *dcd;

	if (dctx->prev_dcd_head && (type != SB_DCD_NOOP) &&
	    ((dctx->prev_dcd_head[0] & 0xff0000ff) == type)) {
		/* Same instruction as before, just append it. */
		ret = sb_grow_dcd(dctx, 8);
		if (ret)
			return ret;
	} else if (type == SB_DCD_NOOP) {
		ret = sb_grow_dcd(dctx, 4);
		if (ret)
			return ret;

		/* Update DCD command block pointer. */
		dctx->prev_dcd_head = dctx->payload +
				dctx->size / sizeof(*dctx->payload) - 1;

		/* NOOP has only 4 bytes and no payload. */
		goto noop;
	} else {
		/*
		 * Either a different instruction block started now
		 * or this is the first instruction block.
		 */
		ret = sb_grow_dcd(dctx, 12);
		if (ret)
			return ret;

		/* Update DCD command block pointer. */
		dctx->prev_dcd_head = dctx->payload +
				dctx->size / sizeof(*dctx->payload) - 3;
	}

	dcd = dctx->payload + dctx->size / sizeof(*dctx->payload) - 2;

	/*
	 * Prepare the command.
	 */
	tok = strtok(cmd->cmd, " ");
	if (!tok) {
		fprintf(stderr, "#%i ERR: Missing DCD address!\n",
			cmd->lineno);
		ret = -EINVAL;
		goto err;
	}

	/* Read DCD destination address. */
	ret = sb_token_to_long(tok, &address);
	if (ret) {
		fprintf(stderr, "#%i ERR: Incorrect DCD address!\n",
			cmd->lineno);
		goto err;
	}

	tok = strtok(NULL, " ");
	if (!tok) {
		fprintf(stderr, "#%i ERR: Missing DCD value!\n",
			cmd->lineno);
		ret = -EINVAL;
		goto err;
	}

	/* Read DCD operation value. */
	ret = sb_token_to_long(tok, &value);
	if (ret) {
		fprintf(stderr, "#%i ERR: Incorrect DCD value!\n",
			cmd->lineno);
		goto err;
	}

	/* Fill in the new DCD entry. */
	dcd[0] = htonl(address);
	dcd[1] = htonl(value);

noop:
	/* Update the DCD command block. */
	length = dctx->size -
		 ((dctx->prev_dcd_head - dctx->payload) *
		 sizeof(*dctx->payload));
	dctx->prev_dcd_head[0] = htonl(type | (length << 8));

err:
	return ret;
}

static int sb_build_section(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd)
{
	struct sb_section_ctx *sctx;
	struct sb_sections_header *shdr;
	char *tok;
	uint32_t bootable = 0;
	uint32_t id;
	int ret;

	sctx = calloc(1, sizeof(*sctx));
	if (!sctx)
		return -ENOMEM;

	/* Read section number. */
	tok = strtok(cmd->cmd, " ");
	if (!tok) {
		fprintf(stderr, "#%i ERR: Section without number!\n",
			cmd->lineno);
		ret = -EINVAL;
		goto err_sect;
	}

	/* Parse the section number. */
	ret = sb_token_to_long(tok, &id);
	if (ret) {
		fprintf(stderr, "#%i ERR: Malformed section number!\n",
			cmd->lineno);
		goto err_sect;
	}

	/* Read section's BOOTABLE flag. */
	tok = strtok(NULL, " ");
	if (tok && (strlen(tok) == 8) && !strncmp(tok, "BOOTABLE", 8))
		bootable = SB_SECTION_FLAG_BOOTABLE;

	sctx->boot = bootable;

	shdr = &sctx->payload;
	shdr->section_number = id;
	shdr->section_flags = bootable;

	/*
	 * The section is now constructed. Append it to the list.
	 * WARNING: The section size is still not computed and will
	 * be updated while parsing it's commands.
	 */
	ictx->sect_count++;

	/* Mark that this section is bootable one. */
	if (bootable) {
		if (ictx->sect_boot_found) {
			fprintf(stderr,
				"#%i WARN: Multiple bootable section!\n",
				cmd->lineno);
		} else {
			ictx->sect_boot = id;
			ictx->sect_boot_found = 1;
		}
	}

	if (!ictx->sect_head) {
		ictx->sect_head = sctx;
		ictx->sect_tail = sctx;
	} else {
		ictx->sect_tail->sect = sctx;
		ictx->sect_tail = sctx;
	}

	return 0;

err_sect:
	free(sctx);
	return ret;
}

static int sb_build_command_nop(struct sb_image_ctx *ictx)
{
	struct sb_section_ctx *sctx = ictx->sect_tail;
	struct sb_cmd_ctx *cctx;
	struct sb_command *ccmd;

	cctx = calloc(1, sizeof(*cctx));
	if (!cctx)
		return -ENOMEM;

	ccmd = &cctx->payload;

	/*
	 * Construct the command.
	 */
	ccmd->header.checksum	= 0x5a;
	ccmd->header.tag	= ROM_NOP_CMD;

	cctx->size = sizeof(*ccmd);

	/*
	 * Append the command to the last section.
	 */
	if (!sctx->cmd_head) {
		sctx->cmd_head = cctx;
		sctx->cmd_tail = cctx;
	} else {
		sctx->cmd_tail->cmd = cctx;
		sctx->cmd_tail = cctx;
	}

	return 0;
}

static int sb_build_command_tag(struct sb_image_ctx *ictx,
				struct sb_cmd_list *cmd)
{
	struct sb_section_ctx *sctx = ictx->sect_tail;
	struct sb_cmd_ctx *cctx;
	struct sb_command *ccmd;
	char *tok;

	cctx = calloc(1, sizeof(*cctx));
	if (!cctx)
		return -ENOMEM;

	ccmd = &cctx->payload;

	/*
	 * Prepare the command.
	 */
	/* Check for the LAST keyword. */
	tok = strtok(cmd->cmd, " ");
	if (tok && !strcmp(tok, "LAST"))
		ccmd->header.flags = ROM_TAG_CMD_FLAG_ROM_LAST_TAG;

	/*
	 * Construct the command.
	 */
	ccmd->header.checksum	= 0x5a;
	ccmd->header.tag	= ROM_TAG_CMD;

	cctx->size = sizeof(*ccmd);

	/*
	 * Append the command to the last section.
	 */
	if (!sctx->cmd_head) {
		sctx->cmd_head = cctx;
		sctx->cmd_tail = cctx;
	} else {
		sctx->cmd_tail->cmd = cctx;
		sctx->cmd_tail = cctx;
	}

	return 0;
}

static int sb_build_command_load(struct sb_image_ctx *ictx,
				 struct sb_cmd_list *cmd)
{
	struct sb_section_ctx *sctx = ictx->sect_tail;
	struct sb_cmd_ctx *cctx;
	struct sb_command *ccmd;
	char *tok;
	int ret, is_ivt = 0, is_dcd = 0;
	uint32_t dest, dcd = 0;

	cctx = calloc(1, sizeof(*cctx));
	if (!cctx)
		return -ENOMEM;

	ccmd = &cctx->payload;

	/*
	 * Prepare the command.
	 */
	tok = strtok(cmd->cmd, " ");
	if (!tok) {
		fprintf(stderr, "#%i ERR: Missing LOAD address or 'IVT'!\n",
			cmd->lineno);
		ret = -EINVAL;
		goto err;
	}

	/* Check for "IVT" flag. */
	if (!strcmp(tok, "IVT"))
		is_ivt = 1;
	if (!strcmp(tok, "DCD"))
		is_dcd = 1;
	if (is_ivt || is_dcd) {
		tok = strtok(NULL, " ");
		if (!tok) {
			fprintf(stderr, "#%i ERR: Missing LOAD address!\n",
				cmd->lineno);
			ret = -EINVAL;
			goto err;
		}
	}

	/* Read load destination address. */
	ret = sb_token_to_long(tok, &dest);
	if (ret) {
		fprintf(stderr, "#%i ERR: Incorrect LOAD address!\n",
			cmd->lineno);
		goto err;
	}

	/* Read filename or IVT entrypoint or DCD block ID. */
	tok = strtok(NULL, " ");
	if (!tok) {
		fprintf(stderr,
			"#%i ERR: Missing LOAD filename or IVT ep or DCD block ID!\n",
			cmd->lineno);
		ret = -EINVAL;
		goto err;
	}

	if (is_ivt) {
		/* Handle IVT. */
		struct sb_ivt_header *ivt;
		uint32_t ivtep;
		ret = sb_token_to_long(tok, &ivtep);

		if (ret) {
			fprintf(stderr,
				"#%i ERR: Incorrect IVT entry point!\n",
				cmd->lineno);
			goto err;
		}

		ivt = calloc(1, sizeof(*ivt));
		if (!ivt) {
			ret = -ENOMEM;
			goto err;
		}

		ivt->header = sb_hab_ivt_header();
		ivt->entry = ivtep;
		ivt->self = dest;

		cctx->data = (uint8_t *)ivt;
		cctx->length = sizeof(*ivt);
	} else if (is_dcd) {
		struct sb_dcd_ctx *dctx = ictx->dcd_head;
		uint32_t dcdid;
		uint8_t *payload;
		uint32_t asize;
		ret = sb_token_to_long(tok, &dcdid);

		if (ret) {
			fprintf(stderr,
				"#%i ERR: Incorrect DCD block ID!\n",
				cmd->lineno);
			goto err;
		}

		while (dctx) {
			if (dctx->id == dcdid)
				break;
			dctx = dctx->dcd;
		}

		if (!dctx) {
			fprintf(stderr, "#%i ERR: DCD block %08x not found!\n",
				cmd->lineno, dcdid);
			goto err;
		}

		asize = roundup(dctx->size, SB_BLOCK_SIZE);
		payload = calloc(1, asize);
		if (!payload) {
			ret = -ENOMEM;
			goto err;
		}

		memcpy(payload, dctx->payload, dctx->size);

		cctx->data = payload;
		cctx->length = asize;

		/* Set the Load DCD flag. */
		dcd = ROM_LOAD_CMD_FLAG_DCD_LOAD;
	} else {
		/* Regular LOAD of a file. */
		ret = sb_load_file(cctx, tok);
		if (ret) {
			fprintf(stderr, "#%i ERR: Cannot load '%s'!\n",
				cmd->lineno, tok);
			goto err;
		}
	}

	if (cctx->length & (SB_BLOCK_SIZE - 1)) {
		fprintf(stderr, "#%i ERR: Unaligned payload!\n",
			cmd->lineno);
	}

	/*
	 * Construct the command.
	 */
	ccmd->header.checksum	= 0x5a;
	ccmd->header.tag	= ROM_LOAD_CMD;
	ccmd->header.flags	= dcd;

	ccmd->load.address	= dest;
	ccmd->load.count	= cctx->length;
	ccmd->load.crc32	= pbl_crc32(0,
					    (const char *)cctx->data,
					    cctx->length);

	cctx->size = sizeof(*ccmd) + cctx->length;

	/*
	 * Append the command to the last section.
	 */
	if (!sctx->cmd_head) {
		sctx->cmd_head = cctx;
		sctx->cmd_tail = cctx;
	} else {
		sctx->cmd_tail->cmd = cctx;
		sctx->cmd_tail = cctx;
	}

	return 0;

err:
	free(cctx);
	return ret;