feat(stm32mp1): add timeout in IO compensation

Use a timeout during IO compensation enable function, when
waiting for ready status. If timeout expires, print a warning
message, to indicate that the SoC recommendation is not followed.

Change-Id: I98c7dcb1364b832f4f4b5fc9a0b85a3741a8af4b
Signed-off-by: Nicolas Le Bayon <nicolas.le.bayon@st.com>
Signed-off-by: Yann Gautier <yann.gautier@st.com>
diff --git a/plat/st/stm32mp1/stm32mp1_syscfg.c b/plat/st/stm32mp1/stm32mp1_syscfg.c
index 793ad71..20dd06e 100644
--- a/plat/st/stm32mp1/stm32mp1_syscfg.c
+++ b/plat/st/stm32mp1/stm32mp1_syscfg.c
@@ -1,16 +1,16 @@
 /*
- * Copyright (c) 2019-2021, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2019-2022, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#include <platform_def.h>
-
 #include <common/debug.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_dt.h>
 #include <stm32mp1_private.h>
 
@@ -54,6 +54,8 @@
 #define SYSCFG_CMPCR_RAPSRC			GENMASK(23, 20)
 #define SYSCFG_CMPCR_ANSRC_SHIFT		24
 
+#define SYSCFG_CMPCR_READY_TIMEOUT_US		10000U
+
 /*
  * SYSCFG_CMPENSETR Register
  */
@@ -129,6 +131,8 @@
 
 void stm32mp1_syscfg_enable_io_compensation(void)
 {
+	uint64_t start;
+
 	/*
 	 * Activate automatic I/O compensation.
 	 * Warning: need to ensure CSI enabled and ready in clock driver.
@@ -139,9 +143,18 @@
 	mmio_setbits_32(SYSCFG_BASE + SYSCFG_CMPENSETR,
 			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);