refactor(stm32mp1): add helpers for IO compensation cells
Add enable_io_comp_cell and disable_io_comp_cell local helpers
to enable or disable an IO compensation cell.
Change-Id: I65295298a7ece572ae939e2db93d10b188de0f9e
Signed-off-by: Yann Gautier <yann.gautier@st.com>
diff --git a/plat/st/stm32mp1/bl2_plat_setup.c b/plat/st/stm32mp1/bl2_plat_setup.c
index 3c6f48a..0c93f27 100644
--- a/plat/st/stm32mp1/bl2_plat_setup.c
+++ b/plat/st/stm32mp1/bl2_plat_setup.c
@@ -1,5 +1,5 @@
/*
- * Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
*
* SPDX-License-Identifier: BSD-3-Clause
*/
@@ -308,6 +308,8 @@
print_reset_reason();
+ stm32mp1_syscfg_enable_io_compensation_finish();
+
#if !STM32MP_USE_STM32IMAGE
fconf_populate("TB_FW", STM32MP_DTB_BASE);
#endif /* !STM32MP_USE_STM32IMAGE */
diff --git a/plat/st/stm32mp1/include/stm32mp1_private.h b/plat/st/stm32mp1/include/stm32mp1_private.h
index 2eec16f..38de1b7 100644
--- a/plat/st/stm32mp1/include/stm32mp1_private.h
+++ b/plat/st/stm32mp1/include/stm32mp1_private.h
@@ -1,5 +1,5 @@
/*
- * Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
*
* SPDX-License-Identifier: BSD-3-Clause
*/
@@ -18,7 +18,8 @@
void stm32mp1_gic_init(void);
void stm32mp1_syscfg_init(void);
-void stm32mp1_syscfg_enable_io_compensation(void);
+void stm32mp1_syscfg_enable_io_compensation_start(void);
+void stm32mp1_syscfg_enable_io_compensation_finish(void);
void stm32mp1_syscfg_disable_io_compensation(void);
void stm32mp1_deconfigure_uart_pins(void);
diff --git a/plat/st/stm32mp1/stm32mp1_syscfg.c b/plat/st/stm32mp1/stm32mp1_syscfg.c
index ac4f255..8698ec1 100644
--- a/plat/st/stm32mp1/stm32mp1_syscfg.c
+++ b/plat/st/stm32mp1/stm32mp1_syscfg.c
@@ -25,6 +25,9 @@
#define SYSCFG_CMPENSETR 0x24U
#define SYSCFG_CMPENCLRR 0x28U
+#define CMPCR_CMPENSETR_OFFSET 0x4U
+#define CMPCR_CMPENCLRR_OFFSET 0x8U
+
/*
* SYSCFG_BOOTR Register
*/
@@ -62,6 +65,45 @@
*/
#define SYSCFG_CMPENSETR_MPU_EN BIT(0)
+static void enable_io_comp_cell_finish(uintptr_t cmpcr_off)
+{
+ uint64_t start;
+
+ start = timeout_init_us(SYSCFG_CMPCR_READY_TIMEOUT_US);
+
+ while ((mmio_read_32(SYSCFG_BASE + cmpcr_off) & SYSCFG_CMPCR_READY) == 0U) {
+ if (timeout_elapsed(start)) {
+ /* Failure on IO compensation enable is not a issue: warn only. */
+ WARN("IO compensation cell not ready\n");
+ break;
+ }
+ }
+
+ mmio_clrbits_32(SYSCFG_BASE + cmpcr_off, SYSCFG_CMPCR_SW_CTRL);
+}
+
+static void disable_io_comp_cell(uintptr_t cmpcr_off)
+{
+ uint32_t value;
+
+ if (((mmio_read_32(SYSCFG_BASE + cmpcr_off) & SYSCFG_CMPCR_READY) == 0U) ||
+ ((mmio_read_32(SYSCFG_BASE + cmpcr_off + CMPCR_CMPENSETR_OFFSET) &
+ SYSCFG_CMPENSETR_MPU_EN) == 0U)) {
+ return;
+ }
+
+ value = mmio_read_32(SYSCFG_BASE + cmpcr_off) >> SYSCFG_CMPCR_ANSRC_SHIFT;
+
+ mmio_clrbits_32(SYSCFG_BASE + cmpcr_off, SYSCFG_CMPCR_RANSRC | SYSCFG_CMPCR_RAPSRC);
+
+ value <<= SYSCFG_CMPCR_RANSRC_SHIFT;
+ value |= mmio_read_32(SYSCFG_BASE + cmpcr_off);
+
+ mmio_write_32(SYSCFG_BASE + cmpcr_off, value | SYSCFG_CMPCR_SW_CTRL);
+
+ mmio_setbits_32(SYSCFG_BASE + cmpcr_off + CMPCR_CMPENCLRR_OFFSET, SYSCFG_CMPENSETR_MPU_EN);
+}
+
void stm32mp1_syscfg_init(void)
{
uint32_t bootr;
@@ -127,13 +169,11 @@
}
}
- stm32mp1_syscfg_enable_io_compensation();
+ stm32mp1_syscfg_enable_io_compensation_start();
}
-void stm32mp1_syscfg_enable_io_compensation(void)
+void stm32mp1_syscfg_enable_io_compensation_start(void)
{
- uint64_t start;
-
/*
* Activate automatic I/O compensation.
* Warning: need to ensure CSI enabled and ready in clock driver.
@@ -141,29 +181,18 @@
*/
clk_enable(SYSCFG);
- mmio_setbits_32(SYSCFG_BASE + SYSCFG_CMPENSETR,
+ mmio_setbits_32(SYSCFG_BASE + CMPCR_CMPENSETR_OFFSET + SYSCFG_CMPCR,
SYSCFG_CMPENSETR_MPU_EN);
-
- start = timeout_init_us(SYSCFG_CMPCR_READY_TIMEOUT_US);
-
- while ((mmio_read_32(SYSCFG_BASE + SYSCFG_CMPCR) &
- SYSCFG_CMPCR_READY) == 0U) {
- if (timeout_elapsed(start)) {
- /*
- * Failure on IO compensation enable is not a issue:
- * warn only.
- */
- WARN("IO compensation cell not ready\n");
- break;
- }
- }
+}
- mmio_clrbits_32(SYSCFG_BASE + SYSCFG_CMPCR, SYSCFG_CMPCR_SW_CTRL);
+void stm32mp1_syscfg_enable_io_compensation_finish(void)
+{
+ enable_io_comp_cell_finish(SYSCFG_CMPCR);
}
void stm32mp1_syscfg_disable_io_compensation(void)
{
- uint32_t value;
+ clk_enable(SYSCFG);
/*
* Deactivate automatic I/O compensation.
@@ -171,18 +200,7 @@
* requested for other usages and always OFF in STANDBY.
* Disable non-secure SYSCFG clock, we assume non-secure is suspended.
*/
- value = mmio_read_32(SYSCFG_BASE + SYSCFG_CMPCR) >>
- SYSCFG_CMPCR_ANSRC_SHIFT;
-
- mmio_clrbits_32(SYSCFG_BASE + SYSCFG_CMPCR,
- SYSCFG_CMPCR_RANSRC | SYSCFG_CMPCR_RAPSRC);
-
- value = mmio_read_32(SYSCFG_BASE + SYSCFG_CMPCR) |
- (value << SYSCFG_CMPCR_RANSRC_SHIFT);
-
- mmio_write_32(SYSCFG_BASE + SYSCFG_CMPCR, value | SYSCFG_CMPCR_SW_CTRL);
-
- mmio_setbits_32(SYSCFG_BASE + SYSCFG_CMPENCLRR, SYSCFG_CMPENSETR_MPU_EN);
+ disable_io_comp_cell(SYSCFG_CMPCR);
clk_disable(SYSCFG);
}