From f829d7df7e261fb8f68e21dbceab8c77ce65aedd Mon Sep 17 00:00:00 2001
From: Gabriel Fernandez <gabriel.fernandez@foss.st.com>
Date: Wed, 20 Apr 2022 10:08:08 +0200
Subject: [PATCH] feat(st-reset): add stm32mp2_reset driver

This driver manages the resets of the peripherals embedded in STM32MP2.
Like clock driver, it also uses the RCC peripheral.

Change-Id: I8217891bdf1b847925aad77f3f6ef542f08d1fba
Signed-off-by: Yann Gautier <yann.gautier@st.com>
---
 drivers/st/reset/stm32mp2_reset.c | 76 +++++++++++++++++++++++++++++++
 plat/st/stm32mp2/platform.mk      |  3 +-
 2 files changed, 78 insertions(+), 1 deletion(-)
 create mode 100644 drivers/st/reset/stm32mp2_reset.c

diff --git a/drivers/st/reset/stm32mp2_reset.c b/drivers/st/reset/stm32mp2_reset.c
new file mode 100644
index 000000000..0918df59e
--- /dev/null
+++ b/drivers/st/reset/stm32mp2_reset.c
@@ -0,0 +1,76 @@
+/*
+ * Copyright (c) 2024, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <errno.h>
+#include <stdbool.h>
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32mp_reset.h>
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+
+#include <platform_def.h>
+
+static uint32_t id2reg_offset(unsigned int reset_id)
+{
+	return ((reset_id & GENMASK(31, 5)) >> 5) * sizeof(uint32_t);
+}
+
+static uint8_t id2reg_bit_pos(unsigned int reset_id)
+{
+	return (uint8_t)(reset_id & GENMASK(4, 0));
+}
+
+static int reset_toggle(uint32_t id, unsigned int to_us, bool reset_status)
+{
+	uint32_t offset = id2reg_offset(id);
+	uint32_t bitmsk = BIT(id2reg_bit_pos(id));
+	uint32_t bit_check;
+	uintptr_t rcc_base = stm32mp_rcc_base();
+
+	if (reset_status) {
+		mmio_setbits_32(rcc_base + offset, bitmsk);
+		bit_check = bitmsk;
+	} else {
+		mmio_clrbits_32(rcc_base + offset, bitmsk);
+		bit_check = 0U;
+	}
+
+	if (to_us != 0U) {
+		uint64_t timeout_ref = timeout_init_us(to_us);
+
+		while ((mmio_read_32(rcc_base + offset) & bitmsk) != bit_check) {
+			if (timeout_elapsed(timeout_ref)) {
+				return -ETIMEDOUT;
+			}
+		}
+	}
+
+	return 0;
+}
+
+int stm32mp_reset_assert(uint32_t id, unsigned int to_us)
+{
+	return reset_toggle(id, to_us, true);
+}
+
+int stm32mp_reset_deassert(uint32_t id, unsigned int to_us)
+{
+	return reset_toggle(id, to_us, false);
+}
+
+void __dead2 stm32mp_system_reset(void)
+{
+	uintptr_t rcc_base = stm32mp_rcc_base();
+
+	mmio_setbits_32(rcc_base + RCC_GRSTCSETR, RCC_GRSTCSETR_SYSRST);
+
+	/* Loop in case system reset is not immediately caught */
+	while (true) {
+		wfi();
+	}
+}
diff --git a/plat/st/stm32mp2/platform.mk b/plat/st/stm32mp2/platform.mk
index d9969ddba..d9a4d79d2 100644
--- a/plat/st/stm32mp2/platform.mk
+++ b/plat/st/stm32mp2/platform.mk
@@ -51,7 +51,8 @@ PLAT_BL_COMMON_SOURCES		+=	lib/cpus/${ARCH}/cortex_a35.S
 PLAT_BL_COMMON_SOURCES		+=	drivers/st/uart/${ARCH}/stm32_console.S
 PLAT_BL_COMMON_SOURCES		+=	plat/st/stm32mp2/${ARCH}/stm32mp2_helper.S
 
-PLAT_BL_COMMON_SOURCES		+=	drivers/st/bsec/bsec3.c
+PLAT_BL_COMMON_SOURCES		+=	drivers/st/bsec/bsec3.c					\
+					drivers/st/reset/stm32mp2_reset.c
 
 PLAT_BL_COMMON_SOURCES		+=	drivers/st/clk/clk-stm32-core.c				\
 					drivers/st/clk/clk-stm32mp2.c