diff --git a/changelog.yaml b/changelog.yaml
index 1bb542a..93785a9 100644
--- a/changelog.yaml
+++ b/changelog.yaml
@@ -446,10 +446,10 @@
 
     subsections:
       - title: FF-A
-        scope: ffa
+        scope: ff-a
 
         deprecated:
-          - ff-a
+          - ffa
 
       - title: RME
         scope: rme
@@ -461,7 +461,9 @@
           - spmc
           - spmd
           - SPMD
-          - spm_mm
+
+      - title: SPM MM
+        scope: spm-mm
 
   - title: Libraries
 
diff --git a/drivers/scmi-msg/entry.c b/drivers/scmi-msg/entry.c
index 3537fbe..399115c 100644
--- a/drivers/scmi-msg/entry.c
+++ b/drivers/scmi-msg/entry.c
@@ -1,6 +1,6 @@
 // SPDX-License-Identifier: BSD-3-Clause
 /*
- * Copyright (c) 2015-2020, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2022, Arm Limited and Contributors. All rights reserved.
  * Copyright (c) 2019-2020, Linaro Limited
  */
 
@@ -84,7 +84,7 @@
 		return;
 	}
 
-	ERROR("Agent %u Protocol 0x%x Message 0x%x: not supported",
+	ERROR("Agent %u Protocol 0x%x Message 0x%x: not supported\n",
 	      msg->agent_id, msg->protocol_id, msg->message_id);
 
 	scmi_status_response(msg, SCMI_NOT_SUPPORTED);
diff --git a/drivers/st/bsec/bsec.c b/drivers/st/bsec/bsec2.c
similarity index 63%
rename from drivers/st/bsec/bsec.c
rename to drivers/st/bsec/bsec2.c
index 01c369e..68d3a5b 100644
--- a/drivers/st/bsec/bsec.c
+++ b/drivers/st/bsec/bsec2.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017-2022, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -7,18 +7,19 @@
 #include <assert.h>
 #include <limits.h>
 
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/st/bsec.h>
+#include <drivers/st/bsec2_reg.h>
+#include <lib/mmio.h>
+#include <lib/spinlock.h>
 #include <libfdt.h>
 
 #include <platform_def.h>
 
-#include <arch_helpers.h>
-#include <common/debug.h>
-#include <drivers/st/bsec.h>
-#include <lib/mmio.h>
-#include <lib/spinlock.h>
-
-#define BSEC_IP_VERSION_1_0	0x10
-#define BSEC_COMPAT		"st,stm32mp15-bsec"
+#define BSEC_IP_VERSION_1_1	U(0x11)
+#define BSEC_IP_VERSION_2_0	U(0x20)
+#define BSEC_IP_ID_2		U(0x100032)
 
 #define OTP_ACCESS_SIZE (round_up(OTP_MAX_SIZE, __WORD_BIT) / __WORD_BIT)
 
@@ -44,11 +45,23 @@
 	}
 }
 
+static bool is_otp_invalid_mode(void)
+{
+	bool ret = ((bsec_get_status() & BSEC_MODE_INVALID) == BSEC_MODE_INVALID);
+
+	if (ret) {
+		ERROR("OTP mode is OTP-INVALID\n");
+	}
+
+	return ret;
+}
+
+#if defined(IMAGE_BL32)
 static int bsec_get_dt_node(struct dt_node_info *info)
 {
 	int node;
 
-	node = dt_get_node(info, -1, BSEC_COMPAT);
+	node = dt_get_node(info, -1, DT_BSEC_COMPAT);
 	if (node < 0) {
 		return -FDT_ERR_NOTFOUND;
 	}
@@ -56,7 +69,6 @@
 	return node;
 }
 
-#if defined(IMAGE_BL32)
 static void enable_non_secure_access(uint32_t otp)
 {
 	otp_nsec_access[otp / __WORD_BIT] |= BIT(otp % __WORD_BIT);
@@ -69,79 +81,74 @@
 static bool non_secure_can_access(uint32_t otp)
 {
 	return (otp_nsec_access[otp / __WORD_BIT] &
-		BIT(otp % __WORD_BIT)) != 0;
+		BIT(otp % __WORD_BIT)) != 0U;
 }
 
-static int bsec_dt_otp_nsec_access(void *fdt, int bsec_node)
+static void bsec_dt_otp_nsec_access(void *fdt, int bsec_node)
 {
 	int bsec_subnode;
 
 	fdt_for_each_subnode(bsec_subnode, fdt, bsec_node) {
 		const fdt32_t *cuint;
-		uint32_t reg;
+		uint32_t otp;
 		uint32_t i;
 		uint32_t size;
-		uint8_t status;
+		uint32_t offset;
+		uint32_t length;
 
 		cuint = fdt_getprop(fdt, bsec_subnode, "reg", NULL);
 		if (cuint == NULL) {
 			panic();
 		}
 
-		reg = fdt32_to_cpu(*cuint) / sizeof(uint32_t);
-		if (reg < STM32MP1_UPPER_OTP_START) {
+		offset = fdt32_to_cpu(*cuint);
+		cuint++;
+		length = fdt32_to_cpu(*cuint);
+
+		otp = offset / sizeof(uint32_t);
+
+		if (otp < STM32MP1_UPPER_OTP_START) {
+			unsigned int otp_end = round_up(offset + length,
+						       sizeof(uint32_t)) /
+					       sizeof(uint32_t);
+
+			if (otp_end > STM32MP1_UPPER_OTP_START) {
+				/*
+				 * OTP crosses Lower/Upper boundary, consider
+				 * only the upper part.
+				 */
+				otp = STM32MP1_UPPER_OTP_START;
+				length -= (STM32MP1_UPPER_OTP_START *
+					   sizeof(uint32_t)) - offset;
+				offset = STM32MP1_UPPER_OTP_START *
+					 sizeof(uint32_t);
+
+				WARN("OTP crosses Lower/Upper boundary\n");
+			} else {
+				continue;
+			}
+		}
+
+		if ((fdt_getprop(fdt, bsec_subnode,
+				 "st,non-secure-otp", NULL)) == NULL) {
 			continue;
 		}
 
-		status = fdt_get_status(bsec_subnode);
-		if ((status & DT_NON_SECURE) == 0U)  {
-			continue;
+		if (((offset % sizeof(uint32_t)) != 0U) ||
+		    ((length % sizeof(uint32_t)) != 0U)) {
+			ERROR("Unaligned non-secure OTP\n");
+			panic();
 		}
 
-		size = fdt32_to_cpu(*(cuint + 1)) / sizeof(uint32_t);
+		size = length / sizeof(uint32_t);
 
-		if ((fdt32_to_cpu(*(cuint + 1)) % sizeof(uint32_t)) != 0) {
-			size++;
-		}
-
-		for (i = reg; i < (reg + size); i++) {
+		for (i = otp; i < (otp + size); i++) {
 			enable_non_secure_access(i);
 		}
 	}
-
-	return 0;
-}
-#endif
-
-static uint32_t otp_bank_offset(uint32_t otp)
-{
-	assert(otp <= STM32MP1_OTP_MAX_ID);
-
-	return ((otp & ~BSEC_OTP_MASK) >> BSEC_OTP_BANK_SHIFT) *
-	       sizeof(uint32_t);
 }
 
-static uint32_t bsec_check_error(uint32_t otp)
-{
-	uint32_t bit = BIT(otp & BSEC_OTP_MASK);
-	uint32_t bank = otp_bank_offset(otp);
-
-	if ((mmio_read_32(bsec_base + BSEC_DISTURBED_OFF + bank) & bit) != 0U) {
-		return BSEC_DISTURBED;
-	}
-
-	if ((mmio_read_32(bsec_base + BSEC_ERROR_OFF + bank) & bit) != 0U) {
-		return BSEC_ERROR;
-	}
-
-	return BSEC_OK;
-}
-
-/*
- * bsec_probe: initialize BSEC driver.
- * return value: BSEC_OK if no error.
- */
-uint32_t bsec_probe(void)
+static void bsec_late_init(void)
 {
 	void *fdt;
 	int node;
@@ -156,10 +163,67 @@
 		panic();
 	}
 
-	bsec_base = bsec_info.base;
+	assert(bsec_base == bsec_info.base);
+
+	bsec_dt_otp_nsec_access(fdt, node);
+}
+#endif
+
+static uint32_t otp_bank_offset(uint32_t otp)
+{
+	assert(otp <= STM32MP1_OTP_MAX_ID);
+
+	return ((otp & ~BSEC_OTP_MASK) >> BSEC_OTP_BANK_SHIFT) *
+	       sizeof(uint32_t);
+}
+
+/*
+ * bsec_check_error: check BSEC error status.
+ * otp: OTP number.
+ * check_disturbed: check only error (false),
+ *	or error and disturbed status (true).
+ * return value: BSEC_OK if no error.
+ */
+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 ((mmio_read_32(bsec_base + BSEC_ERROR_OFF + bank) & bit) != 0U) {
+		return BSEC_ERROR;
+	}
+
+	if (!check_disturbed) {
+		return BSEC_OK;
+	}
+
+	if ((mmio_read_32(bsec_base + BSEC_DISTURBED_OFF + bank) & bit) != 0U) {
+		return BSEC_DISTURBED;
+	}
+
+	return BSEC_OK;
+}
+
+/*
+ * bsec_probe: initialize BSEC driver.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_probe(void)
+{
+	bsec_base = BSEC_BASE;
+
+	if (is_otp_invalid_mode()) {
+		return BSEC_ERROR;
+	}
+
+	if ((((bsec_get_version() & BSEC_IPVR_MSK) != BSEC_IP_VERSION_1_1) &&
+	     ((bsec_get_version() & BSEC_IPVR_MSK) != BSEC_IP_VERSION_2_0)) ||
+	    (bsec_get_id() != BSEC_IP_ID_2)) {
+		panic();
+	}
 
 #if defined(IMAGE_BL32)
-	bsec_dt_otp_nsec_access(fdt, node);
+	bsec_late_init();
 #endif
 	return BSEC_OK;
 }
@@ -180,7 +244,11 @@
 uint32_t bsec_set_config(struct bsec_config *cfg)
 {
 	uint32_t value;
-	int32_t result;
+	uint32_t result;
+
+	if (is_otp_invalid_mode()) {
+		return BSEC_ERROR;
+	}
 
 	value = ((((uint32_t)cfg->freq << BSEC_CONF_FRQ_SHIFT) &
 						BSEC_CONF_FRQ_MASK) |
@@ -259,15 +327,21 @@
 uint32_t bsec_shadow_register(uint32_t otp)
 {
 	uint32_t result;
+	bool value;
 	bool power_up = false;
 
-	if (otp > STM32MP1_OTP_MAX_ID) {
-		return BSEC_INVALID_PARAM;
+	if (is_otp_invalid_mode()) {
+		return BSEC_ERROR;
 	}
 
-	/* Check if shadowing of OTP is locked */
-	if (bsec_read_sr_lock(otp)) {
-		VERBOSE("BSEC: OTP %i is locked and will not be refreshed\n",
+	result = bsec_read_sr_lock(otp, &value);
+	if (result != BSEC_OK) {
+		ERROR("BSEC: %u Sticky-read bit read Error %u\n", otp, result);
+		return result;
+	}
+
+	if (value) {
+		VERBOSE("BSEC: OTP %u is locked and will not be refreshed\n",
 			otp);
 	}
 
@@ -283,14 +357,13 @@
 
 	bsec_lock();
 
-	/* Set BSEC_OTP_CTRL_OFF and set ADDR with the OTP value */
 	mmio_write_32(bsec_base + BSEC_OTP_CTRL_OFF, otp | BSEC_READ);
 
 	while ((bsec_get_status() & BSEC_MODE_BUSY_MASK) != 0U) {
 		;
 	}
 
-	result = bsec_check_error(otp);
+	result = bsec_check_error(otp, true);
 
 	bsec_unlock();
 
@@ -311,22 +384,18 @@
  */
 uint32_t bsec_read_otp(uint32_t *val, uint32_t otp)
 {
-	uint32_t result;
+	if (is_otp_invalid_mode()) {
+		return BSEC_ERROR;
+	}
 
 	if (otp > STM32MP1_OTP_MAX_ID) {
 		return BSEC_INVALID_PARAM;
 	}
 
-	bsec_lock();
-
 	*val = mmio_read_32(bsec_base + BSEC_OTP_DATA_OFF +
 			    (otp * sizeof(uint32_t)));
 
-	result = bsec_check_error(otp);
-
-	bsec_unlock();
-
-	return result;
+	return BSEC_OK;
 }
 
 /*
@@ -338,24 +407,29 @@
 uint32_t bsec_write_otp(uint32_t val, uint32_t otp)
 {
 	uint32_t result;
+	bool value;
 
-	if (otp > STM32MP1_OTP_MAX_ID) {
-		return BSEC_INVALID_PARAM;
+	if (is_otp_invalid_mode()) {
+		return BSEC_ERROR;
 	}
 
-	/* Check if programming of OTP is locked */
-	if (bsec_read_sw_lock(otp)) {
-		VERBOSE("BSEC: OTP %i is locked and write will be ignored\n",
+	result = bsec_read_sw_lock(otp, &value);
+	if (result != BSEC_OK) {
+		ERROR("BSEC: %u Sticky-write bit read Error %u\n", otp, result);
+		return result;
+	}
+
+	if (value) {
+		VERBOSE("BSEC: OTP %u is locked and write will be ignored\n",
 			otp);
 	}
 
+	/* Ensure integrity of each register access sequence */
 	bsec_lock();
 
 	mmio_write_32(bsec_base + BSEC_OTP_DATA_OFF +
 		      (otp * sizeof(uint32_t)), val);
 
-	result = bsec_check_error(otp);
-
 	bsec_unlock();
 
 	return result;
@@ -372,14 +446,28 @@
 {
 	uint32_t result;
 	bool power_up = false;
+	bool sp_lock;
+	bool perm_lock;
 
-	if (otp > STM32MP1_OTP_MAX_ID) {
-		return BSEC_INVALID_PARAM;
+	if (is_otp_invalid_mode()) {
+		return BSEC_ERROR;
 	}
 
-	/* Check if programming of OTP is locked */
-	if (bsec_read_sp_lock(otp)) {
+	result = bsec_read_sp_lock(otp, &sp_lock);
+	if (result != BSEC_OK) {
+		ERROR("BSEC: %u Sticky-prog bit read Error %u\n", otp, result);
+		return result;
+	}
+
+	result = bsec_read_permanent_lock(otp, &perm_lock);
+	if (result != BSEC_OK) {
+		ERROR("BSEC: %u permanent bit read Error %u\n", otp, result);
+		return result;
+	}
+
+	if (sp_lock || perm_lock) {
 		WARN("BSEC: OTP locked, prog will be ignored\n");
+		return BSEC_PROG_FAIL;
 	}
 
 	if ((mmio_read_32(bsec_base + BSEC_OTP_LOCK_OFF) &
@@ -399,10 +487,8 @@
 
 	bsec_lock();
 
-	/* Set value in write register */
 	mmio_write_32(bsec_base + BSEC_OTP_WRDATA_OFF, val);
 
-	/* Set BSEC_OTP_CTRL_OFF and set ADDR with the OTP value */
 	mmio_write_32(bsec_base + BSEC_OTP_CTRL_OFF, otp | BSEC_WRITE);
 
 	while ((bsec_get_status() & BSEC_MODE_BUSY_MASK) != 0U) {
@@ -412,7 +498,7 @@
 	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();
@@ -438,6 +524,10 @@
 	uint32_t data;
 	uint32_t addr;
 
+	if (is_otp_invalid_mode()) {
+		return BSEC_ERROR;
+	}
+
 	if (otp > STM32MP1_OTP_MAX_ID) {
 		return BSEC_INVALID_PARAM;
 	}
@@ -464,10 +554,8 @@
 
 	bsec_lock();
 
-	/* Set value in write register */
 	mmio_write_32(bsec_base + BSEC_OTP_WRDATA_OFF, data);
 
-	/* Set BSEC_OTP_CTRL_OFF and set ADDR with the OTP value */
 	mmio_write_32(bsec_base + BSEC_OTP_CTRL_OFF,
 		      addr | BSEC_WRITE | BSEC_LOCK);
 
@@ -478,7 +566,7 @@
 	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();
@@ -493,31 +581,24 @@
 }
 
 /*
- * 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.
+ * return value: none.
  */
-uint32_t bsec_write_debug_conf(uint32_t val)
+void bsec_write_debug_conf(uint32_t val)
 {
-	uint32_t result = BSEC_ERROR;
-	uint32_t masked_val = val & BSEC_DEN_ALL_MSK;
-
-	bsec_lock();
-
-	mmio_write_32(bsec_base + BSEC_DEN_OFF, masked_val);
-
-	if ((mmio_read_32(bsec_base + BSEC_DEN_OFF) ^ masked_val) == 0U) {
-		result = BSEC_OK;
+	if (is_otp_invalid_mode()) {
+		return;
 	}
 
+	bsec_lock();
+	mmio_write_32(bsec_base + BSEC_DEN_OFF, val & BSEC_DEN_ALL_MSK);
 	bsec_unlock();
-
-	return result;
 }
 
 /*
- * bsec_read_debug_conf: read debug configuration.
+ * bsec_read_debug_conf: return debug configuration register value.
  */
 uint32_t bsec_read_debug_conf(void)
 {
@@ -525,6 +606,34 @@
 }
 
 /*
+ * bsec_write_scratch: write value in scratch register.
+ * val: value to write.
+ * return value: none.
+ */
+void bsec_write_scratch(uint32_t val)
+{
+#if defined(IMAGE_BL32)
+	if (is_otp_invalid_mode()) {
+		return;
+	}
+
+	bsec_lock();
+	mmio_write_32(bsec_base + BSEC_SCRATCH_OFF, val);
+	bsec_unlock();
+#else
+	mmio_write_32(BSEC_BASE + BSEC_SCRATCH_OFF, val);
+#endif
+}
+
+/*
+ * bsec_read_scratch: return scratch register value.
+ */
+uint32_t bsec_read_scratch(void)
+{
+	return mmio_read_32(bsec_base + BSEC_SCRATCH_OFF);
+}
+
+/*
  * bsec_get_status: return status register value.
  */
 uint32_t bsec_get_status(void)
@@ -533,7 +642,7 @@
 }
 
 /*
- * bsec_get_hw_conf: return hardware configuration.
+ * bsec_get_hw_conf: return hardware configuration register value.
  */
 uint32_t bsec_get_hw_conf(void)
 {
@@ -541,7 +650,7 @@
 }
 
 /*
- * bsec_get_version: return BSEC version.
+ * bsec_get_version: return BSEC version register value.
  */
 uint32_t bsec_get_version(void)
 {
@@ -549,7 +658,7 @@
 }
 
 /*
- * bsec_get_id: return BSEC ID.
+ * bsec_get_id: return BSEC ID register value.
  */
 uint32_t bsec_get_id(void)
 {
@@ -557,7 +666,7 @@
 }
 
 /*
- * 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)
 {
@@ -565,229 +674,194 @@
 }
 
 /*
- * 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);
 
-	bsec_lock();
-
-	bank_value = mmio_read_32(bsec_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.
-		 */
-		mmio_write_32(bsec_base + BSEC_SRLOCK_OFF + bank, bank_value);
-		result = true;
+	if (is_otp_invalid_mode()) {
+		return BSEC_ERROR;
 	}
 
+	if (otp > STM32MP1_OTP_MAX_ID) {
+		return BSEC_INVALID_PARAM;
+	}
+
+	bsec_lock();
+	mmio_write_32(bsec_base + BSEC_SRLOCK_OFF + bank, otp_mask);
 	bsec_unlock();
 
-	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 = mmio_read_32(bsec_base + BSEC_SRLOCK_OFF + bank);
-
-	return (bank_value & otp_mask) != 0U;
-}
-
-/*
- * bsec_write_sw_lock: write 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.
- */
-bool bsec_write_sw_lock(uint32_t otp, uint32_t value)
-{
-	bool result = false;
-	uint32_t bank = otp_bank_offset(otp);
-	uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
 	uint32_t bank_value;
 
-	bsec_lock();
-
-	bank_value = mmio_read_32(bsec_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.
-		 */
-		mmio_write_32(bsec_base + BSEC_SWLOCK_OFF + bank, bank_value);
-		result = true;
+	if (otp > STM32MP1_OTP_MAX_ID) {
+		return BSEC_INVALID_PARAM;
 	}
 
+	bank_value = mmio_read_32(bsec_base + BSEC_SRLOCK_OFF + bank);
+
+	*value = ((bank_value & otp_mask) != 0U);
+
+	return BSEC_OK;
+}
+
+/*
+ * bsec_set_sw_lock: set shadow-write lock.
+ * otp: OTP number.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_set_sw_lock(uint32_t otp)
+{
+	uint32_t bank = otp_bank_offset(otp);
+	uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
+
+	if (is_otp_invalid_mode()) {
+		return BSEC_ERROR;
+	}
+
+	if (otp > STM32MP1_OTP_MAX_ID) {
+		return BSEC_INVALID_PARAM;
+	}
+
+	bsec_lock();
+	mmio_write_32(bsec_base + BSEC_SWLOCK_OFF + bank, otp_mask);
 	bsec_unlock();
 
-	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 = mmio_read_32(bsec_base + BSEC_SWLOCK_OFF + bank);
+	uint32_t bank_value;
 
-	return (bank_value & otp_mask) != 0U;
+	if (otp > STM32MP1_OTP_MAX_ID) {
+		return BSEC_INVALID_PARAM;
+	}
+
+	bank_value = mmio_read_32(bsec_base + BSEC_SWLOCK_OFF + bank);
+
+	*value = ((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);
 
-	bsec_lock();
-
-	bank_value = mmio_read_32(bsec_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.
-		 */
-		mmio_write_32(bsec_base + BSEC_SPLOCK_OFF + bank, bank_value);
-		result = true;
+	if (is_otp_invalid_mode()) {
+		return BSEC_ERROR;
 	}
 
+	if (otp > STM32MP1_OTP_MAX_ID) {
+		return BSEC_INVALID_PARAM;
+	}
+
+	bsec_lock();
+	mmio_write_32(bsec_base + BSEC_SPLOCK_OFF + bank, otp_mask);
 	bsec_unlock();
 
-	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 = mmio_read_32(bsec_base + BSEC_SPLOCK_OFF + bank);
+	uint32_t bank_value;
 
-	return (bank_value & otp_mask) != 0U;
-}
-
-/*
- * bsec_wr_lock: Read permanent lock status.
- * otp: OTP number.
- * return: true if OTP is locked, else false.
- */
-bool bsec_wr_lock(uint32_t otp)
-{
-	uint32_t bank = otp_bank_offset(otp);
-	uint32_t lock_bit = BIT(otp & BSEC_OTP_MASK);
-
-	if ((mmio_read_32(bsec_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 = mmio_read_32(bsec_base + BSEC_SPLOCK_OFF + bank);
+
+	*value = ((bank_value & otp_mask) != 0U);
+
+	return BSEC_OK;
 }
 
 /*
- * 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.
+ * bsec_read_permanent_lock: Read permanent lock status.
+ * otp: OTP number.
+ * value: read value (true or false).
+ * return value: BSEC_OK if no error.
  */
-uint32_t bsec_otp_lock(uint32_t service, uint32_t value)
+uint32_t bsec_read_permanent_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;
+
+	if (otp > STM32MP1_OTP_MAX_ID) {
+		return BSEC_INVALID_PARAM;
+	}
+
+	bank_value = mmio_read_32(bsec_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.
+ * service: Service to lock, see header file.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_otp_lock(uint32_t service)
 {
 	uintptr_t reg = bsec_base + BSEC_OTP_LOCK_OFF;
 
+	if (is_otp_invalid_mode()) {
+		return BSEC_ERROR;
+	}
+
 	switch (service) {
 	case BSEC_LOCK_UPPER_OTP:
-		mmio_write_32(reg, value << BSEC_LOCK_UPPER_OTP);
+		mmio_write_32(reg, BIT(BSEC_LOCK_UPPER_OTP));
 		break;
 	case BSEC_LOCK_DEBUG:
-		mmio_write_32(reg, value << BSEC_LOCK_DEBUG);
+		mmio_write_32(reg, BIT(BSEC_LOCK_DEBUG));
 		break;
 	case BSEC_LOCK_PROGRAM:
-		mmio_write_32(reg, value << BSEC_LOCK_PROGRAM);
+		mmio_write_32(reg, BIT(BSEC_LOCK_PROGRAM));
 		break;
 	default:
 		return BSEC_INVALID_PARAM;
@@ -799,7 +873,7 @@
 /*
  * 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 power)
 {
@@ -818,7 +892,6 @@
 
 	mmio_write_32(bsec_base + BSEC_OTP_CONF_OFF, register_val);
 
-	/* Waiting loop */
 	if (power) {
 		while (((bsec_get_status() & BSEC_MODE_PWR_MASK) == 0U) &&
 		       (timeout != 0U)) {
@@ -841,7 +914,7 @@
 }
 
 /*
- * 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.
@@ -852,13 +925,13 @@
 
 	result = bsec_shadow_register(word);
 	if (result != BSEC_OK) {
-		ERROR("BSEC: %u Shadowing Error %i\n", word, result);
+		ERROR("BSEC: %u Shadowing Error %u\n", word, result);
 		return result;
 	}
 
 	result = bsec_read_otp(otp_value, word);
 	if (result != BSEC_OK) {
-		ERROR("BSEC: %u Read Error %i\n", word, result);
+		ERROR("BSEC: %u Read Error %u\n", word, result);
 	}
 
 	return result;
@@ -867,7 +940,7 @@
 /*
  * 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)
 {
@@ -877,11 +950,8 @@
 	}
 
 	if (otp >= STM32MP1_UPPER_OTP_START) {
-		/* Check if BSEC is in OTP-SECURED closed_device state. */
-		if (stm32mp_is_closed_device()) {
-			if (!non_secure_can_access(otp)) {
-				return BSEC_ERROR;
-			}
+		if (!non_secure_can_access(otp)) {
+			return BSEC_ERROR;
 		}
 	}
 #endif
diff --git a/drivers/st/gpio/stm32_gpio.c b/drivers/st/gpio/stm32_gpio.c
index 5c54762..708989f 100644
--- a/drivers/st/gpio/stm32_gpio.c
+++ b/drivers/st/gpio/stm32_gpio.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2021, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2016-2022, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -8,10 +8,6 @@
 #include <errno.h>
 #include <stdbool.h>
 
-#include <libfdt.h>
-
-#include <platform_def.h>
-
 #include <common/bl_common.h>
 #include <common/debug.h>
 #include <drivers/clk.h>
@@ -19,6 +15,9 @@
 #include <drivers/st/stm32mp_clkfunc.h>
 #include <lib/mmio.h>
 #include <lib/utils_def.h>
+#include <libfdt.h>
+
+#include <platform_def.h>
 
 #define DT_GPIO_BANK_SHIFT	12
 #define DT_GPIO_BANK_MASK	GENMASK(16, 12)
@@ -26,6 +25,10 @@
 #define DT_GPIO_PIN_MASK	GENMASK(11, 8)
 #define DT_GPIO_MODE_MASK	GENMASK(7, 0)
 
+static void set_gpio(uint32_t bank, uint32_t pin, uint32_t mode, uint32_t type,
+		     uint32_t speed, uint32_t pull, uint32_t od,
+		     uint32_t alternate, uint8_t status);
+
 /*******************************************************************************
  * This function gets GPIO bank node in DT.
  * Returns node offset if status is okay in DT, else return 0
@@ -100,6 +103,8 @@
 		uint32_t pin;
 		uint32_t mode;
 		uint32_t alternate = GPIO_ALTERNATE_(0);
+		uint32_t type;
+		uint32_t od = GPIO_OD_OUTPUT_LOW;
 		int bank_node;
 		int clk;
 
@@ -129,7 +134,23 @@
 		}
 
 		if (fdt_getprop(fdt, node, "drive-open-drain", NULL) != NULL) {
-			mode |= GPIO_OPEN_DRAIN;
+			type = GPIO_TYPE_OPEN_DRAIN;
+		} else {
+			type = GPIO_TYPE_PUSH_PULL;
+		}
+
+		if (fdt_getprop(fdt, node, "output-high", NULL) != NULL) {
+			if (mode == GPIO_MODE_INPUT) {
+				mode = GPIO_MODE_OUTPUT;
+				od = GPIO_OD_OUTPUT_HIGH;
+			}
+		}
+
+		if (fdt_getprop(fdt, node, "output-low", NULL) != NULL) {
+			if (mode == GPIO_MODE_INPUT) {
+				mode = GPIO_MODE_OUTPUT;
+				od = GPIO_OD_OUTPUT_LOW;
+			}
 		}
 
 		bank_node = ckeck_gpio_bank(fdt, bank, pinctrl_node);
@@ -146,7 +167,7 @@
 		/* Platform knows the clock: assert it is okay */
 		assert((unsigned long)clk == stm32_get_gpio_bank_clock(bank));
 
-		set_gpio(bank, pin, mode, speed, pull, alternate, status);
+		set_gpio(bank, pin, mode, type, speed, pull, od, alternate, status);
 	}
 
 	return 0;
@@ -160,7 +181,7 @@
 int dt_set_pinctrl_config(int node)
 {
 	const fdt32_t *cuint;
-	int lenp = 0;
+	int lenp;
 	uint32_t i;
 	uint8_t status;
 	void *fdt;
@@ -201,8 +222,9 @@
 	return 0;
 }
 
-void set_gpio(uint32_t bank, uint32_t pin, uint32_t mode, uint32_t speed,
-	      uint32_t pull, uint32_t alternate, uint8_t status)
+static void set_gpio(uint32_t bank, uint32_t pin, uint32_t mode, uint32_t type,
+		     uint32_t speed, uint32_t pull, uint32_t od,
+		     uint32_t alternate, uint8_t status)
 {
 	uintptr_t base = stm32_get_gpio_bank_base(bank);
 	unsigned long clock = stm32_get_gpio_bank_clock(bank);
@@ -211,41 +233,42 @@
 
 	clk_enable(clock);
 
-	mmio_clrbits_32(base + GPIO_MODE_OFFSET,
-			((uint32_t)GPIO_MODE_MASK << (pin << 1)));
-	mmio_setbits_32(base + GPIO_MODE_OFFSET,
-			(mode & ~GPIO_OPEN_DRAIN) << (pin << 1));
+	mmio_clrsetbits_32(base + GPIO_MODE_OFFSET,
+			   (uint32_t)GPIO_MODE_MASK << (pin << 1),
+			   mode << (pin << 1));
 
-	if ((mode & GPIO_OPEN_DRAIN) != 0U) {
-		mmio_setbits_32(base + GPIO_TYPE_OFFSET, BIT(pin));
-	} else {
-		mmio_clrbits_32(base + GPIO_TYPE_OFFSET, BIT(pin));
-	}
+	mmio_clrsetbits_32(base + GPIO_TYPE_OFFSET,
+			   (uint32_t)GPIO_TYPE_MASK << pin,
+			   type << pin);
 
-	mmio_clrbits_32(base + GPIO_SPEED_OFFSET,
-			((uint32_t)GPIO_SPEED_MASK << (pin << 1)));
-	mmio_setbits_32(base + GPIO_SPEED_OFFSET, speed << (pin << 1));
+	mmio_clrsetbits_32(base + GPIO_SPEED_OFFSET,
+			   (uint32_t)GPIO_SPEED_MASK << (pin << 1),
+			   speed << (pin << 1));
 
-	mmio_clrbits_32(base + GPIO_PUPD_OFFSET,
-			((uint32_t)GPIO_PULL_MASK << (pin << 1)));
-	mmio_setbits_32(base + GPIO_PUPD_OFFSET, pull << (pin << 1));
+	mmio_clrsetbits_32(base + GPIO_PUPD_OFFSET,
+			   (uint32_t)GPIO_PULL_MASK << (pin << 1),
+			   pull << (pin << 1));
 
 	if (pin < GPIO_ALT_LOWER_LIMIT) {
-		mmio_clrbits_32(base + GPIO_AFRL_OFFSET,
-				((uint32_t)GPIO_ALTERNATE_MASK << (pin << 2)));
-		mmio_setbits_32(base + GPIO_AFRL_OFFSET,
-				alternate << (pin << 2));
+		mmio_clrsetbits_32(base + GPIO_AFRL_OFFSET,
+				   (uint32_t)GPIO_ALTERNATE_MASK << (pin << 2),
+				   alternate << (pin << 2));
 	} else {
-		mmio_clrbits_32(base + GPIO_AFRH_OFFSET,
-				((uint32_t)GPIO_ALTERNATE_MASK <<
-				 ((pin - GPIO_ALT_LOWER_LIMIT) << 2)));
-		mmio_setbits_32(base + GPIO_AFRH_OFFSET,
-				alternate << ((pin - GPIO_ALT_LOWER_LIMIT) <<
-					      2));
+		size_t shift = (pin - GPIO_ALT_LOWER_LIMIT) << 2;
+
+		mmio_clrsetbits_32(base + GPIO_AFRH_OFFSET,
+				   (uint32_t)GPIO_ALTERNATE_MASK << shift,
+				   alternate << shift);
 	}
 
+	mmio_clrsetbits_32(base + GPIO_OD_OFFSET,
+			   (uint32_t)GPIO_OD_MASK << pin,
+			   od << pin);
+
 	VERBOSE("GPIO %u mode set to 0x%x\n", bank,
 		mmio_read_32(base + GPIO_MODE_OFFSET));
+	VERBOSE("GPIO %u type set to 0x%x\n", bank,
+		mmio_read_32(base + GPIO_TYPE_OFFSET));
 	VERBOSE("GPIO %u speed set to 0x%x\n", bank,
 		mmio_read_32(base + GPIO_SPEED_OFFSET));
 	VERBOSE("GPIO %u mode pull to 0x%x\n", bank,
@@ -254,16 +277,22 @@
 		mmio_read_32(base + GPIO_AFRL_OFFSET));
 	VERBOSE("GPIO %u mode alternate high to 0x%x\n", bank,
 		mmio_read_32(base + GPIO_AFRH_OFFSET));
+	VERBOSE("GPIO %u output data set to 0x%x\n", bank,
+		mmio_read_32(base + GPIO_OD_OFFSET));
 
 	clk_disable(clock);
 
 	if (status == DT_SECURE) {
 		stm32mp_register_secure_gpio(bank, pin);
+#if !IMAGE_BL2
 		set_gpio_secure_cfg(bank, pin, true);
+#endif
 
 	} else {
 		stm32mp_register_non_secure_gpio(bank, pin);
+#if !IMAGE_BL2
 		set_gpio_secure_cfg(bank, pin, false);
+#endif
 	}
 }
 
@@ -287,7 +316,8 @@
 
 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(bank, pin, GPIO_MODE_ANALOG, GPIO_TYPE_PUSH_PULL,
+		 GPIO_SPEED_LOW, GPIO_NO_PULL, GPIO_OD_OUTPUT_LOW,
+		 GPIO_ALTERNATE_(0), DT_DISABLED);
 	set_gpio_secure_cfg(bank, pin, stm32_gpio_is_secure_at_reset(bank));
 }
diff --git a/fdts/stm32mp151.dtsi b/fdts/stm32mp151.dtsi
index ca93f0c..454e124 100644
--- a/fdts/stm32mp151.dtsi
+++ b/fdts/stm32mp151.dtsi
@@ -1,6 +1,6 @@
 // SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
 /*
- * Copyright (c) 2017-2021, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017-2022, STMicroelectronics - All Rights Reserved
  * Author: Ludovic Barre <ludovic.barre@st.com> for STMicroelectronics.
  */
 #include <dt-bindings/interrupt-controller/arm-gic.h>
@@ -19,9 +19,31 @@
 			compatible = "arm,cortex-a7";
 			device_type = "cpu";
 			reg = <0>;
+			nvmem-cells = <&part_number_otp>;
+			nvmem-cell-names = "part_number";
 		};
 	};
 
+	nvmem_layout: nvmem_layout@0 {
+		compatible = "st,stm32-nvmem-layout";
+
+		nvmem-cells = <&cfg0_otp>,
+			      <&part_number_otp>,
+			      <&monotonic_otp>,
+			      <&nand_otp>,
+			      <&uid_otp>,
+			      <&package_otp>,
+			      <&hw2_otp>;
+
+		nvmem-cell-names = "cfg0_otp",
+				   "part_number_otp",
+				   "monotonic_otp",
+				   "nand_otp",
+				   "uid_otp",
+				   "package_otp",
+				   "hw2_otp";
+	};
+
 	psci {
 		compatible = "arm,psci-1.0";
 		method = "smc";
@@ -457,12 +479,38 @@
 			reg = <0x5c005000 0x400>;
 			#address-cells = <1>;
 			#size-cells = <1>;
+
+			cfg0_otp: cfg0_otp@0 {
+				reg = <0x0 0x1>;
+			};
+			part_number_otp: part_number_otp@4 {
+				reg = <0x4 0x1>;
+			};
+			monotonic_otp: monotonic_otp@10 {
+				reg = <0x10 0x4>;
+			};
+			nand_otp: nand_otp@24 {
+				reg = <0x24 0x4>;
+			};
+			uid_otp: uid_otp@34 {
+				reg = <0x34 0xc>;
+			};
+			package_otp: package_otp@40 {
+				reg = <0x40 0x4>;
+			};
+			hw2_otp: hw2_otp@48 {
+				reg = <0x48 0x4>;
+			};
 			ts_cal1: calib@5c {
 				reg = <0x5c 0x2>;
 			};
 			ts_cal2: calib@5e {
 				reg = <0x5e 0x2>;
 			};
+			mac_addr: mac_addr@e4 {
+				reg = <0xe4 0x8>;
+				st,non-secure-otp;
+			};
 		};
 
 		etzpc: etzpc@5c007000 {
diff --git a/fdts/stm32mp157c-ed1.dts b/fdts/stm32mp157c-ed1.dts
index 5c9818f..44c7016 100644
--- a/fdts/stm32mp157c-ed1.dts
+++ b/fdts/stm32mp157c-ed1.dts
@@ -1,6 +1,6 @@
 // SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
 /*
- * Copyright (c) 2017-2021, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017-2022, STMicroelectronics - All Rights Reserved
  * Author: Ludovic Barre <ludovic.barre@st.com> for STMicroelectronics.
  */
 /dts-v1/;
@@ -33,8 +33,7 @@
 &bsec {
 	board_id: board_id@ec {
 		reg = <0xec 0x4>;
-		status = "okay";
-		secure-status = "okay";
+		st,non-secure-otp;
 	};
 };
 
@@ -196,6 +195,26 @@
 	status = "okay";
 };
 
+&nvmem_layout {
+	nvmem-cells = <&cfg0_otp>,
+		      <&part_number_otp>,
+		      <&monotonic_otp>,
+		      <&nand_otp>,
+		      <&uid_otp>,
+		      <&package_otp>,
+		      <&hw2_otp>,
+		      <&board_id>;
+
+	nvmem-cell-names = "cfg0_otp",
+			   "part_number_otp",
+			   "monotonic_otp",
+			   "nand_otp",
+			   "uid_otp",
+			   "package_otp",
+			   "hw2_otp",
+			   "board_id";
+};
+
 &pwr_regulators {
 	vdd-supply = <&vdd>;
 	vdd_3v3_usbfs-supply = <&vdd_usb>;
diff --git a/fdts/stm32mp15xx-dkx.dtsi b/fdts/stm32mp15xx-dkx.dtsi
index 975d749..2eb3a57 100644
--- a/fdts/stm32mp15xx-dkx.dtsi
+++ b/fdts/stm32mp15xx-dkx.dtsi
@@ -1,6 +1,6 @@
 // SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
 /*
- * Copyright (c) 2019-2021, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2019-2022, STMicroelectronics - All Rights Reserved
  * Author: Alexandre Torgue <alexandre.torgue@st.com> for STMicroelectronics.
  */
 
@@ -183,6 +183,26 @@
 	secure-status = "okay";
 };
 
+&nvmem_layout {
+	nvmem-cells = <&cfg0_otp>,
+		      <&part_number_otp>,
+		      <&monotonic_otp>,
+		      <&nand_otp>,
+		      <&uid_otp>,
+		      <&package_otp>,
+		      <&hw2_otp>,
+		      <&board_id>;
+
+	nvmem-cell-names = "cfg0_otp",
+			   "part_number_otp",
+			   "monotonic_otp",
+			   "nand_otp",
+			   "uid_otp",
+			   "package_otp",
+			   "hw2_otp",
+			   "board_id";
+};
+
 &pwr_regulators {
 	vdd-supply = <&vdd>;
 	vdd_3v3_usbfs-supply = <&vdd_usb>;
diff --git a/include/drivers/st/bsec.h b/include/drivers/st/bsec.h
index d833e7a..60dcf3c 100644
--- a/include/drivers/st/bsec.h
+++ b/include/drivers/st/bsec.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017-2022, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -19,13 +19,6 @@
 #define BSEC_OTP_BANK_SHIFT		5
 #define BSEC_TIMEOUT_VALUE		0xFFFF
 
-#define ADDR_LOWER_OTP_PERLOCK_SHIFT	0x03
-#define DATA_LOWER_OTP_PERLOCK_BIT	0x03U /* 2 significants bits are used */
-#define DATA_LOWER_OTP_PERLOCK_MASK	GENMASK(2, 0)
-#define ADDR_UPPER_OTP_PERLOCK_SHIFT	0x04
-#define DATA_UPPER_OTP_PERLOCK_BIT	0x01U /* 1 significants bits are used */
-#define DATA_UPPER_OTP_PERLOCK_MASK	GENMASK(3, 0)
-
 /*
  * Return status
  */
@@ -35,110 +28,34 @@
 #define BSEC_INVALID_PARAM		0xFFFFFFFCU
 #define BSEC_PROG_FAIL			0xFFFFFFFBU
 #define BSEC_LOCK_FAIL			0xFFFFFFFAU
-#define BSEC_WRITE_FAIL			0xFFFFFFF9U
-#define BSEC_SHADOW_FAIL		0xFFFFFFF8U
-#define BSEC_TIMEOUT			0xFFFFFFF7U
+#define BSEC_TIMEOUT			0xFFFFFFF9U
+#define BSEC_RETRY			0xFFFFFFF8U
+#define BSEC_NOT_SUPPORTED		0xFFFFFFF7U
+#define BSEC_WRITE_LOCKED		0xFFFFFFF6U
+#define BSEC_ERROR_INVALID_FVR		0xFFFFFFF5U
 
 /*
- * BSEC REGISTER OFFSET (base relative)
+ * OTP MODE
  */
-#define BSEC_OTP_CONF_OFF		0x000U
-#define BSEC_OTP_CTRL_OFF		0x004U
-#define BSEC_OTP_WRDATA_OFF		0x008U
-#define BSEC_OTP_STATUS_OFF		0x00CU
-#define BSEC_OTP_LOCK_OFF		0x010U
-#define BSEC_DEN_OFF			0x014U
-#define BSEC_DISTURBED_OFF		0x01CU
-#define BSEC_DISTURBED1_OFF		0x020U
-#define BSEC_DISTURBED2_OFF		0x024U
-#define BSEC_ERROR_OFF			0x034U
-#define BSEC_ERROR1_OFF			0x038U
-#define BSEC_ERROR2_OFF			0x03CU
-#define BSEC_WRLOCK_OFF			0x04CU /* Safmem permanent lock */
-#define BSEC_WRLOCK1_OFF		0x050U
-#define BSEC_WRLOCK2_OFF		0x054U
-#define BSEC_SPLOCK_OFF			0x064U /* Program safmem sticky lock */
-#define BSEC_SPLOCK1_OFF		0x068U
-#define BSEC_SPLOCK2_OFF		0x06CU
-#define BSEC_SWLOCK_OFF			0x07CU /* Write in OTP sticky lock */
-#define BSEC_SWLOCK1_OFF		0x080U
-#define BSEC_SWLOCK2_OFF		0x084U
-#define BSEC_SRLOCK_OFF			0x094U /* Shadowing sticky lock */
-#define BSEC_SRLOCK1_OFF		0x098U
-#define BSEC_SRLOCK2_OFF		0x09CU
-#define BSEC_JTAG_IN_OFF		0x0ACU
-#define BSEC_JTAG_OUT_OFF		0x0B0U
-#define BSEC_SCRATCH_OFF		0x0B4U
-#define BSEC_OTP_DATA_OFF		0x200U
-#define BSEC_IPHW_CFG_OFF		0xFF0U
-#define BSEC_IPVR_OFF			0xFF4U
-#define BSEC_IP_ID_OFF			0xFF8U
-#define BSEC_IP_MAGIC_ID_OFF		0xFFCU
+#define BSEC_MODE_OPEN1			0x00U
+#define BSEC_MODE_SECURED		0x01U
+#define BSEC_MODE_OPEN2			0x02U
+#define BSEC_MODE_INVALID		0x04U
 
 /*
- * BSEC_CONFIGURATION Register
- */
-#define BSEC_CONF_POWER_UP_MASK		BIT(0)
-#define BSEC_CONF_POWER_UP_SHIFT	0
-#define BSEC_CONF_FRQ_MASK		GENMASK(2, 1)
-#define BSEC_CONF_FRQ_SHIFT		1
-#define BSEC_CONF_PRG_WIDTH_MASK	GENMASK(6, 3)
-#define BSEC_CONF_PRG_WIDTH_SHIFT	3
-#define BSEC_CONF_TREAD_MASK		GENMASK(8, 7)
-#define BSEC_CONF_TREAD_SHIFT		7
-
-/*
- * BSEC_CONTROL Register
- */
-#define BSEC_READ			0x000U
-#define BSEC_WRITE			0x100U
-#define BSEC_LOCK			0x200U
-
-/*
- * BSEC_OTP_LOCK register
- */
-#define UPPER_OTP_LOCK_MASK		BIT(0)
-#define UPPER_OTP_LOCK_SHIFT		0
-#define DENREG_LOCK_MASK		BIT(2)
-#define DENREG_LOCK_SHIFT		2
-#define GPLOCK_LOCK_MASK		BIT(4)
-#define GPLOCK_LOCK_SHIFT		4
-
-/*
- * BSEC_OTP_STATUS Register
- */
-#define BSEC_MODE_STATUS_MASK		GENMASK(2, 0)
-#define BSEC_MODE_BUSY_MASK		BIT(3)
-#define BSEC_MODE_PROGFAIL_MASK		BIT(4)
-#define BSEC_MODE_PWR_MASK		BIT(5)
-#define BSEC_MODE_BIST1_LOCK_MASK	BIT(6)
-#define BSEC_MODE_BIST2_LOCK_MASK	BIT(7)
-
-/* OTP MODE*/
-#define BSEC_MODE_OPEN1			0x00
-#define BSEC_MODE_SECURED		0x01
-#define BSEC_MODE_OPEN2			0x02
-#define BSEC_MODE_INVALID		0x04
-
-/* BSEC_DENABLE Register */
-#define BSEC_HDPEN			BIT(4)
-#define BSEC_SPIDEN			BIT(5)
-#define BSEC_SPINDEN			BIT(6)
-#define BSEC_DBGSWGEN			BIT(10)
-#define BSEC_DEN_ALL_MSK		GENMASK(10, 0)
-
-/* BSEC_FENABLE Register */
-#define BSEC_FEN_ALL_MSK		GENMASK(14, 0)
-
-/*
- * OTP Lock services definition
- * Value must corresponding to the bit number in the register
+ * OTP Lock services definition.
+ * Value must corresponding to the bit number in the register.
+ * Special case: (bit number << 1) for BSEC3.
  */
 #define BSEC_LOCK_UPPER_OTP		0x00
+#define BSEC_LOCK_GWLOCK		0x01
 #define BSEC_LOCK_DEBUG			0x02
 #define BSEC_LOCK_PROGRAM		0x03
+#define BSEC_LOCK_KVLOCK		0x04
 
-/* Values for struct bsec_config::freq */
+/*
+ * Values for struct bsec_config::freq
+ */
 #define FREQ_10_20_MHZ			0x0
 #define FREQ_20_30_MHZ			0x1
 #define FREQ_30_45_MHZ			0x2
@@ -146,22 +63,28 @@
 
 /*
  * Device info structure, providing device-specific functions and a means of
- * adding driver-specific state
+ * adding driver-specific state.
  */
 struct bsec_config {
+	uint8_t den_lock;	/*
+				 * Debug enable sticky lock
+				 * 1 debug enable is locked until next reset
+				 */
+
+	/*  BSEC2 only */
 	uint8_t tread;		/* SAFMEM Reading current level default 0 */
 	uint8_t pulse_width;	/* SAFMEM Programming pulse width default 1 */
-	uint8_t freq;		/* SAFMEM CLOCK see freq value define
+	uint8_t freq;		/*
+				 * SAFMEM CLOCK see freq value define
 				 * default FREQ_45_67_MHZ
 				 */
 	uint8_t power;		/* Power up SAFMEM. 1 power up, 0 power off */
-	uint8_t prog_lock;	/* Programming Sticky lock
+	uint8_t prog_lock;	/*
+				 * Programming Sticky lock
 				 * 1 programming is locked until next reset
 				 */
-	uint8_t den_lock;	/* Debug enable sticky lock
-				 * 1 debug enable is locked until next reset
-				 */
-	uint8_t upper_otp_lock;	/* Shadowing of upper OTP sticky lock
+	uint8_t upper_otp_lock;	/*
+				 * Shadowing of upper OTP sticky lock
 				 * 1 shadowing of upper OTP is locked
 				 * until next reset
 				 */
@@ -179,10 +102,11 @@
 uint32_t bsec_program_otp(uint32_t val, uint32_t otp);
 uint32_t bsec_permanent_lock_otp(uint32_t otp);
 
-uint32_t bsec_write_debug_conf(uint32_t val);
+void bsec_write_debug_conf(uint32_t val);
 uint32_t bsec_read_debug_conf(void);
-uint32_t bsec_write_feature_conf(uint32_t val);
-uint32_t bsec_read_feature_conf(uint32_t *val);
+
+void bsec_write_scratch(uint32_t val);
+uint32_t bsec_read_scratch(void);
 
 uint32_t bsec_get_status(void);
 uint32_t bsec_get_hw_conf(void);
@@ -190,14 +114,14 @@
 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);
 
 uint32_t bsec_shadow_read_otp(uint32_t *otp_value, uint32_t word);
 uint32_t bsec_check_nsec_access_rights(uint32_t otp);
diff --git a/include/drivers/st/bsec2_reg.h b/include/drivers/st/bsec2_reg.h
new file mode 100644
index 0000000..f895020
--- /dev/null
+++ b/include/drivers/st/bsec2_reg.h
@@ -0,0 +1,106 @@
+/*
+ * Copyright (c) 2022, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef BSEC2_REG_H
+#define BSEC2_REG_H
+
+#include <lib/utils_def.h>
+
+/* IP configuration */
+#define ADDR_LOWER_OTP_PERLOCK_SHIFT	0x03
+#define DATA_LOWER_OTP_PERLOCK_BIT	0x03U /* 2 significants bits are used */
+#define DATA_LOWER_OTP_PERLOCK_MASK	GENMASK(2, 0)
+#define ADDR_UPPER_OTP_PERLOCK_SHIFT	0x04
+#define DATA_UPPER_OTP_PERLOCK_BIT	0x01U /* 1 significants bits are used */
+#define DATA_UPPER_OTP_PERLOCK_MASK	GENMASK(3, 0)
+
+/* BSEC REGISTER OFFSET (base relative) */
+#define BSEC_OTP_CONF_OFF		U(0x000)
+#define BSEC_OTP_CTRL_OFF		U(0x004)
+#define BSEC_OTP_WRDATA_OFF		U(0x008)
+#define BSEC_OTP_STATUS_OFF		U(0x00C)
+#define BSEC_OTP_LOCK_OFF		U(0x010)
+#define BSEC_DEN_OFF			U(0x014)
+#define BSEC_DISTURBED_OFF		U(0x01C)
+#define BSEC_DISTURBED1_OFF		U(0x020)
+#define BSEC_DISTURBED2_OFF		U(0x024)
+#define BSEC_ERROR_OFF			U(0x034)
+#define BSEC_ERROR1_OFF			U(0x038)
+#define BSEC_ERROR2_OFF			U(0x03C)
+#define BSEC_WRLOCK_OFF			U(0x04C) /* Safmem permanent lock */
+#define BSEC_WRLOCK1_OFF		U(0x050)
+#define BSEC_WRLOCK2_OFF		U(0x054)
+#define BSEC_SPLOCK_OFF			U(0x064) /* Program safmem sticky lock */
+#define BSEC_SPLOCK1_OFF		U(0x068)
+#define BSEC_SPLOCK2_OFF		U(0x06C)
+#define BSEC_SWLOCK_OFF			U(0x07C) /* Write in OTP sticky lock */
+#define BSEC_SWLOCK1_OFF		U(0x080)
+#define BSEC_SWLOCK2_OFF		U(0x084)
+#define BSEC_SRLOCK_OFF			U(0x094) /* Shadowing sticky lock */
+#define BSEC_SRLOCK1_OFF		U(0x098)
+#define BSEC_SRLOCK2_OFF		U(0x09C)
+#define BSEC_JTAG_IN_OFF		U(0x0AC)
+#define BSEC_JTAG_OUT_OFF		U(0x0B0)
+#define BSEC_SCRATCH_OFF		U(0x0B4)
+#define BSEC_OTP_DATA_OFF		U(0x200)
+#define BSEC_IPHW_CFG_OFF		U(0xFF0)
+#define BSEC_IPVR_OFF			U(0xFF4)
+#define BSEC_IP_ID_OFF			U(0xFF8)
+#define BSEC_IP_MAGIC_ID_OFF		U(0xFFC)
+
+#define BSEC_WRLOCK(n)			(BSEC_WRLOCK_OFF + U(0x04) * (n))
+#define BSEC_SPLOCK(n)			(BSEC_SPLOCK_OFF + U(0x04) * (n))
+#define BSEC_SWLOCK(n)			(BSEC_SWLOCK_OFF + U(0x04) * (n))
+#define BSEC_SRLOCK(n)			(BSEC_SRLOCK_OFF + U(0x04) * (n))
+
+/* BSEC_CONFIGURATION Register */
+#define BSEC_CONF_POWER_UP_MASK		BIT(0)
+#define BSEC_CONF_POWER_UP_SHIFT	0
+#define BSEC_CONF_FRQ_MASK		GENMASK(2, 1)
+#define BSEC_CONF_FRQ_SHIFT		1
+#define BSEC_CONF_PRG_WIDTH_MASK	GENMASK(6, 3)
+#define BSEC_CONF_PRG_WIDTH_SHIFT	3
+#define BSEC_CONF_TREAD_MASK		GENMASK(8, 7)
+#define BSEC_CONF_TREAD_SHIFT		7
+
+/* BSEC_CONTROL Register */
+#define BSEC_READ			0U
+#define BSEC_WRITE			BIT(8)
+#define BSEC_LOCK			BIT(9)
+
+/* BSEC_OTP_LOCK register */
+#define UPPER_OTP_LOCK_MASK		BIT(0)
+#define UPPER_OTP_LOCK_SHIFT		0
+#define DENREG_LOCK_MASK		BIT(2)
+#define DENREG_LOCK_SHIFT		2
+#define GPLOCK_LOCK_MASK		BIT(4)
+#define GPLOCK_LOCK_SHIFT		4
+
+/* BSEC_OTP_STATUS Register */
+#define BSEC_MODE_STATUS_MASK		GENMASK(2, 0)
+#define BSEC_MODE_SECURE_MASK		BIT(0)
+#define BSEC_MODE_FULLDBG_MASK		BIT(1)
+#define BSEC_MODE_INVALID_MASK		BIT(2)
+#define BSEC_MODE_BUSY_MASK		BIT(3)
+#define BSEC_MODE_PROGFAIL_MASK		BIT(4)
+#define BSEC_MODE_PWR_MASK		BIT(5)
+#define BSEC_MODE_BIST1_LOCK_MASK	BIT(6)
+#define BSEC_MODE_BIST2_LOCK_MASK	BIT(7)
+
+/* BSEC_DENABLE Register */
+#define BSEC_HDPEN			BIT(4)
+#define BSEC_SPIDEN			BIT(5)
+#define BSEC_SPINDEN			BIT(6)
+#define BSEC_DBGSWGEN			BIT(10)
+#define BSEC_DEN_ALL_MSK		GENMASK(10, 0)
+
+/* BSEC_FENABLE Register */
+#define BSEC_FEN_ALL_MSK		GENMASK(14, 0)
+
+/* BSEC_IPVR Register */
+#define BSEC_IPVR_MSK			GENMASK(7, 0)
+
+#endif /* BSEC2_REG_H */
diff --git a/include/drivers/st/stm32_gpio.h b/include/drivers/st/stm32_gpio.h
index b072345..eeef9da 100644
--- a/include/drivers/st/stm32_gpio.h
+++ b/include/drivers/st/stm32_gpio.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2021, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2015-2022, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -13,6 +13,7 @@
 #define GPIO_TYPE_OFFSET	U(0x04)
 #define GPIO_SPEED_OFFSET	U(0x08)
 #define GPIO_PUPD_OFFSET	U(0x0C)
+#define GPIO_OD_OFFSET		U(0x14)
 #define GPIO_BSRR_OFFSET	U(0x18)
 #define GPIO_AFRL_OFFSET	U(0x20)
 #define GPIO_AFRH_OFFSET	U(0x24)
@@ -26,31 +27,35 @@
 #define GPIO_ALTERNATE_(_x)	U(_x)
 #define GPIO_ALTERNATE_MASK	U(0x0F)
 
-#define GPIO_MODE_INPUT		0x00
-#define GPIO_MODE_OUTPUT	0x01
-#define GPIO_MODE_ALTERNATE	0x02
-#define GPIO_MODE_ANALOG	0x03
+#define GPIO_MODE_INPUT		U(0x00)
+#define GPIO_MODE_OUTPUT	U(0x01)
+#define GPIO_MODE_ALTERNATE	U(0x02)
+#define GPIO_MODE_ANALOG	U(0x03)
 #define GPIO_MODE_MASK		U(0x03)
 
-#define GPIO_OPEN_DRAIN		U(0x10)
+#define GPIO_TYPE_PUSH_PULL	U(0x00)
+#define GPIO_TYPE_OPEN_DRAIN	U(0x01)
+#define GPIO_TYPE_MASK		U(0x01)
 
-#define GPIO_SPEED_LOW		0x00
-#define GPIO_SPEED_MEDIUM	0x01
-#define GPIO_SPEED_HIGH		0x02
-#define GPIO_SPEED_VERY_HIGH	0x03
+#define GPIO_SPEED_LOW		U(0x00)
+#define GPIO_SPEED_MEDIUM	U(0x01)
+#define GPIO_SPEED_HIGH		U(0x02)
+#define GPIO_SPEED_VERY_HIGH	U(0x03)
 #define GPIO_SPEED_MASK		U(0x03)
 
-#define GPIO_NO_PULL		0x00
-#define GPIO_PULL_UP		0x01
-#define GPIO_PULL_DOWN		0x02
+#define GPIO_NO_PULL		U(0x00)
+#define GPIO_PULL_UP		U(0x01)
+#define GPIO_PULL_DOWN		U(0x02)
 #define GPIO_PULL_MASK		U(0x03)
 
+#define GPIO_OD_OUTPUT_LOW	U(0x00)
+#define GPIO_OD_OUTPUT_HIGH	U(0x01)
+#define GPIO_OD_MASK		U(0x01)
+
 #ifndef __ASSEMBLER__
 #include <stdint.h>
 
 int dt_set_pinctrl_config(int node);
-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__*/
diff --git a/plat/renesas/common/include/platform_def.h b/plat/renesas/common/include/platform_def.h
index 1213a3c..ab071ec 100644
--- a/plat/renesas/common/include/platform_def.h
+++ b/plat/renesas/common/include/platform_def.h
@@ -40,7 +40,7 @@
 #define PLATFORM_STACK_SIZE	U(0x400)
 #endif
 #elif IMAGE_BL31
-#define PLATFORM_STACK_SIZE	U(0x400)
+#define PLATFORM_STACK_SIZE	U(0x800)
 #elif IMAGE_BL32
 #define PLATFORM_STACK_SIZE	U(0x440)
 #endif
diff --git a/plat/renesas/common/plat_pm.c b/plat/renesas/common/plat_pm.c
index cc677f3..9810596 100644
--- a/plat/renesas/common/plat_pm.c
+++ b/plat/renesas/common/plat_pm.c
@@ -178,19 +178,22 @@
 		ERROR("BL3-1:Failed the SYSTEM-RESET.\n");
 #endif
 #else
-	u_register_t cpu = read_mpidr_el1() & 0x0000ffffU;
+	u_register_t mpidr = read_mpidr_el1();
+	u_register_t cpu = mpidr & 0x0000ffffU;
 	int32_t rtn_on;
 
-	rtn_on = rcar_pwrc_cpu_on_check(cpu);
+	rtn_on = rcar_pwrc_cpu_on_check(mpidr);
 
-	if (cpu == rcar_boot_mpidr)
+	if (cpu != rcar_boot_mpidr) {
 		panic();
+	}
 
-	if (rtn_on)
+	if (rtn_on != 0) {
 		panic();
+	}
 
-	rcar_pwrc_cpuoff(cpu);
-	rcar_pwrc_clusteroff(cpu);
+	rcar_pwrc_cpuoff(mpidr);
+	rcar_pwrc_clusteroff(mpidr);
 
 #endif /* PMIC_ROHM_BD9571 */
 	wfi();
diff --git a/plat/st/common/include/stm32mp_common.h b/plat/st/common/include/stm32mp_common.h
index 7508004..cc06f5c 100644
--- a/plat/st/common/include/stm32mp_common.h
+++ b/plat/st/common/include/stm32mp_common.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2018-2021, STMicroelectronics - All Rights Reserved
+ * Copyright (C) 2018-2022, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -37,6 +37,11 @@
 /* Check MMU status to allow spinlock use */
 bool stm32mp_lock_available(void);
 
+int stm32_get_otp_index(const char *otp_name, uint32_t *otp_idx,
+			uint32_t *otp_len);
+int stm32_get_otp_value(const char *otp_name, uint32_t *otp_val);
+int stm32_get_otp_value_from_idx(const uint32_t otp_idx, uint32_t *otp_val);
+
 /* Get IWDG platform instance ID from peripheral IO memory base address */
 uint32_t stm32_iwdg_get_instance(uintptr_t base);
 
diff --git a/plat/st/common/include/stm32mp_dt.h b/plat/st/common/include/stm32mp_dt.h
index a87f941..b7bf1d0 100644
--- a/plat/st/common/include/stm32mp_dt.h
+++ b/plat/st/common/include/stm32mp_dt.h
@@ -1,6 +1,6 @@
 /*
- * Copyright (c) 2020-2021, STMicroelectronics - All Rights Reserved
- * Copyright (c) 2017-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2020-2022, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -40,6 +40,7 @@
 struct rdev *dt_get_vdd_regulator(void);
 struct rdev *dt_get_cpu_regulator(void);
 const char *dt_get_board_model(void);
+int dt_find_otp_name(const char *name, uint32_t *otp, uint32_t *otp_len);
 int fdt_get_gpio_bank_pin_count(unsigned int bank);
 
 #endif /* STM32MP_DT_H */
diff --git a/plat/st/common/stm32mp_common.c b/plat/st/common/stm32mp_common.c
index fb8e08e..2297cd6 100644
--- a/plat/st/common/stm32mp_common.c
+++ b/plat/st/common/stm32mp_common.c
@@ -135,6 +135,55 @@
 					   STM32MP_DDR_MAX_SIZE);
 }
 
+int stm32_get_otp_index(const char *otp_name, uint32_t *otp_idx,
+			uint32_t *otp_len)
+{
+	assert(otp_name != NULL);
+	assert(otp_idx != NULL);
+
+	return dt_find_otp_name(otp_name, otp_idx, otp_len);
+}
+
+int stm32_get_otp_value(const char *otp_name, uint32_t *otp_val)
+{
+	uint32_t otp_idx;
+
+	assert(otp_name != NULL);
+	assert(otp_val != NULL);
+
+	if (stm32_get_otp_index(otp_name, &otp_idx, NULL) != 0) {
+		return -1;
+	}
+
+	if (stm32_get_otp_value_from_idx(otp_idx, otp_val) != 0) {
+		ERROR("BSEC: %s Read Error\n", otp_name);
+		return -1;
+	}
+
+	return 0;
+}
+
+int stm32_get_otp_value_from_idx(const uint32_t otp_idx, uint32_t *otp_val)
+{
+	uint32_t ret = BSEC_NOT_SUPPORTED;
+
+	assert(otp_val != NULL);
+
+#if defined(IMAGE_BL2)
+	ret = bsec_shadow_read_otp(otp_val, otp_idx);
+#elif defined(IMAGE_BL32)
+	ret = bsec_read_otp(otp_val, otp_idx);
+#else
+#error "Not supported"
+#endif
+	if (ret != BSEC_OK) {
+		ERROR("BSEC: idx=%u Read Error\n", otp_idx);
+		return -1;
+	}
+
+	return 0;
+}
+
 #if  defined(IMAGE_BL2)
 static void reset_uart(uint32_t reset)
 {
diff --git a/plat/st/common/stm32mp_dt.c b/plat/st/common/stm32mp_dt.c
index 863a90f..ea71571 100644
--- a/plat/st/common/stm32mp_dt.c
+++ b/plat/st/common/stm32mp_dt.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -319,6 +319,73 @@
 }
 
 /*******************************************************************************
+ * dt_find_otp_name: get OTP ID and length in DT.
+ * name: sub-node name to look up.
+ * otp: pointer to read OTP number or NULL.
+ * otp_len: pointer to read OTP length in bits or NULL.
+ * return value: 0 if no error, an FDT error value otherwise.
+ ******************************************************************************/
+int dt_find_otp_name(const char *name, uint32_t *otp, uint32_t *otp_len)
+{
+	int node;
+	int index, len;
+	const fdt32_t *cuint;
+
+	if ((name == NULL) || (otp == NULL)) {
+		return -FDT_ERR_BADVALUE;
+	}
+
+	node = fdt_node_offset_by_compatible(fdt, -1, DT_NVMEM_LAYOUT_COMPAT);
+	if (node < 0) {
+		return node;
+	}
+
+	index = fdt_stringlist_search(fdt, node, "nvmem-cell-names", name);
+	if (index < 0) {
+		return index;
+	}
+
+	cuint = fdt_getprop(fdt, node, "nvmem-cells", &len);
+	if (cuint == NULL) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	if ((index * (int)sizeof(uint32_t)) > len) {
+		return -FDT_ERR_BADVALUE;
+	}
+
+	cuint += index;
+
+	node = fdt_node_offset_by_phandle(fdt, fdt32_to_cpu(*cuint));
+	if (node < 0) {
+		ERROR("Malformed nvmem_layout node: ignored\n");
+		return node;
+	}
+
+	cuint = fdt_getprop(fdt, node, "reg", &len);
+	if ((cuint == NULL) || (len != (2 * (int)sizeof(uint32_t)))) {
+		ERROR("Malformed nvmem_layout node: ignored\n");
+		return -FDT_ERR_BADVALUE;
+	}
+
+	if (fdt32_to_cpu(*cuint) % sizeof(uint32_t)) {
+		ERROR("Misaligned nvmem_layout element: ignored\n");
+		return -FDT_ERR_BADVALUE;
+	}
+
+	if (otp != NULL) {
+		*otp = fdt32_to_cpu(*cuint) / sizeof(uint32_t);
+	}
+
+	if (otp_len != NULL) {
+		cuint++;
+		*otp_len = fdt32_to_cpu(*cuint) * CHAR_BIT;
+	}
+
+	return 0;
+}
+
+/*******************************************************************************
  * This function gets the pin count for a GPIO bank based from the FDT.
  * It also checks node consistency.
  ******************************************************************************/
@@ -337,6 +404,9 @@
 
 	fdt_for_each_subnode(node, fdt, pinctrl_node) {
 		const fdt32_t *cuint;
+		int pin_count;
+		int len;
+		int i;
 
 		if (fdt_getprop(fdt, node, "gpio-controller", NULL) == NULL) {
 			continue;
@@ -355,12 +425,22 @@
 			return 0;
 		}
 
-		cuint = fdt_getprop(fdt, node, "ngpios", NULL);
-		if (cuint == NULL) {
-			return -FDT_ERR_NOTFOUND;
+		/* Parse gpio-ranges with its 4 parameters */
+		cuint = fdt_getprop(fdt, node, "gpio-ranges", &len);
+		len /= sizeof(*cuint);
+		if ((len % 4) != 0) {
+			return -FDT_ERR_BADVALUE;
 		}
 
-		return (int)fdt32_to_cpu(*cuint);
+		/* Get the last defined gpio line (offset + nb of pins) */
+		pin_count = fdt32_to_cpu(*(cuint + 1)) + fdt32_to_cpu(*(cuint + 3));
+		for (i = 0; i < len / 4; i++) {
+			pin_count = MAX(pin_count, (int)(fdt32_to_cpu(*(cuint + 1)) +
+							 fdt32_to_cpu(*(cuint + 3))));
+			cuint += 4;
+		}
+
+		return pin_count;
 	}
 
 	return 0;
diff --git a/plat/st/stm32mp1/bl2_plat_setup.c b/plat/st/stm32mp1/bl2_plat_setup.c
index b5fc3ff..13ba5ab 100644
--- a/plat/st/stm32mp1/bl2_plat_setup.c
+++ b/plat/st/stm32mp1/bl2_plat_setup.c
@@ -155,6 +155,40 @@
 #endif /* STM32MP_USE_STM32IMAGE */
 }
 
+static void update_monotonic_counter(void)
+{
+	uint32_t version;
+	uint32_t otp;
+
+	CASSERT(STM32_TF_VERSION <= MAX_MONOTONIC_VALUE,
+		assert_stm32mp1_monotonic_counter_reach_max);
+
+	/* Check if monotonic counter needs to be incremented */
+	if (stm32_get_otp_index(MONOTONIC_OTP, &otp, NULL) != 0) {
+		panic();
+	}
+
+	if (stm32_get_otp_value_from_idx(otp, &version) != 0) {
+		panic();
+	}
+
+	if ((version + 1U) < BIT(STM32_TF_VERSION)) {
+		uint32_t result;
+
+		/* Need to increment the monotonic counter. */
+		version = BIT(STM32_TF_VERSION) - 1U;
+
+		result = bsec_program_otp(version, otp);
+		if (result != BSEC_OK) {
+			ERROR("BSEC: MONOTONIC_OTP program Error %u\n",
+			      result);
+			panic();
+		}
+		INFO("Monotonic counter has been incremented (value 0x%x)\n",
+		     version);
+	}
+}
+
 void bl2_el3_plat_arch_setup(void)
 {
 	const char *board_model;
@@ -163,6 +197,10 @@
 	uintptr_t pwr_base;
 	uintptr_t rcc_base;
 
+	if (bsec_probe() != 0U) {
+		panic();
+	}
+
 	mmap_add_region(BL_CODE_BASE, BL_CODE_BASE,
 			BL_CODE_END - BL_CODE_BASE,
 			MT_CODE | MT_SECURE);
@@ -205,10 +243,6 @@
 		;
 	}
 
-	if (bsec_probe() != 0) {
-		panic();
-	}
-
 	/* Reset backup domain on cold boot cases */
 	if ((mmio_read_32(rcc_base + RCC_BDCR) & RCC_BDCR_RTCSRC_MASK) == 0U) {
 		mmio_setbits_32(rcc_base + RCC_BDCR, RCC_BDCR_VSWRST);
@@ -309,6 +343,8 @@
 
 	print_reset_reason();
 
+	update_monotonic_counter();
+
 	stm32mp1_syscfg_enable_io_compensation_finish();
 
 #if !STM32MP_USE_STM32IMAGE
diff --git a/plat/st/stm32mp1/platform.mk b/plat/st/stm32mp1/platform.mk
index 6f7c79b..65eaa74 100644
--- a/plat/st/stm32mp1/platform.mk
+++ b/plat/st/stm32mp1/platform.mk
@@ -19,6 +19,8 @@
 BL2_IN_XIP_MEM		:=	1
 endif
 
+# Please don't increment this value without good understanding of
+# the monotonic counter
 STM32_TF_VERSION	?=	0
 
 # Enable dynamic memory mapping
@@ -96,8 +98,14 @@
 FDT_SOURCES		+=	$(addprefix ${BUILD_PLAT}/fdts/, $(patsubst %.dtb,%-bl32.dts,$(DTB_FILE_NAME)))
 endif
 endif
+
+$(eval DTC_V = $(shell $(DTC) -v | awk '{print $$NF}'))
+$(eval DTC_VERSION = $(shell printf "%d" $(shell echo ${DTC_V} | cut -d- -f1 | sed "s/\./0/g")))
 DTC_CPPFLAGS		+=	${INCLUDES}
 DTC_FLAGS		+=	-Wno-unit_address_vs_reg
+ifeq ($(shell test $(DTC_VERSION) -ge 10601; echo $$?),0)
+DTC_FLAGS		+=	-Wno-interrupt_provider
+endif
 
 # Macros and rules to build TF binary
 STM32_TF_ELF_LDFLAGS	:=	--hash-style=gnu --as-needed
@@ -216,7 +224,7 @@
 				drivers/clk/clk.c					\
 				drivers/delay_timer/delay_timer.c			\
 				drivers/delay_timer/generic_delay_timer.c		\
-				drivers/st/bsec/bsec.c					\
+				drivers/st/bsec/bsec2.c					\
 				drivers/st/clk/stm32mp_clkfunc.c			\
 				drivers/st/clk/stm32mp1_clk.c				\
 				drivers/st/ddr/stm32mp_ddr.c				\
@@ -361,8 +369,6 @@
 	${Q}${MAKE} --no-print-directory -C ${STM32IMAGEPATH} clean
 
 check_dtc_version:
-	$(eval DTC_V = $(shell $(DTC) -v | awk '{print $$NF}'))
-	$(eval DTC_VERSION = $(shell printf "%d" $(shell echo ${DTC_V} | cut -d- -f1 | sed "s/\./0/g")))
 	@if [ ${DTC_VERSION} -lt 10404 ]; then \
 		echo "dtc version too old (${DTC_V}), you need at least version 1.4.4"; \
 		false; \
diff --git a/plat/st/stm32mp1/services/bsec_svc.c b/plat/st/stm32mp1/services/bsec_svc.c
index a1d7fc6..1fb44b4 100644
--- a/plat/st/stm32mp1/services/bsec_svc.c
+++ b/plat/st/stm32mp1/services/bsec_svc.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2021, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2016-2022, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -8,6 +8,7 @@
 
 #include <common/debug.h>
 #include <drivers/st/bsec.h>
+#include <drivers/st/bsec2_reg.h>
 
 #include <stm32mp1_smc.h>
 
diff --git a/plat/st/stm32mp1/stm32mp1_boot_device.c b/plat/st/stm32mp1/stm32mp1_boot_device.c
index 6a05707..714ab80 100644
--- a/plat/st/stm32mp1/stm32mp1_boot_device.c
+++ b/plat/st/stm32mp1/stm32mp1_boot_device.c
@@ -20,13 +20,11 @@
 #if STM32MP_RAW_NAND || STM32MP_SPI_NAND
 static int get_data_from_otp(struct nand_device *nand_dev, bool is_slc)
 {
-	int result;
 	uint32_t nand_param;
 
 	/* Check if NAND parameters are stored in OTP */
-	result = bsec_shadow_read_otp(&nand_param, NAND_OTP);
-	if (result != BSEC_OK) {
-		ERROR("BSEC: NAND_OTP Error %i\n", result);
+	if (stm32_get_otp_value(NAND_OTP, &nand_param) != 0) {
+		ERROR("BSEC: NAND_OTP Error\n");
 		return -EACCES;
 	}
 
diff --git a/plat/st/stm32mp1/stm32mp1_dbgmcu.c b/plat/st/stm32mp1/stm32mp1_dbgmcu.c
index 1826783..08e332a 100644
--- a/plat/st/stm32mp1/stm32mp1_dbgmcu.c
+++ b/plat/st/stm32mp1/stm32mp1_dbgmcu.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2021, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2016-2022, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -7,14 +7,14 @@
 #include <assert.h>
 #include <errno.h>
 
-#include <platform_def.h>
-
 #include <common/debug.h>
 #include <drivers/st/bsec.h>
+#include <drivers/st/bsec2_reg.h>
 #include <drivers/st/stm32mp1_rcc.h>
 #include <lib/mmio.h>
 #include <lib/utils_def.h>
 
+#include <platform_def.h>
 #include <stm32mp1_dbgmcu.h>
 
 #define DBGMCU_IDC		U(0x00)
diff --git a/plat/st/stm32mp1/stm32mp1_def.h b/plat/st/stm32mp1/stm32mp1_def.h
index c63efd5..76f3585 100644
--- a/plat/st/stm32mp1/stm32mp1_def.h
+++ b/plat/st/stm32mp1/stm32mp1_def.h
@@ -347,19 +347,19 @@
 
 #define OTP_MAX_SIZE			(STM32MP1_OTP_MAX_ID + 1U)
 
-/* OTP offsets */
-#define DATA0_OTP			U(0)
-#define PART_NUMBER_OTP			U(1)
-#define NAND_OTP			U(9)
-#define UID0_OTP			U(13)
-#define UID1_OTP			U(14)
-#define UID2_OTP			U(15)
-#define PACKAGE_OTP			U(16)
-#define HW2_OTP				U(18)
+/* OTP labels */
+#define CFG0_OTP			"cfg0_otp"
+#define PART_NUMBER_OTP			"part_number_otp"
+#define PACKAGE_OTP			"package_otp"
+#define HW2_OTP				"hw2_otp"
+#define NAND_OTP			"nand_otp"
+#define MONOTONIC_OTP			"monotonic_otp"
+#define UID_OTP				"uid_otp"
+#define BOARD_ID_OTP			"board_id"
 
 /* OTP mask */
-/* DATA0 */
-#define DATA0_OTP_SECURED		BIT(6)
+/* CFG0 */
+#define CFG0_CLOSED_DEVICE		BIT(6)
 
 /* PART NUMBER */
 #define PART_NUMBER_OTP_PART_MASK	GENMASK_32(7, 0)
@@ -416,6 +416,9 @@
 /* NAND number of planes */
 #define NAND_PLANE_BIT_NB_MASK		BIT(14)
 
+/* MONOTONIC OTP */
+#define MAX_MONOTONIC_VALUE		32
+
 /* UID OTP */
 #define UID_WORD_NB			U(3)
 
@@ -486,6 +489,7 @@
 #define DT_BSEC_COMPAT			"st,stm32mp15-bsec"
 #define DT_DDR_COMPAT			"st,stm32mp1-ddr"
 #define DT_IWDG_COMPAT			"st,stm32mp1-iwdg"
+#define DT_NVMEM_LAYOUT_COMPAT		"st,stm32-nvmem-layout"
 #define DT_PWR_COMPAT			"st,stm32mp1,pwr-reg"
 #define DT_RCC_CLK_COMPAT		"st,stm32mp1-rcc"
 
diff --git a/plat/st/stm32mp1/stm32mp1_private.c b/plat/st/stm32mp1/stm32mp1_private.c
index 9016b0d..075d1d7 100644
--- a/plat/st/stm32mp1/stm32mp1_private.c
+++ b/plat/st/stm32mp1/stm32mp1_private.c
@@ -278,7 +278,7 @@
 		return part_number;
 	}
 
-	if (bsec_shadow_read_otp(&part_number, PART_NUMBER_OTP) != BSEC_OK) {
+	if (stm32_get_otp_value(PART_NUMBER_OTP, &part_number) != 0) {
 		panic();
 	}
 
@@ -294,7 +294,7 @@
 {
 	uint32_t package;
 
-	if (bsec_shadow_read_otp(&package, PACKAGE_OTP) != BSEC_OK) {
+	if (stm32_get_otp_value(PACKAGE_OTP, &package) != 0) {
 		panic();
 	}
 
@@ -397,35 +397,9 @@
 
 void stm32mp_print_boardinfo(void)
 {
-	uint32_t board_id;
-	uint32_t board_otp;
-	int bsec_node, bsec_board_id_node;
-	void *fdt;
-	const fdt32_t *cuint;
+	uint32_t board_id = 0;
 
-	if (fdt_get_address(&fdt) == 0) {
-		panic();
-	}
-
-	bsec_node = fdt_node_offset_by_compatible(fdt, -1, DT_BSEC_COMPAT);
-	if (bsec_node < 0) {
-		return;
-	}
-
-	bsec_board_id_node = fdt_subnode_offset(fdt, bsec_node, "board_id");
-	if (bsec_board_id_node <= 0) {
-		return;
-	}
-
-	cuint = fdt_getprop(fdt, bsec_board_id_node, "reg", NULL);
-	if (cuint == NULL) {
-		panic();
-	}
-
-	board_otp = fdt32_to_cpu(*cuint) / sizeof(uint32_t);
-
-	if (bsec_shadow_read_otp(&board_id, board_otp) != BSEC_OK) {
-		ERROR("BSEC: PART_NUMBER_OTP Error\n");
+	if (stm32_get_otp_value(BOARD_ID_OTP, &board_id) != 0) {
 		return;
 	}
 
@@ -462,12 +436,11 @@
 {
 	uint32_t value;
 
-	if ((bsec_shadow_register(DATA0_OTP) != BSEC_OK) ||
-	    (bsec_read_otp(&value, DATA0_OTP) != BSEC_OK)) {
+	if (stm32_get_otp_value(CFG0_OTP, &value) != 0) {
 		return true;
 	}
 
-	return (value & DATA0_OTP_SECURED) == DATA0_OTP_SECURED;
+	return (value & CFG0_CLOSED_DEVICE) == CFG0_CLOSED_DEVICE;
 }
 
 uint32_t stm32_iwdg_get_instance(uintptr_t base)
@@ -487,13 +460,7 @@
 	uint32_t iwdg_cfg = 0U;
 	uint32_t otp_value;
 
-#if defined(IMAGE_BL2)
-	if (bsec_shadow_register(HW2_OTP) != BSEC_OK) {
-		panic();
-	}
-#endif
-
-	if (bsec_read_otp(&otp_value, HW2_OTP) != BSEC_OK) {
+	if (stm32_get_otp_value(HW2_OTP, &otp_value) != 0) {
 		panic();
 	}
 
@@ -515,29 +482,34 @@
 #if defined(IMAGE_BL2)
 uint32_t stm32_iwdg_shadow_update(uint32_t iwdg_inst, uint32_t flags)
 {
+	uint32_t otp_value;
 	uint32_t otp;
 	uint32_t result;
 
-	if (bsec_shadow_read_otp(&otp, HW2_OTP) != BSEC_OK) {
+	if (stm32_get_otp_index(HW2_OTP, &otp, NULL) != 0) {
 		panic();
 	}
 
-	if ((flags & IWDG_DISABLE_ON_STOP) != 0U) {
-		otp |= BIT(iwdg_inst + HW2_OTP_IWDG_FZ_STOP_POS);
+	if (stm32_get_otp_value(HW2_OTP, &otp_value) != 0) {
+		panic();
 	}
 
-	if ((flags & IWDG_DISABLE_ON_STANDBY) != 0U) {
-		otp |= BIT(iwdg_inst + HW2_OTP_IWDG_FZ_STANDBY_POS);
+	if ((flags & IWDG_DISABLE_ON_STOP) != 0) {
+		otp_value |= BIT(iwdg_inst + HW2_OTP_IWDG_FZ_STOP_POS);
 	}
 
-	result = bsec_write_otp(otp, HW2_OTP);
+	if ((flags & IWDG_DISABLE_ON_STANDBY) != 0) {
+		otp_value |= BIT(iwdg_inst + HW2_OTP_IWDG_FZ_STANDBY_POS);
+	}
+
+	result = bsec_write_otp(otp_value, otp);
 	if (result != BSEC_OK) {
 		return result;
 	}
 
 	/* Sticky lock OTP_IWDG (read and write) */
-	if (!bsec_write_sr_lock(HW2_OTP, 1U) ||
-	    !bsec_write_sw_lock(HW2_OTP, 1U)) {
+	if ((bsec_set_sr_lock(otp) != BSEC_OK) ||
+	    (bsec_set_sw_lock(otp) != BSEC_OK)) {
 		return BSEC_LOCK_FAIL;
 	}
 
diff --git a/plat/st/stm32mp1/stm32mp1_syscfg.c b/plat/st/stm32mp1/stm32mp1_syscfg.c
index 01a6439..3f34af1 100644
--- a/plat/st/stm32mp1/stm32mp1_syscfg.c
+++ b/plat/st/stm32mp1/stm32mp1_syscfg.c
@@ -7,11 +7,11 @@
 #include <common/debug.h>
 #include <drivers/clk.h>
 #include <drivers/delay_timer.h>
-#include <drivers/st/bsec.h>
 #include <drivers/st/stpmic1.h>
 #include <lib/mmio.h>
 
 #include <platform_def.h>
+#include <stm32mp_common.h>
 #include <stm32mp_dt.h>
 #include <stm32mp1_private.h>
 
@@ -116,8 +116,9 @@
 
 static void stm32mp1_syscfg_set_hslv(void)
 {
-	uint32_t otp = 0;
+	uint32_t otp_value;
 	uint32_t vdd_voltage;
+	bool product_below_2v5;
 
 	/*
 	 * High Speed Low Voltage Pad mode Enable for SPI, SDMMC, ETH, QSPI
@@ -134,26 +135,26 @@
 	 *   => TF-A enables the low power mode only if VDD < 2.7V (in DT)
 	 *      but this value needs to be consistent with board design.
 	 */
-	if (bsec_read_otp(&otp, HW2_OTP) != BSEC_OK) {
+	if (stm32_get_otp_value(HW2_OTP, &otp_value) != 0) {
 		panic();
 	}
 
-	otp = otp & HW2_OTP_PRODUCT_BELOW_2V5;
+	product_below_2v5 = (otp_value & HW2_OTP_PRODUCT_BELOW_2V5) != 0U;
 
 	/* Get VDD supply */
 	vdd_voltage = dt_get_pwr_vdd_voltage();
 
 	/* Check if VDD is Low Voltage */
 	if (vdd_voltage == 0U) {
-		WARN("VDD unknown");
+		WARN("VDD unknown\n");
 	} else if (vdd_voltage < 2700000U) {
 		enable_high_speed_mode_low_voltage();
 
-		if (otp == 0U) {
+		if (!product_below_2v5) {
 			INFO("Product_below_2v5=0: HSLVEN protected by HW\n");
 		}
 	} else {
-		if (otp != 0U) {
+		if (product_below_2v5) {
 			ERROR("Product_below_2v5=1:\n");
 			ERROR("\tHSLVEN update is destructive,\n");
 			ERROR("\tno update as VDD > 2.7V\n");
diff --git a/plat/st/stm32mp1/stm32mp1_usb_dfu.c b/plat/st/stm32mp1/stm32mp1_usb_dfu.c
index 70fbba6..33b12d0 100644
--- a/plat/st/stm32mp1/stm32mp1_usb_dfu.c
+++ b/plat/st/stm32mp1/stm32mp1_usb_dfu.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2021-2022, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -157,27 +157,28 @@
 static void update_serial_num_string(void)
 {
 	uint8_t i;
-	uint32_t result;
 	char serial_string[SIZ_STRING_SERIAL + 2U];
-	uint32_t deviceserial[UID_WORD_NB];
+	/* serial number is set to 0 */
+	uint32_t deviceserial[UID_WORD_NB] = {0U, 0U, 0U};
+	uint32_t otp;
+	uint32_t len;
 	uint16_t length;
 
-	for (i = 0U; i < UID_WORD_NB; i++) {
-		result = bsec_shadow_register(i + UID0_OTP);
-		if (result != BSEC_OK) {
-			ERROR("BSEC: UID%d Shadowing Error\n", i);
-			break;
-		}
-		result = bsec_read_otp(&deviceserial[i], i + UID0_OTP);
-		if (result != BSEC_OK) {
-			ERROR("BSEC: UID%d Read Error\n", i);
-			break;
-		}
+	if (stm32_get_otp_index(UID_OTP, &otp, &len) != 0) {
+		ERROR("BSEC: Get UID_OTP number Error\n");
+		return;
 	}
-	/* On bsec error: serial number is set to 0 */
-	if (result != BSEC_OK) {
-		for (i = 0; i < UID_WORD_NB; i++) {
-			deviceserial[i] = 0U;
+
+	if ((len / __WORD_BIT) != UID_WORD_NB) {
+		ERROR("BSEC: Get UID_OTP length Error\n");
+		return;
+	}
+
+	for (i = 0; i < UID_WORD_NB; i++) {
+		if (bsec_shadow_read_otp(&deviceserial[i], i + otp) !=
+		    BSEC_OK) {
+			ERROR("BSEC: UID%d Error\n", i);
+			return;
 		}
 	}
 	/* build serial number with OTP value as in ROM code */