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);
 }