From cf3bedfbdc744325e0039e35f37a8e12c0c63bc0 Mon Sep 17 00:00:00 2001
From: Kevin O'Connor <kevin@koconnor.net>
Date: Tue, 22 Apr 2025 01:25:03 -0400
Subject: [PATCH] stm32: Enable VOS0 power mode on stm32h723 if frequency above
 400Mhz

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
---
 src/stm32/stm32h7.c | 26 +++++++++++++++-----------
 1 file changed, 15 insertions(+), 11 deletions(-)

diff --git a/src/stm32/stm32h7.c b/src/stm32/stm32h7.c
index bac39c96..74732624 100644
--- a/src/stm32/stm32h7.c
+++ b/src/stm32/stm32h7.c
@@ -123,20 +123,24 @@ clock_setup(void)
         // Set PLL1P cpu clock frequency
         | ((pll_freq/CONFIG_CLOCK_FREQ - 1) << RCC_PLL1DIVR_P1_Pos);
 
-    // Enable VOS1 power mode
-    PWR->D3CR = 3 << PWR_D3CR_VOS_Pos;
-    while (!(PWR->D3CR & PWR_D3CR_VOSRDY))
-        ;
-
-    // Enable VOS0 (overdrive) on stm32h743/stm32h750
-#if !CONFIG_MACH_STM32H723
-    if (CONFIG_CLOCK_FREQ > 400000000) {
-        enable_pclock((uint32_t)SYSCFG);
-        SYSCFG->PWRCR |= SYSCFG_PWRCR_ODEN;
+    // Enable VOS1 power mode (or VOS0 if freq>400Mhz)
+    if (CONFIG_MACH_STM32H723) {
+        PWR->D3CR = (CONFIG_CLOCK_FREQ > 400000000 ? 0 : 3) << PWR_D3CR_VOS_Pos;
         while (!(PWR->D3CR & PWR_D3CR_VOSRDY))
             ;
-    }
+    } else {
+        PWR->D3CR = 3 << PWR_D3CR_VOS_Pos;
+        while (!(PWR->D3CR & PWR_D3CR_VOSRDY))
+            ;
+        if (CONFIG_CLOCK_FREQ > 400000000) {
+            enable_pclock((uint32_t)SYSCFG);
+#ifdef SYSCFG_PWRCR_ODEN
+            SYSCFG->PWRCR |= SYSCFG_PWRCR_ODEN;
 #endif
+            while (!(PWR->D3CR & PWR_D3CR_VOSRDY))
+                ;
+        }
+    }
 
     SCB_EnableICache();
     SCB_EnableDCache();