1 files changed, 19 insertions, 4 deletions
diff --git a/os/hal/platforms/STM32F4xx/hal_lld.c b/os/hal/platforms/STM32F4xx/hal_lld.c
index bbe45fe43..6dcac805b 100644
--- a/os/hal/platforms/STM32F4xx/hal_lld.c
+++ b/os/hal/platforms/STM32F4xx/hal_lld.c
@@ -153,7 +153,7 @@ void stm32_clock_init(void) {
 
   /* PWR initialization.*/
 #if defined(STM32F4XX) || defined(__DOXYGEN__)
-  PWR->CR = STM32_VOS & STM32_VOS_MASK;
+  PWR->CR = STM32_VOS;
   while ((PWR->CSR & PWR_CSR_VOSRDY) == 0)
     ;                           /* Waits until power regulator is stable.   */
 #else
@@ -191,16 +191,31 @@ void stm32_clock_init(void) {
   RCC->PLLCFGR = STM32_PLLQ | STM32_PLLSRC | STM32_PLLP | STM32_PLLN |
                  STM32_PLLM;
   RCC->CR |= RCC_CR_PLLON;
-  while (!(RCC->CR & RCC_CR_PLLRDY))
-    ;                           /* Waits until PLL is stable.               */
+
+#if STM32_OVERDRIVE_REQUIRED
+  /* Overdrive activation performed after activating the PLL in order to save
+     time as recommended in RM in "Entering Over-drive mode" paragraph.*/
+  PWR->CR |= PWR_CR_ODEN;
+  while (!(PWR->CSR & PWR_CSR_ODRDY)
+      ;
+  PWR->CR |= PWR_CR_ODSWEN;
+  while (!(PWR->CSR & PWR_CSR_ODSWRDY)
+      ;
 #endif
 
+  /* Waiting for PLL lock.*/
+  while (!(RCC->CR & RCC_CR_PLLRDY))
+    ;
+#endif /* STM32_OVERDRIVE_REQUIRED */
+
 #if STM32_ACTIVATE_PLLI2S
   /* PLLI2S activation.*/
   RCC->PLLI2SCFGR = STM32_PLLI2SR | STM32_PLLI2SN;
   RCC->CR |= RCC_CR_PLLI2SON;
+
+  /* Waiting for PLL lock.*/
   while (!(RCC->CR & RCC_CR_PLLI2SRDY))
-    ;                           /* Waits until PLLI2S is stable.            */
+    ;
 #endif
 
   /* Other clock-related settings (dividers, MCO etc).*/