Merge "docs(ff-a): boot order field of SPs manifest" into integration
diff --git a/Makefile b/Makefile
index 73007b4..32eb501 100644
--- a/Makefile
+++ b/Makefile
@@ -267,6 +267,16 @@
 ENABLE_FEAT_SB		= 	1
 endif
 
+# Determine and enable FEAT_FGT to access HDFGRTR_EL2 register for v8.6 and higher versions.
+ifeq "8.6" "$(word 1, $(sort 8.6 $(ARM_ARCH_MAJOR).$(ARM_ARCH_MINOR)))"
+ENABLE_FEAT_FGT		=	1
+endif
+
+# Determine and enable FEAT_ECV to access CNTPOFF_EL2 register for v8.6 and higher versions.
+ifeq "8.6" "$(word 1, $(sort 8.6 $(ARM_ARCH_MAJOR).$(ARM_ARCH_MINOR)))"
+ENABLE_FEAT_ECV		=	1
+endif
+
 ifneq ($(findstring armclang,$(notdir $(CC))),)
 TF_CFLAGS_aarch32	=	-target arm-arm-none-eabi $(march32-directive)
 TF_CFLAGS_aarch64	=	-target aarch64-arm-none-eabi $(march64-directive)
@@ -1041,6 +1051,9 @@
         ENABLE_FEAT_HCX \
         ENABLE_MPMM \
         ENABLE_MPMM_FCONF \
+        ENABLE_FEAT_FGT \
+        ENABLE_FEAT_AMUv1 \
+        ENABLE_FEAT_ECV \
 )))
 
 $(eval $(call assert_numerics,\
@@ -1153,6 +1166,9 @@
         ENABLE_FEAT_HCX \
         ENABLE_MPMM \
         ENABLE_MPMM_FCONF \
+        ENABLE_FEAT_FGT \
+        ENABLE_FEAT_AMUv1 \
+        ENABLE_FEAT_ECV \
 )))
 
 ifeq (${SANITIZE_UB},trap)
diff --git a/docs/getting_started/build-options.rst b/docs/getting_started/build-options.rst
index 7662a14..d77875e 100644
--- a/docs/getting_started/build-options.rst
+++ b/docs/getting_started/build-options.rst
@@ -246,6 +246,24 @@
    builds, but this behaviour can be overridden in each platform's Makefile or
    in the build command line.
 
+-  ``ENABLE_FEAT_AMUv1``: Boolean option to enable access to the HAFGRTR_EL2
+   (Hypervisor Activity Monitors Fine-Grained Read Trap Register) during EL2
+   to EL3 context save/restore operations. It is an optional feature available
+   on v8.4 and onwards and must be set to 1 alongside ``ENABLE_FEAT_FGT``, to
+   access the HAFGRTR_EL2 register. Defaults to ``0``.
+
+-  ``ENABLE_FEAT_ECV``: Boolean option to enable support for the Enhanced Counter
+   Virtualization feature, allowing for access to the CNTPOFF_EL2 (Counter-timer
+   Physical Offset register) during EL2 to EL3 context save/restore operations.
+   Its a mandatory architectural feature in Armv8.6 and defaults to ``1`` for
+   v8.6 or later CPUs.
+
+-  ``ENABLE_FEAT_FGT``: Boolean option to enable support for FGT (Fine Grain Traps)
+   feature allowing for access to the HDFGRTR_EL2 (Hypervisor Debug Fine-Grained
+   Read Trap Register)  during EL2 to EL3 context save/restore operations.
+   Its a mandatory architectural feature in Armv8.6 and defaults to ``1`` for
+   v8.6 or later CPUs.
+
 -  ``ENABLE_FEAT_HCX``: This option sets the bit SCR_EL3.HXEn in EL3 to allow
    access to HCRX_EL2 (extended hypervisor control register) from EL2 as well as
    adding HCRX_EL2 to the EL2 context save/restore operations.
diff --git a/drivers/arm/css/scmi/scmi_private.h b/drivers/arm/css/scmi/scmi_private.h
index 61437f6..a684ca5 100644
--- a/drivers/arm/css/scmi/scmi_private.h
+++ b/drivers/arm/css/scmi/scmi_private.h
@@ -136,7 +136,7 @@
 	uint64_t res_b; /* Reserved */
 	uint32_t flags;
 	volatile uint32_t len;
-	uint32_t msg_header;
+	volatile uint32_t msg_header;
 	uint32_t payload[];
 } mailbox_mem_t;
 
diff --git a/drivers/st/gpio/stm32_gpio.c b/drivers/st/gpio/stm32_gpio.c
index 7d63262..51276f4 100644
--- a/drivers/st/gpio/stm32_gpio.c
+++ b/drivers/st/gpio/stm32_gpio.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2020, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2016-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -283,3 +283,10 @@
 
 	stm32mp_clk_disable(clock);
 }
