meta-st-stm32mp/recipes-security/optee/optee-os/0002-st-updates-r2.patch

From 31808ba1976f47e71086ac168a2f64178ea2edcd Mon Sep 17 00:00:00 2001
From: Romuald JEANNE <romuald.jeanne@st.com>
Date: Tue, 17 Sep 2019 14:01:51 +0200
Subject: [PATCH] st updates r2

---
 .../arm/fdts/stm32mp15-ddr3-1x4Gb-1066-binG.dtsi   |  13 +-
 .../arm/fdts/stm32mp15-ddr3-2x4Gb-1066-binG.dtsi   |  10 +-
 core/arch/arm/fdts/stm32mp157a-dk1.dts             |   6 +-
 core/arch/arm/fdts/stm32mp157c-ed1.dts             |   6 +-
 .../arm/plat-stm32mp1/drivers/stm32mp1_calib.c     | 105 ++++---
 core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clk.c |  76 ++++-
 .../arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.c |   3 +-
 .../arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.h |   8 +-
 core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pwr.h |   4 +-
 core/arch/arm/plat-stm32mp1/drivers/stm32mp1_rcc.h |   6 +-
 core/arch/arm/plat-stm32mp1/main.c                 |   4 +-
 core/arch/arm/plat-stm32mp1/pm/context.c           |  15 +-
 core/arch/arm/plat-stm32mp1/pm/low_power.c         | 242 ++++++++++++---
 core/arch/arm/plat-stm32mp1/pm/pm_helpers.S        |   4 +-
 core/arch/arm/plat-stm32mp1/pm/psci.c              |   2 +-
 core/arch/arm/plat-stm32mp1/shared_resources.c     |   6 +-
 core/arch/arm/plat-stm32mp1/stm32_util.h           |   1 +
 core/drivers/stm32_bsec.c                          | 331 ++++++++-------------
 core/drivers/stm32_etzpc.c                         |   6 +-
 core/drivers/stm32_i2c.c                           |  22 +-
 core/drivers/stm32_iwdg.c                          |   6 +
 core/drivers/stm32_timer.c                         |  25 +-
 core/drivers/stpmic1.c                             |  17 +-
 core/include/drivers/stm32_bsec.h                  |  21 +-
 24 files changed, 571 insertions(+), 368 deletions(-)

diff --git a/core/arch/arm/fdts/stm32mp15-ddr3-1x4Gb-1066-binG.dtsi b/core/arch/arm/fdts/stm32mp15-ddr3-1x4Gb-1066-binG.dtsi
index 16b8cf6..11e8f2b 100644
--- a/core/arch/arm/fdts/stm32mp15-ddr3-1x4Gb-1066-binG.dtsi
+++ b/core/arch/arm/fdts/stm32mp15-ddr3-1x4Gb-1066-binG.dtsi
@@ -1,8 +1,8 @@
 // SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
 /*
  * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
- */
-/* STM32MP157C DK1/DK2 BOARD configuration
+ *
+ * STM32MP157C DK1/DK2 BOARD configuration
  * 1x DDR3L 4Gb, 16-bit, 533MHz.
  * Reference used NT5CC256M16DP-DI from NANYA
  *
@@ -16,8 +16,7 @@
  * address mapping : RBC
  * Tc > + 85C : N
  */
-
-#define DDR_MEM_NAME "DDR3-1066/888 bin G 1x4Gb 533MHz v1.41"
+#define DDR_MEM_NAME "DDR3-1066/888 bin G 1x4Gb 533MHz v1.45"
 #define DDR_MEM_SPEED 533000
 #define DDR_MEM_SIZE 0x20000000
 
@@ -90,7 +89,7 @@
 #define DDR_PTR2 0x042DA068
 #define DDR_ACIOCR 0x10400812
 #define DDR_DXCCR 0x00000C40
-#define DDR_DSGCR 0xF200001F
+#define DDR_DSGCR 0xF200011F
 #define DDR_DCR 0x0000000B
 #define DDR_DTPR0 0x38D488D0
 #define DDR_DTPR1 0x098B00D8
@@ -109,11 +108,11 @@
 #define DDR_DX1DLLCR 0x40000000
 #define DDR_DX1DQTR 0xFFFFFFFF
 #define DDR_DX1DQSTR 0x3DB02000
-#define DDR_DX2GCR 0x0000CE81
+#define DDR_DX2GCR 0x0000CE80
 #define DDR_DX2DLLCR 0x40000000
 #define DDR_DX2DQTR 0xFFFFFFFF
 #define DDR_DX2DQSTR 0x3DB02000
-#define DDR_DX3GCR 0x0000CE81
+#define DDR_DX3GCR 0x0000CE80
 #define DDR_DX3DLLCR 0x40000000
 #define DDR_DX3DQTR 0xFFFFFFFF
 #define DDR_DX3DQSTR 0x3DB02000
diff --git a/core/arch/arm/fdts/stm32mp15-ddr3-2x4Gb-1066-binG.dtsi b/core/arch/arm/fdts/stm32mp15-ddr3-2x4Gb-1066-binG.dtsi
index 82e7104..4b70b60 100644
--- a/core/arch/arm/fdts/stm32mp15-ddr3-2x4Gb-1066-binG.dtsi
+++ b/core/arch/arm/fdts/stm32mp15-ddr3-2x4Gb-1066-binG.dtsi
@@ -1,9 +1,8 @@
 // SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
 /*
  * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
- */
-
-/* STM32MP157C ED1 BOARD configuration
+ *
+ * STM32MP157C ED1 BOARD configuration
  * 2x DDR3L 4Gb each, 16-bit, 533MHz, Single Die Package in flyby topology.
  * Reference used NT5CC256M16DP-DI from NANYA
  *
@@ -17,8 +16,7 @@
  * address mapping : RBC
  * Tc > + 85C : N
  */
-
-#define DDR_MEM_NAME "DDR3-1066/888 bin G 2x4Gb 533MHz v1.41"
+#define DDR_MEM_NAME "DDR3-1066/888 bin G 2x4Gb 533MHz v1.45"
 #define DDR_MEM_SPEED 533000
 #define DDR_MEM_SIZE 0x40000000
 
@@ -91,7 +89,7 @@
 #define DDR_PTR2 0x042DA068
 #define DDR_ACIOCR 0x10400812
 #define DDR_DXCCR 0x00000C40
-#define DDR_DSGCR 0xF200001F
+#define DDR_DSGCR 0xF200011F
 #define DDR_DCR 0x0000000B
 #define DDR_DTPR0 0x38D488D0
 #define DDR_DTPR1 0x098B00D8
diff --git a/core/arch/arm/fdts/stm32mp157a-dk1.dts b/core/arch/arm/fdts/stm32mp157a-dk1.dts
index e42bdcd..78cb849 100644
--- a/core/arch/arm/fdts/stm32mp157a-dk1.dts
+++ b/core/arch/arm/fdts/stm32mp157a-dk1.dts
@@ -149,7 +149,9 @@
 };
 
 &pwr {
-	pwr-supply = <&vdd>;
+	pwr-regulators {
+		vdd-supply = <&vdd>;
+	};
 };
 
 /* RNG1 is used by non secure at run time and by secure (low power sequences) */
@@ -251,7 +253,7 @@
 &timers15 {
 	secure-status = "okay";
 	st,hsi-cal-input = <7>;
-	st,csi_cal-input = <8>;
+	st,csi-cal-input = <8>;
 };
 
 /* Low-power states of regulators */
diff --git a/core/arch/arm/fdts/stm32mp157c-ed1.dts b/core/arch/arm/fdts/stm32mp157c-ed1.dts
index 8462d23..81ae94c 100644
--- a/core/arch/arm/fdts/stm32mp157c-ed1.dts
+++ b/core/arch/arm/fdts/stm32mp157c-ed1.dts
@@ -144,7 +144,9 @@
 };
 
 &pwr {
-	pwr-supply = <&vdd>;
+	pwr-regulators {
+		vdd-supply = <&vdd>;
+	};
 };
 
 /* RNG1 is used by non secure at run time and by secure (low power sequences) */
@@ -256,7 +258,7 @@
 &timers15 {
 	secure-status = "okay";
 	st,hsi-cal-input = <7>;
-	st,csi_cal-input = <8>;
+	st,csi-cal-input = <8>;
 };
 
 /* Low-power states of regulators */
diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_calib.c b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_calib.c
index 583bf94..7050e97 100644
--- a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_calib.c
+++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_calib.c
@@ -11,16 +11,18 @@
 #include <dt-bindings/clock/stm32mp1-clks.h>
 #include <initcall.h>
 #include <keep.h>
+#include <kernel/delay.h>
 #include <kernel/generic_boot.h>
 #include <kernel/interrupt.h>
 #include <kernel/panic.h>
+#include <limits.h>
 #include <mm/core_memprot.h>
 #include <stm32_util.h>
 #include <stm32mp_dt.h>
 #include <stm32mp_pm.h>
 #include <string.h>
 
-#define CAL_MAX_RETRY		20U
+#define CALIBRATION_TIMEOUT_US	10000U
 
 /* List of forbiden values for HSI and CSI */
 static uint16_t fbv_hsi[] = {
@@ -64,7 +66,7 @@ static const struct stm32mp1_clk_cal hsi_calib_config = {
 	.trim_max = 63,
 	.trim_min = -64,
 	.ref_freq = 0,
-	.freq_margin = 1,
+	.freq_margin = 5,
 	.set_trim = hsi_set_trim,
 	.get_trim = hsi_get_trimed_cal,
 };
@@ -74,11 +76,16 @@ static const struct stm32mp1_clk_cal csi_calib_config = {
 	.trim_max = 15,
 	.trim_min = -16,
 	.ref_freq = 0,
-	.freq_margin = 2,
+	.freq_margin = 8,
 	.set_trim = csi_set_trim,
 	.get_trim = csi_get_trimed_cal,
 };
 
+static int get_signed_value(uint8_t val)
+{
+	return ((int8_t)(val << 1)) >> 1;
+}
+
 static void hsi_set_trim(unsigned int cal)
 {
 	int clk_trim = (int)cal - (int)hsi_calib->cal_ref;
@@ -95,7 +102,7 @@ static unsigned int hsi_get_trimed_cal(void)
 	uint32_t utrim = (mmio_read_32(stm32_rcc_base() + RCC_HSICFGR) &
 			  RCC_HSICFGR_HSITRIM_MASK) >>
 			 RCC_HSICFGR_HSITRIM_SHIFT;
-	int trim = (int)utrim - hsi_calib->trim_max;
+	int trim = get_signed_value((uint8_t)utrim);
 
 	if (trim + (int)hsi_calib->cal_ref < 0)
 		return 0;
@@ -184,32 +191,52 @@ static unsigned int trim_decrease(struct stm32mp1_clk_cal *clk_cal,
 
 static void rcc_calibration(struct stm32mp1_clk_cal *clk_cal)
 {
-	unsigned long margin = (clk_cal->ref_freq * clk_cal->freq_margin) / 100;
+	unsigned long margin = (clk_cal->ref_freq *
+				clk_cal->freq_margin) / 1000;
 	unsigned long min = clk_cal->ref_freq - margin;
 	unsigned long max = clk_cal->ref_freq + margin;
 	unsigned long freq = clk_cal->get_freq();
-	int cal = clk_cal->get_trim();
-	unsigned int nb_retries;
+	int trim, new_trim;
+	unsigned long conv;
+	unsigned long min_conv = ULONG_MAX;
+	uint64_t timeout_ref;
 
-	for (nb_retries = 0; nb_retries < CAL_MAX_RETRY; nb_retries++) {
-		if ((freq >= min) && (freq <= max)) {
-			break;
-		}
+	if ((freq >= min) && (freq <= max))
+		return;
+
+	trim = clk_cal->get_trim();
+	timeout_ref = utimeout_init(CALIBRATION_TIMEOUT_US);
+	do {
+		if (freq < clk_cal->ref_freq)
+			new_trim = trim_increase(clk_cal, trim);
+		else
+			new_trim = trim_decrease(clk_cal, trim);
 
-		if (freq < min) {
-			cal = trim_increase(clk_cal, cal);
-		} else {
-			cal = trim_decrease(clk_cal, cal);
+		clk_cal->set_trim(new_trim);
+		freq = clk_cal->get_freq();
+		if (freq == 0U) {
+			/* Calibration will be stopped */
+			clk_cal->ref_freq = 0U;
+			return;
+		}
+		conv = (clk_cal->ref_freq < freq) ?
+			freq - clk_cal->ref_freq : clk_cal->ref_freq - freq;
+		if (conv < min_conv) {
+			min_conv = conv;
+			trim = new_trim;
 		}
 
-		clk_cal->set_trim(cal);
+		if (utimeout_elapsed(CALIBRATION_TIMEOUT_US, timeout_ref))
+			break;
 
-		freq = clk_cal->get_freq();
-	}
+	} while (conv == min_conv);
 
+	clk_cal->set_trim(trim);
+	freq = clk_cal->get_freq();
 	if ((freq < min) || (freq > max)) {
-		DMSG("Calibration failed");
-		panic("Calibration");
+		EMSG("%s Calibration : Freq %lu , trim %i\n",
+		     (clk_cal->set_trim == hsi_set_trim) ? "HSI" : "CSI",
+		     freq, trim);
 	}
 }
 
@@ -387,17 +414,20 @@ int stm32mp_start_clock_calib(unsigned int clock_id)
 	return 0;
 }
 
-static void init_hsi_calibration(void *fdt, int node)
+static int init_hsi_calibration(void *fdt, int node)
 {
 	if (!fdt_getprop(fdt, node, "st,hsi-cal", NULL))
-		return;
+		return 0;
 
 	hsi_calib = calloc(1, sizeof(*hsi_calib));
 	assert(hsi_calib);
 	memcpy(hsi_calib, &hsi_calib_config, sizeof(*hsi_calib));
 
 	stm32_timer_freq_func(&hsi_calib->get_freq, HSI_CAL);
-	assert(hsi_calib->get_freq);
+	if (hsi_calib->get_freq == NULL) {
+		free(hsi_calib);
+		return -1;
+	}
 
 	hsi_calib->ref_freq = stm32mp1_clk_get_rate(CK_HSI);
 
@@ -406,41 +436,41 @@ static void init_hsi_calibration(void *fdt, int node)
 			      RCC_HSICFGR_HSICAL_SHIFT;
 
 	trim_table_init(hsi_calib);
-
 	hsi_calib->set_trim(hsi_calib->cal_ref);
-
 	stm32mp_start_clock_calib(CK_HSI);
+	return 1;
 }
 
-static void init_csi_calibration(void *fdt, int node)
+static int init_csi_calibration(void *fdt, int node)
 {
 	if (!fdt_getprop(fdt, node, "st,csi-cal", NULL))
-		return;
+		return 0;
 
 	csi_calib = calloc(1, sizeof(*csi_calib));
 	assert(csi_calib);
 	memcpy(csi_calib, &csi_calib_config, sizeof(*csi_calib));
 
 	stm32_timer_freq_func(&csi_calib->get_freq, CSI_CAL);
-	assert(csi_calib->get_freq);
+	if (csi_calib->get_freq == NULL) {
+		free(csi_calib);
+		return -1;
+	}
 
 	csi_calib->ref_freq = stm32mp1_clk_get_rate(CK_CSI);
-
 	csi_calib->cal_ref = (mmio_read_32(stm32_rcc_base() + RCC_CSICFGR) &
 			      RCC_CSICFGR_CSICAL_MASK) >>
 			     RCC_CSICFGR_CSICAL_SHIFT;
-
 	trim_table_init(csi_calib);
-
 	csi_calib->set_trim(csi_calib->cal_ref);
-
 	stm32mp_start_clock_calib(CK_CSI);
+	return 1;
 }
 
 static TEE_Result init_stm32mp1_calib(void)
 {
 	void *fdt;
 	int rcc_node = -1;
+	int res_csi, res_hsi;
 
 	fdt = get_dt_blob();
 	if (fdt)
@@ -448,9 +478,14 @@ static TEE_Result init_stm32mp1_calib(void)
 	if (rcc_node < 0)
 		panic();
 
-	init_hsi_calibration(fdt, rcc_node);
-	init_csi_calibration(fdt, rcc_node);
-	init_periodic_calibration(fdt, rcc_node);
+	res_hsi = init_hsi_calibration(fdt, rcc_node);
+	if (res_hsi < 0)
+		panic("HSI calibration init failed");
+	res_csi = init_csi_calibration(fdt, rcc_node);
+	if (res_csi < 0)
+		panic("CSI calibration init failed");
+	if (res_csi || res_hsi)
+		init_periodic_calibration(fdt, rcc_node);
 
 	return TEE_SUCCESS;
 }
diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clk.c b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clk.c
index acf5d60..69c3c74 100644
--- a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clk.c
+++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clk.c
@@ -32,6 +32,8 @@
 #include <libfdt.h>
 #endif
 
+#define CLKSRC_TIMEOUT_US	200000U
+
 enum stm32mp1_parent_id {
 /* Oscillators are defined in enum stm32mp_osc_id */
 
@@ -78,6 +80,8 @@ enum stm32mp1_parent_sel {
 	_UART24_SEL,
 	_UART35_SEL,
 	_UART78_SEL,
+	_SDMMC12_SEL,
+	_SDMMC3_SEL,
 	_ASS_SEL,
 	_MSS_SEL,
 	_USBPHY_SEL,
@@ -349,13 +353,23 @@ static const struct stm32mp1_clk_gate stm32mp1_clk_gate[] = {
 	_CLK_SC_SELEC(RCC_MP_APB1ENSETR, 19, UART8_K, _UART78_SEL),
 	_CLK_SC_SELEC(RCC_MP_APB2ENSETR, 13, USART6_K, _UART6_SEL),
 #endif
+	_CLK_SC_SELEC(RCC_MP_APB4ENSETR, 0, LTDC_PX, _UNKNOWN_SEL),
 	_CLK_SC_SELEC(RCC_MP_APB4ENSETR, 8, DDRPERFM, _UNKNOWN_SEL),
 	_CLK_SC_SELEC(RCC_MP_APB4ENSETR, 15, IWDG2, _UNKNOWN_SEL),
 	_CLK_SC_SELEC(RCC_MP_APB4ENSETR, 16, USBPHY_K, _USBPHY_SEL),
+	_CLK_SC_SELEC(RCC_MP_AHB2ENSETR, 0, DMA1, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(RCC_MP_AHB2ENSETR, 1, DMA2, _UNKNOWN_SEL),
 	_CLK_SC_SELEC(RCC_MP_AHB2ENSETR, 8, USBO_K, _USBO_SEL),
+	_CLK_SC_SELEC(RCC_MP_AHB2ENSETR, 16, SDMMC3_K, _SDMMC3_SEL),
 	_CLK_SELEC(RCC_DBGCFGR, 8, CK_DBG, _UNKNOWN_SEL),
 	_CLK_SC_FIXED(RCC_MP_APB1ENSETR, 6, TIM12_K, _PCLK1),
 	_CLK_SC_FIXED(RCC_MP_APB2ENSETR, 2, TIM15_K, _PCLK2),
+	_CLK_SC_SELEC(RCC_MP_AHB6ENSETR, 0, MDMA, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(RCC_MP_AHB6ENSETR, 5, GPU, _UNKNOWN_SEL),
+	_CLK_SC_FIXED(RCC_MP_AHB6ENSETR, 10, ETHMAC, _ACLK),
+	_CLK_SC_SELEC(RCC_MP_AHB6ENSETR, 16, SDMMC1_K, _SDMMC12_SEL),
+	_CLK_SC_SELEC(RCC_MP_AHB6ENSETR, 17, SDMMC2_K, _SDMMC12_SEL),
+	_CLK_SC_SELEC(RCC_MP_AHB6ENSETR, 24, USBH, _UNKNOWN_SEL),
 };
 KEEP_PAGER(stm32mp1_clk_gate);
 
@@ -389,6 +403,14 @@ static const uint8_t uart234578_parents[] = {
 	_PCLK1, _PLL4_Q, _HSI_KER, _CSI_KER, _HSE_KER
 };
 
+static const uint8_t sdmmc12_parents[] = {
+	_HCLK6, _PLL3_R, _PLL4_P, _HSI_KER
+};
+
+static const uint8_t sdmmc3_parents[] = {
+	_HCLK2, _PLL3_R, _PLL4_P, _HSI_KER
+};
+
 static const uint8_t ass_parents[] = {
 	_HSI, _HSE, _PLL2
 };
@@ -417,6 +439,8 @@ static const struct stm32mp1_clk_sel stm32mp1_clk_sel[_PARENT_SEL_NB] = {
 	_CLK_PARENT(_UART24_SEL, RCC_UART24CKSELR, 0, 0x7, uart234578_parents),
 	_CLK_PARENT(_UART35_SEL, RCC_UART35CKSELR, 0, 0x7, uart234578_parents),
 	_CLK_PARENT(_UART78_SEL, RCC_UART78CKSELR, 0, 0x7, uart234578_parents),
+	_CLK_PARENT(_SDMMC12_SEL, RCC_SDMMC12CKSELR, 0, 0x7, sdmmc12_parents),
+	_CLK_PARENT(_SDMMC3_SEL, RCC_SDMMC3CKSELR, 0, 0x7, sdmmc3_parents),
 	_CLK_PARENT(_ASS_SEL, RCC_ASSCKSELR, 0, 0x3, ass_parents),
 	_CLK_PARENT(_MSS_SEL, RCC_MSSCKSELR, 0, 0x3, mss_parents),
 	_CLK_PARENT(_USBPHY_SEL, RCC_USBCKSELR, 0, 0x3, usbphy_parents),
@@ -582,6 +606,26 @@ static enum stm32mp1_parent_id stm32mp1_clk_get_fixed_parent(int i)
 	return (enum stm32mp1_parent_id)gate_ref(i)->fixed;
 }
 
+static int stm32mp1_set_clksrc(unsigned int clksrc)
+{
+	uintptr_t address = stm32_rcc_base() + (clksrc >> 4);
+	uint64_t timeout_ref;
+
+	mmio_clrsetbits_32(address, RCC_SELR_SRC_MASK,
+			   clksrc & RCC_SELR_SRC_MASK);
+
+	timeout_ref = utimeout_init(CLKSRC_TIMEOUT_US);
+	while ((mmio_read_32(address) & RCC_SELR_SRCRDY) == 0U) {
+		if (utimeout_elapsed(CLKSRC_TIMEOUT_US, timeout_ref)) {
+			EMSG("CLKSRC %x start failed @ 0x%x: 0x%x\n",
+			      clksrc, address, mmio_read_32(address));
+			return -1;
+		}
+	}
+
+	return 0;
+}
+
 static int stm32mp1_clk_get_parent(unsigned long id)
 {
 	const struct stm32mp1_clk_sel *sel;
@@ -1069,10 +1113,6 @@ static void stm32mp1_osc_init(void)
 		DMSG("Osc %s frequency: %lu", name[i], stm32mp1_osc[i]);
 	}
 }
-#else
-static void stm32mp1_osc_init(void)
-{
-}
 #endif
 
 /*
@@ -1478,10 +1518,36 @@ static void sync_earlyboot_clocks_state(void)
 	stm32mp_register_clock_parents_secure(CRYP1);
 }
 
+static void _clock_mpu_suspend(void)
+{
+	uintptr_t mpckselr = stm32_rcc_base() + RCC_MPCKSELR;
+
+	if (((mmio_read_32(mpckselr) & RCC_SELR_SRC_MASK)) ==
+	    RCC_MPCKSELR_PLL) {
+		if (stm32mp1_set_clksrc(CLK_MPU_PLL1P_DIV)) {
+			panic();
+		}
+	}
+}
+
+static void _clock_mpu_resume(void)
+{
+	uintptr_t mpckselr = stm32_rcc_base() + RCC_MPCKSELR;
+
+	if (((mmio_read_32(mpckselr) & RCC_SELR_SRC_MASK)) ==
+	    RCC_MPCKSELR_PLL_MPUDIV) {
+		if (stm32mp1_set_clksrc(CLK_MPU_PLL1P)) {
+			panic();
+		}
+	}
+}
+
 static void _clock_resume(void)
 {
 	unsigned int idx;
 
+	_clock_mpu_resume();
+
 	/* Sync secure and shared clocks physical state on functional state */
 	for (idx = 0; idx < NB_GATES; idx++) {
 		struct stm32mp1_clk_gate const *gate = gate_ref(idx);
@@ -1504,7 +1570,7 @@ void stm32mp_clock_suspend_resume(enum pm_op op)
 {
 	switch (op) {
 	case PM_OP_SUSPEND:
-		/* Nothing to do */
+		_clock_mpu_suspend();
 		break;
 	case PM_OP_RESUME:
 		_clock_resume();
diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.c b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.c
index ed38575..f2b7fe1 100644
--- a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.c
+++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.c
@@ -499,7 +499,8 @@ static bool initialize_pmic_i2c(void)
 
 	ret = stm32_i2c_init(i2c, &i2c_init);
 	if (ret != 0) {
-		EMSG("Cannot initialize I2C %x (%d)\n", i2c->pbase, ret);
+		EMSG("Cannot initialize I2C %" PRIx32 "  (%d)",
+		     i2c_info.base, ret);
 		panic();
 	}
 
diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.h b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.h
index f50943d..01f15fd 100644
--- a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.h
+++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.h
@@ -14,17 +14,17 @@ void stm32mp_pmic_apply_lp_config(const char *lp_state);
 void stm32mp_get_pmic(void);
 void stm32mp_put_pmic(void);
 #else
-void stm32mp_pmic_apply_boot_on_config(void)
+static inline void stm32mp_pmic_apply_boot_on_config(void)
 {
 }
-void stm32mp_pmic_apply_lp_config(const char *lp_state)
+static inline void stm32mp_pmic_apply_lp_config(const char *lp_state __unused)
 {
 }
-void stm32mp_get_pmic(void)
+static inline void stm32mp_get_pmic(void)
 {
 	panic();
 }
-void stm32mp_put_pmic(void)
+static inline void stm32mp_put_pmic(void)
 {
 	panic();
 }
diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pwr.h b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pwr.h
index 1d44e8b..ed23259 100644
--- a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pwr.h
+++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pwr.h
@@ -1,6 +1,6 @@
 /* SPDX-License-Identifier: BSD-3-Clause */
 /*
- * Copyright (c) 2017-2018, STMicroelectronics
+ * Copyright (c) 2017-2019, STMicroelectronics
  */
 
 #ifndef __STM32MP1_PWR_H__
@@ -24,6 +24,8 @@
 
 #define PWR_CR2_BREN		BIT(0)
 #define PWR_CR2_RREN		BIT(1)
+#define PWR_CR2_BRRDY		BIT(16)
+#define PWR_CR2_RRRDY		BIT(17)
 
 #define PWR_CR3_VBE		BIT(8)
 #define PWR_CR3_VBRS		BIT(9)
diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_rcc.h b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_rcc.h
index 6a7a665..690c76f 100644
--- a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_rcc.h
+++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_rcc.h
@@ -387,7 +387,8 @@
 #define RCC_HSICFGR_HSITRIM_SHIFT	8
 #define RCC_HSICFGR_HSITRIM_MASK	GENMASK_32(14, 8)
 #define RCC_HSICFGR_HSICAL_SHIFT	16
-#define RCC_HSICFGR_HSICAL_MASK		GENMASK_32(27, 16)
+#define RCC_HSICFGR_HSICAL_MASK		GENMASK_32(24, 16)
+#define RCC_HSICFGR_HSICAL_TEMP_MASK	GENMASK_32(27, 25)
 
 /* Fields of RCC_CSICFGR register */
 #define RCC_CSICFGR_CSITRIM_SHIFT	8
@@ -503,6 +504,9 @@
 #define RCC_AHB5RSTSETR_RNG1RST			BIT(6)
 #define RCC_AHB5RSTSETR_AXIMCRST		BIT(16)
 
+/* RCC_MP_AHB6RST(SET|CLR)R bit fields */
+#define RCC_AHB6RSTSETR_GPURST			BIT(5)
+
 /* RCC_MP_AHB5EN(SET|CLR)R bit fields */
 #define RCC_MP_AHB5ENSETR_GPIOZEN_POS		0
 #define RCC_MP_AHB5ENSETR_CRYP1EN_POS		4
diff --git a/core/arch/arm/plat-stm32mp1/main.c b/core/arch/arm/plat-stm32mp1/main.c
index 1921678..b501541 100644
--- a/core/arch/arm/plat-stm32mp1/main.c
+++ b/core/arch/arm/plat-stm32mp1/main.c
@@ -136,7 +136,7 @@ const struct thread_handlers *generic_boot_get_handlers(void)
 #define CONSOLE_WITHOUT_CLOCK_MAGIC	~0UL
 
 static struct stm32_uart_pdata console_data;
-static struct serial_chip *serial_console;
+static struct serial_chip *serial_console __maybe_unused;
 
 void console_init(void)
 {
@@ -202,7 +202,7 @@ void console_flush(void)
 }
 #endif
 
-#if CFG_DT
+#ifdef CFG_DT
 /* Probe console once clocks inits (service_init level) are completed */
 static TEE_Result stm32_uart_console_probe(void)
 {
diff --git a/core/arch/arm/plat-stm32mp1/pm/context.c b/core/arch/arm/plat-stm32mp1/pm/context.c
index 2ce72b3..20e9e84 100644
--- a/core/arch/arm/plat-stm32mp1/pm/context.c
+++ b/core/arch/arm/plat-stm32mp1/pm/context.c
@@ -273,15 +273,16 @@ static struct mobj *teeram_bkp_mobj;
 static void init_retram_resume_resources(void)
 {
 	struct retram_resume_ctx *ctx = get_retram_resume_ctx();
-	const size_t size = (uintptr_t)stm32mp_bkpsram_image_end -
-			    (uintptr_t)stm32mp_bkpsram_resume;
+	size_t __maybe_unused csize;
 	paddr_t __maybe_unused pa;
 
 	COMPILE_TIME_ASSERT(sizeof(struct pm_mailbox) <
 			    BKPSRAM_PM_MAILBOX_SIZE);
 	COMPILE_TIME_ASSERT(sizeof(struct retram_resume_ctx) <
 			    BKPSRAM_PM_CONTEXT_SIZE);
-	assert((sizeof(*ctx) + size) < BKPSRAM_PM_CONTEXT_SIZE);
+	csize = (uintptr_t)stm32mp_bkpsram_image_end -
+		(uintptr_t)stm32mp_bkpsram_resume;
+	assert((sizeof(*ctx) + csize) < BKPSRAM_PM_CONTEXT_SIZE);
 
 	teeram_bkp_mobj = mobj_mm_alloc(mobj_sec_ddr, TEE_RAM_PH_SIZE,
 					&tee_mm_sec_ddr);
@@ -424,21 +425,25 @@ static void save_teeram_in_ddr(void)
 static void enable_pm_mailbox(unsigned int suspend)
 {
 	struct pm_mailbox *mailbox = get_pm_mailbox();
-	uint32_t magic = BOOT_API_A7_CORE0_MAGIC_NUMBER;
+	uint32_t magic = 0;
 	uint32_t hint = 0;
 
 	assert(stm32_clock_is_enabled(BKPSRAM) &&
 	       stm32_clock_is_enabled(RTCAPB));
 
 	if (suspend) {
+		magic = BOOT_API_A7_CORE0_MAGIC_NUMBER;
+		mailbox->magic = STANDBY_CONTEXT_MAGIC;
+
 		hint = virt_to_phys(&get_retram_resume_ctx()->resume_sequence);
+	} else {
+		mailbox->magic = 0;
 	}
 
 	write32(magic, stm32mp_bkpreg(BCKR_CORE1_MAGIC_NUMBER));
 	write32(hint, stm32mp_bkpreg(BCKR_CORE1_BRANCH_ADDRESS));
 
 	mailbox->core0_resume_ep = hint;
-	mailbox->magic = STANDBY_CONTEXT_MAGIC;
 }
 
 static void gate_pm_context_clocks(bool enable)
diff --git a/core/arch/arm/plat-stm32mp1/pm/low_power.c b/core/arch/arm/plat-stm32mp1/pm/low_power.c
index bd28014..48bddb7 100644
--- a/core/arch/arm/plat-stm32mp1/pm/low_power.c
+++ b/core/arch/arm/plat-stm32mp1/pm/low_power.c
@@ -1,6 +1,6 @@
 // SPDX-License-Identifier: BSD-3-Clause
 /*
- * Copyright (c) 2017-2018, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017-2019, STMicroelectronics - All Rights Reserved
  */
 
 #include <arm.h>
@@ -9,6 +9,7 @@
 #include <console.h>
 #include <drivers/gic.h>
 #include <drivers/stm32_iwdg.h>
+#include <drivers/stm32_reset.h>
 #include <drivers/stm32_rtc.h>
 #include <drivers/stm32mp1_ddrc.h>
 #include <drivers/stm32mp1_pmic.h>
@@ -16,7 +17,9 @@
 #include <drivers/stm32mp1_rcc.h>
 #include <drivers/stpmic1.h>
 #include <dt-bindings/clock/stm32mp1-clks.h>
+#include <dt-bindings/etzpc/stm32-etzpc.h>
 #include <dt-bindings/power/stm32mp1-power.h>
+#include <dt-bindings/reset/stm32mp1-resets.h>
 #include <initcall.h>
 #include <io.h>
 #include <keep.h>
@@ -26,6 +29,7 @@
 #include <kernel/panic.h>
 #include <mm/core_memprot.h>
 #include <platform_config.h>
+#include <sm/psci.h>
 #include <stdbool.h>
 #include <stm32mp_pm.h>
 #include <stm32_util.h>
@@ -139,11 +143,20 @@ void stm32_pm_cpu_wfi(void)
 	cpu_wfi();
 }
 
-/*If IWDG is not supported, provide a stubbed weak watchdog kicker */
+/* If IWDG is not supported, provide a stubbed weak watchdog kicker */
 void __weak stm32_iwdg_refresh(uint32_t __unused instance)
 {
 }
 
+#define ARM_CNTXCTL_IMASK	BIT(1)
+
+static void stm32mp_mask_timer(void)
+{
+	/* Mask timer interrupts */
+	write_cntp_ctl(read_cntp_ctl() | ARM_CNTXCTL_IMASK);
+	write_cntv_ctl(read_cntv_ctl() | ARM_CNTXCTL_IMASK);
+}
+
 /*
  * stm32_enter_cstop - Prepare CSTOP mode
  *
@@ -192,6 +205,16 @@ int stm32_enter_cstop(uint32_t mode)
 
 	ddr_in_selfrefresh = (rc == 0);
 
+	if (mode == STM32_PM_CSTOP_ALLOW_STANDBY_DDR_SR) {
+		/* Keep retention and backup RAM content in standby */
+		mmio_setbits_32(pwr_base + PWR_CR2_OFF, PWR_CR2_BREN |
+				PWR_CR2_RREN);
+		while ((mmio_read_32(pwr_base + PWR_CR2_OFF) &
+			(PWR_CR2_BRRDY | PWR_CR2_RRRDY)) == 0U) {
+			;
+		}
+	}
+
 	return rc;
 }
 
@@ -200,6 +223,7 @@ int stm32_enter_cstop(uint32_t mode)
  */
 void stm32_exit_cstop(void)
 {
+	uintptr_t pwr_base = stm32_pwr_base();
 	uintptr_t rcc_base = stm32_rcc_base();
 
 	if (ddr_in_selfrefresh) {
@@ -220,27 +244,163 @@ void stm32_exit_cstop(void)
 
 	dsb();
 	isb();
+
+	/* Disable retention and backup RAM content after stop */
+	mmio_clrbits_32(pwr_base + PWR_CR2_OFF, PWR_CR2_BREN | PWR_CR2_RREN);
+}
+
+/*
+ * GIC support required in low power sequences and reset sequences
+ */
+#define GICC_IAR			0x00C
+#define GICC_IT_ID_MASK			0x3ff
+#define GICC_EOIR			0x010
+#define GICC_HPPIR			0x018
+#define GICC_AHPPIR			0x028
+#define GIC_PENDING_G1_INTID		1022U
+#define GIC_SPURIOUS_INTERRUPT		1023U
+#define GIC_NUM_INTS_PER_REG		32
+#define GIC_MAX_SPI_ID			1020
+#define GICD_ICENABLER(n)		(0x180 + (n) * 4)
+
+static void clear_pending_interrupts(void)
+{
+	uint32_t id;
+	uintptr_t gicc_base = get_gicc_base();
+	uintptr_t gicd_base = get_gicd_base();
+
+	do {
+		id = read32(gicc_base + GICC_HPPIR) & GICC_IT_ID_MASK;
+
+		/*
+		 * Find out which interrupt it is under the
+		 * assumption that the GICC_CTLR.AckCtl bit is 0.
+		 */
+		if (id == GIC_PENDING_G1_INTID)
+			id = read32(gicc_base + GICC_AHPPIR) & GICC_IT_ID_MASK;
+
+		if (id < GIC_MAX_SPI_ID) {
+			size_t idx = id / GIC_NUM_INTS_PER_REG;
+			uint32_t mask = 1 << (id % GIC_NUM_INTS_PER_REG);
+
+			write32(id, gicc_base + GICC_EOIR);
+
+			write32(mask, gicd_base + GICD_ICENABLER(idx));
+
+			dsb_ishst();
+		}
+	} while (id < GIC_MAX_SPI_ID);
+}
+
+void stm32mp_gic_set_end_of_interrupt(uint32_t it)
+{
+	uintptr_t gicc_base = get_gicc_base();
+
+	write32(it, gicc_base + GICC_EOIR);
+}
+
+static void __noreturn wait_cpu_reset(void)
+{
+	dcache_op_all(DCACHE_OP_CLEAN_INV);
+	write_sctlr(read_sctlr() & ~SCTLR_C);
+	dcache_op_all(DCACHE_OP_CLEAN_INV);
+	__asm__("clrex");
+
+	dsb();
+	isb();
+
+	for ( ; ; ) {
+		clear_pending_interrupts();
+		wfi();
+	}
 }
 
+/*
+ * tzc_source_ip contains the TZC transaction source IPs that need to be reset
+ * before a C-A7 subsystem is reset (i.e. independent reset):
+ * - C-A7 subsystem is reset separately later in the sequence,
+ * - C-M4 subsystem is not concerned here,
+ * - DAP is excluded for debug purpose,
+ * - IPs are stored with their ETZPC IDs (STM32MP1_ETZPC_MAX_ID if not
+ *   applicable) because some of them need to be reset only if they are not
+ *   configured in MCU isolation mode inside ETZPC device tree.
+ */
+struct tzc_source_ip {
+	uint32_t reset_id;
+	uint32_t clock_id;
+	uint32_t decprot_id;
+};
+
+#define _TZC_FIXED(res, clk)				\
+	{						\
+		.reset_id = (res),			\
+		.clock_id = (clk),			\
+		.decprot_id = STM32MP1_ETZPC_MAX_ID,	\
+	}
+
+#define _TZC_COND(res, clk, decprot)			\
+	{						\
+		.reset_id = (res),			\
+		.clock_id = (clk),			\
+		.decprot_id = (decprot),		\
+	}
+
+static const struct tzc_source_ip tzc_source_ip[] = {
+	_TZC_FIXED(LTDC_R, LTDC_PX),
+	_TZC_FIXED(GPU_R, GPU),
+	_TZC_FIXED(USBH_R, USBH),
+	_TZC_FIXED(SDMMC1_R, SDMMC1_K),
+	_TZC_FIXED(SDMMC2_R, SDMMC2_K),
+	_TZC_FIXED(MDMA_R, MDMA),
+	_TZC_COND(USBO_R, USBO_K, STM32MP1_ETZPC_OTG_ID),
+	_TZC_COND(SDMMC3_R, SDMMC3_K, STM32MP1_ETZPC_SDMMC3_ID),
+	_TZC_COND(ETHMAC_R, ETHMAC, STM32MP1_ETZPC_ETH_ID),
+	_TZC_COND(DMA1_R, DMA1, STM32MP1_ETZPC_DMA1_ID),
+	_TZC_COND(DMA2_R, DMA2, STM32MP1_ETZPC_DMA2_ID),
+};
+
 static void __noreturn reset_cores(void)
 {
 	uintptr_t rcc_base = stm32_rcc_base();
-	uint32_t reset_mask;
+	uint32_t reset_mask = RCC_MP_GRSTCSETR_MPUP0RST |
+			      RCC_MP_GRSTCSETR_MPUP1RST;
 	uint32_t target_mask;
+	uint32_t id;
+
+	/* Mask timer interrupts */
+	stm32mp_mask_timer();
+
+	for (id = 0U; id < ARRAY_SIZE(tzc_source_ip); id++) {
+		if ((!stm32mp1_clk_is_enabled(tzc_source_ip[id].clock_id)) ||
+		    ((tzc_source_ip[id].decprot_id != STM32MP1_ETZPC_MAX_ID) &&
+		     (etzpc_get_decprot(tzc_source_ip[id].decprot_id) ==
+		      TZPC_DECPROT_MCU_ISOLATION))) {
+			continue;
+		}
+
+		if (tzc_source_ip[id].reset_id != GPU_R) {
+			stm32_reset_assert(tzc_source_ip[id].reset_id);
+			stm32_reset_deassert(tzc_source_ip[id].reset_id);
+		} else {
+			/* GPU reset automatically cleared by hardware */
+			mmio_setbits_32(rcc_base + RCC_AHB6RSTSETR,
+					RCC_AHB6RSTSETR_GPURST);
+		}
+	}
 
 	if (get_core_pos() == 0) {
-		reset_mask = RCC_MP_GRSTCSETR_MPUP0RST;
 		target_mask = TARGET_CPU1_GIC_MASK;
 	} else {
-		reset_mask = RCC_MP_GRSTCSETR_MPUP1RST;
 		target_mask = TARGET_CPU0_GIC_MASK;
 	}
 
 	itr_raise_sgi(GIC_SEC_SGI_1, target_mask);
-	dcache_op_all(DCACHE_OP_CLEAN_INV);
+
+	clear_pending_interrupts();
+
 	write32(reset_mask, rcc_base + RCC_MP_GRSTCSETR);
-	cpu_wfi();
-	panic("Cores reset");
+
+	wait_cpu_reset();
 }
 
 /*
@@ -252,6 +412,25 @@ void __noreturn stm32_cores_reset(void)
 }
 KEEP_PAGER(stm32_cores_reset);
 
+static void reset_other_core(void)
+{
+	uintptr_t rcc_base = stm32_rcc_base();
+	uint32_t reset_mask;
+	uint32_t target_mask;
+
+	if (get_core_pos() == 0) {
+		reset_mask = RCC_MP_GRSTCSETR_MPUP1RST;
+		target_mask = TARGET_CPU1_GIC_MASK;
+	} else {
+		reset_mask = RCC_MP_GRSTCSETR_MPUP0RST;
+		target_mask = TARGET_CPU0_GIC_MASK;
+	}
+
+	itr_raise_sgi(GIC_SEC_SGI_1, target_mask);
+
+	write32(reset_mask, rcc_base + RCC_MP_GRSTCSETR);
+}
+
 /*
  * stm32_enter_cstop_shutdown - Shutdown CPUs to target low power mode
  * @mode - Target low power mode
@@ -271,9 +450,13 @@ void __noreturn stm32_enter_cstop_shutdown(uint32_t mode)
 	case STM32_PM_CSTOP_ALLOW_STANDBY_DDR_OFF:
 #ifdef STM32MP1_USE_MPU0_RESET
 		stm32mp_pm_shutdown_context();
+		reset_other_core();
 		stm32_enter_cstop(mode);
-		cpu_wfi();
-		reset_cores();
+		dsb();
+		isb();
+		for ( ; ; ) {
+			wfi();
+		}
 #else
 		if (stm32mp_with_pmic()) {
 			wait_console_flushed();
@@ -304,14 +487,10 @@ void __noreturn stm32_enter_cstop_reset(uint32_t mode)
 		udelay(100);
 		break;
 	default:
-#ifdef STM32MP1_USE_MPU0_RESET
-		reset_cores();
-#else
 		IMSG("Forced system reset");
 		wait_console_flushed();
 		write32(RCC_MP_GRSTCSETR_MPSYSRST, rcc_base + RCC_MP_GRSTCSETR);
 		udelay(100);
-#endif
 		break;
 	}
 
@@ -336,13 +515,6 @@ void stm32_enter_csleep(void)
 	stm32_pm_cpu_wfi();
 }
 
-/*
- * Secure interrupts used in the low power sequences
- */
-#define GICC_IAR		0x00C
-#define GICC_IAR_IT_ID_MASK	0x3ff
-#define GICC_EOIR		0x010
-
 /* RCC Wakeup interrupt is used to wake from suspeneded mode */
 static enum itr_return rcc_wakeup_it_handler(struct itr_handler *hdl __unused)
 {
@@ -360,21 +532,14 @@ KEEP_PAGER(rcc_wakeup_handler);
 /* SGI9 (secure SGI 1) informs targeted CPU it shall reset */
 static enum itr_return sgi9_it_handler(struct itr_handler *handler)
 {
-	uintptr_t rcc_base = stm32_rcc_base();
-	uint32_t reset_mask;
-	uintptr_t gicc_base = get_gicc_base();
+	stm32mp_mask_timer();
 
-	write32(handler->it, gicc_base + GICC_EOIR);
+	stm32mp_gic_set_end_of_interrupt(handler->it);
 
-	if (get_core_pos() == 0) {
-		reset_mask = RCC_MP_GRSTCSETR_MPUP0RST;
-	} else {
-		reset_mask = RCC_MP_GRSTCSETR_MPUP1RST;
-	}
+	clear_pending_interrupts();
+
+	wait_cpu_reset();
 
-	dcache_op_all(DCACHE_OP_CLEAN_INV);
-	write32(reset_mask, rcc_base + RCC_MP_GRSTCSETR);
-	cpu_wfi();
 	panic("Core reset");
 
 	return ITRR_HANDLED;
@@ -388,18 +553,12 @@ KEEP_PAGER(sgi9_reset_handler);
 
 static TEE_Result init_low_power(void)
 {
-	uintptr_t pwr_base = stm32_pwr_base();
-
 	itr_add(&rcc_wakeup_handler);
 	itr_enable(rcc_wakeup_handler.it);
 
 	itr_add(&sgi9_reset_handler);
 	itr_enable(sgi9_reset_handler.it);
 
-	/* Enable retention for BKPSRAM and BKPREG */
-	io_mask32(pwr_base + PWR_CR2_OFF,
-		  PWR_CR2_BREN | PWR_CR2_RREN, PWR_CR2_BREN | PWR_CR2_RREN);
-
 	return TEE_SUCCESS;
 }
 service_init(init_low_power);
@@ -412,11 +571,8 @@ void __noreturn stm32_pm_cpu_power_down_wfi(void)
 	if (get_core_pos() == 0) {
 		void (*reset_ep)(void) = stm32mp_sysram_resume;
 
-		wait_console_flushed();
+		stm32_pm_cpu_wfi();
 
-		dsb();
-		isb();
-		wfi();
 		/* STANDBY not reached: resume from retained SYSRAM */
 		stm32_exit_cstop();
 		stm32mp_cpu_reset_state();
diff --git a/core/arch/arm/plat-stm32mp1/pm/pm_helpers.S b/core/arch/arm/plat-stm32mp1/pm/pm_helpers.S
index 91c7580..d9ed746 100644
--- a/core/arch/arm/plat-stm32mp1/pm/pm_helpers.S
+++ b/core/arch/arm/plat-stm32mp1/pm/pm_helpers.S
@@ -621,14 +621,14 @@ UNWIND(	.save	{r12, lr})
 	bl	psci_armv7_cpu_off
 
 	write_bpiall
-	isb
 	dsb
+	isb
 	read_sctlr r0
 	bic	r0, r0, #SCTLR_M
 	bic	r0, r0, #SCTLR_I
 	write_sctlr r0
-	isb
 	dsb	sy
+	isb
 
 	pop	{r12, pc}
 UNWIND(	.fnend)
diff --git a/core/arch/arm/plat-stm32mp1/pm/psci.c b/core/arch/arm/plat-stm32mp1/pm/psci.c
index 6e8219e..d3fb974 100644
--- a/core/arch/arm/plat-stm32mp1/pm/psci.c
+++ b/core/arch/arm/plat-stm32mp1/pm/psci.c
@@ -322,7 +322,7 @@ int psci_system_suspend(uintptr_t entry, uint32_t context_id __unused,
 {
 	int ret = PSCI_RET_INVALID_PARAMETERS;
 	uint32_t soc_mode;
-	int pos = get_core_pos();
+	int __maybe_unused pos = get_core_pos();
 
 	DMSG("core %u", pos);
 
diff --git a/core/arch/arm/plat-stm32mp1/shared_resources.c b/core/arch/arm/plat-stm32mp1/shared_resources.c
index 0eb929c..69b87cd 100644
--- a/core/arch/arm/plat-stm32mp1/shared_resources.c
+++ b/core/arch/arm/plat-stm32mp1/shared_resources.c
@@ -264,7 +264,7 @@ static unsigned int decprot2shres(unsigned int decprot_id)
 	return SHRES_INVALID;
 }
 
-static const char *decprot2str(unsigned int decprot_id)
+static const char __maybe_unused *decprot2str(unsigned int decprot_id)
 {
 	size_t i;
 
@@ -279,7 +279,7 @@ static const char *decprot2str(unsigned int decprot_id)
 }
 
 /* GPIOZ bank may have several number of pins */
-#if CFG_DT
+#ifdef CFG_DT
 static int gpioz_nbpin = -1;
 
 static unsigned int get_gpioz_nbpin_unpg(void)
@@ -982,7 +982,7 @@ static TEE_Result stm32mp1_init_drivers(void)
 	registering_locked = true;
 
 	for (id = 0; id < STM32MP1_SHRES_COUNT; id++) {
-		uint8_t *state = &shres_state[id];
+		uint8_t __maybe_unused *state = &shres_state[id];
 
 #if TRACE_LEVEL == TRACE_INFO
 		/* Display only the secure and shared resources */
diff --git a/core/arch/arm/plat-stm32mp1/stm32_util.h b/core/arch/arm/plat-stm32mp1/stm32_util.h
index 0164855..347499b 100644
--- a/core/arch/arm/plat-stm32mp1/stm32_util.h
+++ b/core/arch/arm/plat-stm32mp1/stm32_util.h
@@ -31,6 +31,7 @@ uintptr_t stm32_get_stgen_base(void);
 /* Platform util for the GIC */
 uintptr_t get_gicc_base(void);
 uintptr_t get_gicd_base(void);
+void stm32mp_gic_set_end_of_interrupt(uint32_t it);
 
 /* Platform util for clock gating. ID refers to clock DT bindings ID. */
 void stm32_clock_enable(unsigned long id);
diff --git a/core/drivers/stm32_bsec.c b/core/drivers/stm32_bsec.c
index 3355e0a..7fcefdb 100644
--- a/core/drivers/stm32_bsec.c
+++ b/core/drivers/stm32_bsec.c
@@ -1,6 +1,6 @@
 // SPDX-License-Identifier: BSD-3-Clause
 /*
- * Copyright (c) 2017-2018, STMicroelectronics
+ * Copyright (c) 2017-2019, STMicroelectronics
  */
 
 #include <assert.h>
@@ -133,23 +133,28 @@ static uintptr_t bsec_get_base(void)
 }
 
 /*
- * bsec_check_error
- * otp : OTP number.
- * return value : BSEC_OK if no error.
+ * bsec_check_error: check BSEC error status.
+ * otp: OTP number.
+ * check_disturbed: check only error (false) or all sources (true).
+ * return value: BSEC_OK if no error.
  */
-static uint32_t bsec_check_error(uint32_t otp)
+static uint32_t bsec_check_error(uint32_t otp, bool check_disturbed)
 {
 	uint32_t bit = BIT(otp & BSEC_OTP_MASK);
 	uint32_t bank = otp_bank_offset(otp);
 
-	if ((read32(bsec_get_base() + BSEC_DISTURBED_OFF + bank) & bit) != 0U) {
-		return BSEC_DISTURBED;
-	}
-
 	if ((read32(bsec_get_base() + BSEC_ERROR_OFF + bank) & bit) != 0U) {
 		return BSEC_ERROR;
 	}
 
+	if (!check_disturbed) {
+		return BSEC_OK;
+	}
+
+	if ((read32(bsec_get_base() + BSEC_DISTURBED_OFF + bank) & bit) != 0U) {
+		return BSEC_DISTURBED;
+	}
+
 	return BSEC_OK;
 }
 
@@ -162,14 +167,16 @@ uint32_t bsec_shadow_register(uint32_t otp)
 {
 	uint32_t result;
 	uint32_t exc;
+	bool value;
 
-	if (otp > stm32mp_get_otp_max()) {
-		return BSEC_INVALID_PARAM;
+	result = bsec_read_sr_lock(otp, &value);
+	if (result != BSEC_OK) {
+		DMSG("BSEC: %u Sticky-read bit read Error %i", otp, result);
+		return result;
 	}
 
-	/* Check if shadowing of OTP is locked */
-	if (bsec_read_sr_lock(otp)) {
-		IMSG("BSEC: OTP locked, register will not be refreshed");
+	if (value) {
+		DMSG("BSEC: OTP locked, register will not be refreshed");
 	}
 
 	result = bsec_power_safmem(true);
@@ -185,7 +192,7 @@ uint32_t bsec_shadow_register(uint32_t otp)
 		;
 	}
 
-	result = bsec_check_error(otp);
+	result = bsec_check_error(otp, true);
 
 	bsec_unlock(exc);
 
@@ -203,22 +210,15 @@ uint32_t bsec_shadow_register(uint32_t otp)
 uint32_t bsec_read_otp(uint32_t *val, uint32_t otp)
 {
 	uint32_t exc;
-	uint32_t result;
 
 	if (otp > stm32mp_get_otp_max()) {
 		return BSEC_INVALID_PARAM;
 	}
 
-	exc = bsec_lock();
-
 	*val = read32(bsec_get_base() + BSEC_OTP_DATA_OFF +
 		      (otp * sizeof(uint32_t)));
 
-	result = bsec_check_error(otp);
-
-	bsec_unlock(exc);
-
-	return result;
+	return BSEC_OK;
 }
 
 /*
@@ -231,23 +231,24 @@ uint32_t bsec_write_otp(uint32_t val, uint32_t otp)
 {
 	uint32_t exc;
 	uint32_t result;
+	bool value;
 
-	if (otp > stm32mp_get_otp_max()) {
-		return BSEC_INVALID_PARAM;
+	result = bsec_read_sw_lock(otp, &value);
+	if (result != BSEC_OK) {
+		DMSG("BSEC: %u Sticky-write bit read Error %i", otp, result);
+		return result;
 	}
 
-	/* Check if programming of OTP is locked */
-	if (bsec_read_sw_lock(otp)) {
+	if (value) {
 		IMSG("BSEC: OTP locked, write will be ignored");
 	}
 
+	/* Ensure integrity of each register access sequence */
 	exc = bsec_lock();
 
 	write32(val, bsec_get_base() + BSEC_OTP_DATA_OFF +
 		(otp * sizeof(uint32_t)));
 
-	result = bsec_check_error(otp);
-
 	bsec_unlock(exc);
 
 	return result;
@@ -264,13 +265,15 @@ uint32_t bsec_program_otp(uint32_t val, uint32_t otp)
 {
 	uint32_t result;
 	uint32_t exc;
+	bool value;
 
-	if (otp > stm32mp_get_otp_max()) {
-		return BSEC_INVALID_PARAM;
+	result = bsec_read_sp_lock(otp, &value);
+	if (result != BSEC_OK) {
+		DMSG("BSEC: %u Sticky-prog bit read Error %i", otp, result);
+		return result;
 	}
 
-	/* Check if programming of OTP is locked */
-	if (bsec_read_sp_lock(otp)) {
+	if (value) {
 		IMSG("BSEC: OTP locked, prog will be ignored");
 	}
 
@@ -296,7 +299,7 @@ uint32_t bsec_program_otp(uint32_t val, uint32_t otp)
 	if ((bsec_get_status() & BSEC_MODE_PROGFAIL_MASK) != 0U) {
 		result = BSEC_PROG_FAIL;
 	} else {
-		result = bsec_check_error(otp);
+		result = bsec_check_error(otp, true);
 	}
 
 	bsec_unlock(exc);
@@ -350,7 +353,7 @@ uint32_t bsec_permanent_lock_otp(uint32_t otp)
 	if ((bsec_get_status() & BSEC_MODE_PROGFAIL_MASK) != 0U) {
 		result = BSEC_PROG_FAIL;
 	} else {
-		result = bsec_check_error(otp);
+		result = bsec_check_error(otp, false);
 	}
 
 	bsec_unlock(exc);
@@ -361,7 +364,7 @@ uint32_t bsec_permanent_lock_otp(uint32_t otp)
 }
 
 /*
- * bsec_write_debug_conf: write value in debug feature
+ * bsec_write_debug_conf: write value in debug feature.
  *	to enable/disable debug service.
  * val: value to write.
  * return value: BSEC_OK if no error.
@@ -386,7 +389,7 @@ uint32_t bsec_write_debug_conf(uint32_t val)
 }
 
 /*
- * bsec_read_debug_conf : read debug configuration.
+ * bsec_read_debug_conf: return debug configuration register value.
  */
 uint32_t bsec_read_debug_conf(void)
 {
@@ -394,7 +397,7 @@ uint32_t bsec_read_debug_conf(void)
 }
 
 /*
- * bsec_get_status : return status register value.
+ * bsec_get_status: return status register value.
  */
 uint32_t bsec_get_status(void)
 {
@@ -402,7 +405,7 @@ uint32_t bsec_get_status(void)
 }
 
 /*
- * bsec_get_hw_conf : return hardware configuration.
+ * bsec_get_hw_conf: return hardware configuration register value.
  */
 uint32_t bsec_get_hw_conf(void)
 {
@@ -410,7 +413,7 @@ uint32_t bsec_get_hw_conf(void)
 }
 
 /*
- * bsec_get_version : return BSEC version.
+ * bsec_get_version: return BSEC version register value.
  */
 uint32_t bsec_get_version(void)
 {
@@ -418,7 +421,7 @@ uint32_t bsec_get_version(void)
 }
 
 /*
- * bsec_get_id : return BSEC ID.
+ * bsec_get_id: return BSEC ID register value.
  */
 uint32_t bsec_get_id(void)
 {
@@ -426,7 +429,7 @@ uint32_t bsec_get_id(void)
 }
 
 /*
- * bsec_get_magic_id : return BSEC magic number.
+ * bsec_get_magic_id: return BSEC magic number register value.
  */
 uint32_t bsec_get_magic_id(void)
 {
@@ -434,232 +437,177 @@ uint32_t bsec_get_magic_id(void)
 }
 
 /*
- * bsec_write_sr_lock: write shadow-read lock.
+ * bsec_set_sr_lock: set shadow-read lock.
  * otp: OTP number.
- * value: value to write in the register.
- *	Must be always 1.
- * return: true if OTP is locked, else false.
+ * return value: BSEC_OK if no error.
  */
-bool bsec_write_sr_lock(uint32_t otp, uint32_t value)
+uint32_t bsec_set_sr_lock(uint32_t otp)
 {
-	bool result = false;
 	uint32_t bank = otp_bank_offset(otp);
-	uint32_t bank_value;
 	uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
 	uint32_t exc;
 
-	exc = bsec_lock();
-
-	bank_value = read32(bsec_get_base() + BSEC_SRLOCK_OFF + bank);
-
-	if ((bank_value & otp_mask) == value) {
-		/*
-		 * In case of write don't need to write,
-		 * the lock is already set.
-		 */
-		if (value != 0U) {
-			result = true;
-		}
-	} else {
-		if (value != 0U) {
-			bank_value = bank_value | otp_mask;
-		} else {
-			bank_value = bank_value & ~otp_mask;
-		}
-
-		/*
-		 * We can write 0 in all other OTP
-		 * if the lock is activated in one of other OTP.
-		 * Write 0 has no effect.
-		 */
-		write32(bank_value, bsec_get_base() + BSEC_SRLOCK_OFF + bank);
-		result = true;
+	if (otp > STM32MP1_OTP_MAX_ID) {
+		return BSEC_INVALID_PARAM;
 	}
 
+	exc = bsec_lock();
+	write32(otp_mask, bsec_get_base() + BSEC_SRLOCK_OFF + bank);
 	bsec_unlock(exc);
 
-	return result;
+	return BSEC_OK;
 }
 
 /*
  * bsec_read_sr_lock: read shadow-read lock.
  * otp: OTP number.
- * return: true if otp is locked, else false.
+ * value: read value (true or false).
+ * return value: BSEC_OK if no error.
  */
-bool bsec_read_sr_lock(uint32_t otp)
+uint32_t bsec_read_sr_lock(uint32_t otp, bool *value)
 {
 	uint32_t bank = otp_bank_offset(otp);
 	uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
-	uint32_t bank_value = read32(bsec_get_base() + BSEC_SRLOCK_OFF + bank);
+	uint32_t bank_value;
 
-	return (bank_value & otp_mask) != 0U;
+	if (otp > STM32MP1_OTP_MAX_ID) {
+		return BSEC_INVALID_PARAM;
+	}
+
+	bank_value = read32(bsec_get_base() + BSEC_SRLOCK_OFF + bank);
+	*value = ((bank_value & otp_mask) != 0U);
+
+	return BSEC_OK;
 }
 
 /*
- * bsec_write_sw_lock: write shadow-write lock.
+ * bsec_set_sw_lock: set shadow-write lock.
  * otp: OTP number.
- * value: Value to write in the register.
- *	Must be always 1.
- * return: true if OTP is locked, else false.
+ * return value: BSEC_OK if no error.
  */
-bool bsec_write_sw_lock(uint32_t otp, uint32_t value)
+uint32_t bsec_set_sw_lock(uint32_t otp)
 {
-	bool result = false;
 	uint32_t bank = otp_bank_offset(otp);
 	uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
-	uint32_t bank_value;
 	unsigned int exc;
 
-	exc = bsec_lock();
-
-	bank_value = read32(bsec_get_base() + BSEC_SWLOCK_OFF + bank);
-
-	if ((bank_value & otp_mask) == value) {
-		/*
-		 * In case of write don't need to write,
-		 * the lock is already set.
-		 */
-		if (value != 0U) {
-			result = true;
-		}
-	} else {
-		if (value != 0U) {
-			bank_value = bank_value | otp_mask;
-		} else {
-			bank_value = bank_value & ~otp_mask;
-		}
-
-		/*
-		 * We can write 0 in all other OTP
-		 * if the lock is activated in one of other OTP.
-		 * Write 0 has no effect.
-		 */
-		write32(bank_value, bsec_get_base() + BSEC_SWLOCK_OFF + bank);
-		result = true;
+	if (otp > STM32MP1_OTP_MAX_ID) {
+		return BSEC_INVALID_PARAM;
 	}
 
+	exc = bsec_lock();
+	write32(otp_mask, bsec_get_base() + BSEC_SWLOCK_OFF + bank);
 	bsec_unlock(exc);
 
-	return result;
+	return BSEC_OK;
 }
 
 /*
  * bsec_read_sw_lock: read shadow-write lock.
  * otp: OTP number.
- * return: true if OTP is locked, else false.
+ * value: read value (true or false).
+ * return value: BSEC_OK if no error.
  */
-bool bsec_read_sw_lock(uint32_t otp)
+uint32_t bsec_read_sw_lock(uint32_t otp, bool *value)
 {
 	uint32_t bank = otp_bank_offset(otp);
 	uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
-	uint32_t bank_value = read32(bsec_get_base() + BSEC_SWLOCK_OFF + bank);
+	uint32_t bank_value;
+
+	if (otp > STM32MP1_OTP_MAX_ID) {
+		return BSEC_INVALID_PARAM;
+	}
+
+	bank_value = read32(bsec_get_base() + BSEC_SWLOCK_OFF + bank);
+	*value = ((bank_value & otp_mask) != 0U);
 
-	return (bank_value & otp_mask) != 0U;
+	return BSEC_OK;
 }
 
 /*
- * bsec_write_sp_lock: write shadow-program lock.
+ * bsec_set_sp_lock: set shadow-program lock.
  * otp: OTP number.
- * value: Value to write in the register.
- *	Must be always 1.
- * return: true if OTP is locked, else false.
+ * return value: BSEC_OK if no error.
  */
-bool bsec_write_sp_lock(uint32_t otp, uint32_t value)
+uint32_t bsec_set_sp_lock(uint32_t otp)
 {
-	bool result = false;
 	uint32_t bank = otp_bank_offset(otp);
-	uint32_t bank_value;
 	uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
 	unsigned int exc;
 
-	exc = bsec_lock();
-
-	bank_value = read32(bsec_get_base() + BSEC_SPLOCK_OFF + bank);
-
-	if ((bank_value & otp_mask) == value) {
-		/*
-		 * In case of write don't need to write,
-		 * the lock is already set.
-		 */
-		if (value != 0U) {
-			result = true;
-		}
-	} else {
-		if (value != 0U) {
-			bank_value = bank_value | otp_mask;
-		} else {
-			bank_value = bank_value & ~otp_mask;
-		}
-
-		/*
-		 * We can write 0 in all other OTP
-		 * if the lock is activated in one of other OTP.
-		 * Write 0 has no effect.
-		 */
-		write32(bank_value, bsec_get_base() + BSEC_SPLOCK_OFF + bank);
-		result = true;
+	if (otp > STM32MP1_OTP_MAX_ID) {
+		return BSEC_INVALID_PARAM;
 	}
 
+	exc = bsec_lock();
+	write32(otp_mask, bsec_get_base() + BSEC_SPLOCK_OFF + bank);
 	bsec_unlock(exc);
 
-	return result;
+	return BSEC_OK;
 }
 
 /*
  * bsec_read_sp_lock: read shadow-program lock.
  * otp: OTP number.
- * return: true if OTP is locked, else false.
+ * value: read value (true or false).
+ * return value: BSEC_OK if no error.
  */
-bool bsec_read_sp_lock(uint32_t otp)
+uint32_t bsec_read_sp_lock(uint32_t otp, bool *value)
 {
 	uint32_t bank = otp_bank_offset(otp);
 	uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
-	uint32_t bank_value = read32(bsec_get_base() + BSEC_SPLOCK_OFF + bank);
+	uint32_t bank_value;
+
+	if (otp > STM32MP1_OTP_MAX_ID) {
+		return BSEC_INVALID_PARAM;
+	}
+
+	bank_value = read32(bsec_get_base() + BSEC_SPLOCK_OFF + bank);
+	*value = ((bank_value & otp_mask) != 0U);
 
-	return (bank_value & otp_mask) != 0U;
+	return BSEC_OK;
 }
 
 /*
- * bsec_wr_lock: Read permanent lock status.
+ * bsec_read_permanent_lock: Read permanent lock status.
  * otp: OTP number.
- * return: true if OTP is locked, else false.
+ * value: read value (true or false).
+ * return value: BSEC_OK if no error.
  */
-bool bsec_wr_lock(uint32_t otp)
+uint32_t bsec_read_permanent_lock(uint32_t otp, bool *value)
 {
 	uint32_t bank = otp_bank_offset(otp);
-	uint32_t lock_bit = BIT(otp & BSEC_OTP_MASK);
+	uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
+	uint32_t bank_value;
 
-	if ((read32(bsec_get_base() + BSEC_WRLOCK_OFF + bank) &
-	     lock_bit) != 0U) {
-		/*
-		 * In case of write don't need to write,
-		 * the lock is already set.
-		 */
-		return true;
+	if (otp > STM32MP1_OTP_MAX_ID) {
+		return BSEC_INVALID_PARAM;
 	}
 
-	return false;
+	bank_value = read32(bsec_get_base() + BSEC_WRLOCK_OFF + bank);
+	*value = ((bank_value & otp_mask) != 0U);
+
+	return BSEC_OK;
 }
 
 /*
- * bsec_otp_lock: Lock Upper OTP or Global programming or debug enable
+ * bsec_otp_lock: Lock Upper OTP or Global Programming or Debug Enable.
  * service: Service to lock see header file.
- * value: Value to write must always set to 1 (only use for debug purpose).
- * return: BSEC_OK if succeed.
+ * return value: BSEC_OK if no error.
  */
-uint32_t bsec_otp_lock(uint32_t service, uint32_t value)
+uint32_t bsec_otp_lock(uint32_t service)
 {
 	uintptr_t reg = bsec_get_base() + BSEC_OTP_LOCK_OFF;
 
 	switch (service) {
 	case BSEC_LOCK_UPPER_OTP:
-		write32(value << BSEC_LOCK_UPPER_OTP, reg);
+		write32(BIT(BSEC_LOCK_UPPER_OTP), reg);
 		break;
 	case BSEC_LOCK_DEBUG:
-		write32(value << BSEC_LOCK_DEBUG, reg);
+		write32(BIT(BSEC_LOCK_DEBUG), reg);
 		break;
 	case BSEC_LOCK_PROGRAM:
-		write32(value << BSEC_LOCK_PROGRAM, reg);
+		write32(BIT(BSEC_LOCK_PROGRAM), reg);
 		break;
 	default:
 		return BSEC_INVALID_PARAM;
@@ -703,7 +651,7 @@ static uint32_t disable_power(void)
 /*
  * bsec_power_safmem: Activate or deactivate SAFMEM power.
  * power: true to power up, false to power down.
- * return: BSEC_OK if succeed.
+ * return value: BSEC_OK if no error.
  */
 static uint32_t bsec_power_safmem(bool enable)
 {
@@ -733,23 +681,7 @@ static uint32_t bsec_power_safmem(bool enable)
 }
 
 /*
- * bsec_mode_is_closed_device: read OTP secure sub-mode.
- * return: false if open_device and true of closed_device.
- */
-bool bsec_mode_is_closed_device(void)
-{
-	uint32_t value;
-
-	if ((bsec_shadow_register(DATA0_OTP) != BSEC_OK) ||
-	    (bsec_read_otp(&value, DATA0_OTP) != BSEC_OK)) {
-		return true;
-	}
-
-	return (value & DATA0_OTP_SECURED) == DATA0_OTP_SECURED;
-}
-
-/*
- * bsec_shadow_read_otp: Load OTP from SAFMEM and provide its value
+ * bsec_shadow_read_otp: Load OTP from SAFMEM and provide its value.
  * otp_value: read value.
  * word: OTP number.
  * return value: BSEC_OK if no error.
@@ -775,7 +707,7 @@ uint32_t bsec_shadow_read_otp(uint32_t *otp_value, uint32_t word)
 /*
  * bsec_check_nsec_access_rights: check non-secure access rights to target OTP.
  * otp: OTP number.
- * return: BSEC_OK if authorized access.
+ * return value: BSEC_OK if authorized access.
  */
 uint32_t bsec_check_nsec_access_rights(uint32_t otp)
 {
@@ -784,16 +716,13 @@ uint32_t bsec_check_nsec_access_rights(uint32_t otp)
 	}
 
 	if (otp >= stm32mp_get_otp_upper_start()) {
-		/* Check if BSEC is in OTP-SECURED closed_device state. */
-		if (bsec_mode_is_closed_device()) {
 #ifdef CFG_DT
-			if (!non_secure_can_access(otp)) {
-				return BSEC_ERROR;
-			}
-#else
+		if (!non_secure_can_access(otp)) {
 			return BSEC_ERROR;
-#endif
 		}
+#else
+		return BSEC_ERROR;
+#endif
 	}
 
 	return BSEC_OK;
diff --git a/core/drivers/stm32_etzpc.c b/core/drivers/stm32_etzpc.c
index fcb32e6..daf7acb 100644
--- a/core/drivers/stm32_etzpc.c
+++ b/core/drivers/stm32_etzpc.c
@@ -103,12 +103,12 @@ static uintptr_t etzpc_base(void)
 	return etzpc_dev.base;
 }
 
-static bool valid_decprot_id(unsigned int id)
+static bool __maybe_unused valid_decprot_id(unsigned int id)
 {
 	return id < (unsigned int)etzpc_dev.num_per_sec;
 }
 
-static bool valid_tzma_id(unsigned int id)
+static bool __maybe_unused valid_tzma_id(unsigned int id)
 {
 	return id < (unsigned int)etzpc_dev.num_tzma;
 }
@@ -251,7 +251,6 @@ static void etzpc_suspend_resume(enum pm_op op, void __unused *handle)
 
 	/* OP-TEE owns the whole in SYSRAM */
 	etzpc_configure_tzma(1, ETZPC_TZMA_ALL_SECURE);
-	etzpc_lock_tzma(1);
 
 	for (n = 0; n < etzpc_dev.num_per_sec; n++) {
 		unsigned int attr = etzpc_dev.periph_cfg[n] & PERIPH_ATTR_MASK;
@@ -328,7 +327,6 @@ static TEE_Result etzpc_init(void)
 
 	/* OP-TEE owns the whole in SYSRAM */
 	etzpc_configure_tzma(1, ETZPC_TZMA_ALL_SECURE);
-	etzpc_lock_tzma(1);
 
 	stm32mp_register_pm_cb(etzpc_suspend_resume, NULL);
 
diff --git a/core/drivers/stm32_i2c.c b/core/drivers/stm32_i2c.c
index f7f0f70..8ba7178 100644
--- a/core/drivers/stm32_i2c.c
+++ b/core/drivers/stm32_i2c.c
@@ -76,18 +76,18 @@ struct i2c_timing_s {
 	bool is_saved;
 };
 
-/**
- * All these values are coming from I2C Specification, Version 6.0, 4th of
- * April 2014.
+/*
+ * I2C specification values as per version 6.0, 4th of April 2014 [1],
+ * table 10 page 48: Characteristics of the SDA and SCL bus lines for
+ * Standard, Fast, and Fast-mode Plus I2C-bus devices.
  *
- * Table10. Characteristics of the SDA and SCL bus lines for Standard, Fast,
- * and Fast-mode Plus I2C-bus devices.
+ * [1] https://www.nxp.com/docs/en/user-guide/UM10204.pdf
  */
 static const struct i2c_spec_s i2c_specs[] = {
 	[I2C_SPEED_STANDARD] = {
 		.rate = STANDARD_RATE,
-		.rate_min = 8000,
-		.rate_max = 120000,
+		.rate_min = (STANDARD_RATE * 80) / 100,
+		.rate_max = (STANDARD_RATE * 120) / 100,
 		.fall_max = 300,
 		.rise_max = 1000,
 		.hddat_min = 0,
@@ -98,8 +98,8 @@ static const struct i2c_spec_s i2c_specs[] = {
 	},
 	[I2C_SPEED_FAST] = {
 		.rate = FAST_RATE,
-		.rate_min = 320000,
-		.rate_max = 480000,
+		.rate_min = (FAST_RATE * 80) / 100,
+		.rate_max = (FAST_RATE * 120) / 100,
 		.fall_max = 300,
 		.rise_max = 300,
 		.hddat_min = 0,
@@ -110,8 +110,8 @@ static const struct i2c_spec_s i2c_specs[] = {
 	},
 	[I2C_SPEED_FAST_PLUS] = {
 		.rate = FAST_PLUS_RATE,
-		.rate_min = 800000,
-		.rate_max = 1200000,
+		.rate_min = (FAST_PLUS_RATE * 80) / 100,
+		.rate_max = (FAST_PLUS_RATE * 120) / 100,
 		.fall_max = 100,
 		.rise_max = 120,
 		.hddat_min = 0,
diff --git a/core/drivers/stm32_iwdg.c b/core/drivers/stm32_iwdg.c
index b2c8a68..f1cdc1a 100644
--- a/core/drivers/stm32_iwdg.c
+++ b/core/drivers/stm32_iwdg.c
@@ -115,6 +115,12 @@ static enum itr_return stm32_iwdg_it_handler(struct itr_handler *handler)
 
 	stm32_clock_disable(iwdg->clock);
 
+	/*
+	 * Ack interrupt as we do not return from next call.
+	 * And interrupt is no more considered as pending here.
+	 */
+	stm32mp_gic_set_end_of_interrupt(handler->it);
+
 	stm32_cores_reset();
 
 	return ITRR_HANDLED;
diff --git a/core/drivers/stm32_timer.c b/core/drivers/stm32_timer.c
index be03577..a99f2d9 100644
--- a/core/drivers/stm32_timer.c
+++ b/core/drivers/stm32_timer.c
@@ -62,6 +62,8 @@
 #define TIM_TIMEOUT_STEP_US	10
 #define TIM_PRESCAL_HSI		10U
 #define TIM_PRESCAL_CSI		7U
+#define TIM_MIN_FREQ_CALIB	50000000U
+
 
 struct stm32_timer_instance {
 	struct io_pa_va base;
@@ -92,13 +94,18 @@ static int timer_get_dt_node(void *fdt, struct dt_node_info *info, int offset)
 	return node;
 }
 
-static void timer_config(struct stm32_timer_instance *timer)
+static int timer_config(struct stm32_timer_instance *timer)
 {
 	uintptr_t base = timer_base(timer);
 
 	stm32_clock_enable(timer->clk);
 
 	timer->freq = stm32_clock_get_rate(timer->clk);
+	if (timer->freq < TIM_MIN_FREQ_CALIB) {
+		EMSG("Timer is not accurate enough for calibration");
+		stm32_clock_disable(timer->clk);
+		return -1;
+	}
 
 	if ((mmio_read_32(base + TIM_TISEL) & TIM_TISEL_TI1SEL_MASK) !=
 	    timer->cal_input) {
@@ -120,6 +127,7 @@ static void timer_config(struct stm32_timer_instance *timer)
 	}
 
 	stm32_clock_disable(timer->clk);
+	return 0;
 }
 
 static uint32_t timer_start_capture(struct stm32_timer_instance *timer)
@@ -130,7 +138,8 @@ static uint32_t timer_start_capture(struct stm32_timer_instance *timer)
 	int twice;
 	uintptr_t base = timer_base(timer);
 
-	timer_config(timer);
+	if (timer_config(timer) < 0)
+		return 0U;
 
 	stm32_clock_enable(timer->clk);
 
@@ -224,17 +233,23 @@ static void _init_stm32_timer(void)
 			timer->clk = dt_timer.clock;
 			timer->freq = stm32_clock_get_rate(timer->clk);
 			timer->cal_input = (uint8_t)fdt32_to_cpu(*cuint);
-			timer_config(timer);
+			if (timer_config(timer) < 0) {
+				timer->base.pa = 0;
+				continue;
+			}
 		}
 
-		cuint = fdt_getprop(fdt, node, "st,csi_cal-input", NULL);
+		cuint = fdt_getprop(fdt, node, "st,csi-cal-input", NULL);
 		if (cuint != NULL) {
 			timer = &stm32_timer[CSI_CAL];
 			timer->base.pa = dt_timer.base;
 			timer->clk = dt_timer.clock;
 			timer->freq = stm32_clock_get_rate(timer->clk);
 			timer->cal_input = (uint8_t)fdt32_to_cpu(*cuint);
-			timer_config(timer);
+			if (timer_config(timer) < 0) {
+				timer->base.pa = 0;
+				continue;
+			}
 		}
 	}
 }
diff --git a/core/drivers/stpmic1.c b/core/drivers/stpmic1.c
index 3aa3afc..3831d40 100644
--- a/core/drivers/stpmic1.c
+++ b/core/drivers/stpmic1.c
@@ -625,20 +625,6 @@ int stpmic1_regulator_voltage_set(const char *name, uint16_t millivolts)
 				       mask);
 }
 
-int stpmic1_regulator_pull_down_set(const char *name)
-{
-	const struct regul_struct *regul = get_regulator_data(name);
-
-	if (regul->pull_down_reg != 0) {
-		return stpmic1_register_update(regul->pull_down_reg,
-					       BIT(regul->pull_down),
-					       LDO_BUCK_PULL_DOWN_MASK <<
-					       regul->pull_down);
-	}
-
-	return 0;
-}
-
 int stpmic1_regulator_mask_reset_set(const char *name)
 {
 	const struct regul_struct *regul = get_regulator_data(name);
@@ -839,7 +825,6 @@ int stpmic1_lp_voltage_cfg(const char *name, uint16_t millivolts,
 
 {
 	uint8_t voltage_index = voltage_to_index(name, millivolts);
-	const struct regul_struct *regul = get_regulator_data(name);
 	uint8_t mask;
 
 	/* Voltage can be set for buck<N> or ldo<N> (except ldo4) regulators */
@@ -852,7 +837,7 @@ int stpmic1_lp_voltage_cfg(const char *name, uint16_t millivolts,
 		return 1;
 	}
 
-	assert(cfg->lp_reg == regul->low_power_reg);
+	assert(cfg->lp_reg == get_regulator_data(name)->low_power_reg);
 	cfg->value = voltage_index << 2;
 	cfg->mask = mask;
 
diff --git a/core/include/drivers/stm32_bsec.h b/core/include/drivers/stm32_bsec.h
index de4d20b..bd33a7b 100644
--- a/core/include/drivers/stm32_bsec.h
+++ b/core/include/drivers/stm32_bsec.h
@@ -1,6 +1,6 @@
 /* SPDX-License-Identifier: BSD-3-Clause */
 /*
- * Copyright (c) 2015-2018, STMicroelectronics
+ * Copyright (c) 2015-2019, STMicroelectronics
  */
 
 #ifndef __STM32_BSEC_H__
@@ -104,7 +104,7 @@
  */
 #define BSEC_LOCK_UPPER_OTP		0x00
 #define BSEC_LOCK_DEBUG			0x02
-#define BSEC_LOCK_PROGRAM		0x03
+#define BSEC_LOCK_PROGRAM		0x04
 
 /* Values for struct bsec_config::freq */
 #define FREQ_10_20_MHZ			0x0
@@ -127,16 +127,15 @@ uint32_t bsec_get_version(void);
 uint32_t bsec_get_id(void);
 uint32_t bsec_get_magic_id(void);
 
-bool bsec_write_sr_lock(uint32_t otp, uint32_t value);
-bool bsec_read_sr_lock(uint32_t otp);
-bool bsec_write_sw_lock(uint32_t otp, uint32_t value);
-bool bsec_read_sw_lock(uint32_t otp);
-bool bsec_write_sp_lock(uint32_t otp, uint32_t value);
-bool bsec_read_sp_lock(uint32_t otp);
-bool bsec_wr_lock(uint32_t otp);
-uint32_t bsec_otp_lock(uint32_t service, uint32_t value);
+uint32_t bsec_set_sr_lock(uint32_t otp);
+uint32_t bsec_read_sr_lock(uint32_t otp, bool *value);
+uint32_t bsec_set_sw_lock(uint32_t otp);
+uint32_t bsec_read_sw_lock(uint32_t otp, bool *value);
+uint32_t bsec_set_sp_lock(uint32_t otp);
+uint32_t bsec_read_sp_lock(uint32_t otp, bool *value);
+uint32_t bsec_read_permanent_lock(uint32_t otp, bool *value);
+uint32_t bsec_otp_lock(uint32_t service);
 
-bool bsec_mode_is_closed_device(void);
 uint32_t bsec_shadow_read_otp(uint32_t *otp_value, uint32_t word);
 uint32_t bsec_check_nsec_access_rights(uint32_t otp);
 
-- 
2.7.4