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>

plat/st/common/bl2_io_storage.c

@@ -162,51 +162,22 @@ 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);
-
-	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 = 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);
-
-	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();
 
 	zeromem(&device_info, sizeof(struct mmc_device_info));
 	zeromem(&params, sizeof(struct stm32_sdmmc2_params));
 
-		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;
-		}
+	device_info.mmc_dev_type = mmc_dev_type;
 
-		switch (boot_context->boot_interface_instance) {
+	switch (boot_interface_instance) {
 	case 1:
 		params.reg_base = STM32MP_SDMMC1_BASE;
 		break;
@@ -218,14 +189,17 @@ void stm32mp_io_setup(void)
 		break;
 	default:
 		WARN("SDMMC instance not found, using default\n");
-			params.reg_base = mmc_default_instance;
+		if (mmc_dev_type == MMC_IS_SD) {
+			params.reg_base = STM32MP_SDMMC1_BASE;
+		} else {
+			params.reg_base = STM32MP_SDMMC2_BASE;
+		}
 		break;
 	}
 
 	params.device_info = &device_info;
 	if (stm32_sdmmc2_mmc_init(&params) != 0) {
-			ERROR("SDMMC%u init failed\n",
-			      boot_context->boot_interface_instance);
+		ERROR("SDMMC%u init failed\n", boot_interface_instance);
 		panic();
 	}
 
@@ -235,8 +209,7 @@ void stm32mp_io_setup(void)
 		panic();
 	}
 
-		io_result = io_dev_open(mmc_dev_con,
-					(uintptr_t)&mmc_block_dev_spec,
+	io_result = io_dev_open(mmc_dev_con, (uintptr_t)&mmc_block_dev_spec,
 				&storage_dev_handle);
 	assert(io_result == 0);
 
@@ -252,8 +225,7 @@ void stm32mp_io_setup(void)
 		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);
+			ERROR("Partition %s not found\n", part->name);
 			panic();
 		}
 
@@ -278,6 +250,37 @@ void stm32mp_io_setup(void)
 				(uintptr_t)&stm32image_dev_info_spec,
 				&image_dev_handle);
 	assert(io_result == 0);
+}
+
+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%d\n",
+		     boot_context->boot_partition_used_toboot);
+	}
+
+	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);
+
+	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: