feat(plat/st): use FIP to load images

BL2 still uses the STM32 header binary format to be loaded from ROM code.
BL32 and BL33 and their respective device tree files are now put together
in a FIP file.
One DTB is created for each BL. To reduce their sizes, 2 new dtsi file are
in charge of removing useless nodes for a given BL. This is done because
BL2 and BL32 share the same device tree files base.

The previous way of booting is still available, the compilation flag
STM32MP_USE_STM32IMAGE has to be set to 1 in the make command. Some files
are duplicated and their names modified with _stm32_ to avoid too much
switches in the code.

Change-Id: I1ffada0af58486d4cf6044511b51e56b52269817
Signed-off-by: Lionel Debieve <lionel.debieve@st.com>
Signed-off-by: Yann Gautier <yann.gautier@foss.st.com>
diff --git a/plat/st/common/bl2_io_storage.c b/plat/st/common/bl2_io_storage.c
index e603267..01c289d 100644
--- a/plat/st/common/bl2_io_storage.c
+++ b/plat/st/common/bl2_io_storage.c
@@ -7,13 +7,11 @@
 #include <assert.h>
 #include <string.h>
 
-#include <platform_def.h>
-
 #include <arch_helpers.h>
 #include <common/debug.h>
 #include <drivers/io/io_block.h>
 #include <drivers/io/io_driver.h>
-#include <drivers/io/io_dummy.h>
+#include <drivers/io/io_fip.h>
 #include <drivers/io/io_mtd.h>
 #include <drivers/io/io_storage.h>
 #include <drivers/mmc.h>
@@ -22,29 +20,27 @@
 #include <drivers/spi_nand.h>
 #include <drivers/spi_nor.h>
 #include <drivers/st/io_mmc.h>
-#include <drivers/st/io_stm32image.h>
 #include <drivers/st/stm32_fmc2_nand.h>
 #include <drivers/st/stm32_qspi.h>
 #include <drivers/st/stm32_sdmmc2.h>
 #include <lib/mmio.h>
 #include <lib/utils.h>
 #include <plat/common/platform.h>
+#include <tools_share/firmware_image_package.h>
+
+#include <platform_def.h>
 
 /* IO devices */
-#ifndef AARCH32_SP_OPTEE
-static const io_dev_connector_t *dummy_dev_con;
-static uintptr_t dummy_dev_handle;
-static uintptr_t dummy_dev_spec;
-#endif
+uintptr_t fip_dev_handle;
+uintptr_t storage_dev_handle;
 
-static uintptr_t image_dev_handle;
-static uintptr_t storage_dev_handle;
+static const io_dev_connector_t *fip_dev_con;
 
 #if STM32MP_SDMMC || STM32MP_EMMC
 static struct mmc_device_info mmc_info;
 static io_block_spec_t gpt_block_spec = {
-	.offset = 0,
-	.length = 34 * MMC_BLOCK_SIZE, /* Size of GPT table */
+	.offset = 0U,
+	.length = 34U * MMC_BLOCK_SIZE, /* Size of GPT table */
 };
 
 static uint32_t block_buffer[MMC_BLOCK_SIZE] __aligned(MMC_BLOCK_SIZE);
@@ -61,30 +57,6 @@
 	},
 	.block_size = MMC_BLOCK_SIZE,
 };
-
-#if STM32MP_EMMC_BOOT
-static io_block_spec_t emmc_boot_ssbl_block_spec = {
-	.offset = PLAT_EMMC_BOOT_SSBL_OFFSET,
-	.length = MMC_BLOCK_SIZE, /* We are interested only in first 4 bytes */
-};
-
-static const io_block_dev_spec_t mmc_block_dev_boot_part_spec = {
-	/* It's used as temp buffer in block driver */
-	.buffer = {
-		.offset = (size_t)&block_buffer,
-		.length = MMC_BLOCK_SIZE,
-	},
-	.ops = {
-		.read = mmc_boot_part_read_blocks,
-		.write = NULL,
-	},
-	.block_size = MMC_BLOCK_SIZE,
-};
-#endif
-
-static struct io_mmc_dev_spec mmc_device_spec = {
-	.use_boot_part = false,
-};
 
 static const io_dev_connector_t *mmc_dev_con;
 #endif /* STM32MP_SDMMC || STM32MP_EMMC */
@@ -103,6 +75,7 @@
 	.ops = {
 		.init = nand_raw_init,
 		.read = nand_read,
+		.seek = nand_seek_bb
 	},
 };
 
@@ -114,6 +87,7 @@
 	.ops = {
 		.init = spi_nand_init,
 		.read = nand_read,
+		.seek = nand_seek_bb
 	},
 };
 #endif
@@ -122,76 +96,42 @@
 static const io_dev_connector_t *spi_dev_con;
 #endif
 
