Merge pull request #1707 from antonio-nino-diaz-arm/an/spm

SPM: Initial prototype based on SPCI and SPRT

services/std_svc/spm_deprecated/spm_main.c

+/*
+ * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch_helpers.h>
+#include <assert.h>
+#include <bl31.h>
+#include <context_mgmt.h>
+#include <debug.h>
+#include <ehf.h>
+#include <errno.h>
+#include <mm_svc.h>
+#include <platform.h>
+#include <runtime_svc.h>
+#include <secure_partition.h>
+#include <smccc.h>
+#include <smccc_helpers.h>
+#include <spinlock.h>
+#include <spm_svc.h>
+#include <utils.h>
+#include <xlat_tables_v2.h>
+
+#include "spm_private.h"
+
+/*******************************************************************************
+ * Secure Partition context information.
+ ******************************************************************************/
+static sp_context_t sp_ctx;
+
+/*******************************************************************************
+ * Set state of a Secure Partition context.
+ ******************************************************************************/
+void sp_state_set(sp_context_t *sp_ptr, sp_state_t state)
+{
+	spin_lock(&(sp_ptr->state_lock));
+	sp_ptr->state = state;
+	spin_unlock(&(sp_ptr->state_lock));
+}
+
+/*******************************************************************************
+ * Wait until the state of a Secure Partition is the specified one and change it
+ * to the desired state.
+ ******************************************************************************/
+void sp_state_wait_switch(sp_context_t *sp_ptr, sp_state_t from, sp_state_t to)
+{
+	int success = 0;
+
+	while (success == 0) {
+		spin_lock(&(sp_ptr->state_lock));
+
+		if (sp_ptr->state == from) {
+			sp_ptr->state = to;
+
+			success = 1;
+		}
+
+		spin_unlock(&(sp_ptr->state_lock));
+	}
+}
+
+/*******************************************************************************
+ * Check if the state of a Secure Partition is the specified one and, if so,
+ * change it to the desired state. Returns 0 on success, -1 on error.
+ ******************************************************************************/
+int sp_state_try_switch(sp_context_t *sp_ptr, sp_state_t from, sp_state_t to)
+{
+	int ret = -1;
+
+	spin_lock(&(sp_ptr->state_lock));
+
+	if (sp_ptr->state == from) {
+		sp_ptr->state = to;
+
+		ret = 0;
+	}
+
+	spin_unlock(&(sp_ptr->state_lock));
+
+	return ret;
+}
+
+/*******************************************************************************
+ * This function takes an SP context pointer and performs a synchronous entry
+ * into it.
+ ******************************************************************************/
+static uint64_t spm_sp_synchronous_entry(sp_context_t *sp_ctx)
+{
+	uint64_t rc;
+
+	assert(sp_ctx != NULL);
+
+	/* Assign the context of the SP to this CPU */
+	cm_set_context(&(sp_ctx->cpu_ctx), SECURE);
+
+	/* Restore the context assigned above */
+	cm_el1_sysregs_context_restore(SECURE);
+	cm_set_next_eret_context(SECURE);
+
+	/* Invalidate TLBs at EL1. */
+	tlbivmalle1();
+	dsbish();
+
+	/* Enter Secure Partition */
+	rc = spm_secure_partition_enter(&sp_ctx->c_rt_ctx);
+
+	/* Save secure state */
+	cm_el1_sysregs_context_save(SECURE);
+
+	return rc;
+}
+
+/*******************************************************************************
+ * This function returns to the place where spm_sp_synchronous_entry() was
+ * called originally.
+ ******************************************************************************/
+__dead2 static void spm_sp_synchronous_exit(uint64_t rc)
+{
+	sp_context_t *ctx = &sp_ctx;
+
+	/*
+	 * The SPM must have initiated the original request through a
+	 * synchronous entry into the secure partition. Jump back to the
+	 * original C runtime context with the value of rc in x0;
+	 */
+	spm_secure_partition_exit(ctx->c_rt_ctx, rc);
+
+	panic();
+}
+
+/*******************************************************************************
+ * Jump to each Secure Partition for the first time.
+ ******************************************************************************/
+static int32_t spm_init(void)
+{
+	uint64_t rc;
+	sp_context_t *ctx;
+
+	INFO("Secure Partition init...\n");
+
+	ctx = &sp_ctx;
+
+	ctx->state = SP_STATE_RESET;
+
+	rc = spm_sp_synchronous_entry(ctx);
+	assert(rc == 0);
+
+	ctx->state = SP_STATE_IDLE;
+
+	INFO("Secure Partition initialized.\n");
+
+	return rc;
+}
+
+/*******************************************************************************
+ * Initialize contexts of all Secure Partitions.
+ ******************************************************************************/
+int32_t spm_setup(void)
+{
+	sp_context_t *ctx;
+
+	/* Disable MMU at EL1 (initialized by BL2) */
+	disable_mmu_icache_el1();
+
+	/* Initialize context of the SP */
+	INFO("Secure Partition context setup start...\n");
+
+	ctx = &sp_ctx;
+
+	/* Assign translation tables context. */
+	ctx->xlat_ctx_handle = spm_get_sp_xlat_context();
+
+	spm_sp_setup(ctx);
+
+	/* Register init function for deferred init.  */
+	bl31_register_bl32_init(&spm_init);
+
+	INFO("Secure Partition setup done.\n");
+
+	return 0;
+}
+
+/*******************************************************************************
+ * Function to perform a call to a Secure Partition.
+ ******************************************************************************/
+uint64_t spm_sp_call(uint32_t smc_fid, uint64_t x1, uint64_t x2, uint64_t x3)
+{
+	uint64_t rc;
+	sp_context_t *sp_ptr = &sp_ctx;
+
+	/* Wait until the Secure Partition is idle and set it to busy. */
+	sp_state_wait_switch(sp_ptr, SP_STATE_IDLE, SP_STATE_BUSY);
+
+	/* Set values for registers on SP entry */
+	cpu_context_t *cpu_ctx = &(sp_ptr->cpu_ctx);
+
+	write_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X0, smc_fid);
+	write_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X1, x1);
+	write_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X2, x2);
+	write_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X3, x3);
+
+	/* Jump to the Secure Partition. */
+	rc = spm_sp_synchronous_entry(sp_ptr);
+
+	/* Flag Secure Partition as idle. */
+	assert(sp_ptr->state == SP_STATE_BUSY);
+	sp_state_set(sp_ptr, SP_STATE_IDLE);
+
+	return rc;
+}
+
+/*******************************************************************************
+ * MM_COMMUNICATE handler
+ ******************************************************************************/
+static uint64_t mm_communicate(uint32_t smc_fid, uint64_t mm_cookie,
+			       uint64_t comm_buffer_address,
+			       uint64_t comm_size_address, void *handle)
+{
+	uint64_t rc;
+
+	/* Cookie. Reserved for future use. It must be zero. */
+	if (mm_cookie != 0U) {
+		ERROR("MM_COMMUNICATE: cookie is not zero\n");
+		SMC_RET1(handle, SPM_INVALID_PARAMETER);
+	}
+
+	if (comm_buffer_address == 0U) {
+		ERROR("MM_COMMUNICATE: comm_buffer_address is zero\n");
+		SMC_RET1(handle, SPM_INVALID_PARAMETER);
+	}
+
+	if (comm_size_address != 0U) {
+		VERBOSE("MM_COMMUNICATE: comm_size_address is not 0 as recommended.\n");
+	}
+
+	/*
+	 * The current secure partition design mandates
+	 * - at any point, only a single core can be
+	 *   executing in the secure partiton.
+	 * - a core cannot be preempted by an interrupt
+	 *   while executing in secure partition.
+	 * Raise the running priority of the core to the
+	 * interrupt level configured for secure partition
+	 * so as to block any interrupt from preempting this
+	 * core.
+	 */
+	ehf_activate_priority(PLAT_SP_PRI);
+
+	/* Save the Normal world context */
+	cm_el1_sysregs_context_save(NON_SECURE);
+
+	rc = spm_sp_call(smc_fid, comm_buffer_address, comm_size_address,
+			 plat_my_core_pos());
+
+	/* Restore non-secure state */
+	cm_el1_sysregs_context_restore(NON_SECURE);
+	cm_set_next_eret_context(NON_SECURE);
+
+	/*
+	 * Exited from secure partition. This core can take
+	 * interrupts now.
+	 */
+	ehf_deactivate_priority(PLAT_SP_PRI);
+
+	SMC_RET1(handle, rc);
+}
+
+/*******************************************************************************
+ * Secure Partition Manager SMC handler.
+ ******************************************************************************/
+uint64_t spm_smc_handler(uint32_t smc_fid,
+			 uint64_t x1,
+			 uint64_t x2,
+			 uint64_t x3,
+			 uint64_t x4,
+			 void *cookie,
+			 void *handle,
+			 uint64_t flags)
+{
+	unsigned int ns;
+
+	/* Determine which security state this SMC originated from */
+	ns = is_caller_non_secure(flags);
+
+	if (ns == SMC_FROM_SECURE) {
+
+		/* Handle SMCs from Secure world. */
+
+		assert(handle == cm_get_context(SECURE));
+
+		/* Make next ERET jump to S-EL0 instead of S-EL1. */
+		cm_set_elr_spsr_el3(SECURE, read_elr_el1(), read_spsr_el1());
+
+		switch (smc_fid) {
+
+		case SPM_VERSION_AARCH32:
+			SMC_RET1(handle, SPM_VERSION_COMPILED);
+
+		case SP_EVENT_COMPLETE_AARCH64:
+			spm_sp_synchronous_exit(x1);
+
+		case SP_MEMORY_ATTRIBUTES_GET_AARCH64:
+			INFO("Received SP_MEMORY_ATTRIBUTES_GET_AARCH64 SMC\n");
+
+			if (sp_ctx.state != SP_STATE_RESET) {
+				WARN("SP_MEMORY_ATTRIBUTES_GET_AARCH64 is available at boot time only\n");
+				SMC_RET1(handle, SPM_NOT_SUPPORTED);
+			}
+			SMC_RET1(handle,
+				 spm_memory_attributes_get_smc_handler(
+					 &sp_ctx, x1));
+
+		case SP_MEMORY_ATTRIBUTES_SET_AARCH64:
+			INFO("Received SP_MEMORY_ATTRIBUTES_SET_AARCH64 SMC\n");
+
+			if (sp_ctx.state != SP_STATE_RESET) {
+				WARN("SP_MEMORY_ATTRIBUTES_SET_AARCH64 is available at boot time only\n");
+				SMC_RET1(handle, SPM_NOT_SUPPORTED);
+			}
+			SMC_RET1(handle,
+				 spm_memory_attributes_set_smc_handler(
+					&sp_ctx, x1, x2, x3));
+		default:
+			break;
+		}
+	} else {
+
+		/* Handle SMCs from Non-secure world. */
+
+		assert(handle == cm_get_context(NON_SECURE));
+
+		switch (smc_fid) {
+
+		case MM_VERSION_AARCH32:
+			SMC_RET1(handle, MM_VERSION_COMPILED);
+
+		case MM_COMMUNICATE_AARCH32:
+		case MM_COMMUNICATE_AARCH64:
+			return mm_communicate(smc_fid, x1, x2, x3, handle);
+
+		case SP_MEMORY_ATTRIBUTES_GET_AARCH64:
+		case SP_MEMORY_ATTRIBUTES_SET_AARCH64:
+			/* SMC interfaces reserved for secure callers. */
+			SMC_RET1(handle, SPM_NOT_SUPPORTED);
+
+		default:
+			break;
+		}
+	}
+
+	SMC_RET1(handle, SMC_UNK);
+}

services/std_svc/spm_deprecated/spm_private.h

+/*
+ * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef SPM_PRIVATE_H
+#define SPM_PRIVATE_H
+
+#include <context.h>
+
+/*******************************************************************************
+ * Constants that allow assembler code to preserve callee-saved registers of the
+ * C runtime context while performing a security state switch.
+ ******************************************************************************/
+#define SP_C_RT_CTX_X19		0x0
+#define SP_C_RT_CTX_X20		0x8
+#define SP_C_RT_CTX_X21		0x10
+#define SP_C_RT_CTX_X22		0x18
+#define SP_C_RT_CTX_X23		0x20
+#define SP_C_RT_CTX_X24		0x28
+#define SP_C_RT_CTX_X25		0x30
+#define SP_C_RT_CTX_X26		0x38
+#define SP_C_RT_CTX_X27		0x40
+#define SP_C_RT_CTX_X28		0x48
+#define SP_C_RT_CTX_X29		0x50
+#define SP_C_RT_CTX_X30		0x58
+
+#define SP_C_RT_CTX_SIZE	0x60
+#define SP_C_RT_CTX_ENTRIES	(SP_C_RT_CTX_SIZE >> DWORD_SHIFT)
+
+#ifndef __ASSEMBLY__
+
+#include <spinlock.h>
+#include <stdint.h>
+#include <xlat_tables_v2.h>
+
+typedef enum sp_state {
+	SP_STATE_RESET = 0,
+	SP_STATE_IDLE,
+	SP_STATE_BUSY
+} sp_state_t;
+
+typedef struct sp_context {
+	uint64_t c_rt_ctx;
+	cpu_context_t cpu_ctx;
+	xlat_ctx_t *xlat_ctx_handle;
+
+	sp_state_t state;
+	spinlock_t state_lock;
+} sp_context_t;
+
+/* Assembly helpers */
+uint64_t spm_secure_partition_enter(uint64_t *c_rt_ctx);
+void __dead2 spm_secure_partition_exit(uint64_t c_rt_ctx, uint64_t ret);
+
+void spm_sp_setup(sp_context_t *sp_ctx);
+
+xlat_ctx_t *spm_get_sp_xlat_context(void);
+
+int32_t spm_memory_attributes_get_smc_handler(sp_context_t *sp_ctx,
+					      uintptr_t base_va);
+int spm_memory_attributes_set_smc_handler(sp_context_t *sp_ctx,
+					  u_register_t page_address,
+					  u_register_t pages_count,
+					  u_register_t smc_attributes);
+
+#endif /* __ASSEMBLY__ */
+
+#endif /* SPM_PRIVATE_H */

services/std_svc/spm_deprecated/spm_shim_private.h

+/*
+ * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef SPM_SHIM_PRIVATE_H
+#define SPM_SHIM_PRIVATE_H
+
+#include <stdint.h>
+#include <utils_def.h>
+
+/* Assembly source */
+IMPORT_SYM(uintptr_t, spm_shim_exceptions_ptr,		SPM_SHIM_EXCEPTIONS_PTR);
+
+/* Linker symbols */
+IMPORT_SYM(uintptr_t, __SPM_SHIM_EXCEPTIONS_START__,	SPM_SHIM_EXCEPTIONS_START);
+IMPORT_SYM(uintptr_t, __SPM_SHIM_EXCEPTIONS_END__,	SPM_SHIM_EXCEPTIONS_END);
+
+/* Definitions */
+
+#define SPM_SHIM_EXCEPTIONS_SIZE	\
+	(SPM_SHIM_EXCEPTIONS_END - SPM_SHIM_EXCEPTIONS_START)
+
+#endif /* SPM_SHIM_PRIVATE_H */

services/std_svc/std_svc_setup.c

@@ -102,7 +102,7 @@ static uintptr_t std_svc_smc_handler(uint32_t smc_fid,
 		SMC_RET1(handle, ret);
 	}
 
-#if ENABLE_SPM
+#if ENABLE_SPM && SPM_DEPRECATED
 	/*
 	 * Dispatch SPM calls to SPM SMC handler and return its return
 	 * value

tools/sptool/Makefile

+#
+# Copyright (c) 2018, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+MAKE_HELPERS_DIRECTORY := ../../make_helpers/
+include ${MAKE_HELPERS_DIRECTORY}build_macros.mk
+include ${MAKE_HELPERS_DIRECTORY}build_env.mk
+
+PROJECT := sptool${BIN_EXT}
+OBJECTS := sptool.o
+V ?= 0
+
+override CPPFLAGS += -D_GNU_SOURCE -D_XOPEN_SOURCE=700
+HOSTCCFLAGS := -Wall -Werror -pedantic -std=c99
+ifeq (${DEBUG},1)
+  HOSTCCFLAGS += -g -O0 -DDEBUG
+else
+  HOSTCCFLAGS += -O2
+endif
+
+ifeq (${V},0)
+  Q := @
+else
+  Q :=
+endif
+
+INCLUDE_PATHS := -I../../include/tools_share
+
+HOSTCC ?= gcc
+
+.PHONY: all clean distclean
+
+all: ${PROJECT}
+
+${PROJECT}: ${OBJECTS} Makefile
+	@echo "  HOSTLD  $@"
+	${Q}${HOSTCC} ${OBJECTS} -o $@ ${LDLIBS}
+	@${ECHO_BLANK_LINE}
+	@echo "Built $@ successfully"
+	@${ECHO_BLANK_LINE}
+
+%.o: %.c Makefile
+	@echo "  HOSTCC  $<"
+	${Q}${HOSTCC} -c ${CPPFLAGS} ${HOSTCCFLAGS} ${INCLUDE_PATHS} $< -o $@
+
+clean:
+	$(call SHELL_DELETE_ALL, ${PROJECT} ${OBJECTS})

tools/sptool/sptool.c

+/*
+ * Copyright (c) 2018, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdarg.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "sptool.h"
+
+#define PAGE_SIZE		4096
+
+/*
+ * Linked list of entries describing entries in the secure
+ * partition package.
+ */
+struct sp_entry_info {
+	/* Location of the files in the host's RAM. */
+	void *sp_data, *rd_data;
+
+	/* Size of the files. */
+	uint64_t sp_size, rd_size;
+
+	/* Location of the binary files inside the package output file */
+	uint64_t sp_offset, rd_offset;
+
+	struct sp_entry_info *next;
+};
+
+static struct sp_entry_info *sp_info_head;
+
+static uint64_t sp_count;
+
+/* Align an address to a power-of-two boundary. */
+static unsigned int align_to(unsigned int address, unsigned int boundary)
+{
+	unsigned int mask = boundary - 1U;
+
+	if ((address & mask) != 0U)
+		return (address + boundary) & ~mask;
+	else
+		return address;
+}
+
+/* Allocate a memory area of 'size' bytes and zero it. */
+static void *xzalloc(size_t size, const char *msg)
+{
+	void *d;
+
+	d = malloc(size);
+	if (d == NULL) {
+		fprintf(stderr, "error: malloc: %s\n", msg);
+		exit(1);
+	}
+
+	memset(d, 0, size);
+
+	return d;
+}
+
+/*
+ * Write 'size' bytes from 'buf' into the specified file stream.
+ * Exit the program on error.
+ */
+static void xfwrite(void *buf, size_t size, FILE *fp)
+{
+	if (fwrite(buf, 1, size, fp) != size) {
+		fprintf(stderr, "error: Failed to write to output file.\n");
+		exit(1);
+	}
+}
+
+/*
+ * Set the file position indicator for the specified file stream.
+ * Exit the program on error.
+ */
+static void xfseek(FILE *fp, long offset, int whence)
+{
+	if (fseek(fp, offset, whence) != 0) {
+		fprintf(stderr, "error: Failed to set file to offset 0x%lx (%d).\n",
+		       offset, whence);
+		perror(NULL);
+		exit(1);
+	}
+}
+
+static void cleanup(void)
+{
+	struct sp_entry_info *sp = sp_info_head;
+
+	while (sp != NULL) {
+		struct sp_entry_info *next = sp->next;
+
+		if (sp->sp_data != NULL)
+			free(sp->sp_data);
+
+		if (sp->rd_data != NULL)
+			free(sp->rd_data);
+
+		free(sp);
+
+		sp = next;
+	}
+
+	sp_count = 0;
+	sp_info_head = NULL;
+}
+
+/*
+ * Allocate a buffer big enough to store the content of the specified file and
+ * load the file into it. Fill 'size' with the file size. Exit the program on
+ * error.
+ */
+static void load_file(const char *path, void **ptr, uint64_t *size)
+{
+	FILE *f = fopen(path, "rb");
+	if (f == NULL) {
+		fprintf(stderr, "error: %s couldn't be opened.\n", path);
+		exit(1);
+	}
+
+	xfseek(f, 0, SEEK_END);
+	*size = ftell(f);
+	if (*size == 0) {
+		fprintf(stderr, "error: Size of %s is 0\n", path);
+		exit(1);
+	}
+
+	rewind(f);
+
+	*ptr = malloc(*size);
+	if (*ptr == NULL) {
+		fprintf(stderr, "error: Not enough memory to load %s\n", path);
+		exit(1);
+	}
+
+	if (fread(*ptr, *size, 1, f) != 1) {
+		fprintf(stderr, "error: Couldn't read %s\n", path);
+		exit(1);
+	}
+
+	fclose(f);
+}
+
+static void load_sp_rd(char *path)
+{
+	char *split_mark = strstr(path, ":");
+
+	*split_mark = '\0';
+
+	char *sp_path = path;
+	char *rd_path = split_mark + 1;
+
+	struct sp_entry_info *sp;
+
+	if (sp_info_head == NULL) {
+		sp_info_head = xzalloc(sizeof(struct sp_entry_info),
+			"Failed to allocate sp_entry_info struct");
+
+		sp = sp_info_head;
+	} else {
+		sp = sp_info_head;
+
+		while (sp->next != NULL) {
+			sp = sp->next;
+		}
+
+		sp->next = xzalloc(sizeof(struct sp_entry_info),
+			"Failed to allocate sp_entry_info struct");
+
+		sp = sp->next;
+	}
+
+	load_file(sp_path, &sp->sp_data, &sp->sp_size);
+	printf("Loaded image file %s (%lu bytes)\n", sp_path, sp->sp_size);
+
+	load_file(rd_path, &sp->rd_data, &sp->rd_size);
+	printf("Loaded RD file %s (%lu bytes)\n", rd_path, sp->rd_size);
+
+	sp_count++;
+}
+
+static void output_write(const char *path)
+{
+	struct sp_entry_info *sp;
+
+	if (sp_count == 0) {
+		fprintf(stderr, "error: At least one SP must be provided.\n");
+		exit(1);
+	}
+
+	/* The layout of the structs is specified in the header file sptool.h */
+
+	printf("Writing %lu partitions to output file.\n", sp_count);
+
+	unsigned int header_size = (sizeof(struct sp_pkg_header) * 8)
+				 + (sizeof(struct sp_pkg_entry) * 8 * sp_count);
+
+	FILE *f = fopen(path, "wb");
+	if (f == NULL) {
+		fprintf(stderr, "error: Failed to open %s\n", path);
+		exit(1);
+	}
+
+	unsigned int file_ptr = align_to(header_size, PAGE_SIZE);
+
+	/* First, save all partition images aligned to page boundaries */
+
+	sp = sp_info_head;
+
+	for (uint64_t i = 0; i < sp_count; i++) {
+		xfseek(f, file_ptr, SEEK_SET);
+
+		printf("Writing image %lu to offset 0x%x (0x%lx bytes)\n",
+		       i, file_ptr, sp->sp_size);
+
+		sp->sp_offset = file_ptr;
+		xfwrite(sp->sp_data, sp->sp_size, f);
+		file_ptr = align_to(file_ptr + sp->sp_size, PAGE_SIZE);
+		sp = sp->next;
+	}
+
+	/* Now, save resource description blobs aligned to 8 bytes */
+
+	sp = sp_info_head;
+
+	for (uint64_t i = 0; i < sp_count; i++) {
+		xfseek(f, file_ptr, SEEK_SET);
+
+		printf("Writing RD blob %lu to offset 0x%x (0x%lx bytes)\n",
+		       i, file_ptr, sp->rd_size);
+
+		sp->rd_offset = file_ptr;
+		xfwrite(sp->rd_data, sp->rd_size, f);
+		file_ptr = align_to(file_ptr + sp->rd_size, 8);
+		sp = sp->next;
+	}
+
+	/* Finally, write header */
+
+	uint64_t version = 0x1;
+	uint64_t sp_num = sp_count;
+
+	xfseek(f, 0, SEEK_SET);
+
+	xfwrite(&version, sizeof(uint64_t), f);
+	xfwrite(&sp_num, sizeof(uint64_t), f);
+
+	sp = sp_info_head;
+
+	for (unsigned int i = 0; i < sp_count; i++) {
+
+		uint64_t sp_offset, sp_size, rd_offset, rd_size;
+
+		sp_offset = sp->sp_offset;
+		sp_size = align_to(sp->sp_size, PAGE_SIZE);
+		rd_offset = sp->rd_offset;
+		rd_size = sp->rd_size;
+
+		xfwrite(&sp_offset, sizeof(uint64_t), f);
+		xfwrite(&sp_size, sizeof(uint64_t), f);
+		xfwrite(&rd_offset, sizeof(uint64_t), f);
+		xfwrite(&rd_size, sizeof(uint64_t), f);
+
+		sp = sp->next;
+	}
+
+	/* All information has been written now */
+
+	fclose(f);
+}
+
+static void usage(void)
+{
+	printf("usage: sptool ");
+#ifdef VERSION
+	printf(VERSION);
+#else
+	/* If built from sptool directory, VERSION is not set. */
+	printf("version unknown");
+#endif
+	printf(" [<args>]\n\n");
+
+	printf("This tool takes as inputs several image binary files and the\n"
+	       "resource description blobs as input and generates a package\n"
+	       "file that contains them.\n\n");
+	printf("Commands supported:\n");
+	printf("  -o <path>            Set output file path.\n");
+	printf("  -i <sp_path:rd_path> Add Secure Partition image and Resource\n"
+	       "                       Description blob (specified in two paths\n"
+	       "                       separated by a colon).\n");
+	printf("  -h                   Show this message.\n");
+	exit(1);
+}
+
+int main(int argc, char *argv[])
+{
+	int ch;
+	const char *outname = NULL;
+
+	while ((ch = getopt(argc, argv, "hi:o:")) != -1) {
+		switch (ch) {
+		case 'i':
+			load_sp_rd(optarg);
+			break;
+		case 'o':
+			outname = optarg;
+			break;
+		case 'h':
+		default:
+			usage();
+		}
+	}
+
+	argc -= optind;
+	argv += optind;
+
+	if (outname == NULL) {
+		fprintf(stderr, "error: An output file path must be provided.\n\n");
+		usage();
+		return 1;
+	}
+
+	output_write(outname);
+
+	cleanup();
+
+	return 0;
+}