stm32mp: split stm32mp_io_setup function

A new static function boot_mmc is created to simplify code maintenance
of stm32mp_io_setup.

Change-Id: I5c416e567e7e174fb1c2b435925a983c9c55fc40
Signed-off-by: Yann Gautier <yann.gautier@st.com>

1 files changed, 89 insertions, 86 deletions

diff --git a/plat/st/common/bl2_io_storage.c b/plat/st/common/bl2_io_storage.c
index f1721602..1aa3df76 100644
--- a/plat/st/common/bl2_io_storage.c
+++ b/plat/st/common/bl2_io_storage.c
@@ -162,122 +162,125 @@ static void print_boot_device(boot_api_context_t *boot_context)
 	}
 }
 
-void stm32mp_io_setup(void)
+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;
 	struct mmc_device_info device_info;
-	uintptr_t mmc_default_instance;
 	const partition_entry_t *entry;
-	boot_api_context_t *boot_context =
-		(boot_api_context_t *)stm32mp_get_boot_ctx_address();
 
-	print_boot_device(boot_context);
+	zeromem(&device_info, sizeof(struct mmc_device_info));
+	zeromem(&params, sizeof(struct stm32_sdmmc2_params));
 
-	if ((boot_context->boot_partition_used_toboot == 1U) ||
-	    (boot_context->boot_partition_used_toboot == 2U)) {
-		INFO("Boot used partition fsbl%d\n",
-		     boot_context->boot_partition_used_toboot);
+	device_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;
 	}
 
-	io_result = register_io_dev_dummy(&dummy_dev_con);
-	assert(io_result == 0);
+	params.device_info = &device_info;
+	if (stm32_sdmmc2_mmc_init(&params) != 0) {
+		ERROR("SDMMC%u init failed\n", boot_interface_instance);
+		panic();
+	}
 
-	io_result = io_dev_open(dummy_dev_con, dummy_dev_spec,
-				&dummy_dev_handle);
+	/* 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);
 
-	switch (boot_context->boot_interface_selected) {
-	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_SD:
-	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_EMMC:
-		dmbsy();
+	partition_init(GPT_IMAGE_ID);
 
-		zeromem(&device_info, sizeof(struct mmc_device_info));
-		zeromem(&params, sizeof(struct stm32_sdmmc2_params));
+	io_result = io_dev_close(storage_dev_handle);
+	assert(io_result == 0);
 
-		if (boot_context->boot_interface_selected ==
-		    BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_EMMC) {
-			device_info.mmc_dev_type = MMC_IS_EMMC;
-			mmc_default_instance = STM32MP_SDMMC2_BASE;
-		} else {
-			device_info.mmc_dev_type = MMC_IS_SD;
-			mmc_default_instance = STM32MP_SDMMC1_BASE;
-		}
+	stm32image_dev_info_spec.device_size =
+		stm32_sdmmc2_mmc_get_device_size();
 
-		switch (boot_context->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");
-			params.reg_base = mmc_default_instance;
-			break;
-		}
-
-		params.device_info = &device_info;
-		if (stm32_sdmmc2_mmc_init(&params) != 0) {
-			ERROR("SDMMC%u init failed\n",
-			      boot_context->boot_interface_instance);
+	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();
 		}
 
-		/* Open MMC as a block device to read GPT table */
-		io_result = register_io_dev_block(&mmc_dev_con);
-		if (io_result != 0) {
-			panic();
-		}
+		part->part_offset = entry->start;
+		part->bkp_offset = 0U;
+	}
 
-		io_result = io_dev_open(mmc_dev_con,
-					(uintptr_t)&mmc_block_dev_spec,
-					&storage_dev_handle);
-		assert(io_result == 0);
+	/*
+	 * 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);
 
-		partition_init(GPT_IMAGE_ID);
+	io_result = io_dev_open(mmc_dev_con, 0, &storage_dev_handle);
+	assert(io_result == 0);
 
-		io_result = io_dev_close(storage_dev_handle);
-		assert(io_result == 0);
+	io_result = register_io_dev_stm32image(&stm32image_dev_con);
+	assert(io_result == 0);
 
-		stm32image_dev_info_spec.device_size =
-			stm32_sdmmc2_mmc_get_device_size();
+	io_result = io_dev_open(stm32image_dev_con,
+				(uintptr_t)&stm32image_dev_info_spec,
+				&image_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();
-			}
+void stm32mp_io_setup(void)
+{
+	int io_result __unused;
+	boot_api_context_t *boot_context =
+		(boot_api_context_t *)stm32mp_get_boot_ctx_address();
 
-			part->part_offset = entry->start;
-			part->bkp_offset = 0U;
-		}
+	print_boot_device(boot_context);
 
-		/*
-		 * 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);
+	if ((boot_context->boot_partition_used_toboot == 1U) ||
+	    (boot_context->boot_partition_used_toboot == 2U)) {
+		INFO("Boot used partition fsbl%d\n",
+		     boot_context->boot_partition_used_toboot);
+	}
 
-		io_result = io_dev_open(mmc_dev_con, 0, &storage_dev_handle);
-		assert(io_result == 0);
+	io_result = register_io_dev_dummy(&dummy_dev_con);
+	assert(io_result == 0);
 
-		io_result = register_io_dev_stm32image(&stm32image_dev_con);
-		assert(io_result == 0);
+	io_result = io_dev_open(dummy_dev_con, dummy_dev_spec,
+				&dummy_dev_handle);
+	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);
+	switch (boot_context->boot_interface_selected) {
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_SD:
+		dmbsy();
+		boot_mmc(MMC_IS_SD, boot_context->boot_interface_instance);
+		break;
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_EMMC:
+		dmbsy();
+		boot_mmc(MMC_IS_EMMC, boot_context->boot_interface_instance);
 		break;
 
 	default: