diff --git a/src/delay.cpp b/src/delay.cpp
index 9fef137..684cf8f 100644
--- a/src/delay.cpp
+++ b/src/delay.cpp
@@ -23,10 +23,12 @@ namespace sta
 
 namespace sta
 {
+	uint32_t gDelayUsMul = 1;
+
 	void delayUs(uint32_t us)
 	{
 		__HAL_TIM_SET_COUNTER(&STA_STM32_DELAY_US_TIM, 0);
-		while (__HAL_TIM_GET_COUNTER(&STA_STM32_DELAY_US_TIM) < us);
+		while (__HAL_TIM_GET_COUNTER(&STA_STM32_DELAY_US_TIM) < us * gDelayUsMul);
 	}
 
 
@@ -53,10 +55,13 @@ namespace sta
 		// Calculate TIM clock frequency
 		uint32_t clkFreq = pclkMul * STA_STM32_GET_HANDLE_PCLK_FREQ_FN(STA_STM32_DELAY_US_TIM)();
 		// Calculate update frequency based on prescaler value
-		uint32_t updateFreq = clkFreq / STA_STM32_DELAY_US_TIM.Init.Prescaler;
+		uint32_t prescaler = (STA_STM32_DELAY_US_TIM.Init.Prescaler) ? STA_STM32_DELAY_US_TIM.Init.Prescaler : 1;
+		uint32_t updateFreq = clkFreq / prescaler;
+
+		gDelayUsMul = updateFreq / 1000000;
 
 		// TIM must have at least microsecond precision (>= 1 MHz frequency)
-		return (updateFreq == 1000000);
+		return (updateFreq >= 1000000);
 	}
 } // namespace sta