diff --git a/Debug/debug.c b/Debug/debug.c
index 643c934..89117bd 100644
--- a/Debug/debug.c
+++ b/Debug/debug.c
@@ -23,6 +23,29 @@ void Delay_Init(void)
 {
     p_us = SystemCoreClock / 8000000;
     p_ms = (uint16_t)p_us * 1000;
+
+    SysTick->CTLR &= ~0x8000001F;
+    SysTick->CTLR |= (1 << 0);
+}
+
+uint64_t SysTick_Read(void)
+{
+    uint64_t ticks;
+    SysTick->CTLR &= ~(1 << 0);
+    ticks = SysTick->CNT;
+    SysTick->CTLR |= (1 << 0);
+
+    return ticks;
+}
+
+uint64_t SysTick_Us(void)
+{
+    return SysTick_Read() / p_us;
+}
+
+uint64_t SysTick_Ms(void)
+{
+    return SysTick_Read() / p_ms;
 }
 
 /*********************************************************************
@@ -41,12 +64,18 @@ void Delay_Us(uint32_t n)
     SysTick->SR &= ~(1 << 0);
     i = (uint32_t)n * p_us;
 
+    SysTick->CMP = SysTick_Read() + i;
+    while((SysTick->SR & (1 << 0)) != (1 << 0));
+
+
+    /*
     SysTick->CMP = i;
     SysTick->CTLR |= (1 << 4);
     SysTick->CTLR |= (1 << 5) | (1 << 0);
 
     while((SysTick->SR & (1 << 0)) != (1 << 0));
     SysTick->CTLR &= ~(1 << 0);
+    */
 }
 
 /*********************************************************************
@@ -65,12 +94,17 @@ void Delay_Ms(uint32_t n)
     SysTick->SR &= ~(1 << 0);
     i = (uint32_t)n * p_ms;
 
+    SysTick->CMP = SysTick_Read() + i;
+    while((SysTick->SR & (1 << 0)) != (1 << 0));
+
+    /*
     SysTick->CMP = i;
     SysTick->CTLR |= (1 << 4);
     SysTick->CTLR |= (1 << 5) | (1 << 0);
 
     while((SysTick->SR & (1 << 0)) != (1 << 0));
     SysTick->CTLR &= ~(1 << 0);
+    */
 }
 
 /*********************************************************************
diff --git a/Debug/debug.h b/Debug/debug.h
index 7274522..afb7ddb 100644
--- a/Debug/debug.h
+++ b/Debug/debug.h
@@ -29,6 +29,9 @@ extern "C" {
 //#define DEBUG   DEBUG_UART3
 
 void Delay_Init(void);
+uint64_t SysTick_Read(void);
+uint64_t SysTick_Us(void);
+uint64_t SysTick_Ms(void);
 void Delay_Us(uint32_t n);
 void Delay_Ms(uint32_t n);
 void USART_Printf_Init(uint32_t baudrate);