-#ifdef AARCH32_SP_OPTEE
-static const struct stm32image_part_info optee_header_partition_spec = {
-	.name = OPTEE_HEADER_IMAGE_NAME,
-	.binary_type = OPTEE_HEADER_BINARY_TYPE,
+static const io_uuid_spec_t bl33_partition_spec = {
+	.uuid = UUID_NON_TRUSTED_FIRMWARE_BL33
 };
 
-static const struct stm32image_part_info optee_core_partition_spec = {
-	.name = OPTEE_CORE_IMAGE_NAME,
-	.binary_type = OPTEE_CORE_BINARY_TYPE,
-};
-
-static const struct stm32image_part_info optee_paged_partition_spec = {
-	.name = OPTEE_PAGED_IMAGE_NAME,
-	.binary_type = OPTEE_PAGED_BINARY_TYPE,
-};
-#else
-static const io_block_spec_t bl32_block_spec = {
-	.offset = BL32_BASE,
-	.length = STM32MP_BL32_SIZE
+static const io_uuid_spec_t tos_fw_config_uuid_spec = {
+	.uuid = UUID_TOS_FW_CONFIG,
 };
-#endif
 
-static const struct stm32image_part_info bl33_partition_spec = {
-	.name = BL33_IMAGE_NAME,
-	.binary_type = BL33_BINARY_TYPE,
+static const io_uuid_spec_t hw_config_uuid_spec = {
+	.uuid = UUID_HW_CONFIG,
 };
 
-enum {
-	IMG_IDX_BL33,
 #ifdef AARCH32_SP_OPTEE
-	IMG_IDX_OPTEE_HEADER,
-	IMG_IDX_OPTEE_CORE,
-	IMG_IDX_OPTEE_PAGED,
-#endif
-	IMG_IDX_NUM
+static const io_uuid_spec_t optee_header_partition_spec = {
+	.uuid = UUID_SECURE_PAYLOAD_BL32
 };
 
-static struct stm32image_device_info stm32image_dev_info_spec __unused = {
-	.lba_size = MMC_BLOCK_SIZE,
-	.part_info[IMG_IDX_BL33] = {
-		.name = BL33_IMAGE_NAME,
-		.binary_type = BL33_BINARY_TYPE,
-	},
-#ifdef AARCH32_SP_OPTEE
-	.part_info[IMG_IDX_OPTEE_HEADER] = {
-		.name = OPTEE_HEADER_IMAGE_NAME,
-		.binary_type = OPTEE_HEADER_BINARY_TYPE,
-	},
-	.part_info[IMG_IDX_OPTEE_CORE] = {
-		.name = OPTEE_CORE_IMAGE_NAME,
-		.binary_type = OPTEE_CORE_BINARY_TYPE,
-	},
-	.part_info[IMG_IDX_OPTEE_PAGED] = {
-		.name = OPTEE_PAGED_IMAGE_NAME,
-		.binary_type = OPTEE_PAGED_BINARY_TYPE,
-	},
-#endif
+static const io_uuid_spec_t optee_core_partition_spec = {
+	.uuid = UUID_SECURE_PAYLOAD_BL32_EXTRA1
 };
 
-static io_block_spec_t stm32image_block_spec = {
-	.offset = 0,
-	.length = 0,
+static const io_uuid_spec_t optee_paged_partition_spec = {
+	.uuid = UUID_SECURE_PAYLOAD_BL32_EXTRA2
 };
+#else
+static const io_uuid_spec_t bl32_partition_spec = {
+	.uuid = UUID_SECURE_PAYLOAD_BL32
+};
+#endif
 
-static const io_dev_connector_t *stm32image_dev_con __unused;
+static io_block_spec_t image_block_spec = {
+	.offset = 0U,
+	.length = 0U,
+};
 
-#ifndef AARCH32_SP_OPTEE
-static int open_dummy(const uintptr_t spec);
-#endif
-static int open_image(const uintptr_t spec);
+static int open_fip(const uintptr_t spec);
 static int open_storage(const uintptr_t spec);
 
 struct plat_io_policy {
@@ -201,33 +141,48 @@
 };
 
 static const struct plat_io_policy policies[] = {
+	[FIP_IMAGE_ID] = {
+		.dev_handle = &storage_dev_handle,
+		.image_spec = (uintptr_t)&image_block_spec,
+		.check = open_storage
+	},
 #ifdef AARCH32_SP_OPTEE
 	[BL32_IMAGE_ID] = {
-		.dev_handle = &image_dev_handle,
+		.dev_handle = &fip_dev_handle,
 		.image_spec = (uintptr_t)&optee_header_partition_spec,
-		.check = open_image
+		.check = open_fip
 	},
 	[BL32_EXTRA1_IMAGE_ID] = {
-		.dev_handle = &image_dev_handle,
+		.dev_handle = &fip_dev_handle,
 		.image_spec = (uintptr_t)&optee_core_partition_spec,
-		.check = open_image
+		.check = open_fip
 	},
 	[BL32_EXTRA2_IMAGE_ID] = {
-		.dev_handle = &image_dev_handle,
+		.dev_handle = &fip_dev_handle,
 		.image_spec = (uintptr_t)&optee_paged_partition_spec,
-		.check = open_image
+		.check = open_fip
 	},
 #else
 	[BL32_IMAGE_ID] = {
-		.dev_handle = &dummy_dev_handle,
-		.image_spec = (uintptr_t)&bl32_block_spec,
-		.check = open_dummy
+		.dev_handle = &fip_dev_handle,
+		.image_spec = (uintptr_t)&bl32_partition_spec,
+		.check = open_fip
 	},
 #endif
 	[BL33_IMAGE_ID] = {
-		.dev_handle = &image_dev_handle,
+		.dev_handle = &fip_dev_handle,
 		.image_spec = (uintptr_t)&bl33_partition_spec,
-		.check = open_image
+		.check = open_fip
+	},
+	[TOS_FW_CONFIG_ID] = {
+		.dev_handle = &fip_dev_handle,
+		.image_spec = (uintptr_t)&tos_fw_config_uuid_spec,
+		.check = open_fip
+	},
+	[HW_CONFIG_ID] = {
+		.dev_handle = &fip_dev_handle,
+		.image_spec = (uintptr_t)&hw_config_uuid_spec,
+		.check = open_fip
 	},
 #if STM32MP_SDMMC || STM32MP_EMMC
 	[GPT_IMAGE_ID] = {
@@ -236,61 +191,17 @@
 		.check = open_storage
 	},
 #endif
-	[STM32_IMAGE_ID] = {
-		.dev_handle = &storage_dev_handle,
-		.image_spec = (uintptr_t)&stm32image_block_spec,
-		.check = open_storage
-	}
 };
 
-#ifndef AARCH32_SP_OPTEE
-static int open_dummy(const uintptr_t spec)
+static int open_fip(const uintptr_t spec)
 {
-	return io_dev_init(dummy_dev_handle, 0);
+	return io_dev_init(fip_dev_handle, (uintptr_t)FIP_IMAGE_ID);
 }
-#endif
-
-static int open_image(const uintptr_t spec)
-{
-	return io_dev_init(image_dev_handle, 0);
-}
 
 static int open_storage(const uintptr_t spec)
 {
 	return io_dev_init(storage_dev_handle, 0);
 }
-
-#if STM32MP_EMMC_BOOT
-static uint32_t get_boot_part_ssbl_header(void)
-{
-	uint32_t magic = 0;
-	int io_result;
-	size_t bytes_read;
-
-	io_result = register_io_dev_block(&mmc_dev_con);
-	if (io_result != 0) {
-		panic();
-	}
-
-	io_result = io_dev_open(mmc_dev_con, (uintptr_t)&mmc_block_dev_boot_part_spec,
-				&storage_dev_handle);
-	assert(io_result == 0);
-
-	io_result = io_open(storage_dev_handle, (uintptr_t) &emmc_boot_ssbl_block_spec,
-			    &image_dev_handle);
-	assert(io_result == 0);
-
-	io_result = io_read(image_dev_handle, (uintptr_t) &magic, sizeof(magic),
-			    &bytes_read);
-	assert(io_result == 0);
-	assert(bytes_read == sizeof(magic));
-
-	io_result = io_dev_close(storage_dev_handle);
-	assert(io_result == 0);
-
-	return magic;
-}
-#endif
 
 static void print_boot_device(boot_api_context_t *boot_context)
 {
@@ -311,7 +222,8 @@
 		INFO("Using SPI NAND\n");
 		break;
 	default:
-		ERROR("Boot interface not found\n");
+		ERROR("Boot interface %u not found\n",
+		      boot_context->boot_interface_selected);
 		panic();
 		break;
 	}
@@ -321,29 +233,12 @@
 	}
 }
 
-static void stm32image_io_setup(void)
-{
-	int io_result __unused;
-
-	io_result = register_io_dev_stm32image(&stm32image_dev_con);
-	assert(io_result == 0);
-
-	io_result = io_dev_open(stm32image_dev_con,
-				(uintptr_t)&stm32image_dev_info_spec,
-				&image_dev_handle);
-	assert(io_result == 0);
-}
-
 #if STM32MP_SDMMC || STM32MP_EMMC
 static void boot_mmc(enum mmc_device_type mmc_dev_type,
 		     uint16_t boot_interface_instance)
 {
 	int io_result __unused;
-	uint8_t idx;
-	struct stm32image_part_info *part;
 	struct stm32_sdmmc2_params params;
-	const partition_entry_t *entry __unused;
-	uint32_t magic __unused;
 
 	zeromem(&params, sizeof(struct stm32_sdmmc2_params));
 
@@ -374,26 +269,6 @@
 		ERROR("SDMMC%u init failed\n", boot_interface_instance);
 		panic();
 	}
-
-	stm32image_dev_info_spec.device_size =
-		stm32_sdmmc2_mmc_get_device_size();
-
-#if STM32MP_EMMC_BOOT
-	magic = get_boot_part_ssbl_header();
-
-	if (magic == BOOT_API_IMAGE_HEADER_MAGIC_NB) {
-		VERBOSE("%s, header found, jump to emmc load\n", __func__);
-		idx = IMG_IDX_BL33;
-		part = &stm32image_dev_info_spec.part_info[idx];
-		part->part_offset = PLAT_EMMC_BOOT_SSBL_OFFSET;
-		part->bkp_offset = 0U;
-		mmc_device_spec.use_boot_part = true;
-
-		goto emmc_boot;
-	} else {
-		WARN("%s: Can't find STM32 header on a boot partition\n", __func__);
-	}
-#endif
 
 	/* Open MMC as a block device to read GPT table */
 	io_result = register_io_dev_block(&mmc_dev_con);
@@ -404,37 +279,6 @@
 	io_result = io_dev_open(mmc_dev_con, (uintptr_t)&mmc_block_dev_spec,
 				&storage_dev_handle);
 	assert(io_result == 0);
-
-	partition_init(GPT_IMAGE_ID);
-
-	io_result = io_dev_close(storage_dev_handle);
-	assert(io_result == 0);
-
-	for (idx = 0U; idx < IMG_IDX_NUM; idx++) {
-		part = &stm32image_dev_info_spec.part_info[idx];
-		entry = get_partition_entry(part->name);
-		if (entry == NULL) {
-			ERROR("Partition %s not found\n", part->name);
-			panic();
-		}
-
-		part->part_offset = entry->start;
-		part->bkp_offset = 0U;
-	}
-
-#if STM32MP_EMMC_BOOT
-emmc_boot:
-#endif
-	/*
-	 * Re-open MMC with io_mmc, for better perfs compared to
-	 * io_block.
-	 */
-	io_result = register_io_dev_mmc(&mmc_dev_con);
-	assert(io_result == 0);
-
-	io_result = io_dev_open(mmc_dev_con, (uintptr_t)&mmc_device_spec,
-				&storage_dev_handle);
-	assert(io_result == 0);
 }
 #endif /* STM32MP_SDMMC || STM32MP_EMMC */
 
@@ -442,8 +286,6 @@
 static void boot_spi_nor(boot_api_context_t *boot_context)
 {
 	int io_result __unused;
-	uint8_t idx;
-	struct stm32image_part_info *part;
 
 	io_result = stm32_qspi_init();
 	assert(io_result == 0);
@@ -456,30 +298,6 @@
 				(uintptr_t)&spi_nor_dev_spec,
 				&storage_dev_handle);
 	assert(io_result == 0);
-
-	stm32image_dev_info_spec.device_size = spi_nor_dev_spec.device_size;
-
-	idx = IMG_IDX_BL33;
-	part = &stm32image_dev_info_spec.part_info[idx];
-	part->part_offset = STM32MP_NOR_BL33_OFFSET;
-	part->bkp_offset = 0U;
-
-#ifdef AARCH32_SP_OPTEE
-	idx = IMG_IDX_OPTEE_HEADER;
-	part = &stm32image_dev_info_spec.part_info[idx];
-	part->part_offset = STM32MP_NOR_TEEH_OFFSET;
-	part->bkp_offset = 0U;
-
-	idx = IMG_IDX_OPTEE_PAGED;
-	part = &stm32image_dev_info_spec.part_info[idx];
-	part->part_offset = STM32MP_NOR_TEED_OFFSET;
-	part->bkp_offset = 0U;
-
-	idx = IMG_IDX_OPTEE_CORE;
-	part = &stm32image_dev_info_spec.part_info[idx];
-	part->part_offset = STM32MP_NOR_TEEX_OFFSET;
-	part->bkp_offset = 0U;
-#endif
 }
 #endif /* STM32MP_SPI_NOR */
 
@@ -487,8 +305,6 @@
 static void boot_fmc2_nand(boot_api_context_t *boot_context)
 {
 	int io_result __unused;
-	uint8_t idx;
-	struct stm32image_part_info *part;
 
 	io_result = stm32_fmc2_init();
 	assert(io_result == 0);
@@ -501,30 +317,6 @@
 	io_result = io_dev_open(nand_dev_con, (uintptr_t)&nand_dev_spec,
 				&storage_dev_handle);
 	assert(io_result == 0);
-
-	stm32image_dev_info_spec.device_size = nand_dev_spec.device_size;
-
-	idx = IMG_IDX_BL33;
-	part = &stm32image_dev_info_spec.part_info[idx];
-	part->part_offset = STM32MP_NAND_BL33_OFFSET;
-	part->bkp_offset = nand_dev_spec.erase_size;
-
-#ifdef AARCH32_SP_OPTEE
-	idx = IMG_IDX_OPTEE_HEADER;
-	part = &stm32image_dev_info_spec.part_info[idx];
-	part->part_offset = STM32MP_NAND_TEEH_OFFSET;
-	part->bkp_offset = nand_dev_spec.erase_size;
-
-	idx = IMG_IDX_OPTEE_PAGED;
-	part = &stm32image_dev_info_spec.part_info[idx];
-	part->part_offset = STM32MP_NAND_TEED_OFFSET;
-	part->bkp_offset = nand_dev_spec.erase_size;
-
-	idx = IMG_IDX_OPTEE_CORE;
-	part = &stm32image_dev_info_spec.part_info[idx];
-	part->part_offset = STM32MP_NAND_TEEX_OFFSET;
-	part->bkp_offset = nand_dev_spec.erase_size;
-#endif
 }
 #endif /* STM32MP_RAW_NAND */
 
@@ -532,8 +324,6 @@
 static void boot_spi_nand(boot_api_context_t *boot_context)
 {
 	int io_result __unused;
-	uint8_t idx;
-	struct stm32image_part_info *part;
 
 	io_result = stm32_qspi_init();
 	assert(io_result == 0);
@@ -546,31 +336,6 @@
 				(uintptr_t)&spi_nand_dev_spec,
 				&storage_dev_handle);
 	assert(io_result == 0);
-
-	stm32image_dev_info_spec.device_size =
-		spi_nand_dev_spec.device_size;
-
-	idx = IMG_IDX_BL33;
-	part = &stm32image_dev_info_spec.part_info[idx];
-	part->part_offset = STM32MP_NAND_BL33_OFFSET;
-	part->bkp_offset = spi_nand_dev_spec.erase_size;
-
-#ifdef AARCH32_SP_OPTEE
-	idx = IMG_IDX_OPTEE_HEADER;
-	part = &stm32image_dev_info_spec.part_info[idx];
-	part->part_offset = STM32MP_NAND_TEEH_OFFSET;
-	part->bkp_offset = spi_nand_dev_spec.erase_size;
-
-	idx = IMG_IDX_OPTEE_PAGED;
-	part = &stm32image_dev_info_spec.part_info[idx];
-	part->part_offset = STM32MP_NAND_TEED_OFFSET;
-	part->bkp_offset = spi_nand_dev_spec.erase_size;
-
-	idx = IMG_IDX_OPTEE_CORE;
-	part = &stm32image_dev_info_spec.part_info[idx];
-	part->part_offset = STM32MP_NAND_TEEX_OFFSET;
-	part->bkp_offset = spi_nand_dev_spec.erase_size;
-#endif
 }
 #endif /* STM32MP_SPI_NAND */
 
@@ -584,53 +349,45 @@
 
 	if ((boot_context->boot_partition_used_toboot == 1U) ||
 	    (boot_context->boot_partition_used_toboot == 2U)) {
-		INFO("Boot used partition fsbl%d\n",
+		INFO("Boot used partition fsbl%u\n",
 		     boot_context->boot_partition_used_toboot);
 	}
 
-#ifndef AARCH32_SP_OPTEE
-	io_result = register_io_dev_dummy(&dummy_dev_con);
+	io_result = register_io_dev_fip(&fip_dev_con);
 	assert(io_result == 0);
 
-	io_result = io_dev_open(dummy_dev_con, dummy_dev_spec,
-				&dummy_dev_handle);
-	assert(io_result == 0);
-#endif
+	io_result = io_dev_open(fip_dev_con, (uintptr_t)NULL,
+				&fip_dev_handle);
 
 	switch (boot_context->boot_interface_selected) {
 #if STM32MP_SDMMC
 	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_SD:
 		dmbsy();
 		boot_mmc(MMC_IS_SD, boot_context->boot_interface_instance);
-		stm32image_io_setup();
 		break;
 #endif
 #if STM32MP_EMMC
 	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_EMMC:
 		dmbsy();
 		boot_mmc(MMC_IS_EMMC, boot_context->boot_interface_instance);
-		stm32image_io_setup();
 		break;
 #endif
 #if STM32MP_SPI_NOR
 	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NOR_QSPI:
 		dmbsy();
 		boot_spi_nor(boot_context);
-		stm32image_io_setup();
 		break;
 #endif
 #if STM32MP_RAW_NAND
 	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NAND_FMC:
 		dmbsy();
 		boot_fmc2_nand(boot_context);
-		stm32image_io_setup();
 		break;
 #endif
 #if STM32MP_SPI_NAND
 	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NAND_QSPI:
 		dmbsy();
 		boot_spi_nand(boot_context);
-		stm32image_io_setup();
 		break;
 #endif
 
@@ -639,7 +396,61 @@
 		      boot_context->boot_interface_selected);
 		panic();
 		break;
+	}
+}
+
+int bl2_plat_handle_pre_image_load(unsigned int image_id)
+{
+	static bool gpt_init_done __unused;
+	uint16_t boot_itf = stm32mp_get_boot_itf_selected();
+
+	switch (boot_itf) {
+#if STM32MP_SDMMC || STM32MP_EMMC
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_SD:
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_EMMC:
+		if (!gpt_init_done) {
+			const partition_entry_t *entry;
+
+			partition_init(GPT_IMAGE_ID);
+			entry = get_partition_entry(FIP_IMAGE_NAME);
+			if (entry == NULL) {
+				ERROR("Could NOT find the %s partition!\n",
+				      FIP_IMAGE_NAME);
+				return -ENOENT;
+			}
+
+			image_block_spec.offset = entry->start;
+			image_block_spec.length = entry->length;
+
+			gpt_init_done = true;
+		}
+
+		break;
+#endif
+
+#if STM32MP_RAW_NAND || STM32MP_SPI_NAND
+#if STM32MP_RAW_NAND
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NAND_FMC:
+#endif
+#if STM32MP_SPI_NAND
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NAND_QSPI:
+#endif
+		image_block_spec.offset = STM32MP_NAND_FIP_OFFSET;
+		break;
+#endif
+
+#if STM32MP_SPI_NOR
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NOR_QSPI:
+		image_block_spec.offset = STM32MP_NOR_FIP_OFFSET;
+		break;
+#endif
+
+	default:
+		ERROR("FIP Not found\n");
+		panic();
 	}
+
+	return 0;
 }
 
 /*
diff --git a/plat/st/common/bl2_stm32_io_storage.c b/plat/st/common/bl2_stm32_io_storage.c
new file mode 100644
index 0000000..2d68a50
--- /dev/null
+++ b/plat/st/common/bl2_stm32_io_storage.c
@@ -0,0 +1,665 @@
+/*
+ * Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/io/io_block.h>
+#include <drivers/io/io_driver.h>
+#include <drivers/io/io_dummy.h>
+#include <drivers/io/io_mtd.h>
+#include <drivers/io/io_storage.h>
+#include <drivers/mmc.h>
+#include <drivers/partition/partition.h>
+#include <drivers/raw_nand.h>
+#include <drivers/spi_nand.h>
+#include <drivers/spi_nor.h>
+#include <drivers/st/io_mmc.h>
+#include <drivers/st/io_stm32image.h>
+#include <drivers/st/stm32_fmc2_nand.h>
+#include <drivers/st/stm32_qspi.h>
+#include <drivers/st/stm32_sdmmc2.h>
+#include <lib/mmio.h>
+#include <lib/utils.h>
+#include <plat/common/platform.h>
+
+#include <platform_def.h>
+
+/* IO devices */
+#ifndef AARCH32_SP_OPTEE
+static const io_dev_connector_t *dummy_dev_con;
+static uintptr_t dummy_dev_handle;
+static uintptr_t dummy_dev_spec;
+#endif
+
+static uintptr_t image_dev_handle;
+static uintptr_t storage_dev_handle;
+
+#if STM32MP_SDMMC || STM32MP_EMMC
+static struct mmc_device_info mmc_info;
+static io_block_spec_t gpt_block_spec = {
+	.offset = 0U,
+	.length = 34U * MMC_BLOCK_SIZE, /* Size of GPT table */
+};
+
+static uint32_t block_buffer[MMC_BLOCK_SIZE] __aligned(MMC_BLOCK_SIZE);
+
+static const io_block_dev_spec_t mmc_block_dev_spec = {
+	/* It's used as temp buffer in block driver */
+	.buffer = {
+		.offset = (size_t)&block_buffer,
+		.length = MMC_BLOCK_SIZE,
+	},
+	.ops = {
+		.read = mmc_read_blocks,
+		.write = NULL,
+	},
+	.block_size = MMC_BLOCK_SIZE,
+};
+
+#if STM32MP_EMMC_BOOT
+static io_block_spec_t emmc_boot_ssbl_block_spec = {
+	.offset = PLAT_EMMC_BOOT_SSBL_OFFSET,
+	.length = MMC_BLOCK_SIZE, /* We are interested only in first 4 bytes */
+};
+
+static const io_block_dev_spec_t mmc_block_dev_boot_part_spec = {
+	/* It's used as temp buffer in block driver */
+	.buffer = {
+		.offset = (size_t)&block_buffer,
+		.length = MMC_BLOCK_SIZE,
+	},
+	.ops = {
+		.read = mmc_boot_part_read_blocks,
+		.write = NULL,
+	},
+	.block_size = MMC_BLOCK_SIZE,
+};
+#endif
+
+static struct io_mmc_dev_spec mmc_device_spec = {
+	.use_boot_part = false,
+};
+
+static const io_dev_connector_t *mmc_dev_con;
+#endif /* STM32MP_SDMMC || STM32MP_EMMC */
+
+#if STM32MP_SPI_NOR
+static io_mtd_dev_spec_t spi_nor_dev_spec = {
+	.ops = {
+		.init = spi_nor_init,
+		.read = spi_nor_read,
+	},
+};
+#endif
+
+#if STM32MP_RAW_NAND
+static io_mtd_dev_spec_t nand_dev_spec = {
+	.ops = {
+		.init = nand_raw_init,
+		.read = nand_read,
+	},
+};
+
+static const io_dev_connector_t *nand_dev_con;
+#endif
+
+#if STM32MP_SPI_NAND
+static io_mtd_dev_spec_t spi_nand_dev_spec = {
+	.ops = {
+		.init = spi_nand_init,
+		.read = nand_read,
+	},
+};
+#endif
+
+#if STM32MP_SPI_NAND || STM32MP_SPI_NOR
+static const io_dev_connector_t *spi_dev_con;
+#endif
+
+#ifdef AARCH32_SP_OPTEE
+static const struct stm32image_part_info optee_header_partition_spec = {
+	.name = OPTEE_HEADER_IMAGE_NAME,
+	.binary_type = OPTEE_HEADER_BINARY_TYPE,
+};
+
+static const struct stm32image_part_info optee_core_partition_spec = {
+	.name = OPTEE_CORE_IMAGE_NAME,
+	.binary_type = OPTEE_CORE_BINARY_TYPE,
+};
+
+static const struct stm32image_part_info optee_paged_partition_spec = {
+	.name = OPTEE_PAGED_IMAGE_NAME,
+	.binary_type = OPTEE_PAGED_BINARY_TYPE,
+};
+#else
+static const io_block_spec_t bl32_block_spec = {
+	.offset = BL32_BASE,
+	.length = STM32MP_BL32_SIZE
+};
+#endif
+
+static const struct stm32image_part_info bl33_partition_spec = {
+	.name = BL33_IMAGE_NAME,
+	.binary_type = BL33_BINARY_TYPE,
+};
+
+enum {
+	IMG_IDX_BL33,
+#ifdef AARCH32_SP_OPTEE
+	IMG_IDX_OPTEE_HEADER,
+	IMG_IDX_OPTEE_CORE,
+	IMG_IDX_OPTEE_PAGED,
+#endif
+	IMG_IDX_NUM
+};
+
+static struct stm32image_device_info stm32image_dev_info_spec __unused = {
+	.lba_size = MMC_BLOCK_SIZE,
+	.part_info[IMG_IDX_BL33] = {
+		.name = BL33_IMAGE_NAME,
+		.binary_type = BL33_BINARY_TYPE,
+	},
+#ifdef AARCH32_SP_OPTEE
+	.part_info[IMG_IDX_OPTEE_HEADER] = {
+		.name = OPTEE_HEADER_IMAGE_NAME,
+		.binary_type = OPTEE_HEADER_BINARY_TYPE,
+	},
+	.part_info[IMG_IDX_OPTEE_CORE] = {
+		.name = OPTEE_CORE_IMAGE_NAME,
+		.binary_type = OPTEE_CORE_BINARY_TYPE,
+	},
+	.part_info[IMG_IDX_OPTEE_PAGED] = {
+		.name = OPTEE_PAGED_IMAGE_NAME,
+		.binary_type = OPTEE_PAGED_BINARY_TYPE,
+	},
+#endif
+};
+
+static io_block_spec_t stm32image_block_spec = {
+	.offset = 0U,
+	.length = 0U,
+};
+
+static const io_dev_connector_t *stm32image_dev_con __unused;
+
+#ifndef AARCH32_SP_OPTEE
+static int open_dummy(const uintptr_t spec);
+#endif
+static int open_image(const uintptr_t spec);
+static int open_storage(const uintptr_t spec);
+
+struct plat_io_policy {
+	uintptr_t *dev_handle;
+	uintptr_t image_spec;
+	int (*check)(const uintptr_t spec);
+};
+
+static const struct plat_io_policy policies[] = {
+#ifdef AARCH32_SP_OPTEE
+	[BL32_IMAGE_ID] = {
+		.dev_handle = &image_dev_handle,
+		.image_spec = (uintptr_t)&optee_header_partition_spec,
+		.check = open_image
+	},
+	[BL32_EXTRA1_IMAGE_ID] = {
+		.dev_handle = &image_dev_handle,
+		.image_spec = (uintptr_t)&optee_core_partition_spec,
+		.check = open_image
+	},
+	[BL32_EXTRA2_IMAGE_ID] = {
+		.dev_handle = &image_dev_handle,
+		.image_spec = (uintptr_t)&optee_paged_partition_spec,
+		.check = open_image
+	},
+#else
+	[BL32_IMAGE_ID] = {
+		.dev_handle = &dummy_dev_handle,
+		.image_spec = (uintptr_t)&bl32_block_spec,
+		.check = open_dummy
+	},
+#endif
+	[BL33_IMAGE_ID] = {
+		.dev_handle = &image_dev_handle,
+		.image_spec = (uintptr_t)&bl33_partition_spec,
+		.check = open_image
+	},
+#if STM32MP_SDMMC || STM32MP_EMMC
+	[GPT_IMAGE_ID] = {
+		.dev_handle = &storage_dev_handle,
+		.image_spec = (uintptr_t)&gpt_block_spec,
+		.check = open_storage
+	},
+#endif
+	[STM32_IMAGE_ID] = {
+		.dev_handle = &storage_dev_handle,
+		.image_spec = (uintptr_t)&stm32image_block_spec,
+		.check = open_storage
+	}
+};
+
+#ifndef AARCH32_SP_OPTEE
+static int open_dummy(const uintptr_t spec)
+{
+	return io_dev_init(dummy_dev_handle, 0);
+}
+#endif
+
+static int open_image(const uintptr_t spec)
+{
+	return io_dev_init(image_dev_handle, 0);
+}
+
+static int open_storage(const uintptr_t spec)
+{
+	return io_dev_init(storage_dev_handle, 0);
+}
+
+#if STM32MP_EMMC_BOOT
+static uint32_t get_boot_part_ssbl_header(void)
+{
+	uint32_t magic = 0U;
+	int io_result;
+	size_t bytes_read;
+
+	io_result = register_io_dev_block(&mmc_dev_con);
+	if (io_result != 0) {
+		panic();
+	}
+
+	io_result = io_dev_open(mmc_dev_con, (uintptr_t)&mmc_block_dev_boot_part_spec,
+				&storage_dev_handle);
+	assert(io_result == 0);
+
+	io_result = io_open(storage_dev_handle, (uintptr_t)&emmc_boot_ssbl_block_spec,
+			    &image_dev_handle);
+	assert(io_result == 0);
+
+	io_result = io_read(image_dev_handle, (uintptr_t)&magic, sizeof(magic),
+			    &bytes_read);
+	assert(io_result == 0);
+	assert(bytes_read == sizeof(magic));
+
+	io_result = io_dev_close(storage_dev_handle);
+	assert(io_result == 0);
+
+	return magic;
+}
+#endif
+
+static void print_boot_device(boot_api_context_t *boot_context)
+{
+	switch (boot_context->boot_interface_selected) {
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_SD:
+		INFO("Using SDMMC\n");
+		break;
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_EMMC:
+		INFO("Using EMMC\n");
+		break;
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NOR_QSPI:
+		INFO("Using QSPI NOR\n");
+		break;
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NAND_FMC:
+		INFO("Using FMC NAND\n");
+		break;
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NAND_QSPI:
+		INFO("Using SPI NAND\n");
+		break;
+	default:
+		ERROR("Boot interface not found\n");
+		panic();
+		break;
+	}
+
+	if (boot_context->boot_interface_instance != 0U) {
+		INFO("  Instance %d\n", boot_context->boot_interface_instance);
+	}
+}
+
+static void stm32image_io_setup(void)
+{
+	int io_result __unused;
+
+	io_result = register_io_dev_stm32image(&stm32image_dev_con);
+	assert(io_result == 0);
+
+	io_result = io_dev_open(stm32image_dev_con,
+				(uintptr_t)&stm32image_dev_info_spec,
+				&image_dev_handle);
+	assert(io_result == 0);
+}
+
+#if STM32MP_SDMMC || STM32MP_EMMC
+static void boot_mmc(enum mmc_device_type mmc_dev_type,
+		     uint16_t boot_interface_instance)
+{
+	int io_result __unused;
+	uint8_t idx;
+	struct stm32image_part_info *part;
+	struct stm32_sdmmc2_params params;
+	const partition_entry_t *entry __unused;
+	uint32_t magic __unused;
+
+	zeromem(&params, sizeof(struct stm32_sdmmc2_params));
+
+	mmc_info.mmc_dev_type = mmc_dev_type;
+
+	switch (boot_interface_instance) {
+	case 1:
+		params.reg_base = STM32MP_SDMMC1_BASE;
+		break;
+	case 2:
+		params.reg_base = STM32MP_SDMMC2_BASE;
+		break;
+	case 3:
+		params.reg_base = STM32MP_SDMMC3_BASE;
+		break;
+	default:
+		WARN("SDMMC instance not found, using default\n");
+		if (mmc_dev_type == MMC_IS_SD) {
+			params.reg_base = STM32MP_SDMMC1_BASE;
+		} else {
+			params.reg_base = STM32MP_SDMMC2_BASE;
+		}
+		break;
+	}
+
+	params.device_info = &mmc_info;
+	if (stm32_sdmmc2_mmc_init(&params) != 0) {
+		ERROR("SDMMC%u init failed\n", boot_interface_instance);
+		panic();
+	}
+
+	stm32image_dev_info_spec.device_size =
+		stm32_sdmmc2_mmc_get_device_size();
+
+#if STM32MP_EMMC_BOOT
+	magic = get_boot_part_ssbl_header();
+
+	if (magic == BOOT_API_IMAGE_HEADER_MAGIC_NB) {
+		VERBOSE("%s, header found, jump to emmc load\n", __func__);
+		idx = IMG_IDX_BL33;
+		part = &stm32image_dev_info_spec.part_info[idx];
+		part->part_offset = PLAT_EMMC_BOOT_SSBL_OFFSET;
+		part->bkp_offset = 0U;
+		mmc_device_spec.use_boot_part = true;
+
+		goto emmc_boot;
+	} else {
+		WARN("%s: Can't find STM32 header on a boot partition\n", __func__);
+	}
+#endif
+
+	/* Open MMC as a block device to read GPT table */
+	io_result = register_io_dev_block(&mmc_dev_con);
+	if (io_result != 0) {
+		panic();
+	}
+
+	io_result = io_dev_open(mmc_dev_con, (uintptr_t)&mmc_block_dev_spec,
+				&storage_dev_handle);
+	assert(io_result == 0);
+
+	partition_init(GPT_IMAGE_ID);
+
+	io_result = io_dev_close(storage_dev_handle);
+	assert(io_result == 0);
+
+	for (idx = 0U; idx < IMG_IDX_NUM; idx++) {
+		part = &stm32image_dev_info_spec.part_info[idx];
+		entry = get_partition_entry(part->name);
+		if (entry == NULL) {
+			ERROR("Partition %s not found\n", part->name);
+			panic();
+		}
+
+		part->part_offset = entry->start;
+		part->bkp_offset = 0U;
+	}
+
+#if STM32MP_EMMC_BOOT
+emmc_boot:
+#endif
+	/*
+	 * Re-open MMC with io_mmc, for better perfs compared to
+	 * io_block.
+	 */
+	io_result = register_io_dev_mmc(&mmc_dev_con);
+	assert(io_result == 0);
+
+	io_result = io_dev_open(mmc_dev_con, (uintptr_t)&mmc_device_spec,
+				&storage_dev_handle);
+	assert(io_result == 0);
+}
+#endif /* STM32MP_SDMMC || STM32MP_EMMC */
+
+#if STM32MP_SPI_NOR
+static void boot_spi_nor(boot_api_context_t *boot_context)
+{
+	int io_result __unused;
+	uint8_t idx;
+	struct stm32image_part_info *part;
+
+	io_result = stm32_qspi_init();
+	assert(io_result == 0);
+
+	io_result = register_io_dev_mtd(&spi_dev_con);
+	assert(io_result == 0);
+
+	/* Open connections to device */
+	io_result = io_dev_open(spi_dev_con,
+				(uintptr_t)&spi_nor_dev_spec,
+				&storage_dev_handle);
+	assert(io_result == 0);
+
+	stm32image_dev_info_spec.device_size = spi_nor_dev_spec.device_size;
+
+	idx = IMG_IDX_BL33;
+	part = &stm32image_dev_info_spec.part_info[idx];
+	part->part_offset = STM32MP_NOR_BL33_OFFSET;
+	part->bkp_offset = 0U;
+
+#ifdef AARCH32_SP_OPTEE
+	idx = IMG_IDX_OPTEE_HEADER;
+	part = &stm32image_dev_info_spec.part_info[idx];
+	part->part_offset = STM32MP_NOR_TEEH_OFFSET;
+	part->bkp_offset = 0U;
+
+	idx = IMG_IDX_OPTEE_PAGED;
+	part = &stm32image_dev_info_spec.part_info[idx];
+	part->part_offset = STM32MP_NOR_TEED_OFFSET;
+	part->bkp_offset = 0U;
+
+	idx = IMG_IDX_OPTEE_CORE;
+	part = &stm32image_dev_info_spec.part_info[idx];
+	part->part_offset = STM32MP_NOR_TEEX_OFFSET;
+	part->bkp_offset = 0U;
+#endif
+}
+#endif /* STM32MP_SPI_NOR */
+
+#if STM32MP_RAW_NAND
+static void boot_fmc2_nand(boot_api_context_t *boot_context)
+{
+	int io_result __unused;
+	uint8_t idx;
+	struct stm32image_part_info *part;
+
+	io_result = stm32_fmc2_init();
+	assert(io_result == 0);
+
+	/* Register the IO device on this platform */
+	io_result = register_io_dev_mtd(&nand_dev_con);
+	assert(io_result == 0);
+
+	/* Open connections to device */
+	io_result = io_dev_open(nand_dev_con, (uintptr_t)&nand_dev_spec,
+				&storage_dev_handle);
+	assert(io_result == 0);
+
+	stm32image_dev_info_spec.device_size = nand_dev_spec.device_size;
+
+	idx = IMG_IDX_BL33;
+	part = &stm32image_dev_info_spec.part_info[idx];
+	part->part_offset = STM32MP_NAND_BL33_OFFSET;
+	part->bkp_offset = nand_dev_spec.erase_size;
+
+#ifdef AARCH32_SP_OPTEE
+	idx = IMG_IDX_OPTEE_HEADER;
+	part = &stm32image_dev_info_spec.part_info[idx];
+	part->part_offset = STM32MP_NAND_TEEH_OFFSET;
+	part->bkp_offset = nand_dev_spec.erase_size;
+
+	idx = IMG_IDX_OPTEE_PAGED;
+	part = &stm32image_dev_info_spec.part_info[idx];
+	part->part_offset = STM32MP_NAND_TEED_OFFSET;
+	part->bkp_offset = nand_dev_spec.erase_size;
+
+	idx = IMG_IDX_OPTEE_CORE;
+	part = &stm32image_dev_info_spec.part_info[idx];
+	part->part_offset = STM32MP_NAND_TEEX_OFFSET;
+	part->bkp_offset = nand_dev_spec.erase_size;
+#endif
+}
+#endif /* STM32MP_RAW_NAND */
+
+#if STM32MP_SPI_NAND
+static void boot_spi_nand(boot_api_context_t *boot_context)
+{
+	int io_result __unused;
+	uint8_t idx;
+	struct stm32image_part_info *part;
+
+	io_result = stm32_qspi_init();
+	assert(io_result == 0);
+
+	io_result = register_io_dev_mtd(&spi_dev_con);
+	assert(io_result == 0);
+
+	/* Open connections to device */
+	io_result = io_dev_open(spi_dev_con,
+				(uintptr_t)&spi_nand_dev_spec,
+				&storage_dev_handle);
+	assert(io_result == 0);
+
+	stm32image_dev_info_spec.device_size =
+		spi_nand_dev_spec.device_size;
+
+	idx = IMG_IDX_BL33;
+	part = &stm32image_dev_info_spec.part_info[idx];
+	part->part_offset = STM32MP_NAND_BL33_OFFSET;
+	part->bkp_offset = spi_nand_dev_spec.erase_size;
+
+#ifdef AARCH32_SP_OPTEE
+	idx = IMG_IDX_OPTEE_HEADER;
+	part = &stm32image_dev_info_spec.part_info[idx];
+	part->part_offset = STM32MP_NAND_TEEH_OFFSET;
+	part->bkp_offset = spi_nand_dev_spec.erase_size;
+
+	idx = IMG_IDX_OPTEE_PAGED;
+	part = &stm32image_dev_info_spec.part_info[idx];
+	part->part_offset = STM32MP_NAND_TEED_OFFSET;
+	part->bkp_offset = spi_nand_dev_spec.erase_size;
+
+	idx = IMG_IDX_OPTEE_CORE;
+	part = &stm32image_dev_info_spec.part_info[idx];
+	part->part_offset = STM32MP_NAND_TEEX_OFFSET;
+	part->bkp_offset = spi_nand_dev_spec.erase_size;
+#endif
+}
+#endif /* STM32MP_SPI_NAND */
+
+void stm32mp_io_setup(void)
+{
+	int io_result __unused;
+	boot_api_context_t *boot_context =
+		(boot_api_context_t *)stm32mp_get_boot_ctx_address();
+
+	print_boot_device(boot_context);
+
+	if ((boot_context->boot_partition_used_toboot == 1U) ||
+	    (boot_context->boot_partition_used_toboot == 2U)) {
+		INFO("Boot used partition fsbl%u\n",
+		     boot_context->boot_partition_used_toboot);
+	}
+
+#ifndef AARCH32_SP_OPTEE
+	io_result = register_io_dev_dummy(&dummy_dev_con);
+	assert(io_result == 0);
+
+	io_result = io_dev_open(dummy_dev_con, dummy_dev_spec,
+				&dummy_dev_handle);
+	assert(io_result == 0);
+#endif
+
+	switch (boot_context->boot_interface_selected) {
+#if STM32MP_SDMMC
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_SD:
+		dmbsy();
+		boot_mmc(MMC_IS_SD, boot_context->boot_interface_instance);
+		stm32image_io_setup();
+		break;
+#endif
+#if STM32MP_EMMC
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_EMMC:
+		dmbsy();
+		boot_mmc(MMC_IS_EMMC, boot_context->boot_interface_instance);
+		stm32image_io_setup();
+		break;
+#endif
+#if STM32MP_SPI_NOR
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NOR_QSPI:
+		dmbsy();
+		boot_spi_nor(boot_context);
+		stm32image_io_setup();
+		break;
+#endif
+#if STM32MP_RAW_NAND
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NAND_FMC:
+		dmbsy();
+		boot_fmc2_nand(boot_context);
+		stm32image_io_setup();
+		break;
+#endif
+#if STM32MP_SPI_NAND
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NAND_QSPI:
+		dmbsy();
+		boot_spi_nand(boot_context);
+		stm32image_io_setup();
+		break;
+#endif
+
+	default:
+		ERROR("Boot interface %d not supported\n",
+		      boot_context->boot_interface_selected);
+		panic();
+		break;
+	}
+}
+
+/*
+ * Return an IO device handle and specification which can be used to access
+ * an image. Use this to enforce platform load policy.
+ */
+int plat_get_image_source(unsigned int image_id, uintptr_t *dev_handle,
+			  uintptr_t *image_spec)
+{
+	int rc;
+	const struct plat_io_policy *policy;
+
+	assert(image_id < ARRAY_SIZE(policies));
+
+	policy = &policies[image_id];
+	rc = policy->check(policy->image_spec);
+	if (rc == 0) {
+		*image_spec = policy->image_spec;
+		*dev_handle = *(policy->dev_handle);
+	}
+
+	return rc;
+}
diff --git a/plat/st/common/include/stm32mp_common.h b/plat/st/common/include/stm32mp_common.h
index edced71..8a5fe48 100644
--- a/plat/st/common/include/stm32mp_common.h
+++ b/plat/st/common/include/stm32mp_common.h
@@ -95,6 +95,7 @@
 /* Initialise the IO layer and register platform IO devices */
 void stm32mp_io_setup(void);
 
+#if STM32MP_USE_STM32IMAGE
 /*
  * Check that the STM32 header of a .stm32 binary image is valid
  * @param header: pointer to the stm32 image header
@@ -102,6 +103,7 @@
  * @return: 0 on success, negative value in case of error
  */
 int stm32mp_check_header(boot_api_image_header_t *header, uintptr_t buffer);
+#endif /* STM32MP_USE_STM32IMAGE */
 
 /* Functions to map DDR in MMU with non-cacheable attribute, and unmap it */
 int stm32mp_map_ddr_non_cacheable(void);
diff --git a/plat/st/common/stm32mp_common.c b/plat/st/common/stm32mp_common.c
index 5e5958b..17ac145 100644
--- a/plat/st/common/stm32mp_common.c
+++ b/plat/st/common/stm32mp_common.c
@@ -76,6 +76,7 @@
 	return (read_sctlr() & c_m_bits) == c_m_bits;
 }
 
+#if STM32MP_USE_STM32IMAGE
 int stm32mp_check_header(boot_api_image_header_t *header, uintptr_t buffer)
 {
 	uint32_t i;
@@ -109,6 +110,7 @@
 
 	return 0;
 }
+#endif /* STM32MP_USE_STM32IMAGE */
 
 int stm32mp_map_ddr_non_cacheable(void)
 {