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>

diff --git a/plat/st/common/bl2_io_storage.c b/plat/st/common/bl2_io_storage.c
index f1721602dfb2329a71cec2e81ce6fbd610d29daa..1aa3df76e709b259e053094ca08bae398d76f478 100644 (file)
--- 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: