diff --git a/os/hal/include/icu.h b/os/hal/include/icu.h
index ef42470cb..7db895fbf 100644
--- a/os/hal/include/icu.h
+++ b/os/hal/include/icu.h
@@ -140,8 +140,10 @@ typedef void (*icucallback_t)(ICUDriver *icup);
  * @notapi
  */
 #define _icu_isr_invoke_width_cb(icup) {                                    \
-  (icup)->state = ICU_IDLE;                                                 \
-  (icup)->config->width_cb(icup);                                           \
+  if ((icup)->state != ICU_WAITING) {                                       \
+    (icup)->state = ICU_IDLE;                                               \
+    (icup)->config->width_cb(icup);                                         \
+  }                                                                         \
 }
 
 /**
diff --git a/os/hal/ports/STM32/LLD/TIMv1/icu_lld.c b/os/hal/ports/STM32/LLD/TIMv1/icu_lld.c
index ae9cc944f..18d7ca7d5 100644
--- a/os/hal/ports/STM32/LLD/TIMv1/icu_lld.c
+++ b/os/hal/ports/STM32/LLD/TIMv1/icu_lld.c
@@ -604,6 +604,7 @@ void icu_lld_stop(ICUDriver *icup) {
  */
 void icu_lld_enable(ICUDriver *icup) {
 
+  icup->tim->EGR |= STM32_TIM_EGR_UG;
   icup->tim->SR = 0;                        /* Clear pending IRQs (if any). */
   if (icup->config->channel == ICU_CHANNEL_1) {
     if (icup->config->period_cb != NULL)