+
+void set_gpio_reset_cfg(uint32_t bank, uint32_t pin)
+{
+	set_gpio(bank, pin, GPIO_MODE_ANALOG, GPIO_SPEED_LOW,
+		 GPIO_NO_PULL, GPIO_ALTERNATE_(0), DT_DISABLED);
+	set_gpio_secure_cfg(bank, pin, stm32_gpio_is_secure_at_reset(bank));
+}
diff --git a/include/drivers/st/stm32_gpio.h b/include/drivers/st/stm32_gpio.h
index e241f58..b072345 100644
--- a/include/drivers/st/stm32_gpio.h
+++ b/include/drivers/st/stm32_gpio.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2015-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -52,6 +52,7 @@
 void set_gpio(uint32_t bank, uint32_t pin, uint32_t mode, uint32_t speed,
 	      uint32_t pull, uint32_t alternate, uint8_t status);
 void set_gpio_secure_cfg(uint32_t bank, uint32_t pin, bool secure);
+void set_gpio_reset_cfg(uint32_t bank, uint32_t pin);
 #endif /*__ASSEMBLER__*/
 
 #endif /* STM32_GPIO_H */
diff --git a/include/lib/el3_runtime/aarch64/context.h b/include/lib/el3_runtime/aarch64/context.h
index 698e208..512d196 100644
--- a/include/lib/el3_runtime/aarch64/context.h
+++ b/include/lib/el3_runtime/aarch64/context.h
@@ -207,8 +207,8 @@
 #define CTX_MPAMVPMV_EL2	U(0x158)
 
 // Starting with Armv8.6
-#define CTX_HAFGRTR_EL2		U(0x160)
-#define CTX_HDFGRTR_EL2		U(0x168)
+#define CTX_HDFGRTR_EL2		U(0x160)
+#define CTX_HAFGRTR_EL2		U(0x168)
 #define CTX_HDFGWTR_EL2		U(0x170)
 #define CTX_HFGITR_EL2		U(0x178)
 #define CTX_HFGRTR_EL2		U(0x180)
diff --git a/include/lib/fconf/fconf_dyn_cfg_getter.h b/include/lib/fconf/fconf_dyn_cfg_getter.h
index 6f8da0d..ff51c6c 100644
--- a/include/lib/fconf/fconf_dyn_cfg_getter.h
+++ b/include/lib/fconf/fconf_dyn_cfg_getter.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2020, Arm Limited. All rights reserved.
+ * Copyright (c) 2019-2021, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,6 +9,8 @@
 
 #include <lib/fconf/fconf.h>
 
+#define FCONF_INVALID_IDX	0xFFFFFFFFU
+
 /* Dynamic configuration related getter */
 #define dyn_cfg__dtb_getter(id)	dyn_cfg_dtb_info_getter(id)
 
@@ -18,6 +20,7 @@
 	unsigned int config_id;
 };
 
+unsigned int dyn_cfg_dtb_info_get_index(unsigned int config_id);
 struct dyn_cfg_dtb_info_t *dyn_cfg_dtb_info_getter(unsigned int config_id);
 int fconf_populate_dtb_registry(uintptr_t config);
 
diff --git a/lib/el3_runtime/aarch64/context.S b/lib/el3_runtime/aarch64/context.S
index e270ad0..c9035e8 100644
--- a/lib/el3_runtime/aarch64/context.S
+++ b/lib/el3_runtime/aarch64/context.S
@@ -145,11 +145,14 @@
 	stp	x11, x12, [x0, #CTX_MPAMVPM7_EL2]
 #endif
 
-#if ARM_ARCH_AT_LEAST(8, 6)
-	mrs	x13, HAFGRTR_EL2
-	mrs	x14, HDFGRTR_EL2
-	stp	x13, x14, [x0, #CTX_HAFGRTR_EL2]
-
+#if ENABLE_FEAT_FGT
+	mrs	x13, HDFGRTR_EL2
+#if ENABLE_FEAT_AMUv1
+   	mrs	x14, HAFGRTR_EL2
+   	stp	x13, x14, [x0, #CTX_HDFGRTR_EL2]
+#else
+   	str	x13, [x0, #CTX_HDFGRTR_EL2]
+#endif
 	mrs	x15, HDFGWTR_EL2
 	mrs	x16, HFGITR_EL2
 	stp	x15, x16, [x0, #CTX_HDFGWTR_EL2]
@@ -157,7 +160,9 @@
 	mrs	x9, HFGRTR_EL2
 	mrs	x10, HFGWTR_EL2
 	stp	x9, x10, [x0, #CTX_HFGRTR_EL2]
+#endif
 
+#if ENABLE_FEAT_ECV
 	mrs	x11, CNTPOFF_EL2
 	str	x11, [x0, #CTX_CNTPOFF_EL2]
 #endif
@@ -319,10 +324,14 @@
 	msr	MPAMVPMV_EL2, x12
 #endif
 
-#if ARM_ARCH_AT_LEAST(8, 6)
-	ldp	x13, x14, [x0, #CTX_HAFGRTR_EL2]
-	msr	HAFGRTR_EL2, x13
-	msr	HDFGRTR_EL2, x14
+#if ENABLE_FEAT_FGT
+#if ENABLE_FEAT_AMUv1
+	ldp	x13, x14, [x0, #CTX_HDFGRTR_EL2]
+	msr	HAFGRTR_EL2, x14
+#else
+	ldr	x13, [x0, #CTX_HDFGRTR_EL2]
+#endif
+	msr	HDFGRTR_EL2, x13
 
 	ldp	x15, x16, [x0, #CTX_HDFGWTR_EL2]
 	msr	HDFGWTR_EL2, x15
@@ -331,7 +340,9 @@
 	ldp	x9, x10, [x0, #CTX_HFGRTR_EL2]
 	msr	HFGRTR_EL2, x9
 	msr	HFGWTR_EL2, x10
+#endif
 
+#if ENABLE_FEAT_ECV
 	ldr	x11, [x0, #CTX_CNTPOFF_EL2]
 	msr	CNTPOFF_EL2, x11
 #endif
diff --git a/lib/fconf/fconf_dyn_cfg_getter.c b/lib/fconf/fconf_dyn_cfg_getter.c
index 25dd7f9..34623fb 100644
--- a/lib/fconf/fconf_dyn_cfg_getter.c
+++ b/lib/fconf/fconf_dyn_cfg_getter.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2020, Arm Limited. All rights reserved.
+ * Copyright (c) 2019-2021, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -40,17 +40,30 @@
 	dtb_info->config_id = config_id;
 }
 
-struct dyn_cfg_dtb_info_t *dyn_cfg_dtb_info_getter(unsigned int config_id)
+/* Get index of the config_id image */
+unsigned int dyn_cfg_dtb_info_get_index(unsigned int config_id)
 {
 	unsigned int index;
 
 	/* Positions index to the proper config-id */
 	for (index = 0U; index < MAX_DTB_INFO; index++) {
 		if (dtb_infos[index].config_id == config_id) {
-			return &dtb_infos[index];
+			return index;
 		}
 	}
 
+	return FCONF_INVALID_IDX;
+}
+
+struct dyn_cfg_dtb_info_t *dyn_cfg_dtb_info_getter(unsigned int config_id)
+{
+	/* Positions index to the proper config-id */
+	unsigned int index = dyn_cfg_dtb_info_get_index(config_id);
+
+	if (index < MAX_DTB_INFO) {
+		return &dtb_infos[index];
+	}
+
 	WARN("FCONF: Invalid config id %u\n", config_id);
 
 	return NULL;
diff --git a/make_helpers/defaults.mk b/make_helpers/defaults.mk
index e88148f..595ba7b 100644
--- a/make_helpers/defaults.mk
+++ b/make_helpers/defaults.mk
@@ -136,6 +136,9 @@
 # Flag to enable access to the HCRX_EL2 register by setting SCR_EL3.HXEn.
 ENABLE_FEAT_HCX			:= 0
 
+# Flag to enable access to the HAFGRTR_EL2 register
+ENABLE_FEAT_AMUv1		:= 0
+
 # By default BL31 encryption disabled
 ENCRYPT_BL31			:= 0
 
diff --git a/plat/st/common/include/stm32mp_common.h b/plat/st/common/include/stm32mp_common.h
index 6183964..fbd0dda 100644
--- a/plat/st/common/include/stm32mp_common.h
+++ b/plat/st/common/include/stm32mp_common.h
@@ -48,11 +48,14 @@
 uint32_t stm32_iwdg_shadow_update(uint32_t iwdg_inst, uint32_t flags);
 #endif
 
-#if STM32MP_UART_PROGRAMMER
+#if STM32MP_UART_PROGRAMMER || !defined(IMAGE_BL2)
 /* Get the UART address from its instance number */
 uintptr_t get_uart_address(uint32_t instance_nb);
 #endif
 
+/* Setup the UART console */
+int stm32mp_uart_console_setup(void);
+
 /*
  * Platform util functions for the GPIO driver
  * @bank: Target GPIO bank ID as per DT bindings
@@ -69,6 +72,7 @@
 uintptr_t stm32_get_gpio_bank_base(unsigned int bank);
 unsigned long stm32_get_gpio_bank_clock(unsigned int bank);
 uint32_t stm32_get_gpio_bank_offset(unsigned int bank);
+bool stm32_gpio_is_secure_at_reset(unsigned int bank);
 
 /* Return node offset for target GPIO bank ID @bank or a FDT error code */
 int stm32_get_gpio_bank_pinctrl_node(void *fdt, unsigned int bank);
@@ -114,4 +118,8 @@
 int stm32mp_map_ddr_non_cacheable(void);
 int stm32mp_unmap_ddr(void);
 
+/* Functions to save and get boot peripheral info */
+void stm32_save_boot_interface(uint32_t interface, uint32_t instance);
+void stm32_get_boot_interface(uint32_t *interface, uint32_t *instance);
+
 #endif /* STM32MP_COMMON_H */
diff --git a/plat/st/common/stm32mp_common.c b/plat/st/common/stm32mp_common.c
index 9120408..072155f 100644
--- a/plat/st/common/stm32mp_common.c
+++ b/plat/st/common/stm32mp_common.c
@@ -7,17 +7,23 @@
 #include <assert.h>
 #include <errno.h>
 
-#include <platform_def.h>
-
 #include <arch_helpers.h>
 #include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32_console.h>
 #include <drivers/st/stm32mp_clkfunc.h>
+#include <drivers/st/stm32mp_reset.h>
 #include <lib/smccc.h>
 #include <lib/xlat_tables/xlat_tables_v2.h>
 #include <plat/common/platform.h>
 #include <services/arm_arch_svc.h>
 
+#include <platform_def.h>
+
 #define HEADER_VERSION_MAJOR_MASK	GENMASK(23, 16)
+#define RESET_TIMEOUT_US_1MS		1000U
+
+static console_t console;
 
 uintptr_t plat_get_ns_image_entrypoint(void)
 {
@@ -128,6 +134,83 @@
 					   STM32MP_DDR_MAX_SIZE);
 }
 
+#if  defined(IMAGE_BL2)
+static void reset_uart(uint32_t reset)
+{
+	int ret;
+
+	ret = stm32mp_reset_assert(reset, RESET_TIMEOUT_US_1MS);
+	if (ret != 0) {
+		panic();
+	}
+
+	udelay(2);
+
+	ret = stm32mp_reset_deassert(reset, RESET_TIMEOUT_US_1MS);
+	if (ret != 0) {
+		panic();
+	}
+
+	mdelay(1);
+}
+#endif
+
+int stm32mp_uart_console_setup(void)
+{
+	struct dt_node_info dt_uart_info;
+	unsigned int console_flags;
+	uint32_t clk_rate;
+	int result;
+	uint32_t boot_itf __unused;
+	uint32_t boot_instance __unused;
+
+	result = dt_get_stdout_uart_info(&dt_uart_info);
+
+	if ((result <= 0) ||
+	    (dt_uart_info.status == DT_DISABLED) ||
+	    (dt_uart_info.clock < 0) ||
+	    (dt_uart_info.reset < 0)) {
+		return -ENODEV;
+	}
+
+#if STM32MP_UART_PROGRAMMER || !defined(IMAGE_BL2)
+	stm32_get_boot_interface(&boot_itf, &boot_instance);
+
+	if ((boot_itf == BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_UART) &&
+	    (get_uart_address(boot_instance) == dt_uart_info.base)) {
+		return -EACCES;
+	}
+#endif
+
+#if defined(IMAGE_BL2)
+	if (dt_set_stdout_pinctrl() != 0) {
+		return -ENODEV;
+	}
+#endif
+
+	stm32mp_clk_enable((unsigned long)dt_uart_info.clock);
+
+#if defined(IMAGE_BL2)
+	reset_uart((uint32_t)dt_uart_info.reset);
+#endif
+
+	clk_rate = stm32mp_clk_get_rate((unsigned long)dt_uart_info.clock);
+
+	if (console_stm32_register(dt_uart_info.base, clk_rate,
+				   STM32MP_UART_BAUDRATE, &console) == 0) {
+		panic();
+	}
+
+	console_flags = CONSOLE_FLAG_BOOT | CONSOLE_FLAG_CRASH |
+			CONSOLE_FLAG_TRANSLATE_CRLF;
+#if !defined(IMAGE_BL2) && defined(DEBUG)
+	console_flags |= CONSOLE_FLAG_RUNTIME;
+#endif
+	console_set_scope(&console, console_flags);
+
+	return 0;
+}
+
 /*****************************************************************************
  * plat_is_smccc_feature_available() - This function checks whether SMCCC
  *                                     feature is availabile for platform.
diff --git a/plat/st/stm32mp1/bl2_plat_setup.c b/plat/st/stm32mp1/bl2_plat_setup.c
index 218f28d..0bf8583 100644
--- a/plat/st/stm32mp1/bl2_plat_setup.c
+++ b/plat/st/stm32mp1/bl2_plat_setup.c
@@ -14,14 +14,12 @@
 #include <common/bl_common.h>
 #include <common/debug.h>
 #include <common/desc_image_load.h>
-#include <drivers/delay_timer.h>
 #include <drivers/generic_delay_timer.h>
 #include <drivers/mmc.h>
 #include <drivers/st/bsec.h>
-#include <drivers/st/stm32_console.h>
 #include <drivers/st/stm32_iwdg.h>
+#include <drivers/st/stm32_uart.h>
 #include <drivers/st/stm32mp_pmic.h>
-#include <drivers/st/stm32mp_reset.h>
 #include <drivers/st/stm32mp1_clk.h>
 #include <drivers/st/stm32mp1_pwr.h>
 #include <drivers/st/stm32mp1_ram.h>
@@ -32,12 +30,8 @@
 #include <lib/xlat_tables/xlat_tables_v2.h>
 #include <plat/common/platform.h>
 
-#include <stm32mp1_context.h>
 #include <stm32mp1_dbgmcu.h>
 
-#define RESET_TIMEOUT_US_1MS		1000U
-
-static console_t console;
 static struct stm32mp_auth_ops stm32mp1_auth_ops;
 
 static void print_reset_reason(void)
@@ -167,11 +161,9 @@
 void bl2_el3_plat_arch_setup(void)
 {
 	int32_t result;
-	struct dt_node_info dt_uart_info;
 	const char *board_model;
 	boot_api_context_t *boot_context =
 		(boot_api_context_t *)stm32mp_get_boot_ctx_address();
-	uint32_t clk_rate;
 	uintptr_t pwr_base;
 	uintptr_t rcc_base;
 
@@ -238,6 +230,16 @@
 
 	generic_delay_timer_init();
 
+#if STM32MP_UART_PROGRAMMER
+	/* Disable programmer UART before changing clock tree */
+	if (boot_context->boot_interface_selected ==
+	    BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_UART) {
+		uintptr_t uart_prog_addr =
+			get_uart_address(boot_context->boot_interface_instance);
+
+		stm32_uart_stop(uart_prog_addr);
+	}
+#endif
 	if (stm32mp1_clk_probe() < 0) {
 		panic();
 	}
@@ -248,45 +250,18 @@
 
 	stm32mp1_syscfg_init();
 
-	result = dt_get_stdout_uart_info(&dt_uart_info);
+	stm32_save_boot_interface(boot_context->boot_interface_selected,
+				  boot_context->boot_interface_instance);
 
-	if ((result <= 0) ||
-	    (dt_uart_info.status == 0U) ||
-	    (dt_uart_info.clock < 0) ||
-	    (dt_uart_info.reset < 0)) {
-		goto skip_console_init;
-	}
+#if STM32MP_USB_PROGRAMMER
+	/* Deconfigure all UART RX pins configured by ROM code */
+	stm32mp1_deconfigure_uart_pins();
+#endif
 
-	if (dt_set_stdout_pinctrl() != 0) {
+	if (stm32mp_uart_console_setup() != 0) {
 		goto skip_console_init;
 	}
 
-	stm32mp_clk_enable((unsigned long)dt_uart_info.clock);
-
-	if (stm32mp_reset_assert((uint32_t)dt_uart_info.reset,
-				 RESET_TIMEOUT_US_1MS) != 0) {
-		panic();
-	}
-
-	udelay(2);
-
-	if (stm32mp_reset_deassert((uint32_t)dt_uart_info.reset,
-				   RESET_TIMEOUT_US_1MS) != 0) {
-		panic();
-	}
-
-	mdelay(1);
-
-	clk_rate = stm32mp_clk_get_rate((unsigned long)dt_uart_info.clock);
-
-	if (console_stm32_register(dt_uart_info.base, clk_rate,
-				   STM32MP_UART_BAUDRATE, &console) == 0) {
-		panic();
-	}
-
-	console_set_scope(&console, CONSOLE_FLAG_BOOT |
-			  CONSOLE_FLAG_CRASH | CONSOLE_FLAG_TRANSLATE_CRLF);
-
 	stm32mp_print_cpuinfo();
 
 	board_model = dt_get_board_model();
@@ -314,12 +289,6 @@
 		INFO("IWDG2 freeze error : %i\n", result);
 	}
 
-	if (stm32_save_boot_interface(boot_context->boot_interface_selected,
-				      boot_context->boot_interface_instance) !=
-	    0) {
-		ERROR("Cannot save boot interface\n");
-	}
-
 	stm32mp1_auth_ops.check_key = boot_context->bootrom_ecdsa_check_key;
 	stm32mp1_auth_ops.verify_signature =
 		boot_context->bootrom_ecdsa_verify_signature;
@@ -352,6 +321,7 @@
 	const struct dyn_cfg_dtb_info_t *config_info;
 	bl_mem_params_node_t *tos_fw_mem_params;
 	unsigned int i;
+	unsigned int idx;
 	unsigned long long ddr_top __unused;
 	const unsigned int image_ids[] = {
 		BL32_IMAGE_ID,
@@ -370,8 +340,14 @@
 		set_config_info(STM32MP_FW_CONFIG_BASE, STM32MP_FW_CONFIG_MAX_SIZE, FW_CONFIG_ID);
 		fconf_populate("FW_CONFIG", STM32MP_FW_CONFIG_BASE);
 
+		idx = dyn_cfg_dtb_info_get_index(TOS_FW_CONFIG_ID);
+
 		/* Iterate through all the fw config IDs */
 		for (i = 0U; i < ARRAY_SIZE(image_ids); i++) {
+			if ((image_ids[i] == TOS_FW_CONFIG_ID) && (idx == FCONF_INVALID_IDX)) {
+				continue;
+			}
+
 			bl_mem_params = get_bl_mem_params_node(image_ids[i]);
 			assert(bl_mem_params != NULL);
 
diff --git a/plat/st/stm32mp1/include/stm32mp1_context.h b/plat/st/stm32mp1/include/stm32mp1_context.h
deleted file mode 100644
index 698415a..0000000
--- a/plat/st/stm32mp1/include/stm32mp1_context.h
+++ /dev/null
@@ -1,14 +0,0 @@
-/*
- * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef STM32MP1_CONTEXT_H
-#define STM32MP1_CONTEXT_H
-
-#include <stdint.h>
-
-int stm32_save_boot_interface(uint32_t interface, uint32_t instance);
-
-#endif /* STM32MP1_CONTEXT_H */
diff --git a/plat/st/stm32mp1/include/stm32mp1_private.h b/plat/st/stm32mp1/include/stm32mp1_private.h
index 729d233..2eec16f 100644
--- a/plat/st/stm32mp1/include/stm32mp1_private.h
+++ b/plat/st/stm32mp1/include/stm32mp1_private.h
@@ -21,6 +21,8 @@
 void stm32mp1_syscfg_enable_io_compensation(void);
 void stm32mp1_syscfg_disable_io_compensation(void);
 
+void stm32mp1_deconfigure_uart_pins(void);
+
 #if STM32MP_USE_STM32IMAGE
 uint32_t stm32mp_get_ddr_ns_size(void);
 #endif /* STM32MP_USE_STM32IMAGE */
diff --git a/plat/st/stm32mp1/platform.mk b/plat/st/stm32mp1/platform.mk
index 2a4ab78..3a76d28 100644
--- a/plat/st/stm32mp1/platform.mk
+++ b/plat/st/stm32mp1/platform.mk
@@ -28,6 +28,8 @@
 TF_CFLAGS		+=	-mfloat-abi=soft
 endif
 
+TF_CFLAGS		+=	-Wsign-compare
+
 # Not needed for Cortex-A7
 WORKAROUND_CVE_2017_5715:=	0
 
@@ -124,13 +126,13 @@
 # Enable flags for C files
 $(eval $(call assert_booleans,\
 	$(sort \
+		PLAT_XLAT_TABLES_DYNAMIC \
 		STM32MP_EMMC \
-		STM32MP_SDMMC \
+		STM32MP_EMMC_BOOT \
 		STM32MP_RAW_NAND \
+		STM32MP_SDMMC \
 		STM32MP_SPI_NAND \
 		STM32MP_SPI_NOR \
-		STM32MP_EMMC_BOOT \
-		PLAT_XLAT_TABLES_DYNAMIC \
 		STM32MP_UART_PROGRAMMER \
 		STM32MP_USB_PROGRAMMER \
 		STM32MP_USE_STM32IMAGE \
@@ -138,23 +140,23 @@
 
 $(eval $(call assert_numerics,\
 	$(sort \
-		STM32_TF_A_COPIES \
 		PLAT_PARTITION_MAX_ENTRIES \
+		STM32_TF_A_COPIES \
 		STM32_TF_VERSION \
 )))
 
 $(eval $(call add_defines,\
 	$(sort \
+		PLAT_PARTITION_MAX_ENTRIES \
+		PLAT_XLAT_TABLES_DYNAMIC \
+		STM32_TF_A_COPIES \
 		STM32_TF_VERSION \
 		STM32MP_EMMC \
-		STM32MP_SDMMC \
+		STM32MP_EMMC_BOOT \
 		STM32MP_RAW_NAND \
+		STM32MP_SDMMC \
 		STM32MP_SPI_NAND \
 		STM32MP_SPI_NOR \
-		STM32MP_EMMC_BOOT \
-		PLAT_XLAT_TABLES_DYNAMIC \
-		STM32_TF_A_COPIES \
-		PLAT_PARTITION_MAX_ENTRIES \
 		STM32MP_UART_PROGRAMMER \
 		STM32MP_USB_PROGRAMMER \
 		STM32MP_USE_STM32IMAGE \
@@ -200,7 +202,6 @@
 				drivers/st/pmic/stpmic1.c				\
 				drivers/st/reset/stm32mp1_reset.c			\
 				plat/st/common/stm32mp_dt.c				\
-				plat/st/stm32mp1/stm32mp1_context.c			\
 				plat/st/stm32mp1/stm32mp1_dbgmcu.c			\
 				plat/st/stm32mp1/stm32mp1_helper.S			\
 				plat/st/stm32mp1/stm32mp1_syscfg.c
diff --git a/plat/st/stm32mp1/sp_min/sp_min_setup.c b/plat/st/stm32mp1/sp_min/sp_min_setup.c
index 1495e02..8106795 100644
--- a/plat/st/stm32mp1/sp_min/sp_min_setup.c
+++ b/plat/st/stm32mp1/sp_min/sp_min_setup.c
@@ -18,7 +18,6 @@
 #include <drivers/generic_delay_timer.h>
 #include <drivers/st/bsec.h>
 #include <drivers/st/etzpc.h>
-#include <drivers/st/stm32_console.h>
 #include <drivers/st/stm32_gpio.h>
 #include <drivers/st/stm32_iwdg.h>
 #include <drivers/st/stm32mp1_clk.h>
@@ -36,8 +35,6 @@
  ******************************************************************************/
 static entry_point_info_t bl33_image_ep_info;
 
-static console_t console;
-
 /*******************************************************************************
  * Interrupt handler for FIQ (secure IRQ)
  ******************************************************************************/
@@ -115,8 +112,6 @@
 void sp_min_early_platform_setup2(u_register_t arg0, u_register_t arg1,
 				  u_register_t arg2, u_register_t arg3)
 {
-	struct dt_node_info dt_uart_info;
-	int result;
 	bl_params_t *params_from_bl2 = (bl_params_t *)arg0;
 #if STM32MP_USE_STM32IMAGE
 	uintptr_t dt_addr = STM32MP_DTB_BASE;
@@ -174,24 +169,7 @@
 		panic();
 	}
 
-	result = dt_get_stdout_uart_info(&dt_uart_info);
-
-	if ((result > 0) && (dt_uart_info.status != 0U)) {
-		unsigned int console_flags;
-
-		if (console_stm32_register(dt_uart_info.base, 0,
-					   STM32MP_UART_BAUDRATE, &console) ==
-		    0) {
-			panic();
-		}
-
-		console_flags = CONSOLE_FLAG_BOOT | CONSOLE_FLAG_CRASH |
-			CONSOLE_FLAG_TRANSLATE_CRLF;
-#ifdef DEBUG
-		console_flags |= CONSOLE_FLAG_RUNTIME;
-#endif
-		console_set_scope(&console, console_flags);
-	}
+	(void)stm32mp_uart_console_setup();
 
 	stm32mp1_etzpc_early_setup();
 }
diff --git a/plat/st/stm32mp1/stm32mp1_context.c b/plat/st/stm32mp1/stm32mp1_context.c
deleted file mode 100644
index cf8a91e..0000000
--- a/plat/st/stm32mp1/stm32mp1_context.c
+++ /dev/null
@@ -1,35 +0,0 @@
-/*
- * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <errno.h>
-
-#include <platform_def.h>
-
-#include <drivers/st/stm32mp1_clk.h>
-#include <dt-bindings/clock/stm32mp1-clks.h>
-#include <lib/mmio.h>
-
-#include <stm32mp1_context.h>
-
-#define TAMP_BOOT_ITF_BACKUP_REG_ID	U(20)
-#define TAMP_BOOT_ITF_MASK		U(0x0000FF00)
-#define TAMP_BOOT_ITF_SHIFT		8
-
-int stm32_save_boot_interface(uint32_t interface, uint32_t instance)
-{
-	uint32_t bkpr_itf_idx = tamp_bkpr(TAMP_BOOT_ITF_BACKUP_REG_ID);
-
-	stm32mp_clk_enable(RTCAPB);
-
-	mmio_clrsetbits_32(bkpr_itf_idx,
-			   TAMP_BOOT_ITF_MASK,
-			   ((interface << 4) | (instance & 0xFU)) <<
-			   TAMP_BOOT_ITF_SHIFT);
-
-	stm32mp_clk_disable(RTCAPB);
-
-	return 0;
-}
diff --git a/plat/st/stm32mp1/stm32mp1_private.c b/plat/st/stm32mp1/stm32mp1_private.c
index 0165cfe..a8ffecb 100644
--- a/plat/st/stm32mp1/stm32mp1_private.c
+++ b/plat/st/stm32mp1/stm32mp1_private.c
@@ -6,13 +6,14 @@
 
 #include <assert.h>
 
+#include <drivers/st/stm32_gpio.h>
+#include <drivers/st/stm32_iwdg.h>
 #include <libfdt.h>
+#include <lib/mmio.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
 
 #include <platform_def.h>
 
-#include <drivers/st/stm32_iwdg.h>
-#include <lib/xlat_tables/xlat_tables_v2.h>
-
 /* Internal layout of the 32bit OTP word board_id */
 #define BOARD_ID_BOARD_NB_MASK		GENMASK(31, 16)
 #define BOARD_ID_BOARD_NB_SHIFT		16
@@ -34,6 +35,10 @@
 					 BOARD_ID_VARFG_SHIFT)
 #define BOARD_ID2BOM(_id)		((_id) & BOARD_ID_BOM_MASK)
 
+#define TAMP_BOOT_MODE_BACKUP_REG_ID	U(20)
+#define TAMP_BOOT_MODE_ITF_MASK		U(0x0000FF00)
+#define TAMP_BOOT_MODE_ITF_SHIFT	8
+
 #if defined(IMAGE_BL2)
 #define MAP_SEC_SYSRAM	MAP_REGION_FLAT(STM32MP_SYSRAM_BASE, \
 					STM32MP_SYSRAM_SIZE, \
@@ -120,6 +125,15 @@
 	return bank * GPIO_BANK_OFFSET;
 }
 
+bool stm32_gpio_is_secure_at_reset(unsigned int bank)
+{
+	if (bank == GPIO_BANK_Z) {
+		return true;
+	}
+
+	return false;
+}
+
 unsigned long stm32_get_gpio_bank_clock(unsigned int bank)
 {
 	if (bank == GPIO_BANK_Z) {
@@ -153,7 +167,7 @@
 	}
 }
 
-#if STM32MP_UART_PROGRAMMER
+#if STM32MP_UART_PROGRAMMER || !defined(IMAGE_BL2)
 /*
  * UART Management
  */
@@ -179,6 +193,53 @@
 }
 #endif
 
+#if STM32MP_USB_PROGRAMMER
+struct gpio_bank_pin_list {
+	uint32_t bank;
+	uint32_t pin;
+};
+
+static const struct gpio_bank_pin_list gpio_list[] = {
+	{	/* USART2_RX: GPIOA3 */
+		.bank = 0U,
+		.pin = 3U,
+	},
+	{	/* USART3_RX: GPIOB12 */
+		.bank = 1U,
+		.pin = 12U,
+	},
+	{	/* UART4_RX: GPIOB2 */
+		.bank = 1U,
+		.pin = 2U,
+	},
+	{	/* UART5_RX: GPIOB4 */
+		.bank = 1U,
+		.pin = 5U,
+	},
+	{	/* USART6_RX: GPIOC7 */
+		.bank = 2U,
+		.pin = 7U,
+	},
+	{	/* UART7_RX: GPIOF6 */
+		.bank = 5U,
+		.pin = 6U,
+	},
+	{	/* UART8_RX: GPIOE0 */
+		.bank = 4U,
+		.pin = 0U,
+	},
+};
+
+void stm32mp1_deconfigure_uart_pins(void)
+{
+	size_t i;
+
+	for (i = 0U; i < ARRAY_SIZE(gpio_list); i++) {
+		set_gpio_reset_cfg(gpio_list[i].bank, gpio_list[i].pin);
+	}
+}
+#endif
+
 uint32_t stm32mp_get_chip_version(void)
 {
 	uint32_t version = 0U;
@@ -500,3 +561,36 @@
 	return ddr_ns_size;
 }
 #endif /* STM32MP_USE_STM32IMAGE */
+
+void stm32_save_boot_interface(uint32_t interface, uint32_t instance)
+{
+	uint32_t bkpr_itf_idx = tamp_bkpr(TAMP_BOOT_MODE_BACKUP_REG_ID);
+
+	stm32mp_clk_enable(RTCAPB);
+
+	mmio_clrsetbits_32(bkpr_itf_idx,
+			   TAMP_BOOT_MODE_ITF_MASK,
+			   ((interface << 4) | (instance & 0xFU)) <<
+			   TAMP_BOOT_MODE_ITF_SHIFT);
+
+	stm32mp_clk_disable(RTCAPB);
+}
+
+void stm32_get_boot_interface(uint32_t *interface, uint32_t *instance)
+{
+	static uint32_t itf;
+
+	if (itf == 0U) {
+		uint32_t bkpr = tamp_bkpr(TAMP_BOOT_MODE_BACKUP_REG_ID);
+
+		stm32mp_clk_enable(RTCAPB);
+
+		itf = (mmio_read_32(bkpr) & TAMP_BOOT_MODE_ITF_MASK) >>
+			TAMP_BOOT_MODE_ITF_SHIFT;
+
+		stm32mp_clk_disable(RTCAPB);
+	}
+
+	*interface = itf >> 4;
+	*instance = itf & 0xFU;
+}