Implement load_image in terms of IO abstraction

The modified implementation uses the IO abstraction rather than
making direct semi-hosting calls.  The semi-hosting driver is now
registered for the FVP platform during initialisation of each boot
stage where it is used.  Additionally, the FVP platform includes a
straightforward implementation of 'plat_get_image_source' which
provides a generic means for the 'load_image' function to determine
how to access the image data.

Change-Id: Ia34457b471dbee990c7b3c79de7aee4ceea51aa6

11 files changed, 478 insertions, 44 deletions

diff --git a/common/bl_common.c b/common/bl_common.c
index d401f8c..7e17bb8 100644
--- a/common/bl_common.c
+++ b/common/bl_common.c
@@ -37,6 +37,8 @@
 #include <platform.h>
 #include <semihosting.h>
 #include <bl_common.h>
+#include "io_storage.h"
+#include "debug.h"
 
 /***********************************************************
  * Memory for sharing data while changing exception levels.
@@ -262,7 +264,7 @@ static void dump_load_info(unsigned long image_load_addr,
 }
 
 /*******************************************************************************
- * Generic function to load an image into the trusted RAM using semihosting
+ * Generic function to load an image into the trusted RAM,
  * given a name, extents of free memory & whether the image should be loaded at
  * the bottom or top of the free memory. It updates the memory layout if the
  * load is successful.
@@ -272,25 +274,49 @@ unsigned long load_image(meminfo *mem_layout,
 			 unsigned int load_type,
 			 unsigned long fixed_addr)
 {
+	io_dev_handle dev_handle;
+	io_handle image_handle;
+	void *image_spec;
 	unsigned long temp_image_base = 0;
 	unsigned long image_base = 0;
 	long offset = 0;
-	int image_flen;
+	size_t image_size = 0;
+	size_t bytes_read = 0;
+	int io_result = IO_FAIL;
+
+	assert(mem_layout != NULL);
+	assert(image_name != NULL);
+
+	/* Obtain a reference to the image by querying the platform layer */
+	io_result = plat_get_image_source(image_name, &dev_handle, &image_spec);
+	if (io_result != IO_SUCCESS) {
+		ERROR("Failed to obtain reference to image '%s' (%i)\n",
+			image_name, io_result);
+		return 0;
+	}
 
-	/* Find the size of the image */
-	image_flen = semihosting_get_flen(image_name);
-	if (image_flen < 0) {
-		printf("ERROR: Cannot access '%s' file (%i).\r\n",
-			image_name, image_flen);
+	/* Attempt to access the image */
+	io_result = io_open(dev_handle, image_spec, &image_handle);
+	if (io_result != IO_SUCCESS) {
+		ERROR("Failed to access image '%s' (%i)\n",
+			image_name, io_result);
 		return 0;
 	}
 
+	/* Find the size of the image */
+	io_result = io_size(image_handle, &image_size);
+	if ((io_result != IO_SUCCESS) || (image_size == 0)) {
+		ERROR("Failed to determine the size of the image '%s' file (%i)\n",
+			image_name, io_result);
+		goto fail;
+	}
+
 	/* See if we have enough space */
-	if (image_flen > mem_layout->free_size) {
-		printf("ERROR: Cannot load '%s' file: Not enough space.\r\n",
+	if (image_size > mem_layout->free_size) {
+		ERROR("ERROR: Cannot load '%s' file: Not enough space.\n",
 			image_name);
-		dump_load_info(0, image_flen, mem_layout);
-		return 0;
+		dump_load_info(0, image_size, mem_layout);
+		goto fail;
 	}
 
 	switch (load_type) {
@@ -299,17 +325,17 @@ unsigned long load_image(meminfo *mem_layout,
 
 	  /* Load the image in the top of free memory */
 	  temp_image_base = mem_layout->free_base + mem_layout->free_size;
-	  temp_image_base -= image_flen;
+	  temp_image_base -= image_size;
 
 	  /* Page align base address and check whether the image still fits */
 	  image_base = page_align(temp_image_base, DOWN);
 	  assert(image_base <= temp_image_base);
 
 	  if (image_base < mem_layout->free_base) {
-		  printf("ERROR: Cannot load '%s' file: Not enough space.\r\n",
-			  image_name);
-		  dump_load_info(image_base, image_flen, mem_layout);
-		  return 0;
+		ERROR("Cannot load '%s' file: Not enough space.\n",
+			image_name);
+		dump_load_info(image_base, image_size, mem_layout);
+		goto fail;
 	  }
 
 	  /* Calculate the amount of extra memory used due to alignment */
@@ -325,12 +351,12 @@ unsigned long load_image(meminfo *mem_layout,
 	  assert(image_base >= temp_image_base);
 
 	  /* Page align base address and check whether the image still fits */
-	  if (image_base + image_flen >
+	  if (image_base + image_size >
 	      mem_layout->free_base + mem_layout->free_size) {
-		  printf("ERROR: Cannot load '%s' file: Not enough space.\r\n",
+		  ERROR("Cannot load '%s' file: Not enough space.\n",
 			  image_name);
-		  dump_load_info(image_base, image_flen, mem_layout);
-		  return 0;
+		  dump_load_info(image_base, image_size, mem_layout);
+		  goto fail;
 	  }
 
 	  /* Calculate the amount of extra memory used due to alignment */
@@ -390,19 +416,19 @@ unsigned long load_image(meminfo *mem_layout,
 
 		/* Check whether the image fits. */
 		if ((image_base < mem_layout->free_base) ||
-		    (image_base + image_flen >
+		    (image_base + image_size >
 		       mem_layout->free_base + mem_layout->free_size)) {
-			printf("ERROR: Cannot load '%s' file: Not enough space.\r\n",
+			ERROR("Cannot load '%s' file: Not enough space.\n",
 				image_name);
-			dump_load_info(image_base, image_flen, mem_layout);
-			return 0;
+			dump_load_info(image_base, image_size, mem_layout);
+			goto fail;
 		}
 
 		/* Check whether the fixed load address is page-aligned. */
 		if (!is_page_aligned(image_base)) {
-			printf("ERROR: Cannot load '%s' file at unaligned address 0x%lx.\r\n",
+			ERROR("Cannot load '%s' file at unaligned address 0x%lx\n",
 				image_name, fixed_addr);
-			return 0;
+			goto fail;
 		}
 
 		/*
@@ -432,7 +458,7 @@ unsigned long load_image(meminfo *mem_layout,
 			 * Calculate the amount of wasted memory within the
 			 * amount of memory used by the image.
 			 */
-			offset = space_used - image_flen;
+			offset = space_used - image_size;
 		} else /* BOT_LOAD */
 			/*
 			 * ------------
@@ -448,13 +474,11 @@ unsigned long load_image(meminfo *mem_layout,
 	}
 
 	/* We have enough space so load the image now */
-	image_flen = semihosting_download_file(image_name,
-					       image_flen,
-					       (void *) image_base);
-	if (image_flen <= 0) {
-		printf("ERROR: Failed to load '%s' file from semihosting (%i).\r\n",
-			image_name, image_flen);
-		return 0;
+	/* TODO: Consider whether to try to recover/retry a partially successful read */
+	io_result = io_read(image_handle, (void *)image_base, image_size, &bytes_read);
+	if ((io_result != IO_SUCCESS) || (bytes_read < image_size)) {
+		ERROR("Failed to load '%s' file (%i)\n", image_name, io_result);
+		goto fail;
 	}
 
 	/*
@@ -463,15 +487,26 @@ unsigned long load_image(meminfo *mem_layout,
 	 * the next EL can see it.
 	 */
 	/* Update the memory contents */
-	flush_dcache_range(image_base, image_flen);
+	flush_dcache_range(image_base, image_size);
 
-	mem_layout->free_size -= image_flen + offset;
+	mem_layout->free_size -= image_size + offset;
 
 	/* Update the base of free memory since its moved up */
 	if (load_type == BOT_LOAD)
-		mem_layout->free_base += offset + image_flen;
+		mem_layout->free_base += offset + image_size;
+
+exit:
+	io_result = io_close(image_handle);
+	/* Ignore improbable/unrecoverable error in 'close' */
+
+	/* TODO: Consider maintaining open device connection from this bootloader stage */
+	io_result = io_dev_close(dev_handle);
+	/* Ignore improbable/unrecoverable error in 'dev_close' */
 
 	return image_base;
+
+fail:	image_base = 0;
+	goto exit;
 }
 
 /*******************************************************************************
diff --git a/drivers/io/io_semihosting.c b/drivers/io/io_semihosting.c
new file mode 100644
index 0000000..12d8315
--- /dev/null
+++ b/drivers/io/io_semihosting.c
@@ -0,0 +1,239 @@
+/*
+ * Copyright (c) 2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <assert.h>
+#include "io_storage.h"
+#include "io_driver.h"
+#include "semihosting.h"
+
+
+
+/* Identify the device type as semihosting */
+static io_type device_type_sh(void)
+{
+	return IO_TYPE_SEMIHOSTING;
+}
+
+
+/* Semi-hosting functions, device info and handle */
+
+static int sh_dev_open(void *spec, struct io_dev_info **dev_info);
+static int sh_file_open(struct io_dev_info *dev_info, const void *spec,
+		struct io_entity *entity);
+static int sh_file_seek(struct io_entity *entity, int mode, ssize_t offset);
+static int sh_file_len(struct io_entity *entity, size_t *length);
+static int sh_file_read(struct io_entity *entity, void *buffer, size_t length,
+		size_t *length_read);
+static int sh_file_write(struct io_entity *entity, const void *buffer,
+		size_t length, size_t *length_written);
+static int sh_file_close(struct io_entity *entity);
+
+static struct io_dev_connector sh_dev_connector = {
+	.dev_open = sh_dev_open
+};
+
+
+static struct io_dev_funcs sh_dev_funcs = {
+	.type = device_type_sh,
+	.open = sh_file_open,
+	.seek = sh_file_seek,
+	.size = sh_file_len,
+	.read = sh_file_read,
+	.write = sh_file_write,
+	.close = sh_file_close,
+	.dev_init = NULL,	/* NOP */
+	.dev_close = NULL,	/* NOP */
+};
+
+
+static struct io_dev_info sh_dev_info = {
+	.funcs = &sh_dev_funcs,
+	.info = (uintptr_t)NULL
+};
+
+
+/* Open a connection to the semi-hosting device */
+static int sh_dev_open(void *spec __unused, struct io_dev_info **dev_info)
+{
+	int result = IO_SUCCESS;
+	assert(dev_info != NULL);
+	*dev_info = &sh_dev_info;
+	return result;
+}
+
+
+/* Open a file on the semi-hosting device */
+static int sh_file_open(struct io_dev_info *dev_info __attribute__((unused)),
+		const void *spec, struct io_entity *entity)
+{
+	int result = IO_FAIL;
+	int sh_result = -1;
+	const io_file_spec *file_spec = (io_file_spec *)spec;
+
+	assert(file_spec != NULL);
+	assert(entity != NULL);
+
+	sh_result = semihosting_file_open(file_spec->path, file_spec->mode);
+
+	if (sh_result > 0) {
+		entity->info = sh_result;
+		result = IO_SUCCESS;
+	} else {
+		result = IO_FAIL;
+	}
+	return result;
+}
+
+
+/* Seek to a particular file offset on the semi-hosting device */
+static int sh_file_seek(struct io_entity *entity, int mode, ssize_t offset)
+{
+	int result = IO_FAIL;
+	int file_handle, sh_result;
+
+	assert(entity != NULL);
+
+	file_handle = (int)entity->info;
+
+	sh_result = semihosting_file_seek(file_handle, offset);
+
+	result = (sh_result == 0) ? IO_SUCCESS : IO_FAIL;
+
+	return result;
+}
+
+
+/* Return the size of a file on the semi-hosting device */
+static int sh_file_len(struct io_entity *entity, size_t *length)
+{
+	int result = IO_FAIL;
+
+	assert(entity != NULL);
+	assert(length != NULL);
+
+	int sh_handle = entity->info;
+	int sh_result = semihosting_file_length(sh_handle);
+
+	if (sh_result >= 0) {
+		result = IO_SUCCESS;
+		*length = (size_t)sh_result;
+	}
+
+	return result;
+}
+
+
+/* Read data from a file on the semi-hosting device */
+static int sh_file_read(struct io_entity *entity, void *buffer, size_t length,
+		size_t *length_read)
+{
+	int result = IO_FAIL;
+	int sh_result = -1;
+	int bytes = length;
+	int file_handle;
+
+	assert(entity != NULL);
+	assert(buffer != NULL);
+	assert(length_read != NULL);
+
+	file_handle = (int)entity->info;
+
+	sh_result = semihosting_file_read(file_handle, &bytes, buffer);
+
+	if (sh_result >= 0) {
+		*length_read = (bytes != length) ? bytes : length;
+		result = IO_SUCCESS;
+	} else
+		result = IO_FAIL;
+
+	return result;
+}
+
+
+/* Write data to a file on the semi-hosting device */
+static int sh_file_write(struct io_entity *entity, const void *buffer,
+		size_t length, size_t *length_written)
+{
+	int result = IO_FAIL;
+	int sh_result = -1;
+	int file_handle;
+	int bytes = length;
+
+	assert(entity != NULL);
+	assert(buffer != NULL);
+	assert(length_written != NULL);
+
+	file_handle = (int)entity->info;
+
+	sh_result = semihosting_file_write(file_handle, &bytes, buffer);
+
+	if (sh_result >= 0) {
+		*length_written = sh_result;
+		result = IO_SUCCESS;
+	} else
+		result = IO_FAIL;
+
+	return result;
+}
+
+
+/* Close a file on the semi-hosting device */
+static int sh_file_close(struct io_entity *entity)
+{
+	int result = IO_FAIL;
+	int sh_result = -1;
+	int file_handle;
+
+	assert(entity != NULL);
+
+	file_handle = (int)entity->info;
+
+	sh_result = semihosting_file_close(file_handle);
+
+	result = (sh_result >= 0) ? IO_SUCCESS : IO_FAIL;
+
+	return result;
+}
+
+
+/* Exported functions */
+
+/* Register the semi-hosting driver with the IO abstraction */
+int register_io_dev_sh(struct io_dev_connector **dev_con)
+{
+	int result = IO_FAIL;
+	assert(dev_con != NULL);
+
+	result = io_register_device(&sh_dev_info);
+	if (result == IO_SUCCESS)
+		*dev_con = &sh_dev_connector;
+
+	return result;
+}
diff --git a/drivers/io/io_semihosting.h b/drivers/io/io_semihosting.h
new file mode 100644
index 0000000..7dc632d
--- /dev/null
+++ b/drivers/io/io_semihosting.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (c) 2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __IO_SH_H__
+#define __IO_SH_H__
+
+int register_io_dev_sh(struct io_dev_connector **dev_con);
+
+#endif /* __IO_SH_H__ */
diff --git a/include/io_storage.h b/include/io_storage.h
index 71f5d26..f67e5d0 100644
--- a/include/io_storage.h
+++ b/include/io_storage.h
@@ -31,6 +31,8 @@
 #ifndef __IO_H__
 #define __IO_H__
 
+#ifndef __ASSEMBLY__
+
 #include <stdint.h>
 #include <stdio.h>	/* For ssize_t */
 
@@ -125,4 +127,5 @@ int io_write(io_handle handle, const void *buffer, size_t length,
 int io_close(io_handle handle);
 
 
+#endif /* __ASSEMBLY__ */
 #endif /* __IO_H__ */
diff --git a/include/semihosting.h b/include/semihosting.h
index d70fcba..0244cad 100644
--- a/include/semihosting.h
+++ b/include/semihosting.h
@@ -61,7 +61,7 @@ int semihosting_connection_supported(void);
 int semihosting_file_open(const char *file_name, unsigned int mode);
 int semihosting_file_seek(int file_handle, unsigned int offset);
 int semihosting_file_read(int file_handle, int *length, void *buffer);
-int semihosting_file_write(int file_handle, int *length, void *buffer);
+int semihosting_file_write(int file_handle, int *length, const void *buffer);
 int semihosting_file_close(int file_handle);
 int semihosting_file_length(int file_handle);
 int semihosting_system(char *command_line);
diff --git a/lib/semihosting/semihosting.c b/lib/semihosting/semihosting.c
index 1a6d156..1a7ac60 100644
--- a/lib/semihosting/semihosting.c
+++ b/lib/semihosting/semihosting.c
@@ -120,7 +120,7 @@ int semihosting_file_read(int file_handle, int *length, void *buffer)
 		return result;
 }
 
-int semihosting_file_write(int file_handle, int *length, void *buffer)
+int semihosting_file_write(int file_handle, int *length, const void *buffer)
 {
 	smh_file_read_write_block write_block;
 
@@ -128,7 +128,7 @@ int semihosting_file_write(int file_handle, int *length, void *buffer)
 		return -EINVAL;
 
 	write_block.handle = file_handle;
-	write_block.buffer = buffer;
+	write_block.buffer = (void *)buffer;
 	write_block.length = *length;
 
 	*length = semihosting_call(SEMIHOSTING_SYS_WRITE,
diff --git a/plat/fvp/bl1_plat_setup.c b/plat/fvp/bl1_plat_setup.c
index 428b1b3..3daa480 100644
--- a/plat/fvp/bl1_plat_setup.c
+++ b/plat/fvp/bl1_plat_setup.c
@@ -110,6 +110,9 @@ void bl1_early_platform_setup(void)
  ******************************************************************************/
 void bl1_platform_setup(void)
 {
+	/* Initialise the IO layer and register platform IO devices */
+	io_setup();
+
 	/* Enable and initialize the System level generic timer */
 	mmio_write_32(SYS_CNTCTL_BASE + CNTCR_OFF, CNTCR_EN);
 
@@ -119,6 +122,7 @@ void bl1_platform_setup(void)
 	return;
 }
 
+
 /*******************************************************************************
  * Perform the very early platform specific architecture setup here. At the
  * moment this only does basic initialization. Later architectural setup
diff --git a/plat/fvp/bl2_plat_setup.c b/plat/fvp/bl2_plat_setup.c
index 567c7d7..4efb436 100644
--- a/plat/fvp/bl2_plat_setup.c
+++ b/plat/fvp/bl2_plat_setup.c
@@ -105,6 +105,9 @@ void bl2_early_platform_setup(meminfo *mem_layout,
  ******************************************************************************/
 void bl2_platform_setup()
 {
+	/* Initialise the IO layer and register platform IO devices */
+	io_setup();
+
 	/* Use the Trusted DRAM for passing args to BL31 */
 	bl2_el_change_mem_ptr = (unsigned char **) TZDRAM_BASE;
 }
diff --git a/plat/fvp/plat_io_storage.c b/plat/fvp/plat_io_storage.c
new file mode 100644
index 0000000..e476106
--- /dev/null
+++ b/plat/fvp/plat_io_storage.c
@@ -0,0 +1,103 @@
+/*
+ * Copyright (c) 2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <assert.h>
+#include <string.h>
+#include "platform.h"
+#include "io_storage.h"
+#include "io_driver.h"
+#include "io_semihosting.h"
+#include "semihosting.h"	/* For FOPEN_MODE_... */
+#include "debug.h"
+
+
+/* IO devices */
+static struct io_plat_data io_data;
+static struct io_dev_connector *sh_dev_con;
+static void *const sh_dev_spec;
+static void *const sh_init_params;
+static io_dev_handle sh_dev_handle;
+
+static io_file_spec bl2_image_spec = {
+	.path = BL2_IMAGE_NAME,
+	.mode = FOPEN_MODE_R
+};
+
+static io_file_spec bl31_image_spec = {
+	.path = BL31_IMAGE_NAME,
+	.mode = FOPEN_MODE_R
+};
+
+
+/* Set up the IO devices present on this platform, ready for use */
+void io_setup(void)
+{
+	/* Initialise the IO layer */
+	io_init(&io_data);
+
+	/* Register a semi-hosting device */
+	int io_result = register_io_dev_sh(&sh_dev_con);
+	assert(io_result == IO_SUCCESS);
+
+	/* Open a connection to the semi-hosting device and cache the handle */
+	io_result = io_dev_open(sh_dev_con, sh_dev_spec, &sh_dev_handle);
+	assert(io_result == IO_SUCCESS);
+
+	/* Ignore improbable errors in release builds */
+	(void)io_result;
+}
+
+
+/* Return an IO device handle and specification which can be used to access
+ * an image */
+int plat_get_image_source(const char *image_name, io_dev_handle *dev_handle,
+				void **image_spec)
+{
+	int result = IO_FAIL;
+	assert((image_name != NULL) && (dev_handle != NULL) &&
+			(image_spec != NULL));
+
+	if (strcmp(BL2_IMAGE_NAME, image_name) == 0) {
+		result = io_dev_init(sh_dev_handle, sh_init_params);
+		if (result == IO_SUCCESS) {
+			*dev_handle = sh_dev_handle;
+			*(io_file_spec **)image_spec = &bl2_image_spec;
+		}
+	} else if (strcmp(BL31_IMAGE_NAME, image_name) == 0) {
+		result = io_dev_init(sh_dev_handle, sh_init_params);
+		if (result == IO_SUCCESS) {
+			*dev_handle = sh_dev_handle;
+			*(io_file_spec **)image_spec = &bl31_image_spec;
+		}
+	} else
+		assert(0);
+
+	return result;
+}
diff --git a/plat/fvp/platform.h b/plat/fvp/platform.h
index 1b0a736..ece882f 100644
--- a/plat/fvp/platform.h
+++ b/plat/fvp/platform.h
@@ -35,6 +35,7 @@
 #include <mmio.h>
 #include <psci.h>
 #include <bl_common.h>
+#include "io_storage.h"
 
 
 /*******************************************************************************
@@ -347,6 +348,11 @@ extern int plat_get_max_afflvl(void);
 extern unsigned int plat_get_aff_count(unsigned int, unsigned long);
 extern unsigned int plat_get_aff_state(unsigned int, unsigned long);
 
+/* Declarations for plat_io_storage.c */
+extern void io_setup(void);
+extern int plat_get_image_source(const char *image_name,
+		io_dev_handle *dev_handle, void **image_spec);
+
 #endif /*__ASSEMBLY__*/
 
 #endif /* __PLATFORM_H__ */
diff --git a/plat/fvp/platform.mk b/plat/fvp/platform.mk
index 5da2acd..2efc7bc 100644
--- a/plat/fvp/platform.mk
+++ b/plat/fvp/platform.mk
@@ -35,14 +35,16 @@ PLAT_INCLUDES		:=	-Idrivers/arm/interconnect/cci-400	\
 PLAT_BL1_C_VPATH	:=	drivers/arm/interconnect/cci-400	\
 				drivers/arm/peripherals/pl011		\
 				lib/semihosting				\
-				lib/stdlib
+				lib/stdlib				\
+				drivers/io
 
 PLAT_BL1_S_VPATH	:=	lib/semihosting/${ARCH}
 
 PLAT_BL2_C_VPATH	:=	drivers/arm/interconnect/cci-400	\
 				drivers/arm/peripherals/pl011		\
 				lib/stdlib				\
-				lib/semihosting
+				lib/semihosting				\
+				drivers/io
 
 PLAT_BL2_S_VPATH	:=	lib/semihosting/${ARCH}
 
@@ -50,7 +52,8 @@ PLAT_BL31_C_VPATH	:=	drivers/arm/interconnect/cci-400	\
 				drivers/arm/peripherals/pl011		\
 				lib/semihosting				\
 				lib/stdlib				\
-				drivers/power
+				drivers/power				\
+				drivers/io
 
 PLAT_BL31_S_VPATH	:=	lib/semihosting/${ARCH}
 
@@ -58,7 +61,9 @@ PLAT_BL_COMMON_OBJS	:=	semihosting_call.o			\
 				mmio.o					\
 				pl011.o					\
 				semihosting.o				\
-				sysreg_helpers.o
+				sysreg_helpers.o			\
+				plat_io_storage.o			\
+				io_semihosting.o
 
 BL1_OBJS		+=	bl1_plat_setup.o			\
 				bl1_plat_helpers.o			\