diff --git a/plat/st/common/include/stm32mp_common.h b/plat/st/common/include/stm32mp_common.h index 9ca5d16e4..750800462 100644 --- a/plat/st/common/include/stm32mp_common.h +++ b/plat/st/common/include/stm32mp_common.h @@ -113,4 +113,8 @@ int stm32mp_unmap_ddr(void); void stm32_save_boot_interface(uint32_t interface, uint32_t instance); void stm32_get_boot_interface(uint32_t *interface, uint32_t *instance); +#if !STM32MP_USE_STM32IMAGE && PSA_FWU_SUPPORT +void stm32mp1_fwu_set_boot_idx(void); +#endif /* !STM32MP_USE_STM32IMAGE && PSA_FWU_SUPPORT */ + #endif /* STM32MP_COMMON_H */ diff --git a/plat/st/stm32mp1/bl2_plat_setup.c b/plat/st/stm32mp1/bl2_plat_setup.c index 0c93f27e3..b5fc3ff75 100644 --- a/plat/st/stm32mp1/bl2_plat_setup.c +++ b/plat/st/stm32mp1/bl2_plat_setup.c @@ -30,6 +30,7 @@ #include #include +#include #include static struct stm32mp_auth_ops stm32mp1_auth_ops; @@ -452,6 +453,9 @@ int bl2_plat_handle_post_image_load(unsigned int image_id) bl32_mem_params = get_bl_mem_params_node(BL32_IMAGE_ID); assert(bl32_mem_params != NULL); bl32_mem_params->ep_info.lr_svc = bl_mem_params->ep_info.pc; +#if !STM32MP_USE_STM32IMAGE && PSA_FWU_SUPPORT + stm32mp1_fwu_set_boot_idx(); +#endif /* !STM32MP_USE_STM32IMAGE && PSA_FWU_SUPPORT */ break; default: diff --git a/plat/st/stm32mp1/stm32mp1_private.c b/plat/st/stm32mp1/stm32mp1_private.c index 0bed12a27..9016b0d99 100644 --- a/plat/st/stm32mp1/stm32mp1_private.c +++ b/plat/st/stm32mp1/stm32mp1_private.c @@ -13,6 +13,7 @@ #include #include +#include #include /* Internal layout of the 32bit OTP word board_id */ @@ -40,6 +41,8 @@ #define TAMP_BOOT_MODE_ITF_MASK U(0x0000FF00) #define TAMP_BOOT_MODE_ITF_SHIFT 8 +#define TAMP_BOOT_COUNTER_REG_ID U(21) + #if defined(IMAGE_BL2) #define MAP_SEC_SYSRAM MAP_REGION_FLAT(STM32MP_SYSRAM_BASE, \ STM32MP_SYSRAM_SIZE, \ @@ -597,3 +600,13 @@ void stm32_get_boot_interface(uint32_t *interface, uint32_t *instance) *interface = itf >> 4; *instance = itf & 0xFU; } + +#if !STM32MP_USE_STM32IMAGE && PSA_FWU_SUPPORT +void stm32mp1_fwu_set_boot_idx(void) +{ + clk_enable(RTCAPB); + mmio_write_32(tamp_bkpr(TAMP_BOOT_COUNTER_REG_ID), + plat_fwu_get_boot_idx()); + clk_disable(RTCAPB); +} +#endif /* !STM32MP_USE_STM32IMAGE && PSA_FWU_SUPPORT */