diff --git a/User/bootstrap.c b/User/bootstrap.c
index da8d7d8..0c9aee0 100644
--- a/User/bootstrap.c
+++ b/User/bootstrap.c
@@ -6,7 +6,7 @@
 #include "bootstrap.h"
 
 /** Global time base */
-volatile uint32_t time_ms;
+volatile uint16_t time_ms;
 
 /**
  * Putchar for printf
@@ -61,8 +61,23 @@ INTERRUPT_HANDLER(TIM4_UPD_OVF_IRQHandler, 23)
 /** Delay ms */
 void Delay(uint16_t ms)
 {
-	uint32_t start = time_ms;
-	while ((time_ms - start) < ms);
+	uint16_t start = time_ms;
+	uint16_t t2;
+	while (1) {
+		t2 = time_ms;
+		if ((t2 - start) >= ms) {
+			break;
+		}
+	}
+}
+
+/** Delay N seconds */
+void Delay_s(uint16_t s)
+{
+	while (s != 0) {
+		Delay(1000);
+		s--;
+	}
 }
 
 /**
diff --git a/User/bootstrap.h b/User/bootstrap.h
index e579c47..2475abd 100644
--- a/User/bootstrap.h
+++ b/User/bootstrap.h
@@ -6,7 +6,7 @@
 #define STM8S_STDINIT_H
 
 /** Global timebase */
-extern volatile uint32_t time_ms;
+extern volatile uint16_t time_ms;
 
 /** Uart IRQ handler */
 void UART1_RX_IRQHandler(void) INTERRUPT(18);